CN103454662A - 一种基于ckf的sins/北斗/dvl组合对准方法 - Google Patents

Abstract

本发明公开了一种基于CKF的SINS/北斗/DVL组合对准方法。首先对SINS/北斗/DVL组合导航系统的状态方程进行非线性建模，建立量测方程时采用将多传感器量测信息拆分成类似单量测信息的方法，即将量测方程分为两组：一组为SINS/北斗滤波器量测方程，另一组为SINS/DVL滤波器量测方程，再在相应时刻分别对其进行滤波和判断，在融合中心进行数据融合。本发明可以增强SINS/北斗/DVL组合导航系统中北斗和DVL异步时的多传感器信息的利用率，大幅提高系统状态变量的估计精度，从而提高组合系统对准精度。

Description

一种基于CKF的SINS/北斗/DVL组合对准方法

技术领域

本发明涉及的是一种基于CKF的SINS/北斗/DVL组合对准方法，特别是涉及一种在非线性SINS/北斗/DVL组合系统中北斗和DVL异步情况下的初始对准方法。

背景技术

现代战争越来越强调其武器的精准性和自主性，对导航系统精度的要求也越来越高。由于捷联惯导系统(SINS)误差随时间积累，因此船用SINS常与辅助传感器组合，利用多传感器间的信息冗余、优势互补来提高系统导航的精度。虽然SINS/GPS组合导航系统在世界先进的舰载武器上使用较为普遍，但是GPS受制于美国政府。北斗(Beidou，BD)卫星定位系统是我国独立研制了一种区域性卫星导航定位通信系统，该系统可以根据用户和卫星之间的距离利用双星定位原理来提供较准确的位置信息。因此，SINS/BD组合模式越来越称为研究的热点。但是在某些情况下北斗信号会受到遮挡而无法工作，因此引入多普勒计程仪(DVL)，它可较准确地测得舰船相对于水的速度，它的引入可以较好地解决上述问题。因此利用SINS/BD/DVL组合导航系统的量测值进行初始对准可以很大程度上提高整个组合导航系统的导航精度和容错性。

由于北斗定位系统是一种主动式双向测距询问—应答的有源定位系统，北斗系统用户进行定位时，信号需要在地面和卫星间传递多次，再加上地面中心控制系统的处理时间，所以会产生很大程度上的时间延迟，造成定位信息滞后，在SINS/BD/DVL组合导航系统中表现在与其它传感器异步，从而严重影响系统的精度。另外，北斗定位系统所有用户的定位解算都是在地面中心控制系统完成的，当多用户同时进行定位解算时，会大幅加重地面中心控制系统的负担，造成信道阻塞甚至是系统崩溃。因此地面中心控制系统的计算能力在一定程度上决定了北斗定位系统的时间延迟大小和用户容量的大小。因此，对SINS/BD/DVL组合导航系统选择一种计算量小的异步组合对准方法势在必行。

发明内容

本发明的目的是提供可以在非线性SINS/BD/DVL组合导航系统中BD和DVL异步情况下大幅提高系统初始对准精度的一种基于CKF的初始对准方法。

本发明的目的是这样是实现的：

一种基于CKF的SINS/北斗/DVL组合对准方法，包括以下步骤：

步骤1、对SINS/北斗/DVL组合导航系统进行预热，并采集各个传感器的量测信息；

步骤2、根据SINS预热后采集的光纤陀螺仪和石英挠性加速度计输出的数据，初步确定此时的姿态信息，完成捷联惯导系统的粗对准；

步骤3、建立SINS/北斗/DVL组合导航系统的非线性状态方程；

步骤4、建立SINS/北斗/DVL组合导航系统的量测方程，利用将多传感器两侧信息拆分成单量测信息组的方法，即将量测方程分为两组：一组为SINS/北斗滤波器量测方程，以伪距和伪距率为观测量；另一组为SINS/DVL滤波器量测方程，以速度误差为观测量；

步骤5、将融合中心的采样间隔设定为两组滤波器采样间隔的最高精度时间单位，即取能够被两组传感器采样间隔同时整除的最大数为融合中心的采样间隔；

步骤6、根据融合中心的采样时刻，利用CKF对各传感器进行状态估计，并在融合中心对估计结果进行判断融合，从而完成SINS/北斗/DVL组合导航系统的初始对准。

所述的方法，所述步骤5中，所述的将融合中心的采样间隔设定为两组滤波器采样间隔的最高精度时间单位，其具体方法为：

