四足机器人中不同优先级任务的执行——Null-Space Projection方法

1. 背景

最近看了一篇论文, 是 MIT 的关于四足机器人全身动力学控制 (Whole Body Control, WBC[1]), 文中有一节是关于不同优先级任务的执行 (Prioritized Task Execution), 用的技术是零空间投影 (Null-Space Projection [2]).

所谓 Prioritized Task 通常用于冗余度机械臂、双足机器人、四足机器人等具有冗余自由度的机器人系统中, 通常有两个或以上的任务需要同时执行, 但不需要同时严格满足执行任务要求; 因为任务又分优先级, 优先满足优先级高的任务要求, 在不影响优先级高的任务执行的情况下, 尽量满足优先级低的任务要求. 打个不一定恰当的比方: 我们写字的时候首先要求“字迹端正”, 这是操作空间的末端轨迹任务要求; 然后要求“坐姿端正”是身体姿态控制的任务要求. 前者优先级高先满足该要求, 后者优先级相对前者低, 在满足第一个任务的要求的情况, 再满足第二个任务要求, 但不能影响第一个任务的实现. 比如四足机器人有身体姿态保持任务、身体位置跟踪任务、每条腿的落足点位置跟踪任务 ($\times 4$ 条腿) 等不同优先级的任务. 具体技术上一般都是利用机械臂运动学的零空间投影法实现 [2], 即使是足式机器人也借鉴机械臂的该方法. 论文 [1] 中涉及的是运动相关任务的执行, 此处也限于该内容的讨论, 力相关不同层级任务的执行同样应用这种零空间投影, 此处不涉及, 可参考 [2].

刚一看论文时对该部分的公式还是比较迷糊的, 这里简单记录自己的理解和推导. 我们以单个任务、两个任务、三个任务、任意多个任务一步一步展开说明.

2. 单个任务的情况

伪逆

参考 [3], 对于关节型机器人的运动方程 $J\dot{q}=\dot{x}$​ ​​​​的解存在, 当且仅当 $J{J}^{+}\dot{x}=\dot{x}$​​​​​ (注意这里已暗示冗余度机器人对应的 Jacobian 的右逆成立, 左逆不成立, 后面会讨论).

其中 $J^{+}$​​​​ 为 $J$​​​​ 的 Moore-Penrose Pseudoinverse [3, 4, 5], 一般称为伪逆. $J\in {\mathrm{R}}^{\mathrm{m}\times \mathrm{n}}$​​​​ 的伪逆存在两种构造方式, 一种为右逆, 另一种为左逆. 当机器人关节数 n > 末端坐标数 m 时, 使用右逆求解. 物理意义是: 冗余度机器人达到末端位姿时, 存在无穷多种解, 但以最小的关节速度代价函数达到目标位姿为最优解.

“$J^{+}=J^{\text{T}}{\left(J{J}^{\text{T}}\right)}^{-1}$​, if $J$​ is fat, $n>m$​, called a right inverse since $J{J}^{+}=I$​​​​” [5]

计算如下:
$$\begin{gathered} \text{min}\Vert \dot{q}{\Vert }^2 \\ \text{s.t.}J\dot{q}=\dot{x},\mathrm{and}J\in {\mathrm{R}}^{\mathrm{m}\times \mathrm{n}}(n>m),J\mathrm{is}\mathrm{full}\mathrm{row}\mathrm{rank} \end{gathered}$$
构造拉格朗日函数 $L=2\Vert \dot{q}{\Vert }^2+{\lambda }^{\text{T}}\left(J\dot{q}-\dot{x}\right)$​​, 分别对 $\dot{q}$​​ 和 $\lambda$​​ 求导,
$${\left(\frac{\partial L}{\partial \dot{q}}\right)}^{\text{T}}=\dot{q}-J^{\text{T}}\lambda =0\text{\hspace{1em}(a)}$$

$${\left(\frac{\partial L}{\partial \lambda }\right)}^{\text{T}}=J\dot{q}-\dot{x}=0\text{\hspace{1em}(b)}$$

$$(a)\Rightarrow \dot{q}=J^{\text{T}}\lambda \text{\hspace{1em}(c)}$$

