基于光電瞄準／SINS組合的在線修正方法

王廣龍，高偉偉，張春熹，陳建輝，高鳳岐

（1．軍械工程學院納米技術與微系統(tǒng)實驗室，河北石家莊050003；2．北京航空航天大學儀器科學與光電工程學院，北京100191）

基于光電瞄準／SINS組合的在線修正方法

王廣龍1，高偉偉1，張春熹2，陳建輝1，高鳳岐1

（1．軍械工程學院納米技術與微系統(tǒng)實驗室，河北石家莊050003；2．北京航空航天大學儀器科學與光電工程學院，北京100191）

針對陸戰(zhàn)裝備捷聯(lián)慣導系統(tǒng)（strapdown inertial navigation system，SINS）誤差的積累性問題，提出了一種光電瞄準解算測速方法，并將其用于SINS主要狀態(tài)參數(shù)的在線修正。推導了光電瞄準速度解算誤差模型，誤差分析結果表明，光電瞄準解算測速精度滿足修正SINS主要狀態(tài)參數(shù)的要求；建立了光電瞄準／SINS在線修正數(shù)學模型，并進行了仿真實驗，實驗結果表明，通過合理設定動基座的運動狀態(tài)，所提方法可準確估計出SINS的主要誤差狀態(tài)參數(shù)，其中失準角的估計精度可達到1′以內(nèi)，陀螺漂移誤差的估計精度可達到0．01°／h。

光電瞄準；捷聯(lián)慣導系統(tǒng)；誤差估計；在線修正

0 引 言

在陸戰(zhàn)裝備火控系統(tǒng)中，一般均配裝光電瞄準設備，采用可視／紅外觀測和激光測距相結合的方法對目標進行捕獲與定位。在動基座條件下，通常采用固聯(lián)于載體上的慣性測量單元（inertial measurement unit，IMU）設備輸出信息，實時解算出載體的姿態(tài)信息和位置信息，稱此系統(tǒng)為捷聯(lián)慣導系統(tǒng)（strapdown inertial navigation system，SINS），并在此基礎上實現(xiàn)穩(wěn)瞄穩(wěn)向、定位定向等功能。由于IMU組成的慣性系統(tǒng)誤差隨時間的增長而積累，嚴重時甚至會導致信息發(fā)散，在應用中通常會采用輔助設備，如衛(wèi)星定位設備、里程計等，其提供的位置或速度信息可用于修正SINS系統(tǒng)的累積性誤差，提高姿態(tài)和位置的解算精度［1－5］。衛(wèi)星定位設備通過接收衛(wèi)星信號（如GPS、GLONASS、北斗等）來實現(xiàn)對載體的定位，由于衛(wèi)星信號易被遮擋和干擾，定位申請發(fā)送的同時容易暴露自身位置，不利于軍事應用上的隱蔽性。里程計受到胎壓大小、車體打滑、路況特征等方面的影響［6－7］。本文以動基座條件下陸戰(zhàn)裝備的穩(wěn)瞄過程為背景，以置于穩(wěn)定平臺的光電觀瞄設備輸出的激光測距值和瞄準軸相對一基準方向角度值為輔助信息源，提出了一種光電瞄準／SINS組合在線修正方法，該方法可在不借助外部設備的條件下，使陸戰(zhàn)裝備獨立實現(xiàn)對SINS主要誤差參數(shù)的在線修正，消除SINS的積累性誤差，為陸戰(zhàn)裝備武器系統(tǒng)提供準確的姿態(tài)信息和位置信息。

1 光電瞄準解算測速方法與誤差分析

1．1 光電瞄準解算測速方法

