王俊龍，張國良，羊帆，敬斌

WANG Junlong1,ZHANG Guoliang1,YANG Fan2,JING Bin1

1.第二炮兵工程大學(xué)301教研室，西安 710025

2.寶雞市150信箱11分箱23號

1.Τeaching and Research Office 301,Τhe Second Artillery Engineering University,Xi’an 710025,China

2.No.23,11 Cases,P.O.Box 150,Baoji

改進(jìn)人工勢場法的機(jī)械臂避障路徑規(guī)劃

王俊龍1，張國良1，羊帆2，敬斌1

WANG Junlong1,ZHANG Guoliang1,YANG Fan2,JING Bin1

1.第二炮兵工程大學(xué)301教研室，西安 710025

2.寶雞市150信箱11分箱23號

1.Τeaching and Research Office 301,Τhe Second Artillery Engineering University,Xi’an 710025,China

2.No.23,11 Cases,P.O.Box 150,Baoji

1 引言

機(jī)械臂的避障路徑規(guī)劃是指在有障礙的環(huán)境下，機(jī)械臂按照給定的某一種方法找到從初始狀態(tài)到末狀態(tài)的無碰撞的運動軌跡。機(jī)械臂常見的路徑規(guī)劃方法有插值法等，典型的機(jī)器人路徑規(guī)劃方法有人工勢場法、柵格法、A*算法、D*算法[1-3]。

機(jī)械臂路徑規(guī)劃的傳統(tǒng)方法是利用機(jī)械臂末端在空間中進(jìn)行規(guī)劃，爾后通過關(guān)節(jié)之間的約束分解得到各個關(guān)節(jié)的運動軌跡。但是這種方法只適用于具有特定結(jié)構(gòu)的機(jī)械臂，即能夠利用解析方法，利用末端位姿，通過運動學(xué)反解，得到各關(guān)節(jié)的角度值的封閉解[4-6]。然而由于機(jī)械臂自身多關(guān)節(jié)、強(qiáng)耦合的特點，即使能夠通過解析法機(jī)型運動學(xué)反解，也存在反解計算量大，甚至得不到與當(dāng)前角度連續(xù)的角度值的問題，即在機(jī)械臂末端變動不大的情況下，反解出來的關(guān)節(jié)角度不連續(xù)，產(chǎn)生奇異解問題[4]。

本文所采用的機(jī)械臂并不具有這種特殊結(jié)構(gòu)，不能夠運用解析的方法進(jìn)行機(jī)械臂運動學(xué)反解，更不能夠?qū)崿F(xiàn)基于傳統(tǒng)方法機(jī)械臂的避障路徑規(guī)劃。

在目標(biāo)點與障礙已知的情況下，可以利用遺傳算法等方法求解得到一組關(guān)節(jié)值，使得在此關(guān)節(jié)值組合下，機(jī)械臂末端達(dá)到目標(biāo)，同時機(jī)械臂遠(yuǎn)離空間障礙，實現(xiàn)機(jī)械臂的反解。

本文提出在得到一組合理的關(guān)節(jié)值后，在關(guān)節(jié)空間內(nèi)進(jìn)行機(jī)械臂路徑規(guī)劃，保證機(jī)械臂在運動過程中，末端朝向目標(biāo)點，最終到達(dá)目標(biāo)，同時避開障礙。本文改進(jìn)人工勢場法的運動控制標(biāo)準(zhǔn)，在機(jī)械臂運動過程中，以機(jī)械臂所受到的引力勢能與斥力勢能的合勢能最速下降為標(biāo)準(zhǔn)控制機(jī)械臂運動。同時提出針對人工勢場法局部極小的問題，本文采用在局部極小附近添加空間虛擬障礙、利用虛擬障礙所產(chǎn)生的虛擬斥力勢能將機(jī)械臂推離局部極小，繞過障礙，到達(dá)目標(biāo)。

2 人工勢場法基本原理

人工勢場法[7-8]是由Khati提出的一種局部路徑規(guī)劃方法，其基本思想是將移動機(jī)器人在環(huán)境中運動視為一種機(jī)器人在虛擬的人工受力場中運動，障礙物對移動機(jī)器人產(chǎn)生斥力，目標(biāo)點對移動機(jī)器人產(chǎn)生引力，斥力和引力的合力作為控制移動機(jī)器人向目標(biāo)點運動的力，由于目標(biāo)點被設(shè)計為合力的全局最小點，因此在合力的作用下，理論上移動機(jī)器人可以到達(dá)目標(biāo)點。

