0 引　言

舰艇编队是现代水面载体海上活动的主要形式之一。舰艇编队组成一个行动的整体，需准确确定编队成员本身位置信息，以及相互之间的位置关系。根据目标需要进行协调一致的编队机动，才能使编队的行动达到快速、灵活，更好地遂行各种任务行动，充分发挥技术器材的作用，完成各项工作^{[1 − 4]}。

舰艇编队导航信息是舰艇编队遂行任务的基础信息。强导航对抗环境可能导致单舰或多舰导航失能。舰艇编队需利用现有技术手段，保持编队整体绝对和相对定位能力。协同导航技术为该条件下舰艇编队有效导航提供了支撑^{[5 − 6]}。

由于舰艇编队协同导航系统是将不同节点之间、不同精度的导航信息进行信息融合，从而获得强鲁棒性的导航信息。因此，数据融合算法是一个核心问题。扩展卡尔曼滤波（Extended Kalman Filter, EKF）算法已广泛应用到协同导航问题解算当中，并取得了一定成效，但由于其存在线性假设，在处理强非线性问题中存在不足之处^[7]。区别于EKF，UKF（Unscented Kalman Filter, 无迹卡尔曼滤波）不需要线性化系统模型，而是使用UT（Unscented Transformation）变换的方法，用一组确定的采样点近似估计后验概率。对于非线性系统，UKF的估计解算精度要优于EKF^[8]。然而，由于UKF的解算需要计算多个采样点来获得状态变量的统计特性，从而其计算过程显得更加复杂^[9]。针对上述问题，因子图和积算法可被用来解决协同导航定位问题。因子图和积算法把全局函数分解成局部函数，可以有效地减少系统计算复杂度和非线性误差。

目前，舰艇编队惯性基协同导航的研究相对较少^[1]，因子图和积算法在舰艇编队协同导航中的应用少有相关文献及报道。张玉鹏等^[10]构建了无人艇协同导航因子图模型，利用改进型滑动窗优化的方法提高协同定位性能，但并未开展实际通信测距试验验证。李倩等^[11]针对室内行人INS/UWB紧组合定位系统，利用因子图和积算法构建数学模型，提高协同定位精度。Ben等^[12]为消除未知洋流扰动对多无人自主水下航行器（Autonomous Underwater Vehicle，AUV）协同导航的影响，采用参数和积算法实现对导航信息和洋流速度的同步估计。但AUV导航设备及参数输出特点、编队通信组网测距方式、工作环境与舰艇编队存在差异。

本文利用编队各舰搭载的不同精度惯性导航基准，开展协同导航技术研究，提出了舰艇编队协同导航模型，设计了因子图和积协同导航算法，开展了利用数据链通信测距的相关试验验证与分析。

1 舰艇编队协同导航模型

主从式舰艇编队协同导航定位系统由主舰艇与从舰艇组成^{[10, 13]}。本文设定主舰艇装备高精度惯性基准，从舰艇装备低精度惯性基准，故主舰艇定位精度高于从舰艇。主舰艇与从舰艇利用数据链设备测量舰艇间距离，并进行舰艇间导航信息交换。从舰艇根据自身惯性基准定位信息、主舰艇定位信息与舰艇间距离信息对自身位置进行舰艇间协同导航校正，从而提高其定位精度。

1.1 运动学模型

对于舰艇等水面运载体来说，一般只分析其在水平面内的二维定位信息，故根据惯性基准当地地理坐标系机械编排，有：

$ {{\boldsymbol{f}}^t} = {\boldsymbol{\dot v}}_e^t + \left( {2{\boldsymbol{\omega }}_{ie}^t + {\boldsymbol{\omega }}_{et}^t} \right) \times {\boldsymbol{v}}_e^t - {{\boldsymbol{g}}^t}。$

(1)

