最近需要解算六轴机器人的解析解，算法已完成，这里记录一下.

一、机器人模型

PUMA560：

全称：Programmable Universal Manipulation Arm
1978年由Unimation 机器人公司的Victor Scheinman研发.

本文使用的模型：

D-H矩阵：

二、机器人正运动学

机器人正运动学就是给定机器人各关节位置，计算机器人连杆上任意点的位姿.

位姿矩阵

机器人位置和姿态可以由一个方阵描述:

$P\quad= \quad \left\{ \begin{matrix} 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{matrix} \right\}$

其中，n,o,a为末端姿态，p为末端位置.

正运动学

$T^0_6=T^0_1T^1_2T^2_3T^3_4T^4_5T^5_6\\ \quad\\ T^i_k为k关节坐标系在i关节坐标系下的描述.$

对于确定的机器人结构，如上图中所示，每个关节在上一关节的描述是确定的，因此正运动学直接计算即可.

$\begin{aligned} n_x&=c_1[c_{23}(c_4c_5c_6+s_4s_6)-s_{23}s_5c_6]+s_1(s_4c_5c_6-c_4s_6)\\ n_y&=s_1[c_{23}(c_4c_5c_6+s_4s_6)-s_{23}s_5c_6]-c_1(s_4c_5c_6-c_4s_6)\\ n_z&=-s_{23}c_4c_5s_6-c_{23}s_5c_6-s_{23}s_4s_6\\ o_x&=c_1[c_{23}(s_4c_6-c_4c_5s_6)+s_{23}s_5s_6]-s_1(s_4c_5s_6+c_4c_6)\\ o_y&=s_1[c_{23}(s_4c_6-c_4c_5s_6)+s_{23}s_5s_6]+c_1(s_4c_5s_6+c_4c_6)\\ o_z&=s_{23}c_4c_5s_6+c_{23}s_5s_6-s_{23}s_4c_6\\ a_x&=c_1(c_{23}c_4s_5+s_{23}c_5)+s_1s_4s_5\\ a_y&=s_1(c_{23}c_4s_5+s_{23}c_5)-c_1s_4s_5\\ a_z&=-c_4s_5s_{23}+c_{23}c_5\\ p_x&=c_1(a_3c_{23}+a_2c_2+a_1-d_4s_{23})\\ p_y&=s_1(a_3c_{23}+a_2c_2+a_1-d_4s_{23})\\ p_z&=-d_4c_{23}-a_3s_{23}-a_2s_2+d_1\\ 式中：\\ s_i&=sin(\theta_i),c_i=cos(\theta_i),i=1,2,...,6\\ s_{23}&=sin(\theta_2+\theta_3),c_{23}=cos(\theta_2+\theta_3) \end{aligned}$

三、机器人逆运动学

求解关节一

$\theta_1=atan2(p_y,p_x)-atan2(0,\pm\sqrt{px^2+py^2});$

求解关节三

$\begin{aligned} &k_1=cos(\theta_1)p_x+sin(\theta_1)p_y-a_1\\ &k_2=-p_z+d_1\\ &k=\frac{k^2_1+k^2_2-(a^2_2+a^2_3+d^2_4)}{2a_2}\\ &\theta_{31,32}=atan2(a_3,d_4)-atan2(k,\pm\sqrt{a^2_3+d^2_4-k^2})\\ &由上式解出的角度范围是[-\pi,\frac{3\pi}{2}],且周期为2\pi，因此，\\ &当\theta\gt\pi/2时，取\theta=\theta-2\pi,否则，\theta不变. \end{aligned}$

求解关节二

