基于级联滤波的多AUV协同定位算法

基于级联滤波的多AUV协同定位算法

马朋

（中国电子科技集团公司第二十研究所，西安 710068）

摘 要：针对交替领航下的多自主水下航行器（AUV）协同定位系统，利用级联滤波架构设计了一种多AUV协同定位自适应滤波算法，通过将协同定位系统中的跟随AUV状态估计分解为航向估计与位置估计两部分，克服了在仅有部分距离量测下航向角估计可观测性差的影响，减小了距离量测误差模型不确定所引起的定位误差波动。同时，引入算法复杂度评价标准对所设计的级联自适应滤波算法进行分析。仿真结果表明，该算法具有较高的定位精度，可有效降低计算复杂度。

关键词：多自主水下航行器；协同定位；级联滤波；复杂度

中图分类号：U675 文献标识码：A 文章编号：1674-7976-(2019)-04-247-06

Multiple AUV Cooperative Localization Algorithm Based on Cascade Filtering

MA Peng

Abstract：For the multiple autonomous underwater vehicles (AUV) cooperative localization system based on two alternate

leaders, by the two-stage cascade filtering method, an adaptive filtering algorithm for multiple AUV cooperative localization is proposed. The state estimation of follower AUV is decomposed into two parts: heading estimation and position estimation, which is able to reduce the influence on the poor observability of heading angle estimation as a result of using the range-only measurements and improve the position estimation performance under range measurement error model uncertainty. In addition, the algorithm complexity criterion is introduced to analyze the proposed cooperative localization algorithm. The simulation experimental results indicate that the performance of proposed algorithm is superior to the traditional approach, in terms of both localization accuracy and complexity.

Key words: Multiple Autonomous Underwater Vehicles; Cooperative Localization; Cascade Filtering; Complexity

满足了AUV长时间、远航程、高精度的定位需求，

0 引言

高精度定位要求已成为自主水下航行器（AUV）执行任务时所面临的主要挑战。多AUV协同定位[1]是在多AUV协作基础上发展而来的新定位方式，其利用水声通信技术，交换共享个体AUV的实时位置与相对距离等信息，提高了系统整体定位精度， 收稿日期：2019-06-15。马朋（1987.12—），陕西渭南人，工程师，博士研究生，研究方向：协同导航，多源信息融合。

在海洋自主采样、海底勘探、水下侦查、目标搜索等许多方面有着广阔的应用前景。

受多AUV间水声通信带宽限制，协同定位系统以领航-跟随式（Leader-Follower）结构为主，且根据领航AUV个数及信息交互方式的不同，可分为单领航者、多领航者模式。研究发现，基于单领航者的多AUV协同定位系统复杂度及对距离量测技术的要求较低，但系统可观测性较差，需要通过设计合理的多AUV航行路径来提高定位性能[2-3]；

·248· 现代导航 2019年

而当协同定位系统中包含多个领航者时，由于跟随AUV可借助距离量测信息增多，系统可观测性及容错性增强，系统的复杂度及对通信能力的要求也随文献5-6在基于移动长基线的多AUV协之增高[4]。

同定位系统中，以两个装备高精度导航设备的AUV交替作为领航者，依据相同间隔分时广播发送位置信息，从而在多AUV编队队形保持不变的前提下，达到单领航AUV不断机动的效果，在降低通信复杂度的同时提高了系统可观测性。

当前，扩展Kalman滤波（EKF）由于可以充分地结合利用量测方程及量测误差的统计信息递推求解状态估计，估计精度较高，适用于非线性系统，因此越来越多的被应用于多AUV协同定位算法的设计中[7-9]。但是，标准EKF在对非线性系统线性化过程中，系统运动模型与量测模型将围绕最新状态估计量进行一阶泰勒展开，从而引起二阶以上信息丢失，且随着量测量的增多，滤波过程中的矩阵求逆会造成算法复杂度的增加。此外，在多机器人协同定位以及目标追踪过程中[10-11]，利用标准EKF在最新状态点处进行线性化求解，使得现实标

