CN103090864A - 一种基于通信延时补偿的多水面无人艇协同定位方法 - Google Patents

Abstract

一种基于通信延时补偿的多水面无人艇协同定位方法，涉及一种导航定位技术。它实现了多水面无人艇的高精度协同定位。其方法：接收艇采集发送艇发送的相对定位信息以及自身的航推定位信息；利用相对定位信息中加载的时间戳计算信息延迟步长；并建立协同定位系统状态方程；利用状态量计算接收艇状态转移矩阵以及延时阶段基于不同时刻状态估计的均方误差矩阵；利用累积计算的接收艇状态转移矩阵，建立协同定位系统量测方程；利用累积计算的不同时刻状态估计均方误差矩阵，计算最小方差估计下的滤波增益矩阵；利用滤波增益矩阵，计算系统状态均方误差矩阵；对协同定位系统由于通信延时造成的定位误差进行估计补偿。本发明适用于多水面无人艇协同定位。

Description

一种基于通信延时补偿的多水面无人艇协同定位方法

技术领域

本发明涉及一种导航定位技术，具体涉及一种多水面无人艇协同定位方法。

背景技术

水面无人艇以其体积小、机动性强、成本低等优势在海洋开发中起着越来越大的作用。由于定位能力是多无人艇协同作业的基础，多艇协同作业是提高作业能力的必要手段。所以有必要研究多水面无人艇协同定位方法。如果多无人艇之间存在直接相对观测或者间接相对观测，那么通过一定的信息交换，就可以实现平台间定位信息的共享，达到提高多艇整体定位能力的目的，这种定位方法称为协同定位。协同定位具有下列优势：可以利用系统中某些艇的高精度定位信息，可以提高整体的定位精度；由于系统中只有某些艇配备了高精度导航设备，提供高精度定位信息，所以降低了系统成本；可以使得系统中每艘艇都具有误差有界的定位能力；当某些艇由于传感器或环境因素丧失独立定位能力时，协同定位可以在一定程度上恢复这些平台的定位能力。由于多艇协同定位时，各艇之间存在着相对观测并且通过通信传输定位信息，所以通信延迟必然会对协同定位精度造成一定的影响，如果不进行补偿，这种影响会随着多无人艇间距的增大而增大。

发明内容

本发明是为了实现多水面无人艇的高精度协同定位，从而提供一种基于通信延时补偿的多水面无人艇协同定位方法。

一种基于通信延时补偿的多水面无人艇协同定位方法，它由以下步骤实现：

步骤一、接收艇采集发送艇发送的相对定位信息，所述相对定位信息包括：接收艇与发送艇的距离信息r和发送艇的位置信息，即：发送艇相对于起始点的相对位置信息(x_a、y_a)；

接收艇采集自身的航推定位信息：所述航推定位信息包括航速V_b和航向角速度w_b；

步骤二、通过接收艇自身携带的世界标准时间系统以及步骤一中采集到的相对定位信息中加载的时间戳，接收艇计算本次接收的信息延时时间，并与滤波周期比较，获得本次接收的相对定位信息延迟步长N，N为实数；

步骤三、利用步骤一采集的相对定位信息，建立基于延迟步长N的协同定位系统状态方程；

系统状态方程为：

X(k+1)＝Φ(k)X(k)；

其中：X(k+7)为T_k·l时刻系统状态量，X(k)为T_k时刻系统状态量，Φ(k)为T_k时刻系统矩阵；

其中： $\mathrm{\Φ}\left(k\right)=I+\mathrm{\δt}\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 1& 0& -\mathrm{\δt}\·V_{\mathrm{bk}}\sin {\mathrm{\φ}}_{\mathrm{bk}}\\ 0& 0& 0& 1& \mathrm{\δt}\·V_{\mathrm{bk}}\cos {\mathrm{\φ}}_{\mathrm{bk}}\\ 0& 0& 0& 0& 1\end{array}\right];$

式中：I为单位阵，δt为系统采样时间，V_bk为T_k时刻接收艇航速，φ_bk为T_k时刻接收艇航向角；

