滿足末端航向約束的人工勢場法算法研究

趙正戴，謝如恒，南 英

(南京航空航天大學(xué)航天學(xué)院，江蘇南京210016)

1 引言

人工勢場法[1-2]是由Khatib教授提出的一種虛擬力場，是目前一種較成熟且高效的路徑規(guī)劃方法。由于其具有算法簡明、實時性好、工程實踐性強等優(yōu)點，人工勢場法逐漸成為飛行器航跡規(guī)劃問題的研究熱點。

近年來，提出了許多基于人工勢場法的航跡規(guī)劃優(yōu)化方案。文獻[3]通過生成預(yù)規(guī)劃路徑弱化了目標(biāo)點對無人機的吸引作用，增加了路徑的連貫性，并在勢場函數(shù)中加入了動態(tài)調(diào)節(jié)因子以減少機動能耗；文獻[4]將局部極小分為兩類，并以連續(xù)可微的對數(shù)函數(shù)構(gòu)造勢場函數(shù)，以改善導(dǎo)彈軌跡的品質(zhì)；文獻[5]引入虛擬力，并在考慮控制對象動力運動約束條件下實現(xiàn)了快速跳出局部極值的效果；文獻[6]引入了參考航路引力場和自適應(yīng)的時間擾動因子，以解決勢場局部最小值問題，并提出一種虛擬目標(biāo)法以解決局部極值陷阱問題；文獻[7]基于虛擬結(jié)構(gòu)和“長機-僚機控制策略， 提出一種復(fù)合矢量人工勢場方法， 有效實現(xiàn)了無人機編隊在三維空間避開障礙物追蹤運動目標(biāo)的目的；文獻[8]提出“選擇穿越法”， 通過有選擇地穿過障礙物以擺脫局部極小狀態(tài)；文獻[9]提出直接對斥力與引力進行建模，將出現(xiàn)的局部極小點定義為障礙物，從而解決極小點問題。

目前尚未對人工勢場法增加末端航向約束，且對末端情況的研究大多集中在制導(dǎo)律的改進[10-11]。由于飛行器任務(wù)不同，某些情況下飛行器需要以指定方向進入目標(biāo)點，如導(dǎo)彈繞開防御層從某一方向進攻目標(biāo)等。針對以上問題，本文對人工勢場法提出改進，通過在目標(biāo)點指定的作用范圍內(nèi)構(gòu)造新的人工勢場，即在此作用范圍內(nèi)重新規(guī)劃引、斥力點的分布及受力模型，形成獨立的入場模型，使飛行器在此入場模型中重新受力，實現(xiàn)了飛行器沿指定航向到達(dá)目標(biāo)的目的，滿足了末端航向約束，并采用比例導(dǎo)引法驗證了規(guī)劃航跡的可飛性，對其導(dǎo)引點提出了進一步要求。該入場模型的設(shè)計可以改變飛行器入目標(biāo)點的方向，引導(dǎo)飛行器以合適的角度進入目標(biāo)點，從而避開障礙物及威脅區(qū)等，提高系統(tǒng)安全性，具有實際應(yīng)用價值。

2 人工勢場法

人工勢場法的基本思想是將運動的物體視為質(zhì)點，規(guī)劃其在由目標(biāo)的引力場和障礙物的斥力場構(gòu)成的虛擬力場中的運動路徑。

1)引力場與目標(biāo)的位置有關(guān)，目標(biāo)形成的引力勢場為

(1)

引力為引力勢場的負(fù)梯度，即

Fat(X)=-?Uat=-kat|X-Xg|

(2)

其中，kat為引力增益系數(shù)，X是物體的位置，Xg是目標(biāo)點的位置。目標(biāo)對運動物體的引力大小隨著運動物體與目標(biāo)點距離的增大而增大。

2)斥力場與障礙物的位置有關(guān)，且斥力場受障礙物的影響范圍限制也有相應(yīng)的作用范圍，障礙物形成的斥力勢場為

(3)

斥力為斥力勢場的負(fù)梯度，即

Fre(X)=-?Ure(X)

(4)

其中，kre為斥力增益系數(shù)，X是物體的位置，Xob是障礙物的位置，ρo是障礙物的影響距離。在障礙物的有效作用范圍內(nèi)，障礙物對運動物體的斥力大小隨著運動物體與障礙物距離的增大而減小。

