高速旋轉彈飛行姿態(tài)磁測解算方法

龍達峰，劉 俊，2，張曉明，李 杰

（中北大學1.電子測試技術重點實驗室；2.儀器科學與動態(tài)測試教育部重點實驗室，太原030051）

常規(guī)彈藥制導化、靈巧化改造是當今常規(guī)彈藥技術發(fā)展的主流方向，彈體飛行過程中姿態(tài)角的實時精確測量是實現(xiàn)其精確控制飛行的前提與技術瓶頸[1－2]。受常規(guī)彈藥的高旋轉、高沖擊、小體積等惡劣應用環(huán)境的限制，所選用的導航傳感器必須滿足抗高過載、小體積、量程與精度合適等需求，能采用的姿態(tài)傳感器甚少，主要有 MEMS陀螺儀、太陽方位角傳感器、地磁場傳感器等[3－4]。以地磁場作為測量基準，通過磁傳感器在彈體內的適當布陣，由相關算法解算彈體姿態(tài)參數(shù)的自主導航方法是當前磁測研究的熱點[2，5－7]。磁測系統(tǒng)的姿態(tài)方程是非線性超越方程組，通常采用弦截法、拋物線法和牛頓迭代法等進行數(shù)值解算姿態(tài)參數(shù)。牛頓法是有效的方法，但其解算的收斂性和實時性與迭代初值相關。針對低旋轉彈體的全彈道姿態(tài)解算，可以把前一解算點姿態(tài)作為當前解算迭代初值，這樣能夠把迭代過程引入收斂區(qū)，加快算法的收斂速度。對于高速旋轉彈體，如采用這種方法，收斂緩慢甚至發(fā)散。因此，本文利用遺傳算法在數(shù)值求解中能快速逼近最優(yōu)解的特性，讓遺傳算法在姿態(tài)區(qū)間內對根進行快速搜索得到近似解，并以此近似解作為牛頓迭代法的迭代初值，然后采用牛頓法求取數(shù)值最優(yōu)解，保證算法的收斂性與實時性。

1 磁測姿態(tài)解算方法

1.1 磁測姿態(tài)解算的基本公式

根據磁阻傳感器姿態(tài)測量原理，捷聯(lián)磁阻傳感器測量值與導航系中地磁分量的關系為

式中：為地磁矢量在彈體坐標系中的分量為地磁矢量在導航坐標系中的分量。以地面發(fā)射坐標系為導航參考坐標系，彈體坐標系的原點O取在彈體質心，Ox軸與彈體縱軸重合，指向頭部為正，Oy軸位于彈體縱向對稱面內與Ox軸垂直，指向上為正，Oz軸垂直于Oxy平面，方向按右手直角坐標系確定。則導航坐標系與彈體坐標系的姿態(tài)矩陣Cbn為

式中：θ為俯仰角，ψ為偏航角，γ為橫滾角。由式（1）整理后可得：

該非線性方程組是姿態(tài)解算的基本公式，在不考慮探測盲區(qū)的情況下，利用數(shù)值解算可以提取全彈道飛行姿態(tài)參數(shù)。

1.2 基于牛頓迭代法的姿態(tài)解算方法

牛頓法是解決非線性無約束最優(yōu)化問題的方法，在給定合適的初值情況下，它能夠快速收斂到方程的解，根據牛頓迭代法求解非線性方程的基本思想，首先將方程組（2）簡寫成如下形式：

對大多數(shù)炮射高速旋轉彈，在標準氣象條件下其距離散布遠大于方向散布，偏航角變化很小。姿態(tài)解算時ψ，和均可視為已知量，通過解算方程組（2）可得到彈體姿態(tài)角θ和γ。假設X＊＝（θ＊γ＊）T為近似解，X0＝（θ0γ0）T為迭代初值。方程組（3）在近似解X＊處的一階Taylor展開式為，則式（4）可簡寫為

若ATA可逆，式（5）存在最小二乘解為ΔX＝（ATA）－1ATB。此時姿態(tài)方程（5）的第一個近似解為X＊＝X0＋ΔX。迭代過程中根據max‖ΔX‖≤ε判斷方程是否收斂，ε為迭代精度。若當次迭代的解不滿足收斂條件，在進行下一次迭代前，需將當前近似解作為下一次迭代的初始值，重復迭代直到滿足精度。因此，通過求解線性方程組（5），可以間接得到原非線性方程組（2）的近似解。

