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

Abstract

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

Description

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

技术领域

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

背景技术

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

发明内容

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

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

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

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

当水下机器人和声信标的距离减小到设定值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的东向坐标、北向坐标和航向角；

(2) $z_k={\left[\begin{array}{cccc}{r}_{k-2}-{\tilde{r}}_{k-2}& r_{k-1}-{\tilde{r}}_{k-1}& r_k-{\tilde{r}}_k& {\mathrm{\ψ}}_k-{\widetilde{\mathrm{\ψ}}}_k\end{array}\right]}^{\′},$ 四个分量从左到右依次为上上次距离测量值与当时距离预测值之差、上一次距离测量值与当时距离预测值之差、当前时刻距离测量值与当前时刻距离预测值之差、当前时刻航向角测量值与当前时刻航向角预测值之差；

(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) $h_k\left(s_k\right)=\left[\begin{array}{c}\sqrt{{(x_k-{\mathrm{\Δx}}_{k-2,k}-x_b)}^2+{(y_k-{\mathrm{\Δy}}_{k-2,k}-y_b)}^2}-{\tilde{k}}_{k-2}\\ \sqrt{{(x_k-{\mathrm{\Δx}}_{k-1,k}-x_b)}^2+{(y_k-{\mathrm{\Δy}}_{k-1,k}-y_b)}^2}-{\tilde{r}}_{k-1}\\ \sqrt{{(x_k-x_b)}^2+{(y_k-y_b)}^2}-{\tilde{r}}_k\\ {\mathrm{\ψ}}_k-{\widetilde{\mathrm{\ψ}}}_k\end{array}\right]$

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

$H_k=\left[\begin{array}{ccc}{-\mathrm{xd}}_{k-2}& -{\mathrm{yd}}_{k-2}& 0\\ {-\mathrm{xd}}_{k-1}& -{\mathrm{yd}}_{k-1}& 0\\ -{\mathrm{xd}}_k& -{\mathrm{yd}}_k& 0\\ 0& 0& 1\end{array}\right]$

其中

${\mathrm{xd}}_{k-2}=x_b-({\tilde{x}}_k-{\mathrm{\Δx}}_{k-2,k})$

${\mathrm{yd}}_{k-2}=y_b-({\tilde{y}}_k-{\mathrm{\Δy}}_{k-2,k})$

${\mathrm{xd}}_{k-1}=x_b-({\tilde{x}}_k-{\mathrm{\Δx}}_{k-1,k})$

${\mathrm{yd}}_{k-1}=y_b-({\tilde{y}}_k-{\mathrm{\Δy}}_{k-1,k})$

${\mathrm{xd}}_k=x_b-{\tilde{x}}_k$

${\mathrm{yd}}_k=y_b-{\tilde{y}}_k$

上式中的

和

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

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

本发明具有以下有益效果及优点

1.装置简单，继承性好。本发明装置仅需要一台声信标和一台测距器，无需其它辅助装置，测距器安装简便，修正算法程序移植性好，可方便移植到各个水下机器人。

2.稳定可靠，修正结果准确。本发明设计了转向角计算程序，使水下机器人逐渐接近声信标，避免航行到声信标作用距离以外的区域；改进扩展卡尔曼滤波器，充分利用航向、航速和距离信息，位置估计稳定收敛，结果准确。

3.安装简单，使用寿命长。本发明装置采用的声信标和测距器，采用现有产品，使用寿命长，而且可以根据需要将声信标固定在海底任何位置，测距器安装于水下机器人背部，对安装精度无要求。

4.应用范围广。本发明不但可以应用于水下机器人，还可以用于其它海洋相关设备，可适用于全海深位置修正。

附图说明

图1是本发明的组成示意图；

图2是本发明的水下机器人转向角控制算法流程图；

图3是本发明的转向角计算示意图；

图4是本发明的水下机器人进行位置估计时的航行轨迹示意图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

一种用单台声信标修正水下机器人位置误差的装置组成，如图1所示，由一台声信标和一台安装在水下机器人上的测距器组成。

