12.1 全身动力学与任务空间控制：基于零空间投影的层级化任务实现

12.1 全身动力学与任务空间控制：基于零空间投影的层级化任务实现

12.1.1 引言：人形机器人全身控制的范式转变

传统工业机械臂的控制通常围绕单一的末端执行器任务（如轨迹跟踪）展开，其控制目标明确且自由度有限。然而，人形机器人是一个具有高度运动冗余（通常拥有30个以上的自由度）的复杂动态系统。其控制目标具有内在的多元性和潜在的冲突性：机器人可能需要同时保持整体平衡、躲避障碍、用视觉跟踪目标、并用手臂执行操作任务。这种多目标控制需求，催生了从“单任务单执行器”到“多任务全身协调”的范式转变。

任务空间控制（Task-Space Control）是这一范式的核心。其基本思想是将控制目标定义为任务空间（如末端执行器位姿、质心位置、躯干姿态）中的期望运动，而非直接控制关节空间的位置。对于冗余机器人，单一的末端执行器任务无法唯一确定所有关节的运动，这就产生了零空间（Null Space），即不改变末端执行器位姿的关节运动集合。零空间的存在，为实现不影响主任务的次级目标（如避关节极限、优化能耗、维持姿态）提供了可能性。

全身动力学与任务空间控制的核心挑战在于：如何在一个统一的框架下，系统性地定义、协调并实时求解多个具有不同优先级的任务，同时严格满足机器人自身的动力学约束（如关节力矩极限、接触力约束）。以零空间投影为基础的分层任务优先级控制，是解决这一挑战的经典且有效的方法论。

12.1.2 任务空间控制与冗余分解

12.1.2.1 任务定义与微分运动学

设机器人具有nnn个关节坐标q∈Rnq \in \mathbb{R}^nq∈Rn，并定义mmm维任务变量x∈Rmx \in \mathbb{R}^mx∈Rm，其与关节坐标的关系由前向运动学x=f(q)x = f(q)x=f(q)描述。在速度层，其微分关系由雅可比矩阵J(q)=∂f/∂q∈Rm×nJ(q) = \partial f / \partial q \in \mathbb{R}^{m \times n}J(q)=∂f/∂q∈Rm×n给出：
x˙=J(q)q˙\dot{x} = J(q) \dot{q}x˙=J(q)q˙​
当m<nm < nm<n时，系统是运动学冗余的。对于一个给定的任务速度指令x˙d\dot{x}_dx˙d​，关节速度的通解为：
q˙=J+x˙d+(In−J+J)q˙0\dot{q} = J^+ \dot{x}_d + (I_n - J^+ J) \dot{q}_0q˙​=J+x˙d​+(In​−J+J)q˙​0​
其中J+J^+J+是雅可比矩阵的伪逆，(In−J+J)(I_n - J^+ J)(In​−J+J)是到JJJ的零空间的投影矩阵，q˙0∈Rn\dot{q}_0 \in \mathbb{R}^nq˙​0​∈Rn是任意关节速度向量。上式清晰地分解为两部分：第一项J+x˙dJ^+ \dot{x}_dJ+x˙d​是保证实现主任务速度的最小范数解（最小关节速度）；第二项(In−J+J)q˙0(I_n - J^+ J) \dot{q}_0(In​−J+J)q˙​0​是零空间内的运动，它不改变主任务速度x˙\dot{x}x˙。

12.1.2.2 零空间投影与次级任务

投影矩阵(In−J+J)(I_n - J^+ J)(In​−J+J)具有幂等性（N2=NN^2 = NN2=N）和对称性（若使用加权伪逆）。次级任务通常也表述为一个希望最小化的代价函数H(q)H(q)H(q)，例如关节角度距离极限的平方和。那么，选择q˙0=−k∇qH(q)\dot{q}_0 = -k \nabla_q H(q)q˙​0​=