$G\left(k\right)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& \mathrm{\δt}\·\cos {\mathrm{\φ}}_k& 0\\ 0& 0& \mathrm{\δt}\·\sin {\mathrm{\φ}}_k& 0\\ 0& 0& 0& \mathrm{\δt}\end{array}\right];$

系统输入量为u(k)＝(x_as，y_as，v_bk，φ_bk)^T；

其中：x_as、y_as为T_s时刻发送艇相对于起始点的相对位置信息；T_s时刻为发送艇上当前次信息采集时刻，T_k时刻为接收艇上当前次信息到达时刻，所述T_s时刻与T_k时刻之间存在着N个滤波步长；

系统的状态量X(k)为：

X(k)＝(x_as，y_as，x_bk，y_bk，φ_bk)^T＝[X_a(s)^TX_b(k)^T]^T；

其中：x_bk、y_bk为T_k时刻接收艇相对于起始点的相对位置信息；X_a(s)^T为T_s时刻发送艇定位系统状态量的转置，上标T代表矩阵转置；X_b(k)^T为T_k时刻接收艇定位系统状态量的转置；

步骤四、利用系统的状态量X(k)通过公式：

${\mathrm{\Φ}}_b\left(i\right)=I+\mathrm{\δt}\begin{array}{c}\left[\begin{array}{ccc}1& 0& -\mathrm{\δt}\·V_{\mathrm{bi}}\sin {\mathrm{\φ}}_{\mathrm{bi}}\\ 0& 1& \mathrm{\δt}\·V_{\mathrm{bi}}\cos {\mathrm{\φ}}_{\mathrm{bi}}\\ 0& 0& 1\end{array}\right]\≈\left[\begin{array}{ccc}1& 0& -{\mathrm{\δt}}^2\·V_{\mathrm{bi}}\sin {\mathrm{\φ}}_{\mathrm{bi}}\\ 0& 1& -{\mathrm{\δt}}^2\·V_{\mathrm{bi}}\cos {\mathrm{\φ}}_{\mathrm{bi}}\\ 0& 0& 1\end{array}\right]\end{array}$

计算接收艇状态转移矩阵Φ_b(i)；

其中：s＝＜i＜＝k；s、i、k分别代表T_s、T_i、T_k时刻；

V_bi为T_i时刻接收艇航速，φ_bi为T_i时刻接收艇航向角；

并根据公式：

${\mathrm{\Φ}}_b{(k,s)}^{-1}=\underset{i=s}{\overset{k}{\mathrm{\Π}}}{\mathrm{\Φ}}_b{\left(i\right)}^{-1}=\begin{array}{}\left[\begin{array}{cc}{I}_2& -\underset{i=s}{\overset{k}{\mathrm{\Σ}}}L\left(i\right)\\ O_{1\×2}& 1\end{array}\right]\end{array}$

其中：k的取值为k＞＝3；I₂为2阶单位阵；O_1×2为零矩阵；L(i)为等价方块阵；

获得T_s到T_k时刻的接收艇状态转移矩阵Φ_b(k，s)^-1；

步骤五、利用步骤四获得的T_s到T_k时刻的接收艇状态转移矩阵Φ_b(k，s)^-1，建立基于N个延迟步长的协同定位系统量测方程：

$Z\left(k\right)=c_a\left(s\right)X_a\left(s\right)+c_b\left(s\right)X_b\left(s\right)$

$=c_a\left(s\right)X_a\left(s\right)+c_b\left(s\right){\mathrm{\Φ}}_b{(k,s)}^{-1}{X}_b\left(k\right)$

$=\left[\begin{array}{cc}{c}_a\left(s\right)& c_b\left(s\right){\mathrm{\Φ}}_b{(k,s)}^{-1}\end{array}\right]\left[\begin{array}{c}{X}_a\left(s\right)\\ X_b\left(k\right)\end{array}\right]$ ；

$=C\left(k\right)X\left(k\right)$

其中：C(k)为T_k时刻系统量测矩阵，c_a(s)为T_s时刻发送艇系统量测矩阵，c_b(s)为T_s时刻接收艇系统量测矩阵；