本发明方法流程图如图2所示，水下机器人的测距器按周期采集运动中的水下机器人和声信标的距离；水下机器人内的计算机根据每两个时刻采集到的上述距离及该时间段内水下机器人的航行距离，计算水下机器人的转向角，控制水下机器人向声信标航行；当水下机器人和声信标的距离减小到设定值R_l时，水下机器人转向绕声信标做运动；根据水下机器人的航向、航速以及采集到的水下机器人和声信标的距离，利用改进的扩展卡尔曼滤波器算法对水下机器人的位置进行在线估计；水下机器人圆周运动结束时，停止滤波，得到水下机器人的估计位置，更新水下机器人当前位置。具体实现如下：

水下机器人上的测距器按一定的测距周期进行测距。设在上一时刻测量声信标的距离为r_i-1、当前时刻到声信标的距离为r_i，前者减去后者得到的距离差为dev＝r_i-1-r_i。而由航速乘以时间可以直接得到水下机器人在上一时刻和当前时刻期间前进的距离，设为d。转向角g的计算示意图如图3所示，

按照转向角控制算法控制水下机器人的航向角，使得水下机器人艏向所转动的角度和方向与转向角一致。设定水下机器人接近距离为R_l(R_l＞0)，当水下机器人距声信标的距离小于或等于R_l时，水下机器人艏向左转90度(或右转90度)，然后以不小于R_l的半径做圆周运动(其圆心不必是声信标)，使声信标位置在水下机器人圆周运动轨迹所包围的范围以内，如图4所示；同时启动扩展卡尔曼滤波器(本发明的扩展卡尔曼滤波器算法，其中将该滤波器的状态变量设为东向坐标、北向坐标和航向角构成的三维向量，观测变量设定为由上上次的距离测量值、上一次距离测量值、当前时刻距离测量值和当前时刻航向角构成的四维向量)，对水下机器人的位置进行实时估计。当运动圈数大于或等于0.5圈时，滤波器停止，当前估计结果就是水下机器人的准确位置。至此，用单台声信标修正水下机器人位置误差的实现过程已完成。

本发明的转向角控制算法：

i)：第一个测距周期结束时，利用公式(1)计算得到转向角g，使水下机器人艏向转动GX＝-g度；同时，得到第一个测距周期的距离差dev，转到下一个测距周期；

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

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

扩展卡尔曼滤波器算法：

设声信标的坐标为(x_b，y_b)，水下机器人航速为v，采样周期为T，采样周期内的角增量为Δψ，则离散***状态方程如下

s_k＝As_k-1+w_k

            (2)

z_k＝h_k(s_k)+v_k

其中

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

(2) $z_k={\left[\begin{array}{cccc}{r}_{k-2}-{\tilde{r}}_{k-2}& r_{k-1}-{\tilde{r}}_{k-1}& r_k-{\tilde{r}}_k& {\mathrm{\ψ}}_k-{\widetilde{\mathrm{\ψ}}}_k\end{array}\right]}^{\′},$ 四个分量从左到右依次为上上次距离测量值与当时距离预测值之差、上一次距离测量值与当时距离预测值之差、当前时刻距离测量值与当前时刻距离预测值之差、当前时刻航向角测量值与当前时刻航向角预测值之差；

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

$h_k\left(s_k\right)=\left[\begin{array}{c}\sqrt{{(x_k-{\mathrm{\Δx}}_{k-2,k}-x_b)}^2+{(y_k-{\mathrm{\Δy}}_{k-2,k}-y_b)}^2}-{\tilde{k}}_{k-2}\\ \sqrt{{(x_k-{\mathrm{\Δx}}_{k-1,k}-x_b)}^2+{(y_k-{\mathrm{\Δy}}_{k-1,k}-y_b)}^2}-{\tilde{r}}_{k-1}\\ \sqrt{{(x_k-x_b)}^2+{(y_k-y_b)}^2}-{\tilde{r}}_k\\ {\mathrm{\ψ}}_k-{\widetilde{\mathrm{\ψ}}}_k\end{array}\right]$

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

$H_k=\left[\begin{array}{ccc}{-\mathrm{xd}}_{k-2}& -{\mathrm{yd}}_{k-2}& 0\\ {-\mathrm{xd}}_{k-1}& -{\mathrm{yd}}_{k-1}& 0\\ -{\mathrm{xd}}_k& -{\mathrm{yd}}_k& 0\\ 0& 0& 1\end{array}\right]$

其中

${\mathrm{xd}}_{k-2}=x_b-({\tilde{x}}_k-{\mathrm{\Δx}}_{k-2,k})$

