基于CKF的多UUV协同定位方法∗

杨 建 罗 涛 魏世乐 王亚波 王红华

（武汉第二船舶设计研究所 武汉 430064）

1 引言

随着海洋开发技术的发展，单自治水下机器人（Unmanned Underwater Vehicle，UUV）越来越难完成复杂的军事及民事任务，从而使得多UUV协同系统在海洋探索及开发、军事作战等方面拥有越来越重要的作用［1］。定位技术是协同作业的前提条件和关键技术。由于多UUV协同定位具有各UUV独自定位所不具有的多种优势，因此多UUV协同定位正逐步成为一个热门研究课题［2］，设计能够提高多UUV定位精度的协同定位算法具有重要的理论价值和现实意义。

如果多UUV群体在定位时存在着相对观测，那么通过一定的信息交换，就可以实现UUV间定位信息的共享，达到提高多UUV系统整体定位能力的目的，这种定位方法称为“协同定位（Coopera⁃tive Localization，CL）”。多UUV协同定位具有下列优势：1）能够充分利用系统中某些UUV的高精度导航信息，从而使得装备低精度导航设备的UUV可以提高自身的导航精度［3］；2）在多UUV系统中，部分UUV具有有界定位误差的导航能力，通过协同定位实现定位信息共享，可以使得系统中每个UUV都具有误差有界的定位能力；3）当某些UUV由于传感器或环境因素丧失独立导航能力时，协同定位可以在一定程度上恢复这些平台的定位能力。

多无人平台协同定位的研究始于20世纪90年代，研究对象包括移动机器人、水面无人艇、无人机、卫星、水下自治潜器等。目前，多UUV协同定位技术的研究总体上处于理论向工程实践转化的阶段，美国麻省理工大学的John J.Leonard教授带领的团队在2009年前后系统地提出了一系列多UUV协同定位方法，并进行了工程实践［4］，但是他们的协同定位方法存在着不足：各UUV的导航传感器精度都比较高，协同仅仅作为一种保证定位精度的辅助手段引入，也就意味着协同定位精度与UUV自身导航定位精度差别不大。Leonard教授的博士 Alexander Bahr在他的博士论文中［5］，根据几何学原理，提出了一种基于主定位器相对观测确定的从潜航器定位区域，然后利用Kullback—Leibler原理，提出一种新的优化目标函数，在上述优化区域内按照这种优化目标函数值最小的原则对潜器进行定位，仿真结果证明了这种优化方法的可行性，这种优化方法理论比较简单，但是由于要在大量区域内寻优，每一次寻优过程都需要进行复杂的寻优计算。

欧盟资助的多水下潜航器项目MAUVs GREX的一个研究成果是只依靠航迹推算和潜航器之间的距离测量信息，在不使用水声定位的情况下，实现潜航器的相对定位［6］。Corp公司研究了地球同步卫星协同定位的分散式算法［7］，同时该公司资助悉尼大学自主系统研究中心以机器人协同定位为背景，开展了相关的定位算法研究。

蒋荣欣［8］等利用主机器人上的传感器获得的方位以及距离信息，然后结合从机器人的自身解算出的位置及航向信息，设计了一种联邦滤波模型，并对其进行EKF滤波处理，通过实验证明了该方法定位精度较高，且无明显的误差累积效应。通过上述文献可以看出，协同定位方法已经在机器人领域展开了相应的研究，目前的各项研究都比较成熟，且实际中有所应用。受到陆地移动机器人的启发，研究人员将目光投向海洋，多UUV系统也逐渐成为协同定位领域里新的热点。

Mourikis等相关研究者分析了机器人协同定位精度与相关导航参数的关系［9］。对于可观系统，所有机器人的稳态定位误差都是有界的，其误差上界取决于机器人之间的相对测量关系、机器人运动传感器的精度以及机器人相对测量传感器的精度，与初始定位误差无关。对于不可观系统，机器人的定位误差会随时间无限增大，定位误差上界包含两个常值项和一个时间函数项，其中一个常值项由初始定位误差决定，另一个常值项由机器人之间相对测量关系和机器人传感器精度决定，时间函数项中时间的系数取决于机器人数目和航迹推算能力。

我国对多UUV协同定位技术的研究起步较晚。西北工业大学的徐德民院士团队提出了一种利用移动长基线技术的多AUV协同定位方法，并设计了AUV协同导航系统的运动学模型，对主AUV的精确定位信息、主从AUV之间的距离测量信息以及从AUV的自身定位信息进行了数据融合尝试［10～11］。国防科技大学的穆华［12］等提出了一种基于增广信息和高斯贝叶斯分布的移动机器人协同定位算法。并且对比分析了分散式信息滤波算法和分散式经典卡尔曼滤波算法的不同。

2 多UUV协同定位状态空间模型

2.1 状态方程

