CN115993121A - 一种室内移动机器人三维地图构建与维护方法 - Google Patents

Abstract

本发明公开了一种室内移动机器人三维地图构建与维护方法，在地图构建阶段，通过对当前传感器数据进行预处理操作和融合操作，得到当前含有语义信息的局部点云地图，构建含有语义信息的全局初始点云地图和物体初始数据库；在地图维护阶段，通过对当前定位数据和传感器数据进行预处理操作，得到当前帧含有语义信息的局部点云地图和物体观测集合；对物体观测集合和当前物体数据库进行数据关联和更新，并根据物体数据库更新情况对三维地图进行更新维护，获得更新后的全局点云地图和物体数据库；解决了现有技术中由于场景变化，导致已构建地图无法良好地表征环境的真实情况，从而降低机器人的定位、导航与作业能力的技术问题。

Description

一种室内移动机器人三维地图构建与维护方法

技术领域

本发明涉及机器人技术领域与地图处理技术领域，尤其涉及到一种室内移动机器人三维地图构建与维护方法。

背景技术

随着科学技术的发展与进步，移动机器人的应用领域逐渐扩大，其所面对的工作环境也越来越复杂，目前，自主移动机器人已被部署在具有挑战性的室内环境中进行各种类型的延伸作业，如仓库机器人、物流机器人和室内监测机器人等。在机器人作业过程中，自主定位与导航是系统应对各种复杂环境产生的难题的重要一步，地图构建与维护则是自主定位与导航的关键基础，可靠三维地图的构建与维护直接影响到移动机器人在作业过程中的准确性和稳定性。

在室内移动机器人应用场景中，为应对复杂环境与工况，常常采用多线激光雷达扫描周围环境特征以构建三维点云地图。在机器人长时间作业过程中，室内场景容易发生变化，环境中的部分物体会经常发生位置上的改变，如货物的存取、移动等，导致已构建的三维地图无法良好地表征环境的真实情况，使得地图质量下降，进而影响机器人的定位、导航及作业任务，造成系统的稳定性与准确性降低。因此，对复杂场景下的地图进行及时更新与维护，对提高机器人的长期定位与作业能力具有重要意义。

发明内容

有鉴于此，本发明的目的是提供一种室内移动机器人三维地图构建与维护方法，用以解决现有技术中由于场景变化，导致已构建地图无法良好地表征环境的真实情况，机器人实时激光扫描的数据与地图的匹配度不佳，从而影响机器人的定位与导航效果，甚至导致机器人无法执行任务的技术问题。

一种室内移动机器人三维地图构建及维护方法，包括三维地图构建及维护过程；所述三维地图构建过程具体包括：

步骤S110：获取移动机器人的惯性测量单元数据、图像数据和激光点云数据；对所述惯性测量单元数据进行预处理操作，得到当前激光帧的位姿变换，对所述图像数据进行预处理操作，得到当前图像帧的语义理解结果，对所述激光点云数据进行预处理操作，得到当前激光帧的局部地图结果；

步骤S120：对所述当前激光帧的位姿变换、语义理解结果、局部地图结果进行融合处理，获得当前含有语义信息的局部点云地图；

步骤S130：对所述局部点云地图与之前获取的所有局部点云地图进行融合处理，构建含有语义信息的全局初始点云地图和初始物体数据库；所述初始物体数据库包含初始状态下的环境各物体情况与属性，各物体情况与属性包括：物体点云，物体3D包围框，物体位姿，物体类别，物体状态概率分布；其中，物体状态概率分布用于表示物体的几何变化程度和固定性似然概率；

所述三维地图维护过程包括：

步骤S210：获取移动机器人的传感器数据，对所述传感器数据进行预处理操作，获得当前定位信息、图像帧语义理解结果和激光帧局部地图；

步骤S220：对所述当前定位信息、图像帧语义理解结果、激光帧局部地图进行融合处理，获得当前帧含有语义信息的局部点云地图和物体观测集合；所述物体观测集合包含当前帧观测到的各物体情况与属性，具体包括：物体点云、物体3D包围框、物体位姿以及物体类别；

步骤S230：对所述物体观测集合和当前物体数据库进行数据关联，根据所述数据关联结果对当前物体数据库中的物体属性进行更新，包括：①将未成功关联上当前物体数据库的观测物体标记为新物体；②将成功关联上当前物体数据库的观测物体存储起来，并将对应的物体数据库内的物体标记为待更新物体；③将未被当前物体观测集合关联上的物体数据库中的物体标记为未成功观测物体；

