曹夢(mèng)磊， 王宏倫， 梁 宵

(北京航空航天大學(xué)，a.飛行器控制一體化技術(shù)重點(diǎn)實(shí)驗(yàn)室; b.無(wú)人駕駛飛行器設(shè)計(jì)研究所，北京 100191)

0 引言

航路規(guī)劃技術(shù)作為無(wú)人機(jī)的關(guān)鍵技術(shù)之一，一直是各國(guó)的研究熱點(diǎn)。國(guó)內(nèi)外學(xué)者提出了各種航路規(guī)劃算法，主要可分為基于圖形法、決策型搜索算法、隨機(jī)型搜索算法和人工勢(shì)場(chǎng)法。基于圖形的方法必須表示出規(guī)劃空間中的所有可能的路徑，否則可能丟失最優(yōu)解，不適用于復(fù)雜地形環(huán)境。決策型搜索算法中具有代表性的是A*算法，該算法簡(jiǎn)單易實(shí)現(xiàn)，搜索效率較高，但存在指數(shù)爆炸問(wèn)題，規(guī)劃時(shí)間隨規(guī)劃空間的增大而呈指數(shù)增長(zhǎng)，并且對(duì)啟發(fā)函數(shù)的依賴性強(qiáng)，而啟發(fā)函數(shù)的選擇沒(méi)有通用的方法［1］。隨機(jī)型搜索算法包括遺傳算法、蟻群算法、粒子群算法等一些仿生算法，它們魯棒性好且具有很強(qiáng)的并行性，但蟻群算法容易陷入局部最優(yōu)［2］，遺傳算法理論上可以求出全局最優(yōu)解，但解的最優(yōu)性和求解時(shí)間存在矛盾，粒子群算法是全局最優(yōu)化算法但其參數(shù)選擇具有偶然性，缺乏理論指導(dǎo)［3］。人工勢(shì)場(chǎng)法不同于搜索算法，它將物體的運(yùn)動(dòng)看成是吸引力和排斥力作用的結(jié)果，該方法的優(yōu)點(diǎn)是構(gòu)造簡(jiǎn)單、規(guī)劃速度快，但其存在局部極小點(diǎn)，易陷入局部最優(yōu)解［4］。

流函數(shù)法是近年來(lái)航路規(guī)劃中興起的一種新算法，其在環(huán)境、冶金等領(lǐng)域的研究中使用的比較早，這種方法通過(guò)借鑒流體力學(xué)概念建立勢(shì)場(chǎng)區(qū)域，能夠很好地避免局部最小問(wèn)題［5］。加州理工大學(xué)的Stephen Waydo和 Richard M Murray［5］率先利用流函數(shù)方法解決了單個(gè)障礙物以及障礙物移動(dòng)時(shí)的路徑規(guī)劃問(wèn)題，康奈爾大學(xué)的Jeff Sullivan［6］等人提出采用附加門限的流函數(shù)方法進(jìn)行多個(gè)障礙物時(shí)的路徑規(guī)劃，波士頓大學(xué)的Guohua Ye等人［7］將旋度場(chǎng)引入到存在障礙物的原勢(shì)場(chǎng)中來(lái)解決流函數(shù)的停滯點(diǎn)問(wèn)題，但該方法較復(fù)雜且使路徑有突變影響了平滑性。美國(guó)奧本大學(xué)的Robert Daily 和 David M Bevly［8］通過(guò)對(duì)單個(gè)障礙物存在時(shí)的流函數(shù)進(jìn)行加權(quán)求和解決了兩個(gè)障礙物接觸時(shí)的路徑規(guī)劃問(wèn)題。

目前，流函數(shù)法作為一種新興的航路規(guī)劃算法已進(jìn)行了一定的研究，但仍存在一些問(wèn)題還沒(méi)有很好地解決。本文針對(duì)流函數(shù)法在無(wú)人機(jī)航路規(guī)劃中的不足，提出了一種解決多障礙物重疊問(wèn)題并結(jié)合無(wú)人機(jī)機(jī)動(dòng)約束的改進(jìn)流函數(shù)航路規(guī)劃方法，并在復(fù)雜地形環(huán)境下進(jìn)行了仿真驗(yàn)證。

