Modern Robotics指数坐标推导雅可比（Jacobian）

红太狼 2022-12-15 15:26 213阅读 0赞

# Modern Robotics指数坐标推导雅可比（Jacobian） #

![在这里插入图片描述][20201018101004636.gif_pic_center]

## 空间雅可比(Space Jacobian) ##

假设一个开链机构的正运动学以指数积形式写成  
 T ( θ ) = e \[ S 1 \] θ 1 … e \[ S n \] θ n M . T(\\theta)=e^\{\[\\mathcal S\_1\]\\theta\_1\}\\dots e^\{\[\\mathcal S\_n\]\\theta\_n\}M. T(θ)=e\[S1\]θ1…e\[Sn\]θnM.  
空间雅可比 J s ( θ ) ∈ R 6 × n J\_s(\\theta)\\in \\mathbb R^\{6\\times n\} Js(θ)∈R6×n关联了关节速度向量 θ ˙ ∈ R n \\dot \\theta \\in\\mathbb R^n θ˙∈Rn和空间旋量 V s \\mathcal V\_s Vs，即 V s = J s ( θ ) θ ˙ \\mathcal V\_s=J\_s(\\theta)\\dot \\theta Vs=Js(θ)θ˙。 J s ( θ ) J\_s(\\theta) Js(θ)第 i i i列由下式确定  
 J s i ( θ ) = A d e \[ S 1 \] θ 1 . . . e \[ S i − 1 \] θ i − 1 ( S i ) , J\_\{si\}(\\theta)=Ad\_\{e^\{\[\\mathcal S\_1\]\\theta\_1\}...e^\{\[\\mathcal S\_\{i-1\}\]\\theta\_\{i-1\}\}\}(\\mathcal S\_i), Jsi(θ)=Ade\[S1\]θ1...e\[Si−1\]θi−1(Si),  
对于 i = 2 , … , n , i=2, \\dots, n, i=2,…,n,第1列 J s 1 = S 1 J\_\{s1\}=\\mathcal S\_1 Js1=S1。螺旋矢量 J s i J\_\{si\} Jsi的物理意义是在任意非零关节角度 θ \\theta θ下，第 i i i个关节在空间固定坐标系下的表达。

**推导**：

已知正运动学指数形式表达为  
 T ( θ ) = e \[ S 1 \] θ 1 … e \[ S n \] θ n M . (5.5) T(\\theta)=e^\{\[\\mathcal S\_1\]\\theta\_1\}\\dots e^\{\[\\mathcal S\_n\]\\theta\_n\}M.\\tag\{5.5\} T(θ)=e\[S1\]θ1…e\[Sn\]θnM.(5.5)  
空间旋量 V s \\mathcal V\_s Vs由齐次变换矩阵给出 \[ V s \] = T ˙ T − 1 \[\\mathcal V\_s\]=\\dot TT^\{-1\} \[Vs\]=T˙T−1, 其中  
 T ˙ = ( d d t e \[ S 1 \] θ 1 ) … e \[ S n \] θ n M + e \[ S 1 \] θ 1 ( d d t e \[ S 2 \] θ 2 ) … e \[ S n \] θ n M + … = \[ S 1 \] θ ˙ 1 e \[ S 1 \] θ 1 … e \[ S n \] θ n M + e \[ S 1 \] θ 1 \[ S 2 \] θ ˙ 2 e \[ S 2 \] θ 2 … e \[ S n \] θ n M + … \\dot T=(\\frac \{d\}\{dt\}e^\{\[\\mathcal S\_1\]\\theta\_1\})\\dots e^\{\[\\mathcal S\_n\]\\theta\_n\}M+e^\{\[\\mathcal S\_1\]\\theta\_1\}(\\frac\{d\}\{dt\}e^\{\[\\mathcal S\_2\]\\theta\_2\})\\dots e^\{\[\\mathcal S\_n\]\\theta\_n\}M+\\dots\\\\ =\[\\mathcal S\_1\]\\dot \\theta\_1e^\{\[\\mathcal S\_1\]\\theta\_1\}\\dots e^\{\[\\mathcal S\_n\]\\theta\_n\}M+e^\{\[\\mathcal S\_1\]\\theta\_1\}\[\\mathcal S\_2\]\\dot \\theta\_2e^\{\[\\mathcal S\_2\]\\theta\_2\}\\dots e^\{\[\\mathcal S\_n\]\\theta\_n\}M+\\dots\\\\ T˙=(dtde\[S1\]θ1)…e\[Sn\]θnM\+e\[S1\]θ1(dtde\[S2\]θ2)…e\[Sn\]θnM\+…=\[S1\]θ˙1e\[S1\]θ1…e\[Sn\]θnM\+e\[S1\]θ1\[S2\]θ˙2e\[S2\]θ2…e\[Sn\]θnM\+…  
