[发明专利]改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法

说明书

本发明提供了改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法，包括：对需要用到的坐标系进行定义；建立水下潜航阶段SINS/DVL子系统状态方程；建立水下潜航阶段SINS/DVL子系统量测方程；建立水面位置修正阶段SINS/GPS子系统的状态方程；建立水面位置修正阶段SINS/GPS子系统的量测方程；建立水下潜航阶段的非线性滤波方程，在水下潜航一段时间以后，AUV浮出水面，建立水面位置修正阶段非线性滤波方程，进行改进的无迹卡尔曼滤波解算，完成时间更新、量测更新和滤波更新，完成定时位置信息校正。本发明能够提高水下组合导航系统的滤波结算效率及导航精度，降低了滤波算法计算的复杂程度，保证实时性和稳定性。

技术领域

本发明属于惯性导航领域，涉及一种改进的无迹卡尔曼滤波算法在水下组合导航SINS/DVL/GPS中的应用方法。

背景技术

水下自主航行器(Autonomous UnderwaterVehicle，AUV)无论在民用还是军事方面都有广泛的应用。军事方面如战区侦察、探测扫除水雷、潜艇对抗、海上预警封锁航线或港口、攻击敌舰船或潜艇、破坏石油设施及通讯网络、水下中继通讯等；民用方面如海洋资源勘察与开发、海洋救险和打捞等。就目前的发展而言，导航问题仍然是AUV所面临的主要技术挑战之一。导航系统必须提供长航时的精确姿态、航向、速度和位置信息，而且精确的导航能力是AUV有效应用和安全回收的一个关键技术。但由于受其大小、重量、电源使用的限制及水介质的特殊性、隐蔽性等因素的影响，实现AUV的精确导航是一项艰难的任务。

远航程AUV因为其远航程的特点，要求长时间、长距离的在水下航行，这就要求导航必需满足一定的精度。惯性导航是一种自主导航，不需要与外界的联系，隐蔽性好，是AUV理想的导航方式。但是，惯导有误差积累的问题，短时间内惯导可以保持很好的导航精度，但是随着时间的延长，误差积累效应非常明显，无法保证导航精度。因此，通常都是采用其它的辅助导航系统配合以修正惯导的累积误差。如多普勒测速，罗兰C等方法。GPS由于具有导航精度高，没有误差积累的特点，是高精度导航定位的首选方法。但是对于水下航行器而言，需要浮出水面才能接收GPS信号，而且为了隐蔽不能长时间漂浮在水面，因此GPS只能作为校正系统使用。此外，由于AUV组合导航系统在本质上具有非线性特性，传统上采用扩展卡尔曼滤波(EKF)对非线性模型进行线性化处理，在此过程中由于忽略了模型的高阶导数，随着时间的增长，导航精度难以得到保证，而且在计算过程中反复计算系统方程的雅各比矩阵，增加了滤波计算量。

发明内容

为解决上述问题，本发明公开了改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法，能够提高水下组合导航系统的滤波结算效率及导航精度，保证实时性和稳定性。

为了达到上述目的，本发明提供如下技术方案：

改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法，包括如下步骤：

(1)对需要用到的坐标系进行定义：

(2)建立水下潜航阶段SINS/DVL子系统的状态方程：以SINS的3轴姿态误差，3轴速度误差，位置误差，3轴陀螺仪随机漂移、3轴加速度计零偏和DVL速度偏移误差、刻度系数误差建立21维状态向量，根据系统误差方程建立该子系统的状态方程；

(3)建立水下潜航阶段SINS/DVL子系统的量测方程：根据SINS解算的3维速度量和DVL测量的速度量之差作为量测量，并结合步骤(2)选取的误差状态向量建立该子系统的量测方程；

(4)建立水面位置修正阶段SINS/GPS子系统的状态方程：不考虑天向运动速度及误差，以SINS的3轴姿态误差、速度误差、位置误差、3轴陀螺仪随机漂移、加速度计零偏建立12维状态向量，根据误差方程建立该子系统的状态方程；