何理，張軍

(廣州數(shù)控設(shè)備有限公司，廣東廣州 510530)

基于簡(jiǎn)化形式的Jacobian矩陣的牛頓迭代法求解6自由度機(jī)器人逆解算法

何理，張軍

(廣州數(shù)控設(shè)備有限公司，廣東廣州 510530)

為解決一般6自由度機(jī)器人的逆解問題，提出一種基于簡(jiǎn)化的Jacobian矩陣形式的牛頓迭代算法逐次逼近目標(biāo)位姿的逆解算法，由于簡(jiǎn)化的Jacobian矩陣不是方陣，需采用SVD分解求廣義逆來避免奇異性問題。該算法有較好的局部收斂性，能夠達(dá)到較好的速度和精度。

機(jī)器人;Jacobian矩陣;牛頓迭代;廣義逆

0 前言

目前國內(nèi)實(shí)用的6R工業(yè)機(jī)器人其逆運(yùn)動(dòng)學(xué)方程是有解析解的，對(duì)于這類機(jī)器人都滿足后3個(gè)關(guān)節(jié)軸交于一點(diǎn)或者軸相互平行，對(duì)于一些設(shè)計(jì)不滿足這樣的條件的機(jī)器人，可以通過矢量運(yùn)算找到14個(gè)逆運(yùn)動(dòng)學(xué)方程，通過分離變量消元轉(zhuǎn)化為求一個(gè)16次多項(xiàng)式求根的問題求逆解，此方法在時(shí)間和空間上對(duì)CPU性能都要求很高，對(duì)嵌入式工業(yè)機(jī)器人系統(tǒng)難以達(dá)到實(shí)時(shí)性。文中以微分運(yùn)動(dòng)為基礎(chǔ)，用簡(jiǎn)化形式的Jacobian矩陣的牛頓迭代法求解非線性方程組方法來求解機(jī)器人的逆運(yùn)動(dòng)學(xué)的數(shù)值解。采用基于SVD分解求矩陣的廣義逆避免矩陣的奇異性問題，通過迭代得到逆運(yùn)動(dòng)的最終解，該算法所需的時(shí)間和空間都較少，能滿足實(shí)時(shí)性要求。

1 牛頓迭代法求逆解

根據(jù)機(jī)器人的D－H參數(shù)和連桿坐標(biāo)系，6R機(jī)器人的正運(yùn)動(dòng)學(xué)方程為

取式 (1)的3個(gè)位置元素和任意3個(gè)姿態(tài)矩陣元素

先將初始值θi通過正解運(yùn)算得到末端位姿Tend1，在初始值θi上分別增加一個(gè)微小變化量，文中取0.001，再通過正解運(yùn)算得到末端位姿Tend2，在初始值θi上分別減小一個(gè)微小變化量，再通過正解運(yùn)算得到末端位姿Tend3，由于0.001比較小，所以取Δθ=0.001，Δf=Tend2－Tend1，Δθ= －0.001，Δf=Tend3－Tend1，

由于Jacobian矩陣不是方陣，所以需要用最小二乘法求最優(yōu)解，但是最小二乘法中也要求JTJ的逆矩陣，由于JTJ的逆可能不存在，所以需要直接對(duì)Jacobia矩陣進(jìn)行基于Householder的SVD分解，得到J=UDVH，其中U為12階酉矩陣，V為6階酉矩陣，D為對(duì)角矩陣且對(duì)角線元素個(gè)數(shù)等于J的秩，所以廣義逆 J+=VD－1UH，D－1為 D的對(duì)角線元素求倒數(shù)，UH為U的共軛轉(zhuǎn)置，VH為V的共軛轉(zhuǎn)置。

2 數(shù)據(jù)測(cè)驗(yàn)

跟原始關(guān)節(jié)角值一樣，且迭代次數(shù)為3次，收斂比較快，當(dāng)初值選取適當(dāng)時(shí)，收斂速度和精度都能達(dá)到要求，在實(shí)際運(yùn)用中可以取上一次的插補(bǔ)點(diǎn)作為迭代初值。雅克比矩陣的廣義逆運(yùn)算占用了逆運(yùn)動(dòng)學(xué)的大部分時(shí)間，采用廣義逆運(yùn)算可以很好地解決矩陣不存在逆的問題，但是增加執(zhí)行時(shí)間。

3 結(jié)論

通過一種簡(jiǎn)化形式的Jacobian矩陣求一般6自由度機(jī)器人逆運(yùn)動(dòng)學(xué)的牛頓迭代算法，并在Matlab7.0和VC6.0兩種平臺(tái)實(shí)現(xiàn)了該算法，并做了相應(yīng)的測(cè)試，測(cè)試表明這種迭代方法在大多數(shù)情況下是收斂的，且有較好精度。

［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］邱起榮.矩陣論與數(shù)值分析－理論及其工程應(yīng)用［M］.北京:清華大學(xué)出版社，2013.

［4］譚民，徐德，侯增廣，等.先進(jìn)機(jī)器人控制［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—)，男，學(xué)士，研究方向?yàn)闄C(jī)器人運(yùn)動(dòng)控制。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.