基于平面約束人工勢(shì)場的無人機(jī)航跡規(guī)劃算法

高璕 張磊 胡高歌 楊子江 高兵兵

摘 要：航跡規(guī)劃是實(shí)現(xiàn)無人機(jī)自主導(dǎo)航飛行的關(guān)鍵。針對(duì)人工勢(shì)場法應(yīng)用于無人機(jī)航跡規(guī)劃時(shí)出現(xiàn)的規(guī)劃失敗、實(shí)用性不強(qiáng)的問題，本文提出一種基于平面約束人工勢(shì)場的航跡規(guī)劃算法。首先，利用約束平面切割障礙物，簡化了障礙物的分布。其次，構(gòu)建航跡的可行性約束條件，并建立了基于約束平面的人工勢(shì)場法。最后，綜合并改進(jìn)了一種額外控制力法和常用的改進(jìn)勢(shì)場函數(shù)。仿真驗(yàn)證表明，該算法克服了傳統(tǒng)人工勢(shì)場法未考慮無人機(jī)物理性能限制、易陷入局部最小值以及目標(biāo)不可達(dá)的問題，同時(shí)提高了航跡規(guī)劃質(zhì)量。通過本文研究，人工勢(shì)場法的不足得到了改善，有利于人工勢(shì)場法在無人機(jī)航跡規(guī)劃領(lǐng)域的應(yīng)用與發(fā)展。

關(guān)鍵詞：無人機(jī)； 航跡規(guī)劃； 人工勢(shì)場法； 避障； 額外控制力法

中圖分類號(hào)：V249 文獻(xiàn)標(biāo)識(shí)碼：A DOI：10.19452/j.issn1007-5453.2023.08.009

無人機(jī)具有尺寸小、成本低、使用靈活、適用范圍廣、生存能力強(qiáng)等優(yōu)點(diǎn)，因此廣泛應(yīng)用于軍事、農(nóng)業(yè)、交通等領(lǐng)域[1-2]。然而，隨著無人機(jī)飛行環(huán)境和所承擔(dān)任務(wù)的復(fù)雜性逐漸增加，作為實(shí)現(xiàn)其自主導(dǎo)航飛行的關(guān)鍵，航跡規(guī)劃的難度也日益增大，已成為制約無人機(jī)發(fā)展的技術(shù)瓶頸之一[3-4]。

航跡規(guī)劃是指根據(jù)飛行任務(wù)的需要，設(shè)計(jì)出一條從起點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)飛行軌跡，要求符合無人機(jī)物理性能約束并使綜合代價(jià)最小。目前，國內(nèi)外學(xué)者已經(jīng)提出許多航跡規(guī)劃方法，如A*算法、快速搜索隨機(jī)樹（RRT）算法、蟻群算法、粒子群算法、人工勢(shì)場法等。A*算法通常能規(guī)劃出具有最短航線長度的航跡，但計(jì)算時(shí)間隨問題規(guī)模增長呈指數(shù)級(jí)增加[5-6]。RRT算法無須對(duì)系統(tǒng)進(jìn)行建模，能實(shí)時(shí)快速地完成航跡規(guī)劃，但存在隨機(jī)性高、路徑并非最優(yōu)等問題[5，7]。蟻群算法具有魯棒性強(qiáng)、信息反饋好等優(yōu)點(diǎn)，但算法效果過于依賴參數(shù)的選擇、易陷入局部最優(yōu)解[8]。粒子群算法原理簡單、執(zhí)行優(yōu)化任務(wù)時(shí)效果好，但容易過早收斂，陷入局部最優(yōu)[9-10]。相比而言，人工勢(shì)場法結(jié)構(gòu)簡單，具有規(guī)劃速度快、航跡平滑、安全性高等優(yōu)點(diǎn)，因此在航跡規(guī)劃領(lǐng)域得到了廣泛應(yīng)用[11]。

