在DH法，我粗略介绍了运动分析法求逆运动的解析解，到现在还有坑没填完。本篇将利用雅可比矩阵，用数据迭代法来求解运动逆解。

Newton-Raphson迭代法

在求解之前，先介绍一下求解方法。数值迭代法一般都是用来求解非线性方程解析解较困难的方法。迭代的方式有很多，比如二值法。此处用到的是Newton-Raphson，也就是牛顿法。

算法概略：

[公式]

     

逆运动数值运算求解

[公式]

[公式]

     

算法的改进

1. 其中 [公式] ，为机械臂雅可比矩阵。在雅可比矩阵不是方阵，或者奇异和接近奇异时，雅可比不可逆。所以将利用伪逆矩阵（pseudo inverse） [公式]

[公式] 如果J是宽矩阵，列数大于行数

[公式] 如果J是长矩阵，行数大于列数

所以迭代式变为 [公式]

2. 另外末端位姿不能简单的表示为T矩阵，应转化为运动旋量。

在物体自身坐标系中， [公式] ， [公式]

所以Step 2判断条件改为，令 [公式] ，Step 3迭代式改为 [公式]

六轴机械臂的逆运算求解

假设目标位姿T矩阵为

[公式]

根据之前得到的Innfos模型和雅可比矩阵来用Matlab来求解到达此位姿的关节角数值解 [公式]

H1=105.03;W1=80.09;L1=174.42;W2=84.51;L2=174.42;W3=80.09;H2=80.09;W4=44.36;
M=[-1 0 0 L1+L2;0 0 1 W1-W2+W3+W4;0 1 0 H1-H2;0 0 0 1]  % Home Configuration of End-Effector 
%% Spatial Twist

Blist = [[0; 1; 0; W1-W2+W3+W4; 0; L1+L2], ...
        [0; 0; 1; H2; -L1-L2; 0],...
        [0; 0; 1; H2; -L2; 0],...
        [0 ;0; 1; H2; 0; 0],...
        [0; -1; 0; -W4; 0;0],...
        [0; 0; 1; 0; 0; 0]];   %The joint screw axes in the end-effector frame
Tsd = [0.2165,0.8750,-0.4330,79.9377;
     0.6250,0.2165,0.7500,177.8882;
     0.7500,-0.4330,-0.500,-155.4122;
     0,0,0,1]
thetalist0 = [pi/6+0.1; pi/6+0.1; pi/6+0.1; pi/6+0.1; pi/6+0.1; pi/6+0.1];
eomg = 0.01;
ev = 0.001;
    
 [thetalist, success] = IKinBody(Blist, M, Tsd, thetalist0, eomg, ev)

function [thetalist, success] = IKinBody(Blist, M, T, thetalist0, eomg, ev)
thetalist = thetalist0;
i = 0;
maxiterations = 20;
Vb = se3ToVec(MatrixLog6(TransInv(FKinBody(M, Blist, thetalist)) * T));
err = norm(Vb(1: 3)) > eomg || norm(Vb(4: 6)) > ev;
while err && i < maxiterations
    thetalist = thetalist + pinv(JacobianBody(Blist, thetalist)) * Vb;
    i = i + 1;
    Vb = se3ToVec(MatrixLog6(TransInv(FKinBody(M, Blist, thetalist)) * T));
    err = norm(Vb(1: 3)) > eomg || norm(Vb(4: 6)) > ev;
end
success = ~ err;
end

计算结果为

得到的结果与设定相符。

但是如果更改不同的初始值 [公式] ，则会得到不同的结果。这代表着逆解有许多不同的解，在数值迭代法下，根据初始值来到的一个最优解。