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

趙正戴，謝如恒，南 英

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

1 引言

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

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

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

2 人工勢場法

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

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

(1)

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

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

(2)

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

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

(3)

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

Fre(X)=-?Ure(X)

(4)

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

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

3)運(yùn)動(dòng)物體在人工勢場中所受的合勢場和合力即為引力場和斥力場的疊加，分別為

(5)

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

圖1 人工勢場法受力分析

3 入場模型設(shè)計(jì)

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

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

(6)

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

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

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

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

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

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

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

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

(7)

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

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

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

4 入場模型仿真

本文模擬導(dǎo)彈的二維平面運(yùn)動(dòng)，采用MATLAB進(jìn)行模擬仿真。

4.1 垂直方向入場

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

圖3 第一象限進(jìn)，以垂直方向入場

圖4 第三象限進(jìn)，以垂直方向入場

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

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

4.2 任意方向入場

若想從其它方向入場，則只需將入場模型整體往所需方向進(jìn)行相應(yīng)角度的旋轉(zhuǎn)，則引力點(diǎn)和斥力點(diǎn)也隨之旋轉(zhuǎn)；因受力點(diǎ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 第三象限進(jìn)，以45°方向入場

圖6 第三象限進(jìn)，以135°方向入場

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

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

4.3 整體飛行對(duì)比

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

圖7 未加末端航向約束圖

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

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

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

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

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

4.4 航跡跟蹤情況

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

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

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

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

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

(8)

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

圖11 航跡跟蹤情況

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

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

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

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

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

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

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

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

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

5 結(jié)束語

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