[发明专利]基于无色卡尔曼滤波的深海机器人超短基线组合导航方法

说明书

技术领域

本发明涉及海洋工程领域的一种深海机器人导航技术，具体地说是一种基于无色卡尔曼滤波的超短基线组合导航方法。

背景技术

精确的导航能力是水下机器人有效作业和安全回收的关键。导航系统必须提供远距离及长时间范围内的精确定位、速度和姿态信息。但是由于受到水下环境的复杂性、机器人自身体积、重量、能源以及隐蔽性等因素的影响，实现高精度的水下机器人导航仍然是一项艰巨的任务。

现有的水下导航方法主要是多传感器组合导航方法，与陆地组合导航方法无异，采用运动学方程对多传感器信息进行滤波。但是针对深海机器人的导航问题，机器人在水中的运动状态更加复杂多变，声学定位数据有较多野值而且有很大的延迟，采用水中动力学方程才能更精确地刻画机器人在水中的运动状态，对于有延迟的位置信息正确融合对提高导航精度也至关重要。

发明内容

针对现有技术的不足，本发明提供一种基于无色卡尔曼滤波的深海机器人组合导航方法，目的在于提高使用超短基线定位系统的深海机器人的导航精度，同时能够平滑深海机器人控制系统所需的航向、深度以及载体坐标系下的速度信息。

本发明为实现上述目的所采用的技术方案是：

一种基于无色卡尔曼滤波的深海机器人超短基线组合导航方法，利用全球定位系统获取深海机器人的初始绝对位置作为航迹推算的初始点，并采集深海机器人的初始信息；采用无色卡尔曼滤波方法对采集到的初始信息进行数据融合，得出融合后的信息。

所述初始信息包括多普勒计程仪采集的线速度信息，运动传感器采集的姿态信息，超短基线声学定位装置采集的位置信息和深度计采集的深度信息。

所述超短基线声学定位装置通过位移修正方法采集位置信息。

所述位移修正方法为用当前时刻接收到的超短基线声学定位装置提供的位置信息加上当前时刻距上次水声通讯时刻时间段内深海机器人走过的位移作为当前时刻超短基线定位装置提供的位置观测值。

在无色卡尔曼滤波过程中，取深海机器人的动力学方程作为系统的状态方程。

所述状态方程建立如下：

其中，M为6×6维的惯性矩阵，包含离心力和哥氏力的6×6维矩阵，为6×6维的水动力矩阵，G(q)包含重力和浮力的6×1维矩阵，q和u分别为6×1维的状态和电机转矩矢量。

在无色卡尔曼滤波过程中的测量方程选择超短基线数据有效时刻的切换观测方程。

所述有效时刻为超短基线数据有更新并且不是野值的时刻。

所述测量方程定义如下：

当超短基线定位装置提供的观测值有效时为：

y_k＝x_k+Nⁿ

x＝[u^x,v^x,w^x,r^x,ξ^x,η^x,ζ^x,ψ^x]^T为状态矢量，其中u^x、v^x、w^x和r^x分别是前向速度，侧向速度，垂向速度和转艏角速度的状态量，ξ^x、η^x、ζ^x和ψ^x分别是北向位置、东向位置、深度和艏向角的状态量；

y＝[u^y,v^y,w^y,r^y,ξ^y,η^y,ζ^y,ψ^y]^T为观测矢量，其中u^y、v^y、w^y、r^y分别是前向速度、侧向速度、垂向速度和转艏角速度的观测量，ξ^y、η^y、ζ^y、ψ^y是北向位置、东向位置、深度和艏向角的观测量；

Nⁿ为观测噪声；

当超短基线定位装置提供的观测值无效时为：