$c_a\left(s\right)=\frac{\∂Z\left(s\right)}{\∂X_a{(s,s-1)}^T}=\left[\begin{array}{cc}1& 0\\ 0& 1\\ \frac{(x_{\mathrm{as}}-x_{\mathrm{bs}})}{r_s}& \frac{(y_{\mathrm{as}}-y_{\mathrm{bs}})}{r_s}\end{array}\right];$

式中：Z(s)为T_s时刻系统量测量；r_s为T_s时刻两艇间间距，上标T代表矩阵转置；

$c_b\left(s\right)=\frac{\∂Z\left(s\right)}{\∂X_b{(s,s-1)}^T}=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ \frac{(x_{\mathrm{bs}}-x_{\mathrm{as}})}{r_s}& \frac{(y_{\mathrm{bs}}-y_{\mathrm{as}})}{r_s}& 0\end{array}\right];$

步骤六、利用步骤一采集的相对定位信息以及步骤五获得的系统量测方程，根据公式：

$M=E\left\{\begin{array}{c}\end{array}\tilde{X}\left(s\right)\tilde{X}{(k,k-1)}^T\right\}$

$=P\left(s\right)\underset{i=1}{\overset{N-1}{\mathrm{\Π}}}\left[\mathrm{\Φ}{(s+i)}^T{(I-K(s+i+1)C(s+i+1))}^T\right]$

其中：为T_s时刻系统状态估计误差；为T_k时刻系统状态一步预测估计误差；P(s)为T_s时刻系统状态估计误差方差阵；Φ(s+i)^T为系统T_s+i时刻系统矩阵的转置；K(s+i+1)为系统T_s+i+1时刻滤波增益矩阵；C(s+i+1)为系统T_s+i+1时刻量测矩阵；

获得延时阶段基于不同时刻状态估计的均方误差矩阵M；

步骤七、利用步骤六获得的不同时刻状态估计均方误差矩阵M，根据公式：

K(k)＝M^TC(s)^T[C(s)P(s)C(s)^T]^-1

获得最小方差估计下的滤波增益矩阵K(k)；

其中：C(s)为T_s时刻系统量测矩阵；

步骤八、利用步骤七获得的最小方差估计下的滤波增益矩阵K(k)，根据公式：

P(k)＝P(k，k-1)-K(k)C(s)M

获得基于N个延时步长的系统状态均方误差矩阵P(k)；

其中：P(k，k-1)＝Φ(k，k-1)P(k)Φ^T(k，k-1)；

Φ(k，k-1)为系统T_k-1到T_k时刻系统矩阵；P(k)为系统T_k-1到T_k时刻系统状态估计误差方差阵；

步骤九、利用步骤五获得的系统基于N个延迟步长的量测方程，以及步骤七获得的最小方差估计下的滤波增益矩阵，采用公式：

${\hat{X}}_k=(I-K\left(k\right)C\left(k\right))\hat{X}(k,k-1)+K\left(k\right)Z\left(k\right)$

其中：

为系统T_k-1到T_k时刻的状态估计；

对协同定位系统由于通信延时造成的定位误差进行估计补偿，进而实现通信延时补偿的接收艇定位；

$\hat{X}(k,k-1)={\left[\begin{array}{cc}{\hat{X}}_a{(s,s-1)}^T& {\hat{X}}_b{(k,k-1)}^T\end{array}\right]}^T;$

其中：

为发送艇T_s-1到T_k时刻的状态估计；

为接收艇T_k-1到T_k时刻的状态估计；

${\hat{X}}_a(s,s-1)=(1+\mathrm{\δt})\left(\begin{array}{c}{X}_{\mathrm{as}-1}\\ y_{\mathrm{as}-1}\end{array}\right);$

其中：x_as-1、y_as-1为发送艇T_s-1时刻相对于起始点的相对位置信息；

${\hat{X}}_b(k,k-1)={\mathrm{\Φ}}_b(k,k-1){\hat{X}}_b(k-1)$

其中：Φ_b(k，k-1)为系统T_k-1到T_k时刻系统矩阵；

为接受艇T_k-1时刻的状态估计。

步骤六中获得的延时阶段基于不同时刻状态估计的均方误差矩阵M是基于Kalman滤波方程实现的；所述Kalman滤波方程为：

