風場環境下的巡飛彈航跡跟蹤運動補償算法

李增彥， 李小民， 劉秋生

(1.軍械工程學院 無人機工程系， 河北 石家莊 050003；2.軍械工程學院 彈藥工程系， 河北 石家莊 050003)

風場環境下的巡飛彈航跡跟蹤運動補償算法

李增彥1， 李小民1， 劉秋生2

(1.軍械工程學院 無人機工程系， 河北 石家莊 050003；2.軍械工程學院 彈藥工程系， 河北 石家莊 050003)

針對巡飛彈航跡跟蹤過程受風速、風向影響的問題，在風場環境下，通過建立運動模型，研究并提出了基于變增益虛擬參考點和系統外回路動態補償的非線性制導律算法。根據制導律參數中固定距離限制的問題，設計了變增益虛擬參考點并分析了航跡跟蹤的穩定性條件，針對系統外回路滯后特性設計了動態反饋補償因子，同時給出了參數計算方法及運動約束條件。通過數學仿真方法分別對無風及有風條件下的制導律進行驗證，考慮到實際飛行試驗困難的問題，基于vc++搭建了半實物實時仿真平臺，通過硬件在回路仿真，驗證了航跡跟蹤性能。結果表明：改進制導律可有效克服風干擾、具有較高的跟蹤精度且易于工程實現。

兵器科學與技術； 巡飛彈； 航跡跟蹤； 非線性制導律； 運動補償； 半實物仿真

0 引言

巡飛彈作為新型智能化彈藥，集無人機技術與彈藥技術于一體[1]，無論是偵察還是攻擊任務，精確的航跡跟蹤是完成飛行任務的必備條件及前提。同時，模擬驗證機載控制器及航跡跟蹤算法的性能是研制過程中性能考核等急需手段。

巡飛彈沿設定的航點自主跟蹤飛行，可節省能源消耗、遂行作戰任務，飛行環境中不可避免地存在風干擾[2-3]，其中側風及順風干擾對航跡跟蹤的影響較為明顯。目前已有大量文獻對飛行器的航跡跟蹤算法進行了相關研究。Sujit等[4]對比分析了實現較易、魯棒性強的常用航跡跟蹤算法，可將其思想總結為5類：虛擬運動目標法[5-6]、視線導引法[7]、線性二次型法[8]、向量場法[9]、虛擬目標距離法[10]。前3類方法的魯棒性受風擾動影響且存在較大的橫向偏差；向量場法橫向誤差最小，但需調整的參數較多，并存在抖震現象。虛擬目標距離法在算法實現、參數調整及橫向偏差方面具有較大優勢。Sanghyuk 等利用虛擬目標距離法的思想研究了非線性制導算法并將該算法推廣到三維航跡跟蹤中[11]，在直線、曲線、圓、跟蹤運動目標等任意類型的航跡跟蹤中均具有較好的實驗結果，但該方法仍存在一定誤差，固定距離L的限制導致不同風向條件下魯棒性較差，從而影響航跡跟蹤的穩定性，同時未考慮系統外回路的反饋補償。

在實驗驗證環節，與常用的現場調試方法相比，大多數算法對飛行器的數學模型進行了簡化，采用邊界條件仿真測試進行分析對比。而在飛行器控制及導航算法研究中，半實物仿真方法具有仿真模型精度高、能夠快速對控制算法及參數進行更改等優點[12]，尤其針對遠程飛行器可以及時地判斷設計的可行性、提高開發效率、降低研制周期和研究成本。

本文首先根據巡飛彈受風場影響的問題建立運動模型，針對非線性制導律中固定距離限制及系統外回路滯后特性的問題進行研究及改進，并從算法實現及工程應用角度給出了參數計算方法及運動約束條件；然后通過數學仿真方法對制導律進行驗證并結合半實物仿真思想，基于vc++搭建實時仿真平臺；最后通過硬件在回路仿真，驗證航跡跟蹤性能。

1 風場條件下的巡飛彈制導律

1.1 運動模型

