0引言智能化移动机器人广泛应用于工业、军事、物流和家用服务等领域。从复杂的工业型机器人，到简单的扫地机器人, 完成同步定位与建图(simultaneous localization and mapping，SLAM)、路径规划和自主导航的能力必不可少。以市面上常见的低成本扫地机器人为例，采用的方案多是通过2D激光雷达获取其与某一平面内物体间的距离信息，利用激光SLAM建立占据栅格地图以支持机器人自主导航、路径规划等功能(Blanco等，2007)。随着智能化服务需求的日益增加，信息量简单的几何地图已不能满足人们的需求。譬如“清扫椅子那边”、“到冰箱那里”，这些任务需要机器人从几何层面上升到内容层面来感知环境。除通过栅格地图完成对环境的几何轮廓描述外，智能机器人还应具备目标识别(Mousavian等，2019)、语义分割(Chen等，2019)或场景分类(Kumari等，2020)等获取语义信息的功能，语义SLAM不仅了解环境的几何信息，还能感知到环境中的内容信息(Doherty等，2019)。SLAM是移动机器人在未知环境进行探索、感知和导航的关键技术(Hess等，2016)，可分为激光SLAM和视觉SLAM两类。激光SLAM测量精确，便于机器人导航和路径规划，但缺乏语义信息(Li等，2020)。而视觉SLAM的图像能提供丰富的语义信息，特征区分度更高，但其构建的地图不能直接用于路径规划和导航(Sualeh和Kim，2019)。为了实现移动机器人构建语义地图并在地图上进行路径规划，本文提出一种基于激光—相机系统的语义栅格建图方法，通过包围盒—分割匹配算法将激光雷达数据与相机图像进行物体级别的信息融合，采用激光SLAM建立语义栅格地图，并在此地图上发布语义任务，实现全局路径规划。本文的主要贡献如下:1) 提出了一种语义栅格建图方法，通过包围盒—分割匹配算法将激光扫描数据精确对应到环境中检测到的各个语义物体，提取语义激光分割数据，并实时同步建立标注物体类别和轮廓的语义栅格地图。2) 在语义栅格地图上进行路径规划，针对建图中易移动物体对机器人导航和路径规划产生的干扰。提出了一种语义赋权方法进行改善。3) 在移动机器人平台上部署激光SLAM算法Cartographer(Hess等，2016)和目标检测算法YOLOv3(you only look once v3)(Redmon和Farhadi，2018)进行语义建图，并采用A*算法(Šišlák等，2009)进行路径规划。通过多种室内环境下的实验验证本文系统的真实效果。1语义栅格建图和路径规划系统设计1.1系统框架为构建可用于路径规划的语义地图，本文以单目相机辅助激光雷达提取物体级别的特征，通过包围盒—分割匹配算法将激光扫描数据精确对应到各个语义物体，得到语义激光分割数据，并同步参与建图。在机器人构建占据栅格地图时，将保存语义信息的栅格称为语义栅格，语义建图更新各物体对应栅格的占据概率和栅格中的语义信息，最后通过全局优化、语义栅格聚类和语义栅格标注等步骤，得到标注物体类别和轮廓的语义栅格地图。此外，在语义栅格地图上发布语义任务，定位机器人在地图中的位置，使用A*算法(Šišlák等，2009)进行全局路径规划。对环境中的易移动物体，采用语义赋权算法识别，对路径规划进行改进。系统结构如图 1所示，主要分为语义激光数据提取、语义栅格建图和路径规划3部分。系统输入包括2D激光雷达的扫描数据和单目相机的图像，输出为可用于路径规划的语义栅格地图。 图1 语义栅格建图和路径规划系统 Semantic grid mapping and path planning systemFig 11.2语义激光数据提取传统SLAM算法采用的单一激光或视觉传感器不能同时满足获取语义和构建栅格地图的任务。为了构建语义栅格地图，对于环境中的每一个可识别物体，都需要得到其激光数据和匹配的语义信息。语义激光分割数据提取基于激光—相机系统，结构如图 2所示，激光雷达给出了空间中某一高度平面内的扫描数据，模块将处于相机视野内的激光分割投影到图像上并与检测框匹配，得到语义激光分割数据。 图2 激光雷达—相机系统 Lidar-camera systemFig 2为了实现匹配，首先要标定激光和相机的相对位置，得到目标检测的包围盒和激光分割数据，根据位置关系，将它们转换在同一坐标系下进行匹配(de Alvis等，2019)。首先，联合标定激光雷达和单目相机获取其相对位置，获得$({\mathit{\boldsymbol{K}}}, D)$和(${\mathit{\boldsymbol{R}}}$，${\mathit{\boldsymbol{T}}}$)。(${\mathit{\boldsymbol{K}}}$，$D$)的获取采用传统的棋盘格平面标定法(Park等，2020)，$D$是广角镜头的径向畸变参数。像素坐标系下坐标${\mathit{\boldsymbol{P}}}$、相机坐标系下坐标${\mathit{\boldsymbol{P}}}_{\rm{c}}$以及激光雷达坐标系下的坐标${\mathit{\boldsymbol{P}}}_{\rm {l}}$可通过以下变换关系转换到同一坐标系 1 $\boldsymbol{P}_{\mathrm{c}}=\boldsymbol{R} \cdot \boldsymbol{P}_{1}+\boldsymbol{T}$ 2 $\boldsymbol{P}=\boldsymbol{K} \cdot \boldsymbol{P}_{\mathrm{c}}$ 式中，${\mathit{\boldsymbol{K}}}$为相机内参矩阵，${\mathit{\boldsymbol{R}}}$表示激光雷达坐标系到相机坐标系的相对旋转关系，${\mathit{\boldsymbol{T}}}$表示两坐标系的相对平移关系。其次，利用目标检测算法获取可识别物体的成像位置，同时得到物体的语义，即类别和置信度。系统采用YOLOv3(Redmon和Farhadi，2018)算法，可得到图像中的物体类别标签$l$，置信度$s$，所处图像位置信息$(x, y)$和物体大小信息$(h, w)$。由物体所处的位置信息和大小信息，可得到物体在图像中的边界信息，即物体的包围盒${\mathit{\boldsymbol{Box}}}=\left\{ {{{(x, y),(h, w)}}} \right\}$，其类别标签$l$和置信度$s$即为物体的语义信息, 检测结果表示为 3 $\boldsymbol{D e t}=\left\{\left(l_{1}, s_{1}, \boldsymbol{B o x}_{1}\right), \cdots, \left(l_{n}, s_{n}, \boldsymbol{B} \boldsymbol{o} \boldsymbol{x}_{n}\right)\right\}$ 然后，获取激光扫描分割数据。激光分割是将激光测距数据分成若干组相邻的扫描点，这些扫描点将被视为与目标相对应的激光分割的候选点。分割过程参考Leutenegger等人(2015)的工作，首先根据欧氏距离将激光扫描点分为若干分组，然后采用迭代端点拟合算法将分组分裂，保留其中具有足够扫描点的分组作为初始分割，利用最小二乘回归计算初始分割的拟合直线，最后将属于同一物体的初始分割合并，合并后可得到某物体精确的激光分割数据，用$Seg_{i}$表示，$i$表示激光扫描时环境中物体的数量。因此，分割结果可表示为 4 $\boldsymbol{S e g}=\left\{\operatorname{Seg}_{1}, \operatorname{Seg}_{2}, \cdots, \operatorname{Seg}_{n}\right\}$ 最后是包围盒—分割匹配，将目标检测的结果${\mathit{\boldsymbol{Det}}}$与激光分割${\mathit{\boldsymbol{Seg}}}$相匹配，得到目标物体对应的精确激光分割，也就是将激光扫描分割数据精确对应到各个语义物体，得到语义激光分割数据。由于激光雷达和相机的频率不同，需要将获得的激光分割数据和语义信息进行时间同步后，根据算法1对它们进行匹配，对于每一个可识别物体，找到其唯一对应的激光分割数据，完成语义信息与激光扫描分割数据的融合。算法分以下几步进行：1) 对于任一激光扫描分割$Seg_{i}$，利用已知${\mathit{\boldsymbol{K}}}$、${\mathit{\boldsymbol{R}}}$和${\mathit{\boldsymbol{T}}}$计算其在视觉图像上的投影${\mathit{\boldsymbol{Pro}}}_{{i}}$；2) 循环匹配每一个包围盒${\mathit{\boldsymbol{Box}}}_{i}$，当投影包含扫描点数大于最小阈值、投影与原始激光扫描分割包含扫描点数的百分比大于最小阈值时，可找到投影唯一对应的包围盒${\mathit{\boldsymbol{Box}}}_{k}$，计算投影${\mathit{\boldsymbol{Pro}}}_{i}$在物体包围盒${\mathit{\boldsymbol{Box}}}_{k}$上的投影${\mathit{\boldsymbol{Pro}}}^k_{i}$作为物体对应的精确分割；3) 将激光扫描分割的位置信息与语义信息封装得到语义激光分割数据$Sls$。算法1  包围盒—分割匹配算法输入：分割${\mathit{\boldsymbol{Seg}}}$，目标检测${\mathit{\boldsymbol{Det}}}$，联合标定${\mathit{\boldsymbol{K}}}, {\mathit{\boldsymbol{R}}}, {\mathit{\boldsymbol{T}}}$；输出：语义激光扫描分割数据$Sls$；1) 初始化$Sls$，给定参数$min\_points，thread$；2) 删除超过相机FOV(field angle of view)的${\mathit{\boldsymbol{Seg}}}$；3) for每个$Seg_{i}$ do4)     初始化在图像上的投影${\mathit{\boldsymbol{Pro}}}_{i}$;5)     for在$Seg_{i}$的每个${\mathit{\boldsymbol{P}}}_{{\rm {l}}j}$ do6)         ${\mathit{\boldsymbol{P}}}_k={\mathit{\boldsymbol{K}}}({\mathit{\boldsymbol{R}}}·{\mathit{\boldsymbol{P}}}_{{\rm {l}}j}+{\mathit{\boldsymbol{T}}})$;7)         if ${\mathit{\boldsymbol{P}}}_{k}$在图像中then8)             ${\mathit{\boldsymbol{Pro}}}_{i}+={\mathit{\boldsymbol{P}}}_k$;9)         end if10)     end for11)     for在$Det$里的每个$(l_k, s_k, {\mathit{\boldsymbol{Box}}}_k)$12)         得到在${\mathit{\boldsymbol{Box}}}_{k}$中的${\mathit{\boldsymbol{Pro}}}_{i}$部分: ${\mathit{\boldsymbol{Pro}}}^k_{i}$13)         if    $ {\mathit{\boldsymbol{Pro}}}^k_{i}$ $min$_$points$ &&          $\frac{\left|\boldsymbol{P r o}_{i}^{k}\right|}{\left|\boldsymbol{P r o}_{i}\right|}$ $thread$ then14)         计算标定标引$RayID$；15)         $Sls+=(l_k, s_k, RayID)$;16)     end if17)   end for18) end for19) return $Sls$1.3语义栅格建图获取语义激光分割数据时要同步进行语义栅格建图，系统在激光数据构建栅格地图时，根据语义信息对栅格中对应物体做语义和轮廓的标记。可分栅格地图构建、栅格语义更新以及语义栅格聚类3步。栅格地图用栅格集合表示环境某一平面的几何结构，传统的栅格仅保存占据概率。本文在赋予栅格初始占据概率的同时，赋予初始物体类别(暂无语义)构成语义栅格。语义栅格会更新栅格获得的语义信息，累计获取栅格的可信语义。若地图原始语义栅格中无待融合栅格的物体语义信息，则将该物体信息直接赋予该栅格；若栅格中为相同物体的语义信息，则对其标签概率求平均值并更新其置信度。本文采用基于密度的DBSCAN(density-based spatial clustering of applications with noise)聚类算法(Tran等，2013)，判定零散语义栅格或处于某一簇中的少量不同类别栅格为噪声，将相同物体类别的栅格进行聚类，得到代表各独立物体的栅格集合。最后，按照栅格集合中的物体语义信息，对占据栅格地图的对应物体位置做不同颜色和文字的标记，得到语义栅格地图。1.4路径规划语义栅格地图含有环境的几何信息和内容信息，能为机器人路径规划提供具体的语义物体作为目标，辅助机器人进行导航。在地图已知的情况下，机器人需要在地图中定位自己的位置，同时发布目标位置来做全局路径规划，绕过地图中障碍物到达目的地。利用自适应蒙特卡罗定位(adaptive Monte Carlo localization，AMCL)对移动机器人在2维环境下进行概率定位(Hanten等，2017)，确定一个机器人的位姿信息。全局路径规划采用A*算法，并根据语义赋权算法对其进行优化。A*算法的估算函数为 5 $F(n)=G(n)+H(n)$ 式中，$G(n)$是从起点出发到达任一个顶点$n$的实际距离，$H(n)$是任意顶点$n$到目标位置点的估算距离，算法计算从起点到目标位置的路径，使代价$F(n)$最小。系统结构如图 3所示。 图3 路径规划系统结构 Path planning system structureFig 3SLAM建立的地图刻画的是实际环境中某时刻的状态(Xiao等，2019)，当其他时刻环境中的物体位置发生变化，如人走动或雨伞被拿走，SLAM地图会产生与实际环境不一致的问题，其定位往往会失效，对机器人导航产生干扰。尤其是建图在含有较多易移动物体的环境中，机器人导航很容易不准确。为降低环境中易移动物体对路径规划的干扰，本文提出了语义赋权算法，主要对语义栅格地图中的语义物体进行判别，将背包、雨伞和人等视为环境中的易移动物体，即建图时存在但实际环境中可能不在原地的物体，通过改变其在全局代价地图中的权重，在全局路径规划时可以得到通过易移动物体的路线。实际运行过程中，若易移动物体仍在原地，则机器人会重新规划路线绕行。首先，语义赋权算法会对栅格地图中的语义物体进行判别，确认其是否为易移动物体。$α$表示物体还在原地的概率，$D_{\rm{cost}}$表示绕开该物体的路径代价，$P_{\rm {cost}}$表示物体不在原地可直接通过的代价，则路径期望代价为 6 $E_{\text {cost }}=D_{\text {cost }} \cdot \alpha+P_{\text {cost }} \cdot(1-\alpha)$ 若期望代价与绕开该物体代价的差值大于实验设定的阈值$β$，则将该物体判断为易移动物体，修改其在全局代价地图中的权重值，将其看做空地，否则仍视为障碍物。全局代价地图${\mathit{\boldsymbol{costmap}}}$通过map_server获取语义栅格地图中的静态地图。静态地图中每个栅格$grid$存放地图的障碍物信息，有1或0两种取值，分别表示该栅格状态为障碍物或空闲。判别后的易移动物体需将其在静态地图中的状态由障碍改为空闲。具体过程如算法2所示。算法2语义赋权算法输入：语义栅格数据$grid$，物体代价表$O_{\rm{cost}}$；输出：全局代价地图${\mathit{\boldsymbol{costmap}}}$;1)     初始化$grid$、$O_{\rm{cost}}$，给定参数$β$；2)     for每个$grid_{i}$3)       if |$ D_{\rm{cost}}-E_{\rm{cost}}$|$β$ then4)         从${\mathit{\boldsymbol{costmap}}}$中得到${\mathit{\boldsymbol{static\_map}}}$5)         令$grid_{i}.cost=Free\_Space.cost$；6)       end if7)     end for8)     return ${\mathit{\boldsymbol{costmap}}}$经语义赋权算法得到的全局代价地图会传给路径规划算法使用，其效果如图 4所示。 图4 语义赋权算法示意图 Semantic cost method schematic diagramFig 42实验系统的硬件设施由移动机器人和控制机组成。机器人平台由Kobuki底盘、激光雷达RS-LiDAR、单目相机组成，控制机笔记本配置为主频2.5 GHz、Intel Core i5-7300HQ处理器、GTX 1050Ti GPU和8 GB内存。同时部署语义建图和路径规划系统作为软件设施，运行在ROS(robot operating system)环境下，测试系统为Ubuntu 16.04。2.1语义激光分割数据提取实验如图 5所示，分别是目标检测、激光扫描分割以及包围盒—匹配的示意图，对应环境中识别的相关物体，将其激光分割数据匹配到物体包围盒上，实现了激光和语义的融合。 图5 包围盒—分割匹配示意图 Box-seg match schematic diagramFig 5实验对语义激光数据提取的过程进行了性能评估，表 1是本文系统在相同条件下对不同物体进行目标检测、激光分割和包围盒—分割匹配的检测精度，其中包围盒—分割匹配是本文算法1的检测结果，经过包围盒—分割匹配处理，采用语义激光数据可以提高单一激光雷达分割的精度。 表1 不同物体的检测精度结果 目标物体 目标检测 激光分割 包围盒—分割匹配 椅子 99.2 88.5 95.4 微波炉 98.3 90.5 93.7 冰箱 98.5 84.7 92.2 雨伞 97.6 81.2 89.5 平均精度 98.4 86.2 92.7 Precision results of various objects  /%Table 1此外，由于雷达和物体距离的增加会导致激光分割数据的失败，从而使包围盒—分割匹配精度降低。实验计算物体上分配到的激光点数量与实际计算中的激光点数量的百分比，由于分割时会删除一些孤立的点，因此该值应当小于1，物体在进行校准、投影和测量中不可避免地会产生误差。表 2展示了不同距离下物体激光分割的精度。 表2 不同距离下物体的分割结果 距离/m 椅子 冰箱 微波炉 雨伞 1 88.2 96.4 92.5 89.4 2 85.3 98.3 98.2 87.7 3 84.5 95.1 96.1 87.9 4 85.6 95.6 96.5 81.8 5 82.6 94.2 fail 60.8 平均精度 85.2 95.9 - 81.5 Segmentation results of objects at different distances  /%Table 2 fail表示检测失败，“-”表示平均精度无法计算。2.2语义栅格建图实验语义栅格建图实现了移动机器人在走廊和办公室内的语义栅格建图。图 6(a)是在实验室利用单一激光传感器根据传统的激光SLAM算法Cartographer(Hess等，2016)建立的占据栅格地图，该地图只能得到环境中的几何信息，不能识别环境中的物体。图 6(b)是采用本文系统建图后得到的语义栅格地图，并对其中标注的语义物体和环境中的对应真实物体进行了对比, 结果证明本文的语义地图在占据栅格地图的基础上很好地融入了物体级别语义，丰富了地图内容，提高了地图的可读性。 图6 实验室语义栅格建图 Semantic grid map of laboratoryFig 6((a) original grid map; (b) comparison of semantic grid map and real environment objects)图 7展示了移动机器人在走廊上进行语义栅格建图的具体过程。移动机器人首先根据激光雷达对环境的扫描数据建立占据栅格地图，同时根据相机获取环境中的物体的语义信息，如图 7中的微波炉和椅子，并将其标示在地图上，最后可得到含有物体的类别和轮廓的语义栅格地图。 图7 走廊语义建图过程 Semantic mapping in a corridorFig 7((a) build a grid map based on laser data; (b) identify the microwave oven; (c) identify the chair; (d) complete semantic grid map construction)2.3路径规划实验经过语义栅格建图实验得到了语义栅格地图，根据地图中的语义信息可以实现基于语义的路径规划。如图 8所示，系统直接发布了“去冰箱处”和“去椅子(chair-2)处”的语义任务, 机器人能够避开地图中的障碍物，分别规划到达冰箱和椅子处的一条路径。结果证明本文的语义栅格地图能感知环境内容，为机器人导航和路径规划提供语义信息，支持机器人智能化服务。 图8 语义栅格地图下的路径规划结果 Path planning results under semantic grid mapFig 8针对易移动物体，本文在采用Cartographer建立的原始栅格地图和本系统建立的语义栅格地图上进行了对比实验。如图 9所示，由于建图时门口有把雨伞，在原始栅格地图中，机器人不能识别雨伞，必须绕行到达目的地。而根据语义赋权算法判别雨伞为易移动物体，实际环境中很可能不在原地，因此为机器人规划了一条穿过原地的路线。但当实际环境中雨伞仍停留在原地时，机器人可以重新规划一条绕行雨伞从当前位置出发到达目的地的路线。与原始地图中路线相比，经过语义判别后的路径更加灵活，更适应建图存在易移动物体的情况。 图9 易移动物体的路径规划对比图 Comparison of path planning with movable objectsFig 9此外，实验在仿真环境gazebo下搭建了一段20 m长的走廊，在其中随机布置了20个易移动物体，通过语义栅格建图得到包含了物体语义信息的语义栅格地图。给定机器人运动的起点和终点，在实际环境中每次随机移开一定数目的物体，分别采用原始路径规划A*算法与经过语义赋权的路径规划算法进行机器人导航，并计算其路径长度。表 3是多次实验后的路径长度对比，可以看出，未经语义判别的路径规划只会按照已建的栅格地图中的物体进行避障运动，无论实际环境中障碍物是否还存在，经过语义判别后的路径相对灵活，路径长度也较短，更适合在较多易移动物体的环境下使用。 表3 易移动物体的路程对比 移开物体数目 3 6 9 12 15 18 A*算法路程/m 27.2 27.2 27.2 27.2 27.2 27.2 语义赋权算法后路程/m 26.5 25.3 24.6 23.8 22.5 21.2 Comparison of distances for easily moving objectsTable 33结论提出了一种基于激光—相机系统的语义建图方法。通过包围盒—分割匹配和同步建图，将语义激光分割数据融入占据栅格地图中，实现了各物体对应语义激光分割的提取和语义栅格地图的标注，得到与真实环境一致的语义栅格地图。此外，系统在语义栅格地图的基础上，利用全局路径规划和语义赋权算法，给定语义任务，对机器人的运动轨迹进行规划，并减少语义地图中易移动物体对路径规划的影响。实验证明了机器人在语义栅格地图下的自主定位和路径规划功能。本文不足之处在于语义栅格建图时场景较小、物体的种类较少，主要原因是实验条件有限，所用室内小型机器人更适合在小场景下运行。因此，下一步的研究方向是尝试把工作转移到中型机器人上，扩展算法的应用条件，在较大场景下建立语义栅格地图，改善大场景下机器人定位易漂移、建图回环效果差的问题。