也有  
 T − 1 = M − 1 e − \[ S n \] θ n … e − \[ S 1 \] θ 1 . T^\{-1\}=M^\{-1\}e^\{-\[\\mathcal S\_n\]\\theta\_n\}\\dots e^\{-\[\\mathcal S\_1\]\\theta\_1\}. T−1=M−1e−\[Sn\]θn…e−\[S1\]θ1.  
计算 T ˙ T − 1 \\dot TT^\{-1\} T˙T−1得到  
 \[ V s \] = \[ S 1 \] θ ˙ 1 + e \[ S 1 \] θ ˙ 1 \[ S 2 \] e − \[ S 1 \] θ ˙ 1 + e \[ S 1 \] θ ˙ 1 e \[ S 2 \] θ ˙ 2 \[ S 3 \] e − \[ S 1 \] θ ˙ 1 e − \[ S 2 \] θ ˙ 2 + … \[\\mathcal V\_s\]=\[\\mathcal S\_1\]\\dot \\theta\_1+e^\{\[\\mathcal S\_1\]\\dot \\theta\_1\}\[\\mathcal S\_2\]e^\{-\[\\mathcal S\_1\]\\dot \\theta\_1\}+e^\{\[\\mathcal S\_1\]\\dot \\theta\_1\}e^\{\[\\mathcal S\_2\]\\dot \\theta\_2\}\[\\mathcal S\_3\]e^\{-\[\\mathcal S\_1\]\\dot \\theta\_1\}e^\{-\[\\mathcal S\_2\]\\dot \\theta\_2\}+\\dots \[Vs\]=\[S1\]θ˙1\+e\[S1\]θ˙1\[S2\]e−\[S1\]θ˙1\+e\[S1\]θ˙1e\[S2\]θ˙2\[S3\]e−\[S1\]θ˙1e−\[S2\]θ˙2\+…  
上式可以进一步写成向量形式的伴随映射  
 V s = S 1 ⏟ J s 1 θ ˙ 1 + Ad e \[ S 1 \] θ 1 ( S 2 ) ⏟ J s 2 θ ˙ 2 + Ad e \[ S 1 \] θ 1 e \[ S 2 \] θ 2 ( S 3 ) ⏟ J s 3 θ ˙ 3 + … (5.6) \\mathcal V\_s=\\underbrace \{\\mathcal S\_1\}\_\{J\_\{s1\}\}\\dot\\theta\_1+\\underbrace\{\\text\{Ad\}\_\{e^\{\[\\mathcal S\_1\]\\theta\_1\}\}(\\mathcal S\_2)\}\_\{J\_\{s2\}\}\\dot \\theta\_2+\\underbrace\{\\text\{Ad\}\_\{e^\{\[\\mathcal S\_1\]\\theta\_1\}\{e^\{\[\\mathcal S\_2\]\\theta\_2\}\}\}(\\mathcal S\_3)\}\_\{J\_\{s3\}\}\\dot \\theta\_3+\\dots \\tag\{5.6\} Vs=Js1S1θ˙1\+Js2Ade\[S1\]θ1(S2)θ˙2\+Js3Ade\[S1\]θ1e\[S2\]θ2(S3)θ˙3\+…(5.6)  
观察发现， V s \\mathcal V\_s Vs是 n n n个空间旋量的和的形式  
 V s = J s 1 θ ˙ 1 + J s 2 ( θ ) θ ˙ 2 + ⋯ + J s n ( θ ) θ ˙ n , (5.7) \\mathcal V\_s=J\_\{s1\}\\dot\\theta\_1+J\_\{s2\}(\\theta)\\dot\\theta\_2+\\dots +J\_\{sn\}(\\theta)\\dot \\theta\_n,\\tag\{5.7\} Vs=Js1θ˙1\+Js2(θ)θ˙2\+⋯\+Jsn(θ)θ˙n,(5.7)  