1 流函數(shù)法航路規(guī)劃

1.1 流函數(shù)法總體思路

流函數(shù)法的總體思路為:在規(guī)劃空間中引入某種流動(dòng)的流場(chǎng)，由流體力學(xué)知識(shí)可得其速度勢(shì)，然后在原流場(chǎng)中加入障礙物，通過(guò)建立障礙物的勢(shì)場(chǎng)并與原勢(shì)場(chǎng)疊加可以得到總的勢(shì)場(chǎng)，對(duì)速度勢(shì)求導(dǎo)即得流場(chǎng)中各處的流速，對(duì)流速積分所得流體的流線就是為無(wú)人機(jī)規(guī)劃出的航路，如圖1所示。

流函數(shù)法模擬水流的機(jī)理，使無(wú)人機(jī)能夠像流水避開(kāi)溪石一樣繞過(guò)障礙物，規(guī)劃出的航路較平滑，并且能夠很好地避免局部最小問(wèn)題。

1.2 流函數(shù)理論基礎(chǔ)

1.2.1 基本概念［9］

速度勢(shì):如果理想流體在某一源點(diǎn)是無(wú)旋流，則該流體在源點(diǎn)周圍也是無(wú)旋流，即在該流體中，漩渦向量ω始終為0(ω=▽×uf=0)，又對(duì)任意標(biāo)量函數(shù)φ，有▽·▽?duì)?0所以滿足無(wú)旋性的條件，可以選擇uf=▽·φ，φ稱為速度勢(shì)，而該無(wú)旋流為勢(shì)流。流速uf可以表示為則有

流函數(shù):在xy平面，流函數(shù)ψ給出了流體速度在x和y方向上的兩個(gè)分量u，v即

復(fù)勢(shì):二維無(wú)旋流的復(fù)勢(shì)ω定義成ω=φ+iψ，其中φ和ψ分別為速度勢(shì)和流函數(shù)。

1.2.2 基本定理［9］

圓定理:在 z平面內(nèi)有二維無(wú)旋流，其復(fù)勢(shì)為f(z)，f(z)的奇點(diǎn)離b的距離均大于a，如果一個(gè)圓柱體被引入這個(gè)流場(chǎng)后，則復(fù)勢(shì)變?yōu)?/p>

1.3 流函數(shù)法進(jìn)行航路規(guī)劃

利用流函數(shù)法進(jìn)行航路規(guī)劃有以下4個(gè)步驟。

1)在規(guī)劃空間內(nèi)建立坐標(biāo)系，定義起始點(diǎn)和目標(biāo)點(diǎn)。

2)在目標(biāo)點(diǎn)處加入流動(dòng)——匯，其復(fù)勢(shì)為

其中:C為流場(chǎng)的強(qiáng)度。則流函數(shù)為

其在x、y方向的速度分量為

3)加入障礙物的坐標(biāo)和半徑大小，根據(jù)圓定理可得流場(chǎng)的復(fù)勢(shì)為

其中:b=bx+iby，(bx，by)為障礙物的坐標(biāo);a為障礙物半徑。則流函數(shù)為

其在x，y方向的速度分量為

其中:m=a2(y－by)+by［(x－bx)2+(y－by)2］;n=a2·(x－bx)+bx［(x－bx)2+(y－by)2］。

4)多個(gè)障礙物時(shí)可采用文獻(xiàn)［3］中對(duì)速度求加權(quán)和的方法。

式中:ui，vi為第 i個(gè)障礙物在 x，y方向的速度;αi為加權(quán)系數(shù)，其計(jì)算公式為

利用流函數(shù)法在多個(gè)障礙物時(shí)的路徑規(guī)劃結(jié)果如圖2所示。

