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

一. 背景

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

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

刚一看论文时对该部分的公式还是比较迷糊的, 简单记录自己的理解和推导. 我们先看两任务的情况.

二. 两个任务的情况

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

定理1

给定任务一 T 1 = ( J 1 ,   x ˙ 1 ) T_1=(J_1,~\dot x_1) T1​=(J1​, x˙1​)​和任务二 T 1 = ( J 2 ,   x ˙ 2 ) T_1=(J_2,~\dot x_2) T1​=(J2​, x˙2​)​, 其中 x 1 x_1 x1​​和 x 2 x_2 x2​​​为对应任务的期望坐标, J 1 J_1 J1​​和 J 2 J_2 J2​​为对应任务的Jacobian矩阵, T 1 T_1 T1​​的优先级高于 T 2 T_2 T2​​​的优先级. 为了同时执行任务一和任务二, 得到的关节空间的解为:
q ˙ = J 1 + x ˙ 1 + [ J 2 ( I − J 1 + J 1 ) ] + ( x ˙ 2 − J 2 ( J 1 + x ˙ 1 ) ) (1) \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) \tag{1} q˙​=J1+​x˙1​+[J2​(I−J1+​J1​)]+(x˙2​−J2​(J1+​x˙1​))(1)

可以定义 N 1 = I − J 1 + J 1 ,   J 2 ∣ 1 = J 2 N 1 N_{1} = I- J^{+}_1 J_{1},~J_{2|1}=J_{2} N_{1} N1​=I−J1+​J1​, J2∣1​=J2​N1​​​.

推导

---

伪逆和零空间

参考[3], 对于关节型机器人的运动方程 J q ˙ = x ˙ J\dot q = \dot x Jq˙​=x˙​​​的解存在, 当且仅当 J J + x ˙ = x ˙ JJ^{+}\dot x=\dot x JJ+x˙=x˙​​​（注意这里的前提是机器人不处于构型奇点 ⇒ \Rightarrow ⇒​存在伪逆）. 其中 J + J^{+} J+​为 J J J​的Moore-Penrose Pseudoinverse[3, 4, 5], 一般称为伪逆. J ∈ R m × n J \in \rm R^{m \times n} J∈Rm×n​的伪逆存在两种构造方式, 一种为右逆, 另一种为左逆. 当机器人关节数n > 末端坐标数m时, 使用右逆求解, 物理意义是冗余度机器人达到末断位姿存在无穷多种解, 求解出以最小的关节运动达到目标位姿.

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

计算如下:
m i n ∣ ∣ q ˙ ∣ ∣ 2 s . t . J q ˙ = x ˙ ,   a n d   J ∈ R m × n   ( n > m ) ,   J   i s   f u l l   r o w   r a n k min||\dot q||_2\\ s.t. J\dot q=\dot x, ~{\rm and} ~ J\in {\rm R^{m\times n}}~(n>m), ~J ~{\rm is ~full~ row~ rank} min∣∣q˙​∣∣2​s.t.Jq˙​=x˙, and J∈Rm×n (n>m), J is full row rank
构造拉格朗日函数 L = 2 ∣ ∣ q ˙ ∣ ∣ 2 + λ T ( J q ˙ − x ˙ ) L=2||\dot q||_2 + \lambda^{T}\left(J\dot q-\dot x\right) L=2∣∣q˙​∣∣2​+λT(Jq˙​−x˙), 分别对 q ˙ \dot q q˙​和 λ \lambda λ求导,
( ∂ L ∂ q ˙ ) T = q ˙ − J T λ = 0 (a) \left(\frac{\partial L}{\partial \dot q}\right)^{T} =\dot q -J^{T}\lambda=0 \tag{a} (∂q˙​∂L​)T=q˙​−JTλ=0(a)

( ∂ L ∂ λ ) T = J q ˙ − x ˙ = 0 (b) \left(\frac{\partial L}{\partial \lambda}\right)^{T}= J\dot q-\dot x =0 \tag{b} (∂λ∂L​)T=Jq˙​−x˙=0(b)

( a ) ⇒ q ˙ = J T λ (c) (a) \Rightarrow\dot q =J^{T} \lambda \tag{c} (a)⇒q˙​=JTλ(c)

( b ) ( c ) ⇒ J q ˙ = J J T λ = x ˙ (d) ({b})({c})\Rightarrow J\dot q =JJ^{T}\lambda =\dot x \tag{d} (b)(c)⇒Jq˙​=JJTλ=x˙(d)

