我的毕设题目定为《基于机械臂触觉伺服的物体操控研究》，这个系列主要用于记录做毕设的过程。
前言：UR系列是优傲公司的代表产品，也是目前比较通用的产品级机械臂。所以我打算用该机械臂进行毕设的仿真实现。关于其运动学建模，网上有很多参考的博客及文献，但是很多都与《机器人学导论》第四版中的建模方式有所出入（不是指建模有问题，只是坐标系定义不太一致）。所以我按照自己的理解进行了运动学模型搭建，并使用webots进行了仿真验证。
1. 正运动学
模型的坐标系建立如下图所示。
DH参数表
注：列表的长度参考了webots中的模型，可能跟实体的参数有所出入。
| a | α \alpha α | d | θ \theta θ |
|---|---|---|---|
| 0 | 0 | 0.1625 | θ 1 \theta_1 θ1 |
| 0 | − π / 2 -\pi/2 −π/2 | 0 | − π / 2 + θ 2 -\pi/2+\theta_2 −π/2+θ2 |
| 0.425 | 0 | 0 | θ 3 \theta_3 θ3 |
| 0.3922 | 0 | 0.127 | π / 2 + θ 4 \pi/2+\theta_4 π/2+θ4 |
| 0 | π / 2 \pi/2 π/2 | 0.1 | θ 5 \theta_5 θ5 |
| 0 | − π / 2 -\pi/2 −π/2 | 0.1 | θ 6 \theta_6 θ6 |
求解
将DH参数列表转为变换矩阵
T
a
=
[
1
0
0
a
0
1
0
0
0
0
1
0
0
0
0
1
]
T
α
=
[
1
0
0
0
0
c
o
s
(
α
)
−
s
i
n
(
α
)
0
0
s
i
n
(
α
)
c
o
s
(
α
)
0
0
0
0
1
]
T
d
=
[
1
0
0
0
0
1
0
0
0
0
1
d
0
0
0
1
]
T
θ
=
[
c
o
s
(
θ
)
−
s
i
n
(
θ
)
0
0
s
i
n
(
θ
)
c
o
s
(
θ
)
0
0
0
0
1
0
0
0
0
1
]
T_a = \begin{bmatrix} 1& 0& 0& a\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1 \end{bmatrix} \space T_\alpha = \begin{bmatrix} 1& 0& 0& 0\\ 0& cos(\alpha)& -sin(\alpha)& 0\\ 0& sin(\alpha)& cos(\alpha)& 0\\ 0& 0& 0& 1 \end{bmatrix} \\ ~\\ T_d = \begin{bmatrix} 1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& d\\ 0& 0& 0& 1 \end{bmatrix} \space T_\theta = \begin{bmatrix} cos(\theta)& -sin(\theta)& 0&0\\ sin(\theta)& cos(\theta)& 0&0\\ 0& 0& 1& 0\\ 0& 0& 0& 1 \end{bmatrix} \\
Ta=
100001000010a001
Tα=
10000cos(α)sin(α)00−sin(α)cos(α)00001
Td=
10000100001000d1
Tθ=
cos(θ)sin(θ)00−sin(θ)cos(θ)0000100001
坐标系变换
i
+
1
i
T
=
i
T
a
∗
i
T
α
∗
i
T
d
∗
i
T
θ
6
0
T
=
1
0
T
∗
2
1
T
∗
3
2
T
∗
4
3
T
∗
5
4
T
∗
6
5
T
_{i+1}^{i}T = \\^{i}T_a * \\^{i}T_\alpha * \\^{i}T_d * \\^{i}T_\theta \\ ~\\ \\_{6}^{0}T = \\_{1}^{0}T * \\_{2}^{1}T * \\_{3}^{2}T * \\_{4}^{3}T *\\_{5}^{4}T *\\_{6}^{5}T
i+1iT=iTa∗iTα∗iTd∗iTθ 60T=10T∗21T∗32T∗43T∗54T∗65T
代码实现
#include <vector>
#include <eigen3/Eigen/Dense>
std::vector<Vec4<T>> getDHparams(void){
/* define link length */
_worldBaseDistance = 0.1625;
_shoulderLinkLength = 0.138;
_upperarmLinkLength = 0.425;
_forearmLinkLength = 0.3922;
_wrist1LinkLength = 0.127;
_wrist2LinkLength = 0.1;
_wrist3LinkLength = 0.1;
Vec4<T> dh_param;
std::vector<Vec4<T>> DH_params;
DH_params.clear();
dh_param << 0,0, _worldBaseDistance, 0; DH_params.push_back(dh_param);
dh_param << 0, -M_PI_2, 0, -M_PI_2; DH_params.push_back(dh_param);
dh_param << _upperarmLinkLength, 0, 0, 0; DH_params.push_back(dh_param);
dh_param << _forearmLinkLength, 0, _wrist1LinkLength, M_PI_2; DH_params.push_back(dh_param);
dh_param << 0, M_PI_2, _wrist2LinkLength, 0; DH_params.push_back(dh_param);
dh_param << 0, -M_PI_2, _wrist3LinkLength, 0; DH_params.push_back(dh_param);
}
/**
* @brief forward kinematics
* @param q joints angle 1 - 6
* @param p data order is x, y, z, pitch, roll, yaw
*/
template <typename T>
void forwardKinematics(const Vec6<T>& q, Vec6<T>* p){
std::vector<Vec4<T>> DH_params = getDHparams();
Mat4<T> Pro_Mat[4];
Mat4<T> Trans_Mat = Mat4<T>::Identity();
for(int i = 0; i < JOINTS_NUM; i ++){
/* init */
for(int j = 0; j < 4; j ++){
Pro_Mat[j] = Mat4<T>::Identity();
}
/* create transform matrix refer to DH params */
Mat2<T> rot;
/* a trans */
T a = DH_params[i][0];
Pro_Mat[0](0, 3) = a;
/* alpha trans */
T alpha = DH_params[i][1];
rot << cos(alpha), -sin(alpha), sin(alpha), cos(alpha);
Pro_Mat[1].block(1, 1, 2, 2) = rot;
/* d trans */
T d = DH_params[i][2];
Pro_Mat[2](2, 3) = d;
/* theta trans */
T theta = q[i] + DH_params[i][3];
rot << cos(theta), -sin(theta), sin(theta), cos(theta);
Pro_Mat[3].block(0, 0, 2, 2) = rot;
/* combine trans matrix */
Trans_Mat = Trans_Mat * Pro_Mat[0] * Pro_Mat[1] * Pro_Mat[2] * Pro_Mat[3];
}
/* xyz */
p->operator()(0) = Trans_Mat(0, 3);
p->operator()(1) = Trans_Mat(1, 3);
p->operator()(2) = Trans_Mat(2, 3);
/* rpy */
Mat3<T> rot = Trans_Mat.block(0,0,3,3);
Vec3<T> rpy = ori::rotationMatrixToRPY(rot);
p->operator()(3) = rpy(0);
p->operator()(4) = rpy(1);
p->operator()(5) = rpy(2);
}
2. 逆运动学
因为UR5e满足Pieper准则（末端三个关节轴相交于一点），所以其逆运动学具有封闭解。这里采用解析法进行求解。求解过程参考如下的文章，因为建模方式不同，求解的公式也有一定的区别，但思路一致。
UR机械臂正逆运动学求解
角度求解用到的公式(推导过程参考上述文章)
已知：
c
o
s
(
θ
)
p
y
−
s
i
n
(
θ
)
p
x
=
d
cos(\theta)p_y-sin(\theta)p_x = d
cos(θ)py−sin(θ)px=d
可求得：
θ
=
A
t
a
n
2
(
p
y
,
p
x
)
−
A
t
a
n
2
(
d
,
±
p
x
2
+
p
y
2
−
d
2
)
(
p
x
2
+
p
y
2
−
d
2
≥
0
)
\theta = Atan2(p_y, p_x) - Atan2(d, \pm \sqrt{p_x^2 + p_y^2 - d^2}) (p_x^2 + p_y^2 - d^2 \ge 0)
θ=Atan2(py,px)−Atan2(d,±px2+py2−d2)(px2+py2−d2≥0)
参数定义
T
=
6
0
T
=
[
r
x
o
x
l
x
p
x
r
y
o
y
l
y
p
y
r
z
o
z
l
z
p
z
0
0
0
1
]
T = \\_{6}^{0}T = \begin{bmatrix} r_x& o_x& l_x& p_x\\ r_y& o_y& l_y& p_y\\ r_z& o_z& l_z& p_z\\ 0& 0& 0& 1 \end{bmatrix} \\
T=60T=
rxryrz0oxoyoz0lxlylz0pxpypz1
以下的推导公式中，
c
1
c_1
c1表示
c
o
s
(
θ
1
)
cos(\theta_1)
cos(θ1)，
s
1
2
s_12
s12表示
s
i
n
(
θ
1
+
θ
2
)
sin(\theta_1+\theta_2)
sin(θ1+θ2)
求解 θ 1 \theta_1 θ1, θ 5 \theta_5 θ5, θ 6 \theta_6 θ6
根据变换矩阵公式可得以下关系
1
0
T
−
1
∗
T
∗
6
5
T
−
1
=
5
1
T
\\_{1}^{0}T^{-1}*T*\\_{6}^{5}T^{-1} = \\_{5}^{1}T
10T−1∗T∗65T−1=51T
等式左边
1
0
T
−
1
∗
T
∗
6
5
T
−
1
=
[
r
x
c
1
c
6
−
o
x
c
1
s
6
+
r
y
c
6
s
1
−
o
y
s
1
s
6
l
x
c
1
+
l
y
s
1
−
o
x
c
1
c
6
−
o
y
c
6
s
1
−
r
x
c
1
s
6
−
r
y
s
1
s
6
p
x
c
1
+
p
y
s
1
−
d
6
l
x
c
1
−
d
6
l
y
s
1
r
y
c
1
c
6
−
o
y
c
1
s
6
−
r
x
c
6
s
1
+
o
x
s
1
s
6
l
y
c
1
−
l
x
s
1
o
x
c
1
s
6
−
o
y
c
1
c
6
−
r
y
c
1
s
6
+
r
x
s
1
s
6
p
y
c
1
−
p
x
s
1
−
d
6
l
y
c
1
+
d
6
l
x
s
1
r
z
c
6
−
o
z
s
6
l
z
−
o
z
c
6
−
r
z
s
6
p
z
−
d
1
−
d
6
l
z
0
0
0
1
]
\\_{1}^{0}T^{-1}*T*\\_{6}^{5}T^{-1} = \\ \begin{bmatrix} r_xc_1c_6 - o_xc_1s_6 + r_yc_6s_1 - o_ys_1s_6& l_xc_1 + l_ys_1& - o_xc_1c_6 - o_yc_6s_1 - r_xc_1s_6 - r_ys_1s_6& p_xc_1 + p_ys_1 - d_6l_xc_1 - d_6l_ys_1\\ r_yc_1c_6 - o_yc_1s_6 - r_xc_6s_1 + o_xs_1s_6& l_yc_1 - l_xs_1& o_xc_1s_6 - o_yc_1c_6 - r_yc_1s_6 + r_xs_1s_6& p_yc_1 - p_xs_1 - d_6l_yc_1 + d_6l_xs_1\\ r_zc_6 - o_zs_6& l_z& - o_zc_6 - r_zs_6& p_z - d_1 - d_6l_z\\ 0& 0& 0& 1 \end{bmatrix} \\
10T−1∗T∗65T−1=
rxc1c6−oxc1s6+ryc6s1−oys1s6ryc1c6−oyc1s6−rxc6s1+oxs1s6rzc6−ozs60lxc1+lys1lyc1−lxs1lz0−oxc1c6−oyc6s1−rxc1s6−rys1s6oxc1s6−oyc1c6−ryc1s6+rxs1s6−ozc6−rzs60pxc1+pys1−d6lxc1−d6lys1pyc1−pxs1−d6lyc1+d6lxs1pz−d1−d6lz1
等式右边
5
1
T
=
[
c
234
c
5
−
c
234
s
5
s
234
a
4
s
23
+
a
3
s
2
+
d
5
s
234
s
5
c
5
0
d
4
−
s
234
c
5
s
234
s
5
c
234
a
4
c
23
+
a
3
c
2
+
d
5
c
234
0
0
0
1
]
\\_{5}^{1}T = \begin{bmatrix} c_{234}c_5& -c_{234}s_5& s_{234}& a_4s_{23}+ a_3s_2 + d_5s_{234}\\ s_5& c_5& 0& d_4\\ -s_{234}c_5& s_{234}s_5& c_{234}& a_4c_{23}+ a_3c_2 + d_5c_{234}\\ 0& 0& 0& 1 \end{bmatrix} \\
51T=
c234c5s5−s234c50−c234s5c5s234s50s2340c2340a4s23+a3s2+d5s234d4a4c23+a3c2+d5c2341
求解 θ 1 \theta_1 θ1
利用等式左右两边第二行，第四列对应相等进行求解。
p
y
c
1
−
p
x
s
1
−
d
6
l
y
c
1
+
d
6
l
x
s
1
=
d
4
p_yc_1 - p_xs_1 - d_6l_yc_1 + d_6l_xs_1 = d_4
pyc1−pxs1−d6lyc1+d6lxs1=d4
整理得
（
p
y
−
d
6
l
y
)
c
1
−
(
p
x
−
d
6
l
x
)
s
1
=
d
4
（p_y - d_6l_y)c_1 - (p_x - d_6l_x) s_1 = d_4
（py−d6ly)c1−(px−d6lx)s1=d4
令
m
=
p
y
−
d
6
l
y
n
=
p
x
−
d
6
l
x
m= p_y - d_6l_y \\ n= p_x - d_6l_x
m=py−d6lyn=px−d6lx
则
θ
1
=
A
t
a
n
2
(
m
,
n
)
−
A
t
a
n
2
(
d
4
,
±
m
2
+
n
2
−
d
4
2
)
\theta_1 = Atan2(m, n) - Atan2(d_4, \pm \sqrt{m^2 + n^2 - d_4^2})
θ1=Atan2(m,n)−Atan2(d4,±m2+n2−d42)
求解 θ 5 \theta_5 θ5
利用等式左右两边第二行，第二列对应相等进行求解。
l
y
c
1
−
l
x
s
1
=
c
5
θ
5
=
±
A
c
o
s
(
l
y
c
1
−
l
x
s
1
)
l_yc_1 - l_xs_1 = c_5 \\ ~\\ \theta_5 = \pm Acos(l_yc_1 - l_xs_1)
lyc1−lxs1=c5 θ5=±Acos(lyc1−lxs1)
求解 θ 6 \theta_6 θ6
利用等式左右两边第二行，第一列对应相等进行求解。
r
y
c
1
c
6
−
o
y
c
1
s
6
−
r
x
c
6
s
1
+
o
x
s
1
s
6
=
s
5
r_yc_1c_6 - o_yc_1s_6 - r_xc_6s_1 + o_xs_1s_6 = s_5 \\
ryc1c6−oyc1s6−rxc6s1+oxs1s6=s5
整理得
（
r
y
c
1
−
r
x
s
1
)
c
6
−
(
o
y
c
1
−
o
x
s
1
)
s
6
=
s
5
（r_yc_1 - r_xs_1)c_6 - (o_yc_1 - o_xs_1) s_6 = s_5
（ryc1−rxs1)c6−(oyc1−oxs1)s6=s5
令
m
=
r
y
c
1
−
r
x
s
1
n
=
o
y
c
1
−
o
x
s
1
m= r_yc_1 - r_xs_1 \\ n= o_yc_1 - o_xs_1
m=ryc1−rxs1n=oyc1−oxs1
则
θ
6
=
A
t
a
n
2
(
m
,
n
)
−
A
t
a
n
2
(
s
5
,
±
m
2
+
n
2
−
s
5
2
)
\theta_6 = Atan2(m, n) - Atan2(s_5, \pm \sqrt{m^2 + n^2 - s_5^2})
θ6=Atan2(m,n)−Atan2(s5,±m2+n2−s52)
由于可以推导得到（这条公式我没有推过，而是在代码中验证所得，读者可以自行推导）
m
2
+
n
2
−
s
5
2
=
0
m^2 + n^2 - s_5^2 = 0
m2+n2−s52=0
则
θ
6
=
A
t
a
n
2
(
m
,
n
)
−
A
t
a
n
2
(
s
5
,
0
)
\theta_6 = Atan2(m, n) - Atan2(s_5, 0)
θ6=Atan2(m,n)−Atan2(s5,0)
求解 θ 2 \theta_2 θ2, θ 3 \theta_3 θ3, θ 4 \theta_4 θ4
根据变换矩阵公式可得以下关系
1
0
T
−
1
∗
T
∗
6
5
T
−
1
∗
5
4
T
−
1
=
4
1
T
\\_{1}^{0}T^{-1}*T*\\_{6}^{5}T^{-1}*\\_{5}^{4}T^{-1} = \\_{4}^{1}T
10T−1∗T∗65T−1∗54T−1=41T
等式左边
1
0
T
−
1
∗
T
∗
6
5
T
−
1
∗
5
4
T
−
1
=
[
r
x
c
1
c
5
c
6
−
l
y
s
1
s
5
−
l
x
c
1
s
5
−
o
x
c
1
c
5
s
6
+
r
y
c
5
c
6
s
1
−
o
y
c
5
s
1
s
6
o
x
c
1
c
6
+
o
y
c
6
s
1
+
r
x
c
1
s
6
+
r
y
s
1
s
6
l
x
c
1
c
5
+
l
y
c
5
s
1
+
r
x
c
1
c
6
s
5
−
o
x
c
1
s
5
s
6
+
r
y
c
6
s
1
s
5
−
o
y
1
1
s
5
s
6
p
x
c
1
+
p
y
s
1
−
d
6
l
x
c
1
−
d
6
l
y
s
1
+
d
5
r
y
s
1
s
6
+
d
5
o
x
c
1
c
6
+
d
5
o
y
c
6
s
1
+
d
5
r
x
c
1
s
6
r
y
c
1
c
5
c
6
+
l
x
s
1
s
5
−
l
y
c
1
s
5
−
o
y
c
1
c
5
s
6
−
r
x
c
5
c
6
s
1
+
o
x
c
5
s
1
s
6
o
y
c
1
c
6
−
o
x
c
6
s
1
+
r
y
c
1
s
6
−
r
x
s
1
s
6
l
y
c
1
c
5
−
l
x
c
5
s
1
+
r
y
c
1
c
6
s
5
−
o
y
c
1
s
5
s
6
−
r
x
c
6
s
1
s
5
+
o
x
1
1
s
5
s
6
p
y
c
1
−
p
x
s
1
−
d
6
l
y
c
1
+
d
6
l
x
s
1
−
d
5
r
x
s
1
s
6
+
d
5
o
y
c
1
c
6
−
d
5
o
x
c
6
s
1
+
d
5
r
y
c
1
s
6
r
z
c
5
c
6
−
l
z
s
5
−
o
z
c
5
s
6
o
z
c
6
+
r
z
s
6
l
z
c
5
+
r
z
c
6
s
5
−
o
z
s
5
s
6
p
z
−
d
1
−
d
6
l
z
+
d
5
o
z
c
6
+
d
5
r
z
s
6
0
0
0
1
]
\\_{1}^{0}T^{-1}*T*\\_{6}^{5}T^{-1}*\\_{5}^{4}T^{-1} = \\ \begin{bmatrix} r_xc_1c_5c_6 - l_ys_1s_5 - l_xc_1s_5 - o_xc_1c_5s_6 + r_yc_5c_6s_1 - o_yc_5s_1s_6& o_xc_1c_6 + o_yc_6s_1 + r_xc_1s_6 + r_ys_1s_6& l_xc_1c_5 + l_yc_5s_1 + r_xc_1c_6s_5 - o_xc_1s_5s_6 + r_yc_6s_1s_5 - o_y1_1s_5s_6& p_xc_1 + p_ys_1 - d_6l_xc_1 - d_6l_ys_1 + d_5r_ys_1s_6 + d_5o_xc_1c_6 + d_5o_yc_6s_1 + d_5r_xc_1s_6\\ r_yc_1c_5c_6 + l_xs_1s_5 - l_yc_1s_5 - o_yc_1c_5s_6 - r_xc_5c_6s_1 + o_xc_5s_1s_6& o_yc_1c_6 - o_xc_6s_1 + r_yc_1s_6 - r_xs_1s_6& l_yc_1c_5 - l_xc_5s_1 + r_yc_1c_6s_5 - o_yc_1s_5s_6 - r_xc_6s_1s_5 + o_x1_1s_5s_6&p_yc_1 - p_xs_1 - d_6l_yc_1 + d_6l_xs_1 - d_5r_xs_1s_6 + d_5o_yc_1c_6 - d_5o_xc_6s_1 + d_5r_yc_1s_6\\ r_zc_5c_6 - l_zs_5 - o_zc_5s_6& o_zc_6 + r_zs_6& l_zc_5 + r_zc_6s_5 - o_zs_5s_6& p_z - d_1 - d_6l_z + d_5o_zc_6 + d_5r_zs_6\\ 0& 0& 0& 1 \end{bmatrix} \\
10T−1∗T∗65T−1∗54T−1=
rxc1c5c6−lys1s5−lxc1s5−oxc1c5s6+ryc5c6s1−oyc5s1s6ryc1c5c6+lxs1s5−lyc1s5−oyc1c5s6−rxc5c6s1+oxc5s1s6rzc5c6−lzs5−ozc5s60oxc1c6+oyc6s1+rxc1s6+rys1s6oyc1c6−oxc6s1+ryc1s6−rxs1s6ozc6+rzs60lxc1c5+lyc5s1+rxc1c6s5−oxc1s5s6+ryc6s1s5−oy11s5s6lyc1c5−lxc5s1+ryc1c6s5−oyc1s5s6−rxc6s1s5+ox11s5s6lzc5+rzc6s5−ozs5s60pxc1+pys1−d6lxc1−d6lys1+d5rys1s6+d5oxc1c6+d5oyc6s1+d5rxc1s6pyc1−pxs1−d6lyc1+d6lxs1−d5rxs1s6+d5oyc1c6−d5oxc6s1+d5ryc1s6pz−d1−d6lz+d5ozc6+d5rzs61
等式右边
4
1
T
=
[
c
234
−
s
234
0
a
4
s
23
+
a
3
s
2
0
0
1
d
4
−
s
234
−
c
234
0
a
4
c
23
+
a
3
c
2
0
0
0
1
]
\\_{4}^{1}T = \begin{bmatrix} c_{234}& -s_{234}& 0& a_4s_{23}+ a_3s_2\\ 0& 0& 1& d_4\\ -s_{234}& -c_{234}& 0& a_4c_{23}+ a_3c_2\\ 0& 0& 0& 1 \end{bmatrix} \\
41T=
c2340−s2340−s2340−c23400100a4s23+a3s2d4a4c23+a3c21
求解 θ 3 \theta_3 θ3
利用等式左右两边第一行，第四列，以及第三行，第四列对应相等进行求解。
a
4
s
23
+
a
3
s
2
=
p
x
c
1
+
p
y
s
1
−
d
6
l
x
c
1
−
d
6
l
y
s
1
+
d
5
r
y
s
1
s
6
+
d
5
o
x
c
1
c
6
+
d
5
o
y
c
6
s
1
+
d
5
r
x
c
1
s
6
a
4
c
23
+
a
3
c
2
=
p
z
−
d
1
−
d
6
l
z
+
d
5
o
z
c
6
+
d
5
r
z
s
6
a_4s_{23}+ a_3s_2 = p_xc_1 + p_ys_1 - d_6l_xc_1 - d_6l_ys_1 + d_5r_ys_1s_6 + d_5o_xc_1c_6 + d_5o_yc_6s_1 + d_5r_xc_1s_6 \\~\\ a_4c_{23}+ a_3c_2 = p_z - d_1 - d_6l_z + d_5o_zc_6 + d_5r_zs_6
a4s23+a3s2=pxc1+pys1−d6lxc1−d6lys1+d5rys1s6+d5oxc1c6+d5oyc6s1+d5rxc1s6 a4c23+a3c2=pz−d1−d6lz+d5ozc6+d5rzs6
令
p
x
c
1
+
p
y
s
1
−
d
6
l
x
c
1
−
d
6
l
y
s
1
+
d
5
r
y
s
1
s
6
+
d
5
o
x
c
1
c
6
+
d
5
o
y
c
6
s
1
+
d
5
r
x
c
1
s
6
=
m
p
z
−
d
1
−
d
6
l
z
+
d
5
o
z
c
6
+
d
5
r
z
s
6
=
n
p_xc_1 + p_ys_1 - d_6l_xc_1 - d_6l_ys_1 + d_5r_ys_1s_6 + d_5o_xc_1c_6 + d_5o_yc_6s_1 + d_5r_xc_1s_6 = m\\ p_z - d_1 - d_6l_z + d_5o_zc_6 + d_5r_zs_6 = n
pxc1+pys1−d6lxc1−d6lys1+d5rys1s6+d5oxc1c6+d5oyc6s1+d5rxc1s6=mpz−d1−d6lz+d5ozc6+d5rzs6=n
则
a
4
s
23
+
a
3
s
2
=
m
−
−
−
(
1
)
a
4
c
23
+
a
3
c
2
=
n
−
−
−
(
2
)
a_4s_{23}+ a_3s_2 =m ---(1)\\ a_4c_{23}+ a_3c_2 = n ---(2)
a4s23+a3s2=m−−−(1)a4c23+a3c2=n−−−(2)
对（1）（2）式求平方和
a
3
2
+
a
4
2
+
2
a
3
a
4
(
s
23
s
2
+
c
23
c
2
)
=
m
2
+
n
2
a_3^2 + a_4^2 + 2a_3a_4(s_{23}s_2 + c_{23}c_2) = m^2 + n^2
a32+a42+2a3a4(s23s2+c23c2)=m2+n2
因为
s
23
s
2
+
c
23
c
2
=
c
3
s_{23}s_2 + c_{23}c_2 = c_3
s23s2+c23c2=c3
则
θ
3
=
±
A
c
o
s
(
m
2
+
n
2
−
a
3
2
−
a
4
2
2
a
3
a
4
)
\theta_3 = \pm Acos(\frac{m^2 + n^2 - a_3^2 - a_4^2}{2a_3a_4})
θ3=±Acos(2a3a4m2+n2−a32−a42)
求解 θ 2 \theta_2 θ2
将（1）（2）式进行展开
a
4
(
s
2
c
3
+
s
3
c
2
)
+
a
3
s
2
=
m
a
4
(
c
2
c
3
−
s
2
s
3
)
+
a
3
c
2
=
n
a_4(s_2c_3+s_3c_2)+ a_3s_2 =m\\ a_4(c_2c_3-s_2s_3)+ a_3c_2 = n
a4(s2c3+s3c2)+a3s2=ma4(c2c3−s2s3)+a3c2=n
整理得
(
a
4
c
3
+
a
3
)
s
2
+
(
a
4
s
3
)
c
2
=
m
(
a
4
c
3
+
a
3
)
c
2
−
(
a
4
s
3
)
s
2
=
n
(a_4c_3+a_3)s_2+(a_4s_3)c_2 =m\\ (a_4c_3+a_3)c_2-(a_4s_3)s_2 = n
(a4c3+a3)s2+(a4s3)c2=m(a4c3+a3)c2−(a4s3)s2=n
解得
s
2
=
m
(
a
4
c
3
+
a
3
)
−
a
4
s
3
n
a
4
2
+
2
a
4
a
3
c
3
+
a
3
2
c
2
=
n
+
(
a
4
s
3
)
s
2
a
4
c
3
+
a
3
θ
2
=
A
t
a
n
2
(
s
2
,
c
2
)
s_2 = \frac{m(a_4c_3+a_3) - a_4s_3n}{a_4^2+2a_4a_3c_3+a_3^2}\\~\\ c_2 = \frac{n + (a_4s_3)s_2}{a_4c_3+a_3}\\~\\ \theta_2 = Atan2(s_2, c_2)
s2=a42+2a4a3c3+a32m(a4c3+a3)−a4s3n c2=a4c3+a3n+(a4s3)s2 θ2=Atan2(s2,c2)
求解 θ 4 \theta_4 θ4
利用等式左右两边第三行，第一列，以及第三行，第二列对应相等进行求解。
s
234
=
−
(
r
z
c
5
c
6
−
l
z
s
5
−
o
z
c
5
s
6
)
c
234
=
−
(
o
z
c
6
+
r
z
s
6
)
s_{234} = -(r_zc_5c_6 - l_zs_5 - o_zc_5s_6) \\ c_{234} = -(o_zc_6 + r_zs_6)\\
s234=−(rzc5c6−lzs5−ozc5s6)c234=−(ozc6+rzs6)
则
θ
4
=
A
t
a
n
2
(
s
234
,
c
234
)
−
θ
2
−
θ
3
\theta_4 = Atan2(s_{234}, c_{234}) - \theta_2 - \theta_3
θ4=Atan2(s234,c234)−θ2−θ3
奇异点
- θ 5 = 0 \theta_5 = 0 θ5=0时， θ 2 ， θ 3 ， θ 4 与 θ 6 \theta_2，\theta_3，\theta_4与\theta_6 θ2，θ3，θ4与θ6平行，此时需要自定义 θ 6 \theta_6 θ6的值使系统可解。
- θ 3 = π \theta_3 = \pi θ3=π时， θ 2 \theta_2 θ2可以为任何值。
对于解析解而言，只要找出求解公式中超出函数定义域或者值域的取值，并将其重新定义为有效值即可避免到达奇异位点。当然，前提是保证目标点位于机械臂的工作空间内，
结果筛选
因为上述推导公式可以得到8组解，所以需要筛选出最合适的那个解进行输出。筛选的方法要根据情景而定，一般是选择与当前位姿最接近的解或者与最优位姿最接近的解。
下面给出一种筛选思路。
参数定义
X
i
：
X_i：
Xi：逆运动学求取的一组解
r
e
f
ref
ref：参考点
ω
：
\bm{\omega}：
ω：权重系数
d
(
x
,
y
)
：
d(x,y)：
d(x,y)：x，y两点的距离函数
对每组解分别求取误差
e
r
r
i
=
ω
∗
d
(
X
i
,
r
e
f
)
err_i = \bm{\omega} * d(X_i, ref)
erri=ω∗d(Xi,ref)
选取最优解
X
o
p
t
i
m
a
l
=
X
j
e
r
r
j
=
m
i
n
(
e
r
r
)
X_{optimal}= X_j\\ err_j = min(err)
Xoptimal=Xjerrj=min(err)
- 权重系数可以设置为各个驱动关节对应的连杆长度，这样子越长的连杆权重越大，最终得到的结果会趋向于移动短连杆，节省能量。
- 距离函数可以选择为绝对值距离或者欧氏距离等。
代码实现
/**
* @brief inverse kinematics
* @param p data order is x, y, z, pitch, roll, yaw
* @param q joints angle 1 - 6
*/
template <typename T>
bool inverseKinematics(const Vec6<T>& p, Vec6<T>* q, const Vec6<T>* ref_q){
/* convert to rotation matrix */
Vec3<T> rpy(p(3), p(4), p(5));
Mat3<T> rot = ori::rpyToRotMat(rpy);
T rx = rot(0, 0); T ox = rot(0, 1); T lx = rot(0, 2); T px = p(0);
T ry = rot(1, 0); T oy = rot(1, 1); T ly = rot(1, 2); T py = p(1);
T rz = rot(2, 0); T oz = rot(2, 1); T lz = rot(2, 2); T pz = p(2);
/* define DH params */
T a3 = DH_params[2][0];
T a4 = DH_params[3][0];
T d1 = DH_params[0][2];
T d4 = DH_params[3][2];
T d5 = DH_params[4][2];
T d6 = DH_params[5][2];
/* consider multiple solution */
Vec6<T> solve_angles[8];
for(int i = 0; i < 8; i ++){
/* change sign */
T signs[3];
for(int s = 0; s < 3; s ++){
signs[s] = i & (1 << s) ? -1 : 1;
}
/* theta1 */
T m = py - d6 * ly;
T n = px - d6 * lx;
T verify = pow(m,2) + pow(n,2) - pow(d4,2);
if(verify < 0) {
verify = 0;
}
T t1 = atan2(m, n) - atan2(d4, signs[0] * sqrt(verify));
/* theta5 */
T t5 = signs[1] * acos(ly * cos(t1) - lx * sin(t1));
/* theta6 */
m = ry * cos(t1) - rx * sin(t1);
n = oy * cos(t1) - ox * sin(t1);
T t6;
if(t5 == 0){
t6 = 0;
}
else{
t6 = atan2(m, n) - atan2(sin(t5), 0);
}
/* theta3 */
m = px*cos(t1) + py*sin(t1) - d6*lx*cos(t1) - d6*ly*sin(t1) + d5*ry*sin(t1)*sin(t6)
+ d5*ox*cos(t1)*cos(t6) + d5*oy*cos(t6)*sin(t1) + d5*rx*cos(t1)*sin(t6);
n = pz - d1 - d6*lz + d5*oz*cos(t6) + d5*rz*sin(t6);
verify = (pow(m,2) + pow(n,2) - pow(a3,2) - pow(a4,2)) / (2*a3*a4);
if(verify > 1) {
verify = 1;
}
else if(verify < -1) {
verify = -1;
}
T t3 = signs[2] * acos(verify);
/* theta2 */
T s2 = (m * (a4*cos(t3) + a3) - a4*sin(t3)*n) / (pow(a4,2) + 2*a4*a3*cos(t3) + pow(a3,2));
T c2 = (n + a4*sin(t3)*s2) / (a4*cos(t3) + a3);
T t2 = atan2(s2, c2);
/* theta4 */
T s234 = -rz*cos(t5)*cos(t6) + lz*sin(t5) + oz*cos(t5)*sin(t6);
T c234 = -oz*cos(t6) - rz*sin(t6);
T t4 = atan2(s234, c234) - t2 - t3;
solve_angles[i] << t1, t2, t3, t4, t5, t6;
}
/* filter optimal solution */
if(ref_q == NULL){
*q = solve_angles[0];
}
else{
uint8_t optimal_index = 0;
T min_err = 0;
for(int i = 0; i < 8; i ++){
T err = 0;
for(int j = 0; j < JOINTS_NUM; j ++){
err += abs(ref_q->operator()(j) - solve_angles[i][j]) * (*links_length[j]);
}
if(!i){
min_err = err;
}
else if(err < min_err){
min_err = err;
optimal_index = i;
}
}
*q = solve_angles[optimal_index];
}
return true;
}