傳統(tǒng)人工勢(shì)場法也存在未考慮無人機(jī)物理性能限制、易陷入局部最小值、目標(biāo)不可達(dá)等問題，最終導(dǎo)致航跡規(guī)劃失敗或規(guī)劃結(jié)果實(shí)用性不高。因此，國內(nèi)外學(xué)者提出了許多優(yōu)化算法。文獻(xiàn)[12]研究了同時(shí)存在硬障礙物和軟障礙物環(huán)境下的無人機(jī)航跡規(guī)劃問題，通過引入基于某些標(biāo)準(zhǔn)（如生存概率）的額外控制量將原問題轉(zhuǎn)化為多目標(biāo)優(yōu)化問題，并利用該額外控制量有效避免了局部最小值的產(chǎn)生。針對(duì)目標(biāo)不可達(dá)問題，文獻(xiàn)[13]提出一種考慮了無人機(jī)與目標(biāo)之間相對(duì)距離的改進(jìn)勢(shì)場函數(shù)，使無人機(jī)能順利抵達(dá)目標(biāo)點(diǎn)。這是一種常用的改進(jìn)人工勢(shì)場法，已被諸多文獻(xiàn)采用和優(yōu)化[14-16]。文獻(xiàn)[17]考慮了實(shí)際應(yīng)用中無人機(jī)或障礙物位置定位不精確的問題，提出一種混合了定位風(fēng)險(xiǎn)場和人工勢(shì)場的航跡規(guī)劃方法，生成的航跡更安全有效。然而文獻(xiàn)[12]、[13]和[17]均專注于局部最小值、目標(biāo)不可達(dá)等問題，卻忽視了或者沒有充分考慮無人機(jī)的物理性能限制，尤其是高度方向上的物理性能限制。

為了解決上述問題，本文提出一種基于平面約束人工勢(shì)場的無人機(jī)航跡規(guī)劃算法。該算法定義了一個(gè)特殊平面為約束平面，并利用該約束平面切割障礙物，簡化了障礙物的分布；建立了基于約束平面的人工勢(shì)場法，利用約束平面和水平面的關(guān)系，使航跡規(guī)劃結(jié)果滿足了可行性約束條件；綜合并改進(jìn)了額外控制力法[18]和文獻(xiàn)[13]提出的勢(shì)場函數(shù)，以克服易陷入局部最小值、目標(biāo)不可達(dá)的不足。最后，本文通過仿真試驗(yàn)對(duì)比了基于約束平面的人工勢(shì)場法、文獻(xiàn)[13]提出的改進(jìn)人工勢(shì)場法、傳統(tǒng)人工勢(shì)場法在面對(duì)無人機(jī)物理性能限制、局部最小值、目標(biāo)不可達(dá)等問題時(shí)的性能，驗(yàn)證了所提出算法的有效性。

1 傳統(tǒng)人工勢(shì)場法的缺陷

人工勢(shì)場法應(yīng)用于航跡規(guī)劃領(lǐng)域時(shí)，主要原理是將無人機(jī)和飛行空間分別抽象為一個(gè)質(zhì)點(diǎn)和一個(gè)虛擬勢(shì)場，進(jìn)而將航跡規(guī)劃問題轉(zhuǎn)化為質(zhì)點(diǎn)在勢(shì)場中的運(yùn)動(dòng)問題。

傳統(tǒng)人工勢(shì)場法的主要缺陷有：一是目標(biāo)不可達(dá)。當(dāng)目標(biāo)在障礙物的影響范圍之內(nèi)時(shí)，隨著無人機(jī)向目標(biāo)的逼近，式（2）中的障礙物勢(shì)場會(huì)快速增加，致使整個(gè)勢(shì)場的全局最小點(diǎn)并不是目標(biāo)點(diǎn)，此時(shí)勢(shì)場中無人機(jī)無法靠近、抵達(dá)目標(biāo)點(diǎn)，航跡規(guī)劃失敗。二是易陷入局部最小值。勢(shì)場產(chǎn)生的引力和斥力合力角度相差為180°，且斥力和引力大小達(dá)到某個(gè)平衡或者接近平衡時(shí)，算法會(huì)產(chǎn)生局部最小值，此時(shí)無人機(jī)停留原地、無法繼續(xù)前進(jìn)，航跡規(guī)劃失敗。三是未考慮無人機(jī)物理性能限制：式（1）、式（2）建立人工勢(shì)場時(shí)未考慮物理性能限制，導(dǎo)致無人機(jī)無法按照預(yù)定航線飛行，此時(shí)航跡規(guī)劃結(jié)果不具有可行性。航跡能夠被無人機(jī)執(zhí)行是保證航跡規(guī)劃算法實(shí)用性的前提，而無人機(jī)的物理性能限制與航跡的可行性密切相關(guān)，它包括無人機(jī)高度限制、最大俯仰角等[20-21]。