${\mathrm{yd}}_{k-2}=y_b-({\tilde{y}}_k-{\mathrm{\Δy}}_{k-2,k})$

${\mathrm{xd}}_{k-1}=x_b-({\tilde{x}}_k-{\mathrm{\Δx}}_{k-1,k})$

${\mathrm{yd}}_{k-1}=y_b-({\tilde{y}}_k-{\mathrm{\Δy}}_{k-1,k})$

${\mathrm{xd}}_k=x_b-{\tilde{x}}_k$

${\mathrm{yd}}_k=y_b-{\tilde{y}}_k$

上式中的

和

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

扩展卡尔曼滤波器算法为

1)初始化

s_0|0＝[0 0 0]

$P_{0|0}=\left[\begin{array}{ccc}{10}^{-6}& 0& 0\\ 0& {10}^{-6}& 0\\ 0& 0& {10}^{-6}\end{array}\right]$

2)预测

${\tilde{s}}_{k|k-1}={\mathrm{As}}_k$

P_k|k-1＝AP_k|k-1A′+BQ_kB′

3)修正

K_k＝P_k|k-1H′_k(H_kP_k|k-1H′_k|k-1+R_k)^-1

$s_{k|k}={\tilde{s}}_{k|k-1}+K_k(z_k-{\tilde{r}}_k)$

${\tilde{r}}_k=H{\tilde{s}}_{k|k-1}$

P_k|k＝(I-K_kH_k)P_k|k-1

其中 $H_k=\frac{{\∂h}_k}{\∂{\tilde{s}}_{k|k-1}},$

Q_k＝diag[(w_k(1))² (w_k(2))² (w_k(3))²]，

R_k＝diag[(v_k(1))² (v_k(2))² (v_k(3))² (v_k(4))²]。

Claims (4)

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) $z_k={\left[\begin{array}{cccc}{r}_{k-2}-{\tilde{r}}_{k-2}& r_{k-1}-{\tilde{r}}_{k-1}& r_k-{\tilde{r}}_k& {\mathrm{\ψ}}_k-{\widetilde{\mathrm{\ψ}}}_k\end{array}\right]}^{\′},$ 四个分量从左到右依次为上上次距离测量值与当时距离预测值之差、上一次距离测量值与当时距离预测值之差、当前时刻距离测量值与当前时刻距离预测值之差、当前时刻航向角测量值与当前时刻航向角预测值之差；

(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) $h_k\left(s_k\right)=\left[\begin{array}{c}\sqrt{{(x_k-{\mathrm{\Δx}}_{k-2,k}-x_b)}^2+{(y_k-{\mathrm{\Δy}}_{k-2,k}-y_b)}^2}-{\tilde{k}}_{k-2}\\ \sqrt{{(x_k-{\mathrm{\Δx}}_{k-1,k}-x_b)}^2+{(y_k-{\mathrm{\Δy}}_{k-1,k}-y_b)}^2}-{\tilde{r}}_{k-1}\\ \sqrt{{(x_k-x_b)}^2+{(y_k-y_b)}^2}-{\tilde{r}}_k\\ {\mathrm{\ψ}}_k-{\widetilde{\mathrm{\ψ}}}_k\end{array}\right]$

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

$H_k=\left[\begin{array}{ccc}{-\mathrm{xd}}_{k-2}& -{\mathrm{yd}}_{k-2}& 0\\ {-\mathrm{xd}}_{k-1}& -{\mathrm{yd}}_{k-1}& 0\\ -{\mathrm{xd}}_k& -{\mathrm{yd}}_k& 0\\ 0& 0& 1\end{array}\right]$

其中

${\mathrm{xd}}_{k-2}=x_b-({\tilde{x}}_k-{\mathrm{\Δx}}_{k-2,k})$

${\mathrm{yd}}_{k-2}=y_b-({\tilde{y}}_k-{\mathrm{\Δy}}_{k-2,k})$

${\mathrm{xd}}_{k-1}=x_b-({\tilde{x}}_k-{\mathrm{\Δx}}_{k-1,k})$

${\mathrm{yd}}_{k-1}=y_b-({\tilde{y}}_k-{\mathrm{\Δy}}_{k-1,k})$

${\mathrm{xd}}_k=x_b-{\tilde{x}}_k$

${\mathrm{yd}}_k=y_b-{\tilde{y}}_k$

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

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

Images