基于SLAM方法的环境重建方法、系统与移动机器人与流程

基于slam方法的环境重建方法、系统与移动机器人
技术领域
1.本发明涉及移动机器人技术领域，具体涉及一种环境重建方法、系统与移动机器人。


背景技术：

2.目前的移动机器人，例如扫地机，无论是基于陀螺仪、摄像头还是单线激光的扫地机，都缺乏对真实地面环境的感知能力，或者说信息量很小，仅可获得对特定高度空间结构的离散二维采样信息或者扫地机顶部空间稀疏的特征点云信息。并且，难以在地图中重建地面上高度在10cm以下环境的几何结构和语义信息，导致目前的扫地机始终无法解决低矮障碍物(拖鞋、线缆、小玩具、宠物粪便等)的感知和碰撞问题。


技术实现要素：

3.针对上述现有技术的缺陷，本发明的目的在于提供一种基于slam(simultaneous localization and mapping，同时定位与地图构建)方法的环境重建方法、系统与移动机器人，以解决上述缺陷或至少部分解决上述缺陷。
4.依据本发明的一个方面，提供了一种基于同时定位与地图构建方法的环境重建方法，包括：利用多个数据源基于同时定位与地图构建方法进行融合以构建一3d语义地图并存储于一地图数据库中；以及根据深度测量和语义信息更新所述3d语义地图。
5.在本发明的一实施例中，所述多个数据源包括惯性测量单元数据、车轮编码器数据、2d点云数据、深度图数据以及ir图数据。
6.在本发明的一实施例中，所述融合包括：根据一移动机器人的运动约束建立运动方程，获取所述移动机器人的当前时刻状态量的先验信息；根据所述移动机器人上的多个传感器的特性建立观测方程，获取所述多个传感器的测量信息；以及融合所述先验信息和所述测量信息，获得融合后的定位信息。
7.在本发明的一实施例中，所述更新包括：利用深度相机采集的深度图数据投影得到3d点云，并结合所述融合后的定位信息更新所述3d语义地图的地图体素内的概率信息。
8.在本发明的一实施例中，所述环境重建方法还包括：利用所述深度相机采集的ir图数据，通过置信直方图方法分析并剔除所述3d点云中的点云噪点。
9.在本发明的一实施例中，所述环境重建方法还包括：对所述3d点云进行降采样处理。
10.在本发明的一实施例中，所述环境重建方法还包括：利用所述深度相机采集的ir图数据，对物体进行语义分割和边缘提取以获得所述物体的语义信息和边缘信息，并结合所述语义信息和所述边缘信息更新所述3d语义地图。
11.在本发明的一实施例中，结合所述语义信息和所述边缘信息更新所述3d语义地图包括：根据所述语义信息和所述边缘信息进行贝叶斯推断，获得推断结果；以及根据所述推断结果对所述3d点云的地图点进行重投影，获得重投影后的3d点云。
12.在本发明的一实施例中，所述物体为透光物体、反光物体及/或吸光物体。
13.在本发明的一实施例中，所述运动方程的状态量包括位置、姿态、线速度、角速度、加速度中的至少一者。
14.在本发明的一实施例中，所述传感器包括激光雷达、深度相机、惯性测量单元和车轮编码器。
15.在本发明的一实施例中，所述融合是采用贝叶斯递归估计算法对所述先验信息和测量信息进行融合。
16.在本发明的一实施例中，在进行融合时，是将所述惯性测量单元和所述车轮编码器所测量得到的快变数据的融合结果作为所述激光雷达和所述深度相机所测量得到的慢变数据的初值，再与一配准结果一起进行贝叶斯推断。
17.在本发明的一实施例中，在进行融合时，还包括：对姿态进行位姿优化。
18.依据本发明的另一方面，本发明另提供一种基于同时定位与地图构建方法的环境重建系统，包括：地图数据库，用于存储数据，其中包括多个数据源；处理器，所述处理器被配置为用于执行：利用所述多个数据源基于同时定位与地图构建方法构建一3d语义地图并存储于所述地图数据库中；以及根据深度测量和语义信息更新所述3d语义地图。
19.在本发明的另一实施例中，所述多个数据源包括惯性测量单元数据、车轮编码器数据、2d点云数据、深度图数据以及ir图数据。
20.在本发明的另一实施例中，所述处理器包括：融合单元，用于融合先验信息和测量信息，获得融合后的定位信息，其中，所述先验信息是根据一移动机器人的运动约束建立运动方程所获取的所述移动机器人的当前时刻状态量的先验信息，所述测量信息是根据所述移动机器人上的多个传感器的特性建立观测方程所获取的所述多个传感器的测量信息；以及更新单元，用于利用深度相机采集的深度图数据投影得到3d点云，并结合所述融合后的定位信息更新所述3d语义地图的地图体素内的概率信息。
21.在本发明的另一实施例中，所述处理器还包括：置信直方图处理单元，用于利用所述深度相机采集的ir图数据，通过置信直方图方法分析并剔除所述3d点云中的点云噪点。
22.在本发明的另一实施例中，所述处理器还包括：降采样处理单元，用于对所述3d点云进行降采样处理。
23.在本发明的另一实施例中，所述处理器还包括：语义分割单元，用于利用所述深度相机采集的ir图数据，对物体进行语义分割以获得所述物体的语义信息；边缘提取单元，用于利用所述深度相机采集的ir图数据，对物体进行边缘提取以获得所述物体的边缘信息；其中，所述处理器是结合所述语义信息和所述边缘信息更新所述3d语义地图。
24.在本发明的另一实施例中，所述处理器还包括：贝叶斯推断单元，用于根据所述语义信息和所述边缘信息进行贝叶斯推断，获得推断结果；以及地图点重投影单元，用于根据所述推断结果对所述3d点云的地图点进行重投影，获得重投影后的3d点云。
25.在本发明的另一实施例中，所述物体为透光物体、反光物体及/或吸光物体。
26.在本发明的另一实施例中，所述运动方程的状态量包括位置、姿态、线速度、角速度、加速度中的至少一者。
27.在本发明的另一实施例中，所述传感器包括激光雷达、深度相机、惯性测量单元和车轮编码器。
28.在本发明的另一实施例中，所述融合单元是采用贝叶斯递归估计算法对所述先验信息和测量信息进行融合。
29.在本发明的另一实施例中，所述融合单元在进行融合时，是将所述惯性测量单元和所述车轮编码器所测量得到的快变数据的融合结果作为所述激光雷达和所述深度相机所测量得到的慢变数据的初值，再与一配准结果一起进行贝叶斯推断。
30.在本发明的另一实施例中，所述处理器还包括：位姿优化单元，用于在进行融合时对姿态进行位姿优化。
31.为了实现上述目的，本发明又提供一种移动机器人，包括：如上所述的基于同时定位与地图构建方法的环境重建系统。
32.在本发明的又一实施例中，所述移动机器人为扫地机。
33.本发明通过利用多个数据源融合，可更精准的构建环境的3d语义地图。并且，本发明通过根据深度测量和语义信息更新3d语义地图，可实现高精环境地图的重建，可让移动机器人真正看见家居环境，从而彻底解决现有扫地机低矮障碍不可见，不能躲避宠物粪便、拖鞋衣袜、线缆等问题。
34.本发明还可利用深度相机采集的ir图数据，通过置信直方图方法分析并剔除置信度低的点云噪点。同时，对于无法测量的透光、反光、吸光物体，可通过结合物体的语义信息和边缘信息来更新3d语义地图，从而可实现全局一致的高精度环境地图重建。
35.上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其它目的、特征和优点能够更明显易懂，以下特举本发明的具体实施方式。
附图说明
36.通过阅读下文优选实施方式的详细描述，本发明各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出优选实施方式的目的，而并不认为是对本发明的限制。而且在整个附图中，用相同的参考符号表示相同的部件。在附图中：
37.图1示出了本发明的基于同时定位与地图构建方法的环境重建系统的示意图；
38.图2示出了本发明的基于同时定位与地图构建方法的环境重建方法的示意图；
39.图3示出了本发明的移动机器人的结构示意图。
具体实施方式
40.下面将参照附图更详细地描述本公开的示例性实施例。虽然附图中显示了本公开的示例性实施例，然而应当理解，可以以各种形式实现本公开而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本公开，并且能够将本公开的范围完整的传达给本领域的技术人员。
41.需要说明的，本说明书中针对“一个实施例”、“实施例”、“示例实施例”等的引用，指的是描述的该实施例可包括特定的特征、结构或特性，但是不是每个实施例必须包含这些特定特征、结构或特性。此外，这样的表述并非指的是同一个实施例。进一步，在结合实施例描述特定的特征、结构或特性时，不管有没有明确的描述，已经表明将这样的特征、结构或特性结合到其它实施例中是在本领域技术人员的知识范围内的。
42.此外，在说明书及后续的权利要求当中使用了某些词汇来指称特定组件或部件，所属领域中具有通常知识者应可理解，制造商可以用不同的名词或术语来称呼同一个组件或部件。本说明书及后续的权利要求并不以名称的差异来作为区分组件或部件的方式，而是以组件或部件在功能上的差异来作为区分的准则。在通篇说明书及后续的权利要求书中所提及的“包括”和“包含”为一开放式的用语，故应解释成“包含但不限定于”。此外，“连接”一词在此是包含任何直接及间接的连接手段。间接的连接手段包括通过其它装置进行连接。
43.如图1所示，本发明的基于同时定位与地图构建(slam)方法的环境重建系统包括有地图数据库100以及处理器200。其中，所述地图数据库100是用于存储数据，其中包括有多个数据源，例如包括但不限于惯性测量单元数据11、车轮编码器数据12、2d点云数据13、深度图数据14以及ir图数据15。所述处理器200被配置为用于执行：利用所述多个数据源基于同时定位与地图构建方法构建一3d语义地图并存储于所述地图数据库100中；以及根据深度测量和语义信息更新所述3d语义地图。
44.更具体地，所述处理器200例如可包括融合单元21以及更新单元22。其中，所述融合单元21可用于融合先验信息和测量信息，获得融合后的定位信息。所述先验信息是根据一移动机器人的运动约束建立运动方程所获取的所述移动机器人的当前时刻状态量的先验信息，所述状态量可包括位置、姿态、线速度、角速度、加速度中的至少一者。所述测量信息是根据所述移动机器人上的多个传感器的特性建立观测方程所获取的所述多个传感器的测量信息。所述传感器例如可包括但不限于激光雷达、深度相机、惯性测量单元(inertial measurement unit，imu)和车轮编码器等。其中，所述激光雷达可为单线激光雷达，通过其可获得2d点云数据13。所述深度相机例如可为结构光、tof、双目视觉等可感知环境深度的视觉图像获取装置，例如可为3d深度相机，通过其可获得深度图数据14、ir数据15等。所述imu例如可包括陀螺仪和加速度计，通过其可获得imu数据11，例如角速度、加速度等数据。通过所述车轮编码器可获得车轮编码器数据12，例如轮速的数据。较佳地，所述融合单元21是采用贝叶斯递归估计算法对所述先验信息和测量信息进行融合。并且，所述融合单元21在进行融合时，是将所述惯性测量单元和所述车轮编码器所测量得到的快变数据的融合结果先作为所述激光雷达和所述深度相机所测量得到的慢变数据的初值，再与一配准结果一起进行贝叶斯推断。
45.所述更新单元22可用于利用深度相机采集的深度图数据14投影得到3d点云，并结合所述融合后的定位信息更新所述3d语义地图的地图体素(voxel)内的概率信息。
46.在本发明中，所述处理器200还可包括置信直方图处理单元23，可用于利用深度相机采集的ir图数据15，通过置信直方图方法分析并剔除所述3d点云中置信度低的点云噪点。
47.在本发明中，所述处理器200还可包括降采样处理单元24，可用于对所述3d点云进行降采样处理。
48.在本发明中，所述处理器200还可包括语义分割单元25以及边缘提取单元26。其中，所述语义分割单元25可用于利用所述深度相机采集的ir图数据15，对物体进行语义分割以获得所述物体的语义信息。所述边缘提取单元26可用于利用所述深度相机采集的ir图数据15，对物体进行边缘提取以获得所述物体的边缘信息。其中，所述处理器是结合所述语
义信息和所述边缘信息更新所述3d语义地图。并且，所述物体可为透光物体、反光物体及/或吸光物体。
49.在本发明中，所述处理器200还可包括贝叶斯推断单元27以及地图点(map point)重投影单元28。所述贝叶斯推断单元27可用于根据所述语义信息和所述边缘信息进行贝叶斯推断，以获得推断结果。所述地图点重投影单元29用于根据所述推断结果对所述3d点云的地图点进行重投影，获得重投影后的3d点云。
50.在本发明中，所述处理器200还可包括位姿优化单元29，可用于在进行融合时对姿态进行位姿优化。
51.如图2所示，本发明的基于同时定位与地图构建方法的环境重建方法包括：
52.步骤s1，利用多个数据源基于同时定位与地图构建方法进行融合以构建一3d语义地图并存储于一地图数据库中。其中，所述多个数据源例如可包括但不限于惯性测量单元数据、车轮编码器数据、2d点云数据、深度图数据以及ir图数据。
53.步骤s2，根据深度测量和语义信息更新所述3d语义地图。
54.在本发明中，所述融合例如可包括：根据一移动机器人的运动约束建立运动方程，获取所述移动机器人的当前时刻状态量的先验信息，其中所述运动方程的状态量例如可包括位置、姿态、线速度、角速度、加速度中的至少一者。根据所述移动机器人上的多个传感器的特性建立观测方程，获取所述多个传感器的测量信息。以及，融合所述先验信息和所述测量信息，获得融合后的定位信息，例如可采用贝叶斯递归估计算法进行融合。优选地，在进行融合时，可将所述惯性测量单元和所述车轮编码器所测量得到的快变数据的融合结果先作为所述激光雷达和所述深度相机所测量得到的慢变数据的初值，再与一配准结果一起进行贝叶斯推断。并且，在进行融合时，还可对姿态进行位姿优化。
55.在本发明中，所述更新例如可包括：利用深度相机采集的深度图数据投影得到3d点云，并结合所述融合后的定位信息更新所述3d语义地图的地图体素内的概率信息。
56.在本发明中，所述环境重建方法还可包括：利用所述深度相机采集的ir图数据，通过置信直方图方法分析并剔除所述3d点云中的点云噪点。
57.在本发明中，所述环境重建方法还可包括：对所述3d点云进行降采样处理。
58.在本发明中，所述环境重建方法还可包括：利用所述深度相机采集的ir图数据，对物体进行语义分割和边缘提取以获得所述物体的语义信息和边缘信息，并结合所述语义信息和所述边缘信息更新所述3d语义地图。例如，可根据所述语义信息和所述边缘信息进行贝叶斯推断，获得推断结果；以及根据所述推断结果对所述3d点云的地图点进行重投影，获得重投影后的3d点云。
59.如图3所示，本发明的基于同时定位与地图构建方法环境重建系统可应用于一移动机器人300上，所述移动机器人300例如可为扫地机。
60.通过单线激光雷达所获得的定位数据可视为绝对定位数据，通过轮速计和陀螺仪等所产生的定位数据可视为相对定位数据，如果3d深度相机在移动过程中仅靠轮速计和陀螺仪等所产生的相对定位数据，会因为相对定位数据的累计误差导致地图拼接。而本发明通过结合单线激光雷达量程远精度高和3d深度相机数据量大、信息丰富的特性，可同时获得环境及障碍物的深度、轮廓、ir等图像信息，实现高精环境地图建模。本发明通过将imu数据、车轮编码器数据、2d点云数据、深度图数据及ir图数据等多个数据源进行有效融合，可
获得高精度的定位信息，从而可更精准的构建环境的3d语义地图。并且，本发明通过根据深度测量和语义信息更新3d语义地图，可实现高精环境地图的重建。如此，本发明可让机器人真正看见家居环境，从而彻底解决现有扫地机低矮障碍不可见，不能躲避宠物粪便、拖鞋衣袜、线缆等问题。
61.此外，由于家庭环境中普遍存在光学传感器难以测量的透光、反光、吸光物体，导致深度相机“测不准、测不到”，降低了定位精度和3d语义地图的一致性，因此，本发明利用深度相机采集的ir图数据，通过置信直方图方法分析并剔除置信度低的点云噪点，同时，对于无法测量的透光物体、反光物体、吸光物体，可通过结合物体的语义信息和边缘信息来更新3d语义地图，从而可实现全局一致的高精度环境地图重建。
62.本发明公开了a1、一种基于同时定位与地图构建方法的环境重建方法，包括：
63.利用多个数据源基于同时定位与地图构建方法进行融合以构建一3d语义地图并存储于一地图数据库中；以及
64.根据深度测量和语义信息更新所述3d语义地图。
65.a2、根据a1所述的环境重建方法，其中，所述多个数据源包括惯性测量单元数据、车轮编码器数据、2d点云数据、深度图数据以及ir图数据。
66.a3、根据a2所述的环境重建方法，其中，所述融合包括：
67.根据一移动机器人的运动约束建立运动方程，获取所述移动机器人的当前时刻状态量的先验信息；
68.根据所述移动机器人上的多个传感器的特性建立观测方程，获取所述多个传感器的测量信息；以及
69.融合所述先验信息和所述测量信息，获得融合后的定位信息。
70.a4、根据a3所述的环境重建方法，其中，所述更新包括：
71.利用深度相机采集的深度图数据投影得到3d点云，并结合所述融合后的定位信息更新所述3d语义地图的地图体素内的概率信息。
72.a5、根据a4所述的环境重建方法，其中，所述环境重建方法还包括：
73.利用所述深度相机采集的ir图数据，通过置信直方图方法分析并剔除所述3d点云中的点云噪点。
74.a6、根据a5所述的环境重建方法，其中，所述环境重建方法还包括：
75.对所述3d点云进行降采样处理。
76.a7、根据a6所述的环境重建方法，其中，所述环境重建方法还包括：
77.利用所述深度相机采集的ir图数据，对物体进行语义分割和边缘提取以获得所述物体的语义信息和边缘信息，并结合所述语义信息和所述边缘信息更新所述3d语义地图。
78.a8、根据a7所述的环境重建方法，其中，结合所述语义信息和所述边缘信息更新所述3d语义地图包括：
79.根据所述语义信息和所述边缘信息进行贝叶斯推断，获得推断结果；以及
80.根据所述推断结果对所述3d点云的地图点进行重投影，获得重投影后的3d点云。
81.a9、根据a8所述的环境重建方法，其中，所述物体为反光物体及/或吸光物体。
82.a10、根据a3～a9任一所述的环境重建方法，其中，所述运动方程的状态量包括位置、姿态、线速度、角速度、加速度中的至少一者。
83.a11、根据a10所述的环境重建方法，其中，所述传感器包括激光雷达、深度相机、惯性测量单元和车轮编码器。
84.a12、根据a11所述的环境重建方法，其中，所述融合是采用贝叶斯递归估计算法对所述先验信息和测量信息进行融合。
85.a13、根据a12所述的环境重建方法，其中，在进行融合时，是将所述惯性测量单元和所述车轮编码器所测量得到的快变数据的融合结果作为所述激光雷达和所述深度相机所测量得到的慢变数据的初值，再与一配准结果一起进行贝叶斯推断。
86.a14、根据a13所述的环境重建方法，其中，在进行融合时，还包括：
87.对姿态进行位姿优化。
88.a15、一种基于同时定位与地图构建方法的环境重建系统，包括：
89.地图数据库，用于存储数据，其中包括多个数据源；
90.处理器，所述处理器被配置为用于执行：
91.利用所述多个数据源基于同时定位与地图构建方法构建一3d语义地图并存储于所述地图数据库中；以及
92.根据深度测量和语义信息更新所述3d语义地图。
93.a16、根据a15所述的环境重建系统，其中，所述多个数据源包括惯性测量单元数据、车轮编码器数据、2d点云数据、深度图数据以及ir图数据。
94.a17、根据a16所述的环境重建系统，其中，所述处理器包括：
95.融合单元，用于融合先验信息和测量信息，获得融合后的定位信息，其中，所述先验信息是根据一移动机器人的运动约束建立运动方程所获取的所述移动机器人的当前时刻状态量的先验信息，所述测量信息是根据所述移动机器人上的多个传感器的特性建立观测方程所获取的所述多个传感器的测量信息；以及
96.更新单元，用于利用深度相机采集的深度图数据投影得到3d点云，并结合所述融合后的定位信息更新所述3d语义地图的地图体素内的概率信息。
97.a18、根据a17所述的环境重建系统，其中，所述处理器还包括：
98.置信直方图处理单元，用于利用所述深度相机采集的ir图数据，通过置信直方图方法分析并剔除所述3d点云中的点云噪点。
99.a19、根据a18所述的环境重建系统，其中，所述处理器还包括：
100.降采样处理单元，用于对所述3d点云进行降采样处理。
101.a20、根据a18所述的环境重建系统，其中，所述处理器还包括：
102.语义分割单元，用于利用所述深度相机采集的ir图数据，对物体进行语义分割以获得所述物体的语义信息；
103.边缘提取单元，用于利用所述深度相机采集的ir图数据，对物体进行边缘提取以获得所述物体的边缘信息；
104.其中，所述处理器是结合所述语义信息和所述边缘信息更新所述3d语义地图。
105.a21、根据a20所述的环境重建系统，其中，所述处理器还包括：
106.贝叶斯推断单元，用于根据所述语义信息和所述边缘信息进行贝叶斯推断，获得推断结果；以及
107.地图点重投影单元，用于根据所述推断结果对所述3d点云的地图点进行重投影，
获得重投影后的3d点云。
108.a22、根据a21所述的环境重建方法，其中，所述物体为反光物体及/或吸光物体。
109.a23、根据a17～a22任一所述的环境重建系统，其中，所述运动方程的状态量包括位置、姿态、线速度、角速度、加速度中的至少一者。
110.a24、根据a23所述的环境重建系统，其中，所述传感器包括激光雷达、深度相机、惯性测量单元和车轮编码器。
111.a25、根据a24所述的环境重建系统，其中，所述融合单元是采用贝叶斯递归估计算法对所述先验信息和测量信息进行融合。
112.a26、根据a25所述的环境重建系统，其中，所述融合单元在进行融合时，是将所述惯性测量单元和所述车轮编码器所测量得到的快变数据的融合结果作为所述激光雷达和所述深度相机所测量得到的慢变数据的初值，再与一配准结果一起进行贝叶斯推断。
113.a27、根据a26所述的环境重建系统，其中，所述处理器还包括：
114.位姿优化单元，用于在进行融合时对姿态进行位姿优化。
115.a28、一种移动机器人，包括：
116.如a15～a27任一所述的基于同时定位与地图构建方法的环境重建系统。
117.a29、根据a28所述的移动机器人，其中，所述移动机器人为扫地机。
118.在此处所提供的说明书中，说明了大量具体细节。然而，能够理解，本发明的实施例可以在没有这些具体细节的情况下实践。在一些实例中，并未详细示出公知的方法、结构和技术，以便不模糊对本说明书的理解。
119.类似地，应当理解，为了精简本公开并帮助理解各个发明方面中的一个或多个，在上面对本发明的示例性实施例的描述中，本发明的各个特征有时被一起分组到单个实施例、图、或者对其的描述中。然而，并不应将该公开的方法解释成反映如下意图：即所要求保护的本发明比在每个权利要求中所明确记载的特征更多的特征。更确切地说，如下面的权利要求所反映的那样，发明方面在于少于前面公开的单个实施例的所有特征。因此，遵循具体实施方式的权利要求书由此明确地并入该具体实施方式，其中每个权利要求本身都作为本发明的单独实施例。
120.本领域的技术人员可以理解，可以对实施例中的设备中的模块或单元或组进行自适应性地改变并且把它们设置在与该实施例不同的一个或多个设备中。可以把实施例中的模块或单元或组件组合成一个模块或单元或组件，以及可以把它们分成多个子模块或子单元或子组件。除了这样的特征和/或过程或者单元中的至少一些是相互排斥之外，可以采用任何组合对本说明书(包括伴随的权利要求、摘要和附图)中公开的所有特征以及如此公开的任何方法或者设备的所有过程或单元进行组合。除非另有明确陈述，本说明书(包括伴随的权利要求、摘要和附图)中公开的每个特征可以由提供相同、等同或相似目的的替代特征来代替。
121.当然，本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。