CN113048983B

CN113048983B - 一种异时序贯量测的改进分层式auv协同导航定位方法

Info

Publication number: CN113048983B
Application number: CN202110332573.4A
Authority: CN
Inventors: 黄浩乾; 韩亦鸣; 李光辉; 田亚威; 郑文豪; 孙世安
Original assignee: Hohai University HHU
Current assignee: Hohai University HHU
Priority date: 2021-03-29
Filing date: 2021-03-29
Publication date: 2023-12-26
Anticipated expiration: 2041-03-29
Also published as: CN113048983A

Abstract

本发明公开了一种异时序贯量测的改进分层式AUV(Autonomous Underwater Vehicle)协同导航定位方法，通过分层式结构的信息共享和传递，并基于移动长基线方法，通过改进的异时序贯滤波方法对异时量测信息加以处理，解决了时钟不同步的问题，以及并行滤波由于水声信号延迟所产生的定位误差问题；采用了优化的扩展卡尔曼滤波方法EKF融合内外部导航信息，并以改进的多传感器数据融合导航系统对导航过程中所得到的姿态，速度，航向信息进行融合解算，反馈到上位机软件，实现了各传感器数据的在线融合，进而对低精度层AUV进行相关状态估计更新，通过分层式编队构形实现了不同精度AUV信息的共享和传递。

Description

一种异时序贯量测的改进分层式AUV协同导航定位方法

技术领域

本发明涉及水下航行器的导航定位方法，尤其涉及一种异时序贯量测的改进分层式AUV(Autonomous Underwater Vehicle)协同导航定位方法。

背景技术

AUV是当今海洋工程领域的热点研究方向，通过合理的编队构形结构，进行AUV之间的高精度协同导航是多AUV水下导航系统的基础。通过多个单体进行信息的共享，使得所有AUV都具有定位能力，增加了整个系统的鲁棒性。当某个AUV受外界因素影响导致导航能力丧失的时候，通过协同导航使这些艇的导航能力得到一定程度的恢复。目前，协同导航的主要结构分为并行式导航结构、主从式协同导航结构和分层式导航。并行式导航的结构单一，通过利用邻近的AUV进行相对位置观测，但由于所有AUV都要求装配高精度装备，成本高昂，目前很难在实际作业中使用。

在主从式协同导航中，主AUV携带高精度导航系统，从AUV携带低精度导航系统，二者利用水声通信装置建立主从之间的通信连接，主AUV给从AUV发送信号，进行信息间的共享。

而分层式导航采用分层式结构，AUV层次分明，将AUV按照导航精度高低分层，同层之间的导航精度相当，处于高精度层的AUV向低精度层的AUV传递导航信息。在此过程中，由于参与构建长基线导航定位系统的各高精度层AUV与低精度层的AUV之间的距离在不同位置不相等，后者在使用问答器发出询问信号后再收到应答信号的时间间隔存在不同程度上的差异，这就导致了时钟不同步的问题。在此期间，需要定位的AUV的航行位置一直在发生改变，即不断发生姿态改变的、需要定位的AUV在发出询问信号后，就不断地接收着来自于分布在高低精度层AUV的应答信号，进而导致了并行滤波在该过程中的不适用性，造成了误差的累积，不利于AUV长时间航行。

因此，如何通过具体方法使低精度层的AUV在水声通信条件下，有效利用高精度层的AUV提供的位置量测信息，从而提高整个导航系统的导航精度，是亟需解决的技术问题。

发明内容

发明目的：针对以上技术问题，本发明提出一种异时序贯量测的改进分层式AUV协同导航定位方法，通过改进的异时序贯量测方法解决时钟不同步的问题，采用扩展卡尔曼滤波方法融合内外部导航信息，并采用多传感器数据融合导航系统实现各传感器数据的在线融合，实现了不同精度AUV之间的信息的共享和传递。

技术方案：本发明异时序贯量测的改进分层式AUV协同导航定位方法包括以下步骤：

步骤(1)，通过优化分层式协同导航编队的结构来进行AUV协同导航，低精度层的AUV_q通过水声信号测量计算出与高精度层AUV_p之间的相对距离和方位角；同时，低精度层AUV_q接收高精度层AUV_p反馈的自身位置信息；

