一种基于状态变换卡尔曼滤波的DVL/SINS组合导航方法技术

本发明专利技术属于导航技术领域，具体涉及一种基于状态变换卡尔曼滤波的DVL/SINS组合导航方法，本发明专利技术将速度误差统一投影在真实导航坐标系中，推导了更严密的SINS姿态、速度和位置误差方程。在新定义的速度误差基础上，构建了改进的DVL/SINS组合导航卡尔曼滤波状态方程和量测方程。仿真和实验对比结果表明，与常规DVL/SINS卡尔曼滤波方法相比，本发明专利技术方法在一定程度上提高了系统的导航性能和精度。定程度上提高了系统的导航性能和精度。定程度上提高了系统的导航性能和精度。

【技术实现步骤摘要】
一种基于状态变换卡尔曼滤波的DVL/SINS组合导航方法


[0001]本专利技术属于导航
，具体涉及一种基于状态变换卡尔曼滤波的DVL/SINS组合导航方法。

技术介绍

[0002]水下无人自主航行器(Unmanned Underwater Vehicle，UUV)在海洋测绘、水下资源探索和水下目标探测等方面发挥着越来越重要的作用。高精度的姿态、速度和位置等导航参数是UUV高效和安全完成任务的重要前提。捷联惯性导航系统(Strapdown Inertial Navigation System，SINS)具有自主性、隐蔽性和主动性等优势，可以给UUV提供姿态、速度和位置等导航参数。但受惯性测量元件(Inertial Measurement Unit，IMU)误差的影响，SINS定位误差会随时间迅速累积，因此必须使用辅助传感器对累积误差进行修正。
[0003]多普勒计程仪(Doppler Velocity Logger，DVL)/SINS组合导航系统是当前应用最为广泛的UUV水下定位系统。根据传感器耦合方式的不同，DVL/SINS组合可分为松组合和紧组合，松组合和紧组合分别使用DVL测量的三维速度和波束频移作为卡尔曼(Kalman)滤波的量测信息。但是，由于目前大部分型号的DVL仅能提供三维速度，实际中应用最多的仍是松组合方式。近年来，许多学者针对DVL/SINS组合导航Kalman滤波中的DVL误差模型、非完整性约束和自适应滤波等方面展开了广泛研究，有效提升了DVL/SINS组合导航系统的定位性能。/>[0004]DVL/SINS组合导航系统Kalman滤波中的状态方程由SINS的姿态、速度和位置方程组成。根据姿态误差表达方式的不同，SINS误差方程可分为欧拉角误差模型和四元数误差模型。针对传统SINS误差模型不严密的问题，有学者提出了一种基于矢量运算坐标系一致的误差构建思想。在其基础上，有学者指出传统SINS速度误差的定义只考虑了实际速度与理论速度大小的差异，忽略了矢量坐标系不一致的影响。进一步，还有学者通过定义新的速度误差提出了状态变换Kalman滤波，并将其成功应用于全球卫星导航系统(Global Navigation Satellite System，GNSS)/SINS组合导航、初始对准和视觉/SINS组合导航等陆地场景。将速度误差定义在计算导航系中，分别推导了非线性GNSS阻尼SINS误差模型的欧拉角形式和四元数形式，并应用于GNSS/SINS大失准角对准中。不过，由于传统SINS误差方程中误差定义存在坐标系不一致的问题，导致导航性能和精度仍旧较低。

技术实现思路

