一种用单台声信标修正水下机器人位置误差的方法

摘要

本发明涉及水下机器人技术领域。水下机器人内的计算机根据每两个时刻采集到的水下机器人和声信标的距离及该时间段内水下机器人的航行距离，计算水下机器人的转向角，控制水下机器人向声信标航行；当水下机器人和声信标的距离减小到设定值时，水下机器人绕声信标做圆周运动；根据水下机器人的航向、航速以及采集到的水下机器人和声信标的距离，利用改进的扩展卡尔曼滤波器算法对水下机器人的位置进行在线估计。本发明装置仅需要一台声信标和一台测距器，无需其它辅助装置，测距器安装简便，修正算法程序移植性好，可方便移植到各个水下机器人。且稳定可靠，修正结果准确，安装简单，使用寿命长，应用范围广。

主权项

一种用单台声信标修正水下机器人位置误差的方法，其特征在于，包括以下步骤：水下机器人的测距器按周期采集运动中的水下机器人和声信标的距离；水下机器人内的计算机根据每两个时刻采集到的上述距离及该时间段内水下机器人的航行距离，计算水下机器人的转向角，控制水下机器人向声信标航行；当水下机器人和声信标的距离减小到设定值R_l时，水下机器人绕声信标做圆周运动，使声信标位置在水下机器人圆周运动轨迹所包围的范围以内；根据水下机器人的航向、航速以及采集到的水下机器人和声信标的距离，利用改进的扩展卡尔曼滤波器算法对水下机器人的位置进行在线估计；水下机器人圆周运动结束时，停止滤波，得到水下机器人的估计位置，用此估计位置更新水下机器人当前位置。