周蘭鳳,孔明月

（上海應(yīng)用技術(shù)大學 計算機科學與信息工程學院,上海 201418）

0 引言

無人機路徑規(guī)劃是根據(jù)特定的規(guī)則,尋求從起始位置到目標位置的一條最優(yōu)、安全且能避開所有障礙物的路徑.在無人機任務(wù)規(guī)劃中,路徑規(guī)劃占有重要地位,是近年來研究的熱點.由于無人機具有飛行靈活、可控性強、抗干擾能力強等優(yōu)點,它受到了越來越多研究者的關(guān)注,在軍事、民用、科研等領(lǐng)域的應(yīng)用越來越廣泛[1].因此,開展路徑規(guī)劃研究具有重要意義.

現(xiàn)有的導(dǎo)航算法可以分為兩大類: 經(jīng)典導(dǎo)航算法和啟發(fā)式導(dǎo)航算法[2].經(jīng)典導(dǎo)航算法包括路線圖構(gòu)建算法、反應(yīng)性算法和人工勢場法.路線圖構(gòu)建算法[3]是將空間中的障礙物擴展為多邊形,建立多邊形障礙物的角點、起點和目標點之間的聯(lián)系,實現(xiàn)最短路徑搜索.反應(yīng)性算法[4]是將機器人的工作空間模擬為一個神經(jīng)元網(wǎng)絡(luò),利用傳感器接收到的信息,如障礙物位置和運動狀態(tài),作為神經(jīng)網(wǎng)絡(luò)的輸入,通過迭代計算,將機器人的運動方向和轉(zhuǎn)向角度作為輸出.機器人可以避開障礙物,向目標點移動.人工勢場法(Artificial Potential Field Method,APF)是虛力法,由Khatib 提出[5],將機器人在環(huán)境空間中的運動轉(zhuǎn)化為虛力場運動,建立由目標產(chǎn)生的引力場和障礙物產(chǎn)生的斥力場組成的混合勢場.斥力場矢量方向與障礙物位置相對,引力場矢量方向與目標位置相對,合力指向下一條路徑的方向.啟發(fā)式導(dǎo)航算法[6]有A*算法[7]、Dijkstra 算法[8]、BFS 算法[9]、生物啟發(fā)算法[10]等.這些算法各有優(yōu)缺點且密切相關(guān),在許多應(yīng)用中,它們之間可以互相結(jié)合,從而推導(dǎo)出更有效的路徑規(guī)劃方式.

無人機需要在廣闊的空中環(huán)境飛行,這對路徑規(guī)劃方法的實時性和三維適應(yīng)性提出了更高的要求[11].而人工勢場法模型簡單,所生成的路徑光滑,非常適用于實時避障.當無人機檢測到未知障礙物時,以當前位置為起點,以全局路徑規(guī)劃中的下一個轉(zhuǎn)折點為目標點.在目標點建立一個吸引勢場,在障礙物周圍建立一個排斥勢場.在吸力和斥力的共同作用下,無人機向目標點移動.然而,傳統(tǒng)的APF存在一些固有的問題: ①在障礙物附近目標不可達問題;② 局部極小值問題.且近年來的研究大多是基于二維環(huán)境.為了克服這些缺點,Yang等[12]使用潛在勢場法,在該區(qū)域設(shè)置了一個額外的勢場,將機器人拉離局部極小值區(qū)域.Liang等[13]提出了扇區(qū)劃分的方法,將虛擬障礙增加到局部極小值區(qū)域范圍內(nèi),在原障礙物和目標點的共同作用下,產(chǎn)生驅(qū)動移動機器人運動的力,但同樣是在二維環(huán)境下進行的實驗,只考慮到二維的障礙,無法適用于無人機.Matoui等[14]使用非最小速度算法處理局部極小值問題,沒有解決目標不可達問題.郜輝等[15]使用“繞墻走”方式解決局部極小值問題,圍繞障礙物的邊緣進行運動,使移動機器人走出局部極小值區(qū)域,但這個方法只適用于在地面行走的機器人,如掃地機器人,對于較為脆弱的無人機,貼墻走有很大的安全隱患.Song等[16]提出了一種利用時間信息和預(yù)測勢來規(guī)劃平滑路徑的預(yù)測人工勢場法,研究了具有車輛動力學和局部極小值區(qū)域可達性的人工勢場原理,同樣是在二維環(huán)境下,且沒有考慮到復(fù)雜環(huán)境.Zhou等[17]提出了通過改進斥力勢場函數(shù),增加子目標點,結(jié)合改進的余弦自適應(yīng)遺傳算法,實現(xiàn)了障礙物環(huán)境下對軌跡的優(yōu)化,提高了機械手的拾取效率,但沒有解決傳統(tǒng)人工勢場法固有的目標不可達和局部極小值問題.Yuan等[18]提出了一種基于制動過程分析的縱向安全距離模型和側(cè)滑角約束下,最小換道時間的換道安全間距模型的改進人工勢場法,并用人工力表示環(huán)境信息的影響,最終仿真結(jié)果表明,該算法具有較好的避障性能,但此算法依賴于人工賦值,缺乏自主性.Yang等[19]提出了通過修改引力場方向和影響范圍、增加虛擬目標和評價函數(shù)來解決局部極小值問題,利用增加吸引力來解決目標不可達問題的方案,并測試了不同規(guī)劃模式對兩棲機器人的影響,但此研究依然是基于二維環(huán)境,當應(yīng)用于三維環(huán)境時,需要考慮其他因素及三維避障.李克玉等[20]提出了一種勢力函數(shù)與模糊算法相結(jié)合的改進方法,使無人機在三維動態(tài)環(huán)境中可以逃離局部極小值區(qū)域.姚遠[21]提出了一種在三維環(huán)境下設(shè)定距離閾值的方法,當無人機與目標點距離大于距離閾值時,將引力視為常數(shù),來避免陷入局部極小值區(qū)域.