假設(shè)移動機(jī)器人當(dāng)前時刻的位置X=[xy]Τ，目標(biāo)點位置Xgoal=[xgoalygoal]Τ，距離移動機(jī)器人最近的障礙物位置Xobs=[xobsyobs]Τ，則移動機(jī)器人所受的引力場被定義為：

式中ξ為正比例系數(shù)；ρ(X，Xgoal)=||X-Xgoal||為機(jī)器人X到目標(biāo)Xgoal的距離。

移動機(jī)器人所受的斥力場定義為：

式中η為正比例系數(shù)；ρ(X，Xobs)為移動機(jī)器人到障礙物的距離；ρ0為不影響移動機(jī)器人安全運動的最小安全距離。

3 機(jī)械臂模型

3.1 機(jī)械臂運動學(xué)模型

本文采用標(biāo)準(zhǔn)D-H法[9]建立機(jī)械臂運動學(xué)模型，結(jié)果如下：

圖1為D-H模型圖，圖2為實物圖，表1為模型參數(shù)說明。

圖1 機(jī)械臂D-H模型

圖2 機(jī)械臂實物圖

通過運動學(xué)正解的方法可以得到在當(dāng)前關(guān)節(jié)角下所對應(yīng)的末端位置。通過求解可以發(fā)現(xiàn)，機(jī)械臂末端位置是關(guān)節(jié)1～5的角度的函數(shù)，而與關(guān)節(jié)6無關(guān)，關(guān)節(jié)6只影響末端的姿態(tài)。

表1 機(jī)械臂D-H模型參數(shù)

本文僅考慮機(jī)械臂的末端位置，因而以機(jī)械臂關(guān)節(jié)1～5的關(guān)節(jié)角度為控制目標(biāo)，不考慮關(guān)節(jié)6角度的影響。

3.2 機(jī)械臂4連桿簡化模型

機(jī)械臂不同于自主移動機(jī)器人，在應(yīng)用人工勢場法時不可看做質(zhì)點，必須考慮障礙物到機(jī)械臂各關(guān)節(jié)連桿的距離。對六自由機(jī)械臂建立簡化4連桿模型如圖3所示。

圖3 機(jī)械臂4連桿簡化模型

將機(jī)械臂的末端定義為關(guān)節(jié)7。其中：

基座、關(guān)節(jié)1、關(guān)節(jié)2構(gòu)成一條線段，定義為直線1；關(guān)節(jié)2、關(guān)節(jié)3、關(guān)節(jié)4構(gòu)成一條線段，定義為直線2；關(guān)節(jié)4、關(guān)節(jié)5構(gòu)成一條線段，定義為直線3；關(guān)節(jié)5、關(guān)節(jié)6、關(guān)節(jié)7構(gòu)成一條線段，定義為直線4。

通過第3.1節(jié)機(jī)械臂運動學(xué)模型，可由機(jī)械臂當(dāng)前關(guān)節(jié)角組合S=(θi1，θi2，…，θi6)求解出基座、關(guān)節(jié)2、關(guān)節(jié)4、關(guān)節(jié)5和關(guān)節(jié)7的空間位置，方法如下：

基座：

上式中，pix，piy，piz分別表示關(guān)節(jié)i在世界坐標(biāo)系下x，y，z三個軸向上的分量，也就是世界坐標(biāo)系下的三維直角坐標(biāo)。通過立體幾何方法，由基座、關(guān)節(jié)2、關(guān)節(jié)4、關(guān)節(jié)5和關(guān)節(jié)7的空間位置求得直線1到直線4的空間解析方程，進(jìn)一步求解得到空間障礙到各個連桿的距離。

4 改進(jìn)人工勢場法的機(jī)械臂路徑規(guī)劃

改進(jìn)的人工勢場法以機(jī)械臂各關(guān)節(jié)角度作為控制和運算的對象。在機(jī)械臂由當(dāng)前關(guān)節(jié)角度運動到目標(biāo)關(guān)節(jié)角度的過程中，實現(xiàn)機(jī)械臂的避障路徑規(guī)劃，保證末端接近目標(biāo)位置，機(jī)械臂避開障礙。

4.1 機(jī)械臂搜索空間

