丁佳宇， 王茂森， 戴勁松

(南京理工大學 機械工程學院，江蘇 南京210094)

0 引 言

當今，無人機(unmanned aerial vehicle,UAV)編隊飛行在軍用與民用領(lǐng)域有著廣闊的應(yīng)用背景。無論是在戰(zhàn)場作戰(zhàn)還是大型表演，無人機編隊飛行都有著不可替代的地位。在實際應(yīng)用環(huán)境中，實時避障是無人機編隊飛行中的一大難題[1]。

針對無人機實時避障問題，國內(nèi)外專家提出的控制方法有人工勢場法[2]、神經(jīng)網(wǎng)絡(luò)[3]和最優(yōu)化理論[4]等方法。其中人工勢場法因計算量小，實現(xiàn)簡單，實時性好而被廣泛運用[5]。人工勢場法是由Khatib提出的一種虛擬力法。它將被控對象在周圍環(huán)境中的運動設(shè)計成一種抽象的人造力場的運動，目標點對被控對象產(chǎn)生“引力”，障礙物對被控對象產(chǎn)生“斥力”，最后通過計算被控對象所受的合力來控制它的運動方向[6]。人工勢場法規(guī)劃出來的路徑一般是比較平滑且安全的，但是這種方法存在局部最優(yōu)與路徑振蕩的問題。為解決局部最優(yōu)問題，國內(nèi)外學者提出了不同的改進方法。例如，文獻[7]在機器人陷入局部極小點后，提出設(shè)置中間目標的方法，使機器人跳出局部極小點。文獻[8]設(shè)計了一種分層勢場法，引入回環(huán)力，實現(xiàn)了無人機的路徑規(guī)劃，并提出了一種路徑規(guī)劃評價機制，但是該方法隨著分層的增加，規(guī)劃速度越慢。文獻[9]在當無人機陷入局部極小點時，系統(tǒng)在原目標點周圍產(chǎn)生一個虛擬目標，從而使無人機所受合力不為零，跳出極小點。

但上述方法僅對部分局部最優(yōu)問題有效，例如當在目標點處存在障礙物時，文獻[7，9]并不能有效地解決此問題。此外，無人機路徑規(guī)劃中存在的振蕩問題，一直不能得到有效的應(yīng)對措施[10，11]。

本文對斥力場作分層處理，分為可逼近層與危險層，以解決不可到達目標點的問題。此外，對路徑點作類似濾波處理的前饋控制策略，將已規(guī)劃的路徑點作為下次規(guī)劃的參考值，并設(shè)定合適的權(quán)重值，從而使規(guī)劃的路徑更為平滑，也可一定程度上解決局部最優(yōu)問題。

為避免由于機群與地面控制平臺失去聯(lián)系，導致無人機群失控，造成巨大的經(jīng)濟損失，本文提出在主機上搭載ARM板，收集主從機的位置與障礙信息，對無人機群作統(tǒng)一的路徑規(guī)劃，從而在地面控制系統(tǒng)失效時，無人機群仍能完成預設(shè)任務(wù)，提高了無人機群控制系統(tǒng)的安全性與穩(wěn)定性[12，13]。

1 改進人工勢場法

1.1 改進斥力場函數(shù)

當障礙物位于目標點附近時，會造成目標點不可到達的問題。本文提出對目標點附近障礙物的斥力場函數(shù)作分層處理，如圖1所示，分為可逼近層與危險層。其中，可逼近層的距離為路徑規(guī)劃中的單位步長l,危險層的半徑為無人機的最大半徑ρ1。

圖1 斥力場分層示意

傳統(tǒng)的斥力場函數(shù)為

(1)

式中η為位置增益系數(shù)，ρ為無人機在空間的位置X與障礙物之間的最短距離，ρ0為一個常數(shù)，代表障礙物的影響距離。n為一個大于零的任意常數(shù)，Xgoal為目標點位置。

對目標點附近障礙的斥力場函數(shù)修改為

Fre(X)=

(2)

式中ρ1為無人機的最大半徑距離，l為路徑規(guī)劃中的2個單位步長。當無人機處于危險層時，斥力函數(shù)不變，而當處于可逼近層時，斥力函數(shù)乘以一個關(guān)于ρ的函數(shù)，使得此時的斥力隨著無人機與障礙物距離的增加而急速減小。從而解決了因目標點附近存在障礙物而導致不可逼近的問題。

圖2為斥力場優(yōu)化前后的路徑規(guī)劃仿真效果。

圖2 斥力場函數(shù)優(yōu)化效果

