工业机器人运动学与Matlab正逆解算法学习笔记（用心总结一文全会）（四）——雅可比矩阵_staubli机器人正逆向运动学实例验证matlab

---

CSDN提示我字数太多，一篇发不下，只好拆分开。。。

---

❤ 2023.8.2 ❤

建立DH模型

→→→【工业机器人运动学与Matlab正逆解算法学习笔记（用心总结一文全会）（一）——DH模型与正运动学】

---

机器人正运动学

→→→【工业机器人运动学与Matlab正逆解算法学习笔记（用心总结一文全会）（一）——DH模型与正运动学】

---

机器人逆运动学

→→→【工业机器人运动学与Matlab正逆解算法学习笔记（用心总结一文全会）（二）——逆运动学P1】

→→→【工业机器人运动学与Matlab正逆解算法学习笔记（用心总结一文全会）（三）——逆运动学P2】

---

机器人雅可比矩阵

△ 机器人速度雅可比矩阵

关于雅可比矩阵的内容，说实话教材看得我一头雾水，感觉自己可能有阅读障碍综合征。。。。然后我在B站找到了这个教程，感觉很简洁实用
→→→【4-1机器人速度雅可比矩阵】

以下是我的学习笔记

○ 雅可比矩阵相关概念

• 雅可比矩阵概念

雅可比矩阵（Jacobian）是机器人操作空间速度与关节空间速度之间的线性映射关系，同时也可以表示两空间之间力的传递关系。

• 机器人末端位姿的表示

齐次变换矩阵

• 角度设定法

采用相对参考坐标系或相对运动坐标系作三次连续转动来规定姿态的方法

绕坐标系的转角正常来说就是欧拉角，是要考虑先后顺序的，但是这里的转角认为是与坐标轴的夹角

故用角度设定法表示机器人末端位姿时，机器人的运动学方程可写成

---

○ 以二连杆平面机器人举例说明雅可比矩阵

如图二连杆平面关节机器人

机器人末端位置可以表示为

对改式求微分得

根据前面的式子可得

于是

---

△ 机器人雅克比矩阵与速度分析

1. 每一列表示其他关节不动而某一关节运动的端点速度
2. 前三行代表手部线速度与关节速度传递比；后三行代表手部角速度与关节速度传递比
3. 雅克比矩阵J(q)是函数的一阶偏导数以一定方式排列成的矩阵，不仅可以用来表示操作空间速度与关节空间速度之间的线性映射关系，同时也用来表示两空间之间力的传递关系。

---

△ 雅克比矩阵的奇异性

在求机器人关节速度时，需要求雅可比矩阵的逆矩阵

在一些位姿下，雅可比矩阵会出现行列式得0的情况，这是雅可比矩阵的逆矩阵就会趋近无穷

在计算机中由于数据存储方式，一般不会等于0，但是会出现一个很小的数，也认为其等于0.

---

○ 机器人的奇异位形（奇异形位、奇异点）

1. 腕部奇异性：机器人4、6轴共线（5轴角度为0），造成四轴和6周大幅运动
2. 肘部（边界）奇异性：机器人到达最大工作范围，造成肘部无法活动，3轴振动剧烈
3. 肩部奇异性：1轴与6轴共线，或者1轴与4、5、6杆的坐标系原点共线，造成肘部大幅快速运动，容易造成危险

【待插入视频：工业机器人奇异性1】

→→→【工业机器人3种奇异点详解，没想到是先天缺陷，无法克服】

【待插入视频：工业机器人奇异性2】

→→→【【涨知识】机器人的奇异性是什么？】

---

△ 雅可比矩阵的建立

---

※ 相邻连杆间的速度关系

• 移动关节

• 旋转关节
  

这部分我木有听懂，原视频链接在这里。。。
→→→【4-5机器人雅克比矩阵的建立】

---

○ 矢量积法

· 矢量积法概念

矢量积法的概念

大概听明白了是什么意思（我自己的总结）

• 首先末端速度是各关节运动产生的速度的叠加
• 先假设某一个关节动，其他关节都不动，求出来这个关节的雅克比矩阵，也就是完整雅克比矩阵中的一列。
• 计算的重点就是求出这个旋转的半径，半径就是这个转动关节的原点到末端点的连线
• 每个关节都如此求解，最后把结果写在一起就是整体的雅克比矩阵。

【！注意！】
矢量积法求得的雅可比矩阵，计算后得到的速度向量是相对于基坐标系（世界坐标系）的

---

· 矢量积法求机器人雅可比矩阵示例

关于矢量积法求雅可比矩阵，之前的教程里没给出示例，这里还有一个视频可以参考
→→→【机器人学 l 3.6 速度雅可比矩阵的矢量积法】

• 首先视频讲了一下矢量积法的概念

---

• 矢量积法求平面2R机器人雅可比矩阵

---

• 矢量积法求scara机器人的雅可比矩阵

【！！！】这里要注意一个问题，我也是在后面自己用矢量积法求6轴机器人的时候才注意到的。

• 这里的坐标轴是固定的基坐标系或者说世界坐标系，而不是建立在各连杆上的连杆坐标系
• 因为各个连杆的运动始终参考基坐标系，所以不需要将各连杆坐标系转换为参考基坐标系的步骤
• 因此这里可以直接计算r，也就是直接把自身关节的影响带进去了
• 如果是正常情况下要先计算以连杆坐标系为基准的向量p，此时不考虑自身关节运动的影响；然后通过旋转矩阵将向量转换为相对基坐标系表示，这个时候才会自身关节的运动。
• over

---

○ 微分变换法

· 坐标系的微分运动

我的理解：
听的不是很明白，感觉视频里的坐标系画的有问题，越看越糊涂，但是大概意思是