这里对任意 i = 2 , … , n i=2, \\dots,n i=2,…,n, 每个 J s i ( θ ) = ( ω s i ( θ , v s i ( θ ) ) ) J\_\{si\}(\\theta)=(\\omega\_\{si\}(\\theta,v\_\{si\}(\\theta))) Jsi(θ)=(ωsi(θ,vsi(θ)))显式地决定于关节变量 θ ∈ R n \\theta\\in\\mathbb R^n θ∈Rn. 写成矩阵形式  
 V s = \[ J s 1 J s 2 ( θ ) … J s n ( θ ) \] \[ θ ˙ 1 θ ˙ ⋮ θ ˙ \] = J s ( θ ) θ ˙ . (5.8) \\mathcal V\_s=\\left\[ \\begin\{matrix\} J\_\{s1\} & J\_\{s2\} (\\theta)&\\dots &J\_\{sn\}(\\theta) \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} \\dot\\theta\_1 \\\\ \\dot \\theta\\\\ \\vdots\\\\\\dot\\theta \\end\{matrix\}\\right\] =J\_s(\\theta)\\dot\\theta .\\tag \{5.8\} Vs=\[Js1Js2(θ)…Jsn(θ)\]⎣⎢⎢⎢⎡θ˙1θ˙⋮θ˙⎦⎥⎥⎥⎤=Js(θ)θ˙.(5.8)  
矩阵 J s ( θ ) J\_s(\\theta) Js(θ)称为在（空间）固定坐标系的雅可比，或称为空间雅可比。

\*\*补充：\*\*这里伴随映射的定义：给定一个齐次变换矩阵 T = \[ R p 0 1 \] ∈ S E ( 3 ) T=\\left\[ \\begin\{matrix\} R&p\\\\0&1 \\end \{matrix\}\\right\]\\in SE(3) T=\[R0p1\]∈SE(3)，其伴随表示 \[ Ad T \] \[\\text\{Ad\}\_T\] \[AdT\]是  
 Ad T = \[ R R \[ p \] R 0 \] ∈ R 6 × 6 \\text\{Ad\}\_T=\\left\[ \\begin\{matrix\} R&R\\\\ \[p\]R&0 \\end \{matrix\}\\right\] \\in \\mathbb R^\{6\\times6\} AdT=\[R\[p\]RR0\]∈R6×6  
这里 \[ p \] \[p\] \[p\] 表示向量 p p p对应的反对称矩阵。  \[ Ad T \] \[\\text\{Ad\}\_T\] \[AdT\]的物理意义实际是对6维旋量进行坐标变换，类似于齐次矩阵对3维向量进行坐标变换。例如，坐标系\{s\}到坐标系\{b\}的其次变换为 T s b T\_\{sb\} Tsb，对一个旋量 V b \\mathcal V\_b Vb进行伴随表示可以写成  
 V s = \[ Ad T s b \] V b = Ad T s b ( V b ) \\mathcal V\_s=\[\\text\{Ad\}\_\{T\_\{sb\}\}\]\\mathcal V\_b=\\text\{Ad\}\_\{T\_\{sb\}\}(\\mathcal V\_b) Vs=\[AdTsb\]Vb=AdTsb(Vb)

## 物体雅可比（Body Jacobian） ##

类似的，若旋量在物体坐标系里描述，那么可以推导出在物体坐标系描述的运动学表达式，进而推导出物体雅可比,即

假设一个开链机构的正运动学以指数积形式写成  
 T ( θ ) = M e \[ B 1 \] θ 1 … e \[ B n \] θ n . T(\\theta)=Me^\{\[\\mathcal B\_1\]\\theta\_1\}\\dots e^\{\[\\mathcal B\_n\]\\theta\_n\}. T(θ)=Me\[B1\]θ1…e\[Bn\]θn.  
