机器人学回炉重造（2-2）：雅可比矩阵的求法——矢量积法、微分变换法、Manipulator Jacobian（Jacobian for short）

文章目录

写在前面
矢量积法——改进D-H法
微分变换法——改进D-H法
Manipulator for Jacobian(Jacobian for short)——标准D-H法
微分变换法——标准D-H法

写在前面

断更了俩月，找工作加毕业论文，心塞心塞，忙里偷闲还是得总结学习一下~

关于雅可比矩阵，其重要性不言而喻。说实话，踩了挺多坑的，看了许多教材和文献，都没有统一求法（也可能是我道行较浅学艺不精没看出来= = ），主要是没有统一建模的方法，导致标准D-H法有自己的雅可比矩阵求法，改进D-H法也有自己相应的求法。这里只是做个简单的总结分类，没有讨论两种模型对应方法之间的互换关系，本质上都是一样的，因为追根溯源还是得分析SDH（标准DH）和MDH（改进DH）之间的关系。至于他俩有啥不同，我前面的一篇博客中机器人学回炉重造（1）：正运动学、标准D-H法与改进D-H法的区别与应用（附ABB机械臂运动学建模matlab代码）已经简单说明了，一是固连的坐标系位置不同，二是坐标系X轴方向的确定方式不同，三是坐标系之间变换的规则不同，但最后都能得到相同的变换矩阵及末端位姿。

下面我陈列一下雅可比矩阵各个求法及对应的建模方法：

1.上一篇博客中，根据定义求雅可比矩阵——改进D-H法

参考文献：Stanford Oussama Khatib大神的《机器人学》公开课及对应的讲义

John Craig的《Introduction to robotics: mechanics and control》

2.矢量积法——改进D-H法

参考文献：Stanford Oussama Khatib大神的《机器人学》公开课及对应的讲义

蔡自兴，谢斌《机器人学》

3.微分变换法——改进D-H法

参考文献：蔡自兴，谢斌《机器人学》

4.Manipulator for Jacobian(Jacobian for short)——标准D-H法

参考文献：《Robotics: Modelling, Planning and Control》

《Robot Dynamics and Control Second Edition》

5.微分变换法——标准D-H法

第一个方法在上一篇博客中已经贴了代码了，推导过程ppt中也都有，这里就不再总结了。

矢量积法——改进D-H法

矢量积法求得的雅可比矩阵是相对于基座标系的，市面上的方法都是基于改进D-H法进行建立的

多翻教材！多翻教材！多翻教材！我只是一个愉快的搬运工~

仔细分析上两幅图，可以看出，对于关节i而言，其在末端产生的角速度为：
ω=∑i=1n(ϵˉiZi)q˙i\omega=\sum_{i=1}^{n}\left(\bar{\epsilon}_{i} Z_{i}\right) \dot{q}_{i} ω=i=1∑n(ϵˉiZi)q˙i
其中，ziz_izi是坐标系{i}的z轴单位向量在基座标系{0}中的表示。

关节i在末端产生的线速度为：
v=∑i=1n[ϵiZi+ϵˉi(Zi×pin0)]q˙iv=\sum_{i=1}^{n}\left[\epsilon_{i} Z_{i}+\bar{\epsilon}_{i}\left(Z_{i} \times p^0_{i n}\right)\right] \dot{q}_{i} v=i=1∑n[ϵiZi+ϵˉi(Zi×pin0)]q˙i
式中，Pin0P^0_{i n}Pin0表示末端坐标原点相对坐标系{i}的位置矢量在基座标系{0}中的表示，即Pin0=i0R∗pinP^0_{i n} = {^{0}_{i}}R * p_{in}Pin0=i0R∗pin

