机械臂的雅克比矩阵推导

1. 线速度和角速度的递推通式推导

机械臂的雅克比矩阵推导_第1张图片
p i = p i − 1 + R i − 1 r i − 1 , i i − 1 \mathbf{p}_{i}=\mathbf{p}_{i-1}+\mathbf{R}_{i-1} \mathbf{r}_{i-1, i}^{i-1} pi=pi1+Ri1ri1,ii1

p i − 1 \mathbf{p}_{i-1} pi1 { i − 1 } \{i-1\} {i1}坐标系的原点的向量, p i \mathbf{p}_{i} pi { i } \{i\} {i}坐标系的原点的向量,对于向量如果没有上标默认在 { 0 } \{0\} {0}坐标系下表示(往后向量不说在哪个坐标系下表示默认是在 { 0 } \{0\} {0}坐标系下表示) r i − 1 , i i − 1 \mathbf{r}_{i-1, i}^{i-1} ri1,ii1 { i } \{i\} {i}坐标系的原点相对于 { i − 1 } \{i-1\} {i1}坐标系的原点构成的位置向量,这个向量在 { i − 1 } \{i-1\} {i1}坐标系下表示。对上式求导:

p ˙ i = p ˙ i − 1 + R i − 1 r ˙ i − 1 , i i − 1 + ω i − 1 × R i − 1 r i − 1 , i i − 1 = p ˙ i − 1 + v i − 1 , i + ω i − 1 × r i − 1 , i \dot{\mathbf{p}}_{i}=\dot{\mathbf{p}}_{i-1}+\mathbf{R}_{i-1} \dot{\mathbf{r}}_{i-1, i}^{i-1}+\mathbf{\omega}_{i-1} \times \mathbf{R}_{i-1} \mathbf{r}_{i-1, i}^{i-1}=\dot{\mathbf{p}}_{i-1}+\mathbf{v}_{i-1, i}+\mathbf{\omega}_{i-1} \times \mathbf{r}_{i-1, i} p˙i=p˙i1+Ri1r˙i1,ii1+ωi1×Ri1ri1,ii1=p˙i1+vi1,i+ωi1×ri1,i

上式中, v i − 1 , i \mathbf{v}_{i-1, i} vi1,i { i } \{i\} {i}坐标系的原点相对于 { i − 1 } \{i-1\} {i1}坐标系的原点构成的线速度向量。

对于旋转矩阵,我们有:

R i = R i − 1 R i i − 1 \mathbf{R}_{i}=\mathbf{R}_{i-1} \mathbf{R}_{i}^{i-1} Ri=Ri1Rii1

对旋转矩阵求导我们有: R ˙ ( t ) = S ( t ) R ( t ) \dot{\mathbf{R}}(t)=\mathbf{S}(t) \mathbf{R}(t) R˙(t)=S(t)R(t),其中 S ( t ) \mathbf{S}(t) S(t)是一个反对称矩阵,带入上式我们有:

S ( ω i ) R i = S ( ω i − 1 ) R i + R i − 1 S ( ω i − 1 , i i − 1 ) R i i − 1 \mathbf{S}(\boldsymbol{\omega}_{i}) \mathbf{R}_{i}=\mathbf{S}(\boldsymbol{\omega}_{i-1}) \mathbf{R}_{i}+\mathbf{R}_{i-1} \mathbf{S}(\boldsymbol{\omega}_{i-1, i}^{i-1}) \mathbf{R}_{i}^{i-1} S(ωi)Ri=S(ωi1)Ri+Ri1S(ωi1,ii1)Rii1

上式中, ω i − 1 , i i − 1 \omega_{i-1, i}^{i-1} ωi1,ii1 { i } \{i\} {i}坐标系的原点相对于 { i − 1 } \{i-1\} {i1}坐标系的原点构成的角速度向量,这个向量在 { i − 1 } \{i-1\} {i1}坐标系下表示。

由于旋转矩阵是正交矩阵:

