我们经常使用微分运动学来计算机器人的逆运动学,对于单个任务的机械臂的逆运动学使用的是梯度投影法:
冗余机械臂求解逆运动学解——梯度投影法
但是对于多任务的逆运动学在一般的机器人学里面很少有提及,最近看到了相关的论述,于是做一下笔记整理一下。
前置说明
设共有 n t n_t nt个任务,对于每个任务 t a s k i task_i taski我们可以给出其雅克比矩阵和期望的速度的表示:
t a s k i : = { J i , w i ∗ } . \begin{align} {task}_{i}: & = \left\{\mathbf{J}_{i}, \mathbf{w}_{i}^{*}\right\} . \end{align} taski:={Ji,wi∗}.
我们怎么计算这里的逆运动学呢?即根据一系列的 w i \mathbf{w}_i wi来计算 q ˙ \dot{\mathbf{q}} q˙呢?我们可以分为无优先级和有优先级的两种情况来考虑。
1. 多任务(优先级同等)
优先级是同等的情况下,我们可以建立关节空间( q ˙ \dot{\mathbf{q}} q˙)和操作空间( w ‾ \overline{\mathbf{w}} w)之间的映射
[
J
1
⋮
J
n
t
]
⏟
J
‾
q
˙
=
(
w
1
⋮
w
n
t
)
⏟
w
‾
\begin{align} \underbrace{\left[\begin{array}{c} \mathbf{J}_{1} \\ \vdots \\ \mathbf{J}_{n_{t}} \end{array}\right]}_{\overline{\mathbf{J}}}\dot{\mathbf{q}} & = \underbrace{\left(\begin{array}{c} \mathbf{w}_{1}^{} \\ \vdots \\ \mathbf{w}_{n_{t}}^{} \end{array}\right)}_{\overline{\mathbf{w}}} \end{align}
J
J1⋮Jnt
q˙=w
w1⋮wnt
直接对上面的方程求伪逆就可以了。
q ˙ = [ J 1 ⋮ J n t ] + ⏟ J ‾ ( w 1 ∗ ⋮ w n t ∗ ) ⏟ w ‾ \begin{align} \dot{\mathbf{q}} & = \underbrace{\left[\begin{array}{c} \mathbf{J}_{1} \\ \vdots \\ \mathbf{J}_{n_{t}} \end{array}\right]^{+}}_{\overline{\mathbf{J}}} \underbrace{\left(\begin{array}{c} \mathbf{w}_{1}^{*} \\ \vdots \\ \mathbf{w}_{n_{t}}^{*} \end{array}\right)}_{\overline{\mathbf{w}}} \end{align} q˙=J J1⋮Jnt +w w1∗⋮wnt∗
一种排优先级的方式是引入一个权重矩阵:
J ‾ + W = ( J ‾ T W J ‾ ) − 1 J ‾ T W \begin{align} \overline{\mathbf{J}}^{+W} & = \left(\overline{\mathbf{J}}^{T} \mathbf{W} \overline{\mathbf{J}}\right)^{-1} \overline{\mathbf{J}}^{T} \mathbf{W} \end{align} J+W=(JTWJ)−1JTW
其中:
W = diag ( w 1 , … , w m ) \mathbf{W}=\operatorname{diag}\left(w_{1}, \ldots, w_{m}\right) W=diag(w1,…,wm)
这等价于计算下面表达式的最小值
∥ W 1 / 2 ( w ‾ − J ‾ q ˙ ) ∥ 2 \left\|\mathbf{W}^{1 / 2}(\overline{\mathbf{w}}-\overline{\mathbf{J}} \dot{\mathbf{q}})\right\|_{2} W1/2(w−Jq˙) 2
对于没有优先级的情况我们可以看到上面的对角矩阵 W \mathbf{W} W的对角元都是1。
2. 多任务(存在优先级)
优先级的意思就是,我们优先保证某个任务的实现,其他任务的实现度可以相对较差。我们不妨从两个任务的情况来看。
2.1 简单案例引入:两任务情况
关于第一个任务我们有:
q ˙ = J 1 † w 1 ∗ + N 1 q ˙ 0 \begin{align} \dot{\mathbf{q}} & = \mathbf{J}_{1}^{\dagger } \mathbf{w}_{1}^{*}+\mathbf{N}_{1} \dot{\mathbf{q}}_{0} \end{align} q˙=J1†w1∗+N1q˙0
对于第二个任务我们代入(5)式有:
w 2 = J 2 q ˙ = J 2 ( J 1 † w 1 ∗ + N 1 q ˙ 0 ) \begin{align} \mathbf{w}_{2} = \mathbf{J}_{2} \dot{\mathbf{q}} = \mathbf{J}_{2}\left(\mathbf{J}_{1}^{\dagger } \mathbf{w}_{1}^{*}+\mathbf{N}_{1} \dot{\mathbf{q}}_{0}\right) \end{align} w2=J2q˙=J2(J1†w1∗+N1q˙0)
这样我们可以解出 q ˙ 0 \dot{\mathbf{q}}_{0} q˙0
q ˙ 0 = ( J 2 N 1 ) † ( w 2 ∗ − J 2 J 1 + w 1 ∗ ) \begin{align} \dot{\mathbf{q}}_{0} & = \left(\mathbf{J}_{2} \mathbf{N}_{1}\right)^{\dagger }\left(\mathbf{w}_{2}^{*}-\mathbf{J}_{2} \mathbf{J}_{1}^{+} \mathbf{w}_{1}^{*}\right) \end{align} q˙0=(J2N1)†(w2∗−J2J1+w1∗)
于是我们带回(5)式
q ˙ = J 1 † w 1 ∗ + N 1 ( J 2 N 1 ) † ( w 2 ∗ − J 2 J 1 + w 1 ∗ ) \begin{align} \dot{\mathbf{q}} & = \mathbf{J}_{1}^{\dagger } \mathbf{w}_{1}^{*}+\mathbf{N}_{1}\left(\mathbf{J}_{2} \mathbf{N}_{1}\right)^{\dagger }\left(\mathbf{w}_{2}^{*}-\mathbf{J}_{2} \mathbf{J}_{1}^{+} \mathbf{w}_{1}^{*}\right) \end{align} q˙=J1†w1∗+N1(J2N1)†(w2∗−J2J1+w1∗)
这样我们就用一个表达式把$ q ˙ \dot{\mathbf{q}} q˙和第一个任务的期望的速度 w 1 \mathbf{w}_1 w1和第二个任务的期望的速度 w 2 \mathbf{w}_2 w2联系了起来。只用一个式子就进行了表示。
那么如果我们有更多的任务我们应该怎么表示呢?我们可以定义一个变换:
J ‾ i = [ J 1 ⋮ J i − 1 ] \begin{align} \overline{\mathbf{J}}_{i} & = \left[\begin{array}{ccc} \mathbf{J}_{1} \\ \vdots\\ \mathbf{J}_{i-1} \end{array}\right] \end{align} Ji= J1⋮Ji−1
而 N ‾ i \overline{\mathbf{N}}_i Ni是 J ‾ i \overline{\mathbf{J}}_{i} Ji对应的零空间,零空间的含义即:
J ‾ i N ‾ i = 0 \begin{align} \overline{\mathbf{J}}_{i}\overline{\mathbf{N}}_i & = \mathbf{0} \end{align} JiNi=0
对于 J ‾ 1 \overline{\mathbf{J}}_{1} J1为单位矩阵 E \mathbf{E} E。那么我们可以把式(8)写作
q ˙ = N ‾ 1 q ˙ 1 + N ‾ 2 q ˙ 2 \begin{align} \dot{\mathbf{q}} & =\overline{\mathbf{N}}_1\dot{\mathbf{q}}_1+\overline{\mathbf{N}}_2\dot{\mathbf{q}}_2 \end{align} q˙=N1q˙1+N2q˙2
其中
N ‾ 1 = E q ˙ 1 = J 1 † w 1 ∗ = ( J 1 N ‾ 1 ) † ( w 1 ∗ ) N ‾ 2 = N 1 q ˙ 2 = ( J 2 N 1 ) † ( w 2 ∗ − J 2 J 1 + w 1 ∗ ) = ( J 2 N ‾ 2 ) † ( w 2 ∗ − J 2 N ‾ 1 q ˙ 1 ) \begin{aligned}\overline{\mathbf{N}}_1&=\mathbf{E}\\ \dot{\mathbf{q}}_1&=\mathbf{J}^{\dagger }_1\mathbf{w}_1^*\\ &=\left(\mathbf{J}_{1} \overline{\mathbf{N}}_{1}\right)^{\dagger }\left(\mathbf{w}_{1}^{*}\right)\\ \overline{\mathbf{N}}_2&=\mathbf{N}_1\\ \dot{\mathbf{q}}_2&=\left(\mathbf{J}_{2} \mathbf{N}_{1}\right)^{\dagger }\left(\mathbf{w}_{2}^{*}-\mathbf{J}_{2} \mathbf{J}_{1}^{+} \mathbf{w}_{1}^{*}\right)\\&=\left(\mathbf{J}_{2} \overline{\mathbf{N}}_{2}\right)^{\dagger }\left(\mathbf{w}_{2}^{*}-\mathbf{J}_{2} \overline{\mathbf{N}}_1\dot{\mathbf{q}}_1\right)\end{aligned} N1q˙1N2q˙2=E=J1†w1∗=(J1N1)†(w1∗)=N1=(J2N1)†(w2∗−J2J1+w1∗)=(J2N2)†(w2∗−J2N1q˙1)
2.2 多任务情况
我们可以发现规律:
单个任务的时候计算逆运动学采用梯度投影法,而增加一个任务 t a s k i task_i taski可以用前面的任务来生成新的 N ‾ i \overline{\mathbf{N}}_i Ni和 q ‾ i \overline{\mathbf{q}}_i qi。类似于递推的原理,我们要根据前面的几个优先的任务 t a s k 1 task_1 task1- t a s k i − 1 task_{i-1} taski−1得到的来推导出接下来的引入 t a s k i task_i taski,如果前面的 1 1 1 ~ i − 1 i-1 i−1的任务我们可以得到它们表示的 q ˙ \dot{\mathbf{q}} q˙
q ˙ = ∑ k = 1 i − 1 N ‾ k q ˙ k \begin{align} \dot{\mathbf{q}} = \sum_{k = 1}^{i-1}\overline{\mathbf{N}}_k\dot{\mathbf{q}}_k \end{align} q˙=k=1∑i−1Nkq˙k
我们引入一个新任务进行扩展:
q ˙ = ∑ k = 1 i − 1 N ‾ k q k ˙ + N ‾ i q ˙ i \begin{align} \dot{\mathbf{q}}=\sum_{k=1}^{i-1}\overline{\mathbf{N}}_k\dot{\mathbf{q}_k}+\overline{\mathbf{N}}_i\dot{\mathbf{q}}_i \end{align} q˙=k=1∑i−1Nkqk˙+Niq˙i
我们根据第 i i i个任务的微分运动并且代入(11)式有:
w i = J i q ˙ = J i ( ∑ k = 1 i − 1 N ‾ k q k ˙ + N ‾ i q ˙ i ) \mathbf{w}_i=\mathbf{J}_i\dot{\mathbf{q}}=\mathbf{J}_i\left(\sum_{k=1}^{i-1}\overline{\mathbf{N}}_k\dot{\mathbf{q}_k}+\overline{\mathbf{N}}_i\dot{\mathbf{q}}_i\right) wi=Jiq˙=Ji(k=1∑i−1Nkqk˙+Niq˙i)
于是我们可以得到:
q ˙ i = ( J i N ‾ i ) + ( w i ∗ − J i ∑ k = 1 i − 1 N ‾ k q ˙ k ) \begin{align} \dot{\mathbf{q}}_{i} = \left(\mathbf{J}_{i} \overline{\mathbf{N}}_{i}\right)^{+}\left(\mathbf{w}_{i}^{*}-\mathbf{J}_{i} \sum_{k = 1}^{i-1} \overline{\mathbf{N}}_{k} \dot{\mathbf{q}}_{k}\right) \end{align} q˙i=(JiNi)+(wi∗−Jik=1∑i−1Nkq˙k)
这就是最后的总的公式:
3. 例子
3.1 单任务
如图平面的三自由度的机械臂,机械臂的长度都是1( l 0 , l 1 , l 2 , l 3 l_0,l_1,l_2,l_3 l0,l1,l2,l3),我们在 t t t时刻观测到关节角度为 q t = ( π / 6 , π / 3 , π / 3 ) T \mathbf{q}_{t}=(\pi / 6, \pi / 3, \pi / 3)^{T} qt=(π/6,π/3,π/3)T,这时候的末端的速度为 0 r ˙ E , t ∗ = ( 1 , 1 ) T { }_{0} \dot{\mathbf{r}}_{E, t}^{*}=(1,1)^{T} 0r˙E,t∗=(1,1)T,求解这时候的各关节的角速度?
解答:
期望速度为:w E , t ∗ = 0 r ˙ E , t ∗ = ( 1 1 ) \mathbf{w}_{E, t}^{*}={ }_{0} \dot{\mathbf{r}}_{E, t}^{*}=\left(\begin{array}{l} 1 \\ 1 \end{array}\right) wE,t∗=0r˙E,t∗=(11)
对于 q t = ( π / 6 , π / 3 , π / 3 ) \mathbf{q}_{t}=(\pi / 6, \pi / 3, \pi / 3) qt=(π/6,π/3,π/3),我们可以计算它的雅克比矩阵,公式为:
J e A P ( q ) = [ l 1 c 1 + l 2 c 12 + l 3 c 123 l 2 c 12 + l 3 c 123 l 3 c 123 − l 1 s 1 − l 2 s 12 − l 3 s 123 − l 2 s 12 − l 3 s 123 − l 3 s 123 ] 其中 c 123 = cos ( q 1 + q 2 + q 3 ) s 123 = sin ( q 1 + q 2 + q 3 ) \begin{aligned} \mathbf{J}_{e A P}(\mathbf{q}) & =\left[\begin{array}{ccc} l_{1} c_{1}+l_{2} c_{12}+l_{3} c_{123} & l_{2} c_{12}+l_{3} c_{123} & l_{3} c_{123} \\ -l_{1} s_{1}-l_{2} s_{12}-l_{3} s_{123} & -l_{2} s_{12}-l_{3} s_{123} & -l_{3} s_{123} \end{array}\right] \end{aligned}\\ \text{其中 }c_{123}=\cos \left(q_{1}+q_{2}+q_{3}\right) \text { } s_{123}=\sin \left(q_{1}+q_{2}+q_{3}\right) JeAP(q)=[l1c1+l2c12+l3c123−l1s1−l2s12−l3s123l2c12+l3c123−l2s12−l3s123l3c123−l3s123]其中 c123=cos(q1+q2+q3) s123=sin(q1+q2+q3)
于是我们有:
J E , t = J E ( q t ) = 1 2 [ 0 − 3 − 3 − 4 − 3 − 1 ] \mathbf{J}_{E, t}=\mathbf{J}_{E}\left(\mathbf{q}_{t}\right)=\frac{1}{2}\left[\begin{array}{ccc} 0 & -\sqrt{3} & -\sqrt{3} \\ -4 & -3 & -1 \end{array}\right] JE,t=JE(qt)=21[0−4−3−3−3−1]
通过计算伪逆的方法我们可以得到:
q ˙ t single = J E , t + w E , t ∗ = ( 0.069 − 0.560 − 0.595 ) \dot{\mathbf{q}}_{t}^{\text {single }}=\mathbf{J}_{E, t}^{+} \mathbf{w}_{E, t}^{*}=\left(\begin{array}{c} 0.069 \\ -0.560 \\ -0.595 \end{array}\right) q˙tsingle =JE,t+wE,t∗= 0.069−0.560−0.595
我们可以把这个结果带回去我们发现是符合末端的速度的要求的。
w E , t = J E , t q ˙ t single = ( 1 1 ) = w E , t ∗ \mathbf{w}_{E, t}=\mathbf{J}_{E, t} \dot{\mathbf{q}}_{t}^{\text {single }}=\left(\begin{array}{l} 1 \\ 1 \end{array}\right)=\mathbf{w}_{E, t}^{*} wE,t=JE,tq˙tsingle =(11)=wE,t∗
3.2 多任务(优先级同等)
除了前面定义的末端速度的任务,我们再额外定义一个任务:我们要求前面两个关节的速度为0:
w j = ( 0 0 ) , J j = [ 1 0 0 0 1 0 ] \mathbf{w}_{j}=\left(\begin{array}{l} 0 \\ 0 \end{array}\right), \quad \mathbf{J}_{j}=\left[\begin{array}{lll} 1 & 0 & 0 \\ 0 & 1 & 0 \end{array}\right] wj=(00),Jj=[100100]
在添加这个条件的情况下,我们再计算一下关节空间的角速度。
解答:
我们根据前面的多任务逆运动学情况有:q ˙ = [ J E J j ] † ( w E ∗ w j ∗ ) \dot{\mathbf{q}}=\left[\begin{array}{c} \mathbf{J}_{E} \\ \mathbf{J}_{j} \end{array}\right]^{\dagger}\left(\begin{array}{c} \mathbf{w}_{E}^{*} \\ \mathbf{w}_{j}^{*} \end{array}\right) q˙=[JEJj]†(wE∗wj∗)
我们同样可以直接计算组合后的雅克比矩阵的伪逆:
q ˙ t stack = [ J E , t J j , t ] † ( w E ∗ w j ∗ ) = ( − 0.133 − 0.067 − 1.132 ) \begin{aligned} \dot{\mathbf{q}}_{t}^{\text {stack }} & =\left[\begin{array}{c} \mathbf{J}_{E, t} \\ \mathbf{J}_{j, t} \end{array}\right]^{\dagger}\left(\begin{array}{l} \mathbf{w}_{E}^{*} \\ \mathbf{w}_{j}^{*} \end{array}\right) \\ & =\left(\begin{array}{c} -0.133 \\ -0.067 \\ -1.132 \end{array}\right) \end{aligned} q˙tstack =[JE,tJj,t]†(wE∗wj∗)= −0.133−0.067−1.132
于是我们会注意到:
[ J E , t J j , t ] q ˙ t stack = ( 1.039 0.933 − 0.133 − 0.067 ) ≠ ( w E ∗ w Δ q ˙ ∗ ) \left[\begin{array}{c} \mathbf{J}_{E, t} \\ \mathbf{J}_{j, t} \end{array}\right] \dot{\mathbf{q}}_{t}^{\text {stack }}=\left(\begin{array}{c} 1.039 \\ 0.933 \\ -0.133 \\ -0.067 \end{array}\right) \neq\left(\begin{array}{c} \mathbf{w}_{E}^{*} \\ \mathbf{w}_{\Delta \dot{q}}^{*} \end{array}\right) [JE,tJj,t]q˙tstack = 1.0390.933−0.133−0.067 =(wE∗wΔq˙∗)
我们可以看到:
∥ w E , t ∗ − J E , t q ˙ t stack ∥ 2 = 0.0059 ∥ w j , t ∗ − J j , t q ˙ t stack ∥ 2 = 0.0223. \begin{aligned} \left\|\mathbf{w}_{E, t}^{*}-\mathbf{J}_{E, t} \dot{\mathbf{q}}_{t}^{\text {stack }}\right\|^{2} & =0.0059 \\ \left\|\mathbf{w}_{j, t}^{*}-\mathbf{J}_{j, t} \dot{\mathbf{q}}_{t}^{\text {stack }}\right\|^{2} & =0.0223 . \end{aligned} wE,t∗−JE,tq˙tstack 2 wj,t∗−Jj,tq˙tstack 2=0.0059=0.0223.
我们可以和前面的单任务进行对比:
∥ w E , t ∗ − J E , t q ˙ t single ∥ 2 = 0 ∥ w j , t ∗ − J j , t q ˙ t single ∥ 2 = 0.319 \begin{aligned} \left\|\mathbf{w}_{E, t}^{*}-\mathbf{J}_{E, t} \dot{\mathbf{q}}_{t}^{\text {single }}\right\|^{2} & =0 \\ \left\|\mathbf{w}_{j, t}^{*}-\mathbf{J}_{j, t} \dot{\mathbf{q}}_{t}^{\text {single }}\right\|^{2} & =0.319 \end{aligned} wE,t∗−JE,tq˙tsingle 2 wj,t∗−Jj,tq˙tsingle 2=0=0.319
我们可以看到为了能实现第二个任务,我们牺牲了末端的跟踪性能。
3.3 多任务(存在优先级)
如果我们向保证末端的速度任务臂保证前面两个关节速度为0有更高的优先级,应该怎么做呢?
解答:
我们使用前面的公式:q ˙ t prio = J 1 + w 1 ∗ + N 1 ( J 2 N 1 ) + ( w 2 ∗ − J 2 J 1 + w 1 ∗ ) \dot{\mathbf{q}}_{t}^{\text {prio }}=\mathbf{J}_{1}^{+} \mathbf{w}_{1}^{*}+\mathbf{N}_{1}\left(\mathbf{J}_{2} \mathbf{N}_{1}\right)^{+}\left(\mathbf{w}_{2}^{*}-\mathbf{J}_{2} \mathbf{J}_{1}^{+} \mathbf{w}_{1}^{*}\right) q˙tprio =J1+w1∗+N1(J2N1)+(w2∗−J2J1+w1∗)
这里:
J 1 = J E , t = 1 2 [ 0 − 3 − 3 − 4 − 3 − 1 ] , J 2 = J j , t = [ 1 0 0 0 1 0 ] , w 1 ∗ = w E , t ∗ = ( 1 1 ) w 2 = w j , t ∗ = ( 0 0 ) \mathbf{J}_{1}=\mathbf{J}_{E, t}=\frac{1}{2}\left[\begin{array}{ccc} 0 & -\sqrt{3} & -\sqrt{3} \\ -4 & -3 & -1 \end{array}\right],\\ \mathbf{J}_{2}=\mathbf{J}_{j, t}=\left[\begin{array}{lll} 1 & 0 & 0 \\ 0 & 1 & 0 \end{array}\right],\\ \mathbf{w}_{1}^{*}=\mathbf{w}_{E, t}^{*}=\left(\begin{array}{l} 1 \\ 1 \end{array}\right)\\ \mathbf{w}_{2}=\mathbf{w}_{j, t}^{*}=\left(\begin{array}{l} 0 \\ 0 \end{array}\right) J1=JE,t=21[0−4−3−3−3−1],J2=Jj,t=[100100],w1∗=wE,t∗=(11)w2=wj,t∗=(00)
我们可以计算一个可能的零空间的矩阵: N 1 = I − J 1 + J 1 = 1 9 [ 1 − 2 2 − 2 4 − 4 2 − 4 4 ] \mathbf{N}_{1}=\mathbb{I}-\mathbf{J}_{1}^{+} \mathbf{J}_{1}=\frac{1}{9}\left[\begin{array}{ccc} 1 & -2 & 2 \\ -2 & 4 & -4 \\ 2 & -4 & 4 \end{array}\right] N1=I−J1+J1=91 1−22−24−42−44
于是我们可以计算得到:
q ˙ t prio = ( − 0.169 − 0.085 − 1.070 ) \dot{\mathbf{q}}_{t}^{\text {prio }}=\left(\begin{array}{c} -0.169 \\ -0.085 \\ -1.070 \end{array}\right) q˙tprio = −0.169−0.085−1.070
现在我们再来看一下误差:
∥ w E , t ∗ − J E , t q ˙ t prio ∥ 2 = 0 ∥ w j , t ∗ − J j , t q ˙ t prio ∥ 2 = 0.036 \begin{aligned} \left\|\mathbf{w}_{E, t}^{*}-\mathbf{J}_{E, t} \dot{\mathbf{q}}_{t}^{\text {prio }}\right\|^{2} & =0 \\ \left\|\mathbf{w}_{j, t}^{*}-\mathbf{J}_{j, t} \dot{\mathbf{q}}_{t}^{\text {prio }}\right\|^{2} & =0.036 \end{aligned} wE,t∗−JE,tq˙tprio 2 wj,t∗−Jj,tq˙tprio 2=0=0.036
与前面的优先级同等的解决方案相比,存在优先级的解总误差更大,但末端执行器任务的位置是准确的。与单个任务优化相比,可以看出总误差减少了。事实上,这是在不改变末端执行器跟踪性能的情况下降低关节速度,在最佳意义上使用了单维度零空间。
参考:文章来源:https://www.toymoban.com/news/detail-576217.html
[1] Robot Dynamics Lecture Notes 43-47文章来源地址https://www.toymoban.com/news/detail-576217.html
到了这里,关于机械臂多任务逆运动学(优先级同等和存在优先级)的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!