人形机器人操作空间与全身控制¶

背景¶

操作空间控制（OSC）在笛卡尔空间中描述机器人任务，全身控制（WBC）则利用冗余关节运动，在不干扰主任务的前提下完成较低优先级目标。这个示例给出一个便于检查的小型层级模型：平面人形机器人上身用右手跟踪移动目标，同时让左手接近另一个目标，并使躯干恢复直立。

这是一个有意简化的加速度级教学示例。它在稳定肩部坐标系中采用关节空间单位阵加速度动力学，并非生产级浮动基/接触力 WBC 模型：模型不包含刚体质量矩阵、重力补偿、力矩限制、接触、摩擦锥、压力中心约束或平衡控制。

运动学模型¶

五个关节坐标为

\[ \boldsymbol q= \begin{bmatrix} q_T&q_{RS}&q_{RE}&q_{LS}&q_{LE} \end{bmatrix}^{\mathsf T}, \]

分别表示躯干姿态、右肩和右肘、左肩和左肘。肩部原点固定在 \(\boldsymbol s=[0,L_T]^{\mathsf T}\)，其中

\[ L_T=0.60\ \mathrm m,\qquad L_1=0.38\ \mathrm m,\qquad L_2=0.30\ \mathrm m. \]

右手和左手位置分别为

\[ \boldsymbol p_R(\boldsymbol q) =\boldsymbol s+ \begin{bmatrix} L_1\cos q_{RS}+L_2\cos(q_{RS}+q_{RE})\\ L_1\sin q_{RS}+L_2\sin(q_{RS}+q_{RE}) \end{bmatrix}, \]

\[ \boldsymbol p_L(\boldsymbol q) =\boldsymbol s+ \begin{bmatrix} -L_1\cos q_{LS}-L_2\cos(q_{LS}+q_{LE})\\ L_1\sin q_{LS}+L_2\sin(q_{LS}+q_{LE}) \end{bmatrix}. \]

由于双手任务都在稳定肩部坐标系中表达，\(q_T\) 不进入任一只手的运动学。这个有意采用的简化让零空间结构清晰可见。

右手主任务¶

固定时域为 \(T=2.5\ \mathrm s\)。定义

\[ \tau=\frac{t}{T}, \qquad h(\tau)=10\tau^3-15\tau^4+6\tau^5, \]

\[ \dot h =\frac{30\tau^2-60\tau^3+30\tau^4}{T}, \qquad \ddot h =\frac{60\tau-180\tau^2+120\tau^3}{T^2}. \]

初始状态为

\[ \boldsymbol q(0) = \begin{bmatrix} 0.25&-0.45&0.95&0.60&-1.10 \end{bmatrix}^{\mathsf T}, \qquad \dot{\boldsymbol q}(0)=\boldsymbol 0. \]

期望右手轨迹为

\[ \boldsymbol p_{R,d}(t) =\boldsymbol p_R(\boldsymbol q(0)) + \begin{bmatrix} 0.04\\0.08 \end{bmatrix} h(\tau). \]

数值上，右手大约从 \([0.605445,0.578541]^{\mathsf T}\ \mathrm m\) 移动到 \([0.645445,0.658541]^{\mathsf T}\ \mathrm m\)。期望速度和加速度分别使用 \(\dot h\) 和 \(\ddot h\)。

令

\[ J=\frac{\partial\boldsymbol p_R}{\partial\boldsymbol q}, \qquad \dot J=\sum_{i=1}^{5}\frac{\partial J}{\partial q_i}\dot q_i. \]

带反馈的加速度参考量为

\[ \boldsymbol a_{\mathrm{ref}} =\ddot{\boldsymbol p}_{R,d} +36\left(\boldsymbol p_{R,d}-\boldsymbol p_R\right) +12\left(\dot{\boldsymbol p}_{R,d}-J\dot{\boldsymbol q}\right). \]

状态动力学采用如下严格任务层级：