機(jī)械臂的運動并不像自主移動機(jī)器人一樣可以在空間范圍內(nèi)自主向各個方向方便地移動，機(jī)械臂由于其自身多關(guān)節(jié)、強(qiáng)耦合的特點，任何一個關(guān)節(jié)運動都會導(dǎo)致其后關(guān)節(jié)連桿的位置變化。按照傳統(tǒng)人工勢場法的思想，必須依次求得各關(guān)節(jié)所受到的力，然后得到一個合力，決定機(jī)械臂的移動方向。然而世界坐標(biāo)空間的運動控制必須經(jīng)過運動學(xué)反解轉(zhuǎn)換到關(guān)節(jié)坐標(biāo)系，得到各個關(guān)節(jié)轉(zhuǎn)動的角度值，這將會使得機(jī)械臂頻繁地進(jìn)行反解運算，不僅計算的難度和計算量很大，更有可能得不到前后連續(xù)的關(guān)節(jié)角度值，即奇異解，這在實際應(yīng)用中是不允許出現(xiàn)的。

本文采用機(jī)械臂關(guān)節(jié)空間作為人工勢場法的搜索空間，基本思路為：以當(dāng)前關(guān)節(jié)狀態(tài)S=(θi1，θi2，…，θi5)為初態(tài)，遍歷當(dāng)前狀態(tài)的相鄰關(guān)節(jié)值，求取出各種關(guān)節(jié)組合所對應(yīng)的合勢能，選擇合適的關(guān)節(jié)組合作為機(jī)械臂下一步的運動目標(biāo)，在此需要設(shè)置一個搜索步長λ，在此設(shè)置為0.5。

以關(guān)節(jié)空間作為搜索空間有以下幾個優(yōu)點：

（1）關(guān)節(jié)空間搜索必連續(xù)，不會出現(xiàn)奇異點。

（2）由關(guān)節(jié)空間通過正解方法得到的機(jī)械臂路徑必可達(dá)。

（3）從關(guān)節(jié)坐標(biāo)系到世界坐標(biāo)系的轉(zhuǎn)換計算難度和計算強(qiáng)度遠(yuǎn)低于世界坐標(biāo)系到關(guān)節(jié)坐標(biāo)系。

4.2 合勢能最速下降的控制標(biāo)準(zhǔn)

在第4.1節(jié)的分析中可以發(fā)現(xiàn)，采用傳統(tǒng)人工勢場法產(chǎn)生一個合力來控制機(jī)械臂運動的方法是行不通的。

通過分析可以發(fā)現(xiàn)，引力勢函數(shù)與斥力勢函數(shù)有這樣的性質(zhì)：

（1）引力勢函數(shù)在整個空間內(nèi)都是有效的，沒有作用范圍的限制；斥力勢函數(shù)僅在斥力場內(nèi)有效。

（2）機(jī)械臂引力勢能可以通過機(jī)械臂當(dāng)前關(guān)節(jié)角度和目標(biāo)角度的差求??；機(jī)械臂斥力勢能可以通過機(jī)械臂到障礙物的距離計算。

（3）引力勢能總是大于等于0的；斥力勢能在其作用范圍內(nèi)也是大于等于0的；當(dāng)機(jī)械臂到達(dá)目標(biāo)點后，引力勢能與斥力勢能同時為零。

機(jī)械臂運動的目標(biāo)應(yīng)是使機(jī)械臂所受到的引力能與斥力勢能的合勢能減小，當(dāng)機(jī)械臂到達(dá)目標(biāo)點后，引力勢能與斥力勢能同時為零。若機(jī)械臂停止運動，而所受到的合勢能不為零，說明機(jī)械臂還未到達(dá)目標(biāo)，機(jī)械臂陷入局部極小。

以機(jī)械臂六個關(guān)節(jié)角度最終位置為目標(biāo)，求解得到機(jī)械臂所受的引力勢能和斥力勢能：

其中Eatt、Erep和Ei分別表示機(jī)械臂所受引力勢能、斥力勢能和各關(guān)節(jié)連桿所受的斥力勢能；ka和kr分別表示所受引力和斥力的比例系數(shù)；di和d0分別表示各關(guān)節(jié)到障礙物的距離和障礙物的斥力影響范圍。

4.3 添加虛擬障礙

4.3.1 局部極小的判斷標(biāo)準(zhǔn)

人工勢場法屬于局部路徑規(guī)劃，只能考慮局部障礙信息，容易陷入局部極小。在機(jī)械臂的應(yīng)用中，以勢能的變化為控制標(biāo)準(zhǔn)，局部極小的表現(xiàn)為：在機(jī)械臂進(jìn)行關(guān)節(jié)空間搜索時，其中的一個角度使得機(jī)械臂的引力勢能與斥力勢能合勢能達(dá)到一個極小點，任何角度的變化會使得兩者之和增大，機(jī)械臂不會繼續(xù)有關(guān)節(jié)角度的變化，停止運動。若當(dāng)前關(guān)節(jié)不是機(jī)械臂運動的目標(biāo)角度值，此時機(jī)械臂陷入當(dāng)前關(guān)節(jié)局部極小。

