工业机器人运动学与Matlab正逆解算法学习笔记（用心总结一文全会）（三）——逆运动学P2_在matlab中求解机器人逆运动学怎样确定最优解

---

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

---

❤ 2023.6.27 ❤

建立DH模型

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

---

机器人正运动学

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

---

机器人逆运动学

△ 求${\theta }_1$、${\theta }_2$、${\theta }_3$

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

---

△ 代数解求${\theta }_4$、${\theta }_5$、${\theta }_6$

○ 求解${\theta }_4$

将式末端位姿描述矩阵两边同时左${{}_3^{2}T}^{-1}{{}_2^{1}T}^{-1}{{}_1^{0}T}^{-1}$得：
$${{}_3^{2}T}^{-1}{{}_2^{1}T}^{-1}{{}_1^{0}T}^{-1}{}_6^{0}T{=}_4^{3}T\left({\theta }_4\right){}_5^{4}T\left({\theta }_5\right){}_6^{5}T\left({\theta }_6\right)$$

其中：
  3 2 T − 1   2 1 T − 1   1 0 T − 1 = [ c 1 c 23 s 1 c 23 s 23 − a 1 c 23 − d 1 s 23 − a 2 c 3 − c 1 s 23 − s 1 s 23 c 23 a 1 s 23 − d 1 c 23 + a 2 s 3 s 1 − c 1 0 0 0 0 0 1 ] \ {{_3^2}T}^{-1}\ {{_2^1}T}^{-1}\ {{_1^0}T}^{-1}=\left[

$$\begin{matrix}c_1c_{23}&s_1c_{23}&s_{23}&-a_1c_{23}-d_1s_{23}-a_2c_3\\-c_1s_{23}&-s_1s_{23}&c_{23}&a_1s_{23}-d_1c_{23}+a_2s_3\\s_1&-c_1&0&0\\0&0&0&1\\\end{matrix}$$ \right]  32​T−1 21​T−1 10​T−1= ​c1​c23​−c1​s23​s1​0​s1​c23​−s1​s23​−c1​0​s23​c23​00​−a1​c23​−d1​s23​−a2​c3​a1​s23​−d1​c23​+a2​s3​01​ ​
$${}_4^{3}T\left({\theta }_4\right){}_5^{4}T\left({\theta }_5\right){}_6^{5}T\left({\theta }_6\right)=\left[\begin{array}{cccc}{c}_4{c}_5{c}_6-s_4{s}_6& -c_6{s}_4-c_4{c}_5{s}_6& c_4{s}_5& a_3\\ c_6{s}_5& -s_5{s}_6& -c_5& -d_4\\ c_4{s}_6+c_5{c}_6{s}_4& c_4{c}_6-c_5{s}_4{s}_6& s_4{s}_5& 0\\ 0& 0& 0& 1\end{array}\right]$$

---

• ※ 计算过程

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

%ZK-500连杆间齐次变换矩阵
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_6t=[  1           0           0       0
        0           1           0       0
        0           0           1       dt
        0           0           0       1];

% 计算T_06和T_16的逆矩阵
T_06=[nx ox ax px;ny oy ay py;nz oz az pz;0 0 0 1];
T_03=T_01*T_12*T_23;
T_36=T_34*T_45*T_56;


% 计算T_01的逆矩阵
T_03_inv = inv(T_03);
T_03_inv=simplify(T_03_inv)
T_36=simplify(T_36)
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

结果

T_03_inv =
 
[ cos(Q2 + Q3)*cos(Q1),  cos(Q2 + Q3)*sin(Q1), sin(Q2 + Q3), - a1*cos(Q2 + Q3) - d1*sin(Q2 + Q3) - a2*cos(Q3)]
[-sin(Q2 + Q3)*cos(Q1), -sin(Q2 + Q3)*sin(Q1), cos(Q2 + Q3),   a1*sin(Q2 + Q3) - d1*cos(Q2 + Q3) + a2*sin(Q3)]
[              sin(Q1),              -cos(Q1),            0,                                                0]
[                    0,                     0,            0,                                                1]
 
 
T_36 =
 
