前往我的博客阅读体验更佳：本文链接
2 逆运动学分析
逆运动学就是已知末端执行器相对于基坐标系的位置和姿态，求对应的六个关节角度。设末端执行器相对于基坐标系的位姿矩阵为：
T
6
0
=
[
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
]
(1)
T^0_6 = \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} \tag{1}
T60=
nxnynz0oxoyoz0axayaz0pxpypz1
(1)
2.1 逆运动学求解
2.1.1 关节1的求解
已知
T
6
0
=
[
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
1
0
⋅
T
2
1
⋅
T
3
2
⋅
T
4
3
⋅
T
5
4
⋅
T
6
5
(2)
T^0_6 = \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} =T^0_1·T^1_2·T^2_3·T^3_4·T^4_5·T^5_6 \tag{2}
T60=
nxnynz0oxoyoz0axayaz0pxpypz1
=T10⋅T21⋅T32⋅T43⋅T54⋅T65(2)
将式(2)两边分别左乘
(
T
1
0
)
−
1
(T^0_1)^{-1}
(T10)−1和右乘
(
T
6
5
)
−
1
(T^5_6)^{-1}
(T65)−1，有：
(
T
1
0
)
−
1
∗
T
6
0
∗
(
T
6
5
)
−
1
=
T
2
1
⋅
T
3
2
⋅
T
4
3
⋅
T
5
4
(3)
(T^0_1)^{-1}*T^0_6*(T^5_6)^{-1}=T^1_2·T^2_3·T^3_4·T^4_5\tag{3}
(T10)−1∗T60∗(T65)−1=T21⋅T32⋅T43⋅T54(3)
因为：
(
T
1
0
)
−
1
=
[
c
1
s
1
0
0
−
s
1
c
1
0
0
0
0
1
−
d
1
0
0
0
1
]
(4)
(T^0_1)^{-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}\tag{4}
(T10)−1=
c1−s100s1c100001000−d11
(4)
( T 6 5 ) − 1 = [ c 6 0 − s 6 0 − s 6 0 − c 6 0 0 1 0 − d 6 0 0 0 1 ] (5) (T^5_6)^{-1} = \begin{bmatrix} c_6 & 0 & -s_6 & 0 \\ -s_6 & 0 & -c_6 & 0 \\ 0 & 1 & 0 & -d_6 \\ 0 & 0 & 0 & 1 \end{bmatrix}\tag{5} (T65)−1= c6−s6000010−s6−c60000−d61 (5)
所以有：
(
T
1
0
)
−
1
∗
T
6
0
∗
(
T
6
5
)
−
1
=
[
q
11
q
12
q
13
q
14
q
21
q
22
q
23
q
24
q
31
q
32
q
33
q
34
0
0
0
1
]
(6)
(T^0_1)^{-1}*T^0_6*(T^5_6)^{-1}= \begin{bmatrix} q_{11} & q_{12} & q_{13} & q_{14} \\ q_{21} & q_{22} & q_{23} & q_{24} \\ q_{31} & q_{32} & q_{33} & q_{34} \\ 0 & 0 & 0 & 1 \end{bmatrix}\tag{6}
(T10)−1∗T60∗(T65)−1=
q11q21q310q12q22q320q13q23q330q14q24q341
(6)
式(6)中，各简式分别为：
{
q
11
=
c
6
(
c
1
n
x
+
s
1
n
y
)
−
s
6
(
c
1
o
x
+
s
1
o
y
)
q
12
=
c
1
a
x
+
s
1
a
y
q
13
=
−
c
6
(
c
1
o
x
+
s
1
o
y
)
−
s
6
(
c
1
n
x
+
s
1
n
y
)
q
14
=
c
1
p
x
+
s
1
p
y
−
(
c
1
a
x
+
s
1
a
y
)
d
6
q
21
=
c
6
(
c
1
n
y
−
s
1
n
x
)
−
s
6
(
c
1
o
y
−
s
1
o
x
)
q
22
=
c
1
a
y
−
s
1
a
x
q
23
=
c
6
(
s
1
o
x
−
c
1
o
y
)
−
s
6
(
c
1
n
y
−
s
1
n
x
)
q
24
=
c
1
p
y
−
s
1
p
x
+
d
6
(
s
1
a
x
−
c
1
a
y
)
q
31
=
c
6
n
z
−
s
6
o
z
q
32
=
a
z
q
33
=
−
s
6
n
z
−
c
6
o
z
q
34
=
p
z
−
d
1
−
a
z
d
6
(7)
\left\{ \begin{array}{lr} q_{11} = c_6(c_1n_x+s_1n_y)-s_6(c_1o_x+s_1o_y) \\ q_{12} = c_1a_x+s_1a_y \\ q_{13} = -c_6(c_1o_x+s_1o_y)-s_6(c_1n_x+s_1n_y) \\ q_{14} = c_1p_x+s_1p_y-(c_1a_x+s_1a_y)d_6 \\ q_{21} = c_6(c_1n_y-s_1n_x)-s_6(c_1o_y-s_1o_x) \\ q_{22} = c_1a_y-s_1a_x \\ q_{23} = c_6(s_1o_x-c_1o_y)-s_6(c_1n_y-s_1n_x) \\ q_{24} = c_1p_y-s_1p_x+d_6(s_1a_x-c_1a_y) \\ q_{31} = c_6n_z-s_6o_z \\ q_{32} = a_z \\ q_{33} = -s_6n_z-c_6o_z \\ q_{34} = p_z-d_1-a_zd_6 \\ \end{array} \right. \tag{7}
⎩
⎨
⎧q11=c6(c1nx+s1ny)−s6(c1ox+s1oy)q12=c1ax+s1ayq13=−c6(c1ox+s1oy)−s6(c1nx+s1ny)q14=c1px+s1py−(c1ax+s1ay)d6q21=c6(c1ny−s1nx)−s6(c1oy−s1ox)q22=c1ay−s1axq23=c6(s1ox−c1oy)−s6(c1ny−s1nx)q24=c1py−s1px+d6(s1ax−c1ay)q31=c6nz−s6ozq32=azq33=−s6nz−c6ozq34=pz−d1−azd6(7)
而式(3)右边结果为：
T
2
1
⋅
T
3
2
⋅
T
4
3
⋅
T
5
4
=
[
c
234
c
5
−
c
234
s
5
s
234
c
23
a
3
+
c
2
a
2
+
s
234
d
5
−
s
5
−
c
5
0
−
d
2
−
d
4
s
234
c
5
−
s
234
s
5
−
c
234
s
23
a
3
+
s
2
a
2
−
c
234
d
5
0
0
0
1
]
(8)
T^1_2·T^2_3·T^3_4·T^4_5= \begin{bmatrix} c_{234}c_5 & -c_{234}s_5 & s_{234} & c_{23}a_3+c_2a_2+s_{234}d_5 \\ -s_5 & -c_5 & 0 & -d_2-d_4 \\ s_{234}c_5 & -s_{234}s_5 & -c_{234} & s_{23}a_3+s_2a_2-c_{234}d_5 \\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{8}
T21⋅T32⋅T43⋅T54=
c234c5−s5s234c50−c234s5−c5−s234s50s2340−c2340c23a3+c2a2+s234d5−d2−d4s23a3+s2a2−c234d51
(8)
由式(6)和式(8)第二行第四列元素相等，可以得到：
−
d
2
−
d
4
=
c
1
p
y
−
s
1
p
x
+
d
6
(
s
1
a
x
−
c
1
a
y
)
=
(
p
y
−
a
y
d
6
)
c
1
−
(
p
x
−
a
x
d
6
)
s
1
(9)
\begin{aligned} -d_2-d_4 & =c_1p_y-s_1p_x+d_6(s_1a_x-c_1a_y) \\ & =(p_y-a_yd_6)c_1 - (p_x-a_xd_6)s_1 \end{aligned} \tag{9}
−d2−d4=c1py−s1px+d6(s1ax−c1ay)=(py−ayd6)c1−(px−axd6)s1(9)
令
m
=
p
y
−
a
y
d
6
,
n
=
p
x
−
a
x
d
6
m=p_y-a_yd_6,n=p_x-a_xd_6
m=py−ayd6,n=px−axd6，则式(9)可化为：
m
c
1
−
n
s
1
=
−
d
2
−
d
4
(10)
mc_1-ns_1=-d_2-d_4\tag{10}
mc1−ns1=−d2−d4(10)
利用辅助角公式可将上式化为：
m
2
+
n
2
sin
(
φ
−
θ
1
)
=
−
d
2
−
d
4
(11)
\sqrt{m^2+n^2}\sin(\varphi-\theta_1)=-d_2-d_4\tag{11}
m2+n2sin(φ−θ1)=−d2−d4(11)
即：
sin
(
φ
−
θ
1
)
=
−
d
2
−
d
4
m
2
+
n
2
(12)
\sin(\varphi-\theta_1)=\frac{-d_2-d_4}{\sqrt{m^2+n^2}}\tag{12}
sin(φ−θ1)=m2+n2−d2−d4(12)
其中，
sin
(
φ
)
=
m
m
2
+
n
2
\sin(\varphi)=\frac{m}{\sqrt{m^2+n^2}}
sin(φ)=m2+n2m，
cos
(
φ
)
=
n
m
2
+
n
2
\cos(\varphi)=\frac{n}{\sqrt{m^2+n^2}}
cos(φ)=m2+n2n，则：
φ
=
A
t
a
n
2
(
m
,
n
)
(13)
\varphi=\mathrm{Atan2}(m,n)\tag{13}
φ=Atan2(m,n)(13)
由式(12)可得：
cos
(
φ
−
θ
1
)
=
±
1
−
(
−
d
2
−
d
4
m
2
+
n
2
)
2
(14)
\cos(\varphi-\theta_1)=\pm\sqrt{1-(\frac{-d_2-d_4}{\sqrt{m^2+n^2}})^2}\tag{14}
cos(φ−θ1)=±1−(m2+n2−d2−d4)2(14)
由式(12)和式(14)可得：
φ
−
θ
1
=
A
t
a
n
2
(
−
d
2
−
d
4
,
±
m
2
+
n
2
−
(
d
2
+
d
4
)
2
)
(15)
\varphi-\theta_1=\mathrm{Atan2}(-d_2-d_4,\pm\sqrt{m^2+n^2-(d_2+d_4)^2})\tag{15}
φ−θ1=Atan2(−d2−d4,±m2+n2−(d2+d4)2)(15)
即：
θ
1
=
φ
−
A
t
a
n
2
(
−
d
2
−
d
4
,
±
m
2
+
n
2
−
(
d
2
+
d
4
)
2
)
(16)
\theta_1=\varphi-\mathrm{Atan2}(-d_2-d_4,\pm\sqrt{m^2+n^2-(d_2+d_4)^2})\tag{16}
θ1=φ−Atan2(−d2−d4,±m2+n2−(d2+d4)2)(16)
将式(13)代入上式可求得
θ
1
\theta_1
θ1：
θ
1
=
A
t
a
n
2
(
m
,
n
)
−
A
t
a
n
2
(
−
d
2
−
d
4
,
±
m
2
+
n
2
−
(
d
2
+
d
4
)
2
)
(17)
\theta_1=\mathrm{Atan2}(m,n)-\mathrm{Atan2}(-d_2-d_4,\pm\sqrt{m^2+n^2-(d_2+d_4)^2})\tag{17}
θ1=Atan2(m,n)−Atan2(−d2−d4,±m2+n2−(d2+d4)2)(17)
共两个解。
2.1.2 关节5的求解
由式(6)和式(8)第二行第二列元素相等，可以得到：
−
c
5
=
c
1
a
y
−
s
1
a
x
(18)
-c_5=c_1a_y-s_1a_x\tag{18}
−c5=c1ay−s1ax(18)
由于
θ
1
\theta_1
θ1已知，则可求得
θ
5
\theta_5
θ5为：
θ
5
=
±
arccos
(
s
1
a
x
−
c
1
a
y
)
(19)
\theta_5=\pm \arccos(s_1a_x-c_1a_y)\tag{19}
θ5=±arccos(s1ax−c1ay)(19)
共两个解。
2.1.3 关节6的求解
由式(6)和式(8)第二行第一列元素相等，可以得到：
−
s
5
=
c
6
(
c
1
n
y
−
s
1
n
x
)
−
s
6
(
c
1
o
y
−
s
1
o
x
)
(20)
-s_5=c_6(c_1n_y-s_1n_x)-s_6(c_1o_y-s_1o_x)\tag{20}
−s5=c6(c1ny−s1nx)−s6(c1oy−s1ox)(20)
由式(6)和式(8)第二行第三列元素相等，可以得到：
0
=
c
6
(
s
1
o
x
−
c
1
o
y
)
−
s
6
(
c
1
n
y
−
s
1
n
x
)
(21)
0=c_6(s_1o_x-c_1o_y)-s_6(c_1n_y-s_1n_x)\tag{21}
0=c6(s1ox−c1oy)−s6(c1ny−s1nx)(21)
令
s
=
c
1
n
y
−
s
1
n
x
s=c_1n_y-s_1n_x
s=c1ny−s1nx，
t
=
c
1
o
y
−
s
1
o
x
t=c_1o_y-s_1o_x
t=c1oy−s1ox，式(20)和式(21)可写成：
s
c
6
−
t
s
6
=
−
s
5
(22)
sc_6-ts_6 =-s_5 \tag{22}
sc6−ts6=−s5(22)
s
s
6
+
t
c
6
=
0
(23)
ss_6+tc_6 =0\tag{23}
ss6+tc6=0(23)
当
s
5
≠
0
s_5\neq 0
s5=0时，将式(22)和式(23)两边平方后相加，可得：
s
2
+
t
2
=
s
5
2
(24)
s^2+t^2=s_5^2\tag{24}
s2+t2=s52(24)
由于式(22)和式(10)形式类似，因此，可得到
θ
6
\theta_6
θ6：
θ
6
=
A
t
a
n
2
(
s
,
t
)
−
A
t
a
n
2
(
−
s
5
,
±
s
2
+
t
2
−
s
5
2
)
=
A
t
a
n
2
(
s
,
t
)
−
A
t
a
n
2
(
−
s
5
,
0
)
(25)
\begin{aligned} \theta_6 & =\mathrm{Atan2}(s,t)-\mathrm{Atan2}(-s_5,\pm\sqrt{s^2+t^2-s_5^2}) \\ & =\mathrm{Atan2}(s,t)-\mathrm{Atan2}(-s_5,0) \end{aligned}\tag{25}
θ6=Atan2(s,t)−Atan2(−s5,±s2+t2−s52)=Atan2(s,t)−Atan2(−s5,0)(25)
共一个解。
若 s 5 = 0 s_5=0 s5=0，此时机器人处于奇异位置。
2.1.4 关节3的求解
对式(3)两边同时右乘
(
T
5
4
)
−
1
(T^4_5)^{-1}
(T54)−1可得：
(
T
1
0
)
−
1
∗
T
6
0
∗
(
T
6
5
)
−
1
⋅
(
T
5
4
)
−
1
=
T
2
1
⋅
T
3
2
⋅
T
4
3
(26)
(T^0_1)^{-1}*T^0_6*(T^5_6)^{-1}·(T^4_5)^{-1}=T^1_2·T^2_3·T^3_4\tag{26}
(T10)−1∗T60∗(T65)−1⋅(T54)−1=T21⋅T32⋅T43(26)
因为
(
T
5
4
)
−
1
(T^4_5)^{-1}
(T54)−1为：
(
T
5
4
)
−
1
=
[
c
5
0
s
5
0
−
s
5
0
c
5
0
0
−
1
0
−
d
5
0
0
0
1
]
(27)
(T^4_5)^{-1} = \begin{bmatrix} c_5 & 0 & s_5 & 0 \\ -s_5 & 0 & c_5 & 0 \\ 0 & -1 & 0 & -d_5 \\ 0 & 0 & 0 & 1 \end{bmatrix}\tag{27}
(T54)−1=
c5−s50000−10s5c50000−d51
(27)
所以式(26)左边为：
(
T
1
0
)
−
1
∗
T
6
0
∗
(
T
6
5
)
−
1
∗
(
T
5
4
)
−
1
=
[
r
11
r
12
r
13
r
14
r
21
r
22
r
23
r
24
r
31
r
32
r
33
r
34
0
0
0
1
]
(28)
(T^0_1)^{-1}*T^0_6*(T^5_6)^{-1}*(T^4_5)^{-1}= \begin{bmatrix} r_{11} & r_{12} & r_{13} & r_{14} \\ r_{21} & r_{22} & r_{23} & r_{24} \\ r_{31} & r_{32} & r_{33} & r_{34} \\ 0 & 0 & 0 & 1 \end{bmatrix}\tag{28}
(T10)−1∗T60∗(T65)−1∗(T54)−1=
r11r21r310r12r22r320r13r23r330r14r24r341
(28)
式(28)中部分简式如下：
{
r
14
=
c
1
p
x
+
s
1
p
y
−
d
6
(
s
1
a
y
+
c
1
a
x
)
+
d
5
[
c
6
(
c
1
o
x
+
s
1
o
y
)
+
s
6
(
c
1
n
x
+
s
1
n
y
)
]
r
34
=
p
z
−
d
1
−
a
z
d
6
+
d
5
(
s
6
n
z
+
c
6
o
z
)
(29)
\left\{ \begin{array}{lr} r_{14}=c_1p_x+s_1p_y-d_6(s_1a_y+c_1a_x)+d_5[c_6(c_1o_x+s_1o_y)+s_6(c_1n_x+s_1n_y)] \\ r_{34}=p_z-d_1-a_zd_6+d_5(s_6n_z+c_6o_z) \end{array} \right.\tag{29}
{r14=c1px+s1py−d6(s1ay+c1ax)+d5[c6(c1ox+s1oy)+s6(c1nx+s1ny)]r34=pz−d1−azd6+d5(s6nz+c6oz)(29)
由于下述计算过程并未使用矩阵中其它元素，故此处未给出。
式(26)右边为：
T
2
1
⋅
T
3
2
⋅
T
4
3
=
[
c
234
−
s
234
0
c
23
a
3
+
c
2
a
2
0
0
−
1
−
d
2
−
d
4
s
234
c
234
0
s
23
a
3
+
s
2
a
2
0
0
0
1
]
(30)
T^1_2·T^2_3·T^3_4= \begin{bmatrix} c_{234} & -s_{234} & 0 & c_{23}a_3+c_2a_2 \\ 0 & 0 & -1 & -d_2-d_4 \\ s_{234} & c_{234} & 0 & s_{23}a_3+s_2a_2 \\ 0 & 0 & 0 & 1 \end{bmatrix}\tag{30}
T21⋅T32⋅T43=
c2340s2340−s2340c23400−100c23a3+c2a2−d2−d4s23a3+s2a21
(30)
由式(28)和式(30)第一行第四列元素和第三行第四列元素分别相等，得：
{
a
3
c
23
+
a
2
c
2
=
r
14
a
3
s
23
+
a
2
s
2
=
r
34
(31)
\left\{ \begin{array}{lr} a_3c_{23}+a_2c_2=r_{14} \\ a_3s_{23}+a_2s_2=r_{34} \end{array} \right.\tag{31}
{a3c23+a2c2=r14a3s23+a2s2=r34(31)
两边平方后相加得到：
a
3
2
+
a
2
2
+
2
a
2
a
3
(
c
23
c
2
+
s
23
s
2
)
=
r
14
2
+
r
34
2
(32)
a_3^2+a_2^2+2a_2a_3(c_{23}c_2+s_{23}s_2)=r_{14}^2+r_{34}^2\tag{32}
a32+a22+2a2a3(c23c2+s23s2)=r142+r342(32)
注意到
c
23
c
2
+
s
23
s
2
=
cos
(
θ
2
+
θ
3
)
cos
(
θ
2
)
+
sin
(
θ
2
+
θ
3
)
sin
(
θ
2
)
=
cos
(
θ
2
+
θ
3
−
θ
2
)
=
cos
(
θ
3
)
c_{23}c_2+s_{23}s_2=\cos(\theta_2+\theta_3)\cos(\theta_2)+\sin(\theta_2+\theta_3)\sin(\theta_2)=\cos(\theta_2+\theta_3-\theta_2)=\cos(\theta_3)
c23c2+s23s2=cos(θ2+θ3)cos(θ2)+sin(θ2+θ3)sin(θ2)=cos(θ2+θ3−θ2)=cos(θ3).
所以
θ
3
\theta_3
θ3为：
θ
3
=
±
arccos
(
r
14
2
+
r
34
2
−
a
3
2
−
a
2
2
2
a
2
a
3
)
(33)
\theta_3=\pm \arccos(\frac{r_{14}^2+r_{34}^2-a_3^2-a_2^2}{2a_2a_3})\tag{33}
θ3=±arccos(2a2a3r142+r342−a32−a22)(33)
共两个解。
2.1.5 关节2的求解
利用三角函数公式，方程组式(31)可以化为：
{
(
a
3
c
3
+
a
2
)
s
2
+
a
3
s
3
c
2
=
r
14
−
a
3
s
3
s
2
+
(
a
3
c
3
+
a
2
)
c
2
=
r
34
(34)
\left\{ \begin{array}{lr} (a_3c_3+a_2)s_2+a_3s_3c_2 = r_{14} \\ -a_3s_3s_2+(a_3c_3+a_2)c_2 = r_{34} \end{array} \right.\tag{34}
{(a3c3+a2)s2+a3s3c2=r14−a3s3s2+(a3c3+a2)c2=r34(34)
求解方程组式(34)，结果如下：
{
s
2
=
(
a
3
c
3
+
a
2
)
r
34
−
a
3
s
3
r
14
a
3
2
+
a
2
2
+
2
a
2
a
3
c
3
c
2
=
a
3
s
3
r
34
+
(
a
3
c
3
+
a
2
)
r
14
a
3
2
+
a
2
2
+
2
a
2
a
3
c
3
(35)
\left\{ \begin{array}{lr} s_2 = \frac{(a_3c_3+a_2)r_{34}-a_3s_3r_{14}}{a_3^2+a_2^2+2a_2a_3c_3} \\ c_2 = \frac{a_3s_3r_{34}+(a_3c_3+a_2)r_{14}}{a_3^2+a_2^2+2a_2a_3c_3} \end{array} \right.\tag{35}
{s2=a32+a22+2a2a3c3(a3c3+a2)r34−a3s3r14c2=a32+a22+2a2a3c3a3s3r34+(a3c3+a2)r14(35)
则
θ
2
\theta_2
θ2为：
θ
2
=
A
t
a
n
2
(
s
2
,
c
2
)
(36)
\theta_2=\mathrm{Atan2}(s_2,c_2)\tag{36}
θ2=Atan2(s2,c2)(36)
共一个解。
2.1.6 关节4的求解
由式(6)和式(8)第一行第三列元素和第三行第三列元素分别相等，可以得到：
{
s
234
=
−
s
6
(
c
1
n
x
+
s
1
n
y
)
−
c
6
(
c
1
o
x
+
s
1
o
y
)
c
234
=
s
6
n
z
+
c
6
o
z
(37)
\left\{ \begin{array}{lr} s_{234}=-s_6(c_1n_x+s_1n_y)-c_6(c_1o_x+s_1o_y) \\ c_{234}=s_6n_z+c_6o_z \end{array} \right.\tag{37}
{s234=−s6(c1nx+s1ny)−c6(c1ox+s1oy)c234=s6nz+c6oz(37)
则
θ
4
\theta_4
θ4为：
θ
4
=
A
t
a
n
2
(
s
234
,
c
234
)
−
θ
2
−
θ
3
(38)
\theta_4=\mathrm{Atan2}(s_{234},c_{234})-\theta_2-\theta_3\tag{38}
θ4=Atan2(s234,c234)−θ2−θ3(38)
共一个解。
2.1.7 总结
综上所述，可以得到六个关节角的解析解表达式如下：
θ
1
=
A
t
a
n
2
(
m
,
n
)
−
A
t
a
n
2
(
−
d
2
−
d
4
,
±
m
2
+
n
2
−
(
d
2
+
d
4
)
2
)
(39)
\theta_1=\mathrm{Atan2}(m,n)-\mathrm{Atan2}(-d_2-d_4,\pm\sqrt{m^2+n^2-(d_2+d_4)^2})\tag{39}
θ1=Atan2(m,n)−Atan2(−d2−d4,±m2+n2−(d2+d4)2)(39)
其中，
m
=
p
y
−
a
y
d
6
m=p_y-a_yd_6
m=py−ayd6，
n
=
p
x
−
a
x
d
6
n=p_x-a_xd_6
n=px−axd6。
θ
5
=
±
arccos
(
s
1
a
x
−
c
1
a
y
)
(40)
\theta_5=\pm \arccos(s_1a_x-c_1a_y)\tag{40}
θ5=±arccos(s1ax−c1ay)(40)
θ
6
=
A
t
a
n
2
(
s
,
t
)
−
A
t
a
n
2
(
−
s
5
,
0
)
(41)
\theta_6=\mathrm{Atan2}(s,t)-\mathrm{Atan2}(-s_5,0)\tag{41}
θ6=Atan2(s,t)−Atan2(−s5,0)(41)
其中，
s
=
c
1
n
y
−
s
1
n
x
s=c_1n_y-s_1n_x
s=c1ny−s1nx，
t
=
c
1
o
y
−
s
1
o
x
t=c_1o_y-s_1o_x
t=c1oy−s1ox。
θ
3
=
±
arccos
(
r
14
2
+
r
34
2
−
a
3
2
−
a
2
2
2
a
2
a
3
)
(42)
\theta_3=\pm \arccos(\frac{r_{14}^2+r_{34}^2-a_3^2-a_2^2}{2a_2a_3})\tag{42}
θ3=±arccos(2a2a3r142+r342−a32−a22)(42)
其中，
r
14
=
c
1
p
x
+
s
1
p
y
−
d
6
(
s
1
a
y
+
c
1
a
x
)
+
d
5
[
c
6
(
c
1
o
x
+
s
1
o
y
)
+
s
6
(
c
1
n
x
+
s
1
n
y
)
]
r_{14}=c_1p_x+s_1p_y-d_6(s_1a_y+c_1a_x)+d_5[c_6(c_1o_x+s_1o_y)+s_6(c_1n_x+s_1n_y)]
r14=c1px+s1py−d6(s1ay+c1ax)+d5[c6(c1ox+s1oy)+s6(c1nx+s1ny)]，
r
34
=
p
z
−
d
1
−
a
z
d
6
+
d
5
(
s
6
n
z
+
c
6
o
z
)
r_{34}=p_z-d_1-a_zd_6+d_5(s_6n_z+c_6o_z)
r34=pz−d1−azd6+d5(s6nz+c6oz)。
θ
2
=
A
t
a
n
2
(
s
2
,
c
2
)
(43)
\theta_2=\mathrm{Atan2}(s_2,c_2)\tag{43}
θ2=Atan2(s2,c2)(43)
其中，
s
2
=
(
a
3
c
3
+
a
2
)
r
34
−
a
3
s
3
r
14
a
3
2
+
a
2
2
+
2
a
2
a
3
c
3
s_2 = \frac{(a_3c_3+a_2)r_{34}-a_3s_3r_{14}}{a_3^2+a_2^2+2a_2a_3c_3}
s2=a32+a22+2a2a3c3(a3c3+a2)r34−a3s3r14，
c
2
=
a
3
s
3
r
34
+
(
a
3
c
3
+
a
2
)
r
14
a
3
2
+
a
2
2
+
2
a
2
a
3
c
3
c_2 = \frac{a_3s_3r_{34}+(a_3c_3+a_2)r_{14}}{a_3^2+a_2^2+2a_2a_3c_3}
c2=a32+a22+2a2a3c3a3s3r34+(a3c3+a2)r14。
θ
4
=
A
t
a
n
2
(
s
234
,
c
234
)
−
θ
2
−
θ
3
(44)
\theta_4=\mathrm{Atan2}(s_{234},c_{234})-\theta_2-\theta_3\tag{44}
θ4=Atan2(s234,c234)−θ2−θ3(44)
其中， s 234 = − s 6 ( c 1 n x + s 1 n y ) − c 6 ( c 1 o x + s 1 o y ) s_{234}=-s_6(c_1n_x+s_1n_y)-c_6(c_1o_x+s_1o_y) s234=−s6(c1nx+s1ny)−c6(c1ox+s1oy)， c 234 = − s 6 n z − c 6 o z c_{234}=-s_6n_z-c_6o_z c234=−s6nz−c6oz。
在上述求解公式中，由于 θ 1 \theta _1 θ1、 θ 5 \theta _5 θ5、 θ 3 \theta _3 θ3存在“ ± \pm ±”号的影响，所以该机器人对同一种末端位姿可能存在八组解。
2.2 程序编写
在2.1.7节中已经给出各个关节角的解析解表达式，利用表达式编写逆运动学求解程序，由于UR3机器人位姿表示方法采用旋转矢量形式，因此程序有齐次变换矩阵及旋转矢量两种输入方式，程序流程图如下图所示。
注意到求解
θ
5
\theta_5
θ5的表达式为：
θ
5
=
±
arccos
(
s
1
a
x
−
c
1
a
y
)
(45)
\theta_5=\pm \arccos(s_1a_x-c_1a_y)\tag{45}
θ5=±arccos(s1ax−c1ay)(45)
如果
∣
cos
θ
5
∣
=
∣
s
1
a
x
−
c
1
a
y
∣
>
1
(46)
|\cos\theta_{5}|=|s_1a_x-c_1a_y|>1\tag{46}
∣cosθ5∣=∣s1ax−c1ay∣>1(46)
但是余弦函数的值域为 [ − 1 , 1 ] [-1,1] [−1,1]，所以式(46)无意义，这种情况下解不存在。同理，求解 θ 3 \theta_3 θ3时也存在同样的问题。也就是说，八组解析解并不一定全都存在。
同时，由于计算机内部存储数据类型的原因，当求解出的角度为零时实际存储的数值并不一定是零，由于误差的累计，后面角度的求解结果会产生较大的偏差，因此在每个角度求解完毕后需要添加如下程序块进行手动判零：
if abs(theta_5) < eps
theta_5 = 0;
end
2.3 验证
采用正逆运动学相互求解的方法验证解的正确与否。首先利用正运动学求得关节角为
Θ
=
[
1
5
∘
1
5
∘
1
5
∘
1
5
∘
1
5
∘
1
5
∘
]
T
\Theta=\left[15^{\circ}\ 15^{\circ}\ 15^{\circ}\ 15^{\circ}\ 15^{\circ}\ 15^{\circ}\right]^T
Θ=[15∘ 15∘ 15∘ 15∘ 15∘ 15∘]T时的位姿矩阵为：
T
6
0
=
[
0.5252
−
0.8478
0.0732
−
313.9360
−
0.1181
−
0.1578
−
0.9804
−
280.8134
0.8428
0.5062
−
0.1830
−
91.7142
0
0
0
1.0000
]
(47)
T^0_6= \begin{bmatrix} 0.5252 & -0.8478 & 0.0732 & -313.9360 \\ -0.1181 & -0.1578 & -0.9804 & -280.8134 \\ 0.8428 & 0.5062 & -0.1830 & -91.7142 \\ 0 & 0 & 0 & 1.0000 \end{bmatrix}\tag{47}
T60=
0.5252−0.11810.84280−0.8478−0.15780.506200.0732−0.9804−0.18300−313.9360−280.8134−91.71421.0000
(47)
利用上述位姿矩阵进行逆运动学求解，求得八组解如下所示:
分别将每一组解利用正运动学计算位姿矩阵，发现得到的位姿矩阵同初始位姿矩阵相同，证明逆运动学解析解表达式无误。
扫一扫
2232
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