R i − 1 S ( ω i − 1 , i i − 1 ) R i i − 1 = R i − 1 S ( ω i − 1 , i i − 1 ) R i − 1 T R i − 1 R i i − 1 \mathbf{R}_{i-1} \mathbf{S}\left(\boldsymbol{\omega}_{i-1, i}^{i-1}\right) \mathbf{R}_{i}^{i-1}=\mathbf{R}_{i-1} \mathbf{S}(\boldsymbol{\omega}_{i-1, i}^{i-1}) \mathbf{R}_{i-1}^{T} \mathbf{R}_{i-1} \mathbf{R}_{i}^{i-1} Ri1S(ωi1,ii1)Rii1=Ri1S(ωi1,ii1)Ri1TRi1Rii1

这里使用公式 R S ( ω ) R T = S ( R ω ) \mathbf{R} \mathbf{S}(\mathbf{\omega}) \mathbf{R}^{T}=\mathbf{S}(\mathbf{R} \mathbf{\omega}) RS(ω)RT=S(Rω),我们有:

R i − 1 S ( ω i − 1 , i i − 1 ) R i i − 1 = S ( R i − 1 ω i − 1 , i i − 1 ) R i \mathbf{R}_{i-1} \mathbf{S}(\boldsymbol{\omega}_{i-1, i}^{i-1}) \mathbf{R}_{i}^{i-1}=\mathbf{S}(\mathbf{R}_{i-1} \boldsymbol{\omega}_{i-1, i}^{i-1}) \mathbf{R}_{i} Ri1S(ωi1,ii1)Rii1=S(Ri1ωi1,ii1)Ri

于是前面的式子变成:

S ( ω i ) R i = S ( ω i − 1 ) R i + S ( R i − 1 ω i − 1 , i i − 1 ) R i \mathbf{S}(\boldsymbol{\omega}_{i}) \mathbf{R}_{i}=\mathbf{S}(\boldsymbol{\omega}_{i-1}) \mathbf{R}_{i}+\mathbf{S}(\mathbf{R}_{i-1} \boldsymbol{\omega}_{i-1, i}^{i-1}) \mathbf{R}_{i} S(ωi)Ri=S(ωi1)Ri+S(Ri1ωi1,ii1)Ri

于是我们有:

ω i = ω i − 1 + R i − 1 ω i − 1 , i i − 1 = ω i − 1 + ω i − 1 , i = \boldsymbol{\omega}_{i}=\boldsymbol{\omega}_{i-1}+\mathbf{R}_{i-1} \boldsymbol{\omega}_{i-1, i}^{i-1}=\boldsymbol{\omega}_{i-1}+\boldsymbol{\omega}_{i-1, i}= ωi=ωi1+Ri1ωi1,ii1=ωi1+ωi1,i=

总结我们得到的线速度和角速度的递推通式为:

机械臂的雅克比矩阵推导_第2张图片

1.1 平移副情况

对于平移副,我们有角速度:

ω i − 1 , i = 0 \boldsymbol{\omega}_{i-1, i}=\mathbf{0} ωi1,i=0

线速度:

v i − 1 , i = d ˙ i z i − 1 \mathbf{v}_{i-1, i}=\dot{d}_{i} \mathbf{z}_{i-1} vi1,i=d˙izi1

z i − 1 \mathbf{z}_{i-1} zi1 { i − 1 } \{i-1\} {i1}坐标系沿着 z \mathbf{z} z轴的单位向量。

带入前面的公式我们有:

ω i = ω i − 1 p ˙ i = p ˙ i − 1 + d ˙ i z i − 1 + ω i × r i − 1 , i \begin{aligned} \boldsymbol{\omega}_{i} & =\boldsymbol{\omega}_{i-1} \\ \dot{\mathbf{p}}_{i} & =\dot{\mathbf{p}}_{i-1}+\dot{d}_{i} \mathbf{z}_{i-1}+\boldsymbol{\omega}_{i} \times \mathbf{r}_{i-1, i} \end{aligned} ωip˙i=ωi1=p˙i1+d˙izi1+ωi×ri1,i

1.2 旋转副情况

对于旋转副,我们有角速度:

ω i − 1 , i = ϑ ˙ i z i − 1 \boldsymbol{\omega}_{i-1, i}=\dot{\vartheta}_{i} \mathbf{z}_{i-1} ωi1,i=ϑ˙izi1

