CN104280024A - 一种深水机器人组合导航装置和方法 - Google Patents

Abstract

本发明涉及深水机器人技术领域，尤其涉及一种用于深水水机器人组合导航的装置和方法。装置包括测距器、信标、传感器和导航计算机。方法为：深水机器人下水后，导航计算机对测距器、航向传感器、航速传感器和深度传感器进行数据采集；当导航计算机采集到测距器的距离数据时，计算深水机器人的初始位置；判断初始位置是否有效；如果初始位置有效，则判断距离数据是否有效，如果距离数据无效，则舍弃该距离数据，返回计算深水机器人的初始位置步骤；如果距离数据有效，则根据距离数据对深水机器人的位置进行在线修正。本发明装置简单，继承性好，稳定可靠，修正结果准确，安装简单，使用寿命长，应用范围广。

Description

一种深水机器人组合导航装置和方法

技术领域

本发明涉及深水机器人技术领域，尤其涉及一种用于深水水机器人组合导航的装置和方法。

背景技术

深水机器人航行深度可达数千米。在水下航行过程中，导航误差会随着路程和时间的增加而逐渐增大。当深水机器人航行一段距离后，需要对其位置进行修正，以提高导航精度。

当前对深水机器人进行位置修正的方法主要是：通过声学定位系统输出的定位信息对深水机器人位置进行直接修正。但这种方法都存在一定的缺陷：

1）无法消除声学传播时延引起的定位误差；

2）深水机器人航行轨迹不平滑，不利于导航控制。

发明内容

为了克服现有方法的不足，本发明要解决的技术问题是提供一种深水机器人组合导航装置与方法，使得航行轨迹平滑且定位误差小。

本发明为实现上述目的所采用的技术方案是：一种深水机器人组合导航装置和方法，包括

测距器，安装于深水机器人壳体外部，连接导航计算机，用于通过声信号测量深水机器人到各个信标的距离；

信标，至少三个，布设于海底；

传感器，包括航向传感器、航速传感器和深度传感器，均安装于深水机器人中并连接导航计算机，分别用于测量深水机器人的的航向、航速和所处深度；

导航计算机，用于采集测距器和传感器的信号并实时计算深水机器人当前时刻的位置。

所述测距器按固定周期进行测距，周期在3秒～60秒之间。

所述导航计算机按固定周期采集数据，周期在0.2秒～2秒之间。

一种深水机器人组合导航方法，包括以下步骤：

深水机器人下水后，导航计算机对测距器、航向传感器、航速传感器和深度传感器进行数据采集；

当导航计算机采集到测距器的距离数据时，计算深水机器人的初始位置；

判断初始位置是否有效；

如果初始位置有效，则判断距离数据是否有效，如果距离数据无效，则舍弃该距离数据，返回计算深水机器人的初始位置步骤；

如果距离数据有效，则根据距离数据对深水机器人的位置进行在线修正。

所述计算深水机器人的初始位置包括以下步骤：

当测距周期内的测距数据数量大于3时，将初始值N置0，对深水机器人水平面坐标进行解算，解算公式如下：

Ax＝v          （1）

式中，

$A=\left[\begin{array}{cc}(x_2-x_1)& (y_2-y_1)\\ (x_3-x_1)& (y_3-y_1)\end{array}\right],x=\left[\begin{array}{c}x\\ y\end{array}\right],v=\left[\begin{array}{c}\frac{1}{2}[({r_1}^2-r_2^2)-(d_1^2-d_2^2)]-(z_2-z_1)\·z\\ \frac{1}{2}[({r_1}^2-r_3^2)-(d_1^2-d_3^2)]-(z_3-z_1)\·z\end{array}\right]$

(x_i,y_i,z_i)(i=1,2,3)为第i号信标的三维坐标，(x,y,z)为深水机器人水平面坐标和深度，为第i号信标到坐标系原点的水平间距，为第i号信标到深水机器人的水平距离，其中R_i表示第i号信标的单程传播距离；

根据解算的深水机器人水平面坐标推算航位：

$x_t=x_0+\underset{k=1}{\mathrm{\Σ}}{v}_e\·\mathrm{\Δt}$

$y_t=y_0+\underset{k=1}{\mathrm{\Σ}}{v}_n\·\mathrm{\Δt}$

式中，(x₀,y₀)为初始点，Δt为采集周期，(x_t,y_t)为推算航位，v_e=vsinθ，v_n=vcosθ，θ为当前航向，v为当前航速。

所述判断初始位置是否有效具体为：

在下一个测距周期到来时，重新解算公式（1）中的深水机器人水平面坐标，将其与所述推算航位求差后求取均方根误差，即：

$e=\sqrt{{(x-x_t)}^2+{(y-y_t)}^t}$

如果e<=Bias