已知 J ∈ R m × n   ( n > m ) ,   J   i s   f u l l   r o w   r a n k J\in {\rm R^{m\times n}}~(n>m), ~J ~is ~full~ row~ rank J∈Rm×n (n>m), J is full row rank, 所以 J J T JJ^{T} JJT可逆, 由 ( d ) ({d}) (d)可得 λ = ( J J T ) − 1 x ˙ \lambda=\left(JJ^{T}\right)^{-1}\dot x λ=(JJT)−1x˙, 最后得到 q ˙ = J T ( J J T ) − 1 x ˙ \dot q = J^{T}\left(JJ^{T}\right)^{-1}\dot x q˙​=JT(JJT)−1x˙​, 所以有 J + = J T ( J J T ) − 1 J^{+}=J^{T}\left(JJ^{T}\right)^{-1} J+=JT(JJT)−1​.

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

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

计算如下:
m i n ∣ ∣ J q ˙ − x ˙ ∣ ∣ 2 w h e r e   J ∈ R m × n   ( n < m ) ,   J   i s   f u l l   c o l u m n   r a n k min||J\dot q - \dot x||_2\\ {\rm where} ~ J\in {\rm R^{m\times n}}~(n<m), ~J ~{\rm is ~full~ column ~ rank} min∣∣Jq˙​−x˙∣∣2​where J∈Rm×n (n<m), J is full column rank
对代价函数进行求导,
∂ ∣ ∣ J q ˙ − x ˙ ∣ ∣ 2 ∂ q ˙ = J T ( J q ˙ − x ˙ ) = 0 ⇒ J T J q ˙ = J T x ˙ (e) \frac{\partial{||J\dot q - \dot x||_2}}{\partial \dot q} = J^{T} \left( J \dot q -\dot x \right)=0 \Rightarrow J^{T}J \dot q = J^{T}\dot x \tag{e} ∂q˙​∂∣∣Jq˙​−x˙∣∣2​​=JT(Jq˙​−x˙)=0⇒JTJq˙​=JTx˙(e)
又因为 J ∈ R m × n   ( n < m ) ,   J   i s   f u l l   c o l u m n   r a n k J\in {\rm R^{m\times n}}~(n<m), ~J ~{\rm is ~full~ column ~ rank} J∈Rm×n (n<m), J is full column rank, 所以 J T J J^{T}J JTJ可逆. 由 ( e ) ({e}) (e)得到 q ˙ = ( J T J ) − 1 J T x ˙ \dot q = \left(J^{T} J\right)^{-1} J^{T}\dot x q˙​=(JTJ)−1JTx˙, 所以有 J + = ( J T J ) − 1 J T J^{+} = \left(J^{T} J\right)^{-1} J^{T} J+=(JTJ)−1JT.

需要说明右逆还是左逆只能存在一个, 不能同时存在. 事实上两者的数学定义是一致的, 数字计算方法也一致（SVD）, 只是机器人应用中的代表的物理意义不同而已, 两者统称为伪逆. 任何矩阵的伪逆是存在且唯一的. 一般机器人中右逆情况较多（冗余*度）, 但是多任务框架下多个任务Jacobian矩阵不能保证都是行满秩（“fat”）, 也有可能是列满秩（“tall”）, 下面不区分也不需要区分是右逆还是左逆.

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

---

任务一的解

故此, 满足任务 T 1 T_1 T1​​​（即 J 1 q ˙ = x ˙ 1 J_1 \dot q = \dot x_1 J1​q˙​=x˙1​​​）的解为
q ˙ = J 1 + x ˙ 1 ⏟ 特 解   q 1 + ( I − J 1 + J 1 ) ⏟ 通 解 z ˙ 2 (2) \dot q = \underbrace{J^{+}_{1} \dot x_1}_{\rm 特解 ~ q_1} + \underbrace{\left(I-J^{+}_1 J_{1}\right)}_{\rm 通解} \dot z_2 \tag{2} q˙​=特解 q1​ J1+​x˙1​​​+通解 (I−J1+​J1​)​​z˙2​(2)
其中 z ˙ 2 \dot z_2 z˙2​​​​​​​为关节速度空间中的任意向量. 事实上, 单独的 J 1 + x ˙ 1 J^{+}_1 \dot x_1 J1+​x˙1​​​​已满足任务一的要求, z 2 z_2 z2​​​​的存在不会对任务一（ q 1 q_1 q1​）产生影响, 因为 ( I − J 1 + J 1 ) \left(I-J^{+}_1 J_1\right) (I−J1+​J1​)对 z ˙ 2 \dot z_2 z˙2​进行了过滤, 将 z ˙ 2 \dot z_2 z˙2​投影到了任务一的Jacobian的零空间上. z ˙ 2 \dot z_2 z˙2​​​​​存在的意义在于任务二.

