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

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

改進人工勢場法的機械臂避障路徑規(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 引言

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

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

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

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

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

2 人工勢場法基本原理

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

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

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

移動機器人所受的斥力場定義為：

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

3 機械臂模型

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

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

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

圖1 機械臂D-H模型

圖2 機械臂實物圖

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

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

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

3.2 機械臂4連桿簡化模型

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

圖3 機械臂4連桿簡化模型

將機械臂的末端定義為關(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é)機械臂運動學(xué)模型，可由機械臂當(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在世界坐標系下x，y，z三個軸向上的分量，也就是世界坐標系下的三維直角坐標。通過立體幾何方法，由基座、關(guān)節(jié)2、關(guān)節(jié)4、關(guān)節(jié)5和關(guān)節(jié)7的空間位置求得直線1到直線4的空間解析方程，進一步求解得到空間障礙到各個連桿的距離。

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

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

4.1 機械臂搜索空間

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

本文采用機械臂關(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é)組合作為機械臂下一步的運動目標，在此需要設(shè)置一個搜索步長λ，在此設(shè)置為0.5。

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

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

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

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

4.2 合勢能最速下降的控制標準

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

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

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

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

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

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

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

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

4.3 添加虛擬障礙

4.3.1 局部極小的判斷標準

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

圖4 機械臂障礙點與機械臂位置示意圖

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

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

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

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

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

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

5 仿真實驗

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

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

初始角度S0=(0，0，0，0，0，0)，目標角度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。

局部較小的判斷標準：運行10次的角度絕對值之差的和小于1.5。

實驗1未添加虛擬障礙

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

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

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

實驗2添加虛擬障礙

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

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

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

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

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

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

6 結(jié)論

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

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

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

[2]朱大奇，顏明重.移動機器人路徑規(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.機器人學(xué)導(dǎo)論——分析、系統(tǒng)及應(yīng)用[M].孫富春，朱繼紅，劉國棟，譯.北京：電子工業(yè)出版社，2004.

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

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

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

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

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

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

改進人工勢場法；合勢能最速下降；機械臂；路徑規(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)域為先進控制理論及應(yīng)用、機械臂路徑規(guī)劃與運動控制；張國良（1970—），男，博士，教授，主要研究領(lǐng)域為先進控制理論及應(yīng)用；羊帆（1984—），男，碩士，主要研究領(lǐng)域為先進控制理論及應(yīng)用、機器人視覺問題；敬斌（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