在三維場景下,為了更加直接地改進算法,本文通過加入距離修正因子來改進斥力場函數(shù),并在無人機陷入局部極小值區(qū)域時,使用正六邊形導(dǎo)向法使其逃離局部極小值區(qū)域.

1 理論基礎(chǔ)

1.1 引力場函數(shù)

設(shè)當前無人機位置坐標為X=(x,y,z)T,目標點坐標為Xg=(xg,yg,zg)T.無人機在到達目標點之前一直受到引力的影響.無人機距離目標點越遠,所受到的吸引力越大.當無人機到達目標點時,所受到的吸引力變?yōu)榱?目標點產(chǎn)生的吸引勢場

無人機所受到的引力Fatt(X) 為引力勢場Uatt(X) 的負梯度,即

引力的方向沿著無人機與目標點之間的直線,指向目標點.

1.2 斥力場函數(shù)

與引力場函數(shù)類似,斥力場函數(shù)也與距離有關(guān).當無人機處于障礙物的影響范圍內(nèi)時,會受到斥力的影響.無人機距離障礙物越近,所受到的斥力越大.設(shè)障礙物坐標為Xo=(xo,yo,zo)T,無人機所受到障礙物產(chǎn)生的排斥勢場

無人機在斥力場中的斥力Frep(X)為Urep(X) 的負梯度,即

斥力的方向是沿著無人機與障礙物之間的直線,指向無人機.

1.3 合成勢力場函數(shù)

在無人機向目標點移動時,它會受到引力場和斥力場的共同作用.合成勢場函數(shù)

三維空間的合力模型建立如圖1 所示.Frep1和Frep2分別是兩個障礙物產(chǎn)生的斥力,Frep是所有障礙物的總排斥力,Fatt是目標點產(chǎn)生的引力,合力F由力的疊加原理求得,為無人機所受到的斥力和吸引力的合力.

圖1 三維空間的合力模型Fig.1 Three dimensional resultant force model

2 改進算法

本文對APF 進行了改進,使其更適用于三維環(huán)境下的無人機路徑規(guī)劃.同時,提出了基于改進的APF 的避障策略.圖2 顯示了本文所提出的基于改進的APF 的無人機路徑規(guī)劃流程圖,其中(xc,yc,zc)為無人機的當前坐標點,本文在原斥力場函數(shù)的基礎(chǔ)上加入距離修正因子,用以解決目標不可達問題.本文將正六邊形導(dǎo)向法與APF 相結(jié)合,幫助無人機逃離局部極小值區(qū)域.無人機可以通過感知外界環(huán)境、自身狀態(tài)和障礙物運動來選擇合適的避障策略.

2.1 對目標不可達問題的改進