空间雅可比 J b ( θ ) ∈ R 6 × n J\_b(\\theta)\\in \\mathbb R^\{6\\times n\} Jb(θ)∈R6×n关联了关节速度向量 θ ˙ ∈ R n \\dot \\theta \\in\\mathbb R^n θ˙∈Rn和空间旋量 V b \\mathcal V\_b Vb，即 V b = J b ( θ ) θ ˙ \\mathcal V\_b=J\_b(\\theta)\\dot \\theta Vb=Jb(θ)θ˙。 J b ( θ ) J\_b(\\theta) Jb(θ)第 i i i列由下式确定  
 J b i ( θ ) = A d e \[ B 1 \] θ 1 . . . e \[ B i − 1 \] θ i − 1 ( B i ) , J\_\{bi\}(\\theta)=Ad\_\{e^\{\[\\mathcal B\_1\]\\theta\_1\}...e^\{\[\\mathcal B\_\{i-1\}\]\\theta\_\{i-1\}\}\}(\\mathcal B\_i), Jbi(θ)=Ade\[B1\]θ1...e\[Bi−1\]θi−1(Bi),  
对于 i = n − 1 , … , 1 , i=n-1, \\dots, 1, i=n−1,…,1,第n列 J B n = B n J\_\{B\_n\}=\\mathcal B\_n JBn=Bn。螺旋矢量 J b i J\_\{bi\} Jbi的物理意义是在任意非零关节角度 θ \\theta θ下，第 i i i个关节在物体坐标系下的表达。

## 空间雅可比与物体雅可比的联系 ##

物体雅可比和空间雅可比的关系用下式表达  
 J s ( θ ) = \[ Ad T s b \] J b ( θ ) , J b ( θ ) = \[ Ad T b s \] J s ( θ ) , J\_s(\\theta)=\[\\text \{Ad\}\_\{T\_\{sb\}\}\]J\_b(\\theta),\\\\ J\_b(\\theta)=\[\\text \{Ad\}\_\{T\_\{bs\}\}\]J\_s(\\theta), Js(θ)=\[AdTsb\]Jb(θ),Jb(θ)=\[AdTbs\]Js(θ),  
这里 T s b = T ( θ ) T\_\{sb\}=T(\\theta) Tsb=T(θ)。

\#\#例子：雅可比计算

**Example 1**: 计算如图的RRRP空间机器人的空间雅可比。

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70_pic_center]

我们知道雅可比的第 i i i列，就是第 i i i个关节的螺旋轴（旋量）在空间坐标系（固定坐标）的表示 J s i = \[ ω s i , v s i \] T J\_\{si\}=\[\\omega\_\{si\},v\_\{si\}\]^T Jsi=\[ωsi,vsi\]T。有时不能直接看出其旋量时，可以通过 \[ Ad T i − 1 \] \[\\text \{Ad\}\_\{T\_\{i-1\}\}\] \[AdTi−1\]矩阵对螺旋轴进行坐标变换，得到其在固定坐标系的表示。

(1)观察发现 ω s 1 \\omega \_\{s1\} ωs1始终沿 z ^ s \\hat z\_s z^s方向不变，因此 ω s 1 = \[ 0 , 0 , 1 \] T \\omega\_\{s1\}=\[0,0,1\]^T ωs1=\[0,0,1\]T。选择坐标原点为 q 1 q\_1 q1,那么 v s 1 = \[ 0 , 0 , 0 \] T v\_\{s1\}=\[0,0,0\]^T vs1=\[0,0,0\]T.

(2) ω s 2 \\omega \_\{s2\} ωs2始终沿 z ^ s \\hat z\_s z^s方向不变，因此 ω s 2 = \[ 0 , 0 , 1 \] T \\omega\_\{s2\}=\[0,0,1\]^T ωs2=\[0,0,1\]T。选择轴线上 \[ L 1 c 1 , L 1 s 1 , 0 \] T \[L\_1c\_1,L\_1s\_1,0\]^T \[L1c1,L1s1,0\]T为 q 2 q\_2 q2,其中 c 1 = cos θ 1 , s 1 = sin ( θ 2 ) c\_1=\\text\{cos\}\\theta\_1,s\_1=\\text\{sin\}(\\theta\_2) c1=cosθ1,s1=sin(θ2),那么 v s 2 = − ω 2 × q 2 = \[ L 1 s 1 , − L 1 c 1 , 0 \] T v\_\{s2\}=-\\omega\_2\\times q\_2=\[L\_1s\_1,-L\_1c\_1,0\]^T vs2=−ω2×q2=\[L1s1,−L1c1,0\]T.

