這種區別僅靠看論文和課本是很難發現的，但是在用代碼實現時卻很容易掉進“坑”里。好在我替大家踩了。
對于雅可比矩陣的算法，本文涉及了兩種方法。
首先分別列出兩種算法的計算公式和對應的Mathematica代碼。
齊次分析其異同點和對機械臂運動學的影響。
方法一
機器人動力學與控制-霍偉
[vnωn]=[b1...bnc1...cn]q˙=Jq˙\begin{bmatrix} v_n \\ \omega_n \end{bmatrix} =\begin{bmatrix} b_1&...& b_n\\ c_1&...&c_n \end{bmatrix} \dot{q} =J\dot{q}[vn?ωn??]=[b1?c1??......?bn?cn??]q˙?=Jq˙?
bi=zi?1×(pn?pi?1)b_i= z_{i-1}\times(p_n-p_{i-1})bi?=zi?1?×(pn??pi?1?)
ci=zi?1c_i=z_{i-1}ci?=zi?1?
pnp_npn?:坐標系n的原點在坐標系0中表示的位置向量
zi?1z_{i-1}zi?1?:坐標系i-1的z軸在坐標系0中的表示
下面代碼以兩自由度機械臂為例
T01 = {{Cos [Subscript[\[Theta], 1]], -Sin[Subscript[\[Theta], 1]], 0,a1 Cos [Subscript[\[Theta], 1]]}, {Sin[Subscript[\[Theta], 1]], Cos [Subscript[\[Theta], 1]], 0, a1 Sin[Subscript[\[Theta], 1]]}, {0, 0, 1, 0}, {0, 0, 0, 1}};
T12 = {{Cos [Subscript[\[Theta], 2]], -Sin[Subscript[\[Theta], 2]], 0,a2 Cos [Subscript[\[Theta], 2]]}, {Sin[Subscript[\[Theta], 2]], Cos [Subscript[\[Theta], 2]], 0, a2 Sin[Subscript[\[Theta], 2]]}, {0, 0, 1, 0}, {0, 0, 0, 1}};
T02 = T01.T12;
R01 = Table[T01[[i, j]], {i, 3}, {j, 3}];
qd1={0, 0.2969, 0.9383, 1.6163, 2.1112, 2.2907, 2.1112, 1.61634942, 0.9383, 0.2969, 0};
qd2={0,-0.1484,-0.4691,-0.8082,-1.0556,-1.1454,-1.0556,-0.8081747,-0.4691,-0.1484, 0};
times = 8;
Subscript[\[Theta], 1] = q1[[times]]; Subscript[\[Theta], 2] = q2[[times]];
a1 = 2; a2 = 1;
p1sta = {2, 0, 0}; p2sta = {1, 0, 0};
(*初始化完畢*)
(******************************************************)z00 = {0, 0, 1};
z01 = R01.{0, 0, 1};p2 = Table[T02[[i, 4]], {i, 3}];
p1 =Table[T01[[i, 4]], {i, 3}];
p0 = {0, 0, 0}; b1 = z00\[Cross](p2 - p0);
c1 = {0, 0, 1};
b2 = z01\[Cross](p2 - p1);
c2 = R01.{0, 0, 1};
j1 = Join[jb1, jc1, 1]; MatrixForm[j1];
j2 = Join[jb2, jc2, 1]; MatrixForm[j2];
jacobian = {j1, j2}; Print["雅可比矩陣 J=" MatrixForm[Transpose[jacobian]]]
計算結果：
雅可比矩陣 J=
(
-2.19608 -0.489264
1.91462 0.872136
0. 0.
0 0.
0 0.
1 1.)
使用上述雅可比矩陣中進行正向速度求解：
endVol = Transpose[jacobian].{qd1[[times]], qd2[[times]]};
Print["在絕對坐標系表示 endVol=", endVol];
結果
在絕對坐標系表示 endVol={-3.15422,2.38986,0.,0.,0.,0.808175}
這個結果說明通過上述雅可比矩陣進行正向速度計算，得到的末端速度是在坐標系0中表示的。
當然我們可以用旋轉矩陣 R20R_2^0R20? ，轉換到末端坐標系中表示。
至此方法一介紹完畢。
方法二
Efficient Computation of the Jacobian for Robot Manipulators
與方法一類似，只是用來迭代的形式計算，這樣計算量比較小。
TN+1N+1=IT_{N+1}^{N+1}=ITN+1N+1?=I
TN+1i?1=Tii?1TN+1iT_{N+1}^{i-1}=T_{i}^{i-1} T_{N+1}^{i}TN+1i?1?=Tii?1?TN+1i?
ciN+1=Ri?1N+1[001]c_{i}^{N+1}=R_{i-1}^{N+1} \begin{bmatrix} 0\\ 0\\ 1 \end{bmatrix}ciN+1?=Ri?1N+1??001??
N+1bij=?(i?1RN+1j×i?1ri?1)T[001]j=1,2,3;i=1,2,...N^{N+1}b_i^{j} = -( ^{i-1}R_{N+1}^{j} \times ^{i-1}r_{i-1} )^T\begin{bmatrix} 0\\ 0\\ 1 \end{bmatrix} j=1,2,3; i=1,2,...NN+1bij?=?(i?1RN+1j?×i?1ri?1?)T?001??j=1,2,3;i=1,2,...N
Mathematica代碼：
T01={{Cos [Subscript[\[Theta], 1]],-Sin[Subscript[\[Theta], 1]],0,a1 Cos [Subscript[\[Theta], 1]]}, {Sin[Subscript[\[Theta], 1]],Cos [Subscript[\[Theta], 1]],0,a1 Sin[Subscript[\[Theta], 1]]}, {0,0,1,0},{0,0,0,1}};
T12={{Cos [Subscript[\[Theta], 2]],-Sin[Subscript[\[Theta], 2]],0,a2 Cos [Subscript[\[Theta], 2]]},{Sin[Subscript[\[Theta], 2]],Cos [Subscript[\[Theta], 2]],0,a2 Sin[Subscript[\[Theta], 2]]},{0,0,1,0},{0,0,0,1}};
T02=T01.T12;
T10=Transpose[T01];T21=Transpose[T12];
R01=Table[T01[[i,j]],{i,3},{j,3}];
R12=Table[T12[[i,j]],{i,3},{j,3}];
R02=Table[T02[[i,j]],{i,3},{j,3}];
R10=Table[T10[[i,j]],{i,3},{j,3}];
R21=Table[T21[[i,j]],{i,3},{j,3}];
r00=Table[T02[[i,4]],{i,3}];MatrixForm[r00];(*(\[InvisiblePrefixScriptBase]^(i-1))Subscript[r, i-1]；i=1 from (\[InvisiblePrefixScriptBase]^(i-1))Subscript[T, N+1]*)
r11=Table[T12[[i,4]],{i,3}];MatrixForm[r11];(*(\[InvisiblePrefixScriptBase]^(i-1))Subscript[r, i-1]；i=2 from (\[InvisiblePrefixScriptBase]^(i-1))Subscript[T, N+1]*)
R03=R02;MatrixForm[R03];(*(\[InvisiblePrefixScriptBase]^(i-1))Subscript[U, N+1] ；i=1*)
R031=Table[R02[[i,1]],{i,3}];MatrixForm[R031];(*((\[InvisiblePrefixScriptBase]^(i-1))Subscript[U, N+1])^j；i=1，j=1*)
R032=Table[R02[[i,2]],{i,3}];MatrixForm[R032];(*((\[InvisiblePrefixScriptBase]^(i-1))Subscript[U, N+1])^j；i=1，j=2*)
R13=R12;MatrixForm[R13];(*(\[InvisiblePrefixScriptBase]^(i-1))Subscript[U, N+1] ；i=2*)
R131=Table[R12[[i,1]],{i,3}];MatrixForm[R131];(*((\[InvisiblePrefixScriptBase]^(i-1))Subscript[U, N+1])^j；i=2，j=1*)
R132=Table[R12[[i,2]],{i,3}];MatrixForm[R132];(*((\[InvisiblePrefixScriptBase]^(i-1))Subscript[U, N+1])^j；i=2，j=2*)
qd1={0, 0.2969, 0.9383, 1.6163, 2.1112, 2.2907, 2.1112, 1.61634942, 0.9383, 0.2969, 0};
qd2={0,-0.1484,-0.4691,-0.8082,-1.0556,-1.1454,-1.0556,-0.8081747,-0.4691,-0.1484, 0};
times = 8;
Subscript[\[Theta], 1] = q1[[times]]; Subscript[\[Theta], 2] = q2[[times]];
a1 = 2; a2 = 1;
p1sta = {2, 0, 0}; p2sta = {1, 0, 0};
(*初始化完畢*)
（************************）r={r00,r11};
R={{R031,R032,{0,0,1}},{R131,R132,{0,0,1}}};
c31=Transpose[R03].{0,0,1};
b311=Cross[R[[1,1]],-r[[1]]].{0,0,1};
b312=Cross[R[[1,2]],-r[[1]]].{0,0,1};
c32=Transpose[R13].{0,0,1};
b321=Cross[R[[2,1]],-r[[2]]].{0,0,1};
b322=Cross[R[[2,2]],-r[[2]]].{0,0,1};
jacobian1={{b311,b312,0},{b321,b322,0}};
jacobian=Join[jacobian1,{c31,c32},2];
Print["雅可比矩陣：",MatrixForm[Transpose[jacobian]]];```
計算結果 ：
雅可比矩陣：
(
-0.978527 0.
2.74427 1.
0 0
0. 0.
0. 0.
1. 1.)
使用上述雅可比矩陣進行正向速度計算：
endVol=Transpose[jacobian].{qd1[[times]],qd2[[times]]}
結果
{-1.58164, 3.62753, 0., 0., 0., 0.808175}
分析兩種方法的異同點
兩種方法求得的雅可比矩陣并不相同
法一：
雅可比矩陣 J=
(
-2.19608 -0.489264
1.91462 0.872136
0. 0.
0 0.
0 0.
1 1.
)法二：
雅可比矩陣：
(
-0.978527 0.
2.74427 1.
0 0
0. 0.
0. 0.
1. 1.
)
但是這兩種雅可比矩陣中都是正確的，因此得出的結果也不相同：
法一： endVol={-3.15422,2.38986,0.,0.,0.,0.808175}
法二：endVol={-1.58164, 3.62753, 0., 0., 0., 0.808175}
法一得到的末端速度是在坐標系0中表示的，而法二得到的末端速度是在坐標系2（題設為兩自由度機械臂）中表示的，實際末端速度向量的模都是一樣的。使用旋轉矩陣 R20R_2^0R20?可以實現這兩種表示方法的轉換。
搞清楚雅可比矩陣的形式和計算結果形式這一點很重要，因為在進行加速度甚至動力學計算式都需要雅可比矩陣，而進行加速度計算是某些形式的雅可比矩陣無法得出正確的結果。如果貪圖計算簡潔而用了一些奇怪的方法，就很有肯導致加速度計算失敗進而動力學建模失敗。我建議還是按照《機器人動力學與控制》里的方法計算雅可比矩陣，這樣比較安全些。
總結
以上是生活随笔為你收集整理的雅可比矩阵的不同算法及其区别的全部內容，希望文章能夠幫你解決所遇到的問題。
如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。
