UR5构型机械臂正逆运动学

前言

整理之前的一个项目，当时看着一个博客硬生生计算了差不多一个星期。尝试用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参数表

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

以x轴为转轴逆时针（从箭头方向从上往下看）旋转alpha度；
以x轴平移长度a；
以z轴为转轴逆时针旋转theta+offset度（图中绘制的是theta=0的情形）；
沿z轴平移长度d（单位为mm）；

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

Joint	alpha	a	d	theta	offset
1	0	0	131.56	theta_1	0
2	PI/2	0	0	theta_2	-PI/2
3	0	-110.4	0	theta_3	0
4	0	-96	64.62	theta_4	-PI/2
5	PI/2	0	73.18	theta_5	PI/2
6	PI/2	0	48.6	theta_6	0

正运动学

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

以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θ00−sinθ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α00−sinαcosα0a001

串联坐标系 T i − 1 T_{i-1} Ti−1与 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} Tii−1=RzRx= cosθicosαisinθisinαicosθi0−sinθicosαisinθisinαicosθi00−sinαicosαi0ai−disinαidicosαi1
因为对x轴变换在先，将 R x R_x Rx放在右侧。这个矩阵可以描述如何将坐标系 T i − 1 T_{i-1} Ti−1变换为 T i T_i Ti，也可以描述将在 T i T_i Ti中的坐标点变换到 T i − 1 T_{i-1} Ti−1中，即在坐标系下的表示。

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

（符号简化表达： 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= c1s100−s1c100001000d11 T21= c20s20−s20c200−1000001 T32= c3s300−s3c3000010a3001 T43= c4s400−s4c4000010a40d41 T54= c50s50−s50c500−1100−d501 T65= c60−s60−s60−c6001000−d601
在知道每个电机的转角 θ 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} Ti−1i矩阵不确定，得到下面等式
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= c1−s100s1c100001000−d11 nxnynz0oxoyoz0axayaz0pxpypz1 = c234c5c6−s234s6−s5c6s234c5c6+c234s60c234c5s6−s234c6s5s6c234s6−s234c5c60−c234s5−c5−s234s50a4c23+a3c2+d5s234−d6c234s5−d6c5−d4a4s23+a3s2−d5c234−d6s234s51
这里 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}\\ nxnynz0oxoyoz0axayaz0px′py′pz′1 = nxnynz0oxoyoz0axayaz0px−ax(d6+l)py−ay(d6+l)pz−az(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} c1−s100s1c100001000−d11 nxnynz0oxoyoz0axayaz0px′py′pz′1 = c234c5c6−s234s6−s5c6s234c5c6+c234s60c234c5s6−s234c6s5s6c234s6−s234c5c60−c234s5−c5−s234s50a4c23+a3c2+d5s234−d4a4s23+a3s2−d5c2341

由矩阵两边(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+py2−d42 )

求解： 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+b2−c2

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+b2−c2 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+b2−c2 )

由于 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+b2−c2 )

(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,s1ax−c1ay)
当 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(s5−s1ox+c1oy,s5s1nx−c1ny)
当 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)pz−d1=a4s23+a3s2−d5c234(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+a32−a42B1=c1px+s1py−d5s234B2=pz−d1+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+B2−C2 )
将 θ 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=a4B2−a3s2c23=a4B1−a3c2
进而可得
θ 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(a4B2−a3s2,a4B1−a3c2)θ3=θ23−θ2θ4=θ234−θ23
至此，求出了6个关节的转角，一共有8组解。对于简单任务如抓取可以任取一组可行解；但是，对于一些如路径规划类任务需要考虑与当前状态“相近”的解，机械臂可以以最小成本完成动作到达目标位姿。