步骤(2)，以移动长基线定位方法对被定位的AUV进行航位推算，得出被定位的AUV的量测模型；

步骤(3)，采用异时序贯量测方法获得被定位AUV的俯仰角、横滚角以及加速度信息，解决并行滤波在移动长基线系统中产生的定位误差问题；

步骤(4)，通过序贯扩展卡尔曼滤波方法对系统状态进行更新；

步骤(5)，通过多传感器数据融合导航系统进行多传感器数据融合解算以进行导航；

步骤(6)，采用多传感器数据融合系统对滤波进行去噪处理。

步骤(2)中，该被定位的AUV的量测模型推算过程如下：

被定位的AUV对其自身位姿进行推算，推算方程为：

x_k+1＝x_k+x_k+1＝x_k+T.V_k.cos(φ_k)

y_k+1＝y_k+y_k+1＝y_k+T.V_k.sin(φ_k)

φ_k+1＝φ_k (1)

其中，T为被定位AUV的采样周期，V_k为k时刻的本体传感器所得的速度，为k时刻的本体传感器所得的航偏角；

令而/>且相互独立；

由此，被定位的AUV的量测模型为：

其中，为k+1时刻两AUV之间的声波传输距离，X_k为k时刻被定位的AUV的位移姿态，Γ(u_k+w_k，T)为非线性项，w_k为过程噪声，/>步骤(4)中，低精度层AUV接收高精度层AUV的位置信息，通过扩展卡尔曼滤波方法融合内部和外部信息完成定位的过程如下：

设AUV_p的坐标为：(x_p(k)，y_p(k)，z_p(k))，AUV_q的坐标为：(x_q(k)，y_q(k)，z_q(k))，由AUV_p与AUV_q之间的几何关系得以下表达式：

AUV_p与AUV_q之间的相对方位角：

AUV_p与AUV_q之间的水平方向距离：

采用扩展卡尔曼滤波方法对系统进行状态更新的步骤如下：

使用传感器量测其被定位的AUV的更新状态，得到被定位的AUV在时刻的状态估计量为/>其中，/>为状态估计量；被定位的AUV₄接收到应答信号的时刻为/>之后通过预测协方差和/>时刻传感器的量测更新系统状态；

时刻系统的预测协方差为：/>其中，/>为状态转移矩阵：

过程噪声分布矩阵的公式如下：

步骤(3)中，采用异时序贯量测方法解决低精度AUV定位误差的步骤如下：

(31)在水下，按照预定的时间周期，在t₀时刻，被定位的低精度AUV发送询问信号，安装应答器的各个高精度AUV接收到询问信号，以不同的频率应答，被定位的低精度AUV接收应答信号后，根据传统长基线确定与安装应答器的高精度层AUV的相对位置关系，这个过程在t₁时刻前完成；

(32)t₀到t₂时刻，搭载应答器的高精度AUV发送水声信息至被定位的低精度AUV，所述水声信息包含高精度AUV各自应答询问信号时刻的位置状态；

(33)被定位的低精度AUV根据解算出的与应答器的相对位置几何关系，以及接收到的高精度AUV位置信息，根据移动长基线方法更新自身位置信息。其中，步骤(32)中，该位置状态包括时间、经度、维度、深度、航向和姿态。

步骤(5)中，通过多传感器数据融合导航系统对航向姿态信息进行融合解算的步骤为：

(51)AUV搭载GPS在水上进行GPS定位，发送精地理坐标位置，进行初步定位；

(52)通过水下磁辅助系统与惯性导航单元收集磁信号在水下进行AUV姿态角、航向角、速度以及位置信息的收集；

(53)水下摄像头进行视觉信号的收集与处理，进行近距离的水下成像，为AUV的避障、近距离导航动作提供环境预判；

(54)多传感器数据融合导航系统融合各传感器信息并将惯性导航系统与水下摄像头进行在线融合，结合GPS信息，进行分析解算，多传感器数据融合导航系统中的融合板在线处理图像数据；