目標不可達問題的根本原因是目標點處的勢場值不是總勢場的最小值.由式(3)可知,如果無人機無限接近障礙物,d(X,Xo) 越接近零,越接近+∞.即隨著無人機接近目標點,一方面,無人機所受到的引力場減小;另一方面,無人機所受到的斥力場迅速增大.圖3(a)顯示了三維空間中的目標不可達問題.虛線圈表示排斥力場的影響范圍.當無人機位于圖3(a)位置時,受到Fatt和Frep共同作用的影響.無人機所受到的斥力比引力大得多,由于物體總是會從高勢場移動到低勢場,無法從低勢場到達高勢場.無人機所受到的合力指向遠離目標點的方向,從而使其無法到達目標點.

為了解決目標不可達問題,本文在斥力場函數(shù)中加入了距離修正因子.距離修正因子能平衡兩種力的變化,特別是當無人機所受到的排斥力迅速增大時.從而,當無人機接近目標點時,無人機所受到的斥力可以逐漸減小.因此距離修正因子可以保證目標點的合成勢場是全局最小的.在吸引勢場不變的情況下,斥力勢場函數(shù)可以定義為

式(7)在式(3)的基礎(chǔ)上增加了dn(X,Xg),dn(X,Xg) 為無人機到目標位置距離的n次方.n是一個任意大于零的實數(shù).加入n是為了平衡距離的變化,以使分子變化比分母更快.隨著無人機與目標點距離的接近,無人機所受到的斥力會趨向于0 而不是無窮大.改進后的斥力勢場函數(shù)

Frep1(X)和Frep2(X) 定義為

圖2 基于改進的APF 的無人機路徑規(guī)劃流程圖Fig.2 UAV’s path planning flow chart based on the proposed improved APF method

圖3(b)所示為改進后的合力模型.Frep2和Frep1的合力Frep是總力場F的一個分力,它們的方向在無人機與障礙物之間的直線上,引導(dǎo)無人機遠離障礙物.Fatt是總力場F的另一個分力,它的方向在無人機與目標點之間的直線上,引導(dǎo)無人機向目標點移動.

對于無人機的受力情況,可以根據(jù)n的值進一步分析.

當n=1 時,Frep1(X)和Frep2(X) 可以分別表示為

當無人機接近目標點時,d(X,Xg) 減小并趨于零.無人機所受到的力Frep1也趨于零,而krep是一個常量.無人機只在引力作用下向目標方向移動.

當n＞1 時,隨著無人機接近目標點,dn(X,Xg)和dn-1(X,Xg) 趨近于零,無人機所受到的總斥力趨近于零.在引力的作用下,無人機可以逐漸到達目標點.

圖3 三維環(huán)境建模Fig.3 3D environment modeling

2.2 對局部極小值問題的改進

在理想情況下,無人機應(yīng)該在到達目標點后停止移動,那里的合力為零或全局最小.然而,無人機也可能到達一個引力和排斥力共線,大小相等但方向相反的位置,且此時無人機受到的合力為零或全局最小.在這種情況下,無人機被困在一個局部極小值區(qū)域內(nèi).

為了解決上述的局部極小值問題,本文提出了一種正六邊形導(dǎo)向法,算法流程如圖4 所示.圖4中,Frep,z和Frep,xy分別為z軸方向和xOy平面上的斥力差值,s為給定值.

圖4 正六邊形導(dǎo)向法流程圖Fig.4 Flow chart for the hexagonal guide method

局部極小值問題的3 種常見情況如圖5(a)—(c)所示.在圖5(a)和圖5(b)中,無人機與障礙物、目標點共線.一開始,無人機所受到的引力大于排斥力,無人機沿著這條線向目標移動,無人機所受到的斥力隨著無人機與障礙物距離的減小而增大.在移動過程中,必然存在一個合力F為零的位置,使無人機處于靜止狀態(tài).多障礙的局部極小值問題如圖5(c)所示.無人機所受到的引力被斥力抵消,使無人機處于靜止狀態(tài).

圖5 局部極小值問題的改進Fig.5 Improvement in the local minimum problem