風擾動條件下，采用北東地大地坐標系，巡飛彈在空間中的質心運動學方程[2]可描述為

(1)

式中：x、y、z表示大地坐標系下3個坐標分量；va、γ、χ分別為空速、航跡傾角和航跡偏角；vw為飛行環境中風速，并在x、y、z方向進行分解。針對二維水平面的航跡跟隨問題，控制巡飛彈航跡傾角γ=0°，則飛行器的高度z恒定，(1)式可簡化為

(2)

式中：vg為巡飛彈的對地速度。

1.2 制導律

巡飛彈航跡跟蹤控制過程如圖1所示。

圖1 非線性制導律幾何示意圖Fig.1 Geometric sketch of nonlinear guidance law

圖1中，期望航跡為A至B，當AB為直線時，巡飛彈至AB間距離為d，參考點P至AB距離為dr.L為巡飛彈至參考點的固定距離，其與vg間夾角為η，當飛行過程中滾轉角為φ時，總升力為FL，O、R、as分別為向心運動的圓心、半徑和加速度。

由圖1中幾何關系及牛頓第二定律可知：

(3)

式中：m為巡飛彈質量

傳統方法在航跡跟蹤控制中，通常采用空速va作為速度控制回路的參考量，一般為恒空速控制。當存在風干擾時，飛行器的空速受風速影響，導致滾轉角控制量對風速較為敏感，因此采用飛行器對地速度vg作為制導律中速度參考，從而得出如下制導律[10-11]：

(4)

為了避免飛行器在風場環境中失速，采用恒空速控制方法。由(2)式及(4)式分析可知，制導律中滾轉角指令同時受vg及參考距離L影響。當L為常量時，飛行過程中風速、風向改變后，vg隨之改變，使得制導律中控制量受風場影響，并且制導律中未考慮受控對象的運動特性。

2 改進制導律

為了改進制導律受風干擾及受控對象影響的特性，首先分析變增益參考距離方法及其穩定性，利用運動補償方法改善控制器滯后問題，并根據制導律特點給出參數計算方法及飛行器運動約束條件。

2.1 變增益法及其穩定性分析

由(4)式中的飛行器制導律可知，當對地速度vg與航線夾角η趨于0°時，滾轉角控制量為0°，飛行器能夠保持穩定水平飛行狀態，即

(5)

(6)

(7)

令C為常量，使得變增益參考距離l∝vg.

為了確定C的取值范圍及對控制系統穩定性的影響，將飛行器滾轉響應特性看成1階慣性環節時，制導律控制回路可描述為圖2中的結構。

圖2 制導律控制回路Fig.2 Guidance law control loop

其特征方程可整理得

(8)

式中：Tr為慣性系統的時間常數。當Tr=0時，認為飛行器滾轉響應不存在慣性特性；當Tr>0時，反應了飛行器的滾轉慣性，且慣性越大，時間常數越大。

通過特征方程利用根軌跡法對系統穩定性進行分析，選取不同時間常數Tr并令C取值為等差數列{0.5,1,…,10}得到圖3特征根趨勢。由圖3可知：當飛行器滾轉過程不存在慣性環節時，全部特征根位于左半平面，系統為穩定狀態且不受C的影響；當飛行器受滾轉慣性及飛行控制系統作用時，隨著C的增大且當C>Tr時系統由不穩定狀態變為穩定狀態，因此必須保證控制參數中C始終大于Tr使系統穩定。

圖3 特征方程根軌跡Fig.3 Root locus of characteristic equation

2.2 制導律反饋補償

圖4 加速度動態反饋補償Fig.4 Dynamic feedback compensation of acceleration

由于側風會對飛行器產生橫側向加速度，利用對地速度vg及空速va估計風速角∠ga，當側風影響較大時，以比例系數K作為反饋增益，δ為分割區間角度，考慮如下區間函數：

(9)

則經過動態反饋補償的改進制導律為

(10)

2.3 制導律參數計算及運動約束

由圖1可知，η為向量vg與向量l之間的夾角，直接計算虛擬目標點坐標較難，采用向量形式對夾角η進行計算。設地速vg及航線AB間夾角η1為