v i − 1 , i = ω i − 1 , i × r i − 1 , i \boldsymbol{v}_{i-1, i}=\boldsymbol{\omega}_{i-1, i} \times \boldsymbol{r}_{i-1, i} vi1,i=ωi1,i×ri1,i

于是我们有:

ω i = ω i − 1 + ϑ ˙ i z i − 1 p ˙ i = p ˙ i − 1 + ω i × r i − 1 , i \begin{aligned} \boldsymbol{\omega}_{i} & =\boldsymbol{\omega}_{i-1}+\dot{\vartheta}_{i} \mathbf{z}_{i-1} \\ \dot{\mathbf{p}}_{i} & =\dot{\mathbf{p}}_{i-1}+\boldsymbol{\omega}_{i} \times \mathbf{r}_{i-1, i} \end{aligned} ωip˙i=ωi1+ϑ˙izi1=p˙i1+ωi×ri1,i

2. 雅克比矩阵计算

2.1 线速度雅克比分量

p ˙ e = ∑ i = 1 n ∂ p e ∂ q i q ˙ i = ∑ i = 1 n J P i q ˙ i \dot{\mathbf{p}}_{e}=\sum_{i=1}^{n} \frac{\partial \mathbf{p}_{e}}{\partial q_{i}} \dot{q}_{i}=\sum_{i=1}^{n} \boldsymbol{J}_{P i} \dot{q}_{i} p˙e=i=1nqipeq˙i=i=1nJPiq˙i

上式可以看到,末端的线速度都是由每个关节的线速度贡献的,其中 q ˙ i \dot{q}_i q˙i是关节速度。

2.1.1 平移副情况

对于平移副我们有 q i = d i q_{i}=d_{i} qi=di

根据:

v i − 1 , i = d ˙ i z i − 1 \mathbf{v}_{i-1, i}=\dot{d}_{i} \mathbf{z}_{i-1} vi1,i=d˙izi1

q ˙ i J P i = d ˙ i z i − 1 \dot{q}_{i} \boldsymbol{J}_{P i}=\dot{d}_{i} \mathbf{z}_{i-1} q˙iJPi=d˙izi1

于是我们有:

J P i = z i − 1 \boldsymbol{J}_{P i}=\mathbf{z}_{i-1} JPi=zi1

2.1.2 旋转副情况

对于旋转副我们有 q i = θ i q_{i}=\theta_{i} qi=θi:
机械臂的雅克比矩阵推导_第3张图片
q ˙ i J P i = ω i − 1 , i × r i − 1 , e = ϑ ˙ i z i − 1 × ( p e − p i − 1 ) \dot{q}_{i} \boldsymbol{J}_{P i}=\boldsymbol{\omega}_{i-1, i} \times \mathbf{r}_{i-1, e}=\dot{\vartheta}_{i} \mathbf{z}_{i-1} \times\left(\mathbf{p}_{e}-\mathbf{p}_{i-1}\right) q˙iJPi=ωi1,i×ri1,e=ϑ˙izi1×(pepi1)

于是:

J P i = z i − 1 × ( p e − p i − 1 ) \boldsymbol{J}_{P i}=\mathbf{z}_{i-1} \times\left(\mathbf{p}_{e}-\mathbf{p}_{i-1}\right) JPi=zi1×(pepi1)

2.2 角速度雅克比分量

对于角速度我们有:

ω e = ω n = ∑ i = 1 n ω i − 1 , i = ∑ i = 1 n J O i q ˙ i \boldsymbol{\omega}_{e}=\boldsymbol{\omega}_{n}=\sum_{i=1}^{n} \boldsymbol{\omega}_{i-1, i}=\sum_{i=1}^{n} \boldsymbol{J}_{O i} \dot{q}_{i} ωe=ωn=i=1nωi1,i=i=1nJOiq˙i

2.2.1 平移副情况

对于平移副我们有 q i = d i q_{i}=d_{i} qi=di

q ˙ i J O i = 0 \dot{q}_{i} \boldsymbol{J}_{O i}=\mathbf{0} q˙iJOi=0

于是有:

ȷ O i = 0 \boldsymbol{\jmath}_{O i}=\mathbf{0} Oi=0

