前言

整理之前的一个项目,当时看着一个博客硬生生计算了差不多一个星期。尝试用MatLab符号推导工具箱化简一部分工作。我使用的大象机器人一款开源入门级协作机器人产品myCobot,开发文档十分完善,但是有部分技术没有开源,如正逆运动学(Forward and inverse kinematics, KF/IK),因此我自己尝试实现了。机器人的具体型号:myCobot 280 M5 2020款。

reference

官方文档:https://docs.elephantrobotics.com/docs/gitbook/

UR构型机械臂正逆运动学模型:https://zhuanlan.zhihu.com/p/391202773

6轴机械臂正逆解运算实现:https://blog.csdn.net/weixin_44562141/article/details/108760576

DH参数表

img

myCobot的DH参数为修正DH参数,串联坐标系之间的变换关系为

  1. x轴为转轴逆时针(从箭头方向从上往下看)旋转alpha度;
  2. x轴平移长度a
  3. z轴为转轴逆时针旋转theta+offset度(图中绘制的是theta=0的情形);
  4. 沿z轴平移长度d(单位为mm);

利用上图信息填写参数表如下

Jointalphaadthetaoffset
100131.56theta_10
2PI/200theta_2-PI/2
30-110.40theta_30
40-9664.62theta_4-PI/2
5PI/2073.18theta_5PI/2
6PI/2048.6theta_60

正运动学

为了将旋转运动与平移运动统一描述,引入齐次坐标系。

z轴为转轴逆时针旋转theta+offset度,沿z轴平移长度d,对应的变换矩阵为
R z = [ c o s θ − s i n θ 0 0 s i n θ c o s θ 0 0 0 0 1 d 0 0 0 1 ] R_z=\begin{bmatrix} cos\theta & -sin\theta & 0 & 0\\ sin\theta & cos\theta & 0 & 0\\ 0 & 0 & 1 & d\\ 0 & 0 & 0 & 1\\ \end{bmatrix} Rz= cosθsinθ00sinθcosθ00001000d1

x轴为转轴逆时针旋转alpha度,以x轴平移长度a,对应的变换矩阵为
R x = [ 1 0 0 a 0 c o s α − s i n α 0 0 s i n α c o s α 0 0 0 0 1 ] R_x=\begin{bmatrix} 1 & 0 & 0 & a\\ 0 & cos\alpha & -sin\alpha & 0\\ 0 & sin\alpha & cos\alpha & 0\\ 0 & 0 & 0 & 1\\ \end{bmatrix} Rx= 10000cosαsinα00sinαcosα0a001

串联坐标系 T i − 1 T_{i-1} Ti1 T i T_{i} Ti之间变换描述为
T i i − 1 = R z R x = [ c o s θ i − s i n θ i 0 a i c o s α i s i n θ i c o s α i s i n θ i − s i n α i − d i s i n α i s i n α i c o s θ i s i n α i c o s θ i c o s α i d i c o s α i 0 0 0 1 ] T_{i}^{i-1}=R_zR_x= \begin{bmatrix} cos{\theta_i} & -sin{\theta_i} & 0 & a_i\\ cos{\alpha_i}sin{\theta_i} & cos{\alpha_i}sin{\theta_i} & -sin\alpha_i & -d_isin{\alpha_i}\\ sin{\alpha_i}cos{\theta_i} & sin{\alpha_i}cos{\theta_i} & cos{\alpha_i} & d_icos{\alpha_i}\\ 0 & 0 & 0 & 1\\ \end{bmatrix} Tii1=RzRx= cosθicosαisinθisinαicosθi0sinθicosαisinθisinαicosθi00sinαicosαi0aidisinαidicosαi1
因为对x轴变换在先,将 R x R_x Rx放在右侧。这个矩阵可以描述如何将坐标系 T i − 1 T_{i-1} Ti1变换为 T i T_i Ti,也可以描述将在 T i T_i Ti中的坐标点变换到 T i − 1 T_{i-1} Ti1中,即在坐标系下的表示。

将DH参数表代入上面的变换矩阵,写出每个 T i i − 1 T_{i}^{i-1} Tii1变换矩阵