在公式（1）和（2）中εi={0revolute 1prismatic \varepsilon_{i}=\left\{\begin{array}{ll}{0} & {\text { revolute }} \\ {1} & {\text { prismatic }}\end{array}\right.εi={01 revolute prismatic 来表示关节是旋转关节还是移动关节，其中εˉ1=1−εi\bar{\varepsilon}_{1}=1-\varepsilon_{i}εˉ1=1−εi

所以，对于旋转关节i，雅可比矩阵第i列为
Ji=[zi×pin0zi]=[zi×(i0R∗pin)zi]J_{i}=\left[\begin{array}{c}{z_{i} \times p_{in}^{0}} \\ {z_{i}}\end{array}\right]=\left[\begin{array}{c}{z_{i} \times\left(_{i}^{0} R * p_{in}\right)} \\ {z_{i}}\end{array}\right] Ji=[zi×pin0zi]=[zi×(i0R∗pin)zi]
对于移动关节i，有
Ji=[zi0]J_{i}=\left[\begin{array}{c}{z_{i}} \\ {0}\end{array}\right] Ji=[zi0]
附上Matlab代码：

% use the vector product method to construct jacobian matrix
% 改进D-H法建模
% 该方法求出的雅可比矩阵是相对于基座标系{0}的
function J = shiliangji_Jacobian_MDH(T)n = length(T); % number of degrees of freedom
k = [0; 0; 1];% preallocationg the Jacobian matrix
Jv = zeros(3, n);
Jw = zeros(3, n);for i = 1: 6R = T{i}(1: 3, 1: 3);T_i_n = (inv(T{i})) * T{n};p_i_n = T_i_n(1: 3, 4);z_i = R * k;Jv(:, i) = cross(z_i, R*p_i_n);Jw(:, i) = z_i;
endJ = [Jv; Jw];endfunction [p_xyz, T] = My_Forward_Kinematics_MDH(theta, d, a, alp)
% 改进D-H建模
%Theta, d, a, Alpha are lists (1 row matrix) with n inputs
%       Where n is the number of links in the arm.
%
% theta:     Joint Angles (theta)
% d:     Link offset
% a:     Link Length
% alp:   Link Twist (alpha)
% n:     Robot degrees of freedom
n = length(theta);
% deg2rad可能需要可能不需要，视情况而定
th = theta;
% th = deg2rad(theta);
% alp = deg2rad(alp);T1_n = eye(4);
T{n} = zeros(4);for i = 1 : nTi_n= [cos(th(i))             -sin(th(i))               0            a(i);sin(th(i))*cos(alp(i))  cos(th(i))*cos(alp(i))  -sin(alp(i))  -sin(alp(i))*d(i);sin(th(i))*sin(alp(i))  cos(th(i))*sin(alp(i))  cos(alp(i))   cos(alp(i))*d(i);0                        0                         0            1];T1_n = T1_n * Ti_n;T{i} = T1_n;
endp_xyz = T{n}(1: 3, 4)';end

微分变换法——改进D-H法

市面上的微分变换法也是基于改进D-H法建立的，不过该方法求得的雅可比矩阵是相对于末端坐标系的，因此最后需要左乘旋转矩阵

我直接贴上蔡自兴《机器人学》中的公式，如下

附上Matlab 代码：

% Differential transformation for jacobian
% 改进D-H法建模
% 该方法求出的雅可比矩阵是相对于末端坐标系{n}的，需要左乘矩阵转变为相对于基系
function J = weifen_Jacobian_MDH(T)n = length(T);
k = [0 0 1];% preallocationg the Jacobian matrix
Jv = zeros(3, n);
Jw = zeros(3, n);for i = 1: 5T_i_n = inv(T{i}) * T{n};J_T_ix = k * cross(T_i_n(1: 3, 4), T_i_n(1: 3, 1));J_T_iy = k * cross(T_i_n(1: 3, 4), T_i_n(1: 3, 2));J_T_iz = k * cross(T_i_n(1: 3, 4), T_i_n(1: 3, 3));Jv(:, i) = [J_T_ix; J_T_iy; J_T_iz];Jw(:, i) = [T_i_n(3, 1); T_i_n(3, 2); T_i_n(3, 3)];
end
J_T = [Jv; Jw];
J_T(:, 6) = [0; 0; 0; 0; 0; 1];R_0_n = T{n}(1: 3, 1: 3);
TT = zeros(6, 6);
TT(1: 3, 1: 3) = R_0_n;
TT(4: 6, 4: 6) = R_0_n;J = TT * J_T; % 将相对于末端的雅可比矩阵变换成相对于基系的end

Manipulator for Jacobian(Jacobian for short)——标准D-H法

在提到的两个参考书中，这个方法都是基于标准D-H坐标系的，并且该方法求得的雅可比矩阵是相对于基座标系的

这个方法的推导过程有些麻烦，但是看懂了理解起来就很清晰了，不过我有些公式没整明白，还得多看看多想想才行。其过程请翻阅《Robot Dynamics and Control Second Edition》中的p107-p116，结果如下

对于旋转关节i，有
Ji=[zi−1×(pn−pi−1)zi−1]J_{i}=\left[\begin{array}{c}{z_{i-1} \times\left(p_{n}-p_{i-1}\right)} \\ {z_{i-1}}\end{array}\right] Ji=[zi−1×(pn−pi−1)zi−1]
对于移动关节i，有
Ji=[zi−10]J_{i}=\left[\begin{array}{c}{z_{i-1}} \\ {0}\end{array}\right] Ji=[zi−10]
其中，zi−1z_{i-1}zi−1表示坐标系{i-1}的z轴单位向量在座标系{0}中的表示；pn−pi−1p_n - p_{i-1}pn−pi−1表示末端坐标系{n}原点相对于坐标系{i-1}原点的位置矢量，pnp_npn由i0T_{i}^{0} Ti0T中的位置矢量给出，pi−1p_{i-1}pi−1由i−10T_{i-1}^{0} Ti−10T中的位置矢量给出。

看着和上面的矢量积法非常类似，就是下标和位置矢量不一样。比较一下两者的不同，我觉得因为在改进D-H法中，关节i的轴线是ziz_izi，而在标准D-H法中，关节i的轴线是zi−1z_{i-1}zi−1，因此这个方法和矢量积法本质是一样的。

附上Matlab代码：

% 求manipulator jacobian or jacobian for short，不需要进行微分计算
% 标准D-H法建模
function J = My_Jacobian(T)n = length(T); % number of degrees of freedom
k = [0; 0; 1];% preallocationg the Jacobian matrix
Jv = zeros(3, n);
Jw = zeros(3, n);for i = 1: 6if i == 1R = eye(3);p_im = T{n}(1: 3, 4);elseR = T{i-1}(1: 3, 1: 3);p_im = T{n}(1: 3, 4) - T{i-1}(1: 3, 4);endzi_1 = R * k;Jv(:, i) = cross(zi_1, p_im);Jw(:, i) = zi_1;
end% Assemblying the Jacobian matrix
J = [Jv; Jw];end% 标准D-H法求正运动学
% p_xyz:    the position vector of the last joint
% T:        homogeneous transformation
function [p_xyz, T] = My_Forward_Kinematics(theta, d, a, alp)
%Theta, d, a, Alpha are lists (1 row matrix) with n inputs
%       Where n is the number of links in the arm.
%
% theta:     Joint Angles (theta)
% d:     Link offset
% a:     Link Length
% alp:   Link Twist (alpha)
% n:     Robot degrees of freedom
n = length(theta);
% deg2rad可能需要可能不需要，视情况而定
th = theta;T1_n = eye(4);
T{n} = zeros(4);for i = 1 : nTi_n= [cos(th(i)),   -sin(th(i))*cos(alp(i)),   sin(th(i))*sin(alp(i)),    a(i)*cos(th(i));sin(th(i)),    cos(th(i))*cos(alp(i)),  -cos(th(i))*sin(alp(i)),    a(i)*sin(th(i));0,               sin(alp(i)),              cos(alp(i)),               d(i);0,                         0,                        0,                 1];T1_n = T1_n * Ti_n;T{i} = T1_n;
endp_xyz = T{n}(1: 3, 4)';
end

微分变换法——标准D-H法

简单来说，微分变换法，不管是基于标准DH法来求还是基于改进DH法来求，两种计算方法的区别最终还是归结于两种建模方式的不同。基于改进DH法，从前面程序J_T(:, 6) = [0; 0; 0; 0; 0; 1];可以看出微分相对于末端旋转本地坐标系就是自己Z轴且没有平移就是000 001，前面五个关节的Ji分别对应T16到T56；而基于标准DH法，关节坐标系从{0}到{5}，{6}直接是末端坐标系，因此六个关节的Ji分别对应T06到T56。

附上Matlab代码：

% 微分变换法
% 标准dh法
% 该方法求出的雅克比矩阵是相对于末端坐标系{n}的，需要左乘矩阵转变为相对于基系
function J = weifen_Jacobian_SDH(T)n = length(T);
k = [0 0 1];% preallocationg the Jacobian matrix
Jv = zeros(3, n);
Jw = zeros(3, n);for i = 1: 6if i == 1T_i_n = T{n};elseT_i_n = inv(T{i-1}) * T{n};endJ_T_ix = k * cross(T_i_n(1: 3, 4), T_i_n(1: 3, 1));J_T_iy = k * cross(T_i_n(1: 3, 4), T_i_n(1: 3, 2));J_T_iz = k * cross(T_i_n(1: 3, 4), T_i_n(1: 3, 3));Jv(:, i) = [J_T_ix; J_T_iy; J_T_iz];Jw(:, i) = [T_i_n(3, 1); T_i_n(3, 2); T_i_n(3, 3)];
end
J_T = [Jv; Jw];R_0_n = T{n}(1: 3, 1: 3);
TT = zeros(6, 6);
TT(1: 3, 1: 3) = R_0_n;
TT(4: 6, 4: 6) = R_0_n;J = TT * J_T;
end