圖4 機(jī)械臂障礙點與機(jī)械臂位置示意圖

局部極小的判斷可以通過以下方式進(jìn)行：判斷當(dāng)前關(guān)節(jié)值與n次之前的角度值絕對值之差，若兩者的差值在一個很小的范圍，表示機(jī)械臂在一個關(guān)節(jié)角度停止不動或者震蕩，則可以認(rèn)為機(jī)械臂陷入了局部極小。

4.3.2 虛擬障礙的位置與基本參數(shù)

文獻(xiàn)[10]提出一種針對二維平面移動機(jī)器人局部極小問題添加虛擬障礙的方法。受其啟發(fā)，本文針對機(jī)械臂局部極小問題，采用在局部極小附近添加空間虛擬障礙，利用虛擬障礙產(chǎn)生的斥力勢能將機(jī)械臂推出局部極小。

如圖4所示虛擬障礙設(shè)置于：虛擬障礙點與機(jī)械臂到障礙點最近位置P的連線垂直于平面X：由當(dāng)前角度下關(guān)節(jié)B、關(guān)節(jié)C所在位置與目標(biāo)角度下關(guān)節(jié)C所在的位置確定。虛擬障礙點到點P的距離可以根據(jù)情況設(shè)置為一個合適的值。

為了使虛擬障礙在局部極小附近成為主要作用場，保證機(jī)械臂脫離局部極小，通常設(shè)定虛擬障礙點的斥力系數(shù)為一般障礙的1～1 000倍、作用范圍為一般障礙物的1.5～3倍[7]。

本文設(shè)置虛擬障礙到點P的距離為機(jī)械臂的安全距離d0，設(shè)置斥力為普通障礙物的10倍，斥力范圍為普通障礙的1.5倍。

5 仿真實驗

為了說明所提出方法的有效性，進(jìn)行了仿真實驗。實驗從當(dāng)前的角度運動到目標(biāo)的角度，在運動過程中，避開障礙，當(dāng)機(jī)械臂成功到達(dá)目標(biāo)角度后，說明機(jī)械臂成功避開障礙，末端到達(dá)目標(biāo)點。

設(shè)定機(jī)械臂運動控制參數(shù)如下：

初始角度S0=(0，0，0，0，0，0)，目標(biāo)角度SE=(45，-45，0，45，0，0)，障礙位置PObj=(25，25，430)，運行步長λ=0.5°，安全距離dsafe=10，引力系數(shù)ka=30，斥力范圍d0=40，斥力系數(shù)kr=50，虛擬障礙斥力系數(shù)kνr=500，虛擬障礙范圍dν0=60。

局部較小的判斷標(biāo)準(zhǔn)：運行10次的角度絕對值之差的和小于1.5。

實驗1未添加虛擬障礙

未添加虛擬障礙時，機(jī)械臂會陷入到局部極小。結(jié)果如圖5所示。

圖5 未添加虛擬障礙機(jī)械臂關(guān)節(jié)運行圖

在未添加虛擬障礙時，機(jī)械臂陷入局部較小，停止在S=(45，-2.5，0，45，0，0)的關(guān)節(jié)角組合上。

實驗2添加虛擬障礙

實驗結(jié)果如圖6～圖8所示。

圖6 斥力勢能、引力勢能、未添加虛擬勢能合勢能變化圖

由圖6、圖7可以看出，當(dāng)機(jī)械臂在進(jìn)入到局部極小之前，機(jī)械臂的合勢能始終下降，機(jī)械臂的關(guān)節(jié)角度不斷變化，機(jī)械臂處于在合勢能最速下降的控制下運動。

當(dāng)運行到第99步，機(jī)械臂利用局部極小判斷準(zhǔn)則，判斷出陷入局部極小。此時添加虛擬障礙。通過虛擬障礙產(chǎn)生的虛擬勢能，使得機(jī)械臂脫離局部極小，最后機(jī)械臂受到的合勢能減小為零，機(jī)械臂到達(dá)目標(biāo)。

圖7 虛擬勢能、添加虛擬勢能合勢能變化圖

圖8 關(guān)節(jié)位置軌跡圖

6 結(jié)論

改進(jìn)人工勢場法在關(guān)節(jié)空間內(nèi)、以機(jī)械臂所受的引力勢能與斥力勢能的合勢能最速下降為標(biāo)準(zhǔn)，控制機(jī)械臂運動的方法，可以有效解決簡單障礙環(huán)境下機(jī)械臂避障路徑規(guī)劃的問題。

