龍達(dá)峰，劉 俊，2，張曉明，李 杰

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3 算法仿真及誤差分析

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

3.1 算法仿真

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

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

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

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

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

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

3.2 偏航誤差對解算的影響

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)束語

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

[1]高峰，張合.基于基準(zhǔn)角和補(bǔ)償角的常規(guī)彈藥滾轉(zhuǎn)角磁探測算法研究[J].探測與控制學(xué)報，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]史金光，韓艷，劉世平，等.制導(dǎo)炮彈飛行姿態(tài)角的一種組合測量方法[J].彈道學(xué)報，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]鮑亞琪，陳國光，吳坤，等.基于磁強(qiáng)計和 MEMS陀螺的彈箭全姿態(tài)探測[J].兵工學(xué)報，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]茍秋雄，劉明喜，李虎軍.基于磁阻傳感器的末制導(dǎo)迫擊炮彈滾轉(zhuǎn)姿態(tài)初始對準(zhǔn)技術(shù)研究[J].彈箭與制導(dǎo)學(xué)報，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]馬國梁，李巖，葛敬飛.磁阻傳感器測量旋轉(zhuǎn)彈滾轉(zhuǎn)姿態(tài)的原理分析[J].彈道學(xué)報，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)仿真學(xué)報，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]李玎，卜雄洙.基于非正交磁傳感器組合的旋轉(zhuǎn)彈體橫滾角測試[J].兵工學(xué)報，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)慣導(dǎo)與組合導(dǎo)航系統(tǒng)高精度初始對準(zhǔn)技術(shù)研究[D].南京：南京航空航天大學(xué)，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.