一种面向动态环境的多层代价地图更新算法

李德胜，孙一杰，张国良

(1.四川轻化工大学 自动化与信息工程学院，四川 宜宾 644000；2.人工智能四川省重点实验室，四川 宜宾 644000)

0 引言

导航技术是指移动机器人基于配置的激光雷达、相机和超声波等传感器对周围状态空间进行同时定位与地图构建(Simultaneous Localization and Mapping，SLAM)，进而规划出可安全执行的路径并有效地完成目标任务。移动机器人进行自主导航时主要使用的地图包括拓扑地图、几何地图、栅格地图和语义地图，这些地图各有优缺点和不同的应用对象[1-5]。当前，自主导航使用最多的是代价地图，其中单层代价地图存在有限的环境信息和固定的语义信息问题，限制了路径规划效率的提升[6]；而多层代价地图将环境信息简化成语义分离的层，扩展了地图所包含的环境信息，但存在匹配性低、耗时长和膨胀效率低等问题[7]。

多层代价地图在移动机器人导航领域具有重要的研究意义。Kong等[8]利用多层代价地图，实现自动导引车在平面环境中自主导航。Coleto等[9]提出一种多层代价地图架构，结合社会规范来生成对人类生活友好的运动轨迹。Groves等[10]设计了生成辐射代价图，并与代价地图进行信息融合，令机器人能探测到高辐射环境并自主离开。Chen等[11]提出新的深度强化学习算法，将局部占用率映射到代理层，提高机器人的决策能力。张福海等[12]优化了代价地图的生成方式，改善了代价地图匹配性低和实时性弱的问题。Han等[13]利用“相遇时间”的定义，将动态对象转换为虚拟静态对象，进而改善开放动态环境下导航方案的鲁棒性。Fang等[14]基于具有行人检测和跟踪功能的全局行人感知对群体互动和个人空间进行建模，生成多层动态代价地图，为全局路径规划提供预测阶段的社会约束信息。上述研究为地图构建模型的设计贡献了多种思路和应用案例，具有较好的启发价值。但这些研究还需进一步加强对动态环境下动态障碍物的处理，提高地图构建在处理环境信息方面的匹配性和实时性。

为获得动态环境下动态障碍物的位置信息，当前国内外学者对动态障碍物的运动轨迹估计问题进行了研究[15-18]。胡玉可等[19]为了提高目标船舶预测的准确性，提出了基于循环神经网络的预测算法。Kamil等[20]利用机器人的传感系统信息，将决策过程与障碍物速度矢量的预测行为相结合，避免了机器人与障碍物发生碰撞。Lindqvist等[21]应用分类方案来区分不同种类的轨迹以预测未来障碍物位置。Krämer等[22]使用在线轨迹优化来预测潜在的碰撞和任务变化，并相应地调整运动计划。这些研究对动态障碍物运动估计问题进行了不同程度的分析和处理，但并未考虑将动态障碍物的信息融合到多层代价地图。

为解决上述问题，提出了一种新的多层代价地图更新算法，对地图构建算法进行改进，在障碍物层添加对动态障碍物的检测和运动轨迹估计，将下一帧动态障碍物信息转换成当前帧的静态障碍物信息并融合进多层代价地图，从而提升代价地图处理环境障碍物信息的效能。

1 多层代价地图

1.1 多层代价地图的代价值

依据Bresenham光线追踪算法对环境信息进行处理，可得到占用一个字节的代价值。令μ表示代价值的比例因子(Cost Scaling Factor)，‖dij-o‖表示当前栅格与障碍物间的度量值，ri表示内切圆半径大小，则栅格(i,j)的代价值c(i,j)为:

c(i,j)=exp(-1.0·μ·(‖dij-o‖-ri))·253。

(1)

