基于SLAM算法的AUV自主导航仿真研究*

赵 辉，杜航原，张 虎

(1.中国移动通信集团山西有限公司，山西太原030032;2.山西大学 计算机学院，山西 太原030006)

海洋空间蕴藏着丰富的海洋生物、矿产资源和各种能源，为人类可持续发展提供了大量物质基础。对海洋的研究和合理开发，成为了人类社会和经济发展的一项重大课题。但由于海洋空间能见度低、环境复杂等特点，人类对海洋的探索面临重重困难。伴随着人类对海洋认识、开发、利用和保护的逐步深入，水下航行器作为人类探索海洋的有力助手经历了从诞生到发展再到应用的过程。其中，自主式水下航行器因其具有安全、实时、体积小、重量轻、耗电小等优势，在军事、科考和工程领域都得到了广泛应用。目前，AUV制造过程所需的材料、结构等相关技术已基本得到解决，但随着AUV向远航程、大深度、多功能、长时间趋势发展，仍有一些关键问题亟需研究和解决。其中水下导航技术对于AUV的发展至关重要，导航定位信息的精确度直接决定AUV能否安全高效的完成水下任务，实现自主导航成为决定AUV发展与应用的关键技术之一。常见的AUV导航技术主要有航位推算法［1］、基于声信号的长基线和短基线导航［2］、基于重力场和磁力场的地球物理导航［3］，还有将多种导航技术进行组合互补的组合导航［4］等。在实际使用中，由于水下导航作业时间长、环境复杂、信源稀少以及隐蔽性要求高的特点，这些传统导航方式在精度、可靠性和成本上难以满足AUV完全自主导航的需要。

起源于机器人领域的同步定位与地图创建算法解决了这样一个问题:处于未知环境中的机器人，由一个存在不确定性(初始误差)的起点出发，在行进过程中利用自身配备的传感器对环境进行不断感知，增进式的构建环境地图，同时利用地图确定机器人自身位置［5］。SLAM算法具有无须先验环境地图、传感器结构相对简单以及易于实施等优点，在水下导航定位技术中极具发展潜力和空间，对于提高AUV的自主性有着重大意义。本文将SLAM算法用于AUV自主导航，在EKF框架下实现了AUV自身位置与环境特征状态的同时估计，仿真实验表明这种导航算法能够充分利用环境信息，有效提高了导航精度。

1 SLAM导航系统原理

在AUV行进和作业过程中，其所处外部海洋环境中存在大量实体(如珊瑚、岩石、沉船等)，在对这些物体进行几何近似提取为特征点的基础上，AUV通过装备的传感器不断感知各个特征点相对于AUV的距离和角度信息，并利用这些信息形成对环境的稀疏描述，即特征地图［6］。在SLAM框架下，如果将坐标原点取于AUV的初始位置，那么随着AUV对环境的不断探索就可以增进式地获取相对于起始点的完整特征地图，同时得到这一过程中每一时刻AUV的位姿状态估计。

令xv(k)表示k时刻AUV的位姿状态;uv(k)表示k时刻AUV的控制输入变量;z(k)表示AUV在行进过程中利用传感器获取的外部环境观测信息;L(k)=［l1(k)，l2(k)，…，lN(k)］表示k时刻用于描述环境的特征地图，其中li(k)为地图中第i个特征，N为地图中包含的特征数目，假设环境保持静止，则有 lik+1=li(k)=li=［xi，yi］T。SLAM 算法就是实现每一时刻对AUV状态和环境地图特征的联合估计，即对如下联合后验概率密度的估计:

在过去的研究中，往往将定位和构图分割处理，进行独立实现，即地图构建是已知AUV位置的地图构建，定位是已知环境地图的定位。然而在SLAM方法中，定位与构图被融为一体。随着AUV不断获取环境特征的相对观测，所有特征位置的估计是相关的，因为它们基于相同的AUV位姿估计。这意味着:要获得完全一致的SLAM解法，则必须对AUV位姿与地图中所有特征进行全状态估计。将AUV系统和环境地图定义在一个二维坐标系下，使用一组包含AUV的状态向量和地图特征向量的联合状态向量表达空间信息，在AUV探索过程中利用扩展卡尔曼滤波(extended kalman filter，EKF)融合传感器观测信息，对联合状态进行最优准则估计。每一时刻AUV获得新的环境特征后，要将新特征加入地图，因此联合状态向量的维数不固定。联合状态向量的协方差矩阵中保存了AUV状态分量之间、状态与环境特征之间以及两个环境特征之间的相关信息，这些信息是联系AUV状态和环境地图的纽带，同时协方差矩阵中包含的相关性也保证了特征地图的收敛性［7］。

2 系统处理流程

在EKF迭代估计框架下，基于SLAM的AUV导航算法执行过程主要包括预测、观测和更新三个部分，是一个递归的过程，算法流程如图1所示。

图1 SLAM导航系统结构图

每一时刻AUV的SLAM导航系统数据处理流程为:在预测阶段，基于上一时刻的联合状态估计值，利用控制输入变量及AUV运动模型和观测模型完成当前AUV位姿和地图特征状态的预测值;在观测阶段，一方面通过传感器获得对外部环境的感知信息，并进行特征的识别和抽取，另一方面还需判断观测信息与环境地图中已有特征的关联关系以及多个观测是否源于同一物理实体，即数据关联过程;在更新阶段，利用数据关联结果将新特征加入地图，对于地图中已有特征的观测值，计算实际观测与估计观测间的新息，利用新息对AUV状态和地图进行更新。在AUV的行进和探索过程中，系统不断进行“估计-校正”的迭代循环，克服了误差累积，能够获得更加准确的自身定位和环境地图。

