0 引　言

在UUV进行水下作业时，需要时刻解算出自身的导航参数，高精度导航是UUV进行水下高可靠性复杂任务的前提^[1]。然而单一的传感器不能满足导航定位的精度需求，比如捷联惯性导航系统（Strapdown Inertial Navigation System, SINS）不受外界环境的影响，自主性强，能够提供较为全面的导航参数^[2]，但其存在一定的漂移误差，且这些误差会随着时间积累，因而需要同其他传感器组合导航进行误差的修正^[3]，从而获得更加准确和可靠的导航参数信息。

典型UUV组合导航系统采用多普勒测速仪（Doppler Velocity Logs, DVL）对速度信息进行修正，为了进一步保证导航的精度，还需对位置信息进行修正，针对这一问题，学者对于智能浮标辅助定位系统，进行了广泛研究。智能浮标辅助定位系统通常由一个浮标或多个浮标构成，多浮标定位系统通过距离交汇进行定位^[4]。相较于多浮标定位系统，单浮标辅助定位系统（Single-buoy-assisted Positioning System, Buoy）硬件复杂度低，Webster等^[5]利用单个移动的浮标，并使用同步时钟保证声学数据可单向传输测距，采用广播的形式保证了多个航行器同时接收声波信号，并通过真实的水下实验验证了设计的有效性。单浮标定位系统布局简单、灵活性高、便于操作，可以对UUV集群进行位置修正。由于海洋环境复杂，波浪和海风都会影响单浮标的位置和姿态，因而本文采用固定式单浮标，以期减少因浮标水面姿态和位置变化造成的定位误差。

在组合导航研究中，许多学者致力于融合算法，这些算法通常假设多个传感器的数据能够同步到达信息融合的处理中心。然而在实际工程应用中，存在时间不同步或量测滞后的情况。针对量测滞后问题，冷雪飞等^[6]利用数组记录滞后时间段内所有的INS状态信息，当存在量测信息时，利用所记录的状态信息和量测信息进行卡尔曼滤波处理，获得过去时刻对应的状态估计和均方误差阵，再利用状态转移矩阵进行时间的更新，推导出当前时刻的状态信息，对INS状态量进行修正。Pere等^[7]设定AUV和USBL的时钟同步，设计了一个有界的环形缓冲器来存储当前和过去时刻航行器的导航参数信息，来对所接收到的USBL位置信息进行查询投影，在对应的时刻进行量测更新，生成延迟的位置修正，所得到的修正信息前向传播到当前位置，进行量测滞后的补偿。杨波等^[8]对卫星导航输出信息进行曲线拟合，推导出当前卫星导航系统的输出值，有效解决了卫星导航输出系统的滞后问题，但是此方法仅适用于短时间延迟的补偿，对于较长时间间隔的延迟，曲线拟合便不能有效的补偿量测滞后。茹江涛等^[9]利用INS的导航信息将其他传感器的量测信息推导到当前时刻，利用所推导的信息进行量测更新，对INS进行修正，解决了在INS/SAR组合导航中，图像匹配信息滞后的问题。

针对传统水声定位系统在多成员UUV集群定位时，存在半双工以及更新频率低等问题，本文基于单浮标辅助定位系统实现UUV集群同步位置修正，实现高频率的水下定位，该辅助定位系统的优势在于：1）UUV成员只安装有水听器阵列，降低了UUV能耗，提升了集群作业的续航能力；2）采用单向无源定位的方式，UUV集群只需接收浮标声脉冲信号便可完成对自身位置的修正，保证了集群的隐蔽性；3）集群各成员可对自身位置进行同步定位，避免了集群各成员定位时声波之间的相互干扰，提升了集群的可拓展性，为大规模集群水下定位提供了一种新的解决方案。

1 SINS/DVL/Buoy组合导航模型

本文定义以下几个坐标系：$i$系为地心惯性坐标系，原点定义为地球中心，其中${O_i}{x_i}$指向春分点，不随地球自转，${O_i}{y_i}$轴在地球赤道平面内，${O_i}{z_i}$为地球自转轴；$e$系为地球坐标系，同地球固连，随地球自转，原点定义为地球中心，其中${O_e}{x_e}$指向本初子午线，${O_e}{y_e}$轴在地球赤道平面内，${O_e}{z_e}$为地球自转轴；选取$NED$坐标系作为导航坐标系$n$系，其中原点定义为水面浮标系统的中心，其中${O_n}N$指向北向，${O_n}E$指向东向，${O_n}D$垂直于当地旋转椭球指向地向；$p$系为虚拟数学平台坐标系，用来计算和$n$系之间的平台失准角偏差；选取前右下坐标系作为载体坐标系$b$系，原点定义为载体的中心，同载体固连，${O_b}{x_b}$指向载体的前向，${O_b}{y_b}$指向载体的右侧，$ {O_b}{z_b} $指向载体的下方，除此之外，还有涉及到水听器阵列的$u$系和多普勒坐标系$d$系，所应用到的坐标系如图1所示。

1.1 SINS误差状态方程

在组合导航中，选取SINS解算的姿态误差、速度误差、位置误差、陀螺仪零偏和加速度计零偏作为状态量，则SINS误差状态方程为：

$ {{\boldsymbol{\dot x}}_\mathrm{SINS}} = {{\boldsymbol{F}}_\mathrm{SINS}}{{\boldsymbol{x}}_\mathrm{SINS}} + {{\boldsymbol{G}}_\mathrm{SINS}}{{\boldsymbol{W}}_\mathrm{SINS}} 。$

(1)

其中：$ \boldsymbol{x}_{\mathrm{SINS}}=\left[ \begin{array}{*{20}{c}}(\boldsymbol{\varphi})^{\text{T}} & (\boldsymbol{\delta}\boldsymbol{v}^n)^{\text{T}} & (\boldsymbol{\delta}\boldsymbol{r}^n)^{\text{T}} & (\boldsymbol{b}_{\boldsymbol{g}})^{\text{T}} & (\boldsymbol{b}_{\boldsymbol{a}})^{\text{T}} \end{array}\right]^{\text{T}} $，$ {\boldsymbol{\varphi }} = {[\begin{array}{*{20}{c}} {{{\boldsymbol{\varphi }}_N}}&{{{\boldsymbol{\varphi }}_E}}&{{{\boldsymbol{\varphi }}_D}} \end{array}]^\mathrm{T}} $依次为$n$系3个方向的平台失准角，$ \boldsymbol{\delta}\boldsymbol{v}^n=[\begin{array}{*{20}{c}}\boldsymbol{\delta}\boldsymbol{v}_N & \boldsymbol{\delta}\boldsymbol{v}_E & \boldsymbol{\delta}\boldsymbol{v}_D\end{array}]\mathrm{^{\rm{T}}} $依次为$n$系3个方向的SINS速度误差，$ \boldsymbol{\delta}\boldsymbol{r}^n=[\begin{array}{*{20}{c}}\boldsymbol{\delta}r_N & \boldsymbol{\delta}r_E & \boldsymbol{\delta}r_D\end{array}]\mathrm{^{\rm{T}}} $依次为$n$系3个方向的SINS位置误差，$ \boldsymbol{b}_{\boldsymbol{g}}=[\begin{array}{*{20}{c}}\boldsymbol{b}_{\boldsymbol{gx}} & \boldsymbol{b}_{\boldsymbol{gy}} & \boldsymbol{b}_{\boldsymbol{gz}}\end{array}]\mathrm{^{\rm{T}}} $为3个轴向的陀螺仪零偏，$ \boldsymbol{b}_a=[\begin{array}{*{20}{c}}\boldsymbol{b}_{ax} & \boldsymbol{b}_{ay} & \boldsymbol{b}_{az}\end{array}]^{\mathrm{T}} $为3个轴向的加速度计零偏。