代价地图中栅格单元存在的代价值为0～254，并且按代价值大小将栅格单元表示为已占用的、自由的和未知的3个状态。由此，具有一定数量的栅格单元被标记已占用的，即被标记为costmap_2d::LETHAL_OBSTACLE代价；具有一定数量的栅格单元被标记未知的，即被标记为costmap_2d::NO_INFORMATION代价；剩下的栅格单元被标记自由的，即被标记为costmap_2d::FREE_SPACE代价。

1.2 多层代价地图的组成与更新

多层代价地图包括静态地图层、障碍物层和膨胀层，各个地图层包含环境的不同方面。在代价地图构建过程中，逐层更新当前地图层数据，最终构建为后续路径规划任务使用的多层代价地图，如图1所示。

图1 多层代价地图的组成Fig.1 Composition of layered costmap

多层代价地图通过维护一个有优先级的地图层列表，将静态地图层、障碍物层和膨胀层的数据信息依次更新到主代价地图中。主代价地图通过updateBounds函数和updateValues函数依次处理各个地图层数据。updateBounds函数循环查询每个地图层，以确定所更新代价地图的数量，然后地图层间依次迭代，为每个当前地图层提供历史地图层已更新的边界框(最初是空白框)。updateValues函数在更新过程中，将连续地图层的代价值依次更新到主代价图边界框中。

静态地图层：静态地图层接收到环境信息时，updateBounds函数将返回一个遍布整个代价地图的边界框，即使在不断地迭代过程中，由于其处理的对象是静态地图，边界框的状态也不会发生变化。当移动机器人在使用多层代价地图进行导航任务时，以及在执行SLAM算法对环境信息进行获取的过程中，代价地图只需独立地更新静态地图层信息，而不干涉障碍物层和膨胀层的地图更新过程。

障碍物层：该地图层通过激光雷达和RGB-D相机等高精度传感器对环境信息进行采集，然后将环境信息转换成代价地图信息存储到二维离散栅格中。高精度传感器读数之间的空间标记为自由移动区域，读数所在的位置标记为已占用区域。在每个循环周期执行updateBounds函数，不断更新的传感器数据被添加到障碍物层的代价地图中，期间边界框连续地进行扩展来跟踪所更新的传感器数据。此时，将障碍物层的代价值同处于代价地图底层的代价值相融合的方式有最大值更新、覆盖值更新和叠加值更新。默认方式是覆盖值更新，即用传感器所采集的数据覆盖、更新静态地图层数据，期间如果静态地图层数据的期望信任性更强，则该地图层中代价值的融合过程调整为只向主代价地图添加致命障碍物所对应的地图信息。

膨胀层：通过将代价地图中的障碍物按一定安全距离进行膨胀处理，使得机器人在后续路径规划时最大限度地避开障碍物。在膨胀处理期间，updateBounds函数将增加对应的边界框信息,以达到最近更新的致命障碍物都将被膨胀的目的，并且历史更新过的边界框范围外的旧致命障碍物也同样被膨胀；在不存储局部代价地图信息副本的情况下，updateValues直接更新主代价地图信息。

1.3 匹配性和实时性问题

经典障碍物层的代价地图构建过程存在一定的局限性。采取Bresenham 光线追踪算法来对周围环境空间存在的障碍物信息进行标记和清除。具体过程为激光雷达对周围环境情况进行数据探测，若探测到致命障碍物，就将其标记并插入到代价地图中；清除操作根据数据探测结果，从激光雷达的原点向四周进行光线跟踪，对代价地图的障碍信息采取删除处理。

当所探测的周围环境出现动态障碍物时，即动态障碍物处于运动的状态(动态障碍物的位置随时都可能发生变化)，激光雷达只能处理当前时刻动态障碍物所存在的位置信息，而无法处理动态障碍物在未来时刻的位置信息，进而无法对动态障碍物在未来时刻的位置信息进行标记。这将影响多层代价地图的构建过程，出现以下不理想的状况。

