AUV协同导航定位算法研究

姜朝宇，王喜龙

(海军驻葫芦岛四三一厂代表室，辽宁葫芦岛125004)

0 引言

随着地面及空中移动平台协同导航研究的成熟，人们开始将协同导航扩展到水下平台，尤其是AUV的协同导航。Hunt首先提出了标准长基线法 (LBL)［1］，Atwood将其用于多 AUV的协同导航［2］，AUV依靠固定在水面或是海底已知位置的信标定位，这种信标至少需要2个，当AUV向信标发出询问信号后，信标立即返回一个应答信号，AUV便可利用该信标的位置并结合声波在水中的传播速率和信号往返的时间差计算出自己相对于信标的可能位置估计，AUV同时相对2个或者2个以上信标的可能位置估计的交点即为AUV实际所处的位置，多AUV之间通过分时询问共享信标。Vaganay等改进了该方法提出了移动长基线法(MLBL)［3］，这种方法将信标接入GPS，可任意移动，极大地扩展了AUV的可操作区域。Matos和Zhang［4－5］对该方法进行了进一步研究，先后提出了基于双领航者和单领航者的协同导航定位方法。Liu［6］等对基于单信标测距的协同导航协同进行了能观性分析。Larsen通过与INS结合，提出了虚拟长基线法 (SLBL)法［7－8］。Rigby［9］等对超短基线法(USLS)进行了研究。为了多AUV协同导航具有更好的可扩展性，Cureio［10］等提出了时钟同步和单程传播时间 (OWTT)距离测量的方法并用ASC进行了实验。Eustice［11－12］等将其扩展到 AUV，使 AUV协同导航系统可以容纳的AUV数目得到了扩展，并对该系统进行了实验。Liu［13－14］等考虑海洋环境影响和AUV在三维空间的动力学特性，提出了未知洋流影响下的协同导航算法并对其能观性进行了分析。为提高AUV协同导航系统的经济型和实用性，Liu等将凸优化算法用于AUV协同定位［15］，利用部分廉价传感器即可取得较好的定位精度。此外，为了提高 LBL的导航性能。Bingham 和 Seering［16］还提出了一种基于假想栅格的方法。Olsen［17］等也进一步研究 LBL提出了 ROBL算法;Ruiz［18］借鉴地面移动平台的方法提出了针对于AUV的同步定位与构图法 (SLAM)。同时为了提高AUV导航的准确度和精度引入了许多估计算法，如扩展卡尔曼滤波 (EKF)、粒子滤波 (PF)和无迹卡尔曼滤波(UKF)［19－21］等。

本文首先对近年来多AUV协同导航领域的研究境况进行综述，然后介绍AUV协同导航的基本结构并分析了其特征;第2部分详细介绍基于Kullback－Leibler距离的多AUV协同导航算法;第3部分通过数值仿真验证第2部分中介绍的有效性，对该算法的性能进行分析。最后总结当前研究的热点并展望AUV协同导航定位的研究方向。

1 AUV协同导航基本结构及特征

多AUV协同导航系统，按照AUV在协同系统中的重要性，可分为主从式和平行式，其中主从式又包括单主、双主和多主等结构。主从式结构中主AUV配备高性能的导航系统并可以依靠自身的传感器测量进行有界误差定位，同时主AUV将承担协同导航中大量的数据存储和计算任务;而从AUV通常配备低精度的导航系统。主AUV通过与从AUV通信实现自己高精度的导航信息在整个AUV群体中的共享。从AUV利用该导航信息并通过相对位置测量计算自己的位置估计。通过这种方式提高整个AUV群体的定位准确度和精度，如图1所示。

平行式结构中，每个AUV配备相同的导航系统和转发器，通过接受来自信标的声波信号修正自己的误差。同时各AUV之间通过相互广播自己的导航信息实现导航信息的共享，并结合相互的距离测量来更新自身的位置，如图2所示。

图1 AUV的主从式结构Fig.1 Master-slave structure AUV

图2 AUV的平行式结构Fig.2 Parallel structure AUV