圖1為光電瞄準設備測速原理圖。如圖1（a）所示，設初始條件下一動基座位于A點，方位指向東，在遠處O點有一目標，動基座上一光電瞄準設備正瞄向該目標，光電瞄準設備置于穩(wěn)定平臺上，瞄準軸向與東向成角度α，激光測距得AO＝X，動基座從A點沿AB方向移動至B點過程中光電瞄準設備始終瞄向O點處目標，在B點瞄準軸向與東向成角度β。根據(jù)相關的數(shù)學定理可以得到AO與BO的夾角θ＝β－α，在現(xiàn)有已知條件下，通過余弦定理可得AB間的距離將以上過程推廣至動基座的整個移動路線中，如圖1（b）所示，將動基座移動路線均勻地分為n段，分別為L1，L2，…，Li…，Ln－1，Ln，整個移動過程中光電瞄準設備始終指向O點處目標，動基座在點A0，A1，…，Ai，…，An－1，An處的激光測距值分別為X0，X1，…，Xi，…，Xn－1，Xn，相應的光電瞄準軸與東向的角度分別為α0，α1，…，αi，…，αn－1，αn。假設相鄰采集點間隔內(nèi)，動基座水平姿態(tài)角未發(fā)生明顯變化，由此可推得相鄰兩點間與O點的夾角θ1，θ2，…，θi，…，θn－1，θn，且θi＝αi－αi－1。當n→∞時，Li可近似用線段Ai－1Ai表示，根據(jù)余弦定理得

圖1 光電瞄準設備測速原理圖

設動基座移動路線中相鄰數(shù)據(jù)采集點Ai－1到Ai的時間間隔為Δt，那么對應Li段動基座的移動速度vi方向沿線段Ai－1Ai方向，vi大小可表示為

1．2 光電瞄準速度解算誤差模型

假設光電瞄準設備輸出的目標距離Xi與相鄰瞄準角差θi采用等間隔時間Δt采樣獲得，則解算得到的速度精度主要取決于Δt間隔內(nèi)的動基座移動距離Li的解算精度，根據(jù)式（1）可知，Li的精度主要取決于Xi與θi的測量精度。對式（1）左右求平方，得

第i點測量時，第i－1點測量值已為定值，因此將式（3）左右求變分，得

進一步整理得

將式（5）除以Δt，得

式（6）可視為光電瞄準輸出解算得到的動基座速度誤差公式，δvi表示第i測量點的速度誤差，δXi、δθi表示第i測量點的目標距離誤差和瞄準角誤差。

1．3 光電瞄準速度解算誤差分析

若想實現(xiàn)光電瞄準／SINS組合在線修正，首先需要保證光電瞄準解算得到的速度信息是有精度保證的。因此需要結合動基座的典型運動情況，對光電瞄準設備速度解算誤差進行分析，以保證光電瞄準／SINS組合在線修正方法的可行性。為研究光電瞄準速度解算誤差與動基座的運動關系，根據(jù)目標與動基座的運動情況，將光電瞄準測速路徑規(guī)劃為幾種情況，正向瞄準直線運動，側向瞄準直線運動以及側向瞄準曲線運動（本文為便于分析，研究了側向瞄準圓周運動），如圖2所示。

圖2 光電瞄準測速路徑規(guī)劃

根據(jù)式（6），對圖2中規(guī)劃路徑下的誤差進行分析。在規(guī)劃路徑圖2（a）中，瞄準線方向與測速方向一致，任一點i處θi＝0，Li＝Xi－Xi－1，因此式（6）可化簡為

由此可見規(guī)劃路徑圖2（a）條件下，在Δt一定時，光電瞄準解算速度誤差δvi僅取決于激光測距誤差δXi。規(guī)劃路徑圖2（b）、圖2（c）的誤差分析公式參考式（6），規(guī)劃路徑圖2（b）初始條件下，動基座距離目標100m，在光電瞄準測速過程中動基座沿直線向東運動，在初始瞄準角為45°條件下，對設定運動速度分別為5m／s，10m／s，30m／s測速誤差進行分析；在運動速度為10m／s條件下，分別對初始瞄準角為5°、10°、45°測速誤差進行分析；規(guī)劃路徑圖2（c）下，動基座距離目標100m，在光電瞄準測速過程中，動基座以目標為圓心做勻速圓周運動，分別在角速度為1°／s和5°／s測速誤差條件下進行分析。設Δt＝0．1s，由于現(xiàn)有激光測距儀100m內(nèi)的精度一般可達到1mm左右［8－10］，因此δXi可近似視為均值為0，標準差為0．000 3m的高斯白噪聲。若光電瞄準設備軸向采用碼盤式角度傳感器，其測角精度一般可達到0．001°，可將δθi近似視為均值為0，標準差為0．000 3°的高斯白噪聲。

