基于边界的无人机主动SLAM算法

沈永福，王希彬

（1.海军军训器材研究所，北京102308；2.海军航空工程学院控制工程系，山东烟台264001）

基于边界的无人机主动SLAM算法

沈永福1，王希彬2

（1.海军军训器材研究所，北京102308；2.海军航空工程学院控制工程系，山东烟台264001）

同步定位与地图构建技术是无人机实现真正自主导航的关键。为克服被动同步定位与地图构建算法的缺陷，研究了基于边界的无人机主动同步定位与地图构建算法。在无人机的探测区域周围产生候选边界点，通过建立合理的目标函数，从候选边界点中选择目标点，控制无人机朝该目标点方向运动，再运用扩展卡尔曼滤波算法更新无人机的运动状态。通过建立的无人机简化模型，对提出的算法和随机同步定位与地图构建算法进行对比研究，仿真结果表明该算法是有效可行的。

主动同步定位与地图构建；边界；扩展卡尔曼滤波；无人机

同步定位与地图构建（Simultaneous Localization and Mapping，SLAM）是无人机实现真正自主导航的关键。其本质就是在先验信息未知的条件下，利用无人机机载传感器获得周围环境的地图，同时确定无人机在地图中的位置[1]。但是现有的SLAM算法都是假定运动轨迹预先已知的，这种被动形式的SLAM与环境的未知性相矛盾。如何主动地、自适应地探索环境成为主动SLAM研究的课题。

主动SLAM问题是由Federal、Leonard和Smith[2]首先提出的。目前，研究主动SLAM[3-7]的方法主要有最优控制方法[8]、基于边界的方法[9]和局部子图法[10]。相对局部子图和基于边界的主动SLAM而言，基于最优控制的方法建立的目标函数是选择航迹规划点的基础，但是容易陷入局部最优。局部子图的方法更适用于多无人机协同的SLAM航迹规划，对于单个无人机而言，除了可以减小一定计算量外，并没有明显的优势。而基于边界的方法，从某种意义上讲，就是多步预测的最优控制的无人机主动SLAM，可以极大地克服最优控制的局部最优现象。

本文研究了基于边界的无人机主动SLAM算法，对产生的边界点设计了一种新的目标函数，通过对预选边界点的评价确定无人机运动的目标点，并利用扩展卡尔曼滤波（EKF）算法更新系统状态。通过建立简化的无人机运动模型，运用Matlab仿真验证了该方法的可行性和有效性。

1 基于边界的主动SLAM

所谓边界就是由已探测区域的边缘点构成的集合。基于边界的方法是一种使用局部点规划来解决SLAM问题的主动自适应方法[11]，给出即时的局部目标点产生控制输入用于探索和作图。首先，在已探索的区域上从边界点得到预选目标点，通过将边界点分组，把具有相似特征的边界点作为整体来对待，能够极大地减少计算复杂性。然后，考虑各种因素，使用性能评价函数从预选目标点中选择局部最优目标点。一旦确定了目标点，以预测步长的形式产生相应的控制输入，这些控制输入会驱动无人机朝着目标点运动。在朝着局部目标点的运动过程中，无人机设法完成探索与作图任务，并且将EKF算法用于更新系统的状态。

由上述可知，基于边界的主动SLAM方法可以分为以下3个关键阶段。

1.1 候选边界点选取

无人机在探索未知环境时，边界点的数量会迅速增长，若要评价所有的边界点则需要很高的计算成本，而且必要性不大。由于每一个边界点通常和邻域点具有相似或相同的特征，把相似或相同特征的边界点编为相似的组并作为整体来评价[12]，在不损失探索和作图性能的前提下可以达到降低计算成本的目的。

如图1所示，无人机用三角形表示，摄像机作为机载传感器，考虑摄像机的运动特性，假设其探索区域为一扇形，周围黑色的点表示边界点。将环境分为大小相等的小单元，每一个单元内的边界点分为相同的组，取每组中的边界点的重心的位置作为候选边界点，这样就可大大减少候选边界点的数量。

图1 边界点及其分组Fig.1 Boundary points and their groups

1.2 预选边界点评价

无人机观测到地标时，目标函数的选取与边界点至地标的距离和地标自身的不确定性有关，并且无人机在边界点应能够观测到更多新的区域。因此，可构建如下目标函数：

