基于双主交替领航的多AUV协同导航方法

高伟，刘亚龙，徐博，唐李军

（1.哈尔滨工程大学自动化学院，黑龙江哈尔滨150001；2.中国舰船研究设计中心，湖北武汉430000）

基于双主交替领航的多AUV协同导航方法

高伟1，刘亚龙1，徐博1，唐李军2

（1.哈尔滨工程大学自动化学院，黑龙江哈尔滨150001；2.中国舰船研究设计中心，湖北武汉430000）

基于单主模式的AUV协同导航系统的可观测性弱，对主AUV的机动性要求较高，实现较为困难，针对这一问题，提出一种基于双主交替领航的多AUV协同导航方法。在相邻量测时刻，从AUV分别利用不同主AUV的距离观测进行导航误差的协同校正。首先建立了AUV协同导航系统的数学模型；然后利用谱条件数法对AUV协同导航系统的可观测性进行了定量分析，明确了系统可观测度大小与相邻时刻主从AUV机动状态的对应关系，为利用双主交替领航提高系统可观测性提供了理论依据。通过与单主方案进行对比试验，验证了双主交替领航方案的有效性和可行性。

AUV；协同导航；非线性系统；可观测性；条件数；机动性

多AUV协作系统是近年来AUV应用领域的一种主流趋势和发展方向。利用多AUV协同作业不仅能够承担单体AUV难以胜任的诸多复杂任务，而且具有高效率、高可靠性的优点，实现了1+1＞2的效果，具有广阔的应用前景。基于水声通信网络的AUV协同导航技术是伴随AUV协作系统发展而来的一种新的AUV水下导航方案，相比传统的惯性导航、声学基线导航，协同导航不仅精度高，而且成本低、鲁棒性好，可以大大增强多AUV系统的协同作业能力，具有重要的研究价值［1-3］。理论上，通过融合多个已知参考AUV的距离观测信息可以实现自身位置信息的精确估计。然而由于水声传输速率低、时间延迟大且信号易阻塞，导致无法实现观测信息的时钟同步性，对处于运动状态的AUV协作系统来说，无论系统中有几个主AUV，相当于每次只能利用单个主AUV的距离观测信息进行协同导航。系统可观测是实现AUV协同导航的前提条件，由于单领航系统的观测信息量不足、系统可观测性弱，因而如何增强系统的可观测性，进而提高系统的协同导航效果对于AUV协同导航系统来说至关重要［4-5］。通过设计复杂的航路规划方案可以满足协同导航系统的可观测性［6-7］，然而对于航速慢，机动性差的AUV系统来说，为了提高系统的可观测性，在实际执行任务过程中进行复杂的航路机动往往是不现实的。针对以上问题，本文提出一种基于双主交替领航的AUV协同导航方法，通过位于不同方位的2个主AUV交替领航，等效于AUV相邻量测时刻的航路机动，增强了系统的可观测性，提高了AUV的协同定位效果。

1 双艇交替协同方案

采用双领航艇，每条领航艇均配备高精度惯性测量单元，结合多普勒计程仪（DVL）、深度传感器等构成组合导航系统，实现相对精确导航。为降低成本，从艇仅利用低精度航向基准系统、DVL以及深度传感器进行航位推算导航。双主交替协同的本质就是通过对位于不同方位的2个领航艇进行交替观测，近似等效于单艇协同方案中在相邻量测时间段领航艇的航路机动，进而增强系统的可观测性，并最终提高系统的协同导航性能。为了实现AUV间的协同导航，主从AUV均配备水声通信Modem，构成水声移动通信网络，用以实现AUV间相对距离的测量以及参考信息的传递。基于水声双程测距的双主交替协同过程如图1所示。其中三角形代表主AUV，五角星代表从AUV，五角星周围阴影区域代表从AUV的位置状态的协方差椭圆。

图1 双主交替协同导航方案Fig.1 Cooperative navigation scheme with two-leader alternation

