基于改進人工勢場法的飛行器軌跡規(guī)劃

范世鵬 吳 廣 王 亮 劉運鵬 李 伶 祁 琪

北京航天自動研究所，北京100854

為解決機器人的路徑規(guī)劃問題，斯坦福大學(xué)Khatib教授于1986年提出并首次應(yīng)用一種虛擬勢場的方法——人工勢場法(artificial potential field method)[1]。該方法提出了引力場和斥力場的概念，通過為障礙物、目標點等人工構(gòu)造勢場，搜索勢函數(shù)的負梯度方向，引導(dǎo)運動體規(guī)避威脅區(qū)，最終到達目標點，使其勢能最小化。該方法具有數(shù)學(xué)描述清晰，計算量小，實時性強，規(guī)劃速度快等優(yōu)點。

隨后，該方法在機器人、機械臂和無人機等路徑規(guī)劃問題上得到了廣泛應(yīng)用[2-8]。Jurgen將運動體視為點電荷，通過在等效放置點電荷形成斥力電勢場，使運動體在電場力的作用下規(guī)避禁飛區(qū)，并運用滑模控制方法，使其跟蹤負梯度方向飛行，最終到達目標點處[2]；采用人工勢場法規(guī)劃出的軌跡特性完全取決于勢函數(shù)的表達形式，因此勢函數(shù)的選取是該方法的關(guān)鍵。Jing Ren針對任意形狀威脅區(qū)，介紹了一種顯式勢場函數(shù)的建立方法，通過調(diào)整單參數(shù)γ即可實現(xiàn)梯度的變化[3]；在評估防空威脅方面，人工勢場法行之有效，因此往往可得到可行解[7]。然而，在復(fù)雜環(huán)境下，人工勢場法常常出現(xiàn)局部極小問題，可能使該方法產(chǎn)生早熟現(xiàn)象，運動體困于陷阱而無法到達目標，如圖1所示。

圖1 人工勢場法的局部極小問題

目前，解決局部極小問題的有效途徑主要分為2類： 1)利用某種手段預(yù)先消除局部極小點; 2)陷入局部極小區(qū)域后以算法“跳出”此區(qū)域。第1類方法構(gòu)造特殊的勢函數(shù)，保證目標點為唯一的極小值點，或盡可能地減少局部極小點的數(shù)目，這就需要對全局環(huán)境進行預(yù)處理，計算量很大，因而通常為離線應(yīng)用，無法滿足實時性和應(yīng)對突發(fā)威脅(pop-up threat)的要求；第2類方法一般是在每次運動體陷入局部極小點后，運用諸如最佳優(yōu)先、模擬退火、隨機搜索等搜索策略，尋找勢場值比當前局部極小點更低逃離點，然后繼續(xù)沿勢場負梯度方法運動直到到達目標點。

本文利用改進的人工勢場法，對飛行器在線軌跡規(guī)劃問題進行了研究?？紤]飛行器的過載約束條件，使得到的標稱軌跡滿足適飛條件，并根據(jù)當前狀態(tài)動態(tài)調(diào)整參數(shù)，改變?nèi)斯輬龅淖饔脜^(qū)域，從而克服人工勢場法容易陷入局部極小的固有缺陷。

1 基本原理

不失一般性，運動體的動能是廣義速度的二次形式：

(1)

其中，m為廣義質(zhì)量矩陣。設(shè)勢能為U(x)，則飛行器的機械能為：

(2)

由拉格朗日原理，可知運動學(xué)方程滿足：

(3)

其中，F(xiàn)為重力以外的合作用力。運動學(xué)方程可以寫為以下形式：

(4)

其中，C為向心力和科里奧利力的合力；G為重力。

人工勢場法是通過構(gòu)造虛擬的人工勢場，將目標點設(shè)置為勢場全局最小，以虛擬引力場將飛行器吸引到目標點處，而使用禁飛區(qū)對應(yīng)的虛擬斥力場，在合力的作用下，使飛行器規(guī)避障礙物和威脅區(qū)，最終使其勢函數(shù)最小化。合力F是總勢場U的負梯度矢量，即：

F(x,y,z)=-▽U(x,y,z)

(5)

總勢能滿足疊加原理，對于N個待建模的環(huán)節(jié)，總勢能可表示為：

(6)

通過人為構(gòu)造的勢場，使虛擬運動體在相應(yīng)的力場作用下運動，所經(jīng)過的軌跡具有連續(xù)可微的特性，可作為待飛行器跟蹤的標稱軌跡。

2 人工勢場法

為簡化問題，這里不考慮虛擬運動體的動力學(xué)特性，則描述虛擬運動體的側(cè)向運動學(xué)方程為：

(7)

其中，V為運動體速度；ψ為航向角；az為運動體在總勢場中對應(yīng)的法向過載。

軌跡規(guī)劃是尋找一條標稱軌跡，在到達目標點位置的同時，滿足控制輸入有限的約束，保證軌跡連續(xù)可微。為保證運動體軌跡在任意一點處的曲率是在飛行器的機動能力范圍之內(nèi)，運動體航向角速率不超過飛行器的最大機動角速率，即航向角速率具有飽和特性：