动态环境与激光雷达如图2所示，图中表示了在不同时刻下动态环境中障碍物所存在的位置，其中A为静态障碍物，B与B′为动态障碍物。

(a) 时刻t

(b) 时刻 t+1图2 动态环境与激光雷达Fig.2 Dynamic environment and lidar

如图2(a)所示，在时刻t下，激光雷达跟踪探测到当前周围环境存在的静态障碍物A并将其标记在代价地图中，此时静态障碍物A的标记过程并不会影响到代价地图构建过程的匹配性和实时性；而对于动态障碍物B的处理过程，能被激光雷达探测到当前时刻下的位置信息并同样被标记在代价地图中。

如图2(b)所示，在时刻t+1下，激光雷达跟踪探测到当前周围环境存在的静态障碍物A，此时静态障碍物A已被标记，并不会发生变化，因而不会影响到代价地图构建过程的匹配性和实时性。对于在时刻t+1下，动态障碍物B′已运动到其他位置，而代价地图中存储的环境信息仍是时刻t下的标记信息，代价地图中存在的标记障碍物信息不能实时地对在时刻t+1下动态障碍物B′的环境信息进行更新，并且存在代价地图所标记存储的环境信息并不完全与真实环境情况相一致，即障碍物动态变化时多层代价地图并不能及时地更新代价地图信息并且匹配性较差。

由此可见，在环境中存在动态障碍物的情况下，经典代价地图构建地图算法缺少对动态障碍物的检测和估计处理，导致所构建的地图并不能实时地处理激光雷达跟踪探测到的动态障碍物信息，并且代价地图与环境信息的匹配性较低。为提高多层代价地图所构建地图的匹配性和实时性，以有利于导航规划过程中安全性和准确性的提高，有必要对动态障碍物进行运动检测，对其运动轨迹进行估计，并将动态障碍物的运动估计信息融合到多层代价地图中。

2 融合动态障碍物信息的障碍物层地图

2.1 动态障碍物的检测

在室内动态环境下，移动机器人附近分布着多个静态或动态的障碍物。考虑环境中障碍物的历史位移信息，即考虑若干个连续相邻帧的点云坐标值，移动机器人可检测出障碍物的状态(静止或运动)信息。因此，在进行多层代价地图构建之前，对激光雷达处理的相邻2帧数据进行距离值匹配，即可判断障碍物是静态还是动态。

令激光雷达连续2帧数据的前一帧为A，后一帧为B，帧A与帧B的间隔时间为Δt，动态障碍物的速度为v1≤vd≤v2，则Δt时间内动态障碍物的位移为v1·Δt≤xd≤v2·Δt。计算帧A与帧B在相同索引值的点云位移值，进行距离值匹配：

① 当位移值小于v1·Δt时，表示帧B中的点云与帧A的点云为同一静态障碍物，此时将其标记为static(即静态障碍物，性质已知)；

② 当位移值大于或者等于v1·Δt且小于或者等于v2·Δt时，表示帧B中的点云与帧A的点云来自同一动态障碍物，此时将其标记为dynamic(即动态障碍物，性质已知)；

③ 当位移值大于v2·Δt时，表示帧B中的点云与帧A的点云为不同障碍物，此时将其标记为new(即新增的障碍物，性质未知)。

初始情况下，帧A中未传入激光雷达数据，在帧B中将点云信息全部标记为new。在初始帧之后的激光雷达点云信息则采取距离值匹配进行处理，处理过程结束后可检测出障碍物的状态信息，从而为后续动态障碍物的运动轨迹预测提供计算依据。

2.2 动态障碍物的运动轨迹估计

移动机器人借助激光雷达实时感知周围障碍物信息，连续多帧的激光雷达数据所对应的障碍物的位置信息构成了障碍物的状态信息。令R表示配置于移动机器人本体的激光雷达，O表示激光雷达感知到的动态障碍物，动态障碍物如图3所示。激光雷达随移动机器人本体从位置P以速度V向前运动到位置P′时，障碍物O从位置o以速度v向前运动到可能出现的位置o″。

