多传感器组合的水下航行器导航定位方法和系统技术方案

本发明专利技术提供了一种多传感器组合的水下航行器导航定位方法和系统，包括：步骤S1：取位置和姿态角的均值作为初始信息；步骤S2：进行卡尔曼滤波器的时间更新；步骤S3：将DVL测得的以自身为中心的速度消除比例误差和杆臂误差，得到SINS中心的速度测量值；步骤S4：根据水下航行器是否浮出水面选择GPS或USBL测量值消除杆臂误差，得到SINS中心的位置测量值；步骤S5：根据速度测量值和位置测量值进行卡尔曼滤波器的量测更新，得到的水下航行器的位置；步骤S6：根据水下航行器的位置计算姿态角，输出导航位置、速度和姿态角。本发明专利技术能够实现水下航行器的高精度、长航时的导航定位。长航时的导航定位。长航时的导航定位。

【技术实现步骤摘要】
多传感器组合的水下航行器导航定位方法和系统


[0001]本专利技术涉及导航定位
，具体地，涉及一种多传感器组合的水下航行器导航定位方法和系统。

技术介绍

[0002]近年来，对海洋资源的探测与水下无人军事预警对抗已经成为国内外研究的热点，水下环境存在着封闭性前、复杂多变等特点，水下航行器一般需要按照预定轨迹在水下完成一系列任务，因此水下航行器的导航系统成为水下航行器设计中的重点和难点。
[0003]专利文献CN102980579A(申请号：CN201210460353.0)公开了一种自主水下航行器自主导航定位方法，特别是涉及一种基于强跟踪容积卡尔曼滤波的自主水下航行器自主导航定位方法。本专利技术包括如下步骤：选取自主水下航行器运动模型；选取自主水下航行器测量模型；通过自主水下航行器运动模型和测量模型构建动态系统；滤波器参数初始化；选取渐消因子；更新滤波器时间；更新滤波器量测数据；由当前时刻更新到下一时刻，重复执行步骤(5)至步骤(7)，直到到达终止时刻，结束计算，输出结果。
[0004]GPS全球卫星定位系统具有全天候，覆本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种多传感器组合的水下航行器导航定位方法，其特征在于，包括：步骤S1：在水下航行器静止时，取一段时间内位置和姿态角的均值作为初始信息；步骤S2：在水下航行器航行时，采用惯性导航系统SINS输出的姿态角和加速度信息进行卡尔曼滤波器的时间更新；步骤S3：将多普勒计程仪DVL测得的以自身为中心的速度消除比例误差和杆臂误差，得到SINS中心的速度测量值；步骤S4：根据水下航行器是否浮出水面选择GPS导航系统或超短基线定位系统USBL测量值消除杆臂误差，得到SINS中心的位置测量值；步骤S5：采用步骤S3和步骤S4的速度测量值和位置测量值进行卡尔曼滤波器的量测更新，得到的水下航行器的位置；步骤S6：根据步骤S5得到的水下航行器的位置计算姿态角，输出导航位置、速度和姿态角。2.根据权利要求1所述的多传感器组合的水下航行器导航定位方法，其特征在于，所述步骤S1包括：以SINS的初始位置为坐标原点，以东向为x轴，北向为y轴，天向为z轴建立导航坐标系，以载体轴向向前为x轴，载体向左为y轴，载体向上为z轴建立载体坐标系；水下航行器在水面静止一段时间，取这段时间GPS测得的位置均值作为水下航行器的初始航行位置，取这段时间SINS测得的姿态角均值作为水下航行器的初始航向姿态信息，记在静止条件下GPS测得的位置均值为SINS测得的姿态角均值为则无人潜航器的初始位置坐标X0(x0,y0)，初始速度为V0(0,0,0)，初始姿态角为所述步骤S2包括：选取卡尔曼滤波器的状态变量为：X＝[x y v
x v
y v
z
]
T
其中，x,y为水下航行器导航坐标系下x轴和y轴方向的位置坐标；v
x
,v
y
,v
z
为水下航行器载体坐标系下的三轴方向速度；根据水下航行器的运动方程，得到卡尔曼滤波器的状态转移方程：其中，表示卡尔曼滤波器的状态变量；F表示卡尔曼滤波器的状态转移矩阵；其中，W是过程噪声，服从正态分布W～(0，Q)；Δt表示卡尔曼滤波器状态转移的时间间隔；Q表示过程噪声的协方差矩阵；将上述卡尔曼滤波器状态方程离散化得：X
k
＝Φ
k
‑1X
k
‑1+W
k
‑1其中，k表示卡尔曼滤波器离散化后第k时刻；
其中，I表示单位矩阵；卡尔曼滤波器进行时间更新计算水下航行器的位置和速度的过程如下：向前推算状态变量，表达式为：X
k
＝Φ
k
‑1X
k
‑1+W
k
‑1向前推算误差协方差矩阵，表达式为：其中，X
k
表示第k时刻的卡尔曼滤波器的状态变量；P
k|k
‑1表示先验协方差估计。3.根据权利要求2所述的多传感器组合的水下航行器导航定位方法，其特征在于，所述步骤S3包括：若DVL中心测得的速度将其乘上比例因子δk进行修正得到DVL中心的测量速度设SINS中心与DVL中心的杆臂为l
DVL
，SINS输出的角加速度为ω，则杆臂误差为ω
×
l
DVL
，将DVL以自身为中心测得的速度减去杆臂误差得到SINS中心的测量速度，表达式为：其中，l
DVL
表示SINS中心到DVL中心的杆臂。4.根据权利要求3所述的多传感器组合的水下航行器导航定位方法，其特征在于，所述步骤S4包括：根据深度计判断水下航行器是否浮出水面，若水下航行器浮出水面，则使用GPS测得的位置，经杆臂误差消除后作为SINS中心的位置测量值；若水下航行器并未浮出水面，则以USBL测得的位置消除杆臂误差作为SINS中心的位置测量值；设SINS中心与GPS中心的杆臂为l
GPS
，SINS中心与USBL中心的杆臂为l
USBL
，SINS输出的姿态角为根据姿态角计算旋转矩阵，表达式为：若水下航行器浮出水面，设GPS测得的位置为则SINS中心的位置测量值为：其中，表示GPS测得的水下航行器的位置；若水下航行器未浮出水面，设USBL基站位置为USBL移动站测得其与USBL基站相距ΔX＝[Δx,Δy]，则SINS中心的位置测量值为：：5.根据权利要求4所述的多传感器组合的水下航行器导航定位方法，其特征在于，所述步骤S5包括：
以位置和速度为量测量，得到卡尔曼滤波器的量测方程为：其中，H表示卡尔曼滤波器的量测矩阵；v
x
表示水下航行器载体坐标系下的x轴方向速度；v
y
表示水下航行器载体坐标系下的y轴方向速度；v
z
表示水下航行器载体坐标系下的z轴方向速度；U是测量噪声，服从正态分布U～(0，R)；R表示量测噪声的协方差矩阵；将卡尔曼滤波器的量测方程分解为：将卡尔曼滤波器的量测方程分解为：表示分解后的第一组量测向量；表示分解后的第二组量测向量；表示分解后的第一组量测矩阵；表示分解后的第二组量测矩阵；表示分解后的第一组量测噪声矩阵；表示分解后的第二组量测噪声矩阵；其中，令x
k
表示第k时刻水下航行器导航坐标系下的x轴位置坐标；y
k
表示第k时刻水下航行器导航坐标系下的y轴位置坐标；v
xk
表示第k时刻水下航行器载体坐标系下的x轴方向速度；v
yk
表示第k时刻水下航行器载体坐标系下的y轴方向速度；v
zk
表示第k时刻水下航行器载体坐标系下的z轴方向速度；表示分解后的第一组量测噪声协方差矩阵；表示分解后的第二组量测噪声协方差矩阵；表示序贯滤波的状态变量初值；X
k|k
‑1表示先验状态变量估计；表示序贯滤波的先验协方差估计初值；P
k|k
‑1表示先验协方差估计初值；根据序贯滤波原理降低量测更新的计算量，若观测到位置测量值，则进行第一次子量测更新：其中，表示序贯滤波第一次卡尔曼增益；表示序贯滤波第一次后验状态变量估计；表示序贯滤波第一次后验协方差估计；表示分解后的第一组量测矩阵；表示分解后的第一组量测向量；若观测到速度测量值，则进行第二次子量测更新：
最终得到状态量的最优估计其中，P
k
表示本次序贯滤波观测最终的后验协方差估计；所述步骤S6包括：将SINS输出的俯仰角θ和滚动角φ作为真值，利用水下航行器前后两次的位置计算偏航角ψ，表达式为：6.一种...

【专利技术属性】
技术研发人员：刘佳鹏，于特，曾青山，卢地华，周畅，
申请(专利权)人：上海船舶电子设备研究所中国船舶集团有限公司第七二六研究所，
类型：发明
国别省市：