從仿真結(jié)果可以發(fā)現(xiàn)，優(yōu)化前的路徑規(guī)劃算法由于目標點處存在障礙物，無人機所受合力的方向一直是遠離目標點的方向，使得其在目標點處徘徊，始終到達不了目標。經(jīng)優(yōu)化后，無人機在滿足目標點處避障的要求外，仍能選擇較短到達目標點的路徑。

1.2 前饋控制優(yōu)化

每次路徑規(guī)劃中，在斥力場與引力場的作用下，無人機所受合力的方向即為路徑規(guī)劃的方向，在規(guī)劃方向上移動一定的步長

Xnext=X+1×cos(Angle)

Ynext=Y+1×cos(Angle)

(3)

式中X，Y為上一路徑點的X，Y坐標值，Angle為當前路徑規(guī)劃的方向角度值，Xnext，Ynext為當前路徑點的X，Y坐標值。

在人工勢場法仿真過程中發(fā)現(xiàn)，當無人機運動到障礙物密集處時，路徑規(guī)劃可能會出現(xiàn)振蕩現(xiàn)象。如圖3所示。

圖3 人工勢場振蕩現(xiàn)象

為解決圖3中的路徑振蕩問題，本文采用前饋控制來消除此問題。

當算法檢測到路徑點出現(xiàn)振蕩時，計算出上一路徑規(guī)劃的方向角度Angle次路徑規(guī)劃方向角度Angle(i)的平均值，在此成為期望角度值ExpectAngle。

ExpectAngle=(Angle(i-1)+Angle(i))/2

(4)

計算出期望角度值ExpectAngle與當前路徑規(guī)劃的角度值A(chǔ)ngle的偏差絕對值E

E=|ExpectAngle-Angle|

求出偏差值與當前路徑規(guī)劃角度值的百分比Er(Er≥0)

Er=|E/Angle|×100 %

(5)

根據(jù)Er的值來決定期望角度值ExpectAngle與本次路徑規(guī)劃計算的角度值在本次實際路徑點中所占的權(quán)重值。

當Er等于0時，表明期望角度值與當前規(guī)劃的角度值一致，則設(shè)定期望角度值的權(quán)重為0，當前角度值的權(quán)重為1。當Er處于0～1之間時，表明期望角度值與當前規(guī)劃的角度值有一定的偏差，此時，期望角度值的權(quán)重由已連續(xù)振蕩的次數(shù)決定,當前規(guī)劃的角度值則是1與期望角度值的權(quán)重的差值。當Er大于1時，期望角度值的權(quán)重值與Er的二次方成反比關(guān)系。具體公式如下

(6)

式中Ca為期望角度值的權(quán)重，Cb為當前路徑規(guī)劃角度值的權(quán)重，c為振蕩次數(shù)。

將計算得到的權(quán)重值代入公式求得實際路徑規(guī)劃的角度值

A=ExpectAngle×Ca+Angle×Cb

(7)

最后將求得的角度值代入式(3)，解算出實際路徑規(guī)劃的坐標值。用MATLAB仿真結(jié)果如圖4所示。

圖4 人工勢場優(yōu)化效果

從圖4可以發(fā)現(xiàn)，在圖3中出現(xiàn)的兩個較大振蕩得到明顯改善。在振蕩產(chǎn)生后，算法能夠?qū)Ξ斍暗穆窂揭?guī)劃做出相應(yīng)的調(diào)整，使路徑更為平整光滑，同時，有不喪失大角度轉(zhuǎn)向的能力。

1.3 無人機群路徑規(guī)劃設(shè)計

在滿足單機路徑規(guī)劃避障要求后，無人機群的路徑規(guī)劃還需考慮無人機之間的防碰撞問題。

本文的設(shè)計思路是先對無人機群進行編號，確定無人機群路徑規(guī)劃的優(yōu)先級，路徑規(guī)劃順序按照無人機優(yōu)先級的高低進行排序。1#機～3#機優(yōu)先級由高到低排列，每次規(guī)劃確定下一路徑點時，1#機優(yōu)先規(guī)劃，并將2#機與3#機的上一路徑點當作障礙點，得出1#機的下一路徑點。接著，2#機將1#機已規(guī)劃的路徑點與3#機上一路徑點當作障礙點進行路徑規(guī)劃，同理，3#機將1#機與2#機已規(guī)劃的路徑點作為障礙點[14]。

當1#機規(guī)劃完當前路徑點后，2#機將1#機已規(guī)劃完的路徑點當作障礙物，但同一時刻2#機的路徑還不應(yīng)與1#機的路徑發(fā)生干涉?？紤]到上述問題，對無人機自身成為障礙時的斥力場進行單獨分層處理。如圖5所示。

圖5 無人機群路徑規(guī)劃斥力場分層示意