(3)不管如何运动， ω s 3 \\omega\_\{s3\} ωs3也是沿固定坐标系 z ^ s \\hat z\_s z^s方向，因此 ω s 3 = \[ 0 , 0 , 1 \] 2 \\omega\_\{s3\}=\[0,0,1\]^2 ωs3=\[0,0,1\]2，选择 q 3 = \[ L 1 c 1 + L 2 c 12 , L 1 s 1 + L 2 s 12 , 0 \] T q\_3=\[L\_1c\_1+L\_2c\_\{12\}, L\_1s\_1+L\_2s\_\{12\},0\]^T q3=\[L1c1\+L2c12,L1s1\+L2s12,0\]T，其中 c 12 = cos ( θ 1 + θ 2 ) , s 12 = sin ( θ 1 + θ 2 ) c\_\{12\}=\\text\{cos\}(\\theta\_1+\\theta\_2),s\_\{12\}=\\text\{sin\}(\\theta\_1+\\theta\_2) c12=cos(θ1\+θ2),s12=sin(θ1\+θ2)，那么 v s 3 = ( L 1 s 1 + L 2 s 12 , − L 1 c 1 − L 2 c 1 2 , 0 ) v\_\{s3\}=(L\_1s\_1+L\_2s\_\{12\},-L\_1c\_1-L\_2c\_12,0) vs3=(L1s1\+L2s12,−L1c1−L2c12,0).

(4)最后一个关节时移动副， ω s 4 = \[ 0 , 0 , 0 \] 2 \\omega\_\{s4\}=\[0,0,0\]^2 ωs4=\[0,0,0\]2,轴的方向为 v s 4 = \[ 0 , 0 , 1 \] 2 v\_\{s4\}=\[0,0,1\]^2 vs4=\[0,0,1\]2

综合上述分析，空间雅可比为  
 J s ( θ ) = \[ 0 0 0 0 0 0 0 0 1 1 1 0 0 L 1 s 1 L 1 s 1 + L 2 s 12 0 0 − L 1 c 1 − L 1 c 1 − L 2 c 12 0 0 0 0 1 \] J\_s(\\theta)=\\left\[ \\begin\{matrix\} 0 & 0 & 0& 0\\\\ 0 &0 &0 &0\\\\ 1 & 1 &1 & 0\\\\ 0 &L\_1s\_1 &L\_1s\_1+L\_2s\_\{12\}&0\\\\ 0 &-L\_1c\_1& -L\_1c\_1-L\_2c\_\{12\}&0\\\\ 0 & 0& 0 &1 \\end\{matrix\}\\right\] Js(θ)=⎣⎢⎢⎢⎢⎢⎢⎡001000001L1s1−L1c10001L1s1\+L2s12−L1c1−L2c120000001⎦⎥⎥⎥⎥⎥⎥⎤  
**Example 2**: 计算如图的RRPRRR空间机器人的空间雅可比。

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70_pic_center 1]

(1)第一个轴线始终保持不变， ω s 1 = \[ 0 , 0 , 1 \] 2 \\omega\_\{s1\}=\[0,0,1\]^2 ωs1=\[0,0,1\]2，选择 q 1 = \[ 0 , 0 , L 1 \] 2 q\_1=\[0,0,L1\]^2 q1=\[0,0,L1\]2,得到 w s 1 = − ω s 1 × q 1 = \[ 0 , 0 , 0 \] T w\_\{s1\}=-\\omega\_\{s1\}\\times q\_1=\[0,0,0\]^T ws1=−ωs1×q1=\[0,0,0\]T

(2)第二个关节的轴线随 θ 1 \\theta\_1 θ1变化， ω s 2 = \[ − c 1 , − s 1 , 0 \] 2 \\omega\_\{s2\}=\[-c\_1,-s\_1,0\]^2 ωs2=\[−c1,−s1,0\]2 ，选择 q 1 = ( 0 , 0 , L 1 ) q\_1=(0,0,L\_1) q1=(0,0,L1) ，那么 v s 2 = − ω s 2 × q 2 = \[ L 1 s 1 , − L 1 c 1 , 0 \] v\_\{s2\}=-\\omega\_\{s2\}\\times q\_2=\[L\_1s\_1,-L\_1c\_1,0\] vs2=−ωs2×q2=\[L1s1,−L1c1,0\]。