圖3 不同規(guī)劃路徑下的光電瞄準解算測速誤差曲線

于是，在路徑圖2（a），圖2（b），圖2（c）條件下，光電瞄準測速誤差δv隨時間變化的曲線如圖3所示。

分析圖3的誤差曲線可知，在動基座直線運動條件下，通過光電瞄準解算測速的速度誤差可控制在0．01m／s以內(nèi)，在勻速圓周運動條件下，測速誤差可控制在0．015m／s以內(nèi)。以上速度誤差在SINS允許范圍內(nèi)，因此光電瞄準解算測速方法可用于對SINS誤差參數(shù)的修正。

2 光電瞄準／SINS在線修正數(shù)學模型

將光電瞄準設備測量解算得到的動基座速度值作為基準值，采用光電瞄準／SINS在線修正示意圖如圖4所示，由于是在陸基環(huán)境下對SINS主要參數(shù)進行在線修正，因此在研究過程中可忽略高度通道，在動基座移動過程中，通過SINS實時輸出的方位角，光電瞄準設備測量解算出的速度信號vi可分解為北向速度vNi和東向速度vEi，將其與動基座上SINS解算得到的速度vN與vE相減便可得到觀測量，通過建立狀態(tài)方程和量測方程，采用Kalman濾波便可估計出SINS主要誤差參數(shù)，通過誤差補償可對各主要參數(shù)進行修正［11－15］。

圖4 光電瞄準／SINS在線修正示意圖

SINS的主要誤差源有陀螺儀誤差εb和加速度計誤差Δb。由于各種誤差源的影響，SINS解算得到的數(shù)學平臺坐標系與實際導航系之間存在姿態(tài)誤差；同樣，SINS解算的載體速度、位置均存在誤差。設導航系n為東－北－天坐標系，b代表載體系；ωie為地球轉動角速度；ωen為導航系相對地球的角速度；fn為加速度計輸出比力在導航系的投影；Vn為導航系下的線速度；φn為SINS計算平臺失準角。在短時間工作條件下，陀螺和加速度計的主要誤差可視為常值誤差。綜上分析，相應的誤差方程［16－20］可表示為

以戰(zhàn)車為研究對象，可認為垂直通道速度始終為零，在建立狀態(tài)方程時，不考慮垂直通道影響。設載體所在的地理緯度為L，經(jīng)度為λ，高度為h，地球長半徑為Re＝6 378 245m，地球橢圓度為e＝1／298．3，當?shù)孛厦鎯?nèi)主曲率半徑［21－25］為RE＝Re／（1－esin2L），與卯酉面垂直平面上的主曲率半徑為RN＝Re／（1＋2e－3esin2L），＝（Tij）3×3。選取狀態(tài)量為

于是，得到SINS誤差狀態(tài)方程為

式中，W為系統(tǒng)噪聲；

選擇SINS解算得到的速度與采用光電瞄準設備解算得到的載體速度之差作為觀測量，得到量測方程可表示為

3 仿真實驗分析

由于動基座在直線運動條件下采用光電瞄準解算測速的誤差較小，本文在光電瞄準／SINS在線修正研究過程中采用直線運動激發(fā)各誤差狀態(tài)量的可觀測性，這樣可保證觀測量的精確性，從而實現(xiàn)對各狀態(tài)參數(shù)的準確估計與修正。設光電瞄準解算測速的誤差為0．01m／s，動基座初始地理經(jīng)度為116．343 6°，緯度為39．977 5°，高度為50m，地球自轉角速率為7．292 115 8e－5rad／s。SINS陀螺儀常值零偏為0．1°／h，隨機漂移為0．01°／h，加速度計常值偏置為100μg，隨機偏置為10μg，SINS俯仰、橫滾、航向3個姿態(tài)角的初始值分別為0°、0°、0°，SINS計算平臺失準角分別為