• 首先，坐标系的微分运动指的是一个坐标系相对于自己的参考坐标系做的运动的微分，包括微分移动和微分转动
• 然后，运动包括旋转运动和平移运动
• 其次，微分转动仅受旋转运动影响，其值为角速度与变换矩阵的n o a向量的点乘
  （【？？】关于为什么noa对应xyz方向，我不是很理解，以后弄明白了我再来补充，现在就当是定义了吧。。。）
• 再次，微分平移同时收到旋转运动和平移运动的影响，其值为对应方向旋转运动的线速度（也就是叉乘）加上对应方向平移运动的分量（也就是平移速度与n o a的点乘）

---

· 机器人的微分运动

我的理解：

• 这个不用我再理解了，就是机器人的关节速度通过雅可比矩阵转换成末端的线速度与加速度的形式
• 雅可比矩阵每一列都表示对应的关节对机器人末端线速度和角速度的影响

---

· 微分变换法概念

我的理解：

• 关于为什么这里雅可比矩阵可以写成这样，视频里up说这个就是前面这个公式的矩阵形式
  
• 但是我觉得他说的 【不对】，如果说这个是6关节机器人的雅可比矩阵，那么后面就没有必要再继续求了，直接就能根据变换矩阵写出对应的雅可比矩阵。而且雅可比矩阵每一列都是对应关节的映射关系，这个矩阵显然不符合。
• 结合接下来的讲解（也就是下面这个图），我大概明白了，这个矩阵是对单关节（或者说关节对应的连杆坐标系）相对上一个关节（或者说上一个连杆坐标系）的雅可比矩阵求解方法，后面乘的向量也不是各关节的速度，而是单关节在xyz三个方向的平移和旋转速度分量。
  【当然也有可能不是相邻的上一个关节，这要看所带入的其次变换矩阵的】
• 因为一个关节往往只对应一个方向的平移或旋转，所以才有如下的结果：

【！注意！】
微分变换法求得的雅可比矩阵，计算后得到的速度向量是相对于末端（工具坐标系）的！

---

※ 关于相对于末端（工具）坐标系的雅可比矩阵

我想了半天没想明白是什么意思，然后我问了问ChatGPT

ChatGPT yyds！

---

· 微分变换法求2连杆机械臂雅可比矩阵示例

【？】这DH坐标系建的不对吧？

我的理解：

• 变换矩阵的连乘，目的是构建末端与所求关节的位置关系。
• 只需考虑当前关节和末端的位置关系即可，不需要考虑和前面的坐标系的关系，最后得到的是末端相对当前关节（或者说当前关节所在的连杆坐标系）的速度关系

---

○ 矢量积法与微分变换法的转换关系

这个问题我找了很多资料都没有提到。

• 首先 矢量积法和微分变换法求得的雅可比矩阵是不相同的

微分变换法自不用说，求解过程很直观，理解起来也容易，他的结果是相对基坐标系（世界坐标系）的映射，也就是我们想要得到的结果

而微分变换法得到的雅可比矩阵是相对于机器人末端坐标系（工具坐标系）的映射，蔡自兴《机器人学》里面有对应的表示

也就是说微分变换法的结果不能直接拿来用，需要将其转换为在基坐标系下的结果。

• 矢量积法与微分变换法的转换关系

我在一个讲雅可比的PPT中看到了他们之间的转换方法

其中${}_n^0{R}^T$表示基坐标到末端的齐次变换矩阵的旋转矩阵部分。

【！！！】至于为什么，我也不知道。。。。。希望有大佬来补充

• 另外关于两种方法以及其他方法的对比

这里有一篇文章有更多的讲解，而且文章附上了对应的matlab代码可以参考

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

---

○ matlab机器人工具箱法

使用机器人工具箱求雅可比，就。。。挺简单的。。。

虽然没有前面的内容，也可以直接用机器人工具箱求雅可比，但是所谓“知其然，又要知其所以然”嘛~

方法可以参考

→→→【matlab机器人工具箱学习笔记——瞬态运动学】

或者我自己的

→→→【Matlab机器人工具箱（Robotics Toolbox）学习笔记】

· jacob0()

• SerialLink.jacob0()
  求出对应某个位姿下世界坐标系中的雅克比矩阵

对应矢量积法的结果

---

· jacobn()

• SerialLink.jacobn()
• 求出对应某个位姿下工具坐标系中的雅克比矩阵

对应微分变换法的结果

---

· 雅可比矩阵的变换

---

· matlab代码

• 代码

%ZK-500机器人雅可比矩阵计算——机器人工具箱

clc
clear all;

%% 输入关节角
%这里的输入为角度
theta1=10;
theta2=10;
theta3=10;
theta4=10;
theta5=10;
theta6=10;

%% ZK500机器人MDH参数（m）
% theta(z)  d(z)     a(x)     alpha(x)  
%[  0       d1       0        0       ]
%[  0+pi/2  0        a1       pi/2    ]
%[  0       0        a2       0       ]
%[  0       d4       a3       pi/2    ]
%[  0       0        0       -pi/2    ]
%[  0       0        0        pi/2    ]
d1=1.05;
a1=0.5;
a2=1.3;
a3=0.15;
d4=1.2;
dt=0.39;

%将角度转化为弧度
q(1)=theta1/180*pi;
q(2)=theta2/180*pi+pi/2;    % 关节2补偿90°
q(3)=theta3/180*pi;
q(4)=theta4/180*pi;
q(5)=theta5/180*pi;
q(6)=theta6/180*pi;

