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

机械臂的雅克比矩阵推导相关推荐

激光SLAM后端优化——雅克比矩阵推导
激光SLAM后端优化--雅克比矩阵推导 Jacobi Matrix Jacobi Matrix In the EKF system, the maintained state quantities i ...
六轴机械臂运动学算法原理及其推导过程
网站上关于六轴机械臂piper算法的讲解有很多,但其腕点姿态的推到较为模糊,故此写一篇关于六轴机械臂piper算法的推导讲解,供有缘人参考,如果您觉得有用,可以点个赞,吾将不胜感激,若是推导过程存在错 ...
关于机械臂逆运动学 jacobian矩阵
机器人逆运动学机器人的逆解问题比较复杂,为了说明问题,下面先以2自由度的机器人为例. 如图所示,已知机器人末端的坐标值(x,y) ,试利用x,y 表示根据图中的几何关系可知: 联立求 ...
基于Mathematica的机器人仿真环境（机械臂篇）
目的本文手把手教你在 Mathematica 科学计算软件中搭建机器人的仿真环境,具体包括以下内容: 1 导入机械臂的三维模型 2 正\逆运动学仿真 3 碰撞检测 4 轨迹规划 5 正\逆动力学仿真 ...
基于 Mathematica 的机器人仿真环境（机械臂篇）
目的本文手把手教你在 Mathematica 软件中搭建机器人的仿真环境,具体包括以下内容(所使用的版本是 Mathematica 11.1,更早的版本可能缺少某些函数,所以请使用最新版.robin ...
[转]基于Mathematica的机器人仿真环境（机械臂篇）
目的本文手把手教你在 Mathematica 科学计算软件中搭建机器人的仿真环境,具体包括以下内容: 1 导入机械臂的三维模型 2 正\逆运动学仿真 3 碰撞检测 4 轨迹规划 5 正\逆动力学仿真 ...
机械臂正逆运动学-----数值解
机械臂正逆运动学-----数值解建立DH坐标系求正运动学单关节齐次传递矩阵正运动学:返回齐次矩阵正运动学:返回欧拉角向量求雅可比矩阵求机械臂逆运动学合成通用运动学类机械臂的运动学包括 ...
【关于四足机器人那些事】雅克比矩阵
一.引入假设有6个函数,每个函数有6个独立的变量,即: y1=f1(x1,x2,x3,x4,x5,x6)y2=f2(x1,x2,x3,x4,x5,x6)⋮y6=f6(x1,x2,x3,x4,x5,x ...
《机器人动力学与控制》第五章——速度运动学之机械臂的雅各比矩阵 5.0 导言
文章目录 <机器人动力学与控制>第五章--速度运动学之机械臂的雅各比矩阵 5.0 导言参考文献 <机器人动力学与控制>第五章--速度运动学之机械臂的雅各比矩阵 5.0 导言 ...

机械臂的雅克比矩阵推导

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

1.1 平移副情况

1.2 旋转副情况

2. 雅克比矩阵计算

2.1 线速度雅克比分量

2.1.1 平移副情况

2.1.2 旋转副情况

2.2 角速度雅克比分量

2.2.1 平移副情况

2.2.2 旋转副情况

2.3 雅可比矩阵综合

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

机械臂的雅克比矩阵推导相关推荐

最新文章

热门文章