多AUV协同导航系统通过信息的共享，可以获得单AUV导航系统无法具备的优势:

1)部分具有高性能导航设备的AUV通过通信共享其导航信息可以实现其高性能导航设备在整个AUV群体中的共享。

2)在多AUV协同导航系统中，只要部分AUV具有有界误差的定位能力，通过协同导航，就可以使系统中的每一个AUV都具有误差有界的定位能力。例如在水下进行协同导航的AUV群体中，某一个AUV浮出水面使用GPS定位进行误差修正后，通过共享其导航信息就可以使得群体中其他AUV的定位误差有界［22］。

3)多AUV协同导航系统具有更强的容错能力和稳定性。当系统中部分AUV因为传感器故障或是环境因素而失去独立导航能力时，协同导航可以在一定程度上恢复这些AUV的导航能力。如果某个AUV自身的导航系统无法正常工作，但是能够和其他正常工作的平台相互通信并且进行距离或者方位观测，那么这个AUV的位置可以通过协同导航来估计。

2 AUV协同导航定位算法原理

AUV的协同导航关键是利用其他AUV的导航信息，通过信息融合以维持一个准确的位置估计，因此近几年针对AUV的协同导航定位提出了多种位置估计方法，如 EKF，PF和 UKF等。本文针对AUV的协同导航系统引入一种新的基于Kullback－Leibler距离的估计算法，该算法通过主从AUV之间的距离量测和导航信息共享可有效提高AUV的定位精度。

2.1 多AUV协同导航的数学模型

针对AUV协同导航问题，由于其深度值可以直接准确测量，故可将三维坐标投影到二维平面，考虑如下二维非线性离散系统，其运动方程为:

式中:xk，yk和θk分别为AUV在二维投影平面中的横向坐标、纵向坐标和偏航角度;Vk和分别为AUV的前向速度和偏航角速度;T为在将连续系统离散化时传感器的采样周期，而其中Vk和可由AUV的本体传感器测量，为系统的控制输入，可建模如下:

假设多AUV协同导航系统为主从式单主结构，其观测方程为:

其中 dxk，k+1和 dyk，，+1分别为 AUV 在采样周期时间内走过的水平位移和纵向位移，可由航位推算传感器测量;上标S为从AUV;上标M为主AUV;Rk为k时刻主、从AUV之间的距离。

由此可将该非线性系统表示为:

2.2 基于Kullback－Leibler距离的协同导航方法

在k时刻，从AUV成功接收到一个数据包后，它将获取以下信息:关于主AUV的位置估计xM(k)=［xM(k)，yM(k)］T，一个表达了xM(k)中各分变量可信度的协方差矩阵PM(k)，一个深度测量值zM(k)和一个主、从AUV之间的距离测量值rS，M(k)。

状态xM(k)和协方差PM(k)可能是来自于运行在主AUV上的导航滤波器的一个快照，也有可能是来自于GPS的一个快照 (在主AUV位于水面上的时候)。距离测量值rS，M(k)通过PPS同步特性而直接算得。许多实验表明该距离测量的误差与距离的远近只具有弱依赖性而且可以使用具有均值rS，M(k)和固定方差的高斯噪声建模。

在t(n)时刻从AUV所有的可能的位置都分布在以主AUV为中心 (假设坐标为xM(n))，为半径的圆上，同理t(m)时刻AUV所有可能的位置都分布在主 AUV为圆心 (设坐标为 xM(m))，r(m)为半径的圆上，将圆xM(n)按照 t(n)到t(m)这段时间内从AUV运动的距离向量=平移后得xM(n)'。求圆xM(n)'和圆xM(m)的交点，解这两个圆组成的线性方程，可得到一个解集，这个解集中可能包含1个、2个或者0个解:

如果将n从1到m－1取值，那么最多可计算出2(m－1)个解。在接下来的计算中假设使用了q个解。求交函数相对于测量和传送参数的雅克比可以用来计算出解xS(m)的协方差矩阵

其中:

同时也定义一个位置矩阵T(m－q)，将过去的从时刻t(m－q)到时刻t(m－1)AUV所有可能的位置存储在其中，同时也包括该位置的协同矩阵(m－o)和一个累积的转换代价cu(m－o)。