(55)通过上位机软件对收集到的导航信息进行融合解算，将所解算出的数据传送到高精度层AUV中进行信息的交互，进而完成协同过程的导航信息的解算工作。工作原理：本发明通过分层式结构的信息共享和传递，并基于移动长基线方法，通过改进的异时序贯滤波方法对异时量测的信息加以处理，解决了时钟不同步的问题，以及并行滤波由于水声信号延迟所产生的定位误差问题；采用了优化的扩展卡尔曼滤波方法EKF融合内外部导航信息，并以改进的多传感器数据融合导航系统对导航过程中所得到的姿态，速度，航向信息进行融合解算，反馈到上位机软件，实现了各传感器数据的在线融合，进而对低精度层AUV进行相关状态估计更新，通过分层式编队构形实现了不同精度AUV之间的信息的共享和传递。

其中，本发明采用异时序贯量测方法的构思思路为：由于参与构建长基线导航定位系统的各高精度层AUV与低精度层的AUV之间的距离远近不等，被定位的低精度层AUV在使用问答器发出询问信号后，再收到应答信号的时间间隔存在不同程度上的差异。与此同时，需要定位的AUV的航行位置则一直发生改变，即不断发生位姿改变的AUV在发出询问信号后就不断地接收来自位于不同位置的AUV的应答信号。而并行滤波的方法忽略了这一点，即并行滤波假设被定位AUV同时接收到应答信号，这与实际情况是相悖的。而本发明采用序贯滤波的方法对异时量测信息加以处理，弥补了此缺陷。

有益效果：与现有技术相比，本发明具有以下优点：

(1)本发明对现有的基于长基线的协同导航方法加以改进，无需高难度计算，分层式编队中的高精度层与低精度层分布有序，实现了同精度层间导航信息的共享与融合，不同精度AUV之间信息的共享和传递，满足了协同导航的要求。

(2)抛开常规的并行式滤波算法，由于时钟同步性假设在建构长基线系统的各大型AUV与被定位的AUV之间不成立，因此本发明采用序贯滤波的方法代替并行式滤波方法，对异时量测信息加以处理，解决了时钟不同步导致的在移动长基线系统中产生的误差问题。

(3)本发明通过改进的多传感器数据融合导航系统对导航过程中所得到的姿态、速度、航向信息进行融合解算，提高了导航精度和系统的鲁棒性，实现了AUV信息间的交互与融合，为进行协同导航的AUV提供了精确的导航信息。

附图说明

图1为本发明异时序贯量测的改进分层式AUV协同导航定位方法的流程图；

图2为本发明改进的分层式协同导航编队构型图；

图3为本发明改进的异时序贯量测的原理图；

图4为本发明采用扩展卡尔曼滤波方法对系统进行更新的原理图；

图5为本发明异时序贯量测解决低精度AUV定位误差的原理图。

具体实施方式

如图1所示，本发明异时序贯量测的改进分层式AUV协同导航定位方法包括以下步骤：

(1)提出分层式协同导航编队结构，进行AUV协同导航，为异时序贯滤波方法提供计算平台；在导航过程中，通过优化分层式协同导航编队的结构，对协同导航编队的队形进行优化，具体如图2所示；

(2)通过移动长基线(Moving Long Baseline，LBL)方法对原有的长基线(LongBaseline，LBL)方法进行改进，并以移动长基线方法进行航位推算，推算出被定位AUV的量测模型；

(3)采用异时序贯量测方法解决并行滤波在移动长基线系统中，由于水声信号延迟所产生的定位误差问题；

(4)通过序贯扩展卡尔曼滤波方法对系统状态进行更新；

(5)通过改进的多传感器数据融合导航系统，操作装置的上位机软件对导航所得的航向姿态信息进行融合解算以进行导航；

(6)采用多传感器数据融合系统对滤波进行去噪处理，该导航系统融合了各传感器信息，将惯性导航与水下成像进行在线匹配；并结合GPS信息，进行分析解算，融合板在线处理图像数据，实现实时处理的效果。

本发明技术方案的具体实施过程如下：

如图1所示，本发明异时序贯量测的改进分层式AUV协同导航定位方法中，首先，进行AUV协同导航，在导航过程中，通过优化分层式协同导航编队的结构，进行AUV之间的协同定位作业，进而实现同精度层之间的信息融合以及高低精度之间的信息传递。

其中，在多AUV系统中，每个AUV自身的导航设备性能存在差异，因此本实施例中，将AUV群中导航精度相同的AUV归为一层，并按导航精度高低来进行分层。导航精度高的AUV在高层，导航精度低的AUV在低层，同层的AUV之间来共享导航信息，高精度层AUV向低精度层AUV传递导航信息。