则N=N+1，返回2），直到N>=Window时，判定当前测距周期内解算的初始位置有效。

否则N=0，返回1）。

上式中，Bias范围为1米～100米，Window范围为2～30。

所述判断距离数据是否有效的方法为：判断公式

$[z_i-\hat{z}(k/k-1)]S{\left(k\right)}^{-1}[z_i-\hat{z}(k/k-1)]\&le;\mathrm{\&gamma;}---\left(3\right)$

S(k)=H_kP_k|k-1H'_k|k-1+R_k

是否成立，如果成立，则使用当前测量值z_i；否则，舍弃；

式中，γ范围为6～500，H_k为观测矩阵，P_k|k-1为预测协方差矩阵，H'_k|k-1为预测观测矩阵，R_k为观测噪声。

所述根据距离数据对深水机器人的位置进行在线修正包括以下步骤：

设任意信标的坐标为(x_i,y_i)(i=1,2,...,)，则离散系统状态方程为

s_k=As_k-1+w_k          （2）

z_k=h_k(s_k)+ν_k

其中，

s_k＝[x_k y_k]^T，x_k，y_k分别表示当前时刻k的东向坐标、北向坐标；

z_k=c·τ_i，表示双程声传播距离，其中c表示声速，τ_i表示第i个信标的双程声传播时间， $h_k\left(s_k\right)=\sqrt{{(x_k-v_e{\mathrm{\&tau;}}_i-x_i)}^2+{(y_k-v_n{\mathrm{\&tau;}}_i-y_i)}^2+{(z_k-z_i)}^2}+\sqrt{{(x_k-x_i)}^2+{(y_k-y_i)}^2+{(z_k-z_i)}^2};$ v_e为深水机器人东向速度，x_i为第i个信标的东向坐标，v_n为深水机器人北向速度，y_i为第i个信标的北向坐标，z_i为第i个信标的深度,z_k为深水机器人当前时刻k的深度；

w_k是一个二维向量，为过程驱动噪声，w_k(1)和w_k(2)为正值且不大于10；

ν_k是一个一维向量，为观测噪声，ν_k为正值且不大于50；

1）初始化

$s_{0|0}={\left[\begin{array}{cc}\stackrel{\&OverBar;}{x}& \stackrel{\&OverBar;}{y}\end{array}\right]}^T$

$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{{\&PartialD;h}_k}{\&PartialD;{\tilde{s}}_{k|k-1}},$

Q_k=diag([w_k(1))² (w_k(2))²])，

$R_k=\mathrm{diag}\left(\right[v_k^2\left]\right).$

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

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

2.稳定可靠，修正结果准确。本发明设计了初始位置窗口算法和测距波门算法，保证了初始值和测量值的可靠、有效，位置估计能够稳定收敛，结果准确。

3.安装简单，使用寿命长。本发明装置采用的信标和测距器，采用现有产品，使用寿命长，测距器安装于深水机器人背部，对安装精度无要求。

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

附图说明

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

图2是本发明的深水机器人导航传感器配置图；

图3是本发明的组合导航方法流程图。

具体实施方式

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

本发明由不少于3台的信标、测距器和深水机器人组成，如图1所示。深水机器人导航传感器配置如图2所示，由测距器、航向传感器、航速传感器、深度传感器组成。

组合导航算法如图3所示，深水机器人下水后进行数据采集，状态置为’A’,当收到测距数据时，采用本发明的初始位置窗口算法计算初始位置。获取有效的初始位置后，状态置为’B’，启动本发明设计的滤波算法，利用距离信息对深水机器人的位置进行在线修正。修正前，采用本发明的测距波门算法判断测距数据是否为野值，如果不是野值，就使用当前测距数据进行修正，否则，舍弃不用。

采用本发明设计的初始位置窗口算法对初始位置有效性进行判断，直到初始位置有效，则启动本发明设计的组合导航算法。

采用本发明设计的测距波门算法对测距数据是否为野值进行判断。如果判为野值，则舍弃该测距数据；否则，使用该测距数据。

所述初始位置窗口算法为：

1）当测距周期内的测距数据数量大于3个时，将初始值N置0，对深水器位置进行解算。解算算法如下：

Ax＝v          （1）

式中 $A=\left[\begin{array}{cc}(x_2-x_1)& (y_2-y_1)\\ (x_3-x_1)& (y_3-y_1)\end{array}\right],x=\left[\begin{array}{c}x\\ y\end{array}\right],v=\left[\begin{array}{c}\frac{1}{2}[({r_1}^2-r_2^2)-(d_1^2-d_2^2)]-(z_2-z_1)\&CenterDot;z\\ \frac{1}{2}[({r_1}^2-r_3^2)-(d_1^2-d_3^2)]-(z_3-z_1)\&CenterDot;z\end{array}\right]$