SINS误差模型状态转移矩阵^[10]为：

$ F_{\mathrm{SINS}}=\left[\begin{array}{*{20}{c}}\boldsymbol{M}_{aa} & \boldsymbol{M}_{av} & \boldsymbol{M}_{ap} & -\boldsymbol{C}_b^n & \boldsymbol{0}_{3\times3} \\ \boldsymbol{M}_{va} & \boldsymbol{M}_{vv} & \boldsymbol{M}_{vp} & \boldsymbol{0}_{3\times3} & \boldsymbol{C}_b^n \\ \boldsymbol{0}_{3\times3} & \boldsymbol{M}_{pv} & \boldsymbol{M}_{pp} & \boldsymbol{0}_{3\times3} & \boldsymbol{0}_{3\times3} \\ & & \boldsymbol{0}_{6\times15} & & \end{array}\right]。$

(2)

在进行组合导航仿真过程中，为了更直观表示状态转移矩阵，本文忽略了陀螺仪和加速度计的比例因子误差和交差耦合误差，选用零偏误差和测量噪声进行陀螺仪和加速度计的建模。其中零偏误差选用随机常数进行建模，测量噪声选用高斯白噪声进行建模，即：$\delta {\boldsymbol{\omega }}_{ib}^b = {{\boldsymbol{b}}_{\boldsymbol{g}}} + {{\boldsymbol{w}}_g}$，$\delta {{\boldsymbol{f}}^b} = {{\boldsymbol{b}}_{\boldsymbol{a}}}{\boldsymbol{ + }}{{\boldsymbol{w}}_{\boldsymbol{a}}}$。

$\left\{\begin{split} \boldsymbol{W}_{\mathrm{SINS}}= \left[\begin{array}{*{20}{c}}\boldsymbol{w}_g^b & \boldsymbol{w}_a^b\end{array}\right]^{\rm{T}}，\qquad\quad\\{{\boldsymbol{G}}_\mathrm{SINS}} = \left[ {\begin{array}{*{20}{c}} { - {\boldsymbol{C}}_b^n}& {{{\boldsymbol{0}}_{3 \times 3}}} \\ {{{\boldsymbol{0}}_{3 \times 3}}}& {{\boldsymbol{C}}_b^n} \\ {{{\boldsymbol{0}}_{3 \times 3}}}&{{{\boldsymbol{0}}_{3 \times 3}}} \\ {{{\boldsymbol{0}}_{3 \times 3}}}&{{{\boldsymbol{0}}_{3 \times 3}}} \\ {{{\boldsymbol{0}}_{3 \times 3}}}&{{{\boldsymbol{0}}_{3 \times 3}}} \end{array}} \right]。\qquad\end{split}\right.$

(3)

1.2 DVL误差状态方程及观测方程

设DVL自身测量坐标系为$d$系，同惯导坐标系($b$系)方向余弦为$C_d^b$，安装角误差为${{\boldsymbol{\eta }}_d}$，刻度因子误差为$\delta {k_d}$，则DVL误差状态量为$ \boldsymbol{x}_{\mathrm{DVL}}=\left[\begin{array}{*{20}{c}}\boldsymbol{\eta}_d & \delta k\end{array}\right]^{\text{T}} $，其状态误差模型为：

$ {{\boldsymbol{\dot x}}_\mathrm{DVL}} = {{\boldsymbol{F}}_\mathrm{DVL}}{{\boldsymbol{x}}_\mathrm{DVL}} + {{\boldsymbol{G}}_\mathrm{DVL}}{{\boldsymbol{W}}_\mathrm{DVL}} 。$

(4)

式中：$ {{\boldsymbol{\eta }}_d} = \left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{\eta }}_{dx}}}&{{{\boldsymbol{\eta }}_{dy}}}&{{{\boldsymbol{\eta }}_{dz}}} \end{array}} \right] $分别为$d$系在$b$系下的安装角误差。安装角误差和刻度因子误差用随机常数进行建模，即${{\boldsymbol{\dot x}}_\mathrm{DVL}} = 0$，因而DVL误差模型状态转移矩阵${{\boldsymbol{F}}_\mathrm{DVL}} = \left[ {{{\boldsymbol{0}}_{4 \times 4}}} \right]$，噪声分配矩阵${{\boldsymbol{G}}_\mathrm{DVL}} = \left[ {{{\boldsymbol{0}}_{4 \times 4}}} \right]$。

选取SINS实时解算速度$ {\tilde{ \boldsymbol v}}_\mathrm{SINS}^n $与DVL测量速度$ {\tilde{ \boldsymbol v}}_\mathrm{DVL}^n $的差值作为速度观测量^[11]：

$ \begin{aligned} {\boldsymbol{z}} = &\left[ {{\tilde{ \boldsymbol v}}_\mathrm{SINS}^n - {\tilde{ \boldsymbol v}}_\mathrm{DVL}^n} \right] = \left[ {{{\boldsymbol{v}}^n} + \delta {\boldsymbol{v}}_\mathrm{SINS}^n - {{\boldsymbol{v}}^n} - \delta {\boldsymbol{v}}_\mathrm{DVL}^n} \right] = \\ &\left[ {\delta {\boldsymbol{v}}_\mathrm{SINS}^n - \delta {\boldsymbol{v}}_\mathrm{DVL}^n} \right] 。\end{aligned}$

(5)

其中$ \delta {\boldsymbol{v}}_\mathrm{SINS}^n $为SINS误差状态量中速度误差$ {\boldsymbol{\delta }}{{\boldsymbol{v}}^n} $；$ \delta {\boldsymbol{v}}_\mathrm{DVL}^n $为DVL速度测量误差，表示为：

$ \delta {\boldsymbol{v}}_\mathrm{DVL}^n = \left( {{\boldsymbol{v}}_\mathrm{DVL}^n \times } \right){\boldsymbol{\varphi }} + {\boldsymbol{C}}_b^n\left( {{\boldsymbol{v}}_\mathrm{DVL}^b \times } \right){{\boldsymbol{\eta }}_{\boldsymbol{d}}} + \delta {k_d}{\boldsymbol{v}}_\mathrm{DVL}^n 。$