式中：$ {{\boldsymbol{f}}^t} = {\left[ {{f_x},{f_y}} \right]^{\mathrm{T}}} $为加速度计输出比力矢量在当地地理坐标系沿东向与北向的投影；$ {\boldsymbol{v}}_e^t = {\left[ {{v_x},{v_y}} \right]^{\mathrm{T}}} $为舰艇相对地球的运动速度在当地地理坐标系沿东向与北向的投影；$ {\boldsymbol{\dot v}}_e^t = {\left[ {{{\dot v}_x},{{\dot v}_y}} \right]^{\mathrm{T}}} $为舰艇相对地球的运动加速度在当地地理坐标系沿东向与北向的投影；$ {\boldsymbol{\omega }}_{ie}^t $为地球坐标系相对惯性空间旋转角速度；$ {\boldsymbol{\omega }}_{et}^t $为当地地理坐标系相对地球坐标系的旋转角速度；$ {{\boldsymbol{g}}^t} $为当地重力加速度矢量。

惯性基准导航解算，通过对加速度计输出比力矢量信息进行有害加速度补偿后，得到舰艇在当地地理坐标系内运动加速度信息，再通过积分运算求得舰艇运动速度：

$ \left\{ \begin{gathered} {v_{x,k + 1}} = {v_{x,k}} + \int_{kT}^{(k + 1)T} {{{\dot v}_{x,k}}} {\mathrm{d}}t ，\\ {v_{y,k + 1}} = {v_{y,k}} + \int_{kT}^{(k + 1)T} {{{\dot v}_{y,k}}} {\mathrm{d}}t 。\\ \end{gathered} \right. $

(2)

式中：T为采样周期。

设舰艇经纬度位置信息为(φ, λ)，其中φ、λ分别为舰艇的纬度和经度，由舰艇惯性基准设备输出。建立惯性基准设备位置更新方程：

$ \left\{ {\begin{array}{*{20}{l}} {{\varphi _{k + 1}} = {\varphi _k} + \int_{kT}^{(k + 1)T} {\dfrac{{{v_{y,k}}}}{{{R_M}}}} \cdot {\mathrm{d}}t}，\\ {{\lambda _{k + 1}} = {\lambda _k} + \int_{kT}^{(k + 1)T} {\dfrac{{{v_{x,k}}}}{{{R_N} \cdot \cos {\varphi _k}}}} \cdot {\mathrm{d}}t} 。\end{array}} \right. $

(3)

式中：${R_M} $为地球参考椭球子午圈曲率半径，$ {R_M} = {{a \cdot \left( {1 - {e^2}} \right)} \mathord{\left/ {\vphantom {{a \cdot \left( {1 - {e^2}} \right)} {{{\left( {1 - {e^2} \cdot {{\sin }^2}{\varphi _k}} \right)}^{3/2}}}}} \right. } {{{\left( {1 - {e^2} \cdot {{\sin }^2}{\varphi _k}} \right)}^{3/2}}}} $；$e $为地球参考椭球偏心率，$e = {{\sqrt {{a^2} - {b^2}} } \mathord{\left/ {\vphantom {{\sqrt {{a^2} - {b^2}} } a}} \right. } a}$；a为地球参考椭球长轴半径，b为地球参考椭球短轴半径。${R_N} $为地球参考椭球卯酉圈曲率半径，$ {R_N} = {{a \mathord{\left/ {\vphantom {a {\left( {1 - {e^2}{{\sin }^2}\varphi } \right)}}} \right. } {\left( {1 - {e^2}{{\sin }^2}\varphi } \right)}}^{1/2}} $。

由于系统采样周期T较短，故本文将式(2)和式(3)进行线性化处理。把惯性基准输出的纬度φ、经度λ等信息，通过弧度近似的方法转换为平面直角坐标系中的位置信息。以地理坐标系中的舰艇二维位置信息(x, y)、东向速度v_x、北向速度v_y为系统状态变量，表示为${{\boldsymbol{X}}_k} = {\left[ {{x_k},{y_k},{v_{x,k}},{v_{y,k}}} \right]^{\mathrm{T}}}$。以舰艇东向加速度、北向加速度构成系统控制输入，即${{\boldsymbol{u}}_k} = {\left[ {{{\dot v}_{x,k}},{{\dot v}_{y,k}}} \right]^{\mathrm{T}}}$，系统状态方程可表示为：

$ {{\boldsymbol{X}}_{{{k}} + 1}} = {\boldsymbol{F}}{{\boldsymbol{X}}_k} + {\boldsymbol{G}}{{\boldsymbol{u}}_k} 。$

(4)

式中：F为状态转移矩阵，可表示为

$ {\boldsymbol{F}} = \left[ {\begin{array}{*{20}{c}} {\begin{array}{*{20}{c}} 1 \\ 0 \\ 0 \\ 0 \end{array}}&{\begin{array}{*{20}{c}} 0 \\ 1 \\ 0 \\ 0 \end{array}}&{\begin{array}{*{20}{c}} T \\ 0 \\ 1 \\ 0 \end{array}}&{\begin{array}{*{20}{c}} 0 \\ T \\ 0 \\ 1 \end{array}} \end{array}} \right] 。$

(5)

G为输入驱动矩阵。${{\boldsymbol{u}}_k} = {{\boldsymbol{u}}_{m,k}} + {{\boldsymbol{w}}_k}$，${{\boldsymbol{u}}_{m,k}}$为$k$时刻真实控制输入，${{\boldsymbol{w}}_k}$为$k$时刻系统输入噪声，包括东向加速度噪声和北向加速度噪声，假设其为高斯白噪声。

1.2 观测模型

相对观测信息引入了编队主从舰艇之间导航状态的函数关系。因此，相对观测量的处理方法是协同导航的核心问题。这与绝对观测在概念上不同。假定零均值与白噪声统计的重复性绝对观测，意味着每次观测都包含新的信息，因此，对应状态的估计误差将趋近于0。相比之下，每次相对观测均不包含完全新的信息，主从舰艇2个状态之间的相对量观测会导致估计不确定性的有限减少，可实现的效果将取决于主从舰艇状态的初始不确定性以及状态之间的观测映射。舰艇编队可获得的相对观测量主要包括相对距离、相对位置与方位等信息，本论文仅考虑相对距离观测信息。

主舰艇i和从舰艇j之间的相对距离观测将会与相应载体的位置状态${{\boldsymbol{p}}_i}$和${{\boldsymbol{p}}_j}$有关，可表述为：

$ {{\boldsymbol{z}}_k} = \left\| {{{\boldsymbol{p}}_{i,k}} - {{\boldsymbol{p}}_{j,k}}} \right\| + {\boldsymbol{w}}_k^r。$

(6)

式中：$ {{\boldsymbol{z}}_k} $为k时刻主舰艇i与从舰艇j之间的距离观测值；每个载体位置状态${{\boldsymbol{p}}_{i,k}}$和${{\boldsymbol{p}}_{j,k}}$为载体完整状态向量的子集；${\boldsymbol{w}}_k^r$为相对距离观测噪声。

2 因子图和积算法

本文采用因子图和积算法进行消息传递。从变量节点向因子节点传递的消息，是与该变量节点直接相连的所有因子节点（除去该因子节点）传递消息的乘积。从因子节点向变量节点（除去该变量节点）传递的消息，是与该因子节点直接相连的所有变量节点传递给该因子的消息与该因子节点的乘积，再对乘积结果求和^{[14 − 16]}。

本文将舰艇编队运动平台因子图模型上的所有消息都约束为高斯参数化消息，以降低计算量与舰艇平台之间的通信量。基于因子图和积算法构建舰艇编队协同导航模型消息传递形式，利用因子图中二次消息更新过程，估计从舰艇导航信息后验概率密度。利用弧度近似的方法，将舰艇惯性基准输出的经纬度信息，转换到舰艇所在点的地理坐标系当中，以获得二维平面直角坐标系x轴和y轴坐标。将二维位置坐标构建为相互独立的变量，以便舰艇二维位置作为相互独立的位置标量分别进行建模。

构建的舰艇编队惯性基协同导航因子图和积算法消息传递流程，如图1所示。正方形为因子节点，圆形为变量节点。实线箭头代表首次消息更新方向，虚线箭头代表二次消息更新方向。

以下从数学公式的角度，针对舰艇编队惯性基协同导航，利用因子图模型进行消息传递过程推导求解。其迭代求解主要包括首次消息更新和二次消息更新2个过程。

2.1 首次消息更新

步骤1　将舰艇编队运动平台因子图模型变量节点消息初始化。

$\begin{aligned} &\nu_{x,0}\sim N(\mu_{\nu_{x,0}},\sigma_{\nu_{x,0}}^2),\nu_{y,0}\sim N(\mu_{\nu_{y,0}},\sigma_{\nu_{y,0}}^2),\\& x_0\sim N(\mu_{x_0},\sigma_{x_0}^2), y_0\sim N(\mu_{y_0},\sigma_{y_0}^2)。\end{aligned}$

步骤2　进行因子图模型时间更新。由变量节点$ {v_{x,k}} $传递给因子节点$ {A_{k + 1}} $的概率密度函数：

$ {I_{{v_{x,k}} \to {A_{k + 1}}}}\left( {{v_{x,k}}} \right) = N\left( {{\mu _{{v_{x,k}}}},\sigma _{{v_{x,k}}}^2} \right)。$

(7)

由变量节点$ {v_{y,k}} $传递给因子节点$ {B_{k + 1}} $进行消息传递的概率密度函数可表示为：

$ {I_{{v_{y,k}} \to {B_{k + 1}}}}\left( {{v_{y,k}}} \right) = N\left( {{\mu _{{v_{y,k}}}},\sigma _{{v_{y,k}}}^2} \right)。$

(8)

由变量节点$ {x_k} $传递给因子节点$ {C_{k + 1}} $的概率密度函数：

$ {I_{{x_k} \to {C_{k + 1}}}}\left( {{x_k}} \right) = N\left( {{\mu _{{x_k}}},\sigma _{{x_k}}^2} \right)。$

(9)

由变量节点$ {y_k} $传递给因子节点$ {D_{k + 1}} $的概率密度函数：

$ {I_{{y_k} \to {D_{k + 1}}}}\left( {{y_k}} \right) = N\left( {{\mu _{{y_k}}},\sigma _{{y_k}}^2} \right) 。$

(10)

由因子节点$ {A_{k + 1}} $传递给变量节点$ {v_{x,k + 1}} $的概率密度函数：

$ \begin{split} {I_{{A_{k + 1}} \to v_{x,k + 1}^ - }}\left( {v_{x,k + 1}^ - } \right)= & \sum\limits_{\sim{v_{x,k + 1}}} {{A_{k + 1}}{I_{{v_{x,k}} \to {A_{k + 1}}}}\left( {{v_{x,k}}} \right)}= \\& N\left( {{\mu _{v_{x,k + 1}^ - }},\sigma _{v_{x,k + 1}^ - }^2} \right)。\end{split} $

(11)

式中：变量右上角“−”号代表先验信息。$ {\mu _{v_{x,k + 1}^ - }} $和$ \sigma _{v_{x,k + 1}^ - }^2 $可通过下式计算：

$ \left\{ \begin{gathered} {\mu _{v_{x,k + 1}^ - }} = {\mu _{{v_{x,k}}}} + {\mu _{{{\dot v}_{x,k + 1}}}} \cdot t, \\ \sigma _{v_{x,k + 1}^ - }^2 = \sigma _{{v_{x,k}}}^2 + \sigma _{{{\dot v}_{x,k + 1}}}^2 \cdot {t^2}。\\ \end{gathered} \right. $

(12)

由因子节点$ {B_{k + 1}} $传递给变量节点$ {v_{y,k + 1}} $的概率密度函数：

$ \begin{split} {I_{{B_{k + 1}} \to v_{y,k + 1}^ - }}\left( {v_{y,k + 1}^ - } \right) = & \sum\limits_{\sim{v_{y,k + 1}}} {{B_{k + 1}}{I_{{v_{y,k}} \to {I_{k + 1}}}}\left( {{v_{y,k}}} \right)}= \\& N\left( {{\mu _{v_{y,k + 1}^ - }},\sigma _{v_{y,k + 1}^ - }^2} \right)。\end{split} $

(13)

$ {\mu _{v_{y,k + 1}^ - }} $和$ \sigma _{v_{y,k + 1}^ - }^2 $可通过下式计算：

$ \left\{ \begin{gathered} {\mu _{v_{y,k + 1}^ - }} = {\mu _{{v_{y,k}}}} + {\mu _{{{\dot v}_{y,k + 1}}}} \cdot t, \\ \sigma _{v_{y,k + 1}^ - }^2 = \sigma _{{v_{y,k}}}^2 + \sigma _{{{\dot v}_{y,k + 1}}}^2 \cdot {t^2}。\\ \end{gathered} \right. $

(14)

基于和积算法，由变量节点$ v_{x,k + 1}^ - $传递给因子节点$ {C_{k + 1}} $的概率密度函数：

$\begin{split} {I_{{A_{k + 1}} \to v_{x,k + 1}^ - }}&\left( {v_{x,k + 1}^ - } \right) = {I_{v_{x,k + 1}^ - \to {C_{k + 1}}}}\left( {v_{x,k + 1}^ - } \right) =\\& N\left( {{\mu _{v_{x,k + 1}^ - }},\sigma _{v_{x,k + 1}^ - }^2} \right)。\end{split}$

(15)

由变量节点$ v_{y,k + 1}^ - $传递给因子节点$ {D_{k + 1}} $的概率密度函数：

$ {I_{{I_{k + 1}} \to v_{y,k + 1}^ - }}\left( {v_{y,k + 1}^ - } \right) = {I_{v_{y,k + 1}^ - \to {D_{k + 1}}}}\left( {v_{y,k + 1}^ - } \right) = N\left( {{\mu _{v_{y,k + 1}^ - }},\sigma _{v_{y,k + 1}^ - }^2} \right)。$

(16)

由因子节点$ {C_{k + 1}} $传递给变量节点$ {x_{k + 1}} $的概率密度函数：

$ \begin{split} I_{C_{k+1} \rightarrow \bar{x}_{k+1}}\left(\bar{x}_{k+1}^-\right) = & \sum_{\sim x\bar{k}+1} C_{k+1} I_{\bar{v}_{x,k+1} \rightarrow C_{k+1}}\left(v_{x,k+1}^-\right) \\&I_{x_k \rightarrow C_{k+1}}\left(x_k\right) = N\left(\mu_{\bar{x}_{k+1}}, \sigma_{\bar{x}_{k+1}}^2\right)。\end{split} $

(17)

由因子节点$ {D_{k + 1}} $传递给变量节点$ {y_{k + 1}} $的概率密度函数：

$ \begin{split} &{I_{{D_{k + 1}} \to y_{k + 1}^ - }}\left( {y_{k + 1}^ - } \right) = \\& \sum\limits_{\sim y_{k + 1}^ - } {{D_{k + 1}}{I_{v_{y.k + 1}^ - \to {D_{k + 1}}}}\left( {v_{y,k + 1}^ - } \right){I_{{y_k} \to {D_{k + 1}}}}\left( {{y_k}} \right)} = \\& N\left( {{\mu _{y_{k + 1}^ - }},\sigma _{y_{k + 1}^ - }^2} \right) 。\end{split} $

(18)

基于上述推导，可以得到先验信息$( x_{k + 1}^ - ,y_{k + 1}^ - , v_{x,k + 1}^ - ,v_{y,k + 1}^ - )$的概率密度函数。

步骤3　进行因子图模型测量更新。

由变量节点${x_{k + 1}}$传递给因子节点${G_{k + 1}}$的概率密度函数：

$ {I_{x_{k + 1}^ - \to {G_{k + 1}}}}\left( {x_{k + 1}^ - } \right) = N\left( {{\mu _{x_{k + 1}^ - }},\sigma _{x_{k + 1}^ - }^2} \right) 。$

(19)

由因子节点${G_{k + 1}}$传递给变量节点$\Delta {x_{k + 1}}$的概率密度函数：

$ \begin{split} &{I_{{G_{k + 1}} \to \Delta {x_{k + 1}}}}\left( {\Delta {x_{k + 1}}} \right) = \sum\limits_{\sim\Delta {x_{k + 1}}} {{G_{k + 1}}{I_{x_{k + 1}^ - \to {G_{k + 1}}}}(x_{k + 1}^ - )} {\text{ = }}\\&\quad N\left( {{\mu _{\Delta {x_{k + 1}}}},\sigma _{\Delta {x_{k + 1}}}^2} \right) = N\left( {{\mu _{x_{k + 1}^m}} - {\mu _{x_{k + 1}^ - }},\sigma _{x_{k + 1}^ - }^2} \right) 。\end{split}$

(20)

量测信息${z_{k + 1}}$通过因子节点${Q_{k + 1}}$进入因子图模型中参与消息传递，由变量节点${z_{k + 1}}$传递给因子节点${Q_{k + 1}}$的概率密度函数：

$ {I_{{z_{k + 1}} \to {Q_{k + 1}}}}\left( {{z_{k + 1}}} \right) = N\left( {{\mu _{{z_{k + 1}}}},\sigma _{{z_{k + 1}}}^2} \right)。$

(21)

由因子节点${Q_{k + 1}}$传递给变量节点${\hat z_{k + 1}}$的概率密度函数：

$ \begin{split} {I_{{Q_{k + 1}} \to {{\hat z}_{k + 1}}}}\left( {{{\hat z}_{k + 1}}} \right) =& \sum\limits_{\sim{{\hat z}_{k + 1}}} {{Q_{k + 1}}{I_{{z_{k + 1}} \to {Q_{k + 1}}}}\left( {{z_{k + 1}}} \right)} =\\&\quad N\left( {{\mu _{{{\hat z}_{k + 1}}}},\sigma _{{{\hat z}_{k + 1}}}^2} \right)。\end{split} $

(22)

根据和积算法，由变量节点${\hat z_{k + 1}}$传递给因子节点$ {P_{k + 1}} $的概率密度函数：

${ {I_{{{\hat z}_{k + 1}} \to {P_{k + 1}}}}\left( {{{\hat z}_{k + 1}}} \right) = {I_{{Q_{k + 1}} \to {{\hat z}_{k + 1}}}}\left( {{{\hat z}_{k + 1}}} \right) = N\left( {{\mu _{{{\hat z}_{k + 1}}}},\sigma _{{{\hat z}_{k + 1}}}^2} \right)。}$

(23)

由因子节点${P_{k + 1}}$传递给变量节点$\Delta {y_{k + 1}}$的概率密度函数：

$\begin{split} & {I_{{P_{k + 1}} \to \Delta {{\hat y}_{k + 1}}}}\left( {\Delta {{\hat y}_{k + 1}}} \right) = \\ &\quad \sum\limits_{\sim\Delta \hat y_{k + 1}^i} {P_{k + 1}^i} {I_{\Delta x_{k + 1}^i \to P_{k + 1}^i}}(\Delta x_{k + 1}^i){I_{\hat z_{k + 1}^i \to P_{k + 1}^i}}(\hat z_{k + 1}^i) = \\ &\quad N\left( {{\mu _{\Delta {{\hat y}_{k + 1}}}},\sigma _{\Delta {{\hat y}_{k + 1}}}^2} \right)。\end{split} $

(24)

由变量节点$\Delta {y_{k + 1}}$传递给因子节点${H_{k + 1}}$的概率密度函数：

$ {I_{{P_{k + 1}} \to \Delta {y_{k + 1}}}} \left( {\Delta {y_{k + 1}}} \right) = {I_{\Delta {y_{k + 1}} \to {H_{k + 1}}}} \left( {\Delta {y_{k + 1}}} \right) = N \left( {{\mu _{\Delta {{\hat y}_{k + 1}}}},\sigma _{\Delta {{\hat y}_{k + 1}}}^2} \right)。$

(25)

由因子节点${H_{k + 1}}$传递给变量节点${y_{k + 1}}$的概率密度函数：

$ \begin{split} & {I_{{H_{k + 1}} \to {y_{k + 1}}}}\left( {{y_{k + 1}}} \right) = \sum\limits_{\sim{y_{k + 1}}} {{H_{k + 1}}{I_{\Delta {y_{k + 1}} \to {H_{k + 1}}}}\left( {\Delta {y_{k + 1}}} \right)} = \\& N\left( {{\mu _{{{\hat y}_{k + 1}}}},\sigma _{{{\hat y}_{k + 1}}}^2} \right) = N\left( {{\mu _{y_{k + 1}^m}} - {\mu _{\Delta {{\hat y}_{k + 1}}}},\sigma _{\Delta {{\hat y}_{k + 1}}}^2} \right) 。\end{split} $

(26)

2.2 二次消息更新

以首次消息传递得到的变量概率密度估计$ N\left( {{\mu _{{{\hat y}_{k + 1}}}},\sigma _{{{\hat y}_{k + 1}}}^2} \right) $与先验概率密度估计$ N\left( {{\mu _{y_{k + 1}^ - }},\sigma _{y_{k + 1}^ - }^2} \right) $进行融合作为二次消息更新的初值$ N\left( {{\mu _{{y_{k + 1,2}}}},\sigma _{{y_{k + 1,2}}}^2} \right) $，下标‘2’表示二次消息更新。

$ \left\{ \begin{gathered} {\mu _{{y_{k + 1,2}}}} = \sigma _{{y_{k + 1,2}}}^2\left( {\frac{{{\mu _{y_{k + 1}^ - }}}}{{\sigma _{y_{k + 1}^ - }^2}} + \frac{{{\mu _{{{\hat y}_{k + 1}}}}}}{{\sigma _{{{\hat y}_{k + 1}}}^2}}} \right), \\ \sigma _{{y_{k + 1,2}}}^2 = \frac{1}{{\frac{1}{{\sigma _{y_{k + 1}^ - }^2}} + \frac{1}{{\sigma _{{{\hat y}_{k + 1}}}^2}}}} 。\\ \end{gathered} \right. $

(27)

以先验信息$ N\left( {{\mu _{v_{x,k + 1}^ - }},\sigma _{v_{x,k + 1}^ - }^2} \right) $和概率密度估计$ N\left( {{\mu _{{{\hat v}_{x,k}}}},\sigma _{{{\hat v}_{x,k}}}^2} \right) $进行消息融合即可得到后验概率密度估计$ N\left( {{\mu _{{v_{x,k + 1}}}},\sigma _{{v_{x,k + 1}}}^2} \right) $。

$ \left\{ \begin{gathered} {\mu _{{v_{x,k + 1}}}} = \sigma _{{v_{x,k + 1}}}^2\left( {\frac{{{\mu _{v_{x,k + 1}^ - }}}}{{\sigma _{v_{x,k + 1}^ - }^2}} + \frac{{{\mu _{{{\hat v}_{x,k + 1}}}}}}{{\sigma _{{{\hat v}_{x,k + 1}}}^2}}} \right) , \\ \sigma _{{v_{x,k + 1}}}^2 = \frac{1}{{\frac{1}{{\sigma _{v_{x,k + 1}^ - }^2}} + \frac{1}{{\sigma _{{{\hat v}_{x,k + 1}}}^2}}}}。\\ \end{gathered} \right. $

(28)

同理，可得${v_{y,k + 1}}$后验概率密度估计$ N( {\mu _{{v_{y,k + 1}}}}, \sigma _{{v_{y,k + 1}}}^2 ) $。

根据上述舰艇编队协同导航定位系统和积算法模型内各节点消息传递，可获得状态变量后验概率密度估计，实现协同导航。

3 试验分析

为了验证协同导航算法的有效性，以舰艇编队为研究对象，开展了协同导航陆基车载试验。由2辆试验车模拟主从舰艇编队，一辆搭载高精度惯性基准的车辆载体模拟主舰艇，以下简称主舰艇；一辆搭载较低精度惯性基准的车辆载体模拟从舰艇，以下简称从舰艇。

主舰艇与从舰艇均搭载卫星导航定位设备用于提供位置基准信息。主舰艇装备有高精度的某型光纤惯性基准，其试验参数为，陀螺零偏稳定性≤0.01°/h，加速度计偏值≤1×10⁻⁴g₀。从舰艇采用低精度的某型光纤惯性基准参数：陀螺零偏稳定性≤0.05°/h，加速度计偏值≤5×10⁻³g₀。试验过程中，主舰艇惯性基准工作在组合导航模式下，从舰艇惯性基准工作在纯惯性导航模式下。

利用扩展卡尔曼滤波协同导航算法与因子图和积协同导航算法进行协同导航定位。主从舰艇之间采用数据链进行导航数据通信并相互测距，数据链测距误差RMS为57.97 m，其测距误差最大值为114.97 m。试验车与试验设备搭载情况，如图2所示。

为模拟舰艇编队实际工作情况，主从舰艇惯性基准在初始对准启动完成后经过一定时间，再通过数据链组网组成舰艇编队协同导航工作模式。试验进行总时间为890 s，主从舰艇在航行过程中，采取不定向、不定速的工作模式，彼此协同运动。为保证主从舰艇数据链通信畅通、测距精度有效，两者之间保持一定的通信和测距距离，通信天线之间信号无遮挡。

主舰艇与从舰艇航行在同一条路径上，真实轨迹十分接近。从舰艇真实轨迹及不同算法下的估计轨迹如图3所示。从舰艇惯性基准在纯惯性导航模式下定位误差，以及不同算法下定位误差如图4所示。

可以看出，在无协同导航辅助定位的情况下，从舰艇惯性基准纯惯性解算航迹与真实航迹偏差较大，而在使用了扩展卡尔曼滤波算法、因子图和积协同导航算法之后，从舰艇估计航迹可保持在真实航迹附近，误差偏差较小。在协同导航情况下，从舰艇真实东速、扩展卡尔曼滤波东速及和积算法东速比对情况，如图5所示；从舰艇真实北速、扩展卡尔曼滤波北速及和积算法北速比对情况，如图6所示。

定位误差采用均方根（Root Mean Square, RMS）值形式描述。利用算法估算舰艇经纬度值和舰艇真实经纬度值，求取舰艇估计位置与真实位置之间的距离，将该距离定义为定位误差。舰艇协同导航定位方式下，从舰艇采用扩展卡尔曼滤波算法、因子图和积算法获得的位置（经纬度）、北向速度、东向速度值，与真实值作比较，计算出定位误差、北向速度估计误差和东向速度估计误差，如表1所示。

从舰艇惯性基准自主工作模式下定位误差RMS为5.56×10³ m。从舰艇的扩展卡尔曼滤波算法估计误差与因子图和积算法估计误差均小于从舰艇纯惯性解算定位误差。从舰艇采用相关滤波算法，扩展卡尔曼滤波算法估计位置产生的定位误差降低至纯惯性解算定位误差的1.64%，扩展卡尔曼滤波算法定位精度相比纯惯性解算定位精度提高98.36%；因子图和积算法估计位置产生的定位误差降低至纯惯性解算定位误差的1.37%，因子图和积算法定位精度相比纯惯性解算定位精度提高98.63%。因子图和积算法定位精度相比扩展卡尔曼滤波算法提高16.69%。因子图和积算法北向速度精度、东向速度精度相比扩展卡尔曼滤波算法分别提高34.94%、48.90%。

4 结　语

舰艇编队协同导航定位是基于数据融合处理技术的导航方法，是解决强导航干扰环境下舰艇编队自主导航的有效手段之一。本文构建了舰艇编队惯性基协同导航定位数学模型，利用数据链组网测距信息，设计了基于编队惯性基信息的因子图和积协同定位算法；利用二次消息传递更新过程，估计导航信息后验概率密度。以一主一从式舰艇编队为研究对象，利用车载试验模拟编队实船环境，进行了试验验证分析。试验结果表明，舰艇编队协同导航定位后，从舰艇导航定位精度相比惯性基准纯惯性与扩展卡尔曼滤波解算定位均有明显提高；验证了基于和积算法的惯性基协同导航定位方法的有效性。研究结论为解决强导航干扰环境下舰艇编队自主导航提供了有效手段。根据船载设备研制先陆上功能性试验，再海上试验的原则，后期将进一步开展实船验证性试验。