[cos(Q4)*cos(Q5)*cos(Q6) - sin(Q4)*sin(Q6), - cos(Q6)*sin(Q4) - cos(Q4)*cos(Q5)*sin(Q6), cos(Q4)*sin(Q5),  a3]
[                          cos(Q6)*sin(Q5),                            -sin(Q5)*sin(Q6),        -cos(Q5), -d4]
[cos(Q4)*sin(Q6) + cos(Q5)*cos(Q6)*sin(Q4),   cos(Q4)*cos(Q6) - cos(Q5)*sin(Q4)*sin(Q6), sin(Q4)*sin(Q5),   0]
[                                        0,                                           0,               0,   1]
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

matlab真好用。。。。

---

令元素（1,3）和（3,3）等号左右相等得到：
$$\begin{gathered} a_x{c}_1{c}_{23}+a_y{s}_1{c}_{23}+a_z{s}_{23}=c_4{s}_5 \\ {a}_x{s}_1-a_y{c}_1=s_4{s}_5 \end{gathered}$$

计算过程见前面求${\theta }_2$的内容
→→→跳转到 计算过程
化简是手动化简的。。。

记
$$\begin{gathered} h_1=a_x{c}_1{c}_{23}+a_y{s}_1{c}_{23}+a_z{s}_{23} \\ {h}_2=a_x{s}_1-a_y{c}_1 \end{gathered}$$

将式（）等号两边取平方和可得
$$s_5^2=h_1^2+h_2^2$$

虽然这里通过
$${\theta }_5=\arcsin \left(\pm \sqrt{\left(h_1^2+h_2^2\right)}\right)$$
就能求出${\theta }_5$但是根据前面的说法，最好用atan2()函数来求解，所以这里的结果只用来判断${\theta }_5$是不是趋向于0

根据程序验证，当${\theta }_5$取值为0时，由于电脑的舍入误差的影响，经过计算的${\theta }_5$的数值并不为0，而是一个极小的值。
如果直接判断${\theta }_5$是否为零，则不但会使判断失效，还会影响${\theta }_4$的取值，因此这里虽然写作判断${\theta }_5$是否为0，但在程序中是判断$s_5^2$的是否小于某一特定值。

当${\theta }_5\ne 0$时
$${\theta }_4=\mathrm{Atan}2\left(h_2,h_1\right)$$
当${\theta }_5=0$时，机械臂处于奇异位形，关节4和关节6重合成一条直线，此时所有解都是${\theta }_4$与${\theta }_6$的和或差。在这种情况下${\theta }_4$可以任意取值，但一般选择保持其当前值。

---

• matlab代码实现

%% theta4求解
h1=ax*cos(theta1)*cos(theta2+theta3)+ay*sin(theta1)*cos(theta2+theta3)+az*sin(theta2+theta3);
h2=ax*sin(theta1)-ay*cos(theta1);
s5sq=h1^2+h2^2; % 用sin(theta5)的平方作为判断机械臂是否处于奇异位形

% 需要判断theta5是否为0
% 这里判断theta5是否为0其实有点问题，因为当theta5接近0的时候就会出问题，所以这里选择判断sin(theta5)的平方是否小于某一特定值，若小于则认为theta5为0
% 当theta5=0时，theta4保持不变
% 当theta5≠0时，根据theta4可能取到两组值
if s5sq<0.0000001
    theta4=q_r(4);  %q_r(4)是上一步theta4的值
else
    theta4_1=atan2(h2,h1);
    theta4_2=atan2(-h2,-h1);
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

---

○ 求解${\theta }_5$

将末端位姿描述矩阵写成
$${{}_4^{3}T}^{-1}{{}_3^{2}T}^{-1}{{}_2^{1}T}^{-1}{{}_1^{0}T}^{-1}{}_6^{0}T{=}_6^{5}T\left({\theta }_6\right){}_5^{4}T\left({\theta }_6\right)$$

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

