js m 数值缩写k_机器人工程师进阶之路（十）六轴机械臂逆运算数值运算求解

梁政：机器人工程师进阶之路（四）6轴机械臂的正逆运动​zhuanlan.zhihu.com

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

Newton-Raphson迭代法

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

算法概略：

求解

的数值解

，其中

可微。

Step 0：假设初始值

，k=0，并列出方程g的一阶泰勒展开式

Step 1：判断

是否满足终止条件，例如

；是，则终止；

Step 2：令

，

；转Step 1。

逆运动数值运算求解

对于末端执行器位姿，我们已知是由各关节角度最后的得到的，总结为公式为

，其中

为末端位姿。那么对于逆运算，我么可以利用牛顿法数值求解。首先设

Step 0：假设初始值

，k=0，并列出方程g的一阶泰勒展开式

Step 1：判断

是否满足终止条件，

；否，则终止；

Step 2：令

，

；转Step 1。

算法的改进

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

计算结果为

得到的结果与设定相符。

但是如果更改不同的初始值

，则会得到不同的结果。这代表着逆解有许多不同的解，在数值迭代法下，根据初始值来到的一个最优解。