%----------
%机器人工具箱建立机器人模型
%        theta(z)   d(z)     a(x)     alpha(x)  
L1=Link([  0       d1       0        0       ],'modified');
L2=Link([  0       0        a1       pi/2    ],'modified');
L3=Link([  0       0        a2       0       ],'modified');
L4=Link([  0       d4       a3       pi/2    ],'modified');
L5=Link([  0       0        0       -pi/2    ],'modified');
L6=Link([  0       0        0        pi/2    ],'modified');
Robot_ZK500=SerialLink([L1,L2,L3,L4,L5,L6],'name','ZK-500');


%----------
%输出结果
disp('世界坐标系中的雅可比矩阵：');
J0=Robot_ZK500.jacob0(q)

disp('工具坐标系中的雅可比矩阵：');
Jn=Robot_ZK500.jacobn(q)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57

• 结果

世界坐标系中的雅可比矩阵：

J0 =

   -0.2345   -1.8038   -0.5430         0         0         0
    1.3301   -0.3181   -0.0957         0         0         0
         0    0.8506    1.0763         0         0         0
    0.0000    0.1736    0.1736    0.9254    0.2295    0.8590
    0.0000   -0.9848   -0.9848    0.1632   -0.9595    0.1208
    1.0000    0.0000    0.0000    0.3420   -0.1632    0.4975

工具坐标系中的雅可比矩阵：
警告: Use jacobe instead of jacobn 
> 位置：SerialLink/jacobn (第 981 行)
位置: Jacob_RTB (第 56 行) 

Jn =

   -0.4584    1.5633    1.1356         0         0         0
   -1.2698   -0.5271   -0.4118         0         0         0
   -0.0407   -1.1647    0.0575         0         0         0
    0.8107    0.3394    0.3394   -0.1710    0.1736         0
   -0.3086    0.9402    0.9402    0.0302    0.9848         0
    0.4975    0.0302    0.0302    0.9848    0.0000    1.0000
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

这个“Use jacobe instead of jacobn ”是什么个情况？
大概是新的函数名吧，e可能是end什么的

---

△ ZK-500六轴工业机器人的雅可比矩阵

○ 矢量积法

机器人为6R构型，所以雅可比矩阵的形式为
J = [ J 1 J 2 … J 6 ] = [ J l 1 J l 2 … J l 6 J a 1 J a 2 … J a 6 ] J=\left[

$$\begin{matrix}J_1&J_2&\ldots&J_6\\\end{matrix}$$ \right]=\left[ $$\begin{matrix}J_{l1}&J_{l2}&\ldots&J_{l6}\\J_{a1}&J_{a2}&\ldots&J_{a6}\\\end{matrix}$$ \right] J=[J1​​J2​​…​J6​​]=[Jl1​Ja1​​Jl2​Ja2​​……​Jl6​Ja6​​]

根据矢量积法推导出的公式

J i = [ z i × r i z i ] = [ z i ×   i p n 0 z i ] z i = i 0 R ⋅ [ 0 0 1 ] T   i p n 0 = i 0 R ⋅   i p n J_i=\left[

$$\begin{matrix}z_i\times r_i\\z_i\\\end{matrix}$$ \right]=\left[ $$\begin{matrix}z_i\times{ {_\ ^i}p}_n^0\\z_i\\\end{matrix}$$ \right]\\ z_i={_i^0}R\cdot\left[ $$\begin{matrix}0&0&1\\\end{matrix}$$ \right]^\mathrm{T}\\ {{_\ ^i}p}_n^0={_i^0}R\cdot{{_\ ^i}p}_n Ji​=[zi​×ri​zi​​]=[zi​× i​pn0​zi​​]zi​=i0​R⋅[0​0​1​]T i​pn0​=i0​R⋅ i​pn​

字母的含义见 矢量积法的概念

---

· 求${}_i^{0}T$

  1 0 T = [ c 1 − s 1 0 0 s 1 c 1 0 0 0 0 1 d 1 0 0 0 1 ] \ {_1^0}T=\left[

$$\begin{matrix}c_1&-s_1&0&0\\s_1&c_1&0&0\\0&0&1&d_1\\0&0&0&1\\\end{matrix}$$ \right]  10​T= ​c1​s1​00​−s1​c1​00​0010​00d1​1​ ​

  2 0 T = [ c 1 c 2 − c 1 s 2 s 1 a 1 c 1 c 2 s 1 − s 1 s 2 − c 1 a 1 s 1 s 2 c 2 0 d 1 0 0 0 1 ] \ {_2^0}T=\left[

$$\begin{matrix}c_1c_2&-c_1s_2&s_1&a_1c_1\\c_2s_1&-s_1s_2&-c_1&a_1s_1\\s_2&c_2&0&d_1\\0&0&0&1\\\end{matrix}$$ \right]  20​T= ​c1​c2​c2​s1​s2​0​−c1​s2​−s1​s2​c2​0​s1​−c1​00​a1​c1​a1​s1​d1​1​ ​

3 0 T = [ c 1 c 23 − c 1 s 23 s 1 a 1 c 1 + a 2 c 1 c 2 s 1 c 23 − s 1 s 23 − c 1 a 1 s 1 + a 2 c 2 s 1 s 23 c 23 0 d 1 + a 2 s 2 0 0 0 1 ] {_3^0}T=\left[

$$\begin{matrix}c_1c_{23}&-c_1s_{23}&s_1&a_1c_1+a_2c_1c_2\\s_1c_{23}&-s_1s_{23}&-c_1&a_1s_1+a_2c_2s_1\\s_{23}&c_{23}&0&d_1+a_2s_2\\0&0&0&1\\\end{matrix}$$ \right] 30​T= ​c1​c23​s1​c23​s23​0​−c1​s23​−s1​s23​c23​0​s1​−c1​00​a1​c1​+a2​c1​c2​a1​s1​+a2​c2​s1​d1​+a2​s2​1​ ​

