基于TLFK的单轴旋转SINS在线自标定方法

胡 杰，严勇杰

(1. 中国电子科技集团公司第二十八研究所，江苏 南京 210007；2. 空中交通管理系统与技术国家重点实验室，江苏 南京 210007)

1 引言

捷联式惯性导航系统(Strapdown Inertial Navigation System， SINS)利用正交安装的陀螺和加速度计敏感得到载体运动角速率和比力信息实时计算载体导航参数，具有全自主、全天候、隐蔽性强等优点，但是SINS中惯性测量单元(Inertial Measurement Unit， IMU)漂移会引起系统随时间累积性误差。目前提高SINS导航精度主要有2种途径：一是提高IMU测量精度；二是采用系统级方法抑制IMU漂移引起的导航误差。短期内提高IMU精度难度较大且成本高，系统级补偿方法具有成本低、见效快等优点[1]。旋转调制技术是一种常用的系统级误差补偿方法，自20世纪以来已经成为国内外高精度惯导领域主要研究方法之一[2-3]。国外旋转式SINS已经广泛应用于水面舰船、潜艇以及水下机器人，典型产品有Sperry公司研制的MK39系列单轴旋转SINS、MK49系列和AN/WSN-7系列双轴旋转式SINS，均采用旋转调制技术抑制惯性器件漂移引起的累积误差[4-5]。自2002年起，我国相关研究机构也开始了旋转式SINS的研究工作，国防科技大学于2007年研制出我国第一套旋转式SINS，在实验室静态测试环境下，其位置误差优于1 n mile/24h[6]。目前，东南大学[7]、北京航空航天大学[8]、哈尔滨工程大学[9]等高校也积极开展旋转式SINS理论与样机研制工作，取得了较大的研究成果。

单轴旋转SINS中，方位轴的周期性旋转调制抵消了水平方向上的IMU误差对导航精度影响，其轴向IMU误差引起的位置累积误差依然存在，如轴向陀螺常值漂移为0.01°/h，纬度为45°时，纯惯性导航24h后位置误差约为10n mile，是高精度长航时SINS主要误差源之一，为此许多学者对如何精确标校单轴旋转SINS中轴向陀螺漂移进行了研究。Sun Wei[10]针对传统单轴旋转方案中轴向陀螺常值漂移无法被调制问题，提出一种旋转轴倾斜45°的单轴旋转方案，并进行了仿真和系统验证实验。于旭东[11]建立了轴向陀螺常值漂移与经纬度误差数学模型，并利用最小二乘法辨识轴向陀螺常值漂移。胡杰[12]在文献[11]的研究基础上，针对经纬度误差中的舒拉振荡误差对陀螺漂移辨识精度影响，提出利用水平阻尼网络抑制舒拉振荡误差以精确估计轴向陀螺漂移。Gao Wei[13]、孙伟[14]建立了旋转式SINS精对准Kalman滤波状态方程和量测方程，在初始对准的同时对IMU误差进行估计，仿真结果显示IMU水平误差、“数学平台”失准角等误差降低了轴向陀螺常值漂移估计精度。综上文献可知，目前最小二乘法和Kalman滤波法是单轴旋转SINS中常用的两种估计轴向陀螺常值漂移的方法，其中，最小二乘法耗时长且需要载体保持静止状态，而Kalman滤波法中其它状态误差会影响轴向陀螺常值漂移估计精度，为此需要结合SINS导航误差模型，分析误差传递特性，研究一种适用于单轴旋转SINS的轴向陀螺常值漂移自标定方法。

本文首先对单轴旋转SINS调制原理进行了分析，并给出了导航误差模型，然后开展在线自标定数学模型研究，给出了在线自标定Kalman滤波状态方程和量测方程，最后针对单轴旋转SINS中轴向陀螺误差传递特性，提出利用两级Kalman滤波器实现IMU误差的精确估计与补偿。进行了计算机仿真和实际系统验证实验，结果表明本文所提出方法的正确性，能够为中等精度单轴旋转SINS的研制提供理论参考。

2 单轴旋转SINS调制原理与误差模型

2.1 旋转调制原理

传统SINS中，IMU安装在固定支架上并且与载体固连，IMU误差会引起位置随时间积累性误差，无法满足高精度导航需求。单轴旋转SINS中，IMU安装在能够绕轴向旋转的内部平台上，使得IMU(s系，oxsyszs)相对载体坐标系(b系，oxbybzb)旋转，从而水平方向上IMU误差呈周期性变化，通过积分予以抵消。

假设初始时刻s系与b系重合，系统通电后s系以角速度Ω绕方位轴旋转，则在任意t时刻s系相对b系的夹角为Ωt，其方向余弦矩阵可表示为

(1)

(2)

(3)