主AUV1首先向从AUV发射ping脉冲测距请求信号进行水声测距，当主AUV1接收到从AUV发送的测距应答信号后，根据水声信号往返传播时间计算出二者之间的相对距离d1；紧接着，主AUV1将获得的水声距离信息d1连同自身位置信息广播发送给从AUV，从AUV利用该参考信息进行协同导航，实现对自身航位推算误差沿观测距离方向的一次校正。一次协同过程完成后，主AUV2重复主AUV1协同过程，即首先对从AUV进行水声测距，获得相对距离信息d2后向从AUV广播发送自身位置信息以及观测到的水声距离信息，进而实现对从AUV的二次协同校正过程。可以看出，每经历一次协同校正过程，从AUV沿观测距离方向的位置误差不确定度就会明显减小。因此，在整个AUV系统运行过程中，按照图中箭头所示，通过位于不同方位的2个主AUV对从AUV进行交替协同，可以更有效地实现AUV航位推算误差的协同校正。

2 双艇交替协同信息融合算法

2.1 系统模型建立

AUV的3个状态xk、yk、zk分别表示经度、纬度和深度信息。由于深度信息可以直接由深度传感器精确获得，为了分析问题简便，接下来只考虑二维平面下的协同导航问题［8］。AUV的运动方程可以表示为

式中：xk-1、yk-1、vk-1、θk-1分别为tk-1时刻，从AUV的东北向位置、速度和航向信息，Δt为采样周期。

由式（1）得到系统状态方程：

式中：Xk＝（xk，yk，θk）T表示AUV在tk时刻的状态向量；Φk，k-1为状态转移矩阵；Γ（uk+wk）为非线性项，其中uk-1＝（vk-1，θk-1）T表示控制输入量；wk＝（wvk，wθk）T为系统过程噪声，包括速度量测噪声和方位量测噪声协方差矩阵Qk：

由于系统噪声的存在，导致从AUV航位推算误差及其不确定度不断增加。多AUV协同导航的本质就是利用具有高精度定位信息的主AUV对从AUV的航位推算误差进行校正，提高从AVU的定位精度。

水声测距得到主从AUV三维空间下的距离z3D，k，已知主从AUV深度信息dC、dA条件下，二维平面下的主从AUV量测距离可以表示为

式中：wdk为水声测距噪声，为相互独立不相关的高斯白噪声，协方差矩阵为

距离量测方程为非线性，已知k时刻主AUV位置信息(xmk，ymk)，则观测方程对于状态Xk的雅可比矩阵可以表示为

2.2 EKF滤波算法

观测方程为非线性，本文采用EKF滤波算法进行信息融合（如图2所示），对从AUV位置状态进行估计。由于水声通信频率低（假设周期为M），在未接收到水声信号时，从AUV以较高的频率进行航位推算（假设周期为T）。由于系统误差存在，协同周期越长，由推位产生的系统误差方差也越大。

图2 信息融合流程图Fig.2 Information fusion flow chart

因此，每对系统状态按照航位推算执行一次状态预测过程之后，相应的要对系统误差协方差矩阵进行一次误差预测，以便为后续的状态更新过程提供正确的系统误差分布。整个信息融合过程可以分为2部分：未接收到主AUV参考信息时，系统执行开环估计过程，对系统状态以及相应的误差协方差矩阵进行预测：

当成功接收到主AUV参考信息后，系统执行状态更新过程，对系统误差进行校正：

3 可观测性分析

所谓量测系统的能观性，是指通过有限时间内的一系列量测量确定系统状态的能力。针对线性离散时变系统

能否由一组观测值Z＝［Z1Z2… Zk］T最终确定系统状态Xk是系统是否可观测的实质。根据给定条件，将量测和状态的对应关系写成矩阵形式：

则有

式中：

由式（15）可以看出，要想根据观测序列Z1，Z2，…，Zk确定状态Xk，充分必要条件是矩阵Wo（0，k）非奇异，这里Wo（0，k）称为系统的能观测性格拉姆判别矩阵。