设SINS/BD滤波器的采样间隔为T_BD，SINS/DVL滤波器的时间间隔为T_DVL，则融合中心的采样间隔ΔT按如下方法设置：

设 $\frac{T_{\mathrm{BD}}}{T_{\mathrm{DVL}}}=\frac{N_{\mathrm{BD}}}{N_{\mathrm{DVL}}}$ （其中

），则 $\mathrm{\ΔT}=\frac{T_{\mathrm{BD}}}{N_{\mathrm{BD}}}.$

所述的方法，所述步骤6中，所述的利用CKF对各传感器进行状态估计并在融合中心进行判断融合，其具体方法为：

(1)当只有SINS/BD滤波器有新的量测信息时，只对有新量测信息的SINS/BD滤波器进行CKF滤波，得到局部估计值

和方差P_BD，经过信息融合中心后的输出为该滤波器的局部估计值：

${\hat{X}}_f\left(k\right)={\hat{X}}_{\mathrm{BD}}\left(k\right)$

(2)当只有SINS/DVL滤波器有新的量测信息时，只对有新量测信息的SINS/DVL滤波器进行CKF滤波，得到局部估计值

和方差P_DVL，经过信息融合中心后的输出为该滤波器的局部估计值：

${\hat{X}}_f\left(k\right)={\hat{X}}_{\mathrm{DVL}}\left(k\right)$

(3)当SINS/BD滤波器和SINS/DVL滤波器均有新的量测信息时，分别对其进行CKF滤波，得到的局部估计值

和方差P_BD、P_DVL，信息融合中心对局部估计值进行加权组合后的输出为：

${\hat{X}}_f\left(k\right)=P_f\left(k\right)\·(P_f^{-1}\left(k\right){\hat{X}}_{\mathrm{BD}}\left(k\right)+P_f^{-1}\left(k\right){\hat{X}}_{\mathrm{DVL}}\left(k\right))$

其中 $P_f^{-1}\left(k\right)=P_{\mathrm{BD}}^{-1}\left(k\right)+P_{\mathrm{DVL}}^{-1}\left(k\right);$

(4)当两个导航设备均没有新的量测信息时，利用前一时刻的估计值只进行时间更新，SINS/BD/DVL组合导航系统的估计值为：

${\hat{X}}_f\left(k\right)=\hat{X}\left(k\right|k-1).$

本发明的优势在于：(1)、通过对SINS、BD和DVL传感器信息的利用，增大了对数据的利用率，提高了系统状态的估计精度，解决了多传感器异步组合导航问题，增强了组合系统的生存能力；(2)、由于运用了CKF，可以很大程度上减小计算量，减轻BD定位系统地面中心控制系统的计算负担，可以减小BD系统的时间延迟，增大用户量，提高组合系统在高动态情况下的实用性。

附图说明

图1为本发明的流程图；

图2为SINS/BD/DVL组合导航系统BD和DVL异步时融合中心的采样原理图；

图3为舰船的航行轨迹；

图4为利用本发明与利用单个传感器信息时天向失准角误差估计的对比曲线；

图5为利用本发明与利用传统方法时东向、北向失准角误差估计的对比曲线；

图6为利用本发明与利用传统方法时天向失准角误差估计的对比曲线；

具体实施方式

以下结合具体实施例，对本发明进行详细说明。

结合图1～6，本发明是一种基于CKF的SINS/BD/DVL组合导航系统中BD和DVL异步时的初始对准方法，其具体实施方式为：

步骤1、对SINS/BD/DVL组合导航系统进行预热，并采集各个传感器的量测信息。

步骤2、根据SINS预热后采集的光纤陀螺仪和石英挠性加速度计输出的数据，采用解析法来完成捷联惯导系统的粗对准，初步确定载体的姿态信息。

步骤3、建立SINS/BD/DVL组合导航系统的非线性状态方程。

步骤4、建立SINS/BD/DVL组合导航系统的量测方程，利用将多传感器两侧信息拆分成单量测信息组的方法，即将量测方程分为两组：一组为SINS/BD滤波器量测方程，以伪距和伪距率为观测量；另一组为SINS/DVL滤波器量测方程，以速度误差为观测量。

步骤5、将融合中心的采样间隔设定为两组滤波器采样间隔的最高精度时间单位，即取能同时被两组传感器采样间隔整除的最大数为融合中心的采样间隔。

