前言
整理之前的一个项目，当时看着一个博客硬生生计算了差不多一个星期。尝试用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+b2cos(θ−β)=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−c2c
π 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组解。对于简单任务如抓取可以任取一组可行解；但是，对于一些如路径规划类任务需要考虑与当前状态“相近”的解，机械臂可以以最小成本完成动作到达目标位姿。