4 0 T = [ s 1 s 4 + c 1 c 4 c 23 s 1 c 4 − c 1 s 4 c 23 c 1 s 23 a 1 c 1 + a 2 c 1 c 2 + a 3 c 1 c 23 + d 4 c 1 s 23 s 1 c 4 c 23 − c 1 s 4 − c 1 c 4 − s 1 s 4 c 23 s 1 s 23 a 1 s 1 + a 2 s 1 c 2 + a 3 s 1 c 23 + d 4 s 1 s 23 c 4 s 23 − s 4 s 23 − c 23 d 1 − d 4 c 23 + a 2 s 2 + a 3 s 23 0 0 0 1 ] {_4^0}T=\left[

$$\begin{matrix}s_1s_4+c_1c_4c_{23}&s_1c_4-c_1s_4c_{23}&c_1s_{23}&a_1c_1+a_2c_1c_2+a_3c_1c_{23}+d_4c_1s_{23}\\s_1c_4c_{23}-c_1s_4&-c_1c_4-s_1s_4c_{23}&s_1s_{23}&a_1s_1+a_2s_1c_2+a_3s_1c_{23}+d_4s_1s_{23}\\c_4s_{23}&-s_4s_{23}&-c_{23}&d_1-d_4c_{23}+a_2s_2+a_3s_{23}\\0&0&0&1\\\end{matrix}$$ \right] 40​T= ​s1​s4​+c1​c4​c23​s1​c4​c23​−c1​s4​c4​s23​0​s1​c4​−c1​s4​c23​−c1​c4​−s1​s4​c23​−s4​s23​0​c1​s23​s1​s23​−c23​0​a1​c1​+a2​c1​c2​+a3​c1​c23​+d4​c1​s23​a1​s1​+a2​s1​c2​+a3​s1​c23​+d4​s1​s23​d1​−d4​c23​+a2​s2​+a3​s23​1​ ​

5 0 T = [ r 11 5 r 12 5 r 13 5 p x 5 r 21 5 r 22 5 r 23 5 p y 5 r 31 5 r 32 5 r 33 5 p z 5 0 0 0 1 ] {_5^0}T=\left[

$$\begin{matrix}r_{11}^5&r_{12}^5&r_{13}^5&p_x^5\\r_{21}^5&r_{22}^5&r_{23}^5&p_y^5\\r_{31}^5&r_{32}^5&r_{33}^5&p_z^5\\0&0&0&1\\\end{matrix}$$ \right] 50​T= ​r115​r215​r315​0​r125​r225​r325​0​r135​r235​r335​0​px5​py5​pz5​1​ ​

其中

r 11 5 = c 5 ( s 1 s 4 + c 1 c 4 c 23 ) − c 1 s 5 s 23 r 12 5 = − s 5 ( s 1 s 4 + c 1 c 4 c 23 ) − c 1 c 5 s 23 r 13 5 = s 1 c 4 − c 1 s 4 c 23 r 21 5 = − c 5 ( c 1 s 4 − s 1 c 4 c 23 ) − s 1 s 5 s 23 r 22 5 = s 5 ( c 1 s 4 − s 1 c 4 c 23 ) − s 1 c 5 s 23 r 23 5 = − c 1 c 4 − s 1 s 4 c 23 r 31 5 = s 5 c 23 + c 4 c 5 s 23 r 32 5 = c 5 c 23 − c 4 s 5 s 23 r 33 5 = − s 4 s 23 p x 5 = a 1 c 1 + a 2 c 1 c 2 + a 3 c 1 c 23 + d 4 c 1 s 23 p y 5 = a 1 s 1 + a 2 s 1 c 2 + a 3 s 1 c 23 + d 4 s 1 s 23 p z 5 = d 1 − d 4 c 23 + a 2 s 2 + a 3 s 23

$$\begin{equation} \begin{aligned} r_{11}^5&=c_5(s_1s_4+c_1c_4c_{23})-c_1s_5s_{23}\\ r_{12}^5&=-{s_5(s}_1s_4+c_1c_4c_{23})-c_1c_5s_{23}\\ r_{13}^5&=s_1c_4-c_1s_4c_{23}\\ r_{21}^5&=-c_5\left(c_1s_4-s_1c_4c_{23}\right)-s_1s_5s_{23}\\ r_{22}^5&=s_5\left(c_1s_4-s_1c_4c_{23}\right)-s_1c_5s_{23}\\ r_{23}^5&=-c_1c_4-s_1s_4c_{23}\\ r_{31}^5&=s_5c_{23}+c_4c_5s_{23}\\ r_{32}^5&=c_5c_{23}-c_4s_5s_{23}\\ r_{33}^5&=-s_4s_{23}\\ p_x^5&=a_1c_1+a_2c_1c_2+a_3c_1c_{23}+d_4c_1s_{23}\\ p_y^5&=a_1s_1+a_2s_1c_2+a_3s_1c_{23}+d_4s_1s_{23}\\ p_z^5&=d_1-d_4c_{23}+a_2s_2+a_3s_{23}\\ \end{aligned} \end{equation}$$ r115​r125​r135​r215​r225​r235​r315​r325​r335​px5​py5​pz5​​=c5​(s1​s4​+c1​c4​c23​)−c1​s5​s23​=−s5​(s1​s4​+c1​c4​c23​)−c1​c5​s23​=s1​c4​−c1​s4​c23​=−c5​(c1​s4​−s1​c4​c23​)−s1​s5​s23​=s5​(c1​s4​−s1​c4​c23​)−s1​c5​s23​=−c1​c4​−s1​s4​c23​=s5​c23​+c4​c5​s23​=c5​c23​−c4​s5​s23​=−s4​s23​=a1​c1​+a2​c1​c2​+a3​c1​c23​+d4​c1​s23​=a1​s1​+a2​s1​c2​+a3​s1​c23​+d4​s1​s23​=d1​−d4​c23​+a2​s2​+a3​s23​​​​