如果初始时候有一个已知的位置xS(0)(比如在水面的时候从GPS中获取)，可利用这个位置初始化T(0)=［xS(0)c(0)=0］。如果没有初始化位置可以利用，就用第一个解的集合S(0)初始化T(0)，只有当接收到连续的信息包后的位置估计才有效。

S(m)中任意一行 (如xSv(m))与T(m－o)的每一行分别代入上式可计算出q2个累积代价，

选取其中的最小值作为cv，然后将其整合到S(m)形成T(m)。

在T(m)中cv最小的那一行所对应的位置xv即为最可能的位置。

3 AUV协同导航定位算法性能分析

为分析算法性能，对上述算法进行仿真研究。在仿真实验中，根据式(1)的运动模型，不考虑AUV的动力学和水流的干扰，假设主、从AUV位于同一水平面内做直线运动，且初始航向角均为45°，前向航速均为5 m/s，偏航角速度均为0。主AUV配备高性能传感器，因此具有较高的定位精度，控制输入噪声为=(0.5 m/s)2，=(0.03°/s)2，从AUV的速度传感器测量噪声取=(2 m/s)2，=(0.05°/s)2，距离测量的噪声=(10 m)2，系统中的噪声均假设为零均值高斯白噪声，更新频率为0.1 Hz。

基于LKD的算法在刚开始一段时间内位置估计误差较大 (若初始位置未知则刚开始一段时间内的位置估计无效)，而后位置估计恢复正常，其仿真结果如图3所示，其误差如图4所示。

图3 KLD估计仿真结果Fig.3 The simulation results of KLD estimation

图4 KLD估计仿真结果Fig.4 The simulation error of KLD estimation

LKD算法计算一个位置估计的复杂度为o(q3)，其中q为需要考虑的过去的测量值的数目。受限于数据包传送过程，LKD算法的计算频率最大为0.1 Hz，且每一个数据包传输时间约为10 s。这样当q≈10时，在主频为1 GHz的PC上计算一个新位置的时间约为0.1 s。

4 AUV协同导航定位算法的研究展望

虽然近年来对AUV协同导航领域已经展开了许多研究，但仍有许多问题尚未解决或需要进一步研究:

1)目前AUV协同导航中的运动建模通常都进行了极大的简化，考虑更多因素，如AUV形状、大小甚至水动力学问题等，与AUV实际运动更相符的建模方法研究;

2)环境因素，如海水的温度、盐度、浑浊度和水下固定或移动障碍物等对AUV协同导航性能影响的定性或定量分析;

3)针对协同定位的策略研究，使AUV群的结构具有更好的灵活性和容错能力性，如主从式结构中若主AUV出现故障的应对策略等;

4)多AUV协同导航系统的智能化研究，特别是借鉴群智能领域的研究成果增加多AUV协同导航协同的智能化水平和应用领域;

5)除对已经提出的估计方法针对特定应用进一步改进外，还需要在非线性理论或其他相关领域中寻找适合于AUV协同导航定位的新方法，特别是借鉴地面和空中移动平台相关研究成果;

6)结合新的传感器或环境感知系统提出新的协同导航方法;

7)协同导航的异构研究，包括协同导航方法异构，如同一协同导航系统中的不同AUV采用不同的估计方法，和平台异构，如水下潜器、水面舰艇和飞行器三方协同。

总之，随着AUV协同导航系统各项研究的开展，一些问题会被解决，同时许多新的问题将被提出。随着这些问题的解决和各项研究的成熟，针对特定应用的各种AUV协同导航系统将会出现，这将极大扩展AUV的应用范围。

［1］ HUNT M，MARQUET W，MOLLER D，et al．An acoustic navigation system［R］．Woods Hole Oceanographic Institution Mass，1974．

［2］ ATWOOD D K，LEONARD J J，BELLINGHAM J G，et al．An acoustic navigation system for multi-vehicle operations［C］International Symposium on Unmanned Uutethered Submersible Technology．University of New Hampshire-Marine Systems，1995:202 －208．