若存在多個障礙物，則只需要將每個障礙物形成的斥力場依次進行矢量疊加即可。

3)運動物體在人工勢場中所受的合勢場和合力即為引力場和斥力場的疊加，分別為

(5)

多障礙物影響的人工勢場法受力分析圖如圖1。

圖1 人工勢場法受力分析

3 入場模型設(shè)計

為使飛行器以指定方向到達(dá)目標(biāo)點，特以人工勢場為基礎(chǔ)設(shè)計一入場模型，當(dāng)?shù)竭_(dá)目標(biāo)點指定的作用范圍內(nèi)且在此作用范圍內(nèi)無障礙物的斥力影響時，以目標(biāo)點為中心建立起新的人工勢場，形成一入場模型，從而滿足末端航向約束條件。

以導(dǎo)彈在二維平面內(nèi)飛行為例，當(dāng)導(dǎo)彈在水平面飛行時，其曲率半徑為

(6)

其中：V為導(dǎo)彈速度；θ為彈道傾角，由于在水平面飛行，θ=0；g為重力加速度，本文取g≈9.8m/s2；n為法向過載。

以2ρ為半徑R，令導(dǎo)彈從上方以垂直方向進入目標(biāo)點，建立末端航向約束的入場模型如圖2。

圖2 末端航向約束的入場模型

圖中，O為目標(biāo)點，兩條虛線之間即為要求的從垂直方向入場的范圍。R1、R2組成一對勢場，且兩者代表的作用點總是相反；R3、R4組成另一對勢場；兩對勢場不同時發(fā)揮作用。

當(dāng)導(dǎo)彈從第一象限進入目標(biāo)點指定的作用范圍時，令R2點為引力點，R1點為斥力點，若進入虛線范圍內(nèi)時，則只受目標(biāo)點O的引力作用。

當(dāng)導(dǎo)彈從第二象限進入時，令R1點為引力點，R2點斥力點，若進入虛線范圍內(nèi)時，同樣也只受目標(biāo)點O的引力作用。

當(dāng)導(dǎo)彈從第三、第四象限進入時，則需先進入第一、二象限方能達(dá)到從上方進入目標(biāo)點的目的。故此時令R3點為引力點，R4點為斥力點，促使導(dǎo)彈先飛入第一、二象限；若進入以目標(biāo)點為圓心、半徑為R的圓O內(nèi)，則只受目標(biāo)點O的斥力作用。

以傳統(tǒng)的人工勢場受力模型為參照，構(gòu)建滿足末端航向約束的入場模型的引力模型和斥力模型分別為

(7)

(之后算例都引用以上四個系數(shù)。)

該入場模型的設(shè)計以傳統(tǒng)的人工勢場為基礎(chǔ)，重新規(guī)劃了引力點和斥力點的位置，并在原有基礎(chǔ)上簡化了斥力模型；且形成了多對引、斥力點對，導(dǎo)彈飛行位置決定了采用哪一對引、斥力點對，應(yīng)用靈活。

此入場模型雖以垂直入場為示例，但只要將整個入場模型進行相應(yīng)角度地旋轉(zhuǎn)，即可達(dá)到以任意角度入場的要求，實用且靈活。

4 入場模型仿真

本文模擬導(dǎo)彈的二維平面運動，采用MATLAB進行模擬仿真。

4.1 垂直方向入場

以第3節(jié)內(nèi)容為基礎(chǔ)，仿真當(dāng)導(dǎo)彈已經(jīng)進入目標(biāo)點指定的作用范圍后，從目標(biāo)上方垂直入場的飛行軌跡，由于入場模型關(guān)于y軸對稱，故航跡也近似關(guān)于y軸對稱。現(xiàn)模擬從第一、第三象限入場的飛行軌跡。如圖3、4。

圖3 第一象限進，以垂直方向入場

圖4 第三象限進，以垂直方向入場

圖3所用時間為0.008s ，圖4所用時間為0.012s。仿真結(jié)果表明，在此入場模型中，雖然每次出發(fā)點的位置和方向不同，但仍然能保證導(dǎo)彈最終以垂直方向入場，且滿足實時性要求。