如圖5(d)—(e)所示,本文假設(shè)無人機被困在局部極小值區(qū)域,并假設(shè)局部極小值區(qū)域的中心點是坐標原點.將無人機與目標點連接的直線作為x軸,取經(jīng)過原點并垂直于x軸的直線作為y軸,z軸的方向按照右手法則,構(gòu)造一個以自定義步長作為邊長的虛擬正六邊形.并將其與當前位置最近的且遠離障礙物方向的角點記為一個虛擬的臨時目標點,當行進到這個臨時目標點時,便判斷當前位置是否處于局部極小值.以此類推,直至脫離局部極小值區(qū)域.當無人機處于局部極小值區(qū)域時,沿x軸產(chǎn)生一個角θ.Fθ的方向便為下一步無人機的初始運動方向,無人機朝著虛擬目標點飛行的過程仍使用APF,從而可以在達到逃離局部極小值區(qū)域的目標的同時,盡可能地保留APF 路徑光滑連續(xù)的優(yōu)勢.當θ為正時,無人機順時針旋轉(zhuǎn)0°～|θ|°.當θ為負時,無人機逆時針旋轉(zhuǎn)0°～|θ|°.無人機可以朝著靠近目標點但不進入障礙物區(qū)域的虛擬正六邊形角點移動,從而擺脫局部極小值區(qū)域,這樣既保證了路徑的平滑性,又能逃離局部極小值區(qū)域.當無人機受到目標點的引力,大于障礙物對其的斥力時,正六邊形導(dǎo)向法將停止運行,切換為人工勢場法.

為了保證無人機能夠在三維環(huán)境下迅速成功識別出局部極小值,提高導(dǎo)航效率,本文對正六邊形導(dǎo)向法進一步分析.

(1)如圖5(f)所示,無人機位于障礙物對稱放置的局部極小值區(qū)域.無人機利用傳感器對周圍障礙物進行掃描,并對障礙物和自身進行定位,構(gòu)造一個二維坐標系和一個虛擬正六邊形.當障礙物面積較大時,可以在第一個正六邊形上增加一些新的正六邊形,作為蜂窩移動網(wǎng)絡(luò).無人機沿著正六邊形的邊移動,直到遠離障礙物為止.

(2)當無人機遇到凹面障礙物時,會出現(xiàn)局部極小值,導(dǎo)致無人機處于閉環(huán)運行狀態(tài),如圖5(g)所示.在某些情況下無人機可以水平向后移動或轉(zhuǎn)身.一旦到達距離局部極小值區(qū)域足夠遠的地方,無人機就建立一個二維坐標系和一個虛擬正六邊形.無人機通過探測和定位障礙物來構(gòu)建引導(dǎo)框架,從而擺脫局部極小值區(qū)域,找到最佳的避障路徑.

(3)對于二維坐標的建立,本文在此加入代碼里的條件判斷,將實際應(yīng)用場景分為以下3 種情況:①當無人機受到的平行于z軸方向上的斥力差Frep,z小于定值s時,說明無人機前方的障礙物比較高,不易跨越,從而本文構(gòu)造一個平行于xOy平面的二維坐標系和一個虛擬正六邊形;② 當無人機受到的平行于z軸方向上的斥力差Frep,z大于s時,且當無人機受到的垂直于當前目標點方向上的斥力差Frep,xy大于s,無人機爬升比平行于地面飛行的耗能更大,本文優(yōu)先選擇平行于地面飛行,構(gòu)造一個平行于xOy平面的二維坐標系和一個虛擬正六邊形;③當無人機受到的平行于z軸的斥力差Frep,z大于s時,無人機受到的垂直于當前目標點方向上的斥力差Frep,xy小于s,本文構(gòu)造一個位于當前位置與目標點所在連線垂直于xOy平面的平面上的二維坐標系和一個虛擬正六邊形,其中正六邊形方向指向平行于z軸斥力更小的方向,如圖5(h)所示.

綜上所述,正六邊形導(dǎo)向法有兩個明顯的優(yōu)勢.首先,該方法可以避免由于參數(shù)選擇不當而產(chǎn)生的折疊和振蕩現(xiàn)象;其次,該方法通過固定參數(shù),大大減少了計算量.

3 仿真驗證與分析