$$\begin{matrix}s_1s_4+c_1c_4c_{23}&s_1c_4c_{23}-c_1s_4&c_4s_{23}&-a_1c_4c_{23}-a_2c_3c_4-a_3c_4-d_1c_4s_{23}\\s_1c_4-c_1s_4c_{23}&-s_1s_4c_{23}-c_1c_4&-s_4s_{23}&a_1s_4c_{23}+a_2c_3c_4+a_3s_4+d_1s_4s_{23}\\c_1s_{23}&s_1s_{23}&-c_{23}&-a_1s_{23}-a_2s_3+d_1c_{23}-d_4\\0&0&0&1\\\end{matrix}$$ \right]  43​T−1 32​T−1 21​T−1 10​T−1= ​s1​s4​+c1​c4​c23​s1​c4​−c1​s4​c23​c1​s23​0​s1​c4​c23​−c1​s4​−s1​s4​c23​−c1​c4​s1​s23​0​c4​s23​−s4​s23​−c23​0​−a1​c4​c23​−a2​c3​c4​−a3​c4​−d1​c4​s23​a1​s4​c23​+a2​c3​c4​+a3​s4​+d1​s4​s23​−a1​s23​−a2​s3​+d1​c23​−d4​1​ ​
$${}_5^{4}T\left({\theta }_5\right){}_6^{5}T\left({\theta }_6\right)=\left[\begin{array}{cccc}{c}_5{c}_6& -c_5{s}_6& s_5& 0\\ s_6& c_6& 0& 0\\ -s_5{c}_6& s_5{s}_6& c_5& 0\\ 0& 0& 0& 1\end{array}\right]$$

---

• 计算过程参考${\theta }_4$的过程

结果如下

T_04_inv =
 
[
sin(Q1)*sin(Q4) + cos(Q1)*cos(Q2)*cos(Q3)*cos(Q4) - cos(Q1)*cos(Q4)*sin(Q2)*sin(Q3), 
cos(Q2)*cos(Q3)*cos(Q4)*sin(Q1) - cos(Q1)*sin(Q4) - cos(Q4)*sin(Q1)*sin(Q2)*sin(Q3),  
sin(Q2 + Q3)*cos(Q4), 
-cos(Q4)*(a3 + a1*cos(Q2 + Q3) + d1*sin(Q2 + Q3) + a2*cos(Q3))
]

[
cos(Q4)*sin(Q1) - cos(Q1)*cos(Q2)*cos(Q3)*sin(Q4) + cos(Q1)*sin(Q2)*sin(Q3)*sin(Q4), 
sin(Q1)*sin(Q2)*sin(Q3)*sin(Q4) - cos(Q2)*cos(Q3)*sin(Q1)*sin(Q4) - cos(Q1)*cos(Q4), 
-sin(Q2 + Q3)*sin(Q4),  
sin(Q4)*(a3 + a1*cos(Q2 + Q3) + d1*sin(Q2 + Q3) + a2*cos(Q3))
]

[
sin(Q2 + Q3)*cos(Q1),                                                                
sin(Q2 + Q3)*sin(Q1),         
-cos(Q2 + Q3),            
d1*cos(Q2 + Q3) - d4 - a1*sin(Q2 + Q3) - a2*sin(Q3)]

[0,0,0,1]
 

T_left =
 
[
nx*(sin(Q1)*sin(Q4) + cos(Q1)*cos(Q2)*cos(Q3)*cos(Q4) - cos(Q1)*cos(Q4)*sin(Q2)*sin(Q3)) - ny*(cos(Q1)*sin(Q4) - cos(Q2)*cos(Q3)*cos(Q4)*sin(Q1) + cos(Q4)*sin(Q1)*sin(Q2)*sin(Q3)) + nz*sin(Q2 + Q3)*cos(Q4), 
ox*(sin(Q1)*sin(Q4) + cos(Q1)*cos(Q2)*cos(Q3)*cos(Q4) - cos(Q1)*cos(Q4)*sin(Q2)*sin(Q3)) - oy*(cos(Q1)*sin(Q4) - cos(Q2)*cos(Q3)*cos(Q4)*sin(Q1) + cos(Q4)*sin(Q1)*sin(Q2)*sin(Q3)) + oz*sin(Q2 + Q3)*cos(Q4), 
ax*(sin(Q1)*sin(Q4) + cos(Q1)*cos(Q2)*cos(Q3)*cos(Q4) - cos(Q1)*cos(Q4)*sin(Q2)*sin(Q3)) - ay*(cos(Q1)*sin(Q4) - cos(Q2)*cos(Q3)*cos(Q4)*sin(Q1) + cos(Q4)*sin(Q1)*sin(Q2)*sin(Q3)) + az*sin(Q2 + Q3)*cos(Q4), 
px*(sin(Q1)*sin(Q4) + cos(Q1)*cos(Q2)*cos(Q3)*cos(Q4) - cos(Q1)*cos(Q4)*sin(Q2)*sin(Q3)) - cos(Q4)*(a3 + a1*cos(Q2 + Q3) + d1*sin(Q2 + Q3) + a2*cos(Q3)) - py*(cos(Q1)*sin(Q4) - cos(Q2)*cos(Q3)*cos(Q4)*sin(Q1) + cos(Q4)*sin(Q1)*sin(Q2)*sin(Q3)) + pz*sin(Q2 + Q3)*cos(Q4)
]