\[ J^\#=J^{\mathsf T}\left(JJ^{\mathsf T}\right)^{-1}, \qquad N=I-J^\#J, \]

\[ \ddot{\boldsymbol q} =J^\#\left(\boldsymbol a_{\mathrm{ref}}-\dot J\dot{\boldsymbol q}\right) +N\boldsymbol z. \]

Pockit 状态为 \(\boldsymbol x=[\boldsymbol q^{\mathsf T},\dot{\boldsymbol q}^{\mathsf T}]^{\mathsf T}\)，控制量为五维零空间指令 \(\boldsymbol z\)，且

\[ \dot{\boldsymbol x} = \begin{bmatrix} \dot{\boldsymbol q}\\ \ddot{\boldsymbol q} \end{bmatrix}. \]

在这个模型中，右手雅可比矩阵只使用 \(q_{RS}\) 和 \(q_{RE}\)。解析伪逆因此给出

\[ N=\operatorname{diag}(1,0,0,1,1), \]

所以躯干和左臂加速度不会改变右手主任务加速度。

零空间目标与约束¶

左手目标由 \((q_{LS},q_{LE})=(-0.45,1.00)\) 生成，对应

\[ \boldsymbol p_L^\star \approx \begin{bmatrix} -0.597927&0.591519 \end{bmatrix}^{\mathsf T}\ \mathrm m. \]

令 \(J_L=\partial\boldsymbol p_L/\partial\boldsymbol q\)，二级目标为

\[ \begin{aligned} \min_{\boldsymbol z}\quad \int_0^T &120\lVert\boldsymbol p_L-\boldsymbol p_L^\star\rVert_2^2 +3\lVert J_L\dot{\boldsymbol q}\rVert_2^2\\ &+20q_T^2 +0.03\lVert\dot{\boldsymbol q}\rVert_2^2 +0.01\lVert\boldsymbol z\rVert_2^2 \,\mathrm dt. \end{aligned} \]

关节范围为

\[ \begin{aligned} -0.55&\le q_T\le0.55,& -1.8&\le q_{RS}\le1.2,\\ 0.35&\le q_{RE}\le1.8,& -2.2&\le q_{LS},q_{LE}\le2.2. \end{aligned} \]

所有关节速度满足 \(|\dot q_i|\le3\ \mathrm{rad/s}\)，所有零空间指令满足 \(|z_i|\le10\ \mathrm{rad/s^2}\)。右肘角的正下界使 \(2\times2\) 手臂雅可比矩阵远离伸直奇异位形。终端关节状态自由。

变量与单位¶

符号	含义	单位
\(q_T\)	躯干姿态角	rad
\(q_{RS},q_{RE}\)	右肩和右肘角度	rad
\(q_{LS},q_{LE}\)	左肩和左肘角度	rad
\(\dot{\boldsymbol q}\)	关节速度	rad/s
\(\boldsymbol z\)	零空间关节加速度指令	rad/s²
\(\boldsymbol p_R,\boldsymbol p_L\)	双手位置	m
\(J,J_L\)	双手雅可比矩阵	m/rad

建模选择¶

平面转动坐标避免了空间姿态模型所需的冗余四元数和单位范数约束。稳定肩部坐标系和单位阵加速度动力学将任务优先级概念与刚体动力学分离。脚本计算 \(J^\#=J^{\mathsf T}(JJ^{\mathsf T})^{-1}\) 的闭式结果；它与矩阵表达式在数学上完全等价，却能显著减少符号 Hessian 的生成时间。

问题使用一个固定时间的 Lobatto 阶段，包含 10 个网格区间，每个区间使用 4 个点。五次关节空间初始猜测让状态向二级目标移动，并在端点保持零速度。优化完成后，脚本在 4,001 个均匀分布的物理时刻重构全部状态和控制；除任务层级外，还会在该密集网格上检查每个关节角、关节速度和零空间加速度边界。

运行示例¶

在仓库根目录运行：

python -m examples.humanoid_whole_body_control

由于需要对非线性层级进行符号求导，首次运行会比其他入门示例更慢。如需保存图片且不打开交互窗口：

python -m examples.humanoid_whole_body_control --save humanoid-whole-body-control.png --no-show

关键实现¶

joint_angles = sp.Matrix(phase.x[:5])
joint_velocities = sp.Matrix(phase.x[5:])
z = sp.Matrix(phase.u)

right_hand, left_hand = _symbolic_hand_positions(joint_angles)
J = right_hand.jacobian(joint_angles)
J_left = left_hand.jacobian(joint_angles)
J_dot = sp.zeros(2, 5)
for i in range(5):
    J_dot += J.diff(joint_angles[i]) * joint_velocities[i]

# Closed form of J.T * (J * J.T)^-1 for the nonsingular right 2R arm.
J_arm = J[:, 1:3]
det_J_arm = (
    J_arm[0, 0] * J_arm[1, 1] - J_arm[0, 1] * J_arm[1, 0]
)
J_arm_inverse = sp.Matrix(
    [[J_arm[1, 1], -J_arm[0, 1]],
     [-J_arm[1, 0], J_arm[0, 0]]]
) / det_J_arm
J_pinv = sp.zeros(5, 2)
J_pinv[1:3, :] = J_arm_inverse
N = sp.diag(1.0, 0.0, 0.0, 1.0, 1.0)

a_ref = (
    desired_acceleration
    + 36.0 * (desired_position - right_hand)
    + 12.0 * (desired_velocity - J * joint_velocities)
)
qdd = J_pinv * (a_ref - J_dot * joint_velocities) + N * z
phase.set_dynamics([*joint_velocities, *qdd])

完整源码还会定义二级积分目标、关节边界、与动力学尺度一致的初始猜测、密集数值层级与边界检查以及全部绘图逻辑。

验证结果¶

每次成功求解后，脚本都会对任务层级进行数值验证：

检查项	验证值
最大零空间泄漏 \(\max_t\lVert JN\rVert_F\)	\(4.286\times10^{-16}\)
右手最大跟踪误差	\(1.535\times10^{-6}\ \mathrm m\)
左手初始目标误差	\(0.0820\ \mathrm m\)
左手终端目标误差	\(0.0055\ \mathrm m\)
最大密集路径边界违约	\(0\)

因此，低优先级运动在保持右手轨迹达到数值精度的同时，把左手任务误差降低了一个数量级以上。结果图使用密集历史展示采样手臂姿态、双手轨迹、任务误差、躯干调节过程和测得的零空间泄漏，且没有裁剪诊断数据。

平面人形机器人手臂姿态、双手轨迹和 OSC/WBC 层级诊断

源代码¶

完整可运行示例见： examples/humanoid_whole_body_control.py。