遨博Aubo-i10机器人正逆运动学公式推导及其C++编程实现

小强的机器人工坊

9118人浏览 · 2019-07-23 09:11:33

小强的机器人工坊 · 2019-07-23 09:11:33 发布

最近在项目中用到了Aubo-i10机器人，遨博机器人跟UR机器人有很多共同点，都是模块化的协作机器人，它们构形相同。但是这两家公司的机器人还是有区别的，比如Aubo机器人的第三个关节是反着转的，跟UR机器人不一样。网上有关于UR机器人的逆运动学推导过程，借鉴一番之后，自己尝试着推导Aubo-i10机器人的正逆运动学，并通过C++编程实现了该求解算法，最后通过记录示教器上的数据进行验证，实验结果表明，关于Aubo-i10机器人的公式推导过程是正确的。
首先，基于遨博官网上给出的Aubo-i10机器人数据，使用标准的DH建模方法建立机器人的运动学参数模型，当然也可以使用改进的DH法来建模，区别只是在 $i$ 坐标系或 $i - 1$ 坐标系上 $x$ 轴的确定不同。

在这里插入图片描述
标准DH参数表：

Transform	$a$	$α\alpha$	$d$	$θ\theta$
0~1	0	$90∘90^{\circ}$	$d_{1}=0.163$	$θ1=0∘\theta_1=0^{\circ}$
1~2	$a_{2}=0.647$	$180∘180^{\circ}$	0	$θ2=90∘\theta_2=90^{\circ}$
2~3	$a_{3}=0.6005$	$180∘180^{\circ}$	0	$θ3=0∘\theta_3=0^{\circ}$
3~4	0	$−90∘-90^{\circ}$	$d_{4}=0.2013$	$θ4=−90∘\theta_4=-90^{\circ}$
4~5	0	$90∘90^{ \circ}$	$d_5=0.1025$	$θ5=0∘\theta_5=0^{\circ}$
5~6	0	$0∘0^{\circ}$	$d_{6}=0.094$	$θ6=0∘\theta_6=0^{\circ}$

标准DH法中相邻坐标系之间的齐次变换关系：
在这里插入图片描述 $^{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}}$

$^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}$

$^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}$

$^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}$

$^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}$

$^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}$

$^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}$

$^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]$

$^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]$

$^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]$

求解 $θ1\theta_{1}$

联立公式 $61T^1_{6}T$ 中的(3, 3)和(3, 4)元素左右相等，有：
$\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 .$
消除 $θ5\theta_{5}$ 有：
$\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}$
由半角正切公式有：

令 $A_{1}=d_{6}r_{23}-p_{y}$ , $B_{1}=p_{x}-d_{6}r_{13}$ , $C_{1}=d_{4}$

有：
$\theta_{1}=Atan2(C_{1}, \pm \sqrt {A_{1}^{2}+B_{1}^{2}-C_{1}^{2}}) - Atan2(A_{1}, B_{1})$
求解 $θ5\theta_{5}$

由公式 $61T^1_{6}T$ 中的(3, 3)元素左右等式相等，有：
$\left( \theta_{5} \right)=r_{13}sin \left(\theta_{1}\right) - r_{23} cos\left( \theta_{1}\right)$
令 $b=r13sin(θ1)−r23cos(θ1)b=r_{13}sin \left(\theta_{1}\right) - r_{23} cos\left( \theta_{1}\right)$ , 有 $cosθ5=bcos\theta_{5}=b$

由半角正切公式有：
$\theta_{5}=Atan2(\pm \sqrt{1-b^2}, b)$
求解 $θ6\theta_{6}$

由公式 $61T^1_{6}T$ 中的 (3, 1) 和 (3, 2) 元素左右等式相等，有：
$\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 .$
当 $θ5≠0\theta_{5} \neq 0$ 时，有：
$\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 .$
由半角正切公式得
$\theta_{6}=Atan2(A_{6}, B_{6})$
当 $sin(θ5)=0sin(\theta_{5})=0$ 时，机器人处于奇异位形，此时关节6轴与2,3,4关节轴平行，机器人存在无数组解。

求解 $θ2\theta_{2}$

由公式 $61T^1_{6}T$ 中的(1, 3) 和(2, 3)元素左右等式相等，有：
$\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 .$
当 $θ5≠0\theta_{5} \neq 0$ 时，有：
$\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 .$
由半角正切公式，有：
$\theta_{2}-\theta_{3}+\theta_{4}=Atan2(A_{234}, B_{234})$
由等式 $61T^1_{6}T$ 的 (1, 4) 和 (2, 4) 元素左右等式相等，有：
$\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 .$
接着分别定义 $M$ 和 $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 .$

$\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^{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})$

$-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$

$M^{2}+N^{2}-a^{2}_{2}-a^{2}_{3}=2(-Msin(\theta_{2})+Ncos(\theta_{2})-a_{2})$

$-Msin(\theta_{2})+Ncos(\theta_{2})=L=\frac{M^{2}+N^{2}+a_{2}^{2}-a_{3}^{2}}{2a_{2}}$

根据半角正切公式，有：
$\theta_{2}=Atan2(N, M) - Atan2(L, \pm\sqrt{M^{2}+N^{2}-L^{2}})$
求解 $θ3\theta_{3}$

由于：
$\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 .$
由半正切公式有：
$\theta_{2}-\theta_{3}=Atan2(A_{23}, B_{23})$
故 :
$\theta_{3}=\theta_{2}-Atan2(A_{23}, B_{23})$
求解 $θ4\theta_{4}$
$\theta_{4}=Atan2(A_{234}, B_{234})-Atan2(A_{23}, B_{23})$

上述公式符号推导所用到的代码：

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，因为它超出了机器人关节角的限制。
在这里插入图片描述