(11)

飛行器到航線AB的垂直距離d為

(12)

則由(12)式可得虛擬距離l與航線AB間夾角η2為

(13)

從而制導律中夾角η可由(11)式～(13)式得出：

(14)

由于固定距離L的限制導致航點切換策略受飛行速度影響，制導律中過大的加速度使滾轉角設定值φc超出飛行器姿態回路動態范圍。因此采用虛擬距離l=Cvg作為航點切換條件，在轉彎半徑中加入接近距離延遲量vgΔt并作如下轉彎半徑及滾轉角約束：

(15)

式中：Rmin為飛行器最小轉彎半徑。

3 數學仿真及結果分析

1)在無風條件下跟蹤四邊形直線航跡如圖5所示。

圖5 無風條件下跟蹤四邊形直線航跡Fig.5 Quadrilateral flight path without wind

由圖5中仿真結果可知：如圖5(a)所示，在無風條件下，由于參數C為定值且vg不受風的影響，非線性制導律及改進后的制導律航跡基本一致；而圖5(b)中橫向偏差d表明，加入動態反饋補償后，控制律可以有效地改善航跡跟蹤能力，提升跟蹤速度；非線性制導律中虛擬參考距離(同時為航點切換條件)為固定值L，從圖5(c)可以看出改進后虛擬距離l成為動態變量，使航點切換時機得到控制及改善；由圖5(d)可知，對加速度進行動態反饋補償后，提高了滾轉角指令值的動態范圍，航點切換時可達最大角度約束，該約束角度可根據飛行控制系統性能進行設置，而在飛行器接近水平飛行狀態時，滾轉角指令更加平滑且無嚴重超調量。

2)通常的風場條件只考慮水平或垂直方向的風向影響，且風速遠小于飛行器速度。為驗證制導律的抗風性能，設飛行環境中存在常值風vw,x為8 m/s，vw,y為6 m/s，約為0.5vg，在該條件下跟蹤四邊形直線航跡，如圖6所示。

圖6 風場條件下跟蹤四邊形直線航跡Fig.6 Quadrilateral straight flight path with wind

由圖6中仿真結果可知：如圖6(a)所示，在非線性制導律的作用下當存在常值風的影響時，飛行器嚴重偏離航線，跟蹤精度、速度受風向、風速的影響較大，而飛行航向的改變使得風場對飛行器的影響也在不斷變化；從圖6(b)中可以看出，改進后制導律的跟蹤速度明顯優于非線性制導律，加入動態反饋補償后，在跟蹤時的快速性得到小幅度提升且超調量變小；圖6(c)則表明，受風向、風速的影響，改進后的制導律能夠自適應調整虛擬參考距離及航點切換條件，使得航點跟蹤時的抗風能力得到加強，而非線性制導律無法改變風場環境對于飛行器速度的影響；在風干擾環境下，通過加速度動態反饋補償，從圖6(d)中可以看出，進入直線航線過程中飛行器滾轉角指令仍然較為平滑且無嚴重超調量，有利于飛行控制系統對于飛行器的控制。

4 半實物仿真實驗驗證

4.1 整體框架及實現方式

巡飛彈作為遠程飛行器，控制及導航算法研究實驗無法直接進行驗證。為了實現其控制及導航等研究需求，必須搭建考慮飛行器氣動參數攝動，傳感器模型等的飛行仿真框架[13-14]，利用可靠的手段完成實驗，在驗證環節，目前常用方法為基于Matlab/Simulink的純軟件數學仿真和硬件在回路仿真，已有的仿真模式暴露出一些不足，如數字仿真全為模擬數據不具備真實性，仿真模型執行效率低且控制周期不等長，控制算法未在飛控系統中運行等。

為了保證仿真模型運行的實時性，基于VS2010采用多線程方式進行編程，搭建了巡飛彈數學模型，代碼執行周期設置為5 ms，并利用Windows RTX硬實時操作系統保證了模型解算的實時性，考慮到飛行器姿態及航跡控制中的調試需求，利用OpenGL實現飛行器狀態可視化；為驗證控制算法硬件可實現性及改進制導律的實用性，將制導律寫入基于STM32F405的飛行控制系統中。