$\begin{aligned} &k_1=\frac{(d_4-sin(\theta_3)a_2)(sin(\theta_1)p_y-a_1+cos(\theta_1)p_x)-(cos(\theta_3)a_2+a_3)(d_1-p_z)}{(a_1-cos(\theta_1)p_x-sin(\theta_1)p_y)(sin(\theta_1)p_y-a_1+cos(\theta_1)p_x)-(d_1-p_z)^2}\\ &k_2=\frac{(d_4-sin(\theta_3)a_2)(d_1-p_z)-(cos(\theta_3)a_2+a_3)(a_1-cos(\theta_1)p_x-sin(\theta_1)p_y)}{(d_1-p_z)^2-(sin(\theta_1)p_y-a_1+cos(\theta_1)p_x)(a_1-cos(\theta_1)p_x-sin(\theta_1)p_y)}\\ \quad\\ &\theta_2=atan2(k_1,k_2)-\theta_3\\ &\theta_2的有效区间为(\frac{-3\pi}{2},\frac{\pi}{2}],且周期为2\pi，因此，\\ &当\theta\gt\pi/2时，取\theta=\theta-2\pi,当\theta\le\frac{-3\pi}{2}时，取\theta=\theta+2\pi. \end{aligned}$

求解关节四

$\begin{aligned} &k_1=sin(\theta_1)a_x-cos(\theta_1)a_y\\ &k_2=cos(\theta_1)cos(\theta_2+\theta_3)a_x+sin(\theta_1)cos(\theta_2+\theta_3)a_y-sin(\theta_2+\theta_3)a_z\\ \quad\\ &当sin(\theta_5)\ne0时，\\ &\theta_4=atan2(k_1,k_2)\\ &\theta_4的有效区间为(-\pi,\pi]，因此， 当\theta_4\gt0时，取\theta_{42}=\theta_4-\pi,否则，取\theta_{42}=\theta_4+\pi.\\ \end{aligned}$

需要从两个解中筛选一个值，通过判断与上一插补值的绝对值大小，选取绝对值小的作为真实解.

求解关节五

$\begin{aligned} &k_1=(sin(\theta_4)sin(\theta_1)+cos(\theta_1)cos(\theta_4)cos(\theta_2+\theta_3))a_x\\ &\quad+(cos(\theta_4)sin(\theta_1)cos(\theta_2+\theta_3))a_y\\ &\quad-sin(\theta_2+\theta_3)cos(\theta_4)a_z\\ &k_2=-sin(\theta_2+\theta_3)cos(\theta_1)a_x-sin(\theta_2+\theta_3)sin(\theta_1)a_y-cos(\theta_2+\theta_3)a_z\\ \quad\\ &\theta_5=atan2(k_1,-k_2)\\ \end{aligned}$

求解关节六

$\begin{aligned} &k_1=(sin(\theta_4)sin(\theta_1)cos(\theta_5)-cos(\theta_1)sin(\theta_5)sin(\theta_2+\theta_3)+cos(\theta_1)cos(\theta_4)cos(\theta_2+\theta_3)cos(\theta_5))o_x\\ &\quad-(cos(\theta_1)sin(\theta_4)cos(\theta_5)+sin(\theta_1)sin(\theta_5)sin(\theta_2+\theta_3)-sin(\theta_1)cos(\theta_4)cos(\theta_5)cos(\theta_2+\theta_3))o_y\\ &\quad+(sin(\theta_2)sin(\theta_3)sin(\theta_5)-cos(\theta_2)cos(\theta_3)sin(\theta_5)-cos(\theta_4)cos(\theta_5)sin(\theta_2+\theta_3))o_z\\ &k_2=(cos(\theta_1)sin(\theta_4)cos(\theta_2+\theta_3)-cos(\theta_4)sin(\theta_1))o_x\\ &\quad+(cos(\theta_1)cos(\theta_4)+sin(\theta_1)sin(\theta_4)cos(\theta_2+\theta_3))o_y\\ &\quad-sin(\theta_2+\theta_3)sin(\theta_4)o_z\\ \quad\\ &\theta_6=atan2(-k_1,k_2)\\ &\theta_6周期为2\pi,当\theta_6\gt0时，取\theta_{62}=\theta_6-2\pi,否则，取\theta_{62}=\theta_6+2\pi. \end{aligned}$

以上过程共解出8组解.