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

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

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

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

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

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

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

0　引言

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

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

1　一般6R機(jī)器人逆運(yùn)動(dòng)學(xué)方程

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

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

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

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

記Tii-1為從第i-1個(gè)坐標(biāo)系到第個(gè)i坐標(biāo)系的齊次變換矩陣，根據(jù)運(yùn)動(dòng)學(xué)正解的定義，6R機(jī)器人末端相對(duì)基座坐標(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僅與前三個(gè)關(guān)節(jié)角有關(guān)，可構(gòu)成變量為θ1、θ2、θ3的三元非線性方程組。前三個(gè)關(guān)節(jié)角確定后，由式（5）～式（13）可構(gòu)成求解執(zhí)行器姿態(tài)的三元非線性方程組。

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

2.1Levenberg-Marquardt方法

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

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

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

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

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

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

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

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

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

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

3　算法MATLAB仿真與分析

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

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

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

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

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

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

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

福建省教育廳科技項(xiàng)目（JA14317）；福建省教育廳科技項(xiàng)目（JAT160513）

鄒麗梅（1982 -），女，福建南平人，講師，碩士，研究方向?yàn)闄C(jī)械電子工程。