数值计算方法 matlab用二分法或简单迭代法求_机器人工程师进阶之路（十）六轴机械臂逆运算数值运算求解...

在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

计算结果为

得到的结果与设定相符。

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

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