(8)

設(shè)運動體速度保持不變，將人工勢場所產(chǎn)生的合力在速度的法向上投影，由法向過載改變軌跡的航向角。目標點和禁飛區(qū)的人工勢場分別表現(xiàn)為對運動體的吸引和排斥作用。圖2給出了典型的目標點和禁飛區(qū)勢場的示意圖。

人工勢場法的重點在于勢函數(shù)的設(shè)計。首先，勢函數(shù)必須滿足二階連續(xù)可微的基本要求，在空間中任意一點處的作用力是連續(xù)可微的，這樣就可以保證軌跡的光滑。其次，勢函數(shù)對軌跡的最優(yōu)性影響很大。當排斥力系數(shù)設(shè)置過小或吸引力系數(shù)設(shè)置過大時，傳統(tǒng)人工勢場法得到的軌跡無法規(guī)避禁飛區(qū)；若排斥力系數(shù)設(shè)置過大，或吸引力系數(shù)設(shè)置過小，則會導(dǎo)致軌跡存在不必要的彎曲。為此，本文根據(jù)運動體到禁飛區(qū)的距離和禁飛區(qū)半徑，動態(tài)調(diào)整吸引力(排斥力)系數(shù)，并設(shè)定排斥力有限的作用區(qū)域。同時，在禁飛區(qū)附近設(shè)置零引力環(huán)狀區(qū)域，有利于規(guī)避禁飛區(qū)，則構(gòu)造目標點形成的引力勢函數(shù)為：

圖2 典型的人工引力場與斥力場

(9)

其中，n為正整數(shù)；η為動態(tài)的吸引力系數(shù)，反映了飛行器到達目標點的強烈程度；Rob為禁飛區(qū)半徑；P為當前位置；Pob為禁飛區(qū)中心位置；Pgoal為目標點位置；Rsafe為環(huán)狀區(qū)域的寬度(如圖3所示)。

同理，減小排斥的作用區(qū)域，可減少軌跡不必要的機動。當運動體縱向位置與障礙物中心齊平時，將該位置定義為跨越點。設(shè)d0為跨越點到障礙物中心的距離，本文采取勢場平滑過渡的策略，禁飛區(qū)再跨越點之后形成的斥力勢函數(shù)為：

(10)

其中，q為正整數(shù)；λ為動態(tài)的排斥力系數(shù)，反映了飛行器規(guī)避禁飛區(qū)的迫切程度；d1為環(huán)狀區(qū)域的外徑(如圖3所示)。

圖3 力場作用區(qū)域示意圖

根據(jù)疊加原理(superposition principle)，將目標點和M個禁飛區(qū)所形成的空間約束統(tǒng)一到勢力場，總勢場函數(shù)為：

(11)

根據(jù)式(5)，可得到運動體所受的合力，將合力投影到速度的法向，由法向過載生成航向角速率從而改變速度方向，而不改變速度大小。即僅對航向角進行實時更新，達到改變運動方向、實現(xiàn)規(guī)避禁飛區(qū)的效果。通過求解模型(7)，即可得到可行的標稱軌跡。

為了評估軌跡的優(yōu)劣，選取的性能指標首先考慮軌跡是否完成避障。在成功避障的前提下，應(yīng)減少不必要的機動飛行。性能指標函數(shù)的具體形式為：

其中，w1，w2為權(quán)重系數(shù)。

為改善軌跡的性能，本文采取區(qū)間對分法，對所涉及的參數(shù)進行調(diào)整，使上述指標函數(shù)足夠小。

3 仿真驗證

假設(shè)單位質(zhì)量的虛擬運動體速度為1，禁飛區(qū)

中心位置(160,0)及半徑(R=20)給定，目標點坐標為(300,0)，航向角速率不大于5(°)/s。合理選擇增益Kg和Kob，在3種不同的航向角初值條件下，傳統(tǒng)人工勢場法得到飛行器平面內(nèi)的標稱軌跡如圖4所示。

圖4 基于傳統(tǒng)人工勢場法二維平面軌跡規(guī)劃結(jié)果

從圖4可以看出，盡管圖(a)和(b)最終都到達了目標點處，但顯然，從曲率和路徑長度方面來講，圖(a)中軌跡要優(yōu)于圖(b)，而圖(c)中由于軌跡上存在受力平衡點，運動體將陷入局部極小值，在平衡點附近做往復(fù)運動而無法到達目標點，規(guī)劃失敗。

為具備全局規(guī)劃能力，在規(guī)劃之前預(yù)先求解空間內(nèi)的局部極小值點。方法中的局部極小值點是合力為0的點，也就是類馬鞍面勢場的鞍點。當運動體到達極小值點處，且運動方向與引力一致時，在該點處增加附加的斥力勢場，使其逃離局部極小的陷阱。另外，在航向角更新時考慮過載約束。在不同的力場系數(shù)下，改進前后所規(guī)劃的結(jié)果如圖5所示。

圖5 傳統(tǒng)與改進的人工勢場法結(jié)果對比