$$(b)(c)\Rightarrow J\dot{q}=J{J}^{\text{T}}\lambda =\dot{x}\text{\hspace{1em}(d)}$$

已知 $J\in {\mathrm{R}}^{\mathrm{m}\times \mathrm{n}}(n>m),J\text{is full row rank}$​​​​, 所以 $J{J}^{\text{T}}$​​​ ​可逆, 由 (d) 可得 $\lambda ={\left(J{J}^{\text{T}}\right)}^{-1}\dot{x}$, 最后得到 $\dot{q}=J^{\text{T}}{\left(J{J}^{\text{T}}\right)}^{-1}\dot{x}$, 所以有 $J^{+}=J^{\text{T}}{\left(J{J}^{\text{T}}\right)}^{-1}$. 显然, $J{J}^{+}\dot{x}=J{J}^{\text{T}}{\left(J{J}^{\text{T}}\right)}^{-1}\dot{x}=\dot{x}$​​​​, 验证了本节开头关于运动方程有解条件的论述.

另外, 当机器人关节数 n < 末端坐标数 m 时, 使用左逆求解. 物理意义是: 少自由度机器人以最小的误差到达目标位姿 (但无法精确到达目标位置).

“$J^{+}={\left(J^{\text{T}}J\right)}^{-1}{J}^{\text{T}}$​​, if $J$​​ is tall, $n<m$​​, called a left inverse since $J^{+}J=I$​​​” [5]

计算如下:
$$\begin{gathered} \text{min}\Vert J\dot{q}-\dot{x}{\Vert }^2 \\ \mathrm{where}J\in {\mathrm{R}}^{\mathrm{m}\times \mathrm{n}}(n<m),J\mathrm{is}\mathrm{full}\mathrm{column}\mathrm{rank} \end{gathered}$$
对代价函数进行求导,
$$\frac{\partial \Vert J\dot{q}-\dot{x}{\Vert }^2}{\partial \dot{q}}=J^{\text{T}}\left(J\dot{q}-\dot{x}\right)=0\Rightarrow J^{\text{T}}J\dot{q}=J^{\text{T}}\dot{x}\text{\hspace{1em}(e)}$$
又因为 $J\in {\mathrm{R}}^{\mathrm{m}\times \mathrm{n}}(n<m),J\mathrm{is}\mathrm{full}\mathrm{column}\mathrm{rank}$​​​​, 所以 $J^{\text{T}}J$​​​​ 可逆. 由 (e) 得到$\dot{q}={\left(J^{\text{T}}J\right)}^{-1}{J}^{\text{T}}\dot{x}$​​, 所以有$J^{+}={\left(J^{\text{T}}J\right)}^{-1}{J}^{\text{T}}$​​. 再次验证本节开头提到的运动方程解存在的条件, $J{J}^{+}\dot{x}=J{\left(J^{\text{T}}J\right)}^{-1}{J}^{\text{T}}\dot{x}\ne \dot{x}$​​, 所以少自由度机器人的Jacobian的左逆求得的$\dot{q}$​​不是运动方程的解, 最多称为近似解. 其满足的方程是 (e), 而不是 $J\dot{q}=\dot{x}$​​​.

需要说明右逆还是左逆只能存在一个, 不能同时存在. 事实上两者的数学定义是一致的, 数字计算方法也一致 (SVD), 只是机器人应用中的代表的物理意义不同而已, 两者统称为伪逆. 任何矩阵的伪逆是存在且唯一的. 一般机器人中右逆情况较多 (冗余自由度), 但是多任务框架下多个任务Jacobian矩阵不能保证都是行满秩 (“fat”), 也有可能是列满秩 (“tall”). 下面的讨论假设高优先级任务Jacobian都能以右逆展开, 只有最低优先级任务Jacobian可能以左逆展开.
(但是要真的碰到了高优先级就无法达到精准任务而以左逆展开,低优先级的任务如何继续规划控制呢?同样的算法还是需要考虑别的? — 这种情况实际中估计遇不到, 因为这样的情况说明一开始机器人选型或者构型设计就有问题.)

3. 两个任务的情况

先以两个任务为例, 计算多任务的 Null-Space Projection. 另外, 以速度层面的任务规划控制展开描述, 角度差或者加速度层面的任务控制的推导说明类似 [1].

定理 1

