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