$X(k,k-1)=\underset{i=1}{\overset{N-1}{\mathrm{\Π}}}\left[(I-K(k-i)C(k-i))\mathrm{\Φ}(k-i)\right]X\left(s\right)$

其中：K(k-i)为系统T_k-i时刻滤波增益矩阵；C(k-i)为系统T_k-i时刻量测矩阵；Φ(k-i)为系统T_k-i时刻系统矩阵。

本发明提出了一种基于通信延时补偿的多水面无人艇协同定位技术，既能够充分利用某些无人艇的高精度定位信息，提高其他艇的定位精度，又能有效的补偿由于各艇之间通信延迟造成的定位误差。本发明的多水面无人艇协同定位方法具有精度高、成本低、可靠性强等显著优点。

附图说明

图1是信息接收艇与发送艇的设计运动轨迹图；图2是本发明的方法对延时补偿后的定位误差仿真示意图；图3是本发明的方法对延时补偿后的定位轨迹仿真示意图。

具体实施方式

具体实施方式一、一种基于通信延时补偿的多水面无人艇协同定位方法，它由以下步骤实现：

步骤一、接收艇采集发送艇发送的相对定位信息：发送艇的位置信息(相对于起始点的相对位置信息x_a、y_a)以及两艇的距离信息r；接收艇采集自身的航推定位信息：航速V_b以及航向角速度w_b；

步骤二、通过接收艇自身携带的世界标准时间系统以及发送艇发送的相对定位信息中加载的时间戳，计算本次接收的信息延时时间，与滤波周期比较，得到本次接受的定位信息延迟步长N；

步骤三、利用步骤一采集的定位信息，建立基于上述延迟步长的协同定位系统状态方程。

定义系统的状态量为：

X(k)＝(x_as，y_as，x_bk，y_bk，φ_bk)^T＝[X_a(s)^TX_b(k)^T]^T；

其中，下标as代表发送艇T_s时刻的状态量，下标bk代表接收艇在T_k时刻的状态量。T_s时刻为发送艇上本次信息采集时刻，T_k时刻为接收艇上本次信息到达时刻，两时刻间存在着N个滤波步长。系统输入量为u(k)＝(x_as，y_as，v_bk，φ_bk)^T。

系统状态方程为：

X(k+1)＝Φ(k)X(k)；

其中 $\mathrm{\Φ}\left(k\right)=I+\mathrm{\δt}\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 1& 0& -\mathrm{\δt}\·V_k\sin {\mathrm{\φ}}_k\\ 0& 0& 0& 1& \mathrm{\δt}\·V_k\cos {\mathrm{\φ}}_k\\ 0& 0& 0& 0& 1\end{array}\right],$

$G\left(k\right)=\frac{\∂f}{{\∂u}_k^T}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& \mathrm{\δt}\·\cos {\mathrm{\φ}}_k& 0\\ 0& 0& \mathrm{\δt}\·\sin {\mathrm{\φ}}_k& 0\\ 0& 0& 0& \mathrm{\δt}\end{array}\right];$

步骤四、利用状态量计算接收艇状态转移矩阵为：

${\mathrm{\Φ}}_b\left(i\right)=I+\mathrm{\δt}\begin{array}{c}\left[\begin{array}{ccc}1& 0& -\mathrm{\δt}\·V_b\left(k\right)\sin {\mathrm{\φ}}_b\left(k\right)\\ 0& 1& \mathrm{\δt}\·V_b\left(k\right)\cos {\mathrm{\φ}}_b\left(k\right)\\ 0& 0& 1\end{array}\right]\≈\left[\begin{array}{ccc}1& 0& -{\mathrm{\δt}}^2\·V_b\left(i\right)\sin {\mathrm{\φ}}_b\left(i\right)\\ 0& 1& -{\mathrm{\δt}}^2\·V_b\left(i\right)\cos {\mathrm{\φ}}_b\left(i\right)\\ 0& 0& 1\end{array}\right]\end{array},$

推导T_s到T_k时刻的接收艇状态转移矩阵Φ_b(k，s)_-1为：