步骤S240：根据所述物体数据库更新情况对室内移动机器人三维地图进行更新维护，获得更新后的全局点云地图和物体数据库，具体为：

1)首先对待更新物体

和对应观测物体

进行几何变化程度估计；如果所述几何变化程度估计值小于设定阈值θ_change，不更新数据库中该物体的固定性似然概率；如果反之，将其固定性似然概率更新为低于一个设定值；

2)对于未成功观测物体，将固定性似然概率更新为低于设定值；

3)然后根据所述当前数据库中各物体更新后的固定性似然概率分布情况，将其与设定阈值θ_p比较；如果所述固定性似然概率大于相应阈值θ_p，则认为所述物体未发生几何变化，当前数据库中对应的物体属性保持不变；如果所述固定性似然概率小于相应阈值θ_p，则认为所述物体发生了较大几何变化，清除当前数据库中对应的物体，若所述物体标记为待更新物体，则将对应的观测物体存入物体数据库，并赋予初始默认属性，若所述物体标记为未成功观测物体，则直接清除；此外，将所有标记为新物体的观测物体存入物体数据库，并赋予初始默认属性；

4)根据上述处理获得更新后的物体数据库，并根据所述新物体数据库更新全局点云地图，获得更新后的新全局点云地图，用以指导后续的机器人任务。

较佳的，所述步骤S230中，对所述物体观测集合和当前物体数据库进行数据关联的方法包括：

对所有小于设定距离阈值的所述物体观测集合中的物体

和当前物体数据库中的物体

分别进行最近邻迭代匹配，获取对应物体对的相对位姿变换关系T_i,j和几何不相似度δ_i,j；其中，相对位姿变换关系T_i,j由对应物体对的位置变换p_i,j和朝向角变换φ_i,j组成；几何不相似度δ_i,j表示经过最近邻迭代匹配后对应物体对的未成功匹配点云点数量比例；

然后，根据每一个物体对之间的相对位姿变换关系和语义一致性构建所述物体对的代价值C(i,j)，并基于所有可用物体对之间的代价值构建所述物体观测集合和当前物体数据库之间的代价矩阵C；其中，

C(i,j)＝λ₁||p_i,j||+λ₂|φ_i,j|+λ₃(1-Π(c_i＝c_j))

其中，λ₁、λ₂、λ₃为比例系数，m,n分别表示物体观测集合中的物体数量和物体数据库中的物体数量；Π(c_i＝c_j)为比较运算函数，用于表示物体对的语义一致性，即当物体对的语义属性c_i与c_j相同时，Π(c_i＝c_j)＝1，当c_i与c_j不相同时，Π(c_i＝c_j)＝0；

如果某物体对的几何不相似度超过设定值或代价值超出设定的截断阈值，将其代价值设置为无穷大；接着，使用匈牙利算法对所述代价矩阵求解，获得物体观测集合和当前物体数据库之间的最优数据关联，获取物体观测集合和当前物体数据库中所有的最优关联物体对。

较佳的，所述几何变化程度表示为：Δ_i,j＝β₁||p_i,j||+β₂|φ_i,j|+β₃δ_i,j，其中，β₁、β₂、β₃为比例系数。

较佳的，所述步骤S110中，将惯性测量单元数据的时间戳与激光点云数据的时间戳进行同步与对齐，对上一帧激光数据帧与当前激光数据帧之间的惯性测量单元数据进行预积分，获得惯性测量单元的位姿变化，并根据惯性测量单元与激光雷达之间的外参关系获得当前激光帧的位姿变换。

较佳的，所述步骤S110中，将图像数据的时间戳与激光点云数据的时间戳进行同步与对齐，对与当前激光点云数据时间戳最相近的图像数据进行语义分割，获取所述图像数据中各个像素的语义类别信息。

较佳的，所述步骤S110中，对所述激光点云数据进行预处理操作的过程包括：对所述激光点云数据进行畸变校正，通过对所述激光点云数据的每一扫描帧进行线性插值，获取对应扫描时刻的惯性测量单元位姿，并根据惯性测量单元与激光雷达之间的外参关系获得对应扫描时刻激光点云的位姿变换，实现点云畸变校正，得到当前激光帧的局部地图结果。

较佳的，所述步骤S120中，对所述当前激光帧位姿变换、当前图像帧语义理解结果、当前激光帧局部地图进行融合处理的过程包括：根据当前图像帧与当前激光帧的外参关系，将当前图像帧语义理解结果投影至当前激光帧局部地图中，对当前激光帧局部地图中所有点云数据赋予语义属性，得到含有语义信息的局部点云地图。