(3)第三个关节是移动副，因此 ω s 3 = \[ 0 , 0 , 0 \] T \\omega\_\{s3\}=\[0,0,0\]^T ωs3=\[0,0,0\]T,轴线方向

v s 3 = R o t ( z ^ , θ 1 ) R o t ( x ^ , − θ 2 ) \[ 0 , 1 , 0 \] T = \[ − s 1 c 2 , c 1 c 2 , − s 2 \] T v\_\{s3\}=Rot(\\hat z,\\theta\_1)Rot(\\hat x,-\\theta\_2)\[0,1,0\]^T=\[-s\_1c\_2,c\_1c\_2,-s\_2\]^T vs3=Rot(z^,θ1)Rot(x^,−θ2)\[0,1,0\]T=\[−s1c2,c1c2,−s2\]T  
(4)后三个轴交于一点

q w = \[ 0 0 L 1 \] + R o t ( z ^ , θ 1 ) R o t ( x ^ , − θ 2 ) \[ 0 L 2 + θ 3 0 \] = \[ − ( L 2 + θ 3 ) s 1 c 2 ( L 2 + θ 3 ) c 1 c 2 L 1 − ( L 2 + θ 3 ) s 2 \] q\_w=\\left\[ \\begin\{matrix\} 0\\\\ 0\\\\ L\_1 \\end\{matrix\}\\right\] +Rot(\\hat z,\\theta\_1)Rot(\\hat x,-\\theta\_2) \\left\[ \\begin\{matrix\} 0\\\\ L\_2+\\theta\_3\\\\ 0 \\end\{matrix\}\\right\] =\\left\[ \\begin\{matrix\} -(L\_2+\\theta\_3)s\_1c\_2\\\\ (L\_2+\\theta\_3)c\_1c\_2\\\\ L\_1-(L\_2+\\theta\_3)s\_2 \\end\{matrix\}\\right\] qw=⎣⎡00L1⎦⎤\+Rot(z^,θ1)Rot(x^,−θ2)⎣⎡0L2\+θ30⎦⎤=⎣⎡−(L2\+θ3)s1c2(L2\+θ3)c1c2L1−(L2\+θ3)s2⎦⎤  
后三个轴的轴线方向为  
 ω s 4 = R o t ( z ^ , θ 1 ) R o t ( x ^ , − θ 2 ) \[ 0 0 1 \] = \[ − s 1 s 2 c 1 s 2 c 2 \] , ω s 5 = R o t ( z ^ , θ 1 ) R o t ( x ^ , − θ 2 ) R o t ( z ^ , θ 4 ) \[ − 1 0 0 \] = \[ − c 1 c 4 + s 1 c 2 s 4 − s 1 c 4 − c 1 c 2 s 4 s 2 s 4 \] , ω s 6 = R o t ( z ^ , θ 1 ) R o t ( x ^ , − θ 2 ) R o t ( z ^ , θ 4 ) R o t ( x ^ , − θ 5 ) \[ 0 1 0 \] = \[ − c 5 ( s 1 c 2 c 4 + c 1 s 4 ) + s 1 s 2 s 5 c 5 ( c 1 c 2 c 4 − s 1 s 4 ) − c 1 s 2 s 5 − s 2 c 4 c 5 − c 2 s 5 \] \\omega\_\{s4\}=Rot(\\hat z,\\theta\_1)Rot(\\hat x,-\\theta\_2) \\left\[ \\begin\{matrix\} 0\\\\ 0\\\\ 1 \\end\{matrix\}\\right\] =\\left\[ \\begin\{matrix\} -s\_1s\_2\\\\ c\_1s\_2\\\\ c\_2 \\end\{matrix\}\\right\],\\\\ \\omega\_\{s5\}=Rot(\\hat z,\\theta\_1)Rot(\\hat x,-\\theta\_2)Rot(\\hat z,\\theta\_4) \\left\[ \\begin\{matrix\} -1\\\\ 0\\\\ 0 \\end\{matrix\}\\right\] =\\left\[ \\begin\{matrix\} -c\_1c\_4+s\_1c\_2s\_4\\\\ -s\_1c\_4-c\_1c\_2s\_4\\\\ s\_2s\_4 \\end\{matrix\}\\right\],\\\\ \\omega\_\{s6\}=Rot(\\hat z,\\theta\_1)Rot(\\hat x,-\\theta\_2)Rot(\\hat z,\\theta\_4)Rot(\\hat x,-\\theta\_5) \\left\[ \\begin\{matrix\} 0\\\\ 1\\\\ 0 \\end\{matrix\}\\right\]\\\\ =\\left\[ \\begin\{matrix\} -c\_5(s\_1c\_2c\_4+c\_1s\_4)+s\_1s\_2s\_5\\\\ c\_5(c\_1c\_2c\_4-s\_1s\_4)-c\_1s\_2s\_5\\\\ -s\_2c\_4c\_5-c\_2s\_5 \\end\{matrix\}\\right\] ωs4=Rot(z^,θ1)Rot(x^,−θ2)⎣⎡001⎦⎤=⎣⎡−s1s2c1s2c2⎦⎤,ωs5=Rot(z^,θ1)Rot(x^,−θ2)Rot(z^,θ4)⎣⎡−100⎦⎤=⎣⎡−c1c4\+s1c2s4−s1c4−c1c2s4s2s4⎦⎤,ωs6=Rot(z^,θ1)Rot(x^,−θ2)Rot(z^,θ4)Rot(x^,−θ5)⎣⎡010⎦⎤=⎣⎡−c5(s1c2c4\+c1s4)\+s1s2s5c5(c1c2c4−s1s4)−c1s2s5−s2c4c5−c2s5⎦⎤  