${\mathrm{\Φ}}_b{(k,s)}^{-1}=\underset{i=s}{\overset{k}{\mathrm{\Π}}}{\mathrm{\Φ}}_b{\left(i\right)}^{-1}=\begin{array}{c}\left[\begin{array}{cc}{I}_2& -\underset{i=s}{\overset{k}{\mathrm{\Σ}}}L\left(i\right)\\ O_{1\×2}& 1\end{array}\right];\end{array}$

步骤五、利用步骤四累积计算的接收艇状态转移矩阵，建立基于N个延迟步长的协同定位系统量测方程：

$Z\left(k\right)=c_a\left(s\right)X_a\left(s\right)+c_b\left(s\right)X_b\left(s\right)$

$=c_a\left(s\right)X_a\left(s\right)+c_b\left(s\right){\mathrm{\Φ}}_b{(k,s)}^{-1}{X}_b\left(k\right)$

$=\left[\begin{array}{cc}{c}_a\left(s\right)& c_b\left(s\right){\mathrm{\Φ}}_b{(k,s)}^{-1}\end{array}\right]\left[\begin{array}{c}{X}_a\left(s\right)\\ X_b\left(k\right)\end{array}\right],$

$=C\left(k\right)X\left(k\right)$

其中 $c_a\left(s\right)=\frac{\∂Z\left(s\right)}{\∂X_a{(s,s-1)}^T}=\left[\begin{array}{cc}1& 0\\ 0& 1\\ \frac{(x_{\mathrm{as}}-x_{\mathrm{bs}})}{r_s}& \frac{(y_{\mathrm{as}}-y_{\mathrm{bs}})}{r_s}\end{array}\right],$

$c_b\left(s\right)=\frac{\∂Z\left(s\right)}{\∂X_b{(s,s-1)}^T}=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ \frac{(x_{\mathrm{bs}}-x_{\mathrm{as}})}{r_s}& \frac{(y_{\mathrm{bs}}-y_{\mathrm{as}})}{r_s}& 0\end{array}\right];$

步骤六、利用步骤一采集的定位信息以及步骤五得到的系统量测矩阵，计算延时阶段基于不同时刻状态估计的均方误差矩阵：

$M=E\left\{\begin{array}{c}\end{array}\tilde{X}\left(s\right)\tilde{X}{(k,k-1)}^T\right\}$

$=P\left(s\right)\underset{i=1}{\overset{N-1}{\mathrm{\Π}}}\left[\mathrm{\Φ}{(s+i)}^T{(I-K(s+i+1)C(s+i+1))}^T\right];$

步骤七、利用步骤六累积计算的不同时刻状态估计均方误差矩阵，计算最小方差估计下的滤波增益矩阵：

K(k)＝M^TC(s)^T[C(s)P(s)C(s)^T]^-1；

步骤八、利用步骤七计算的滤波增益矩阵，计算基于N个延时步长的系统状态均方误差矩阵：

P(k)＝P(k，k-1)-K(k)C(s)M；

其中P(k，k-1)＝Φ(k，k-1)P(k)Φ^T(k，k-1)；

步骤九、利用步骤五得到的系统基于N个延迟步长的量测方程，以及步骤七得到的最小方差估计下的滤波增益矩阵，对协同定位系统由于通信延时造成的定位误差进行估计补偿

${\hat{X}}_k=(I-K\left(k\right)C\left(k\right))\hat{X}(k,k-1)+K\left(k\right)Z*\left(k\right),$

其中 $\hat{X}(k,k-1)={\left[\begin{array}{cc}{\hat{X}}_a{(s,s-1)}^T& {\hat{X}}_b{(k,k-1)}^T\end{array}\right]}^T,$ ${\hat{X}}_a(s,s-1)=(1+\mathrm{\δt})\left(\begin{array}{c}{X}_{\mathrm{as}-1}\\ y_{\mathrm{as}-1}\end{array}\right),$ ${\hat{X}}_b(k,k-1)={\mathrm{\Φ}}_b(k,k-1){\hat{X}}_b(k-1);$

本发明提供的基于通信延时补偿的多水面无人艇协同定位方法具有以下优点：

