一种改进的串联机械臂逆运动学问题数值解法

马建伟，闫惠腾，沈亚彬，张红园，吕 琦，高 松

(大连理工大学 精密与特种加工教育部重点实验室，辽宁 大连 116000)

0 引言

串联机械臂逆运动学问题指根据给定的末端位姿求解对应的各关节角度(位移)值，是串联机械臂运动控制的基础[1-2]。自从串联机械臂问世以来，其逆运动学问题受到广泛关注，求解方法主要分为解析法和数值法两类。解析法借助串联机械臂的几何结构和代数方法，推导逆运动学问题解析解，计算速度快，计算结果准确，但需要串联机械臂几何构型满足Pieper准则，即3个相邻关节轴相交于一点或3个相邻关节轴相互平行[3]。数值法通常将机械臂逆运动学问题看作非线性方程组，并基于迭代方法求解，无需串联机械臂几何构型满足Pieper准则，收敛结果由迭代初始条件决定。近年来，这两类逆运动学求解方法的研究也随着机械臂的普及而不断涌现。

对于自由度小于6的串联机械臂和满足Pieper准则的6自由度串联机械臂的逆运动学问题，求解逆运动学问题的过程比较简单[3]。Calderon等[4]使用解析法在5自由度机械臂保持末端执行器坐标系中单轴方向不变的情况下，求解给定位姿对应的逆运动学解。应文杰等[5]基于牛顿下山法建立了3自由度机械臂的逆运动数值求解方法。

对于自由度冗余的串联机械臂和不满足Pieper准则的6自由度串联机械臂的逆运动学问题，求解过程则相对比较复杂。在国内学者的研究中，石建平等[6]以机械臂末端位姿误差最小为目标，将冗余串联机械臂逆运动学问题转化为一个等效的最优化问题，并提出一种改进的粒子群优化算法求解，算法具有较高的收敛精度。林阳等[7]将多种群遗传算法应用于不满足Pieper准则的一般串联机械臂逆运动学问题中，提出一种高精度并行计算的逆运动学求解算法，该算法采用动态变异率的变异算子以提高收敛速度，并将位姿分别设为目标函数以保证二者收敛精度一致。董云等[8]针对平面3自由度冗余串联机械臂的逆运动学问题，提出一种解析法和遗传算法结合的求解方法，该算法相比单独的遗传算法具有计算量小和适应性强的优点。刘松国等[9]提出一种优化的一般6R串联机械臂逆运动学解析解法，将逆运动学问题中6个基础方程变形处理，并基于符号运算将目标矩阵从24阶降低到16阶，提高了算法的计算速度和稳定性。林明等[10]采用自适应的交叉、变异算子，改进了排序方法与遗传算法，以求解PUMA560机械臂的逆运动学问题。在国外学者的研究中，Husty等[11]利用经典多维几何构造方法，将6自由度串联机械臂运动链从中间断为2个3自由度运动链，通过分别求解2个3自由度运动链以及2个2自由度运动链的逆运动学问题，获得16组能够使末端执行器达到相同位姿的逆运动学解，其中包含复数解。Erleben等[12]使用精确的Hessian矩阵代替Levenberg-Marquardt(LM)迭代法中近似的Hessian矩阵，迭代求解机械臂逆运动学问题，通过与拟牛顿法和LM迭代法对比，证明了方法具有较快的收敛速度和较强的收敛能力。Sugihara等[13]基于LM迭代法利用迭代方程残差的平方模量作为参数因子，提出了一种具有高数值稳定性和收敛性能的串联机械臂逆运动学问题数值解法。综上，对于求解串联机械臂逆运动学问题，已有许多学者提出了解决方法，其中不乏一些优秀的数值解法。然而，由于基于智能算法的数值解法计算过程复杂，计算量大，迭代法仍是最常见的数值解法。但是，常用数值迭代法如牛顿迭代法、共轭梯度法等，收敛速度较慢，且合适的迭代初始值难获取，致使常规基于迭代法的数值解法在实际计算过程中，仍存在收敛速度慢、易出现不收敛的问题，影响算法鲁棒性。

为解决上述问题，提出了一种改进的串联机械臂逆运动学问题数值解法。创新将LM迭代法中每一迭代步的参数因子μ由当前迭代步对应的微分运动向量的二范数确定，并额外设置步长因子用于以提高每一迭代步的迭代步长。提出的方法显著提高了算法的收敛速度和收敛能力，有效弥补了常规的数值法鲁棒性不足的缺点。

1 常规的数值解法

以图1所示的6自由度串联机械臂为例，首先介绍常规的数值法求解逆运动学问题的计算过程。按照式(1)计算6自由度串联机械臂正运动学问题。

图1 串联机械臂