对于冗余自由度机器人, 给定任务一 $T_1=(J_1,{\dot{x}}_1)$​​​​ 和任务二 $T_1=(J_2,{\dot{x}}_2)$​​​​, 其中 $x_1$​​​​ 和 $x_2$​​​​​ 为对应任务的期望坐标, $J_1$​​​​ 和 $J_2$​​​​ 为对应任务的 Jacobian 矩阵, $T_1$​​​​ 的优先级高于 $T_2$​​​​​ 的优先级. 为了同时执行任务一和任务二, 得到的关节空间的解为:
$$\dot{q}=J_1^{+}{\dot{x}}_1+{\left[J_2\left(I-J_1^{+}{J}_1\right)\right]}^{+}\left({\dot{x}}_2-J_2\left(J_1^{+}{\dot{x}}_1\right)\right)\text{\hspace{1em}(1)}$$

可以定义 $N_1=I-J_1^{+}{J}_1,J_{2|1}=J_2{N}_1$​​​.

推导

---

零空间投影

直接伪逆求解是对应机器人只需要执行一个任务的情况, 对于同时执行两个或者以上任务需要引入零空间投影的概念.

如果此时该关节型机器人的运动方程的解为 $\dot{q}=J^{+}\dot{x}+\left(I-J^{+}J\right)\dot{z}$​​​​​​​, $\forall z\in {\mathrm{R}}^{\mathrm{n}}$​​​​, 代回该运动运动方程有
$$\begin{gathered} J\dot{q}=J{J}^{+}\dot{x}+J\left(I-J^{+}J\right)\dot{z}=\dot{x} \\ \mathrm{Moore}-\mathrm{Penrose}\mathrm{conditions}\Rightarrow J{J}^{+}J=J\Rightarrow J\left(I-J^{+}J\right)\dot{z}=0 \end{gathered}$$
可以看出 $\left(I-J^{+}J\right)$​​​​​ 可以把任意关节空间速度投影到 Jacobian 矩阵 $J$​​ ​​​的 Null-Space, 而不会引起机器人操作空间速度的任何变化, 称 $\left(I-J^{+}J\right)$​​​​​ 为 Null-Space Projection 矩阵. $\left(I-J^{+}J\right)$​​​​​ 的作用可以认为是对任意向量 $\dot{z}$​​​​ ​ 进行过滤, 使其不对 $J^{+}\dot{x}$​​​​​​​ 构成影响. 投影矩阵需要满足对称和幂等两个条件(实数情况下), 在下面引理 1 中都有说明.

虽然已说明了在高优先级以右逆展开讨论, 但此处我们再纠结一下左逆的问题. 很显然, 如果 $J^{+}$​ 是左逆的话, $I-J^{+}J=0$​, 而剩下的 $\dot{q}=J^{+}\dot{x}$ ​也不会精确成立. 也就是说伪逆为左逆情况下, 当前任务都无法精准完成, 更不要想着存在冗余的零空间给更低优先级任务以施展空间了.

---

任务一的解

故此, 满足任务 $T_1$​​​ (即 $J_1\dot{q}={\dot{x}}_1$​​​) 的解为
$$\dot{q}=\underset{\text{特解}{\mathrm{q}}_1}{J_1^{+}{\dot{x}}_1}+\underset{\text{通解}}{\left(I-J_1^{+}{J}_1\right){\dot{z}}_2}\text{\hspace{1em}(2)}$$
其中 ${\dot{z}}_2$​​​​​​​​​ 为关节速度空间中的任意向量. 事实上, 单独的 $J_1^{+}{\dot{x}}_1$​​​​​​ 已满足任务一的要求, $z_2$​​​​​​ 的存在不会对任务一 ($q_1$​​​) 产生影响, 因为 $\left(I-J_1^{+}{J}_1\right)$​​​ 对 ${\dot{z}}_2$​​​ 进行了过滤, 将 ${\dot{z}}_2$​​​ 投影到了任务一的 Jacobian 的零空间上. ${\dot{z}}_2$​​​​​​​ 存在的意义在于任务二.

---

任务二的解

