基于光电瞄准／SINS组合的在线修正方法

王广龙，高伟伟，张春熹，陈建辉，高凤岐

（1．军械工程学院纳米技术与微系统实验室，河北石家庄050003；2．北京航空航天大学仪器科学与光电工程学院，北京100191）

基于光电瞄准／SINS组合的在线修正方法

王广龙1，高伟伟1，张春熹2，陈建辉1，高凤岐1

（1．军械工程学院纳米技术与微系统实验室，河北石家庄050003；2．北京航空航天大学仪器科学与光电工程学院，北京100191）

针对陆战装备捷联惯导系统（strapdown inertial navigation system，SINS）误差的积累性问题，提出了一种光电瞄准解算测速方法，并将其用于SINS主要状态参数的在线修正。推导了光电瞄准速度解算误差模型，误差分析结果表明，光电瞄准解算测速精度满足修正SINS主要状态参数的要求；建立了光电瞄准／SINS在线修正数学模型，并进行了仿真实验，实验结果表明，通过合理设定动基座的运动状态，所提方法可准确估计出SINS的主要误差状态参数，其中失准角的估计精度可达到1′以内，陀螺漂移误差的估计精度可达到0．01°／h。

光电瞄准；捷联惯导系统；误差估计；在线修正

0 引 言

在陆战装备火控系统中，一般均配装光电瞄准设备，采用可视／红外观测和激光测距相结合的方法对目标进行捕获与定位。在动基座条件下，通常采用固联于载体上的惯性测量单元（inertial measurement unit，IMU）设备输出信息，实时解算出载体的姿态信息和位置信息，称此系统为捷联惯导系统（strapdown inertial navigation system，SINS），并在此基础上实现稳瞄稳向、定位定向等功能。由于IMU组成的惯性系统误差随时间的增长而积累，严重时甚至会导致信息发散，在应用中通常会采用辅助设备，如卫星定位设备、里程计等，其提供的位置或速度信息可用于修正SINS系统的累积性误差，提高姿态和位置的解算精度［1－5］。卫星定位设备通过接收卫星信号（如GPS、GLONASS、北斗等）来实现对载体的定位，由于卫星信号易被遮挡和干扰，定位申请发送的同时容易暴露自身位置，不利于军事应用上的隐蔽性。里程计受到胎压大小、车体打滑、路况特征等方面的影响［6－7］。本文以动基座条件下陆战装备的稳瞄过程为背景，以置于稳定平台的光电观瞄设备输出的激光测距值和瞄准轴相对一基准方向角度值为辅助信息源，提出了一种光电瞄准／SINS组合在线修正方法，该方法可在不借助外部设备的条件下，使陆战装备独立实现对SINS主要误差参数的在线修正，消除SINS的积累性误差，为陆战装备武器系统提供准确的姿态信息和位置信息。

1 光电瞄准解算测速方法与误差分析

1．1 光电瞄准解算测速方法

图1为光电瞄准设备测速原理图。如图1（a）所示，设初始条件下一动基座位于A点，方位指向东，在远处O点有一目标，动基座上一光电瞄准设备正瞄向该目标，光电瞄准设备置于稳定平台上，瞄准轴向与东向成角度α，激光测距得AO＝X，动基座从A点沿AB方向移动至B点过程中光电瞄准设备始终瞄向O点处目标，在B点瞄准轴向与东向成角度β。根据相关的数学定理可以得到AO与BO的夹角θ＝β－α，在现有已知条件下，通过余弦定理可得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。假设相邻采集点间隔内，动基座水平姿态角未发生明显变化，由此可推得相邻两点间与O点的夹角θ1，θ2，…，θi，…，θn－1，θn，且θi＝αi－αi－1。当n→∞时，Li可近似用线段Ai－1Ai表示，根据余弦定理得

图1 光电瞄准设备测速原理图

设动基座移动路线中相邻数据采集点Ai－1到Ai的时间间隔为Δt，那么对应Li段动基座的移动速度vi方向沿线段Ai－1Ai方向，vi大小可表示为

1．2 光电瞄准速度解算误差模型