(1)

式中: 关节0为基坐标系，n为末端关节坐标系；ηx、ηy、ηz分别为末端关节坐标系n的xn轴与基坐标系的x0、y0、z0轴的夹角余弦值；ox、oy、oz分别为末端关节坐标系n的yn轴与基坐标系的x0、y0、z0轴的夹角余弦值；ax、ay、az分别为末端关节坐标系n的zn轴与基坐标系的x0、y0、z0轴的夹角余弦值；px、py、pz为末端关节坐标系n原点在基坐标系中的笛卡尔坐标。

6自由度串联机械臂正运动学问题表达式还可由旋转和平移组合变换表示，即：

0T6=Trans(px,py,pz)Rot(z,rx)Rot(y,ry)Rot(x,rz)=

(2)

式中:crx、cry、crz、srx、sry、srz分别是cosrx、cosry、cosrz、sinrx、sinry、sinrz的简写，Trans(px,py,pz)表示末端关节坐标系沿基坐标系的x0、y0、z0轴平移px、py、pz距离，Rot(z,rx)Rot(y,ry)Rot(x,rz)表示末端关节坐标系先绕着基坐标系z轴旋转rz角，再绕基坐标系y轴旋转ry角，再绕着基坐标系x轴旋转rx角。

联立式(1)和(2)，得到各关节转角θi(i=1,2,3,4,5,6)与旋转角度rx、ry、rz和平移距离px、py、pz之间的关系式，即：

(3)

(4)

在迭代计算过程中，待求解逆运动学问题的机械臂末端位姿记为Tend，末端位姿对应的逆运动学解记为θend，当前迭代步对应的逆运动学解记为θcur，迭代步逆运动学解对应的机械臂末端位姿记为Tcur。上述定义确定后，则按照如下步骤计算给定末端位姿的逆运动学问题。

① 由Tend和Tcur按照式(5)计算当前迭代步微分算子Δcur。

(5)

② 计算当前迭代步逆运动学解θcur对应的雅可比矩阵J，并按照式(6)计算当前迭代步搜索方向。

(6)

式中：μ为参数因子，其数值需预先设定，且数值大小在步骤③中按照迭代结果自适应地改变。引入参数因子μ的目的，为了防止当JTJ接近奇异时dθ过大，从而影响迭代算法的收敛能力。随后，将θcur与dθ相加得到新的逆运动学解θnew，再利用正运动学计算新的逆运动学解对应的末端位姿Tnew。

③ 由Tend和Tnew按照式(5)计算新的微分算子Δnew，并从中确定新的微分运动向量Dnew，并比较当前迭代步微分运动向量Dcur与新的微分运动向量Dnew的二范数大小。若||Dnew||2<||Dcur||2，则接受新的逆运动学解θnew，令μ=μ/2，θcur=θnew，Dcur=Dnew，返回步骤①进行下一次迭代；若||Dnew||2≥||Dcur||2，则拒绝新的逆运动学解θnew，令μ=2μ，返回步骤①进行下一次迭代。

重复步骤①～③，直到当前迭代步微分运动向量Dcur的二范数小于迭代精度e，算法收敛，得到机械臂末端位姿Tend对应的逆运动学问题的一个解θend。或者迭代次数超出设定上限，算法未能在规定迭代次数内收敛，未能得到满足迭代精度的机械臂逆运动学解。

2 改进的数值解法

同样以图1所示的6自由度串联机械臂为例，提出一种改进的串联机械臂逆运动学问题数值解法，改进的数值法流程图见图2。

图2 改进的数值法流程框图

具体计算过程如下：

① 由Tend和Tcur按照式(5)计算当前迭代步微分算子Δcur，并由微分算子Δcur确定当前迭代步微分运动向量Dcur。判断当前迭代步微分运动向量Dcur的二范数是否小于迭代精度e。若||Dcur||2

② 创新将LM迭代法中每一迭代步的参数因子由当前迭代步对应的微分运动向量的二范数确定，并额外设置步长因子以提高每一代步的迭代步长，即按照式(7)计算参数因子μ。

μ=λ||Dcur||2

(7)

其中，λ为步长因子，是小于1的正参数，其数值越小，当前迭代步在搜索方向上步长越大，算法收敛速度越快。但λ不能太小，过小的λ反而会减慢算法的收敛速度。从经验角度，λ需根据具体问题在1×10-3～1×10-2灵活设定。然后，计算当前迭代步逆运动学解θcur对应的雅可比矩阵J，并按式(6)计算当前迭代步搜索方向dθ。

③ 将θcur与dθ相加得到新的逆运动学解θnew，再利用正运动学计算新的逆运动学解对应的机械臂末端位姿Tnew。在改进的数值法中无需通过比较自适应地调整参数因子μ的值，直接令Tcur=Tnew，θcur=θnew，返回步骤①进行下一次迭代。