任务一已经满足的情况下, 即 $J_1\dot{q}={\dot{x}}_1$​ 完全成立; 然后尽量使得 $\dot{q}$​ 同时也去满足任务二的要求, 即使得 $J_2\dot{q}={\dot{x}}_2$​​​ 也尽量成立. 将 (2) 代入得到,
$$\begin{gathered} J_2{J}_1^{+}{\dot{x}}_1+J_2\left(I-J_1^{+}{J}_1\right){\dot{z}}_2={\dot{x}}_2\Rightarrow \\ \underset{J^{{}^{\prime }}}{J_2\left(I-J_1^{+}{J}_1\right)}{\dot{z}}_2=\underset{{\dot{x}}^{{}^{\prime }}}{{\dot{x}}_2-J_2{J}_1^{+}{\dot{x}}_1}\text{\hspace{1em}(3)} \end{gathered}$$
此时 (3)​​ 可能有无穷多解需要找到一个最少动作的最优解, 即
$$\begin{gathered} \text{min}\Vert {\dot{z}}_2{\Vert }^2 \\ \text{s.t.}{J}^{{}^{\prime }}{\dot{z}}_2={\dot{x}}^{{}^{\prime }},\mathrm{and}{J}^{{}^{\prime }}\in {\mathrm{R}}^{\mathrm{m}\times \mathrm{n}}(n>m),J^{{}^{\prime }}\mathrm{is}\mathrm{full}\mathrm{row}\mathrm{rank} \end{gathered}$$
此时 (3)​ 也可能无法找到满足任务二的解 (overdetermined), 只能寻找最小误差情况下尽量满足任务二的解, 即
$$\begin{gathered} \min \Vert J^{{}^{\prime }}{\dot{z}}_2-{\dot{x}}^{{}^{\prime }}{\Vert }^2 \\ \mathrm{where}{J}^{{}^{\prime }}\in {\mathrm{R}}^{\mathrm{m}\times \mathrm{n}}(n<m),J^{{}^{\prime }}\mathrm{is}\mathrm{full}\mathrm{column}\mathrm{rank} \end{gathered}$$
当然也可能找到唯一解, 是上述两种情况的特殊形式. 对任务二中 ${\dot{z}}_2$​​ 的计算过程和任务一中 $\dot{q}$​​​​ 的计算过程一致, 用伪逆表示就是
$${\dot{z}}_2={\left[J_2\left(I-J_1^{+}{J}_1\right)\right]}^{+}\left({\dot{x}}_2-J_2{J}_1^{+}{\dot{x}}_1\right)\text{\hspace{1em}(4)}$$
式中 $+$​​​ 对应的可能是右逆也可能是左逆, 视最低优先级任务(任务二)是否精准实现确定. 比较 (2), 此处 (4) 只考虑了满足极值条件的特解. 因为当前讨论只涉及两个任务, 所以不需要包含, 任务二可以包含但是会破坏对应极值条件的通解部分 ${\dot{z}}_3$​​. 这部分未表示的通解 ${\dot{z}}_3$​​ 是更多层级任务规划控制时所需要的, 我们在下面扩展第三个任务时再讨论. 将 (4) 代回 (2)​​​ 得到,
$$\dot{q}=J_1^{+}{\dot{x}}_1+\left(I-J_1^{+}{J}_1\right){\left[J_2\left(I-J_1^{+}{J}_1\right)\right]}^{+}\left({\dot{x}}_2-J_2{J}_1^{+}{\dot{x}}_1\right)\text{\hspace{1em}(5)}$$

---

为了简化 (5), 引入如下引理 1 [6]

If $A\in {\mathrm{K}}^{\mathrm{n}\times \mathrm{m}}$ is Hermitian and idempotent (true if anf only if it represents an orthogonal projection), then for any matrix $B\in {\mathrm{K}}^{\mathrm{m}\times \mathrm{n}}$ the following equation holds,
$$A{\left(BA\right)}^{+}={\left(BA\right)}^{+}$$
另外, 伪逆的 Moore-Penrose conditions [6] 有,
$$A{A}^{+}A=A,A^{+}A{A}^{+}=A^{+},{\left(A{A}^{+}\right)}^{\ast }=A{A}^{+},{\left(A^{+}A\right)}^{\ast }=A^{+}A$$
对照 $\left(I-J_1^{+}{J}_1\right)$ ​​有,
$${\left(I-J_1^{+}{J}_1\right)}^{\ast }=I^{\ast }-{\left(J_1^{+}{J}_1\right)}^{\ast }=I-J_1^{+}{J}_1\Rightarrow \mathrm{Hermitian}$$