一方面，充分利用了信息发送艇的高精度定位信息，提高了信息接收艇的定位精度，使的信息接收艇具有误差有界的定位能力；另一方面，建立了基于N个延迟步长的系统状态、量测方程，得到了基于最小方差估计下的滤波增益矩阵。因此，在实际工程使用中，当各艇之间存在通信延时，利用上述步骤对延时误差进行补偿，从而使信息接收艇能够利用发送艇的延时信息进行准确定位，减少由于通信延时造成的定位误差。

为了进一步说明所述方法的有益效果，在以下初始条件下分别对不存在通信延时、存在通信延时不补偿、存在通信延时补偿情况下的两艘无人艇协同定位情况进行了仿真，仿真结果如图1、图2与图3所示，并对其进行了分析比较。

初始条件：

假设领航艇的起点坐标是(0 0)单位为米；

初始航向角为0°，角速度为0.08°/s，航速为4kn；

跟随艇的起点坐标是(10 2.5)，单位为米；

初始航向角为30°，做直线运动750秒后以固定角速度-0.9°/s拐弯，200秒后转弯180°，再做直线运动750秒后以固定角速度0.9°/s拐弯180°，重复以上过程。航速为8kn；

通信延时计算公式为

秒，其中1500m/s为信号在水中传播速度，5秒为设备处理信号时间；

滤波周期为1秒，仿真时长为1小时；

接收艇测速误差为0.5kn/h，航向角速度测量误差0.1°/s，两艇之间距离测量误差为10m。

分析比较：

图1给出了发送艇和接收艇的运动轨迹，这种相对运动基本上包括了实际工作过程中所有的相对运动方式(直线、拐弯运动)，所以仿真结果具有代表性。根据图2可以看出，如果存在通信延迟而不进行误差补偿，会产尘非常大的定位误差，从而导致协同定位失效。从图3可以看出，通过对通信延迟的补偿，可以完全补偿由于延迟造成的定位误差，并且定位精度比不考虑延时情况下都要高，这证明了本发明采用的状态最小方差估计有效，在处理延时情况时，比传统扩展Kalman得到更优估计结果。

Claims (2)

1.一种基于通信延时补偿的多水面无人艇协同定位方法，其特征是：它由以下步骤实现：

步骤一、接收艇采集发送艇发送的相对定位信息，所述相对定位信息包括：接收艇与发送艇的距离信息r和发送艇的位置信息，即：发送艇相对于起始点的相对位置信息(x_a、y_a)；

接收艇采集自身的航推定位信息：所述航推定位信息包括航速V_b和航向角速度w_b；

步骤二、通过接收艇自身携带的世界标准时间系统以及步骤一中采集到的相对定位信息中加载的时间戳，接收艇计算本次接收的信息延时时间，并与滤波周期比较，获得本次接收的相对定位信息延迟步长N，N为实数；

步骤三、利用步骤一采集的相对定位信息，建立基于延迟步长N的协同定位系统状态方程；

系统状态方程为：

X(k+1)＝Φ(k)X(k)；

其中：X(k+1)为T_k-1时刻系统状态量，X(k)为T_k时刻系统状态量，Φ(k)为T_k时刻系统矩阵；

其中： $\mathrm{\Φ}\left(k\right)=I+\mathrm{\δt}\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 1& 0& -\mathrm{\δt}\·V_{\mathrm{bk}}\sin {\mathrm{\φ}}_{\mathrm{bk}}\\ 0& 0& 0& 1& \mathrm{\δt}\·V_{\mathrm{bk}}\cos {\mathrm{\φ}}_{\mathrm{bk}}\\ 0& 0& 0& 0& 1\end{array}\right];$

式中：I为单位阵，δt为系统采样时间，V_bk为T_k时刻接收艇航速，φ_bk为T_k时刻接收艇航向角；

$G\left(k\right)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& \mathrm{\δt}\·\cos {\mathrm{\φ}}_k& 0\\ 0& 0& \mathrm{\δt}\·\sin {\mathrm{\φ}}_k& 0\\ 0& 0& 0& \mathrm{\δt}\end{array}\right];$

系统输入量为u(k)＝(x_as，y_as，v_bk，φ_bk)^T；

具中：x_as、y_as为T_s时刻发送艇相对于起始点的相对位置信息；T_s时刻为发送艇上当前次信息采集时刻，T_k时刻为接收艇上当前次信息到达时刻，所述T_s时刻与T_k时刻之间存在着N个滤波步长；

