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
今天的文章机械臂的雅可比矩阵_机器人雅可比矩阵的意义分享到此就结束了,感谢您的阅读。
版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 举报,一经查实,本站将立刻删除。
如需转载请保留出处:https://bianchenghao.cn/88741.html