一种AUV 自主导航系统的设计

赵 辉，杜航原，张 虎

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

随着海洋开发和利用的逐步深入，以及海洋战略地位的显著提升，自主式水下航行器(Autonomous Underwater Vehicle，AUV)因其具有安全(无人)、实时(信息由机器人实时处理)、体积小、重量轻、耗电小等优势，在军事和工程领域都得到了广泛应用。高精度导航和定位对AUV 安全航行和高效完成预定任务具有决定性作用，实现自主导航成为决定AUV 发展与应用的关键技术之一。在实际使用中，由于水下导航作业时间长、环境复杂、信源稀少以及隐蔽性要求高的特点，传统导航方式［1-3］在精度、可靠性和成本上难以满足AUV 完全自主导航的需要。

近年来提出的同步定位与地图创建(simultaneous localization and mapping，SLAM)算法描述了这样一个问题:运载体在位置环境下由一个未知起点出发，在移动过程中通过传感器对环境特征进行探索，创建环境地图，同时利用地图实现自身定位［4］。SLAM 算法具有无须先验环境地图、传感器结构相对简单、易于实施等优点，为AUV 导航技术开辟了新的研究途径和领域。本文设计了一种基于SLAM 算法的AUV 导航系统，为实现AUV 自主导航提供了一种有效途径。

1 自主导航系统描述

随着在AUV 的航行过程中，外部传感器不断感知到周围环境中实体相对于AUV 的距离和角度信息，将这些物体进行几何近似提取为特征点，利用这组特征表示环境，可以形成对环境的稀疏描述，即特征地图［5，6］。以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 为地图中包含的特征数目，假设环境保持静止，则有li(k +1)=li(k)=li=［xi，yi］T。在过去的研究中都是将定位和构图分离，进行独立实现，然而在SLAM 问题中，定位与构图被融为一体。SLAM 问题就是要解决每一时刻如下联合后验概率密度的估计:

2 系统执行流程

在扩展卡尔曼滤波(extended kalman filter，EKF)框架［5］下通过运动模型和观测模型实现对AUV 状态和环境地图的同时估计，首先利用控制变量完成AUV 位姿和地图特征的预测，然后利用外部传感器获取环境特征的观测信息，再计算实际观测和估计观测间的新息，利用新息对AUV 状态和地图进行更新，最后将之前未观测到的环境特征加入地图。这样，AUV 在探索过程中通过不断地进行“估计－校正”循环，消除累积误差，得到尽可能准确的自身定位和环境地图。

基于SLAM 的AUV 导航算法的执行过程主要分为预测、观测和更新三个阶段，是一个递归的过程，系统执行流程如图1 所示。

图1 SLAM 导航系统结构图

3 系统实现

3.1 系统状态方程

AUV 通过自身的推位传感器测量相邻时刻的状态变化量，通过建立系统状态方程进行自身状态预测。将AUV 的状态向量表示为xV(k)=［x(k)，y(k)，Ψ(k)］T，分别记录k时刻AUV 的二维平面坐标和航向角，AUV 的运动方程为:

其中，FV(k)是状态转移矩阵，vV(k)是均值为零方差为QV(k)的系统噪声。系统的联合状态向量包含AUV 状态和所有地图特征:

同理对式(1)进行扩维得到导航系统的状态方程:

其中Ili为单位阵，0li为零向量。

3.2 系统观测模型

AUV 利用外部传感器如声纳、水下照相机等感知周围环境中的特征，得到观测信息，这些观测信息是环境特征关于AUV 的相对位置。假设观测是线性并且能够即时获得的，第i 个环境特征的观测模型可以写成:

其中，wi(k)是与观测不相关的噪声，假设其服从均值为零方差为Ri(k)的正态分布;Hi是观测矩阵，对传感器获得的第i 个环境特征的观测zi(k)和状态向量X(k)建立联系，它具有如下形式:

这个结构表明了AUV 和环境特征间存在着相关性。