圖2 流函數(shù)法路徑規(guī)劃圖Fig.2 The path planned by stream function method

2 基于改進(jìn)流函數(shù)的航路規(guī)劃算法

2.1 虛擬目標(biāo)法解決障礙物重疊情形

按照以上所述流函數(shù)法進(jìn)行航路規(guī)劃在多個(gè)障礙物分離時(shí)能夠規(guī)劃出航路，但當(dāng)有兩個(gè)障礙物重疊(例如復(fù)雜地形中山峰重疊的情形)時(shí)流線有可能穿過(guò)障礙物內(nèi)部無(wú)法規(guī)劃出航路，如圖3所示。

圖3 障礙物重疊時(shí)流函數(shù)法規(guī)劃圖Fig.3 The path planned by stream function in the situation of obstacle superposition

障礙物重疊時(shí)出現(xiàn)穿越的區(qū)域見(jiàn)圖4。

圖4 障礙物重疊時(shí)出現(xiàn)穿越的區(qū)域Fig.4 Dangerous zone in the situation of obstacle superposition

通過(guò)分析流函數(shù)的機(jī)理可知出現(xiàn)穿越的原因是:圖4中障礙物1作用于無(wú)人機(jī)使其繞過(guò)障礙物1的勢(shì)場(chǎng)，與障礙物2作用于無(wú)人機(jī)使其繞過(guò)障礙物2的勢(shì)場(chǎng)強(qiáng)度相當(dāng)，這樣在一定的區(qū)域里兩個(gè)勢(shì)場(chǎng)疊加后互相抵消一部分，最后剩下原流場(chǎng)中匯的勢(shì)場(chǎng)起主要作用，所以形成穿越障礙物的情況。如圖4所示將出現(xiàn)穿越的區(qū)域定義為危險(xiǎn)區(qū)域。

針對(duì)以上穿越問(wèn)題，本文提出虛擬目標(biāo)法來(lái)引導(dǎo)無(wú)人機(jī)繞過(guò)障礙物，總體思路為:在流線穿越障礙物內(nèi)部之前預(yù)先判斷出危險(xiǎn)區(qū)域，這時(shí)可以將目標(biāo)點(diǎn)移動(dòng)到障礙物的邊沿(如圖4的T1，T2)，這樣就會(huì)引導(dǎo)流線沿障礙物邊沿流動(dòng)，當(dāng)離開(kāi)危險(xiǎn)區(qū)域后，將目標(biāo)點(diǎn)恢復(fù)到原來(lái)位置。具體步驟如圖5所示。

圖5 障礙物重疊時(shí)航路規(guī)劃步驟Fig.5 Route planning process in the situation of obstacle superposition

采用以上改進(jìn)的流函數(shù)法后對(duì)兩個(gè)障礙物重疊情況規(guī)劃出的路徑如圖6所示。

圖6 改進(jìn)流函數(shù)法解決障礙物重疊情形Fig.6 The path planned by improved stream function method in the situation of obstacle superposition

圖6 中，首先判斷出無(wú)人機(jī)前面的障礙物出現(xiàn)重疊情況，然后計(jì)算起始點(diǎn)與目標(biāo)點(diǎn)連線的斜率，判斷出無(wú)人機(jī)進(jìn)入危險(xiǎn)區(qū)域，通過(guò)計(jì)算無(wú)人機(jī)起始點(diǎn)距兩個(gè)障礙物的距離可知應(yīng)將虛擬目標(biāo)設(shè)在障礙物的上邊沿(如圖4中的T1)，這樣能夠使得規(guī)劃的航路較短并且曲率較小。經(jīng)過(guò)改進(jìn)后的流函數(shù)法進(jìn)行航路規(guī)劃就避免了圖3中的穿越障礙物內(nèi)部的問(wèn)題。

2.2 虛擬障礙法加入無(wú)人機(jī)機(jī)動(dòng)約束