(6)

代入式(5)可以求得：

$ {\boldsymbol{z}} = \left[ {{\tilde{ \boldsymbol v}}_\mathrm{SINS}^n - {\tilde{ \boldsymbol v}}_\mathrm{DVL}^n} \right] = {{\boldsymbol{H}}_\mathrm{SINS/DVL}}{{\boldsymbol{x}}_\mathrm{SINS/DVL}} + {{\boldsymbol{V}}_\mathrm{SINS/DVL}} 。$

(7)

$ {{\boldsymbol{H}}_\mathrm{SINS/DVL}} $展开为：

$ {{{\boldsymbol{H}}_\mathrm{SINS/DVL}} = {\left[ {\begin{array}{*{20}{c}} { - \left( {{\boldsymbol{v}}_\mathrm{DVL}^n \times } \right)}& {{{\boldsymbol{I}}_{3 \times 3}}}& {{{\boldsymbol{0}}_{3 \times 9}}}& { - {\boldsymbol{C}}_b^n\left( {{\boldsymbol{v}}_\mathrm{DVL}^b \times } \right)} & { - {\boldsymbol{v}}_\mathrm{DVL}^n} \end{array}} \right]_{3 \times 19}}}。$

(8)

1.3 Buoy定位原理及误差模型

在UUV集群中，为了实现更高的导航定位精度，需对其位置误差进行修正。本文提出的单浮标辅助定位系统，安装有声学接收阵的UUV集群个体无需发射声信号，只接收来自单浮标发射的特定频率脉冲信号，即可解算自身的位置信息。UUV集群位置修正过程如图2所示。

1）初始化

设固定于水面的浮标绝对位置为$ \left[\begin{array}{*{20}{c}}L_\mathrm{T0} & \lambda_\mathrm{T0} & h_\mathrm{T0}\end{array}\right]\mathrm{^{\rm{T}}} $。UUV集群下水前首先与浮标进行时钟同步，并存储浮标位置。

2）UUV集群同步定位

完成时钟同步后，浮标周期性（如5 s）向水下广播特定频率声脉冲。若UUV位于该声脉冲有效传播范围内，则声学接收阵各单元均可以接收到此脉冲。

3）UUV集群位置信息解算

位置解算过程如下：①UUV与浮标时间同步，可根据到达水听器阵列的时间差计算与浮标的距离；②根据水听器阵列接收到声脉冲的相位差，可以计算出浮标相对于UUV的方位角；③根据存储的浮标位置可推算UUV的绝对位置。

UUV水听器阵列根据接收的声脉冲信号进行定位的原理如图3所示。

进行单浮标辅助定位时，设固定浮标到1号水听器的距离为$R$，则$R = c\tau $，其中$c$为水声速度，$\tau $为声脉冲从水面浮标到1号水听器的传输时间，设声波传输方向同${x_u}、{y_u}$轴的夹角分别为$\alpha、\beta $，则在$u$系下测得浮标的坐标为：

$ r_{UB}^u=\left[\begin{array}{*{20}{c}}x_B & y_B & z_B\end{array}\right]^{\mathrm{T}} $

(9)

式中：$B$为浮标；$U$为$ \mathrm{UUV} $；$ r_{UB}^u $表示浮标相对于$ \mathrm{UUV} $的位置在$u$系上的投影。

$ {{x_B} = R\cos \alpha } ；{{y_B} = R\cos \beta } ；{{z_B} = - \sqrt {{R^2} - x_B^2 - y_B^2} } ；$

所测得的脉冲声信号到达1号水听器和2号水听器的相位差为${\phi _{12}} = {\phi _2} - {\phi _1}$，到达1号水听器和3号水听器的相位差为${\phi _{13}} = {\phi _3} - {\phi _1}$，则：