3.3 数据关联

由于AUV 同一时刻可能获得多个特征观测，这些观测值的来源可以分为两类:一类是已进入环境地图的特征;另一类是之前尚未观测到的特征。不同来源的观测量有不同的处理方法:新特征的观测需要加入到环境地图中，即对联合状态向量进行扩维;对已有特征的观测可用于状态更新;而虚假观测则直接剔除。这种对观测量与系统状态变量进行匹配的过程称为数据关联［6］。

3.4 状态估计过程

在基于SLAM 的导航算法中，对于数据关联成功的观测量(假设为第i 个特征)，使用EKF 对AUV 和环境特征的状态进行估计，也就是利用新的观测信息，通过式(4)描述的状态模型和式(5)描述的观测模型对系统联合状态进行递归估计，这个过程分为以下三个阶段:

1)预测阶段:以k 时刻的状态估计X(k|k)和系统状态协方差矩阵P(k|k)为基础，系统首先通过状态模型和观测模型对k+1 时刻的系统状态、第i 个特征的观测量以及状态协方差进行时间预测:

2)观测阶段:假设k +1 时刻AUV 通过外部传感器获得的一个观测量与地图中第i 个特征关联，计算观测新息和新息协方差如下:

3)更新阶段:利用新息对预测阶段的系统状态和状态协方差进行更新:

其中卡尔曼增益矩阵Ki(k+1)的计算如下:

通过以上三个阶段，就可以实现对AUV 位姿和环境地图状态的同步更新。

3.5 新特征加入

AUV 在航行过程中利用外部传感器不断获取环境信息，新特征要纳入到环境地图中，也就是对联合状态向量进行扩维处理，将新特征加入其中，建立AUV 状态与地图中已有特征和新特征的相关联系，使地图随AUV 探索区域增大自动扩展。假设已确定观测来源于某一新特征，将其转换到全局坐标系下，加入到系统联合状态向量中并进行初始化。假设当前状态为:

对新特征lnew进行初始化

其中g(·)将新特征的相对位置转换成全局坐标下的位置。

将新特征加入状态向量:

其中，Puvv=▽gx·Pvv，Puvl=▽gx·Pvl，Pu=▽gx·Pw·，▽gx和▽gz分别为g(·)关于xV(k)和z(k)的Jacobian 矩阵。

4 结论

本文基于同步定位与地图创建的原理，针对自助式水下航行器导航定位问题，设计了一种能够实现自身定位同时获取外部环境地图的自主式导航系统。建立了AUV 导航系统的状态模型和观测模型，利用卡尔曼滤波框架实现了未知环境下AUV 的自主定位导航。该系统具有较高的自主性，受环境因素影响较低，能够获得较高的定位精度，并且具有良好的收敛性，为水下航行器的导航与定位问题提供了一种新的解决办法。此外，如何利用SLAM 算法在大规模、非结构化以及动态性的水下环境中实现AUV 自主定位是今后将要进一步研究的内容。

［1］Dissanayake G，Newman P，Clark S，et al.A Solution to the Simultaneous Localization and Map Building(SLAM)Problem［J］.IEEE Trans on Robotics and Automation，2001，17(3):229－241.

［2］田坦.水下定位与导航技术［M］.北京:国防工业出版社，2007.

［3］穆华，任治新，胡小平，等.船用惯性/地磁导航系统信息融合策略与性能［J］.中国惯性技术学报，2007，15(3):322－326.

［4］Smith R，Self M，Cheeseman P.Estimating Uncertain Spatial Relationships in Robotics［A］.Proceedings of Conference on Uncertainty in Artificial Intelligence［C］.1988:435－461.

［5］陆国生.GNSS 高精度定位接收技术研究［D］.杭州:浙江大学，2014.3.

［6］曲丽萍.移动机器人同步定位与地图构建关键技术的研究［D］.哈尔滨:哈尔滨工程大学，2013.3.