為評估本文所提出的算法的性能,通過Matlab R2018a 對算法進行了驗證.MateBook14 的硬件信息為: Intel(R) Core(TM) i7-8565U、2.4 GHz 四核處理器,8 GB 內(nèi)存,512 GB 固態(tài)硬盤.假設(shè)無人機為一個質(zhì)點,仿真參數(shù)根據(jù)上文中提的(最不利的)條件進行選擇和設(shè)置.為了使模擬簡單,在無人機和障礙物之間的模擬測量范圍內(nèi)不考慮噪聲、移動障礙物等不確定參數(shù)的影響.無人機初始位置為Xo=(0,0,0)T,目標點為Xg=(175,180,40)T,單個障礙的影響范圍設(shè)置為d0=6 .障礙物的位置坐標是固定的.引力勢場常數(shù)katt=0.04,斥力場常數(shù)krep=0.01,最大步長為3,同時設(shè)置正六邊形導(dǎo)向法中的初始邊長為一步步長,在實驗中會根據(jù)障礙物大小進行調(diào)整,安全距離rs=6,預(yù)測距離Rp=4,斥力差值|s|=3,迭代計算次數(shù)為520.在仿真三維環(huán)境中,將本文算法與傳統(tǒng)的人工勢場法進行了比較,以說明正六邊形導(dǎo)向法對兩個固有缺陷的改進程度.考慮到在真實場景中,最常見的障礙物都是從地面上延伸出來的,本文將障礙物模型定義為柱狀物,其中半徑、高度和平面坐標可以隨實驗需要調(diào)節(jié).

3.1 實驗結(jié)果

由于障礙物的特點對無人機的路徑規(guī)劃有較大的影響,所以,本文在相同的障礙條件下,對這些情況進行了算法改進前后的模擬,考慮了5 種不同的情況進行比較,以驗證改進后算法的優(yōu)越性.同時,對障礙物的位置進行了調(diào)整,使其在自由環(huán)境中恰好出現(xiàn)局部極小值問題.在多種不同障礙條件下,使用傳統(tǒng)算法與改進后算法運行出的無人機運行路線對比圖如圖6 所示.其中以一個俯視圖和三維視圖為一組,左邊為俯視圖,右邊為相同條件下的三維視圖.

圖6(a1)和圖6(a2)為單個的障礙物的情況(如圖5(a)所示).障礙物的中心位置為 (150,160,30)T.起點、障礙中心坐標和目標點共線,目標點在障礙物的斥力場的影響范圍內(nèi).無人機向目標點移動,并在靠近障礙物時進入局部極小值區(qū)域.如圖6(a1)和圖6(a2)所示,紅色軌跡為傳統(tǒng)APF 的運行結(jié)果,當無人機運行進入目標點引力和障礙物斥力相等的區(qū)域時,無人機便停止運行,無法逃離局部極小值區(qū)域.黑色軌跡為改進后的APF 的運行結(jié)果,此時由于距離修正因子的加入,保證了目標點的合成勢場是全局最小的,無人機可以成功到達目標點,沒有產(chǎn)生折疊和振蕩.

圖6(b1)、圖6(b2)、圖6(c1)、圖6(c2)、圖6(d1)和圖6(d2)為圖5 中非凹面障礙物造成的對稱環(huán)境的情況(如圖5(b)和圖5(d)所示).起點、障礙物中心位置和目標點共線.圖6(b1)和圖6(b2)中障礙物的中心位置坐標分別為(140,140,50)T和(150,150,60)T.圖5(c)中障礙物的中心位置坐標為(130,130,30)T、(140,140,50)T和(150,150,60)T.由于無人機處于閉環(huán)運行狀態(tài),如圖6(b1)和圖6(b2)、圖6(c1)和圖6(c2)、圖6(d1)和圖6(d2)所示,紅色軌跡為傳統(tǒng)APF 的運行結(jié)果,當無人機到達障礙物附近時,由于目標點引力場和障礙物斥力場的作用,無人機與障礙物相撞,從而無法到達目標點.黑色軌跡為改進后APF 的運行結(jié)果,改進后的APF 由于對斥力場函數(shù)進行了調(diào)整,當無人機沒有到達目標點的時候,所受到的斥力會大于引力,正六邊形導(dǎo)向法會引導(dǎo)無人機進行轉(zhuǎn)向,使無人機能夠沿障礙物所產(chǎn)生的斥力影響范圍邊緣獲得平滑的規(guī)劃路徑,由于目標點的合成勢場所產(chǎn)生的斥力全局最小,無人機可以順利到達目標點.