---

任务二的解

任务一已经满足的情况下, 即 J 1 q ˙ = x ˙ 1 J_{1} \dot q =\dot x_1 J1​q˙​=x˙1​完全成立; 然后尽量使得 q ˙ \dot q q˙​同时也去满足任务二的要求, 即使得 J 2 q ˙ = x ˙ 2 J_2 \dot q =\dot x_2 J2​q˙​=x˙2​​​也尽量成立. 将 ( 2 ) ({2}) (2)代入得到,
J 2 J 1 + x ˙ 1 + J 2 ( I − J 1 + J 1 ) z ˙ 2 = x ˙ 2    ⇒ J 2 ( I − J 1 + J 1 ) ⏟ J ′ z ˙ 2 = x ˙ 2 − J 2 J 1 + x ˙ 1 ⏟ x ˙ ′ (3) J_{2} {J^{+}_{1} \dot x_1} + J_{2} {\left(I-J^{+}_1 J_{1}\right)} \dot z_2 = \dot{x}_2 ~~\Rightarrow\\ \underbrace{ J_{2} {\left(I-J^{+}_1 J_{1}\right)}}_{J^{'}} \dot z_2 =\underbrace{ \dot{x}_2 - J_{2} {J^{+}_{1} \dot x_1}}_{\dot x^{'}} \tag{3} J2​J1+​x˙1​+J2​(I−J1+​J1​)z˙2​=x˙2​  ⇒J′ J2​(I−J1+​J1​)​​z˙2​=x˙′ x˙2​−J2​J1+​x˙1​​​(3)
此时 ( 3 ) ({3}) (3)​可能有无穷多解需要找到一个最少动作的最优解, 即
m i n ∣ ∣ z ˙ 2 ∣ ∣ 2 s . t . J ′ z ˙ 2 = x ˙ ′ ,   a n d   J ′ ∈ R m × n   ( n > m ) ,   J ′   i s   f u l l   r o w   r a n k min||\dot z_2||_2\\ s.t. J^{'}\dot z_2={\dot x^{'}}, ~{\rm and} ~ J^{'}\in {\rm R^{m\times n}}~(n>m), ~J^{'} ~{\rm is ~full~ row~ rank} min∣∣z˙2​∣∣2​s.t.J′z˙2​=x˙′, and J′∈Rm×n (n>m), J′ is full row rank
此时 ( 3 ) ({3}) (3)也可能无法找到满足任务二的解（overdetermined）, 只能寻找最小误差情况下尽量满足任务二的解, 即
m i n ∣ ∣ J ′ z ˙ 2 − x ˙ ′ ∣ ∣ 2 w h e r e   J ′ ∈ R m × n   ( n < m ) ,   J ′   i s   f u l l   c o l u m n   r a n k min||J^{'}\dot z_{2} - \dot x^{'}||_2\\ {\rm where} ~ J^{'}\in {\rm R^{m\times n}}~(n<m), ~J^{'} ~{\rm is ~full~ column ~ rank} min∣∣J′z˙2​−x˙′∣∣2​where J′∈Rm×n (n<m), J′ is full column rank
当然也可能找到唯一解, 是上述两种情况的特殊形式. 对任务二中 z ˙ 2 \dot z_{2} z˙2​的计算过程和任务一中 q ˙ \dot q q˙​​​的计算过程一致, 用伪逆表示就是
z ˙ 2 = [ J 2 ( I − J 1 + J 1 ) ] + ( x ˙ 2 − J 2 J 1 + x ˙ 1 ) (4) \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) \tag{4} z˙2​=[J2​(I−J1+​J1​)]+(x˙2​−J2​J1+​x˙1​)(4)
比较 ( 2 ) ({2}) (2), 此处 ( 4 ) ({4}) (4)只考虑了满足极值条件的特解. 因为当前讨论只涉及两个任务, 所以暂时不包含任务二的 z ˙ 2 \dot z_2 z˙2​可以包含但是会破坏对应极值条件的通解部分. 这部分未表示通解是更多层级任务规划控制时所需要的, 我们在下面扩展第三个任务时再讨论. 将 ( 4 ) ({4}) (4)代回 ( 2 ) ({2}) (2)​得到,
q ˙ = J 1 + x ˙ 1 + ( I − J 1 + J 1 ) [ J 2 ( I − J 1 + J 1 ) ] + ( x ˙ 2 − J 2 J 1 + x ˙ 1 ) (5) \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) \tag{5} q˙​=J1+​x˙1​+(I−J1+​J1​)[J2​(I−J1+​J1​)]+(x˙2​−J2​J1+​x˙1​)(5)