假设光电瞄准设备输出的目标距离Xi与相邻瞄准角差θi采用等间隔时间Δt采样获得，则解算得到的速度精度主要取决于Δt间隔内的动基座移动距离Li的解算精度，根据式（1）可知，Li的精度主要取决于Xi与θi的测量精度。对式（1）左右求平方，得

第i点测量时，第i－1点测量值已为定值，因此将式（3）左右求变分，得

进一步整理得

将式（5）除以Δt，得

式（6）可视为光电瞄准输出解算得到的动基座速度误差公式，δvi表示第i测量点的速度误差，δXi、δθi表示第i测量点的目标距离误差和瞄准角误差。

1．3 光电瞄准速度解算误差分析

若想实现光电瞄准／SINS组合在线修正，首先需要保证光电瞄准解算得到的速度信息是有精度保证的。因此需要结合动基座的典型运动情况，对光电瞄准设备速度解算误差进行分析，以保证光电瞄准／SINS组合在线修正方法的可行性。为研究光电瞄准速度解算误差与动基座的运动关系，根据目标与动基座的运动情况，将光电瞄准测速路径规划为几种情况，正向瞄准直线运动，侧向瞄准直线运动以及侧向瞄准曲线运动（本文为便于分析，研究了侧向瞄准圆周运动），如图2所示。

图2 光电瞄准测速路径规划

根据式（6），对图2中规划路径下的误差进行分析。在规划路径图2（a）中，瞄准线方向与测速方向一致，任一点i处θi＝0，Li＝Xi－Xi－1，因此式（6）可化简为

由此可见规划路径图2（a）条件下，在Δt一定时，光电瞄准解算速度误差δvi仅取决于激光测距误差δXi。规划路径图2（b）、图2（c）的误差分析公式参考式（6），规划路径图2（b）初始条件下，动基座距离目标100m，在光电瞄准测速过程中动基座沿直线向东运动，在初始瞄准角为45°条件下，对设定运动速度分别为5m／s，10m／s，30m／s测速误差进行分析；在运动速度为10m／s条件下，分别对初始瞄准角为5°、10°、45°测速误差进行分析；规划路径图2（c）下，动基座距离目标100m，在光电瞄准测速过程中，动基座以目标为圆心做匀速圆周运动，分别在角速度为1°／s和5°／s测速误差条件下进行分析。设Δt＝0．1s，由于现有激光测距仪100m内的精度一般可达到1mm左右［8－10］，因此δXi可近似视为均值为0，标准差为0．000 3m的高斯白噪声。若光电瞄准设备轴向采用码盘式角度传感器，其测角精度一般可达到0．001°，可将δθi近似视为均值为0，标准差为0．000 3°的高斯白噪声。

图3 不同规划路径下的光电瞄准解算测速误差曲线

于是，在路径图2（a），图2（b），图2（c）条件下，光电瞄准测速误差δv随时间变化的曲线如图3所示。

分析图3的误差曲线可知，在动基座直线运动条件下，通过光电瞄准解算测速的速度误差可控制在0．01m／s以内，在匀速圆周运动条件下，测速误差可控制在0．015m／s以内。以上速度误差在SINS允许范围内，因此光电瞄准解算测速方法可用于对SINS误差参数的修正。

2 光电瞄准／SINS在线修正数学模型

将光电瞄准设备测量解算得到的动基座速度值作为基准值，采用光电瞄准／SINS在线修正示意图如图4所示，由于是在陆基环境下对SINS主要参数进行在线修正，因此在研究过程中可忽略高度通道，在动基座移动过程中，通过SINS实时输出的方位角，光电瞄准设备测量解算出的速度信号vi可分解为北向速度vNi和东向速度vEi，将其与动基座上SINS解算得到的速度vN与vE相减便可得到观测量，通过建立状态方程和量测方程，采用Kalman滤波便可估计出SINS主要误差参数，通过误差补偿可对各主要参数进行修正［11－15］。

图4 光电瞄准／SINS在线修正示意图