(符号简化表达: c o s θ i = c i , s i n θ i = s i , s i n ( θ 1 + θ 2 ) = s 12 , c o s ( θ 1 + θ 2 ) = c 12 cos\theta_i=c_i,sin\theta_i=s_i,sin(\theta_1+\theta_2)=s_{12},cos(\theta_1+\theta_2)=c_{12} cosθi=ci,sinθi=si,sin(θ1+θ2)=s12,cos(θ1+θ2)=c12,以此类推)
T 1 0 = [ c 1 − s 1 0 0 s 1 c 1 0 0 0 0 1 d 1 0 0 0 1 ] T 2 1 = [ c 2 − s 2 0 0 0 0 − 1 0 s 2 c 2 0 0 0 0 0 1 ] T 3 2 = [ c 3 − s 3 0 a 3 s 3 c 3 0 0 0 0 1 0 0 0 0 1 ] T 4 3 = [ c 4 − s 4 0 a 4 s 4 c 4 0 0 0 0 1 d 4 0 0 0 1 ] T 5 4 = [ c 5 − s 5 0 0 0 0 − 1 − d 5 s 5 c 5 1 0 0 0 0 1 ] T 6 5 = [ c 6 − s 6 0 0 0 0 1 − d 6 − s 6 − c 6 0 0 0 0 0 1 ] T_{1}^{0}=\begin{bmatrix} c_1 & -s_1 & 0 & 0\\ s_1 & c_1 & 0 & 0\\ 0 & 0 & 1 & d_1\\ 0 & 0 & 0 & 1\\ \end{bmatrix}\\ T_{2}^{1}=\begin{bmatrix} c_2 & -s_2 & 0 & 0\\ 0 & 0 & -1 & 0\\ s_2 & c_2 & 0 & 0\\ 0 & 0 & 0 & 1\\ \end{bmatrix}\\ T_{3}^{2}=\begin{bmatrix} c_3 & -s_3 & 0 & a_3\\ s_3 & c_3 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ \end{bmatrix}\\ T_{4}^{3}=\begin{bmatrix} c_4 & -s_4 & 0 & a_4\\ s_4 & c_4 & 0 & 0\\ 0 & 0 & 1 & d_4\\ 0 & 0 & 0 & 1\\ \end{bmatrix}\\ T_{5}^{4}=\begin{bmatrix} c_5 & -s_5 & 0 & 0\\ 0 & 0 & -1 & -d_5\\ s_5 & c_5 & 1 & 0\\ 0 & 0 & 0 & 1\\ \end{bmatrix}\\ T_{6}^{5}=\begin{bmatrix} c_6 & -s_6 & 0 & 0\\ 0 & 0 & 1 & -d_6\\ -s_6 & -c_6 & 0 & 0\\ 0 & 0 & 0 & 1\\ \end{bmatrix}\\ T10= c1s100s1c100001000d11 T21= c20s20s20c2001000001 T32= c3s300s3c3000010a3001 T43= c4s400s4c4000010a40d41 T54= c50s50s50c5001100d501 T65= c60s60s60c6001000d601
在知道每个电机的转角 θ i \theta_i θi的情况下,每个矩阵是确定的。这里设末端位姿在 T 6 T_6 T6坐标系下的表示为单位矩阵,因此,末端在世界坐标系下的表示为
T 6 0 = T 1 0 T 2 1 T 3 2 T 4 3 T 5 4 T 6 5 I T^{0}_{6}=T_{1}^{0}T_{2}^{1}T_{3}^{2}T_{4}^{3}T_{5}^{4}T_{6}^{5}I T60=T10T21T32T43T54T65I
至此,求出机械臂末端在 T 0 T_0 T0坐标系(通常设为世界坐标系)下的表示,完成机械臂正运动学的求解。

逆运动学

