基于改進(jìn)人工勢(shì)場算法的無人機(jī)群避障算法研究

陳麒杰，晉玉強(qiáng)，王陶昱

(海軍航空大學(xué)，煙臺(tái) 264001)

0 引言

隨著世界科學(xué)技術(shù)的發(fā)展和應(yīng)用，無人機(jī)的性能發(fā)展越來越全面[1]。近年來，無人機(jī)不論在軍事還是民用領(lǐng)域都發(fā)揮著越來越重要的作用，發(fā)展?jié)撃芤仓饾u被挖掘[2]，具有自主執(zhí)行任務(wù)能力的無人機(jī)是未來發(fā)展的必然趨勢(shì)。路徑規(guī)劃則是保證無人機(jī)自主飛行，提高生存能力和安全指標(biāo)的關(guān)鍵技術(shù)[3]。

無人機(jī)路徑規(guī)劃是綜合考慮無人機(jī)在有障礙物或者威脅環(huán)境中，以一定的控制算法，尋找一條從起始位置到達(dá)目標(biāo)位置的無碰撞路徑，綜合考慮了障礙目標(biāo)導(dǎo)引[4]。近年來，國內(nèi)外的眾多學(xué)者對(duì)路徑規(guī)劃問題做了大量的研究，并提出了許多可行的算法，如隨機(jī)路圖法(Probabilistic Road Maps，PRM)、A*算法、快速擴(kuò)展隨機(jī)樹法(Rapidly Exploring Random Tree，RRT)以及他們的改進(jìn)算法，到遺傳算法等智能算法[6]，再到蟻群算法[7]、鴿群算法[8]、魚群算法[9]等生物仿真算法。以上的各類算法在進(jìn)行單機(jī)避障路徑計(jì)算時(shí)，都有較好的避障性能，但進(jìn)行多機(jī)協(xié)同避障時(shí)，計(jì)算量會(huì)成幾何倍數(shù)增長，實(shí)時(shí)控制性不夠好，最終導(dǎo)致避障失敗。

無人機(jī)單機(jī)執(zhí)行任務(wù)時(shí)，生存率低，完成任務(wù)失敗率高，偵察效率低，因此無人機(jī)機(jī)群控制成為必然。目前，無人機(jī)機(jī)群控制多基于長機(jī)-僚機(jī)[10]的主從關(guān)系控制，在一定程度上解決了無人機(jī)機(jī)群協(xié)同控制問題，但此方法降低了每一架無人機(jī)的靈活度，限制了其中個(gè)體無人機(jī)的性能發(fā)揮。本文基于改進(jìn)的人工勢(shì)場[11]，提出了一種適合于無人機(jī)群的控制方法，該方法中，重新定義的斥力函數(shù)作為障礙物對(duì)無人機(jī)的斥力，解決了無人機(jī)因?yàn)槟繕?biāo)點(diǎn)附近障礙物斥力較大無法到達(dá)目標(biāo)點(diǎn)的問題； 新增加了前置形心的概念，前置形心對(duì)無人機(jī)之間有相應(yīng)的引力，解決了無人機(jī)陷入局部極小值的問題。

1 基于人工勢(shì)場法的無人機(jī)路徑規(guī)劃

1.1 人工勢(shì)場概述

人工勢(shì)場法最初是由Khatib[12]提出的一種解決路徑規(guī)劃問題的方法。路徑規(guī)劃的方法是將機(jī)器人在環(huán)境中的運(yùn)功，設(shè)計(jì)成為一種抽象的人造引力場中的運(yùn)動(dòng)，目標(biāo)點(diǎn)對(duì)無人機(jī)的移動(dòng)提供引力[13]，障礙物對(duì)移動(dòng)的無人機(jī)提供斥力，最后通過求合力以達(dá)到控制無人機(jī)避障的目的[14]。該方法優(yōu)點(diǎn)在于規(guī)劃的路徑較為光滑，避免了大角度的轉(zhuǎn)彎造成無人機(jī)失速引起的危險(xiǎn)；缺點(diǎn)一是容易陷入極小值點(diǎn)[15]，二是在離目標(biāo)點(diǎn)較近，且目標(biāo)點(diǎn)周圍有較大障礙物時(shí)，障礙物斥力大于目標(biāo)引力，無人機(jī)不能到達(dá)目標(biāo)點(diǎn)。

1.2 無人機(jī)群前置形心定義

為了解決人工勢(shì)場法存在的缺陷，本文定義了一個(gè)新的引力源，即前置形心(如圖1所示)。前置形心為無人機(jī)機(jī)群形心位置向目標(biāo)點(diǎn)方向固定步長的點(diǎn)，在無人機(jī)前方無障礙物，目標(biāo)引力和障礙物斥力相等時(shí)，以前置形心作為無人機(jī)群的另一引力源，引力源對(duì)無人有機(jī)引力作用，打破了無人機(jī)的受力平衡，使無人機(jī)向目標(biāo)點(diǎn)方向機(jī)動(dòng)。

