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参数，串联坐标系之间的变换关系为

1. 以x轴为转轴逆时针（从箭头方向从上往下看）旋转alpha度；
2. 以x轴平移长度a；
3. 以z轴为转轴逆时针旋转theta+offset度（图中绘制的是theta=0的情形）；
4. 沿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θ00​0010​00d1​ ​

以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​= ​1000​0cosαsinα0​0−sinαcosα0​a001​ ​

串联坐标系$T_{i-1}$与$T_i$之间变换描述为
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​=Rz​Rx​= ​cosθi​cosαi​sinθi​sinαi​cosθi​0​−sinθi​cosαi​sinθi​sinαi​cosθi​0​0−sinαi​cosαi​0​ai​−di​sinαi​di​cosαi​1​ ​
因为对x轴变换在先，将$R_x$放在右侧。这个矩阵可以描述如何将坐标系$T_{i-1}$变换为$T_i$，也可以描述将在$T_i$中的坐标点变换到$T_{i-1}$中，即在坐标系下的表示。

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

（符号简化表达：$cos{\theta }_i=c_i,sin{\theta }_i=s_i,sin({\theta }_1+{\theta }_2)=s_{12},cos({\theta }_1+{\theta }_2)=c_{12}$，以此类推）
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​= ​c1​s1​00​−s1​c1​00​0010​00d1​1​ ​T21​= ​c2​0s2​0​−s2​0c2​0​0−100​0001​ ​T32​= ​c3​s3​00​−s3​c3​00​0010​a3​001​ ​T43​= ​c4​s4​00​−s4​c4​00​0010​a4​0d4​1​ ​T54​= ​c5​0s5​0​−s5​0c5​0​0−110​0−d5​01​ ​T65​= ​c6​0−s6​0​−s6​0−c6​0​0100​0−d6​01​ ​
在知道每个电机的转角${\theta }_i$的情况下，每个矩阵是确定的。这里设末端位姿在$T_6$坐标系下的表示为单位矩阵，因此，末端在世界坐标系下的表示为
$$T_6^0=T_1^0{T}_2^1{T}_3^2{T}_4^3{T}_5^4{T}_6^{5}I$$
至此，求出机械臂末端在$T_0$坐标系（通常设为世界坐标系）下的表示，完成机械臂正运动学的求解。

逆运动学

现已知末端的位姿矩阵，但${\theta }_i$未知，因而上面$T_{i-1}^i$矩阵不确定，得到下面等式
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​=T10​T21​T32​T43​T54​T65​= ​nx​ny​nz​0​ox​oy​oz​0​ax​ay​az​0​px​py​pz​1​ ​
左右乘以$T_1^0$的逆，得
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​=T21​T32​T43​T54​T65​= ​c1​−s1​00​s1​c1​00​0010​00−d1​1​ ​ ​nx​ny​nz​0​ox​oy​oz​0​ax​ay​az​0​px​py​pz​1​ ​= ​c234​c5​c6​−s234​s6​−s5​c6​s234​c5​c6​+c234​s6​0​c234​c5​s6​−s234​c6​s5​s6​c234​s6​−s234​c5​c6​0​−c234​s5​−c5​−s234​s5​0​a4​c23​+a3​c2​+d5​s234​−d6​c234​s5​−d6​c5​−d4​a4​s23​+a3​s2​−d5​c234​−d6​s234​s5​1​ ​
这里$d_6$这一项是末端坐标系的z向偏移，可以和末端执行器一起考虑（这个机械臂配了一个夹爪，在运动学建模中最后也是一个z向偏移），设其长度为$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}\\ ​nx​ny​nz​0​ox​oy​oz​0​ax​ay​az​0​px′​py′​pz′​1​ ​= ​nx​ny​nz​0​ox​oy​oz​0​ax​ay​az​0​px​−ax​(d6​+l)py​−ay​(d6​+l)pz​−az​(d6​+l)1​ ​
这样原先的$d_6$一项可以置为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​−s1​00​s1​c1​00​0010​00−d1​1​ ​ ​nx​ny​nz​0​ox​oy​oz​0​ax​ay​az​0​px′​py′​pz′​1​ ​= ​c234​c5​c6​−s234​s6​−s5​c6​s234​c5​c6​+c234​s6​0​c234​c5​s6​−s234​c6​s5​s6​c234​s6​−s234​c5​c6​0​−c234​s5​−c5​−s234​s5​0​a4​c23​+a3​c2​+d5​s234​−d4​a4​s23​+a3​s2​−d5​c234​1​ ​