$ \left\{ {\begin{aligned} &{{d_{12}}\cos \alpha = \frac{{{\phi _{12}}}}{{2{\text{π}} }}\lambda }，\\ & {{d_{13}}\cos \beta = \frac{{{\phi _{13}}}}{{2{\text{π}} }}\lambda }。\end{aligned}} \right. $

(10)

则可求得$\cos \alpha 、\cos \beta $分别为：

$ \left\{ {\begin{aligned} &{\cos \alpha = \frac{\lambda }{{2{\text{π}} {d_{12}}}}{\phi _{12}} = {k_{12}}{\phi _{12}}}，\\ &{\cos \beta = \frac{\lambda }{{2{\text{π}} {d_{13}}}}{\phi _{13}} = {k_{13}}{\phi _{13}}} 。\end{aligned}} \right. $

(11)

为了简化后续公式，使用${k_{12}},{k_{13}}$代替常量$ \displaystyle{\lambda }/{{2{\text{π}} {d_{12}}}} $和$ \displaystyle{\lambda }/{{2{\text{π}} {d_{13}}}} $，其中$\lambda $为声波在水下的波长，${d_{12}}、{d_{13}}$分别为1号水听器和2号水听器、3号水听器的距离。

综上可得：

$ \left\{ {\begin{aligned} &{{x_B} = c\tau {k_{12}}{\phi _{12}}} ，\\ &{{y_B} = c\tau {k_{13}}{\phi _{13}}} ，\\ &{{z_B} = - c\tau \sqrt {1 - \left( {k_{12}^2\phi _{12}^2 + k_{13}^2\phi _{13}^2} \right)} } 。\end{aligned}} \right. $

(12)

式中：$c$为声波在水下传播速度；${k_{12}}、{k_{13}}$为同声波长和水听器阵列布置相关的常量，测量值为声脉冲从浮标到1号水听器的传输时间${\tau _1}$和2个相位差${\phi _{12}},{\phi _{13}}$。

单浮标辅助定位系统的误差模型基于安装角误差和输出测量误差建立。如图4所示，安装在UUV上的水声接收器测量坐标系为$u$系，同$b$系的方向余弦为$C_u^b$，安装角误差为${{\boldsymbol{\eta }}_u}$，测量误差为$ \delta {\boldsymbol{r}}_{UB}^u $，$u$系的坐标原点相对于$b$系的位置杆臂为${{\boldsymbol{r}}^b}$，则根据图4 可计算浮标测量UUV位置的理想值在导航坐标系下投影${\boldsymbol{r}}_{OU}^n$为：

$ {\boldsymbol{r}}_{OU}^n = {\boldsymbol{r}}_{OB}^n - {\boldsymbol{C}}_b^n\left( {{\boldsymbol{C}}_u^b{\boldsymbol{r}}_{UB}^u + {{\boldsymbol{r}}^b}} \right)。$

(13)

考虑到安装角误差为$ {{\boldsymbol{\eta }}_u} = {\left[ {\begin{array}{*{20}{c}} {{\eta _{ux}}}&{{\eta _{uy}}}&{{\eta _{uz}}} \end{array}} \right]^{\rm{T}}} $，可认为安装相对准确，使用较小的常值误差进行建模，因而可以表示为$ {{\tilde {\boldsymbol{C}}}}_u^b = ({\boldsymbol{I}} - {{\boldsymbol{\eta }}_u} \times ){\boldsymbol{C}}_u^b $；考虑到输出测量误差$\delta {\boldsymbol{r}}_{UB}^u$，可认为测量误差为较小的常值误差，可表示成${\tilde{ \boldsymbol r}}_{UB}^u = {\boldsymbol{r}}_{UB}^u + \delta {\boldsymbol{r}}_{UB}^u$；考虑到在导航坐标系中的平台失准角误差，可表示成${\boldsymbol{C}}_b^{\hat n} = {\boldsymbol{C}}_n^{\hat n}{\boldsymbol{C}}_b^n = (I - {\boldsymbol{\varphi }} \times ){\boldsymbol{C}}_b^n$，则浮标对UUV位置的测量值${\tilde{ \boldsymbol r}}_{OU}^n$表示成：

$ {\tilde{ \boldsymbol r}}_{OU}^n = {\boldsymbol{r}}_{OB}^n - {\boldsymbol{C}}_b^{\hat n}\left( {{\tilde{ \boldsymbol C}}_u^b{\tilde{ \boldsymbol r}}_{UB}^u + {{\boldsymbol{r}}^b}} \right) + {{\boldsymbol{w}}_\mathrm{Buoy}}。$

(14)

将以上误差方程代入可得：

$\begin{aligned} {\tilde{ \boldsymbol r}}_{OU}^n &= {\boldsymbol{r}}_{OB}^n - {\boldsymbol{C}}_b^{\hat n}{\tilde{ \boldsymbol C}}_u^b{\tilde{ \boldsymbol r}}_{UB}^u = {\boldsymbol{r}}_{OB}^n - (I - {\boldsymbol{\varphi }} \times )\times \\ &{\boldsymbol{C}}_b^n\left[ {({\boldsymbol{I}} - {{\boldsymbol{\eta }}_{\boldsymbol{u}}} \times ){\boldsymbol{C}}_u^b({\boldsymbol{r}}_{UB}^u + \delta {\boldsymbol{r}}_{UB}^u) + {{\boldsymbol{r}}^b}} \right] + {{\boldsymbol{w}}_\mathrm{Buoy}}。\end{aligned}$

(15)

忽略二阶小量，则原式可以展开成：

$ \begin{split} {\tilde{ \boldsymbol r}}_{OU}^n = &{\boldsymbol{r}}_{OB}^n - {\boldsymbol{C}}_b^n{\boldsymbol{C}}_u^b{\boldsymbol{r}}_{UB}^u + \left( {{\boldsymbol{\varphi }} \times } \right){\boldsymbol{C}}_b^n{\boldsymbol{C}}_u^b{\boldsymbol{r}}_{UB}^u + {\boldsymbol{C}}_b^n({{\boldsymbol{\eta }}_{\boldsymbol{u}}} \times ){\boldsymbol{C}}_u^b{\boldsymbol{r}}_{UB}^u -\\ &{\boldsymbol{C}}_b^n{\boldsymbol{C}}_u^b\delta {\boldsymbol{r}}_{UB}^u - {\boldsymbol{C}}_b^n{{\boldsymbol{r}}^b} + \left( {{\boldsymbol{\varphi }} \times } \right){\boldsymbol{C}}_b^n{{\boldsymbol{r}}^b} + {{\boldsymbol{w}}_\mathrm{Buoy}}= \\ & {\boldsymbol{r}}_{OB}^n - {\boldsymbol{C}}_b^n{\boldsymbol{C}}_u^b{\boldsymbol{r}}_{UB}^u - {\boldsymbol{C}}_b^n{{\boldsymbol{r}}^b} + \left( {{\boldsymbol{\varphi }} \times } \right){\boldsymbol{r}}_{UB}^n + {\boldsymbol{C}}_b^n({{\boldsymbol{\eta }}_{\boldsymbol{u}}} \times ){\boldsymbol{r}}_{UB}^b - \\ &{\boldsymbol{C}}_b^n{\boldsymbol{C}}_u^b\delta {\boldsymbol{r}}_{UB}^u + \left( {{\boldsymbol{\varphi }} \times } \right){\boldsymbol{C}}_b^n{{\boldsymbol{r}}^b} + {{\boldsymbol{w}}_\mathrm{Buoy}} 。\\[-1pt] \end{split} $

(16)

则浮标定位误差模型为：

$ \begin{split} \delta {\boldsymbol{r}}_{OU}^n =& {\tilde{ \boldsymbol r}}_{OU}^n - {\boldsymbol{r}}_{OU}^n = - \left( {{\boldsymbol{r}}_{UB}^n \times } \right){\boldsymbol{\varphi }} - {\boldsymbol{C}}_b^n({\boldsymbol{r}}_{UB}^b \times ){{\boldsymbol{\eta }}_{\boldsymbol{u}}} - \\ &{\boldsymbol{C}}_b^n{\boldsymbol{C}}_u^b\delta {\boldsymbol{r}}_{UB}^u - \left( {{{\boldsymbol{r}}^n} \times } \right){\boldsymbol{\varphi }} + {{\boldsymbol{w}}_\mathrm{Buoy}} 。\end{split}$

(17)

1.4 Buoy误差状态方程及观测方程

设水下浮标的误差状态量为$ \boldsymbol{x}_{\mathrm{Buoy}}=\left[\begin{array}{*{20}{c}} \left(\boldsymbol{\eta}_u\right)^{\mathrm{T}} & \left(\delta\boldsymbol{r}_{UB}^u\right)\mathrm{^{\rm{T}} }\end{array}\right]^{\text{T}} $，则其误差状态方程为：

$ {{\boldsymbol{\dot x}}_\mathrm{Buoy}} = {{\boldsymbol{F}}_\mathrm{Buoy}}{{\boldsymbol{x}}_\mathrm{Buoy}} + {{\boldsymbol{G}}_\mathrm{Buoy}}{{\boldsymbol{W}}_\mathrm{Buoy}}。$

(18)

式中：$ {{\boldsymbol{\eta }}_u} = \left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{\eta }}_{ux}}}&{{{\boldsymbol{\eta }}_{uy}}}&{{{\boldsymbol{\eta }}_{uz}}} \end{array}} \right] $分别为水声接收器测量坐标系$u$系到SINS坐标系$b$系的3个方向的安装角误差，安装角误差和测量误差都选用随机常数进行建模，即${{\boldsymbol{\dot x}}_\mathrm{Buoy}} = 0$，因而浮标误差模型状态转移矩阵${{\boldsymbol{F}}_\mathrm{Buoy}} = \left[ {{{\boldsymbol{0}}_{6 \times 6}}} \right]$，${{\boldsymbol{G}}_\mathrm{Buoy}} = \left[ {{{\boldsymbol{0}}_{6 \times 6}}} \right]$。