因此，空间雅可比可以写成矩阵的形式为  
 J s ( θ ) = \[ ω s 1 ω s 2 0 ω s 4 ω s 5 ω s 6 0 − ω s 2 × q 2 v s 3 − ω s 4 × q w − ω s 5 × q w − ω s 6 × q w \] J\_s(\\theta)=\\left\[ \\begin\{matrix\} \\omega\_\{s1\}&\\omega\_\{s2\}&0 &\\omega\_\{s4\}&\\omega\_\{s5\}& \\omega\_\{s6\}\\\\ 0&-\\omega\_\{s2\}\\times q\_2 & v\_\{s3\}&-\\omega\_\{s4\}\\times q\_w &-\\omega\_\{s5\}\\times q\_w& -\\omega\_\{s6\}\\times q\_w \\end\{matrix\}\\right\] Js(θ)=\[ωs10ωs2−ωs2×q20vs3ωs4−ωs4×qwωs5−ωs5×qwωs6−ωs6×qw\]  
注意到，我们不需要对正运动学模型进行微分，就可以直接获得整个雅可比。

## 算法实现（C语言） ##

基于指数坐标计算雅可比的算法，我已用C语言实现，包括两个函数JacobianBody和JacobianSpace 。下面只列出这两个函数的实现。代码不断更新迭代，完整的最新源码参见github–[链接][Link 1]