陀螺和加速计的测量值为

(4)

由式(2)、式(3)和式(4)可以得

(5)

由式(5)可以看出，在单轴旋转SINS中，当IMU绕方位轴旋转时， IMU水平误差在b系中呈周期性变化，通过积分可以抵消，IMU轴向误差依然保持不变，因此对于单轴旋转SINS而言，有必要采取其它措施对IMU轴向误差进行估计与补偿，以进一步提高SINS导航精度。

2.2 导航误差模型

旋转调制技术的本质是通过控制IMU做周期性的转动以改变SINS姿态矩阵，使得短时间内IMU误差对导航系统的积分等于零。旋转式SINS导航算法与非旋转时具有一致性，因此，可得旋转式SINS导航误差方程如下所示。

旋转式SINS姿态误差方程为

(6)

旋转式SINS速度误差方程为

(7)

旋转SINS位置误差方程为

(8)

式中，δL、δλ和δh分别表示导航系统计算得载体纬度误差、经度误差和高度误差，δvE、δvN、δvU分别东向、北向和天向速度误差，RE和RN分别表示地球子午圈和卯酉圈的曲率半径。

3 在线自标定数学模型

(9)

式中，A为系统状态转移矩阵，W为系统过程噪声矢量，X为系统误差状态矢量，其表达式如式(5)所示

(10)

式中，∇x、∇y为沿着旋转系水平方向上加速度计常值偏置，εx、εy、εz为沿着旋转系三个方向陀螺常值漂移，δKgx、δKgy、δKgz为沿着旋转系三个方向陀螺标度因数误差。根据旋转式SINS误差方程式(6)～(8)可以得到精对准状态转移矩阵A为

(11)

式中

选取位置误差δL、δλ和水平速度误差δvE、δvN为观测值，量测方程为

Z=HX+V

(12)

式中，Z为观测向量，Z=[δLδλδvEδvN]T，V为系统量测噪声矢量，H为量测矩阵，其表达式为

(13)

4 两级Kalman滤波算法流程

单轴旋转SINS中水平方向IMU误差可观测度高，而轴向IMU误差可观测度相对较低，为了提高轴向IMU误差标定精度，需要对SINS误差传递形式进行分析，图1为与轴向陀螺常值漂移估计相关的主要误差传递过程。

图1 轴向陀螺常值漂移传递形式

图1中，εE、εU分别表示等效东向、轴向陀螺常值漂移，δvN、δL分别表示SINS北向速度误差和纬度误差，φx、φz分别表示SINS“数学平台”东向和天向失准角。由图1可以看出，当Kalman滤波器利用速度或者位置误差作为观测量进行状态最优估计时，等效东向陀螺常值漂移、方位失准角、等效北向加速度计偏置等都会影响滤波器轴向陀螺常值漂移估计精度，为此需要设计一个合理的自标定流程，以最大限度地减小这些误差因素的影响。

针对单轴旋转SINS中轴向陀螺常值漂移传递特性，为了实现其精确估计，设计了两级Kalman滤波器，并在第二级Kalman滤波器中完成轴向陀螺常值漂移和标度因素误差的精度估计，其自标定流程图如图2所示。

图2 基于两级Kalman滤波的自标定流程图

由图2可以看出，首先，第一级Kalman滤波器利用速度和位置基准信息估计数学平台“失准角”以及水平方向陀螺常值漂移和加速度计常值偏置，并进行补偿后作为第二级Kalman滤波器的输入，然后，进一步利用基准速度和位置信息对轴向陀螺常值漂移以及陀螺标度因数误差进行在线估计，从而完成系统自标定工作并转入导航工作模式。

5 仿真与分析

5.1 仿真参数设置

假设陀螺常值漂移为0.01°/h，角度随机游走系数为0.005°/h1/2，标度因数误差为10ppm，安装误差均为5″；加速度计常值偏置为100μg，随机游走系数为80μg·s1/2，标度因数误差为10ppm，安装误差均为5″。初始经度106.69°，纬度26.51°；仿真系统模拟摇摆运动，姿态角变化规律分别为：俯仰角θ=5°sin(2πt/5)，横摇角γ=2°sin(2πt/1.25)，航向角ψ=5°sin(2πt/5)，假设载体处于系泊状态，线速度为0。

仿真中转位机构采用四位置正反转停旋转方案，如图3所示。旋转机构角加速度为8°/s2，当角速度达到16°/s时保持转位机构匀速旋转运动，并在到达指定位置后以-8°/s2的加速度减速，直至最终角速度为0，其测角装置误差假设为5″。

图3 四位置旋转方案

转位1 IMU由A点逆时针转动180°到达C点，然后停止5min；

转位2 IMU由C点逆时针旋转90°到达D点，然后停止5min；

