[发明专利]一种基于5阶SSRCKF的SINS/DVL组合导航滤波方法

说明书

本发明公开了一种基于5阶SSRCKF的SINS/DVL组合导航滤波方法，具体包括：以水下航行器为研究对象，建立了捷联惯性导航非线性误差模型，并以多普勒测速仪作为辅助建立了组合导航模型；采用SSR规则选取CKF的容积点，并建立5阶SSRCKF的非线性滤波模型；本发明采用SSR规则对标准CKF滤波方法进行改进，在一定程度上减少了CKF滤波方法的容积采样点，在高阶系统中效果显著，解决了传统CKF算法在高阶系统中容积采样点的数目太大导致算法的实时性不好和效率不高的问题。

技术领域

本发明属于导航领域，特别是涉及一种基于5阶SSRCKF的SINS/DVL组合导航滤波方法。

背景技术

捷联惯性导航系统能实时测量系统所处的位置、当前速度和姿态等导航信息，并能在短时间内保持高精度，具有多输出信号、强隐蔽性、强自主式等优点，其系统噪声较低，但作为一种导航信息具有连续性的系统，系统误差会随时间积累，若不采取外界导航信息校正，其精度将会快速下降而导致无法使用。

发明内容

发明目的：为了提高AUV组合导航的精度及稳定性，并且考虑到实时的计算量，本发明提出了一种基于5阶SSRCKF的SINS/DVL组合导航滤波方法。

技术方案：一种基于5阶SSRCKF的SINS/DVL组合导航滤波方法，包括如下步骤：

1)建立SINS的非线性误差模型：在SINS中选取东北天地理坐标系作为导航坐标系，以水下航行器自身建立载体坐标系，并给定实际计算导航坐标系，建立惯性器件陀螺仪和速度计的误差模型，并求解系统的姿态误差方程、位置误差方程；

2)建立DVL的非线性误差模型：针对DVL误差中的刻度误差以及随机测量误差，以一阶马尔科夫过程来拟合误差方程；

3)根据步骤1和步骤2分别得到的SINS非线性误差模型和DVL非线性误差模型建立组合导航滤波模型：选取SINS的三维姿态角误差、速度误差的前两维、位置误差的前两维，三维陀螺常值漂移和加速度计的水平方向的两维常值误差的12维状态变量，选取DVL的刻度系数误差和随机常值测量误差作为状态量；然后根据步骤1和步骤2分别得到的SINS非线性误差模型和DVL非线性误差模型共同建立组合导航的状态方程；以DVL和SINS的东向、北向速度分量的差值作为观测量,建立SINS/DVL组合导航系统的观测方程；

4)建立5阶SSRCKF滤波模型，输出导航系统参数：利用5阶SSRCKF对SINS和DVL输出的信息进行滤波、融合，得到导航参数的最优估计并修正速度、位置和姿态矩阵，得到精确的导航信息。

进一步的，所述步骤1)中建立SINS非线性误差模型的具体步骤包括：

选取东-北-天坐标系为SINS的导航坐标系并记为n系，载体系选用右-前-上记为b系，SINS解算的导航坐标系记为n′系；系统真实姿态角为真实速度Vⁿ＝[V_E,V_N,V_U]^T，真实地理坐标P＝[L,λ,H]^T；记陀螺仪和加速度计的随机常值误差分别为ε_b和陀螺仪和加速度计对应的扰动白噪声分别为w_g和w_f，则陀螺仪误差ε^b＝ε_b+w_g，加速度计误差陀螺仪和加速度计的测量值为和和分别为地球自转角速度和导航坐标系相对地球坐标系的旋转角速度；V_N和V_E分别表示载体的运动速度的北向分量和东向分量；R_N和R_E分别为卯酉圈半径和子午圈半径；

建立SINS的速度、姿态和位置误差方程，具体如下：

速度方程为：

姿态误差方程为：