利用流函數(shù)法進(jìn)行航路規(guī)劃的優(yōu)點(diǎn)之一就是規(guī)劃出的航路較平滑，比較適合無(wú)人機(jī)的飛行。但當(dāng)無(wú)人機(jī)當(dāng)前位置與目標(biāo)點(diǎn)連線經(jīng)過(guò)障礙物圓心時(shí)會(huì)產(chǎn)生停滯點(diǎn)問(wèn)題，即使采用隨機(jī)走動(dòng)法使無(wú)人機(jī)離開(kāi)停滯點(diǎn)，也存在規(guī)劃的航路曲率較大，不能滿足無(wú)人機(jī)自身的機(jī)動(dòng)約束條件，如圖7中所示。

圖7 停滯點(diǎn)問(wèn)題與隨機(jī)走動(dòng)法示意圖Fig.7 Illustration of stagnation problem and random motion method

本文通過(guò)將無(wú)人機(jī)的機(jī)動(dòng)約束轉(zhuǎn)化為虛擬障礙來(lái)滿足無(wú)人機(jī)的機(jī)動(dòng)要求，同時(shí)也解決了停滯點(diǎn)問(wèn)題。虛擬障礙的設(shè)置方法如圖8所示。

圖8 虛擬障礙法加入機(jī)動(dòng)約束示意圖Fig.8 Illustration of adding maneuver constraints by virtual obstacle method

首先，以起始點(diǎn)和目標(biāo)點(diǎn)連線為x′軸，建立新的坐標(biāo)系x′y′，如圖8所示相對(duì)于原坐標(biāo)系順時(shí)針旋轉(zhuǎn)了α，再根據(jù)無(wú)人機(jī)自身的機(jī)動(dòng)約束條件(主要考慮無(wú)人機(jī)的最大轉(zhuǎn)彎角，假設(shè)為60°)，過(guò)B點(diǎn)作真實(shí)障礙物的相切圓交x′軸于點(diǎn)A和C，并且使相切圓在C點(diǎn)的切線與x′軸夾角正好為無(wú)人機(jī)的最大轉(zhuǎn)彎角θ=60°，所作的包含障礙圓的相切圓就是設(shè)置的虛擬障礙。設(shè)虛擬圓的半徑為R，真實(shí)障礙圓的半徑為r，根據(jù)幾何關(guān)系

可計(jì)算出虛擬障礙圓的半徑R=2r。再由O1O2可得圓心O2在坐標(biāo)系x′y′中坐標(biāo)，經(jīng)過(guò)坐標(biāo)變換

可得其在xy坐標(biāo)系中的坐標(biāo)。

以設(shè)置的虛擬障礙物應(yīng)用流函數(shù)法規(guī)劃出的航路水平轉(zhuǎn)彎角肯定小于60°，滿足無(wú)人機(jī)的機(jī)動(dòng)要求，并且避免了停滯點(diǎn)問(wèn)題，其規(guī)劃出的航路與傳統(tǒng)流函數(shù)法、隨機(jī)走動(dòng)法解決停滯點(diǎn)問(wèn)題所規(guī)劃出的航路對(duì)比如圖9所示。

圖9 虛擬障礙法加入機(jī)動(dòng)約束Fig.9 The path planned after adding maneuver constraints by virtual obstacle method

由圖9可以看出由于無(wú)人機(jī)當(dāng)前位置與目標(biāo)點(diǎn)連線經(jīng)過(guò)障礙物圓心，傳統(tǒng)流函數(shù)法由于停滯點(diǎn)問(wèn)題而不能到達(dá)目標(biāo)點(diǎn)，隨機(jī)走動(dòng)法規(guī)劃的路徑曲率較大不適于無(wú)人機(jī)飛行，虛擬障礙物法既克服了停滯點(diǎn)問(wèn)題又滿足無(wú)人機(jī)機(jī)動(dòng)約束條件。

3 仿真實(shí)驗(yàn)

