级联卡尔曼滤波在初始对准中的应用

2018-09-19张复建腾红磊

计算机测量与控制 2018年9期

张复建，单斌，杨波，薛亮，腾红磊

(火箭军工程大学导弹工程学院，西安 710025)

0 引言

随着微机电系统 (Micro－Electro Mechanical System，MEMS)技术的不断发展，在民用和军事方面，都应用的越来越广泛。传统的高精度惯性器件在捷联惯性导航系统(Strap－down Inertial Navigation Systems，SINS) 应用中，最突出的是成本和重量问题，为缓解这个问题，基于微机电技术的微型陀螺仪和加速度因具有低成本、功耗低、体积小等优势，在许多应用中，逐渐取代传统的陀螺仪和加速度计［1］。

捷联惯性导航系统进行初始对准的目的是提供系统的初始姿态矩阵，便于在导航坐标系中进行导航解算。因此，许多学者对此进行大量研究，推导了捷联惯性导航系统的对准模型，许多成果在实际中取得广泛运用［2-3］，然而，大部分导航系统的惯性器件都采用了高精度的陀螺仪和加速度计，对于低成本的微型惯性测量单元［4］(Miniature Inertial Measurement Units，MIMU，简称:微惯组)，由于受微机电技术工艺等所限，微惯组的量测噪声、零偏以及漂移等误差较大，其测量精度相对传统的惯性器件要低很多，以至于不能采取相同的方法实现微惯组的初始对准。针对此问题，许多学者也作出了相应研究成果，其中，李士心［5］针对低成本的IMU偏差和噪声大的问题，采用GPS提供的速度信息辅助对准，对IMU参数误差和航向角误差取得较好的估计，在仿真和实际应用都得到较好的效果;庞春雷［6］针对低精度的捷联惯性导航无法实现航向角的自对准和GPS双天线侧向需要较长时间初始化的问题，研究了二者相互辅助的算法，建立互相辅助的算法模型，通过实验验证了算法的简单实用性。

本文基于微惯组的捷联惯性导航系统无法实现自对准的问题，依靠外部测量信息辅助对准，根据全球卫星定位系统 (the global positioning system，GPS)与捷联惯性导航系统优势互补的特点，当载体静止时，可以采用GPS系统提供的速度信息及磁强计输出的航向角信息，辅助基于微惯组的捷联惯性导航系统完成初始对准。

1 初始对准原理

当载体处于静止状态时，针对微惯导不能实现自主对准，本文采用四个级联式卡尔曼滤波器的滤波方法解决微惯导对准问题，四个滤波器主要分为水平姿态角滤波器、航向角滤波器、陀螺仪误差滤波器和速度滤波器，其中，水平姿态角滤波器主要是根据加速度计输出作为观测量，估计出状态值，并反解出水平姿态角 (包括俯仰角和横滚角)，并将水平姿态角送入航向角滤波器计算滤波观测向量数据;在航向角滤波器中，航向角观测信息由磁强计输出并融合水平姿态角信息提供，通过滤波得到状态值，并反解得到载体航向角;在陀螺仪误差滤波器中，主要考虑陀螺仪误差为漂移误差和随机误差，通过滤波实时估计出当前陀螺仪误差值，用于修正水平姿态滤波和航向角滤波中的状态空间模型，改善滤波估计精度;在速度滤波中，根据GSP对静止载体的速度观测值，可作为速度误差观测量，通过滤波估计出加速度计零偏误差和载体失准角，加速度零偏可用于水平姿态角滤波中的观测量修正，减小观测误差，估计的失准角用于对姿态矩阵作进一步修正，提高姿态矩阵的稳定性。

1.1 水平姿态角滤波

记地心惯性坐标系i系;地球坐标系e系;以“东－北－天”地理坐标系为导航坐标系n系;以“右－前－上”为载体坐标系b系;计算导航坐标系记为n'系;失准角矢量［ɸx，ɸy，ɸz］T定义为理想导航系 n系依次按 z－x－y顺序分别绕ɸx，ɸy，ɸz旋转后得到计算导航系n'系，记载体的俯仰角、横滚角和航向角分别为θ，γ，ψ且n系绕“z－x－y”轴的顺序依次旋转θ，γ，ψ得到b系，则n系到b系的转换矩阵为:

