基于子地图的智能车辆同步定位与地图创建*

胡玉文，龚建伟，姜 岩，熊光明

(北京理工大学机械与车辆学院，北京 100081)



2015040

基于子地图的智能车辆同步定位与地图创建*

胡玉文，龚建伟，姜 岩，熊光明

(北京理工大学机械与车辆学院，北京 100081)

为使智能车辆能在无法预先确定环境范围的条件下创建环境栅格地图并实时定位，提出了一种基于子地图框架的同步定位与地图创建方法。在子地图中设置重合区域与切换区域，以避免相邻子地图间的连续切换。实验结果表明，该方法可保证车辆在同步定位与地图创建过程中地图范围的动态增长和子地图间的稳定切换，具有较高的实时性和定位精度。

智能车辆；同步定位与地图创建；子地图；栅格地图

前言

同步定位与地图创建(simultaneous localization and mapping, SLAM)是指在未知环境中创建环境地图的同时进行定位的技术[1-2]。SLAM最早源于机器人研究领域，被誉为实现真正自主的关键技术，并衍生出采用不同技术实现SLAM的方案[3-5]。

在智能车辆研究中SLAM也是热点。一方面精确定位有助于智能车辆的实时控制，另一方面实时生成的环境地图，特别是栅格地图的创建，有助于智能车辆进行全局和局部的路径规划。但当无法预先确定行驶范围或车辆进行长距离行驶时，在地图创建过程中车辆位置可能超过栅格地图所能表示的最大边界，造成算法出现越界错误。

为解决这一问题，可以采用子地图的方式构建全局地图[6]。全局地图由相邻的子地图序列拼接组成，智能车辆通过新建或调用子地图以维护一定范围内的环境信息。全局地图范围随子地图的动态创建而增加，在任意时刻只需对一张子地图进行更新。采用子地图时需要注意两个问题：首先，在新建或切换子地图后，新的子地图中应该保留部分已知地图信息，以保证地图信息的连续性和匹配定位结果的准确性；其次要建立恰当的地图切换机制，避免在两个相邻子地图间连续切换的状况发生。

针对以上问题，本文中提出了一种基于子地图框架的粒子滤波SLAM方法，通过子地图的方式实现在户外大范围环境中环境栅格地图的创建以及地图范围的动态增长。通过对相邻的两张子地图间设置重合部分，保证子地图切换后新子地图中保留了旧子地图中的部分环境信息。并进一步对重合区域进行细分，设置切换区域，保证在子地图的创建与调用过程中不会出现在两张相邻子地图间连续切换。

1 基于粒子滤波的SLAM

基于粒子滤波的方法将SLAM过程分为地图创建和定位两个过程进行。在地图创建过程中假设已知车辆位置，根据环境传感器数据和相应的模型对地图状态进行维护。在定位过程中对位置的估计采用粒子滤波方法，通过激光雷达数据与已知地图进行地图匹配，估计车辆位姿的后验概率分布。

1.1 地图创建

1.1.1 栅格地图创建

本文中通过激光雷达建立环境栅格地图[7]。在栅格地图M中，环境被等分为二维栅格m。每个栅格都关联一个数值p(m)(p(m)∈[0,1])描述了栅格m中存在障碍物的概率，即

(1)

式中：OCC表示栅格m被占据；UNKOWN表示栅格m状态未知；EMP表示栅格m中不存在障碍物。初始时刻为对环境进行观测时，地图初始化为p(m)=0.5。

1.1.2 栅格地图更新

(2)

式中：t为数据采集时刻；X表示车辆位姿；Z表示激光雷达测量数据。

式(2)表示智能车辆根据其在不同时刻位置上对环境的测量结果对地图栅格中存在障碍物的概率进行更新的过程。由于式(2)的计算需要记录大量的历史数据，根据文献[8]中的推导，对地图栅格进行概率更新的递归公式为

(3)

式中：p(M|X1:t-1,Z1:t-1)为t-1时刻的地图栅格概率；p(M|Xt,Zt)为t时刻激光雷达测量的栅格概率。使用激光雷达时可通过反式传感器模型采用光线追踪方法计算，如图1(a)所示。图1(b)中黑色为障碍物，灰色区域为雷达扫描范围。图1(c)中黑色表示栅格位置存在障碍物，白色表示栅格中不存在障碍物，灰色表示栅格状态未知。