/** * @brief Description: Algorithm module of robotics, according to the * book[modern robotics : mechanics, planning, and control]. * @file: RobotAlgorithmModule.c * @author: Brian * @date: 2019/03/01 12:23 * Copyright(c) 2019 Brian. All rights reserved. * * Contact https://blog.csdn.net/Galaxy_Robot * @warning: */
    
    /** *@brief Description: Computes the body Jacobian Jb(theta) in 6×n given a list of joint screws Bi expressed in the body frame and a list of joint angles. *@param[in] Blist The joint screw axes in the end - effector frame when the manipulator is * at the home position, in the format of a matrix with the screw axes as the row. *@param[in] thetalist A list of joint coordinates. *@param[out] Jb Body Jacobian matrix. *@return No return value. *@note: when Blist and Jb are matrixes ,make sure that columns number of Slist or Jb is equal to JointNum, * rows number of Slist or Jb is 6 .The function call should be written as * JacobianSpace(JointNum,(double *)Slist,thetalist,(double *)Jb). *@waring: */
    void JacobianBody(int JointNum,double *Blist, double *thetalist,double *Jb)
    { 
    	int i;
    	int j;
    	int k;
    	double T1[4][4];
    	double T2[4][4];
    	double se3mat[4][4];
    	double V[6];
    	double AdT[6][6];
    	double T[4][4] = { 
    		1,0,0,0,
    		0,1,0,0,
    		0,0,1,0,
    		0,0,0,1
    	};
    	//Fist column of Jbn.
    	for (i=0;i<6;i++)
    	{ 
    		Jb[i*JointNum+JointNum -1] = Blist[i*JointNum + JointNum-1];
    	}
    	//Jbi for i=n-1,n-2,...,1.
    	for (i= JointNum -2;i>=0;i--)
    	{ 
    		for (j=0;j<6;j++)
    		{ 
    			V[j] = -1.0*Blist[j*JointNum+i+1];
    		}
    		VecTose3(V, se3mat);
    		Matrix4MultValue(se3mat, thetalist[i + 1], se3mat);
    		MatrixExp6(se3mat, T1);
    		Matrix4Mult(T, T1,T2);
    		Matrix4Equal(T2, T);
    		Adjoint(T, AdT);
    		for (j=0;j<6; j++)
    		{ 
    			Jb[j*JointNum + i] = 0;
    			for (k=0;k<6;k++)
    			{ 
    				Jb[j*JointNum + i] = Jb[j*JointNum + i] + AdT[j][k] * Blist[k*JointNum + i];
    			}
    		}
    	}
    	return ;
    }
    
    /** *@brief Description:Computes the space Jacobian Js(theta) in R6 x n given a list of joint screws Si * expressed in the fixed space frame and a list of joint angles. *@param[in] Slist The joint screw axes expressed in the fixed space frame when the manipulator is * at the home position, in the format of a matrix with the screw axes as the column. *@param[in] thetalist A list of joint coordinates. *@param[out] Js Space Jacobian matrix. *@return No return value. *@note: when Slist and Js are matrixes ,make sure that columns number of Slist or Js is equal to JointNum, * rows number of Slist or Js is 6 .The function call should be written as * JacobianSpace(JointNum,(double *)Slist,thetalist,(double *)Js). *@waring: */
    void JacobianSpace(int JointNum, double *Slist, double *thetalist, double *Js)
    { 
    	int i;
    	int j;
    	int k;
    	double T1[4][4];
    	double T2[4][4];
    	double se3mat[4][4];
    	double V[6];
    	double AdT[6][6];
    	double T[4][4] = { 
    		1,0,0,0,
    		0,1,0,0,
    		0,0,1,0,
    		0,0,0,1
    	};
    	//Fist column of Js.
    	for (i = 0; i < 6; i++)
    	{ 
    		Js[i*JointNum + 0] = Slist[i*JointNum + 0];
    	}
    	//Jsi for i=2,3,...,n.
    	for (i = 1; i <JointNum; i++)
    	{ 
    		for (j = 0; j < 6; j++)
    		{ 
    			V[j] = Slist[j*JointNum + i - 1];
    		}
    		VecTose3(V, se3mat);
    		Matrix4MultValue(se3mat, thetalist[i - 1], se3mat);
    		MatrixExp6(se3mat, T1);
    		Matrix4Mult(T, T1, T2);
    		Matrix4Equal(T2, T);
    		Adjoint(T, AdT);
    		for (j = 0; j < 6; j++)
    		{ 
    			Js[j*JointNum + i] = 0;
    			for (k = 0; k < 6; k++)
    			{ 
    				Js[j*JointNum + i] = Js[j*JointNum + i] + AdT[j][k] * Slist[k*JointNum + i];
    			}
    		}
    	}
    	return;
    }

--------------------

参考文献

Kenvin M. Lynch , Frank C. Park, Modern Robotics Mechanics,Planning , and Control. May 3, 2017

[20201018101004636.gif_pic_center]: /images/20221123/6b326b20557a48cfa4c48a8b1f3b67e7.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70_pic_center]: /images/20221123/1785b2f563194b0b975aeeccb77bf4a5.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70_pic_center 1]: /images/20221123/237dfed40f6940c0afe85db569ecb311.png
[Link 1]: https://github.com/libing403/RoboticsAlgorithm