选择目标评价函数J最大的边界点作为目标点，使得无人机朝该点运动一步。

1.3 朝目标点运动，并使用EKF算法更新系统状态

确定目标点后，根据无人机的运动模型和传感器观测模型，使用EKF算法更新系统状态变量。EKF算法通过一阶泰勒级数展开来近似表示非线性模型，是非线性系统的线性估计，广泛应用于定位、地图创建、导航和目标跟踪等领域，被认为是解决SLAM问题非常有效的方法，它是一种包含系统状态预测和更新的递推运算[13]。

状态预测：

式（2）～（4）中：u(k)为控制输入向量；z(k)为传感器量测向量分别为f(·)相对于状态变量X(k)和运动噪声向量v(k)的雅可比矩阵，即有：

状态更新：

2 简化的无人机运动模型

本文的仿真模型采用了简化的无人机平面运动模型，见图2。无人机的状态Xv=[x y ψ]T，其中，x、y为无人机在二维平面中的位置，ψ为方位角；控制量U=[ΔdΔψ]T，其中，Δd为一个时间步长对应的无人机运动距离，Δψ为姿态变化量。

图2 无人机二维模型Fig.2 2D model of UAV

各状态变量的变化方程为：

无人机状态方程为：

设第i地标的状态为mi=[mxmy]T，假设地标是静止的，则有：mi,k=mi,k-1。

无人机与第i地标的测量方程为

3 仿真试验

假定环境是一个40 m×40 m的方形区域，无人机的初始位置为[0 0 0]T，并朝向x轴正向，传感器采用CCD摄像机，探测距离为10 m，探测角度为90°，环境中均匀分布着20个地标。目标函数式（1）中的权值取值为：ω1=0.5，ω2=0.5。在Matlab下，分别采用本文提出的基于边界的主动SLAM算法和随机SLAM，应用简化的无人机模型采用扩展卡尔曼滤波算法进行仿真，仿真运行200步以后，无人机的运动轨迹和地标的位置估计见图3、4。图中，实线为估计的无人机的运动轨迹，“+”号为地标的真实位置，“＊”号和椭圆分别为估计的地标位置和方差。

图3 基于边界的主动SLAM算法的仿真结果Fig.3 Simulation results of active SLAM based on boundary

图4 随机SLAM的仿真结果Fig.4 Simulation result of random SLAM

从图3和图4中可以看出，随机运动时无人机的探测环境有限，并且无人机的运动状态呈现混乱无规律的特点，经常会陷入某个区域而不能离开。而采用基于边界的主动SLAM航迹规划，则能够较好地克服这种情况，无人机航迹平滑有序，就观测地标数而言，也大大多于采用随机SLAM的方法。显然，采用基于边界的主动SLAM算法时，无人机可以探测到更大范围的环境，并得到了较高的地标估计值。可见，基于边界的无人机主动SLAM明显优于随机SLAM。

4 结论

本文研究了基于边界的无人机主动SLAM算法，其实质与基于最优控制的方法相同，也是通过构造合理的目标函数来确定无人机的运动输入。基于最优控制的方法从无人机的一步步长到达的区域点中选择最优的运动目标点，而基于边界的主动SLAM算法则从传感器探测的边界点中选择下一步运动目标点，相比而言，目标点的选择范围可更宽，并能在一定程度上避免陷入局部最优。本文能通过建立的无人机运动简化模型，运用Matlab对提出的算法与随机SLAM进行仿真比较，表明了该算法的有效性和可行性。实际应用中，外界的三维环境非常复杂，可能还包含许多动态的目标，对此还需进一步深入研究。

[1]NEWMAN P M.On the structure and solution of the simultaneous localization and map building problem[D]. Sydney：University of Sydney，1999.

[2]张恒，樊晓平.移动机器人同步定位与地图构建过程中的轨迹规划研究[J].机器人，2006，28（3）：285-290. ZHANG HENG，FAN XIAOPING.Research on trajectory programming in the process of simultaneous location and mapping for mobile robots[J].Robot，2006，28（3）：285-290.（in Chinese）