如图2所示，设SINS/BD滤波器的采样间隔为T_BD=3s，SINS/DVL滤波器的最小时间间隔为T_DVL=2s，则

所以融合中心的采样间隔ΔT为： $\mathrm{\ΔT}=\frac{T_{\mathrm{BD}}}{N_{\mathrm{BD}}}=\frac{3s}{3}=1s.$

步骤6、根据融合中心的采样时刻，对各传感器进行判断，并利用CKF对各传感器进行状态估计，并在融合中心对估计结果进行融合：

(1)、当只有SINS/BD滤波器有新的量测信息时，只对有新量测信息的SINS/BD滤波器进行CKF滤波，得到局部估计值

和方差P_BD，经过信息融合中心后的输出为该滤波器的局部估计值：

${\hat{X}}_f\left(k\right)={\hat{X}}_{\mathrm{BD}}\left(k\right)---\left(1\right)$

(2)、当只有SINS/DVL滤波器有新的量测信息时，类似地，只对有新量测信息的SINS/DVL滤波器进行CKF滤波，得到局部估计值

和方差P_DVL，经过信息融合中心后的输出为该滤波器的局部估计值：

${\hat{X}}_f\left(k\right)={\hat{X}}_{\mathrm{DVL}}\left(k\right)---\left(2\right)$

(3)、当SINS/BD滤波器和SINS/DVL滤波器均有新的量测信息时，分别对其进行CKF滤波，得到的局部估计值和方差P_BD、P_DVL，信息融合中心对局部估计值进行加权组合后的输出为：

${\hat{X}}_f\left(k\right)=P_f\left(k\right)\·\left(P_f^{-1}\left(k\right){\hat{X}}_{\mathrm{BD}}\left(k\right){+P_f^{-1\left(k\right)}\hat{X}}_{\mathrm{DVL}}\left(k\right)\right)---\left(3\right)$

其中： $P_f^{-1}\left(k\right)=P_{\mathrm{BD}}^{-1}\left(k\right)+P_{\mathrm{DVL}}^{-1}\left(k\right).$

(4)、当两个导航设备均没有新的量测信息时，利用前一时刻的估计值只进行时间更新，SINS/BD/DVL组合导航系统的估计值为：

${\hat{X}}_f\left(k\right)=\hat{X}\left(k\right|k-1)---\left(5\right)$

利用上述方法可以估计出非线性组合导航系统的平台失准角，利用系统的平台失准角修正系统的捷联初始姿态矩阵可以得到精确的捷联初始矩阵，即

从而完成SINS/BD/DVL组合导航系统的初始对准。

对本发明进行仿真验证：

假设水面舰船的摇摆模型为：

