6轴机械臂正逆解运算实现
6轴机械臂正逆解运算实现
利用Gluon-6L3机械臂模型的参数，对机械臂进行运动学分析。
这里采用标准DH坐标系，并将d6设置为0，方便后续计算。
首先，SDH的变换矩阵为：
i i − 1 T = A i = ^{i-1}_iT=A_i= ii−1T=Ai= [ c i − s i c α i s i s α i a i c i s i c i c α i − c i s α i a i s i 0 s α i c α i d i 0 0 0 1 ] \begin{bmatrix} c_i & -s_ic\alpha_i & s_is\alpha_i& a_ic_i \\ s_i & c_ic\alpha_i & -c_is\alpha_i& a_is_i\\ 0 & s\alpha_i & c\alpha_i&d_i\\ 0&0&0&1\end{bmatrix} ⎣⎢⎢⎡cisi00−sicαicicαisαi0sisαi−cisαicαi0aiciaisidi1⎦⎥⎥⎤
我们将机械臂参数表中的数据带入，得到6个变换矩阵
1 0 T = ^{0}_1T= 10T= [ c 1 0 s 1 0 s 1 0 − c 1 0 0 1 0 d 1 0 0 0 1 ] \begin{bmatrix} c_1& 0 & s_1 &0\\ s_1 & 0 & -c_1 & 0\\ 0 & 1 & 0 &d_1\\ 0 &0 &0 &1 \end{bmatrix} ⎣⎢⎢⎡c1s1000010s1−c10000d11⎦⎥⎥⎤ 2 1 T = ^{1}_2T= 21T= [ c 2 − s 2 0 a 2 c 2 s 2 c 2 0 a 2 s 2 0 0 1 0 0 0 0 1 ] \begin{bmatrix} c_2& -s_2 & 0 &a_2c_2\\ s_2 & c_2 & 0 & a_2s_2\\ 0 & 0 & 1 &0\\ 0 &0 &0 &1 \end{bmatrix} ⎣⎢⎢⎡c2s200−s2c2000010a2c2a2s201⎦⎥⎥⎤
3 2 T = ^{2}_3T= 32T= [ c θ 3 − s θ 3 0 a 3 c θ 3 s θ 3 c θ 3 0 a 3 s θ 3 0 0 1 0 0 0 0 1 ] \begin{bmatrix} c\theta_3& -s\theta_3 & 0 &a_3c\theta_3\\ s\theta_3 & c\theta_3 & 0 & a_3s\theta_3\\ 0 & 0 & 1 &0\\ 0 &0 &0 &1 \end{bmatrix} ⎣⎢⎢⎡cθ3sθ300−sθ3cθ3000010a3cθ3a3sθ301⎦⎥⎥⎤ 4 3 T = ^{3}_4T= 43T= [ c θ 4 0 s θ 4 0 s θ 4 0 − c θ 4 0 0 1 0 d 4 0 0 0 1 ] \begin{bmatrix} c\theta_4& 0 & s\theta_4 &0\\ s\theta_4 & 0 & -c\theta_4 & 0\\ 0 & 1 & 0 &d_4\\ 0 &0 &0 &1 \end{bmatrix} ⎣⎢⎢⎡cθ4sθ4000010sθ4−cθ40000d41⎦⎥⎥⎤
5 4 T = ^{4}_5T= 54T= [ c θ 5 0 − s θ 5 0 s θ 5 0 c θ 5 0 0 − 1 0 d 5 0 0 0 1 ] \begin{bmatrix} c\theta_5& 0 & -s\theta_5 &0\\ s\theta_5 & 0 & c\theta_5 & 0\\ 0 & -1 & 0 &d_5\\ 0 &0 &0 &1 \end{bmatrix} ⎣⎢⎢⎡cθ5sθ50000−10−sθ5cθ50000d51⎦⎥⎥⎤ 6 5 T = ^{5}_6T= 65T= [ c θ 6 − s θ 6 0 0 s θ 6 c θ 6 0 0 0 0 1 0 0 0 0 1 ] \begin{bmatrix} c\theta_6& -s\theta_6 & 0 &0\\ s\theta_6 & c\theta_6 & 0 & 0\\ 0 & 0 & 1 &0\\ 0 &0 &0 &1 \end{bmatrix} ⎣⎢⎢⎡cθ6sθ600−sθ6cθ60000100001⎦⎥⎥⎤
机械臂的位姿矩阵T为：
T = 1 0 T 2 1 T 3 2 T 4 3 T 5 4 T 6 5 T = T=^{0}_1T^{1}_2T^{2}_3T^{3}_4T^{4}_5T^{5}_6T= T=10T21T32T43T54T65T= [ 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 ] \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} ⎣⎢⎢⎡nxnynz0oxoyoz0axayaz0pxpypz1⎦⎥⎥⎤
n x = c 1 c 5 c 6 c 234 − c 1 s 6 s 234 + s 1 s 5 s 6 n_x=c_1c_5c_6c_{234}-c_1s_6s_{234}+s_1s_5s_6 nx=c1c5c6c234−c1s6s234+s1s5s6
n y = s 1 c 5 c 6 c 234 − c 1 c 6 c 6 − s 1 s 5 s 234 n_y=s_1c_5c_6c_{234}-c_1c_6c_{6}-s_1s_5s_{234} ny=s1c5c6c234−c1c6c6−s1s5s234
n z = c 5 c 6 s 234 + s 6 c 234 n_z=c_5c_6s_{234}+s_6c_{234} nz=c5c6s234+s6c234
o x = − c 1 c 5 s 6 c 234 − s 1 s 5 s 6 − c 1 c 6 s 234 o_x=-c_1c_5s_6c_{234}-s_1s_5s_6-c_1c_6s_{234} ox=−c1c5s6c234−s1s5s6−c1c6s234
o y = − s 1 c 5 s 6 c 234 + c 1 c 5 s 6 − s 1 c 6 s 234 o_y=-s_1c_5s_6c_{234}+c_1c_5s_6-s_1c_6s_{234} oy=−s1c5s6c234+c1c5s6−s1c6s234
o z = c 6 c 234 − s 6 c 5 s 234 o_z=c_6c_{234}-s_6c_5s_{234} oz=c6c234−s6c5s234
a x = c 5 s 1 − c 1 c 5 c 234 a_x=c_5s_1-c_1c_5c_{234} ax=c5s1−c1c5c234
a y = − c 5 c 1 − s 1 s 5 c 234 a_y=-c_5c_1-s_1s_5c_{234} ay=−c5c1−s1s5c234
a z = − s 5 s 234 a_z=-s_5s_{234} az=−s5s234
p x = c 1 d 5 s 234 + s 1 d 4 + a 3 c 1 c 23 + a 2 c 2 c 1 p_x=c_1d_5s_{234}+s_1d_4+a_3c_1c_{23}+a_2c_2c_1 px=c1d5s234+s1d4+a3c1c23+a2c2c1
p y = s 1 d 5 s 234 − c 1 d 4 + a 3 s 1 c 23 + a 2 c 2 s 1 p_y=s_1d_5s_{234}-c_1d_4+a_3s_1c_{23}+a_2c_2s_1 py=s1d5s234−c1d4+a3s1c23+a2c2s1
p z = − c 234 d 5 + a 3 s 23 + a 2 s 2 + d 1 p_z=-c_{234}d_5+a_3s_{23}+a_2s_2+d_1 pz=−c234d5+a3s23+a2s2+d1
运动学正解
Euler Angles-由T推算按angles
bata = atan2(sqrt(pow(T.matric[2][0], 2) + pow(T.matric[2][1], 2)), T.matric[2][2]);
alpha = atan2(T.matric[1][2] / sin(bata), T.matric[0][2] / sin(bata));
gama = atan2(T.matric[2][1] / sin(bata), -T.matric[2][0] / sin(bata));
运动学逆解
将末端位姿输入，反求T中的元素
CaculateThta(double x, double y, double z, double alpha, double bata, double gama)
根据末端位姿写出的T矩阵
（根据台大机器人运动学讲解P11，P12写的T,详细了解https://www.bilibili.com/video/BV1v4411H7ez）
T = [ c α c β c γ − s α s γ − c α c β s γ − s α c γ c α s β p x s α c β c γ + c α s γ − s α c β c γ + c α c γ s α s β p y − s β c γ s β s γ c β p z 0 0 0 1 ] T=\begin{bmatrix} c_\alpha c_\beta c_\gamma-s_\alpha s_\gamma& -c_\alpha c_\beta s_\gamma-s_\alpha c_\gamma &c_\alpha s_\beta &p_x\\ s_\alpha c_\beta c_\gamma+c_\alpha s_\gamma& -s_\alpha c_\beta c_\gamma+c_\alpha c_\gamma& s_\alpha s_\beta & p_y \\ -s_\beta c_\gamma& s_\beta s_\gamma &c_\beta & p_z \\ 0 &0 &0 &1 \end{bmatrix} T=⎣⎢⎢⎡cαcβcγ−sαsγsαcβcγ+cαsγ−sβcγ0−cαcβsγ−sαcγ−sαcβcγ+cαcγsβsγ0cαsβsαsβcβ0pxpypz1⎦⎥⎥⎤ = [ 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 ] \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} ⎣⎢⎢⎡nxnynz0oxoyoz0axayaz0pxpypz1⎦⎥⎥⎤
首先，我们对矩阵 1 0 T ^{0}_1T 10T进行逆变换
1 0 T − 1 T = 2 1 T 3 2 T 4 3 T 5 4 T 6 5 T = [ c 5 c 6 c 234 − s 6 s 234 − c 5 s 6 c 234 − c 6 s 234 − s 5 c 234 d 5 s 234 + a 3 c 23 + a 2 c 2 c 5 c 6 s 234 − s 6 c 234 − c 5 s 6 s 234 + c 6 c 234 − s 5 s 234 − d 5 c 234 + a 3 s 23 + a 2 s 2 c 6 s 5 − s 6 s 5 c 5 d 4 0 0 0 1 ] = ^{0}_1T^{-1}T=^{1}_2T^{2}_3T^{3}_4T^{4}_5T^{5}_6T=\begin{bmatrix} c_5c_6c_{234}-s_6s_{234}& -c_5s_6c_{234}-c_6s_{234} & -s_5c_{234} &d_5s_{234}+a_3c_{23}+a_2c_2\\ c_5c_6s_{234}-s_6c_{234}& -c_5s_6s_{234}+c_6c_{234} & -s_5s_{234} &-d_5c_{234}+a_3s_{23}+a_2s_2\\ c_6s_5 &-s_6s_5 & c_5 &d_4\\ 0 &0 &0 &1 \end{bmatrix}= 10T−1T=21T32T43T54T65T=⎣⎢⎢⎡c5c6c234−s6s234c5c6s234−s6c234c6s50−c5s6c234−c6s234−c5s6s234+c6c234−s6s50−s5c234−s5s234c50d5s234+a3c23+a2c2−d5c234+a3s23+a2s2d41⎦⎥⎥⎤= [ c 1 n x + s 1 n y c 1 o x + s 1 o y c 1 a x + s 1 a y c 1 p x + s 1 p y n z o z a z p y − d 1 s 1 n x − c 1 n y s 1 o x − c 1 o y s 1 a x − c 1 a y s 1 p x − c 1 p y 0 0 0 1 ] \begin{bmatrix} c_1n_x+s_1n_y& c_1o_x+s_1o_y & c_1a_x+s_1a_y &c_1p_x+s_1p_y\\ n_z & o_z& a_z & p_y-d_1 \\ s_1n_x-c_1n_y& s_1o_x-c_1o_y & s_1a_x-c_1a_y &s_1p_x-c_1p_y\\ 0 &0 &0 &1 \end{bmatrix} ⎣⎢⎢⎡c1nx+s1nynzs1nx−c1ny0c1ox+s1oyozs1ox−c1oy0c1ax+s1ayazs1ax−c1ay0c1px+s1pypy−d1s1px−c1py1⎦⎥⎥⎤
到这一步某些 θ \theta θ角已经现形了，这里计算尽量统一位atan2。
计算 θ 1 \theta_1 θ1
d 4 = s 1 p x − c 1 p y d_4=s_1p_x-c_1p_y d4=s1px−c1py (1)
令 p x = c o s ϕ , p y = s i n ϕ p_x=cos\phi,p_y=sin\phi px=cosϕ,py=sinϕ带入(1)得
s i n ( θ 1 − ϕ ) = d 4 sin(\theta_1-\phi)=d_4 sin(θ1−ϕ)=d4
c o s ( θ 1 − ϕ ) = ± 1 − d 4 2 cos(\theta_1-\phi)=\pm\sqrt{1-d_4^2} \quad cos(θ1−ϕ)=±1−d42
θ 1 = a t a n 2 ( p y , p x ) + a t a n 2 ( d 4 , ± 1 − d 4 2 ) \theta_1=atan2(p_y,p_x)+atan2(d_4,\pm\sqrt{1-d_4^2} \quad) θ1=atan2(py,px)+atan2(d4,±1−d42)
计算 θ 5 \theta_{5} θ5
已知 c 5 = s 1 a x − c 1 a y c_{5}=s_1a_x-c_1a_y c5=s1ax−c1ay
s 5 = ± 1 − c 5 2 s_{5}=\pm\sqrt{1-c_5^2} \quad s5=±1−c52
θ 5 = a t a n 2 ( s 5 , c 5 ) \theta_{5}=atan2(s_{5},c_{5}) θ5=atan2(s5,c5)
计算 θ 6 \theta_6 θ6
c 6 s 5 = s 1 n x − c 1 n y c_6s_5=s_1n_x-c_1n_y c6s5=s1nx−c1ny
− s 6 s 5 = s 1 o x − c 1 o y -s_6s_5=s_1o_x-c_1o_y −s6s5=s1ox−c1oy
θ 6 = a t a n 2 ( ( s 1 n x − c 1 n y ) s 5 , ( s 1 o x − c 1 o y ) − s 5 ) \theta_6=atan2(\frac{(s_1n_x-c_1n_y)}{s_5} \quad,\frac{(s_1o_x-c_1o_y)}{-s_5} \quad) θ6=atan2(s5(s1nx−c1ny),−s5(s1ox−c1oy))
计算 θ 3 \theta_3 θ3
已知 s 234 = a z − s 5 , c 234 = c 1 a x + s 1 a y − s 5 s_{234}=\frac{a_z}{-s_5} \quad,c_{234}=\frac{c_1a_x+s_1a_y}{-s_5} \quad s234=−s5az,c234=−s5c1ax+s1ay（2）
已知 d 5 s 234 + a 3 c 23 + a 2 c 2 = c 1 p x + s 1 p y d_5s_{234}+a_3c_{23}+a_2c_2=c_1p_x+s_1p_y d5s234+a3c23+a2c2=c1px+s1py（3）
已知 − d 5 c 234 + a 3 s 23 + a 2 s 2 = p z − d 1 -d_5c_{234}+a_3s_{23}+a_2s_2=p_z-d_1 −d5c234+a3s23+a2s2=pz−d1（4）
令 k 1 = c 1 p x + s 1 p y − d 5 s 234 = a 3 c 23 + a 2 c 2 , k 2 = p z − d 1 + d 5 c 234 = a 3 s 23 + a 2 s 2 k_1=c_1p_x+s_1p_y-d_5s_{234}=a_3c_{23}+a_2c_2 ,k_2=p_z-d_1+d_5c_{234}=a_3s_{23}+a_2s_2 k1=c1px+s1py−d5s234=a3c23+a2c2,k2=pz−d1+d5c234=a3s23+a2s2
c 3 = k 1 2 + k 2 2 − a 2 2 − a 3 3 2 a 2 a 3 c_3=\frac{k_1^2+k_2^2-a_2^2-a_3^3}{2a_2a_3} \quad c3=2a2a3k12+k22−a22−a33
s 3 = ± 1 − c 3 2 s_3=\pm\sqrt{1-c_3^2} \quad s3=±1−c32
θ 3 = a t a n 2 ( s 3 , c 3 ) \theta_{3}=atan2(s_{3},c_{3}) θ3=atan2(s3,c3)
计算 θ 2 \theta_2 θ2
继续利用上面三个公式（2）（3）（4）求解 θ 2 \theta_2 θ2
s 2 = ( k 2 ( a 2 + a 3 c 3 ) − k 1 a 3 s 3 ) a 2 2 + a 3 2 + 2 a 1 a 2 c 3 s_2=\frac{(k_2(a_2+a_3c_3)-k_1a_3s_3)}{a_2^2+a_3^2+2a_1a_2c_3} \quad s2=a22+a32+2a1a2c3(k2(a2+a3c3)−k1a3s3)
c 2 = k 2 − s 2 ( a 2 + a 3 c 3 ) a 3 s 3 c_2=\frac{k_2-s_2(a_2+a_3c_3)}{a_3s_3} \quad c2=a3s3k2−s2(a2+a3c3)
θ 2 = a t a n 2 ( s 2 , c 2 ) \theta_{2}=atan2(s_{2},c_{2}) θ2=atan2(s2,c2)
计算 θ 4 \theta_4 θ4
s 234 = s 4 c 23 + s 23 c 4 s_{234}=s_4c_{23}+s_{23}c_4 s234=s4c23+s23c4
c 234 = c 4 c 23 − s 23 s 4 c_{234}=c_4c_{23}-s_{23}s_4 c234=c4c23−s23s4
两式联立得
s 4 = c 23 s 234 − s 23 c 234 s_4=c_{23}s_{234}-s_{23}c_{234} s4=c23s234−s23c234
c 4 = c 234 + s 4 s 23 c 23 c_4=\frac{c_{234}+s_{4}s_{23}}{c_{23}} \quad c4=c23c234+s4s23
θ 4 = a t a n 2 ( s 4 , c 4 ) \theta_{4}=atan2(s_{4},c_{4}) θ4=atan2(s4,c4)
至此，六个角全部求完，根据上式子的表达一共八组解