具体如图2所示，本发明对改进的分层式协同导航编队进行排布时，将高精度层AUV记为AUV_p(p＝1，2)，低精度层的AUV记为AUV_q(q＝3，4，5，6)，即AUV₁和AUV₂在同一层，为高精度层，AUV₃、AUV₄和AUV₅在同一层，为低精度层。由此，相邻AUV之间的斜距L(k)由以下公式计算出：

L(k)＝cΔt (3)

其中，c为声信号在海水中的传播速度，Δt为脉冲在两个AUV间的传播时间。

在划分完高低精度后，低精度层的AUV_q通过水声信号测量计算出与高精度层AUV_p之间的相对距离、方位角；同时，低精度层AUV_q接收高精度层AUV_p反馈的自身位置信息，通过扩展卡尔曼滤波方法融合内部和外部信息完成低精度层AUV_q的定位，具体过程如下：

设高精度层中任意一个AUV_p的坐标为：(x_p(k)，y_p(k)，z_p(k))，其中，x_p(k)是高精度层中AUV在x轴上的位移，y_p(k)是高精度层中AUV在y轴上的位移，z_p(k)是高精度层中AUV在z轴上的位移。

设低精度层中任意一个AUV_q的坐标为：(x_q(k)，y_q(k)，z_q(k))，其中，x_q(k)是低精度层中的AUV在x轴上的位移，y_q(k)是低精度层中的AUV在y轴上的位移，z_q(k)是低精度层中的AUV在z轴上的位移。

由AUV_p与AUV_q之间的几何关系得到以下表达式：

AUV_p与AUV_q之间的相对方位角：

AUV_p与AUV_q之间的水平方向距离：

接下来对分层式协同导航编队的队形进行优化，即在进行AUV协同编队构型时寻找到最优的队形，使得高精度层的AUV_p和低精度层AUV_q的AUV误差椭圆面积最小，以对分层式的结构进行一个优化处理。

为将不确定性量化，按照常规选取误差椭圆的面积来度量高低精度层中任意一个AUV的位置的不确定性。该误差椭圆的面积为：

α²指的是目标落在误差椭圆内的概率的度量，P_k+1/k代表后验协方差矩阵。因为在分析基于扩展卡尔曼滤波的定位精度时，通常采用后验协方差矩阵P_k+1/k来进行衡量。

其中，误差椭圆的面积越小，则位置的不确定性越小，误差就越小。

本实施例中，低精度层的AUV中，AUV₅自身导航精度高于AUV₄自身的导航精度，因此AUV₄被选来定位，并以移动长基线定位方法对被定位的AUV₄进行航位推算，即推算被定位的AUV₄的量测模型；然后以异时序贯量测方法获得被定位AUV₄的俯仰角、横滚角以及加速度信息；使用序贯扩展卡尔曼滤波方法对系统状态进行更新，最后通过多传感器数据融合导航系统进行多传感器数据融合解算以进行导航，并采用多传感器数据融合系统对滤波进行去噪处理。

其中，被定位的AUV₄的量测模型如下：

AUV₄利用自身携带的航位推算装置对其自身位姿进行估计，其推算方程表示为：

x_k+1＝x_k+x_k+1＝x_k+T.V_k.cos(φ_k)

y_k+1＝y_k+y_k+1＝y_k+T.V_k.sin(φ_k)

φ_k+1＝φ_k (₁)

其中，T为AUV₄的采样周期，V_k为k时刻的本体传感器所得的速度，为k时刻的本体传感器所得的航偏角。

令而/>且相互独立。

由此，被定位的AUV₄的量测模型为：

其中，为k+1时刻两AUV之间的声波传输距离，X_k为k时刻AUV₄的位移姿态，Γ(u_k+w_k，T)为非线性项，w_k为过程噪声，/>

对于移动长基线，由于参与构建长基线导航定位系统的各高精度层AUV与低精度层的AUV之间的距离远近不等，需要定位的AUV₄的航行位置则一直发生改变。假设搭载应答器的AUV接收到询问信号后立即应答，则经过这段时间，声传播距离表示为：

