[发明专利]一种基于改进的S-H自适应联邦滤波的智能水下机器人多源组合导航方法

说明书

本发明公开了一种基于改进的S‑H自适应联邦滤波的智能水下机器人多源组合导航方法，属于智能水下机器人领域。所述改进的S‑H自适应联邦滤波方法包括以下步骤：S100、进行水下多源组合导航系统建模，得到导航传感器及其误差模型；S200、以误差模型为基础，提出改进S‑H自适应联邦滤波方法。本发明的一种基于改进的S‑H自适应联邦滤波的智能水下机器人多源组合导航方法，可以对多传感器的误差进行修正，且基于多源的特点，可以对各传感器的误差进行修正，且选择联邦滤波器对多源组合导航系统进行数据融合，具有计算量较小，结构简单，容错性和实时性好等优点。

技术领域

本发明涉及一种基于改进的S-H自适应联邦滤波的智能水下机器人多源组合导航方法，属于智能水下机器人领域。

背景技术

高精度导航系统是智能水下机器人AUV设计过程中的重点和难点，也是其能够顺利完成任务并成功返回的保障。目前，捷联惯性导航系统SINS由于其在短时间内导航精度高，更新信息速度快，且不需外部信息输入，已成为水下常用的导航传感器，但是其误差会随着时间的增加而不断累积，无法单独完成导航任务，需要与其他导航传感器，如超短基线定位系统(Ultra Short Baseline Positioning System，USBL)、多普勒测速仪(DopplerVelocity Log，DVL)、磁航向仪(Magnetic Compass，MCP)进行组合使用来抑制随时间积累产生的误差。

随着科技的发展，水下导航传感器的种类和功能也越加多样化，如何有效的对多传感器组成的组合导航系统进行信息融合得到最优导航信息也就成为了研究的重点。

高精度导航系统是AUV设计过程中的重点和难点，也是其能够顺利完成任务并成功返回的保障。目前，SINS由于其在短时间内导航精度高，更新信息速度快，且不需外部信息输入，已成为水下常用的导航传感器，但是其误差会随着时间的增加而不断累积，无法单独完成导航任务，需要与其他导航传感器，如USBL、DVL、MCP进行组合使用来抑制随时间积累产生的误差。

同时，高精度导航系统的关键也在于如何设计合适的导航滤波器，针对水下环境复杂多变导致量测噪声统计特性未知或时变的特点，经验证可知S-H自适应滤波方法效果最好，应用在多源组合导航数据融合中，由于系统的维数过大等特点，容易导致其出现发散，所以需要对其进行分析改进，使其适合应用于联邦滤波结构。

发明内容

本发明提出了一种基于改进的S-H自适应联邦滤波的智能水下机器人多源组合导航方法，已解决现有技术中存在的问题。

一种基于改进的S-H自适应联邦滤波的智能水下机器人多源组合导航方法，所述改进的S-H自适应联邦滤波方法包括以下步骤：

S100、进行水下多源组合导航系统建模，得到导航传感器及其误差模型；

S200、以误差模型为基础，提出改进S-H自适应联邦滤波方法。

进一步的，在S100中，具体包括以下步骤：

S110、首先建立导航系统常用坐标系和坐标转换关系；

S120、在S110的基础上获得SINS、USBL、DVL和MCP四种传感器的误差模型。

进一步的，在S110中，具体包括以下步骤：

S111、假定SINS、USBL、DVL和MCP四种传感器均安装于水下机器人载体中心，则所述四种传感器所获得的数据都是基于载体坐标系下的，需要将其统一到导航坐标系下；

S112、导航坐标系围绕OZ_n轴旋转ψ，围绕OX_n1轴旋转θ，然后绕OY_n2旋转γ，即：