圖1 前置形心示意圖Fig.1 Pre-centroid diagram

目標(biāo)點(diǎn)對(duì)該前置點(diǎn)的引力Fat歸一化后的方向作為前置形心前進(jìn)的目標(biāo)方向，即

(1)

前置形心的計(jì)算公式為

(2)

1.3 引力函數(shù)定義

目標(biāo)對(duì)無人機(jī)的引力場，目的在于導(dǎo)引無人機(jī)向目標(biāo)點(diǎn)飛行，定義目標(biāo)引力場為

(3)

其中，Ka是引力增益常數(shù)；Pa是單位向量，方向由無人機(jī)指向目標(biāo)點(diǎn)；ρgoal為無人機(jī)距離目標(biāo)的距離；ρmax為設(shè)定的目標(biāo)點(diǎn)對(duì)無人機(jī)作用的最大距離。當(dāng)無人機(jī)距離目標(biāo)點(diǎn)較遠(yuǎn)時(shí)，對(duì)無人機(jī)的引力過大，可能因?yàn)檎系K物的斥力不夠，導(dǎo)致避障失敗。所以設(shè)定閾值，避免目標(biāo)點(diǎn)的引力過大，造成避障失敗。

飛行過程中，前置形心對(duì)無人機(jī)產(chǎn)生引力，避免無人機(jī)陷入極小值點(diǎn)，前置形心的引力定義為

Fatp=KbρupPup

(4)

式中，Kb為前置形心引力增益常量；ρup為無人機(jī)距離前置形心的距離；Pup為單位向量，方向由無人機(jī)指向前置形心。

1.4 斥力函數(shù)定義

障礙物斥力場，目的在于對(duì)無人機(jī)飛行產(chǎn)生斥力，使無人機(jī)適時(shí)做出避障動(dòng)作。由于改進(jìn)的人工勢(shì)場函數(shù)中，對(duì)斥力的定義容易造成無人機(jī)在到達(dá)目標(biāo)點(diǎn)附近時(shí)，引力不夠大，從而造成無人機(jī)在目標(biāo)點(diǎn)附近徘徊，達(dá)不到目標(biāo)點(diǎn)的位置。因此在原有斥力的基礎(chǔ)上，增加與目標(biāo)點(diǎn)距離的影響因子，構(gòu)造新的斥力函數(shù)為

(5)

其中，Ke為障礙物斥力增益常量；ρobs為無人機(jī)距離障礙物的距離；ρ0為障礙物斥力作用的范圍；λ為根據(jù)障礙物和ρmax可調(diào)整的常數(shù)；Poe為單位向量，方向由障礙物指向無人機(jī)。

在無人機(jī)飛行過程中，添加無人機(jī)之間的斥力，將無人機(jī)的當(dāng)前位置信息作為新的障礙點(diǎn)，使無人機(jī)之間產(chǎn)生斥力，避免了無人機(jī)之間的撞擊，斥力方程定義為

(6)

其中，Ku無人機(jī)斥力增益常數(shù)，ρuu為2架無人機(jī)之間的距離；ρu0為無人機(jī)之間作用力的距離，當(dāng)超過該距離時(shí)，無人機(jī)之間無影響力；Puu為單位向量，方向由編號(hào)在前的無人機(jī)指向編號(hào)在后的無人機(jī)。

因此，無人機(jī)在人工勢(shì)場中的受力為

(7)

圖2所示為無人機(jī)在飛行過程中受到目標(biāo)點(diǎn)的引力和障礙物的斥力的示意圖。

圖2 無人機(jī)受力示意圖Fig.2 Force diagram of UAV

圖3所示為無人機(jī)飛行過程中，無人機(jī)之間斥力示意圖。

圖3 無人機(jī)之間斥力示意圖Fig.3 Diagram of repulsion between UAVs

2 無人機(jī)的編隊(duì)避障控制思想

假設(shè)在規(guī)定的飛行區(qū)域內(nèi)，有n架無人機(jī)進(jìn)行編組飛行，第i架無人機(jī)在T時(shí)刻的位置信息為(xi,yi)，計(jì)算無人機(jī)到目標(biāo)點(diǎn)的距離ρgoal，同時(shí)計(jì)算目標(biāo)點(diǎn)對(duì)無人機(jī)的引力fatt，根據(jù)公式

(8)

由于無人機(jī)飛行過程中，存在很多障礙物，有些障礙物對(duì)無人機(jī)并不造成影響，因此在避障過程中，規(guī)定無人機(jī)避開目標(biāo)方向上距離自己最近的障礙物即為避障成功。由此可取距離無人機(jī)最近的障礙物計(jì)算障礙物斥力。由無人機(jī)信息采集功能測得無人機(jī)距離最近障礙物的距離為ρobs，根據(jù)式(5)計(jì)算障礙物斥力Fre，根據(jù)式(7)計(jì)算無人機(jī)所受合力。

3 仿真驗(yàn)證