[
nx*(cos(Q4)*sin(Q1) - cos(Q1)*cos(Q2)*cos(Q3)*sin(Q4) + cos(Q1)*sin(Q2)*sin(Q3)*sin(Q4)) - ny*(cos(Q1)*cos(Q4) + cos(Q2)*cos(Q3)*sin(Q1)*sin(Q4) - sin(Q1)*sin(Q2)*sin(Q3)*sin(Q4)) - nz*sin(Q2 + Q3)*sin(Q4), 
ox*(cos(Q4)*sin(Q1) - cos(Q1)*cos(Q2)*cos(Q3)*sin(Q4) + cos(Q1)*sin(Q2)*sin(Q3)*sin(Q4)) - oy*(cos(Q1)*cos(Q4) + cos(Q2)*cos(Q3)*sin(Q1)*sin(Q4) - sin(Q1)*sin(Q2)*sin(Q3)*sin(Q4)) - oz*sin(Q2 + Q3)*sin(Q4), 
ax*(cos(Q4)*sin(Q1) - cos(Q1)*cos(Q2)*cos(Q3)*sin(Q4) + cos(Q1)*sin(Q2)*sin(Q3)*sin(Q4)) - ay*(cos(Q1)*cos(Q4) + cos(Q2)*cos(Q3)*sin(Q1)*sin(Q4) - sin(Q1)*sin(Q2)*sin(Q3)*sin(Q4)) - az*sin(Q2 + Q3)*sin(Q4), 
px*(cos(Q4)*sin(Q1) - cos(Q1)*cos(Q2)*cos(Q3)*sin(Q4) + cos(Q1)*sin(Q2)*sin(Q3)*sin(Q4)) + sin(Q4)*(a3 + a1*cos(Q2 + Q3) + d1*sin(Q2 + Q3) + a2*cos(Q3)) - py*(cos(Q1)*cos(Q4) + cos(Q2)*cos(Q3)*sin(Q1)*sin(Q4) - sin(Q1)*sin(Q2)*sin(Q3)*sin(Q4)) - pz*sin(Q2 + Q3)*sin(Q4)
]

[                                                                                                                                          
ny*sin(Q2 + Q3)*sin(Q1) - nz*cos(Q2 + Q3) + nx*sin(Q2 + Q3)*cos(Q1),
oy*sin(Q2 + Q3)*sin(Q1) - oz*cos(Q2 + Q3) + ox*sin(Q2 + Q3)*cos(Q1),
ax*sin(Q2 + Q3)*cos(Q1) - az*cos(Q2 + Q3) + ay*sin(Q2 + Q3)*sin(Q1),
d1*cos(Q2 + Q3) - d4 - pz*cos(Q2 + Q3) - a1*sin(Q2 + Q3) - a2*sin(Q3) + py*sin(Q2 + Q3)*sin(Q1) + px*sin(Q2 + Q3)*cos(Q1)
]

[0,0,0,1]
 


T_46 =
 
[ cos(Q5)*cos(Q6), -cos(Q5)*sin(Q6), sin(Q5), 0]
[         sin(Q6),          cos(Q6),       0, 0]
[-cos(Q6)*sin(Q5),  sin(Q5)*sin(Q6), cos(Q5), 0]
[               0,                0,       0, 1]
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