半實物仿真系統框架如圖7所示：硬件在回路仿真時，先將飛行控制系統固聯于三軸轉臺內框中并進行航點裝訂；仿真計算機中運行六自由度仿真模型，仿真數據(姿態角及角速度)通過VMIC反射內存卡發送至轉臺控制計算機[13](通信協議基于UDP實現)，該數據可直接驅動跟蹤精度為角秒級的三軸轉臺進行姿態跟蹤；同時仿真模型中模擬傳感器數據(包括全球定位系統(GPS)、空速、加速度、磁強、氣壓等，在理想飛行器仿真模型的基礎上加入傳感器誤差模型噪聲[13-14])通過仿真計算機發送至飛行控制系統，而轉臺旋轉角速率信息可利用飛行控制系統中陀螺儀直接測量，飛行控制系統接收傳感器數據并通過制導律中控制參數解算輸出飛行器控制舵量，該控制舵量直接發送至仿真計算機，仿真模型接收到控制信號后進行模型的姿態控制，形成閉環回路。

圖7 半實物仿真系統框架Fig.7 Framework of HIL system

4.2 含GPS信息參數計算

由于第3節中位置坐標位于平面坐標系，而GPS采集的位置信息屬于經緯高(LLA)坐標系，為了計算兩點間距離及航向需轉換到地心地固(ECEF)坐標系。假設飛行控制系統中兩點為A=(λA,φA)，B=(λB,φB)，λ和φ分別代表緯度和經度，利用球面半正矢公式可知AB間存在如下關系：

(16)

式中：Re為地球半徑；Δφ、Δλ分別為A、B兩點之間的經度、緯度差。又因為Haversine公式hav(·)為

(17)

(18)

利用等距方位投影公式可知向量AB可分解為

(19)

式中：ABe表示AB的水平分量；ABn表示AB的垂直分量。從而得出AB的航向角ψAB為

(20)

最終飛行控制系統制導律中含GPS信息的夾角η可由(18)式和(20)式求得，而加速度反饋中航向角ψ的導數為

(21)

式中：r、q分別為飛行器機體坐標系下運動的偏航角速度和滾轉角速度；θ為大地坐標系下運動的俯仰角。從而根據4.2節公式，適用于工程實現的完整制導律公式中滾轉角期望值可由GPS信息及航點位置進行計算。

4.3 實驗條件及結果

針對某小型巡飛彈進行自動航跡跟隨實驗，六自由度仿真模型中存在一定范圍氣動參數攝動值，傳感器模型中加入隨機白噪聲，初始高度500 m，設定巡飛速度20 m/s，風速vw，x、vw，y為-7 m/s，約為0.5va；在半實物仿真實驗驗證環節，設制導律中參數C=4.33，k=-0.7，|φc|≤50°，任務航點仍采用四航點導航方式，位置見表1，表中h為飛行高度。

表1 航點位置Tab.1 Waypoint position

巡飛彈經過約20 s空中搭載，通過空投方式進入自動任務階段。分別對改進制導律、L非線性制導律在有風及無風條件下進行仿真實驗，參數調整過程中，為了保證與數值仿真條件相對應，設置恒空速控制方式使其速度穩定到20 m/s，設置高度控制器使其高度穩定到200 m. 經過控制器參數調整，仿真結果如圖8所示，其中圖8(a)中圖例適用于圖8(b)和圖8(c)。

圖8 巡飛彈半實物仿真結果Fig.8 HIL simulation result of loitering munition