假设多UUV群体中有N个主UUV，M个从UUV，为了研究的方便，先考虑2个主UUV对1个从UUV进行协同定位的情况，定义tk时刻系统状态 为Xk=(，，)T，单 个UUV状 态Xik=(xik，yik，T，其中 i=1，2，3分别为三个UUV的编号，其中1，2为两主UUV，3为从UUV，且定义L1k=(x1k，y1k)T为第i个UUV在tk时刻的位置信息，ϕi表示第i个UUV在tk时刻的航向信息，根据UUV的运动特性，定义Vik、wik分别表示第i个UUV在tk时刻的自身传感器测量的前向合成速度以及航向角速度，δt为采样周期，则第i个UUV的运动学方程可表示为

定义uik=(Vik，ωik)T为第i个UUV系统的输入：

若假设各UUV的运动相互之间无影响，则可以将式（4）分解为三个UUV的方程：

结合式（1）中的UUV运动学方程，利用EKF原理可将第i个UUV的状态方程线性化：

其中：Fik、Gik分别为线性化后的第i个UUV系统矩阵、系统噪声激励矩阵，具体形式为

2.2 量测方程

如图1所示，假设UUV1在tk时刻与从UUV进行协同定位，则UUV3在tk时刻接收到的相对观测信息包括UUV1在tk时刻的位置信息为L1k=(x1k，y1k)T以及两UUV之间的水声测量距离r1k。

则根据主UUV1、从UUV3之间的几何位置关系有：

选取 L1k=(x1k，y1k)T、r1k作为tk时刻系统的量测信息，则量测方程可写为

其中：v13k为tk时刻UUV1与UUV3之间的量测噪声，利用Jacobian矩阵将式（9）线性化可得：

3 基于CKF的协同定位滤波方法

3.1 时间更新

假设k-1时刻状态估计协方差 p(xk-1)已知，通过Cholesky分解误差协方差Pk-1|k-1：

其中n为UUV个数，x̂k-1|k-1为k-1时刻系统状态估计值，且定义以下变量：

通过2.1节定义的状态方程（7）传播Cubature点：

3.2 量测更新

通过Cholesky分解Pk|k-1：

计算Cubature点i=1，2，...m，m=2n：

通过2.2节定义的观测方程传播Cubature点：

估计自相关协方差阵：

估计卡尔曼增益：

结合上述CKF时间以及量测更新，可以将通用的CKF计算流程总结为图2。

4 仿真试验分析

为了分析影响多UUV协同定位误差的主要因素以及验证CKF方法对协同定位精度的改进，本文将利用Matlab软件编写相应算法程序对实验数据进行离线处理以验证算法的有效性。

和本文理论分析的情况一样，试验采用两个主UUV轮替对从UUV进行协同定位，在没有通信丢包情况下，每个主UUV与从UUV的通信周期为5s，由于是两主UUV轮替与从UUV进行协同定位，因此在没有通信丢包情况下，从UUV 2s～3s即会收到某个主UUV的相对定位信息。滤波中设定距离测量噪声方差为=(10m)2，从UUV采用MIMU/TAM组合系统结合实现自身定位，其中MIMU中微机械陀螺常值漂移为10°h，加速度计常值漂移为0.01m/s2，加速度计随机误差5×10-3m/s2/Hz，磁强计测量噪声为10-8T，磁航向测量误差小于0.5°。仿真时长为2000s，离散时间间隔T设为1s。

主、从UUV运动轨迹见图3，三UUV近似为直线运动，从UUV运动轨迹在两主UUV之间。

图4为从UUV导航轨迹，从图中可看出，从UUV利用CKF产生的协同导航轨迹与真实轨迹基本重合，而利用自身导航传感器进行的航推（DR）定位轨迹与真实轨迹相差较大，证明了基于CKF的协同定位方法的有效性，图5为具体的定位误差比较图。

5 结语

本文在建立多UUV协同定位方案设计基础上，利用UUV运动学模型，建立多UUV协同定位状态空间非线性模型，在此基础上，提出利用CKF方法实现多UUV协同定位最优状态估计，通过试验验证，证明了基于CKF的多UUV协同定位方法的有效性。

［1］许真珍，封锡盛.多UUV协作系统的研究现状与发展［J］.机器人，2007，29（2）：186-192.

［2］P.Baccou，B.Jouvencel，V.Creuze.Single Beacon Acous⁃tic for AUVNavigation［C］//InternationalCoference on Ad⁃vanced Robotics，Budapest ICAR，2001：1253-1260.

［3］李闻白，刘明雍，张立川等.单领航者相对位移测量的多自主水下航行器协同导航［J］.兵工学报，2011，32（8）：1002-1007.

［4］A.Bahr，M.R.Walter，and J.J.Leonard.Consistentcoop⁃erative localization.In Robotics and Automation［C］//IEEE International Conference on Robotics and Automa⁃tion（ICRA'09），2009：3415-3422.

［5］Bahr A.Cooperative Localization for Autonomous Under⁃water Vehicles［D］.MIT，2009.

［6］Aguiary A，Almeiday J，Bayaty M，etal.Cooperative au⁃tonomousmarine vehiclemotion control in the scope of the EU GREX project：theory and practice［C］//Oceans 2009-Europe.IEEE，2009：1-10.

［7］Diba Mirza.Strategies in Tracking and Localization of Dis⁃tributed Underwater Systems［D］.University of Califor⁃nia，San Diego.2010.

［8］蒋荣欣，田翔，谢立，陈耀武.一种多机器人编队协同定位的方法［J］.哈尔滨工业大学学报，2010（1）：152-157.

［9］Mourikis A I，Roumeliotis S I.Performance analysis of multirobot cooperative localization［J］.Robotics，IEEE Transactionson，2006，22（4）：666-681.

［10］张立川，刘明雍，徐德民等.基于水声传播延迟的主从式多无人水下航行器协同导航定位研究［J］.兵工学报，2009，30（12）：1674-1678.

［11］张立川，刘明雍，徐德民等.多UUV协同导航与定位研究［J］.系统仿真学报，2008，20（19）：5342-5349.

［12］穆华.多运动平台协同导航的分散式算法研究［D］.长沙：国防科学技术大学，2010.