---

令元素（1,3）和（3,3）等号左右相等得到：
$$\begin{gathered} a_x\left(s_1{s}_4+c_1{c}_4{c}_{23}\right)+a_y\left(s_1{c}_4{c}_{23}-c_1{s}_4\right)+a_z\left(c_4{s}_{23}\right)=s_5 \\ {a}_x\left(c_1{s}_{23}\right)+a_y\left(s_1{s}_{23}\right)+a_z\left(-c_{23}\right)=c_5 \end{gathered}$$
令
$$\begin{gathered} p_1=a_x\left(s_1{s}_4+c_1{c}_4{c}_{23}\right)+a_y\left(s_1{c}_4{c}_{23}-c_1{s}_4\right)+a_z\left(c_4{s}_{23}\right) \\ {p}_2=a_x\left(c_1{s}_{23}\right)+a_y\left(s_1{s}_{23}\right)+a_z\left(-c_{23}\right) \end{gathered}$$

则
$${\theta }_5=\mathrm{Atan}2\left(p_1,p_2\right)$$

---

• matlab代码实现

p1=ax*(sin(theta1)*sin(theta4)+cos(theta1)*cos(theta4)*cos(theta2+theta3))+...
    ay*(sin(theta1)*cos(theta4)*cos(theta2+theta3)-cos(theta1)*sin(theta4))+...
    az*cos(theta4)*sin(theta2+theta3);
p2=ax*cos(theta1)*sin(theta2+theta3)+ay*sin(theta1)*sin(theta2+theta3)-az*cos(theta2+theta3);
theta5_1=atan2(p1,p2);
theta5_2=atan2(-p1,-p2);
1
2
3
4
5
6

---

○ 求解${\theta }_6$

将等式写成
$${{}_5^{4}T}^{-1}{{}_4^{3}T}^{-1}{{}_3^{2}T}^{-1}{{}_2^{1}T}^{-1}{{}_1^{0}T}^{-1}{}_6^{0}T{=}_6^{5}T\left({\theta }_6\right)$$
记
  5 4 T − 1   4 3 T − 1   3 2 T − 1   2 1 T − 1   1 0 T − 1 = [ m 11 m 12 m 13 m 14 m 21 m 22 m 23 m 24 m 31 m 32 m 33 m 34 0 0 0 1 ] \ {{_5^4}T}^{-1}\ {{_4^3}T}^{-1}\ {{_3^2}T}^{-1}\ {{_2^1}T}^{-1}\ {{_1^0}T}^{-1}=\left[

$$\begin{matrix}m_{11}&m_{12}&m_{13}&m_{14}\\m_{21}&m_{22}&m_{23}&m_{24}\\m_{31}&m_{32}&m_{33}&m_{34}\\0&0&0&1\\\end{matrix}$$ \right]  54​T−1 43​T−1 32​T−1 21​T−1 10​T−1= ​m11​m21​m31​0​m12​m22​m32​0​m13​m23​m33​0​m14​m24​m34​1​ ​

其中

m 11 = s 1 s 4 c 5 − c 1 s 5 s 23 + c 1 c 4 c 5 c 23 m 12 = − c 1 s 4 c 5 − s 1 s 5 s 23 + s 1 c 4 c 5 c 23 m 13 = s 5 c 23 + c 4 c 5 s 23 m 31 = s 1 c 4 − c 1 s 4 c 23 m 32 = − c 1 c 4 − s 1 s 4 c 23 m 33 = − s 4 s 23

$$\begin{aligned} m_{11}&=s_1s_4c_5-c_1s_5s_{23}+c_1c_4c_5c_{23}\\ m_{12}&=-c_1s_4c_5-s_1s_5s_{23}+s_1c_4c_5c_{23}\\ m_{13}&=s_5c_{23}+c_4c_5s_{23}\\ m_{31}&=s_1c_4-c_1s_4c_{23}\\ m_{32}&=-c_1c_4-s_1s_4c_{23}\\ m_{33}&=-s_4s_{23}\\ \end{aligned}$$ m11​m12​m13​m31​m32​m33​​=s1​s4​c5​−c1​s5​s23​+c1​c4​c5​c23​=−c1​s4​c5​−s1​s5​s23​+s1​c4​c5​c23​=s5​c23​+c4​c5​s23​=s1​c4​−c1​s4​c23​=−c1​c4​−s1​s4​c23​=−s4​s23​​