通過飛行控制系統數據記錄儀中數據對半實物仿真實驗結果進行對比分析，如圖8所示：巡飛彈自動任務模式下通過了4個航點，從圖8(a)中可以看出，巡飛彈均能按照指定的路徑進行航跡跟隨，無風條件下，L制導律與改進制導律具有相同的參考距離，因此其飛行軌跡基本一致，當存在一定角度約為0.5va的風場影響時，采用L制導律的飛行器隨著風向變化，航跡跟隨過程產生嚴重的偏差，而采用改進制導律的飛行器能夠根據風向自適應調整航點切換時機，航線跟隨誤差也顯著減小，巡飛彈能夠快速進入指定的航線；圖8(b)中繪制了航跡跟蹤過程中飛行器高度變化情況，由于高度控制器的影響，巡飛彈實際飛行高度與第1節中理想條件會存在一定偏差，存在“掉高”現象，但高度誤差始終保持在10 m以內，可知存在高度誤差影響時，改進制導律仍然適用；圖8(c)為巡飛彈飛行過程中的空速變化，采用速度控制回路，巡飛彈能夠保證在(20±5) m/s的速度下執行飛行任務，制導律對空速要求較低，受空速變化影響小。

5 結論

風場環境下的巡飛彈執行自動航線任務時，傳統制導律方法受風速、風向影響較大，實現過程復雜，本文在建立風場條件下巡飛彈運動模型的基礎上，采用了非線性制導律；針對非線性制導律中固定距離限制的問題，設計了變增益虛擬參考點，分析并給出了航跡跟蹤的穩定性條件；針對系統外回路滯后特性進行了加速度反饋補償設計，同時給出了制導律中參數計算方法及運動約束條件；針對實際飛行驗證困難的問題，結合半實物仿真思想，基于vc++搭建了實時仿真平臺，采用硬件在回路仿真閉環試驗方法，驗證了航跡跟蹤精度。數學及半實物仿真實驗結果表明，所設計的控制器具有如下特點：

1)在非線性制導律中設計變增益虛擬目標距離l使得飛行器能夠克服約飛行速度50%左右任意方向風的影響，可以自適應調整航點切換距離。

2)引入加速度動態反饋補償后，改進制導律提高了滾轉角指令值的動態范圍，跟蹤精度及快速性得到提升。

3)當飛行器在航點切換或者高度變化情況下，改進制導律仍具有較好的航跡跟蹤能力，且受飛行過程中空速變化帶來的影響較小。

4)半實物仿真實驗及制導律的硬件實現表明，所設計的控制器易于工程實現，參數計算方法為研發人員提供了借鑒。

References)

[1] 龐艷珂，韓磊，張民權，等. 攻擊型巡飛彈技術現狀及發展趨勢[J]. 兵工學報, 2010, 31(增刊2): 149-152. PANG Yan-ke, HAN Lei, ZHANG Min-quan, et al. Status and development trends of loitering attack missiles [J]. Acta Armamentarii, 2010, 31(S2): 149-152. (in Chinese)

[2] 張坤，高曉光. 未知風場擾動下無人機三維航跡跟蹤魯棒最優控制[J]. 電子與信息學報, 2015, 37(12): 3009-3015. ZHANG Kun, GAO Xiao-guang. Robust optimal control for unmanned aerial vehicles’ three-dimensional trajectory tracking in wind disturbance [J]. Journal of Electronics & Information Technology, 2015, 37(12): 3009-3015. (in Chinese)

[3] 管軍，易文俊，常思江，等. 某型無人機三維空間航跡跟蹤控制方法研究[J]. 兵工學報, 2016, 37(1): 64-70. GUAN Jun, YI Wen-jun, CHANG Si-jiang, et al. Study of flight path tracking and control of an UAV in 3D space [J]. Acta Armamentarii, 2016, 37(1): 64-70. (in Chinese)

[4] Sujit P B, Srikanth S, Jo?o B S. Unmanned aerial vehicle path following: a survey and analysis of algorithms for fixed-wing unmanned aerial vehicles [J]. IEEE Control System Magazine, 2014, 34(1): 42-59.

[5] Zhang J, Li Q, Cheng N, et al. Nonlinear path-following method for fixed-wing unmanned aerial vehicles [J]. Journal of Zhejiang University-Science C(Computers & Electronics), 2013, 14(2): 125-132.

[6] Eran D B M, Peter W G. Synthetic-waypoint guidance algorithm for following a desired flight trajectory [J]. Journal of Guidance Control and Dynamics, 2010, 33(2): 601-606.

