基于L-M方法的6R機器人軌跡規(guī)劃快速逆解算法

鄒麗梅，郭 波，錢學(xué)毅

（武夷學(xué)院 機電工程學(xué)院，武夷山 354300）

基于L-M方法的6R機器人軌跡規(guī)劃快速逆解算法

鄒麗梅，郭 波，錢學(xué)毅

（武夷學(xué)院 機電工程學(xué)院，武夷山 354300）

應(yīng)用Denavit-Hartenberg建模方法，建立6R機器人正運動學(xué)模型，組成非線性方程組，提出運用Levenberg-Marquardt方法，求取機器人運動學(xué)逆解。利用MATLAB編程環(huán)境，實現(xiàn)軌跡規(guī)劃位置逆解算法。經(jīng)仿真驗證，算法可完成復(fù)雜的軌跡輪廓規(guī)劃逆解求解，有較高的效率和精度，可應(yīng)用于一般6R機器人，也可適用自由度冗余更高的工業(yè)機器人。

Levenberg-Marquardt方法；工業(yè)機器人；軌跡規(guī)劃；逆解；非線性方程組；

0　引言

機器人高效求運動學(xué)逆解算法開發(fā)一直是機器人運動學(xué)領(lǐng)域的熱點問題，是實現(xiàn)機器人運動控制和軌跡規(guī)劃的基礎(chǔ)。當(dāng)前常見6R工業(yè)機器人是典型帶約束的多輸入輸出非線性系統(tǒng)，關(guān)節(jié)運動相互關(guān)聯(lián)耦合，傳統(tǒng)逆解求法將已知機器人末端位姿矩陣逐個左乘齊次變換矩陣逆陣，循序解出各關(guān)節(jié)角度值。求解過程不僅涉及矩陣求逆的復(fù)雜運算，且可能的逆解組合上限多達16組實數(shù)［1］，先求出的關(guān)節(jié)角度不適用機器人預(yù)期位姿時，求解過程就得重試，因此缺陷較多。近年發(fā)展的求逆解算法有兩類，第一類是利用仿生智能算法直接在關(guān)節(jié)角解空間尋優(yōu)或者搜索位姿矩陣與解空間的映射關(guān)系。吳振宇等建立優(yōu)化算法的目標(biāo)函數(shù)，在粒子群算法中引入遺傳操作求解問題屬于前者［2］。陳桂等應(yīng)用優(yōu)化的BP神經(jīng)網(wǎng)絡(luò)進行逆運動學(xué)求解屬于后者［3］。仿生智能算法實現(xiàn)簡單精度較高，但求解速度慢。運動學(xué)逆解答案不唯一，仿生智能尋優(yōu)轉(zhuǎn)變?yōu)閺?fù)雜的多目標(biāo)帶約束優(yōu)化問題，易陷入局部最優(yōu)。第二類是分解齊次變換矩陣，構(gòu)造非線性逆運動學(xué)方程，求解方程的根獲得逆解。LEE等提出傳統(tǒng)求逆問題可構(gòu)造為一元16次方程求根問題。劉松國等對其求逆解法予以化簡改進，提高了算法精度與速度［4］。錢東海等提出利用旋量法建立機器人運動學(xué)模型，得到6R工業(yè)機器人逆解顯式解析式。呂世增、陳慶誠等［5，6］對其模型與子問題算法予以改進，增強適用性。構(gòu)造非線性方程組求逆解計算效率高，可一次計算獲得運動學(xué)全解，但方程物理意義不明顯，構(gòu)造困難。