仿真軌跡設定如下：

軌跡1 動基座以初速為10m／s的速度運動50s，接著以加速度為0．5m／s2的速度運動50s，接著勻速行駛至第350s結束；

軌跡2 動基座以初始速度為10m／s的速度行駛50s后，以加速度為－0．5m／s2的速度運動50s（減速10s后逆向行駛），接著以加速度為0．5m／s2的速度運動100s，接著再以加速度為－0．5m／s2的速度運動50s，再勻速行駛至第350s結束；

軌跡3 動基座以初始速度為10m／s，加速度為0．5m／s2的速度運動50s，接著以加速度為－0．5m／s2的速度運動50s，接著再以加速度為0．5m／s2的速度運動100s，以加速度為－0．5m／s2的速度運動50s，再勻速行駛至第350s結束。

在設定軌跡條件下主要狀態(tài)參數(shù)估計結果如圖5～圖7所示。

圖5 軌跡1主要誤差參數(shù)估計

圖6 軌跡2主要誤差參數(shù)估計

圖7 軌跡3主要誤差參數(shù)估計

分析設定軌跡下的狀態(tài)估計結果，可得軌跡1中前50s勻速運動條件下，僅有狀態(tài)量δVx、δVy、φx、φy可估計，第51s加速機動后狀態(tài)量φz快速得到估計，加速結束后以及εx、εy逐漸變得可估計，并在第250s附近接近設定值；但εz始終無法估計。其中，在勻速段，失準角φx、φy的估計精度可達到1′以內(nèi)，加速開始一段時間φz的估計精度在1′以內(nèi)，隨著時間的增長，3個失準角的估計精度逐漸降低，但能保持在3′左右；第250s后，陀螺漂移εx、εy估計精度在0．02°／h以內(nèi)，加表漂移Δx、Δ

y的估計精度在10μg左右。在軌跡2條件下，通過減速—加速—減速過程，εz的狀態(tài)可觀測性逐漸提高，但估計精度較差，其他狀態(tài)量的估計精度與軌跡1條件下相當。在軌跡3條件下，通過加速—減速—加速—減速過程，εz的估計精度提高，在250s后εx、εy、εz的估計精度保持在0．01°／h以內(nèi)，其他狀態(tài)量的估計精度與軌跡1、軌跡2相當。綜上分析，3種設定軌跡下，軌跡3條件下可實現(xiàn)模型狀態(tài)的完全可觀測，并且部分狀態(tài)量的估計精度較高。采用估計誤差參數(shù)值對SINS主要狀態(tài)參數(shù)進行補償，便實現(xiàn)了光電瞄準／SINS組合在線修正。

4 結 論

提出了一種光電瞄準解算測速方法，推導了光電瞄準解算測速誤差模型，通過誤差分析得出光電瞄準解算測速可用于SINS主要狀態(tài)參數(shù)的在線修正。建立了光電瞄準／SINS在線修正數(shù)學模型，通過仿真實驗論證了光電瞄準／SINS在線修正的可行性。本文方法利用了陸戰(zhàn)裝備的自身優(yōu)勢，在不增加額外成本的基礎上可獨立實現(xiàn)對SINS主要狀態(tài)參數(shù)的在線修正，在現(xiàn)有研究基礎上提供了一種新的SINS在線修正方法，具有重要的軍事應用價值。

［1］Wen Y Z，Wu J．Carrier phase／SINS integrated navigation for spacecraft on high earth orbit［J］．Journal of Beijing University of Aeronautics and Astronautics，2012，38（6）：726－730．（溫永智，吳杰．載波相位／捷聯(lián)慣導組合對高軌飛行器的導航［J］．北京航空航天大學學報，2012，38（6）：726－730．）