圖5中將無人機作為障礙點時的斥力場分為兩層，里面一層的半徑為步長l，最外層的半徑為ρ1，這種分層處理保證了同時刻無人機路徑之間不會發(fā)生干涉，達到無人機群防碰撞的要求。以下為上述斥力場函數(shù)表達式

Fre(X)=

(8)

以三架無人機為例，用MATLAB仿真結(jié)果如圖6所示。

圖6 多無人機避障仿真結(jié)果

從圖6中可以發(fā)現(xiàn)，在無人機路徑不發(fā)生干涉時，路徑規(guī)劃的效果符合避障要求?？紤]到無人機群在實際路徑規(guī)劃中很有可能會出現(xiàn)路徑干涉的現(xiàn)象，現(xiàn)對路徑出現(xiàn)干涉的情況進行仿真，仿真結(jié)果如圖7所示。

圖7 路徑干涉下的多無人機避障仿真結(jié)果

圖7中的無人機群路徑規(guī)劃曲線出現(xiàn)干涉現(xiàn)象,但在同一時刻,三架無人機之間的距離仍控制在安全距離內(nèi)。此次仿真所選取的步長l與障礙物最大影響距離L都相對來說較小，這是為了令仿真結(jié)果盡可能凸顯出路徑規(guī)劃的細節(jié)，為后續(xù)算法優(yōu)化提供參考。而在實際運用中，考慮到無人機的響應(yīng)速度與飛行姿態(tài)與位置的連貫性，通常將步長l選取為無人機的半徑，障礙物最大影響距離L選取為步長l的5倍以上。

圖8為選取實際路徑規(guī)劃中參數(shù)的仿真示意。

在選取較大的步長與障礙物最大影響距離后，可以發(fā)現(xiàn)無人機群路徑路線更為平滑，距離更短，滿足無人機群避障的要求。

2 基于ARM上位機的無人機群控制系統(tǒng)搭建

2.1 ARM上位機與無人機群之間的通信

ARM上位機與無人群之間采用無線通信連接，ARM板將傳輸?shù)男畔⑼ㄟ^串口發(fā)送給ZigBee，ZigBee再將信息傳輸給整個無人機群，通信鏈路圖如圖9所示。

圖9 ARM與無人機群通信鏈路

ZigBee具有自組網(wǎng)能力，在同一網(wǎng)絡(luò)可容納65 000個節(jié)點，滿足大多數(shù)無人機群的數(shù)量要求，并且它的安全性與可靠性較高，適合用于無人機群的內(nèi)部通信。

圖10為實驗現(xiàn)場搭建效果。

圖10 無人機群通信搭建實驗效果

由于目前主流通信協(xié)議MAVLink是在串口通信基礎(chǔ)上設(shè)計的，因而可以直接將根據(jù)MAVLink通信協(xié)議打包而成的數(shù)據(jù)信息發(fā)送給ZigBee，再發(fā)送給無人機群。而MAVLink可根據(jù)不同固件號的無人機打包其對應(yīng)的消息幀，避免了無人機群間通信的干擾。圖11為在ARM板上收發(fā)消息幀的實驗效果。

圖11 ARM板收發(fā)消息幀實驗效果

2.2 路徑規(guī)劃算法實現(xiàn)

在上文路徑規(guī)劃算法中，將每次解算出的路徑點賦值給MAVLink中的航點命令打包函數(shù)，并通過串口發(fā)送給對應(yīng)的無人機，完成路徑規(guī)劃的控制。如圖12所示。

圖12 人工勢場法路徑規(guī)劃點效果

在確定無人機群各個單位的起始點與目標點后，將他們的經(jīng)緯度值轉(zhuǎn)換為XY坐標值，路徑規(guī)劃算法結(jié)算完實時路徑點的坐標后，再將坐標值轉(zhuǎn)換為經(jīng)緯度發(fā)送給無人機。在無人機群規(guī)劃距離較短時，利用投影變換法即可滿足飛行要求。

3 結(jié)束語

針對傳統(tǒng)人工勢場法出現(xiàn)的不可到達問題，改進了目標點附近障礙點的斥力函數(shù)。提出了一種基于前饋控制的人工勢場改進法，有效解決了規(guī)劃過程中出現(xiàn)的振蕩與停滯問題，并一定程度上優(yōu)化了路徑曲線。在此基礎(chǔ)上，實現(xiàn)了改進后無人機群的人工勢場路徑規(guī)劃算法，本無人機群在保證避障要求的基礎(chǔ)上，優(yōu)化了無人機群路徑曲線，避免陷入局部最優(yōu)。利用上述改進的無人機群避障算法，搭建了無人機群控制通信鏈路，并將算法與其融合，完成了無人機群的系統(tǒng)設(shè)計，為無人機群飛提供了一種新的搭建思路。