2 遺傳－牛頓法的姿態(tài)優(yōu)化算法

上述牛頓迭代法中，過程的收斂性與初始近似解X0的選取相關，初始點的選取只能在X＊的合適鄰域內，但實際上很難檢驗所選X0是否靠近X＊。針對低旋轉彈體的全彈道姿態(tài)解算，可以把前一解算點姿態(tài)作為當前解算迭代初值，這樣能夠把迭代過程引入收斂區(qū)，加快算法的收斂速度。但對于旋轉速度達30 r/s的高速旋轉彈體，這種方法收斂緩慢甚至發(fā)散。為了克服牛頓法的上述缺點，利用遺傳算法在數(shù)值求解中能快速逼近最優(yōu)解的特性，首先確定目標函數(shù)f（X）解的上下界，讓遺傳算法在盡量小的區(qū)間內對根進行搜索近似解，以該近似解作為牛頓法的迭代初值X0，以保證算法的收斂性[8]。

本文所述遺傳－牛頓法的姿態(tài)優(yōu)化算法步驟如下[9－10]。

①根據俯仰角的定義域[－90°，90°]，選定遺傳算法的初始群體Ppop（s），s＝1，2，…，7。初始群體由7個染色體組成，染色體是俯仰角的8位二進制編碼，分別為俯仰角－90°，－60°，－30°，0°，30°，60°，90°對應的二進制編碼。

②對Ppop（s）初始群體中的每一個染色體分別計算其適應度函數(shù)fi，若fi趨于穩(wěn)定，計算概率：

③以此概率分布從Ppop（s）中隨機選出染色體用于構成新種群Pnewpop（s＋1）：Pnewpop（s＋1）＝｛Ppop（s）｜s＝1，2，…，7｝，設交叉概率Pc＝0.5，變異概率Pm＝0.01，通過交叉與變異操作形成一個新的種群Pnewpop（s＋1），重復上述計算操作（包括適應度函數(shù)與概率、交叉以及變異等），直到俯仰角的解算精度達到1°之內，并將此解作為應用牛頓法初始值θ0。

④橫滾角的初值γ0求解方法與俯仰角基本相同，篇幅所限，不再列出詳細解算過程，最后，以X0＝（θ0γ0）作為初始值，利用牛頓迭代法解算姿態(tài)方程的最優(yōu)解。

3 算法仿真及誤差分析

以某高速旋轉彈作為仿真對象，本文采用六自由度剛體彈道方程，在標準氣象條件下，利用 Matlab仿真計算得到全彈道姿態(tài)角數(shù)據。假定發(fā)射點所在地地理參數(shù)：東經105°24′，北緯38°01′，海拔800m，射向α＝42.25°，射角β＝42°，全彈道偏航角變化范圍約為0.4°，俯仰角在42°～－59.6°之間變化，彈丸旋轉平均轉速約為40r/s。根據IGRF模型，仿真生成各軸磁傳感器輸出數(shù)據。

3.1 算法仿真

算法解算時，取原始姿態(tài)角數(shù)據中的偏航角作為每次迭代解算的偏航角初值，即假定偏航角為理論值，取初始俯仰角為射角42°，初始橫滾角為0°，迭代精度設置為0.001°。用遺傳－牛頓法解算全彈道飛行姿態(tài)，并與彈道方程所得的理想全彈道姿態(tài)進行了比較。姿態(tài)解算結果如圖1、圖2所示。

圖1 姿態(tài)解算結果

圖2 解算姿態(tài)角誤差

在未知初始姿態(tài)情況下，初值X0的搜索過程必須遍歷姿態(tài)角的整個姿態(tài)定義域，解算算法的精度取決于遍歷過程中步長的設置，需要在精度與速度間做合適的選擇，減小步長有利于提高姿態(tài)角的姿態(tài)解算精度，但也會導致遍歷搜索速度變慢，從而影響算法的實時性。牛頓－遺傳算法與常規(guī)遍歷算法的解算精度、速度比較如表1所示，表中，St為搜索區(qū)間，h為步長，t為計算時間，M為進化代數(shù)，N為迭代次數(shù)，Δθ和Δγ分別為θ和γ姿態(tài)角的解算誤差。

表1 牛頓－遺傳算法與常規(guī)遍歷算法的解算精度、速度比較