---

• 计算过程参考${\theta }_4$的过程

结果如下

T_05_inv =
 
[
cos(Q5)*sin(Q1)*sin(Q4) - cos(Q1)*cos(Q2)*sin(Q3)*sin(Q5) - cos(Q1)*cos(Q3)*sin(Q2)*sin(Q5) - cos(Q1)*cos(Q4)*cos(Q5)*sin(Q2)*sin(Q3) + cos(Q1)*cos(Q2)*cos(Q3)*cos(Q4)*cos(Q5), 
%=c5s1s4-c1c2s3s5-c1c3s2s5-c1c4c5s2s3+c1c2c3c4c5
%=c5s1s4-c1s5(c2s3+s2c3)+c1c4c5(c2c3-s2s3)
%=c5s1s4-c1s5s23+c1c4c5c23

cos(Q2)*cos(Q3)*cos(Q4)*cos(Q5)*sin(Q1) - cos(Q2)*sin(Q1)*sin(Q3)*sin(Q5) - cos(Q3)*sin(Q1)*sin(Q2)*sin(Q5) - cos(Q4)*cos(Q5)*sin(Q1)*sin(Q2)*sin(Q3) - cos(Q1)*cos(Q5)*sin(Q4), 
%=c2c3c4c5s1-c2s1s3s5-c3s1s2s5-c4c5s1s2s3-c1c5s4
%=s1c4c5(c2c3-s2s3)-s1s5(s2c3+c2s3)-c1c5s4
%=s1c4c5c23-s1s5s23-c1c5s4

cos(Q2)*cos(Q3)*sin(Q5) - sin(Q2)*sin(Q3)*sin(Q5) + cos(Q2)*cos(Q4)*cos(Q5)*sin(Q3) + cos(Q3)*cos(Q4)*cos(Q5)*sin(Q2), 
%=c2c3s5-s2s3s5+c2c4c5s3+c3c4c5s2
%=s5(c2c3-s2s3)+c4c5(c2s3+s2c3)
%=s5c23+c4c5s23

d4*sin(Q5) - a3*cos(Q4)*cos(Q5) + a2*sin(Q3)*sin(Q5) + d1*sin(Q2)*sin(Q3)*sin(Q5) - a2*cos(Q3)*cos(Q4)*cos(Q5) - d1*cos(Q2)*cos(Q3)*sin(Q5) + a1*cos(Q2)*sin(Q3)*sin(Q5) + a1*cos(Q3)*sin(Q2)*sin(Q5) - a1*cos(Q2)*cos(Q3)*cos(Q4)*cos(Q5) - d1*cos(Q2)*cos(Q4)*cos(Q5)*sin(Q3) - d1*cos(Q3)*cos(Q4)*cos(Q5)*sin(Q2) + a1*cos(Q4)*cos(Q5)*sin(Q2)*sin(Q3)
]

