求解PUMA560 六軸機械臂運動學

最近需要解算六軸機器人的解析解，算法已完成，這裏記錄一下.

一、機器人模型

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組解.