$$\left(I-J_1^{+}{J}_1\right)\left(I-J_1^{+}{J}_1\right)=I-J_1^{+}{J}_1-J_1^{+}{J}_1+J_1^{+}{J}_1{J}_1^{+}{J}_1=I-J_1^{+}{J}_1\Rightarrow \mathrm{idempotent}$$

所以由引理 1 得到,
$$\left(I-J_1^{+}{J}_1\right){\left[J_2\left(I-J_1^{+}{J}_1\right)\right]}^{+}={\left[J_2\left(I-J_1^{+}{J}_1\right)\right]}^{+}$$

---

方程 (5) ​​简化为,
$$\dot{q}=J_1^{+}{\dot{x}}_1+{\left[J_2\left(I-J_1^{+}{J}_1\right)\right]}^{+}\left({\dot{x}}_2-J_2{J}_1^{+}{\dot{x}}_1\right)\text{\hspace{1em}(6)}$$

---

对 (6) ​做一点解读,
q ˙ ⏟ q ˙ 2 = J 1 + x ˙ 1 ⏟ q ˙ 1 + [ J 2 ( I − J 1 + J 1 ) ⏟ N 1 ] + ⏟ J 2 ∣ 1 + ( x ˙ 2 − J 2 J 1 + x ˙ 1 ⏟ x ˙ 2 中已被 x ˙ 1 完成的部分 ) (7) \underbrace{\dot q}_{\dot q_2} =\underbrace{ {J^{+}_{1} \dot x_1}}_{\dot q_1} + \underbrace{ \left[J_{2} \underbrace{\left(I-J^{+}_1 J_{1}\right)}_{N_1}\right]^{+}}_{J^{+}_{2|1}} \left( \dot{x}_2 - \underbrace{ J_{2} {J^{+}_{1} \dot x_1}}_{\dot x_2 {\rm \text{中已被}}\dot x_1 {\rm \text{完成的部分}}} \right) \tag{7} q˙​2​ q˙​​​=q˙​1​ J1+​x˙1​​​+J2∣1+​ ​J2​N1​ (I−J1+​J1​)​​ ​+​​ ​x˙2​−x˙2​中已被x˙1​完成的部分 J2​J1+​x˙1​​​ ​(7)

其中 $\dot{q}$​​​​​ 或者 ${\dot{q}}_2$​​​​ ​是任务一和任务二同时执行的关节速度控制指令, ${\dot{q}}_1$​​​​​ 是单独执行任务一 (第一优先级, 不要考虑低优先级任务) 的关节速度控制指令, 等式右侧第二部分是尽量执行任务二但又不能影响任务一的执行的关节速度控制指令. $J_2{J}_1^{+}{\dot{x}}_1$​​​​​ 是任务一 (高优先级) 执行过程中能对任务二 (低优先级) 产生影响的部分, 因为任务二中的这一部分已被任务一“顺便”执行完毕了, 所以从任务二中去除, 即任务二需要额外独立执行部分 $\left({\dot{x}}_2-J_2{J}_1^{+}{\dot{x}}_1\right)$​​​​​. $J_{2|1}^{+}$​​​​​ 是对 $\left({\dot{x}}_2-J_2{J}_1^{+}{\dot{x}}_1\right)$​​​​​ 进行过滤, 先通过 $N_1$​​​​​ (即 $J_1$ ​​​​​的 Null-Space Projection 矩阵), 使得 $J_{2|1}$​​​​ 的像空间 (Range Space) 与任务一关节运动控制 ${\dot{q}}_1$ ​​​​的空间正交. $J_{2|1}^{+}$​​​​ 是将任务二需要额外独立执行部分 $\left({\dot{x}}_2-J_2{J}_1^{+}{\dot{x}}_1\right)$​​​​​​ 映射到关节速度空间上, 能够保证这部分速度控制指令不会影响任务一的执行. ​

需要说明, 加速度层面的任务控制中使用了 Dynamically Consistent Pseudo-Inverse [1, 2], 在任务优先级规划中不受伪逆类型的影响, (7) ​​仍然成立.