式中，c表示余弦函数，s表示正弦函数 (下同)。按式(3)(4)(5)，由姿态矩阵转换为姿态角:

姿态矩阵微分方程［7］表示如下:

通过式 (6)可提取出状态微分方程:

对式 (9)进行离散化得到水平姿态角的状态空间模型和量测模型如下:

采用式 (12) (13) 分别计算状态转移矩阵 ɸ1，k－1，过程噪声 w1，k－1［8］:

式中，I3×3为三阶的单位矩阵，H1，k为观测矩阵，ηG为陀螺仪量测噪声向量，可假设为零均值的高斯白噪声，珘x1，k为向量x1，k构造的反对称矩阵，v1，k为加速度计的随机量测噪声向量，可假设为零均值的高斯白噪声，T为卡尔曼滤波周期。

实际观测向量z1，k为加速度计在b系中的投影且经过误差补偿的输出值，即:

系统过程噪声方差矩阵 Q1，k－1和量测噪声方差矩阵 R1，k计算如下［8］:

在k时刻时，由卡尔曼滤波估计后得到重力矢量在b系的状态估计值:

由式 (3)(4)得到载体的俯仰角和横滚角估计值，之后可给航向角滤波器的观测值提供输入。

事实上，当载体处于静止时，重力矢量在载体系的投影即为加速度计输出，有如下关系:

单位化后得到:

将式 (1)代入式 (22)并整理得到:

1.2 航向角滤波

在航向角滤波中，观测向量来源于水平姿态滤波的输出与磁强计输出融合计算得到，类似于水平姿态滤波的方法，选取式 (1)中姿态矩阵的第一列元素作为状态量，即:

通过式 (6)可提取出状态微分方程:

对式 (26)进行离散化得到水平姿态角的状态空间模型和量测模型如下:

类似于水平姿态滤波中的方法，得到如下关系式:

航向角可通过磁强计输出得到，文中详细阐述了高精度航向角可通过磁强计与加速度计融合输出得到［9］［11］，在此不再赘述，由 (23)(24)得到航向角:

至此，在k时刻时，可得到航向角滤波器的观测值z2，k计算如下:

在静止状态下，为有效利用水平姿态角滤波数据，降低航向角量测误差，采取求平均的方式提高量测质量，即一个航向角量测值为前50个量测值与k时刻量测值的平均值作为当前量测更新:

类似于水平姿态系统过程噪声方差矩阵，航向角系统过程噪声方差矩阵 Q2，k－1计算如下:

同样使用卡尔曼滤波算法得到状态最优解:

航向角最优估计值ψm如下:

1.3 陀螺仪误差滤波

采用随机游走过程表示陀螺仪的误差(本文仅考虑陀螺仪漂移和随机误差)，其系统状态空间模型表示如下［10］:

式中，x3，k表示由x，y，z轴三个陀螺仪误差构成的向量，ɸ3，k－1=I3×3为状态转移矩阵，H3，k=I3×3为观测矩阵，w3，k－1和v3，k为过程噪声和量测噪声。

在水平姿态卡尔曼滤波与航向角卡尔曼滤波中，由于陀螺仪误差的存在，在利用反对称矩阵计算状态转移矩阵 ɸ1，k－1和 ɸ2，k－1时，对状态量的一步预测状态和 x造成误差传递。对此，用分别按式 (19) (20) (38)计算得到姿态角由状态最优估计值和采用同样的方法计算得到的姿态角θm，γm，ψm，二者必然存在差异，其根本原因有陀螺仪的误差引起，由此可计算在k时刻时，陀螺仪输出角速度的误差观测量:

类似于航向角滤波器观测量的方法，对陀螺仪误差滤波器的观测量也采取求平均的方法。

1.4 速度滤波

在速度卡尔曼滤波器中，考虑载体在静止状态进行对准，可忽略天向速度的影响，载体的失准角和加速度计误差将在此得到最优估计值，其中，失准角用来校正由式(19)(20) (38)计算的姿态矩阵，加速度计误差用来校正水平姿态滤波器的观测值。

