人形机器人质心运动重定向
背景
运动重定向需要把人体采集或设计的动作映射到身体比例不同的机器人。按腿长缩放位置可以得到有用的运动学参考，却不能保证该参考满足机器人的 Newton-Euler 动力学和接触限制。
本例把完整且预先已知的深蹲前伸参考作为预览信息，在整个时域内同时优化平面质心（CoM）轨迹、机体俯仰和双脚接触力与力矩。跟踪项使结果接近缩放后的人体动作；质心动力学、单边接触、Coulomb 摩擦、压力中心（CoP）、接触载荷及其变化率约束，则保证结果在该降阶模型内具有动力学一致性。
几何缩放参考
固定时域为 \(T=2.4\ \mathrm{s}\)。令归一化时间 \(\tau=t/T\)，定义首尾静止且二阶连续的隆起函数
\[
b(t)=64\tau^3(1-\tau)^3.
\]
其前两阶导数为
\[
\dot b(t)=\frac{64}{T}
\left(3\tau^2-12\tau^3+15\tau^4-6\tau^5\right),
\]
\[
\ddot b(t)=\frac{64}{T^2}
\left(6\tau-36\tau^2+60\tau^3-30\tau^4\right).
\]
腿长比例为
\[
s=\frac{L_{\mathrm{robot}}}{L_{\mathrm{human}}}
=\frac{0.82}{0.96}=0.854166\ldots.
\]
期望质心位置、速度、俯仰角和俯仰角速度为
\[
\boldsymbol c_d(t)=
\begin{bmatrix}
s(0.085)b(t)\\
0.84-s(0.19)b(t)
\end{bmatrix},
\qquad
\boldsymbol v_d(t)=\dot{\boldsymbol c}_d(t),
\]
\[
\theta_d(t)=0.13b(t),
\qquad
\omega_d(t)=0.13\dot b(t).
\]
因此，缩放参考在中点向前移动 \(0.072604\ \mathrm m\)、下蹲
\(0.162292\ \mathrm m\)，并达到 \(0.13\ \mathrm{rad}\) 俯仰角。参考加速度只用于构造近似平衡的初始接触载荷猜测，不作为轨迹约束强制施加。
状态、控制与单位
令 \(i\in\{L,R\}\) 分别表示左脚和右脚。状态与控制为
\[
\boldsymbol x=
\begin{bmatrix}
c_x&c_z&v_x&v_z&\theta&\omega&
f_{Lx}&f_{Lz}&m_L&f_{Rx}&f_{Rz}&m_R
\end{bmatrix}^{\mathsf T},
\]
\[
\boldsymbol u=
\begin{bmatrix}
\dot f_{Lx}&\dot f_{Lz}&\dot m_L&
\dot f_{Rx}&\dot f_{Rz}&\dot m_R
\end{bmatrix}^{\mathsf T}.
\]
| 符号 |
含义 |
单位或数值 |
| \(c_x,c_z\) |
矢状面质心位置与高度 |
m |
| \(v_x,v_z\) |
质心速度 |
m/s |
| \(\theta,\omega\) |
机体俯仰角与俯仰角速度 |
rad、rad/s |
| \(f_{ix},f_{iz}\) |
地面对机器人的切向力与法向力 |
N |
| \(m_i\) |
绕平面外轴的足部接触力矩 |
\(\mathrm{N\,m}\) |
| \(\dot f_{ix},\dot f_{iz},\dot m_i\) |
接触载荷变化率 |
\(\mathrm{N/s}\)、\(\mathrm{N\,m/s}\) |
| \(M\) |
机器人质量 |
\(62\ \mathrm{kg}\) |
| \(I_y\) |
常数质心俯仰惯量 |
\(8\ \mathrm{kg\,m^2}\) |
| \(g\) |
重力加速度 |
\(9.81\ \mathrm{m/s^2}\) |
| \(a\) |
足中心偏置；两脚中心位于 \(x=\pm a\) |
\(0.14\ \mathrm m\) |
| \(\ell\) |
足部半长 |
\(0.10\ \mathrm m\) |
| \(\mu\) |
Coulomb 摩擦系数 |
\(0.55\) |
质心动力学
两脚固定在 \((-a,0)\) 和 \((a,0)\)。平移动力学为
\[
\dot c_x=v_x,
\qquad
\dot c_z=v_z,
\]
\[
M\dot v_x=f_{Lx}+f_{Rx},
\qquad
M\dot v_z=f_{Lz}+f_{Rz}-Mg.
\]
俯仰动力学为
\[
\dot\theta=\omega,
\]
\[
I_y\dot\omega=
(-a-c_x)f_{Lz}+c_zf_{Lx}+m_L
+(a-c_x)f_{Rz}+c_zf_{Rx}+m_R.
\]
其余六个状态方程是接触载荷积分器：
\[
\frac{\mathrm d}{\mathrm dt}
\begin{bmatrix}f_{Lx}&f_{Lz}&m_L&f_{Rx}&f_{Rz}&m_R\end{bmatrix}^{\mathsf T}
=\boldsymbol u.
\]
跟踪与正则化目标
跟踪误差与归一化尺度定义为
\[
\begin{aligned}
\boldsymbol e_c&=\boldsymbol c-\boldsymbol c_d,&
\boldsymbol e_v&=\boldsymbol v-\boldsymbol v_d,\\
F_0&=Mg,&M_0&=Mg\ell.
\end{aligned}
\]
归一化接触载荷向量为
\[
\overline{\boldsymbol w}=
\begin{bmatrix}
f_{Lx}/F_0\\
(f_{Lz}-F_0/2)/F_0\\
m_L/M_0\\
f_{Rx}/F_0\\
(f_{Rz}-F_0/2)/F_0\\
m_R/M_0
\end{bmatrix}.
\]
归一化变化率 \(\overline{\boldsymbol u}\) 的每个分量，等于对应控制量除以其变化率上限。目标函数为
\[
\begin{aligned}
J=\int_0^T \big[&2400e_{cx}^2+3200e_{cz}^2
+80e_{vx}^2+100e_{vz}^2\\
&+500(\theta-\theta_d)^2
+35(\omega-\omega_d)^2\\
&+0.025\lVert\overline{\boldsymbol w}\rVert_2^2
+0.004\lVert\overline{\boldsymbol u}\rVert_2^2\big]\,\mathrm dt.
\end{aligned}
\]
接触载荷正则项倾向于近似均分静态支撑力，但没有固定双脚分配；变化率正则项用于产生平滑的接触历程。
接触与路径约束
法向力上限为
\[
F_{z,\max}=0.9Mg=547.398\ \mathrm N.
\]
每只脚都满足单边接触、平面 Coulomb 摩擦锥和 CoP 约束：
\[
0\le f_{iz}\le F_{z,\max},
\qquad
|f_{ix}|\le\mu f_{iz},
\qquad
|m_i|\le\ell f_{iz}.
\]
最后一个不等式把合力压力中心限制在 \(0.20\ \mathrm m\) 足部支撑长度内。额外的分量边界为
\[
|f_{ix}|\le F_{z,\max},
\qquad
|m_i|\le\ell F_{z,\max}=54.7398\ \mathrm{N\,m}.
\]
质心状态边界为
\[
\begin{gathered}
-0.20\le c_x\le0.20\ \mathrm m,
\qquad 0.60\le c_z\le0.94\ \mathrm m,\\
|v_x|\le0.55\ \mathrm{m/s},
\qquad |v_z|\le0.70\ \mathrm{m/s},\\
|\theta|\le0.28\ \mathrm{rad},
\qquad |\omega|\le0.80\ \mathrm{rad/s}.
\end{gathered}
\]
每只脚的接触载荷变化率边界为
\[
|\dot f_{ix}|\le900\ \mathrm{N/s},
\qquad
|\dot f_{iz}|\le2000\ \mathrm{N/s},
\qquad
|\dot m_i|\le220\ \mathrm{N\,m/s}.
\]
边界条件
两个端点都是相同的对称静态支撑状态：
\[
\boldsymbol c(0)=\boldsymbol c(T)=
\begin{bmatrix}0&0.84\end{bmatrix}^{\mathsf T}\ \mathrm m,
\]
\[
\boldsymbol v(0)=\boldsymbol v(T)=\boldsymbol0,
\qquad
\theta(0)=\theta(T)=\omega(0)=\omega(T)=0,
\]
\[
f_{Lx}=f_{Rx}=m_L=m_R=0,
\qquad
f_{Lz}=f_{Rz}=\frac{Mg}{2}=304.11\ \mathrm N
\]
在 \(t=0\) 和 \(t=T\) 均成立。
数值方法与独立验证
默认问题采用 \(10\times4\) Lobatto 网格。初始猜测跟随缩放参考，把满足动力学所需的接触载荷分配到双脚，并令足部自由力矩为零。随后 Pockit 同时优化全部状态和接触载荷变化率。
求解后，脚本在 4,001 个时刻重构轨迹，检查状态、接触载荷变化率、摩擦和 CoP 约束。脚本还对优化后的接触载荷变化率拟合三次样条，并用 solve_ivp 独立积分全部 12 个连续状态，其中 \(\mathrm{rtol}=2\times10^{-9}\)、\(\mathrm{atol}=2\times10^{-11}\)。比较时，力误差除以 \(Mg\)，力矩误差除以 \(Mg\ell\)，前六个状态误差使用各自原生单位尺度，最后取所有分量的最大值。
验证结果
| 指标 |
验证值 |
| 质心 \(x\) 跟踪 RMS 误差 |
\(0.012\ \mathrm{mm}\) |
| 质心 \(z\) 跟踪 RMS 误差 |
\(0.010\ \mathrm{mm}\) |
| 俯仰跟踪 RMS 误差 |
\(0.0005^\circ\) |
| 最小摩擦裕度 (\mu f_z- |
f_x |
| 最小 CoP 裕度 (\ell f_z- |
m |
| 最大密集状态边界违约 |
\(0\) |
| 最大独立积分缩放误差 |
\(4.933\times10^{-4}\) |
严格为正的接触裕度说明该局部最优解没有触及摩擦或 CoP 边界，但这并不能证明完整关节级人形机器人模型可行。
适用边界
本例只进行平面质心预览与运动重定向。模型采用固定双脚、常数俯仰惯量和连续双脚支撑，不包含关节坐标与力矩、运动学可达性、自碰撞、执行器限制、连杆惯量变化、接触切换、三维摩擦或扰动下平衡。
因此，优化得到的质心、俯仰和足部接触载荷历程还必须传递给下游全身逆运动学或轨迹优化阶段，由后者施加机器人的完整多体动力学与碰撞约束。
运行示例
python -m examples.humanoid_motion_retargeting
无界面运行并保存图片：
python -m examples.humanoid_motion_retargeting --save humanoid-motion-retargeting.png --no-show
可选的 --quick 参数使用较小的 \(8\times3\) 网格进行快速冒烟测试；上面的验证结果来自默认网格。
system, phase = build_problem()
guess = initial_guess(phase)
solution = solve_problem(system, guess)
plot_solution(solution)
源代码
完整可运行示例见：
examples/humanoid_motion_retargeting.py。