2 基于平面約束人工勢(shì)場的無人機(jī)航跡規(guī)劃算法

2.1 航跡可行性約束條件的構(gòu)建

本文以固定翼無人機(jī)為研究對(duì)象，采用以油門桿、升降舵、副翼、方向舵控制無人機(jī)質(zhì)心位移、繞質(zhì)心轉(zhuǎn)動(dòng)的模型，并規(guī)定無人機(jī)近似勻速飛行。受無人機(jī)的物理性能限制，三維環(huán)境下航跡為了保證可行性，必須滿足以下約束條件。

（1） 航跡高度約束

2.2 障礙物模型的建立

無人機(jī)飛行時(shí)會(huì)遭遇一些無法穿越或存在危險(xiǎn)的障礙物，因此在航跡規(guī)劃之前必須對(duì)障礙物進(jìn)行合理的描述。本文采用幾何圖形來建立障礙物模型，并通過包裹、連鎖、膨脹處理將障礙物分布從密集復(fù)雜變得簡單方便，解決了人工勢(shì)場法有時(shí)因障礙物過于密集而無法完成航跡規(guī)劃任務(wù)的問題。

處理過程如圖1所示，將兩個(gè)不規(guī)則障礙物優(yōu)化為了圓形障礙物。在三維環(huán)境下，包裹、連鎖、膨脹處理后得到的是球形障礙物。圖1中，r1、r2分別為障礙物1、2中心到邊界的最大距離；r12是包裹處理后兩圓形障礙物之間的距離；r3為連鎖處理后得到的障礙物半徑；r′為膨脹處理時(shí)半徑擴(kuò)大的增量。

包裹處理：以不規(guī)則障礙物為對(duì)象，將不規(guī)則障礙物優(yōu)化為圓形/球體障礙物。連鎖處理：以包裹后r12≤2ρ0的密集障礙物為對(duì)象，優(yōu)化為一個(gè)較大的障礙物。若包裹后r12> 2ρ0，則跳過連鎖處理。膨脹處理：包裹和連鎖（或跳過連鎖）處理后，為降低無人機(jī)與障礙物碰撞的概率，拓展所有障礙物的邊界。

同時(shí)，需要指出的是，此時(shí)障礙物已經(jīng)被約束平面切割，從球形變成了圓形切面。因此在式（15）～式（20）所涉及的計(jì)算過程中，障礙物參數(shù)均使用r和ob′的數(shù)據(jù)。

（3）結(jié)合額外控制力法的過程

3 航跡規(guī)劃算法分析

基于平面約束人工勢(shì)場的無人機(jī)航跡規(guī)劃算法的優(yōu)勢(shì)主要體現(xiàn)在克服了未考慮無人機(jī)物理性能限制、易陷入局部最小值、目標(biāo)不可達(dá)的問題，并且能提高航跡規(guī)劃質(zhì)量。

3.1 無人機(jī)物理性能限制

基于約束平面的人工勢(shì)場、傳統(tǒng)人工勢(shì)場法以及文獻(xiàn)[13]的改進(jìn)人工勢(shì)場法航跡規(guī)劃過程的對(duì)比如圖3所示。

3.3 航跡規(guī)劃質(zhì)量

約束平面切割障礙物的過程中，會(huì)出現(xiàn)如圖4所示的兩種情形。

障礙物2和約束平面不相交，則該障礙物不產(chǎn)生斥力勢(shì)場，即在規(guī)劃航跡時(shí)認(rèn)為該障礙物不存在。這種情況的存在能有效減少障礙物的數(shù)量，進(jìn)而減少航跡長度、提高規(guī)劃成功率。另一種情況是障礙物1則被約束平面切割成圓形，則圓形切面的半徑r一定滿足r≤R，這種情況下障礙物半徑被縮小，因此減少了避障需求對(duì)航跡的影響，進(jìn)而減少航跡長度、提高目標(biāo)的可達(dá)性。