选取SINS实时解算UUV的位置信息在导航坐标系下的投影$ {\tilde{ \boldsymbol p}}_\mathrm{SINS}^n $和单浮标系统测量得到UUV的位置信息在导航坐标系下的投影${\tilde{ \boldsymbol r}}_{OU}^n$差值作为观测量：

$ \begin{split}{\boldsymbol{z}} =& \left[ {{\tilde{ \boldsymbol p}}_\mathrm{SINS}^n - {\tilde{ \boldsymbol p}}_\mathrm{Buoy}^n} \right] = \left[ {{{\boldsymbol{r}}^n} + \delta {\boldsymbol{r}}_\mathrm{SINS}^n - {\boldsymbol{r}}_{OU}^n - \delta {\boldsymbol{r}}_{OU}^n} \right] =\\ &\left[ {\delta {\boldsymbol{r}}_\mathrm{SINS}^n - \delta {\boldsymbol{r}}_{OU}^n} \right] 。\end{split}$

(19)

已知$ \delta {\boldsymbol{r}}_{OU}^n = {\tilde{ \boldsymbol r}}_{OU}^n - {\boldsymbol{r}}_{OU}^n = - \left( {{\boldsymbol{r}}_{UB}^n \times } \right){\boldsymbol{\varphi }} - {\boldsymbol{C}}_b^n({\boldsymbol{r}}_{UB}^b \times ){{\boldsymbol{\eta }}_{\boldsymbol{u}}} - {\boldsymbol{C}}_b^n{\boldsymbol{C}}_u^b\delta {\boldsymbol{r}}_{UB}^u - \left( {{{\boldsymbol{r}}^n} \times } \right){\boldsymbol{\varphi }} + {{\boldsymbol{w}}_\mathrm{Buoy}} $，代入可求得$ {\boldsymbol{z}} = \left[ {{\tilde{ \boldsymbol p}}_\mathrm{SINS}^n - {\tilde{ \boldsymbol r}}_{OU}^n} \right] = {{\boldsymbol{H}}_\mathrm{SINS/BD}}{{\boldsymbol{x}}_\mathrm{SINS/BD}} + {{\boldsymbol{V}}_\mathrm{SINS/BD}} $，$ {{\boldsymbol{H}}_\mathrm{SINS/BD}} $展开形式为：

$ {\begin{split}&{{\boldsymbol{H}}_\mathrm{SINS/Buoy}} = \\ &{\left[ {\begin{array}{*{20}{c}} {\left( {{\boldsymbol{r}}_{UB}^n \times } \right) + \left( {{{\boldsymbol{r}}^n} \times } \right)}& {{{\boldsymbol{0}}_{3 \times 3}}}& {{{\boldsymbol{I}}_{3 \times 3}}}& {{{\boldsymbol{0}}_{3 \times 3}}}& {{{\boldsymbol{0}}_{3 \times 3}}} & {{\boldsymbol{C}}_b^n({\boldsymbol{r}}_{UB}^b \times )}& {{\boldsymbol{C}}_b^n{\boldsymbol{C}}_u^b} \end{array}} \right]_{3 \times 21}}。\\[-1pt]\end{split}} $

(20)

1.5 集中式卡尔曼滤波模型

在多传感器时间完全同步的理想状态下，SINS/Buoy/DVL组合导航的集中式滤波算法模型的状态方程为：

$ {{\boldsymbol{\dot x}}_{S/B/D}} = {{\boldsymbol{F}}_{S/B/D}}{{\boldsymbol{x}}_{S/B/D}} + {{\boldsymbol{G}}_{S/B/D}}{{\boldsymbol{W}}_{S/B/D}}。$

(21)

其中，状态变量为：

$ {{\boldsymbol{x}}_{S/B/D}} = \left[ {\begin{array}{*{20}{c}} {{{({{\boldsymbol{x}}_\mathrm{SINS}})}^{\text{T}}}}&{{{({{\boldsymbol{x}}_\mathrm{Buoy}})}^{\text{T}}}}&{{{({{\boldsymbol{x}}_\mathrm{DVL}})}^{\text{T}}}} \end{array}} \right]_{25 \times 1}^{\text{T}} 。$

(22)

展开为：

$ {{{\boldsymbol{x}}_{S/B/D}} = \left[ {\begin{array}{*{20}{c}} {{{({\boldsymbol{\varphi }})}^{\text{T}}}} & {{{({\boldsymbol{\delta }}{{\boldsymbol{v}}^n})}^{\text{T}}}} & {{{({\boldsymbol{\delta p}})}^{\text{T}}}} & {{{({{\boldsymbol{b}}_{\boldsymbol{g}}})}^{\text{T}}}} & {{{({{\boldsymbol{b}}_{\boldsymbol{a}}})}^{\text{T}}}} & {{{({{\boldsymbol{\eta }}_{\boldsymbol{u}}})}^{\text{T}}}} & {{{(\delta {\boldsymbol{r}}_{UB}^u)}^{\text{T}}}} & {{{({{\boldsymbol{\eta }}_{\boldsymbol{d}}})}^{\text{T}}}} & {\delta k} \end{array}} \right]_{25 \times 1}^{\text{T}}} 。$

(23)

状态转移矩阵${{\boldsymbol{F}}_{S/B/D}}$为：

$ {{\boldsymbol{F}}_{S/B/D}} = {\left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{F}}_\mathrm{SINS}}}&{{{\boldsymbol{0}}_{15 \times 6}}}&{{{\boldsymbol{0}}_{15 \times 4}}} \\ {{{\boldsymbol{0}}_{6 \times 15}}}&{{{\boldsymbol{F}}_\mathrm{Buoy}}}&{{{\boldsymbol{0}}_{6 \times 4}}} \\ {{{\boldsymbol{0}}_{4 \times 15}}}&{{{\boldsymbol{0}}_{6 \times 4}}}&{{{\boldsymbol{F}}_\mathrm{DVL}}} \end{array}} \right]_{25 \times 25}}。$

(24)

${{\boldsymbol{G}}_{S/B/D}}$和${{\boldsymbol{W}}_{S/B/D}}$为系统噪声相关矩阵。

量测方程为：

$ \begin{split}{{\boldsymbol{z}}_{S/B/D}} =& {\left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{z}}_{S/B}}} \\ {{{\boldsymbol{z}}_{S/D}}} \end{array}} \right]_{6 \times 1}} = \left[ {\begin{array}{*{20}{c}} {{\tilde{ \boldsymbol p}}_\mathrm{SINS}^n - {\tilde{ \boldsymbol r}}_{OU}^n} \\ {{\tilde{ \boldsymbol v}}_\mathrm{SINS}^n - {\tilde{ \boldsymbol v}}_\mathrm{DVL}^n} \end{array}} \right] =\\ &{{\boldsymbol{H}}_{S/B/D}}{{\boldsymbol{x}}_{S/B/D}} + {{\boldsymbol{V}}_{S/B/D}}。\end{split} $

(25)