1.2 地图创建

SLAM中的定位通过激光雷达数据与已知环境地图间的匹配对车辆位姿进行修正，其过程可看作对车辆位姿最优估计问题的求解，表示为

(4)

在获得预测的粒子集分布以及粒子权重后，通过地图匹配对粒子的权重进行更新。地图匹配则通过将激光雷达的观测数据以每个粒子所表示的位姿投影到地图中进行。本文中采用end-point方法[11]

(5)

通过式(4)计算并归一化后，可得到地图匹配后的粒子权重，权重越大表示该粒子越接近真实车辆位姿。定位结果以权重最大的粒子所表示的位姿输出。定位结束后通过对粒子重采样避免样本集的衰竭。

1.3 动态障碍物检测

由于SLAM过程假设环境为静态，而在真实环境中不可避免地存在动态目标，如行人和车辆等。在基于概率方法的地图更新过程中，动态目标对于单个栅格的状态影响时间极短，一般不会对其状态(OCC/EMP/UNKOWN)造成影响。为进一步消除动态障碍物的影响，在地图匹配过程中对激光雷达数据的来源进行了判断：在匹配过程中，当激光雷达的测量点所在栅格状态为OCC时，标记该测量来源于动态目标。被标记的测量点将不参与地图的更新。

2 基于子地图框架的全局地图创建

任何形式的地图都受其范围限制，只能表达范围内的环境信息。当车辆位置超出地图边界时，将会导致SLAM算法出现越界错误。因此在地图创建过程中，采用子地图的方式组建全局地图，并对子地图间的切换机制进行了研究。

2.1 全局地图的子地图框架

本文中坐标系定义为：将车辆起点定义为全局地图的原点，以正东方向为x轴正方向，正北方向为y轴正方向，建立全局地图坐标系。坐标轴上的单位长度等于相邻子地图中心间的距离。子地图如图2所示。在子地图框架下，全局栅格地图M由子地图序列{…,M(i,j),…}构成。子地图M(i,j)为栅格地图中一系列相邻栅格的集合，(i,j)表示子地图中心在全局地图坐标系中的位置，为边长是n个栅格的正方形，例如，车辆起点位置所在子地图为M(0,0)。子地图在全局地图中等间距分布，相邻子地图中心沿x轴和y轴的栅格距离均为d。通过这样的设置，每张子地图都具有其唯一的编号即中心位置。这些编号可用于在地图创建过程中对已生成子地图的检索，或通过编号所表示的相对位置关系重组全局栅格地图。在地图创建过程中任意时刻只须对当前子地图进行更新。

如图2所示，相邻子地图间均存在一定程度的重合区域。重合区域大小主要参考激光雷达的有效测量范围。当从子地图M(i-1,j)切换到M(i,j)时，M(i,j)保留了重合区域中M(i-1,j)的环境信息，即栅格被占据的概率。这样在地图切换后仍保留了车辆当前位置周围的历史环境信息，避免由于子地图切换导致环境信息丢失而增大地图匹配定位的误差。

2.2 相邻子地图间的切换机制

子地图切换的基本思路是：当车辆进入子地图重合区域时，则切换至相应的相邻子地图。但在算法测试过程中发现，这样简单的设置容易导致在相邻子地图间连续切换。如图3所示，令V表示车辆所在位置，且车辆保持向右的运动。子地图中灰色为相邻子地图M(i-1,j)与M(i,j)间的重合区域。如果简单地以车辆进入重合区域作为判断是否进行切换的标志，当车辆从M(i-1,j)进入重合区域，则子地图切换到M(i,j)。下一时刻，车辆位于M(i,j)中的重合区域，则继续切换回M(i-1,j)。这样就造成了在两张子地图间的相互切换，导致子地图切换的失败。

为避免这种情况的出现，须为相邻子地图分别设置用于判断是否切换子地图的切换区域，如图4所示，同样灰色为M(i-1,j)与M(i,j)的重合区域，其中1区与3区重合，2区与4区重合。将2区和3区分别设为M(i-1,j)和M(i,j)的切换区域。当车辆位于1区时，不进行切换操作。当车辆进入2区时，执行切换操作并切换到M(i,j)。切换后车辆位于4区中，不是M(i,j)的切换区域，保证了在一次子地图切换过程中的单向性。