较佳的，对所述局部点云地图和之前获取的所有局部点云地图进行融合处理的过程包括：根据所述局部点云地图、之前获取的所有局部点云地图及其对应的位姿变换关系生成全局点云地图，并对所述全局点云地图进行降采样处理，获得全局初始点云地图，所述全局初始点云地图包含初始状态下的环境点云几何信息和语义信息；然后对所述全局初始点云地图进行地面分割和聚类，提取点云数据中的地面和物体情况，并构建物体初始数据库。

较佳的，所述物体3D包围框为物体点云的3D包络框，其Z轴方向垂直于地面向上，X轴方向和Y轴方向与物体在水平面上的长轴和短轴一致；物体位姿由物体点云的形心位置与物体3D包围框的姿态组成；物体类别表示物体的语义属性。

较佳的，所述步骤S220的具体方法包括：在获得所述当前定位数据、当前图像帧语义理解结果、当前激光帧局部地图之后，通过标定得到的当前图像帧与当前激光帧的外参关系，将当前图像帧中每一个像素的语义理解结果投影至当前激光帧局部地图中，对当前激光帧局部地图中所有点云数据赋予对应的语义属性，此时得到当前时刻含有语义信息的局部点云地图；然后对所述含有语义信息的局部点云地图进行地面分割，提取出点云数据中的地面点云部分和非地面点云部分，并对所述去除地面点云部分后的全局点云地图进行带有语义信息约束的欧式聚类，提取出所述全局点云地图中的所有物体情况，结合所述物体情况和当前定位数据构建当前帧物体观测集合。

本发明具有如下有益效果：

通过上述技术方案，构建了含有语义信息的室内环境三维点云地图和含有物体属性的物体数据库，通过对比室内移动机器人的当前观测与当前物体数据库，基于贝叶斯推理更新当前物体数据库的所有物体属性，并在此基础上完成对物体数据库和三维点云地图更新与维护，能够大大提高室内移动机器人三维地图的准确性、稳定性和可靠性，保证移动机器人的建图质量，提高移动机器人的长期定位、导航与作业能力。

附图说明

图1为本发明中一种室内移动机器人三维地图构建方法的流程示意图。

图2为本发明中一种室内移动机器人三维地图维护方法的流程示意图。

图3为本发明中提供的一种室内移动机器人三维地图构建与维护装置的结构框图。

图4为本发明中提供的计算机设备的一种结构框图。

图5为本发明中提供的计算机可读存储介质的结构框图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

参阅图1，图1示出了本申请一个实施例提供的室内移动机器人三维地图构建方法的流程示意图。下面将针对图1所示流程进行详细阐述，所述地图构建方法具体可以包括以下步骤：

步骤S110：获取移动机器人的传感器数据，对所述传感器数据进行预处理操作，获得当前激光帧位姿变换、图像帧语义理解结果和激光帧局部地图。

具体而言，所述一种室内三维地图构建的方法应用于所述一种室内三维地图构建的装置，可以通过惯性测量单元数据、图像数据和激光点云数据建立初始三维点云地图和初始物体数据库。所述惯性测量单元、图像采集传感器、激光雷达均刚性设置于移动机器人上，在移动机器人运动过程中，惯性测量单元采集机器人的高频惯性传感数据，图像采集传感器采集机器人周边环境的图像数据，激光雷达采集机器人周边环境的激光点云数据。此外，通过与所述一种室内三维地图构建的装置通信连接，将实时扫描到的环境数据传递给系统进行分析和预处理。通过将惯性测量单元数据的时间戳与激光点云数据的时间戳进行同步与对齐，对上一帧激光数据帧与当前激光数据帧之间的惯性测量单元数据进行预积分，获得惯性测量单元的位姿变化，并根据惯性测量单元与激光雷达之间的外参关系获得当前激光帧的位姿变换。通过将图像数据的时间戳与激光点云数据的时间戳进行同步与对齐，对与当前激光点云数据时间戳最相近的图像数据进行语义分割，获取所述图像数据中各个像素的语义类别信息。通过对所述激光点云数据进行畸变校正，通过对所述激光点云数据的每一扫描帧进行线性插值，获取对应扫描时刻的惯性测量单元位姿，并根据惯性测量单元与激光雷达之间的外参关系获得对应扫描时刻激光点云的位姿变换，实现点云畸变校正，得到当前激光帧的局部地图结果。通过获得以上预处理结果，为后续机器人三维地图构建提供了数据基础。

步骤S120：对所述当前激光帧位姿变换、图像帧语义理解结果、激光帧局部地图进行融合处理，获得当前含有语义信息的局部点云地图。

