求解PUMA560 六轴机械臂运动学

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

一、机器人模型

PUMA560：

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

本文使用的模型：

D-H矩阵：

二、机器人正运动学

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

位姿矩阵

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

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

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

 

正运动学

$$\begin{gathered} T_6^0=T_1^0{T}_2^1{T}_3^2{T}_4^3{T}_5^4{T}_6^5 \\ {T}_k^i为k关节坐标系在i关节坐标系下的描述. \end{gathered}$$

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

$$\begin{array}{rl}{n}_x& =c_1[c_{23}(c_4{c}_5{c}_6+s_4{s}_6)-s_{23}{s}_5{c}_6]+s_1(s_4{c}_5{c}_6-c_4{s}_6)\\ n_y& =s_1[c_{23}(c_4{c}_5{c}_6+s_4{s}_6)-s_{23}{s}_5{c}_6]-c_1(s_4{c}_5{c}_6-c_4{s}_6)\\ n_z& =-s_{23}{c}_4{c}_5{s}_6-c_{23}{s}_5{c}_6-s_{23}{s}_4{s}_6\\ o_x& =c_1[c_{23}(s_4{c}_6-c_4{c}_5{s}_6)+s_{23}{s}_5{s}_6]-s_1(s_4{c}_5{s}_6+c_4{c}_6)\\ o_y& =s_1[c_{23}(s_4{c}_6-c_4{c}_5{s}_6)+s_{23}{s}_5{s}_6]+c_1(s_4{c}_5{s}_6+c_4{c}_6)\\ o_z& =s_{23}{c}_4{c}_5{s}_6+c_{23}{s}_5{s}_6-s_{23}{s}_4{c}_6\\ a_x& =c_1(c_{23}{c}_4{s}_5+s_{23}{c}_5)+s_1{s}_4{s}_5\\ a_y& =s_1(c_{23}{c}_4{s}_5+s_{23}{c}_5)-c_1{s}_4{s}_5\\ a_z& =-c_4{s}_5{s}_{23}+c_{23}{c}_5\\ p_x& =c_1(a_3{c}_{23}+a_2{c}_2+a_1-d_4{s}_{23})\\ p_y& =s_1(a_3{c}_{23}+a_2{c}_2+a_1-d_4{s}_{23})\\ p_z& =-d_4{c}_{23}-a_3{s}_{23}-a_2{s}_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{array}$$

 

三、机器人逆运动学

 

求解关节一

$${\theta }_1=atan2(p_y,p_x)-atan2(0,\pm \sqrt{p{x}^2+p{y}^2});$$

 

求解关节三

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

 

求解关节二

$$\begin{array}{rl}& 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)}\\ & \\ & {\theta }_2=atan2(k_1,k_2)-{\theta }_3\\ & {\theta }_2的有效区间为(\frac{-3\pi }{2},\frac{\pi }{2}],且周期为2\pi ，因此，\\ & 当\theta >\pi /2时，取\theta =\theta -2\pi ,当\theta \le \frac{-3\pi }{2}时，取\theta =\theta +2\pi .\end{array}$$

 

求解关节四

$$\begin{array}{rl}& 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\\ & \\ & 当sin({\theta }_5)\ne 0时，\\ & {\theta }_4=atan2(k_1,k_2)\\ & {\theta }_4的有效区间为(-\pi ,\pi ]，因此，当{\theta }_4>0时，取{\theta }_{42}={\theta }_4-\pi ,否则，取{\theta }_{42}={\theta }_4+\pi .\end{array}$$

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

 

求解关节五

$$\begin{array}{rl}& k_1=(sin({\theta }_4)sin({\theta }_1)+cos({\theta }_1)cos({\theta }_4)cos({\theta }_2+{\theta }_3))a_x\\ & +(cos({\theta }_4)sin({\theta }_1)cos({\theta }_2+{\theta }_3))a_y\\ & -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\\ & \\ & {\theta }_5=atan2(k_1,-k_2)\end{array}$$

 

求解关节六

k 1 = ( s i n ( θ 4 ) s i n ( θ 1 ) c o s ( θ 5 ) − c o s ( θ 1 ) s i n ( θ 5 ) s i n ( θ 2 + θ 3 ) + c o s ( θ 1 ) c o s ( θ 4 ) c o s ( θ 2 + θ 3 ) c o s ( θ 5 ) ) o x − ( c o s ( θ 1 ) s i n ( θ 4 ) c o s ( θ 5 ) + s i n ( θ 1 ) s i n ( θ 5 ) s i n ( θ 2 + θ 3 ) − s i n ( θ 1 ) c o s ( θ 4 ) c o s ( θ 5 ) c o s ( θ 2 + θ 3 ) ) o y + ( s i n ( θ 2 ) s i n ( θ 3 ) s i n ( θ 5 ) − c o s ( θ 2 ) c o s ( θ 3 ) s i n ( θ 5 ) − c o s ( θ 4 ) c o s ( θ 5 ) s i n ( θ 2 + θ 3 ) ) o z k 2 = ( c o s ( θ 1 ) s i n ( θ 4 ) c o s ( θ 2 + θ 3 ) − c o s ( θ 4 ) s i n ( θ 1 ) ) o x + ( c o s ( θ 1 ) c o s ( θ 4 ) + s i n ( θ 1 ) s i n ( θ 4 ) c o s ( θ 2 + θ 3 ) ) o y − s i n ( θ 2 + θ 3 ) s i n ( θ 4 ) o z θ 6 = a t a n 2 ( − k 1 , k 2 ) θ 6 周 期 为 2 π , 当 θ 6 > 0 时 ， 取 θ 62 = θ 6 − 2 π , 否 则 ， 取 θ 62 = θ 6 + 2 π . \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} ​k1​=(sin(θ4​)sin(θ1​)cos(θ5​)−cos(θ1​)sin(θ5​)sin(θ2​+θ3​)+cos(θ1​)cos(θ4​)cos(θ2​+θ3​)cos(θ5​))ox​−(cos(θ1​)sin(θ4​)cos(θ5​)+sin(θ1​)sin(θ5​)sin(θ2​+θ3​)−sin(θ1​)cos(θ4​)cos(θ5​)cos(θ2​+θ3​))oy​+(sin(θ2​)sin(θ3​)sin(θ5​)−cos(θ2​)cos(θ3​)sin(θ5​)−cos(θ4​)cos(θ5​)sin(θ2​+θ3​))oz​k2​=(cos(θ1​)sin(θ4​)cos(θ2​+θ3​)−cos(θ4​)sin(θ1​))ox​+(cos(θ1​)cos(θ4​)+sin(θ1​)sin(θ4​)cos(θ2​+θ3​))oy​−sin(θ2​+θ3​)sin(θ4​)oz​θ6​=atan2(−k1​,k2​)θ6​周期为2π,当θ6​>0时，取θ62​=θ6​−2π,否则，取θ62​=θ6​+2π.​

 
 

以上过程共解出8组解.