SINS的主要误差源有陀螺仪误差εb和加速度计误差Δb。由于各种误差源的影响，SINS解算得到的数学平台坐标系与实际导航系之间存在姿态误差；同样，SINS解算的载体速度、位置均存在误差。设导航系n为东－北－天坐标系，b代表载体系；ωie为地球转动角速度；ωen为导航系相对地球的角速度；fn为加速度计输出比力在导航系的投影；Vn为导航系下的线速度；φn为SINS计算平台失准角。在短时间工作条件下，陀螺和加速度计的主要误差可视为常值误差。综上分析，相应的误差方程［16－20］可表示为

以战车为研究对象，可认为垂直通道速度始终为零，在建立状态方程时，不考虑垂直通道影响。设载体所在的地理纬度为L，经度为λ，高度为h，地球长半径为Re＝6 378 245m，地球椭圆度为e＝1／298．3，当地卯酉面内主曲率半径［21－25］为RE＝Re／（1－esin2L），与卯酉面垂直平面上的主曲率半径为RN＝Re／（1＋2e－3esin2L），＝（Tij）3×3。选取状态量为

于是，得到SINS误差状态方程为

式中，W为系统噪声；

选择SINS解算得到的速度与采用光电瞄准设备解算得到的载体速度之差作为观测量，得到量测方程可表示为

3 仿真实验分析

由于动基座在直线运动条件下采用光电瞄准解算测速的误差较小，本文在光电瞄准／SINS在线修正研究过程中采用直线运动激发各误差状态量的可观测性，这样可保证观测量的精确性，从而实现对各状态参数的准确估计与修正。设光电瞄准解算测速的误差为0．01m／s，动基座初始地理经度为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个姿态角的初始值分别为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结束。

在设定轨迹条件下主要状态参数估计结果如图5～图7所示。

图5 轨迹1主要误差参数估计

图6 轨迹2主要误差参数估计

图7 轨迹3主要误差参数估计

分析设定轨迹下的状态估计结果，可得轨迹1中前50s匀速运动条件下，仅有状态量δVx、δVy、φx、φy可估计，第51s加速机动后状态量φz快速得到估计，加速结束后以及εx、εy逐渐变得可估计，并在第250s附近接近设定值；但εz始终无法估计。其中，在匀速段，失准角φx、φy的估计精度可达到1′以内，加速开始一段时间φz的估计精度在1′以内，随着时间的增长，3个失准角的估计精度逐渐降低，但能保持在3′左右；第250s后，陀螺漂移εx、εy估计精度在0．02°／h以内，加表漂移Δx、Δ

y的估计精度在10μg左右。在轨迹2条件下，通过减速—加速—减速过程，εz的状态可观测性逐渐提高，但估计精度较差，其他状态量的估计精度与轨迹1条件下相当。在轨迹3条件下，通过加速—减速—加速—减速过程，εz的估计精度提高，在250s后εx、εy、εz的估计精度保持在0．01°／h以内，其他状态量的估计精度与轨迹1、轨迹2相当。综上分析，3种设定轨迹下，轨迹3条件下可实现模型状态的完全可观测，并且部分状态量的估计精度较高。采用估计误差参数值对SINS主要状态参数进行补偿，便实现了光电瞄准／SINS组合在线修正。

4 结 论

提出了一种光电瞄准解算测速方法，推导了光电瞄准解算测速误差模型，通过误差分析得出光电瞄准解算测速可用于SINS主要状态参数的在线修正。建立了光电瞄准／SINS在线修正数学模型，通过仿真实验论证了光电瞄准／SINS在线修正的可行性。本文方法利用了陆战装备的自身优势，在不增加额外成本的基础上可独立实现对SINS主要状态参数的在线修正，在现有研究基础上提供了一种新的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．（温永智，吴杰．载波相位／捷联惯导组合对高轨飞行器的导航［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的弹载捷联惯导动基座传递对准技术［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］．系统工程与电子技术，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］．光电子·激光，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－），通信作者，男，博士研究生，主要研究方向为微型测控技术、姿态测量、稳瞄稳向技术。

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－hui1，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；网络优先出版日期：2014－11－06。

网络优先出版地址：http：／／www．cnki．net／kcms／detail／11．2422．TN．20141106．1349．004．html

国家自然科学基金（50875015）资助课题