［3］ VAGANAY J，LEONARD J J，CURCIO J A，et al．Experimental validation of the moving long base-line navigation concept［C］//Autonomous Underwater Vehicles，2004 IEEE/OES．IEEE，2004:59 －65．

［4］ MATOS A，CRUZ N．AUV navigation and guidance in a moving acoustic network［C］//Oceans 2005 － Europe．IEEE，2005，1:680 －685．

［5］ ZHANG L，LIU M，XU D，et al．Cooperative localization and navigation for multiple UUVs［J］．Journal of System Simulation，2008，19:065．

［6］ 刘明雍，李闻白，刘富樯，等．基于单信标测距的水下导航系统可观测性分析［J］．西北工业大学学报，2011，29(1):87－92．

［7］ LARSEN M B．High performance Doppler-inertial navigationexperimental results［C］//Oceans 2000 MTS/IEEE Conference and Exhibition．IEEE，2000，2:1449 －1456．

［8］ LARSEN M B．Synthetic long baseline navigation of underwater vehicles［C］//OCEANS 2000 MTS/IEEE Conference and Exhibition．IEEE，2000，3:2043 －2050．

［9］ RIGBY P，PIZARRO O，WILLIAMS S B．Towards georeferenced AUV navigation through fusion of USBL and DVL measurements［C］//OCEANS 2006．IEEE，2006:1－6．

［10］ CURCIO J，LEONARD J，VAGANAY J，et al．Experiments in moving baseline navigation using autonomous surface craft［C］//OCEANS，2005．Proceedings of MTS/IEEE．IEEE，2005:730 －735．

［11］ GADRE A S，MACZKA D K，SPINELLO D，et al．Cooperative localization of an acoustic source using towed hydrophone arrays［C］//Autonomous Underwater Vehicles，2008．AUV 2008．IEEE/OES．IEEE，2008:1 －8．

［12］ EUSTICE R M，WHITCOMB L L，SINGH H，et al．Experimental results in synchronous-clock one-way-traveltime acoustic navigation for autonomous underwater vehicles［C］//Robotics and Automation，2007 IEEE International Conference on．IEEE，2007:4257 －4264．

［13］李闻白，刘明雍，高俊钗，等．洋流影响下基于单领航者的多AUV 协同导航［J］．鱼雷技术，2010，18(004):277－281．

［14］ LI W，LIU M，LEI X，et al．Observability analysis for cooperative navigation system in autonomous underwater vehicles［C］//Intelligent Computing and Intelligent Systems(ICIS)，2010 IEEE International Conference on．IEEE，2010，1:155 －161．

［15］ LI M Y，LI W B，PEI X．Convex optimization algorithms for cooperative localization in autonomous underwater vehicles［J］．Acta Automatica Sinica．v36 i5，704 －710．

［16］ BINGHAM B，SEERING W．Hypothesis grids:Improving long baseline navigation for autonomous underwater vehicles［J］．Oceanic Engineering，IEEE Journal of，2006，31(1):209－218．

［17］ OLSON E，LEONARD J J，TELLER S．Robust range － only beacon localization［J］．Oceanic Engineering，IEEEJournal of，2006，31(4):949 －958．

［18］ TENA Ruiz I，RAUCOURT S，PETILLOT Y，et al．Concurrent mapping and localization using sidescan sonar［J］．Oceanic Engineering，IEEE Journal of，2004，29(2):442－456．

［19］ RUI G，CHITRE M．Cooperative positioning using rangeonly measurements between two AUVs［C］//Proc．Oceans，2010:1－6．

［20］ FOX D，BURGARD W，KRUPPA H，et al．A probabilistic approach to collaborative multi－ robot localization［J］．Autonomous Robots，2000，8(3):325 －344．

［21］刘明雍，沈超，张立川，等．一种基于UKF的AUV移动声学网络协同导航方法［J］．西北工业大学学报，2012，29(6):934－938．

［22］ BAHR A，WALTER M，LEONARD J．Consistent cooperative localisation［C］//In IEEE International Conference on Robotics and Automation，2009．