机械臂运动学逆解（牛顿法）

机械臂运动学逆解（牛顿法）

  常用的工业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左边式已知量。采用牛顿法求解运动学逆解，已知机械臂末端姿态为$$\left[\begin{array}{cccc}{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{array}\right]$$，构造目标函数，如式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\ast (f_{11}-r_{11}{)}^2+0.08\ast (f_{12}-r_{12}{)}^2+0.08\ast (f_{13}-r_{13}{)}^2+0.08\ast (f_{31}-r_{31}{)}^2+0.08\ast (f_{32}-r_{32}{)}^2+0.08\ast (f_{34}-r_{34}{)}^2\text{\hspace{1em}(2)}$$

目标函数的雅可比矩阵为$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}]$
目标函数的雅克比矩阵为 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​​ ​

迭代步长$\Delta \Theta =-H\ast J^T$

程序验证

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108

注意事项

1. matlab在构造雅可比函数、函数矩阵的时候比较慢；
2. 使用四元数建立运动学模型，效率更低（暂时未发现什么原因）；
3. 可通过设置迭代的初始值，获得其它的逆解；