具体而言，在获取到所述当前激光帧位姿变换、当前图像帧语义理解结果和当前激光帧局部地图后，根据通过标定得到的当前图像帧与当前激光帧的外参关系，将当前图像帧中每一个像素的语义理解结果投影至当前激光帧局部地图中，对当前激光帧局部地图中所有点云数据赋予语义属性，即每个点云都包含XYZRGB信息，此时可得到当前时刻含有语义信息的局部点云地图。

步骤S130：对所述局部点云地图和之前获取的所有局部点云地图进行融合处理，构建含有语义信息的全局初始点云地图和物体初始数据库。

具体而言，将所述当前时刻处理得到的含有语义信息的局部点云地图和之前处理得到的所有含有语义信息的局部点云地图进行融合处理，根据所述局部点云地图、之前获取的所有局部点云地图及其对应的位姿变换关系拼接生成全局点云地图，拼接过程通过局部优化、全局优化等方式减小拼接误差，并对所述优化过后的全局点云地图进行降采样处理，获得全局初始点云地图，所述全局初始点云地图包含初始状态下的环境点云几何信息和语义信息等；然后先对所述全局初始点云地图进行地面分割，提取点云数据中的地面点云信息，并对所述去除地面后的全局点云地图进行欧式聚类，提取点云数据中物体情况，根据所述物体情况构建物体初始数据库，所述物体初始数据库包含初始状态下的环境各物体情况与属性，可以较为完整地表征环境中的物体情况。

其中，所述物体初始数据库中各物体情况与属性包括：物体点云，物体3D包围框，物体位姿，物体类别，物体状态概率分布。所述物体点云为物体在三维地图中的点云簇信息，主要包含点云的几何信息；所述物体3D包围框为物体点云的3D包络框，其Z轴方向垂直于地面向上，X轴方向和Y轴方向与物体在水平面上的长轴和短轴一致，其通过对物体点云信息进行主成分分析确定；所述物体位姿由物体点云的形心位置与物体3D包围框的姿态组成；所述物体类别表示物体的语义属性；所述物体状态概率分布用于表示物体的几何变化程度和固定性似然概率。

参阅图2，图2示出了本申请一个实施例提供的室内移动机器人三维地图维护方法的流程示意图。下面将针对图2所示流程进行详细阐述，所述地图维护方法具体可以包括以下步骤：

步骤S210：获取移动机器人的传感器数据，对所述传感器数据进行预处理操作，获得当前定位信息、图像帧语义理解结果和激光帧局部地图。

具体而言，所述一种室内三维地图维护的方法应用于所述一种室内三维地图维护的装置，可以通过定位模块数据、图像数据和激光点云数据更新维护三维点云地图和物体数据库。所述定位模块、图像采集传感器、激光雷达均刚性设置于移动机器人上，在移动机器人运动过程中，定位模块采集机器人的相应定位数据，如惯性传感器数据等，图像采集传感器采集机器人周边环境的图像数据，激光雷达采集机器人周边环境的激光点云数据。此外，通过与所述一种室内三维地图构建的装置通信连接，将实时扫描到的环境数据传递给系统进行分析和预处理。通过将定位模块采集到的数据时间戳与激光点云数据时间戳进行同步与对齐，将当前时刻激光点云数据与当前三维点云地图进行匹配和优化，并融合惯性测量单元等传感器数据获得机器人的位姿变化，实现机器人的自我定位，并根据机器人与激光雷达之间的外参关系获得当前激光帧的位姿变换。通过将图像数据的时间戳与激光点云数据的时间戳进行同步与对齐，对与当前激光点云数据时间戳最相近的图像数据进行语义分割，获取所述图像数据中各个像素的语义类别信息。通过对所述激光点云数据进行畸变校正，通过对所述激光点云数据的每一扫描帧进行线性插值，获取对应扫描时刻的惯性测量单元位姿，并根据惯性测量单元与激光雷达之间的外参关系获得对应扫描时刻激光点云的位姿变换，实现点云畸变校正，得到当前激光帧的局部地图结果。通过获得以上预处理结果，为后续机器人三维地图更新维护提供了数据基础。

步骤S220：对所述当前定位信息、图像帧语义理解结果、激光帧局部地图进行融合处理，获得当前帧含有语义信息的局部点云地图和物体观测集合。