图3 动态障碍物Fig.3 Dynamic obstacles

令激光雷达所探测到的动态障碍物Ok在任意时刻的位置为pk(t)，其中p=(xy)T。根据自回归(Autoregressive)模型建立动态障碍物的运动轨迹数学模型。已知动态障碍物连续时刻的位置序列值{pk(t),t=1,2,3,…}，则n阶自回归模型为：

(2)

式中，e(t)为预测误差矩阵；{αi,1

考虑到动态障碍物Ok的加速度变化较小(且激光雷达采样间隔时间很短)，用一阶自回归模型表示Ok的加速度ak(t)为：

ak(t)=βk,tak(t-1)+ω(t)，

(3)

式中，ω(t)为预测误差矩阵；由于移动机器人在二维空间中运动，则βk,t是普通参数矩阵。

假设βk,t与时间不相关，则位置pk(t)、速度vk(t)和加速度ak(t)相互间的关系为：

ak(t)=vk(t)-vk(t-1)=|pk(t)-pk(t-1)|-|pk(t-1)-pk(t-2)|=pk(t)-2pk(t-1)+pk(t-2)，

(4)

于是,可求解出：

pk(t)=(2I+βk,t)pk(t-1)-(2βk,t+I)pk(t-2)+βk,tpk(t-3)+ω(t)。

(5)

由于βk,t不与时间相关，βk,t=βk。利用最小二乘法拟合式(4)得到的加速度序列{ak(t),3≤t≤N} ，于是βk被估计为：

[ak(i)-βkak(i-1)]，

(6)

其解为：

(7)

在βk被估计出结果的前提下，动态障碍物下一时刻的位置为：

(8)

2.3 障碍层的地图构建

障碍层onInitialize函数初始化地图信息，创建观察缓冲区，对“LaserScan”“PointCloud”和“PointCloud2”类型的数据分别调用相应的回调函数来处理各自对应的激光雷达数据，处理结束后被添加到标记观察缓冲区。障碍层updateBounds函数更新边界，获取标记观察和清除观察数据，并更新当前状态信息；调用raytraceFreespace函数清理出激光雷达到障碍物间的自由活动区域，处理越界的障碍物点云数据后将新观测到障碍物信息添加到观察缓冲区；调用updateFootprint函数更新机器人轮廓，以及updateCosts函数采用最大值更新或覆盖更新将障碍物信息更新到mastermap。

经典算法在调用回调函数处理激光雷达数据的过程中，并未对动态环境的运动物体进行特殊处理，使得对运动物体的观测信息存在延时，导致地图信息更新不及时。为使障碍物层能处理运动物体，对激光雷达数据进行动态障碍物的检测，若激光雷达数据检测为动态障碍物，则根据历史雷达数据进行运动轨迹估计，便可在当前帧的点云数据中融合下一帧点云的运动估计数据，进而将下一帧动态障碍物信息转换成当前帧的静态障碍物信息并融合进多层代价地图；若激光雷达数据检测为静态障碍物，采用经典的障碍层构建算法进行障碍层地图的构建。

动态障碍物的信息融合进多层代价地图的方式采用叠加值更新法，具体为：

(9)

式中，cno为未知状态下的代价值；cins为障碍物位于移动机器人底盘内切圆内的代价值；cstat(i,j)为静态地图层中栅格坐标(i,j)对应的代价值；cdyn(i,j)为栅格坐标(i,j)处动态障碍物所对应的代价值；cmas(i,j)为总的多层代价地图中栅格坐标(i,j)对应的代价值。

3 仿真实验与结果验证

为实现所提出的面向动态环境的多层代价地图构建算法，在ROS Navigation中costmap_2d功能包的obstacle_layer插件增加动态障碍物的检测、运动预测和融合算法进行实验测试。采用ROS Navigation 导航功能框架，在Gazebo中搭建动态仿真环境，如图4所示。已建好的静态代价地图如图5所示，在动态环境(动态障碍物速度为0.05 m/s≤vd≤0.15 m/s)下进行实验。所使用的导航功能框架运行在Ubuntu16.04操作系统，ROS版本为Kinetic，CPU的主频2.2 GHz，运行内存8 GB。

图4 Gazebo动态仿真环境Fig.4 Gazebo dynamic simulation environment

图5 静态代价地图Fig.5 Static costmap

保证移动机器人和激光雷达在位姿恒定不变的前提下，动态障碍物在距离移动机器人6 m的位置(位于激光雷达扫描的范围以外)以0.1 m/s的速度向机器人底盘中心运动，激光雷达跟踪观察代价地图中动态障碍物的信息变化。Gazebo环境情况及代价地图的信息，如图6所示(从左至右分别为Gazebo环境模型、经典算法构建的多层代价地图和本文算法构建的多层代价地图)。此时代价地图并未出现动态障碍物的信息，只存在有3个静态障碍物和1个移动机器人模型。

图6 Gazebo仿真环境与多层代价地图的初始状态Fig.6 Gazebo simulation environment and the initial state of layered costmaps

当动态障碍物进入激光雷达扫描范围并逐渐接近移动机器人底盘中心时，Gazebo仿真环境如图7(a)所示。在该仿真环境下，经典算法构建的多层代价地图，如图7(b)所示；本文算法构建的多层代价地图，如图7(c)所示。图7(b)的红色线圈出的区域(被蓝色区域环绕的红色标记点对应环境中障碍物的点云点)为未优化的ROS Navigation 导航功能框架对当前激光雷达扫描周期内的障碍物信息进行更新，并将更新的障碍物信息标记在多层代价地图中。由于动态障碍物在未来时刻的运动轨迹信息并未融合到激光雷达扫描周期内的点云集中，经典算法未对动态障碍物的运动轨迹进行特殊处理。图7(c)的红色线圈出的区域(被蓝色区域环绕的红色标记点对应环境中障碍物的点云点)为优化后的ROS Navigation 导航功能框架将未来时刻的动态障碍物位置信息进行估计后,融合到当前激光雷达扫描周期内的障碍物信息中进行更新，并将其标记在多层代价地图中。优化的算法增加对未来时刻动态障碍物运动轨迹信息处理，使得多层代价地图中的标记信息在处理了当前激光雷达扫描周期内的障碍物信息的基础上，包含了后续激光雷达扫描周期中将处理的障碍物信息(即图7(c)的结果在图7(b)结果的基础上增加了动态障碍物运动轨迹估计信息所对应的红色标记点)。

(a) Gazebo仿真环境

(b) 经典算法

(c) 本文算法图7 Gazebo仿真环境与多层代价地图对比Fig.7 Comparison of Gazebo simulation environment and layered costmap

通过对比实验结果可以看出，优化后的算法解决了经典算法在多层代价地图构建过程中存在的匹配性和实时性问题，实现了“运动即可标记”的多层代价地图。

4 结束语

针对经典多层代价地图处理动态环境的实时性和匹配性问题，本文提出了一种面向动态环境的多层代价地图更新算法，对激光雷达的点云进行距离值匹配，以此从获取到的环境信息中检测出动态障碍物，接着采用自回归模型以改变对激光雷达信息的处理方式，使得构建的代价地图可融合下一帧动态障碍物的位置信息。研究障碍物层的地图构建过程，改善了经典地图构建算法在处理动态环境存在的匹配性和实时性问题。根据具有差异性的代价地图实验结果，验证了本文所提出的优化方案在更新动态环境的地图信息方面确有明显改善。后续工作将在面向动态环境结合本算法进行代价地图构建的基础上，探索移动机器人导航中更高效的导航规划算法。