领航AUV2 准EKF滤波的不可观测子空间很难与理想EKF滤波相匹配，将来自于量测的欺骗性信息带入滤波算法中，从而导致航向估计不确定性的错误性减小，引起状态估计的不一致。

文献12-13则通过大量仿真研究发现，为了提高EKF-SLAM状态估计的一致性，应尽可能减小航向估计的不确定性及滤波线性化误差。

本文针对交替领航下的多AUV协同定位系统，如图1所示，在仅有部分距离量测信息条件下，提出了一种独立航向信息递推估计算法，并在此基础上，利用级联滤波架构设计了一种多AUV协同定位自适应滤波算法，以求减小航向估计不确定性对定位系统状态估计性能的影响，且在获得较高的协同定位精度的同时，有效降低整体算法的计算复杂度。论文整体结构如下：第2节建立了多AUV协同定位系统模型；第3节在级联滤波结构下，分别设计了跟随AUV航向与位置估计算法；第4节对算法的复杂度进行了比对分析；第5节则通过仿真试验验证了所设计协同定位算法的有效性。

2,k跟随AUV 2,k+11,k领航AUV1 1,k+1TTkk时刻时刻时刻Tkk+1+1时刻

图1 交替领航下多AUV协同定位

1 多AUV协同定位系统模型

由于多AUV编队通常在定深稳态控制下航行，且通过精确的深度传感器测量信息容易将三维位置关系转换为二维平面位置关系，因而定义k时刻

T跟随AUV运动状态为Xk[pk

Xk+1f(Xk,uk,wk)

xkVkcos(k)

TVsin() （1） ykkkkk

Vk其中，T为采样周期，uk(Vk,k)T为控制输入，和k分别为跟随AUV自身传感器所测前向合成速度和航向角速度，均受高斯白噪声干扰。假设传感器噪声满足wk~N(0,Qk)，则噪声协方差为

k]T，其中

pk[xkyk]T和k分别为跟随AUV的位置向量和

航向角，则跟随AUV的二维离散运动学模型为

第4期 马朋：基于级联滤波的多AUV协同定位算法 ·249·

22

Qkdiag(V,k,,k)。

在仅有部分距离量测信息情况下，为改善多

类似地定义领航AUVi在k时刻的状态向量为

Xi,k[piT,k

AUV协同定位系统航向角状态估计效果以及其对位置估计的影响，可将系统航向角独立分解出来，设计包含两个滤波器的级联滤波算法。如图2所示，其中uk(Vk,k)T为系统输入向量，第一级滤波器为航向角信息k估计过程，第二级滤波器则为位置从而，可根据运动学模型（1）信息(xk,yk)估计过程。

将跟随AUV状态估计过程分解为两部分，分别为：

ˆ(1)f(Xˆ(1),ˆ(1)Tˆk （3） ˆk)XXk1kk

ˆ(1)) （4）ˆ(2)(V,Xˆ(1))Xˆ(2)f(Xˆ(2),V,X Xk1kkkkkk

ˆ(1)ˆk为跟随AUV航向角估计，其中，Xkˆ(2)(xˆk,yˆk)T为跟随Xkˆ(1))TVcos(ˆk)(Vk,Xkk

i,k]T[xi,kyi,k，领i,k]T（i1,2）

航与跟随AUV之间的距离量测模型可表示为：

Zi,kh(pi,k,pk)i,kpi,kpki,k （2） 其中，di,kpi,kpk表示领航AUVi与跟随AUV间的欧氏距离，i,k为距离量测噪声，且满足下标符号i(k)1,2随所获得i,k~N(0,Ri,k)。

外部距离量测信息的不同而变化，当k2（N），设定k2，否则设定(k)1。

2 多AUV协同定位自适应滤波算法