(x_i,y_i,z_i)(i=1,2,3)指第i号信标的三维坐标，已知。

(x,y,z)指深水机器人水平面坐标和深度，深度已知。

是第i号海底信标到坐标系原点的水平间距

表示各信标到深水机器人的水平距离，其中R_i表示第i信标的单程传播距离。

2）以式（1）解算的位置作为深水机器人初始点进行航位推算，设θ为当前航向，v为当前航速，则航位推算算法为：

$x_t=x_0+\underset{k=1}{\mathrm{\&Sigma;}}{v}_e\&CenterDot;\mathrm{\&Delta;t}$

$y_t=y_0+\underset{k=1}{\mathrm{\&Sigma;}}{v}_n\&CenterDot;\mathrm{\&Delta;t}$

式中(x₀,y₀)为初始点，(x_t,y_t)为推算航位，Δt为采集周期，v_e=vsinθ，v_n=vcosθ。

3）在下一个测距周期到来时，返回1）得到（1）解算的位置(x,y)，将其与2）得到的推算位置求差后求取均方根误差，如下：

$e=\sqrt{{(x-x_t)}^2+{(y-y_t)}^t}$

如果e<=Bias

则N=N+1，返回2），直到N>=Window时，判定当前测距周期内解算的初始位置有效。

否则N=0，返回1）。

上式中，Bias范围为1米～100米，Window范围为2～30。

所述滤波算法为：

设任意信标的坐标为(x_i,y_i)(i=1,2,...,)，则离散系统状态方程如下

s_k=As_k-1+w_k          （2）

z_k=h_k(s_k)+ν_k

其中

s_k＝[x_k y_k]^T，x_k，y_k分别表示当前时刻k的东向坐标、北向坐标；

z_k=c·τ_i，表示双程声传播距离，其中c表示声速，τ_i表示第i个信标的双程声传播时间， $h_k\left(s_k\right)=\sqrt{{(x_k-v_e{\mathrm{\&tau;}}_i-x_i)}^2+{(y_k-v_n{\mathrm{\&tau;}}_i-y_i)}^2+{(z_k-z_i)}^2}+\sqrt{{(x_k-x_i)}^2+{(y_k-y_i)}^2+{(z_k-z_i)}^2};$ w_k是一个二维向量，为过程驱动噪声，w_k(1)和w_k(2)为正值且不大于10；

ν_k是一个一维向量，为观测噪声，ν_k为正值且不大于50。

采用如下滤波算法对深水机器人位置进行滤波：

1）初始化

$s_{0|0}={\left[\begin{array}{cc}\stackrel{\&OverBar;}{x}& \stackrel{\&OverBar;}{y}\end{array}\right]}^T$

$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{{\&PartialD;h}_k}{\&PartialD;{\tilde{s}}_{k|k-1}},$

Q_k=diag([w_k(1))² (w_k(2))²])，

$R_k=\mathrm{diag}\left(\right[v_k^2\left]\right).$

所述测距波门算法为：

$[z_i-\hat{z}(k/k-1)]S{\left(k\right)}^{-1}[z_i-\hat{z}(k/k-1)]\&le;\mathrm{\&gamma;}---\left(3\right)$

S(k)=H_kP_k|k-1H'_k|k-1+R_k

式中γ范围为6～500。如果式（3）成立，则使用当前测量值z_i；否则，舍弃。

Claims (8)

1.一种深水机器人组合导航装置，其特征在于，包括

测距器，安装于深水机器人壳体外部，连接导航计算机，用于通过声信号测量深水机器人到各个信标的距离；

信标，至少三个，布设于海底；

传感器，包括航向传感器、航速传感器和深度传感器，均安装于深水机器人中并连接导航计算机，分别用于测量深水机器人的的航向、航速和所处深度；

导航计算机，用于采集测距器和传感器的信号并实时计算深水机器人当前时刻的位置。

2.根据权利要求1所述的一种深水机器人组合导航装置，其特征在于，所述测距器按固定周期进行测距，周期在3秒～60秒之间。

3.根据权利要求1所述的一种深水机器人组合导航装置，其特征在于，所述导航计算机按固定周期采集数据，周期在0.2秒～2秒之间。

4.一种深水机器人组合导航方法，其特征在于，包括以下步骤：

深水机器人下水后，导航计算机对测距器、航向传感器、航速传感器和深度传感器进行数据采集；

当导航计算机采集到测距器的距离数据时，计算深水机器人的初始位置；

判断初始位置是否有效；

如果初始位置有效，则判断距离数据是否有效，如果距离数据无效，则舍弃该距离数据，返回计算深水机器人的初始位置步骤；

如果距离数据有效，则根据距离数据对深水机器人的位置进行在线修正。

5.根据权利要求4所述的一种深水机器人组合导航方法，其特征在于，所述计算深水机器人的初始位置包括以下步骤：