系统的状态量X(k)为：

X(k)＝(x_as，y_as，x_bk，y_bk，φ_bk) ^T＝[X_a(s)^TX_b(k)^T]^T；

其中：x_bk、y_bk为T_k时刻接收艇相对于起始点的相对位置信息；X_a(s)^T为T_s时刻发送艇定位系统状态量的转置，上标T代表矩阵转置；X_b(k)^T为T_k时刻接收艇定位系统状态量的转置；

步骤四、利用系统的状态量X(k)通过公式：

${\mathrm{\Φ}}_b\left(i\right)=I+\mathrm{\δt}\begin{array}{c}\left[\begin{array}{ccc}1& 0& -\mathrm{\δt}\·V_{\mathrm{bi}}\sin {\mathrm{\φ}}_{\mathrm{bi}}\\ 0& 1& \mathrm{\δt}\·V_{\mathrm{bi}}\cos {\mathrm{\φ}}_{\mathrm{bi}}\\ 0& 0& 1\end{array}\right]\≈\left[\begin{array}{ccc}1& 0& -{\mathrm{\δt}}^2\·V_{\mathrm{bi}}\sin {\mathrm{\φ}}_{\mathrm{bi}}\\ 0& 1& -{\mathrm{\δt}}^2\·V_{\mathrm{bi}}\cos {\mathrm{\φ}}_{\mathrm{bi}}\\ 0& 0& 1\end{array}\right]\end{array}$

计算接收艇状态转移矩阵Φ_b(i)；

其中：s＝＜i＜＝k；s、i、k分别代表T_s、T_i、T_k时刻；

V_bi为T_i时刻接收艇航速，φ_bi为T_i时刻接收艇航向角；

并根据公式：

${\mathrm{\Φ}}_b{(k,s)}^{-1}=\underset{i=s}{\overset{k}{\mathrm{\Π}}}{\mathrm{\Φ}}_b{\left(i\right)}^{-1}=\begin{array}{c}\left[\begin{array}{cc}{I}_2& -\underset{i=s}{\overset{k}{\mathrm{\Σ}}}L\left(i\right)\\ O_{1\×2}& 1\end{array}\right]\end{array}$

其中：k的取值为k＞＝3；I₂为2阶单位阵；O_1×2为零矩阵；L(i)为等价方块阵；

获得T_s到T_k时刻的接收艇状态转移矩阵Φ_b(k，s)^-1；

步骤五、利用步骤四获得的T_s到T_k时刻的接收艇状态转移矩阵Φ_b(k，s)^-1，建立基于N个延迟步长的协同定位系统量测方程：

$Z\left(k\right)=c_a\left(s\right)X_a\left(s\right)+c_b\left(s\right)X_b\left(s\right)$

$=c_a\left(s\right)X_a\left(s\right)+c_b\left(s\right){\mathrm{\Φ}}_b{(k,s)}^{-1}{X}_b\left(k\right)$

$=\left[\begin{array}{cc}{c}_a\left(s\right)& c_b\left(s\right){\mathrm{\Φ}}_b{(k,s)}^{-1}\end{array}\right]\left[\begin{array}{c}{X}_a\left(s\right)\\ X_b\left(k\right)\end{array}\right]$ ；

$=C\left(k\right)X\left(k\right)$

其中：C(k)为T_k时刻系统量测矩阵，c_a(s)为T_s时刻发送艇系统量测矩阵，c_b(s)为T_s时刻接收艇系统量测矩阵；

$c_a\left(s\right)=\frac{\∂Z\left(s\right)}{\text{\∂}{X}_a{(s,s-1)}^T}=\left[\begin{array}{cc}1& 0\\ 0& 1\\ \frac{(x_{\mathrm{as}}-x_{\mathrm{bs}})}{r_s}& \frac{(y_{\mathrm{as}}-y_{\mathrm{bs}})}{r_s}\end{array}\right];$

式中：Z(s)为T_s时刻系统量测量；r_s为T_s时刻两艇间间距，上标T代表矩阵转置；

