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

本文介绍: 使用牛顿法求解6轴串联机械臂的运动学逆解，并用matlab进行验证

迭代步长

−

∗

Delta Theta = -H*J^T

$ΔΘ = - H * 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