假設(shè)規(guī)劃空間為50×50，用旋轉(zhuǎn)拋物面來(lái)近似模擬山峰，為模擬較復(fù)雜的地形設(shè)定12個(gè)山峰，其中既有起始點(diǎn)目標(biāo)點(diǎn)連線經(jīng)過(guò)山峰中心的情況，又有兩個(gè)山峰重疊的情況。將無(wú)人機(jī)設(shè)定在某一高度(此處為60 m)飛行，假設(shè)無(wú)人機(jī)最大轉(zhuǎn)彎角為60°，為了更清楚地顯示航路，在山峰的等高線圖中只畫出了當(dāng)前60 m高度處的障礙圓，分別采用傳統(tǒng)流函數(shù)法及本文提出的改進(jìn)流函數(shù)法規(guī)劃出航路，如圖10所示。

圖10 改進(jìn)流函數(shù)法航路規(guī)劃等高線圖Fig.10 Contrast of routes planned on terrain contour

由圖10可以看出，采用本文提出的改進(jìn)流函數(shù)法規(guī)劃出的航路在經(jīng)過(guò)坐標(biāo)為(35，35)的山峰時(shí)能夠以較小的曲率繞過(guò)該山峰，在經(jīng)過(guò)坐標(biāo)為(7，6)和(4，10)的相互重疊的山峰時(shí)，能夠沿著山峰的邊沿繞過(guò)。

4 結(jié)束語(yǔ)

本文給出的基于改進(jìn)流函數(shù)的無(wú)人機(jī)航路規(guī)劃算法，在傳統(tǒng)流函數(shù)法基礎(chǔ)上，采用虛擬目標(biāo)點(diǎn)方法解決了多障礙物重疊情形的航路規(guī)劃問(wèn)題，同時(shí)充分考慮了無(wú)人機(jī)自身的機(jī)動(dòng)約束，將其轉(zhuǎn)化為虛擬的障礙物，使得規(guī)劃出的航路滿足了無(wú)人機(jī)的機(jī)動(dòng)約束，無(wú)需再進(jìn)行修正。仿真結(jié)果表明了該算法應(yīng)用在較復(fù)雜地形環(huán)境下的航路規(guī)劃問(wèn)題中的有效性。

［1］ 王維平，劉娟.無(wú)人飛行器航跡規(guī)劃方法綜述［J］.飛行力學(xué)，2010，28(2):6-10.

［2］ LEE J W.Improved ant colony optimization algorithm by path crossover for optimal path planning［C］//Proceedings of 2009 IEEE International Symposium on Industrial Electronics，Korea:IEEE，2009:1979-1983.

［3］ FOO J L，KNUTZON J.Path planning of unmanned aerial vehicles using B-splines and particle swarm optimization［J］.Journal of Aerospace Computing，Information，and Communication，2009(6):271-290.

［4］ JIA Q A.Path planning for mobile robots based on a modified potential model［C］//Proceedings of 2009 IEEE International Conference on Mechatronics and Automation，Nanjing，China:IEEE，2009:4946-4951.

［5］ WAYDO S，MURRAY R M.Vehicle motion planning using stream functions［C］//Proceedings of the IEEE International Conference on Robotics and Automation，Taipei，Taiwan:IEEE Computer Society Press，2003:2484-2491.

［6］ SULLIVAN J，WAYDO S，CAMPBELL M.Using stream functions for complex behavior and path generation［C］//Proceedings of the AIAA Guidance，Navigation and Control Conference.Austin，Texas:AIAA Press，2003:3-5.

［7］ YE G H，WANG H O，TANAKA K.Coordinated motion control of swarms with dynamic connectivity in potential flows［DB/OL］.http://www.yama.mce.uec.ac.jpwangh.pdf，2005.

［8］ DAILY R，BEVLY D M.Harmonic potential field path planning for high speed vehicles［C］//Proceedings of 2008 American Control Conference.Seattle，Washington:AACC，2008:4609-4614.

［9］ 湯姆森·米爾恩.理論流體力學(xué)［M］.北京:機(jī)械工業(yè)出版社，1984:169-177.