6 0 T = [ r 11 r 12 r 13 p x r 21 r 22 r 23 p y r 31 r 32 r 33 p z 0 0 0 1 ] {_6^0}T=\left[

$$\begin{matrix}r_{11}&r_{12}&r_{13}&p_x\\r_{21}&r_{22}&r_{23}&p_y\\r_{31}&r_{32}&r_{33}&p_z\\0&0&0&1\\\end{matrix}$$ \right] 60​T= ​r11​r21​r31​0​r12​r22​r32​0​r13​r23​r33​0​px​py​pz​1​ ​

其中：

---

· 求$z_i$

z 1 = [ 0 0 1 ] T z 2 = [ s 1 − c 1 0 ] T z 3 = [ s 1 − c 1 0 ] T z 4 = [ c 1 s 23 s 1 s 23 − c 23 ] T z 5 = [ s 1 c 4 − c 1 s 4 c 23 − c 1 c 4 − s 1 s 4 c 23 − s 4 s 23 ] z 6 = [ c 5 c 1 s 23 + s 5 ( s 1 s 4 + c 4 c 1 c 23 ) c 5 s 1 s 23 − s 5 ( c 1 s 4 − c 4 s 1 c 23 ) c 4 s 5 s 23 − c 5 c 23 ]

$$\begin{equation} \begin{aligned} z_1&=\left[\begin{matrix}0&0&1\\\end{matrix}\right]^\mathrm{T}\\ z_2&=\left[\begin{matrix}s_1&-c_1&0\\\end{matrix}\right]^\mathrm{T}\\ z_3&=\left[\begin{matrix}s_1&-c_1&0\\\end{matrix}\right]^\mathrm{T}\\ z_4&=\left[\begin{matrix}c_1s_{23}&s_1s_{23}&-c_{23}\\\end{matrix}\right]^\mathrm{T}\\ z_5&=\left[\begin{matrix}s_1c_4-c_1s_4c_{23}\\-c_1c_4-s_1s_4c_{23}\\-s_4s_{23}\\\end{matrix}\right]\\ z_6&=\left[\begin{matrix}c_5c_1s_{23}+s_5\left(s_1s_4+c_4c_1c_{23}\right)\\c_5s_1s_{23}-s_5\left(c_1s_4-c_4s_1c_{23}\right)\\c_4s_5s_{23}-c_5c_{23}\\\end{matrix}\right]\\ \end{aligned} \end{equation}$$ z1​z2​z3​z4​z5​z6​​=[0​0​1​]T=[s1​​−c1​​0​]T=[s1​​−c1​​0​]T=[c1​s23​​s1​s23​​−c23​​]T= ​s1​c4​−c1​s4​c23​−c1​c4​−s1​s4​c23​−s4​s23​​ ​= ​c5​c1​s23​+s5​(s1​s4​+c4​c1​c23​)c5​s1​s23​−s5​(c1​s4​−c4​s1​c23​)c4​s5​s23​−c5​c23​​ ​​​​

---

· 求${}_{}^i{p}_6$

这个位置矢量是根据机器人的几何构型和DH模型所建立的坐标系来确定的，算是这个方法唯一不能直接从其次变换矩阵里面推导出来的东西
【2023.12.6 后来看了大佬的程序，知道这个也是可以从齐次变换矩阵里面提取出来的，毕竟机器人的构型和齐次变换矩阵其实是同一个东西，具体方法等以后我想起来了再整理。。。（stiff_orient_calc.m）】

  1 p 6 = [ a 1 + a 2 c 2 + a 3 c 23 + d 4 s 23 0 a 2 s 2 + a 3 s 23 − d 4 c 23 ]   2 p 6 = [ a 2 + a 3 c 3 + d 4 s 3 a 3 s 3 − d 4 c 3 0 ]   3 p 6 = [ a 3 − d 4 0 ] T   4 p 6 = [ 0 0 0 ] T   5 p 6 = [ 0 0 0 ] T   6 p 6 = [ 0 0 0 ] T

$$\begin{equation} \begin{aligned} { {_\ ^1}p}_6&=\left[\begin{matrix}a_1+a_2c_2+a_3c_{23}+d_4s_{23}\\0\\a_2s_2+a_3s_{23}-d_4c_{23}\\\end{matrix}\right]\\ { {_\ ^2}p}_6&=\left[\begin{matrix}a_2+a_3c_3+d_4s_3\\a_3s_3-d_4c_3\\0\\\end{matrix}\right]\\ { {_\ ^3}p}_6&=\left[\begin{matrix}a_3&-d_4&0\\\end{matrix}\right]^\mathrm{T}\\ { {_\ ^4}p}_6&=\left[\begin{matrix}0&0&0\\\end{matrix}\right]^\mathrm{T}\\ { {_\ ^5}p}_6&=\left[\begin{matrix}0&0&0\\\end{matrix}\right]^\mathrm{T}\\ { {_\ ^6}p}_6&=\left[\begin{matrix}0&0&0\\\end{matrix}\right]^\mathrm{T}\\ \end{aligned} \end{equation}$$  1​p6​ 2​p6​ 3​p6​ 4​p6​ 5​p6​ 6​p6​​= ​a1​+a2​c2​+a3​c23​+d4​s23​0a2​s2​+a3​s23​−d4​c23​​ ​= ​a2​+a3​c3​+d4​s3​a3​s3​−d4​c3​0​ ​=[a3​​−d4​​0​]T=[0​0​0​]T=[0​0​0​]T=[0​0​0​]T​​​

