基于簡化形式的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.