C为声波在水中的传输速度，标声传播距离，t_k+1表示AUV₄向AUV_i发送询问信息的时刻(i＝1，2，3)；t′_k+1表示AUV₄接收到AUV_i应答信息的时刻(i＝1，2，3)，AUV_i即分布在高低精度层中的AUV，具体如图3所示。

但是在应用移动长基线过程中，无论是主AUV应答器，还是被定位AUV，都处在运动状态。由于水声信号传播速度慢，因此会产生时间延迟问题。被定位AUV发送询问信号后，询问信号到达主AUV应答器需要一定时间，主AUV接收询问信号后，随即发送应答信号，应答信号传播到被定位AUV，而此时被定位AUV已经不在原来的位置。因此，在应用移动长基线时会产生误差，即基于时间延迟原理所解算出k+1时的声波传输距离有误差，其误差可表示为：

其中：分别设AUV₁、AUV₂和AUV₃在k+1时刻各AUV所在位置分别为

设AUV₄只配备低精度的航位推算系统，k+1时刻的AUV₄的位置坐标为(x_k+1，y_k+1)，

高精度层的AUV₁以T为周期，发射询问信号，低精度层携带应答器的AUV接收到信号后回复应答信号。因为低精度层各携带应答器的AUV与AUV₁的距离不同，所以AUV₁接收到信号的时间也有差别。

假设AUV₁经过的携带应答器的AUV依次为AUV₂，AUV₃和AUV₄，此时AUV₁与依次经过携带应答器的AUV₂、AUV₃、AUV₄之间的距离。

该距离的距离方程为：

其中，为/>时刻AUV₁得到的应答器发出回应信号的位置。按照顺序，在每一个应答器上都进行一个周期的异时量测处理，获得下一个周期的位置初值。如图3和图5所示，由于这种基于时间延迟原理所解算出k+1时的声波传输距离有误差，因此在移动长基线背景下，步骤(3)中，本发明采用异时序贯量测方法来解决低精度AUV₄的定位误差问题的具体过程如下：

(1)在水下，按照预定的时间周期，在t₀时刻，被定位的低精度AUV₄发送询问信号，安装应答器的主AUV₁、AUV₂、AUV₃接收到询问信号，以不同的频率应答，被定位的低精度AUV₄接收应答信号后，根据移动长基线的原理，确定与安装应答器的高精度层AUV相对位置关系，这个过程在t₁时刻前完成；

(2)t₀到t₂时刻，搭载应答器的AUV，即AUV₁、AUV₂、AUV₃通过水声通信装置以广播的形式发送水声信息，此信息包含主搭载应答器的AUV，即AUV₁、AUV₂、AUV₃各自应答询问信号时刻的位置状态，该状态包括时间、经度、纬度、深度、航向和姿态，被定位的低精度AUV₄被动接收此水声信息。

(3)如图5所示，被定位的低精度AUV₄根据解算出的与应答器的相对位置几何关系，以及接收到的高精度层AUV位置信息，依照移动长基线方法，更新自身位置信息。高低精度层的AUV在运动时进行自主导航，与此同时，高低精度层AUV之间也进行相对位置的量测，反馈出水声定位信息。扩展卡尔曼滤波法以最新的状态估计作为先验信息，结合先前高低精度层的水声定位信息、，进行异时序贯量测。

如图4所示，运用异时序贯量测方法对在移动长基线系统中得到的AUV的异时量测信息进行处理，并对系统状态进行更新，具体过程为：同精度的AUV分别在高低精度层进行运动，通过高低精度层间AUV的相对位置量测进行导航；然后再利用EKF滤波器，对获得的水声定位信息进行去噪处理；最后以最新的状态估计作为先验信息，进行异时序贯量测。

步骤(4)中，采用序贯扩展卡尔曼滤波方法EKF对系统进行状态更新的步骤如下：

使用传感器量测其被定位的AUV₄的更新状态，得到被定位的AUV₄在时刻的状态估计量为/>其中，/>为状态估计量，被定位的AUV₄接收到应答信号的时刻为/>之后通过该多AUV系统的预测协方差和/>时刻传感器量测更新系统状态。其中，该多AUV系统包括以高精度层与低精度层组成的AUV集群。

时刻系统的预测协方差为：/>其中，/>为状态转移矩阵：

过程噪声分布矩阵的公式如下：