---

· 求${}_{}^i{p}_6^0$

  1 p 6 0 = [ c 1 ( a 1 + a 2 c 2 + a 3 c 23 + d 4 s 23 ) s 1 ( a 1 + a 2 c 2 + a 3 c 23 + d 4 s 23 ) a 2 s 2 + a 3 s 23 − d 4 c 23 ]   2 p 6 0 = [ c 1 c 2 ( a 2 + a 3 c 3 + d 4 s 3 ) − c 1 s 2 ( a 3 s 3 − d 4 c 3 ) s 1 c 2 ( a 2 + a 3 c 3 + d 4 s 3 ) − s 1 s 2 ( a 3 s 3 − d 4 c 3 ) s 2 ( a 2 + a 3 c 3 + d 4 s 3 ) + c 2 ( a 3 s 3 − d 4 c 3 ) ]   3 p 6 0 = [ a 3 c 1 c 23 + d 4 c 1 s 23 a 3 s 1 c 23 + d 4 s 1 s 23 a 3 s 23 − c 23 d 4 ]   4 p 6 0 = [ 0 0 0 ] T   5 p 6 0 = [ 0 0 0 ] T   6 p 6 0 = [ 0 0 0 ] T

$$\begin{equation} \begin{aligned} { {_\ ^1}p}_6^0&=\left[\begin{matrix}c_1\left(a_1+a_2c_2+a_3c_{23}+d_4s_{23}\right)\\s_1\left(a_1+a_2c_2+a_3c_{23}+d_4s_{23}\right)\\a_2s_2+a_3s_{23}-d_4c_{23}\\\end{matrix}\right]\\ { {_\ ^2}p}_6^0&=\left[\begin{matrix}c_1c_2\left(a_2+a_3c_3+d_4s_3\right)-c_1s_2\left(a_3s_3-d_4c_3\right)\\s_1c_2\left(a_2+a_3c_3+d_4s_3\right)-s_1s_2\left(a_3s_3-d_4c_3\right)\\s_2\left(a_2+a_3c_3+d_4s_3\right)+c_2\left(a_3s_3-d_4c_3\right)\\\end{matrix}\right]\\ { {_\ ^3}p}_6^0&=\left[\begin{matrix}a_3c_1c_{23}+d_4c_1s_{23}\\a_3s_1c_{23}+d_4s_1s_{23}\\a_3s_{23}-c_{23}d_4\\\end{matrix}\right]\\ { {_\ ^4}p}_6^0&=\left[\begin{matrix}0&0&0\\\end{matrix}\right]^\mathrm{T}\\ { {_\ ^5}p}_6^0&=\left[\begin{matrix}0&0&0\\\end{matrix}\right]^\mathrm{T}\\ { {_\ ^6}p}_6^0&=\left[\begin{matrix}0&0&0\\\end{matrix}\right]^\mathrm{T}\\ \end{aligned} \end{equation}$$  1​p60​ 2​p60​ 3​p60​ 4​p60​ 5​p60​ 6​p60​​= ​c1​(a1​+a2​c2​+a3​c23​+d4​s23​)s1​(a1​+a2​c2​+a3​c23​+d4​s23​)a2​s2​+a3​s23​−d4​c23​​ ​= ​c1​c2​(a2​+a3​c3​+d4​s3​)−c1​s2​(a3​s3​−d4​c3​)s1​c2​(a2​+a3​c3​+d4​s3​)−s1​s2​(a3​s3​−d4​c3​)s2​(a2​+a3​c3​+d4​s3​)+c2​(a3​s3​−d4​c3​)​ ​= ​a3​c1​c23​+d4​c1​s23​a3​s1​c23​+d4​s1​s23​a3​s23​−c23​d4​​ ​=[0​0​0​]T=[0​0​0​]T=[0​0​0​]T​​​

---

· 求$J_i$

J 1 = [ − s 1 ( a 1 + a 2 c 2 + a 3 c 23 + d 4 s 23 ) c 1 ( a 1 + a 2 c 2 + a 3 c 23 + d 4 s 23 ) 0 0 0 1 ] J_1=\left[

$$\begin{matrix}-s_1\left(a_1+a_2c_2+a_3c_{23}+d_4s_{23}\right)\\c_1\left(a_1+a_2c_2+a_3c_{23}+d_4s_{23}\right)\\0\\0\\0\\1\\\end{matrix}$$ \right] J1​= ​−s1​(a1​+a2​c2​+a3​c23​+d4​s23​)c1​(a1​+a2​c2​+a3​c23​+d4​s23​)0001​ ​