2.3 子地图的切换

由于车辆的行驶可以朝向任何方向，因此完整的子地图切换机制还应能够确定所切换的新子地图坐标。本文中通过车辆位置相对子地图中心的方向判断应创建或调用的子地图坐标。假设t时刻车辆相对起点位置为(x,y)，且车辆定位的坐标系与坐标轴方向与地图坐标系重合。令当前子地图为M(i,j)，且车辆已经处于该子地图的切换区域中，则下一时刻应切换至的子地图坐标(l,k)为

(6)

式中：r为地图栅格尺寸；n为以栅格数统计的子地图边长。

获得子地图坐标(l,k)后，子地图的操作包括创建和调用两种方式。在地图创建过程中，算法同时维护了子地图坐标列表listt={…,(i,j),…}。通过对listt进行搜索，如果(l,k)不存在列表中，则新建子地图M(l,k)，同时将坐标(l,k)加入坐标列表中；如果(l,k)已存在，则调用对应的子地图信息。最后将M(i,j)与M(l,k)重合部分的信息复制到M(l,k)中对应的区域。

3 基于子地图的同步定位与地图创建实验

3.1 实验平台

如图5所示，本文中的实验平台为基于丰田陆地巡洋舰改造的BIT号智能车辆，二维激光雷达使用德国施克公司生产的LMS291，具有80m/180°的扫描范围和0.25°的角分辨率。并通过里程计和惯性导航设备进行航迹推算，获得车辆行驶距离以及航向信息。

3.2 校园环境实验

实验中车辆进行同步定位与地图创建。车辆运动的起、终点和行驶路线如图6所示。整个过程中车辆行驶距离约为1.9km，地图范围约为370m×600m。场景中包含树木和建筑物等静态物体，以及少量行人和车辆等运动目标。

3.2.1 子地图的生成

图7为在图6所示场景中创建的子地图序列，图中坐标为各个子地图的坐标,箭头表示在SLAM过程中子地图的访问顺序，M(0,0)为起点地图。在地图创建过程中总共建立6张子地图。由于实验过程中起点与终点位置重合，M(0,0)在创建后重新调用了一次。实验中当对已存在的子地图进行调用时，只根据SLAM的定位结果和传感器数据对地图进行更新，不另作路径的闭环处理。

图8为通过基于子地图框架的同步定位与地图创建过程后，由子地图序列组建的全局环境栅格地图。比较图8与图6可以看出，建立的全局地图与实际实验环境具有较好的一致性。

3.2.2 定位偏差分析

图9中比较了航迹推算和基于子地图的SLAM的定位结果。图中坐标系以正东方向为x轴正方向，正北方向为y轴正方向，以航迹推算的起点为原点。通过采用所提出的基于子地图框架的同步定位与地图创建方法，车辆行驶过程中航迹推算的定位结果得到了不断修正，如图9(a)所示。

图9(b)以航迹推算定位结果为基准，比较了航迹推算的定位结果与基于子地图的SLAM定位在数据采集过程中沿坐标轴方向的偏差dx和dy。由于研究中的航迹推算是根据车辆运动的距离和航向变化计算相邻时刻间的直线距离，其累计误差随着车辆的行驶，特别是车辆的转向动作而发生相对较大的变化。dx和dy的变化趋势与之相同。在实验最后运动路径闭合处即起、终点，dx和dy分别为-1.2和-4.7m。

3.2.3 实时性分析

图10反映出基于子地图的SLAM的实时性。图中横坐标表示车辆行驶过程中的各个时刻，纵坐标表示采用基于子地图的同步定位与地图创建方法在每个定位周期中的时间消耗。实验中采用的是大小为2500×2500个栅格的子地图，采用VS2008软件编写算法。可以看出，在实验过程中，绝大多数定位周期均小于50ms。个别定位周期大于50ms，但也小于车辆航迹推算的定位周期100ms，如图10中虚线所示。