4 航跡規(guī)劃仿真

4.1 仿真參數(shù)

將基于約束平面的人工勢(shì)場法與傳統(tǒng)人工勢(shì)場法、文獻(xiàn)[13]的改進(jìn)人工勢(shì)場法進(jìn)行對(duì)比試驗(yàn)。三種算法基本參數(shù)見表1。

在無人機(jī)物理性能限制、局部最小值、目標(biāo)不可達(dá)的相關(guān)問題上，為驗(yàn)證所提出算法的性能，并比較三種算法的實(shí)用性、航跡規(guī)劃質(zhì)量，分別設(shè)置不同的環(huán)境參數(shù)，見表2～表4。表2中，障礙物2、3經(jīng)過連鎖處理后簡化為了一個(gè)障礙物。表3、表4中，為了方便觀察，將起始點(diǎn)、目標(biāo)點(diǎn)、障礙物球心均設(shè)置在同一高度，此時(shí)航跡規(guī)劃問題的環(huán)境實(shí)際簡化為了二維，約束平面就是該高度的水平面。因此在仿真結(jié)果中，不給出三維環(huán)境下才有意義的航跡與水平面的夾角變化曲線、航跡高度變化曲線。

4.2 仿真結(jié)果與分析

無人機(jī)物理性能限制相關(guān)環(huán)境參數(shù)（見表2）得到的仿真結(jié)果如圖5所示。由圖5可知，基于約束平面的人工勢(shì)場法得到的航跡僅需無人機(jī)做出兩個(gè)簡單的轉(zhuǎn)彎機(jī)動(dòng)，即可躲避障礙物到達(dá)目標(biāo)點(diǎn)?？梢钥闯?，基于約束平面的人工勢(shì)場法能得到更簡單實(shí)用的航跡，節(jié)約了無人機(jī)的飛行資源。航跡與水平面的夾角變化曲線、航跡高度變化曲線，如圖6、圖7所示?；诩s束平面的人工勢(shì)場法曲線變化平緩，滿足了無人機(jī)物理性能限制，因此實(shí)用性更好。而傳統(tǒng)人工勢(shì)場法和改進(jìn)人工勢(shì)場法航跡與水平面的夾角變化劇烈，最大值分別達(dá)到了-1.074～1.162rad，明顯超出了常用無人機(jī)維持控制所能達(dá)到的最大俯仰角；高度最低時(shí)分別達(dá)到了-64.07m、-81.26m，明顯超出了起始點(diǎn)到目標(biāo)點(diǎn)的高度范圍0～200m，有超出航跡高度約束的風(fēng)險(xiǎn)。

同時(shí)，基于約束平面的人工勢(shì)場法、傳統(tǒng)人工勢(shì)場法以及改進(jìn)人工勢(shì)場法的航跡總長度分別為2.95km、3.01km、3.05km。相比于傳統(tǒng)人工勢(shì)場法和改進(jìn)人工勢(shì)場法，基于約束平面的人工勢(shì)場法分別減少了2.00%、3.28%的航跡長度，提高了航跡規(guī)劃質(zhì)量。局部最小值相關(guān)環(huán)境參數(shù)（見表3）得到的仿真結(jié)果如圖8所示。

由圖8可知，改進(jìn)人工勢(shì)場法和傳統(tǒng)人工勢(shì)場法分別在（841.5m 841.5m 100m）、（851.7m 851.7m 100m）處陷入局部最小值，航跡規(guī)劃失敗。而基于約束平面的人工勢(shì)場法能克服局部最小值，順利完成了航跡規(guī)劃任務(wù)。此時(shí)，基于約束平面的人工勢(shì)場法得到的航跡總長2.97km。

