基于SLAM的水下导航算法及仿真分析

第２９卷第１０期　仪　器　仪　表　学　报　Ｃｈｉｎｅｓｅ　Ｊｏｕｒｎａ】ｏｆ　Ｓｃｉｅｎｔｉｆｉｃ　Ｉｎｓｔｒｕｍｅｎｔ　Ｖｏ１．２９　Ｎｏ．１Ｏ　２００８年ｌ０月　０ｃｔ．２００８　基于ＳＬＡＭ的水下导航算法及仿真分析术　孙枫　，王文晶。，刘付强　，朱哈尔滨怡　１５０００１；　（１哈尔滨工程大学自动化学院２北京空问飞行器总体设计部摘北京１０００９４）　要：起源于移动机器人定位的同步构『皋【定位（ｓｉｍｕｌｔａｎｅｏｕｓ　ｌｏｃａｌｉｚａｔｉｏｎ　ａｎｄ　ｍａｐｐｉｎｇ，ＳＬＡＭ）算法可以克服基于先验网的水下　导航算法的　足，实现水卜载体的自主导航。本文给出了基于ＳＬＡＭ算法的水下导航算法的系统构建和执行过程，建立了状态　方程和观测方程，讨论了新特征加入的问题，并利用模拟数据进行了仿真。仿真结果表明，无论是采用航渡还是区域探索航行　方式，该导航算法的定位精度均高于　纯推位方法，具有广阔的应用前景。　关键词：水下导航；同步构冈定位；扩展卡尔曼滤波；仿真　中图分类号：ＴＰ３０１　文献标识码：Ａ　国家标准学科分类代码：５１０．８０　Ｕｎｄｅｒｗａｔｅｒ　ｎａｖｉｇａｔｉｏｎ　ａｌｇｏｒｉｔｈｍ　ｂａｓｅｄ　ｏｎ　ＳＬＡＭ　ａｎｄ　ｉｔｓ　ｓｉｍｕｌａｔｉｏｎ　ａｎａｌｙｓｉｓ　Ｓｕｎ　Ｆｅｎｇ　，Ｗａｎｇ　Ｗｅｎｊｉｎｇ　，Ｌｉｕ　Ｆｕｑｉａｎｇ　，Ｚｈｕ　Ｙｉ’　（　Ａｕｔｏｍａｔｉｏｎ　Ｃｏｌｌｅｇｅ，Ｈａｒｂｉｎ　Ｅｎｇｉｎｅｅｒｉｎｇ　Ｕｎｉｖｅｒｓｉｔｙ，Ｈａｒｂｉｎ　１５０００１，Ｃｈｉｎａ；　２　Ｂｅｒｉｎｇ　Ｉｎｓｔｉｔｕｔｅ　ｏｆ　Ｓｐａｃｅｃｒ￣＆ｓｔｅｍ　Ｅｎｇｉ￣ｅｒｉｎｇ，Ｂｅｉｊｉｎｇ　１０００９４，Ｃｈｉｎａ）　Ａｂｓｔｒａｃｔ：Ｔｈｅ　ｓｉｍｕｌｔａｎｅｏｕｓ　ｌｏｃａｌｉｚａｔｉｏｎ　ａｎｄ　ｍａｐｐｉｎｇ　ａｌｇｏｒｉｔｈｍ，ｗｈｉｃｈ　ｉｓ　ｏｒｉｇｉｎａｔｅｄ　ｆｒｏｍ　ｍｏｂｉｌｅ　ｒｏｂｏｔ　ｌｏｃａｌｉｚａｔｉｏｎ，　ｃａｎ　ｏｖｅｒｃｏｍｅ　ｔｈｅ　ｄｉｓａｄｖａｎｔａｇｅｓ　ｏｆ　ｔｈｅ　ｕｎｄｅｒｗａｔｅｒ　ｎａｖｉｇａｔｉｏｎ　ｍｅｔｈｏｄｓ　ｂａｓｅｄ　ｏｎ　ｐｒｉｏｒ　ｍａｐｓ，ｈｅｎｃｅ　ｒｅａｌｉｚｅ　ｔｒｕｅ　ａｕｔｏｎ—　ｏｍｏｕｓ　ｎａｖｉｇａｔｉｏｎ　ｏｆ　ｔｈｅ　ｕｎｄｅｒｗａｔｅｒ　ｖｅｈｉｃｌｅｓ．Ｔｈｅ　ｃｏｎｓｔｕｃｔｉｏｎ　ａｎｄ　ｅｘｅｃｕｔｒｉｏｎ　ｏｆ　ｔｈｅ　ｕｎｄｅｒｗａｔｅｒ　ｎａｖｉｇａｔｉｏｎ　ｓｙｓｔｅｍ　ｂａｓｅｄ　ｏｎ　ＳＬＡＭ　ａｒｅ　ｉｎｔｒｏｄｕｃｅｄ．ａｎｄ　ｔｈｅ　ｓｔａｔｅ　ｅｑｕａｔｉｏｎ　ａｎｄ　ｏｂｓｅｒｖａｔｉｏｎ　ｅｑｕａｔｉｏｎ　ａｒｅ　ｅｓｔａｂｌｉｓｈｅｄ．Ｆｕｒｔｈｅｒｍｏｒｅ，ｔｈｅ　ｉｓｓｕｅ　ｏｆ　ａｄｄｉｎｇ　ｎｅｗ　ｆｅａｔｕｒｅｓ　ｉｓ　ｄｉｓｃｕｓｓｅｄ．ａｎｄ　ｓｉｍｕｌａｔｉｏｎ　ｄａｔａ　ａｒｅ　ｕｓｅｄ　ｔｏ　ｔｅｓｔ　ｔｈｅ　ｓｙｓｔｅｍ　ｐｅｒｆｏｒｍａｎｃｅ．Ｓｉｍｕｌａｔｉｏｎ　ｒｅｓｕｌｔｓ　ｓｈｏｗ　ｔｈａｔ　ｔｈｅ　ａｌｇｏｒｉｔｈｍ　ｃａｎ　ａｃｈｉｅｖｅ　ｂｅｔｔｅｒ　ｐｒｅｃｉｓｉｏｎ　ｔｈａｎ　ｐｕｒｅ　ｄｅａｄ—ｒｅｃｋｏｎｉｎｇ　ｉｎ　ｂｏｔｈ　ｓｔｒａｉｇｈｔ　ｓａｉｌｉｎｇ　ａｎｄ　ｌｏｃａｌ　ｓｅａｒｃｈｉｎｇ　ｐａｔｔｅｒｎｓ，ａｎｄ　ｈａｓ　ａ　ｗｉｄｅ　ａｐｐｌｉｃａｔｉｏｎ　ｐｒｏｓｐｅｃｔ．　Ｋｅｙ　ｗｏｒｄｓ：ｕｎｄｅｒｗａｔｅｒ　ｎａｖｉｇａｔｉｏｎ；ｓｉｍｕｌｔａｎｅｏｕｓ　ｌｏｃａｌｉｚａｔｉｏｎ　ａｎｄ　ｍａｐｐｉｎｇ；ｅｘｔｅｎｄｅｄ　Ｋａｌｍａｎ　ｆｉｌｔｅｒ；ｓｉｍｕｌａｔｉｏｎ　足，开拓了无图导航的新方向，有望为水下导航开辟一条　１　引　言　新的途径。　同步构图定位（ｓｉｍｕｌｔａｎｅｏｕｓ　ｌｏｃａｌｉｚａｔｉｏｎ　ａｎｄ　ｍａｐｐｉｎｇ，　地球物理导航是上世纪末期出现的新型水下导航方　ＳＬＡＭ）算法的雏形是由Ｓｍｉｔｈ、Ｓｅｌｆ和Ｃｈｅｅｓｅｍａｎ　于２０　法，主要包括地磁导航、重力导航、地形导航等。虽然地　球物理导航方法可以达到很高的导航精度，但是由于它　们属于基于先验图的导航方法，导航精度受先验图测绘　精度的影响很大，且航行区域受到先验图大小的限制。　２０世纪末发展起来的同步构图定位算法，克服了上述有　图方法易受先验图测绘精度和实时测量精度影响的不　收稿日期：２００７－０５　Ｒｅｃｅｉｖｅｄ　Ｄａｔｅ：２００７－０５　世纪８０年代提出的，最先用于陆上机器人的导航。该算　法主要是在运载体运动时预测自身位置，并依靠对周围　环境的感知即对位置保持不变的“特征”或称“路标”的　测量，通过滤波估计来修正运载体和特征的位置估计，在　实现对运载体导航的同时，构造用这些特征表示的精确　的环境地图。目前，算法的应用已不仅仅局限于室内　基金项目：国家自然科学基金（６０７７５００１）资助项目　第ｌ０期　孙枫等：基于ＳＬＡＭ的水下导航算法及仿真分析　２０７３　环境，而是进一步延伸到了室外　、水下　和航空　环　境。本文着重研究基于ＳＬＡＭ的水下导航应用方法。　ＡＸ＝【　Ａｙ△　］ｒ，协方差为△Ｐ，则由坐标变换公　为：　式可知，航行器预测位置　２水下ＳＬＡＭ导航算法描述　与日常生活中用江河、湖泊、街道、房屋等标志性建　筑表示地图的方法类似，告水Ｆ航行器具有通过自身携　带的传感器感知周围环境・Ｉｌ明显的静止物体的能力，诸　如珊瑚礁、水下建筑、失事船等，则可将这些显而易见的　物体提取为二维点特征，用这些二维点特征在全局坐标　系下的位置作为“路标”，由此构成一幅可描述当前水下　环境的“特征地图”。　为此，算法首先定义了一个全局坐标系作为基准坐　标系，它是以航行器的初始位置为原点、以东向为　轴而　建立起来的二维直角坐标系，航行器状态及地图状态均　是在这个坐标系下表示的。航行器状态用航行器在全局　坐标系下的位置和航向表示为Ｘ　＝［　Ｙ　］　，协方　差为Ｐ　把航行器观测到的所有特征组成另一个地图状　态向量　即Ｘ　　，＝　［Ｆ　…　Ｆ　］。　＝　［　。Ｙ．　…　Ｙ　］　，协方差为Ｐ　。由于航行器一边　航行一边观测特征，每一时刻都可能有以前未观测过的　新特征进入视野，在观测到新特征后需要将新特征也加　入到地图向量中，故而地图状态　的维数不固定，可用　新特征的位置对其进行扩维。为使航行器和地图相关并　在卡尔曼滤波的框架下对航行器和特征的位置同时进行　更新，算法定义了一个系统联合状态向量　，其维数可随　的变化而作相应扩展。系统向量及其协方差矩阵分　埘为：　］’Ｐ＿　状态协方差Ｐ的各部　分体现了航行器状态之间、航行器与特征之间、特征与特　征之间的相关性，是将航行器状态与地图特征状态联系　起来的桥梁和纽带。正是凶为协方差矩阵Ｐ能够保持这　种相关性，才保证了算法生成的特征地图的收敛性。　３系统执行过程　算法的执行是一个递推的过程，可以分为２个阶段：　预测（时间更新）阶段和修正（测量更新）阶段　，具体流　程如图１所示。　３．１状态方程的建立　水下航行器首先通过自身携带的推位传感器如　测速、测向仪器等，测量航行器在相邻时刻的状态变　化量，建立系统的状态方程，预测自身位置。例如，若　航行器状态改变量用相邻时刻的航向改变量以及在　航向方向上和垂直航向方向上的距离变化量表示为　罔１导航算法执行过程　Ｆｉｇ．１　Ｓｃｈｅｍａｔｉｃ　ｏｆ　ＳＬＡＭ　ａｌｇｏｒｉｔｈｍ　ｉｍｐｌｅｍｅｎｔａｔｉｏｎ　：［　Ｙ　】　＝－厂（Ｘ　，ＡＸ）＝　ｒ量　＋Ａｘｃｏｓ　一Ａｙｓｉｎ　］　ｌ多　＋Ａｘｓｉｎ　＋Ａｙｃｏｓ　Ｉ　（１）　Ｌ　＋△　Ｊ　认为环境中的特征静止不动，因此，系统状态中的特　征部分不发生改变，故特征预测位置可表示如下：　＝Ｘ　（２）　结合式（１）和式（２），系统预测状态向量　一可表　示为：　ｒ　１　Ｘ－　【　Ｊ　ｇ　（　）　其状态预测协方差Ｐ一为：　Ｐ一＝Ｖｇ　ＰＶｇ　Ｔ十Ｖｇ△　△ＰＶｇ　（４）　式中：Ｖｇ　代表ｇ关于ＡＸ在估计（Ｘ，ＡＸ）处的雅可比矩　阵，其余表示方法类似，以后不再赘述。　３．２建立观测模型　在预测自身状态的同时，航行器利用外部传感器如　水下照相机、侧扫声纳、前视声纳、合成孔径声纳等感知　周围环境是否存在特征，得到一些量测值。这些量测可　能产生于系统状态变量中已有的特征（即以前观测过的　特征），也可能来源于新的特征（即尚未观测过的特征），　还可能是虚警（因测量传感器存在误差导致）。产生来　源不同，对量测的处理方法也不同：只有那些已有特征的　２０７４　仪器仪表学报　第２　９卷　量测＿『用了滤波更新过程；新特征的量测需要转换后加　入到状念向量中，成为地图巾新的成员，具体见３．３节；　而虚警则　接从量测集中剔除，不予考虑。因此，需要将　量测量与已有特　进行匹配，考察量测的来源，以确定该　量测的用途，这一过程就被称为数据关联过程．．数据关　联完成后，利用与已有特　关联上的观测量通过扩展卡　尔曼滤波来对状态进行更新，可实现对航行器和特征位　置的同时修ｒＦ。　没航行器利用外部测量传感器得到的量测量为各特　征相对于当前航行器的方位和距离．例如可设在　时刻　获得的・个量测为Ｚ＝［ｒ　０］　。若确定了该量测与状态　变量中某个已存在的特征Ｆ　＝（　，　）　关联，则根据预　测航行器他置和特　Ｆ　位置，可得到估汁观测模型为：　『　（王　－－，）：＋（　一　！］　ｚ＝　…（　）－６　ｊ　其测量方差为Ｒ。则测量新息Ｉ，：　＝Ｚ—Ｚ　（６）　新息协　：　Ｓ＝Ｖ　ｈ　Ｐ　Ｖ　ｈ：＋Ｒ　（７）　卡尔曼增益＇’，：　Ｗ＝Ｐ—Ｖ　ｈ：Ｓ。。　（８）　更新后的位置　和协方差Ｐ　：　Ｘ　＝Ｘ一＋Ｗｖ　（９）　Ｐ　＝Ｐ一一ＷＳＷ　（１０）　通过式（５）～（１０），就【１Ｉ以利用测量完成对航行器　状态和特征状态的同步更新过程。若没有观测到新的特　，则可以进入下一时刻状态预测和更新的迭代递推过　程；否则，必须进行扩维，实现地图的自动扩展。　３．３新特征的加入　存航行器运动过程中，它不断探索新的环境并发现　新的特征，　此存完成对已有特征的更新后，需要对其状　态变量进行扩维处理，将新特征加入到状态中去，建立航　行器状态及地图已有特征与新特征间的关系，以构造精　确、收敛的地图。　设已确定量测Ｚ＝［ｒ　０］　来源于一新特征，则可将　新特征存全局坐标系下的位置表示为：　ｒ　＋＃＇ＣＯＳ（０＋（ｂ　）１　。＝“（　ｚ）＝Ｉ【　＋ｒｓｉｎ（０＋￥　）ＪＩ　　（１１）　将新特征加入到系统特征向量中，扩充后的新系统　状态向量和协方差分别为：　＝　。（１２）　ｒ　Ｐ　尸：　Ｐ　］　Ｐ：。ｌ　Ｐ　Ｐ　Ｐ＿　Ｉ　（１３）　Ｌ　Ｐ　Ｐ　Ｐ　．　式中：　为观测方差，Ｐ　＝ＶｉｉｘＰ　Ｐ　＝Ｖ　Ｈ　ｐ　ｒ，Ｐ：　，。ｒｖ　Ｕｘ　ＰＶ＋　Ｖ比：　ＲＶ　Ｈ　。　，通过加入新的特元素，算法实现＿ｒ地图的逐步自　动扩展，使导航免受航行区域大小的限制，这也是无图方　法相比丁有图方法的优点之一。　４仿真结果及分析　为验证算法精度，利用ＭＡＴＬＡＢ软件对水下航行器　分别采用区域探索和航渡这两种方式航行的情况进行了　仿真，根据仿真结果分析算法性能。　４．１区域探索试验　区域探索仿真环境设为一片２００　ｉｌｌ×２００　Ｉｌｌ的海域，　该范同内均匀分布着１６个点特征。针对区域探索航行　方式的特点，假设航行器从区域的（５０　ｍ，５０　ｍ）位置处　发，沿边长为１００　Ｉｌｌ的正方形轨迹以匀速５　ｍ／ｓ逆时　针航行，即每８０　Ｓ绕区域１周，共绕区域航行　５周，总　运行时间为４００　Ｓ。　航行器携带的传感器视野可覆盖以自身位置为中　心、半径１００　ｍ内的前方半圆形区域，测量频率为１　Ｈｚ。　设传感器无虚警，测量值存在高斯随机误差，其距离测量　误差均值为零，方差为０．２５　ｍ　，方位测量误差均值也为　零，方差为２．２５×１０　ｒａｄ　。航行器推位过程中同样存　存均值为零的高斯随机误差作用。　方向距离、Ｙ方向距　离和航向的误差方差分别为１　Ｉｌｑ　、１　Ｉｌｌ＝和５．２９　Ｘ　１０一　ｔａｄ　。　用航行器第一次观测到的所有特征位置对地图向量　进行初始化。设航行器的初始位置和方位存在偏差，　为Ｘ，　＝［５２．１　４８．３　０．００８］’，初始航行器协方差矩阵　『２０　０　０　］　和观测方差阵分别为：Ｐ　＝ｌｌ　　０　２００　０　０．　０　ｌ００１　Ｊ　，　Ｒ＝　。．　图２是单纯推位得到的航行器航行５周的航迹，图３　是相应的误差曲线。在与单纯推位同样的航行器状态预　测误差作用下，利用基于ＳＬＡＭ的导航算法得到了另一　组定位结果，如图４所示。　第１０期　孙枫等：基于ＳＬＡＭ的水下导航算法及仿真分析　２０７５　尽　罔２推位航迹　Ｆｉｇ．２　Ｄｅａｄ—ｒｅｃｋｏｎｉｎｇ　ｔｒａｊｅｃｔｏｒｙ　６０　４０　２Ｏ　０　２０　０　５０　１００　ｌ５Ｏ　２００　２５０　３００　３５０　４００　０　５Ｏ　ｌＯＯ　ｌ５Ｏ　２００　２５０　３００　３５０　４００　时间／ｓ　３网２对应的推位误差变化曲线　Ｆｉｇ．３　Ｄｅａｄ—ｒｅｃｋｏｎｉｎｇ　ｅｒｒｏｒ　ｆｏｒ　ｆｉｇ．２　２Ｏ０　１８０　１６０　１４０　１２０　景１００　８０　６０　４Ｏ　２Ｏ　０　ｘ／ｍ　罔４　域探索方式，航行器真实、　推位、滤波位置及特征地图　Ｆｉｇ．４　Ｖｅｈｉｃｌｅ　ｒｅａｌ　ｐｏｓｉｔｉｏｎ，ｄｅａｄ—ｒｅｃｋｏｎｉｎｇ　ｐｏｓｉｔｉｏｎ，　ｉｆｌｔｅｒｅｄ　ｐｏｓｉｔｉｏｎ　ａｎｄ　ｆｅａｔｕｒｅ　ｍａｐ　ｉｎ　ａｒｅａ　ｒｅｃｏｎｎａｉｓｓａｎｃｅ　ｍｏｄｅ　图４中圆圈、方块和五角星分别代表最后１周（即最　后８０　ｓ内）的航行器真文位置、单纯推位和ＳＬＡＭ滤波后　的位置。凶为单纯推位和ＳＬＡＭ滤波中的航行器状态预　测误差相同，从这两条轨迹的对比，可以很自然地得出这　样的结论，即经过ＳＬＡＭ滤波环节后得到的定位精度要　大大高于无滤波的单纯推位方法。　吸／　匠椒　ｐＥＩ／粼　足　图５是经ＳＩ　ＡＭ滤波后航行器状态估计误差及误差　２ｏ＂边界的变化曲线。由于航行器的位置和方位误差在　零值附近小幅变化，且始终未超出由２０＂协方差边界定义　的９５％置信界限，说明算法十分稳定，滤波性能良好。　由此可以看出，若航行器可以重复探索区域，在此过程中　对区域内的特征进行多次观测，则航行器状态可以达到　误差有界　ｇ　１０　５　０　雹　５　１０　０　５０　ｌ００　１５０　２００　２５０　３００　３５０　４００　１　ｊ；１ｌｊ　＿叵．１　０．　０．０　　Ｉ篡　厦一０．０　５｝ｒ、＾　、、ｒ　接一０　ｌ　ｌ：　．．．．．．．．．．．．．．。．．．．．．．．．．．．．．．．　．．．．．．．．．　．．．．．　．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．　．．．．．．．．．．．．．．．．　．．．．．．一　０　５０　１Ｏ０　ｌ５０　２００　２５０　３００　３５０　４００　时问／ｓ　冈５航行器状态误差及２０＂协方差边界变化曲线　５　５　Ｏ　Ｆｉｇ．５　Ｖｅｈｉｃｌｅ　ｅｓｔｉｍａｔｉｏｎ　ｅｒｒｏｒ　ａｎｄ　２０＂ｅｏｖａｒｉａｎｃｅ　ｂｏｕｎｄｓ　｛ｌ　娶。　０　５Ｏ　１Ｏ０　１５Ｏ　２００　２５０　３００　３５０　４００　冈６特征１～４的估计误差变化曲线　Ｆｉｇ．６　Ｅｓｔｉｍａｔｉｏｎ　ｅｒｒｏｒｓ　ｆｏｒ　ｆｅａｔｕｒｅ　１—４　图４中均匀散布在整个仿真区域内的ｌ６个三角形是　仿真设定的特征位置，星号是仿真结束时的特征估计位　置。将地图中的特征按其首次被观测的次序编号，分别位　于（９０　ｍ，１２５　ｍ）、（１４５　ｍ，２０　ｍ）、（１２５　ｍ，１００　ｉｌ１）和　（８０　ｍ，３５　ｍ）处的４个特征均是最先被观测到的，将它们　依次标记为特征１－４，图６给出了其估计位置与真实位置　之间误差随时间变化的曲线。可以看出，经过第１圈探索　即８０　ｓ之后，由于对系统状态中已有的特征进行再次观　测，各特征的误差变化幅度开始减小，在探索３圈即２４０　ｓ　之后，各特征误差逐渐趋于一个恒值，但不趋于Ｏ。这个常　值误差是由航行器状态初始不确定度产生的”ｊ。　２０７６　仪器仪表学报　第２　９卷　Ｅ　面　糕　犍　一一　ｋ　一臂‰　一蝴　一“　一肚＾州　一　Ｉｍ一　一一～一’一１　０　５０　１００　Ｉ　５０　２００　２５０　３００　３５０　４００　Ｏ　２　童０　。。　＿１）＿ｌ　０．２　０　５０　１００　１５０　２００　２５０　３００　３５０　４００　时间／ｓ　罔７　区域探索方式　的新息序列　Ｆ　ｉｇ．７　Ｉｎｎｏｖａｔｉｏｎ　ｓｅｒｉｅｓ　ｉｎ　ａｒｅａ　ｒｅｃｏｎｎａｉｓｓａｎｃｅ　ｍｏｄｅ　图７足新息随时间的变化曲线。观测新息均值为０　的现象冉次蜕明了ＳＬＡＭ导航算法是具有收敛性的。　４．２航渡试验　航渡试验的仿真环境设为一片ｌ　２００　ｍ×ｌ　２００　ｎ　的　海域，该区域内随机分布着１２０个特征。航行器从（１０　ＩＴＩ，　４ＯＯ　ｎ１）处以恒速１０　ｎＶｓ沿　轴方向直线运动了１００　Ｓ。设　使用的推位及测量传感器、所添加的推位及测量随机噪声　的类型和指标均与４．１节文验相同，不同之处仅在于测量　传感器的　程选用大量程２５０　Ｉ］１。航行器初始状态为　Ｘ　＝［１ｌ。４　３９９．２　０．０１　。　存图８所示的推位误差作用卜，得到了如图９和图　ｊ０所示的定位结果。图９中图例与图４完全相同。由这　组图像可以看出，算法大幅减缓了定位误差的增长速率，　其对航行器位置和方位的修正是卓有成效的。即使在推　位得到的结果已经极其不可靠的情况下，由ＳＩ　ＡＭ获得　的估计还是可以令人满意的。　｛２０　１０　苣０　－ｌ０　鳓飘１００　｝一一一—／／　／——一　　５０　—商—　—布—　—斋—　—　０　焉０．３　差．０　．０　ｊ０　２Ｏ　３Ｏ　４０　５０　６０　７０　８０　９Ｏ　１ＯＯ　时间，ｓ　罔８推位误差变化ｆｆｆｌ线　Ｆｉｇ．８　Ｄｅａｄ—ｒｅｃｋｏｎｉｎｇ　ｅｒｒｏｒｓ　ｉｎ　ｓｔｒａｉｇｈｔ　ｌｉｎｅ　ｔｒａｊｅｃｔｏｒｙ　罔９航渡方式，航行器真实、　推位、滤波位置及特征地罔　¨ｇ．９　Ｖｅｈｉｃｌｅ　ｒｅａｌ　ｐｏｓｉｔｉｏｎ，ｄｅａｄ—ｒｅｃｋｏｎｉｎｇ　ｐｏｓｉｔｉｏｎ．　ｆｉｌｔｅｒｅｄ　ｐｏｓｉｔｉｏｎ　ａｎｄ　ｆｅａｔｕｒｅ　ｍａｐ　ｉｎ　ｓｔｒａｉｇｈｔ　ｌｉｎｅ　ｔｒａｊｅｃｔｍｗ　暑　宴　歪　０．１　ｒ　－ＩＸ　一０．０５Ｕ　三至乏　至三圣一～一　墨一０．Ｉ　Ｌ——　———　———　———．．———　——　———　———　——一　０　１０　２０　３０　４０　５Ｏ　时间，ｓ　罔ｌ０航渡方式，航行器状态误差　及２０＂协方差边界变化　线　Ｆｉｇ．１０　Ｖｅｈｉｃｌｅ　ｅｓｔｉｍａｔｉｏｎ　ｅｒｒｏｒ　ａｎｄ　２盯ｃｏｖａｒｉａｎｅｅ　ｂｏｕｎｄｓ　ｉｎ　ｓｔｒａｉｇｈｔ　ｌｉｎｅ　ｔｒａｊｅｃｔｏｒｙ　下面对图８～ｌ０进行具体分析。理论上讲，根据式　（１），在Ａｘ和ｂｙ随机误差量级相同的情况＿卜，经式（１）　运算后得到的　，　的误差增长速率通常要比互　大。多次　仿真结果也不约而同地证明了这一点：如图８所示，推位　中＇，方向误差要远远大于　方向误差。由于好的推位也　会得到相对较好的滤波效果，因此，图ｌ０中　方向的滤　波效果要优于ｙ方向。这就是当航行器沿　方向直线航　行时，滤波后通常　方向误差变化很小、而Ｙ方向误差增　大的原因所在。　在整个航行过程中共观测到了４８个特征，图ｌｌ是　仿真结束时的各特征估计误差，特征序号标记方法同４．１　节。由于航行器始终沿一个方向航行，对每个特征只能　进行有限次观测，因此随着航行器定位误差缓慢增长，后　第ｌ０期　孙枫等：基于ＳＬＡＭ的水下导航算法及仿真分析　２０７７　观测到的特征估计误差也相对较大。如图ｌ　ｌ所示，大序　号特征估计误差总体上要大于小序号特征的误差。　ｇ　鞭　七　特征序号　ｌ　１特征估计误差　Ｆｉｇ．１　１　Ｅｓｔｉｍａｔｉｏｎ　ｅｒｒｏｒ　ｆｏｒ　ｏｂｓｅｒｖｅｄ　ｆｅａｔｕｒｅｓ　需要说明的是，因为仿真目的是验证算法可行性，故　４．１～４．２节中所有仿真结果都是在数据关联完全正确　的条件下给出的。事实上，由于水下实际情况的复杂性　和测量误差的影响，测量与特征问很可能产生错误的数　据关联，一旦发生较为严重的错误关联，将导致滤波不稳　定甚至发散。此外，当工作范围很大、环境特征数目过多　时，系统状态维数的快速增长将使滤波过程中的矩阵计　算复杂度迅速膨胀，这会对处理器提出过高的要求。　此，如何提高算法的鲁棒性、容错性以及实时性，将是今　后研究的重点。　５　结　论　本文概述了同步构图与定位算法的原理，建立了系　统状态方程和量测方程，利用扩展卡尔曼滤波对算法进　行了仿真。针对水下航行器两种常用的航行方式即区域　探索和航渡方式，利用仿真结果证明了在同样的导航测　量干扰误差作用下，尽管误差的累积作用导致了单纯推　位方法的导航结果不可靠，但是由基于ＳＬＡＭ的水下导　航算法获得的导航定位精度却远高于推位精度，且表现　出了良好的收敛性，证实了基于ＳＬＡＭ的水下导航算法　的巨大应用潜力。尽管如此，数据关联模糊度和算法复　杂性却有可能引起算法的不稳定以及在线执行的困难，　在算法改进方面仍有很多工作需要完成。　参考文献　ＳＭＩＴＨ　Ｒ，ＳＥＬＦ　Ｍ，ＣＨＥＥＳＥＭＡＮ　Ｐ．Ｅｓｔｉｍａｔｉｎｇ　ｎｎｃｅｒ—　ｔａｉｎ　ｓｐａｔ—ｉａｌ　ｒｅｌａｔｉｏｎｓｈｉｐｓ　ｉｎ　ｒｏｂｏｔｉｃｓ［Ａ］．Ｐｒｏｃｅｅｄｉｎｇｓ　ｏｆ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ　Ｕｎｃｅｒｔａｉｎｔｙ　ｉｎ　Ａｒｔｉｉｆｃｉａｌ　Ｉｎｔｅｌｌｉｇｅｎｃｅ［Ｃ］．　１９８８：４３５－４６１．　［２］　ＬＩ　Ｍ　Ｈ，ＨＯＮＧ　Ｂ　Ｒ，ＣＡＩ　Ｚ　Ｓ，ｅｔ　ａ１．Ｎｏｖｅｌ　ｒａｏ—ｂｌａｃｋ一　ｗｅｌｌｉｚｅｄ　ｐａｒｔｉｃｌｅ　ｆｉｌｔｅｒ　ｆｏｒ　ｍｏｂｉｌｅ　ｒｏｂｏｔ　ＳＬＡＭ　ｕｓｉｎｇ　ｍｏ—　ｎｏｅｕｌａｒ　ｖｉｓｉｏｎ『Ｊ］．Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｊｏｕｒｎａｌ　ｏｆ　Ｉｎｔｅｌｌｉｇｅｎｔ　Ｔｅｃｈｎｏｌｏｇｙ，．２００６，１（１）：６３—６９．　［３］　ＣＨＲＩＳＴＩＡＮ　Ｂ，ＯＬＩＶＥＲ　Ｗ，ＢＥＲＮＡＲＤＯ　Ｗ．Ｕｓｉｎｇ　３　Ｄ　ｌａｓｅｒ　ｒａｎｇｅ　ｄａｔａ　ｆｏｒ　ＳＬＡＭ　ｉｎ　ｏｕｔｄｏｏｒ　ｅｎｖｉｒｏｎｍｅｎｔｓ［Ａ］．　Ｐｒｏｃｅｅｄｉｎｇｓ　ｏｆ　ｔｈｅ　２００３　１ＥＥＥ／ＲＳＪ　Ｉｎｔ．Ｃｏｎｆ．ｏｎ　ｌｎｔｅｌｌｉ—　ｇｅｎｔ　Ｒｏｂｏｔｓ　ａｎｄ　Ｓｙｓｔｅｍｓ［Ｃ］．２００３：１８８—１９３．　［４］　ＩＯＳＥＢＡ　Ｔ　Ｒ，ＳＥＢＡＳＴＩＥＮ　Ｄ　Ｒ．Ｃｏｎｃｕｒｒｅｎｔ　ｍａｐｐｉｎｇ　ａｎｄ　ｌｏｃａｌｉｚａｔｉｏｎ　ｕｓｉｎｇ　ｓｉｄｅｓｃａｎ　ｓｏｎａｒ　ｌ　Ｊ］．ＩＥＥＥ　Ｊｏｕｒｎａｌ　ｏｆ　Ｏｃｅａｎｉｃ　Ｅｎｇｉｎｅｅｒｉｎｇ，２００４，２９（２）：４４２４５６．　［５］　ＪＯＮＧ　Ｈ　Ｋ．ＳＡＬＡＨ　Ｓ．Ａｉｒｂｏｒｎｅ　ｓｉｍｕｈａｎｅｏｕｓ　ｌｏｅａｌｉｚａ—　ｔｉｏｎ　ａｎｄ　ｍａｐ　Ｂｕｉｌｄｉｎｇ［Ａ］．Ｐｒｏｃｅｅｄｉｎｇｓ　ｏｆ　ｔｈｅ　２００３　ｌＥＥＥ　Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ　Ｒｏｂｏｔｉｃｓ＆Ａｕｔｏｍａｔｉｏｎ　『Ｃ］．２００３：４０６－４１１．　［６］　ＨＵＧＨ　Ｄ　Ｗ．ＴＪＭ　Ｂ．Ｓｉｍｕｌｔａｎｅｏｕｓ　ｌｏｃａｌｉｚａｔｉｏｎ　ａｎｄ　ｍａｐ—　ｐｉｎｇ：Ｐａｒｔ　Ｉ［Ｊ］．１ＥＥＥ　Ｒｏｂｏｔｉｃｓ＆Ａｕｔｏｍａｔｉｏｎ　Ｍａｇａｚｉｎｅ，　２００６，１３（２）：９９—１０８．　［７］　ＧＡＭＩＮＩ　Ｍ　Ｗ　Ｍ　Ｄ．ＰＡＵＬ　Ｎ．ＨＵＧＨ　Ｆ　Ｄ　Ｗ．Ａ　ｓｏｌｕ—　ｔｉｏｎ　ｔ０　ｔｈｅ　ｓｉｍｕｌｔａｎｅｏｕｓ　ｌｏｃａｌｉｚａｔｉｏｎ　ａｎｄ　ｍａｐ　ｂｕｉｌｄｉｎｇ　（ＳＬＡＭ）ｐｒｏｂｌｅｍ［Ｊ］．ＩＥＥＥ　Ｔｒａｎｓａｃｔｉｏｎｓ　ｏｎ　Ｒｏｂｏｔｉｃｓ　ａｎｄ　Ａｕｔｏｎｍｔｉｏｎ，２００１，１７（３）：２２９—２４１．　［８］　ＰＡＵＬ　Ｎ．ＬＥＯＮＡＲＤ　Ｊ　Ｊ．Ｐｕｒｅ　ｒａｎｇｅ—ｏｎｌｙ　ｓｕｂ—ｓｅａ　ＳＬＡＭ　Ｉ　Ａ．Ｐｒｏｃｅｅｄｉｎｇｓ　ｏｆ　ｔｈｅ　２００３　ＩＥＥＥ　ｉｎｔｅｍａｔｉｏｎａｌ　ｃｏｎｆｅｒ—　ｅｎｃｅ　ｏｎ　Ｒｏｂｏｔｉｃｓ＆Ａｕｔｏｍａｔｉｏｎｌ　Ｃ【．２００３：１９２１—１９２６．　作者简介　ｆｒｏｍ　Ｈａｒｂｉｎ　Ｅｎｇｉｎｅｅｒｉｎｇ　Ｕｎｉｖｅｒｓｉｔｙ　ｉｎ　２００７；ｎｏｗ　ｓｈｅ　ｉｓ　ａ　ＰｈＤ　ｃａｎｄｉｄａｔｅ　ｉｎ　Ｈａｒｂｉｎ　Ｅｎｇｉｎｅｅｒｉｎｇ　Ｕｎｉｖｅｒｓｉｔｙ．Ｈｅｒ　ｒｅｓｅａｒｃｈ　ｉｎｔｅｒ—　ｅｓｔｓ　ｉｎｃｌｕｄｅ　ｍａｒｉｎｅ　ｐａｓｓｉｖｅ　ｎａｖｉｇａｔｉｏｎ，ａｎｄ　ｓｉｍｕｌｔａｎｅｏｕｓ　ｌｏｅａｌｉｚａ—　ｔｉｏｎ　ａｎｄ　ｍａｐｐｉｎｇ．　Ｅ—ｍａｉｌ：ｃｒｉｓｔｉｎｗｗｊ＠１６３．ｃｏｎ