具体而言，在获得所述当前定位数据、当前图像帧语义理解结果、当前激光帧局部地图之后，通过标定得到的当前图像帧与当前激光帧的外参关系，将当前图像帧中每一个像素的语义理解结果投影至当前激光帧局部地图中，对当前激光帧局部地图中所有点云数据赋予对应的语义属性，此时可得到当前时刻含有语义信息的局部点云地图；然后对所述含有语义信息的局部点云地图进行地面分割，提取出点云数据中的地面点云部分和非地面点云部分，并对所述去除地面点云部分后的全局点云地图进行带有语义信息约束的欧式聚类，提取出所述全局点云地图中的所有物体情况，结合所述物体情况和当前定位数据构建当前帧物体观测集合，所述物体观测集合包含当前帧观测到的各物体情况与属性，可以较为完整地表征当前机器人扫描到的环境中的物体情况。

其中，所述当前物体观测集合中各物体情况与属性包括：物体点云，物体3D包围框，物体位姿以及物体类别。所述物体点云为物体在三维地图中的点云簇信息，主要包含点云的几何信息；所述物体3D包围框为物体点云的3D包络框，其Z轴方向垂直于地面向上，X轴方向和Y轴方向与物体在水平面上的长轴和短轴一致，其通过对物体点云信息进行主成分分析确定；所述物体位姿由物体点云的形心位置与物体3D包围框的姿态组成；所述物体类别表示物体的语义属性。

步骤S230：对所述物体观测集合和当前物体数据库进行数据关联，根据所述数据关联结果对当前物体数据库中的物体属性进行更新。

具体而言，在获得当前帧含有语义信息的局部点云地图和物体观测集合后，先对所述物体观测集合和当前物体数据库进行数据关联。对所有小于一定距离阈值的所述物体观测集合中的物体

和当前物体数据库中的物体

分别进行最近邻迭代匹配，获取对应物体对的相对位姿变换关系T_i,j和几何不相似度δ_i,j。其中，相对位姿变换关系T_i,j由对应物体对的位置变换p_i,j和朝向角变换φ_i,j组成；几何不相似度δ_i,j表示经过最近邻迭代匹配后对应物体对的未成功匹配点云点数量比例。

然后，根据每一个物体对之间的相对位姿变换关系和语义一致性构建所述物体对的代价值C(i,j)，并基于所有可用物体对之间的代价值构建所述物体观测集合和当前物体数据库之间的代价矩阵C。其中，

C(i,j)＝λ₁||p_i,j||+λ₂|φ_i,j|+λ₃(1-Π(c_i＝c_j))

其中，λ₁、λ₂、λ₃为比例系数，m,n分别表示物体观测集合中的物体数量和物体数据库中的物体数量；Π(c_i＝c_j)为比较运算函数，用于表示物体对的语义一致性，即当物体对的语义属性c_i与c_j相同时，Π(c_i＝c_j)＝1，当c_i与c_j不相同时，Π(c_i＝c_j)＝0。

如果某物体对的几何不相似度过高或代价值超出设定的截断阈值，其代价值会被设置为无穷大。接着，使用匈牙利算法对所述代价矩阵求解获得物体观测集合和当前物体数据库之间的最优数据关联，获取物体观测集合和当前物体数据库中所有的最优关联物体对。

在获得所述数据关联结果后，根据所述数据关联结果对当前物体数据库中的物体属性进行更新：①将未成功关联上当前物体数据库的观测物体标记为新物体；②将成功关联上当前物体数据库的观测物体存储起来，并将对应的数据库内的物体标记为待更新物体；③将未被当前物体观测集合关联上的物体数据库中的物体标记为未成功观测物体。

步骤S240：根据所述物体数据库更新情况对室内移动机器人三维地图进行更新维护，获得更新后的全局点云地图和物体数据库，具体而言，在得到所述物体数据库更新情况后对室内移动机器人三维地图进行更新维护。首先基于贝叶斯推理对标记为待更新和未成功观测的当前数据库中的物体进行固定性概率分布更新，具体为：

1)首先对待更新物体

和对应观测物体

进行几何变化程度估计，几何变化程度Δ_i,j＝β₁||p_i,j||+β₂|φ_i,j|+β₃δ_i,j，其中，β₁、β₂、β₃为比例系数，p_i,j、φ_i,j分别表示对应物体对的位置变换和朝向角变换，δ_i,j表示对应物体对的几何不相似度。

如果所述几何变化程度估计值小于设定阈值θ_change，说明当前观测可以良好地表征数据库中对应的物体，则不更新数据库中该物体的固定性概率；如果反之，说明当前观测无法良好地表征所述物体，则更新其固定性概率为一个较低值。

2)对于未成功观测物体，更新其固定性概率低于设定值。