[0005]本专利技术的目的在于提供一种基于状态变换卡尔曼滤波的DVL/SINS组合导航方法，用以解决由于速度误差定义存在坐标系不一致导致导航精度低的问题。
[0006]为解决上述技术问题，本专利技术所提供的技术方案以及技术方案对应的有益效果如下：
[0007]本专利技术的一种基于状态变换卡尔曼滤波的DVL/SINS组合导航方法，包括以下步骤：
[0008]1)获取导航状态参数的初始值；
[0009]2)利用改进的卡尔曼滤波算法对所述导航状态参数的初始值进行滤波，以得到导航状态参数的最优值；
[0010]改进的卡尔曼滤波算法的状态方程利用SINS的姿态方程、速度方程和位置方程确定；所述SINS的姿态方程、速度方程和位置方程依据定义的速度误差确定；所述定义的速度误差的公式为：
[0011][0012]其中，δv
n
表示SINS的速度误差，表示SINS在计算导航坐标系中实际计算的速度，v
n
表示在真实导航系中的理论速度，φ表示失准角，表示计算导航坐标系至真实导航坐标系的姿态转移矩阵；
[0013]3)利用得到的导航状态参数的最优值，进行DVL/SINS组合导航。
[0014]上述技术方案的有益效果为：本专利技术采用统一矢量运算坐标系的思想，将状态变换卡尔曼滤波引入DVL/SINS组合导航系统。针对传统SINS误差方程中误差定义存在坐标系不一致的问题，本专利技术将实际计算的通过姿态转移矩阵投影至真实导航系中，再与真值v
n
作差得到真实导航系中的速度误差从而实现将速度误差统一投影在真实导航坐标系中，构建了更严密的SINS误差方程(SINS的姿态方程、速度方程和位置方程)；进一步，基于新定义的速度误差推导了改进的DVL/SINS组合导航卡尔曼滤波状态方程。与现有技术中提出的状态变换误差模型相比，本专利技术的改进的卡尔曼滤波算法更为严密，导航性能和精度更高。通过仿真数据和实测数据验证了本专利技术算法的有效性，即面对同样存在坐标系不一致的情况下，使用本专利技术改进的卡尔曼滤波算法进行导航，导航性能更准确。
[0015]进一步地，所述INS的姿态方程的公式为：
[0016][0017][0018]其中，表示失准角φ的微分，表示n系相对于惯性系的角速度在n系上的投影，表示实际计算中带有误差的表示地球自转角速度在n系上的投影，表示由载体速度引起的角速度在n系上的投影，表示SINS获得的姿态矩阵，表示载体坐标系至导航坐标系的姿态转移矩阵，和分别表示SINS中的IMU的陀螺量测值及其误差，表示的误差，R
Mh
＝R
M
+h，R
Nh
＝R
N
+h；R
M
和R
N
分别表示SINS所在位置的子午圈与卯酉圈曲率半径，L和h分别为SINS的纬度和高程；
[0019]SINS的速度方程为：
[0020][0021]其中，表示定义的速度误差的微分，δg
n
表示由SINS位置误差引起的重力误差，表示实际计算中带有误差的重力矢量，表示含有误差量的地球自转角速度在n系上的投影，δf
b
表示加速度计测量误差，表示含有误差量的由载体速度引起的角速度在n系上的投影；
[0022]SINS的位置方程为：
[0023][0024][0025]其中，表示SINS的位置误差δp的微分，δp表示SINS的位置误差，表示含有误差的导航坐标系中速度的北向分量，表示含有误差的导航坐标系中速度的东向分量。
[0026]进一步地，所述改进的卡尔曼滤波算法的量测方程依据DVL测量的三维速度确定，所述改进的卡尔曼滤波算法的量测方程为：
[0027][0028]其中，表示计算导航坐标系至载体坐标系的姿态转移矩阵，Z表示量测向量，X表示状态向量，表示DVL实际观测的三维速度，表示载体坐标系相对于地球坐标系的角速度在载体坐标系上的投影，w
d
表示三维速度的白噪声。
[0029]上述技术方案的有益效果为：第一，由于本专利技术方法估计的天向失准角收敛速度更快，所以使得本专利技术方法的导航估计结果具有较强的一致性。第二，由于本专利技术算法的速度误差公式中不含比力项，而是由更加稳定的重力矢量代替，所以使得本专利技术方法的状态转移矩阵更加本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于状态变换卡尔曼滤波的DVL/SINS组合导航方法，其特征在于：包括以下步骤：1)获取导航状态参数的初始值；2)利用改进的卡尔曼滤波算法对所述导航状态参数的初始值进行滤波，以得到导航状态参数的最优值；改进的卡尔曼滤波算法的状态方程利用SINS的姿态方程、速度方程和位置方程确定；所述SINS的姿态方程、速度方程和位置方程依据定义的速度误差确定；所述定义的速度误差的公式为：其中，δv
n
表示SINS的速度误差，表示SINS在计算导航坐标系中实际计算的速度，v
n
表示在真实导航系中的理论速度，φ表示失准角，表示计算导航坐标系至真实导航坐标系的姿态转移矩阵；3)利用得到的导航状态参数的最优值，进行DVL/SINS组合导航。2.根据权利要求1所述的基于状态变换卡尔曼滤波的DVL/SINS组合导航方法，其特征在于：所述SINS的姿态方程为：在于：所述SINS的姿态方程为：其中，表示失准角φ的微分，表示n系相对于惯性系的角速度在n系上的投影，表示实际计算中带有误差的表示地球自转角速度在n系上的投影，表示由载体速度引起的角速度在导航坐标系上的投影，表示SINS获得的姿态矩阵，表示载体坐标系至导航坐标系的姿态转移矩阵，和分别表示SINS中的IMU的陀螺量测值及其误差，表示的误差；R
Mh
＝R
M
+h，R
Nh
＝R
N
+h；R
M
和R
N
分别表示SINS所在位置的子午圈与卯酉圈曲率半径，L和h分别为SINS的纬度和高程；SINS的速度方程为：其中，表示定义的速度误差的微分，δg
n
表示由SINS位置误差引起的重力误差，表示实际计算中带有误差的重力矢量，表示含有误差量的地球自转角速度在n系上的投影，δf
b
表示加速度计测量误差，表示含有误差量的由载体速度引起的角速度在n系上的投影；
SINS的位置方程为：SINS的位置方程为：其中，表示SINS的位置误差δp的微分，δp表示SINS的位置误差和表示含有误差的导航坐标系中速度的北向分量，表示含有误差的导航坐标系中速度的东向分量。3.根据权利要求1所述的基于状态变换卡尔曼滤波的DVL/SINS组合导航方法，其特征在于：所述改进的卡尔曼滤波算法的...

【专利技术属性】
技术研发人员：靳凯迪，柴洪洲，宿楚涵，关小果，李明，
申请(专利权)人：中国人民解放军战略支援部队信息工程大学，
类型：发明
国别省市：