小节

以上就是两个任务情况下的多任务执行的计算证明. 整理如下,
$${\dot{q}}_2={\dot{q}}_1+J_{2|1}^{+}\left({\dot{x}}_2-J_2{\dot{q}}_1\right)\text{\hspace{1em}(8)}$$

其中 $J_{2|1}=J_2{N}_1,N_1=I-J_1^{+}{J}_1$​​. $J_{2|1}$​ 将 $J_2$​ 的 Range space 中包含的 $J_1$​ 的 Range Space 部分投影去除.

4. 三个任务的情况

我们扩展一下, 再多一个任务呢? 存在第三个需要同时执行的任务 $T_3=\left(J_3,{\dot{x}}_3\right)$​​​, 对应优先级排在任务二的后面, 优先级为 3. 此时是 ${\dot{z}}_2$​​ 的通解部分 ${\dot{z}}_3$​​ 施展的时候; 另一方面, 要求在执行完任务一和任务二的时候机器人的运动空间仍然有冗余, 即 $J_{2|1}$​ ​还是"fat", 还有对应的零空间投影存在. 类比"两个任务的情况"的分析, $T_3$​​​ 在 $T_1$​​​ 和 $T_2$​​​ 的基础上进行规划或控制, 是优先满足高优先级任务的情况下尽量满足 $T_3$​​​ 的任务需求.

定理 2

已知 ${\dot{q}}_2$​​​​​ 是 $T_1$​​​​​ 和 $T_2$​​​​​ 两个任务执行命令的合, 故 $T_3$​​​​​ 的任务执行命令是在 ${\dot{q}}_2$​​​​​​ 的基础上展开,
$$\dot{q}={\dot{q}}_2+J_{3|2}^{+}\left({\dot{x}}_3-J_3{\dot{q}}_2\right)\text{\hspace{1em}(9)}$$
其中 $J_{3|2}=J_3{N}_2$, $N_2=N_1(I-J_{2|1}^{+}{J}_{2|1})$, 和 ${\dot{z}}_3=J_{3|2}^{+}({\dot{x}}_3-J_3{\dot{q}}_2)$.​

---

推导

在执行任务 $T_1$​​​​​ 和 $T_2$​​​​​ 的时候, 为了满足 $T_1$​​​​​ 和 $T_2$​​ ​​​的任务需求, 已经对任务 $T_3$​​​​​ 的执行产生了影响 (反之则不被允许) — $J_3{\dot{q}}_2$​​​​​. 也就是说, 低优先级任务被高优先级任务"顺便/被动"地执行了一部分. 所以先要从原始的任务 $T_3$​​​​​ 需求中减去在已被执行的部分, 以防止重复执行, 即 (9) 中的 $\left({\dot{x}}_3-J_3{\dot{q}}_2\right)$​​​​​ 部分.

$J_{3|2}^{+}$​​​ 的作用是先通过 Null-Space Projection 的方法去除执行低优先级任务 $T_3$​​​ 时对高优先级任务 $T_1$​​​ 和 $T_2$​​​​ 的影响 (过滤或投影), 再通过 Jacobian 矩阵获得关节空间任务命令.

首先 $T_3$​​​​​​​ 不能影响到 $T_1$​​​​​​​, 类比 (8) 对 Jacobian 矩阵 $J_3$​​​​​ 进行映射, 得到 $J_{3|1}=J_3{N}_1$​​​​​; 此时, $T_2$​​​​​ 也同样不能影响到 $T_1$​​​​​, 由上部分已知 $J_{2|1}=J_2{N}_1$​​​​​. 此时$J_{3|1}$​​​​​ 的 Range Sapce 和 $J_{2|1}$​​​​​ 的 Range Sapce 都在 $J_1$​​​​​ 的 Null Space 中, 故 $J_{3|1}$ ​​​​​和 $J_{2|1}$​​​​​ 是各自去除对任务一影响后的任务三和任务二的"新的 Jacobian 矩阵". 所以, 要求 $T_3$ ​​​​​同时对 $T_1$ ​​​​​和 $T_2$ ​​​​​不构成影响, 此时只要使得新得到的 $J_{3|1}$ ​​​​​的 Range Sapce 不与 $J_{2|1}$ ​​​​​​​的 Range Space 存在交集 (两者之间正交) 即可. 数学表示
$$J_{3|2}=J_{3|1}{N}_{2|1}=J_3{N}_1{N}_{2|1}=J_3{N}_2\text{\hspace{1em}(10)}$$
其中, $N_2=N_1{N}_{2|1}$​, $N_{2|1}=I-J_{2|1}^{+}{J}_{2|1}$​​. 他们之间的投影关系图形示意如下,
$$\begin{gathered} J_{3|1}:J_3\stackrel{[\perp ]}{⟶}{J}_1 \\ {J}_{2|1}:J_2\stackrel{[\perp ]}{⟶}{J}_1 \\ {J}_{3|2}:J_{3|1}\stackrel{[\perp ]}{⟶}{J}_{2|1} \end{gathered}$$