結果表明，如果偏航角與理論值一致，解算得到的俯仰角和橫滾角誤差非常小，在（10－8）°數(shù)量級，完全可以忽略不計。對比表1可知，牛頓－遺傳算法相比常規(guī)遍歷算法確實提高了算法實時性與解算精度。在實彈飛行環(huán)境下，可以通過增大步長，適當?shù)貭奚藨B(tài)解算的精度來提高計算速度。

3.2 偏航誤差對解算的影響

上述算法仿真時，偏航角假定已知，在沒有其他輔助測量情況下，偏航角是無法確定的。在分析偏航誤差對解算的影響時，假定全彈道無偏航，偏航角ψ＝0，其他仿真條件與3.1中相同，解算姿態(tài)角誤差如圖3所示。

圖3 解算姿態(tài)角誤差（偏航為零）

從圖3中可以看出，假定偏航角不變，會造成偏航角誤差，在解算俯仰角和橫滾角時帶來誤差。偏航角誤差約0.4°，俯仰角誤差約0.15°，橫滾角誤差約0.4°?？梢娖浇钦`差對俯仰角和橫滾角的解算誤差影響程度不同。

為進一步分析偏航誤差對解算結果的影響，假設上述給定的偏航角在彈體飛行過程中分別改變了1°，5°和10°，其余仿真條件不變，由此解算姿態(tài)角誤差曲線如圖4所示。

圖4 偏航變化對解算精度的影響

從仿真結果中可以看出，當偏航角變化1°時，俯仰角誤差約為0.2°，橫滾角誤差約為0.5°；當偏航角變化5°時，俯仰角誤差約為1°，橫滾角誤差約為2°；當偏航角變化10°時，俯仰角誤差約為2°，橫滾角誤差約為6°。實際上常規(guī)彈藥在射程內偏航角變化10°的可能性很小，通常在1°～5°范圍內。因此，即使惡劣環(huán)境下橫滾角誤差也不會超過2°，而環(huán)境條件較好的情況下俯仰角誤差和橫滾角誤差分別低于0.2°和0.5°。數(shù)值計算表明，假定偏航角不變，解算得到的俯仰角和橫滾角的精度能夠滿足測量要求，算法具有較高的解算精度。

3.3 磁測數(shù)據誤差對解算的影響

三軸磁阻傳感器存在的靈敏度誤差、零偏誤差、非正交誤差和安裝誤差以及周圍電磁干擾等均會造成磁測量數(shù)據不準確。為了分析磁測數(shù)據誤差對解算的影響，仿真中假設三軸磁阻傳感器存在10～500nT的隨機測量誤差，并設置偏航角與理論值一致，其余仿真條件與3.1中相同。圖5為500nT磁測數(shù)據誤差時的解算誤差曲線。由仿真結果可知，磁測數(shù)據存在100nT隨機測量誤差時，俯仰角誤差幅值小于0.3°，橫滾角誤差幅值小于0.5°；而最大偏差為500nT時，俯仰角誤差幅值小于1.5°，滾轉角的誤差幅值最大也不超過3°。

圖5 磁測數(shù)據誤差對解算精度的影響

此外，為分析初始地磁三分量數(shù)據的不準確對解算結果的影響，分別假設初始地磁三分量有100～1 000nT的誤差（ΔH0），其余條件不變，仿真結果如圖6所示。

圖6 初始地磁三分量不準確導致的姿態(tài)角誤差

從結果來看，初始地磁三分量和磁阻傳感器測量數(shù)據的變化對姿態(tài)角解算帶來不同的誤差影響，初始地磁三分量存在固定的常數(shù)誤差，導致的姿態(tài)角誤差呈單調變化；而磁測數(shù)據三軸分量存在固定的常數(shù)誤差，這導致姿態(tài)角誤差隨彈丸旋轉而產生周期性變化。這種情況表明，對數(shù)據進行后期處理時，可以通過頻率的不同區(qū)別不同誤差。

4 結束語

受常規(guī)彈藥的高旋轉、高沖擊、小體積等惡劣應用環(huán)境的限制，利用捷聯(lián)磁傳感器測量高速旋轉彈飛行姿態(tài)是一種有工程應用價值的測試方法。針對磁測姿態(tài)方程是非線性超越方程組，磁測解算中牛頓迭代法存在收斂緩慢甚至發(fā)散的問題。本文利用遺傳算法在姿態(tài)角定義域內對根進行快速搜索得到近似解，并以此近似解作為牛頓迭代法的迭代初值，保證算法的收斂與實時性。以某高速旋轉彈為算例，進行了姿態(tài)角解算，分別從給定姿態(tài)角誤差、初始固定參數(shù)誤差和磁測數(shù)據誤差三個方面進行了算法誤差影響分析，數(shù)值仿真結果表明，牛頓－遺傳算法確實能夠解決算法收斂緩慢與發(fā)散的問題，且提高了算法實時性與解算精度，驗證了算法的可行性，但由于算法的仿真計算是建立在普通PC機平臺的離線數(shù)據處理方式上，特別是由于彈上計算機性能的限制，算法在實彈飛行環(huán)境下必須綜合考慮姿態(tài)計算的速度與精度平衡。

[1]高峰，張合.基于基準角和補償角的常規(guī)彈藥滾轉角磁探測算法研究[J].探測與控制學報，2008，30（5）：11－15.GAO Feng，ZHANG He.Algorithm of roll angle determination of conventional ammunitions based on benchmark angle and compensation angle[J].Journal of Detection and Control，2008，30（5）：11－15.（in Chinese）

[2]史金光，韓艷，劉世平，等.制導炮彈飛行姿態(tài)角的一種組合測量方法[J].彈道學報，2011，23（3）：37－38.SHI Jin－guang，HAN Yan，LIU Shi－ping，et al.An approach of combination measurement for flight attitude angles of guided projectile[J].Journal of Ballistics，2011，23（3）：37－38.（in Chinese）

[3]鮑亞琪，陳國光，吳坤，等.基于磁強計和 MEMS陀螺的彈箭全姿態(tài)探測[J].兵工學報，2008，29（10）：1 227－1 231.BAO Ya－qi，CHEN Guo－guang，WU Kun，et al.Research on attitude determination using magnetometers and MEMS inertial sensors[J].Acta Armamentarii，2008，29（10）：1 227－1 231.（in Chinese）

[4]茍秋雄，劉明喜，李虎軍.基于磁阻傳感器的末制導迫擊炮彈滾轉姿態(tài)初始對準技術研究[J].彈箭與制導學報，2008，28（3）：45－47.GOU Qiu－xiong，LIU Ming－xi，LI Hu－jun.Research of terminal guide mortar bomb roller attitude initial alignment technique based on magnetic resistance sensor[J].Journal of Projectiles，Rockets，Missiles and Guidance，2008，28（3）：45－47.（in Chinese）

[5]馬國梁，李巖，葛敬飛.磁阻傳感器測量旋轉彈滾轉姿態(tài)的原理分析[J].彈道學報，2012，24（1）：32－36.MA Guo－liang，LI Yan，GE Jing－fei.Principle analysis for roll attitude spinning projectile using magnetic measurement of resistance sensor[J].Journal of Ballistics，2012，24（1）：32－36.（in Chinese）

[6]史連艷，張自賓，宋文淵.基于 MR/GPS的彈體姿態(tài)解算方法研究[J].系統(tǒng)仿真學報，2010，22（12）：2 948－2 951.SHI Lian－yan，ZHANG Zi－bin，SONG Wen－yuan.Study of rocket body attitude solving method based on MR/GPS[J].Journal of System Simulation，2010，22（12）：2 948－2 951.（in Chinese）

[7]李玎，卜雄洙.基于非正交磁傳感器組合的旋轉彈體橫滾角測試[J].兵工學報，2010，31（10）：1 316－1 321.LI Ding，BU Xiong－zhu.Roll angle measurement of spinning projectile based on non－orthogonal magnetic sensors[J].Acta Armamentarii，2010，31（10）：1 316－1 321.（in Chinese）

[8]錢偉行.捷聯(lián)慣導與組合導航系統(tǒng)高精度初始對準技術研究[D].南京：南京航空航天大學，2010.QIAN Wei－xing.Research on high－precision initial alignment of strapdown inertial and integrated navigation system[D].Nanjing：University of Aeronautics and Astronautics，2010.（in Chinese）

[9]HOLLAND J H.Adaptation in natural and artificial systems：an introductory analysis with applications to biology，control，and artificial intelligence[M].Ann Arbor：University of Michigan，1975.

[10]DORIGO M，MANIEZZO V，COLORNI A.Ant system：optimization by a colony of cooperative agents[J].IEEE Transactions on System，Man，and Cybernetics，1996，26（1）：29－41.