1、2011年12月第29卷第6期西北工业大学学报Journal of Northwestern Polytechnical UniversityDec201lV0129 No6一种基于UKF的AUV移动声学网络协同导航方法刘明雍，沈 超，张立川，龚雪晴(西北工业大学航海学院，陕西西安710072)摘要：针对扩展卡尔曼滤波(EKF)在自主水下航行器(AUV)移动声学网络协同导航中，存在强非线性观测方程条件下线性化误差大、计算复杂等缺点，文章提出一种基于无迹卡尔曼滤波(UKF)的AUV移动声学网络协同导航方法。利用移动长基线原理和UKF方法，建立基于UKF的协同导航滤波算法，并通过仿真实验与传统的E
2、KF协同导航算法进行对比。仿真结果表明，基于UKF的协同导航算法能明显减小导航定位误差，在导航精度上优于EKF方法，是AUV协同导航中一种更加简单有效的导航滤波方法。关键词：无迹卡尔曼滤波，移动声网络，协同导航，AUV中图分类号：TP2423 文献标识码：A 文章编号：1000-2758(2011)06-0934-05随着水下滑翔器编队、水下漂流器编队以及蜂群式AUV等小型化低成本AUV的大批量使用，不可能每个AUV均拥有完善的导航系统，需要充分利用群体导航信息进行个体导航辅助定位。AUV协同导航是解决海洋中间层水下导航问题的重要途径，成为近年来国内外研究的热点。2000年MBIAtrsen提
3、出了虚拟长基线(SLBL)的概念，并且该团队利用自己设计的SLBL和航位推算系统MARPOS旧1进行了试验，取得不错的效果。剑桥大学的。Vaganayo团队采用水面信标GPS辅助导航的方法大大提高了AUV的导航精度。2004年波尔图大学的Anibal Matos【4 J、Nuno Cruz在此基础上提出的使用移动信标来替代固定信标，并对双领航者协同定位方法进行了仿真研究。2008年，NWPU水下航行器研究所的张立川研究了单领航者协同导航定位的研究”1。2010年NWPU水下航行器研究所的李闻白在基于单信标测距的水下协同导航系统中给出了可观测性分析1。在研究中发现，实际的AUV导航模型具有的非线
4、性很强，而现阶段世界各个研究机构对于协同导航算法通常只采用扩展的卡尔曼滤波器(EKF)进行滤波实现。然而，基于EKF的导航算法是将非线性方程围绕状态估值进行泰勒展开，并进行基于一阶泰勒截断的线性化方法，虽然具有一定的精度H6 J，但是实际应用中存在很明显的缺陷：(1)当非线性观测方程的泰勒展开式中的高阶项不能忽略时，EKF会导致很大的线性化误差，造成滤波器难以稳定；(2)Jacobian矩阵的推导十分复杂，有时候甚至无法得到具体解析形式，导致EKF失效。本文提出采用无迹卡尔曼滤波方法(UKF)对移动声网络协同导航算法进行滤波实现，不需要系统模型的具体解析形式，克服了EKF的不足，并充分考虑了随
5、机变量的噪声统计特性，算法简单，易于实现。仿真实验表明，在理想仿真条件下UKF方法能达到在l m以内的精度明显优于EKF方法。1移动声网络协同导航基本原理双领航者移动声网络导航是属于基于移动长基线(MLBL)的导航定位技术07I。信标与主AUV固连，两主AUV之间形成移动基线，用于从AUV的导航定位。主AUV装备高精度的导航设备，从AUV装备低精度的航位推算系统，主、从AUV间利用水声Modem进行测距和通讯。如图1所示，在t，时刻主AUVl发出声脉冲向收稿日期：2011-03-08 基金项目：自然科学基金(50979093、51179156、51109179)与航空科学基金(2011ZC53
6、043)资助作者简介：刘明雍(1971一)，西北工业大学教授、博士生导师，主要从事水下航行器导航与控制的研究。万方数据箜兰期 刘明雍等：一种基于UKF的AUV移动声学网络协同导航方法 935、rl 黔 。 辽霉f0 i_7丑a)主AUVIPING b)从AUV得到c)主AUV2PING d)从AUV得到相对位置 相对位置图1 移动长基线导航定位原理示意图声网络中的从AUV进行位置广播，声脉冲信息包括时间、经度、纬度、深度、航向、姿态等。t：时刻从AUV收到声脉冲信号，解算自身相对于主AUVl的位置信息；3时刻，主AUV2发出声脉冲位置广播；t4时刻从AUV收到声脉冲信号，解算得相对于主AUV2
7、的位置信息。图2移动长基线算法由于AUV的深度D可以直接从深度传感器得到，故将AUV之间三维位置关系向水平面内投影，转换为二维平面位置关系，如图2所示。在水平面内，主AUVl和AUV2的坐标分别为(X，y1)，(五，y2)，它们之间的距离(斜距)分别为R。和R：，深度分别为D。和D：，则根据几何位置关系，有P。=撕F可Fj酽P2=属1珊(1)砰=(xX。)2+(Yy1)2暖=(x一恐)2+(Yy2)2求解上式得到(x，y)的值。由图2可见得到的位置解是双解，因此需借助初始位置，运动轨迹等判断从AUV是否穿过基线。在仿真中，被定位的从AUV始终不穿过移动基线。2基于UKF的滤波算法实现21 AU
8、V的运动数学模型AUV的三个状态(石t，YI，)分别表示AUV坐标与深度，由于深度z可以由深度传感器直接获得，无需进行数学模型解算。AUV运动方程可表示为戈I+1=戈I+T。KCOS(妒k)+丁吁Yk+1=Yk+TKsin(9)+rW妒+l=9I (2)1暇1=曙嵫。=曙式中，r为解算周期，K为AUV前向合成速度，输入“t=(K，也)1圪为AUV前向合成速度，机为AUV航向角，假设两者均受高斯白噪声干扰，且方差已知，分别为矿k和矿2十。曙和曙分别为洋流的东向速度和北向速度。22基于UKF的导航算法实现Julier等人提出的采用基于UT变换的UKF滤波方法9 J，采用采样策略逼近非线性分布，改善
9、对非线性问题进行滤波的效果。 一设，l维随机向量工一(j，只)，m维随机向量z为工的某一非线性函数 ，z=八工) (3)z的统计特性是(j，只)，通过非线性函数以)进行传播得到z的统计特性(乏，只)。设计一系列点靠，i=0，1，称其为盯点；对设计的盯点计算其经过八)传播所得的结果7i，i=0，1，；然后基于7l，i=0，1，计算(乏，Pz)。在本文仿真中，为了简化系统模型，将被定位AUV的状态变量z取为(髫，)，妒)，其中戈，y表示AUV横纵坐标，9表示航向角。则(2)式中略去洋流项即为系统状态方程简写为z+l=以工)+WI (4)将(1)式作为UKF的量测信息，那么量测方程表示如下zt=c戈
10、，y，+V：=毳】+V：=(、，茗，。k-Xl,k，)2+(Yk-yx,。,)2X2,k(Yk Y22y：c5，、， ， e： 、 ，式中，l，：为量测噪声，假设为相互独立不相关的零均值高斯白噪声序列，其方差为万方数据西北工业大学学报 第29卷R=ET= 眩曼】 (6)简写为zI=h(x)+叱 (7)每个时间段UKF计算一个循环的具体步骤如下：1)对于给定的叠l，Pm，用UT法求状态一步预测量枞，以及预报误差的协方差阵P胍一。计算盯点毵三限。，i=o，1，n，即孝蹴=萱ml毹三m。=或川+(以i丽)ii=1，2，凡 (8)毹三限。=毛川一(以订丽)；i=rt+1，n+2，2凡计算盯点器，i=o
11、，1，n，通过状态方程的传播，即器=(氍三她。)，i=0，1，2n一。=蝌D 江o(9)H=：。(一一。)(手D一叠劓I1)+Q一12)用UT求盯点譬枞扪P枞一。通过量测方程的传播。计算tr点j肌扪P叭一。通过量测方程对z。的传播，即毹=屯=萱枞一。+(以i丽)；i=1，2，rt (10)=叠枞一。一(以i丽)ii=，l+1，n+2，2n图3仿真实验AUV轨迹图计算输出的一步提前预测，即俄订=I(毹D)，i=o，1，2n2。=磁nj 2-lo ，1只。=：。(矗n一乏枞一。)(矗n一乏枞一。)+R。I ”羔【只以=：。(毵n一碧肌一。)(矗n一磊l。一。)(11)3)在获得新的量测Z。后，进行
12、滤波更新。r叠聃k=叠M k一1+蠡气(zk一乏甜I1)甄=只臃P二1 (12)P鼬=P肌一。一瓦P二1式中瓦为滤波增益矩阵。3仿真结果利用Madab对以上算法进行仿真。由于AUV深度可以由自带的深度传感器精确得到，所以在本算法仿真中将AUV三维运动投影到同_平面内，简化成二维位置关系。 7如图3所示，仿真中，主AUVl和主AUV2沿水平直线航行，航速为1 ms；从AUV沿圆弧航行，航速为1 ms，航向角每秒变化，为仿真步数，采样周期T=1 s。仿真中引入水声定位斜距的量测噪声，分别为盯2P=盯2P，=(1 m)2的零均值高斯白噪声，两者独立且不相关。仿真时间为200 s，东向误差与北向误差如
13、图4所示。0 40a)80 120ts东向位置误差40_40 40 80 120 160 200tsb) 北向位置误差图4导航位置误差O0O02ll万方数据第6期 刘明雍等：一种基于UKF的AUV移动声学网络协同导航方法在图4中可以看到，待滤波器收敛以后，UKF算法的东向与北向误差都较EKF要小。下面通过4 结 论表1中的数据，对仿真结果进行分析。表1滤波器收敛后2种算法的最大误差比较(m)从表l可以看到，待滤波器收敛以后，采用UKF滤波的AUV东向与北向的定位误差均小于1 m的数量级，明显优于EKF方法。这表明选择UKF对AUV移动声网络协同导航进行滤波实现是一种导航精度较高的导航方法，能有
14、效抑制由白噪声引起的误差增长。参考文献：本文将UKF滤波算法应用于水下航行器的声网络协同导航中，和传统的EKF方法相比，基于UKF的导航算法精度较高，并能有效避免计算Jacobian矩阵带来的计算复杂度，算法简单，易于实现。仿真结果表明，该导航滤波算法计算精度高，收敛性好，有较好的理论和工程实用价值，有望为AUV路径规划和多AUV协同导航等方面提供理论研究。另外，本文中并未考虑水下通讯时延，以及滤波算法实时性等方面，这将是后续工作中需要重点解决的问题。1Larsen M BHigh Performance Doppler-Inertial NavigationExperimental Resu
15、ltsOceans 2000 MTSIEEE Conference and Exhibition t，2000，144914562 IALrsen M BSynthetic Long Baseline Navigation of Underwater VehiclesOCEANS 2000 MTSIEEE Conference and Exhibition200020432050 ， ，3 Vaganay J，Baccou P，Jouvencel BHoming by Acoustic Ranging to a Single BeaconOceans 2000 MTSIEEEConferenc
16、eand Exhibition，20004Motos A，Cruz NAUV Navigation and Guidance in a Moving Acoustic NetworkOCEANS 2005Europe5 Zhang Hchuan，Liu Mingyong，Xu Deming，Yan WeishengCooperative Localization and Navigation for Multiple UUVsJour-hal of System Simulation，200819-0656 刘明雍，李闻白，刘富樯，沈超基于单信标测距的水下导航系统可观测性分析西北工业大学学报，
17、2011，29(1)：8792Liu Mingyeng，Li Wenbai，Iju Fuqiang，Shen ChaoObservability Analysis for Underwater Navigation System Based on RangeMeasurements with a Single BeaconJournal of Northwestern Polytechnial University，2011，29(1)：8792(in Chinese)7Jirome Vaganay，John J Leonard，et a1Experimental Validation of
18、the Moving Long Base-Line Navigation ConceptIEEEOESAutonomous Underwater Vehicles，2004，59658 Julier S J，Uhlmann J K，et a1A New Method for the Nonlinear Transformation of Means and Covariances in Filters and EstimatorsIEEE Trans on Automatic Control，2000，45：4774829Zhu Hongqian，Hu HuoshengAdaptive Uns
19、cented Kalman Filter for Deep-Sea Tracked Vehicle LocalizationICIA09 Intemational Conference on Information and Automation，2009万方数据938 西北工业大学学报 第29卷A New and Effective AUV Cooperative Navigation MethodBased on UKF in a Moving Acoustic NetworkLiu Mingyong，Shen Chao，Zhang Lichuan，Gong Xueqing(College
20、of Marine Engineering，Northwestern Polytechnieal University，Xian 710072，China)Abstract：The core of the introduction of the full paper consists of：in view of the defects such as great linearizationeiTor and hi【gh calculating complexity which exist when the extended Kalman filter(EKF)is applied in coo
21、perativenavigation method of AUV(autonomous underwater vehicles)in a network of moving acoustic beacons，a new method based on unscented Kalman filter(UKF)，which we believe is more effective than those already available，isproposedOur new method is explained in sections l and 2Section l briefs the exp
22、erimentally validated MLBL(moving long baseline)concept proposed by Jirome Vagenay et al in Ref7Section 2 is entitled“Implementationof Our UKF Based Filtering Algorithm”Subsection 21 establishes its kinematic modelSubsection 22 implemerits our algorithm using MLBL concept and UKF；it ends up with a 3
23、-step detailed procedure for performing itera-tive calculationsSimulation results，presented in Fig4 and Table 1 and comparing UKF with traditional EKF，show preliminarily that UKF operation call suppress the errors of navigation positions and it performs better in navigation precision，therefore，it is
24、 indeed a more effective filter for AUV cooperative navigationKey words：algorithms，auonomous underwater vehicles，calculations，computational complexity，errors，iterativemethods，Kalman filtering，kinematics，models，navigation，nonlinear iystems，simulation，underwater acoustics；cooperative navigation，moving
25、 acoustic networkr琴a萍苔鸳莽岛氧祭苔鹫莽塔分零凌薄笞譬雾喀昂祭笞啦莽g譬瀑瘩黾豸啦莽若嗡祭苔啦莽g譬零茜鱼岔弛莽苍黾祭稿啦莽苍邕疹迎$苕叼莽茜氧牙弛$笞啦薄西北工业大学学报)2008年至2010年论文英文摘要字符数在2000以上的情况西北工业大学学报)2008年(第26卷)、2009年(第27卷)、2010年(第28卷)的全部学术论文被工程索引数据库(Ei Compendex)收录的英文摘要字符数在2000以上的有5篇，具体情况如下表：定 编号 卷 期 页码 作者 字符数2008 2008151 l 195859 26 l 1519 高俊宁、黄英 20592008 2008
26、15 11 195875 26 1 99103 姜红开、王仲生、何正嘉 21062008 20081511195877 26 1 110115 张科施、李为吉 2039张莹、徐伟、Zhang Tianshu，2008 2I)09041 187344726 6 659667 390lYang Xiaoli，Xu Cunli，方同2009 20093012214347 27 3 305309 张群、徐华胜、钟华贵、侯敏杰 2045徐滨2011年12月万方数据一种基于UKF的AUV移动声学网络协同导航方法作者： 刘明雍， 沈超， 张立川， 龚雪晴， Liu Mingyong， Shen Chao， Zhang Lichuan， GongXueqing作者单位： 西北工业大学航海学院,陕西西安,710072刊名： 西北工业大学学报英文刊名： Journal of Northwestern Polytechnical University年，卷(期)： 2011,29(6)本文链接：http:/