圖6(e1)和圖6(e2)顯示了由于凹形障礙物導(dǎo)致的局部極小值問題(如圖5(g)所示).在使用傳統(tǒng)的APF 時,無人機會盡量避開每一個障礙物,最終進入一個閉合的循環(huán),并在閉合的循環(huán)中持續(xù)運行直到迭代完畢.改進的APF 可以使無人機擺脫這種局部極小值區(qū)域,同時圖6(e1)和圖6(e2)中存在多個不同大小障礙的復(fù)雜環(huán)境(如圖5(g)所示).傳統(tǒng)的APF 如圖6(e1)和圖6(e2)中的紅色軌跡所示,障礙物緊密附著,兩組相互排斥的勢場疊加在無人機上,當無人機行進到凹形障礙物區(qū)域時,無論怎么迭代都無法繼續(xù)向前移動,并產(chǎn)生大量路徑折疊.而改進的APF 如圖6(e1)和圖6(e2)中的黑色軌跡所示,無人機采用正六邊形導(dǎo)向法,每走一步檢測此處是否為局部極小值區(qū)域,若是則繼續(xù)沿指定方向飛行,直至繞過障礙物,成功到達目標位置,而且路徑平滑簡短無折疊.若不是則調(diào)用APF,直接向終點航行.

圖6(f1)和圖6(f2) 為對稱垂直方向上的凹形障礙的情況,此時的目標點坐標為Xg=(175,180,20)T,由圖6 可知,目標點和初始點均低于障礙物高度.傳統(tǒng)的APF 如圖6(f1)和圖6(f2)中的紅色軌跡所示,此時無人機所受到的目標點的吸引力方向指向目標點方向,而所受到障礙物的斥力合力指向無人機遠離障礙物的方向,即無人機的后下方.無論怎么迭代都無法跨越障礙,并產(chǎn)生大量錯誤路徑.而改進的APF 如圖6(f1)和圖6(f2)中的黑色軌跡所示,無人機采用正六邊形導(dǎo)向法,此時由于無人機所受到的平行于z軸方向上的斥力差Frep,z大于s,且無人機受到的垂直于當前目標點方向上的斥力差Frep,xy小于s,從而本文構(gòu)造一個位于當前位置與目標點所在連線垂直于xOy平面上的二維坐標系.并在這個二維坐標系上構(gòu)造一個虛擬正六邊形,如圖5 (g) 所示,并以其角點為虛擬目標點,從而無人機此時的飛行方向為前上方,在到達虛擬目標點之前的路徑規(guī)劃規(guī)劃依然使用APF,到達虛擬目標點之后,判斷無人機是否已經(jīng)脫離局部極小值區(qū)域,切換算法到APF,無人機成功逃離局部極小值區(qū)域.

圖6 多種不同障礙物下的實驗結(jié)果Fig.6 Experimental results under various complex obstacles

3.2 分析

本文以一個自己組裝的四旋翼無人機為例,在實際應(yīng)用中演示了該方法的性能.主要組裝元件有大疆的F450 機架 (含腳架),Pixhawk 飛控,2812 無刷電機等.真實環(huán)境和實驗條件如圖7(a)—(b)所示,本文以仿真實驗中的柱體為障礙物,使用無人機進行自主避障.

本文將改進后的代碼,使用C++實現(xiàn)并在ROS (Robot Operating System)上編譯運行.本文的實驗環(huán)境為Ubuntu 18.04,RTX 2080ti 顯卡,Ryzen 2600x 處理器,在ROS 環(huán)境下的Rviz 中構(gòu)建如圖6 (e2)中多重障礙物環(huán)境下的實驗結(jié)果如圖8 所示,其中仿真無人機搭載有三維雷達傳感器.由圖8 可以看出,在多重障礙物環(huán)境下,仿真模擬的無人機路徑的平滑度較高,避障效果較好.

最終在真實環(huán)境下,本文進行了簡單模擬陷入局部極小值區(qū)域的單個障礙物的真機實驗,實驗環(huán)境中的障礙物如圖6 (e2) 所示,從起飛位置至目標位置,共用時25 s.在飛行過程中,收集到的在真實環(huán)境下的實驗數(shù)據(jù)如圖9 所示.

在坐標系中,以無人機頭為x軸正方向,以無人機左側(cè)為y軸正方向,以垂直地面的上方為z軸正方向.其中圖9 (a) 為加速度曲線,圖9 (b) 為速度曲線,圖9 (c) 為俯仰角曲線.由數(shù)據(jù)可以看出,本文所提出的算法在無人機避障飛行中是可行及有效的.