該方法主要是針對關(guān)節(jié)空間內(nèi)進(jìn)行，以角度為運算對象，存在的主要不足之處在于該方法的實現(xiàn)必須在通過其他的方法得到一組合理的關(guān)節(jié)角度解后，以該組角度解作為目標(biāo)，進(jìn)而實現(xiàn)避障路徑規(guī)劃。下一步的努力方向在于拓展該方法的適應(yīng)性，降低該方法對其他方法的依賴。

[1]李愛萍，李元宗.機(jī)器人路徑規(guī)劃方法的研究[J].機(jī)械工程與自動化，2009（10）：194-197.

[2]朱大奇，顏明重.移動機(jī)器人路徑規(guī)劃技術(shù)綜述[J].控制與決策，2010（7）：961-967.

[3]Gerke M.Genetic path planning for mobile robots[C]//Proc of American Control Conference，San Diego，CA，USA，1999：596-601.

[4]Niku S B.機(jī)器人學(xué)導(dǎo)論——分析、系統(tǒng)及應(yīng)用[M].孫富春，朱繼紅，劉國棟，譯.北京：電子工業(yè)出版社，2004.

[5]余達(dá)太，馬香峰.工業(yè)機(jī)器人應(yīng)用工程[M].北京：冶金工業(yè)出版社，2001.

[6]吳振彪，王正家.工業(yè)機(jī)器人[M].2版.武漢：華中科技大學(xué)出版社，2006.

[7]劉春陽，程億強(qiáng)，柳長安.基于改進(jìn)勢場法的移動機(jī)器人避障路徑規(guī)劃[J].東南大學(xué)學(xué)報，2009（9）：116-120.

[8]張建英，劉暾.基于人工勢場法的移動機(jī)器人最優(yōu)路徑規(guī)劃[J].東南大學(xué)學(xué)報，2007（8）：183-188.

[9]李宏慶.基于視覺的六自由機(jī)械臂控制技術(shù)研究[D].南京：南京理工大學(xué)，2009.

[10]黃健生.移動機(jī)器人的路徑規(guī)劃研究[D].杭州：浙江大學(xué)，2008.

Aiming at the shortcoming of traditional method of path planning using merely on typical structure of manipulator,after the result of using a third way as genetic algorithm to get an ideal data,a new method of improved artificial method with combined potential energy steepest descent based on arthrosis space is proposed.Using the method of adding virtual obstacle in the minimum field to produce virtual potential energy in solving the problem of local minimum,the manipulator successfully achieves the goal avoiding the obstacle.Τhe simulation results show that the proposed method is valid and effective in path planning of manipulator.

improved artificial field method;combined potential energy steepest descent;manipulator;path planning;virtual obstacle

針對傳統(tǒng)機(jī)械臂路徑規(guī)劃只適用于具有特殊構(gòu)型的機(jī)械臂這一不足，提出在利用遺傳算法等方法求解得到一組合理的關(guān)節(jié)值后，運用人工勢場法在機(jī)械臂關(guān)節(jié)空間內(nèi)搜索、采用合勢能最速下降為控制標(biāo)準(zhǔn)的機(jī)械臂路徑規(guī)劃方法。同時利用在局部極小附近添加虛擬障礙，產(chǎn)生虛擬斥力勢能的方法解決了局部極小問題，使機(jī)械臂成功繞過障礙到達(dá)目標(biāo)。仿真驗證了該方法的正確性和有效性。

改進(jìn)人工勢場法；合勢能最速下降；機(jī)械臂；路徑規(guī)劃；虛擬障礙

A

ΤP242

10.3778/j.issn.1002-8331.1201-0360

WANG Junlong,ZHANG Guoliang,YANG Fan,et al.Improved artificial field method on obstacle avoidance path planning for manipulator.Computer Engineering and Applications,2013,49（21）：266-270.

王俊龍（1987—），男，碩士，主要研究領(lǐng)域為先進(jìn)控制理論及應(yīng)用、機(jī)械臂路徑規(guī)劃與運動控制；張國良（1970—），男，博士，教授，主要研究領(lǐng)域為先進(jìn)控制理論及應(yīng)用；羊帆（1984—），男，碩士，主要研究領(lǐng)域為先進(jìn)控制理論及應(yīng)用、機(jī)器人視覺問題；敬斌（1976—），男，講師，主要研究領(lǐng)域為多傳感器信息融合、模式識別。E-mail：junlong87@126.com

2012-01-19

2012-03-05

1002-8331（2013）21-0266-05

CNKI出版日期：2012-06-29http://www.cnki.net/kcms/detail/11.2127.ΤP.20120629.1645.001.html