CN103398712B - 交替领航的协同导航方法 - Google Patents

Abstract

一种交替领航的协同导航方法，包括以下步骤：步骤1，在第一时刻，第一领航艇向跟随艇广播第一水声信号和第一无线电时统信号；步骤2，所述跟随艇在接收到所述第一无线电时统信号时开始计时、并在接收到所述第一水声信号时停止计时以得到第一时间，从而根据所述第一时间及水声通信的传输速度得到所述第一领航艇与所述跟随艇之间的第一距离；步骤3，根据所述第一距离、所述跟随艇组合推位的位置信息建立单领航艇的协同导航数学模型，从而实现对所述跟随艇的定位；步骤4，在所述第一时刻之后的第二时刻，第二领航艇重复上述步骤进行定位。本发明解决了单领航艇方案对载体机动性要求高，无法满足的问题，不需要双领航艇的时间同步性要求。

Description

交替领航的协同导航方法

技术领域

本发明涉及协同导航领域，特别是涉及一种交替领航的协同导航方法。

背景技术

多艇协同导航利用系统中某些艇的高精度导航信息，通过一定的信息交换，实现艇间导航资源的共享，装备低精度导航设备的艇可以提高自身导航精度。当某些艇由于传感器或环境因素丧失独立导航能力时，协同导航可以在一定程度上恢复这些艇的导航能力。因此研究多艇的协同导航具有重要的理论价值和实际意义。

多艇协同导航方案有多种，常用的有单艇领航和多艇领航，各种方案中的艇间通信方案各不相同。单艇领航涉及一条艇的信息广播和接收，成本低，易操作，但其对主从艇运动状态要求较高，实际应用过程中无法满足。双艇领航对主从艇的相对机动方式没有要求，但其需要2条领航艇，需要双艇同时广播自身位置信息，时间同步很难保证，且跟随艇需要安装2套水声通信设备，成本高。因此，有必要提出一种新的协同导航方案。

发明内容

为解决上述技术问题，作为本发明的一个方面，提供了一种交替领航的协同导航方法，包括以下步骤：步骤1，在第一时刻，第一领航艇向跟随艇广播第一水声信号和第一无线电时统信号；步骤2，所述跟随艇在接收到所述第一无线电时统信号时开始计时、并在接收到所述第一水声信号时停止计时以得到第一时间，从而根据所述第一时间及水声通信的传输速度得到所述第一领航艇与所述跟随艇之间的第一距离；步骤3，根据所述第一距离、所述跟随艇组合推位的位置信息建立单领航艇的协同导航数学模型，从而实现对所述跟随艇的定位；步骤4，在所述第一时刻之后的第二时刻，第二领航艇向所述跟随艇广播第二水声信号和第二无线电时统信号；步骤5，所述跟随艇在接收到所述第二无线电时统信号时开始计时、并在接收到所述第二水声信号时停止计时以得到第二时间，从而根据所述第二时间及水声通信的传输速度得到所述第二领航艇与所述跟随艇之间的第二距离；步骤6，根据所述第二距离、所述跟随艇组合推位的位置信息建立单领航艇的协同导航数学模型，从而实现对所述跟随艇的定位。

进一步地，所述第一时刻与第二时刻之间的间隔为一个滤波周期。

进一步地，所述协同导航数学模型为：