3 仿真实验及结果分析

利用仿真实验将基于SLAM的AUV导航算法与航位推算法进行定位精度的比较，根据仿真结果分析算法性能。

3.1 仿真环境

仿真的硬件环境为Intel(R)Core2 Duo CPU T7250@2.00 GHz，2G RAM，Windows XP 操作系统，软件利用 Matlab R2007b编写。AUV的探索仿真环境设为图2所示的平面矩形海域，环境中分布着一定数量的环境特征点，用“*”表示，AUV由(0 m，0 m)点出发(其初始位置的确定可借助GPS实现)，沿图中绿色曲线逆时针匀速运动2周，在运动过程中传感器可以对前向180度范围内的特征点进行观测，其最大测量距离为100 m。

设AUV的运动方程为:

式中:dt为两时刻间的间隔时间;Uk和α(k)为k时刻的控制输入量;L代表AUV的长度;wx，wy和wψ为系统噪声。

观测方程为:

式中:(xV(k)，yV(k))和(xi(k)，yi(k))分别为 k时刻AUV的坐标和第i个路标的坐标;wr和wθ为观测噪声。设AUV 长度为4 m，初始状态为 X(0)v=［0，0，0］T，运行线速度为3 m/s，最大舵角为30°;线速度误差为0.2 m/s，角度误差为 0.2 °/s;传感器测距误差为1 m/s，测角误差为0.5°/s，这些误差包含传感器自身噪声以及特征提取过程引入的误差。

图2 仿真环境

3.2 仿真结果分析

为避免实验随机性对结果造成的影响，进行20次独立重复实验，利用实验结果数据的统计值进行分析，引入均方根误差(root mean squared error，RMSE)对算法性能进行比较。

图3是使用航位推算法产生的AUV定位RMSE随时间变化情况，可以看出由于系统误差和观测误差的存在，定位误差不断累积，总体上呈上升趋势，最终得到的推位结果十分不可靠。图4是使用SLAM导航算法对AUV状态估计的RMSE随时间变化曲线，从总体上看，后半段的定位误差相比前半段有所降低，这说明AUV在完成一个探索闭环后再次对环境进行探索的重定位过程可以进一步提高估计精度。SLAM算法的定位误差相比航位推算法的误差大大降低，误差的增长速度也受到有效控制，证明算法的估计精度较高，并且具有较好的收敛性。

图3 航位推算定位误差

图4 SLAM定位误差

图5是使用基于SLAM的导航算法对AUV在整个行进过程中传感器观测到的特征的位置估计误差。由于AUV初始状态中存在不确定性，并且在AUV行驶过程中只能对特征进行有限次数的观测，因此环境特征的位置估计也受到AUV状态估计误差的影响。由图5可以看出，各特征的定位误差保持在较低水平，说明SLAM导航算法能够获得比较可靠的构图精度。误差分布的总体趋势是位于AUV行驶路线后半段的特征位置估计误差相对较大，这与AUV状态估计误差的分布情况基本一致。考虑到AUV的探索范围、控制误差和传感器观测误差的大小以及传感器间同步问题，该SLAM导航算法获得的定位和环境构图精度是可接受的。

仿真实验仅以验证算法的可行性和有效性为目的，因此假设数据关联完全正确，并且只选用了较小的区域作为仿真环境，而实际当中需要考虑水下环境非结构特性对传感器观测、特征提取以及数据关联等造成的影响。另外，根据实际任务的需要，AUV可能需要进行大范围、长时间的航行，这些对系统的稳定性和计算效率提出了更高的要求，同时也为SLAM算法在水下自主导航中的应用研究提供了更加广阔的前景。

图5 环境特征定位误差

4 结论

本文基于同步定位与地图创建的原理，建立了AUV导航系统的状态模型和观测模型，利用卡尔曼滤波框架实现了未知环境下AUV的自主定位导航。利用MATLAB进行了区域探索仿真实验，试验结果表明:在存在控制误差和观测误差的情况下，基于SLAM的导航算法获得了较高的定位精度，并且具有良好的收敛性，证明这种导航算法具有广阔的应用前景。另外，如何利用SLAM算法在大规模、非结构化以及动态性的水下环境中实现AUV自主定位是今后将要进一步研究的内容。

［1］Elbert S.Maloney Editer Dutton’s Navigation and Piloting［D］.Annapolis，M D Naval Institute Press，1985.

［2］张立川，徐德民，刘明雍，等.基于移动长基线的多AUV 协同系统［J］.机器人，2009，31(6):581 －593.

［3］寇义民，温奇咏.一种基于地磁场曲面线性化拟合的快速导航定位方法［J］.宇航学报，2009，30(2):498 －501.

［4］杨洋.GPS/SINS深组合导航中的关键技术研究［D］.南京:南京理工大学，2012.

［5］孙凤池，黄亚楼，康叶伟.基于视觉的移动机器人同时定位与建图研究进展［J］.控制理论与应用，2010，27(4):488－494.

［6］宋宇，李庆玲，康轶非，等.平方根容积 Rao-Blackwillised粒子滤波 SLAM 算法［J］.自动化学报，2014，40(2):357－367.

［7］Camini M W M D，Paul N，Hugh F D W.A solution to the simultaneous and map building(SLAM)problem［J］.IEEE Transactions on Robotics and Automation，2001，17(3):229－241.