$c_b\left(s\right)=\frac{\∂Z\left(s\right)}{\∂X_b{(s,s-1)}^T}=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ \frac{(x_{\mathrm{bs}}-x_{\mathrm{as}})}{r_s}& \frac{(y_{\mathrm{bs}}-y_{\mathrm{as}})}{r_s}& 0\end{array}\right];$

步骤六、利用步骤一采集的相对定位信息以及步骤五获得的系统量测方程，根据公式：

$M=E\left\{\begin{array}{c}\end{array}\tilde{X}\left(s\right)\tilde{X}{(k,k-1)}^T\right\}$

$=P\left(s\right)\underset{i=1}{\overset{N-1}{\mathrm{\Π}}}\left[\mathrm{\Φ}{(s+i)}^T{(I-K(s+i+1)C(s+i+1))}^T\right]$

其中：

为T_s时刻系统状态估计误差；

为T_k时刻系统状态一步预测估计误差；P(s)为T_s时刻系统状态估计误差方差阵；Φ(s+i)^T为系统T_s+1时刻系统矩阵的转置；K(s+i+1)为系统T_s+i+1时刻滤波增益矩阵；C(s+i+1)为系统T_s+i+1时刻量测矩阵；

获得延时阶段基于不同时刻状态估计的均方误差矩阵M；

步骤七、利用步骤六获得的不同时刻状态估计均方误差矩阵M，根据公式：

K(k)＝M^TC(s)^T[C(s)P(s)C(s)^T]^-1

获得最小方差估计下的滤波增益矩阵K(k)；

其中：C(s)为T_s时刻系统量测矩阵；

步骤八、利用步骤七获得的最小方差估计下的滤波增益矩阵K(k)，根据公式：

P(k)＝P(k，k-1)-K(k)C(s)M

获得基于N个延时步长的系统状态均方误差矩阵P(k)；

其中：P(k，k-1)＝Φ(k，k-1)P(k)Φ^T(k，k-1)；

Φ(k，k-1)为系统T_k-1到T_k时刻系统矩阵；P(k)为系统T_k-1到T_k时刻系统状态估计误差方差阵，上标T代表矩阵转置；

步骤九、利用步骤五获得的系统基于N个延迟步长的量测方程，以及步骤七获得的最小方差估计下的滤波增益矩阵，采用公式：

${\hat{X}}_k=(I-K\left(k\right)C\left(k\right))\hat{X}(k,k-1)+K\left(k\right)Z\left(k\right)$

其中：为系统T_k-1到T_k时刻的状态估计；

对协同定位系统由于通信延时造成的定位误差进行估计补偿，进而实现通信延时补偿的接收艇定位；

$\hat{X}(k,k-1)={\left[\begin{array}{cc}{\hat{X}}_a{(s,s-1)}^T& {\hat{X}}_b{(k,k-1)}^T\end{array}\right]}^T;$

其中：为发送艇T_s-1到T_s时刻的状态估计；为接收艇T_k-1到T_k时刻的状态估计；

${\hat{X}}_a(s,s-1)=(1+\mathrm{\δt})\left(\begin{array}{c}{X}_{\mathrm{as}-1}\\ y_{\mathrm{as}-1}\end{array}\right);$

其中：x_as-1、y_as-1为发送艇T_s-1时刻相对于起始点的相对位置信息；

${\hat{X}}_b(k,k-1)={\mathrm{\Φ}}_b(k,k-1){\hat{X}}_b(k-1)$

其中：Φ_b(k，k-1)为系统T_k-1到T_k时刻系统矩阵；

为接受艇T_k-1时刻的状态估计。

2.根据权利要求1所述的一种基于通信延时补偿的多水面无人艇协同定位方法，具特征在于步骤六中获得的延时阶段基于不同时刻状态估计的均方误差矩阵M是基于Kalman滤波方程实现的；所述Kalman滤波方程为：

$X(k,k-1)=\underset{i=1}{\overset{N-1}{\mathrm{\Π}}}\left[(I-K(k-i)C(k-i))\mathrm{\Φ}(k-i)\right]X\left(s\right)$

其中：K(k-i)为系统T_k-i时刻滤波增益矩阵；C(k-i)为系统T_k-i时刻量测矩阵；Φ(k-i)为系统T_k-i时刻系统矩阵。

Images