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

权利要求书

1.一种用单台声信标修正水下机器人位置误差的方法，其特征在于，包括以下步骤：

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

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

当水下机器人和声信标的距离减小到设定值R_l时，水下机器人绕声信标做圆周运动，使声信标位置在水下机器人圆周运动轨迹所包围的范围以内；

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

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

2.根据权利要求1所述的一种用单台声信标修正水下机器人位置误差的方法，其特征在于，所述水下机器人绕声信标做运动的圈数不小于0.5圈，其半径不小于上述设定值R_l。

3.根据权利要求1所述的一种用单台声信标修正水下机器人位置误差的方法，其特征在于，所述计算水下机器人的转向角，具体为：

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)。

4.根据权利要求1所述的一种用单台声信标修正水下机器人位置误差的方法，其特征在于，所述改进的扩展卡尔曼滤波器的观测方程为：

z_k＝h_k(s_k)+v_k

其中，

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

(2)zk=rk-2-r~k-2rk-1-r~k-1rk-r~kψk-ψ~k′,]]>四个分量从左到右依次为上上次距离测量值与当时距离预测值之差、上一次距离测量值与当时距离预测值之差、当前时刻距离测量值与当前时刻距离预测值之差、当前时刻航向角测量值与当前时刻航向角预测值之差；

(3)w_k是一个三维向量，为过程驱动噪声，w_k(1)和w_k(2)为正值且不大于10，w_k(3)为正值且不大于0.0873；v_k是一个四维向量，为观测噪声，v_k(1)、v_k(2)和v_k(3)为正值且不大于50，v_k(4)为正值且不大于0.0873；

(4)hk(sk)=(xk-Δxk-2,k-xb)2+(yk-Δyk-2,k-yb)2-k~k-2(xk-Δxk-1,k-xb)2+(yk-Δyk-1,k-yb)2-r~k-1(xk-xb)2+(yk-yb)2-r~kψk-ψ~k]]>

对观测方程的h_k(s_k)进行线性化可得

Hk=-xdk-2-ydk-20-xdk-1-ydk-10-xdk-ydk0001]]>

其中

xdk-2=xb-(x~k-Δxk-2,k)]]>

ydk-2=yb-(y~k-Δyk-2,k)]]>

xdk-1=xb-(x~k-Δxk-1,k)]]>

ydk-1=yb-(y~k-Δyk-1,k)]]>

xdk=xb-x~k]]>

ydk=yb-y~k]]>

上式中的和表示k时刻东向坐标和北向坐标的预测值，Δx_k-i，k，Δy_k-i，k(i＝1，2)分别表示k-i到k时刻之间的东向和北向坐标增量；

单信标的坐标为(x_b，y_b)，水下机器人航速为v，采样周期为T，采样周期内的角增量为Δψ。