[发明专利]基于FMSRUPF算法的AUV对接回收自主导航方法

说明书

本发明公开了一种基于FMSRUPF算法的AUV对接回收自主导航方法，即针对AUV对接回收基于SINS/USBL/DVL的自主导航，提供了一种衰减记忆平方根无迹粒子滤波算法即FMSRUPF算法，并对其重采样部分采用系统组合粒子重采样方法。所采用的FMSRUPF算法利用衰减因子减小历史信息对滤波的影响，增强当前量测信息在滤波计算中的作用，然后将协方差矩阵的平方根阵代替协方差矩阵进行滤波解算，最后用系统组合粒子重采样方法对重采样过程进行改进。本发明通过FMSRUPF算法和系统组合粒子重采样方法解决了AUV对接回收系统模型建立粗糙或失真引起的滤波发散问题，并且结合了良好的数值特性和中等的计算负担，有效提升了AUV对接回收自主导航系统的定位精度和稳定性。

技术领域

本发明涉及一种基于FMSRUPF算法的AUV对接回收自主导航方法，属于机器人技术领域。

背景技术

AUV(Autonomous Underwater Vehicle无缆自主水下机器人)，这类水下机器人通过自身携带的能源设备提供动力，具有下潜深度深、活动范围广、隐蔽性高等特点，但是由于自身能源限制，AUV作业时间有限，这就需要及时回收机器人进行能源补给和维修。在实际回收过程中，复杂的水下环境和水下回收的高精度的控制要求又使得AUV回收困难重重，怎样精确节能的回收AUV更是成为当前水下机器人研究的重中之重。

在回收过程中，导航问题仍然是面临的主要技术挑战之一。导航系统必须为水下运载器提供远距离、长时间范围内的精确姿态、速度及位置信息。精确的导航能力是水下运载器有效应用和安全回收的一个关键技术，但由于水下机器人质量轻、惯性小，对噪声响应灵敏，加上有限的测量传感器，实现水下运载器的精确导航是一项复杂而艰巨的任务。水声定位系统在水下的特殊环境中具有独特的优势，水声信号在水中传播速度快且相对的能量衰减率小，水声定位系统可以提供三维的位置信息，定位精度较高，动态性能较好。

USBL(Ultra-Short Baseline超短基线定位系统)利用声学定位技术实现对水下目标的定位和跟踪。该导航系统定位精度较高，机动范围较大，可以适应深海复杂的作业环境。与其他基于声学定位的基线系统相比，USBL基线尺寸较小(一般在几厘米至十几厘米的量级)、灵活性高、操作简单、成本较低，因此在水下作业和海洋开发等工程领域得到了广泛应用，但是USBL由于易受海洋环境的影响，经常会出现数据跳动或数据丢失现象，连续性差，在某些需要高精度导航定位的场合受到较大的限制。

SINS(Strapdown Inertial Navigation Systems捷联惯性导航系统)利用陀螺仪和加速度计测量载体相对于惯性空间的角速度和加速度信息，通过解算可求出载体的三维速度、位置和姿态信息。SINS不需要任何外来信息，也不向外辐射信息，仅依靠系统自身就能在全球范围内实现全天候、全天时、自主、隐蔽、连续的定位和定向。该系统已被广泛地应用于航海、航空、航天、交通等各个领域。但是，随着系统工作时间的延长，SINS系统的导航误差会随之积累增长，此时就需要利用外部传感器的观测信息通过滤波算法来修正补偿SINS系统，以抑制其随时间积累的误差。

DVL(Doppler Velocity Log多普勒计程仪)是广泛应用于水下及水面航行器组合系统的速度测量装置，具有实时输出载体三维速度与航程能力的声学导航定位系统。DVL系统利用安装在载体上的超声换能器向海底发射超声波，并根据多普勒效应原理测量载体速度，具有抗干扰能力强、反应快、隐蔽性好、速度测量精确和稳定性高等优点。但是当DVL的波束超出了有效射程范围，测速精度会严重下降，且DVL的测速精度还受到水温、水压、地形和盐度等外界因素的影响。