基于随机信标的水下SLAM导航方法

2015-06-05刘明雍董婷婷张立川

系统工程与电子技术 2015年12期

关键词：信标航行数量

刘明雍，董婷婷，张立川

（西北工业大学航海学院，陕西西安710072）

基于随机信标的水下SLAM导航方法

刘明雍，董婷婷，张立川

（西北工业大学航海学院，陕西西安710072）

研究了基于随机信标的水下同时制图定位（simultaneous localization and mapping，SLAM）导航定位方法。信标导航是目前导航领域的研究热点，但往往需要提前对信标位置进行标定。对此文中提出一种无需位置标定的随机信标导航方法，即在信标随机散布的情况下，通过量测信标和航行器间的距离和方位，用SLAM方法对随机信标位置进行估计，从而实现对航行器的导航，并在不同的信标密度和观测误差下分析了其导航精度。仿真结果表明，该导航方法具有良好的收敛性和定位精度。

自主水下航行器；随机信标；导航；同时制图定位

0 引言

现阶段，无需人类参与的自主水下航行器被广泛应用于海洋资源的开发利用中，而航行器的自主导航能力是衡量其性能的重要指标之一。虽然导航方法种类繁多，但是由于水下环境的特殊限制，使得水下导航方法受到一定限制。目前，水下导航方法多为惯导和其他一些辅助导航设备的组合导航系统，常用的有惯导／长基线组合导航系统，长基线水声定位系统由安装在海底的应答器和被定位载体的问答器组成，需要提前对水下应答器进行位置校准，人力物力花费量大，且维护困难；惯性／全球定位系统的组合导航方法需要定期接收全球定位系统（global position system，GPS）信号，导致隐蔽性差，且不利于深水航行等［12］；惯导／多普勒组合导航方法没有直接量测位置，对位置进行校正，因此误差仍旧呈较明显的发散趋势，不利于长时间航行。

因此，信标导航方法由于不需要GPS信号，且直接对航行器位置进行校准等特点成为导航研究领域一种新的趋势［3］，目前信标导航方法的研究多为基于固定信标的导航［4-5］，固定信标导航分为单信标导航和多信标导航。单信标导航方法简单，容易实施，且具有很好收敛性［6］，但也具有一定的缺陷性。由文献［7- 8］对单信标定位可观测性分析可知，单信标导航在一定条件下可以满足导航性能的要求，可是该方法是利用单个信标距离信息确定航行器的位置，使得求解不充分，所以必须在设计适当的机动路径的条件下利用该方法进行导航。考虑一般自主水下航行器（autonomous umderwater vehicle，AUV）的实际航行路径，在一些情况下，并不能满足这个要求。文献［9］提出的基于固定信标的导航方法需提前对信标位置进行标定，花费大量的人力物力，不经济也不能满足导航及时性的要求。文献［10- 11］提出了基于移动信标的导航方法，该方法对导航范围没有限制，且能满足可观测性的要求，但对信标自身的要求较高且需要提前对信标位置进行标定。

目前，国内外关于SLAM导航方法的研究很多［12］，尤其是在室内［13］、室外［14］、陆地机器人［1516］以及航空领域［17］，然而水下SLAM研究相对较少，主要是因为传感器在水下应用时感知能力减弱，且水下环境中参考点较少，使得算法执行过程中的特征点难以提取。

本文提出了一种基于随机信标的水下SLAM导航方法，并重点分析了AUV区域导航中该方法的优势。该方法信标位置无需提前标定，且根据精度要求，信标数量可以人为控制，也能当作特征参考点，克服了水下SLAM参考点不足的缺陷。

1 随机信标定位问题描述

随机信标是指位置事先未知但具有通信功能的信标。在一些特殊的情况下，如AUV自身携带的导航设备发生故障而又对航行器导航精度要求较高等，需要临时对AUV进行定位和导航，可以利用飞机向一定范围海域散布信标，但信标位置未知，考虑利用同时制图定位（simultaneous localization and mapping，SLAM）算法对航行器和信标进行定位，如图1所示，通过测量航行器和信标之间的距离R1，R2，…，Ri（i＝1，2，…，N）和方位角对航行器不断进行定位和SLAM地图信标的不断扩充。

图1 随机信标定位示意图