3)然后根据所述当前数据库中各物体更新后的固定性概率分布情况，将其与设定阈值θ_p比较。如果所述固定性概率大于相应阈值θ_p，则认为所述物体未发生几何变化，当前数据库中对应的物体属性保持不变；如果所述固定性概率小于相应阈值θ_p，则认为所述物体发生了较大几何变化，应当清除当前数据库中对应的物体，若所述物体标记为待更新物体，则将对应的观测物体存入物体数据库，并赋予初始默认属性，若所述物体标记为未成功观测物体，则直接清除。此外，将所有标记为新物体的观测物体存入物体数据库，并赋予初始默认属性。

4)接着，根据上述处理获得更新后的物体数据库，并根据所述新物体数据库更新全局点云地图，获得更新后的新全局点云地图，用以指导后续的机器人任务。达到了提高室内移动机器人三维地图的准确性、稳定性和可靠性，保证移动机器人的建图质量，提高移动机器人的长期定位、导航与作业能力的有益作用。

参阅图3，其示出了本申请一个实施例提供的一种室内移动机器人三维地图构建与维护装置300的结构框图，地图构建与维护装置300包括：传感器数据获取模块310、数据处理模块320、地图构建模块330、定位模块340、地图维护模块350。其中，所述传感器数据获取模块310用于获取激光雷达数据、图像数据、惯性测量单元数据等，所述传感器刚性连接于系统，所述传感器数据通过通信连接于系统，且由同一时钟源进行授时；所述数据处理模块320用于处理所述传感器数据获取模块获取的传感器数据，包括数据时间戳同步对齐、惯性测量数据预积分、激光点云去畸变、图像语义理解、多传感器数据融合等；所述地图构建模块330用于将含有语义信息的局部点云进行融合并降采样，生成全局三维点云地图，并通过地面分割、聚类等方式构建物体数据库；所述定位模块340用于根据当前三维地图，通过配准、融合等方式获取机器人在地图中的位姿信息；所述地图维护模块350用于通过比较当前机器人对环境的物体观测和三维地图中的物体数据库，对当前物体数据库的情况和属性进行更新，并在此基础上对物体数据库和三维点云地图完成更新与维护。

参阅图4，其示出了本申请一个实施例提供的一种计算机设备400的结构框图。本申请中的计算机设备400可以包括一个或多个如下部件：处理器410、存储器420、以及一个或多个应用程序，其中一个或多个应用程序可以被存储在存储器420中并被配置为由一个或多个处理器410执行，一个或多个程序配置用于执行如前述方法实施例所描述的方法。

处理器410可以包括一个或者多个处理核。处理器410利用各种接口和线路连接整个计算机设备内的各个部分，通过运行或执行存储在存储器420内的指令、程序、代码集或指令集，以及调用存储在存储器420内的数据，执行计算机设备的各种功能和处理数据。可选地，处理器410可以采用数字信号处理(Digital Signal Processing，DSP)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)、可编程逻辑阵列(Programmable LogicArray，PLA)中的至少一种硬件形式来实现。处理器410可集成中央处理器(CentralProcessing Unit，CPU)、图形处理器(Graphics Processing Unit，GPU)和调制解调器等中的一种或几种的组合。其中，CPU主要处理操作系统、用户界面和应用程序等；GPU用于负责显示内容的渲染和绘制；调制解调器用于处理无线通信。可以理解的是，上述调制解调器也可以不集成到处理器410中，单独通过一块通信芯片进行实现。

存储器420可以包括随机存储器(Random Access Memory，RAM)，也可以包括只读存储器(Read-Only Memory)。存储器420可用于存储指令、程序、代码、代码集或指令集。存储器420可包括存储程序区和存储数据区，其中，存储程序区可存储用于实现操作系统的指令、用于实现至少一个功能的指令(比如触控功能、声音播放功能、图像播放功能等)、用于实现下述各个方法实施例的指令等。存储数据区还可以存储计算机设备在使用中所创建的数据(比如电话本、音视频数据、聊天记录数据)等

参阅图5，其示出了本申请一个实施例提供的一种计算机可读存储介质的结构框图。该计算机可读存储介质500中存储有程序代码，所述程序代码可被处理器调用执行上述方法实施例中所描述的方法。计算机可读存储介质500可以是诸如闪存、EEPROM(电可擦除可编程只读存储器)、EPROM、硬盘或者ROM之类的电子存储器。可选地，计算机可读存储介质500包括非易失性计算机可读介质。计算机可读存储介质500具有执行上述方法中的任何方法步骤的程序代码510的存储空间。这些程序代码可以从一个或者多个计算机程序产品中读出或者写入到这一个或者多个计算机程序产品中。程序代码510可以例如以适当形式进行压缩。