重复步骤①～③，直到当前迭代步微分运动向量Dcur的二范数小于迭代精度e，算法收敛，得到串联机械臂末端位姿Tend对应的逆运动学问题的一个解θend。或者迭代次数超出设定上限，算法未能在规定迭代次数内收敛，未能得到满足迭代精度的逆运动学解。需要说明的是，改进的串联机械臂逆运动学问题数值解法理论上是全局收敛的，但迭代收敛的过程与初始条件有关，为避免较长的迭代过程，需要设置迭代次数上限。

通过对比可以看出，常规的数值法和改进的数值法均使用了LM迭代法。不同的是，常规的数值法中参数因子μ按照每一次迭代结果自适应地改变，而改进的数值法基于当前迭代步对应的微分运动向量的二范数确定参数因子μ，这使得迭代算法的收敛速度和收敛能力显著提高[14-15]。

3 实验研究

通过实验验证改进的数值法的有效性。实验对象为图1所示的6自由度串联机械臂，其几何参数和关节旋转范围如表1、2所示。实验是在Intel i5-10400F 2.9 GHz CPU和16Gb RAM的Windows系统下Matlab中进行的。

表1 6自由度串联机械臂改进DH参数

表2 6自由度串联机械臂各关节旋转范围

Matlab Robotics Toolbox包含了丰富的机器人运动学库函数，其逆运动学求解函数正是基于雅可比伪逆矩阵的原理，使用LM迭代法进行求解[16]。按照改进的DH参数法确定的各关节连杆几何参数，在Matlab中借助robotics工具箱建立6自由度串联机械臂数学模型，并由工具箱提供的函数(fkine函数)计算机械臂正运动学问题。其中，θ为连杆夹角，d为连杆距离，a为连杆长度，α为连杆扭角。都是机械臂各关节连杆几何参数。偏置为建模辅助参数，代表了改进DH参数法定义的关节零位(各关节连杆夹角θi均为0)与SR20A机械臂关节零位(各关节转角均为0)之间的偏差。

在验证过程中，首先在6自由度串联机械臂工作空间范围内随机生成10个末端位姿，如表3所示。形式满足式(2)中的旋转和平移组合变换。

表3 随机生成的末端位姿

从表4中可以看出，在使用Matlab Robotics Toolbox中ikine函数的数值解法计算10个机械臂末端位姿逆运动学问题的过程中，在其中5个机械臂末端位姿处算法未能在迭代次数上限内，收敛到一个满足迭代精度的机械臂逆运动学解，则收敛能力为50%。而在使用改进的数值法计算10个机械臂末端位姿逆运动学问题的过程中，在9个末端位姿处算法均能在迭代次数上限内收敛到一个满足迭代精度的逆运动学解，则改进的数值法收敛能力为90%。因此，改进的数值法相比于ikine函数的数值法，算法的收敛能力提升了1.8倍。

表4 2种数值法的收敛能力

在2种数值法均能够收敛的末端位姿处，对2种数值法的收敛速度进行了比较，结果如表5所示。在这些末端位姿处，2种算法收敛过程如图3所示。

表5 2种数值法的收敛速度

图3 2种数值法微分运动向量二范数下降速度

从表5中可以看出，在2种数值法均能够收敛的6自由度串联机械臂末端位姿处，ikine函数的数值法的平均收敛时间为0.337 3 s，而改进的数值法的平均收敛时间为0.042 5 s。因此，改进的数值法相比于ikine函数的数值法，算法的收敛速度提升了7.9倍。此外，从图3中也可以直观地看出，改进的数值法在计算过程中作为迭代结束依据的微分运动向量二范数||Dcur||2下降更快，迭代次数更少，收敛速度提升显著。

4 结论

针对常规的多轴串联机械臂逆运动学数值解法存在的收敛速度慢、易出现不收敛等问题，基于LM迭代法提出了一种改进的多轴串联机械臂逆运动学数值解法。本方法在常规数值解法的基础上，创新将LM迭代法中每一迭代步的参数因子由当前迭代步对应的微分运动向量二范数确定，进一步提升迭代步搜索方向的准确性，并额外设置步长因子以提高每一迭代步的迭代步长。

实验结果表明，提出的基于当前迭代步微分运动向量二范数计算参数因子的改进的数值法，相比与Matlab Robotics Toolbox中ikine函数的数值解法，无论在收敛能力还是收敛速度上，都有显著提升，有效解决了数值法鲁棒性不足的问题，能够在解析法无法应用的场合中快速稳定求解多轴串联机械臂逆运动学问题。