量测矩阵$ {{\boldsymbol{H}}_{S/B/D}} $为：

$ {\begin{split}&{{\boldsymbol{H}}_{S/B/D}} = \\ &{\left[ {\begin{array}{*{20}{c}} {\left( {{\boldsymbol{r}}_{UB}^n \times } \right) + \left( {{{\boldsymbol{r}}^n} \times } \right)}& {{{\boldsymbol{0}}_{3 \times 3}}}& {{{\boldsymbol{I}}_{3 \times 3}}}& {{{\boldsymbol{0}}_{3 \times 3}}}& {{{\boldsymbol{0}}_{3 \times 3}}}& {{\boldsymbol{C}}_b^n({\boldsymbol{r}}_{UB}^b \times )}& {{\boldsymbol{C}}_b^n{\boldsymbol{C}}_d^b}& {{{\boldsymbol{0}}_{3 \times 3}}}& {{{\boldsymbol{0}}_{3 \times 1}}} \\ { - \left( {{\boldsymbol{v}}_\mathrm{DVL}^n \times } \right)}& {{{\boldsymbol{I}}_{3 \times 3}}}&{{{\boldsymbol{0}}_{3 \times 3}}} & {{{\boldsymbol{0}}_{3 \times 3}}}&{{{\boldsymbol{0}}_{3 \times 3}}} & {{{\boldsymbol{0}}_{3 \times 3}}}&{{{\boldsymbol{0}}_{3 \times 3}}} & { - {\boldsymbol{C}}_b^n\left( {{\boldsymbol{v}}_\mathrm{DVL}^b \times } \right)}& { - {\boldsymbol{v}}_{DVL}^n} \end{array}} \right]_{6 \times 25}}\end{split}}。$

(26)

$ {{\boldsymbol{V}}_{S/B/D}} $为观测噪声相关矩阵。

对式子进行离散化，可以得到理想状态下SINS/Buoy/DVL组合导航的集中式Kalman滤波算法模型为：

