??常用的工业6轴机械臂采用6轴串联结构，虽然其运动学正解比较容易，但是其运动学逆解非常复杂，其逆解的方程组高度非线性，且难以化简。
??由于计算机技术的发展，依靠其强大的算力，可以通过数值解的方式对机械臂的运动学逆解方程组进行求解。以下将使用牛顿法详解整个求解过程。
机械臂运动学正解方程组如式1所示。
[
f
11
f
12
f
13
f
14
f
21
f
22
f
23
f
24
f
31
f
32
f
33
f
34
0
0
0
1
]
=
f
(
θ
1
,
θ
2
,
θ
3
,
θ
4
,
θ
5
,
θ
6
)
(1)
\begin{bmatrix} f_{11}&f_{12}&f_{13} & f_{14}\\ f_{21}&f_{22}&f_{23} & f_{24}\\ f_{31}&f_{32}&f_{33} & f_{34}\\ 0&0&0 & 1\\ \end{bmatrix} =f(\theta_1, \theta_2, \theta_3, \theta_4, \theta_5, \theta_6) \tag1
?f11?f21?f31?0?f12?f22?f32?0?f13?f23?f33?0?f14?f24?f34?1?
?=f(θ1?,θ2?,θ3?,θ4?,θ5?,θ6?)(1)
对于运动学正解，式1右边是已知量，对于运动学逆解，式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
]
\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}
?r11?r21?r31?0?r12?r22?r32?0?r13?r23?r33?0?r14?r24?r34?1?
?，构造目标函数，如式2所示。
F
(
θ
1
,
θ
2
,
θ
3
,
θ
4
,
θ
5
,
θ
6
)
=
(
f
14
?
r
14
)
2
+
(
f
24
?
r
24
)
2
+
(
f
34
?
r
34
)
2
+
0.08
?
(
f
11
?
r
11
)
2
+
0.08
?
(
f
12
?
r
12
)
2
+
0.08
?
(
f
13
?
r
13
)
2
+
0.08
?
(
f
31
?
r
31
)
2
+
0.08
?
(
f
32
?
r
32
)
2
+
0.08
?
(
f
34
?
r
34
)
2
(2)
F(\theta_1, \theta_2, \theta_3, \theta_4, \theta_5, \theta_6) =(f_{14}-r_{14})^2+(f_{24}-r_{24})^2+(f_{34}-r_{34})^2+0.08*(f_{11}-r_{11})^2+0.08*(f_{12}-r_{12})^2+0.08*(f_{13}-r_{13})^2+0.08*(f_{31}-r_{31})^2+0.08*(f_{32}-r_{32})^2+0.08*(f_{34}-r_{34})^2 \tag2
F(θ1?,θ2?,θ3?,θ4?,θ5?,θ6?)=(f14??r14?)2+(f24??r24?)2+(f34??r34?)2+0.08?(f11??r11?)2+0.08?(f12??r12?)2+0.08?(f13??r13?)2+0.08?(f31??r31?)2+0.08?(f32??r32?)2+0.08?(f34??r34?)2(2)
目标函数的雅可比矩阵为
J
=
[
?
F
?
θ
1
,
?
F
?
θ
2
,
?
F
?
θ
3
,
?
F
?
θ
4
,
?
F
?
θ
5
,
?
F
?
θ
6
]
J= [\frac{\partial F}{\partial\theta_1}, \frac{\partial F}{\partial\theta_2},\frac{\partial F}{\partial\theta_3},\frac{\partial F}{\partial\theta_4},\frac{\partial F}{\partial\theta_5},\frac{\partial F}{\partial\theta_6}]
J=[?θ1??F?,?θ2??F?,?θ3??F?,?θ4??F?,?θ5??F?,?θ6??F?]
目标函数的雅克比矩阵为
H
=
[
?
2
F
?
θ
1
2
?
2
F
?
θ
1
?
θ
2
?
2
F
?
θ
1
?
θ
3
?
2
F
?
θ
1
?
θ
4
?
2
F
?
θ
1
?
θ
5
?
2
F
?
θ
1
?
θ
6
?
2
F
?
θ
2
?
θ
1
?
2
F
?
θ
2
2
?
2
F
?
θ
2
?
θ
3
?
2
F
?
θ
2
?
θ
4
?
2
F
?
θ
2
?
θ
5
?
2
F
?
θ
2
?
θ
6
?
2
F
?
θ
3
?
θ
1
?
2
F
?
θ
3
?
θ
2
?
2
F
?
θ
3
2
?
2
F
?
θ
3
?
θ
4
?
2
F
?
θ
3
?
θ
5
?
2
F
?
θ
3
?
θ
6
?
2
F
?
θ
4
?
θ
1
?
2
F
?
θ
4
?
θ
2
?
2
F
?
θ
4
?
θ
3
?
2
F
?
θ
4
2
?
2
F
?
θ
4
?
θ
5
?
2
F
?
θ
4
?
θ
6
?
2
F
?
θ
5
?
θ
1
?
2
F
?
θ
5
?
θ
2
?
2
F
?
θ
5
?
θ
3
?
2
F
?
θ
5
?
θ
4
?
2
F
?
θ
5
2
?
2
F
?
θ
5
?
θ
6
?
2
F
?
θ
6
?
θ
1
?
2
F
?
θ
6
?
θ
2
?
2
F
?
θ
6
?
θ
3
?
2
F
?
θ
6
?
θ
4
?
2
F
?
θ
6
?
θ
5
?
2
F
?
θ
6
2
]
H=\begin{bmatrix} \frac{\partial^2 F}{\partial\theta_1^2} & \frac{\partial^2 F}{\partial\theta_1 \partial\theta_2} & \frac{\partial^2 F}{\partial\theta_1 \partial\theta_3} & \frac{\partial^2 F}{\partial\theta_1 \partial\theta_4} & \frac{\partial^2 F}{\partial\theta_1 \partial\theta_5} & \frac{\partial^2 F}{\partial\theta_1 \partial\theta_6} \\ \frac{\partial^2 F}{\partial\theta_2 \partial\theta_1} & \frac{\partial^2 F}{\partial\theta_2^2} & \frac{\partial^2 F}{\partial\theta_2 \partial\theta_3} & \frac{\partial^2 F}{\partial\theta_2 \partial\theta_4} & \frac{\partial^2 F}{\partial\theta_2 \partial\theta_5} & \frac{\partial^2 F}{\partial\theta_2 \partial\theta_6} \\ \frac{\partial^2 F}{\partial\theta_3 \partial\theta_1} & \frac{\partial^2 F}{\partial\theta_3 \partial\theta_2} & \frac{\partial^2 F}{\partial\theta_3^2} & \frac{\partial^2 F}{\partial\theta_3 \partial\theta_4} & \frac{\partial^2 F}{\partial\theta_3 \partial\theta_5} & \frac{\partial^2 F}{\partial\theta_3 \partial\theta_6} \\ \frac{\partial^2 F}{\partial\theta_4 \partial\theta_1} & \frac{\partial^2 F}{\partial\theta_4 \partial\theta_2} & \frac{\partial^2 F}{\partial\theta_4 \partial\theta_3} &\frac{\partial^2 F}{\partial\theta_4^2} & \frac{\partial^2 F}{\partial\theta_4 \partial\theta_5} & \frac{\partial^2 F}{\partial\theta_4 \partial\theta_6} \\ \frac{\partial^2 F}{\partial\theta_5 \partial\theta_1} & \frac{\partial^2 F}{\partial\theta_5 \partial\theta_2} & \frac{\partial^2 F}{\partial\theta_5 \partial\theta_3} & \frac{\partial^2 F}{\partial\theta_5 \partial\theta_4} & \frac{\partial^2 F}{\partial\theta_5^2} & \frac{\partial^2 F}{\partial\theta_5 \partial\theta_6} \\ \frac{\partial^2 F}{\partial\theta_6 \partial\theta_1} & \frac{\partial^2 F}{\partial\theta_6 \partial\theta_2} & \frac{\partial^2 F}{\partial\theta_6 \partial\theta_3} & \frac{\partial^2 F}{\partial\theta_6 \partial\theta_4} & \frac{\partial^2 F}{\partial\theta_6 \partial\theta_5} & \frac{\partial^2 F}{\partial\theta_6^2} \\ \end{bmatrix}
H=
??θ12??2F??θ2??θ1??2F??θ3??θ1??2F??θ4??θ1??2F??θ5??θ1??2F??θ6??θ1??2F???θ1??θ2??2F??θ22??2F??θ3??θ2??2F??θ4??θ2??2F??θ5??θ2??2F??θ6??θ2??2F???θ1??θ3??2F??θ2??θ3??2F??θ32??2F??θ4??θ3??2F??θ5??θ3??2F??θ6??θ3??2F???θ1??θ4??2F??θ2??θ4??2F??θ3??θ4??2F??θ42??2F??θ5??θ4??2F??θ6??θ4??2F???θ1??θ5??2F??θ2??θ5??2F??θ3??θ5??2F??θ4??θ5??2F??θ52??2F??θ6??θ5??2F???θ1??θ6??2F??θ2??θ6??2F??θ3??θ6??2F??θ4??θ6??2F??θ5??θ6??2F??θ62??2F??
?
迭代步长 Δ Θ = ? H ? J T \Delta \Theta = -H*J^T ΔΘ=?H?JT
clear;
clc;
rng(1); %固定随机数种子
%构造运动学模型
syms a0 a1 a2 a3 a4 a5;
FK = FKinematics(a0, a1, a2, a3, a4, a5);
%构造目标函数
syms T14 T24 T34 T11 T12 T13 T31 T32 T33;
opt_F(a0, a1, a2, a3, a4, a5, T14, T24, T34, T11, T12, T13, T31, T32, T33) = (FK(1, 4) - T14)^2 + ...
(FK(2, 4) - T24)^2 + ...
(FK(3, 4) - T34)^2 + ...
0.08 * (FK(1, 1) - T11)^2 + ...
0.08 * (FK(1, 2) - T12)^2 + ...
0.08 * (FK(1, 3) - T13)^2 + ...
0.08 * (FK(3, 1) - T31)^2 + ...
0.08 * (FK(3, 2) - T32)^2 + ...
0.08 * (FK(3, 3) - T33)^2
opt_F = matlabFunction(opt_F);
%构造目标函数的雅可比函数矩阵
J(a0, a1, a2, a3, a4, a5, T14, T24, T34, T11, T12, T13, T31, T32, T33) = jacobian(opt_F, [a0 a1 a2 a3 a4 a5])
J = matlabFunction(J);
%构造目标函数的海塞矩阵
H(a0, a1, a2, a3, a4, a5, T14, T24, T34, T11, T12, T13, T31, T32, T33) = jacobian(J, [a0 a1 a2 a3 a4 a5])
H = matlabFunction(H);
T = FKinematics(2, 0.5, -1.6, 0.6, 1.5, -0.9)
X = IKinematics(opt_F, J, H, T);
X
T
FKinematics(X(1), X(2), X(3), X(4), X(5), X(6))
function T = FKinematics(x1, x2, x3, x4, x5, x6)
T1 = urdfJoint(0, 0, 0.3015, 0, 0, 0, x1);
T2 = urdfJoint(0.077746, -0.0869967, 0.1465, 1.5708, 1.5708, 0, x2);
T3 = urdfJoint(-0.64, 0, -0.015, 0, 0, 0, x3);
T4 = urdfJoint(-0.195, 0.9055, -0.072, -1.5708, 0, 0, x4);
T5 = urdfJoint(0, 0, 0, -1.6876, -1.5708, -3.0248, x5);
T6 = urdfJoint(0, 0, 0, -1.5708, 0, -1.5708, x6);
T7 = urdfJoint(0, 0, 0.08, 0, 0, 0, 0); %法兰盘的位姿
T = T1 * T2 * T3 * T4 * T5 * T6 * T7;
end
function X = IKinematics(opt_F, J, H, T)
X = [0; 0; 0; 0; 0; 0];
X0 = [0; 0; 0; 0; 0; 0];
min_opt_value = opt_F(X0(1), X0(2), X0(3), X0(4), X0(5), X0(6), T(1, 4), T(2, 4), T(3, 4), T(1, 1), T(1, 2), T(1, 3), T(3, 1), T(3, 2), T(3, 3));
X_opt = X0;
last_opt_value = min_opt_value;
t0 = clock;
for i = 1 : 1000
i
Jn = J(X0(1), X0(2), X0(3), X0(4), X0(5), X0(6), T(1, 4), T(2, 4), T(3, 4), T(1, 1), T(1, 2), T(1, 3), T(3, 1), T(3, 2), T(3, 3));
Hn = H(X0(1), X0(2), X0(3), X0(4), X0(5), X0(6), T(1, 4), T(2, 4), T(3, 4), T(1, 1), T(1, 2), T(1, 3), T(3, 1), T(3, 2), T(3, 3));
%[U, S, V] = svd(Hn);
%det_X = V * inv(S) * U' * Jn';
det_X = inv(Hn) * Jn';
X0 = X0 - det_X;
X0';
opt_value = opt_F(X0(1), X0(2), X0(3), X0(4), X0(5), X0(6), T(1, 4), T(2, 4), T(3, 4), T(1, 1), T(1, 2), T(1, 3), T(3, 1), T(3, 2), T(3, 3));
if(min_opt_value > opt_value)
min_opt_value = opt_value;
X_opt = X0;
end
if(min_opt_value < 0.0001)
break;
end
if(abs(last_opt_value - opt_value) < 0.00001)
fprintf('陷入局部最小解，将重新生成迭代初始值');
X0 = randn(6, 1);
end
last_opt_value = opt_value;
end
t = etime(clock,t0);
fprintf('solve time: %f', t);
T;
X = X_opt';
T1 = FKinematics(X_opt(1), X_opt(2), X_opt(3), X_opt(4), X_opt(5), X_opt(6));
end
function T = urdfJoint(x0, y0, z0, R0, P0, Y0, theta)
r1 = [1 0 0;
0 cos(R0) -sin(R0);
0 sin(R0) cos(R0)];
r2 = [ cos(P0) 0 sin(P0);
0 1 0;
-sin(P0) 0 cos(P0)];
r3 = [cos(Y0) -sin(Y0) 0;
sin(Y0) cos(Y0) 0;
0 0 1];
r = r3 * r2 * r1;
T0 = [r(1, 1) r(1, 2) r(1, 3) x0;
r(2, 1) r(2, 2) r(2, 3) y0;
r(3, 1) r(3, 2) r(3, 3) z0;
0 0 0 1];
T = T0 * [cos(theta) -sin(theta) 0 0;
sin(theta) cos(theta) 0 0;
0 0 1 0;
0 0 0 1];
end