SLAM是指自主航行器在未知环境中行驶时，依赖传感器所获得的信息进行增量式环境建模，同时利用所创建的环境地图估测其位姿［1820］。假设航行器初始位置已知，通过观测信标，可对信标相对位置进行估计，重新观测到以前观测的信标可以对航行器的位置进行更新，从而对航行器进行定位。假设信标是具有零输入扰动的静态路标，则第i个信标的模型可以简化为

当观测到下一个新信标时，这个信标的位置信息就被扩充到地图中，初始条件的不确定性和相互关系也相应地扩充到了组合SLAM滤波器中。

2 基于随机信标的SLAM导航算法

基于随机信标的SLAM导航算法分为3个主要步骤。

2.1 状态方程

在航行器运动学模型已知的情况下，通过观测得到相对航行器的信标位置，可以建立增广的状态估计数学模型，运用扩展卡尔曼滤波对状态进行估计。定义系统的状态量为

式中，xv（k）是航行器运动状态，包括位置和姿态；xb（k）是信标地图状态向量，随着观测到的新的信标位置的加入，其维数不断增加。

式中，p（k）＝［x（k） y（k）］，表示航行器的位置坐标；φ（k）表示航向；bi（k）（i＝1，2，…，n）表示信标在地图中的位置。

若航行器状态改变量用相邻时刻的航向改变量以及在航向方向上和垂直航向方向上的距离变化量表示为ΔX＝［Δx Δy Δφ］T，则航行器位置可以描述为

根据上述状态方程求雅克比矩阵

式中，I是随信标位置求导，为单位矩阵。

2.2 观测方程

对上述观测方程求其雅克比矩阵：

所以，随机信标对航行器的定位问题转化为标准的非线性滤波问题，由于状态方程和量测方程的雅克比矩阵均已求得，可直接对其进行扩展卡尔曼滤波。

其测量方差为R。测量新息v：

新息协方差矩阵S：

卡尔曼滤波增益W：

更新后的位置^X*和协方差P*：

通过式（9）～式（13），就可以利用测量完成对航行器状态和特征状态的同步更新过程。若没有观测到新的特征，则可以进入下一时刻状态预测和更新的迭代递推过程。否则，必须进行扩维，实现地图的自动扩展。

2.3 观测更新

航行器在运动过程中，不断探索新的环境并发现新的信标，因此在完成对已有信标位置的更新后，需要对其状态变量进行扩维处理，将新信标位置加入到状态中去，建立航行器状态及地图已有特征与新特征间的关系，以构造精确、收敛的地图。

设已确定量测z＝［r φ］T来源于一新特征，其中x和y表示航行器的位置坐标；r和φ分别表示新特征和航行器之间的相对距离和方位角，可将新特征在全局坐标系下的位置表示为

将新特征加入到系统特征向量中，扩充后的新系统状态向量为

通过加入新的信标，算法实现了地图的逐步自动扩展。

3 仿真与分析

在设定好初始条件下，使航行器分别实现圆圈形和蛇形区域搜索两种轨迹，并将此方法与航位推算方法进行比较，航位推算与该方法量测误差相同，仿真结果如图2～图5所示。

图2 圆圈型区域搜索

图3 圆圈型区域搜索导航误差

图4 蛇型区域搜索

图5 蛇型区域搜索导航误差

由图2和图3得出，基于随机信标的导航方法精度远远高于航位推算，在仿真800多秒的时间里，随机信标导航误差在7 m之内，而航位推算导航误差达到了100 m左右，基于随机信标的SLAM导航方法具有更好的收敛性。

由图4和图5可以看出，在前700 s的仿真时间里，随机信标定位误差基本保持在6 m之内，而航位推算误差达到了100多米，基于随机信标的SLAM导航方法表现出了明显的优势，相比航位推算具有更好的收敛性。由图4也可以看出x轴上0～350 m内信标分布较密集，而基于随机信标的导航精度基本控制在6 m之内，而350～450 m内信标分布稀疏，误差突然增大到10 m左右，可以看出信标相对较密集的区域导航精度较高。

为了更进一步探究信标密度对导航精度的影响，在蛇形搜索方式下，将信标数量减少到8个，对该方法与航位推算方法进行了仿真，结果如图6和图7所示。

图6 蛇型区域搜索（信标数量为8）

图7 蛇型区域搜索导航误差（信标数量为8）