对于本文所研究的离散时变观测系统，可以进一步给出系统可观测判别矩阵的等价形式：

式中：Γ（k-N+1，k）为离散时变系统的可观测秩判别矩阵，n为系统状态向量维数。

对于一维观测信息Z，需要至少进行N＝2次观测才可能完全估计出系统的二维位置状态。已知相邻量测时刻2条主AUV的位置信息，根据观测距离得到系统的可观测矩阵：

根据系统可观测性判别条件，系统可观测当且仅当系统可观测矩阵满秩，即系统的可观测矩阵行列式不为零：

即可观测。对于双艇交替领航系统来说，当且仅当2条主艇相对从艇距离向量方向相同时系统不可观测，在其他条件下均可观测。为了进一步明确系统可观测度大小与系统状态的量化对应关系，下面采用基于谱条件数的可观测性分析理论，进一步对系统的可观测性进行定量分析［9］。条件数反应了线性方程组

的解对b的误差或不确定度的敏感性的度量。A的条件数越大，b的微小改变引起解X的改变越大，数值稳定性越差。A的条件数越小，b的微小改变引起X的改变也越小，数值稳定性也越好。

设A∈Cn×n，‖·‖是一种算子范数：

称cond（A）为矩阵A（关于求逆或求解线性方程组）的关于算子范数‖·‖的条件数。常用的关于p范数‖·‖p的条件数记为cond（A）p，称cond（A）2为谱条件数［10］。

矩阵的条件数越大，数值稳定性越差，反之，数值稳定性越好。因此，通过观测矩阵条件数的大小即可以反应出系统的可观测性强弱。由于矩阵条件数是一个大于等于1的正数，定义系统可观测度为可观测矩阵谱条件数的倒数：

令θk为k时刻主从AUV距离向量方位角，则观测矩阵可以进一步表示为

其中，定义相邻时刻主从AUV距离向量方位变化Δθ＝θk-θk-1，得到系统可观测度解析表达式：

通过对式（25）进行仿真分析可以看出，系统可观测度大小与相邻时刻从AUV相对2个主AUV的观测距离向量夹角有关，向量夹角越接近于90°，系统可观测度越大。对于采用单领航方案来说，根据上述可观测条件，要想得到较好的系统可观测性，需要主从AUV在相邻量测时刻发生较大的相对位置状态变化，而这在实际应用过程中往往是比较困难的。由于在协同导航系统中，从AUV只需要根据接收到的参考位置信息以及相对于该参考位置的距离信息进行协同定位，而对于该参考位置信息到底来自于哪个主AUV效果是一样的。因此，通过采用双领航交替协同方案可以很容易满足系统的可观测性要求。只要保证主从AUV一定的位置关系，在相邻时刻对不同主AUV交替进行信息观测，等效于主从AUV在相邻时刻发生较大的状态机动，系统获得较高的可观测性，如图3所示。需要说明的是，采用分步式滤波方案，系统的定位精度与相邻量测时间间隔有关。由于每一次量测更新只是对一个方向上的定位误差进行修正，如果量测时间间隔过长，由于推算误差的累积，系统的协同精度会受到影响。

图3 系统可观测度与相邻时刻主从AUV距离向量关系Fig.3 Relationship between the observability and the master-slave AUV distance vector at adjacent time

4 试验验证

4.1 试验条件

2012年10月份，于无锡太湖水域进行了基于水声通信网络的多AUV协同导航验证性试验。利用水面艇模拟AUV分别对单主协同和双主交替协同方案进行了验证。试验水面艇及相关试验设备如图4所示。

图4 试验用水面艇及相关试验设备Fig.4 Vessel used in our work and the on-board equipments

水声通信设备采用Teledyne Benthos公司的ATM-885型低频水声通信设备，该设备具有全向及定向水声信号发射的能力，同时还可以实现设备间一对一的水声双程测距。试验过程中，利用该设备构建水声通信网络（如图5所示），实现主从艇间距离的量测和位置信息的传送。