當Kob=1時，規(guī)劃結(jié)果未能實現(xiàn)避障?？梢?，參數(shù)Kob的取值決定了排斥力的大小，這直接關(guān)系到軌跡的平直程度。若取值過小，則有可能進入禁飛區(qū)；取值過大，則軌跡曲率有所增大，對飛行器機動能力要求較高，且控制能耗相對較多。

當禁飛區(qū)的位置(120,20)和半徑(R=30)均發(fā)生變化時，圖6給出了在Kob不同取值條件下，人工勢場法所得到的標稱軌跡。

圖6 禁飛區(qū)發(fā)生變化時規(guī)劃的標稱軌跡

在上述情形下，當Kob=1時所得到的軌跡可實現(xiàn)避障，且軌跡相對平直，相比較Kob=2和Kob=3所得到的軌跡品質(zhì)更好。通過2種情形下的仿真對比，可知最優(yōu)參數(shù)與具體的約束條件密切相關(guān)。

以上數(shù)學(xué)仿真，在主頻2.08G的PC機Matlab平臺上運行，經(jīng)統(tǒng)計，耗時均在0.1s以內(nèi)，因此，該方法適用于在線規(guī)劃。

4 結(jié)論

人工勢場法是一種適用于軌跡規(guī)劃的高效方法。其形式簡單，參數(shù)相對較少，具有計算速度快、占用內(nèi)存少的優(yōu)點，適用于在線實現(xiàn)與應(yīng)用。由該方法規(guī)劃得到的軌跡能保證其二階可微，同時能滿足飛行器可用過載的約束，整個軌跡始終處于飛行器的飛行包絡(luò)中。

利用改進的人工勢場法解決了二維平面的軌跡規(guī)劃問題，結(jié)果可作為待跟蹤的標稱軌跡，具有一定的工程應(yīng)用價值。通過對比研究，改進的人工勢場法可預(yù)防陷入局部極小問題，且軌跡對應(yīng)的機動過載是連續(xù)的。然而，這種方法得到的標稱軌跡并非某種指標下的最優(yōu)軌跡。因此，應(yīng)引入軌跡性能的評價機制，優(yōu)化設(shè)計參數(shù)，以改善軌跡的最優(yōu)性。這方面的研究有待在后續(xù)工作中展開。

[1] Khatib O. Real-time Obstacle Avoidance for Manipulators and Mobile Robots [J]. International Journal of Robotics Research, 1986, 5(1): 90-98.

[2] Jurgen. G, Vadim I. Sliding Mode Control for Gradient Tracking and Robot Navigation Using Artificial Potential Fields [J]. IEEE Trans. on Robotics and Automation, 1995, 11(2): 247-254.

[3] Jing Ren, Kenneth A. McIsaac, Rajni V. Patel, et al. A Potential Field Model Using Generalized Sigmoid Functions [J]. IEEE Transactions on Systems, Man, Aad Cybernetics—Part B: Cybernetics, 2007, 37(4): 477-484.

[4] 張建英，劉暾. 基于人工勢場法的移動機器人最優(yōu)路徑規(guī)劃[J]. 航空學(xué)報，2007，28(8): S183-S188.(Zhang Jianying, Liu Tun.Optimized Path Planning of Mobile Robot Based on Artificial Potential Field [J]. Acta Aeronautica Et Astronautica Sinica, 2007，28(8): S183-S188.)

[5] 李滋堤，孫富春，吳鳳鴿等．基于人工勢場的空間遙操作共享控制[J]．清華大學(xué)學(xué)報(自然科學(xué)版)，2010，50(10):1728-1732．(Li Zidi, Sun Fuchun, Liu Huaping, et al. Shared Control for Space Teleoperation Using Artificial Potential Field [J]. Tsinghua Univ (Sci & Tech), 2010,50(10):1728-1732.)

[6] 朱毅, 張濤, 宋靖雁. 未知環(huán)境下勢場法路徑規(guī)劃的局部極小問題研究[J]. 自動化學(xué)報，2010, 26(8): 1122-1130.(Zhu Y, Zhang T, Song J Y, Study on the Local Minima Problem of Path Planning Using Potential Field Method in Unknown Environments[J]. ACTA Automatica Sinica, 2010, 26(8): 1122-1130.)

[7] 彭建亮，孫秀霞，蔡滿意等. 基于人工勢場的防空威脅建模與仿真[J]. 系統(tǒng)工程與電子技術(shù)，2010, 32(2): 338-341.(Peng Jianliang, Sun Xiuxia, Cai Manyi, et al. Modeling and Simulation of Air Defense Threat Based on Artificial Potential Field [J]. System Engineering and Electronics, 2010, 32(2): 338-341.)

[8] 羅冠辰，于劍橋，張思宇等. 穿越惡劣天氣區(qū)域的無人機航跡規(guī)劃[J]. 北京理工大學(xué)學(xué)報，2014, 34(10): 1054-1059.( Luo Guanchen, Yu Jianqiao, Zhang Siyu, et al. UAV Path Planning Across Weather Threat [J]. Transactions of Beijing Institute of Technology, 2014, 34(10): 1054-1059.)