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

摘要

本发明涉及一种基于无色卡尔曼滤波的深海机器人超短基线组合导航方法，利用全球定位系统获取深海机器人的初始绝对位置作为航迹推算的初始点，并采集初始信息；采用无色卡尔曼滤波方法对采集到的初始信息进行数据融合，得出融合后的信息。本方法可以提高使用超短基线定位系统的深海机器人的导航精度，同时能够平滑深海机器人控制系统所需的航向、深度以及载体坐标系下的速度信息。

主权项

一种基于无色卡尔曼滤波的深海机器人超短基线组合导航方法，其特征在于：利用全球定位系统获取深海机器人的初始绝对位置作为航迹推算的初始点，并采集深海机器人的初始信息；采用无色卡尔曼滤波方法对采集到的初始信息进行数据融合，得出融合后的信息；在无色卡尔曼滤波过程中，取深海机器人的动力学方程作为系统的状态方程；所述初始信息包括多普勒计程仪采集的线速度信息，运动传感器采集的姿态信息，超短基线声学定位装置采集的位置信息和深度计采集的深度信息；所述超短基线声学定位装置通过位移修正方法采集位置信息；所述位移修正方法为用当前时刻接收到的超短基线声学定位装置提供的位置信息加上当前时刻距上次水声通讯时刻时间段内深海机器人走过的位移作为当前时刻超短基线定位装置提供的位置观测值；所述状态方程建立如下：Mq··+C(q·)q·+D(q·)q·+G(q)=u]]>其中，M为6×6维的惯性矩阵，包含离心力和哥氏力的6×6维矩阵，为6×6维的水动力矩阵，G(q)包含重力和浮力的6×1维矩阵，q和u分别为6×1维的状态和电机转矩矢量。