為了驗(yàn)證試驗(yàn)猜想，下面使用MATLAB2014a進(jìn)行仿真，仿真流程圖如圖4所示。

圖4 仿真流程示意圖Fig.4 Schematic diagram of simulation process

設(shè)定1號(hào)無人機(jī)初始位置為(x1,y1)=(450,150)，2號(hào)無人機(jī)初始位置為(x2,y2)=(350,250)，3號(hào)無人機(jī)初始位置為 (x3,y3)=(250,350)，4號(hào)無人機(jī)初始位置為(x4,y4)=(150,450)；無人機(jī)速度為10；無人機(jī)大小設(shè)置為10；引力場增益系數(shù)Ka為30；斥力場增益系數(shù)Kb設(shè)置為30；得到如圖5所示的仿真結(jié)果。

圖5 4臺(tái)無人機(jī)路徑規(guī)劃仿真圖Fig.5 Simulation diagram of path planning for four UAVs

從圖5可以看出，無人機(jī)在勻速向目標(biāo)點(diǎn)前進(jìn)的過程中，遇到障礙物時(shí)，能夠及時(shí)做出相應(yīng)的避障動(dòng)作，并且因?yàn)?號(hào)無人機(jī)和3號(hào)無人機(jī)之間避障路徑相似，使無人機(jī)之間產(chǎn)生相應(yīng)的斥力，可以看出，2號(hào)無人機(jī)在前進(jìn)過程中對(duì)于3號(hào)機(jī)的避讓效果。在無人機(jī)飛出障礙區(qū)之后，1號(hào)無人機(jī)和2號(hào)無人機(jī)之間，由于無人機(jī)之間的斥力和目標(biāo)點(diǎn)對(duì)于無人機(jī)的引力，1號(hào)無人機(jī)和2號(hào)無人機(jī)重新規(guī)劃了相應(yīng)的飛行路徑。

當(dāng)增加無人機(jī)目標(biāo)區(qū)域障礙物，其余條件不變，設(shè)定無人機(jī)初始位置為(x1,y1)=(450,150)、(x2,y2)=(350,250)、(x3,y3)=(250,350)、(x4,y4)=(150,450)，使編隊(duì)處于無序狀態(tài)時(shí)，其余參數(shù)不變，得到如圖6所示的仿真結(jié)果。

圖6 4臺(tái)無人機(jī)目標(biāo)區(qū)域有障礙物路徑規(guī)劃仿真圖Fig.6 Simulation diagram of obstacles path planning for four UAVs in target area

由圖6可以看出，在無人機(jī)開始避障之前，由于無人機(jī)之間相互斥力的作用，每一架無人機(jī)的路徑規(guī)劃都有明顯的避讓行為。以5號(hào)無人機(jī)為例，在進(jìn)入障礙區(qū)前，該機(jī)將序號(hào)靠前的無人機(jī)的路徑點(diǎn)作為當(dāng)前障礙物點(diǎn)，并重新規(guī)劃路徑；進(jìn)入障礙區(qū)之后，以其他無人機(jī)和障礙物的斥力為依據(jù)，規(guī)劃路徑，成功避障。在所有無人機(jī)到達(dá)目標(biāo)區(qū)域時(shí)，由圖可以看出，由于改進(jìn)了斥力函數(shù)，增加了目標(biāo)點(diǎn)距離的影響因子，從而使得無人機(jī)在目標(biāo)區(qū)域周圍有障礙物時(shí)，也能夠成功到達(dá)目標(biāo)區(qū)域。

4 結(jié)論

本文針對(duì)無人機(jī)編隊(duì)飛行中的路徑規(guī)劃問題，提出了一種基于改進(jìn)人工勢(shì)場的無人機(jī)路徑規(guī)劃方法，通過實(shí)驗(yàn)仿真可以看出：

1)飛行過程中，避免了無人機(jī)進(jìn)入極小值點(diǎn)，造成避障失敗的問題。本文提出的增加前置形心引力和無人機(jī)之間的斥力的合力，有效避免了無人機(jī)進(jìn)入極小值點(diǎn)之后的受力平衡。

2)編隊(duì)飛行過程中，在前置形心引力和無人機(jī)之間斥力的作用下，既保證了編隊(duì)飛行的穩(wěn)定性，又保證了單架無人機(jī)的自主性，提高了整體的魯棒性。本文只對(duì)無人機(jī)所受的虛擬力進(jìn)行定義和約束，對(duì)無人機(jī)的行為不做約束，因此在遇到外界新增障礙物時(shí)，能夠有效地做出避障動(dòng)作。

3)到達(dá)目標(biāo)點(diǎn)后，基于改進(jìn)勢(shì)場的斥力函數(shù)，由于指數(shù)函數(shù)的存在，衰減得更快，避免了無人機(jī)到達(dá)目標(biāo)點(diǎn)附近，卻不能到達(dá)目標(biāo)點(diǎn)的情況，有效增強(qiáng)了無人機(jī)到達(dá)目標(biāo)點(diǎn)的能力。