[3]张恒，樊晓平，刘艳丽.移动机器人同步定位与地图构建研究进展[J].数据采集与处理，2005，20（4）：458-465. ZHANG HENG，FAN XIAOPING，LIU YANLI.Simultaneous localization and mapping for mobile robots[J]. Journal of Data Acquisition&Processing，2005，20（4）：458-465.（in Chinese）

[4]武涛，孙凤池，苑晶，等.一种基于线段特征的室内环境主动SLAM方法[J].机器人，2009，31（2）：166-170. WU TAO，SUN FENGCHI，YUAN JING，et al.An active SLAM approach based on line segment feature in indoorenvironment[J].Robot，2009，31（2）：166-170.（in Chinese）

[5]吴晓琳，宋萌，苑晶，等.通讯范围受限条件下的多机器人主动SLAM[J].系统工程与电子技术，2012，34（10）：2121-2128. WU XIAOLIN，SONG MENG，YUAN JING，et al.Multirobot active SLAM under limit range[J].Systems Engineering and Electronics，2012，34（10）：2121-2128.（in Chinese）

[6]陶通，黄亚楼，苑晶，等.基于协助校正方法的多机器人主动同时定位与建图[J].模式识别与人工智能，2012，25（3）：534-543. TAO TONG，HUANG YALOU，YUAN JING，et al. Multi-robot active simultaneous localization and mapping based on cooperative correction approach[J].Patter Recognition and Artifical Intelligence，2012，25（3）：534-543.（in Chinese）

[7]苑晶，黄亚楼，陶通，等.基于局部子地图方法的多机器人主动同时定位与地图创建[J].机器人，2009，31（2）：97-103. YUAN JING，HUANG YALOU，TAO TONG，et al.Multirobot active SLAM based on local sub-map approach[J]. Robot，2009，31（2）：97-103.（in Chinese）

[8]FEDER H，LEONARD J，SMITH C.Adaptive mobile robot navigation and mapping[J].International Journal of Robotics Research，1999，18（7）：650-668.（in Chinese）

[9]Y LIU，F SUN，et al.A solution to active simultaneous localization and mapping problem based on optimal control [C]//Proceedings of IEEE International Conference on Mechatronics and Automation.Harbin：IEEE，2007：314-319.

[10]苑晶.未知环境中移动机器人主动同时定位与地图构建技术研究[D].天津：南开大学，2007. YUAN JING.Research on mobile robots active simultaneous location and mapping technologies in unknown environments[D].Tianjin：Nankai University，2007.（in Chinese）

[11]TAO TONG，HUANG YALOU，SUN FENGCHI，et al. Motion planning for SLAM based on frontier exploration [C]//Proceedings of the 2007 IEEE International Conference on Mechatronics and Automation.Harbin：IEEE，2007：2120-2125.

[12]YAMAUCHI B.A frontier-based approach for autonomous exploration[C]//IEEE International Symposium on Computational Intelligence in Robotics and Automation. Monterey，1997：146-151.

[13]WEBB T，PRAZENICA R，KURDIAL A，et al.Visionbased state estimation for autonomous micro air vehicles [J].Journal of Guidance，Control，and Dynamics，2007，30（3）：816-826.

UAV Active SLAM Algorithm Based on Boundary

SHEN Yong-fu1,WANG Xi-bin2
（1.Naval Training Equipment Institute,Beijing 102308,China; 2.Department of Control Engineering,NAAU,Yantai Shandong 264001,China）

The technology of simultaneous localization and mapping(SLAM)is the key for an unmanned aerial vehicle (UAV)to realize truly autonomous navigation.To overcome the disadvantage of the passive SLAM,the active SLAM meth⁃od based on boundary for UAV was studied.Firstly,the candidate boundary points were produced around the exploration area,and the destination point was selected from those points by building a proper objective function.Then the UAV moved towards this point and its movement state was updated by extended Kalman filtering(EKF)algorithm.Using a sim⁃plified model of UAV,comparative research was carried out between the proposed algorithm and random SLAM.The simu⁃lation results with Matlab showed that this algorithm was effective and feasible.

active simultaneous localization and mapping;boundary;extended kalman filtering(EKF);unmanned aerial ve⁃hicle(UAV)

V24

A

1673-1522（2014）05-0461-04

10.7682/j.issn.1673-1522.2014.05.013

2014-06-09；

2014-07-17

沈永福（1974-），男，工程师，硕士。