J 2 = [ − c 1 s 2 ( a 2 + a 3 c 3 + d 4 s 3 ) − c 1 c 2 ( a 3 s 3 − d 4 c 3 ) − s 1 s 2 ( a 2 + a 3 c 3 + d 4 s 3 ) − s 1 c 2 ( a 3 s 3 − d 4 c 3 ) s 1 ( s 1 c 2 ( a 2 + a 3 c 3 + d 4 s 3 ) − s 1 s 2 ( a 3 s 3 − d 4 c 3 ) ) + … c 1 ( c 1 c 2 ( a 2 + a 3 c 3 + d 4 s 3 ) − c 1 s 2 ( a 3 s 3 − d 4 c 3 ) ) s 1 − c 1 1 ] J_2=\left[

$$\begin{matrix}-c_1s_2\left(a_2+a_3c_3+d_4s_3\right)-c_1c_2\left(a_3s_3-d_4c_3\right)\\-s_1s_2\left(a_2+a_3c_3+d_4s_3\right)-s_1c_2\left(a_3s_3-d_4c_3\right)\\s_1\left(s_1c_2\left(a_2+a_3c_3+d_4s_3\right)-s_1s_2\left(a_3s_3-d_4c_3\right)\right)+\ldots\\c_1\left(c_1c_2\left(a_2+a_3c_3+d_4s_3\right)-c_1s_2\left(a_3s_3-d_4c_3\right)\right)\\s_1\\-c_1\\1\\\end{matrix}$$ \right] J2​= ​−c1​s2​(a2​+a3​c3​+d4​s3​)−c1​c2​(a3​s3​−d4​c3​)−s1​s2​(a2​+a3​c3​+d4​s3​)−s1​c2​(a3​s3​−d4​c3​)s1​(s1​c2​(a2​+a3​c3​+d4​s3​)−s1​s2​(a3​s3​−d4​c3​))+…c1​(c1​c2​(a2​+a3​c3​+d4​s3​)−c1​s2​(a3​s3​−d4​c3​))s1​−c1​1​ ​

J 3 = [ − c 1 ( a 3 s 23 − d 4 c 23 ) − s 1 ( a 3 s 23 − d 4 c 23 ) s 1 ( a 3 s 1 c 23 + d 4 s 1 s 23 ) + c 1 ( a 3 c 1 c 23 + d 4 c 1 s 23 ) s 1 − c 1 1 ] J_3=\left[

$$\begin{matrix}-c_1\left(a_3s_{23}-d_4c_{23}\right)\\-s_1\left(a_3s_{23}-d_4c_{23}\right)\\s_1\left(a_3s_1c_{23}+d_4s_1s_{23}\right)+c_1\left(a_3c_1c_{23}+d_4c_1s_{23}\right)\\s_1\\-c_1\\1\\\end{matrix}$$ \right] J3​= ​−c1​(a3​s23​−d4​c23​)−s1​(a3​s23​−d4​c23​)s1​(a3​s1​c23​+d4​s1​s23​)+c1​(a3​c1​c23​+d4​c1​s23​)s1​−c1​1​ ​

J 4 = [ 0 0 0 c 1 s 23 s 1 s 23 − c 23 ] J_4=\left[

$$\begin{matrix}0\\0\\0\\c_1s_{23}\\s_1s_{23}\\-c_{23}\\\end{matrix}$$ \right] J4​= ​000c1​s23​s1​s23​−c23​​ ​

J 5 = [ 0 0 0 s 1 c 4 − c 1 s 4 s 23 − c 1 c 4 − s 1 s 4 c 23 − s 4 s 23 ] J_5=\left[

$$\begin{matrix}0\\0\\0\\s_1c_4-c_1s_4s_{23}\\-c_1c_4-s_1s_4c_{23}\\-s_4s_{23}\\\end{matrix}$$ \right] J5​= ​000s1​c4​−c1​s4​s23​−c1​c4​−s1​s4​c23​−s4​s23​​ ​

J 6 = [ 0 0 0 s 5 ( s 1 s 4 + c 1 c 4 c 23 ) + c 1 c 5 s 23 s 1 c 5 s 23 − s 5 ( c 1 s 4 − s 1 c 4 c 23 ) c 4 s 5 s 23 − c 5 c 23 ] J_6=\left[

$$\begin{matrix}0\\0\\0\\s_5\left(s_1s_4+c_1c_4c_{23}\right)+c_1c_5s_{23}\\s_1c_5s_{23}-s_5\left(c_1s_4-s_1c_4c_{23}\right)\\c_4s_5s_{23}-c_5c_{23}\\\end{matrix}$$ \right] J6​= ​000s5​(s1​s4​+c1​c4​c23​)+c1​c5​s23​s1​c5​s23​−s5​(c1​s4​−s1​c4​c23​)c4​s5​s23​−c5​c23​​ ​

---

· 雅可比矩阵

J = [ J 1 J 2 J 3 J 4 J 5 J 6 ] J=\left[

$$\begin{matrix}J_1&J_2&J_3&J_4&J_5&J_6\\\end{matrix}$$ \right] J=[J1​​J2​​J3​​J4​​J5​​J6​​]

---

※ 矢量积法matlab程序验证

· 计算过程matlab符号变量表示

%ZK-500机器人矢量积法求雅可比矩阵过程

clc
clear all;

syms Q1 Q2 Q3 Q4 Q5 Q6  d1 d4 dt a1 a2 a3  nx ny nz ox oy oz ax ay az px py pz

%% 机器人各连杆间的齐次变换矩阵

% 根据MDH法建立的各连杆的齐次变换矩阵
% 过程见我的论文或CSDN
T_01 =[ cos(Q1),   -sin(Q1),    0,      0
        sin(Q1),    cos(Q1),    0,      0
        0,          0,          1,      d1
        0,          0,          0,      1];
T_12 =[ cos(Q2),   -sin(Q2),    0,      a1
        0,          0,         -1,      0
        sin(Q2),    cos(Q2),    0,      0
        0,          0,          0,      1];
T_23 =[ cos(Q3),   -sin(Q3),    0,      a2
        sin(Q3),    cos(Q3),    0,      0
        0,          0,          1,      0
        0,          0,          0,      1];
T_34 =[ cos(Q4),   -sin(Q4),    0,      a3
        0,          0,         -1,     -d4
        sin(Q4),    cos(Q4),    0,      0
        0,          0,          0,      1];
T_45 =[ cos(Q5),   -sin(Q5),    0,      0
        0,          0,          1,      0
       -sin(Q5),   -cos(Q5),    0,      0
        0,          0,          0,      1];