然后，通过改进的多传感器数据融合导航系统，操作装置的上位机软件对导航所得的航向姿态信息进行融合解算。具体为，通过改进的多传感器数据融合导航系统对协同导航过程中所得的速度、姿态、航向等导航与定位信息进行融合解算。

其中，改进的多传感器数据融合导航系统包括GPS、水下磁辅助系统、惯性导航单元和水下摄像头。该改进的多传感器数据融合导航系统，对导航所得的航向姿态信息进行融合解算的分工与步骤如下：

(51)AUV搭载GPS在水上进行GPS定位，发送精准的地理坐标位置，进行初步定位。

(52)由于GPS信号在水下受到法拉第笼效应影响，即由于GPS信号接收器在水下受到静电屏蔽作用，无法接收到GPS信号，因此GPS不可用，只能采用其他传感器信号，因此通过水下磁辅助系统与惯性导航单元收集磁信号在水下进行AUV姿态角、航向角、速度以及位置信息的收集。

(53)由于水下环境复杂多变，在此期间，水下摄像头将进行视觉信号的收集与处理，进行近距离的水下成像，为AUV的避障、近距离导航动作提供精准的环境预判。

(54)多传感器数据融合导航系统融合各传感器信息并将惯性导航系统与水下摄像头进行在线融合，结合GPS信息，进行分析解算，多传感器数据融合导航系统中的融合板在线处理图像数据，实现实时处理的效果。

(55)最后，通过上位机软件对收集到的导航信息进行融合解算，将所解算出的数据传送到高精度层AUV之中，实现信息的交互，从而完成整个协同过程的导航信息的解算工作。

其中步骤(54)中，将惯性导航系统与水下摄像头进行在线融合，进行实时处理，具体包括以下内容：

(a)多传感器数据融合系统对滤波进行去噪处理；

(b)惯性导航系统融合了各传感器信息，将惯性导航系统与水下成像系统进行在线融合，提高不同传感器之间信息的匹配度；

(c)结合GPS信息，进行分析解算，集成板在线处理图像数据，实现实时处理的效果。

Claims

1.一种异时序贯量测的改进分层式AUV协同导航定位方法，其特征在于：所述方法包括以下步骤：

被定位的AUV的量测模型推算过程如下：

所述被定位的AUV对其自身位姿进行推算，推算方程为：

x_k+1＝x_k+x_k+1＝x_k+T.V_k.cos(φ_k)

y_k+1＝y_k+y_k+1＝y_k+T.V_k.sin(φ_k)

φ_k+1＝φ_k (1)

令而/>且相互独立；

由此，被定位的AUV的量测模型为：

其中，为k+1时刻两AUV之间的声波传输距离，X_k为k时刻被定位的AUV的位移姿态，Γ(u_k+w_k,T)为非线性项，w_k为过程噪声，/>

步骤(4)，通过序贯扩展卡尔曼滤波方法对系统状态进行更新；步骤如下：

使用传感器量测其被定位的AUV的更新状态，得到被定位的AUV在时刻的状态估计量为/>其中，/>为状态估计量；被定位的AUV接收到应答信号的时刻为/>之后通过预测协方差和/>时刻传感器的量测更新系统状态；

时刻系统的预测协方差为：/>

其中，为状态转移矩阵：

过程噪声分布矩阵的公式如下：

步骤(6)，采用多传感器数据融合系统对滤波进行去噪处理。

2.根据权利要求1所述的异时序贯量测的改进分层式AUV协同导航定位方法，其特征在于：步骤(3)中，采用异时序贯量测方法解决低精度AUV定位误差的步骤如下：

(33)被定位的低精度AUV根据解算出的与应答器的相对位置几何关系，以及接收到的高精度AUV位置信息，根据移动长基线方法更新自身位置信息。

3.根据权利要求2所述的异时序贯量测的改进分层式AUV协同导航定位方法，其特征在于：步骤(32)中，所述位置状态包括时间、经度、维度、深度、航向和姿态。

4.根据权利要求1至3中任一项所述的异时序贯量测的改进分层式AUV协同导航定位方法，其特征在于：步骤(5)中，通过多传感器数据融合导航系统对航向姿态信息进行融合解算的步骤为：

(55)通过上位机软件对收集到的导航信息进行融合解算，将所解算出的数据传送到高精度层AUV中进行信息的交互，进而完成协同过程的导航信息的解算工作。