[发明专利]一种用单台声信标修正水下机器人位置误差的方法

说明书

技术领域

本发明涉及水下机器人技术领域，尤其涉及一种基于固定声信标的水下机器人导航方法，该方法利用水下机器人到一只位置固定且已知的声信标的距离测量值对水下机器人位置进行修正。

背景技术

水下机器人在水下航行过程中，导航误差会随着路程和时间的增加而逐渐增大，当航行一段距离后，需要对水下机器人的位置进行修正，以提高导航精度。当前水下机器人进行位置修正的方法主要有两种：卫星定位修正和长基线声学定位修正，但这些方法都存在一定的缺陷。对于卫星定位修正方法，水下机器人需要定时浮出水面接收无线电信号，位置修正过程在水面完成，不利于水下机器人水下航行，且风险大。而对于长基线声学定位修正方法，需要给水下机器人配备复杂且昂贵的声学定位系统，操作过程繁琐，应用成本很高。近期相关文献介绍的水下机器人单信标导航方法，就是预先给水下机器人设定一个误差较小的初始位置，水下机器人以一定的航速航行，不断测量到信标的距离，来实时修正水下机器人的位置误差。但是实际情况会有这种情况发生，就是在设定水下机器人初始位置时，不能保证位置误差足够小，当误差过大时，这种方法对位置误差的修正结果会出错，导致位置误差修正失败。

发明内容

为了克服现有方法的不足，本发明要解决的技术问题是提供一种水下机器人位置误差自主修正方法。该方法不依靠卫星定位和长基线声学定位系统，仅依靠固定在海底的单台声信标即可自主修正实现水下机器人的位置误差，并且该方法不依赖于初始位置误差的大小。

本发明为实现上述目的所采用的技术方案是：一种用单台声信标修正水下机器人位置误差的方法，包括以下步骤：

水下机器人的测距器按周期采集运动中的水下机器人和声信标的距离；

水下机器人内的计算机根据每两个时刻采集到的上述距离及该时间段内水下机器人的航行距离，计算水下机器人的转向角，控制水下机器人向声信标航行；

当水下机器人和声信标的距离减小到设定值R_l时，水下机器人绕声信标做圆周运动；

根据水下机器人的航向、航速以及采集到的水下机器人和声信标的距离，利用改进的扩展卡尔曼滤波器算法对水下机器人的位置进行在线估计；

水下机器人圆周运动结束时，停止滤波，得到水下机器人的估计位置，用此估计位置更新水下机器人当前位置。

所述水下机器人绕声信标做运动的圈数不小于0.5圈，其半径不小于上述设定值R_l，声信标位置在水下机器人圆周运动轨迹所包围的范围以内。

所述计算水下机器人的转向角，具体为：

i)第一个测距周期结束时，计算转向角

其中，r_i-1为上一时刻测量到的水下机器人和声信标的距离，r_i为当前时刻测量到的水下机器人和声信标的距离，d水下机器人在上一时刻和当前时刻期间前进的距离，由航速乘以时间得到；使水下机器人艏向转动GX＝-g度；同时，得到第一个测距周期的距离差dev＝r_i-1-r_i，转到下一个测距周期；

ii)当第二个测距周期的dev小于第一个测距周期的dev时，GX＝g，反之GX＝-g，转到下一个测距周期；

iii)当前测距周期结束时，利用余弦公式计算上一周期测量到的距离与d之间的夹角为如果该夹角大于90°，则GX＝g，返回到i)，反之，GX＝0；如果上一周期测量到的距离乘以该夹角的正弦，即所得的结果大于设定的接近半径R_l，则返回到i)。

所述改进的扩展卡尔曼滤波器的观测方程为：

z_k＝h_k(s_k)+v_k

其中，

(1)s_k＝[x_k y_k ψ_k]′，x_k，y_k和ψ_k分别表示当前时刻k的东向坐标、北向坐标和航向角；