现已知末端的位姿矩阵,但 θ i \theta_i θi未知,因而上面 T i − 1 i T_{i-1}^{i} Ti1i矩阵不确定,得到下面等式
T 6 0 = T 1 0 T 2 1 T 3 2 T 4 3 T 5 4 T 6 5 = [ 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 ] T^{0}_{6}=T_{1}^{0}T_{2}^{1}T_{3}^{2}T_{4}^{3}T_{5}^{4}T_{6}^{5} =\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}\\ T60=T10T21T32T43T54T65= nxnynz0oxoyoz0axayaz0pxpypz1
左右乘以 T 1 0 T_{1}^{0} T10的逆,得
T 6 1 = T 2 1 T 3 2 T 4 3 T 5 4 T 6 5 = [ 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 234 c 5 c 6 − s 234 s 6 c 234 c 5 s 6 − s 234 c 6 − c 234 s 5 a 4 c 23 + a 3 c 2 + d 5 s 234 − d 6 c 234 s 5 − s 5 c 6 s 5 s 6 − c 5 − d 6 c 5 − d 4 s 234 c 5 c 6 + c 234 s 6 c 234 s 6 − s 234 c 5 c 6 − s 234 s 5 a 4 s 23 + a 3 s 2 − d 5 c 234 − d 6 s 234 s 5 0 0 0 1 ] T^{1}_{6}=T_{2}^{1}T_{3}^{2}T_{4}^{3}T_{5}^{4}T_{6}^{5} =\begin{bmatrix} c_1 & s_1 & 0 & 0\\ -s_1 & c_1 & 0 & 0\\ 0 & 0 & 1 & -d_1\\ 0 & 0 & 0 & 1\\ \end{bmatrix} \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}\\ =\begin{bmatrix} c_{234}c_5c_6-s_{234}s_6 & c_{234}c_5s_6-s_{234}c_6 & -c_{234}s_5 & a_4c_{23}+a_3c_2+d_5s_{234}-d_6c_{234}s_{5}\\ -s_5c_6 & s_5s_6 & -c_5 & -d_6c_5-d_4\\ s_{234}c_5c_6+c_{234}s_6 & c_{234}s_6-s_{234}c_5c_6 & -s_{234}s_5 & a_{4}s_{23}+a_3s_2-d_5c_{234}-d_{6}s_{234}s_5\\ 0 & 0 & 0 & 1\\ \end{bmatrix} T61=T21T32T43T54T65= c1s100s1c100001000d11 nxnynz0oxoyoz0axayaz0pxpypz1 = c234c5c6s234s6s5c6s234c5c6+c234s60c234c5s6s234c6s5s6c234s6s234c5c60c234s5c5s234s50a4c23+a3c2+d5s234d6c234s5d6c5d4a4s23+a3s2d5c234d6s234s51
这里 d 6 d_6 d6这一项是末端坐标系的z向偏移,可以和末端执行器一起考虑(这个机械臂配了一个夹爪,在运动学建模中最后也是一个z向偏移),设其长度为 l l l,左乘该变换矩阵的逆,得到
[ 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 ] = [ n x o x a x p x − a x ( d 6 + l ) n y o y a y p y − a y ( d 6 + l ) n z o z a z p z − a z ( d 6 + l ) 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}\\ =\begin{bmatrix} n_x & o_x & a_x & p_x-a_x(d_6+l)\\ n_y & o_y & a_y & p_y-a_y(d_6+l)\\ n_z & o_z & a_z & p_z-a_z(d_6+l)\\ 0 & 0 & 0 & 1\\ \end{bmatrix}\\ nxnynz0oxoyoz0axayaz0pxpypz1 = nxnynz0oxoyoz0axayaz0pxax(d6+l)pyay(d6+l)pzaz(d6+l)1
这样原先的 d 6 d_6 d6一项可以置为0得到
[ 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 234 c 5 c 6 − s 234 s 6 c 234 c 5 s 6 − s 234 c 6 − c 234 s 5 a 4 c 23 + a 3 c 2 + d 5 s 234 − s 5 c 6 s 5 s 6 − c 5 − d 4 s 234 c 5 c 6 + c 234 s 6 c 234 s 6 − s 234 c 5 c 6 − s 234 s 5 a 4 s 23 + a 3 s 2 − d 5 c 234 0 0 0 1 ] \begin{bmatrix} c_1 & s_1 & 0 & 0\\ -s_1 & c_1 & 0 & 0\\ 0 & 0 & 1 & -d_1\\ 0 & 0 & 0 & 1\\ \end{bmatrix} \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}\\ =\begin{bmatrix} c_{234}c_5c_6-s_{234}s_6 & c_{234}c_5s_6-s_{234}c_6 & -c_{234}s_5 & a_4c_{23}+a_3c_2+d_5s_{234}\\ -s_5c_6 & s_5s_6 & -c_5 & -d_4\\ s_{234}c_5c_6+c_{234}s_6 & c_{234}s_6-s_{234}c_5c_6 & -s_{234}s_5 & a_{4}s_{23}+a_3s_2-d_5c_{234}\\ 0 & 0 & 0 & 1\\ \end{bmatrix} c1s100s1c100001000d11 nxnynz0oxoyoz0axayaz0pxpypz1 = c234c5c6s234s6s5c6s234c5c6+c234s60c234c5s6s234c6s5s6c234s6s234c5c60c234s5c5s234s50a4c23+a3c2+d5s234d4a4s23+a3s2d5c2341