目標(biāo)不可達(dá)相關(guān)環(huán)境參數(shù)（見表4）得到的仿真結(jié)果如圖9所示。傳統(tǒng)人工勢(shì)場法在（1999m 1927m 100m）處由于障礙物2的影響而無法抵達(dá)目標(biāo)點(diǎn)，此時(shí)距離目標(biāo)點(diǎn)還有73.01m。但基于約束平面的人工勢(shì)場法和改進(jìn)人工勢(shì)場法均能完成航跡規(guī)劃。

同時(shí)，基于約束平面的人工勢(shì)場法、改進(jìn)人工勢(shì)場法的航跡總長度分別為2.90km、2.91km。結(jié)合圖9航跡可知，在二維環(huán)境下，若兩種算法都能完成航跡規(guī)劃任務(wù)，則它們的規(guī)劃結(jié)果是相似的。

5 結(jié)論

本文針對(duì)傳統(tǒng)人工勢(shì)場法存在的規(guī)劃失敗、實(shí)用性不強(qiáng)的問題，提出一種基于平面約束人工勢(shì)場的航跡規(guī)劃算法。本文創(chuàng)新性在于通過利用約束平面切割障礙物提高了航跡規(guī)劃質(zhì)量；通過建立基于約束平面的人工勢(shì)場克服了未考慮無人機(jī)物理性能限制的問題；通過綜合并改進(jìn)一種額外控制力法和常用的改進(jìn)勢(shì)場函數(shù)，克服了易陷入局部最小值以及目標(biāo)不可達(dá)的問題。經(jīng)仿真驗(yàn)證，相比于傳統(tǒng)人工勢(shì)場法和文獻(xiàn)[13]的改進(jìn)人工勢(shì)場法，基于約束平面的人工勢(shì)場法順利地完成了航跡規(guī)劃任務(wù)，得到的航跡規(guī)劃結(jié)果簡單平緩，總長更短。

參考文獻(xiàn)

[1]彭錦城， 彭俠夫， 張霄力， 等. 基于改進(jìn)Hybrid A*的旋翼無人機(jī)路徑規(guī)劃算法[J]. 航空科學(xué)技術(shù)， 2022， 33（12）： 105-110. Peng Jincheng， Peng Xiafu， Zhang Xiaoli， et al. Path planning method for rotorcraft unmanned aerial vehicles based on improved Hybrid A* algorithm[J]. Aeronautical Science & Technology， 2022， 33（12）： 105-110. （in Chinese）

[2]Raparelli E， Bajocco S. A bibliometric analysis on the use of unmanned aerial vehicles in agricultural and forestry studies[J]. International Journal of Remote Sensing， 2019， 40（24）： 9070-9083.

[3]吳坤， 池沛， 王英勛， 等. 基于混沌灰狼優(yōu)化的多無人機(jī)協(xié)同航路規(guī)劃[J]. 航空科學(xué)技術(shù)， 2022， 33（10）： 82-95. Wu Kun， Chi Pei， Wang Yingxun， et al. Collaborative path planning of multiple unmanned aerial vehicles based on chaotic grey wolf optimization[J]. Aeronautical Science & Technology， 2022， 33（10）： 82-95. （in Chinese）

[4]Khan M T R， Saad M M， Yang Ru， et al. Aspects of unmanned aerial vehicles path planning： Overview and applications[J]. International Journal of Communication Systems， 2021， 34（10）： e4827.

[5]Tsai Y J， Lee C S， Lin Chunliang， et al. Development of flight path planning for multirotor aerial vehicles[J]. Aerospace， 2015， 2（2）： 171-188.

[6]Zhao Yijing， Zheng Zheng， Liu Yang. Survey on computational-intelligence-based UAV path planning[J]. Knowledge-Based Systems， 2018，158： 54-64.

[7]Yang Fan， Fang Xi， Gao Fei， et al. Obstacle avoidance path planning for UAV based on improved RRT algorithm[J]. Discrete Dynamics in Nature and Society， 2022，15： 1-9.

[8]Huang Chenxi， Lan Yisha， Liu Yuchen， et al. A new dynamic path planning approach for unmanned aerial vehicles[J]. Complexity， 2018，15： 1-17.

[9]Huang Chen， Zhou Xiangbing， Ran Xiaojuan， et al. Adaptive cylinder vector particle swarm optimization with differential evolution for UAV path planning[J]. Engineering Applications of Artificial Intelligence， 2023，121： 105942.