由图6和图7可以看出，信标数量由25个降低到8个时，基于随机信标的导航在700 s误差由6 m左右增加到15 m左右，虽然精度有所下降，但是降低了成本，并且和航位推算方法相比仍然表现出了明显的优势。在不同情况下对导航精度要求的不同散布适当数量的信标，使得满足不同情况下对导航精度的要求。

为此，按式（16）选取绝对误差平均值σ作为刻画导航误差的指标，σ越大，表示误差越大，精度越低。

在不同数量信标导航下，利用蒙特卡罗法进行大量仿真实验，通过计算和分析仿真结果，分别得到不同数量信标下的导航误差σ，绘制出误差σ散点图与误差拟合曲线，如图8所示。

图8 信标数量与导航误差的关系图

由图8可以看出，当信标数量在70个以内时，导航误差σ随着信标数量的增加而减小，而当信标数量达70个以上时，误差σ基本保持在5.5 m附近，依然呈递减趋势。主要原因是当信标较稀疏时，导航误差较大是由于信标数量不足引起的，随着信标数量增多，加入到滤波器中的状态量增多，对航行器的位置校正就越来越准确，而当信标数量达到一定程度时，由于受系统噪声及传感器的固有噪声影响，不可能无限制的消除误差，最终使得导航误差保持在一定的范围内。

表1 不同观测方差阵下的导航精度

通过分析在不同的量测误差下基于随机信标导航方法的误差，得出测量精度较高的情况下，该算法具有较高的精度。因此在不同的搜索目的下，可根据实际情况选择精度不同的测量装置，既能满足导航精度的要求，还能降低导航成本。

4 结束语

本文对基于随机信标的SLAM导航算法进行了研究，建立了系统状态方程和量测方程，应用同时制图定位方法，将随机信标定位问题转化为非线性滤波问题。通过建立增广矩阵，利用扩展卡尔曼滤波同时对航行器与随机信标的位置进行估计，并针对不同信标密度的情况，对该算法进行了MATLAB仿真。仿真结果证明了在同样的导航测量误差干扰下，基于随机信标的水下SLAM导航算法获得的定位精度都远远高于航位推算导航效果，且具有很好的收敛性，此算法既克服了目前水声导航需提前布放基阵，且需对其位置进行校准的缺点，又解决了水下SLAM参考点难以捕捉的问题，所以在水下导航方面具有很大的应用潜力。尽管如此，信标散布后位置受洋流影响使得航行器导航会有所误差，所以该算法仍需要改进以及系统地完善。

［1］Wang W.Status and development trend of inertial technology［J］.Acta Automatica Sinica，2013，39（6）：723- 729.（王巍.惯性技术研究现状及发展趋势［J］.自动化学报，2013，39（6）：723- 729.）

［2］Paull L，Saeedi S，Seto M，et al.AUV navigation and localization：a review［J］.IEEE Journal of Oceanic Engineering，2014，39（1）：131- 149.

［3］Ferreira B，Matos A，Cruz N.Single beacon navigation：localization and control of the MARES AUV［C］∥Proc.of the IEEE Oceans，2010：1- 9.

［4］Bahr A，Leonard J J，Martinoli A.Dynamic positioning of beacon vehicles for cooperative underwater navigation［C］∥Proc.of the IEEE／RSJ International Conference on Intelligent Robots and Systems，2012：3760- 3767.

［5］Liu J，Ji D X.Navigation error amendment for underwater robot using single beacon［J］.Control and Decision，2010，25（9）：1354-1358.（刘健，冀大雄.用固定单信标修正水下机器人导航误差［J］.控制与决策，2010，25（9）：1354- 1358.）

［6］Webster S E，Eustice R M，Singh H，et al.Advances in singlebeacon one-way-travel-time acoustic navigation for underwater vehicles［J］.The International Journal of Robotics Research，2012，31：935- 950.

［7］Liu M Y，Li W B，Liu F Q，et al.Underwater navigation system observability analysis based on a single beacon ranging［J］.Journal of Northwestern Polytechnical University，2011，29（1）：87- 92.（刘明雍，李闻白，刘富樯，等.基于单信标测距的水下导航系统可观测性分析［J］.西北工业大学学报，2011，29（1）：87- 92.）

［8］Batista P，Silvestre C，Oliveira P.Single beacon navigation：observability analysis and filter design［C］∥Proc.of the American Control Conference，2010：6191- 6196.