T_56 =[ cos(Q6),   -sin(Q6),    0,      0
        0,          0,         -1,      0
        sin(Q6),    cos(Q6),    0,      0
        0,          0,          0,      1];

%% 求基坐标系到各个连杆坐标系的其次变换矩阵T_0i

% 计算T_0i
% 这里使用cell数组，cell数组可以包含不同类型的元素，包括符号表达式
T_0i = cell(1, 6);  % 创建1行6列的cell数组，cell数组需要用大括号来访问
T_0i{1} = T_01;
T_0i{2} = T_01 * T_12;
T_0i{3} = T_01 * T_12 * T_23;
T_0i{4} = T_01 * T_12 * T_23 * T_34;
T_0i{5} = T_01 * T_12 * T_23 * T_34 * T_45;
T_0i{6} = T_01 * T_12 * T_23 * T_34 * T_45 * T_56;

% 化简结果
for i=1 : 6
    T_0i{i}=simplify(T_0i{i});
end


% 输出结果
% Ctrl+R注释与Ctrl+T取消注释
% for i=1 : 6
%     T_0i{i}
% end

%% 计算各关节轴线的单位向量zi

% 计算变换矩阵T_0i的旋转矩阵R_0i
R_0i = cell(1, 6);
for i=1:6
    R_0i{i}=T_0i{i}(1:3, 1:3);
end

% 计算zi
%R_0i*[0;0;1]
zi = cell(1, 6);
for i=1:6
    zi{i}=R_0i{i}*[0;0;1];
end

% 输出结果
% Ctrl+R注释与Ctrl+T取消注释
% for i=1 : 6
%     zi{i}
% end

%% 计算坐标系i到末端位置矢量p6_i
% 这一步是通过对机器的几何构型和DH模型所建立的坐标系来确定的

p6_i = cell(1, 6);
p6_i{1}=[   a1+a2*cos(Q2)+a3*cos(Q2+Q3)+d4*sin(Q2+Q3)
            0
            a2*sin(Q2)+a3*sin(Q2+Q3)-d4*cos(Q2+Q3)      ];
p6_i{2}=[   a2+a3*cos(Q3)+d4*sin(Q3)
            a3*sin(Q3)-d4*cos(Q3)
            0                       ];
p6_i{3}=[   a3;-d4;   0     ];
p6_i{4}=[   0;   0;   0     ];
p6_i{5}=[   0;   0;   0     ];
p6_i{6}=[   0;   0;   0     ];

%% 计算位置矢量在基坐标系下的表示p6_0i

% R_0i*p6_i
p6_0i = cell(1, 6);
for i=1:6
    p6_0i{i}=R_0i{i}*p6_i{i};
end

% 输出结果
% Ctrl+R注释与Ctrl+T取消注释
% for i=1 : 6
%     p6_0i{i}
% end

%% 计算雅可比矩阵的各列Ji

%Ji=[   zi × p6_0i
%       zi          ]
Ji = cell(1, 6);
for i=1:6
    Ji{i}=[cross(zi{i},p6_0i{i});zi{i}];
end

% 输出结果
% Ctrl+R注释与Ctrl+T取消注释
% for i=1 : 6
%     Ji{i}
% end

%% 得到雅可比矩阵J

J=[Ji{1},Ji{2},Ji{3},Ji{4},Ji{5},Ji{6}];

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129

---

· 雅可比矩阵计算结果对比机器人工具箱验证

这个过程太简单了可惜这里没有足够的空间写下我的证明过程（嘿嘿）

我只能说结果一模一样

---

○ 微分变换法

（内容待补充。。。）

[   T d x   T d y   T d z   T φ x   T φ y   T φ z ] = [ n x n y n z ( p × n ) x ( p × n ) y ( p × n ) z o x o y o z ( p × o ) x ( p × o ) y ( p × o ) z a x a y a z ( p × a ) x ( p × a ) y ( p × a ) z 0 0 0 n x n y n z 0 0 0 o x o y o z 0 0 0 a x a y a z ] [ d x d y d z φ x φ y φ z ] \left[

$$\begin{matrix}{_\ ^T}d_x\\{_\ ^T}d_y\\{_\ ^T}d_z\\{_\ ^T}\varphi_x\\{_\ ^T}\varphi_y\\{_\ ^T}\varphi_z\\\end{matrix}$$ \right]=\left[ $$\begin{matrix}n_x&n_y&n_z&\left(p\times n\right)_x&\left(p\times n\right)_y&\left(p\times n\right)_z\\o_x&o_y&o_z&\left(p\times o\right)_x&\left(p\times o\right)_y&\left(p\times o\right)_z\\a_x&a_y&a_z&\left(p\times a\right)_x&\left(p\times a\right)_y&\left(p\times a\right)_z\\0&0&0&n_x&n_y&n_z\\0&0&0&o_x&o_y&o_z\\0&0&0&a_x&a_y&a_z\\\end{matrix}$$ \right]\left[ $$\begin{matrix}d_x\\d_y\\d_z\\\varphi_x\\\varphi_y\\\varphi_z\\\end{matrix}$$ \right] ​ T​dx​ T​dy​ T​dz​ T​φx​ T​φy​ T​φz​​ ​= ​nx​ox​ax​000​ny​oy​ay​000​nz​oz​az​000​(p×n)x​(p×o)x​(p×a)x​nx​ox​ax​​(p×n)y​(p×o)y​(p×a)y​ny​oy​ay​​(p×n)z​(p×o)z​(p×a)z​nz​oz​az​​ ​ ​dx​dy​dz​φx​φy​φz​​ ​

---