---

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

If A ∈ K n × m A\in {\rm K^{n\times m}} A∈Kn×m is Hermitian and idempotent (true if anf only if it represents an orthogonal projection), then for any matrix B ∈ K m × n B \in {\rm K^{m\times n}} B∈Km×n the following equation holds,
A ( B A ) + = ( B A ) + A\left(BA\right)^{+} = \left(BA\right)^{+} A(BA)+=(BA)+
另外, 伪逆的Moore-Penrose conditions[6]有,
A A + A = A ,   A + A A + = A + ,    ( A A + ) ∗ = A A + ,    ( A + A ) ∗ = A + A AA^{+}A=A,~ A^{+}AA^{+}=A^{+},~~ \left(AA^{+}\right)^{*}=AA^{+},~ ~\left(A^{+}A\right)^{*}=A^{+}A AA+A=A, A+AA+=A+,  (AA+)∗=AA+,  (A+A)∗=A+A
对照 ( I − J 1 + J 1 ) {\left(I-J^{+}_1 J_{1}\right)} (I−J1+​J1​)​有,
( I − J 1 + J 1 ) ∗ = I ∗ − ( J 1 + J 1 ) ∗ = I − J 1 + J 1    ⇒    H e r m i t i a n {\left(I-J^{+}_1 J_{1}\right)} ^{*}=I^{*}-\left(J^{+}_1 J_{1}\right)^{*}=I-J^{+}_1 J_{1}~ ~\Rightarrow ~~{\rm Hermitian} (I−J1+​J1​)∗=I∗−(J1+​J1​)∗=I−J1+​J1​  ⇒  Hermitian

