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

xuuyann

已于 2022-09-20 19:19:13 修改

阅读量1.1w

点赞数 34

分类专栏：机器人学文章标签：机器人学 Matlab 雅可比矩阵

于 2019-11-29 22:21:31 首次发布

本文链接：https://blog.csdn.net/qq_26565435/article/details/103317240

版权

机器人学专栏收录该内容

35 篇文章 286 订阅

订阅专栏

文章目录

写在前面

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

关于雅可比矩阵，其重要性不言而喻。说实话，踩了挺多坑的，看了许多教材和文献，都没有统一求法（也可能是我道行较浅学艺不精没看出来= = ），主要是没有统一建模的方法，导致标准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而言，其在末端产生的角速度为：
$\omega=\sum_{i=1}^{n}\left(\bar{\epsilon}_{i} Z_{i}\right) \dot{q}_{i}$
其中， $z_i$ 是坐标系{i}的z轴单位向量在基座标系{0}中的表示。

关节i在末端产生的线速度为：
$v=\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}$
式中， $P^0_{i n}$ 表示末端坐标原点相对坐标系{i}的位置矢量在基座标系{0}中的表示，即 $P^0_{i n} = {^{0}_{i}}R * p_{in}$

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

所以，对于旋转关节i，雅可比矩阵第i列为
$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]$
对于移动关节i，有
$J_{i}=\left[\begin{array}{c}{z_{i}} \\ {0}\end{array}\right]$
附上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: 6
   R = 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;
end

J = [Jv; Jw];

end


function [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 : n
    Ti_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;
end

p_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: 5
    T_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，有
$J_{i}=\left[\begin{array}{c}{z_{i-1} \times\left(p_{n}-p_{i-1}\right)} \\ {z_{i-1}}\end{array}\right]$
对于移动关节i，有
$J_{i}=\left[\begin{array}{c}{z_{i-1}} \\ {0}\end{array}\right]$
其中， $z_{i-1}$ 表示坐标系{i-1}的z轴单位向量在座标系{0}中的表示； $p_n - p_{i-1}$ 表示末端坐标系{n}原点相对于坐标系{i-1}原点的位置矢量， $p_n$ 由 $_{i}^{0} T$ 中的位置矢量给出， $p_{i-1}$ 由 $_{i-1}^{0} T$ 中的位置矢量给出。

看着和上面的矢量积法非常类似，就是下标和位置矢量不一样。比较一下两者的不同，我觉得因为在改进D-H法中，关节i的轴线是 $z_i$ ，而在标准D-H法中，关节i的轴线是 $z_{i-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: 6
   if i == 1
       R = eye(3);
       p_im = T{n}(1: 3, 4);
   else
       R = T{i-1}(1: 3, 1: 3);
       p_im = T{n}(1: 3, 4) - T{i-1}(1: 3, 4);
   end
   
   zi_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 : n
    Ti_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;
end

p_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: 6
    if i == 1
        T_i_n = T{n};
    else
        T_i_n = inv(T{i-1}) * T{n};
    end
    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];

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

xuuyann

关注

34
点赞
踩
164

收藏

觉得还不错? 一键收藏
打赏
5
评论
机器人学回炉重造（2-2）：雅可比矩阵的求法——矢量积法、微分变换法、Manipulator Jacobian（Jacobian for short）

写在前面断更了俩月，找工作加毕业论文，心塞心塞，忙里偷闲还是得总结学习一下~关于雅克比矩阵，其重要性不言而喻。说实话，踩了挺多坑的，看了许多教材和文献，都没有统一求法（也可能是我道行较浅学艺不精没看出来= = ），主要是没有统一建模的方法，导致标准D-H法有自己的雅克比矩阵求法，改进D-H法也有自己相应的求法。这里只是做个简单的总结分类，没有讨论两种模型对应方法之间的互换关系，本质上都是一样的...
复制链接

扫一扫