［2］Zhao W W，Zhang L X，Meng Y S，et al．The appilcation research of global navgiation constellaiton for HEO（high earth orbit）satellties and deep－space satellties［J］．Science and Physics Mechanical of Astronomy，2011，41（5）：581－588．

［3］Pamlerini G B，Sabatini M，Perrotta G．Enroute to the moon using GNSS signals［J］．Acta Astronautica，2009，64（4）：467－483．

［4］Wu F，Qin Y Y，Cheng Y．Transfer alignment for missile－borne SINS using airborne GPS on moving base［J］．Journal of Chinese Inertial Technology，2013，21（1）：56－60．（吳楓，秦永元，成研．基于GPS的彈載捷聯(lián)慣導動基座傳遞對準技術［J］．中國慣性技術學報，2013，21（1）：56－60．）

［5］Song M，Wu W Q．Evaluation and optimization of strapdown velocity numerical integration algorithms for SINS in spinning ballistic missiles［J］．Journal of Central South University，2013，20（4）：942－949．

［6］Zhang X Y，Yang G L，Zhang C X．Integrated navigation method for SINS and odometer［J］．Journal of Beijing University ofAeronautics and Astronautics，2013，39（7）：922－926．

［7］Wang Y G，Yang J S，Yu Y，et al．On－the－move alignment for SINS based on odometer aiding［J］．Systems Engineering and Electronics，2013，35（5）：1060－1063．（王躍鋼，楊家勝，蔚躍，等．基于里程計輔助的SINS動基座初始對準方法［J］．系統(tǒng)工程與電子技術，2013，35（5）：1060－1063．）

［8］Huang Z L，Li B，Li Q．Modern optical targeting systems［J］．Optical and Electrical Information，2011，28（3）：58－64．

［9］Zhang H F，Meng W D，Wu Z B，et al．One way laser ranging and its measuring experiment［J］．Chinese Journal of Lasers，2013，40（3）：1－7．（張海峰，孟文東，吳志波，等．單向激光測距及其測量試驗［J］．中國激光，2013，40（3）：1－7．）

［10］Zhang Z L，Zhu J G，Zhou H，et al．Laser automatic theodolite measurement system based on vision guidance［J］．Journal of Optoelectronics·Laser，2011，22（1）：115－119．（張滋黎，邾繼貴，周虎，等．一種基于視覺引導的激光經(jīng)緯儀自動測量系統(tǒng)［J］．光電子·激光，2011，22（1）：115－119．）

［11］Ruotsalainen L，Kuusniemi H，Mohammad Z，et al．A two－dimensional pedestrian navigation solution aided with a visual gyroscope and a visual odometer［J］．The Journal of Navigation，2013，66（3）：575－586．

［12］Ilyas M，Zhang R，Qian Q S，et al．Performance analysis of medium accuracy FOG－based IMU integrated with optical odometer for land vehicle navigation［J］．Journal of Applied Mechanics and Materials，2012，245（3）：334－339．

［13］Gualda D，Ruiz D．Fusion of the odometer and external position system with different coverage areas［J］．Advances in Intelligent Systems and Computing，2014，253（3）：261－273．

［14］Liu D B．The design of sport bicycle speed odometer［J］．Research Journal of Applied Sciences，Engineering and Technology，2013，5（13）：3634－3640．

［15］Redouane A，Bacha A，Gruyer D，et al．A robust hybrid multisource data fusion approach for vehicle localization［J］．Optik，2013，23（4）：271－281．

［16］Broquetas A，Comerón A，Gelonch A．Track detection in railway sidings based on MEMS gyroscope sensors［J］．Sensors，2012，30（12）：16228－16249．

［17］Wang Y，Zhang H，Zhou Q F．Adaptive integrated navigation filtering based on accelerometer calibration［J］．Indonesian Journal of Electrical Engineering，2012，30（10）：1603－1610．