最后应说明的是：以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不驱使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围。

Claims (10)

1.一种室内移动机器人三维地图构建及维护方法，其特征在于，包括三维地图构建及维护过程；所述三维地图构建过程具体包括：

步骤S110：获取移动机器人的惯性测量单元数据、图像数据和激光点云数据；对所述惯性测量单元数据进行预处理操作，得到当前激光帧的位姿变换，对所述图像数据进行预处理操作，得到当前图像帧的语义理解结果，对所述激光点云数据进行预处理操作，得到当前激光帧的局部地图结果；

步骤S120：对所述当前激光帧的位姿变换、语义理解结果、局部地图结果进行融合处理，获得当前含有语义信息的局部点云地图；

步骤S130：对所述局部点云地图与之前获取的所有局部点云地图进行融合处理，构建含有语义信息的全局初始点云地图和初始物体数据库；所述初始物体数据库包含初始状态下的环境各物体情况与属性，各物体情况与属性包括：物体点云，物体3D包围框，物体位姿，物体类别，物体状态概率分布；其中，物体状态概率分布用于表示物体的几何变化程度和固定性似然概率；

所述三维地图维护过程包括：

步骤S210：获取移动机器人的传感器数据，对所述传感器数据进行预处理操作，获得当前定位信息、图像帧语义理解结果和激光帧局部地图；

步骤S220：对所述当前定位信息、图像帧语义理解结果、激光帧局部地图进行融合处理，获得当前帧含有语义信息的局部点云地图和物体观测集合；所述物体观测集合包含当前帧观测到的各物体情况与属性，具体包括：物体点云、物体3D包围框、物体位姿以及物体类别；

步骤S230：对所述物体观测集合和当前物体数据库进行数据关联，根据所述数据关联结果对当前物体数据库中的物体属性进行更新，包括：①将未成功关联上当前物体数据库的观测物体标记为新物体；②将成功关联上当前物体数据库的观测物体存储起来，并将对应的物体数据库内的物体标记为待更新物体；③将未被当前物体观测集合关联上的物体数据库中的物体标记为未成功观测物体；

步骤S240：根据所述物体数据库更新情况对室内移动机器人三维地图进行更新维护，获得更新后的全局点云地图和物体数据库，具体为：

1)首先对待更新物体

和对应观测物体

进行几何变化程度估计；如果所述几何变化程度估计值小于设定阈值θ_change，不更新数据库中该物体的固定性似然概率；如果反之，将其固定性似然概率更新为低于一个设定值；

2)对于未成功观测物体，将固定性似然概率更新为低于设定值；

3)然后根据所述当前数据库中各物体更新后的固定性似然概率分布情况，将其与设定阈值θ_p比较；如果所述固定性似然概率大于相应阈值θ_p，则认为所述物体未发生几何变化，当前数据库中对应的物体属性保持不变；如果所述固定性似然概率小于相应阈值θ_p，则认为所述物体发生了较大几何变化，清除当前数据库中对应的物体，若所述物体标记为待更新物体，则将对应的观测物体存入物体数据库，并赋予初始默认属性，若所述物体标记为未成功观测物体，则直接清除；此外，将所有标记为新物体的观测物体存入物体数据库，并赋予初始默认属性；

4)根据上述处理获得更新后的物体数据库，并根据所述新物体数据库更新全局点云地图，获得更新后的新全局点云地图，用以指导后续的机器人任务。

2.根据权利要求1所述的一种室内移动机器人三维地图构建及维护方法，其特征在于，所述步骤S230中，对所述物体观测集合和当前物体数据库进行数据关联的方法包括：

对所有小于设定距离阈值的所述物体观测集合中的物体

和当前物体数据库中的物体

分别进行最近邻迭代匹配，获取对应物体对的相对位姿变换关系T_i,j和几何不相似度δ_i,j；其中，相对位姿变换关系T_i,j由对应物体对的位置变换p_i,j和朝向角变换φ_i,j组成；几何不相似度δ_i,j表示经过最近邻迭代匹配后对应物体对的未成功匹配点云点数量比例；

然后，根据每一个物体对之间的相对位姿变换关系和语义一致性构建所述物体对的代价值C(i,j)，并基于所有可用物体对之间的代价值构建所述物体观测集合和当前物体数据库之间的代价矩阵C；其中，

C(i,j)＝λ₁||p_i,j||+λ₂|φ_i,j|+λ₃(1-Π(c_i＝c_j))