在级联滤波架构下，可通过多个子滤波器分解估计系统状态量，将前一子滤波器独立估计结果作为后一滤波器估计过程的最优输入，可实现系统状态估计性能的提高与计算工作量的降低。

AUV位置估计，

ˆk)，详细TVksin(T

的多AUV协同定位自适应滤波算法则由以下2.1和2.2两节内容组成。

kFirst Filter （航向估计） Vkˆ(1)Pˆ(1)XkkSecond Filter （位置估计） ˆ(2)Pˆ(2)Xk1k1

图2 协同定位级联滤波原理

2.1 航向角估计滤波器

由于在多AUV航行过程中，集群无较大机动性，可将航向角速度视为随机噪声并统计建模，则航向角速度的估计均值和方差分别为[14]：

ˆkdkbkii （5） i1k

ˆ(1)Xˆ(1)T[(1d)ˆk1dkk] （9） Xk1kk

继而定义航向角速度估计误差为

k1k1ˆk1，则航向角估计误差为： k1k1ˆk1k1(ˆkTkTˆk)k（10）

ˆdbki(ˆk)2 （6） Pkki

i1

k

所以航向角误差协方差为：

其中，dk(1b)/(1bk)，b(0,1)为衰退因子。从而，递推式自适应航向角速度估计器为

ˆk(1dk)ˆk1dkk （7） ˆ(1d)Pˆ(1d)d(ˆk1)2 （8） Pkkk1kkk

Tˆk(1)P1E[k1k1]

kTkTk)(k)T]E[(kkT]E[kkT]TE[kT]TTE[kkkT]TTE[ （11）

将式（7）代入运动学模型（3）中，可得航向角递推估计为：

kkT]PkkT]0，ˆk(1)，E[kT]E[k又因为：E[ˆk，可简化式（11）为： kkT]PE[ˆ(1)Pˆ(1)TPˆT （12） Pk1kk

·250· 现代导航 2019年

将式（8）代入等式（12），可得航向角误差协方差递推估计为：

Pˆ(1)k1Pˆ(1)kT(1dk)Pˆk1(1dk)dk(kˆk1)2

T （13）

从而，式（9）和（13）组成了有关航向角估计的第一级滤波器。

2.2 位置估计滤波器

为了减小距离量测噪声协方差矩阵Rk不确定性对位置估计带来的影响，在所得航向角估计结果的基础上，利用Sage-Husa自适应算法设计位置估计滤波器，其具体过程如下。

状态一步预测方程：

Xˆ(2)k1/kXˆ(2)k

qˆk （14） 一步预测均方误差：

Pˆ(2)k1/kPˆ(2)kQ1,k

(Dk)Pˆ(1)k(Dk)T （15） 其中，

DkTVˆT

ksin(k)TVkcos(ˆk)，

Q22

1,kdiag(Vk,Vk

)。 自适应量测误差协方差：

r

ˆ(2)

k1(1d2,k1)rˆkd2,k1(Zk1Hk1Xˆk1/k

) （16） vk1Zk1Hk1Xˆ(2)k1/k

rˆk1

（17）

Sk1Hk1Pˆ(2)k1/kHTk1

（18） Rˆk1(1dT2,k1)Rˆkd2,k1(vk1vk1Sk1) （19）

其中，dk1

2,k1(1b2)/(1b

2)，b2(0,1)为衰退因子，H(2)k1为量测方程关于X

ˆk1

的雅克比矩阵，且

Hk+1hX

pT

i,k+1p

ˆk+1kpi,k+1p

ˆk+1k



滤波增益：

Kˆ(2)HTSRˆ1

k1Pk1/kk1k1k1 （20） 状态估计协方差：

Pˆ(2)k1(IKk1Hk1)Pˆ(2)k1/k(IKk1Hk1

)TKk1Rˆk1KTk1

（21） 状态估计：

Xˆ(2)k1Xˆ(2)k1/kKk1vk1

（22） qˆk1(1d2,k1)qˆd2,k1(Xˆ(2)k1Xˆ(2)kk

) （23） 从而，式（14）～式（23）组成了关于位置信息估计的第二级自适应滤波器。

3 算法复杂度分析

算法复杂度，作为算法优劣的一个评价标准，分为时间复杂度和空间复杂度[15]。时间复杂度用于度量算法执行的时间长短，空间复杂度则用于度量算法所需储存空间的大小。为了分析所涉及协同定位算法的执行效率，引入时间复杂度表示O(n)，且针对算法中的矩阵运算，描述矩阵运算行为与其复杂度的相对应关系如下：

SUM(NM,NM)O(NM)SUB(NM,NM)O(NM) MUL(NM,NP)O(NMP)

INV(NM)O(N3)

其中，N表示状态向量维数，M表示量测数目，

SUM、SUB、MUL及INV则分别为矩阵间的加、减、乘及求逆运算。

针对式（3）～（23）分析可知，协同定位算法的计算复杂度由AUV运动状态向量维数N和AUV间的距离量测数目M决定，N与M越小，则算法计算复杂度越低。在交替领航下的多AUV协同定位系统中，由于领航AUV个数固定，因此迭代计算过程中的距离量测数目为固定值（M1）。在利用标准自适应Kalman滤波（AKF）设计协同定位算法[16]时，参与运算的状态向量维数为N3；而在利用所设计的协同定位算法中，通过级联分解，有关航向角估计的滤波过程均为标量运算，复杂度

保持为O(1)，

有关位置信息估计的自适应滤波过程中，参与运算的状态向量维数则变为N2，从而

实现了定位算法计算复杂度的降低。

对于协同定位算法运行过程中所涉及的状态

第4期 马朋：基于级联滤波的多AUV协同定位算法 ·251·

方程及距离量测方程的雅可比矩阵，由于进行了级联分解，因此相关雅可比矩阵只存在于关于位置信息估计的第二级滤波器中。与标准AKF进行对比可以发现，运动方程中关于状态向量的雅可比矩阵由33阶变为了21阶，量测方程中关于状态向量的雅可比矩阵由13阶变为了12阶，而运动方程关于控制输入量的雅可比矩阵则不存在。随着相关雅可比矩阵大小的减小和消除，其在协同定位算法运行过程中所占的储存空间也将相应减小，从而协同定位算法的空间复杂度实现了降低。

4 仿真结果

为了验证文中所设计协同定位算法的有效性和实用性，利用Matlab进行仿真分析研究。

假定领航与跟随AUVs以相同航速沿直线航行，均为4m/s，相对距离测量频率为0.1Hz。跟随AUV

测速误差为22

V,k(1m/s)的零均值高斯白噪声，

航向角速度量测误差为22,k(0.01rad/s)的零均值高斯白噪声。两领航AUV与跟随AUV间测距误差相同，均为零均值高斯白噪声，且在(5m)2

～(10m)

2

范围内随机变动。

图3给出了分别在所设计级联滤波算法和标准

AKF算法下的跟随AUV航向角误差比较结果。从图中可以看出，由于领航与跟随AUVs间距离量测误差的随机变化以及系统可观测性差的影响，AKF算法下的航向角估计结果波动较大，航向角估计误差较大；反之，所设计级联滤波（CF）算法下关于航向角的自适应滤波器（即第一级滤波器）性能更优，航向角估计误差更小。

20 CF15AKF10o5/误差角0航向-5-10-15-20 0100200300400500600时间/s

图3 协同定位航向角误差

图4与图5分别给出了跟随AUV在两种不同算法下的位置估计误差（分别包括X轴和Y轴）。从图中可以对比发现，当领航与跟随AUV间的距离量测误差不确定时，在获得更小航向角估计误差的前提下，级联滤波（CF）算法中关于位置信息估计的自适应滤波器（即第二级滤波器）较之AKF算法，误差波动幅度更小，整体定位精度更高。

6 CFAKF42m/误差0轴X-2-4-6 0100200300400500600时间/s

图4 协同定位X轴误差

6 CF4AKF2m/0差误轴Y-2-4-6-80 100200300400500600时间/s

图5 协同定位Y轴误差

5 结论

本文针对交替领航下的多AUV协同定位系统，在仅有部分距离量测信息的情况下，为克服定位系统航向角估计可观测性差所造成的影响，设计了独立的航向角信息自适应滤波算法。在级联滤波架构

基础上，结合航向角估计结果设计了关于位置信息

估计的Sage-Husa自自适应滤波器，有效减小了距离量测误差模型不准确以及航向角估计可观测性

·252· 现代导航 2019年

差所造成的协同定位误差波动，获得了更佳的定位估计效果，降低了算法的计算复杂度。 参考文献：

[1] Curcio J., Leonard J., Vaganay J., et al. Experiments in

moving baseline navigation using autonomous surface craft[C]. Proceedings of MTS/IEEE OCEANS, 2005, 1:730-735.

[2] Antonelli G., Arrichiello F., Chiaverini S., et al.

Observability analysis of relative localization for AUVs based on ranging and depth measurements[C]. 2010 IEEE International Conference on Robotics and Automation (ICRA), 2010: 4276-4281.

[3] G. Antonelli, A. Caiti, V. Calabro, et al. Designing

Behaviors to Improve Observability for Relative Localization of AUVs[C]. 2010 IEEE International Conference on Robotics and Automation (ICRA), 2010:4270-4275.

[4] Ma P., Zhang F., Xu D., et al. Observability analysis for

AUV cooperative navigation system based on moving long baseline[C]. 2013 International Conference on Sensors, Measurement and lntelligent Materials, 2013: 609-615.

[5] Willcox S., Goldberg D., Vaganay J., et al., Multi-Vehicle

Cooperative Navigation and Autonomy with the Bluefin CADRE System. Proceedings of International Federation of Automatic Control Conference (IFAC), 2006:1-6. [6] 高伟, 刘亚龙, 徐博 等. 基于双主交替领航的多AUV

协同导航方法[J]. 哈尔滨工程大学学报, 2014, 35(11): 735-740.

[7] Baccou P., Jouvencel B., Creuze V., et al. Cooperative

positioning and navigation for multiple AUV operations[C]. MTS/IEEE Conference and Exhibition OCEANS, 2001, 3:1816-1821.

[8] 李闻白, 刘明雍, 张立川 等. 单领航者相对位移测量

的多自主水下航行器协同导航[J]. 兵工学报, 2011, 32(8): 1002-1007.

[9] 张立川, 徐德民, 刘明雍 等. 基于移动长基线的多

AUV协同导航[J]. 机器人, 2009, 31(6): 581-585.

[10] Huang G., Trawny N., Mourikis A., et al.

Observability-based consistent EKF estimators for multi-robot cooperative localization [J]. Autonomous Robots, 2011, 30(1): 99-122.

[11] Huang G., Roumeliotis S.. On filter consistency of

discrete-time nonlinear systems with partial-state measurements[C]. 2013 American Control Conference, 2013:5468-5475.

[12] Tamjidi A., Taghirad H., Aghamohammadi A.. On the

consistency of EKF-SLAM: focusing on the observation models[C]. 2009 IEEE/RSJ International Conference on Intelligent Robots and System, 2009: 2083-2088.

[13] Bailey T., Nieto J., Guivant J., et al. Consistency of the

EKF-SLAM algorithm[C]. 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2006: 3562-3568.

[14] 姚尧, 徐德民, 张立川 等. 通信延迟下的多UUV协同

定位—基于航迹预测的实时更新算法[J]. 机器人, 2011, 33(1): 161-168.

[15] Sage A., Husa G.. Adaptive filtering with unknown prior

statistics[C]. Proceedings of Joint Automatic Control Conference, 1969: 760-769.