$\left\{\begin{array}{c}\mathrm{\Ψ}={\mathrm{\Ψ}}_m\sin \left({\mathrm{\ω}}_{\mathrm{\Ψ}}t\right)+{\mathrm{\Ψ}}_k\\ \mathrm{\θ}={\mathrm{\θ}}_m\sin \left({\mathrm{\ω}}_{\mathrm{\θ}}t\right)+{\mathrm{\θ}}_k\\ \mathrm{\γ}={\mathrm{\γ}}_m\sin \left({\mathrm{\ω}}_{\mathrm{\γ}}t\right)+{\mathrm{\γ}}_k\end{array}\right.$

其中θ、Ψ、γ分别为舰船的纵摇角、横摇角、航向角；

摇摆幅值：θ_m=10°，Ψ_m=6°，γ_m=8°·

摇摆周期：T_m=10°，T_m=6°，T_m=8°·

初始姿态角：θ_k=0°，Ψ_k=0°，γ_k=45°·

水面舰船的初始经纬度：

舰船的运动模型为：舰船先系泊50s，再以东向加速度a_x=0.25m/s²和北向加速度a_y=0.3m/s²加速40s，再匀速航行80s，接着以a_x=0.05m/s²、a_y=-0.4m/s²加速40s，再匀速航行120s，再以a_x=-0.25m/s²、a_y=0.2m/s²加速40s，之后舰船一直匀速运动；

初始失准角：φ_x=1°，φ_y=1°，φ_z=10°;

陀螺常值漂移：ε_x=ε_y=ε_z=0.01°/h；

陀螺随机噪声：w_τx=w_τy=w_τz0.005°/h；

加速度计零偏： ${\▿}_x={\▿}_y={\▿}_z={10}^{-4}{g}_0$ ；

加速度计随机噪声：w_ax=w_ay=w_az5×10^-5g₀；

仿真时间：900s。

根据以上初始值，利用本发明所述的方法得到载体方位失准角估计误差曲线与分别利用SINS/DVL和SINS/BD量测数据得到的方位失准角估计误差曲线如图4所示；随后又将本发明与现有的基于UKF的异步融合方法进行了比较，水平、方位失准角估计误差曲线对比结果如图5和图6所示。

通过图4～图6可以看出，在SINS/BD/DVL组合导航系统中BD和DVL异步情况下，利用本发明可以快速、精确的估计出失准角，从而有效的提高SINS/BD/DVL组合导航系统的初始对准精度。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (3)

1.一种基于CKF的SINS/北斗/DVL组合对准方法，其特征在于，包括以下步骤：

步骤1、对SINS/北斗/DVL组合导航系统进行预热，并采集各个传感器的量测信息；

步骤2、根据SINS预热后采集的光纤陀螺仪和石英挠性加速度计输出的数据，初步确定此时的姿态信息，完成捷联惯导系统的粗对准；

步骤3、建立SINS/北斗/DVL组合导航系统的非线性状态方程；

步骤4、建立SINS/北斗/DVL组合导航系统的量测方程，利用将多传感器两侧信息拆分成单量测信息组的方法，即将量测方程分为两组：一组为SINS/北斗滤波器量测方程，以伪距和伪距率为观测量；另一组为SINS/DVL滤波器量测方程，以速度误差为观测量；

步骤5、将融合中心的采样间隔设定为两组滤波器采样间隔的最高精度时间单位，即取能够被两组传感器采样间隔同时整除的最大数为融合中心的采样间隔；

步骤6、根据融合中心的采样时刻，利用CKF对各传感器进行状态估计，并在融合中心对估计结果进行判断融合，从而完成SINS/北斗/DVL组合导航系统的初始对准。

2.根据权利要求1所述的方法，其特征在于，所述步骤5中，所述的将融合中心的采样间隔设定为两组滤波器采样间隔的最高精度时间单位，其具体方法为：

设SINS/BD滤波器的采样间隔为T_BD，SINS/DVL滤波器的时间间隔为T_DVL，则融合中心的采样间隔ΔT按如下方法设置：

设 $\frac{T_{\mathrm{BD}}}{T_{\mathrm{DVL}}}=\frac{N_{\mathrm{BD}}}{N_{\mathrm{DVL}}}$ （其中

），则 $\mathrm{\ΔT}=\frac{T_{\mathrm{BD}}}{N_{\mathrm{BD}}}.$

3.根据权利要求2所述的方法，其特征在于，所述步骤6中，所述的利用CKF对各传感器进行状态估计并在融合中心进行判断融合，其具体方法为：

(1)当只有SINS/BD滤波器有新的量测信息时，只对有新量测信息的SINS/BD滤波器进行CKF滤波，得到局部估计值

和方差P_BD，经过信息融合中心后的输出为该滤波器的局部估计值：

${\hat{X}}_f\left(k\right)={\hat{X}}_{\mathrm{BD}}\left(k\right)$

(2)当只有SINS/DVL滤波器有新的量测信息时，只对有新量测信息的SINS/DVL滤波器进行CKF滤波，得到局部估计值

和方差P_DVL，经过信息融合中心后的输出为该滤波器的局部估计值：

${\hat{X}}_f\left(k\right)={\hat{X}}_{\mathrm{DVL}}\left(k\right)$

(3)当SINS/BD滤波器和SINS/DVL滤波器均有新的量测信息时，分别对其进行CKF滤波，得到的局部估计值

和方差P_BD、P_DVL，信息融合中心对局部估计值进行加权组合后的输出为：

${\hat{X}}_f\left(k\right)=P_f\left(k\right)\·(P_f^{-1}\left(k\right){\hat{X}}_{\mathrm{BD}}\left(k\right)+P_f^{-1}\left(k\right){\hat{X}}_{\mathrm{DVL}}\left(k\right))$

其中 $P_f^{-1}\left(k\right)=P_{\mathrm{BD}}^{-1}\left(k\right)+P_{\mathrm{DVL}}^{-1}\left(k\right);$

(4)当两个导航设备均没有新的量测信息时，利用前一时刻的估计值只进行时间更新，SINS/BD/DVL组合导航系统的估计值为：

${\hat{X}}_f\left(k\right)=\hat{X}\left(k\right|k-1).$

Images