$ \left\{ {\begin{aligned} & {{{\boldsymbol{x}}_k} = {{\boldsymbol{\varPhi }}_{k/k - 1}}{{\boldsymbol{x}}_{k - 1}} + {{\boldsymbol{G}}_{k - 1}}{{\boldsymbol{w}}_{k{\text{ - }}1}}}，\\ &{{z_k} = {{\boldsymbol{H}}_k}{{\boldsymbol{x}}_k} + {{\boldsymbol{v}}_k}} 。\end{aligned}} \right. $

(27)

2 单浮标辅助定位随机时延补偿算法

在使用单浮标对UUV进行导航修正过程中，其采样周期间隔会随着航行器和浮标之间距离变化而变化，存在量测周期非均匀现象。除此之外，尽管声信号单向传播定位的方式可以有效避免声波双向传递造成的量测时延，但是为了保证三维定位的精度，在工程上常使用压力传感器对单浮标定位系统的深度信息进行修正，在解算和融合定位信息的过程中，仍存在一定的设备信号处理及信息传输延迟，此过程导致了输出的位置信息滞后于量测时刻，存在量测滞后现象，需设计相应的算法对其进行量测滞后的补偿。

如图5所示，浮标预设好的发射声波的时间节点为$ {t}_\mathrm{send }={t}_{n},{t}_{n+5},{t}_{n+10}\mathrm{......} $，UUV水听器阵列接受到声脉冲的时间点分别为$ {t}_\mathrm{receive }={t}_{n}+\Delta {t}_{1},{t}_{n+5}+\Delta {t}_{2},{t}_{n+10}+ \Delta {t}_{3}\mathrm{......} $，其中$\Delta t$为声波单向传递的时间，随着航行器与浮标距离变换而变化，安装在UUV上的水听器阵列根据声波单向传播时间和所测得的相位差进行UUV位置解算，并将其传输至UUV的导航处理中心，导航处理中心接收位置信息的时刻为：

$ t_{BD-\mathrm{Buoy}}=t_n+\Delta t_1+t_{\mathrm{delay}1},t_{n+5}+\Delta t_2+t_{\mathrm{delay}2}......。$

(28)

由于SINS更新频率最高，选择其作为滤波器更新的参照时间基准，SINS的更新时刻即为导航参数的输出时刻，为了更加直观表示，将UUV水听器阵列和导航处理中心接收时间也映射在滤波器的基准时间戳上，则UUV听器阵列接收信号时刻${t_n} + \Delta {t_1}$可表示为${t_a}$，落在$\left[ {{t_{k - i - 1}},{t_{k - i}}} \right]$周期中；UUV导航处理中心接收信号的时刻${t_n} + \Delta {t_1} + {t_{\mathrm{delay} - 1}}$可表示为${t_b}$，落在$\left[ {{t_{k - 1}},{t_k}} \right]$周期中，由于${t_n} + \Delta {t_1}$随着水听器与浮标的距离变化而不断变化，会存在同滤波基准时间戳不同步的情况。

首先需要对采样周期不均匀和时间不同步问题进行处理，在每个SINS更新周期内，寻找Buoy的更新时间节点，如果在某一更新周期内没有Buoy的量测数值，则仅进行SINS更新，即时间的更新；若在SINS的某个更新周期内，获取到Buoy位置量测值，且距离SINS周期末时间为$\Delta {t_b}$，则对此部分时间不同步信息进行外推。由于$\Delta {t_d}$最大值为SINS更新周期，一般来说，SINS输出频率较高，一个更新周期仅为0.01 s或0.02 s，因而可将此更新周期内的运动等效成匀加速运动，求取等效速度为$ {{\boldsymbol{v}}_\mathrm{equ}} = \left( {{{\boldsymbol{v}}_{k - i}} + {{\boldsymbol{v}}_{k - i - 1}}} \right)/2 $，将${t_a}$时刻的量测信息外推至${t_{k - i}}$，得到在滤波基准时间${t_{k - i}}$时刻的对应的量测值：

$ {\tilde{ \boldsymbol r}}_{OU}^n{|_{{t_{k - i}}}} = {\tilde{ \boldsymbol r}}_{OU}^n{|_{{t_a}}} + {{\boldsymbol{v}}_\mathrm{equ}}\left( {{t_{k - i}} - {t_a}} \right)。$

(29)

针对量测滞后问题，由上文分析可得延迟时间为${t_\mathrm{delay}}$，跨域多个SINS量测周期，在工程应用中，此段延迟时间可能会达到3 s甚至更长，如果使用位移等效外推的方式将${t_{k - i}}$时刻的量测值推导至${t_k}$，会产生较大的误差，不同于上文中一个SINS更新周期内的等效，不可将其简单等效成匀加速运动，为了将量测滞后信息同当前时刻的状态信息进行融合，本文采用基于时延的量测方程重构补偿算法，图6为其具体的实现原理。

经过时间的同步，获得$k - i$时刻的量测值$ {{\boldsymbol{z}}_{k - i}} = {\tilde{ \boldsymbol r}}_{OU}^n{|_{{t_{k - i}}}} = {\tilde{ \boldsymbol r}}_{OU}^n{|_{{t_a}}} + {{\boldsymbol{v}}_\mathrm{equ}}{t_{s1}} $，其对应$k - i$时刻的状态值为：

$ {z_{k - i}} = {H_{k - i}}{x_{k - i}} + {v_{k - i}} 。$

(30)

需获得$k - i$时刻量测值同$k$时刻状态值关系。此时，SINS时间更新已更新到$k$时刻，对其状态转移矩阵推导可得$k - i$时刻的状态量同$k$时刻状态量之间的关系如下：

$ \begin{split} {{\boldsymbol{x}}_k} = &{{\boldsymbol{\varPhi }}_{k/k - 1}}{{\boldsymbol{x}}_{k - 1}} + {{\boldsymbol{\varGamma }}_{k - 1}}{{\boldsymbol{w}}_{k{\text{ - }}1}} = \\ &{{\boldsymbol{\varPhi }}_{k/k - 1}}{{\boldsymbol{\varPhi }}_{k - 1/k - 2}}{{\boldsymbol{x}}_{k - 2}} + {{\boldsymbol{\varPhi }}_{k/k - 1}}{{\boldsymbol{\varGamma }}_{k - 2}}{{\boldsymbol{w}}_{k{\text{ - }}2}} + {{\boldsymbol{\varGamma }}_{k - 1}}{{\boldsymbol{w}}_{k{\text{ - }}1}} = \\ &{{\boldsymbol{\varPhi }}_{k/k - 1}}...{{\boldsymbol{\varPhi }}_{k - i + 1/k - i}}{{\boldsymbol{x}}_{k - i}} + ... + {{\boldsymbol{\varGamma }}_{k - 1}}{{\boldsymbol{w}}_{k{\text{ - }}1}}= \\ & ... =\\ &{{\boldsymbol{\varPhi }}_{k/k - i}}{{\boldsymbol{x}}_{k - i}} + {{\boldsymbol{\varPhi }}_{k/k - i + 1}}{{\boldsymbol{\varGamma }}_{k - i}}{{\boldsymbol{w}}_{k{\text{ - }}i}} + ... + \\ &{{\boldsymbol{\varPhi }}_{k/k - 1}}{{\boldsymbol{\varGamma }}_{k - 2}}{{\boldsymbol{w}}_{k{\text{ - }}2}} + {{\boldsymbol{\varGamma }}_{k - 1}}{{\boldsymbol{w}}_{k{\text{ - }}1}}。\\[-1pt] \end{split} $

(31)

其中：

$ {{\boldsymbol{\varPhi }}_{k/k - i}} = {{\boldsymbol{\varPhi }}_{k/k - 1}}{{\boldsymbol{\varPhi }}_{k - 1/k - 2}}...{{\boldsymbol{\varPhi }}_{k - i + 1/k - i}} = \prod\limits_{j = 0}^{i - 1} {{{\boldsymbol{\varPhi }}_{k - j/k - j - 1}}} 。$

(32)

则将变换后可得：

$\begin{split} & {x_{k - i}} = {\boldsymbol{\varPhi }}_{k/k - i}^{ - 1}{{\boldsymbol{x}}_k} - {\boldsymbol{\varPhi }}_{k/k - i}^{ - 1} \times \\ &({{\boldsymbol{\varPhi }}_{k/k - i + 1}}{{\boldsymbol{\varGamma }}_{k - i}}{{\boldsymbol{w}}_{k{\text{ - }}i}} + ... + {{\boldsymbol{\varPhi }}_{k/k - 1}}{{\boldsymbol{\varGamma }}_{k - 2}}{{\boldsymbol{w}}_{k{\text{ - }}2}} + {{\boldsymbol{\varGamma }}_{k - 1}}{{\boldsymbol{w}}_{k{\text{ - }}1}})。\\ \end{split}$

(33)

则将其代入量测方程中，可得：

$ {\begin{split} &{z_{k - i}} = {H_{k - i}}{\boldsymbol{\varPhi }}_{k/k - i}^{ - 1}{{\boldsymbol{x}}_k} - {H_{k - i}}{\boldsymbol{\varPhi }}_{k/k - i}^{ - 1}\\ &({{\boldsymbol{\varPhi }}_{k/k - i + 1}}{{\boldsymbol{\varGamma }}_{k - i}}{{\boldsymbol{w}}_{k{\text{ - }}i}} + ... + {{\boldsymbol{\varPhi }}_{k/k - 1}}{{\boldsymbol{\varGamma }}_{k - 2}}{{\boldsymbol{w}}_{k{\text{ - }}2}} + {{\boldsymbol{\varGamma }}_{k - 1}}{{\boldsymbol{w}}_{k{\text{ - }}1}}) + {v_{k - i}}= \\ & {H_{k - i}}{\boldsymbol{\varPhi }}_{k/k - i}^{ - 1}{{\boldsymbol{x}}_k} + {{\bar v}_{k - i}} 。\\[-15pt] \end{split} }$

(34)

经过时间同步和量测滞后补偿，基于时延的卡尔曼滤波模型为：

$ \left\{\begin{array}{*{20}{l}} {{{\boldsymbol{x}}_k} = {{\boldsymbol{\varPhi }}_{k/k - 1}}{{\boldsymbol{x}}_{k - 1}} + {{\boldsymbol{\varGamma }}_{k - 1}}{{\boldsymbol{w}}_{k{\text{ - }}1}}} ，\\ {{z_{k - i}} = {H_{k - i}}{\boldsymbol{\varPhi }}_{k/k - i}^{ - 1}{{\boldsymbol{x}}_k} + {{\bar v}_{k - i}}} 。\end{array} \right.$

(35)

对于单浮标辅助定位系统的随机时延，先进行时间同步，再进行基于时延的量测方程重构，可根据延时的量测信息实时对当前时刻进行估计，则基于时延的卡尔曼滤波模型的状态更新和量测更新步骤类似于式(31)，使用$ {H_{k - i}}{\boldsymbol{\varPhi }}_{k/k - i}^{ - 1} $代替${{\boldsymbol{H}}_k}$，使用${{\boldsymbol{\bar R}}_k}$代替$ {v_{k - i}} $的协方差矩阵${{\boldsymbol{R}}_k}$，如果时延为0，该滤波算法即为经典的卡尔曼滤波。

3 仿真与验证

为了验证导航定位算法准确性，对其仿真模拟。设定仿真验证总时间为912.5 s，运动轨迹包括了UUV加速、下潜、匀速直线和左右转运动。设定3个方向的初始姿态角为$ [\begin{array}{*{20}{c}}0 & 0 & 0\end{array}]^{\mathrm{T}} $，(°)；初始的速度为$ [\begin{array}{*{20}{c}}0 & 0 & 0\end{array}]^{\mathrm{T}}，\mathrm{m}/\mathrm{s} $；初始的纬度为$ 31.0209^\circ $，初始的经度为$ 121.4290^\circ $；导航参数初始速度误差为$ \delta\boldsymbol{v}^n= [\begin{array}{*{20}{c}}0.2 & 0.1 & 0\end{array}]^{\mathrm{T}}，\mathrm{m}/\mathrm{s} $；陀螺常值零偏为${0.01^{\text{°}}}/\mathrm h$；角度随机游走为$ 0.001^{\text{°}}/\sqrt{\mathrm{h}} $；加速度计零偏误差为$100\;{\mathrm{ug}}$；速度随机游走为$ 10\;{\mathrm{ug}}/\sqrt{\mathrm{Hz}} $；压力传感器采用一阶马尔科夫过程进行仿真模拟，其中压力模拟输出的标准差为1，一阶马尔可夫过程的相关时间为10 s，零输入输出的偏置为0.8 m；DVL的三轴安装误差分别为$ [\begin{array}{*{20}{c}}0.8 & 0.5 & 0.2\end{array}]^{\mathrm{T}}$，(°)；刻度因子误差为0.05；单浮标的三轴安装误差为$ [\begin{array}{*{20}{c}}0.5 & 0.4 & 0.2\end{array}]\mathrm{^{\rm{T}}} $，(°)；测量误差$ \delta {\boldsymbol{r}}_{UB}^u $为$ [\begin{array}{*{20}{c}}0.02 & 0.02 & 0.04\end{array}]^{\mathrm{T}} $；设定SINS输出频率为50 Hz，DVL输出频率为1 Hz；Buoy输出频率为0.02 Hz；压力传感器输出频率为50 Hz。

3.1 理想条件下集中式卡尔曼滤波效果验证

对理想条件下纯惯导仿真（图示中表示成SINS）、惯导和DVL组合导航（图示中表示为SINS/DVL）、惯导、多普勒测速仪和单浮标辅助定位组合导航（图示中表示为SINS/DVL/Buoy）进行对比验证，以验证所设计单浮标辅助定位和组合导航算法的有效性。图7和图8分别为预设轨迹、SINS、SINS/DVL、SINS/DVL/Buoy三维轨迹仿真对比图和北东导航坐标系下轨迹仿真对比，图9和图10分别为导航坐标系下北向和东向速度误差对比和位置对比。

可知，相较于SINS仿真输出和SINS/DVL组合导航，单浮标辅助定位系统的引入，可以有效的减低了速度和位置误差，验证了本系统和所涉及集中式卡尔曼滤波模型的有效性。

3.2 异步信息融合卡尔曼滤波器验证

为了验证在时间异步情况和存在量测滞后的情况下，所设计补偿算法的准确性，初始导航参数同上，其中DVL初始采样时刻比SINS滞后0.01 s，Buoy存在1.2 s的固定信号处理时延和0.5×rand(1)的随机时延，分别对时间对齐的理想情况、时间异步未补偿、时间异步位移等效补偿、本文所提出的考虑时间异步和量测滞后的补偿算法进行对比仿真，其中图11和图12为三维轨迹仿真对比图示和北东导航坐标系下轨迹仿真对比，图13和图14分别为导航坐标系下北向和东向速度误差对比和位置对比。

为了进一步验证算法性能，对误差进行定量分析，设定位置误差（Position Error, PE），位置均方根误差（Position Root Mean Square Error, P-RMSE），速度误差（Velocity Error, PE），速度均方根误差（Velocity Root Mean Square Error, V-RMSE）为以下形式：

$ \mathrm{PE}(k) = \sqrt {{{\left( {\tilde p_k^N - p_k^N} \right)}^2} + {{\left( {\tilde p_k^E - p_k^E} \right)}^2}}，$

(36)

$ \mathrm{P} - \mathrm{RMSE} = \sqrt {\frac{1}{N}\sum\limits_{k = 1}^N {\left( {{{\left( {\tilde p_k^N - p_k^N} \right)}^2} + {{\left( {\tilde p_k^E - p_k^E} \right)}^2}} \right)} } ，$

(37)

$ \mathrm{VE}(k) = \sqrt {{{\left( {\tilde v_k^N - v_k^N} \right)}^2} + {{\left( {\tilde v_k^E - v_k^E} \right)}^2}}，$

(38)

$ \mathrm{V} - \mathrm {RMSE} = \sqrt {\frac{1}{N}\sum\limits_{k = 1}^N {\left( {{{\left( {\tilde v_k^N - v_k^N} \right)}^2} + {{\left( {\tilde v_k^E - v_k^E} \right)}^2}} \right)} } 。$

(39)

式中：$ \tilde p_k^N $、$ \tilde p_k^E $为北向和东向组合导航位置解算值；$ p_k^N $、$ p_k^E $为北向和东向预设轨迹；$ \tilde v_k^N $、$ \tilde v_k^E $为北向和东向组合导航速度解算值；$ v_k^N $、$ v_k^E $为北向和东向预设速度。

则位置误差（PE）和速度误差（AE）如图15和图16所示。

通过表1可知，本文采用的补偿算法在位置均方根误差（P-RMSE）和速度均方根误差（V-RMSE）方面的性能表现分别为13.7406 m和0.2177 m/s。相较于未应用补偿时，P-RMSE和V-RMSE分别减少了63.36%和33.32%。值得注意的是，本文提出的补偿算法表现出与无时延、时间同步的理想条件下极为接近的性能。这表明补偿算法能够有效地应对时间异步和量测滞后问题。

由仿真轨迹对比示意图和误差对比示意图可以看出，当存在随机时延时，未进行补偿处理的组合导航轨迹会产生较大的速度误差和位置误差，明显偏离理想轨迹，不能满足高精度导航的需要，本文所设计的基于随机时延的量测矩阵重构补偿算法，可以有效地减少速度和位置误差，同无时延的理想情况误差相差很小，有效地消除了量测滞后的影响，保证了系统的高精度导航性能。

4 结　语

为了应对UUV采用传统USBL辅助定位时存在的更新频率低、相互干扰以及水声信号时延等挑战，本文提出了单浮标辅助定位系统。构建了该系统的误差状态模型，并建立了集中式卡尔曼滤波系统，为了解决工程应用中多传感器信息融合中的时间异步和由于硬件计算引起的量测滞后问题，设计了一种基于时间异步和量测滞后的量测方程重构卡尔曼滤波算法。该算法能够有效进行时间同步补偿，将滞后的量测巧妙地应用于当前状态的融合估计。设计了仿真实验验证，结果表明，该方法能够充分利用随机时延信息，降低了补偿时延的计算成本，保证了组合导航系统的精度和实时性。此外，采用无源的方式，保证了集群UUV的隐蔽性。接下来将进行基于单浮标辅助定位的集群UUV湖上实验验证，将固定浮标固定于岸边或浮台，在每个UUV上安装水听器阵列，同传统USBL辅助定位精度进行对比，进一步验证单浮标辅助定位在集群UUV导航定位中的优势。