转位3 IMU由D点顺时针旋转180°到达B点，然后停止5min；

转位4 IMU由B点顺时针旋转90°到达A点，然后停止5min；

然后以此顺序进行循环运动。

5.2 实验结果

单轴旋转SINS启动上电后即进行粗对准，粗对准时间为3min，假设系统粗对准后姿态角误差为Δψ=0.5°，Δθ=Δγ=0.1°，随后进行一级Kalman滤波估计，经过15min后可以得到较为精确的数学平台“失准角”与IMU水平方向误差并进行补偿，随后进行第二级Kalman滤波，对轴向陀螺常值漂移和陀螺标度因数误差进行估计，持续时间为2h，标定结果如图4～图8所示，标定结果统计如表1～表4所示。

图4 “数学平台”失准角估计误差曲线

图5 水平方向加速度计常值偏置估计曲线

图6 水平方向陀螺常值漂移估计曲线

图7 轴向陀螺常值漂移估计曲线

图8 陀螺标度因数误差误差估计曲线

在单轴旋转SINS中，周期性转动IMU抵消了水平方向常值漂移误差对失准角估计精度影响，由图4和表1可以看出，三个方向失准角估计收敛精度高，特别是方位失准角收敛误差为0.0021′，满足高精度SINS对初始对准精度需求。同时，由图5、图6以及表2、表3可以看出，水平方向IMU误差经过15min滤波估计后，最终能够收敛至真值附近。

表1 失准角估计误差

表2 加速度计常值偏置估计值

表3 陀螺常值漂移估计值

第一级Kalman滤波器完成初始失准角以及水平方向IMU误差估计后转至执行第二级Kalman滤波，开始对轴向陀螺常值漂移以及陀螺标度因数误差进行估计，由于轴向陀螺常值漂移和陀螺标度因数误差可观测度相对较低，因此，其估计收敛时间较长，由图7、图8以及表3、表4可以看出，经过2h在线估计后，轴向陀螺常值漂移及其标度因数误差得以精确估计，实现了单轴旋转SINS在线自标定目的。

完成两级Kalman滤波后进行24h纯惯性导航仿真，对比有无标校实验前后SINS长时间导航精度，结果如图9所示。

图9 有无在线自标定SINS位置误差对比

表5 不同实验方案下的定位误差

由图9和表5可以看出，如果不进行IMU常值漂移误差系统标校，捷联惯导系统24h定位误差为6.71n mile，通过在线系统级标校，主要是完成转轴方向上陀螺漂移误差的估计与补偿，其后长时间定位仅有1.96n mile，相比无系统级标校，其定位精度得到提高。

6 系统验证实验

利用实验室研制的单轴旋转SINS进行初始对准验证实验，其中，IMU由3个光纤陀螺和3个石英挠性加速度计组成，其主要性能指标如表6所示。

表6 单轴旋转SINS主要性能指标

单轴旋转SINS以及三轴转台如图10所示，将SINS放置在三轴转台上，控制转台做三轴摇摆运动，为了模拟水中兵器受海浪影响而产生的角运动，设置俯仰角摇摆幅度为5°，周期为5s，横摇角摇摆幅度为2°，周期为1.25s，航向角摇摆幅度为5°，周期为5s，Kalman滤波器更新周期为1s。

图10 实验设备及环境

采用一组转台IMU实测数据进行半物理验证实验，图11为有无在线自标定过程时SINS位置误差对比曲线，图中蓝色曲线为无在线自标定过程时SINS导航12h时位置误差曲线，红色点画线为有在线自标定过程时SINS导航12h位置误差曲线。

图11 有无在线自标定SINS位置误差对比

表7 不同实验方案下的定位误差

由图11和表7可以看出，实际导航系统中，对IMU误差在线自标定提高了SINS长时间导航定位精度，其12h位置误差由5.14n mile减小为2.13n mile，具有工程应用价值。

7 结束语

本文对单轴旋转SINS轴向陀螺漂移在线自标定方法进行了研究。首先分析了旋转调制原理，并给出了在线自标定Kalman滤波状态方程和量测方程。然后对轴向陀螺漂移误差传递路径进行了分析，指出等效东向陀螺漂移、“数学平台”失准角、等效北向加速度计偏置等会影响轴向陀螺常值漂移估计精度，为此提出了一种基于两级Kalman滤波的单轴旋转SINS轴向陀螺漂移自标校方法。最后进行了仿真和实际系统验证实验，可以看出，第二级Kalman滤波器能够较好地实现轴向陀螺常值漂移和标度因数误差的精确估计，同时转台验证实验中，单轴旋转SINS经过在线自标定与补偿后，其位置误差减小了58.6%，由此验证了本文所提出方法的正确性。