由矩阵两边(2,4)位置对应相等得
$$\begin{gathered} -s_1{p}_x+c_1{p}_y=-d_4 \\ {\theta }_1=atan2(p_y,p_x)-atan2(-d_4,\pm \sqrt{p_x^2+p_y^2-d_4^2}) \end{gathered}$$

求解：$asin\theta +bcos\theta =c$

利用和角公式得$\sqrt{a^2+b^2}cos(\theta -\beta )=c$

其中$b=cos\beta ,a=sin\beta$

代入$1+ta{n}^2\theta =\frac{1}{co{s}^2\theta }$得

$tan(\theta -\beta )=\pm \frac{\sqrt{a^2+b^2-c^2}}{c}$

$tan(\frac{\pi }{2}-\theta +\beta )=\pm \frac{c}{\sqrt{a^2+b^2-c^2}}$

$\frac{\pi }{2}-\theta +\beta =atan2(c,\pm \sqrt{a^2+b^2-c^2})$

由于$b=cos\beta =sin(\frac{\pi }{2}+\beta ),a=sin\beta =-cos(\frac{\pi }{2}+\beta )$

$\theta =atan2(b,-a)-atan2(c,\pm \sqrt{a^2+b^2-c^2})$

(2,1),(2,2),(2,4)等式：
$$\begin{gathered} -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) \end{gathered}$$
由(5)(6)式得
$$s_5=\pm \sqrt{(-s_1{n}_x+c_1{n}_y{)}^2+(-s_1{o}_x+c_1{o}_y{)}^2}(8)$$

由(7)(8)得
$${\theta }_5=atan2(s_5,s_1{a}_x-c_1{a}_y)$$
当$s_5\ne 0$时，由(5)(6)式得
$${\theta }_6=atan2(\frac{-s_1{o}_x+c_1{o}_y}{s_5},\frac{s_1{n}_x-c_1{n}_y}{s_5})$$
​ 当$s_5=0$，即${\theta }_5=0,\pi$时，此时机械臂机构发生奇异，无法求出${\theta }_6$。

得到${\theta }_1,{\theta }_5,{\theta }_6$之后，剩余的三个关节均在一个平面转动，问题退化为二维。

(1,3),(3,3)等式：
$$\begin{gathered} a_z=-s_{234}{s}_5 \\ {c}_1{a}_x+s_1{a}_y=-c_{234}{s}_5 \end{gathered}$$
将两式相除得到
$${\theta }_{234}=atan2(-\frac{a_z}{s_5},-\frac{c_1{a}_x+s_1{a}_y}{s_5})$$
(1,4),(3,4)等式：
$$\begin{gathered} 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) \end{gathered}$$
消去${\theta }_{23}$得
$$-A{s}_2+B{c}_2=C$$
其中，
$$\begin{gathered} A=2B_2{a}_3 \\ B=2B_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} \end{gathered}$$
从而得到
$${\theta }_2=atan2(B,A)-atan2(C,\pm \sqrt{A^2+B^2-C_2})$$
将${\theta }_1,{\theta }_2,{\theta }_{234}$代入(13)(14)得
$$\begin{gathered} s_{23}=\frac{B_2-a_3{s}_2}{a_4} \\ {c}_{23}=\frac{B_1-a_3{c}_2}{a_4} \end{gathered}$$
进而可得
$$\begin{gathered} {\theta }_{23}=atan2(\frac{B_2-a_3{s}_2}{a_4},\frac{B_1-a_3{c}_2}{a_4}) \\ {\theta }_3={\theta }_{23}-{\theta }_2 \\ {\theta }_4={\theta }_{234}-{\theta }_{23} \end{gathered}$$
至此，求出了6个关节的转角，一共有8组解。对于简单任务如抓取可以任取一组可行解；但是，对于一些如路径规划类任务需要考虑与当前状态“相近”的解，机械臂可以以最小成本完成动作到达目标位姿。