其中，λ₁、λ₂、λ₃为比例系数，m,n分别表示物体观测集合中的物体数量和物体数据库中的物体数量；Π(c_i＝c_j)为比较运算函数，用于表示物体对的语义一致性，即当物体对的语义属性c_i与c_j相同时，Π(c_i＝c_j)＝1，当c_i与c_j不相同时，Π(c_i＝c_j)＝0；

如果某物体对的几何不相似度超过设定值或代价值超出设定的截断阈值，将其代价值设置为无穷大；接着，使用匈牙利算法对所述代价矩阵求解，获得物体观测集合和当前物体数据库之间的最优数据关联，获取物体观测集合和当前物体数据库中所有的最优关联物体对。

3.根据权利要求2所述的一种室内移动机器人三维地图构建及维护方法，其特征在于，所述几何变化程度表示为：Δ_i,j＝β₁||p_i,j||+β₂|φ_i,j|+β₃δ_i,j，其中，β₁、β₂、β₃为比例系数。

4.根据权利要求1所述的一种室内移动机器人三维地图构建及维护方法，其特征在于，所述步骤S110中，将惯性测量单元数据的时间戳与激光点云数据的时间戳进行同步与对齐，对上一帧激光数据帧与当前激光数据帧之间的惯性测量单元数据进行预积分，获得惯性测量单元的位姿变化，并根据惯性测量单元与激光雷达之间的外参关系获得当前激光帧的位姿变换。

5.根据权利要求1所述的一种室内移动机器人三维地图构建及维护方法，其特征在于，所述步骤S110中，将图像数据的时间戳与激光点云数据的时间戳进行同步与对齐，对与当前激光点云数据时间戳最相近的图像数据进行语义分割，获取所述图像数据中各个像素的语义类别信息。

6.根据权利要求1所述的一种室内移动机器人三维地图构建及维护方法，其特征在于，所述步骤S110中，对所述激光点云数据进行预处理操作的过程包括：对所述激光点云数据进行畸变校正，通过对所述激光点云数据的每一扫描帧进行线性插值，获取对应扫描时刻的惯性测量单元位姿，并根据惯性测量单元与激光雷达之间的外参关系获得对应扫描时刻激光点云的位姿变换，实现点云畸变校正，得到当前激光帧的局部地图结果。

7.根据权利要求1所述的一种室内移动机器人三维地图构建及维护方法，其特征在于，所述步骤S120中，对所述当前激光帧位姿变换、当前图像帧语义理解结果、当前激光帧局部地图进行融合处理的过程包括：根据当前图像帧与当前激光帧的外参关系，将当前图像帧语义理解结果投影至当前激光帧局部地图中，对当前激光帧局部地图中所有点云数据赋予语义属性，得到含有语义信息的局部点云地图。

8.根据权利要求1所述的一种室内移动机器人三维地图构建及维护方法，其特征在于，对所述局部点云地图和之前获取的所有局部点云地图进行融合处理的过程包括：根据所述局部点云地图、之前获取的所有局部点云地图及其对应的位姿变换关系生成全局点云地图，并对所述全局点云地图进行降采样处理，获得全局初始点云地图，所述全局初始点云地图包含初始状态下的环境点云几何信息和语义信息；然后对所述全局初始点云地图进行地面分割和聚类，提取点云数据中的地面和物体情况，并构建物体初始数据库。

9.根据权利要求1所述的一种室内移动机器人三维地图构建及维护方法，其特征在于，所述物体3D包围框为物体点云的3D包络框，其Z轴方向垂直于地面向上，X轴方向和Y轴方向与物体在水平面上的长轴和短轴一致；物体位姿由物体点云的形心位置与物体3D包围框的姿态组成；物体类别表示物体的语义属性。

10.根据权利要求1所述的一种室内移动机器人三维地图构建及维护方法，其特征在于，所述步骤S220的具体方法包括：在获得所述当前定位数据、当前图像帧语义理解结果、当前激光帧局部地图之后，通过标定得到的当前图像帧与当前激光帧的外参关系，将当前图像帧中每一个像素的语义理解结果投影至当前激光帧局部地图中，对当前激光帧局部地图中所有点云数据赋予对应的语义属性，此时得到当前时刻含有语义信息的局部点云地图；然后对所述含有语义信息的局部点云地图进行地面分割，提取出点云数据中的地面点云部分和非地面点云部分，并对所述去除地面点云部分后的全局点云地图进行带有语义信息约束的欧式聚类，提取出所述全局点云地图中的所有物体情况，结合所述物体情况和当前定位数据构建当前帧物体观测集合。

Images