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

这种区别仅靠看论文和课本是很难发现的，但是在用代码实现时却很容易掉进“坑”里。好在我替大家踩了。
对于雅可比矩阵的算法，本文涉及了两种方法。
首先分别列出两种算法的计算公式和对应的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]=[b1c1......bncn]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−1TN+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+1001
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)T001j=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可以实现这两种表示方法的转换。
搞清楚雅可比矩阵的形式和计算结果形式这一点很重要，因为在进行加速度甚至动力学计算式都需要雅可比矩阵，而进行加速度计算是某些形式的雅可比矩阵无法得出正确的结果。如果贪图计算简洁而用了一些奇怪的方法，就很有肯导致加速度计算失败进而动力学建模失败。我建议还是按照《机器人动力学与控制》里的方法计算雅可比矩阵，这样比较安全些。

雅可比矩阵的不同算法及其区别相关推荐

朴素贝叶斯算法和逻辑回归算法的区别？
朴素贝叶斯算法和逻辑回归算法的区别? 1.两种算法的模型不同: Naive Bayes是一个生成模型,在计算P(y|x)之前,先要从训练数据中计算P(x|y)和P(y)的概率,从而利用贝叶斯公式计算P ...
从活动选择问题看动态规划和贪心算法的区别与联系
这篇文章主要用来记录我对<算法导论> 贪心算法一章中的"活动选择问题"的动态规划求解和贪心算法求解的思路和理解. 主要涉及到以下几个方面的内容: ①什么是活动选择问题 ...
（转）动态规划和贪心算法的区别
(转自)http://hi.baidu.com/35661327/blog/item/d5463e17f1e8d011972b439c.html 动态规划和贪心算法的区别 2009-07-27 13: ...
yolo和以往算法的区别/yolo为什么叫yolo
yolov3官网: YOLO: Real-Time Object Detection 在算法流程上,yolo和以往算法的区别,以往算法需要先滚一遍图片,看看有没有物体,然后再滚一遍有物体的区域,看看是 ...
KNN算法和Kernel KNN算法的区别
KNN算法和Kernel KNN算法的区别 KNN算法 KNN(K-Nearest Neighbor,简称KNN)算法,是一种常用的监督学习方法,其工作机制为:给定测试样本,基于某种距离度量找出训练集 ...
Dijkstra算法、Floyd算法的区别与联系，并由此谈到greedy和DP
首先,Dijkstra算法与Floyd算法都是广度优先搜索的算法.都可以用来求单源点到其他所有点的最短路径.那么这两者的原理分别是怎样?彼此又有什么区别呢? 求此有向图中起点1到其他所有点的最短距离 ...
详解五种最短路径算法及其区别（c++）
目录一.朴素Dijkstra算法二.堆优化的Dijkstra 三.bellman_ford算法四.spfa算法五.floyd算法使用区别: 所有边权都是正数的单源最短路:朴素Dijkstra ...
限流——漏桶算法和令牌桶算法的区别
文章目录限流漏桶算法令牌桶算法漏桶算法和令牌桶算法的区别时间窗口时间窗口和令牌桶优缺点限流在开发高并发系统时有三把利器用来保护系统:缓存.降级和限流缓存:缓存的目的是提升系统访问速度 ...
分治算法,动态规划算法和贪心算法的区别和联系
分治算法,动态规划算法和贪心算法的区别和联系 (一)分治算法分治算法为什么叫分治算法? 分治这个名字可以分成两部: 第一部分是分,表示把一个原问题分解成很多个小问题,逐个解决; 第二部分是治, 表示 ...

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

方法一

方法二

分析两种方法的异同点

雅可比矩阵的不同算法及其区别相关推荐

最新文章

热门文章