主艇的位置参考信息由艇上GPS提供，从艇利用磁罗经提供的航向信息以及由GPS获得的速度信息进行初步的航位推算，当接收到主艇发送的位置和距离参考信息后，采用扩展卡尔曼滤波（EKF）技术对自身的航位推算误差进行协同校正。试验过程中，主艇每隔30 s对从艇进行一次水声测距，成功测距后主艇将该距离信息连同自身当前时刻的位置信息一同发送给从艇。从艇成功接收主艇的水声信号后，融合该信息进行位置状态的估计，实现航位推算误差的协同校正。

图5 水声通信网络示意图Fig.5 Underwater acoustic communication network

4.2 试验结果

4.2.1 水声测距误差分布

水声测距误差直接影响AUV的协同导航效果，试验过程中首先通过静态条件下选取不同基准距离对水声测距精度进行了评估。从水声测距误差分布直方图6可以看出，测距误差满足高斯分布（均值0.21 m，方差0.528 m），最大测距误差不超过2 m。

图6 静态条件下水声测距误差分布Fig.6 Acoustic ranging error distribution in static

4.2.2 协同导航

试验1 采用单主协同模式，主从AUV运行轨迹如图7所示，试验共进行了50 min，试验过程中主从AUV成功进行了70次水声通信，通信成功率约为70%，平均协同周期约为42 s，AUV运行速度2.1 m／s。通过比对图8定位误差曲线与图9距离方位变化曲线可以看出，前30 min，由于载体机动不明显，协同定位效果较差，30 min以后，由于相邻量测时刻主从AUV距离向量发生较大的方向变化，协同定位效果较好。证明了采用单领航方案需要主从AUV保持较高的航路机动，否则协同效果会较差。

图7 单主协同AUV运行轨迹Fig.7 AUVs’trajectory based on single leader

图8 单主协同定位误差Fig.8 Positioning error based on single leader

图9 相邻量测时刻观测方位变化量（单主）Fig.9 The relative azimuth variation of leader and follower between adjacent measurement time（single leader）

试验2 采用双主交替协同模式，试验过程中，主从AUV基本未做较大的状态机动，运行轨迹如图10所示。试验时间约为13 min，共成功进行了20次水声通信，平均协同周期约为39 s，AUV运行速度2.5 m／s。由误差曲线图可以看出，整个过程中，协同定位效果比较稳定。对比单主协同模式，之所以最后协同定位精度会比单主协同精度低，这是由于单主协同过程后一阶段主AUV做圆周运动，本身起到了对航推算误差的校正作用，若不做类似的机动运动定位效果会很差。

图10 双主协同AUV运行轨迹Fig.10 AUVs’trajectory based on two leaders

图11 双主协同定位误差Fig.11 Positioning error based on two leaders

图12 相邻量测时刻观测方位变化量（单主）Fig.12 The relative azimuth variation of leader and follower between adjacent measurement time（two leaders）

5 结束语

本文采用双主交替领航，近似等效于单主模式中相邻量测时刻主从AUV相对观测方位的变化，有效克服了协同导航系统对主AUV的机动性要求，增强了协同导航性能。通过采用谱条件数法对协同导航系统的可观测性进行定量分析，明确了系统可观测度大小与相邻量测时刻主从AUV距离向量夹角之间的关系，向量夹角越接近于90°，系统可观测度越大。理论上证明了，只要2个主AUV相对从AUV保持一定的几何位置关系，在系统运行过程中不需要进行航路机动即可保证系统的可观测性。通过湖上验证性试验，对单主协同以及双主交替协同模式进行了比较分析，得出了双主交替领航可以在不需要主AUV机动情况下实现较高协同导航效果的结论，进一步验证了双主交替领航模式的有效性。本文提出的协同导航方法简单、有效，具有重要的实际应用价值。由于试验条件所限，本文的验证性试验是采用水面艇代替进行的，由于近水面水声环境复杂，影响水声通信效果，后续的研究工作将逐步采用水下AUV进行协同导航效果验证。