“$J_{i|pre}$​ is the projection of the i-th task Jacobian into the null space of the prior tasks.”[1]

定理 2 由此推到获得. 在任务 $T_1$​​​、$T_2$​​​、$T_3$​​​ 同时执行的情况下, 最终的关节空间速度命令 ${\dot{q}}_3$​​​ 由三级任务合成,
$$\{\begin{array}{lr}{\dot{q}}_1=J_1^{+}{\dot{x}}_1,& 1st\mathrm{Priority}\\ {\dot{q}}_2={\dot{q}}_1+{\left[J_{2|1}\right]}^{+}\left({\dot{x}}_2-J_2{\dot{q}}_1\right),& 2nd\mathrm{Priority}\\ {\dot{q}}_3={\dot{q}}_2+{\left[J_{3|2}\right]}^{+}\left({\dot{x}}_3-J_3{\dot{q}}_2\right),& 3rd\mathrm{Priority}\end{array}\text{\hspace{1em}(11)}$$
其中 ${\dot{q}}_1$ ​和 ${\dot{q}}_2$​ 只用于中间计算环节, 最终输出是合成的关节空间速度命令 ${\dot{q}}_3$​​.

5. 任意多个任务

如果存在第四级、第五级甚至更多任务情况下，依上一节描述类推.

6. 源代码比对

源代码参见 MIT 开源的 “[GitHub - mit-biomimetics/Cheetah-Software]” [1, 7], 这里不展开讨论, 只是原文引用以提示和上面推导计算对应的程序实现部分.

“LocomotionCtrl.cpp” 中对应的多层级任务的设置/更新:

template<typename T>
void LocomotionCtrl<T>::_ContactTaskUpdate(void* input, ControlFSMData<T> & data){
  _input_data = static_cast<LocomotionCtrlData<T>* >(input);

  _ParameterSetup(data.userParameters);
  
  // Wash out the previous setup
  _CleanUp();

  _quat_des = ori::rpyToQuat(_input_data->pBody_RPY_des);

  Vec3<T> zero_vec3; zero_vec3.setZero();
  _body_ori_task->UpdateTask(&_quat_des, _input_data->vBody_Ori_des, zero_vec3);
  _body_pos_task->UpdateTask(
      &(_input_data->pBody_des), 
      _input_data->vBody_des, 
      _input_data->aBody_des);

  WBCtrl::_task_list.push_back(_body_ori_task);
  WBCtrl::_task_list.push_back(_body_pos_task);

  for(size_t leg(0); leg<4; ++leg){
    if(_input_data->contact_state[leg] > 0.){ // Contact
      _foot_contact[leg]->setRFDesired((DVec<T>)(_input_data->Fr_des[leg]));
      _foot_contact[leg]->UpdateContactSpec();
      WBCtrl::_contact_list.push_back(_foot_contact[leg]);

    }else{ // No Contact (swing)
      _foot_task[leg]->UpdateTask(
          &(_input_data->pFoot_des[leg]), 
          _input_data->vFoot_des[leg], 
          _input_data->aFoot_des[leg]);
          //zero_vec3);
      WBCtrl::_task_list.push_back(_foot_task[leg]);
    }
  }
}

“KinWBC.cpp” 对应的多层级任务规划的计算实现:

template <typename T>
bool KinWBC<T>::FindConfiguration(
    const DVec<T>& curr_config, const std::vector<Task<T>*>& task_list,
    const std::vector<ContactSpec<T>*>& contact_list, DVec<T>& jpos_cmd,
    DVec<T>& jvel_cmd) {

  // Contact Jacobian Setup
  DMat<T> Nc(num_qdot_, num_qdot_); Nc.setIdentity();
  if(contact_list.size() > 0){
    DMat<T> Jc, Jc_i;
    contact_list[0]->getContactJacobian(Jc);
    size_t num_rows = Jc.rows();

    for (size_t i(1); i < contact_list.size(); ++i) {
      contact_list[i]->getContactJacobian(Jc_i);
      size_t num_new_rows = Jc_i.rows();
      Jc.conservativeResize(num_rows + num_new_rows, num_qdot_);
      Jc.block(num_rows, 0, num_new_rows, num_qdot_) = Jc_i;
      num_rows += num_new_rows;
    }

    // Projection Matrix
    _BuildProjectionMatrix(Jc, Nc);
  }

  // First Task
  DVec<T> delta_q, qdot;
  DMat<T> Jt, JtPre, JtPre_pinv, N_nx, N_pre;

  Task<T>* task = task_list[0];
  task->getTaskJacobian(Jt);
  JtPre = Jt * Nc;
  _PseudoInverse(JtPre, JtPre_pinv);

  delta_q = JtPre_pinv * (task->getPosError());
  qdot = JtPre_pinv * (task->getDesVel());

  DVec<T> prev_delta_q = delta_q;
  DVec<T> prev_qdot = qdot;

  _BuildProjectionMatrix(JtPre, N_nx);
  N_pre = Nc * N_nx;

  for (size_t i(1); i < task_list.size(); ++i) {
    task = task_list[i];

    task->getTaskJacobian(Jt);
    JtPre = Jt * N_pre;

    _PseudoInverse(JtPre, JtPre_pinv);
    delta_q =
        prev_delta_q + JtPre_pinv * (task->getPosError() - Jt * prev_delta_q);
    qdot = prev_qdot + JtPre_pinv * (task->getDesVel() - Jt * prev_qdot);

    // For the next task
    _BuildProjectionMatrix(JtPre, N_nx);
    N_pre *= N_nx;
    prev_delta_q = delta_q;
    prev_qdot = qdot;
  }
  for (size_t i(0); i < num_act_joint_; ++i) {
    jpos_cmd[i] = curr_config[i + 6] + delta_q[i + 6];
    jvel_cmd[i] = qdot[i + 6];
  }
  return true;
}

7. 总结

以上是自己阅读四足机器人一篇论文 [1] 过程中涉及的多优先级任务同步执行知识点的相对浅显和直觉的理解, 查阅更多论文可以发现更多严格的论述和证明.

欢迎讨论和指出问题, 请不啬指教!

(终于写完了)

参考文献

[1] D. Kim, J. Carlo, B. Katz, G. Bledt, Sangbae Kim, Highly Dynamic Quadruped Locomotion via Whole-Body Impulse Control and Model Predictive Control (https://www.researchgate.net/publication/335855260_Highly_Dynamic_Quadruped_Locomotion_via_Whole-Body_Impulse_Control_and_Model_Predictive_Control)

[2] Alexander Dietrich1, Christian Ott1, Alin Albu-Schaffer, An Overview of Null Space Projections for Redundant, Torque Controlled Robots (https://elib.dlr.de/101443/2/NullspaceSurvey.pdf)

[3] The Moore-Penrose Pseudoinverse (Math 33A: Laub) (https://www.math.ucla.edu/~laub/33a.2.12s/mppseudoinverse.pdf)

[4] Chapter 11 Least Squares, Pseudo-Inverses, PCA & SVD (http://sci.utah.edu/~gerig/CS6640-F2012/Materials/pseudoinverse-cis61009sl10.pdf)

[5] Kevin M. Lynch, Frank C. Park, Modern robotics: mechanics, planning, and control, Page 229 (https://www.amazon.com/Modern-Robotics-Mechanics-Planning-Control-ebook/dp/B073XK7KP8)

[6] Moore-Penrose inverse, Wikipedia

[7] GitHub - mit-biomimetics/Cheetah-Software