由矩阵两边(2,4)位置对应相等得
− s 1 p x + c 1 p y = − d 4 θ 1 = a t a n 2 ( p y , p x ) − a t a n 2 ( − d 4 , ± p x 2 + p y 2 − d 4 2 ) -s_{1}p_x+c_1p_y=-d_4\\ \theta_1=atan2(p_y,p_x)-atan2(-d_4,\pm\sqrt{p_x^2+p_y^2-d_4^2}) s1px+c1py=d4θ1=atan2(py,px)atan2(d4,±px2+py2d42 )

求解: a s i n θ + b c o s θ = c asin\theta+bcos\theta=c asinθ+bcosθ=c

利用和角公式得 a 2 + b 2 c o s ( θ − β ) = c \sqrt{a^2+b^2}cos(\theta-\beta)=c a2+b2 cos(θβ)=c

其中 b = c o s β , a = s i n β b=cos\beta,a=sin\beta b=cosβ,a=sinβ

代入 1 + t a n 2 θ = 1 c o s 2 θ 1+tan^2\theta=\frac{1}{cos^2\theta} 1+tan2θ=cos2θ1

t a n ( θ − β ) = ± a 2 + b 2 − c 2 c tan(\theta-\beta)=\pm\frac{\sqrt{a^2+b^2-c^2}}{c} tan(θβ)=±ca2+b2c2

t a n ( π 2 − θ + β ) = ± c a 2 + b 2 − c 2 tan(\frac{\pi}{2}-\theta+\beta)=\pm\frac{c}{\sqrt{a^2+b^2-c^2}} tan(2πθ+β)=±a2+b2c2 c

π 2 − θ + β = a t a n 2 ( c , ± a 2 + b 2 − c 2 ) \frac{\pi}{2}-\theta+\beta=atan2(c,\pm\sqrt{a^2+b^2-c^2}) 2πθ+β=atan2(c,±a2+b2c2 )

由于 b = c o s β = s i n ( π 2 + β ) , a = s i n β = − c o s ( π 2 + β ) b=cos\beta=sin(\frac{\pi}{2}+\beta),a=sin\beta=-cos(\frac{\pi}{2}+\beta) b=cosβ=sin(2π+β),a=sinβ=cos(2π+β)

θ = a t a n 2 ( b , − a ) − a t a n 2 ( c , ± a 2 + b 2 − c 2 ) \theta=atan2(b,-a)-atan2(c,\pm\sqrt{a^2+b^2-c^2}) θ=atan2(b,a)atan2(c,±a2+b2c2 )

(2,1),(2,2),(2,4)等式:
− s 1 n x + c 1 n y = − s 5 c 6 ( 5 ) − s 1 o x + c 1 o y = s 5 s 6 ( 6 ) − s 1 a x + c 1 a y = − c 5 ( 7 ) -s_1n_x+c_1n_y=-s_5c_6 (5)\\ -s_1o_x+c_1o_y=s_5s_6 (6)\\ -s_1a_x+c_1a_y=-c_5 (7) s1nx+c1ny=s5c6(5)s1ox+c1oy=s5s6(6)s1ax+c1ay=c5(7)
(5)(6)式得
s 5 = ± ( − s 1 n x + c 1 n y ) 2 + ( − s 1 o x + c 1 o y ) 2 ( 8 ) s_5=\pm\sqrt{(-s_1n_x+c_1n_y)^2+(-s_1o_x+c_1o_y)^2} (8) s5=±(s1nx+c1ny)2+(s1ox+c1oy)2 (8)