[7] Venkatraman K, Mani V, Kothari M, et al. A suboptimal path planning algorithm using rapidly-exploring random trees [J]. International Journal of Aerospace Innovations, 2010, 2(1): 93-104.

[8] Ratnoo A. Adaptive optimal path following for high wind flights [C]∥Proceedings of the 18th IFAC World Congress. Milano, Italy: IFAC, 2011: 12985-12990.

[9] Nelson D R, Barber D B, Mclain T W, et al. Vector field path following for miniature air vehicles [J]. IEEE Transactions on Robotics, 2007, 23(3): 519-529.

[10] Park S, Deyst J, How J P. A new nonlinear guidance logic for trajectory tracking [C]∥AIAA Guidance, Navigation, and Control Conference and Exhibit. Rhode Island, US: AIAA, 2004:16-19.

[11] Park S. Autonomous aerobatic on commanded path [J]. Aerospace Science and Technology, 2012, 22(1): 64-74.

[12] 余波，朱紀洪，范勇，等. 飛行控制系統大閉環半物理仿真研究[J]. 航天控制, 2010, 28(2): 51-55. YU Bo, ZHU Ji-hong, FAN Yong, et al. Research on large-scale hardware-in-closed-loop simulation of flight control system [J]. Aerospace Control, 2010, 28(2): 51-55. (in Chinese)

[13] 李軍偉，程詠梅，陳克喆，等. 基于飛行仿真的捷聯慣導算法測試平臺[J]. 中國慣性技術學報, 2012, 20(5): 530-535. LI Jun-wei, CHENG Yong-mei, CHEN Ke-zhe, et al. Test platform for SINS algorithm based on flight simulation [J]. Journal of Chinese Inertial Technology, 2012, 20(5): 530-535. (in Chinese)

[14] 劉莉，王巖松，周思達，等. 考慮彈體彈性的導彈半物理仿真方法與影響分析[J]. 北京航空航天大學學報, 2016, 42(4): 639-645. LIU Li, WANG Yan-song, ZHOU Si-da, et al. Hardware-in-the-loop simulation method and influence analysis of missiles considering the elasticity of the body [J]. Journal of Beijing University of Aeronautics and Astronautics, 2016, 42(4): 639-645. (in Chinese)

Trajectory Tracking Algorithm for Motion Compensation of Loitering Munition under Wind Environment

LI Zeng-yan1， LI Xiao-min1， LIU Qiu-sheng2

(1.Department of Unmanned Aerial Vehicle Engineering, Ordnance Engineering College, Shijiazhuang 050003, Hebei, China;2.Department of Ammunition Engineering, Ordnance Engineering College, Shijiazhuang 050003, Hebei, China)

The trajectory tracking process of loitering munition is affected by wind speed and wind direction. A motion model with wind is established, and a nonlinear guidance law based on the variable gain virtual reference point and outer loop motion compensation is proposed. According to the fixed distance limit of the guidance law parameters, the variable gain virtual reference point is designed, and the stability condition of trajectory tracking is analyzed. Then the dynamic feedback compensation factor is designed for the hysteresis characteristic of the system’s outer loop, and the calculation method of the parameters and the kinematic constraints are also given. The guidance law is verified with wind and without wind through mathematical simulation. Considering the difficulty of actual flight, the tracking performance is tested on the semi physical real-time simulation platform which is built based on vc++. The results show that the improved guidance law can effectively overcome the wind disturbance, and it has high tracking accuracy but is easy to be realized.

ordnance science and technology; loitering munition; trajectory tracking; nonlinear guidance law; motion compensation; semi physical simulation

2016-06-08

武器裝備“十二五”預先研究項目(51325050101)

李增彥(1987—), 男, 博士研究生。E-mail: lizengyan2012@163.com; 李小民(1968—), 男, 教授, 博士生導師。E-mail: lxmfy2000@263.net

V249.1

A

1000-1093(2016)12-2377-08

10.3969/j.issn.1000-1093.2016.12.025