本文展示了路徑規(guī)劃的結(jié)果,將改進的APF 能夠使無人機避開簡單和復(fù)雜障礙物進行演示.在目標位置設(shè)定徑向距離為0.3 m,如果無人機到達的位置在這個限制范圍內(nèi),本文就假定無人機已經(jīng)到達目標位置.本文將實驗環(huán)境中的柱狀物作為障礙物,設(shè)置單個障礙的影響范圍為d0=0.6 m,無人機的初始速度為v0=0.5 m·s-1.

圖7 真實環(huán)境和實驗條件Fig.7 Experimental conditions in a real environment

圖8 多重障礙物環(huán)境下的實驗結(jié)果Fig.8 Results of the proposed algorithm under the Rviz environment with multiple obstacles

由圖6 (d1)和圖6(d2) 可知,對于中間目標點的情況,可以通過距離修正因子的加入使目標點的勢場合力最小,從而使無人機順利到達目標點.由圖6 (a1)、圖6(a2)、圖6 (b1)、圖6(b2)、圖6 (c1)和圖6(c2) 可知,對于中間障礙物的目標不可達情況,可以通過距離修正因子的加入使目標點的勢場合力最小,同時通過圖5 (d) 中的解決方案來實現(xiàn)無人機轉(zhuǎn)向,使其逃離局部極小值區(qū)域.由圖6 (e1)和圖6(e2) 可知,對于復(fù)雜的凹形多重障礙物情況,可以使用圖5 (g)—(h) 中的解決方案,構(gòu)建多重或單個正六邊形,直至無人機逃離局部極小值區(qū)域.仿真結(jié)果證明了改進后算法在無人機三維避障中的有效性.

本文所提出的路徑規(guī)劃算法涉及APF 的改進和避障策略的選擇,在為無人機尋找平滑、無障礙的路徑方面具有一定的優(yōu)勢.當無人機處于障礙物影響范圍內(nèi)時,可以避開障礙物,到達目標點.當無人機在障礙物影響范圍之外時,不避開障礙物直接到達目標點.即使在復(fù)雜環(huán)境中,在整個避障過程中,無人機都可以獲得平滑的運動路徑并最終到達目標點,該方法繼承了原APF 的優(yōu)點,大大縮小了導(dǎo)航數(shù)據(jù)庫的體積和在航行過程中的計算量.

圖9 真實環(huán)境下的數(shù)據(jù)結(jié)果Fig.9 Data results of the proposed algorithm in a real environment

4 結(jié)論

本文提出了一種基于改進的APF 的實時路徑規(guī)劃方法,該方法可以解決靜態(tài)環(huán)境中未知障礙物的路徑規(guī)劃問題.無人機可以通過感知外界環(huán)境、自身狀態(tài)和障礙物的運動來做出避障決策.在傳統(tǒng)APF 的基礎(chǔ)上,通過在排斥勢場函數(shù)中加入距離修正因子來解決目標不可達問題.使無人機能夠到達距離障礙物很近的目標點.正六邊形導(dǎo)向法與APF 相結(jié)合,克服了局部極小值問題.當無人機處于局部極小值區(qū)域時,構(gòu)造虛擬正六邊形.改進的APF 可以引導(dǎo)無人機沿著正六邊形移動,從而擺脫局部極小值區(qū)域,到達目標點.在仿真環(huán)境和真實環(huán)境中,對APF 的改進和避障策略的可行性進行了評價.實驗結(jié)果表明,該方法在簡單和復(fù)雜的障礙物環(huán)境下均能提供可行、有效的避障路徑.此外,本文所提出的方法繼承了原APF 的優(yōu)點,減小了導(dǎo)航數(shù)據(jù)庫的體積和在航行過程中的計算量.

在未來的工作中,首先,對安全實時導(dǎo)航進一步研究.當障礙物突然加速或改變移動方向時,可能會引起碰撞.為保證無人機的安全航行,需要對排斥勢場函數(shù)進一步修改或采取其他策略.然后,會繼續(xù)尋找改進策略,使無人機能夠逃離局部極小值區(qū)域的同時飛行曲線平滑.最后,對規(guī)劃路徑進行優(yōu)化,改善無人機在規(guī)劃過程中的不連續(xù),使規(guī)劃路徑更加平滑且安全.