(7)(8)
θ 5 = a t a n 2 ( s 5 , s 1 a x − c 1 a y ) \theta_5=atan2(s_5,s_1a_x-c_1a_y) θ5=atan2(s5,s1axc1ay)
s 5 ≠ 0 s_5\neq0 s5=0时,由(5)(6)式得
θ 6 = a t a n 2 ( − s 1 o x + c 1 o y s 5 , s 1 n x − c 1 n y s 5 ) \theta_6=atan2(\frac{-s_1o_x+c_1o_y}{s_5},\frac{s_1n_x-c_1n_y}{s_5}) θ6=atan2(s5s1ox+c1oy,s5s1nxc1ny)
​ 当 s 5 = 0 s_5=0 s5=0,即 θ 5 = 0 , π \theta_5=0,\pi θ5=0,π时,此时机械臂机构发生奇异,无法求出 θ 6 \theta_6 θ6

得到 θ 1 , θ 5 , θ 6 \theta_1,\theta_5,\theta_6 θ1,θ5,θ6之后,剩余的三个关节均在一个平面转动,问题退化为二维。

(1,3),(3,3)等式:
a z = − s 234 s 5 c 1 a x + s 1 a y = − c 234 s 5 a_z=-s_{234}s_5\\ c_1a_x+s_1a_y=-c_{234}s_5 az=s234s5c1ax+s1ay=c234s5
将两式相除得到
θ 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}{s_5},-\frac{c_1a_x+s_1a_y}{s_5}) θ234=atan2(s5az,s5c1ax+s1ay)
(1,4),(3,4)等式:
c 1 p x + s 1 p y = a 4 c 23 + a 3 c 2 + d 5 s 234 ( 13 ) p z − d 1 = a 4 s 23 + a 3 s 2 − d 5 c 234 ( 14 ) c_1p_x+s_1p_y=a_4c_{23}+a_3c_2+d_5s_{234} (13)\\ p_z-d_1=a_4s_{23}+a_3s_2-d_5c_{234} (14) c1px+s1py=a4c23+a3c2+d5s234(13)pzd1=a4s23+a3s2d5c234(14)
消去 θ 23 \theta_{23} θ23
− A s 2 + B c 2 = C -As_2+Bc_2=C As2+Bc2=C
其中,
A = 2 B 2 a 3 B = 2 B 1 a 3 C = B 1 2 + B 2 2 + a 3 2 − a 4 2 B 1 = c 1 p x + s 1 p y − d 5 s 234 B 2 = p z − d 1 + d 5 c 234 A=2B_2a_3\\ B=2B_1a_3\\ C=B_1^2+B_2^2+a_3^2-a_4^2\\ B_1=c_1p_x+s_1p_y-d_5s_{234}\\ B_2=p_z-d_1+d_5c_{234} A=2B2a3B=2B1a3C=B12+B22+a32a42B1=c1px+s1pyd5s234B2=pzd1+d5c234
从而得到
θ 2 = a t a n 2 ( B , A ) − a t a n 2 ( C , ± A 2 + B 2 − C 2 ) \theta_2=atan2(B,A)-atan2(C,\pm\sqrt{A^2+B^2-C_2}) θ2=atan2(B,A)atan2(C,±A2+B2C2 )
θ 1 , θ 2 , θ 234 \theta_1,\theta_2,\theta_{234} θ1,θ2,θ234代入(13)(14)
s 23 = B 2 − a 3 s 2 a 4 c 23 = B 1 − a 3 c 2 a 4 s_{23}=\frac{B_2-a_3s_2}{a_4}\\ c_{23}=\frac{B_1-a_3c_2}{a_4} s23=a4B2a3s2c23=a4B1a3c2
进而可得
θ 23 = a t a n 2 ( B 2 − a 3 s 2 a 4 , B 1 − a 3 c 2 a 4 ) θ 3 = θ 23 − θ 2 θ 4 = θ 234 − θ 23 \theta_{23}=atan2(\frac{B_2-a_3s_2}{a_4},\frac{B_1-a_3c_2}{a_4})\\ \theta_3=\theta_{23}-\theta_2\\ \theta_4=\theta_{234}-\theta_{23} θ23=atan2(a4B2a3s2,a4B1a3c2)θ3=θ23θ2θ4=θ234θ23
至此,求出了6个关节的转角,一共有8组解。对于简单任务如抓取可以任取一组可行解;但是,对于一些如路径规划类任务需要考虑与当前状态“相近”的解,机械臂可以以最小成本完成动作到达目标位姿。

Logo

开放原子开发者工作坊旨在鼓励更多人参与开源活动,与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动,如meetup、训练营等,主打技术交流,干货满满,真诚地邀请各位开发者共同参与!

更多推荐