( I − J 1 + J 1 ) ( I − J 1 + J 1 ) = I − J 1 + J 1 − J 1 + J 1 + J 1 + J 1 J 1 + J 1 = I − J 1 + J 1    ⇒    i d e m p o t e n t {\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 ~~ {\rm idempotent} (I−J1+​J1​)(I−J1+​J1​)=I−J1+​J1​−J1+​J1​+J1+​J1​J1+​J1​=I−J1+​J1​  ⇒  idempotent

所以由引理1得到,
( I − J 1 + J 1 ) [ J 2 ( I − J 1 + J 1 ) ] + = [ J 2 ( I − J 1 + J 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]^{+} (I−J1+​J1​)[J2​(I−J1+​J1​)]+=[J2​(I−J1+​J1​)]+

---

方程 ( 5 ) ({5}) (5)​简化为,
q ˙ = J 1 + x ˙ 1 + [ J 2 ( I − J 1 + J 1 ) ] + ( x ˙ 2 − J 2 J 1 + x ˙ 1 ) (6) {\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) \tag{6} q˙​=J1+​x˙1​+[J2​(I−J1+​J1​)]+(x˙2​−J2​J1+​x˙1​)(6)

---

对 ( 6 ) ({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 中已被}\dot x_1 {\rm 完成的部分}} \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)

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

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

小节

以上就是两个任务情况下的多任务执行的计算证明. 整理如下,
q ˙ 2 = q ˙ 1 + J 2 ∣ 1 + ( x ˙ 2 − J 2 q ˙ 1 ) (8) \dot q_2=\dot q_1 + J_{2|1}^{+}\left(\dot x_2 - J_2 \dot q_1\right) \tag{8} q˙​2​=q˙​1​+J2∣1+​(x˙2​−J2​q˙​1​)(8)

其中 J 2 ∣ 1 = J 2 N 1 ,   N 1 = I − J 1 + J 1 J_{2|1}=J_{2} N_{1},~N_{1} = I- J^{+}_1 J_{1} J2∣1​=J2​N1​, N1​=I−J1+​J1​​. J 2 ∣ 1 J_{2|1} J2∣1​将 J 2 J_2 J2​的Range space中包含的 J 1 J_1 J1​的Range Space部分投影去除.

三. 三个任务的情况

我们扩展一下, 再多一个任务呢? 存在第三个需要同时执行的任务 T 3 = ( J 3 , x ˙ 3 ) T_3=\left(J_{3}, \dot x_{3}\right) T3​=(J3​,x˙3​), 对应优先级排在任务二的后面, 为3. 类比"两个任务的情况"的分析, T 3 T_3 T3​在 T 1 T_1 T1​和 T 2 T_2 T2​的基础上进行规划或控制, 是优先满足高优先级任务的情况下尽量满足 T 3 T_3 T3​的任务需求.

定理2

已知 q ˙ 2 \dot q_2 q˙​2​是 T 1 T_1 T1​和 T 2 T_2 T2​两个任务执行命令的合, 故 T 3 T_3 T3​的任务执行命令是在 q 2 q_2 q2​​的基础上展开,
q ˙ = q ˙ 2 + J 3 ∣ 2 + ( x ˙ 3 − J 3 q ˙ 2 ) (9) \dot q = \dot q_2 + J^{+}_{3|2}\left(\dot x_3 -J_3 \dot q_2\right) \tag{9} q˙​=q˙​2​+J3∣2+​(x˙3​−J3​q˙​2​)(9)
其中 J 3 ∣ 2 = J 3 N 2 ,   N 2 = N 1 ( I − J 2 ∣ 1 + J 2 ∣ 1 ) J_{3|2}=J_3 N_2,~ N_2=N_1 \left(I-J^{+}_{2|1} J_{2|1}\right) J3∣2​=J3​N2​, N2​=N1​(I−J2∣1+​J2∣1​)​.

---

推导

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

J 3 ∣ 2 + J^{+}_{3|2} J3∣2+​的作用是先通过Null-Space Projection的方法去除执行低优先级任务 T 3 T_3 T3​时对高优先级任务 T 1 T_{1} T1​和 T 2 T_2 T2​​的影响(过滤或投影), 再通过雅克比矩阵获得关节空间任务命令.

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

“ J i ∣ p r e J_{i|pre} Ji∣pre​​ is the projection of the i-th task Jacobian into the null space of the prior tasks.”[1]

定理2由此推到获得. 在任务 T 1 T_1 T1​、 T 2 T_2 T2​、 T 3 T_3 T3​同时执行的情况下, 最终的关节空间速度命令 q ˙ 3 \dot q_3 q˙​3​由三级任务合成,
{ q ˙ 1 = J 1 + x ˙ 1 , 1 s t   P r i o r i t y q ˙ 2 = q ˙ 1 + [ J 2 ∣ 1 ] + ( x ˙ 2 − J 2 q ˙ 1 ) , 2 n d   P r i o r i t y q ˙ 3 = q ˙ 2 + [ J 3 ∣ 2 ] + ( x ˙ 3 − J 3 q ˙ 2 ) , 3 r d   P r i o r i t y (11) \left\{ \begin{array}{lr} \dot q_1 = J^{+}_1 \dot x_1, &{\rm 1st~ Priority}\\ \dot q_2 = \dot q_1 +\left[J_{2|1}\right]^{+}\left(\dot x_2 - J_2 \dot q_1\right), &{\rm 2nd~Priority}\\ \dot q_3 = \dot q_2 +\left[J_{3|2}\right]^{+}\left(\dot x_3 - J_3 \dot q_2\right), &{\rm 3rd~Priority}\\ \end{array} \right. \tag{11} ⎩⎨⎧​q˙​1​=J1+​x˙1​,q˙​2​=q˙​1​+[J2∣1​]+(x˙2​−J2​q˙​1​),q˙​3​=q˙​2​+[J3∣2​]+(x˙3​−J3​q˙​2​),​1st Priority2nd Priority3rd Priority​(11)
其中 q ˙ 1 \dot q_1 q˙​1​和 q ˙ 2 \dot q_2 q˙​2​只用于中间计算环节, 最终输出是合成的关节空间速度命令 q ˙ 3 \dot q_3 q˙​3​​.

如果存在第四级、第五级甚至更多任务情况下，以此类推.

四. 源代码比对

源代码参见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;
}

五. 总结

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

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

(终于写完了