机械臂运动学逆解（牛顿法）
常用的工业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
f11f21f310f12f22f320f13f23f330f14f24f341
=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}
r11r21r310r12r22r320r13r23r330r14r24r341
，构造目标函数，如式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
注意事项
- matlab在构造雅可比函数、函数矩阵的时候比较慢；
- 使用四元数建立运动学模型，效率更低（暂时未发现什么原因）；
- 可通过设置迭代的初始值，获得其它的逆解；
