[发明专利]自主式水下机器人组合导航系统及方法

说明书

技术领域

本发明涉及一种导航技术，特别是一种自主式水下机器人组合导航系统及方法，属于海洋工程领域。

背景技术

伴随着现代科学技术的发展，自主式水下机器人（Autonomous Underwater Vehicle，AUV）无论在军用还是民用方面都有广泛的应用，例如战区侦查、探测扫除水雷、海上预警、水下搜索和测绘、近海石油工程作业等。就当前的发展而言，导航技术是实现AUV自主航行的关键，导航系统必须提供远距离及长时间范围内的精确定位、速度及姿态信息。但由于受其大小、重量、电源使用的限制及水介质的特殊性、隐蔽性等因素的影响，实现AUV的精确导航是一项艰难的任务。

就AUV而言，可供应用的主要有两类导航方式：基于外部信号的非自主导航和基于传感器的自主导航。非自主导航方式，如罗兰、欧米加、GPS等，仅在接收机能接收到信号时能够完成导航，且由于电波在水中快速衰减的原因，这些基于无线电的导航方式，在AUV上的使用受到很大限制。基于传感器的自主导航方式靠AUV自身携带的装备如惯性测量装置（IMU）、声换能器、地形匹配或地磁传感等手段完成导航。

目前，国际上水下导航已由单一导航方式向高精度、高可靠、综合化、智能化的组合导航系统发展；信息处理方法也由单一数据源的处理，向多导航传感器多数据源的信息融合发展，总的来说，组合导航代表了未来水下导航的发展方向，克服了传统导航的缺陷和不足，使水下导航领域呈现出崭新的面貌，具有无比广阔的发展前景。

现有的水下机器人通常利用惯性导航系统（Inertial Navigation System，INS）采集的姿态及加速度信息或磁罗经（Magnctic Compass，MC）采集姿态信息，以及多普勒计程仪（Doppler Velocity Log，DVL）采集的速度信息进行组合导航。但是由于上述姿态和速度数据存在一定误差，且这些误差会随时间积累到不可接受的程度。而研究结果表明基于定时接收GPS修正的组合导航系统可提供比常规惯性导航技术高一到两个数量级的测量精度，并且能够把横滚角误差和俯仰角误差抑制到一定的精度，但是很难抑制组合导航系统航向角误差的积累。磁航向仪（MCP）可以输出地磁场矢量在载体坐标系的投影，且精度不发散，故利用磁航向仪的信息与捷联惯性导航系统（SINS）、GPS、多普勒计程仪进行组合，可以抑制航向角误差的发散，可显著提高AUV整体导航的精度。

发明内容

本发明的目的在于提供一种自主式水下机器人组合导航系统及方法，针对现有技术存在的缺陷，将磁航向仪的信息与捷联惯性导航系统（SINS）、GPS、多普勒计程仪进行组合，抑制航向角误差的发散，显著提高AUV导航系统整体的精度。

本发明的目的通过以下技术方案予以实现：

一种自主式水下机器人组合导航系统，由捷联惯性导航系统1、全球定位导航系统2、多普勒测速仪3、磁航向仪4组成，所述捷联惯性导航系统1利用陀螺仪和加速度计计算出相对地球沿地理坐标系的速度信息、位置信息和姿态信息；所述全球定位导航系统2获取自主式水下机器人的初始绝对位置信息和速度信息；所述多普勒测速仪3计算出速度信息；所述磁航向仪4计算出航向信息；所述全球定位导航系统2对捷联惯性导航系统1定时重调，系统对捷联惯导系统1输出的速度信息、位置信息和姿态信息，多普勒测速仪3所计算的速度信息，磁航向仪4所计算的航向信息进行数据融合，得到组合导航信息。

一种自主式水下机器人组合导航系统的导航数据融合方法，步骤如下：

1）对于水下组合导航系统，线性化后离线系统状态方程和观测方程如下：

X(k)=Φ(k/k-1)X(k-1)+Γ(k-1)W(k-1)

Z(k)=H(k)X(k)+V(k)

式中：X(k)为系统状态向量；Z(k)为量测向量；Φ(k/k-1)为一步转移阵；Γ(k-1)为系统噪声驱动阵；W(k)为系统激励噪声序列；H(k)为量测阵；V(k)为量测噪声序列，W(k)、V(k)是不相关的高斯白噪声序列，其均值、方差分别为E[ω(k)]=0，E[ω(k)ω^T(j)]=Q(k)δ_kj；E[v(k)]=0，E[v(k)v^T(j)]=R(k)δ_kj;cov[ω(k)v(j)]=0

2）模糊自适应卡尔曼滤波算法编排：