当测距周期内的测距数据数量大于3时，将初始值N置0，对深水机器人水平面坐标进行解算，解算公式如下：

Ax=v          （1）

式中，

$A=\left[\begin{array}{cc}(x_2-x_1)& (y_2-y_1)\\ (x_3-x_1)& (y_3-y_1)\end{array}\right],x=\left[\begin{array}{c}x\\ y\end{array}\right],v=\left[\begin{array}{c}\frac{1}{2}[({r_1}^2-r_2^2)-(d_1^2-d_2^2)]-(z_2-z_1)\&CenterDot;z\\ \frac{1}{2}[({r_1}^2-r_3^2)-(d_1^2-d_3^2)]-(z_3-z_1)\&CenterDot;z\end{array}\right]$

(x_i,y_i,z_i)(i=1,2,3)为第i号信标的三维坐标，(x,y,z)为深水机器人水平面坐标和深度，为第i号信标到坐标系原点的水平间距，为第i号信标到深水机器人的水平距离，其中R_i表示第i号信标的单程传播距离；

根据解算的深水机器人水平面坐标推算航位：

$x_t=x_0+\underset{k=1}{\mathrm{\&Sigma;}}{v}_e\&CenterDot;\mathrm{\&Delta;t}$

$y_t=y_0+\underset{k=1}{\mathrm{\&Sigma;}}{v}_n\&CenterDot;\mathrm{\&Delta;t}$

式中，(x₀,y₀)为初始点，Δt为采集周期，(x_t,y_t)为推算航位，v_e=vsinθ，v_n=vcosθ，θ为当前航向，v为当前航速。

6.根据权利要求4所述的一种深水机器人组合导航方法，其特征在于，所述判断初始位置是否有效具体为：

在下一个测距周期到来时，重新解算公式（1）中的深水机器人水平面坐标，将其与所述推算航位求差后求取均方根误差，即：

$e=\sqrt{{(x-x_t)}^2+{(y-y_t)}^t}$

如果e<=Bias

则N=N+1，返回2），直到N>=Window时，判定当前测距周期内解算的初始位置有效。

否则N=0，返回1）。

上式中，Bias范围为1米～100米，Window范围为2～30。

7.根据权利要求4所述的一种深水机器人组合导航方法，其特征在于，所述判断距离数据是否有效的方法为：判断公式

$[z_i-\hat{z}(k/k-1)]S{\left(k\right)}^{-1}[z_i-\hat{z}(k/k-1)]\&le;\mathrm{\&gamma;}---\left(3\right)$

S(k)=H_kP_k|k-1H'_k|k-1+R_k

是否成立，如果成立，则使用当前测量值z_i；否则，舍弃；

式中，γ范围为6～500，H_k为观测矩阵，P_k|k-1为预测协方差矩阵，H'_k|k-1为预测观测矩阵，R_k为观测噪声。

8.根据权利要求4所述的一种深水机器人组合导航方法，其特征在于，所述根据距离数据对深水机器人的位置进行在线修正包括以下步骤：

设任意信标的坐标为(x_i,y_i)(i=1,2,...,)，则离散系统状态方程为

s_k=As_k-1+w_k          （2）

z_k=h_k(s_k)+ν_k

其中，

s_k＝[x_k y_k]^T，x_k，y_k分别表示当前时刻k的东向坐标、北向坐标；

z_k=c·τ_i，表示双程声传播距离，其中c表示声速，τ_i表示第i个信标的双程声传播时间， $h_k\left(s_k\right)=\sqrt{{(x_k-v_e{\mathrm{\&tau;}}_i-x_i)}^2+{(y_k-v_n{\mathrm{\&tau;}}_i-y_i)}^2+{(z_k-z_i)}^2}+\sqrt{{(x_k-x_i)}^2+{(y_k-y_i)}^2+{(z_k-z_i)}^2};$ v_e为深水机器人东向速度，x_i为第i个信标的东向坐标，v_n为深水机器人北向速度，y_i为第i个信标的北向坐标，z_i为第i个信标的深度,z_k为深水机器人当前时刻k的深度；

w_k是一个二维向量，为过程驱动噪声，w_k(1)和w_k(2)为正值且不大于10；

ν_k是一个一维向量，为观测噪声，ν_k为正值且不大于50；

1）初始化

$s_{0|0}={\left[\begin{array}{cc}\stackrel{\&OverBar;}{x}& \stackrel{\&OverBar;}{y}\end{array}\right]}^T$

$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{{\&PartialD;h}_k}{\&PartialD;{\tilde{s}}_{k|k-1}},$

Q_k=diag([w_k(1))² (w_k(2))²])，

$R_k=\mathrm{diag}\left(\right[v_k^2\left]\right).$