[10]Shao Shikai， Peng Yu， He Chenglong， et al. Efficient path planning for UAV formation via comprehensively improved particle swarm optimization[J]. ISA Transactions， 2020， 97： 415-430.

[11]Aggarwal S， Kumar N. Path planning techniques for unmanned aerial vehicles： A review， solutions， and challenges[J]. Computer Communications， 2020，149： 270-299.

[12]Luo Guanchen， Yu Jianqiao， Mei Yuesong， et al. UAV path planning in mixed‐obstacle environment via artificial potential field method improved by additional control force[J]. Asian Journal of Control， 2015， 17（5）： 1600-1610.

[13]Ge S S， Cui Y J. New potential functions for mobile robot path planning[J]. IEEE Transactions on Robotics and Automation， 2000，16（5）： 615-620.

[14]Chen Yanli， Bai Guiqiang， Zhan Yin， et al. Path planning and obstacle avoiding of the USV based on improved ACO-APF hybrid algorithm with adaptive early-warning[J]. IEEE Access， 2021（9）： 40728-40742.

[15]Li Weihao， Yang Chenguang， Jiang Yiming， et al. Motion planning for omnidirectional wheeled mobile robot by potential field method[J]. Journal of Advanced Transportation， 2017（3）： 1-11.

[16]Li Zhenhua， Yang Xiaojun， Sun Xiangdong， et al. Improved artificial potential field based lateral entry guidance for waypoints passage and no-fly zones avoidance[J]. Aerospace Science and Technology， 2019，86： 119-131.

[17]Shin Yujin， Kim E. Hybrid path planning using positioning risk and artificial potential fields[J]. Aerospace Science and Technology， 2021，112： 106640.

[18]Wang Honglun， Lyu Wentao， Yao Peng， et al. Threedimensional path planning for unmanned aerial vehicle based on interfered fluid dynamical system[J]. Chinese Journal of Aeronautics， 2015， 28（1）： 229-239.

[19]Khatib O. Real-time obstacle avoidance for manipulators and mobile robots[C]//Proceedings of 1985 IEEE International Conference on Robotics and Automation. IEEE， 1985.

[20]Phung M D， Ha Q P. Safety-enhanced UAV path planning with spherical vector-based particle swarm optimization[J]. Applied Soft Computing， 2021， 107： 107376.

[21]Han Bing， Qu Tengteng， Tong Xiaochong， et al. Gridoptimized UAV indoor path planning algorithms in a complex environment[J]. International Journal of Applied Earth Observation and Geoinformation， 2022，111： 102857.

UAV Path Planning Method Based on Plane-constrained Artificial Potential Field

Gao Xun1，2， Zhang Lei3， Hu Gaoge1，2， Yang Zijiang1，2， Gao Bingbing1，2

1. Northwest Polytechnical University， Xi’an 711072， China

2. Research & Development Institute of Northwestern Polytechnical University in Shenzhen， Shenzhen 518057，China

3. AVIC Aeronautical Computing Technique Research Institute， Xi’an 710065， China

Abstract： Path planning is the key to realize autonomous navigation flight of Unmanned Aerial Vehicles （UAV）. This paper proposed an UAV path planning method based on plane-constrained artificial potential field in order to overcome the problems of planning failure and low practicality when applying the artificial potential field method to UAV path planning. Firstly， the use of the constraint plane to cut obstacles simplified the distribution of obstacles. Secondly， the feasibility constraints of flight path were constructed， and the artificial potential field based on the constraint plane was established. Finally， this paper synthesized and improved the extra control force method and commonly used improved potential field functions. Simulations have verified that the proposed method overcomes the problems that traditional artificial potential field method does not consider the physical performance limitations of UAVs， is easy to fall into local minima and unable to reach targets. At the same time， the path planning quality is improved. Through the research， the shortcomings of artificial potential field method have been improved， which is conducive to the application and development of artificial potential field method in the field of UAV path planning.

Key Words： unmanned aerial vehicle； path planning； artificial potential field method； obstacle avoidance path； extra control force method