[
cos(Q1)*cos(Q4)*sin(Q2)*sin(Q3)*sin(Q5) - cos(Q1)*cos(Q2)*cos(Q5)*sin(Q3) - cos(Q1)*cos(Q3)*cos(Q5)*sin(Q2) - sin(Q1)*sin(Q4)*sin(Q5) - cos(Q1)*cos(Q2)*cos(Q3)*cos(Q4)*sin(Q5), 
cos(Q1)*sin(Q4)*sin(Q5) - cos(Q2)*cos(Q5)*sin(Q1)*sin(Q3) - cos(Q3)*cos(Q5)*sin(Q1)*sin(Q2) - cos(Q2)*cos(Q3)*cos(Q4)*sin(Q1)*sin(Q5) + cos(Q4)*sin(Q1)*sin(Q2)*sin(Q3)*sin(Q5), 
cos(Q2)*cos(Q3)*cos(Q5) - cos(Q5)*sin(Q2)*sin(Q3) - cos(Q2)*cos(Q4)*sin(Q3)*sin(Q5) - cos(Q3)*cos(Q4)*sin(Q2)*sin(Q5), 
d4*cos(Q5) + a2*cos(Q5)*sin(Q3) + a3*cos(Q4)*sin(Q5) + d1*cos(Q5)*sin(Q2)*sin(Q3) - d1*cos(Q2)*cos(Q3)*cos(Q5) + a1*cos(Q2)*cos(Q5)*sin(Q3) + a1*cos(Q3)*cos(Q5)*sin(Q2) + a2*cos(Q3)*cos(Q4)*sin(Q5) + a1*cos(Q2)*cos(Q3)*cos(Q4)*sin(Q5) + d1*cos(Q2)*cos(Q4)*sin(Q3)*sin(Q5) + d1*cos(Q3)*cos(Q4)*sin(Q2)*sin(Q5) - a1*cos(Q4)*sin(Q2)*sin(Q3)*sin(Q5)
]

[
cos(Q4)*sin(Q1) - cos(Q1)*cos(Q2)*cos(Q3)*sin(Q4) + cos(Q1)*sin(Q2)*sin(Q3)*sin(Q4),
%=c4s1-c1c2c3s4+c1s2s3s4
%=s1c4-c1s4(c2c3-s2s3)

sin(Q1)*sin(Q2)*sin(Q3)*sin(Q4) - cos(Q2)*cos(Q3)*sin(Q1)*sin(Q4) - cos(Q1)*cos(Q4),
%=s1s2s3s4-c2c3s1s4-c1c4
%=-c1c4-s1s4c23

-sin(Q2 + Q3)*sin(Q4),
sin(Q4)*(a3 + a1*cos(Q2 + Q3) + d1*sin(Q2 + Q3) + a2*cos(Q3))
]

[0,0,0,1]
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

---

将以上结果带入（）中，通过观察等式两边的矩阵，令元素（1,1）和（3,1）等号左右相等得到：

$$n_x{m}_{11}+n_y{m}_{12}+n_z{m}_{13}=c_6{n}_x{m}_{31}+n_y{m}_{32}+n_z{m}_{33}=s_6$$

这里就不放过程了

解得

$${\theta }_6=\mathrm{Atan}2\left(n_x{m}_{31}+n_y{m}_{32}+n_z{m}_{33},n_x{m}_{11}+n_y{m}_{12}+n_z{m}_{13}\right)$$

---

• matlab代码实现

%% theta6求解
m11=sin(theta1)*sin(theta4)*cos(theta5)-cos(theta1)*sin(theta5)*sin(theta2+theta3)+cos(theta1)*cos(theta4)*cos(theta5)*cos(theta2+theta3);
m12=-cos(theta1)*sin(theta4)*cos(theta5)-sin(theta1)*sin(theta5)*sin(theta2+theta3)+sin(theta1)*cos(theta4)*cos(theta5)*cos(theta2+theta3);
m13=sin(theta5)*cos(theta2+theta3)+cos(theta4)*cos(theta5)*sin(theta2+theta3);
m31=sin(theta1)*cos(theta4)-cos(theta1)*sin(theta4)*cos(theta2+theta3);
m32=-cos(theta1)*cos(theta4)-sin(theta1)*sin(theta4)*cos(theta2+theta3);
m33=-sin(theta4)*sin(theta2+theta3);

theta6_1=atan2(nx*m31+ny*m32+nz*m33,nx*m11+ny*m12+nz*m13);
theta6_2=atan2(-(nx*m31+ny*m32+nz*m33),-(nx*m11+ny*m12+nz*m13));
1
2
3
4
5
6
7
8
9
10

---

△ 三轴相交的Pieper解法

【等待补充。。。】

参考《机器人学导论》中内容

---

△ 机器人逆运动学多解的判断

【等待补充。。。】

可以参考

→→→【机器人逆解中存在多个解，怎么对每个解进行筛选和判断】

---

机器人雅可比矩阵

雅可比的内容有点长，单独开了一篇

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