$\left\{\begin{array}{c}{x}_{k+1}=x_k+\mathrm{\Δ}t\·v_k\·{\mathrm{cos\φ}}_k\\ y_{k+1}=y_k+\mathrm{\Δ}t\·v_k\·{\mathrm{sin\φ}}_k\\ {\mathrm{\φ}}_{k+1}={\mathrm{\φ}}_k+\mathrm{\ω}\·\mathrm{\Δ}t\end{array}\right.$

其中，

x_k，x_k+1分别为k，k+1时刻x方向的位置坐标；

y_k，y_k+1分别为k，k+1时刻y方向的位置坐标；

Δt为采样时间间隔；

v_k为k时刻跟随艇的速度，

φ_k，φ_k+1分别为k，k+1时刻的航向角；

ω为角速率。

进一步地，在建立单领航艇的协同导航数学模型后，还包括：建立量测方程，

Z_k+1＝h[X_k+1，k+1]+w_z

其中Z_k+1为k+1时刻的观测量，

w_z为观测噪声，

$h\[X_{k+1},k+1\]=\left[\begin{array}{c}{R}_k^2\\ R_{k+1}^2\end{array}\right]$ 为关于系统状态量的函数，

$\begin{array}{c}{R}_k^2={(x_{k+1}^S-d_{x_{k,k+1}}-x_k^M)}^2+{(y_{k+1}^S-d_{y_{k,k+1}}-y_k^M)}^2\\ R_{k+1}^2={(x_{k+1}^S-x_{k+1}^M)}^2+{(y_{k+1}^S-y_{k+1}^M)}^2\end{array}---\left(1\right)$

式中

分别为k+1时刻跟随艇在x，y方向的位置，

S表示跟随艇；

分别为k，k+1时刻领航艇在x，y方向的位置，

M表示领航艇；

R_k，R_k+1分别为k，k+1时刻的跟随艇和领航艇的距离；

分别为x，y方向上k到k+1时刻的距离。

进一步地，在建立量测方程之后还包括：利用EKF滤波方法，得到系统估计状态，从而实现对跟随艇的定位，所述系统估计状态为：

${\hat{X}}_{k+1}={\hat{X}}_{k+1,k}+K_{k+1}(Z_{k+1}-H_{k+1}{X}_{k+1,k})$

其中，

为k到k+1时刻的状态预测值，

为k+1时刻的状态估计值，

K_k+1为k+1时刻的增益矩阵。

进一步地，所述第一领航艇和第二领航艇安装有惯性导航设备和多普勒计程仪，其导航以所述多普勒计程仪测量的绝对速度作为所述惯性导航设备的外部输入。

进一步地，所述跟随艇安装有MEMS惯性系统和多普勒计程仪。

本发明不仅解决了单领航艇方案对载体机动性要求高，无法满足的问题，而且不需要双领航艇的时间同步性要求，容易实施，另外减少了跟随艇安装的通信设备，在保证定位精度较高的同时降低了成本。

附图说明

图1示意性示出了艇间测距与通信方案；

图2示意性示出了基于单领航艇的协同导航示意图；

图3示意性示出了EKF滤波流程图；

图4示意性示出了双艇交替领航协同导航流程图；以及

图5示意性示出了双艇交替领航协同导航仿真图一；

图6示意性示出了双艇交替领航协同导航仿真图二。

具体实施方式

以下对本发明的实施例进行详细说明，但是本发明可以由权利要求限定和覆盖的多种不同方式实施。

请参考图1至图4，本发明中的交替领航的协同导航方法，包括以下步骤：

步骤1，在第一时刻t1，第一领航艇A向跟随艇C广播第一水声信号和第一无线电时统信号；

步骤2，所述跟随艇C在接收到所述第一无线电时统信号时开始计时、并在接收到所述第一水声信号时停止计时以得到第一时间，从而根据所述第一时间及水声通信的传输速度得到所述第一领航艇A与所述跟随艇C之间的第一距离；

步骤3，根据所述第一距离、所述跟随艇C组合推位的位置信息建立单领航艇的协同导航数学模型，从而实现对所述跟随艇C的定位；

步骤4，在所述第一时刻t1之后的第二时刻t2，第二领航艇B向所述跟随艇C广播第二水声信号和第二无线电时统信号；优选地，所述第一时刻与第二时刻之间的间隔为一个滤波周期。

步骤5，所述跟随艇C在接收到所述第二无线电时统信号时开始计时、并在接收到所述第二水声信号时停止计时以得到第二时间，从而根据所述第二时间及水声通信的传输速度得到所述第二领航艇B与所述跟随艇C之间的第二距离；

步骤6，根据所述第二距离、所述跟随艇组合推位的位置信息建立单领航艇的协同导航数学模型，从而实现对所述跟随艇C的定位。

本发明中，两个领航艇A、B交替进行对跟随艇的信息传递，在两个相邻滤波周期内，跟随艇C分别利用不同领航艇的基准信息进行位置估计。由于两个领航艇位置不同，这样相邻时刻相对跟随艇C的方位信息发生了很大的变化，相当于领航艇和跟随艇之间发生了大的机动。这样，不仅解决了单领航艇方案对载体机动性要求高，无法满足的问题，而且不需要双领航艇的时间同步性要求，容易实施，在保证定位精度较高的同时降低了成本。

优选地，所述协同导航数学模型为：

$\left\{\begin{array}{c}{x}_{k+1}=x_k+\mathrm{\Δ}t\·v_k\·{\mathrm{cos\φ}}_k\\ y_{k+1}=y_k+\mathrm{\Δ}t\·v_k\·{\mathrm{sin\φ}}_k\\ {\mathrm{\φ}}_{k+1}={\mathrm{\φ}}_k+\mathrm{\ω}\·\mathrm{\Δ}t\end{array}\right.$

其中，

x_k，x_k+1分别为k，k+1时刻x方向的位置坐标；

y_k，y_k+1分别为k，k+1时刻y方向的位置坐标；

Δt为采样时间间隔；

v_k为k时刻跟随艇的速度，

φ_k，φ_k+1分别为k，k+1时刻的航向角；

ω为角速率。

进一步地，运动方程可简写为：

X_k+1＝f(x_k，u_k，w_k)＝X_k+Γ(u_k+w_k)

式中，

X_k＝(x_k，y_k，φ_k)^T表示跟随艇在k时刻的状态，

X_k+1为k+1时刻的状态量；

u_k＝(v_k φ_k)^T为k时刻系统的输入量；

f(x_k，u_k，w_k)为关于x_k，u_k，w_k的函数；

Γ(u_k+w_k)为非线性项，

w_k为高斯白噪声，

且：

$Q_k=E\left(w_k{w}_k^T\right)=\left[\begin{array}{cc}{\mathrm{\σ}}_{vk}^2& 0\\ 0& {\mathrm{\σ}}_{\mathrm{\φ}k}^2\end{array}\right]$

式中

Q_k为噪声方差阵，为的均值，为w_k的转置；分别为速度噪声方差、航向噪声方差。

优选地，在建立单领航艇的协同导航数学模型后，还包括：建立量测方程，其中，所述量测方程为：

Z_k+1＝h[X_k+1，k+1]+w_z

其中Z_k+1为k+1时刻的观测量，

w_z为观测噪声，

$h\[X_{k+1},k+1\]=\left[\begin{array}{c}{R}_k^2\\ R_{k+1}^2\end{array}\right]$ 为关于系统状态量的函数，

$\begin{array}{c}{R}_k^2={(x_{k+1}^S-d_{x_{k,k+1}}-x_k^M)}^2+{(y_{k+1}^S-d_{y_{k,k+1}}-y_k^M)}^2\\ R_{k+1}^2={(x_{k+1}^S-x_{k+1}^M)}^2+{(y_{k+1}^S-y_{k+1}^M)}^2\end{array}---\left(1\right)$

式中

分别为k+1时刻跟随艇在x，y方向的位置，

S表示跟随艇；

分别为k，k+1时刻领航艇在x，y方向的位置，

M表示领航艇；

R_k，R_k+1分别为k，k+1时刻的跟随艇和领航艇的距离；

分别为x，y方向上k到k+1时刻的距离。

特别地，(1)式可通过下述方式得到：根据跟随艇从时刻t_k到时刻t_k+1运动的距离矢量，将t_k时刻跟随艇与领航艇的几何位置平移到t_k+1时刻，如图2所示，t_k时刻、t_k+1时刻以领航艇为圆心、两艇间距为半径的两个圆有交点，由图中的几何关系，可得到两条艇间距离的表达式即为(1)式。

优选地，在建立量测方程之后还包括：利用EKF滤波方法，

EKF滤波过程为：

一步状态预测为：

${\hat{X}}_{k+1,k}=F_k{\hat{X}}_k$

其中系统矩阵F_k为： $F_k=I+\mathrm{\Δ}t\·\frac{\∂f}{\∂X_k}=I+\mathrm{\Δ}t\·\left[\begin{array}{ccc}1& 0& -\mathrm{\Δ}t\·v_k{\mathrm{sin\φ}}_k\\ 0& 1& \mathrm{\Δ}t\·v_k{\mathrm{cos\φ}}_k\\ 0& 0& 1\end{array}\right],$

式中I为单位阵，f为式(2)中的函数。

一步预测协方差阵P_k+1，k为：

P_k+1，k＝F_kP_kF_k ^T+G_kQ_kG_k ^T

其中状态估计误差协方差阵P_k＝(I-K_kH_k)P_k，k-1，F_k ^T为F_k的转置，

系统噪声激励矩阵G_k为： $G_k=\frac{\∂f}{{{\∂u}_k}^T}=\left[\begin{array}{cc}\mathrm{\Δt}\·\cos {\mathrm{\φ}}_k& 0\\ \mathrm{\Δt}\·\sin {\mathrm{\φ}}_k& 0\\ 0& \mathrm{\Δt}\end{array}\right],$ G_k ^T为G_k的转置，u_k ^T为u_k的转置。

滤波增益矩阵K_k+1为：

K_k+1＝P_k+1，kH_k+1 ^T[H_k+1P_k+1，kH_k+1 ^T+R_k+1]^-1

其中量测矩阵 $H_{k+1}=\frac{\∂h\[X_{k+1},k+1\]}{\∂X_{k+1}}=\left[\begin{array}{ccc}2(x_{k+1}^S-d_{x_{k,k+1}}-x_k^M)& 2(y_{k+1}^S-d_{y_{k,k+1}}-y_k^M)& 0\\ 2(x_{k+1}^S-x_{k+1}^M)& 2(y_{k+1}^S-y_{k+1}^M)& 0\end{array}\right],$ H_k+1 ^T为H_k+1的转置，R_k+1为量测噪声方差阵。

状态估计为：

${\hat{X}}_{k+1}={\hat{X}}_{k+1,k}+K_{k+1}\[Z_{k+1}-H_{k+1}{\hat{X}}_{k+1,k}\]$

进一步地，上式实际上代表了利用滤波周期内跟随艇航推信息对系统量测信息Z_k+1的校正。

以此过程逐次递推至状态估计误差协方差阵达到稳定值，即得到系统状态(包含位置和航向)的估计值，从而实现对跟随艇的定位。

优选地，所述第一领航艇和第二领航艇安装有惯性导航设备和多普勒计程仪，其导航以所述多普勒计程仪测量的绝对速度作为所述惯性导航设备的外部输入。

优选地，所述跟随艇安装有MEMS惯性系统和多普勒计程仪。

本发明具有以下优点：

(1)两个领航艇交替进行对跟随艇的信息传递，在两个相邻滤波周期内，跟随艇分别利用不同领航艇的基准信息进行位置估计，避免了跟随艇安装2套水声通信设备，降低成本。

(2)由于两个领航艇位置不同，这样相邻时刻相对跟随艇的方位信息发生了很大的变化，相当于领航艇和跟随艇之间发生了大的机动，解决了单领航艇方案对载体机动性要求高，无法满足的问题。

(3)双艇交替领航不涉及到双领航艇时间同步的问题，容易实施，而且定位精度较高。

为了进一步说明本发明的有益效果，在以下初始条件下对交替领航协同导航方案进行了仿真验证：

仿真时间：1小时

领航艇、跟随艇速度：30公里/小时

跟随艇航向角误差：10度/小时

跟随艇测速误差：1％

艇间距测量误差：0.2m

艇间距：200m

请参考图5和图6，分析比较：双-单艇协同方案需要2条领航艇，不涉及到双艇时间同步的问题，成本较低，易操作，鲁棒性强。与单艇协同方案相比，协同定位的可观测性得到很大提高，对无人艇的机动要求降低很多。在保证定位精度的前提下，避免了双领航艇带来的时间同步问题。

本发明不仅解决了单领航艇方案对载体机动性要求高，无法满足的问题，而且不需要双领航艇的时间同步性要求，容易实施，另外减少了跟随艇安装的通信设备，在保证定位精度较高的同时降低了成本。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种交替领航的协同导航方法，其特征在于，包括以下步骤：

步骤1，在第一时刻，第一领航艇向跟随艇广播第一水声信号和第一无线电时统信号；

步骤2，所述跟随艇在接收到所述第一无线电时统信号时开始计时、并在接收到所述第一水声信号时停止计时以得到第一时间，从而根据所述第一时间及水声通信的传输速度得到所述第一领航艇与所述跟随艇之间的第一距离；

步骤3，根据所述第一距离、所述跟随艇组合推位的位置信息建立单领航艇的协同导航数学模型，从而实现对所述跟随艇的定位；

步骤4，在所述第一时刻之后的第二时刻，第二领航艇向所述跟随艇广播第二水声信号和第二无线电时统信号；

步骤5，所述跟随艇在接收到所述第二无线电时统信号时开始计时、并在接收到所述第二水声信号时停止计时以得到第二时间，从而根据所述第二时间及水声通信的传输速度得到所述第二领航艇与所述跟随艇之间的第二距离；

步骤6，根据所述第二距离、所述跟随艇组合推位的位置信息建立单领航艇的协同导航数学模型，从而实现对所述跟随艇的定位。

2.根据权利要求1所述的协同导航方法，其特征在于，所述第一时刻与第二时刻之间的间隔为一个滤波周期。

3.根据权利要求1所述的协同导航方法，其特征在于，所述协同导航数学模型为：

$\left\{\begin{array}{c}{x}_{k+1}=x_k+\mathrm{\Δ}t\·v_k\·{\mathrm{cos\φ}}_k\\ y_{k+1}=y_k+\mathrm{\Δ}t\·v_k\·{\mathrm{sin\φ}}_k\\ {\mathrm{\φ}}_{k+1}={\mathrm{\φ}}_k+\mathrm{\ω}\·\mathrm{\Δ}t\end{array}\right.$

其中，

x_k，x_k+1分别为k，k+1时刻x方向的位置坐标；

y_k，y_k+1分别为k，k+1时刻y方向的位置坐标；

Δt为采样时间间隔；

v_k为k时刻跟随艇的速度，

φ_k，φ_k+1分别为k，k+1时刻的航向角；

ω为角速率。

4.根据权利要求1所述的协同导航方法，其特征在于，在建立单领航艇的协同导航数学模型后，还包括：建立量测方程，

所述量测方程为：

Z_k+1＝h[X_k+1，k+1]+w_z

其中

Z_k+1为k+1时刻的观测量，

w_z为观测噪声，

为关于系统状态量的函数，

其中，

$R_k^2={(x_{k+1}^S-d_{x_{k,k+1}}-x_k^M)}^2+{(y_{k+1}^S-d_{y_{k,k+1}}-y_k^M)}^2$

$R_{k+1}^2={(x_{k+1}^S-x_{k+1}^M)}^2+{(y_{k+1}^S-y_{k+1}^M)}^2$

式中

分别为k+1时刻跟随艇在x，y方向的位置，

S表示跟随艇；

分别为k，k+1时刻领航艇在x，y方向的位置，

M表示领航艇；

R_k，R_k+1分别为k，k+1时刻的跟随艇和领航艇的距离；

分别为x，y方向上k到k+1时刻的距离。

5.根据权利要求4所述的协同导航方法，其特征在于，在建立量测方程之后还包括：利用EKF滤波方法，得到系统状态的估计值，从而实现对跟随艇的定位。

6.根据权利要求1所述的协同导航方法，其特征在于，所述第一领航艇和第二领航艇安装有惯性导航设备和多普勒计程仪，其导航以所述多普勒计程仪测量的绝对速度作为所述惯性导航设备的外部输入。

7.根据权利要求1所述的协同导航方法，其特征在于，所述跟随艇安装有MEMS惯性系统和多普勒计程仪。