改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法技术

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

Application of improved unscented Kalman filter algorithm in Underwater Integrated Navigation

The invention provides an application method of improved unscented Kalman filter algorithm in Underwater Integrated navigation, which includes: defining the coordinate system needed; establishing the state equation of SINS/DVL subsystem in underwater submarine stage; establishing the measurement equation of SINS/DVL subsystem in underwater submarine stage; establishing the state equation of SINS/GPS subsystem in water surface position correction stage; and establishing the state equation of SINS/GPS subsystem in underwater submarine stage. The measurement equation of stage SINS/GPS subsystem; the non-linear filtering equation of underwater submarine phase is established. After a period of underwater submarine, AUV surfaces, and the non-linear filtering equation of surface position correction phase is established. The improved unscented Kalman filter is solved to complete time update, measurement update and filtering update, and to complete the correction of timing and position information. The invention can improve the filtering settlement efficiency and navigation accuracy of the underwater integrated navigation system, reduce the complexity of filtering algorithm calculation, and ensure real-time and stability.

【技术实现步骤摘要】
改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法
本专利技术属于惯性导航领域，涉及一种改进的无迹卡尔曼滤波算法在水下组合导航SINS/DVL/GPS中的应用方法。
技术介绍
水下自主航行器(AutonomousUnderwaterVehicle，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维状态向量，根据误差方程建立该子系统的状态方程；(5)建立水面位置修正阶段SINS/GPS子系统的量测方程：以SINS解算出来的位置、速度与GPS输出的位置、速度之差作为量测量，并结合步骤(4)选取的误差状态向量建立该子系统的量测方程；(6)综合步骤(2)和步骤(3)建立水下潜航阶段的非线性滤波方程，在水下潜航一段时间以后，AUV浮出水面，结合步骤(4)和步骤(5)建立水面位置修正阶段非线性滤波方程，进行改进的无迹卡尔曼滤波解算，完成时间更新、量测更新和滤波更新，完成定时位置信息校正。进一步的，所述步骤(1)建立的坐标系具体包括：i——惯性坐标系：不随地球旋转，原点位于地球中心，zi轴指向北极，xi轴指向春分点，yi轴与xi、zi构成右手坐标系；e——地球坐标系：与地球固联，原点位于地心，xe轴穿越本初子午线与赤道交点，ze轴指向北极，ye轴xe、ze构成右手坐标系；b——载体坐标系：原点位于运载体中心，zb轴垂直运载体向上，xb指向运载体前方，yb与xb、zb构成右手坐标系；p——实际计算得出的平台坐标系；n——与东-北-天地理坐标系重合的导航坐标系。进一步的，所述步骤(2)中建立水下潜航阶段SINS/DVL子系统的状态方程具体包括如下步骤：取姿态误差角(φEφNφU)、速度误差(δvEδvNδvU)位置误差(δLδλδh)、陀螺常值漂移(εbxεbyεbz)和加速度计随机常值误差作为SINS系统的状态量，记为:通过理想惯导比力方程的速度微分方程和捷联惯导系统的实际速度微分方程，推导出四元数表示的速度误差方程：其中，Vn是载体在n系下的理想速度，是n系下的地球自转角速度，为n系相对于e系的角速度在n系下的投影，fb为b系下的比力；式中，为p系到n系的转换四元数，为b系到p系的转换四元数，和分别代表p系到n系以及b系到p系的转换矩阵；式中，“～”表示载体实际测量值，δ表示载体的理想值与实际测量值之间的误差，gn为n系下的重力加速度，为b系下的加速度计误差向量；四元数姿态误差方程：其中，表示n系相对于i系的旋转角速度在n系下的投影，为n系到p系的方向余弦矩阵，εb为陀螺仪误差向量在b系下的投影，B为关于四元数的4×3维矩阵；位置误差方程：其中，RM和RN分别表示地球的子午圈曲率半径和卯酉圈曲率半径，L表示载体的纬度，λ表示载体的经度，h表示载体的高度；SINS系统的噪声:WN(t)＝[ωgxωgyωgzωaxωayωaz]T则SINS的系统误差方程可以表示为：FN[·]为非线连续函数；取DVL速度偏移误差(δVdxδVdyδVdz)和刻度系数误差(ΔkdxΔkdyΔkdz)作为DVL系统状态变量，记为:XD(t)＝[δVdxδVdyδVdzδkdxδkdyδkdz]TDVL的误差模型为：其中，βd表示速度偏移误差的相关时间倒数，ωd表示激励白噪声；相应的误差状态方程为：式中：WD(t)＝[ωdxωdyωdz]T；ωdi(i＝x,y,z)为激励白噪声；GD(t)＝[I3×3O3×3]；τdi(i＝x,y,z)表示速度偏移误差的相关时间；选取SINS和DVL子系统的误差状态变量，则组合导航系统的状态向量为系统的噪声向量为系统的状态方程表示为：式中，状态函数F1[·]为非线性连续函数，Γ1(t)为该子系统噪声阵。进一步的，所述步骤(3)中建立水下潜航阶段SINS/DVL子系统的量测方程具体包括如下步骤：由SINS和DVL形成的量测量：其中，由SINS提供变化所用的姿态矩阵vEI,vNI,vUI分别表示SINS解算出的三轴速度，vED,vND,vUD分别表示DVL测出的三轴速度，vE,vN,vU表示载体在导航坐标系下的真实速度，vx,vy,vz表示在载体坐标系下的真实速度，δvDE，δvDN，δvDU为DVL测速误差换算到导航坐标系后的误差；将上式展开，并结合之前选取的系统误差状态得该组合系统的量测方程为：Z1＝H1[X1(t),t]+V1(t)式中：H1[·]为非线性连续函数；V1(t)为量测噪声。进一步的，所述步骤(4)中建立水面位置修正阶段SINS/GPS子系统的状态方程具体包括如下步骤：取状态变量：系统的噪声误差为：W2(t)＝[00ωgxωgyωgzωa本文档来自技高网...

【技术保护点】
1.改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法，其特征在于，包括如下步骤：(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维状态向量，根据误差方程建立该子系统的状态方程；(5)建立水面位置修正阶段SINS/GPS子系统的量测方程：以SINS解算出来的位置、速度与GPS输出的位置、速度之差作为量测量，并结合步骤(4)选取的误差状态向量建立该子系统的量测方程；(6)综合步骤(2)和步骤(3)建立水下潜航阶段的非线性滤波方程，在水下潜航一段时间以后，AUV浮出水面，结合步骤(4)和步骤(5)建立水面位置修正阶段非线性滤波方程，进行改进的无迹卡尔曼滤波解算，完成时间更新、量测更新和滤波更新，完成定时位置信息校正。...

【技术特征摘要】
1.改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法，其特征在于，包括如下步骤：(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维状态向量，根据误差方程建立该子系统的状态方程；(5)建立水面位置修正阶段SINS/GPS子系统的量测方程：以SINS解算出来的位置、速度与GPS输出的位置、速度之差作为量测量，并结合步骤(4)选取的误差状态向量建立该子系统的量测方程；(6)综合步骤(2)和步骤(3)建立水下潜航阶段的非线性滤波方程，在水下潜航一段时间以后，AUV浮出水面，结合步骤(4)和步骤(5)建立水面位置修正阶段非线性滤波方程，进行改进的无迹卡尔曼滤波解算，完成时间更新、量测更新和滤波更新，完成定时位置信息校正。2.根据权利要求1所述的改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法，其特征在于，所述步骤(1)建立的坐标系具体包括：i——惯性坐标系：不随地球旋转，原点位于地球中心，zi轴指向北极，xi轴指向春分点，yi轴与xi、zi构成右手坐标系；e——地球坐标系：与地球固联，原点位于地心，xe轴穿越本初子午线与赤道交点，ze轴指向北极，ye轴xe、ze构成右手坐标系；b——载体坐标系：原点位于运载体中心，zb轴垂直运载体向上，xb指向运载体前方，yb与xb、zb构成右手坐标系；p——实际计算得出的平台坐标系；n——与东-北-天地理坐标系重合的导航坐标系。3.根据权利要求1所述的改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法，其特征在于，所述步骤(2)中建立水下潜航阶段SINS/DVL子系统的状态方程具体包括如下步骤：取姿态误差角(φEφNφU)、速度误差(δvEδvNδvU)位置误差(δLδλδh)、陀螺常值漂移(εbxεbyεbz)和加速度计随机常值误差(▽bx▽by▽bz)作为SINS系统的状态量，记为:XN(t)＝[φEφNφUδvEδvNδvUδLδλδhεbxεbyεbz▽bx▽by▽bz]T，通过理想惯导比力方程的速度微分方程和捷联惯导系统的实际速度微分方程，推导出四元数表示的速度误差方程：其中，Vn是载体在n系下的理想速度，是n系下的地球自转角速度，为n系相对于e系的角速度在n系下的投影，fb为b系下的比力；式中，为p系到n系的转换四元数，为b系到p系的转换四元数，和分别代表p系到n系以及b系到p系的转换矩阵；式中，“～”表示载体实际测量值，δ表示载体的理想值与实际测量值之间的误差，gn为n系下的重力加速度，▽b为b系下的加速度计误差向量；四元数姿态误差方程：其中，表示n系相对于i系的旋转角速度在n系下的投影，为n系到p系的方向余弦矩阵，εb为陀螺仪误差向量在b系下的投影，B为关于四元数的4×3维矩阵；位置误差方程：其中，RM和RN分别表示地球的子午圈曲率半径和卯酉圈曲率半径，L表示载体的纬度，λ表示载体的经度，h表示载体的高度；SINS系统的噪声:WN(t)＝[ωgxωgyωgzωaxωayωaz]T则SINS的系统误差方程可以表示为：FN[·]为非线连续函数；取DVL速度偏移误差(δVdxδVdyδVdz)和刻度系数误差(ΔkdxΔkdyΔkdz)作为DVL系统状态变量，记为:XD(t)＝[δVdxδVdyδVdzδkdxδkdyδkdz]TDVL的误差模型为：其中，βd表示速度偏移误差的相关时间倒数，ωd表示激励白噪声；相应的误差状态方程为：式中：WD(t)＝[ωdxωdyωdz]T；ωdi(i＝x,y,z)为激励白噪声；GD(t)＝[I3×3O3×3]；τdi(i＝x,y,z)表示速度偏移误差的相关时间；选取SINS和DVL子系统的误差状态变量，则组合导航系统的状态向量为系统的噪声向量为系统的状态方程表示为：式中，状态函数F1[·]为非线性连续函数，Γ1(t)为该子系统噪声阵。4.根据权利要求1所述的改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法，其特征在于，所述步骤(3)中建立水下潜航阶段SINS/DVL子系统的量测方程具体包括如下步骤：由SINS和DVL形成的量测量：其中，由SI...

【专利技术属性】
技术研发人员：徐晓苏，杨阳，袁杰，翁铖铖，梁紫依，刘兴华，
申请(专利权)人：东南大学，
类型：发明
国别省市：江苏,32