机械臂的雅克比矩阵推导

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=pi−1+Ri−1ri−1,ii−1

p i − 1 \mathbf{p}{i-1} pi−1是 { i − 1 } \{i-1\} {i−1}坐标系的原点的向量, 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} ri−1,ii−1是 { i } \{i\} {i}坐标系的原点相对于 { i − 1 } \{i-1\} {i−1}坐标系的原点构成的位置向量,这个向量在 { i − 1 } \{i-1\} {i−1}坐标系下表示。对上式求导:

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˙i−1+Ri−1r˙i−1,ii−1+ωi−1×Ri−1ri−1,ii−1=p˙i−1+vi−1,i+ωi−1×ri−1,i

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

对于旋转矩阵,我们有:

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

对旋转矩阵求导我们有: 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(ωi−1)Ri+Ri−1S(ωi−1,ii−1)Rii−1

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

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

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} Ri−1S(ωi−1,ii−1)Rii−1=Ri−1S(ωi−1,ii−1)Ri−1TRi−1Rii−1

这里使用公式 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} Ri−1S(ωi−1,ii−1)Rii−1=S(Ri−1ωi−1,ii−1)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(ωi−1)Ri+S(Ri−1ωi−1,ii−1)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=ωi−1+Ri−1ωi−1,ii−1=ωi−1+ωi−1,i=

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

1.1 平移副情况

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

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

线速度:

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

z i − 1 \mathbf{z}_{i-1} zi−1是 { i − 1 } \{i-1\} {i−1}坐标系沿着 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=ωi−1=p˙i−1+d˙izi−1+ωi×ri−1,i

1.2 旋转副情况

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

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

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} vi−1,i=ωi−1,i×ri−1,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=ωi−1+ϑ˙izi−1=p˙i−1+ωi×ri−1,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=1∑n∂qi∂peq˙i=i=1∑nJPiq˙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} vi−1,i=d˙izi−1

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˙izi−1

于是我们有:

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

2.1.2 旋转副情况

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

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=ωi−1,i×ri−1,e=ϑ˙izi−1×(pe−pi−1)

于是:

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=zi−1×(pe−pi−1)

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=1∑nωi−1,i=i=1∑nJOiq˙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=ϑ˙izi−1

于是有:

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

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= JP1JO1...JPnJOn

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

[ 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]=⎩ ⎨ ⎧[zi−10][zi−1×(pe−pi−1)zi−1] 平移副 旋转副

关于上式的 z i − 1 \mathbf{z}_{i-1} zi−1、 p e \mathbf{p}e pe、 p i − 1 \mathbf{p}{i-1} pi−1计算如下:

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

相关推荐
kuan_li_lyg1 个月前
MATLAB - 浮动基座机器人的逆运动学
开发语言·人工智能·matlab·机器人·自动驾驶·机械臂·逆运动学
kuan_li_lyg1 个月前
MATLAB - 机器人机械臂设计轨迹规划器
开发语言·人工智能·matlab·机器人·自动驾驶·机械臂·轨迹规划
kuan_li_lyg1 个月前
MATLAB - 机械臂手眼标定(眼在手内) - 估计安装在机器人上的移动相机的姿态
开发语言·人工智能·matlab·机器人·ros·机械臂·手眼标定
kuan_li_lyg1 个月前
MATLAB - 机械臂手眼标定(眼在手外) - 估算固定相机相对于机器人基座的姿态
开发语言·人工智能·matlab·机器人·ros·机械臂·手眼标定
yukoyu2 个月前
齐次变换矩阵的原理与应用
矩阵·机器人·机械臂
极客小张2 个月前
构建基于I2C与UART通信的智能嵌入式机械臂抓取系统,结合OpenCV技术进行高效物体识别与动作控制的综合解决方案(代码示例)
c++·人工智能·opencv·物联网·计算机视觉·机器人·机械臂
大象机器人3 个月前
大象机器人水星MercuryX1轮式人形机器人基于物体标记建模的键盘点按操作!
人工智能·python·机器人·开源·机械臂
不做签到员3 个月前
虚拟机Ubuntu20.04 利用串口调试机械臂
ubuntu·串口·机械臂·pyserial
大象机器人3 个月前
使用水星Mecury人形机器人搭建VR遥操作控制平台!
人工智能·python·yolo·机器人·机械臂·vr
大象机器人4 个月前
交互式乘法 myCobot 让学习充满乐趣!
人工智能·python·学习·yolo·机械臂·具身智能