［18］Wei L，Wang J L．Effective adaptive Kalman filter for MEMSIMU／magne tometers integrated attitude and heading reference systems［J］．The Journal of Navigation，2013，66（2）：99－113．

［19］Fang J C，Han X Y．In－flight alignment algorithm based on ADD2for airborne POS［J］．The Journal of Navigation，2013，66（3）：209－225．

［20］Pehlivanogu A G，Ercan Y．Investigation of flexure effect on transfer alignment performance［J］．The Journal of Navigation，2013，66（2）：1－15．

［21］Yang D F，Wang S C，Li H B，et al．Performance enhancement of large－ship transfer alignment：a moving horizon approach［J］．The Journal of Navigation，2012，65（1）：1－17．

［22］Yang D F，Wang S C，Li H B，et al．Performance enhancement of large－ship transfer alignment：a moving horizon approach［J］．The Journal of Navigation，2013，66（3）：17－33．

［23］Yi J，Zhang L，Shu R，et al．Initial alignment for SINS based on lowcost IMU［J］．Journal of Computers，2012，33（6）：1080－1085．

［24］Liu G B，Chen H M，Deng C L．The application of Kalman filtering in SINS／GPS／CNS intergrated navigation system［J］．International Journal of Wireless and Microwave Technologies，2012，2（2）：12－19．

［25］Xu G，Huang G R，Peng X Z．Comparison of different integrated patterns of SINS／GPS system undercondition of GPS signal invalidation［J］．Application Research of Computers，2012，29（10）：3888－3890．

E－mail：glwang2005＠163．com

高偉偉（1986－），通信作者，男，博士研究生，主要研究方向為微型測控技術、姿態(tài)測量、穩(wěn)瞄穩(wěn)向技術。

E－mail：weiweizhiwa＠126．com

張春熹（1965－），男，教授，博士，主要研究方向為光學工程。

E－mail：zchunxi＠hotmail．com

陳建輝（1965－），男，教授，主要研究方向為微型測控技術。

E－mail：281483257＠qq．com

高鳳岐（1965－），男，副教授，碩士，主要研究方向為微型測控技術。

E－mail：281483258＠qq．com

Method of online correction based on optical targeting/SINS combination

WANG Guang－long1，GAO Wei－wei1，ZHANG Chun－xi2，CHEN Jian－h(huán)ui1，GAO Feng－qi1
（1．Nanotechnology and Microsystems Laboratory，Ordnance Engineering College，Shijiazhuang 050003，China；2．School of Instrument Science and Opto－Electronics Engineering，Beihang University，Beijing 100191，China）

For solving the strapdown inertial navigation system（SINS）error accumulation problem of land combat equipment，a method is proposed for computing the speed using the optical targeting process，which is used for online correction of SINS state parameters．The optical targeting speed error model is deduced，and the error analysis shows that the speed measurement using the optical targeting process meets the accuracy requirements for correcting the main state parameters of the SINS．The optical targeting／SINS mathematical correction model is established，and the simulation experiment is performed．Experimental results show that the proposed method can accurately estimate the main parameters of the SINS error status by setting a reasonable state of motion for the moving base，as the misalignment angle estimation accuracy can reach less than 1arcmin，and the gyro drift error estimation accuracy can reach 0．01degree per hour．

optical targeting；strapdown inertial navigation system（SINS）；error estimate；online correction

TP 253

A

10．3969／j．issn．1001－506X．2015．06．22

王廣龍（1964－），男，教授，博士，主要研究方向為微型測控技術。

1001－506X（2015）06－1370－07

2014－05－26；

2014－10－16；網(wǎng)絡優(yōu)先出版日期：2014－11－06。

網(wǎng)絡優(yōu)先出版地址：http：／／www．cnki．net／kcms／detail／11．2422．TN．20141106．1349．004．html

國家自然科學基金（50875015）資助課題