选取载体速度误差，失准角和加速度计零偏误差为系统状态量:

建立状态空间模型如下:

式中，ɸ4，k－1为状态转移矩阵，w4，k－1为过程噪声，v4，k为量测噪声，Bx，Bv，Bz为加速度计零偏误差，H4，k为观测矩阵，相关矩阵具体参数详见文献［5］。

由于GPS存在测量误差，在载体静止是，GPS输出的水平速度值 δVE，GPS，δVN，GPS和 δVU，GPS认为是速度误差观测向量。

经过卡尔曼滤波后得到k时刻的状态最优估计值:

利用此时刻估计的加速度计零偏误差修正水平姿态滤波中式 (15)的加速度计输出值，进入下一时刻的滤波更新。

1.5 姿态矩阵修正

以上叙述的四个级联式的卡尔曼滤波完成一个滤波周期更新时，得到一组姿态角及失准角，为获得更精确的姿态矩阵，充分利用最优估计信息，可采用如下方式:由姿态滤波器输出的姿态角θm，γm，ψm按式 (1)计算出待修正的姿态矩阵最后利用速度滤波器估计的失准角ɸx，ɸv，ɸz按式(52)对进行最终的修正，得到更精确和稳定的估计值，完成整个初始对准任务，当达到导航预设条件时，可转入导航状态更新解算。

1.6 卡尔曼滤波算法

在上述提到的滤波算法中，均采用经典卡尔曼滤波最优估计算法。卡尔曼滤波以线性最小方差为估计准则，通过量测值按一定递推算法实现状态向量的估计。假设系统状态空间模型如下:

其递推算法过程［10］如下:

2)时间更新 (k=1，…，K):计算系统状态一步预测估计及一步误差协方差矩阵:

3)量测更新 (k=1，…，K):通过量测信息及反馈增益估计系统状态及对应的误差协方差阵:

式中，xk为k时刻的状态向量，ɸk－1为状态转移矩阵，wk－1为系统噪声，vk为量测噪声，Rk为量测噪声方差阵，Qk为系统噪声方差阵，δki为Kronecker符号，满足:

2 实验结果与分析

仿真参数进行如下设置:

1)载体静止的参数为:纬度，经度，和高度分别为34°，108°，380 m，俯仰角，横滚角和航向角分别为60°，5°，40°，采样频率50 Hz，系统仿真时间为600 s;

3)GPS输出的位置误差均为10 m，测速误差0.1 m/s，输出频率1 Hz。

4)GPS水平位置误差10 m，高度误差20 m，测速精度0．1 m/s，输出频率 1 Hz。

在水平姿态滤波器中，直接采用加速度计的输出反解载体的俯仰角和横滚角，称为解算值，其结果波动较大(数据显示频率为1 Hz)，见图1。然而，经过水平姿态滤波估计后，降低了水平姿态角输出的波动程度，有效提高了水平姿态角的稳定性，统计信息见表1，对后续的航向角滤波器提供高质量的观测信息。

图1 俯仰角估计

图2 横滚角估计

在陀螺仪误差滤波估计中，陀螺仪误差仅考虑漂移和随机误差，滤波估计结果见图3。

_表1 水平姿态角统计数据

表2 稳定后的航向角ψ统计数据

图3 陀螺仪误差估计

在航向角滤波中，从图4明显看出，航向角ψ在系统运行100 s后，开始向真实值收敛。然而，在100 s后的收敛过程中，经过航向角滤波器估计的航向角 (称为滤波值)波动范围较大，且最大航向角误差在0.03°之内，表2统计了航向角ψ的滤波值和修正值在100～600 s之间的数据均值与方差;采用速度滤波估计的失准角向量，经过式 (52)修正得到姿态矩阵，经过式 (3)(4)(5)换算得到载体的新姿态角，称其为修正值，其中俯仰角和横滚角的变化差异不大，在此不再叙述，航向角修正值最大误差在0.02°之内，重点是修正值能有效提高航向角输出的稳定性，进而表明经过修正的姿态矩阵更稳定，为导航解算提供高精度的初始姿态矩阵。