相关背景
在多学科技术的推动作用下，机器人技术得到了长足发展。其中，六轴多关节机器人在可达性、精度等方面具有较大优势，已应用在包含喷涂、焊接等多种应用中。作为一种特殊的机械臂，协作臂在安全性方面有较大提升，广泛应用在物体抓取、机器人精细操作等应用中。本文将传统的机械臂与协作臂统称为机械臂。随着近年来人形机器人日益火爆，作为核心操作臂的多关节机械臂更是被推到风口浪尖上，大量学者涌入多关节机器人控制、多模态信息辅助下的智能操作研究中。在机械臂的众多控制方式中，给定关节角或末端位姿进行控制是较为常见的方式。然而商用臂一般采用数值迭代方式进行逆向解求解，仅提供一组逆向解，经常出现实际可达但反馈不可达情况的发生，给高级控制算法的开发带来较大影响。
机器人的运动学，着重研究各个坐标系之间的运动关系。在这个领域中，关节空间到末端笛卡尔空间的映射以及末端笛卡尔空间到关节空间的映射是运动学研究的主要问题，同时也是本项目的主要研究内容。关节空间的坐标到末端位姿的映射，称为正向运动学；末端位姿到关节空间坐标的映射，称为逆向运动学。对于上述问题，到目前为止，已出现了大量用于解决类似的串联机械臂的运动学问题的方法。R. M. Murray等[1]基于李群李代数理论系统地研究了串联机器人的运动学模型建模的问题；张连东[2]基于活动标架法系统地分析了机械臂的正向运动学问题；谭民等[3-5]提出以解析法以及解析法与投影法相结合的方法进行机器人逆向运动学求解的方法；金媛媛等[6-7]采取数值迭代的方法对工业机器人的逆向运动学问题进行求解；E. Sariyildiz等[8]基于螺旋变换理论并以紧凑的形式完成了工业机器人运动学模型的建模和参数求解过程。
本文描述对象
本文以常见的国产机械臂JAKA ZU7为例，描述其运动学模型构建、解析法为基础的正逆向运动学求解过程。
JAKA ZU7 机械臂运动学模型构建
作为一种典型的六轴多关节机械臂，参照D-H连杆参数法进行建模，针对6个关节分别构建坐标系，并在机械臂底座位置构建全局参考系，如下图所示：
上图为6个轴均未发生旋转时建模情况，各轴均绕自身z轴旋转，各轴对应的坐标系绑定在对应轴上，随轴的旋转而变化。{Base}为与JAKA机械臂底座绑定的全局参考系，该坐标系的xoy平面为世界水平方向，z轴垂直向上；在全局参考系的z轴正向l1位置处，构建轴1和轴2的坐标系，轴1坐标系的三个轴方向与{Base}坐标系三轴朝向一致，轴2坐标系x轴与轴1坐标系的x轴一致，轴2坐标系的y轴与轴1坐标系的z轴方向一致，轴2坐标系的z轴沿轴1坐标系y轴负向；轴3坐标系的原点与轴2坐标系的原点在同一高度上，沿轴2坐标系的z轴平移l4-l2, 同时沿轴2坐标系的x轴平移l3, 坐标系朝向与轴2的相同；轴4构建的坐标系与轴3坐标系高度一致，沿轴3的x轴平移l5，同时沿轴3的z轴负向平移l6, 坐标系朝向与轴3的相同；轴5构建的坐标系沿轴4坐标系的y轴负向平移l7, x轴朝向与轴4的x轴一致，y轴朝向轴4的z轴正向，z轴为轴4的y轴负向；轴六(法兰盘)坐标系沿轴5的y轴正向平移l8, 其x轴正向与轴5的x轴正向同向，y轴沿轴5的z轴负向，z轴沿轴5的y轴正向。
正向运动学计算
机械臂正向运动学即给定各关节角计算末端位姿的过程。各轴的旋转角度以
θ
1
\theta_1
θ1、
θ
2
\theta_2
θ2、
θ
3
\theta_3
θ3、
θ
4
\theta_4
θ4、
θ
5
\theta_5
θ5、
θ
6
\theta_6
θ6表示，以world代表全局基坐标系，则轴6坐标系（法兰盘）在全局world中的表示为:
T
6
w
=
T
1
w
∗
T
2
1
∗
T
3
2
∗
T
4
3
∗
T
5
4
∗
T
6
5
T_6^w=T_1^w*T_2^1*T_3^2*T_4^3*T_5^4*T_6^5
T6w=T1w∗T21∗T32∗T43∗T54∗T65
其中，
T
1
w
=
[
c
o
s
(
θ
1
)
−
s
i
n
(
θ
1
)
0
0
s
i
n
(
θ
1
)
c
o
s
(
θ
1
)
0
0
0
0
1
l
1
0
0
0
1
]
T_1^w=\begin{bmatrix} {cos(\theta_1)}&{-sin(\theta_1)}&{0}&{0}\\ {sin(\theta_1)}&{cos(\theta_1)}&{0}&{0}\\ {0}&{0}&{1}&{l_1}\\ {0}&{0}&{0}&{1}\\ \end{bmatrix}
T1w=
cos(θ1)sin(θ1)00−sin(θ1)cos(θ1)00001000l11
T
2
1
=
T
1
T
2
=
[
1
0
0
0
0
0
−
1
0
0
1
0
0
0
0
0
1
]
[
c
o
s
(
θ
2
)
−
s
i
n
(
θ
2
)
0
0
s
i
n
(
θ
2
)
c
o
s
(
θ
2
)
0
0
0
0
1
0
0
0
0
1
]
=
[
c
o
s
(
θ
2
)
−
s
i
n
(
θ
2
)
0
0
0
0
−
1
0
s
i
n
(
θ
2
)
c
o
s
(
θ
2
)
0
0
0
0
0
1
]
T_2^1=T_1T_2=\begin{bmatrix} {1}&{0}&{0}&{0}\\ {0}&{0}&{-1}&{0}\\ {0}&{1}&{0}&{0}\\ {0}&{0}&{0}&{1}\\ \end{bmatrix}\begin{bmatrix} {cos(\theta_2)}&{-sin(\theta_2)}&{0}&{0}\\ {sin(\theta_2)}&{cos(\theta_2)}&{0}&{0}\\ {0}&{0}&{1}&{0}\\ {0}&{0}&{0}&{1}\\ \end{bmatrix}\\=\begin{bmatrix} {cos(\theta_2)}&{-sin(\theta_2)}&{0}&{0}\\ {0}&{0}&{-1}&{0}\\ {sin(\theta_2)}&{cos(\theta_2)}&{0}&{0}\\ {0}&{0}&{0}&{1}\\ \end{bmatrix}
T21=T1T2=
100000100−1000001
cos(θ2)sin(θ2)00−sin(θ2)cos(θ2)0000100001
=
cos(θ2)0sin(θ2)0−sin(θ2)0cos(θ2)00−1000001
T
3
2
=
[
c
o
s
(
θ
3
)
−
s
i
n
(
θ
3
)
0
l
3
s
i
n
(
θ
3
)
c
o
s
(
θ
3
)
0
0
0
0
1
l
4
−
l
2
0
0
0
1
]
T_3^2=\begin{bmatrix} {cos(\theta_3)}&{-sin(\theta_3)}&{0}&{l_3}\\ {sin(\theta_3)}&{cos(\theta_3)}&{0}&{0}\\ {0}&{0}&{1}&{l_4-l_2}\\ {0}&{0}&{0}&{1}\\ \end{bmatrix}
T32=
cos(θ3)sin(θ3)00−sin(θ3)cos(θ3)000010l30l4−l21
T
4
3
=
[
c
o
s
(
θ
4
)
−
s
i
n
(
θ
4
)
0
l
5
s
i
n
(
θ
4
)
c
o
s
(
θ
4
)
0
0
0
0
1
−
l
6
0
0
0
1
]
T_4^3=\begin{bmatrix} {cos(\theta_4)}&{-sin(\theta_4)}&{0}&{l_5}\\ {sin(\theta_4)}&{cos(\theta_4)}&{0}&{0}\\ {0}&{0}&{1}&{-l_6}\\ {0}&{0}&{0}&{1}\\ \end{bmatrix}
T43=
cos(θ4)sin(θ4)00−sin(θ4)cos(θ4)000010l50−l61
T
5
4
=
T
3
T
4
=
[
1
0
0
0
0
0
−
1
−
l
7
0
1
0
0
0
0
0
1
]
[
c
o
s
(
θ
5
)
−
s
i
n
(
θ
5
)
0
0
s
i
n
(
θ
5
)
c
o
s
(
θ
5
)
0
0
0
0
1
0
0
0
0
1
]
=
[
c
o
s
(
θ
5
)
−
s
i
n
(
θ
5
)
0
0
0
0
−
1
−
l
7
s
i
n
(
θ
5
)
c
o
s
(
θ
5
)
0
0
0
0
0
1
]
T_5^4=T_3T_4=\begin{bmatrix} {1}&{0}&{0}&{0}\\ {0}&{0}&{-1}&{-l_7}\\ {0}&{1}&{0}&{0}\\ {0}&{0}&{0}&{1}\\ \end{bmatrix}\begin{bmatrix} {cos(\theta_5)}&{-sin(\theta_5)}&{0}&{0}\\ {sin(\theta_5)}&{cos(\theta_5)}&{0}&{0}\\ {0}&{0}&{1}&{0}\\ {0}&{0}&{0}&{1}\\ \end{bmatrix}\\=\begin{bmatrix} {cos(\theta_5)}&{-sin(\theta_5)}&{0}&{0}\\ {0}&{0}&{-1}&{-l_7}\\ {sin(\theta_5)}&{cos(\theta_5)}&{0}&{0}\\ {0}&{0}&{0}&{1}\\ \end{bmatrix}
T54=T3T4=
100000100−1000−l701
cos(θ5)sin(θ5)00−sin(θ5)cos(θ5)0000100001
=
cos(θ5)0sin(θ5)0−sin(θ5)0cos(θ5)00−1000−l701
T
6
5
=
T
5
T
6
=
[
1
0
0
0
0
0
1
l
8
0
−
1
0
0
0
0
0
1
]
[
c
o
s
(
θ
6
)
−
s
i
n
(
θ
6
)
0
0
s
i
n
(
θ
6
)
c
o
s
(
θ
6
)
0
0
0
0
1
0
0
0
0
1
]
=
[
c
o
s
(
θ
6
)
−
s
i
n
(
θ
6
)
0
0
0
0
1
l
8
−
s
i
n
(
θ
6
)
−
c
o
s
(
θ
6
)
0
0
0
0
0
1
]
T_6^5=T_5T_6=\begin{bmatrix} {1}&{0}&{0}&{0}\\ {0}&{0}&{1}&{l_8}\\ {0}&{-1}&{0}&{0}\\ {0}&{0}&{0}&{1}\\ \end{bmatrix}\begin{bmatrix} {cos(\theta_6)}&{-sin(\theta_6)}&{0}&{0}\\ {sin(\theta_6)}&{cos(\theta_6)}&{0}&{0}\\ {0}&{0}&{1}&{0}\\ {0}&{0}&{0}&{1}\\ \end{bmatrix}\\=\begin{bmatrix} {cos(\theta_6)}&{-sin(\theta_6)}&{0}&{0}\\ {0}&{0}&{1}&{l_8}\\ {-sin(\theta_6)}&{-cos(\theta_6)}&{0}&{0}\\ {0}&{0}&{0}&{1}\\ \end{bmatrix}
T65=T5T6=
100000−1001000l801
cos(θ6)sin(θ6)00−sin(θ6)cos(θ6)0000100001
=
cos(θ6)0−sin(θ6)0−sin(θ6)0−cos(θ6)001000l801
对于JAKA ZU7,
l
1
=
120.058
m
m
l_1=120.058mm
l1=120.058mm,
l
2
=
113.768
m
m
l_2=113.768mm
l2=113.768mm,
l
3
=
360.66
m
m
l_3=360.66mm
l3=360.66mm,
l
4
=
113.768
m
m
l_4=113.768mm
l4=113.768mm,
l
5
=
303.315
m
m
l_5=303.315mm
l5=303.315mm,
l
6
=
115.193
m
m
l_6=115.193mm
l6=115.193mm,
l
7
=
113.768
m
m
l_7=113.768mm
l7=113.768mm,
l
8
=
107.542
m
m
l_8=107.542mm
l8=107.542mm. 在这种情况下，将各轴转角带入, 可得末端位姿，即对应的正向运动学。
逆向运动学求解
逆向运动学为给定末端位姿求解各关节转角的过程。为了获取全部逆向解，本课题采用消元法进行求解。注：所有求取的转角，会根据机械臂相应轴转角范围进行周期拓展，同时实现程序中使用了数值化方法。轴6坐标系(法兰盘)在基坐标系中的的表示假定为(末端位姿):
T
6
w
=
[
n
x
o
x
a
x
t
x
n
y
o
y
a
y
t
y
n
z
o
z
a
z
t
z
0
0
0
1
]
T_6^w=\begin{bmatrix} {n_x}&{o_x}&{a_x}&{t_x}\\ {n_y}&{o_y}&{a_y}&{t_y}\\ {n_z}&{o_z}&{a_z}&{t_z}\\ {0}&{0}&{0}&{1}\\ \end{bmatrix}
T6w=
nxnynz0oxoyoz0axayaz0txtytz1
易知:
(
T
1
w
)
−
1
T
6
w
(
T
6
5
)
−
1
=
T
2
1
T
3
2
T
4
3
T
5
4
(T_1^w)^{-1}T_6^w(T_6^5)^{-1}=T_2^1T_3^2T_4^3T_5^4
(T1w)−1T6w(T65)−1=T21T32T43T54，带入
T
1
w
、
T
2
1
、
T
3
2
、
T
4
3
、
T
5
4
、
T
6
5
、
T
6
w
T_1^w、T_2^1、T_3^2、T_4^3、T_5^4、T_6^5、T_6^w
T1w、T21、T32、T43、T54、T65、T6w，可得上式的左侧和右侧分别为：
[
c
θ
1
(
n
x
c
θ
6
−
o
x
s
θ
6
)
+
s
θ
1
(
n
y
c
θ
6
−
o
y
s
θ
6
)
a
x
c
θ
1
+
a
y
s
θ
1
−
c
θ
1
(
n
x
s
θ
6
+
o
x
c
θ
6
)
−
s
θ
1
(
n
y
s
θ
6
+
o
y
c
θ
6
)
c
θ
1
(
t
x
−
l
8
a
x
)
+
s
θ
1
(
t
y
−
l
8
a
y
)
−
s
θ
1
(
n
x
c
θ
6
−
o
x
s
θ
6
)
+
c
θ
1
(
n
y
c
θ
6
−
o
y
s
θ
6
)
−
a
x
s
θ
1
+
a
y
c
θ
1
s
θ
1
(
n
x
s
θ
6
+
o
x
c
θ
6
)
−
c
θ
1
(
n
y
s
θ
6
+
o
y
c
θ
6
)
−
s
θ
1
(
t
x
−
l
8
a
x
)
+
c
θ
1
(
t
y
−
l
8
a
y
)
n
z
c
θ
6
−
o
z
s
θ
6
a
z
−
n
z
s
θ
6
−
o
z
c
θ
6
t
z
−
l
8
a
z
−
l
1
0
0
0
1
]
(
1
)
\begin{bmatrix} {c\theta_1(n_xc\theta_6-o_xs\theta_6)+s\theta_1(n_yc\theta_6-o_ys\theta_6)}& {a_xc\theta_1+a_ys\theta_1}& {-c\theta_1(n_xs\theta_6+o_xc\theta_6)-s\theta_1(n_ys\theta_6+o_yc\theta_6)}& {c\theta_1(t_x-l_8a_x)+s\theta_1(t_y-l_8a_y)}\\ {-s\theta_1(n_xc\theta_6-o_xs\theta_6)+c\theta_1(n_yc\theta_6-o_ys\theta_6)}& {-a_xs\theta_1+a_yc\theta_1}& {s\theta_1(n_xs\theta_6+o_xc\theta_6)-c\theta_1(n_ys\theta_6+o_yc\theta_6)}& {-s\theta_1(t_x-l_8a_x)+c\theta_1(t_y-l_8a_y)}\\ {n_zc\theta_6-o_zs\theta_6}&{a_z}&{-n_zs\theta_6-o_zc\theta_6}&{t_z-l_8a_z-l_1}\\ {0}&{0}&{0}&{1}\\ \end{bmatrix}(1)
cθ1(nxcθ6−oxsθ6)+sθ1(nycθ6−oysθ6)−sθ1(nxcθ6−oxsθ6)+cθ1(nycθ6−oysθ6)nzcθ6−ozsθ60axcθ1+aysθ1−axsθ1+aycθ1az0−cθ1(nxsθ6+oxcθ6)−sθ1(nysθ6+oycθ6)sθ1(nxsθ6+oxcθ6)−cθ1(nysθ6+oycθ6)−nzsθ6−ozcθ60cθ1(tx−l8ax)+sθ1(ty−l8ay)−sθ1(tx−l8ax)+cθ1(ty−l8ay)tz−l8az−l11
(1)
[
c
o
s
(
θ
2
+
θ
3
+
θ
4
)
c
o
s
θ
5
−
c
o
s
(
θ
2
+
θ
3
+
θ
4
)
s
i
n
θ
5
s
i
n
(
θ
2
+
θ
3
+
θ
4
)
l
7
s
i
n
(
θ
2
+
θ
3
+
θ
4
)
+
l
3
c
o
s
θ
2
+
l
5
c
o
s
(
θ
2
+
θ
3
)
−
s
i
n
θ
5
−
c
o
s
θ
5
0
l
6
+
l
2
−
l
4
s
i
n
(
θ
2
+
θ
3
+
θ
4
)
c
o
s
θ
5
−
s
i
n
(
θ
2
+
θ
3
+
θ
4
)
s
i
n
θ
5
−
c
o
s
(
θ
2
+
θ
3
+
θ
4
)
−
l
7
c
o
s
(
θ
2
+
θ
3
+
θ
4
)
+
l
3
s
i
n
θ
2
+
l
5
s
i
n
(
θ
2
+
θ
3
)
0
0
0
1
]
(
2
)
\begin{bmatrix} {cos(\theta_2+\theta_3+\theta_4)cos\theta_5}& {-cos(\theta_2+\theta_3+\theta_4)sin\theta_5}& {sin(\theta_2+\theta_3+\theta_4)}& {l_7sin(\theta_2+\theta_3+\theta_4)+l_3cos\theta_2+l_5cos(\theta_2+\theta_3)}\\ {-sin\theta_5}&{-cos\theta_5}&{0}&{l_6+l_2-l_4}\\ {sin(\theta_2+\theta_3+\theta_4)cos\theta_5}& {-sin(\theta_2+\theta_3+\theta_4)sin\theta_5}& {-cos(\theta_2+\theta_3+\theta_4)}& {-l_7cos(\theta_2+\theta_3+\theta_4)+l_3sin\theta_2+l_5sin(\theta_2+\theta_3)}\\ {0}&{0}&{0}&{1}\\ \end{bmatrix}(2)
cos(θ2+θ3+θ4)cosθ5−sinθ5sin(θ2+θ3+θ4)cosθ50−cos(θ2+θ3+θ4)sinθ5−cosθ5−sin(θ2+θ3+θ4)sinθ50sin(θ2+θ3+θ4)0−cos(θ2+θ3+θ4)0l7sin(θ2+θ3+θ4)+l3cosθ2+l5cos(θ2+θ3)l6+l2−l4−l7cos(θ2+θ3+θ4)+l3sinθ2+l5sin(θ2+θ3)1
(2)
对比上面两式：
★计算
θ
1
\theta_1
θ1
对比第二行第四列元素, 有：
−
s
i
n
θ
1
(
t
x
−
l
8
a
x
)
+
c
o
s
θ
1
(
t
y
−
l
8
a
y
)
=
l
6
+
l
2
−
l
4
-sin\theta_1(t_x-l_8a_x)+cos\theta_1(t_y-l_8a_y)=l_6+l_2-l_4
−sinθ1(tx−l8ax)+cosθ1(ty−l8ay)=l6+l2−l4, 有
θ
1
=
a
r
c
s
i
n
(
(
l
6
+
l
2
−
l
4
)
/
(
(
t
x
−
l
8
a
x
)
2
+
(
t
y
−
l
8
a
y
)
2
)
)
−
ψ
1
\theta_1=arcsin((l_6+l_2-l_4)/(\sqrt{(t_x-l_8a_x)^2+(t_y-l_8a_y)^2))}-\psi_1
θ1=arcsin((l6+l2−l4)/((tx−l8ax)2+(ty−l8ay)2))−ψ1, 其中，
ψ
1
=
a
t
a
n
2
(
t
y
−
l
8
a
y
,
−
(
t
x
−
l
8
a
x
)
)
\psi_1=atan2(t_y-l_8a_y, -(t_x-l_8a_x))
ψ1=atan2(ty−l8ay,−(tx−l8ax))。
★计算
θ
5
\theta_5
θ5
对比第二行第二列元素，有：
−
a
x
s
i
n
θ
1
+
a
y
c
o
s
θ
1
=
−
c
o
s
θ
5
-a_xsin\theta_1+a_ycos\theta_1=-cos\theta_5
−axsinθ1+aycosθ1=−cosθ5, 从而，
θ
5
=
a
c
o
s
(
a
x
s
i
n
θ
1
−
a
y
c
o
s
θ
1
)
\theta_5=acos(a_xsin\theta_1-a_ycos\theta_1)
θ5=acos(axsinθ1−aycosθ1)
★计算
θ
2
\theta_2
θ2、
θ
3
\theta_3
θ3、
θ
4
\theta_4
θ4、
θ
6
\theta_6
θ6
分两种情况：
1） 情况一（
s
i
n
θ
5
≠
0
sin\theta_5\neq0
sinθ5=0）
对照第一行第二列以及第三行第二列，有：
{
s
i
n
(
θ
2
+
θ
3
+
θ
4
)
=
−
a
z
/
s
i
n
θ
5
c
o
s
(
θ
2
+
θ
3
+
θ
4
)
=
−
(
a
x
c
o
s
θ
1
+
a
y
s
i
n
θ
1
)
/
s
i
n
θ
5
\begin{cases} sin(\theta_2+\theta_3+\theta_4)=-a_z/sin\theta_5\\ cos(\theta_2+\theta_3+\theta_4)=-(a_xcos\theta_1+a_ysin\theta_1)/sin\theta_5\\ \end{cases}
{sin(θ2+θ3+θ4)=−az/sinθ5cos(θ2+θ3+θ4)=−(axcosθ1+aysinθ1)/sinθ5
从而，
θ
2
+
θ
3
+
θ
4
=
a
t
a
n
2
(
−
a
z
/
s
i
n
θ
5
,
−
(
a
x
c
o
s
θ
1
+
a
y
s
i
n
θ
1
)
/
s
i
n
θ
5
)
\theta_2+\theta_3+\theta_4=atan2(-a_z/sin\theta_5, -(a_xcos\theta_1+a_ysin\theta_1)/sin\theta_5)
θ2+θ3+θ4=atan2(−az/sinθ5,−(axcosθ1+aysinθ1)/sinθ5), 对照第二行第一列，有:
(
o
y
c
o
s
θ
1
−
o
x
s
i
n
θ
1
)
s
i
n
θ
6
+
(
n
x
s
i
n
θ
1
−
n
y
c
o
s
θ
1
)
c
o
s
θ
6
=
s
i
n
θ
5
(o_ycos\theta_1-o_xsin\theta_1)sin\theta_6+(n_xsin\theta_1-n_ycos\theta_1)cos\theta_6=sin\theta_5
(oycosθ1−oxsinθ1)sinθ6+(nxsinθ1−nycosθ1)cosθ6=sinθ5, 从而，
θ
6
=
a
s
i
n
(
s
i
n
θ
5
/
(
o
y
c
o
s
θ
1
−
o
x
s
i
n
θ
1
)
2
+
(
n
x
s
i
n
θ
1
−
n
y
c
o
s
θ
1
)
2
)
−
a
t
a
n
2
(
n
x
s
i
n
θ
1
−
n
y
c
o
s
θ
1
,
o
y
c
o
s
θ
1
−
o
x
s
i
n
θ
1
)
\theta_6=asin(sin\theta_5/\sqrt{(o_ycos\theta_1-o_xsin\theta_1)^2+(n_xsin\theta_1-n_ycos\theta_1)^2})-atan2(n_xsin\theta_1-n_ycos\theta_1, o_ycos\theta_1-o_xsin\theta_1)
θ6=asin(sinθ5/(oycosθ1−oxsinθ1)2+(nxsinθ1−nycosθ1)2)−atan2(nxsinθ1−nycosθ1,oycosθ1−oxsinθ1), 从而可得
θ
6
\theta_6
θ6。
对比第一行第四列元素和第三行第四列元素，并进行整理可得:
c
o
s
θ
3
=
(
(
c
θ
1
(
t
x
−
l
8
a
x
)
+
s
θ
1
(
t
y
−
l
8
a
y
)
−
l
7
s
i
n
(
θ
2
+
θ
3
+
θ
4
)
)
2
+
(
t
z
−
l
8
a
z
−
l
1
+
l
7
c
o
s
(
θ
2
+
θ
3
+
θ
4
)
)
2
−
l
5
2
−
l
3
2
)
/
(
2
l
3
l
5
)
cos\theta_3=((c\theta_1(t_x-l_8a_x)+s\theta_1(t_y-l_8a_y)-l_7sin(\theta_2+\theta_3+\theta_4))^2+(t_z-l_8a_z-l_1+l_7cos(\theta_2+\theta_3+\theta_4))^2-l_5^2-l_3^2)/(2l_3l_5)
cosθ3=((cθ1(tx−l8ax)+sθ1(ty−l8ay)−l7sin(θ2+θ3+θ4))2+(tz−l8az−l1+l7cos(θ2+θ3+θ4))2−l52−l32)/(2l3l5), 从而可得
θ
3
\theta_3
θ3, 且为:
θ
3
=
a
c
o
s
(
(
(
c
θ
1
(
t
x
−
l
8
a
x
)
+
s
θ
1
(
t
y
−
l
8
a
y
)
−
l
7
s
i
n
(
θ
2
+
θ
3
+
θ
4
)
)
2
+
(
t
z
−
l
8
a
z
−
l
1
+
l
7
c
o
s
(
θ
2
+
θ
3
+
θ
4
)
)
2
−
l
5
2
−
l
3
2
)
/
(
2
l
3
l
5
)
)
\theta_3=acos(((c\theta_1(t_x-l_8a_x)+s\theta_1(t_y-l_8a_y)-l_7sin(\theta_2+\theta_3+\theta_4))^2+(t_z-l_8a_z-l_1+l_7cos(\theta_2+\theta_3+\theta_4))^2-l_5^2-l_3^2)/(2l_3l_5))
θ3=acos(((cθ1(tx−l8ax)+sθ1(ty−l8ay)−l7sin(θ2+θ3+θ4))2+(tz−l8az−l1+l7cos(θ2+θ3+θ4))2−l52−l32)/(2l3l5))。在此基础上，第一行第四列进行展开，可得：
−
(
l
5
s
i
n
θ
3
)
s
i
n
θ
2
+
(
l
5
c
o
s
θ
3
+
l
3
)
c
o
s
θ
2
=
c
o
s
θ
1
(
t
x
−
l
8
a
x
)
+
s
i
n
θ
1
(
t
y
−
l
8
a
y
)
−
l
7
s
i
n
(
θ
2
+
θ
3
+
θ
4
)
-(l_5sin\theta_3)sin\theta_2+(l_5cos\theta_3+l_3)cos\theta_2=cos\theta_1(t_x-l_8a_x)+sin\theta_1(t_y-l_8a_y)-l_7sin(\theta_2+\theta_3+\theta_4)
−(l5sinθ3)sinθ2+(l5cosθ3+l3)cosθ2=cosθ1(tx−l8ax)+sinθ1(ty−l8ay)−l7sin(θ2+θ3+θ4),
从而可得
θ
2
\theta_2
θ2, 且为:
θ
2
=
a
s
i
n
(
(
c
o
s
θ
1
(
t
x
−
l
8
a
x
)
+
s
i
n
θ
1
(
t
y
−
l
8
a
y
)
−
l
7
s
i
n
(
θ
2
+
θ
3
+
θ
4
)
)
/
(
(
l
5
s
i
n
θ
3
)
2
+
(
l
5
c
o
s
θ
3
+
l
3
)
2
)
)
−
a
t
a
n
2
(
l
5
c
o
s
θ
3
+
l
3
,
−
l
5
s
i
n
θ
3
)
\theta_2=asin((cos\theta_1(t_x-l_8a_x)+sin\theta_1(t_y-l_8a_y)-l_7sin(\theta_2+\theta_3+\theta_4))/(\sqrt{(l_5sin\theta_3)^2+(l_5cos\theta_3+l_3)^2}))-atan2(l_5cos\theta_3+l_3, -l_5sin\theta_3)
θ2=asin((cosθ1(tx−l8ax)+sinθ1(ty−l8ay)−l7sin(θ2+θ3+θ4))/((l5sinθ3)2+(l5cosθ3+l3)2))−atan2(l5cosθ3+l3,−l5sinθ3)。
此外，由于
θ
2
+
θ
3
+
θ
4
\theta_2+\theta_3+\theta_4
θ2+θ3+θ4前面已获取，从而可直接得
θ
4
=
(
θ
2
+
θ
3
+
θ
4
)
−
θ
2
−
θ
3
\theta_4=(\theta_2+\theta_3+\theta_4)-\theta_2-\theta_3
θ4=(θ2+θ3+θ4)−θ2−θ3。
2） 情况二（
s
i
n
θ
5
=
0
sin\theta_5=0
sinθ5=0）
这种情况比较特殊，
θ
2
\theta_2
θ2、
θ
3
\theta_3
θ3、
θ
4
\theta_4
θ4包含无穷多个解。在确定
θ
2
\theta_2
θ2、
θ
3
\theta_3
θ3、
θ
4
\theta_4
θ4后，带入下式:
T
5
−
1
(
T
5
4
)
−
1
(
T
4
3
)
−
1
(
T
3
2
)
−
1
(
T
2
1
)
−
1
(
T
1
w
)
−
1
T
6
w
=
T
6
T_5^{-1}(T_5^4)^{-1}(T_4^3)^{-1}(T_3^2)^{-1}(T_2^1)^{-1}(T_1^w)^{-1}T_6^w=T_6
T5−1(T54)−1(T43)−1(T32)−1(T21)−1(T1w)−1T6w=T6
假设
T
6
T_6
T6前两行第一个元素分别为：
v
a
l
1
val_1
val1与
v
a
l
2
val_2
val2, 则可得:
θ
6
=
a
t
a
n
2
(
v
a
l
2
,
v
a
l
1
)
\theta_6=atan2(val_2, val_1)
θ6=atan2(val2,val1)。
对于此种情况下的
θ
2
\theta_2
θ2、
θ
3
\theta_3
θ3、
θ
4
\theta_4
θ4，首先确定一个中间参考系，为轴2处坐标系原始位置（轴2未发生旋转时其对应的坐标系）。针对确定的中间参考系，需要计算轴5处构建的坐标系在中间参考系中的位置。为了获取该表示，首先计算轴6处坐标系(法兰盘)相对于中间参考系的表示，为：
T
1
−
1
(
T
1
w
)
−
1
T
6
w
=
T
2
T
3
2
T
4
3
T
5
4
T
6
5
T_1^{-1}(T_1^w)^{-1}T_6^w=T_2T_3^2T_4^3T_5^4T_6^5
T1−1(T1w)−1T6w=T2T32T43T54T65
假设轴6处坐标系(法兰盘)相对于中间参考系的表示对应的平移部分为
t
⃗
\vec{t}
t，轴6坐标系的z轴在相对于中间参考系的表示为
a
⃗
\vec{a}
a，则轴5处构建的坐标系在中间参考系中的位置表示如下：
x
⃗
=
t
⃗
−
a
⃗
∗
l
8
\vec{x}=\vec{t}-\vec{a}*l_8
x=t−a∗l8
设上式的前两维分别为
x
1
,
y
1
x_1,y_1
x1,y1, 由于轴2、3、4的旋转轴平行，则，轴2、3、4的转角之和可使用如下特殊值：
θ
2
+
θ
3
+
θ
4
=
(
(
l
3
+
l
5
+
l
7
−
x
1
2
+
y
1
2
)
π
/
(
l
3
+
l
5
+
l
7
)
)
+
a
t
a
n
2
(
y
1
,
x
1
)
\theta_2+\theta_3+\theta_4=((l_3+l_5+l_7-\sqrt{x_1^2+y_1^2})\pi/(l_3+l_5+l_7))+atan2(y_1, x_1)
θ2+θ3+θ4=((l3+l5+l7−x12+y12)π/(l3+l5+l7))+atan2(y1,x1)
在这种情况，轴4处坐标系原点在中间参考系中的x坐标和y坐标分别为：
{
x
2
=
x
1
−
l
7
c
o
s
(
θ
2
+
θ
3
+
θ
4
)
y
2
=
y
1
−
l
7
s
i
n
(
θ
2
+
θ
3
+
θ
4
)
\begin{cases} x_2=x_1-l_7cos(\theta_2+\theta_3+\theta_4)\\ y_2=y_1-l_7sin(\theta_2+\theta_3+\theta_4)\\ \end{cases}
{x2=x1−l7cos(θ2+θ3+θ4)y2=y1−l7sin(θ2+θ3+θ4)
可得下式：
{
l
3
c
o
s
θ
2
+
l
5
c
o
s
(
θ
2
+
θ
3
)
=
x
2
l
3
s
i
n
θ
2
+
l
5
s
i
n
(
θ
2
+
θ
3
)
=
y
2
(
3
)
\begin{cases} l_3cos\theta_2+l_5cos(\theta_2+\theta_3)=x_2\\ l_3sin\theta_2+l_5sin(\theta_2+\theta_3)=y_2\\ \end{cases} (3)
{l3cosθ2+l5cos(θ2+θ3)=x2l3sinθ2+l5sin(θ2+θ3)=y2(3)
从而可得
θ
3
=
a
c
o
s
(
(
x
2
2
+
y
2
2
−
l
3
2
−
l
5
2
)
/
(
2
l
3
l
5
)
)
\theta_3=acos((x_2^2+y_2^2-l_3^2-l_5^2)/(2l_3l_5))
θ3=acos((x22+y22−l32−l52)/(2l3l5))
式3中带入
θ
3
\theta_3
θ3, 可得:
[
−
l
5
s
i
n
θ
3
l
3
+
l
5
c
o
s
θ
3
l
3
+
l
5
c
o
s
θ
3
l
5
s
i
n
θ
3
]
[
s
i
n
θ
2
c
o
s
θ
2
]
=
[
x
2
y
2
]
\begin{bmatrix} -l_5sin\theta_3&l_3+l_5cos\theta_3\\ l_3+l_5cos\theta_3&l_5sin\theta_3\\ \end{bmatrix} \begin{bmatrix} sin\theta_2\\ cos\theta_2\\ \end{bmatrix}= \begin{bmatrix} x_2\\ y_2\\ \end{bmatrix}
[−l5sinθ3l3+l5cosθ3l3+l5cosθ3l5sinθ3][sinθ2cosθ2]=[x2y2]
从而可得:
[
s
i
n
θ
2
c
o
s
θ
2
]
=
[
−
l
5
s
i
n
θ
3
l
3
+
l
5
c
o
s
θ
3
l
3
+
l
5
c
o
s
θ
3
l
5
s
i
n
θ
3
]
−
1
[
x
2
y
2
]
\begin{bmatrix} sin\theta_2\\ cos\theta_2\\ \end{bmatrix}=\begin{bmatrix} -l_5sin\theta_3&l_3+l_5cos\theta_3\\ l_3+l_5cos\theta_3&l_5sin\theta_3\\ \end{bmatrix}^{-1}\begin{bmatrix} x_2\\ y_2\\ \end{bmatrix}
[sinθ2cosθ2]=[−l5sinθ3l3+l5cosθ3l3+l5cosθ3l5sinθ3]−1[x2y2]
最终可得
θ
2
=
a
t
a
n
2
(
s
i
n
θ
2
,
c
o
s
θ
2
)
\theta_2=atan2(sin\theta_2, cos\theta_2)
θ2=atan2(sinθ2,cosθ2), 在此基础上同时可得
θ
4
=
(
θ
2
+
θ
3
+
θ
4
)
−
θ
2
−
θ
3
\theta_4=(\theta_2+\theta_3+\theta_4)-\theta_2-\theta_3
θ4=(θ2+θ3+θ4)−θ2−θ3
代码
设计代码依赖于Eigen库，在ubuntu操作系统中Eigen库的安装参照以下两个步骤：
步骤一、sudo apt update
步骤二、sudo apt install libeigen3-dev
代码头文件为: FK_IK.h, 内容如下：
// 正向运动学计算
// 第一个参数: angle_arr, 是一个包含六个元素的一维数组, 是六个轴的转角
// 第二个参数：rpy角，包含3个元素，roll, pitch, yaw, 是末端法兰盘坐标系在世界坐标系中表示的旋转部分，单位为弧度
// 第三个参数: trans, 包含3个元素，是末端法兰盘坐标系在世界坐标系中表示的平移部分，单位为毫米
// 第四个参数: flange_data, 计算得到的末端法兰盘坐标系在世界坐标系中的表示，是4*4的旋转矩阵，注意其中的平移部分的单位是毫米
// 第五个参数: IsRadian, 表示angle_arr中元素的单位是否为弧度, --- true表示单位是弧度
// --- false表示单位是度
void FK_Execute(double angle_arr[], double rpy[], double trans[], double flange_data[][4], bool IsRadian);
// 逆向运动学计算
// 第一个参数：flange_data为末端法兰盘坐标系在世界坐标系中的表示（平移部分为旋转）(平移部分的单位为毫米)
// 第二个参数: angle_matrix[][4]为二维矩阵，每行包含六个元素，为逆向运动学计算得到的六个轴的转角，单位为弧度
// 第三个参数: SolutionCnt为逆向运动学计算得到的解的个数，是一个引用
void IK_Execute(double flange_data[][4], double angle_matrix[][6], int &SolutionCnt);
可执行文件为: FK_IK.cpp, 内容如下：
#include <jaka_zu7_use/FK_IK.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <vector>
using namespace Eigen;
#define AngleSameThresh 0.005 //两个角度（单位为度）之间偏差的绝对值如果小于这个阈值，认为是一个角度
#define TriFuncThresh 0.1 // cos或sin值的绝对值减去1如果大于这个阈值，认为方程是错误的，否则还可以截断（假如a是一个cos的值，fabs(a)-1>TriFuncThresh, 则不用计算了，认为方程错误；如果 fabs(a)-1 <= TriFuncThresh, 在这个时候，如果a>1, 则a赋值为1，如果a<-1,则a赋值为-1）
#define ZeroThresh 0.000001 //0的阈值
#define ZeroThresh_determant 0.0001 //矩阵行列式的绝对值是否为0的阈值
#define LengthSameDeviateThresh 0.5 //单位为毫米
#define LoopAngleSameThresh 0.1 //宽松的阈值，如果两个角度差别小于该阈值，则认为是一个角度(单位为度)
#define LoopAngleSameThresh_radian (0.1*M_PI/180.0) // 与上同，但是单位为弧度
double AxisRegionDataMatrix[6][2] = {
{-360.0*M_PI/180.0, 360.0*M_PI/180.0},
{-10.0*M_PI/180.0, 200.0*M_PI/180.0},
{-128.0*M_PI/180.0, 128.0*M_PI/180.0},
{-85.0*M_PI/180.0, 265.0*M_PI/180.0},
{-360.0*M_PI/180.0, 360.0*M_PI/180.0},
{-360.0*M_PI/180.0, 360.0*M_PI/180.0}};
struct Data
{
Data()
{
IsValid = true;
for(int i=0; i<6; i++)
{
angle_arr[i] = 0;
}
}
void Copy(Data sData)
{
total = sData.total;
IsValid = sData.IsValid;
for(int i=0; i<6; i++)
{
angle_arr[i] = sData.angle_arr[i];
}
}
float angle_arr[6];
float total; //theta2 + theta3 + theta4
bool IsValid;
};
struct AxisData
{
float angle_arr[6];
void Copy(AxisData sData)
{
for(int i=0; i<6; i++)
{
angle_arr[i] = sData.angle_arr[i];
}
}
};
struct TriData
{
float angle_arr[3];
};
// 轴AxisIndex转角Angle是否合格
// AxisIndex: 轴索引，0～5
// Angle单位为弧度
bool IsInValidRegion(const int AxisIndex, const float Angle)
{
if(Angle>=AxisRegionDataMatrix[AxisIndex][0] && Angle<=AxisRegionDataMatrix[AxisIndex][1])
return true;
else
return false;
}
// 已知轴AxisIndex的一个转角angle_ref（单位为弧度）, 得到在范围内的所有解，并存放到AngleList中
void GetAngleList(const int AxisIndex, const double angle_ref, std::vector<double> &AngleList)
{
std::vector<double>().swap(AngleList);
double angle;
// 最好先保存这个，防止外面弄错，有可能外面先把angle_ref赋值给外面的数据结构，然后对于AngleList的其余元素压入堆栈，这种情况必须先压入angle_ref， 否则会错
AngleList.push_back(angle_ref);
//负向
angle = angle_ref;
while(true)
{
angle = angle - 2.0 * M_PI;
if(true == IsInValidRegion(AxisIndex, angle))
{
AngleList.push_back(angle);
}
else
{
break;
}
}
//正向
angle = angle_ref;
while(true)
{
angle = angle + 2.0 * M_PI;
if(true == IsInValidRegion(AxisIndex, angle))
{
AngleList.push_back(angle);
}
else
{
break;
}
}
}
void Calc(const double x, const double y, const double l1, const double l2, const double l3, std::vector<TriData> &TriDataContainer)
{
double angle_arr1[10], angle_arr2[10], angle_arr3[10];
int cnt_1, cnt_2, cnt_3;
int i;
TriData sTriData;
double Dis;
Eigen::Matrix<double, 2, 2, Eigen::RowMajor> A;
Eigen::Matrix<double, 2, 1> Right, Solution;
double angle, S, angle_c, total;
double x2, y2;
double x_new, y_new;
double a, b, c;
double r;
double theta1, theta2, theta3;
double val;
double angle_added;
S = sqrt(x*x + y*y);
angle = (l1 + l2 + l3 -S)/(l1 + l2 + l3)*M_PI;
angle_c = atan2(y, x);
total = angle + angle_c;
x2 = x - cos(total) * l3;
y2 = y - sin(total) * l3;
//l1*cos(theta1) + l2*cos(theta1+theta2) = x2
//l1*sin(theta1) + l2*sin(theta1+theta2) = y2
// l1^2 + l2^2 + 2.0*l1*l2*cos(theta2) = x2^2 + y2^2
val = (pow(x2, 2) + pow(y2, 2) - pow(l1, 2) - pow(l2, 2))/(2.0*l1*l2);
if(fabs(val) - 1 > 0.1)
{
return;
}
if(val>=1)
val = 1;
else if(val<=-1)
val = -1;
theta2 = acos(val);
if(fabs(fabs(theta2) - M_PI) < AngleSameThresh)
{
// 转角5只有一个角度
cnt_2 = 1;
angle_arr2[0] = theta2;
}
else
{
//转角5至少有两个解(其它都是周期性的)
if(fabs(theta2) > 0.1*180.0/M_PI)
{
cnt_2 = 2;
angle_arr2[0] = theta2;
angle_arr2[1] = -theta2;
}
else
{
cnt_2 = 1;
angle_arr2[0] = theta2;
}
}
for(i=0; i<cnt_2; i++)
{
theta2 = angle_arr2[i];
// l1*cos(theta1) + l2*cos(theta1)*cos(theta2) - l2*sin(theta1)*sin(theta2) = x2
// l1*sin(theta1) + l2*cos(theta2)*sin(theta1) + l2*sin(theta2)*cos(theta1) = y2
A<< -l2*sin(theta2), l1+l2*cos(theta2),
l1+l2*cos(theta2), l2*sin(theta2);
Right << x2, y2;
if(fabs(A.determinant()) < ZeroThresh_determant)
continue;
Solution = A.inverse() * Right;
//sin(theta1) = Solution(0,0)
//cos(theta1) = Solution(1, 0)
theta1 = atan2(Solution(0,0), Solution(1, 0));
theta3 = total - theta1 - theta2;
x_new = l1*cos(theta1) + l2*cos(theta1 + theta2) + l3*cos(theta1 + theta2 + theta3);
y_new = l1*sin(theta1) + l2*sin(theta1 + theta2) + l3*sin(theta1 + theta2 + theta3);
Dis = sqrt(pow(x-x_new, 2) + pow(y - y_new, 2));
if(Dis < LengthSameDeviateThresh)
{
sTriData.angle_arr[0] = theta1;
sTriData.angle_arr[1] = theta2;
sTriData.angle_arr[2] = theta3+M_PI/2.0; // 这里加上90度是因为jaka机械臂轴4零位的特殊姿态
TriDataContainer.push_back(sTriData);
}
}
}
void Special(const double theta1, const double theta5, Eigen::Matrix<double, 4, 4, Eigen::RowMajor> set_data, std::vector<TriData> &TriDataContainer)
{
float x, y, z;
Eigen::Matrix<double, 4, 4, Eigen::RowMajor> flange_in_axis2;
Eigen::Matrix<double, 4, 4, Eigen::RowMajor> matrix_1, matrix_2;
double nx, ny, ox, oy, val;
double l1 = 120.058, l2 = 113.768, l3 = 360.66, l4 = 113.768, l5 = 303.315, l6 = 115.193, l7 = 113.768, l8 = 107.542;
matrix_1 << cos(theta1), -sin(theta1), 0, 0,
sin(theta1), cos(theta1), 0, 0,
0, 0, 1, l1,
0, 0, 0, 1;
matrix_2 << 1, 0, 0, 0,
0, 0, -1, 0,
0, 1, 0, 0,
0, 0, 0, 1;
flange_in_axis2 = matrix_2.inverse() * matrix_1.inverse() * set_data;
x = flange_in_axis2(0, 3) - flange_in_axis2(0, 2) * l8;
y = flange_in_axis2(1, 3) - flange_in_axis2(1, 2) * l8;
z = flange_in_axis2(2, 3) - flange_in_axis2(2, 2) * l8;
// (x, y, z )为axis5中心在axix2坐标系下的坐标
Calc(x, y, l3, l5, l7, TriDataContainer);
}
void CalcTheta6(std::vector<Data> &Solution, Eigen::Matrix<double, 4, 4, Eigen::RowMajor> flange_set_data)
{
int i, j, SolutionCnt;
std::vector<double> AngleList;
Data sSolution;
double nx, ny;
double l1 = 120.058, l2 = 113.768, l3 = 360.66, l4 = 113.768, l5 = 303.315, l6 = 115.193, l7 = 113.768, l8 = 107.542;
Eigen::Matrix<double, 4, 4, Eigen::RowMajor> matrix_1, matrix_2, matrix_3, matrix_4, matrix_5, matrix_6, matrix_7, matrix_8, matrix_9, matrix_left;
double angle_1, angle_2, angle_3, angle_4, angle_5, angle_6;
SolutionCnt = Solution.size();
for(i=0; i<SolutionCnt; i++)
{
angle_1 = Solution[i].angle_arr[0];
angle_2 = Solution[i].angle_arr[1];
angle_3 = Solution[i].angle_arr[2];
angle_4 = Solution[i].angle_arr[3];
angle_5 = Solution[i].angle_arr[4];
matrix_1 << cos(angle_1), -sin(angle_1), 0, 0,
sin(angle_1), cos(angle_1), 0, 0,
0, 0, 1, l1,
0, 0, 0, 1;
matrix_2 << 1, 0, 0, 0,
0, 0, -1, 0,
0, 1, 0, 0,
0, 0, 0, 1;
matrix_3 << cos(angle_2), -sin(angle_2), 0, 0,
sin(angle_2), cos(angle_2), 0, 0,
0, 0, 1, 0,
0, 0, 0, 1;
matrix_4 << cos(angle_3), -sin(angle_3), 0, l3,
sin(angle_3), cos(angle_3), 0, 0,
0, 0, 1, l4 - l2,
0, 0, 0, 1;
matrix_5 << cos(angle_4), -sin(angle_4), 0, l5,
sin(angle_4), cos(angle_4), 0, 0,
0, 0, 1, -l6,
0, 0, 0, 1;
matrix_6 << 1, 0, 0, 0,
0, 0, -1, -l7,
0, 1, 0, 0,
0, 0, 0, 1;
matrix_7 << cos(angle_5), -sin(angle_5), 0, 0,
sin(angle_5), cos(angle_5), 0, 0,
0, 0, 1, 0,
0, 0, 0, 1;
matrix_8 << 1, 0, 0, 0,
0, 0, 1, l8,
0, -1, 0, 0,
0, 0, 0, 1;
matrix_left = matrix_1 * matrix_2 * matrix_3 * matrix_4 * matrix_5 * matrix_6 * matrix_7 * matrix_8;
matrix_9 = matrix_left.inverse() * flange_set_data;
nx = matrix_9(0, 0);
ny = matrix_9(1, 0);
//Solution[i].angle_arr[5] = atan2(ny, nx);
angle_6 = atan2(ny, nx);
std::vector<double>().swap(AngleList);
GetAngleList(5, angle_6, AngleList);
Solution[i].angle_arr[5] = AngleList[0];
sSolution.Copy(Solution[i]);
for(j=1; j<AngleList.size(); j++)
{
sSolution.angle_arr[5] = AngleList[j];
Solution.push_back(sSolution);
}
std::vector<double>().swap(AngleList);
}
}
//根据轴索引，sin,cos,或tan数值得到所有非周期性的转角列表
// AxisIndex: 轴索引 0 ～ 5
// AngleDisplace: 角度偏移（单位为弧度）
// funcval : 对应的三角函数值, f(axis_angle + AngleDisPlace) = funcval
// Type: 0---sin 的值；1 --- cos的值； 2 ---tan的数值
// AngleList 对应的角度列表(里面已经去除了偏移)
void GetAngleList(const int AxisIndex, const double AngleDisplace, double funcval, const int Type, std::vector<double> &AngleList)
{
// 已知角度最大范围也不会超过-3.0PI ~ 3.0*PI
double theta_total;
double theta;
int Cnt, i;
bool Flag;
std::vector<double> angle_candidate;
std::vector<double>().swap(AngleList);
if(AxisIndex <0 || AxisIndex > 5)
return;
if(fabs(funcval) - 1.0 > TriFuncThresh)
{
return;
}
if(funcval >= 1.0)
funcval = 1.0;
else if(funcval <= -1.0)
funcval = -1.0;
// 初步保证轴转角在(-3.0*M_PI, 3.0*M_PI)
// 后面在详细判定是否在轴转角的实际确定范围内
if(0 == Type)
{
theta_total = asin(funcval);
if(fabs(fabs(theta_total) - M_PI/2.0) < LoopAngleSameThresh_radian)
{
//theta_total = -PI/2.0 或者 PI/2.0
// 为了让真正轴转角在范围[-3.0*PI, 3.0*PI]
// theta_total - 2.0*PI - AngleDisplace, theta_total - AngleDisplace, theta_total + 2.0*PI - AngleDisplace
angle_candidate.push_back(theta_total - 2.0*M_PI - AngleDisplace);
angle_candidate.push_back(theta_total - AngleDisplace);
angle_candidate.push_back(theta_total + 2.0*M_PI - AngleDisplace);
}
else
{
if(theta_total > 0)
{
// theta_total为: asin(funcval), M_PI-asin(funcval)
//theta_total - AngleDisplace - 2.0*M_PI, M_PI - theta_total - AngleDisplace - 2.0*M_PI
//theta_total - AngleDisplace, M_PI - theta_total - AngleDisplace
//theta_total - AngleDisplace + 2.0*M_PI, M_PI - theta_total - AngleDisplace + 2.0*M_PI
angle_candidate.push_back(theta_total - AngleDisplace - 2.0*M_PI);
angle_candidate.push_back(theta_total - AngleDisplace);
angle_candidate.push_back(theta_total - AngleDisplace + 2.0*M_PI);
angle_candidate.push_back(M_PI - theta_total - AngleDisplace - 2.0*M_PI);
angle_candidate.push_back(M_PI - theta_total - AngleDisplace);
angle_candidate.push_back(M_PI - theta_total - AngleDisplace + 2.0*M_PI );
}
else
{
// theta_total为: asin(funcval), -M_PI+asin(funcval)
// theta_total - AngleDisplace - 2.0*M_PI, -M_PI - theta_total - AngleDisplace - 2.0*M_PI
// theta_total - AngleDisplace, -M_PI - theta_total - AngleDisplace
// theta_total - AngleDisplace + 2.0*M_PI, -M_PI - theta_total - AngleDisplace + 2.0*M_PI
angle_candidate.push_back(theta_total - AngleDisplace - 2.0*M_PI);
angle_candidate.push_back(theta_total - AngleDisplace);
angle_candidate.push_back(theta_total - AngleDisplace + 2.0*M_PI);
angle_candidate.push_back(-M_PI - theta_total - AngleDisplace - 2.0*M_PI);
angle_candidate.push_back(-M_PI - theta_total - AngleDisplace);
angle_candidate.push_back(-M_PI - theta_total - AngleDisplace + 2.0*M_PI );
}
}
}
else if(1 == Type)
{
theta_total = acos(funcval);
if(fabs(theta_total) < LoopAngleSameThresh_radian)
{
// theta_toal 为 0
// theta_toal - AngleDisplace - 2.0*M_PI
// theta_toal - AngleDisplace
// theta_toal - AngleDisplace + 2.0*M_PI
angle_candidate.push_back(theta_total - AngleDisplace - 2.0*M_PI );
angle_candidate.push_back(theta_total - AngleDisplace);
angle_candidate.push_back(theta_total - AngleDisplace + 2.0*M_PI);
}
else
{
// theta_total为: acos(funcval) , - acos(funcval)
// theta_total - AngleDisplace - M_PI*2.0, -theta_total - AngleDisplace - M_PI*2.0
// theta_total - AngleDisplace, -theta_total - AngleDisplace
// theta_total - AngleDisplace + M_PI*2.0, -theta_total - AngleDisplace + M_PI*2.0
angle_candidate.push_back(theta_total - AngleDisplace - 2.0*M_PI);
angle_candidate.push_back(theta_total - AngleDisplace);
angle_candidate.push_back(theta_total - AngleDisplace + 2.0*M_PI);
angle_candidate.push_back(-theta_total - AngleDisplace - 2.0*M_PI);
angle_candidate.push_back(-theta_total - AngleDisplace);
angle_candidate.push_back(-theta_total - AngleDisplace + 2.0*M_PI );
}
}
else if(2 == Type)
{
theta_total = atan(funcval);
// theta_total为: atan(funcval)
// theta_total - AngleDisplace - 2.0*M_PI
// theta_total - AngleDisplace - 1.0*M_PI
// theta_total - AngleDisplace
// theta_total - AngleDisplace + 1.0*M_PI
// theta_total - AngleDisplace + 2.0*M_PI
angle_candidate.push_back(theta_total - AngleDisplace - 2.0*M_PI);
angle_candidate.push_back(theta_total - AngleDisplace - 1.0*M_PI);
angle_candidate.push_back(theta_total - AngleDisplace);
angle_candidate.push_back(theta_total - AngleDisplace + 1.0*M_PI);
angle_candidate.push_back(theta_total - AngleDisplace + 2.0*M_PI);
}
else
return;
Cnt = angle_candidate.size();
for(i=0; i<Cnt; i++)
{
theta = angle_candidate[i];
//判定其是否在范围之内
Flag = IsInValidRegion(AxisIndex, theta);
if(true == Flag)
{
AngleList.push_back(theta);
}
}
std::vector<double>().swap(angle_candidate);
}
void FK_Execute(double angle_arr[], double rpy[], double trans[], double flange_data[][4], bool IsRadian = false)
{
int i, j;
double l1 = 120.058, l2 = 113.768, l3 = 360.66, l4 = 113.768, l5 = 303.315, l6 = 115.193, l7 = 113.768, l8 = 107.542;
Eigen::Matrix<double, 4, 4, Eigen::RowMajor> matrix_1, matrix_2, matrix_3, matrix_4, matrix_5, matrix_6, matrix_7, matrix_8, matrix_9, matrix_result;
Eigen::Matrix<double, 3, 3> RotateMatrix;
Eigen::Vector3d rpy_median, trans_median;
double angle_1, angle_2, angle_3, angle_4, angle_5, angle_6;
angle_1 = angle_arr[0];
angle_2 = angle_arr[1];
angle_3 = angle_arr[2];
angle_4 = angle_arr[3];
angle_5 = angle_arr[4];
angle_6 = angle_arr[5];
if(false == IsRadian)
{
angle_1 = angle_1 * M_PI / 180.0;
angle_2 = angle_2 * M_PI / 180.0;
angle_3 = angle_3 * M_PI / 180.0;
angle_4 = angle_4 * M_PI / 180.0;
angle_5 = angle_5 * M_PI / 180.0;
angle_6 = angle_6 * M_PI / 180.0;
}
matrix_1 << cos(angle_1), -sin(angle_1), 0, 0,
sin(angle_1), cos(angle_1), 0, 0,
0, 0, 1, l1,
0, 0, 0, 1;
matrix_2 << 1, 0, 0, 0,
0, 0, -1, 0,
0, 1, 0, 0,
0, 0, 0, 1;
matrix_3 << cos(angle_2), -sin(angle_2), 0, 0,
sin(angle_2), cos(angle_2), 0, 0,
0, 0, 1, 0,
0, 0, 0, 1;
matrix_4 << cos(angle_3), -sin(angle_3), 0, l3,
sin(angle_3), cos(angle_3), 0, 0,
0, 0, 1, l4 - l2,
0, 0, 0, 1;
matrix_5 << cos(angle_4), -sin(angle_4), 0, l5,
sin(angle_4), cos(angle_4), 0, 0,
0, 0, 1, -l6,
0, 0, 0, 1;
matrix_6 << 1, 0, 0, 0,
0, 0, -1, -l7,
0, 1, 0, 0,
0, 0, 0, 1;
matrix_7 << cos(angle_5), -sin(angle_5), 0, 0,
sin(angle_5), cos(angle_5), 0, 0,
0, 0, 1, 0,
0, 0, 0, 1;
matrix_8 << 1, 0, 0, 0,
0, 0, 1, l8,
0, -1, 0, 0,
0, 0, 0, 1;
matrix_9 << cos(angle_6), -sin(angle_6), 0, 0,
sin(angle_6), cos(angle_6), 0, 0,
0, 0, 1, 0,
0, 0, 0, 1;
matrix_result = matrix_1 * matrix_2 * matrix_3 * matrix_4 * matrix_5 * matrix_6 * matrix_7 * matrix_8 * matrix_9;
trans_median = matrix_result.block(0,3,3,1);
trans[0] = trans_median(0, 0);
trans[1] = trans_median(1, 0);
trans[2] = trans_median(2, 0);
RotateMatrix = matrix_result.block(0, 0, 3, 3);
// 先绕z轴，再扰y轴，最后绕x轴
// rpy_median[0] --- yaw
// rpy_median[1] --- pitch
// rpy_median[2] --- roll
rpy_median = RotateMatrix.matrix().eulerAngles(2, 1, 0);
//由于参数名称为rpy, 给人roll, pitch, yaw的顺序，所以把顺序摆正，rpy: 第一个元素：roll, 第二个元素： pitch, 第三个元素: yaw
rpy[0] = rpy_median(2, 0);
rpy[1] = rpy_median(1, 0);
rpy[2] = rpy_median(0, 0);
for(i=0; i<4; i++)
{
for(j=0; j<4; j++)
{
flange_data[i][j] = matrix_result(i, j);
}
}
}
void IK_Execute(double flange_data[][4], double angle_matrix[][6], int &SolutionCnt)
{
int TriSolutionCnt;
double matrix[4][4];
double rpy_arr[3], trans_arr[3];
// 需要验证结果的旋转部分,欧拉角有多个解，不好比较，直接比较旋转矩阵各列的夹角即可
Eigen::Vector3d n_ref, o_ref, a_ref;
Eigen::Vector3d n_curr, o_curr, a_curr;
std::vector<TriData> TriDataContainer;
double arr[2][2];
double AxisAngleArr[6];
bool IsSpecial;
double l1 = 120.058, l2 = 113.768, l3 = 360.66, l4 = 113.768, l5 = 303.315, l6 = 115.193, l7 = 113.768, l8 = 107.542;
AxisData sAxisData;
double axis1_arr[32], axis2_arr[32], axis3_arr[32], axis4_arr[32], axis5_arr[32], axis6_arr[32];
double theta1, theta2, theta3, theta4, theta5, theta6, theta;
int axis1_solution_cnt, axis2_solution_cnt, axis3_solution_cnt, axis4_solution_cnt, axis5_solution_cnt, axis6_solution_cnt;
int LocalSolutionCnt, m;
double a, b, c, val;
double c1, c2;
double a1, b1;
double a2, b2;
double cosval;
double angle_added;
double angle_total;
double sinval;
double val1, val2;
int i, j, n;
std::vector<double> AngleList;
bool Flag;
int Index;
std::vector<Data> Solution;
Data sSolution_inner;
Eigen::Matrix<double, 2, 2, Eigen::RowMajor> A;
Eigen::Matrix<double, 2, 1> b_col_2;
Eigen::Matrix<double, 2, 1> col_2;
Data sSolution;
Eigen::Vector3d rpy, trans, trans_initial, trans_deviate;
Eigen::Matrix<double, 4, 4, Eigen::RowMajor> matrix_result;
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> RotateMatrix;
Eigen::Matrix<double, 4, 4, Eigen::RowMajor> flange_set_data;
for(i=0; i<4; i++)
{
for(j=0; j<4; j++)
{
flange_set_data(i, j) = flange_data[i][j];
}
}
SolutionCnt = 0;
a = -(flange_set_data(0,3) - l8 * flange_set_data(0, 2));
b = flange_set_data(1,3) - l8 * flange_set_data(1, 2);
c = sqrt(a*a + b*b);
if(fabs(c) < ZeroThresh)
{
return;
}
// a*sin（angle) + b*cos(angle) = l6
angle_added = atan2(b, a);
// sin(angle + angle_added) = l6/c
val = (l6 + l2 - l4) / c;
std::vector<double>().swap(AngleList);
GetAngleList(0, angle_added, val, 0, AngleList);
axis1_solution_cnt = AngleList.size();
for(m=0; m<axis1_solution_cnt; m++)
{
axis1_arr[m] = AngleList[m];
}
// 轴1的转角共axis1_solution_cnt个
///////////////////////////////////////////////
// 计算轴五转角
///////////////////////////////////////////////
for(i=0; i<axis1_solution_cnt; i++)
{
theta1 = axis1_arr[i];
sSolution.angle_arr[0] = theta1;
val = flange_set_data(0,2)*sin(theta1) - flange_set_data(1,2)*cos(theta1);
std::vector<double>().swap(AngleList);
GetAngleList(4, 0, val, 1, AngleList);
axis5_solution_cnt = AngleList.size();
if(axis5_solution_cnt >= 1)
{
for(m=0; m<axis5_solution_cnt; m++)
{
theta5 = AngleList[m];
sSolution.angle_arr[4] = theta5;
Solution.push_back(sSolution);
}
}
}
///////////////////////////////////////////////
///////////////////////////////////////////////
// 计算theta2+theta3+theta4与theta6
// IsSpecial为true时表示这部分已经计算出了theta2, theta3, theta4, 后面不需要计算了，但需要计算theta6
// 已经验证过，在sin(theta5) = 0时，一般theta2, theta3, theta4有无穷多组解
IsSpecial = false;
LocalSolutionCnt = Solution.size();
for(i=0; i<LocalSolutionCnt; i++)
{
theta1 = Solution[i].angle_arr[0];
theta5 = Solution[i].angle_arr[4];
sinval = sin(theta5);
cosval = cos(theta5);
if(fabs(sinval) < ZeroThresh)//特殊情况，有可能没有解
{
std::vector<TriData>().swap(TriDataContainer);
Special(theta1, theta5, flange_set_data, TriDataContainer);
if(TriDataContainer.size() <= 0)
{
Solution[i].IsValid = false;
continue;
}
else
{
IsSpecial = true;
Solution[i].angle_arr[1] = TriDataContainer[0].angle_arr[0];
Solution[i].angle_arr[2] = TriDataContainer[0].angle_arr[1];
Solution[i].angle_arr[3] = TriDataContainer[0].angle_arr[2];
Solution[i].total = Solution[i].angle_arr[1] + Solution[i].angle_arr[2] + Solution[i].angle_arr[3];
if(1 == TriDataContainer.size())
{
continue;
}
sSolution = Solution[i];
for(m=1; m<TriDataContainer.size(); m++)
{
sSolution.angle_arr[1] = TriDataContainer[m].angle_arr[0];
sSolution.angle_arr[2] = TriDataContainer[m].angle_arr[1];
sSolution.angle_arr[3] = TriDataContainer[m].angle_arr[2];
sSolution.total = sSolution.angle_arr[1] + sSolution.angle_arr[2] + sSolution.angle_arr[3];
Solution.push_back(sSolution);
}
}
}
else//否则
{
// 可计算得到theta2+theta3+theta4
/////////////////////////////
a = -flange_set_data(2,2) / sinval;
b = -(flange_set_data(0,2)*cos(theta1) + flange_set_data(1,2)*sin(theta1)) / sinval;
// sin(theta2+theta3+theta4) = a
// cos(theta2+theta3+theta4) = b
// 需要防止sinval太小(利用atan2的性质，不用除以sinval了)
a = -flange_set_data(2,2);
b = -(flange_set_data(0,2)*cos(theta1) + flange_set_data(1,2)*sin(theta1));
if(sinval < 0)
{
a = -a;
b = -b;
}
Solution[i].total = atan2(a, b);
theta = Solution[i].total;
/////////////////////////////
//计算theta6
/////////////////////////////
// a1*sin(theta6)+b1*cos(theta6) = c1
// a2*sin(theta6) + b2 *cos(theta6) = c2
a1 = -cos(theta1) * flange_set_data(0,0)-sin(theta1)*flange_set_data(1,0);
b1 = -cos(theta1) * flange_set_data(0,1) - sin(theta1) * flange_set_data(1,1);
c1 = sin(theta);
a2 = -flange_set_data(2,0);
b2 = -flange_set_data(2, 1);
c2 = -cos(theta);
A << a1, b1, a2, b2;
b_col_2 << c1, c2;
if(fabs(A.determinant()) > ZeroThresh_determant)
{
col_2 = A.inverse() * b_col_2;
// sin(theta6) = col_2(0,0), cos(theta6) = col_2(1,0)
theta6 = atan2(col_2(0,0), col_2(1,0));
//Solution[i].angle_arr[5] = theta6;
std::vector<double>().swap(AngleList);
// 这个必然有解，至少一个
GetAngleList(5, theta6, AngleList);
sSolution_inner.Copy(Solution[i]);
Solution[i].angle_arr[5] = AngleList[0];
for(int index_inner=1; index_inner<AngleList.size(); index_inner++)
{
sSolution_inner.angle_arr[5] = AngleList[index_inner];
Solution.push_back(sSolution_inner);
}
}
else
{
val1 = a1*a1 + b1*b1;
val2 = a2*a2 + b2*b2;
if(val1 > val2)
{
a = a1;
b = b1;
c = c1;
}
else
{
a = a2;
b = b2;
c = c2;
}
//a*sin(theta6) + b*cos(theta6) = c
val = sqrt(a*a+b*b);
val = c / val;
// cos(angle_added)*sin(theta6) + sin(angle_added)*cos(theta6) = c
angle_added = atan2(b, a);
std::vector<double>().swap(AngleList);
GetAngleList(5, angle_added, val, 0, AngleList);
axis6_solution_cnt = AngleList.size();
if(axis6_solution_cnt<=0)
{
Solution[i].IsValid = false;
continue;
}
else
{
theta6 = AngleList[0];
Solution[i].angle_arr[5] = theta6;
sSolution =Solution[i];
for(m=1; m<axis6_solution_cnt; m++)
{
theta6 = AngleList[m];
sSolution.angle_arr[5] = theta6;
Solution.push_back(sSolution);
}
}
}
/////////////////////////////
}
}
///////////////////////////////////////////////
if(true == IsSpecial)
{
//计算theta6
CalcTheta6(Solution, flange_set_data);
}
else
{
// 计算theta2, theta3, theta4
LocalSolutionCnt = Solution.size();
for(i=0; i<LocalSolutionCnt; i++)
{
theta1 = Solution[i].angle_arr[0];
theta6 = Solution[i].angle_arr[5];
theta5 = Solution[i].angle_arr[4];
theta = Solution[i].total; // theta2 + theta3 + theta4
a = cos(theta1) * (flange_set_data(0, 3) - l8 * flange_set_data(0, 2)) + sin(theta1) * (flange_set_data(1, 3) - l8 * flange_set_data(1, 2)) - l7 * sin(theta);
b = flange_set_data(2, 3) - l8 * flange_set_data(2, 2) - l1 + l7 * cos(theta);
// l5*cos(theta2+theta3) + l3*cos(theta2) = a
// l5*sin(theta2+theta3) + l3*sin(theta2) = b
// l5^2 + l3^2 + 2*l3*l5*cos(theta3) = a^2 + b^2
val = (a*a + b*b - l5*l5 - l3*l3) / (2.0 * l3 * l5);
std::vector<double>().swap(AngleList);
GetAngleList(2, 0, val, 1, AngleList);
axis3_solution_cnt = AngleList.size();
for(m=0; m<axis3_solution_cnt; m++)
{
axis3_arr[m] = AngleList[m];
}
if(axis3_solution_cnt <= 0)
{
Solution[i].IsValid = false;
continue;
}
Flag = false;
for(m=0; m<axis3_solution_cnt; m++)
{
theta3 = axis3_arr[m];
// theta3 introduce into 5*cos(theta2+theta3) + l3*cos(theta2) = a
a1 = l5*cos(theta3)+l3;
b1 = -l5*sin(theta3);
// a1*cos(theta2) + b1*sin(theta2) = a
c = sqrt(a1*a1 + b1*b1);
if(fabs(c) > ZeroThresh)
{
// a1/a*cos(theta2) + b1/a*sin(theta2) = 1
angle_added = atan2(a1, b1);
// sin(theta2 + angle_added) = a/c
val = a/c;
std::vector<double>().swap(AngleList);
GetAngleList(1, angle_added, val, 0, AngleList);
TriSolutionCnt = AngleList.size();
if(TriSolutionCnt<=0)
{
continue;
}
for(n=0; n<TriSolutionCnt; n++)
{
theta2 = AngleList[n];
theta4 = Solution[i].total - theta3 - theta2;
// total那块不是光atan2那个数值，由于是3个角度之和，所以需要延扩，对theta3, theta2没影响，仅对theta4有影响，所以这里演扩也是一样的
axis4_arr[0] = theta4;
axis4_arr[1] = theta4 - 2.0*M_PI;
axis4_arr[2] = theta4 + 2.0*M_PI;
for(int index_ = 0; index_ <3; index_++)
{
theta4 = axis4_arr[index_];
if(true == IsInValidRegion(3, theta4))
{
if(Flag = false)
{
Solution[i].angle_arr[1] = theta2;
Solution[i].angle_arr[2] = theta3;
Solution[i].angle_arr[3] = theta4;
Flag = true;
}
else
{
sSolution = Solution[i];
sSolution.angle_arr[1] = theta2;
sSolution.angle_arr[2] = theta3;
sSolution.angle_arr[3] = theta4;
Solution.push_back(sSolution);
}
}
}
}
}
}
if(false == Flag)
{
Solution[i].IsValid = false;
continue;
}
}
}
//
trans_initial = flange_set_data.block(0,3,3,1);
n_ref = flange_set_data.block(0, 0, 3, 1);
o_ref = flange_set_data.block(0, 1, 3, 1);
a_ref = flange_set_data.block(0, 2, 3, 1);
LocalSolutionCnt = Solution.size();
for(i=0; i<LocalSolutionCnt; i++)
{
for(j=0; j<6; j++)
{
AxisAngleArr[j] = Solution[i].angle_arr[j];
}
FK_Execute(AxisAngleArr, rpy_arr, trans_arr, matrix, true);
for(j=0; j<3; j++)
{
rpy(j, 0) = rpy_arr[j];
trans(j, 0) = trans_arr[j];
}
for(int m=0; m<4; m++)
{
for(int n=0; n<4; n++)
{
matrix_result(m, n) = matrix[m][n];
}
}
n_curr = matrix_result.block(0, 0, 3, 1);
o_curr = matrix_result.block(0, 1, 3, 1);
a_curr = matrix_result.block(0, 2, 3, 1);
a = n_ref.dot(n_curr) / (n_ref.norm() * n_curr.norm());
if(a >= 1.0)
a = 1.0;
else if(a <= -1.0)
a = -1.0;
a = acos(a) * 180.0 / M_PI;
b = o_ref.dot(o_curr) / (o_ref.norm() * o_curr.norm());
if(b >= 1.0)
b = 1.0;
else if(b <= -1.0)
b = -1.0;
b = acos(b) * 180.0 / M_PI;
c = a_ref.dot(a_curr) / (a_ref.norm() * a_curr.norm());
if(c >= 1.0)
c = 1.0;
else if(c <= -1.0)
c = -1.0;
c = acos(c) * 180.0 / M_PI;
if((fabs(a) < LoopAngleSameThresh && fabs(b) < LoopAngleSameThresh && fabs(c) < LoopAngleSameThresh))
{
Flag = true;
}
else
{
Flag = false;
}
if(true == Flag)
{
trans_deviate = trans - trans_initial;
if(trans_deviate.norm() < LengthSameDeviateThresh)
{
//合格
Flag = true;
}
else
{
Flag = false;
//不合格
}
}
if(false == Flag)
{
//不合格
Solution[i].IsValid = false;
}
if(true == Solution[i].IsValid)
{
for(j=0; j<6; j++)
{
angle_matrix[SolutionCnt][j] = Solution[i].angle_arr[j];
}
SolutionCnt++;
}
}
std::vector<Data>().swap(Solution); // 过程变量清空内存
std::vector<TriData>().swap(TriDataContainer);
std::vector<double>().swap(AngleList);
}
讨论
经过实验验证，设计代码可稳定可靠实现JAKA ZU7正逆向运动学求解过程。对于机器人可达情况，根据实际情况逆向解可获取8组、16组，甚至更多。在实际使用中，可根据控制算法需要选择特定的解，可有效提高商用臂控制的灵活性。此外，JAKA ZU机械臂构型与UR机械臂等常用臂高度同源，文中求解策略对其它臂运动学求解过程同样具有可参考性。
与文中采用解析求解的策略不同，目前也存在可计算机械臂运动学问题的开源库，例如KDL。经过测试，KDL难以获取所有逆向解，存在实际可达但反馈无有效逆向解情况的发生。
参考文献
[1] R. M. Murray, Z. Li, S. S. Sastry. A mathematical introduction to robotic manipulation [M]. CRC Press, 1994.
[2] 张连东. 基于微分几何学的机器人操作性能的研究 [D]; 大连理工大学, 2004.
[3]谭民, 徐德, 侯增广, 等. 先进机器人控制 [M]. 高等教育出版社, 2007.
[4]蔡自兴. 机器人学 [M]. 清华大学出版社, 2000.
[5]Niku S B. Introduction to Robotics: Analysis, System, Applications [M]. N.J.: Prentice-Hall, 2001.
[6]金媛媛. 机器人逆运动学的模拟退火自适应遗传算法研究 [D]; 重庆大学, 2007.
[7] 臧庆凯, 李春贵, 钟宛余. 基于三个并行BP神经网络的机器人逆运动学求解 [J]. 计算机测量与控制, 2012, 20(08): 2244-2247.
[8]E. Sariyildiz, H. Temeltas. Solution of inverse kinematic problem for serial robot using quaterninons [C]. In Proceedings of the International Conference on Mechatronics and Automation (ICMA), 2009, pp. 26-31.
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