本文提出利用物理意義明確的機器人運動學(xué)Denavit-Hartenberg建模方法，簡稱D-H法，獲得一般6R工業(yè)機器人正運動學(xué)位姿矩陣。根據(jù)一般6R工業(yè)機器人最后3個關(guān)節(jié)為旋轉(zhuǎn)關(guān)節(jié)而且軸線相交于一點［7］，其前3個關(guān)節(jié)決定末端執(zhí)行器的位置構(gòu)建為位置相關(guān)非線性方程組，后3個關(guān)節(jié)決定末端執(zhí)行器的姿態(tài)構(gòu)建為姿態(tài)相關(guān)方程組。先后分別以Levenberg-Marquardt方法，簡稱L-M方法，實現(xiàn)最小二乘迭代求解，實現(xiàn)高效率的逆解運算，獲得較高的計算精度。

1　一般6R機器人逆運動學(xué)方程

以某焊接生產(chǎn)線用6R工業(yè)機器人為例，建立D-H坐標(biāo)系如圖1所示［8］。相應(yīng)連桿參數(shù)如表1所示。

表1　6R工業(yè)機器人連桿D-H參數(shù)

圖1　6R工業(yè)機器人D-H坐標(biāo)系定義

表1中任何一個連桿i，兩端有關(guān)節(jié)i和i+1，兩個關(guān)節(jié)軸線沿公垂線的距離αi為連桿長度；αi是在垂直αi的平面內(nèi)兩個軸線的夾角，即連桿扭角；di是沿關(guān)節(jié)i軸線的兩個公垂線的距離；θi是垂直于關(guān)節(jié)i軸線的平面內(nèi)兩公垂線的夾角。

記Tii-1為從第i-1個坐標(biāo)系到第個i坐標(biāo)系的齊次變換矩陣，根據(jù)運動學(xué)正解的定義，6R機器人末端相對基座坐標(biāo)系的位姿矩陣可表示為式（1）。

式中，N=（nx，ny，nz）T為末端執(zhí)行器的法向矢量；O=（ox，oy，oz）T為末端執(zhí)行器的端面矢量；A=（ax，ay，az）T為末端執(zhí)行器的逼近矢量；P=（px，py，pz）T為末端執(zhí)行器的中心位置矢量，均有明確的物理意義。

由式（2）～式（4）可知，位置矢量P=（px，py，pz）T僅與前三個關(guān)節(jié)角有關(guān)，可構(gòu)成變量為θ1、θ2、θ3的三元非線性方程組。前三個關(guān)節(jié)角確定后，由式（5）～式（13）可構(gòu)成求解執(zhí)行器姿態(tài)的三元非線性方程組。

2　L-M方法求非線性方程組逆解

2.1Levenberg-Marquardt方法

機器人D-H法建模獲得運動學(xué)方程為三角函數(shù)相關(guān)非線性方程，不存在求根公式，難以獲得精確數(shù)值解，一般通過逼近求取近似解。為實現(xiàn)機器人高速精確的軌跡控制，求解方法必須有足夠的精度以及速度。機器人末端執(zhí)行器沿預(yù)定軌跡運動時，所需各關(guān)節(jié)角度變量是唯一且應(yīng)平滑過渡，逆解算法需實現(xiàn)該目標(biāo)。

Levenberg-Marquardt方法是由Levenberg（1944）［9］和Marquardt（1963）［10］提出的最小二乘問題求解方法，是對Gauss-Newton法的一種修正。為了防止非線性最小二乘問題求解中Jacobian陣奇異或接近奇異時試探步過長，通過求解方程組：

得到，下一個迭代點xk+1=xk+d。比例系數(shù)μk是一個正數(shù)，當(dāng)μk=0時，L-M方法等同于Gauss-Newton法；當(dāng)μk取值很大，L-M方法接近梯度下降法，μk隨著迭代成功不斷減少。

算法實現(xiàn)步驟：1）初始化給定方程f（x）、給定關(guān)節(jié)角θ初值、步長μk初值、步長放大系數(shù)α、步長縮小系數(shù)β、終止參數(shù)ε。2）求解式（14）獲得d。3）當(dāng)則令并返回2）步。4）令迭代終止，否則返回2）步。實踐證明，L-M方法收斂速度遠優(yōu)于梯度下降法，適用于全局目標(biāo)收斂，收斂位置較為依賴給定的初值。

