UR机械臂逆运动学解析解
UR机械臂逆运动学解析解
ur10机械臂的DH（修正）参数为
d1=0.1273;a2=-0.612;a3=-0.5723;d4=0.163941;d5=0.1157;d6=0.6922;
首先，推导机械臂的正向运动学方程，根据DH参数可以计算机械臂的正向运动学方程表示为：
0 T 6 = 0 T 1 1 T 2 2 T 3 3 T 4 4 T 5 5 T 6 = [ 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 ] {}^0T_6={}^0T_1 {}^1T_2 {}^2T_3 {}^3T_4 {}^4T_5 {}^5T_6=\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] 0T6=0T11T22T33T44T55T6=⎣⎢⎢⎡nxnynz0oxoyoz0axayaz0pxpypz1⎦⎥⎥⎤
其中
0 T 1 = [ c 1 − s 1 0 0 s 1 c 1 0 0 0 0 1 d 1 0 0 0 1 ] {}^0T_1 =\left[ \begin{array}{cccc} c1 & -s1 &0 &0\\ s1&c1 &0 &0\\ 0 &0 & 1&d_1\\ 0 &0 & 0&1 \end{array} \right] 0T1=⎣⎢⎢⎡c1s100−s1c100001000d11⎦⎥⎥⎤， 1 T 2 = [ c 2 − s 2 0 0 0 0 − 1 0 s 2 c 2 0 0 0 0 0 1 ] {}^1T_2 =\left[ \begin{array}{cccc} c2 & -s2 &0 &0\\ 0 &0 & -1&0\\ s2&c2 &0 &0\\ 0 &0 & 0&1 \end{array} \right] 1T2=⎣⎢⎢⎡c20s20−s20c200−1000001⎦⎥⎥⎤，
2 T 3 = [ c 3 − s 3 0 a 2 s 3 c 3 0 0 0 0 1 0 0 0 0 1 ] {}^2T_3 =\left[ \begin{array}{cccc} c3 & -s3 &0 &a_2\\ s3&c3 &0 &0\\ 0 &0 & 1&0\\ 0 &0 & 0&1 \end{array} \right] 2T3=⎣⎢⎢⎡c3s300−s3c3000010a2001⎦⎥⎥⎤， 3 T 4 = [ c 4 − s 4 0 a 3 s 4 c 4 0 0 0 0 1 d 4 0 0 0 1 ] {}^3T_4 =\left[ \begin{array}{cccc} c4 & -s4 &0 &a_3\\ s4&c4 &0 &0\\ 0 &0 & 1&d_4\\ 0 &0 & 0&1 \end{array} \right] 3T4=⎣⎢⎢⎡c4s400−s4c4000010a30d41⎦⎥⎥⎤，
4 T 5 = [ c 5 − s 5 0 0 0 0 − 1 − d 5 s 5 c 5 0 0 0 0 0 1 ] {}^4T_5 =\left[ \begin{array}{cccc} c5& -s5 &0 &0\\ 0 &0 & -1&-d_5\\ s5&c5 &0 &0\\ 0 &0 & 0&1 \end{array} \right] 4T5=⎣⎢⎢⎡c50s50−s50c500−1000−d501⎦⎥⎥⎤， 5 T 6 = [ c 6 − s 6 0 0 0 0 1 d 6 − s 6 − c 6 0 0 0 0 0 1 ] {}^5T_6 =\left[ \begin{array}{cccc} c6 & -s6 &0 &0\\ 0 &0 & 1&d_6\\ -s6&-c6 &0 &0\\ 0 &0 & 0&1 \end{array} \right] 5T6=⎣⎢⎢⎡c60−s60−s60−c6001000d601⎦⎥⎥⎤
接下来，根据给定的末端位姿 0 T 6 {}^0T_6 0T6求解对应的关节构型，也就是求解逆运动学：
由于关节2，3，4旋转轴方向相同，因此 1 T 4 {}^1T_4 1T4可以表示为
1 T 4 = [ c 234 − s 234 0 a 2 c 2 + a 3 c 23 0 0 − 1 − d 4 s 234 c 234 0 a 2 s 2 + a 3 s 23 0 0 0 1 ] {}^1T_4 =\left[ \begin{array}{cccc} c234 & -s234 &0 &a_2c2+a_3c23\\ 0 &0 & -1&-d_4\\ s234&c234 &0 &a_2s2+a_3s23\\ 0 &0 & 0&1 \end{array} \right] 1T4=⎣⎢⎢⎡c2340s2340−s2340c23400−100a2c2+a3c23−d4a2s2+a3s231⎦⎥⎥⎤
那么 1 T 5 {}^1T_5 1T5可以表示为：
1 T 5 = [ c 234 c 5 − c 234 s 5 s 234 s 234 d 5 + a 2 c 2 + a 3 c 23 − s 5 − c 5 0 − d 4 s 234 c 5 − s 234 s 5 − c 234 − c 234 d 5 + a 2 s 2 + a 3 s 23 0 0 0 1 ] {}^1T_5 =\left[ \begin{array}{cccc} c234c5 & -c234s5 &s234 &s234d_5+a_2c2+a_3c23\\ -s5 &-c5 & 0&-d_4\\ s234c5&-s234s5 &-c234 &-c234d_5+a_2s2+a_3s23\\ 0 &0 & 0&1 \end{array} \right] 1T5=⎣⎢⎢⎡c234c5−s5s234c50−c234s5−c5−s234s50s2340−c2340s234d5+a2c2+a3c23−d4−c234d5+a2s2+a3s231⎦⎥⎥⎤
同时 1 T 5 {}^1T_5 1T5还可以表示为：
1 T 5 = ( 0 T 1 ) − 1 0 T 6 ( 5 T 6 ) − 1 = [ c 1 s 1 0 0 − s 1 c 1 0 0 0 0 1 − d 1 0 0 0 1 ] [ 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 ] [ c 6 0 − s 6 0 − s 6 0 − c 6 0 0 1 0 − d 6 0 0 0 1 ] = [ c 1 n x c 6 − c 1 o x s 6 + s 1 n y c 6 − s 1 o y s 6 c 1 a x + s 1 a y − c 1 n x s 6 − c 1 o x c 6 − s 1 n y s 6 − s 1 o y c 6 − c 1 a x d 6 + c 1 p x − s 1 a y d 6 + s 1 p y − s 1 n x c 6 + s 1 o x s 6 + c 1 n y c 6 − c 1 o y s 6 − s 1 a x + c 1 a y s 1 n x s 6 + s 1 o x c 6 − c 1 n y s 6 − c 1 o y c 6 s 1 a x d 6 − s 1 p x − c 1 a y d 6 + c 1 p y n z c 6 − o z s 6 a z − n z s 6 − o z c 6 − a z d 6 + p z − d 1 0 0 0 1 ] {}^1T_5=({}^0T_1)^{-1} \;{}^0T_6\;({}^5T_6)^{-1}=\left[ \begin{array}{cccc} c1 & s1 &0 &0\\ -s1&c1 &0 &0\\ 0 &0 & 1&-d_1\\ 0 &0 & 0&1 \end{array} \right]\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]\left[ \begin{array}{cccc} c6 & 0&-s6 &0\\ -s6 &0 & -c6&0\\ 0&1 &0 &-d_6\\ 0 &0 & 0&1 \end{array} \right]= \left[ \begin{array}{cccc} c1n_xc6-c1o_xs6+s1n_yc6-s1o_ys6 & c1a_x+s1a_y&-c1n_xs6-c1o_xc6 -s1n_ys6-s1o_yc6&-c1a_xd_6+c1p_x-s1a_yd_6+s1p_y\\ -s1n_xc6+s1o_xs6 +c1n_yc6-c1o_ys6& -s1a_x+ c1a_y&s1n_xs6+s1o_xc6-c1n_ys6-c1o_yc6 &s1a_xd_6-s1p_x-c1a_yd_6+c1p_y\\ n_zc6-o_zs6 & a_z&-n_zs6-o_zc6 &-a_zd_6+p_z-d1\\ 0 &0 & 0&1 \end{array} \right] 1T5=(0T1)−10T6(5T6)−1=⎣⎢⎢⎡c1−s100s1c100001000−d11⎦⎥⎥⎤⎣⎢⎢⎡nxnynz0oxoyoz0axayaz0pxpypz1⎦⎥⎥⎤⎣⎢⎢⎡c6−s6000010−s6−c60000−d61⎦⎥⎥⎤=⎣⎢⎢⎡c1nxc6−c1oxs6+s1nyc6−s1oys6−s1nxc6+s1oxs6+c1nyc6−c1oys6nzc6−ozs60c1ax+s1ay−s1ax+c1ayaz0−c1nxs6−c1oxc6−s1nys6−s1oyc6s1nxs6+s1oxc6−c1nys6−c1oyc6−nzs6−ozc60−c1axd6+c1px−s1ayd6+s1pys1axd6−s1px−c1ayd6+c1py−azd6+pz−d11⎦⎥⎥⎤
通过两个 1 T 5 {}^1T_5 1T5矩阵对比可以得到：
− s 1 ( p x − a x d 6 ) + c 1 ( p y − a y d 6 ) = − d 4 -s1(p_x-a_xd_6)+c1(p_y-a_yd_6)=-d4 −s1(px−axd6)+c1(py−ayd6)=−d4
通过上图的角度关系可以计算 θ 1 \theta_1 θ1
θ 1 = a t a n 2 ( p y − a y d 6 , p x − a x d 6 ) ± a c o s ( d 4 ( p x − a x d 6 ) 2 + ( p y − a y d 6 ) 2 ) + π 2 \theta_1=atan2(p_y-a_yd_6,p_x-a_xd_6)\pm acos(\frac{d4}{\sqrt{(p_x-a_xd_6)^2+(p_y-a_yd_6)^2}})+\frac{\pi}{2} θ1=atan2(py−ayd6,px−axd6)±acos((px−axd6)2+(py−ayd6)2d4)+2π
这里解存在要求 ( p x − a x d 6 ) 2 + ( p y − a y d 6 ) 2 > d 4 \sqrt{(p_x-a_xd_6)^2+(p_y-a_yd_6)^2}>d_4 (px−axd6)2+(py−ayd6)2>d4；
θ 5 = ± a c o s ( p x s 1 − p y c 1 − d 4 d 6 ) \theta_5=\pm acos(\frac{p_xs1-p_yc1-d_4}{d_6}) θ5=±acos(d6pxs1−pyc1−d4)
这里解存在要求 ∣ p x s 1 − p y c 1 − d 4 ∣ ≤ d 6 |p_xs1-p_yc1-d_4|\le d_6 ∣pxs1−pyc1−d4∣≤d6；
θ 6 = a t a n 2 ( − o x s 1 + o y c 1 s 5 , n x s 1 − n y c 1 s 5 ) \theta_6=atan2(\frac{-o_xs1+o_yc1}{s5},\frac{n_xs1-n_yc1}{s5}) θ6=atan2(s5−oxs1+oyc1,s5nxs1−nyc1)
关节234是平行的，通过两个矩阵对比可以得到 − c 234 s 5 = c 1 a x + s 1 a y -c234s5=c1a_x+s1a_y −c234s5=c1ax+s1ay; − s 234 s 5 = a z -s234s5 =a_z −s234s5=az
θ 234 = a t a n 2 ( − a z s 5 , − c 1 a x + s 1 a y s 5 ) \theta_{234}=atan2(-\frac{a_z}{s5},-\frac{c1a_x+s1a_y}{s5}) θ234=atan2(−s5az,−s5c1ax+s1ay)
通过两个矩阵对比还可以得到
a 2 c 2 + a 3 c 23 = A 1 ; A 1 = − c 1 a x d 6 + c 1 p x − s 1 a y d 6 + s 1 p y − s 234 d 5 a_2c2+a_3c23=A_1; A_1=-c1a_xd_6+c1p_x-s1a_yd_6+s1p_y-s234d_5 a2c2+a3c23=A1;A1=−c1axd6+c1px−s1ayd6+s1py−s234d5
a 2 s 2 + a 3 s 23 = A 2 ; A 2 = − a z d 6 + p z − d 1 + c 234 d 5 a_2s2+a_3s23=A_2; A_2=-a_zd_6+p_z-d1+c234d_5 a2s2+a3s23=A2;A2=−azd6+pz−d1+c234d5
上式可以消除23（ c 2 3 2 + s 2 3 2 = 1 c23^2+s23^2=1 c232+s232=1）得到
2 a 2 A 2 s 2 + 2 a 2 A 1 c 2 = A 1 2 + A 2 2 + a 2 2 − a 3 2 2a_2A_2s2+2a_2A_1c2=A_1^2+A_2^2+a_2^2-a_3^2 2a2A2s2+2a2A1c2=A12+A22+a22−a32
根据万能公式可以计算得到
θ 2 = a t a n 2 ( 2 a 2 A 1 , − 2 a 2 A 2 ) − a t a n 2 ( C , ± ( − 2 a 2 A 2 ) 2 + ( 2 a 2 A 1 ) 2 − C 2 ) \theta_2=atan2(2a_2A_1,-2a_2A_2)-atan2(C,\pm \sqrt{(-2a_2A_2)^2+(2a_2A_1)^2-C^2}) θ2=atan2(2a2A1,−2a2A2)−atan2(C,±(−2a2A2)2+(2a2A1)2−C2)
其中 C = A 1 2 + A 2 2 + a 2 2 − a 3 2 C=A_1^2+A_2^2+a_2^2-a_3^2 C=A12+A22+a22−a32;
将 θ 2 \theta_2 θ2代回可以得到
a 3 c 23 = A 1 − a 2 c 2 a_3c23=A_1-a_2c2 a3c23=A1−a2c2
a 3 s 23 = A 2 − a 2 s 2 a_3s23=A_2-a_2s2 a3s23=A2−a2s2
θ 23 = a t a n 2 ( A 2 − a 2 s 2 a 3 , A 1 − a 2 c 2 a 3 ) \theta_{23}=atan2(\frac{A_2-a_2s2}{a_3},\frac{A_1-a_2c2}{a_3}) θ23=atan2(a3A2−a2s2,a3A1−a2c2)
解存在要求： ∣ ∣ a 2 ∣ − ∣ a 3 ∣ ∣ ≤ ( a 2 c 2 + a 3 c 23 ) 2 + ( a 2 s 2 + a 3 s 23 ) 2 ≤ ∣ a 2 ∣ + ∣ a 3 ∣ ||a_2|-|a_3||\le\sqrt{(a_2c2+a_3c23)^2+(a_2s2+a_3s23)^2}\le|a_2|+|a_3| ∣∣a2∣−∣a3∣∣≤(a2c2+a3c23)2+(a2s2+a3s23)2≤∣a2∣+∣a3∣
因此，可以计算得到
θ 3 = θ 23 − θ 2 \theta_3=\theta_{23}-\theta_2 θ3=θ23−θ2
θ 4 = θ 234 − θ 23 \theta_4=\theta_{234}-\theta_{23} θ4=θ234−θ23