［9］Chitre M.Path planning for cooperative underwater range-only navigation using a single beacon［C］∥Proc.of the IEEE International Conference on Autonomous and Intelligent Systems，2010：1- 6.

［10］Scherbatyuk A P，Dubrovin F S.Some algorithms of AUV positioning based on one moving beacon［C］∥Proc.of the Navigation，Guidance and Control of Underwater Vehicles，2012，3（1）：1- 6.

［11］Liu J，Dong S F，Wen D，et al.Improved DV-Hop algorithm based on mobile beacon［J］.Transducer and Microsystem Technologies，2013：28- 32.

［12］Bailey T，Nieto J，Guivant J，et al.Consistency of the EKFSLAM algorithm［C］∥Proc.of the IEEE／RSJ International Conference on Intelligent Robot and Systems，2006：3562- 3568.

［13］Lee D.Experiments on localization of an AUV using graphbased SLAM［C］∥Proc.of the 10th IEEE International Conference on Ubiquitous Robots and Ambient Intelligence，2013：526- 527.

［14］Wang H J，Wang J，Bian X Q，et al.SLAM of AUV based on the combined EKF［J］.Robot，2012，34（1）：56- 64.（王宏健，王晶，边信黔，等.基于组合EKF的自主水下航行器SLAM［J］.机器人，2012，34（1）：56- 64.）

［15］Zeng J，Guo X S，Zhang G L.Study on method of autonomous mobile robot integrated navigation based on SLAM［J］.Applied Mechanics and Materials，2013，263：1290- 1297.

［16］Wang X H，Fu W P.New active SLAM algorithm for autonomous mobile robot［J］.Systems Engineering and Electronics，2012，34（11）：2334- 2338.（王晓华，傅卫平.一种新的自主移动机器人主动式SLAM算法［J］.系统工程与电子技术，2012，34（11）：2334- 2338.）

［17］Achtelik M W，Lynen S，Weiss S，et al.Visual-inertial SLAM for a small helicopter in large outdoor environments［C］∥Proc. of the IEEE／RSJ International Conference on Intelligent Robots and Systems，2012：2651- 2652.

［18］Newman P，Leonard J.Pure range-only sub-sea SLAM［C］∥Proc.of the IEEE International Conference on Robotics and Automation，2003：1921- 1926.

［19］Durrant-Whyte H，Bailey T.Simultaneous localization and mapping：part I［J］.IEEE Robotics&Automation Magazine，2006，13（2）：99- 110.

［20］Johnson T，Seeling P.Mobile node localization using cooperation and static beacons［C］∥Proc.of the IEEE International Conference on Communications Workshops，2013：281- 285.

Underwater SLAM navigation algorithm based on random beacons

LIU Ming-yong，DONG Ting-ting，ZHANG Li-chuan
（School of Marine Science and Technology，Northwestern Polytechnical University，Xi’an 710072，China）

An underwater simultaneous localization and mapping（SLAM）navigation algorithm based on random beacons is studied.At present，beacon navigation is extensively studied in the field of navigation，but it often needs to locate the beacon’s positon in advance.In this paper，we propose a navigation method based on random beacons that does not need position calibration，that is，when beacons are randomly dispersed，we use the SLAM method to estimate the position of the random beacons，thus to navigate the autonomous underwater vehicle（AUV）by observing the distance and orientation between the beacons and the AUV.We also analyze the navigation accuracy under different beacons density and different observation errors.Simulation results show that the algorithm performs well in both convergence and precision.

autonomous underwater vehicle（AUV）；random beacons；navigation；simultaneous localization and mapping（SLAM）

TP 24

10.3969／j.issn.1001-506X.2015.12.25

刘明雍（1971- ），男，教授，博士，主要研究方向为水下导航、惯性技术及非线性控制。

E-mail：liumingyong＠nwpu.edu.cn

董婷婷（1989- ），女，硕士研究生，主要研究方向为水下导航。

E-mail：584845790＠qq.com

张立川（198-2- ），男，副教授，博士，主要研究方向为水下导航与制导、惯性技术。

E-mail：zlc＠nwpu.edu.cn

1001-506X（2015）12-2830-05

2014- 11- 26；

2015- 04- 10；网络优先出版日期：2015- 08- 31。