基于简化形式的Jacobian矩阵的牛顿迭代法求解6自由度机器人逆解算法

何理，张军

(广州数控设备有限公司，广东广州 510530)

基于简化形式的Jacobian矩阵的牛顿迭代法求解6自由度机器人逆解算法

何理，张军

(广州数控设备有限公司，广东广州 510530)

为解决一般6自由度机器人的逆解问题，提出一种基于简化的Jacobian矩阵形式的牛顿迭代算法逐次逼近目标位姿的逆解算法，由于简化的Jacobian矩阵不是方阵，需采用SVD分解求广义逆来避免奇异性问题。该算法有较好的局部收敛性，能够达到较好的速度和精度。

机器人;Jacobian矩阵;牛顿迭代;广义逆

0 前言

目前国内实用的6R工业机器人其逆运动学方程是有解析解的，对于这类机器人都满足后3个关节轴交于一点或者轴相互平行，对于一些设计不满足这样的条件的机器人，可以通过矢量运算找到14个逆运动学方程，通过分离变量消元转化为求一个16次多项式求根的问题求逆解，此方法在时间和空间上对CPU性能都要求很高，对嵌入式工业机器人系统难以达到实时性。文中以微分运动为基础，用简化形式的Jacobian矩阵的牛顿迭代法求解非线性方程组方法来求解机器人的逆运动学的数值解。采用基于SVD分解求矩阵的广义逆避免矩阵的奇异性问题，通过迭代得到逆运动的最终解，该算法所需的时间和空间都较少，能满足实时性要求。

1 牛顿迭代法求逆解

根据机器人的D－H参数和连杆坐标系，6R机器人的正运动学方程为

取式 (1)的3个位置元素和任意3个姿态矩阵元素

先将初始值θi通过正解运算得到末端位姿Tend1，在初始值θi上分别增加一个微小变化量，文中取0.001，再通过正解运算得到末端位姿Tend2，在初始值θi上分别减小一个微小变化量，再通过正解运算得到末端位姿Tend3，由于0.001比较小，所以取Δθ=0.001，Δf=Tend2－Tend1，Δθ= －0.001，Δf=Tend3－Tend1，

由于Jacobian矩阵不是方阵，所以需要用最小二乘法求最优解，但是最小二乘法中也要求JTJ的逆矩阵，由于JTJ的逆可能不存在，所以需要直接对Jacobia矩阵进行基于Householder的SVD分解，得到J=UDVH，其中U为12阶酉矩阵，V为6阶酉矩阵，D为对角矩阵且对角线元素个数等于J的秩，所以广义逆 J+=VD－1UH，D－1为 D的对角线元素求倒数，UH为U的共轭转置，VH为V的共轭转置。

2 数据测验

跟原始关节角值一样，且迭代次数为3次，收敛比较快，当初值选取适当时，收敛速度和精度都能达到要求，在实际运用中可以取上一次的插补点作为迭代初值。雅克比矩阵的广义逆运算占用了逆运动学的大部分时间，采用广义逆运算可以很好地解决矩阵不存在逆的问题，但是增加执行时间。

3 结论

通过一种简化形式的Jacobian矩阵求一般6自由度机器人逆运动学的牛顿迭代算法，并在Matlab7.0和VC6.0两种平台实现了该算法，并做了相应的测试，测试表明这种迭代方法在大多数情况下是收敛的，且有较好精度。

［1］KURFESS Thomas R.Robotics and Automation Handbook［M］.USA:CRC Press LLC，2005:47－63.

［2］RAGHAVEN M，ROTH B.Kinematic Analysis of the 6R Manipulator of General Geometry［C］//The fifth international symposium on Robotics research.MIT Press，1991:263－269.

［3］邱起荣.矩阵论与数值分析－理论及其工程应用［M］.北京:清华大学出版社，2013.

［4］谭民，徐德，侯增广，等.先进机器人控制［M］.北京:高等教育出版社，2007.

Inverse Algorithm Solution of 6-DOF Robot Base on New ton Iterative Method of Simp lified Jacobian Matrix

HE Li，ZHANG Jun
(GSK CNC EQUIPMENT Co.，Ltd.，Guangzhou Guangdong 510530，China)

Robot;Jacobian matrix;Iterative Newton;Preudo-inverse

TP242.2

A

1001－3881(2015)21－107－2

10.3969/j.issn.1001 －3881.2015.21.025

2014－09－15

何理 (1986—)，男，学士，研究方向为机器人运动控制。E－mail:xiyouheli@163.com。

Absruct:In order to solve the inverse kinematics problem of robotwith the general six degree of freedom(6-DOF)，an inverse algorithm solution was proposed base on Newton iterativemethod of simplified Jacobianmatrix to approach the target position in differential.Because the simplified Jacobian was not squarematrix，the SVD decomposition was required for solving preudo-inverse to avoid the singularity problem.The algorithm has quite good local convergence，which can achieve the ideal speed and higher precision.