2.2.2 旋转副情况

对于旋转副我们有 q i = θ i q_{i}=\theta_{i} qi=θi:

q ˙ i J O i = ϑ ˙ i z i − 1 \dot{q}_{i} \boldsymbol{J}_{O i}=\dot{\vartheta}_{i} \mathbf z_{i-1} q˙iJOi=ϑ˙izi1

于是有:

J O i = z i − 1 \boldsymbol{J}_{O i}=\mathbf{z}_{i-1} JOi=zi1

2.3 雅可比矩阵综合

我们可以把线速度和角速度的雅克比矩阵的分量合成,于是我们有:

J = [ J P 1 J P n … J O 1 J O n ] \mathbf{J}=\left[\begin{array}{lll} \boldsymbol{J}_{P 1} & & \boldsymbol{J}_{P n} \\ & \ldots & \\ \boldsymbol{J}_{O 1} & & \boldsymbol{J}_{O n} \end{array}\right] J= JP1JO1JPnJOn

可以分成平移副和旋转副来讨论雅克比矩阵中的分量:

[ J P i J O i ] = { [ z i − 1 0 ]  平移副  [ z i − 1 × ( p e − p i − 1 ) z i − 1 ]  旋转副  \left[\begin{array}{l} \boldsymbol{J}_{P i} \\ \boldsymbol{J}_{O i} \end{array}\right]=\left\{\begin{array}{ll} {\left[\begin{array}{c} \boldsymbol{z}_{i-1} \\ \mathbf{0} \end{array}\right]} & \text { 平移副 } \\ {\left[\begin{array}{c} \boldsymbol{z}_{i-1} \times\left(\boldsymbol{p}_{e}-\boldsymbol{p}_{i-1}\right) \\ \boldsymbol{z}_{i-1} \end{array}\right]} & \text { 旋转副 } \end{array}\right. [JPiJOi]= [zi10][zi1×(pepi1)zi1] 平移副  旋转副 

关于上式的 z i − 1 \mathbf{z}_{i-1} zi1 p e \mathbf{p}_e pe p i − 1 \mathbf{p}_{i-1} pi1计算如下:

机械臂的雅克比矩阵推导_第4张图片

3. 不同坐标系下雅克比矩阵的转换

不同坐标系下线速度和加速度的转换如下:

[ p ˙ e u ω e u ] = [ R u 0 0 R u ] [ p ˙ e ω e ] , \left[\begin{array}{c} \dot{\mathbf{p}}_{e}^{u} \\ \boldsymbol{\omega}_{e}^{u} \end{array}\right]=\left[\begin{array}{cc} \mathbf{R}^{u} & \mathbf{0} \\ \mathbf{0} & \mathbf{R}^{u} \end{array}\right]\left[\begin{array}{c} \dot{\mathbf{p}}_{e} \\ \boldsymbol{\omega}_{e} \end{array}\right], [p˙euωeu]=[Ru00Ru][p˙eωe],

于是有:

[ p ˙ e u ω e u ] = [ R u 0 0 R u ] J q ˙ \left[\begin{array}{c} \dot{\mathbf{p}}_{e}^{u} \\ \boldsymbol{\omega}_{e}^{u} \end{array}\right]=\left[\begin{array}{cc} \mathbf{R}^{u} & \mathbf{0} \\ \mathbf{0} & \mathbf{R}^{u} \end{array}\right] \mathbf{J} \dot{\mathbf{q}} [p˙euωeu]=[Ru00Ru]Jq˙

则有:

J u = [ R u 0 0 R u ] J \mathbf{J}^{u}=\left[\begin{array}{cc} \mathbf{R}^{u} & \mathbf{0} \\ \mathbf{0} & \mathbf{R}^{u} \end{array}\right] \mathbf{J} Ju=[Ru00Ru]J

这里的 J u \mathbf{J}^{u} Ju就是在 { u } \{u\} {u}坐标系下表示的雅克比矩阵。

参考资料

[1]Siciliano B, Sciavicco L, Villani L, et al. . Robotics: Modelling, Planning and Control, 106-113

你可能感兴趣的:(机器人学,机器人学,雅克比矩阵,机械臂)