雅可比矩阵的不同算法及其区别

最新推荐文章于 2024-07-19 13:59:11 发布

卑微的电子

最新推荐文章于 2024-07-19 13:59:11 发布

阅读量468

点赞数

文章标签：算法矩阵机器学习

本文链接：https://blog.csdn.net/qq_44751034/article/details/129211950

版权

这种区别仅靠看论文和课本是很难发现的，但是在用代码实现时却很容易掉进“坑”里。好在我替大家踩了。
对于雅可比矩阵的算法，本文涉及了两种方法。
首先分别列出两种算法的计算公式和对应的Mathematica代码。
齐次分析其异同点和对机械臂运动学的影响。

方法一

机器人动力学与控制-霍伟

$\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}$
$b_i= z_{i-1}\times(p_n-p_{i-1})$
$c_i=z_{i-1}$
$p_n$ :坐标系n的原点在坐标系0中表示的位置向量
$z_{i-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中表示的。
当然我们可以用旋转矩阵 $R_2^0$ ，转换到末端坐标系中表示。
至此方法一介绍完毕。

方法二

Efficient Computation of the Jacobian for Robot Manipulators

与方法一类似，只是用来迭代的形式计算，这样计算量比较小。
$T_{N+1}^{N+1}=I$
$T_{N+1}^{i-1}=T_{i}^{i-1} T_{N+1}^{i}$
$c_{i}^{N+1}=R_{i-1}^{N+1} \begin{bmatrix} 0\\ 0\\ 1 \end{bmatrix}$
$^{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,...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（题设为两自由度机械臂）中表示的，实际末端速度向量的模都是一样的。使用旋转矩阵 $R_2^0$ 可以实现这两种表示方法的转换。
搞清楚雅可比矩阵的形式和计算结果形式这一点很重要，因为在进行加速度甚至动力学计算式都需要雅可比矩阵，而进行加速度计算是某些形式的雅可比矩阵无法得出正确的结果。如果贪图计算简洁而用了一些奇怪的方法，就很有肯导致加速度计算失败进而动力学建模失败。我建议还是按照《机器人动力学与控制》里的方法计算雅可比矩阵，这样比较安全些。