由于软件编写采用单线程的方式，因此子地图的切换过程与定位过程使用同一进程。大于100ms的6个定位周期分别对应于在实验过程中的6次子地图切换，包含了子地图调用、存储和重合区域复制等操作。

3.3 大范围城市动态环境实验

本实验为在较大范围城市动态环境中同步定位与地图创建实验。车辆行驶距离约为4.4km,地图范围约为1km×1km。行驶环境中存在较多的车辆等动态目标。其它条件与校园环境实验相同，如图11所示。在未改变程序和参数的条件下，采用子地图框架的同步定位与地图创建实现了地图范围的动态增长。

4 结论

提出一种基于子地图框架的全局定位与栅格地图创建方法，通过子地图的形式解决了在车辆行驶过程中全局地图范围的动态增长。同时设计了子地图间的切换机制，避免了在相邻子地图间连续切换的状况出现。最后通过在不同环境范围内的地图创建实验验证了该方案的可行性，能够实现在不预先设置行驶范围的前提下创建全局栅格地图，并具有较好的实时性。在以后的研究中应进一步研究地图的组织与存储形式，提高地图调用的效率；同时须加强对环境中动态障碍物的检测和跟踪，以保证SLAM环境静止的假设。

[1]Durrant-WhyteH,BaileyT.SimultaneousLocalizationandMapping(SLAM):PartI[J].IEEERoboticsandAutomationMagazine,2006,13(2):99-108.

[2]BaileyT,Durrant-WhyteH.SimultaneousLocalizationandMapping(SLAM):PartII[J].IEEERoboticsandAutomationMagazine,2006,13(3):108-117.

[3] 刘洞波,刘国荣,王迎旭,等.基于区间分析无迹粒子滤波的移动机器人SLAM方法[J].农业机械学报,2012,43(10):155-160.

[4] 武二永,项志宇,沈敏一,等.大规模环境下基于激光雷达的机器人SLAM算法[J].浙江大学学报(工学版),2007,41(12):1982-1986.

[5]BarkbyS,WilliamsSB,PizarroO,etal.AFeaturelessApproachtoEfficientBathymetricSLAMUsingDistributedParticleMapping[J].JournalofFieldRobotics,2011,28(1):19-39.

[6]KonradM,SzczotM,SchuleF,etal.GenericGridMappingforRoadCourseEstimation[C].IEEEIntelligentVehiclesSymposium,2011:851-856.

[7]ElfesA.UsingOccupancyGridsforMobileRobotPerceptionandNavigation[J].Computer,1989,22(6):46-57.

[8]WeissT,SchieleB,DietmayerK.RobustDrivingPathDetectioninUrbanandHighwayScenariosUsingaLaserScannerandOnlineOccupancyGrids[C].IEEEIntelligentVehiclesSymposium,2007:184-189.

[9] 胡士强,敬忠良.粒子滤波算法综述[J].控制与决策,2005,20(4):361-365.

[10]ThrunS.ProbabilisticRobotics[J].CommunicationsoftheACM,2002,45(3):52-57.

[11]Trung-DungV,BurletJ,AycardO.Grid-basedLocalizationandLocalMappingwithMovingObjectDetectionandTracking[J].InformationFusion,2011,12(1):58-69.

A Sub-map-based Simultaneous Localization andMapping Technique for Intelligent Vehicles

Hu Yuwen, Gong Jianwei, Jiang Yan & Xiong Guangming

SchoolofMechanicalEngineering,BeijingInstituteofTechnology,Beijing100081

For enabling intelligent vehicle to create environmental grid map and achieve real-time positioning in a condition with unpredictable environment scope, a simultaneous localization and mapping (SLAM) technique base on sub-map frame is proposed.The overlapped zones and switching zones are set in sub-maps to avoid endless successive switching of adjacent sub-maps.The results of experiment show that the technique proposed can ensure the dynamic growth of map scope and stable switching between sub-maps in SLAM process of vehicle.

intelligent vehicle; SLAM; sub-map; grid maps

*国家自然科学基金(91120010)、教育部博士点基金(20121101120015)和北京理工大学基础研究基金(20120342011)资助。

原稿收到日期为2013年4月6日，修改稿收到日期为2013年7月3日。