2.26R機器人軌跡規(guī)劃逆解算法設(shè)計

工業(yè)機器人軌跡規(guī)劃是指以末端執(zhí)行器的的初始位姿和用戶給定的目標(biāo)位姿，按預(yù)定軌跡運動過程中，確定每個關(guān)節(jié)角的位置與速度參數(shù)?；痉譃榈芽柨臻g軌跡規(guī)劃及關(guān)節(jié)空間軌跡規(guī)劃兩類。笛卡爾坐標(biāo)系復(fù)雜軌跡映射入關(guān)節(jié)軌跡空間十分困難，而在笛卡爾空間完成軌跡規(guī)劃，將軌跡輪廓按精度分解為序列坐標(biāo)點，逐一求解各點逆解，或?qū)崿F(xiàn)關(guān)鍵點以外關(guān)節(jié)角插值是一種較簡單的方法。

L-M方法求解精度高，速度快，但算法結(jié)果依賴給定的迭代初值，并且L-M方法是全局收斂算法，無法直接將迭代結(jié)果約束于關(guān)節(jié)角θi的可行域范圍內(nèi)，軌跡逆解結(jié)果易出現(xiàn)不可行的關(guān)節(jié)角。為解決上述問題，設(shè)計軌跡控制位置逆解算法流程圖如圖2所示。

圖2　軌跡規(guī)劃位置逆解算法流程圖

算法比較可行的軌跡首點逆解結(jié)果集tt1與終點逆解結(jié)果集tt2，取距離最短的兩組解。軌跡中間序列點的迭代初值均使用前一點逆解結(jié)果的圓整值，即可控制逆解結(jié)果為系列連續(xù)變化的關(guān)節(jié)角矩陣。

將圖2求得的關(guān)節(jié)角θ1、θ2、θ3代入式（5）～式（13），由機器人指定姿態(tài)的N、O、A向量構(gòu)成姿態(tài)三元非線性方程組，采用類似算法可求變量θ4～θ6。

3　算法MATLAB仿真與分析

為驗證算法有效性，利用MATLAB R2012b軟件，編制軌跡控制逆逆解法程序，使用Intel Core i3 3.2GHz CPU、 4GB內(nèi)存的計算機硬件環(huán)境仿真運行。

為直觀比較預(yù)定軌跡坐標(biāo)與算法逆解結(jié)果差異，設(shè)計軌跡控制目標(biāo)為水平面正弦曲線如圖3所示，星號點為逆解結(jié)果軌跡仿真結(jié)果，圖中可見，設(shè)計軌跡與逆解結(jié)果軌跡各軸坐標(biāo)誤差低于10-7。逆解輸出矩陣?yán)L制關(guān)節(jié)角θ1～θ3變化曲線如圖4所示。

圖3　設(shè)計軌跡與逆解結(jié)果軌跡仿真對比

圖4　逆解關(guān)節(jié)角θ1～θ3變化曲線

設(shè)定首點與終點逆解集上限為10次，求解圖3軌跡900個坐標(biāo)點，重復(fù)執(zhí)行算法5次，設(shè)計軌跡與逆解結(jié)果軌跡三軸坐標(biāo)最大誤差為［3.009e-08，3.41e-13，7.751e-08］，中間插值點逆解經(jīng)3次迭代終止，解算時間如表2

【】【】所示。與表2仿生智能算法相比，解算精度誤差數(shù)量級同小于10-7務(wù)件下，L-M方法的迭代次數(shù)與耗時遠優(yōu)于微分進化逆解算法。

Fast inverse solution algorithm for trajectory planning of 6R robot based on L-M method

ZOU Li-mei， GUO Bo， QIAN Xue-yi

TP24

A

1009-0134（2016）10-0092-03

2016-06-27

福建省教育廳科技項目（JA14317）；福建省教育廳科技項目（JAT160513）

鄒麗梅（1982 -），女，福建南平人，講師，碩士，研究方向為機械電子工程。