在此入場模型中規(guī)劃的航跡在滿足始端、終端的切線方向和最小拐彎半徑的前提下得到了初始狀態(tài)和終止?fàn)顟B(tài)之間的最短曲線，符合dubins路徑[12-13]要求，此入場模型滿足實際需求。

4.2 任意方向入場

若想從其它方向入場，則只需將入場模型整體往所需方向進行相應(yīng)角度的旋轉(zhuǎn)，則引力點和斥力點也隨之旋轉(zhuǎn)；因受力點發(fā)生偏轉(zhuǎn)，則導(dǎo)彈在入場模型中的受力方向也隨之往指定方向偏轉(zhuǎn)，亦可達(dá)到以指定的其它方向入場的要求。

現(xiàn)仿真導(dǎo)彈在同一位置以同一速度方向分別從45°入場及135°入場情況(其它方向可根據(jù)具體仿真情況相應(yīng)地調(diào)節(jié)增益系數(shù))，規(guī)劃出的航跡如圖5、6所示。

圖5 第三象限進，以45°方向入場

圖6 第三象限進，以135°方向入場

圖5將入場模型順時針旋轉(zhuǎn)45°，導(dǎo)彈最終以45°方向進入目標(biāo)點，飛行所用時間為0.019s；圖6將入場模型逆時針旋轉(zhuǎn)45°，導(dǎo)彈最終則以135°方向進入目標(biāo)點，飛行所用時間為0.012s。仿真結(jié)果表明，只需將入場模型進行相應(yīng)角度地旋轉(zhuǎn)，盡管出發(fā)點的位置和方向不變，但仍能按照指定要求的角度進入目標(biāo)點；且由于入場模型的旋轉(zhuǎn)不受限制，故導(dǎo)彈能夠從360°各個方向都進入目標(biāo)點，應(yīng)用靈活；飛行所用時間也滿足實時性要求。

由以上兩小節(jié)內(nèi)容可見，該入場模型不僅能滿足末端航向約束，使飛行器以指定方向入場，且應(yīng)用靈活，只需將入場模型進行相應(yīng)角度地旋轉(zhuǎn)即可隨意改變?nèi)雸龇较颍哂休^好的應(yīng)用價值。

4.3 整體飛行對比

將末端入場模型融入到整個人工勢場算法中，并將傳統(tǒng)的人工勢場法和加入末端航向約束的人工勢場法進行對比，若要求導(dǎo)彈最終以-45°方向進入目標(biāo)點，兩者仿真如下圖7圖8。

圖7 未加末端航向約束圖

圖8 末端以-45°入場的航跡圖

圖7未添加任何末端航向約束，導(dǎo)彈最終在引力作用下到達(dá)目標(biāo)點，但并不是從-45°方向進入，方向最終由目標(biāo)點引力和導(dǎo)彈速度決定，所用時間為0.856s；圖8在末端添加了入場模型，導(dǎo)彈最終能夠以-45°方向進入了目標(biāo)點，滿足末端航向約束，所用時間為0.738s。加入末端入場模型后，雖航行路程增加，但由于進入指定范圍后，需調(diào)動入場模型程序，無需再計算每個障礙物對當(dāng)前點的斥力大小，使程序計算量降低，提高了實時性。

增加末端航向約束后，航跡末端發(fā)生了明顯的偏轉(zhuǎn)，將末端進入目標(biāo)點的航跡進行放大，得到如圖9。

圖9 末端以-45°入場的局部放大圖

經(jīng)末端局部放大后可觀察到，當(dāng)即將到達(dá)目標(biāo)點時，航跡發(fā)生明顯偏轉(zhuǎn)，即導(dǎo)彈朝-45°方向偏移直至接近目標(biāo)點，最終以-45°方向進入目標(biāo)點，由此證明該入場模型放在實時應(yīng)用中也能滿足末端航向約束。

4.4 航跡跟蹤情況

為驗證所規(guī)劃航跡是否具有可飛性，需對其進行航跡跟蹤以判斷合理性。

比例導(dǎo)引法[14-15]理論成熟，成果豐富，因其形式簡單、導(dǎo)引精度高、工程技術(shù)上易于實現(xiàn)等優(yōu)點，成為一種較為普遍的航跡跟蹤方法。目前世界各國的導(dǎo)彈大多采用比例導(dǎo)引律，制導(dǎo)系統(tǒng)結(jié)構(gòu)簡單且射擊精度高、可靠性高。

