最近在项目中用到了Aubo-i10机器人，遨博机器人跟UR机器人有很多共同点，都是模块化的协作机器人，它们构形相同。但是这两家公司的机器人还是有区别的，比如Aubo机器人的第三个关节是反着转的，跟UR机器人不一样。网上有关于UR机器人的逆运动学推导过程，借鉴一番之后，自己尝试着推导Aubo-i10机器人的正逆运动学，并通过C++编程实现了该求解算法，最后通过记录示教器上的数据进行验证，实验结果表明，关于Aubo-i10机器人的公式推导过程是正确的。
首先，基于遨博官网上给出的Aubo-i10机器人数据，使用标准的DH建模方法建立机器人的运动学参数模型，当然也可以使用改进的DH法来建模，区别只是在
i
i
i 坐标系或
i
−
1
i-1
i−1 坐标系上
x
x
x 轴的确定不同。
标准DH参数表：
| Transform | a a a | α \alpha α | d d d | θ \theta θ |
|---|---|---|---|---|
| 0~1 | 0 | 9 0 ∘ 90^{\circ} 90∘ | d 1 = 0.163 d_{1}=0.163 d1=0.163 | θ 1 = 0 ∘ \theta_1=0^{\circ} θ1=0∘ |
| 1~2 | a 2 = 0.647 a_{2}=0.647 a2=0.647 | 18 0 ∘ 180^{\circ} 180∘ | 0 | θ 2 = 9 0 ∘ \theta_2=90^{\circ} θ2=90∘ |
| 2~3 | a 3 = 0.6005 a_{3}=0.6005 a3=0.6005 | 18 0 ∘ 180^{\circ} 180∘ | 0 | θ 3 = 0 ∘ \theta_3=0^{\circ} θ3=0∘ |
| 3~4 | 0 | − 9 0 ∘ -90^{\circ} −90∘ | d 4 = 0.2013 d_{4}=0.2013 d4=0.2013 | θ 4 = − 9 0 ∘ \theta_4=-90^{\circ} θ4=−90∘ |
| 4~5 | 0 | 9 0 ∘ 90^{ \circ} 90∘ | d 5 = 0.1025 d_5=0.1025 d5=0.1025 | θ 5 = 0 ∘ \theta_5=0^{\circ} θ5=0∘ |
| 5~6 | 0 | 0 ∘ 0^{\circ} 0∘ | d 6 = 0.094 d_{6}=0.094 d6=0.094 | θ 6 = 0 ∘ \theta_6=0^{\circ} θ6=0∘ |
标准DH法中相邻坐标系之间的齐次变换关系：
6
0
T
=
[
n
x
o
x
a
x
p
x
n
y
o
y
a
y
p
y
n
z
o
z
a
z
p
z
0
0
0
1
]
=
1
0
T
2
1
T
3
2
T
4
3
T
5
4
T
6
5
T
^{0}_{6}T=\begin{bmatrix}n_x & o_x & a_x & p_x\\ n_y & o_y & a_y & p_y\\ n_z & o_z & a_z & p_z\\ 0 & 0 & 0 & 1\end{bmatrix}={{^{0}_{1}T}{^{1}_{2}T}{^2_{3}T}{^3_{4}T}{^4_{5}T}{^5_{6}T}}
60T=⎣⎢⎢⎡nxnynz0oxoyoz0axayaz0pxpypz1⎦⎥⎥⎤=10T21T32T43T54T65T
(1) 1 0 T = [ cos ( θ 1 ) 0 sin ( θ 1 ) 0 sin ( θ 1 ) 0 − cos ( θ 1 ) 0 0 1 0 d 1 0 0 0 1 ] ^0_{1}T=\left[\begin{matrix}\cos{\left(\theta_{1} \right)} & 0 & \sin{\left(\theta_{1} \right)} & 0\\\sin{\left(\theta_{1} \right)} & 0 & - \cos{\left(\theta_{1} \right)} & 0\\0 & 1 & 0 & d_{1}\\0 & 0 & 0 & 1\end{matrix}\right] \tag{1} 10T=⎣⎢⎢⎡cos(θ1)sin(θ1)000010sin(θ1)−cos(θ1)0000d11⎦⎥⎥⎤(1)
(2) 2 1 T = [ c o s ( θ 2 + 9 0 ∘ ) s i n ( θ 2 + 9 0 ∘ ) 0 a 2 c o s ( θ 2 + 9 0 ∘ ) s i n ( θ 2 + 9 0 ∘ ) − c o s ( θ 2 + 9 0 ∘ ) 0 a 2 s i n ( θ 2 + 9 0 ∘ ) 0 0 − 1 0 0 0 0 1 ] = [ − sin ( θ 2 ) cos ( θ 2 ) 0 − a 2 sin ( θ 2 ) cos ( θ 2 ) sin ( θ 2 ) 0 a 2 cos ( θ 2 ) 0 0 − 1 0 0 0 0 1 ] ^1_{2}T=\begin{bmatrix} cos(\theta_2 + 90^{\circ}) & sin(\theta_2 + 90^{\circ}) & 0 & a_{2} cos(\theta_2+90^{\circ})\\ sin(\theta_2 + 90^{\circ}) & -cos(\theta_2 + 90^{\circ}) & 0 & a_{2} sin(\theta_2+90^{\circ})\\ 0 & 0 & -1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}=\left[\begin{matrix}- \sin{\left(\theta_{2} \right)} & \cos{\left(\theta_{2} \right)} & 0 & - a_{2} \sin{\left(\theta_{2} \right)}\\\cos{\left(\theta_{2} \right)} & \sin{\left(\theta_{2} \right)} & 0 & a_{2} \cos{\left(\theta_{2} \right)}\\0 & 0 & -1 & 0\\0 & 0 & 0 & 1\end{matrix}\right] \tag{2} 21T=⎣⎢⎢⎡cos(θ2+90∘)sin(θ2+90∘)00sin(θ2+90∘)−cos(θ2+90∘)0000−10a2cos(θ2+90∘)a2sin(θ2+90∘)01⎦⎥⎥⎤=⎣⎢⎢⎡−sin(θ2)cos(θ2)00cos(θ2)sin(θ2)0000−10−a2sin(θ2)a2cos(θ2)01⎦⎥⎥⎤(2)
(3) 3 2 T = [ cos ( θ 3 ) sin ( θ 3 ) 0 a 3 cos ( θ 3 ) sin ( θ 3 ) − cos ( θ 3 ) 0 a 3 sin ( θ 3 ) 0 0 − 1 0 0 0 0 1 ] ^2_{3}T=\left[\begin{matrix}\cos{\left(\theta_{3} \right)} & \sin{\left(\theta_{3} \right)} & 0 & a_{3} \cos{\left(\theta_{3} \right)}\\\sin{\left(\theta_{3} \right)} & - \cos{\left(\theta_{3} \right)} & 0 & a_{3} \sin{\left(\theta_{3} \right)}\\0 & 0 & -1 & 0\\0 & 0 & 0 & 1\end{matrix}\right] \tag{3} 32T=⎣⎢⎢⎡cos(θ3)sin(θ3)00sin(θ3)−cos(θ3)0000−10a3cos(θ3)a3sin(θ3)01⎦⎥⎥⎤(3)
(4) 4 3 T = [ c o s ( θ 4 − 9 0 ∘ ) 0 − s i n ( θ 4 − 9 0 ∘ ) 0 s i n ( θ 4 − 9 0 ∘ ) 0 c o s ( θ 4 − 9 0 ∘ ) 0 0 − 1 0 d 4 0 0 0 1 ] = [ sin ( θ 4 ) 0 cos ( θ 4 ) 0 − cos ( θ 4 ) 0 sin ( θ 4 ) 0 0 − 1 0 d 4 0 0 0 1 ] ^3_{4}T=\begin{bmatrix} cos(\theta_4 - 90^{\circ}) & 0 & -sin(\theta_4 - 90^{\circ}) & 0\\ sin(\theta_4 - 90^{\circ}) & 0 & cos(\theta_4 - 90^{\circ}) & 0\\ 0 & -1 & 0 & d_4\\ 0 & 0 & 0 & 1 \end{bmatrix}=\left[\begin{matrix}\sin{\left(\theta_{4} \right)} & 0 & \cos{\left(\theta_{4} \right)} & 0\\- \cos{\left(\theta_{4} \right)} & 0 & \sin{\left(\theta_{4} \right)} & 0\\0 & -1 & 0 & d_{4}\\0 & 0 & 0 & 1\end{matrix}\right] \tag{4} 43T=⎣⎢⎢⎡cos(θ4−90∘)sin(θ4−90∘)0000−10−sin(θ4−90∘)cos(θ4−90∘)0000d41⎦⎥⎥⎤=⎣⎢⎢⎡sin(θ4)−cos(θ4)0000−10cos(θ4)sin(θ4)0000d41⎦⎥⎥⎤(4)
(5) 5 4 T = [ cos ( θ 5 ) 0 sin ( θ 5 ) 0 sin ( θ 5 ) 0 − cos ( θ 5 ) 0 0 1 0 d 5 0 0 0 1 ] ^4_{5}T=\left[\begin{matrix}\cos{\left(\theta_{5} \right)} & 0 & \sin{\left(\theta_{5} \right)} & 0\\\sin{\left(\theta_{5} \right)} & 0 & - \cos{\left(\theta_{5} \right)} & 0\\0 & 1 & 0 & d_{5}\\0 & 0 & 0 & 1\end{matrix}\right] \tag{5} 54T=⎣⎢⎢⎡cos(θ5)sin(θ5)000010sin(θ5)−cos(θ5)0000d51⎦⎥⎥⎤(5)
(6) 6 5 T = [ cos ( θ 6 ) − sin ( θ 6 ) 0 0 sin ( θ 6 ) cos ( θ 6 ) 0 0 0 0 1 d 6 0 0 0 1 ] ^5_{6}T=\left[\begin{matrix}\cos{\left(\theta_{6} \right)} & - \sin{\left(\theta_{6} \right)} & 0 & 0\\\sin{\left(\theta_{6} \right)} & \cos{\left(\theta_{6} \right)} & 0 & 0\\0 & 0 & 1 & d_{6}\\0 & 0 & 0 & 1\end{matrix}\right] \tag{6} 65T=⎣⎢⎢⎡cos(θ6)sin(θ6)00−sin(θ6)cos(θ6)00001000d61⎦⎥⎥⎤(6)
6 0 T = [ ( − sin ( θ 1 ) sin ( θ 5 ) + cos ( θ 1 ) cos ( θ 5 ) cos ( θ 2 − θ 3 + θ 4 ) ) cos ( θ 6 ) − sin ( θ 6 ) sin ( θ 2 − θ 3 + θ 4 ) cos ( θ 1 ) ( sin ( θ 1 ) sin ( θ 5 ) − cos ( θ 1 ) cos ( θ 5 ) cos ( θ 2 − θ 3 + θ 4 ) ) sin ( θ 6 ) − sin ( θ 2 − θ 3 + θ 4 ) cos ( θ 1 ) cos ( θ 6 ) sin ( θ 1 ) cos ( θ 5 ) + sin ( θ 5 ) cos ( θ 1 ) cos ( θ 2 − θ 3 + θ 4 ) − a 2 sin ( θ 2 ) cos ( θ 1 ) − a 3 sin ( θ 2 − θ 3 ) cos ( θ 1 ) + d 4 sin ( θ 1 ) − d 5 sin ( θ 2 − θ 3 + θ 4 ) cos ( θ 1 ) + d 6 sin ( θ 1 ) cos ( θ 5 ) + d 6 sin ( θ 5 ) cos ( θ 1 ) cos ( θ 2 − θ 3 + θ 4 ) ( sin ( θ 1 ) cos ( θ 5 ) cos ( θ 2 − θ 3 + θ 4 ) + sin ( θ 5 ) cos ( θ 1 ) ) cos ( θ 6 ) − sin ( θ 1 ) sin ( θ 6 ) sin ( θ 2 − θ 3 + θ 4 ) − ( sin ( θ 1 ) cos ( θ 5 ) cos ( θ 2 − θ 3 + θ 4 ) + sin ( θ 5 ) cos ( θ 1 ) ) sin ( θ 6 ) − sin ( θ 1 ) sin ( θ 2 − θ 3 + θ 4 ) cos ( θ 6 ) sin ( θ 1 ) sin ( θ 5 ) cos ( θ 2 − θ 3 + θ 4 ) − cos ( θ 1 ) cos ( θ 5 ) − a 2 sin ( θ 1 ) sin ( θ 2 ) − a 3 sin ( θ 1 ) sin ( θ 2 − θ 3 ) − d 4 cos ( θ 1 ) − d 5 sin ( θ 1 ) sin ( θ 2 − θ 3 + θ 4 ) + d 6 sin ( θ 1 ) sin ( θ 5 ) cos ( θ 2 − θ 3 + θ 4 ) − d 6 cos ( θ 1 ) cos ( θ 5 ) sin ( θ 6 ) cos ( θ 2 − θ 3 + θ 4 ) + sin ( θ 2 − θ 3 + θ 4 ) cos ( θ 5 ) cos ( θ 6 ) − sin ( θ 6 ) sin ( θ 2 − θ 3 + θ 4 ) cos ( θ 5 ) + cos ( θ 6 ) cos ( θ 2 − θ 3 + θ 4 ) sin ( θ 5 ) sin ( θ 2 − θ 3 + θ 4 ) a 2 cos ( θ 2 ) + a 3 cos ( θ 2 − θ 3 ) + d 1 + d 5 cos ( θ 2 − θ 3 + θ 4 ) + d 6 sin ( θ 5 ) sin ( θ 2 − θ 3 + θ 4 ) 0 0 0 1 ] ^0_{6}T=\left[\begin{matrix}\left(- \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{5} \right)} + \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{5} \right)} \cos{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)}\right) \cos{\left(\theta_{6} \right)} - \sin{\left(\theta_{6} \right)} \sin{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} \cos{\left(\theta_{1} \right)} & \left(\sin{\left(\theta_{1} \right)} \sin{\left(\theta_{5} \right)} - \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{5} \right)} \cos{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)}\right) \sin{\left(\theta_{6} \right)} - \sin{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{6} \right)} & \sin{\left(\theta_{1} \right)} \cos{\left(\theta_{5} \right)} + \sin{\left(\theta_{5} \right)} \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} & - a_{2} \sin{\left(\theta_{2} \right)} \cos{\left(\theta_{1} \right)} - a_{3} \sin{\left(\theta_{2} - \theta_{3} \right)} \cos{\left(\theta_{1} \right)} + d_{4} \sin{\left(\theta_{1} \right)} - d_{5} \sin{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} \cos{\left(\theta_{1} \right)} + d_{6} \sin{\left(\theta_{1} \right)} \cos{\left(\theta_{5} \right)} + d_{6} \sin{\left(\theta_{5} \right)} \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)}\\\left(\sin{\left(\theta_{1} \right)} \cos{\left(\theta_{5} \right)} \cos{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} + \sin{\left(\theta_{5} \right)} \cos{\left(\theta_{1} \right)}\right) \cos{\left(\theta_{6} \right)} - \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{6} \right)} \sin{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} & - \left(\sin{\left(\theta_{1} \right)} \cos{\left(\theta_{5} \right)} \cos{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} + \sin{\left(\theta_{5} \right)} \cos{\left(\theta_{1} \right)}\right) \sin{\left(\theta_{6} \right)} - \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} \cos{\left(\theta_{6} \right)} & \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{5} \right)} \cos{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} - \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{5} \right)} & - a_{2} \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{2} \right)} - a_{3} \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{2} - \theta_{3} \right)} - d_{4} \cos{\left(\theta_{1} \right)} - d_{5} \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} + d_{6} \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{5} \right)} \cos{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} - d_{6} \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{5} \right)}\\\sin{\left(\theta_{6} \right)} \cos{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} + \sin{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} \cos{\left(\theta_{5} \right)} \cos{\left(\theta_{6} \right)} & - \sin{\left(\theta_{6} \right)} \sin{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} \cos{\left(\theta_{5} \right)} + \cos{\left(\theta_{6} \right)} \cos{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} & \sin{\left(\theta_{5} \right)} \sin{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} & a_{2} \cos{\left(\theta_{2} \right)} + a_{3} \cos{\left(\theta_{2} - \theta_{3} \right)} + d_{1} + d_{5} \cos{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} + d_{6} \sin{\left(\theta_{5} \right)} \sin{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)}\\0 & 0 & 0 & 1\end{matrix}\right] 60T=⎣⎢⎢⎡(−sin(θ1)sin(θ5)+cos(θ1)cos(θ5)cos(θ2−θ3+θ4))cos(θ6)−sin(θ6)sin(θ2−θ3+θ4)cos(θ1)(sin(θ1)cos(θ5)cos(θ2−θ3+θ4)+sin(θ5)cos(θ1))cos(θ6)−sin(θ1)sin(θ6)sin(θ2−θ3+θ4)sin(θ6)cos(θ2−θ3+θ4)+sin(θ2−θ3+θ4)cos(θ5)cos(θ6)0(sin(θ1)sin(θ5)−cos(θ1)cos(θ5)cos(θ2−θ3+θ4))sin(θ6)−sin(θ2−θ3+θ4)cos(θ1)cos(θ6)−(sin(θ1)cos(θ5)cos(θ2−θ3+θ4)+sin(θ5)cos(θ1))sin(θ6)−sin(θ1)sin(θ2−θ3+θ4)cos(θ6)−sin(θ6)sin(θ2−θ3+θ4)cos(θ5)+cos(θ6)cos(θ2−θ3+θ4)0sin(θ1)cos(θ5)+sin(θ5)cos(θ1)cos(θ2−θ3+θ4)sin(θ1)sin(θ5)cos(θ2−θ3+θ4)−cos(θ1)cos(θ5)sin(θ5)sin(θ2−θ3+θ4)0−a2sin(θ2)cos(θ1)−a3sin(θ2−θ3)cos(θ1)+d4sin(θ1)−d5sin(θ2−θ3+θ4)cos(θ1)+d6sin(θ1)cos(θ5)+d6sin(θ5)cos(θ1)cos(θ2−θ3+θ4)−a2sin(θ1)sin(θ2)−a3sin(θ1)sin(θ2−θ3)−d4cos(θ1)−d5sin(θ1)sin(θ2−θ3+θ4)+d6sin(θ1)sin(θ5)cos(θ2−θ3+θ4)−d6cos(θ1)cos(θ5)a2cos(θ2)+a3cos(θ2−θ3)+d1+d5cos(θ2−θ3+θ4)+d6sin(θ5)sin(θ2−θ3+θ4)1⎦⎥⎥⎤
6 1 T = ( 1 0 T − 1 ) 6 0 T = [ r 11 cos ( θ 1 ) + r 21 sin ( θ 1 ) r 12 cos ( θ 1 ) + r 22 sin ( θ 1 ) r 13 cos ( θ 1 ) + r 23 sin ( θ 1 ) p x cos ( θ 1 ) + p y s i n ( θ 1 ) r 31 r 32 r 33 − d 1 + p z r 11 sin ( θ 1 ) − r 21 cos ( θ 1 ) r 12 sin ( θ 1 ) − r 22 cos ( θ 1 ) r 13 sin ( θ 1 ) − r 23 cos ( θ 1 ) p x sin ( θ 1 ) − p y cos ( θ 1 ) 0 0 0 1 ] ^1_{6}T=({^0_{1}T^{-1}}){^0_{6}T}=\left[\begin{matrix}r_{11} \cos{\left(\theta_{1} \right)} + r_{21} \sin{\left(\theta_{1} \right)} & r_{12} \cos{\left(\theta_{1} \right)} + r_{22} \sin{\left(\theta_{1} \right)} & r_{13} \cos{\left(\theta_{1} \right)} + r_{23} \sin{\left(\theta_{1} \right)} & p_{x} \cos{\left(\theta_{1} \right)} + p_y sin{\left( \theta_1 \right)}\\r_{31} & r_{32} & r_{33} & - d_{1} + p_{z}\\r_{11} \sin{\left(\theta_{1} \right)} - r_{21} \cos{\left(\theta_{1} \right)} & r_{12} \sin{\left(\theta_{1} \right)} - r_{22} \cos{\left(\theta_{1} \right)} & r_{13} \sin{\left(\theta_{1} \right)} - r_{23} \cos{\left(\theta_{1} \right)} & p_{x} \sin{\left(\theta_{1} \right)} - p_{y} \cos{\left(\theta_{1} \right)}\\0 & 0 & 0 & 1\end{matrix}\right] 61T=(10T−1)60T=⎣⎢⎢⎡r11cos(θ1)+r21sin(θ1)r31r11sin(θ1)−r21cos(θ1)0r12cos(θ1)+r22sin(θ1)r32r12sin(θ1)−r22cos(θ1)0r13cos(θ1)+r23sin(θ1)r33r13sin(θ1)−r23cos(θ1)0pxcos(θ1)+pysin(θ1)−d1+pzpxsin(θ1)−pycos(θ1)1⎦⎥⎥⎤
6 1 T = 2 1 T 3 2 T 4 3 T 5 4 T 6 5 T = [ − sin ( θ 6 ) sin ( θ 2 − θ 3 + θ 4 ) + cos ( θ 5 ) cos ( θ 6 ) cos ( θ 2 − θ 3 + θ 4 ) − sin ( θ 6 ) cos ( θ 5 ) cos ( θ 2 − θ 3 + θ 4 ) − sin ( θ 2 − θ 3 + θ 4 ) cos ( θ 6 ) sin ( θ 5 ) cos ( θ 2 − θ 3 + θ 4 ) − a 2 sin ( θ 2 ) − a 3 sin ( θ 2 − θ 3 ) − d 5 sin ( θ 2 − θ 3 + θ 4 ) + d 6 sin ( θ 5 ) cos ( θ 2 − θ 3 + θ 4 ) sin ( θ 6 ) cos ( θ 2 − θ 3 + θ 4 ) + sin ( θ 2 − θ 3 + θ 4 ) cos ( θ 5 ) cos ( θ 6 ) − sin ( θ 6 ) sin ( θ 2 − θ 3 + θ 4 ) cos ( θ 5 ) + cos ( θ 6 ) cos ( θ 2 − θ 3 + θ 4 ) sin ( θ 5 ) sin ( θ 2 − θ 3 + θ 4 ) a 2 cos ( θ 2 ) + a 3 cos ( θ 2 − θ 3 ) + d 5 cos ( θ 2 − θ 3 + θ 4 ) + d 6 sin ( θ 5 ) sin ( θ 2 − θ 3 + θ 4 ) − sin ( θ 5 ) cos ( θ 6 ) sin ( θ 5 ) sin ( θ 6 ) cos ( θ 5 ) d 4 + d 6 cos ( θ 5 ) 0 0 0 1 ] ^1_{6}T={^1_{2}T}{^2_{3}T}{^3_{4}T}{^4_{5}T}{^5_{6}T}=\left[\begin{matrix}- \sin{\left(\theta_{6} \right)} \sin{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} + \cos{\left(\theta_{5} \right)} \cos{\left(\theta_{6} \right)} \cos{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} & - \sin{\left(\theta_{6} \right)} \cos{\left(\theta_{5} \right)} \cos{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} - \sin{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} \cos{\left(\theta_{6} \right)} & \sin{\left(\theta_{5} \right)} \cos{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} & - a_{2} \sin{\left(\theta_{2} \right)} - a_{3} \sin{\left(\theta_{2} - \theta_{3} \right)} - d_{5} \sin{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} + d_{6} \sin{\left(\theta_{5} \right)} \cos{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)}\\\sin{\left(\theta_{6} \right)} \cos{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} + \sin{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} \cos{\left(\theta_{5} \right)} \cos{\left(\theta_{6} \right)} & - \sin{\left(\theta_{6} \right)} \sin{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} \cos{\left(\theta_{5} \right)} + \cos{\left(\theta_{6} \right)} \cos{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} & \sin{\left(\theta_{5} \right)} \sin{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} & a_{2} \cos{\left(\theta_{2} \right)} + a_{3} \cos{\left(\theta_{2} - \theta_{3} \right)} + d_{5} \cos{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)} + d_{6} \sin{\left(\theta_{5} \right)} \sin{\left(\theta_{2} - \theta_{3} + \theta_{4} \right)}\\- \sin{\left(\theta_{5} \right)} \cos{\left(\theta_{6} \right)} & \sin{\left(\theta_{5} \right)} \sin{\left(\theta_{6} \right)} & \cos{\left(\theta_{5} \right)} & d_{4} + d_{6} \cos{\left(\theta_{5} \right)}\\0 & 0 & 0 & 1\end{matrix}\right] 61T=21T32T43T54T65T=⎣⎢⎢⎡−sin(θ6)sin(θ2−θ3+θ4)+cos(θ5)cos(θ6)cos(θ2−θ3+θ4)sin(θ6)cos(θ2−θ3+θ4)+sin(θ2−θ3+θ4)cos(θ5)cos(θ6)−sin(θ5)cos(θ6)0−sin(θ6)cos(θ5)cos(θ2−θ3+θ4)−sin(θ2−θ3+θ4)cos(θ6)−sin(θ6)sin(θ2−θ3+θ4)cos(θ5)+cos(θ6)cos(θ2−θ3+θ4)sin(θ5)sin(θ6)0sin(θ5)cos(θ2−θ3+θ4)sin(θ5)sin(θ2−θ3+θ4)cos(θ5)0−a2sin(θ2)−a3sin(θ2−θ3)−d5sin(θ2−θ3+θ4)+d6sin(θ5)cos(θ2−θ3+θ4)a2cos(θ2)+a3cos(θ2−θ3)+d5cos(θ2−θ3+θ4)+d6sin(θ5)sin(θ2−θ3+θ4)d4+d6cos(θ5)1⎦⎥⎥⎤
求解 θ 1 \theta_{1} θ1
联立公式
6
1
T
^1_{6}T
61T 中的(3, 3)和(3, 4)元素左右相等，有：
{
r
13
sin
(
θ
1
)
−
r
23
cos
(
θ
1
)
=
cos
(
θ
5
)
p
x
sin
(
θ
1
)
−
p
y
cos
(
θ
1
)
=
d
4
+
d
6
cos
(
θ
5
)
\left\{\begin{matrix} r_{13} \sin{\left(\theta_{1} \right)} - r_{23} \cos{\left(\theta_{1} \right)} = \cos{\left(\theta_{5} \right)}\\ p_{x} \sin{\left(\theta_{1} \right)} - p_{y} \cos{\left(\theta_{1} \right)}= d_{4} + d_{6} \cos{\left(\theta_{5} \right)} \end{matrix}\right .
{r13sin(θ1)−r23cos(θ1)=cos(θ5)pxsin(θ1)−pycos(θ1)=d4+d6cos(θ5)
消除
θ
5
\theta_{5}
θ5 有：
(
d
6
r
23
−
p
y
)
c
o
s
(
θ
1
)
+
(
p
x
−
d
6
r
13
)
s
i
n
(
θ
1
)
=
d
4
\left(d_{6}r_{23}-p_{y}\right)cos \left(\theta_{1}\right) + \left(p_{x}-d_{6}r_{13}\right) sin \left(\theta_{1} \right)=d_{4}
(d6r23−py)cos(θ1)+(px−d6r13)sin(θ1)=d4
由半角正切公式有：
令 A 1 = d 6 r 23 − p y A_{1}=d_{6}r_{23}-p_{y} A1=d6r23−py, B 1 = p x − d 6 r 13 B_{1}=p_{x}-d_{6}r_{13} B1=px−d6r13, C 1 = d 4 C_{1}=d_{4} C1=d4
有：
θ
1
=
A
t
a
n
2
(
C
1
,
±
A
1
2
+
B
1
2
−
C
1
2
)
−
A
t
a
n
2
(
A
1
,
B
1
)
\theta_{1}=Atan2(C_{1}, \pm \sqrt {A_{1}^{2}+B_{1}^{2}-C_{1}^{2}}) - Atan2(A_{1}, B_{1})
θ1=Atan2(C1,±A12+B12−C12)−Atan2(A1,B1)
求解
θ
5
\theta_{5}
θ5
由公式
6
1
T
^1_{6}T
61T 中的(3, 3)元素左右等式相等，有：
c
o
s
(
θ
5
)
=
r
13
s
i
n
(
θ
1
)
−
r
23
c
o
s
(
θ
1
)
cos \left( \theta_{5} \right)=r_{13}sin \left(\theta_{1}\right) - r_{23} cos\left( \theta_{1}\right)
cos(θ5)=r13sin(θ1)−r23cos(θ1)
令
b
=
r
13
s
i
n
(
θ
1
)
−
r
23
c
o
s
(
θ
1
)
b=r_{13}sin \left(\theta_{1}\right) - r_{23} cos\left( \theta_{1}\right)
b=r13sin(θ1)−r23cos(θ1), 有
c
o
s
θ
5
=
b
cos\theta_{5}=b
cosθ5=b
由半角正切公式有：
θ
5
=
A
t
a
n
2
(
±
1
−
b
2
,
b
)
\theta_{5}=Atan2(\pm \sqrt{1-b^2}, b)
θ5=Atan2(±1−b2,b)
求解
θ
6
\theta_{6}
θ6
由公式
6
1
T
^1_{6}T
61T 中的 (3, 1) 和 (3, 2) 元素左右等式相等，有：
{
−
s
i
n
(
θ
5
)
c
o
s
(
θ
6
)
=
r
11
s
i
n
(
θ
1
)
−
r
21
c
o
s
(
θ
1
)
s
i
n
(
θ
5
)
s
i
n
(
θ
6
)
=
r
12
s
i
n
(
θ
1
)
−
r
22
c
o
s
(
θ
1
)
\left\{\begin{matrix} -sin\left(\theta_{5}\right)cos\left(\theta_{6}\right)=r_{11}sin\left(\theta_{1}\right)-r_{21}cos\left(\theta_{1}\right)\\ sin\left(\theta_{5}\right)sin\left(\theta_{6}\right)=r_{12}sin\left(\theta_{1}\right)-r_{22}cos\left(\theta_{1}\right) \end{matrix}\right .
{−sin(θ5)cos(θ6)=r11sin(θ1)−r21cos(θ1)sin(θ5)sin(θ6)=r12sin(θ1)−r22cos(θ1)
当
θ
5
≠
0
\theta_{5} \neq 0
θ5̸=0 时，有：
{
A
6
=
r
12
s
i
n
(
θ
1
)
−
r
22
c
o
s
(
θ
1
)
s
i
n
(
θ
5
)
B
6
=
r
21
c
o
s
(
θ
1
)
−
r
11
s
i
n
(
θ
1
)
s
i
n
(
θ
5
)
\left\{\begin{matrix} A_{6}=\frac{r_{12}sin\left(\theta_{1}\right)-r_{22}cos\left(\theta_{1}\right)}{sin\left(\theta_{5}\right)} \\ B_{6}=\frac{r_{21}cos\left(\theta_{1}\right)-r_{11}sin\left(\theta_{1}\right)}{sin\left(\theta_{5}\right)} \end{matrix}\right .
{A6=sin(θ5)r12sin(θ1)−r22cos(θ1)B6=sin(θ5)r21cos(θ1)−r11sin(θ1)
由半角正切公式得
θ
6
=
A
t
a
n
2
(
A
6
,
B
6
)
\theta_{6}=Atan2(A_{6}, B_{6})
θ6=Atan2(A6,B6)
当
s
i
n
(
θ
5
)
=
0
sin(\theta_{5})=0
sin(θ5)=0 时，机器人处于奇异位形，此时关节6轴与2,3,4关节轴平行，机器人存在无数组解。
求解 θ 2 \theta_{2} θ2
由公式
6
1
T
^1_{6}T
61T 中的(1, 3) 和(2, 3)元素左右等式相等，有：
{
s
i
n
(
θ
5
)
c
o
s
(
θ
2
−
θ
3
+
θ
4
)
=
r
13
c
o
s
(
θ
1
)
+
r
23
s
i
n
(
θ
1
)
s
i
n
(
θ
5
)
s
i
n
(
θ
2
−
θ
3
+
θ
4
)
=
r
33
\left\{\begin{matrix} sin(\theta_{5})cos(\theta_{2}-\theta_{3}+\theta_{4})=r_{13}cos(\theta_{1}) + r_{23}sin(\theta_{1}) \\ sin(\theta_{5})sin(\theta_{2}-\theta_{3}+\theta_{4})=r_{33} \end{matrix} \right .
{sin(θ5)cos(θ2−θ3+θ4)=r13cos(θ1)+r23sin(θ1)sin(θ5)sin(θ2−θ3+θ4)=r33
当
θ
5
≠
0
\theta_{5} \neq 0
θ5̸=0 时，有：
{
A
234
=
s
i
n
(
θ
2
−
θ
3
+
θ
4
)
=
r
33
s
i
n
(
θ
5
)
B
234
=
c
o
s
(
θ
2
−
θ
3
+
θ
4
)
=
r
13
c
o
s
(
θ
1
)
+
r
23
s
i
n
(
θ
1
)
s
i
n
(
θ
5
)
\left\{\begin{matrix} A_{234}=sin(\theta_{2}-\theta_{3}+\theta_{4})=\frac{r_{33}}{sin(\theta_{5})} \\ B_{234}=cos(\theta_{2}-\theta_{3}+\theta_{4})=\frac{r_{13}cos(\theta_{1})+r_{23}sin(\theta_{1})}{sin(\theta_{5})} \end{matrix} \right .
{A234=sin(θ2−θ3+θ4)=sin(θ5)r33B234=cos(θ2−θ3+θ4)=sin(θ5)r13cos(θ1)+r23sin(θ1)
由半角正切公式，有：
θ
2
−
θ
3
+
θ
4
=
A
t
a
n
2
(
A
234
,
B
234
)
\theta_{2}-\theta_{3}+\theta_{4}=Atan2(A_{234}, B_{234})
θ2−θ3+θ4=Atan2(A234,B234)
由等式
6
1
T
^1_{6}T
61T 的 (1, 4) 和 (2, 4) 元素左右等式相等，有：
{
−
a
2
s
i
n
(
θ
2
)
−
a
3
s
i
n
(
θ
2
−
θ
3
)
−
d
5
s
i
n
(
θ
2
−
θ
3
+
θ
4
)
+
d
6
s
i
n
(
θ
5
)
c
o
s
(
θ
2
−
θ
3
+
θ
4
)
=
p
x
c
o
s
(
θ
1
)
+
p
y
s
i
n
(
θ
1
)
a
2
c
o
s
(
θ
2
)
+
a
3
c
o
s
(
θ
2
−
θ
3
)
+
d
5
c
o
s
(
θ
2
−
θ
3
+
θ
4
)
+
d
6
s
i
n
(
θ
5
)
s
i
n
(
θ
2
−
θ
3
+
θ
4
)
=
−
d
1
+
p
z
\left\{\begin{matrix} -a_{2}sin(\theta_{2})-a_{3}sin(\theta_{2}-\theta_{3})-d_{5}sin(\theta_{2}-\theta_{3}+\theta_{4})+d_{6}sin(\theta_{5})cos(\theta_{2}-\theta_{3}+\theta_{4})=p_{x}cos(\theta_{1})+p_{y}sin(\theta_{1}) \\ a_{2}cos(\theta_{2})+a_{3}cos(\theta_{2}-\theta_{3})+d_{5}cos(\theta_{2}-\theta_{3}+\theta_{4})+d_{6}sin(\theta_{5})sin(\theta_{2}-\theta_{3}+\theta_{4})=-d_{1}+p_{z} \end{matrix} \right .
{−a2sin(θ2)−a3sin(θ2−θ3)−d5sin(θ2−θ3+θ4)+d6sin(θ5)cos(θ2−θ3+θ4)=pxcos(θ1)+pysin(θ1)a2cos(θ2)+a3cos(θ2−θ3)+d5cos(θ2−θ3+θ4)+d6sin(θ5)sin(θ2−θ3+θ4)=−d1+pz
接着分别定义
M
M
M 和
N
N
N：
{
−
a
2
s
i
n
(
θ
2
)
−
a
3
s
i
n
(
θ
2
−
θ
3
)
=
M
a
2
c
o
s
(
θ
2
)
+
a
3
c
o
s
(
θ
2
−
θ
3
)
=
N
\left\{\begin{matrix} -a_{2}sin(\theta_{2})-a_{3}sin(\theta_{2}-\theta_{3})=M \\ a_{2}cos(\theta_{2})+a_{3}cos(\theta_{2}-\theta_{3})=N \end{matrix} \right .
{−a2sin(θ2)−a3sin(θ2−θ3)=Ma2cos(θ2)+a3cos(θ2−θ3)=N
{ M = d 5 s i n ( θ 2 − θ 3 + θ 4 ) − d 6 s i n ( θ 5 ) c o s ( θ 2 − θ 3 + θ 4 ) + p x c o s ( θ 1 ) + p y s i n ( θ 1 ) N = − d 5 c o s ( θ 2 − θ 3 + θ 4 ) − d 6 s i n ( θ 5 ) s i n ( θ 2 − θ 3 + θ 4 ) − d 1 + p z \left\{\begin{matrix} M = d_{5}sin(\theta_{2}-\theta_{3}+\theta_{4})-d_{6}sin(\theta_{5})cos(\theta_{2}-\theta_{3}+\theta_{4}) + p_{x}cos(\theta_{1})+p_{y}sin(\theta_{1})\\ N = -d_{5}cos(\theta_{2}-\theta_{3}+\theta_{4})-d_{6}sin(\theta_{5})sin(\theta_{2}-\theta_{3}+\theta_{4})-d_{1}+p_{z} \end{matrix} \right . {M=d5sin(θ2−θ3+θ4)−d6sin(θ5)cos(θ2−θ3+θ4)+pxcos(θ1)+pysin(θ1)N=−d5cos(θ2−θ3+θ4)−d6sin(θ5)sin(θ2−θ3+θ4)−d1+pz
有：
M
2
+
N
2
=
a
2
2
s
i
n
2
(
θ
2
)
+
a
2
2
c
o
s
2
(
θ
2
)
+
a
3
2
s
i
n
2
(
θ
2
−
θ
3
)
+
a
3
2
c
o
s
2
(
θ
2
−
θ
3
)
+
2
a
2
a
3
s
i
n
(
θ
2
)
s
i
n
(
θ
2
−
θ
3
)
+
2
a
2
a
3
c
o
s
(
θ
2
)
c
o
s
(
θ
2
−
θ
3
)
M^{2}+N^{2}=a_{2}^{2}sin^{2}(\theta_{2})+a_{2}^{2}cos^{2}(\theta_{2})+a_{3}^{2}sin^{2}(\theta_{2}-\theta_{3}) + a_{3}^{2}cos^{2}(\theta_{2}-\theta_{3}) + 2a_{2}a_{3}sin(\theta_{2})sin(\theta_{2}-\theta_{3}) + 2a_{2}a_{3}cos(\theta_{2})cos(\theta_{2}-\theta_{3})
M2+N2=a22sin2(θ2)+a22cos2(θ2)+a32sin2(θ2−θ3)+a32cos2(θ2−θ3)+2a2a3sin(θ2)sin(θ2−θ3)+2a2a3cos(θ2)cos(θ2−θ3)
− M s i n ( θ 2 ) + N c o s ( θ 2 ) = a 2 s i n 2 ( θ 2 ) + a 3 s i n ( θ 2 ) s i n ( θ 2 − θ 3 ) + a 2 c o s 2 ( θ 2 ) + a 3 c o s ( θ 2 ) c o s ( θ 2 − θ 3 ) = L -Msin(\theta_{2})+Ncos(\theta_{2})=a_{2}sin^{2}(\theta_{2})+a_{3}sin(\theta_{2})sin(\theta_{2}-\theta_{3})+a_{2}cos^{2}(\theta_{2})+a_{3}cos(\theta_{2})cos(\theta_{2}-\theta_{3})=L −Msin(θ2)+Ncos(θ2)=a2sin2(θ2)+a3sin(θ2)sin(θ2−θ3)+a2cos2(θ2)+a3cos(θ2)cos(θ2−θ3)=L
M 2 + N 2 − a 2 2 − a 3 2 = 2 ( − M s i n ( θ 2 ) + N c o s ( θ 2 ) − a 2 ) M^{2}+N^{2}-a^{2}_{2}-a^{2}_{3}=2(-Msin(\theta_{2})+Ncos(\theta_{2})-a_{2}) M2+N2−a22−a32=2(−Msin(θ2)+Ncos(θ2)−a2)
− M s i n ( θ 2 ) + N c o s ( θ 2 ) = L = M 2 + N 2 + a 2 2 − a 3 2 2 a 2 -Msin(\theta_{2})+Ncos(\theta_{2})=L=\frac{M^{2}+N^{2}+a_{2}^{2}-a_{3}^{2}}{2a_{2}} −Msin(θ2)+Ncos(θ2)=L=2a2M2+N2+a22−a32
根据半角正切公式，有：
θ
2
=
A
t
a
n
2
(
N
,
M
)
−
A
t
a
n
2
(
L
,
±
M
2
+
N
2
−
L
2
)
\theta_{2}=Atan2(N, M) - Atan2(L, \pm\sqrt{M^{2}+N^{2}-L^{2}})
θ2=Atan2(N,M)−Atan2(L,±M2+N2−L2)
求解
θ
3
\theta_{3}
θ3
由于：
{
s
i
n
(
θ
2
−
θ
3
)
=
−
M
−
a
2
s
i
n
(
θ
2
)
a
3
=
A
23
c
o
s
(
θ
2
−
θ
3
)
=
N
−
a
2
c
o
s
(
θ
2
)
a
3
=
B
23
\left\{\begin{matrix} sin(\theta_{2}-\theta_{3})=\frac{-M-a_{2}sin(\theta_{2})}{a_{3}}=A_{23} \\ cos(\theta_{2}-\theta_{3})=\frac{N-a_{2}cos(\theta_{2})}{a_{3}}=B_{23} \end{matrix} \right .
{sin(θ2−θ3)=a3−M−a2sin(θ2)=A23cos(θ2−θ3)=a3N−a2cos(θ2)=B23
由半正切公式有：
θ
2
−
θ
3
=
A
t
a
n
2
(
A
23
,
B
23
)
\theta_{2}-\theta_{3}=Atan2(A_{23}, B_{23})
θ2−θ3=Atan2(A23,B23)
故 :
θ
3
=
θ
2
−
A
t
a
n
2
(
A
23
,
B
23
)
\theta_{3}=\theta_{2}-Atan2(A_{23}, B_{23})
θ3=θ2−Atan2(A23,B23)
求解
θ
4
\theta_{4}
θ4
θ
4
=
A
t
a
n
2
(
A
234
,
B
234
)
−
A
t
a
n
2
(
A
23
,
B
23
)
\theta_{4}=Atan2(A_{234}, B_{234})-Atan2(A_{23}, B_{23})
θ4=Atan2(A234,B234)−Atan2(A23,B23)
上述公式符号推导所用到的代码：
from sympy import *
import sympy
a2, a3 = symbols('a2 a3', real=True)
d1, d4, d5, d6 = symbols('d1 d4 d5 d6', real=True)
theta1, theta2, theta3, theta4, theta5, theta6 = symbols('theta1 theta2 theta3 theta4 theta5 theta6', real=True)
r11, r12, r13, r21, r22, r23, r31, r32, r33, p_x, p_y, p_z = symbols('r11 r12 r13 r21 r22 r23 r31 r32 r33 p_x p_y p_z', real=True)
T01=Matrix([[cos(theta1), 0, sin(theta1), 0],
[sin(theta1), 0, -cos(theta1), 0],
[0, 1, 0, d1],
[0, 0, 0, 1]])
T12=Matrix([[-sin(theta2), cos(theta2), 0, -a2*sin(theta2)],
[cos(theta2), sin(theta2), 0, a2*cos(theta2)],
[0, 0, -1, 0],
[0, 0, 0, 1]])
T23=Matrix([[cos(theta3), sin(theta3), 0, a3*cos(theta3)],
[sin(theta3), -cos(theta3), 0, a3*sin(theta3)],
[0, 0, -1, 0],
[0, 0, 0, 1]])
T34=Matrix([[sin(theta4), 0, cos(theta4), 0],
[-cos(theta4), 0, sin(theta4), 0],
[0, -1, 0, d4],
[0, 0, 0, 1]])
T45=Matrix([[cos(theta5), 0, sin(theta5), 0],
[sin(theta5), 0, -cos(theta5), 0],
[0, 1, 0, d5],
[0, 0, 0, 1]])
T56=Matrix([[cos(theta6), -sin(theta6), 0, 0],
[sin(theta6), cos(theta6), 0, 0],
[0, 0, 1, d6],
[0, 0, 0, 1]])
T10=T01**-1
T21=T12**-1
T32=T23**-1
T43=T34**-1
T54=T45**-1
T65=T56**-1
# print(latex(T01))
# print(latex(T12))
# print(latex(T23))
# print(latex(T34))
# print(latex(T45))
# print(latex(T56))
# print(latex(simplify(T10)))
# print(latex(simplify(T21)))
T06=Matrix([[r11, r12, r13, p_x], [r21, r22, r23, p_y], [r31, r32, r33, p_z], [0, 0, 0, 1]])
# print(latex(simplify(T06)))
T16_left=(T01**-1)*T06
T16_right=T12*T23*T34*T45*T56
print(latex(simplify(T16_left)))
print(latex(simplify(T16_right)))
# print(latex(simplify(T16_left[2, 2] - T16_right[2, 2])))
# print(latex(simplify(T16_left[2, 3] - T16_right[2, 3])))
# output = sympy.solve([T16_left[2, 2] - T16_right[2, 2], T16_left[2, 3] - T16_right[2, 3]], [theta1])
# print(latex(simplify(output)))
# print(latex(simplify(T16_left)))
# print(latex(simplify(T16_right)))
# print(latex(simplify(T16_left[0, 0])))
# T06=T01*T12*T23*T34*T45*T56
# print(latex(simplify(T06)))
逆运动学求解器算法C++实现的代码片段：
void AuboClosedFormIKSolver::solve2() {
/**
* @brief Get the solution result of Joint1 angle
* @author Liu Qiang
* @date 2019-07-22
*/
count = 8;
double A1 = d6 * EndPose(1, 2) - EndPose(1, 3);
double B1 = EndPose(0, 3) - d6 * EndPose(0, 2);
double C1 = d4;
double angle1_1 = atan2(C1, sqrt(A1 * A1 + B1 * B1 - C1 * C1)) - atan2(A1, B1);
double angle1_2 = atan2(C1, -sqrt(A1 * A1 + B1 * B1 - C1 * C1)) - atan2(A1, B1);
for (long i = 0; i < 4; ++i) {
Results(i, 0) = angle1_1;
Results(i + 4, 0) = angle1_2;
}
/**
* @brief Get the solution result of Joint5 angle
* @author Liu Qiang
* @date 2019-07-22
*/
for(long i = 0; i < 2; ++i) {
double cos5 = EndPose(0, 2) * sin(Results(i * 4, 0)) - EndPose(1, 2) * cos(Results(i * 4, 0));
double sin5 = sqrt(1 - cos5 * cos5);
double angle5_1 = atan2(sin5, cos5);
double angle5_2 = atan2(-sin5, cos5);
Results(4 * i, 4) = angle5_1;
Results(4 * i + 1, 4) = angle5_1;
Results(4 * i + 2, 4) = angle5_2;
Results(4 * i + 3, 4) = angle5_2;
}
for (int i = 0; i < 8; i++) {
/**
* @brief Get the solution result of Joint6 angle
* @author Liu Qiang
* @date 2019-07-22
*/
double A6 = (EndPose(0, 1) * sin(Results(i, 0)) - EndPose(1, 1) * cos(Results(i, 0))) / sin(Results(i, 4));
double B6 = -(EndPose(0, 0) * sin(Results(i, 0)) - EndPose(1, 0) * cos(Results(i, 0))) / sin(Results(i, 4));
Results(i, 5) = atan2(A6, B6);
/**
* @brief A234 = sin(theta2 - theta2 + theta4)
* @author Liu Qiang
* @date 2019-07-22
*/
double A234 = EndPose(2, 2) / sin(Results(i, 4));
double B234 = (EndPose(0, 2) * cos(Results(i, 0)) + EndPose(1, 2) * sin(Results(i, 0))) / sin(Results(i, 4));
/**
* @brief Get the solution result of Joint2 angle
* @author Liu Qiang
* @date 2019-07-22
*/
double M2 = d5 * A234 - d6 * sin(Results(i, 4)) * B234 + EndPose(0, 3) * cos(Results(i, 0)) + EndPose(1, 3) * sin(Results(i, 0));
double N2 = -d5 * B234 - d6 * sin(Results(i, 4)) * A234 - d1 + EndPose(2, 3);
double L2 = (M2 * M2 + N2 * N2 + a2 * a2 - a3 * a3) / (2 * a2);
if (i % 2 == 0) {
Results(i, 1) = atan2(N2, M2) - atan2(L2, sqrt(M2 * M2 + N2 * N2 - L2 * L2));
} else {
Results(i, 1) = atan2(N2, M2) - atan2(L2, -sqrt(M2 * M2 + N2 * N2 - L2 * L2));
}
double A23 = (-M2 - a2 * sin(Results(i, 1))) / a3;
double B23 = (N2 - a2 * cos(Results(i, 1))) / a3;
/**
* @brief Get the solution result of Joint3 angle
* @author Liu Qiang
* @date 2019-07-22
*/
Results(i, 2) = Results(i, 1) - atan2(A23, B23);
/**
* @brief Get the solution result of Joint4 angle
* @author Liu Qiang
* @date 2019-07-22
*/
Results(i, 3) = atan2(A234, B234) - atan2(A23, B23);
}
/**
* @brief Limit the joint angle value within -180 or 180 degrees
* @author Liu Qiang
* @date 2019-07-22
*/
for (int i = 0; i < 8; i++) {
for(int j = 0; j < 6; j++) {
if (Results(i, j) > M_PI) {
Results(i, j) = Results(i, j) - 2 * M_PI;
} else if(Results(i, j) < -M_PI) {
Results(i, j) = Results(i, j) + 2 * M_PI;
}
if ((Results(i, j) > 175 * M_PI / 180) || (Results(i, j) < -175 * M_PI / 180)) {
Results(i, 2) = std::numeric_limits<double>::quiet_NaN();
}
}
}
/**
* @brief Remove invalid values
* @author Liu Qiang
* @date 2019-07-22
*/
Result.resize(8, 6);
int j = 0;
for (int i = 0; i < 8; i++) {
if (std::isnan(Results(i, 2))) {
count--;
} else {
Result.block(j, 0, 1, 6) = Results.block(i, 0, 1, 6);
j++;
}
}
if (count > 0) {
Result.conservativeResize(count, 6); //Resizes the matrix while leaving old values untouched.
}
}
/**
* @brief Return a set of optimal solutions
* @author Liu Qiang
* @date 2019-07-22
*/
Eigen::Matrix<double, 1, 6> AuboClosedFormIKSolver::solve(Eigen::Matrix<double, 1, 6> theta) {
solve();
int row = static_cast<int>(Result.rows());
Eigen::MatrixXd dis;
dis.resize(1, row);
for (int i = 0; i < row; i++) {
Eigen::Matrix<double, 1, 6> delta = theta - Result.block(i, 0, 1, 6);
dis(i) = delta.norm();
}
int rowID, colID;
dis.minCoeff(&rowID, &colID);
return Result.block(colID, 0, 1, 6);
}
基于给定的一组关节角[-23.140575, -14.042009, 109.599879, 38.842449, 9.966328, 53.998950]，实际上是在Aubo-i10机器人的示教器上获取的。计算出一组最优解，可以看出有一组的数值被赋为nan，因为它超出了机器人关节角的限制。
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