［1］AKYILDIZ I F，POMPILI D，MELODIA T.Underwater acoustic sensor networks：research challenges［J］.Ad Hoc Networks，2005，3（3）：257-279.

［2］HEIDEMANN J，YE W，WILLS J et al.Research challenges and applications for underwater sensor networking［C］／／Proceedings of Wireless Communications and Networking Conference.Las Vegas，USA，2006：228-235.

［3］WEBSTER S E，EUSTICE R M，MURPHY C，et al.Toward a platform-independent acoustic communications and navigation system forunderwatervehicles［C］／／Proceedingsof OCEANS 2009，MTS／IEEE Biloxi-Marine Technology for Our Future：Global and Local Challenges.［S.l.］，2009：1-7.

［4］GADRE A.Observability analysis in navigation systems with an underwater vehicle application［D］.［S.l.］：Virginia Polytechnic Institute and State University，2007：12-23.

［5］FALLON M F，PAPADOPOULOS G，LEONARD J J，et al.Cooperative AUV navigation using a single maneuvering surface craft［J］.The International Journal of Robotics Research，2010，29（12）：1461-1474.

［6］CHIRE M.Path planning for cooperative underwater rangeonly navigation using a single beacon［C］／／Proceedings of International Conference on Autonomous and Intelligent Systems（AIS）.［S.l.］，2010：1-6.

［7］TECK T Y，CHITRE M.Single beacon cooperative path planning using Cross-Entropy method［C］／／Proceedings of OCEANS 2011.［S.l.］，2011：1-6.

［8］张立川，徐德民，刘明雍，等.基于移动长基线的多AUV协同导航［J］.机器人，2009，31（06）：581-585.ZHANG Lichuan，XU Demin，LIU Mingyong，et al.Cooperative navigation of multiple AUVs using moving long baseline［J］.Robot，2009，31（06）：581-585.

［9］钱伟行，刘建业，赵伟，等.基于转动基座的SINS初始对准方法研究［J］.宇航学报，2008，29（3）：928-932.QIAN Weixing，LIU Jianye，ZHAO Wei，et al.SINS initial alignment based on revolving base［J］.Journal of Astronautics，2008，29（3）：928-932.

［10］戴华.矩阵论［M］.北京：科学出版社，2001：189-193.

Multiple-AUV cooperative navigation based on two-leader alternated navigation

GAO Wei1，LIU Yalong1，XU Bo1，TANG Lijun2
（1.School of Automation，Harbin Engineering University，Harbin 150001，China；2.China Ship Development and Design Center，Wuhan 430000，China）

Due to the weak observability of autonomous underwater vehicles（AUVs）cooperative navigation with a single leader，high maneuverability is required for the main AUV as leader.However，it is always difficult to implement in practice.A cooperative navigation method based on two-leader alternated navigation has been proposed，and the range information observed from different leaders in adjacent measuring time was used for the AUVs＇cooperation.The mathematical model of the cooperative navigation system was first established，and then the observability was analyzed quantitatively by using the spectral condition number method.The relationship between the observability and the maneuvering state of the leader-follower in adjacent time was confirmed and the theoretical basis of the alternated support with the two-leader AUVs was provided.In comparison with the traditional method with only one leader，this proposed method was verified to be more effective and feasible.

autonomous underwater vehicles；cooperative navigation；nonlinear system；observability；spectral condition number；maneuverability

10.3969／j.issn.1006-7043.201307022

http：／／www.cnki.net／kcms／doi／10.3969／j.issn.1006-7043.201307022.html

U666.11

A

1006-7043（2014）06-0735-06

2013-07-10.网络出版时间：2014-05-15 16：46：34.

国家自然科学基金资助项目（61203225）；国家博士后基金资助项目（2012M510083）.

高伟（1977-），男，教授，博士生导师；刘亚龙（1986-），男，博士研究生.

刘亚龙，E-mail：yalong＠163.com