比例導(dǎo)引法要求導(dǎo)彈在飛向目標(biāo)點的導(dǎo)引過程中，速度矢量的旋轉(zhuǎn)角速度與目標(biāo)線的旋轉(zhuǎn)角速度成比例，本文中目標(biāo)點的位置靜止不動，導(dǎo)引關(guān)系如圖所10示。

圖10 導(dǎo)彈-目標(biāo)相對關(guān)系示意圖

當(dāng)導(dǎo)彈和目標(biāo)在同一平面內(nèi)，其導(dǎo)引關(guān)系式為

(8)

采用比例導(dǎo)引法對以上所規(guī)劃的航跡進行跟蹤，如圖11所示。

圖11 航跡跟蹤情況

由圖11可見，跟蹤的航跡與規(guī)劃的航跡基本吻合，由此證明引入此入場模型而規(guī)劃出的航跡仍然具有可飛性，具有實際應(yīng)用價值。

當(dāng)導(dǎo)彈進入入場模型作用范圍的速度方向與規(guī)定的進入目標(biāo)點的方向相差較大時，在入場模型內(nèi)可能出現(xiàn)較大的轉(zhuǎn)彎角度。當(dāng)末端具有較大轉(zhuǎn)彎角度時，末端導(dǎo)引點不能與入場前的航跡導(dǎo)引點一概而論，末端航跡導(dǎo)引點的質(zhì)量決定了導(dǎo)彈能否順利按照規(guī)劃航跡以指定方向到達(dá)目標(biāo)點。

導(dǎo)引點的數(shù)量是影響航跡跟蹤情況的關(guān)鍵因素之一。導(dǎo)引點數(shù)量的多少往往影響著導(dǎo)彈攻擊目標(biāo)的效率和質(zhì)量，對圖11的末端航跡進行等分取導(dǎo)引點，現(xiàn)分別仿真取不同數(shù)量的導(dǎo)引點進行跟蹤，如圖12圖13所示。

圖12 含4導(dǎo)引點的跟蹤情況

圖13 含3導(dǎo)引點的跟蹤情況

經(jīng)比較，含4導(dǎo)引點的航跡跟蹤情況與規(guī)劃航跡基本吻合；而在只有3個導(dǎo)引點的情況下，末端航跡與規(guī)劃航跡偏差較大，故在仍然采用3個導(dǎo)引點的情況下需重新調(diào)整其導(dǎo)引點的位置，將其中兩個導(dǎo)引點分別放置在轉(zhuǎn)彎波動較大的位置，方便其按照規(guī)劃航跡轉(zhuǎn)彎，改進后的跟蹤情況如下圖所示：

圖14 改進3導(dǎo)引點跟蹤情況

對3個導(dǎo)引點的位置重新進行分布后，跟蹤出的航跡與規(guī)劃航跡誤差不大，在不改變導(dǎo)引點數(shù)量的前提下其位置分布對整段航跡跟蹤具有重要的影響，需將導(dǎo)引點安置在滿足轉(zhuǎn)彎角度的位置上。

可見，對于末端具有較大角度變動的情況，需要根據(jù)角度變化幅度選取合適的航跡點進行追蹤，如此飛行器方能按照指定航向到達(dá)目標(biāo)點。故與傳統(tǒng)取點方式不同，在該種情況下采用比例導(dǎo)引法進行跟蹤航跡時，需將整段航跡分為兩部分進行取點：第一部分為入場前的航跡，該段航跡的導(dǎo)引點只需按照正常情況下取點即可；第二部分為入場航跡，該段航跡需要根據(jù)角度變化幅度選取合適質(zhì)量(數(shù)量及位置)的導(dǎo)引點，從而使飛行器在滿足自身性能的情況下以指定航向進入目標(biāo)點。

5 結(jié)束語

以目標(biāo)點為中心，建立入場模型，實現(xiàn)了飛行器以指定方向進入目標(biāo)點的目的，滿足了末端航向約束，比例導(dǎo)引法也驗證了其可行性。仿真結(jié)果表明，此入場模型能夠使飛行器以指定方向入場，且任意方向都能夠?qū)崿F(xiàn)，并滿足可飛性。此外，本文將末端航向約束簡化為二維問題，為三維問題的求解提供了較好的理論依據(jù)。