用于机器人的位姿估计和运动规划方法、装置和机器人与流程

1.本技术涉及自主机器人导航领域，更具体地涉及一种用于机器人的位姿估计方法、运动规划方法、位姿估计装置、运动规划装置和机器人。


背景技术：

2.具有自主导航功能的移动机器人(例如，扫地机器人、餐厅服务机器人、无人机等)在工业和生活中的作用越来越重要，其可替代人力、能够显著提高生产力。
3.这类机器人在实际运作中需建立导航地图，并能够精确地到达目标点以执行预定任务，例如，餐厅服务机器人需基于导航地图到达预设餐桌送餐。然后，由于传感器感知误差、移动机器人控制误差、导航地图构建误差，以及，结构类似环境等误差，移动机器人在进行位姿估计时会产生误差，从而造成无法精确地到达目标点，为预定任务的精确执行造成困难。
4.因此，需要一种优化的用于机器人的导航方案。
5.申请内容
6.本技术的一个优势在于提供一种用于机器人的位姿估计方法、运动规划方法、位姿估计装置、运动规划装置和机器人，其中，基于所述位姿估计方法和运动规划方法，所述机器人的应用场景能够得以拓展。例如，基于所述位姿估计方法和运动规划方法，所述机器人能够基于已构建的导航地图，适配长走廊等结构相似的环境，提升到达目标站点的精度。
7.本技术的另一个优势在于提供一种用于机器人的位姿估计方法、运动规划方法、位姿估计装置、运动规划装置和机器人，其中，所述机器人的运动规划方案分为两个阶段：第一个阶段由现有的导航方案驱动至目标站点附近，以及，第二个阶段基于所述机器人的传感设备所采集的目标站点的局部数据对所述目标站点进行二次位姿估计，通过这样的方式，能有效地降低长时累计误差且具有较高的位姿估计精度。也就是，根据本技术实施例的位姿估计方法包括两个阶段：粗位姿估计阶段和精位姿估计阶段。
8.本技术的另一优势在于提供一种用于机器人的位姿估计方法、运动规划方法、位姿估计装置、运动规划装置和机器人，其中，基于更为精确的位姿估计，所述机器人能够更为精确地进行运动规划，以更为精确地到达目标站点从而能够为预定任务的精确执行提供良好的基础。
9.本技术的另一优势在于提供一种用于机器人的位姿估计方法、运动规划方法、位姿估计装置、运动规划装置和机器人，其中，基于更为精确的位姿估计，所述机器人在粗位姿估计阶段的可接受误差更大，以使得所述机器人具有更有的扩展能力和适配能力。
10.为了实现上述至少一个优势，本技术提供了一种用于机器人的位姿估计方法，其包括：
11.通过机器人的图像传感器获取目标空间中各站点的多帧初始彩色图像和与所述初始彩色图像分别对应的多帧初始深度图像；
12.构建用于描述目标空间的导航地图；
13.搜索所述机器人在所述导航地图上自起点至目标站点之间的无碰路径；
14.按所述无碰路径，驱动所述机器人至所述目标站点的附近；
15.通过所述图像传感器获取所述目标站点的多帧局部彩色图像和与所述局部彩色图像分别对应的多帧局部深度图像；
16.基于所述多帧局部彩色图像和所述多帧局部深度图像进行非线性优化，以生成用于描述所述目标站点的局部地图；以及
17.基于所述局部地图进行非线性优化，以确定所述机器人在所述目标站点附近相对于所述目标站点的位姿。
18.在根据本技术的位姿估计方法中，所述导航地图包括用于描述所述目标空间的栅格地图，其中，搜索所述机器人在所述导航地图上自起点至目标站点之间的无碰路径，包括：搜索所述机器人在所述栅格地图上自起点至目标站点之间的无碰路径。
19.在根据本技术的位姿估计方法中，基于所述多帧局部彩色图像和所述多帧局部深度图像进行非线性优化，以生成用于描述所述目标站点的局部地图，包括：基于所述多帧局部彩色图像和所述多帧局部深度图像以所述机器人提供的初始位姿进行非线性优化，以生成用于描述所述目标站点的局部地图。
20.在根据本技术的位姿估计方法中，所述初始位姿基于所述轮式里程计采集的第一位姿数据和所述惯性测量单元获得的第二位姿数据生成。
21.在根据本技术的位姿估计方法中，基于所述多帧局部彩色图像和所述多帧局部深度图像以所述机器人提供的初始位姿进行非线性优化，以生成用于描述所述目标站点的局部地图，包括：基于所述多帧局部彩色图像和所述多帧局部深度图像以所述初始位姿进行稀疏优化和边缘优化，以生成用于描述所述目标站点的局部地图。
22.在根据本技术的位姿估计方法中，稀疏优化的过程，包括：
23.提取多帧所述局部彩色图像的局部orb特征点；
24.以第一帧局部彩色图像作为第一帧关键帧局部彩色图像，并以第一帧局部彩色图像对应的第一帧局部点云作为第一关键帧局部点云；
25.以所述轮式里程计采集的第一位姿数据和所述惯性测量单元获得的第二位姿数据的组合作为初始位姿进行帧间位姿递推，以获得第二帧局部彩色图像的位姿，其中，所述第二帧局部彩色图像的位姿为所述机器人相对于所述目标站点的实时位姿；
26.响应于所述第二帧局部彩色图像的位姿满足预设条件，将所述第二帧局部彩色图像确定为第二帧关键帧局部彩色图像，通过这样的方式，获得预设数量的关键帧局部彩色图像；
27.基于所述关键帧局部彩色图像生成关键帧局部彩色图像词袋，其中，所述关键帧局部彩色图像词袋包括每一所述关键帧局部彩色图像的局部orb特征点的特征描述、每一所述关键帧局部彩色图像的局部orb特征点的三维坐标以及所述关键帧局部彩色图像的位姿；
28.获得与所述关键帧局部彩色图像对应数量的关键帧局部点云；
29.基于所述关键帧局部彩色图像的局部orb特征点和与所述关键帧局部彩色图像对应的局部深度图像提供的所述局部orb特征点的深度值，生成局部特征点地图；以及
30.以第一帧关键帧局部点云初始化生成初始局部栅格地图，并基于第二帧关键帧局
部点云和第二帧关键帧局部彩色图像的位姿通过执行如下所述的损失函数进行配准，以获得优化的所述第二帧关键帧局部彩色图像的位姿并更新初始局部栅格地图，通过这样的方式迭代优化以实现稀疏优化；
31.其中，所述损失函数的表达式为：
32.cost_function＝gridmap_costfunction keypoints_reprojects，其中，gridmap_costfunction表示由关键帧局部点云生成的稠密线性点云与栅格地图的栅格点之间的配准概率；keypoints_reprojects 表示所述局部特征点地图中的点投影到基于局部彩色图像构建的坐标系上与对应所述关键帧局部彩色图像的orb特征点之间的重投影误差。
33.在根据本技术的位姿估计方法中，所述预设条件为如下任意之一:
34.所述第二帧局部彩色图像的位姿的旋转分量大于第一阈值；
35.所述第二帧局部彩色图像的位姿的平移分量大于第二阈值；
36.所述第二帧局部彩色图像与所述第一帧局部彩色图像之间的时间差大于第三阈值。
37.在根据本技术的位姿估计方法中，以所述机器人的轮式里程计采集的位姿数据和所述机器人的惯性测量单元获得的位姿数据的组合作为初始位姿，包括：基于滤波函数融合所述轮式里程计采集的位姿数据和所述惯性测量单元获得的位姿数据，以生成所述初始位姿。
38.在根据本技术的位姿估计方法中，边缘优化的过程，包括：
39.以包含预设帧数关键帧的滑动窗口采用与稀疏优化过程中相一致的损失函数进行配准，以优化所述关键帧局部彩色图像的位姿；
40.去除所述滑动窗口内的首帧关键帧局部彩色图像并在所述滑动窗口的尾帧关键帧局部彩色图像后添加一帧新的关键帧局部彩色图像，并采用与稀疏优化过程中相一致的损失函数进行配准，通过这样的方式，以迭代优化所述关键帧局部彩色图像的位姿；以及
41.以包含所述预设帧数关键帧的滑动窗口基于由关键帧局部点云生成的稠密线性点云采用与稀疏优化过程中相一致的损失函数进行配准，以优化所述局部栅格地图。
42.在根据本技术的位姿估计方法中，所述预设帧数为8帧至12帧。
43.在根据本技术的位姿估计方法中，基于所述局部地图进行非线性优化，以确定所述机器人在所述目标站点附近相对于所述目标站点的位姿，包括：
44.提取所述目标站点对应的所述初始彩色图像的初始orb特征点；
45.以所述初始orb特征点在所述关键帧局部彩色图像词袋中进行查询，以获得与所述初始彩色图像匹配的备选关键帧局部彩色图像；
46.以所述初始彩色图像的初始orb特征点与所述备选关键帧局部彩色图像的局部orb特征点进行pnp匹配，生成所述机器人在所述目标站点附近的初始位姿；以及
47.基于所述初始彩色图像的初始orb特征点与所述初始彩色图像对应的初始点云使用如下损失函数进行迭代优化，以获得所述机器人在所述目标站点附近的迭代位姿，其中，所述迭代位姿为机器人在所述目标站点附近相对于所述目标站点的位姿；
48.其中，所述损失函数的表达式为：
49.cost_function＝gridmap_costfunction keypoints_reprojects，其中，
gridmap_costfunction表示由初始点云生成的稠密线性点云与栅格地图的栅格点之间的配准概率；keypoints_reprojects表示所述局部特征点地图中的点投影到基于所述初始彩色图像构建的坐标系上与对应所述初始彩色图像的初始orb特征点之间的重投影误差。
50.根据本技术的另一方面，还提供一种用于机器人的运动规划方法，其包括：
51.基于如上所述的位姿估计方法，获得所述机器人在所述目标站点附近相对于所述目标站点的位姿；
52.获得所述机器人在所述目标站点附近相对于所述目标站点的位姿与所述机器人的实时位姿之间的位姿差；以及
53.基于所述位姿差，移动所述机器人。
54.在根据本技术的运动规划方法中，所述运动规划方法进一步包括：循环获得所述机器人在所述目标站点附近相对于所述目标站点的位姿、获得所述机器人在所述目标站点附近相对于所述目标站点的位姿与所述机器人的实时位姿之间的位姿差和基于所述位姿差，移动所述机器人，直至所述位姿差中平移分量和旋转分量小于预设阈值。
55.根据本技术的又一方面，还提供一种用于机器人的位姿估计装置，其包括：
56.初始图像采集单元，用于通过机器人的图像传感器获取目标空间中各站点的多帧初始彩色图像和与所述初始彩色图像分别对应的多帧初始深度图像；
57.导航地图构建单元，用于构建用于描述目标空间的导航地图；
58.路径搜索单元，用于搜索所述机器人在所述导航地图上自起点至目标站点之间的无碰路径；
59.驱动单元，按所述无碰路径驱动所述机器人至所述目标站点的附近；
60.局部图像采集单元，用于通过所述图像传感器获取所述目标站点的多帧局部彩色图像和与所述局部彩色图像分别对应的多帧局部深度图像；
61.局部地图构建单元，用于基于所述多帧局部彩色图像和所述多帧局部深度图像进行非线性优化，以生成用于描述所述目标站点的局部地图；以及
62.估计单元，用于基于所述局部地图进行非线性优化，以确定所述机器人在所述目标站点附近相对于所述目标站点的位姿。
63.根据本技术的再一方面，还提供一种用于机器人的运动规划装置，其包括：
64.如上所述的位姿估计装置，用于确定所述机器人在所述目标站点附近相对于所述目标站点的位姿；
65.位姿差获取单元，用于获得所述机器人在所述目标站点附近相对于所述目标站点的位姿与所述机器人的实时位姿之间的位姿差；以及
66.移动单元，用于基于所述位姿差，移动所述机器人。
67.根据本技术的又一方面，还提供一种机器人，其包括：
68.机器人主体；
69.安装于所述机器人主体的图像传感器，用于采集目标空间的彩色图像和深度图像；
70.安装于所述机器人主体的惯性测量单元；
71.安装于所述机器人主体的轮式编码器；以及
72.安装于所述机器人主体的控制器，其中，所述控制器用于执行如上所述的位姿估
计方法或如上所述的运动规划方法。
73.在根据本技术的机器人中，所述图像传感器包括：tof摄像单元，用于采集目标空间的深度图像；以及，rgb摄像单元，用于采集目标空间的彩色图像。
74.通过对随后的描述和附图的理解，本技术进一步的目的和优势将得以充分体现。
75.本技术的这些和其它目的、特点和优势，通过下述的详细说明，附图和权利要求得以充分体现。
附图说明
76.从下面结合附图对本技术实施例的详细描述中，本技术的这些和/或其它方面和优点将变得更加清楚并更容易理解，其中：
77.图1图示了根据本技术实施例的机器人的架构示意图。
78.图2图示了根据本技术实施例的机器人为扫地机器人的示意图。
79.图3图示了根据本技术实施例的用于机器人的位姿估计方法的流程图。
80.图4图示了根据本技术实施例的所述位姿估计方法中稀疏优化的示意图。
81.图5图示了根据本技术实施例的所述位姿估计方法中边缘优化的流程图。
82.图6图示了根据本技术实施例的运动规划方法的流程图。
83.图7图示了根据本技术实施例的用于机器人的位姿估计装置的框图示意图。
84.图8图示了根据本技术实施例的用于机器人的位姿估计装置中局部地图构建单元的框图。
85.图9图示了根据本技术实施例的用于机器人的位姿估计装置中位姿估计单元的框图。
86.图10图示了根据本技术实施例的用于机器人的运动规划装置的框图示意图。
具体实施方式
87.以下说明书和权利要求中使用的术语和词不限于字面的含义，而是仅由本技术人使用以使得能够清楚和一致地理解本技术。因此，对本领域技术人员很明显仅为了说明的目的而不是为了如所附权利要求和它们的等效物所定义的限制本技术的目的而提供本技术的各种实施例的以下描述。
88.可以理解的是，术语“一”应理解为“至少一”或“一个或多个”，即在一个实施例中，一个元件的数量可以为一个，而在另外的实施例中，该元件的数量可以为多个，术语“一”不能理解为对数量的限制。
89.虽然比如“第一”、“第二”等的序数将用于描述各种组件，但是在这里不限制那些组件。该术语仅用于区分一个组件与另一组件。例如，第一组件可以被称为第二组件，且同样地，第二组件也可以被称为第一组件，而不脱离发明构思的教导。在此使用的术语“和 /或”包括一个或多个关联的列出的项目的任何和全部组合。
90.在这里使用的术语仅用于描述各种实施例的目的且不意在限制。如在此使用的，单数形式意在也包括复数形式，除非上下文清楚地指示例外。另外将理解术语“包括”和/或“具有”当在该说明书中使用时指定所述的特征、数目、步骤、操作、组件、元件或其组合的存在，而不排除一个或多个其它特征、数目、步骤、操作、组件、元件或其组的存在或者附加。
91.示意性用于机器人的位姿估计方法
92.根据本技术实施例的所述机器人基于其配置的传感设备所采集的数据并基于特定的位姿估计算法能够获得所述机器人相对于目标点的更为精确的位姿估计。
93.图1图示了根据本技术实施例的所述机器人的架构示意图。如图 1所示，所述机器人10包括机器人主体11，安装于所述机器人主体 11的图像传感器12、安装于所述机器人主体11的惯性测量单元13、安装于所述机器人主体11的轮式编码器14、以及，安装于所述机器人主体11的控制器15、以及，激光阵列16，其中，所述图像传感器12用于采集目标空间的彩色图像和深度图像；所述惯性测量单元13和所述轮式编码器14用于测量所述机器人10的位姿；所述控制器15用于执行相应程序以实现位姿估计等功能，所述激光测距仪用于测量深度信息。这里，基于所述惯性测量单元13和所述轮式编码器14所测的所述机器人10的位姿的精度较低，但其能够作为额外的信息增益来提高所述机器人10的位姿估计的精度。
94.也就是，在本技术实施例中，以所述机器人为轮式机器人为示例，应可以理解，当所述机器人被实施为其他类型的机器人时，其还能够配置其他类型的用于定位的传感器，例如，gps模块、蓝牙模块、蜂窝通信模块、超宽带(uwb)通信模块等，对此，并不为本技术所局限。
95.在本技术实施例中，所述机器人10的图像传感器12能够采集目标空间的彩色图像和深度图像。特别地，在本技术实施例中，所述图像传感设备为实施为rgbd相机，其包括深度摄像单元121和rgb 摄像单元122，所述深度摄像单元121用于采集目标对象的深度图像，所述rgb摄像单元122用于采集目标对象的彩色图像。更特别地，在本技术实施例中，所述深度摄像单元121被实施为tof摄像单元，其基于时间飞行法则采集目标对象的深度图像，所述rgb摄像单元122为全通相机，其支持暗光和无光环境的拍摄，并且，所述 rgbd相机的tof摄像单元和所述rgb摄像单元122之间已配准。
96.在具体实施例中，所述机器人10(轮式机器人)可被实施为扫地机器人(如图2所示)、送餐机器人、酒店机器人等，应可以理解，所述机器人10的具体类型并不为本技术所局限。
97.图3图示了根据本技术实施例的用于机器人10的位姿估计方法的流程图。如图3所示，根据本技术实施例的所述用于机器人10的位姿估计方法，包括步骤：s110，通过机器人10的图像传感器12 获取目标空间中各站点的多帧初始彩色图像和与所述初始彩色图像分别对应的多帧初始深度图像；s120，构建用于描述目标空间的导航地图；s130，搜索所述机器人10在所述栅格地图上自起点至目标站点之间的无碰路径；s140，按所述无碰路径，驱动所述机器人10至所述目标站点的附近；s150，通过所述图像传感器12获取所述目标站点的多帧局部彩色图像和与所述局部彩色图像分别对应的多帧局部深度图像；s160，基于所述多帧局部彩色图像和所述多帧局部深度图像进行非线性优化，以生成用于描述所述目标站点的局部地图，以生成用于描述所述目标站点的局部地图；以及,s170，基于所述局部地图进行非线性优化，以确定所述机器人10在所述目标站点附近相对于所述目标站点的位姿。
98.应可以理解，在步骤s110至步骤s140，所述机器人10通过slam 算法(simultaneous localization and mapping,同时位姿估计和地图构建)以构建用于描述目标空间的导航地图，进而，基于所述导航地图，规划所述机器人10的运动路线以驱动所述机器人10到达目标站点。
99.更具体地，在步骤s110中，通过机器人10的图像传感器12获取目标空间中各站点的多帧初始彩色图像和与所述初始彩色图像分别对应的多帧初始深度图像。如前所述，在本技术实施例中，所述图像传感器12包括：tof摄像单元，用于采集目标空间的深度图像；以及，rgb摄像单元122，用于采集目标空间的彩色图像。
100.特别地，在本技术实施例中，在移动所述机器人10采集所述目标空间的图像的过程中，在所述目标空间中设置目标站点列(例如， obj_1,obj_2,
……
),同时，采集各站点处的初始彩色图像和与所述初始彩色图像对应的初始深度图像(例如，obj_1的初始彩色图像被设定为obj_1_rgb,obj_1的初始深度图像被设定为obj_1_depth)。
101.进一步地，在步骤s110中，可进一步地提取所述初始彩色图像中的初始orb特征点，这里，orb特征点包括图像的特征点和特征点的描述子，其中，所述描述子用于描述所述特征点周围的特征。在具体实施例中，orb特征点可采用fast(features from acceleratedsegment test)算法来检测特征点，fast核心思想就是找出那些鹤立鸡群的点，即拿一个点跟它周围的点比较，如果它和其中大部分的点都不一样就可以认为它是一个特征点。为了便于描述将初始彩色图像的初始orb特征点的集合表示为obj_i_orb，例如，第一目标站点的初始orb特征点的集合可表示为obj_1_orb。
102.并且，可基于所述初始深度图像生成初始点云。更具体地，该过程包括：首先基于所述初始深度图像，生成基于所述初始彩色图像构建的坐标系的点云；然后旋转所述点云中深度值在预设阈值范围内的点云，以获得所述初始点云。在具体实施中，该预设范围可被设置为 d1＜pt＜d2，通过这样的方式，可对点云进行滤波以提高精度。为了便于表述将初始点云表示为obj_i_pts，例如，第一站点的初始点云可表示为obj_1_pts。
103.在步骤s120中，构建用于描述目标空间的导航地图。此处，可通过任何现有的构图算法来构建用于描述空间的导航地图，例如，通过所述激光阵列16、所述惯性测量单元13、以及，所述轮式编码器 14所测得的数据构建所述导航地图。具体地，所述导航地图包括用于描述所述目标空间的栅格地图和用于描述所述目标空间的特征点云地图。
104.在步骤s130中，搜索所述机器人10在所述导航地图上自起点至目标站点之间的无碰路径。具明确地，搜索所述机器人10在所述栅格地图上自起点至目标站点之间的无碰路径。在本技术实施例中，搜索所述无碰路径的具体算法算法并不为本技术所局限，例如，其可采用a*算法、d*算法，人工势场法、dijkstra算法等，其原因在于在步骤s110-步骤s140的阶段中对于机器人10的位姿估计的精度要求不高，其仅需能够达到目标点附近即可。
105.在步骤s140中，按所述无碰路径，驱动所述机器人10至所述目标站点的附近。也就是，基于导航栅格地图，按照规划的行驶路线，所述机器人10抵达目标站点，例如，设定预设的目标站点为obj_1。
106.纵观步骤s110和步骤s140应可以理解，根据本技术实施例的所述位姿估计方法的第一阶段完成，其中，在第一阶段中，所述位姿估计方法通过现有的导航方案驱动所述机器人10至目标站点附近。步骤s150至步骤s170为所述位姿估计方法的第二阶段，其目的在于获得所述机器人10相对于所述目标站点的精度更高的位姿估计。
107.具体地，在步骤s150中，通过所述图像传感器12获取所述目标站点的多帧局部彩色图像和与所述局部彩色图像分别对应的多帧局部深度图像。也就是，在所述机器人10抵达目标站点附近后，进一步地通过图像传感器12采集目标站点的图像数据。在具体运行中，
控制所述机器人10相对于所述目标站点旋转，并在旋转的过程中，采集目标站点的图像数据。为了便于描述，在本技术实施例中，所述局部彩色图像被设定为rgb_search，所述局部深度图像被设定为 depth_search。
108.在步骤s160中，基于所述多帧局部彩色图像和所述多帧局部深度图像进行非线性优化，以生成用于描述所述目标站点的局部地图。应可以理解，在进行非线性优化的过程中，需先提供初始位姿。特别地，在本技术实施例中，所述初始位姿可由所述机器人10提供，例如，由所述机器人10的轮式里程计采集的第一位姿数据和所述机器人10的惯性测量单元13获得的第二位姿数据提供。当然，应可以理解，当所述机器人10被实施为其他类型的机器人时，所述初始位姿还可以由所述机器人的其他传感器提供，例如，gps模块、蓝牙模块、超宽带模块等，对此，并不为本技术所局限。
109.还值得一提的是，即使所述机器人10没有用于提供初始位姿数据的传感器，所述机器人10依然能够提供所述初始位姿，例如，所述初始位置可被设置为在步骤s110至步骤s140中，所述机器人10 被驱动至目标站点附近的位姿作为初始位姿，也就是，基于导航地图所确定的位姿。当然，通过所述机器人10的其他传感器提供的位姿数据作为初始位姿具有更高的精度，能优化位姿估计结果。
110.相应地，当所述初始位姿由所述机器人10的轮式里程计采集的第一位姿数据和所述机器人10的惯性测量单元13获得的第二位姿数据提供时，基于所述多帧局部彩色图像和所述多帧局部深度图像进行非线性优化，以生成用于描述所述目标站点的局部地图，包括：基于所述多帧局部彩色图像、所述多帧局部深度图像、所述机器人10 的轮式里程计采集的第一位姿数据和所述机器人10的惯性测量单元 13获得的第二位姿数据进行非线性优化，以生成用于描述所述目标站点的局部地图。
111.具体地，在步骤s160中，非线性优化包括两个过程：稀疏优化和边缘优化。也就是，在本技术实施例中，基于所述多帧局部彩色图像、所述多帧局部深度图像、所述机器人10的轮式里程计采集的第一位姿数据和所述机器人10的惯性测量单元13获得的第二位姿数据进行非线性优化，以生成用于描述所述目标站点的局部地图，包括：基于所述多帧局部彩色图像、所述多帧局部深度图像、所述机器人10 的轮式里程计采集的第一位姿数据和所述机器人10的惯性测量单元 13获得的第二位姿数据进行稀疏优化和边缘优化，以生成用于描述所述目标站点的局部地图。
112.具体来说，稀疏优化的过程，包括：
113.首先，提取多帧所述局部彩色图像的局部orb特征点；
114.然后，以第一帧局部彩色图像作为第一帧关键帧局部彩色图像，并以第一帧局部彩色图像对应的第一帧局部点云作为第一关键帧局部点云；这里，为了便于描述将第一帧关键帧局部彩色图像设定为k_rgb_0，第一关键帧局部点云被设定为k_pts_0。
115.接着，以所述轮式里程计采集的第一位姿数据和所述惯性测量单元13获得的第二位姿数据的组合作为初始位姿进行帧间位姿递推，以获得第二帧局部彩色图像的位姿，其中，所述第二帧局部彩色图像的位姿为所述机器人10相对于所述目标站点的实时位姿,审定为 t_3；
116.特别地，在该步骤中，以所述机器人10的轮式里程计采集的位姿数据和所述机器人10的惯性测量单元13获得的位姿数据的组合作为初始位姿，包括：基于滤波函数融合所
述轮式里程计采集的位姿数据和所述惯性测量单元13获得的位姿数据，以生成所述初始位姿，所述滤波函数可被设置为etf滤波函数，当然，也可以采用其他滤波函数。
117.然后，响应于所述第二帧局部彩色图像的位姿满足预设条件，将所述第二帧局部彩色图像确定为第二帧关键帧局部彩色图像，通过这样的方式，获得预设数量的关键帧局部彩色图像；也就是说，如果满足预设条件，则新建关键帧k_rgb_1,以此类推，持续构建关键帧 k_rgb_i。
118.特别地，在该步骤中，所述预设条件为如下任意之一:所述第二帧局部彩色图像的位姿的旋转分量大于第一阈值；所述第二帧局部彩色图像的位姿的平移分量大于第二阈值；或者，所述第二帧局部彩色图像与所述第一帧局部彩色图像之间的时间差大于第三阈值。
119.接着，基于所述关键帧局部彩色图像生成关键帧局部彩色图像词袋，其中，所述关键帧局部彩色图像词袋包括每一所述关键帧局部彩色图像的局部orb特征点的特征描述、每一所述关键帧局部彩色图像的局部orb特征点的三维坐标以及所述关键帧局部彩色图像的位姿；这里，为了便于描述将关键帧局部彩色图像词袋设定为orb_dbow,每一所述关键帧局部彩色图像的局部orb特征点的三维坐标设定为 map_point_pose，所述关键帧局部彩色图像的位姿设定为frame_pose。
120.然后，获得与所述关键帧局部彩色图像对应数量的关键帧局部点云；这里，应可以理解，与k_rgb_0对应的局部点云为k_pts_0，以此类推持续构建关键帧k_pts_i。
121.然后，基于所述关键帧局部彩色图像的局部orb特征点和与所述关键帧局部彩色图像对应的局部深度图像提供的所述局部orb特征点的深度值，生成局部特征点地图；
122.接着，以第一帧关键帧局部点云初始化生成初始局部栅格地图，并基于第二帧关键帧局部点云和第二帧关键帧局部彩色图像的位姿通过执行如下所述的损失函数进行配准，以获得优化的所述第二帧关键帧局部彩色图像的位姿并更新初始局部栅格地图，通过这样的方式迭代优化以实现稀疏优化；
123.其中，所述损失函数的表达式为：
124.cost_function＝gridmap_costfunction keypoints_reprojects，其中，gridmap_costfunction表示由关键帧局部点云生成的稠密线性点云与栅格地图的栅格点之间的配准概率；keypoints_reprojects 表示所述局部特征点地图中的点投影到基于局部彩色图像构建的坐标系上与对应所述关键帧局部彩色图像的orb特征点之间的重投影误差。
125.图4图示了根据本技术实施例的所述位姿估计方法中稀疏优化的示意图。如图4所示，在本技术实施例中，所述稀疏优化的过程，包括步骤：s210，提取多帧所述局部彩色图像的局部orb特征点；s220，以第一帧局部彩色图像作为第一帧关键帧局部彩色图像，并以第一帧局部彩色图像对应的第一帧局部点云作为第一关键帧局部点云；s230，以所述轮式里程计采集的第一位姿数据和所述惯性测量单元13获得的第二位姿数据的组合作为初始位姿进行帧间位姿递推，以获得第二帧局部彩色图像的位姿，其中，所述第二帧局部彩色图像的位姿为所述机器人10相对于所述目标站点的实时位姿；s240，响应于所述第二帧局部彩色图像的位姿满足预设条件，将所述第二帧局部彩色图像确定为第二帧关键帧局部彩色图像，通过这样的方式，获得预设数量的关键帧局部彩色图像；s250，基于所述关键帧局部彩
色图像生成关键帧局部彩色图像词袋，其中，所述关键帧局部彩色图像词袋包括每一所述关键帧局部彩色图像的局部orb特征点的特征描述、每一所述关键帧局部彩色图像的局部orb特征点的三维坐标以及所述关键帧局部彩色图像的位姿；s260，获得与所述关键帧局部彩色图像对应数量的关键帧局部点云；s270，基于所述关键帧局部彩色图像的局部 orb特征点和与所述关键帧局部彩色图像对应的局部深度图像提供的所述局部orb特征点的深度值，生成局部特征点地图；s280,以第一帧关键帧局部点云初始化生成初始局部栅格地图，并基于第二帧关键帧局部点云和第二帧关键帧局部彩色图像的位姿通过执行如下所述的损失函数进行配准，以获得优化的所述第二帧关键帧局部彩色图像的位姿并更新初始局部栅格地图，通过这样的方式迭代优化以实现稀疏优化；
126.其中，所述损失函数的表达式为：
127.cost_function＝gridmap_costfunction keypoints_reprojects，其中，gridmap_costfunction表示由关键帧局部点云生成的稠密线性点云与栅格地图的栅格点之间的配准概率；keypoints_reprojects 表示所述局部特征点地图中的点投影到基于局部彩色图像构建的坐标系上与对应所述关键帧局部彩色图像的orb特征点之间的重投影误差。
128.进一步地，在本技术实施例中，所述边缘优化的过程，包括：
129.首先以包含预设帧数关键帧的滑动窗口采用与稀疏优化过程中相一致的损失函数进行配准，以优化所述关键帧局部彩色图像的位姿；也就是，设置滑动窗口，其内含预设帧数关键帧，通过共视图，采用如上所述的损失函数，优化机器人10的位姿。优选地，在本技术实施例中，所述预设帧数关键帧的范围为8帧至12帧，例如，可被实施为10帧。
130.然后，去除所述滑动窗口内的首帧关键帧局部彩色图像并在所述滑动窗口的尾帧关键帧局部彩色图像后添加一帧新的关键帧局部彩色图像，并采用与稀疏优化过程中相一致的损失函数进行配准，通过这样的方式，以迭代优化所述关键帧局部彩色图像的位姿。也就是，去除窗口内的第一帧数据，在窗口末尾扩展一个关键帧，继续优化机器的位姿。
131.同时，在进行边缘化的而过程中，还包括以包含所述预设帧数关键帧的滑动窗口基于由关键帧局部点云生成的稠密线性点云采用与稀疏优化过程中相一致的损失函数进行配准，以优化所述局部栅格地图。
132.图5图示了根据本技术实施例的所述位姿估计方法中边缘优化的流程图。如图5所示，在本技术实施例中，所述边缘优化的过程，包括步骤：s310,以包含预设帧数关键帧的滑动窗口采用与稀疏优化过程中相一致的损失函数进行配准，以优化所述关键帧局部彩色图像的位姿；s320,去除所述滑动窗口内的首帧关键帧局部彩色图像并在所述滑动窗口的尾帧关键帧局部彩色图像后添加一帧新的关键帧局部彩色图像，并采用与稀疏优化过程中相一致的损失函数进行配准，通过这样的方式，以迭代优化所述关键帧局部彩色图像的位姿；以及， s330，以包含所述预设帧数关键帧的滑动窗口基于由关键帧局部点云生成的稠密线性点云采用与稀疏优化过程中相一致的损失函数进行配准，以优化所述局部栅格地图。
133.在步骤s170中，基于所述局部地图进行非线性优化，以确定所述机器人10在所述目标站点附近相对于所述目标站点的位姿。
134.具体来说，在本技术实施例中，基于所述局部地图进行非线性优化的过程，包括：
135.首先，提取所述目标站点对应的所述初始彩色图像的初始orb特征点。
136.然后，以所述初始彩色图像的初始orb特征点在所述关键帧局部彩色图像词袋中进行查询，以获得与所述初始彩色图像匹配的备选关键帧局部彩色图像。在本技术的示例中，也就是说，obj_1_orb与 obj_1的局部地图的orb_dbow匹配，备选关键帧局部彩色图像 k_rgb_j(其中，j＝1,2,
……
)。
137.然后，以所述初始彩色图像的初始orb特征点与所述备选关键帧局部彩色图像的局部orb特征点进行pnp匹配，生成所述机器人10 在所述目标站点附近的初始位姿；也就是，根据k_rgb_j与obj_1_orb 进行pnp匹配，得出初始位姿，设定为t_1。
138.接着，基于所述初始彩色图像的初始orb特征点与所述初始彩色图像对应的初始点云使用如下损失函数进行迭代优化，以获得所述机器人10在所述目标站点附近的迭代位姿，其中，所述迭代位姿为机器人10在所述目标站点附近相对于所述目标站点的位姿；
139.其中，所述损失函数的表达式为：
140.cost_function＝gridmap_costfunction keypoints_reprojects，其中，gridmap_costfunction表示由初始点云生成的稠密线性点云与栅格地图的栅格点之间的配准概率；keypoints_reprojects表示所述局部特征点地图中的点投影到基于所述初始彩色图像构建的坐标系上与对应所述初始彩色图像的初始orb特征点之间的重投影误差。
141.也就是，以t_1作为初始，obj_1_orb和obj_1_pts作为输入，使用如上所述的损失函数迭代优化，得出obj_1_orb与obj_1区域地图的位姿t_2。
142.综上，基于本技术实施例的用于机器人10的位姿估计方法被阐明，其中，根据本技术实施例的位姿估计方法包括两个阶段：粗位姿估计阶段和精位姿估计阶段，其中，在粗位姿估计阶段由现有的导航方案驱动至目标站点附近，在精位姿估计阶段基于所述机器人10的传感设备所采集的目标站点的局部数据对所述目标站点进行二次位姿估计，通过这样的方式，能有效地降低长时累计误差且具有较高的位姿估计精度。具体地，在本技术实施例中，步骤s110至步骤s140 为粗位姿估计阶段，步骤s150至步骤s170为精位姿估计阶段。示意性用于机器人的运动规划方法
143.应可以理解，通过如上所述的位姿估计方法能够获得更为精确的位姿估计，从而所述机器人10能够更为精确地进行运动规划，以更为精确地到达目标站点从而能够为预定任务的精确执行提供良好的基础。并且，基于更为精确的位姿估计，所述机器人10在粗位姿估计阶段的可接受误差更大，以使得所述机器人10具有更有的扩展能力和适配能力。
144.相应地，根据本技术另一方面，如图6所示，还提供一种用于机器人10的运动规划方法，其包括步骤：s410,基于如上所述的位姿估计方法，获得所述机器人10在所述目标站点附近相对于所述目标站点的位姿；s420，获得所述机器人10在所述目标站点附近相对于所述目标站点的位姿与所述机器人10的实时位姿之间的位姿差；以及，s430，基于所述位姿差，移动所述机器人10。
145.在步骤s410中，基于如上所述的位姿估计方法，获得所述机器人10在所述目标站点附近相对于所述目标站点的位姿。如上所述，此时的位姿为t_3。
146.在步骤s420中，获得所述机器人10在所述目标站点附近相对于所述目标站点的位姿与所述机器人10的实时位姿之间的位姿差。也就是，t_4＝t2-t3，其中，t4表示位姿差，t3表示所述机器人10的实时位姿。
147.在步骤s430中，基于所述位姿差，移动所述机器人10。也就是，基于所述位姿差调
整所述机器人10相对于所述目标站点的位姿。
148.相应地，如果调整之后的位姿精度仍不满足要求，则可循环获得所述机器人10在所述目标站点附近相对于所述目标站点的位姿、获得所述机器人10在所述目标站点附近相对于所述目标站点的位姿与所述机器人10的实时位姿之间的位姿差和基于所述位姿差，移动所述机器人10，直至所述位姿差中平移分量和旋转分量小于预设阈值。
149.也就是，在本技术实施例中，所述用于机器人10的运动规划方法，进一步包括：循环获得所述机器人10在所述目标站点附近相对于所述目标站点的位姿、获得所述机器人10在所述目标站点附近相对于所述目标站点的位姿与所述机器人10的实时位姿之间的位姿差和基于所述位姿差，移动所述机器人10，直至所述位姿差中平移分量和旋转分量小于预设阈值。
150.综上，基于本技术实施例的用于机器人10的运动规划方法被阐明，其基于具有更高精度的位姿估计，使得所述机器人10能够更为精确地进行运动规划，以更为精确地到达目标站点从而能够为预定任务的精确执行提供良好的基础。
151.示意性用于机器人的位姿估计装置
152.根据本技术又一方面，还提供了一种用于机器人10的位姿估计装置。
153.图7图示了根据本技术实施例的用于机器人的位姿估计装置 400的框图示意图。
154.如图7所示，根据本技术实施例的位姿估计装置700，包括：初始图像采集单元710，用于通过机器人10的图像传感器12获取目标空间中各站点的多帧初始彩色图像和与所述初始彩色图像分别对应的多帧初始深度图像；导航地图构建单元720，用于构建用于描述目标空间的导航地图；路径搜索单元730，用于搜索所述机器人10 在所述导航地图上自起点至目标站点之间的无碰路径；驱动单元740，用于按所述无碰路径，驱动所述机器人10至所述目标站点的附近；局部图像采集单元750，用于通过所述图像传感器12获取所述目标站点的多帧局部彩色图像和与所述局部彩色图像分别对应的多帧局部深度图像；局部地图构建单元760，用于基于所述多帧局部彩色图像和所述多帧局部深度图像进行非线性优化，以生成用于描述所述目标站点的局部地图；以及，位姿估计单元770，用于基于所述局部地图进行非线性优化，以确定所述机器人10在所述目标站点附近相对于所述目标站点的位姿。
155.在一个示例中，在上述位姿估计装置700中，所述路径搜索单元，用于搜索所述机器人在所述栅格地图上自起点至目标站点之间的无碰路径。
156.在一个示例中，在上述位姿估计装置700中，如图8所示，所述局部地图构建单元760，包括：局部orb特征点提取子单元761，用于提取多帧所述局部彩色图像的局部orb特征点；第一帧关键帧设定子单元762，用于以第一帧局部彩色图像作为第一帧关键帧局部彩色图像，并以第一帧局部彩色图像对应的第一帧局部点云作为第一关键帧局部点云；位姿递推子单元763，用于以所述轮式里程计采集的第一位姿数据和所述惯性测量单元13获得的第二位姿数据的组合作为初始位姿进行帧间位姿递推，以获得第二帧局部彩色图像的位姿，其中，所述第二帧局部彩色图像的位姿为所述机器人10相对于所述目标站点的实时位姿；关键帧筛选子单元764，用于响应于所述第二帧局部彩色图像的位姿满足预设条件，将所述第二帧局部彩色图像确定为第二帧关键帧局部彩色图像，通过这样的方式，获得预设数量的关键帧局部彩色图像；词袋生成子单元765，用于基于所述关键帧局部彩色图像生成关键帧局部彩色图像词袋，其中，所述关键帧局部彩色图像词袋包括每一所述关键帧局部
彩色图像的局部orb特征点的特征描述、每一所述关键帧局部彩色图像的局部orb特征点的三维坐标以及所述关键帧局部彩色图像的位姿；关键帧局部点云获取子单元 766，用于获得与所述关键帧局部彩色图像对应数量的关键帧局部点云；局部特征点地图生成子单元767，用于基于所述关键帧局部彩色图像的局部orb特征点和与所述关键帧局部彩色图像对应的局部深度图像提供的所述局部orb特征点的深度值，生成局部特征点地图；以及，配准优化子单元768，用于以第一帧关键帧局部点云初始化生成初始局部栅格地图，并基于第二帧关键帧局部点云和第二帧关键帧局部彩色图像的位姿通过执行如下所述的损失函数进行配准，以获得优化的所述第二帧关键帧局部彩色图像的位姿并更新初始局部栅格地图，通过这样的方式迭代优化以实现稀疏优化；其中，所述损失函数的表达式为：
157.cost_function＝gridmap_costfunction keypoints_reprojects，其中，gridmap_costfunction表示由关键帧局部点云生成的稠密线性点云与栅格地图的栅格点之间的配准概率；keypoints_reprojects表示所述局部特征点地图中的点投影到基于局部彩色图像构建的坐标系上与对应所述关键帧局部彩色图像的orb特征点之间的重投影误差。
158.在一个示例中，在上述位姿估计装置700中，所述局部地图构建单元760，进一步用于：以包含预设帧数关键帧的滑动窗口采用与稀疏优化过程中相一致的损失函数进行配准，以优化所述关键帧局部彩色图像的位姿；以及，去除所述滑动窗口内的首帧关键帧局部彩色图像并在所述滑动窗口的尾帧关键帧局部彩色图像后添加一帧新的关键帧局部彩色图像，并采用与稀疏优化过程中相一致的损失函数进行配准，通过这样的方式，以迭代优化所述关键帧局部彩色图像的位姿。
159.在一个示例中，在上述位姿估计装置700中，如图9所示，所述位姿估计单元770，包括：初始orb特征点提取子单元771，用于提取所述目标站点对应的所述初始彩色图像的初始orb特征点；匹配子单元772，用于以所述初始彩色图像的初始orb特征点在所述关键帧局部彩色图像词袋中进行查询，以获得与所述初始彩色图像匹配的备选关键帧局部彩色图像；初始位姿确定子单元773，以所述初始彩色图像的初始orb特征点与所述备选关键帧局部彩色图像的局部orb特征点进行pnp匹配，生成所述机器人10在所述目标站点附近的初始位姿；以及，位姿迭代子单元774，用于基于所述初始彩色图像的初始orb特征点与所述初始彩色图像对应的初始点云使用如下损失函数进行迭代优化，以获得所述机器人10在所述目标站点附近的迭代位姿，其中，所述迭代位姿为机器人10在所述目标站点附近相对于所述目标站点的位姿；
160.其中，所述损失函数的表达式为：
161.cost_function＝gridmap_costfunction keypoints_reprojects，其中， gridmap_costfunction表示由初始点云生成的稠密线性点云与栅格地图的栅格点之间的配准概率；keypoints_reprojects表示所述局部特征点地图中的点投影到基于所述初始彩色图像构建的坐标系上与对应所述初始彩色图像的初始orb特征点之间的重投影误差。
162.这里，本领域技术人员可以理解，上述位姿估计装置700中的各个单元和模块的具体功能和操作已经在上面参考图1到图5描述的位姿估计方法中详细介绍，并因此，将省略其重复描述。
163.如上所述，根据本技术实施例的位姿估计装置700可以实现在各种机器人10中，例
如扫地机器人10。在一个示例中，根据本技术实施例的位姿估计装置700可以作为一个软件模块和/或硬件模块而集成到所述机器人10中。例如，该位姿估计装置700可以是该机器人10的操作系统中的一个软件模块，或者可以是针对于该机器人 10所开发的一个应用程序；当然，该位姿估计装置700同样可以是该机器人10的众多硬件模块之一。
164.替换地，在另一示例中，该位姿估计装置700与该机器人10也可以是分立的设备，并且该位姿估计装置700可以通过有线和/或无线网络连接到该机器人10，并且按照约定的数据格式来传输交互信息。
165.示意性用于机器人的运动规划装置
166.根据本技术又一方面，还提供一种用于机器人10的运动规划装置。
167.图10图示了根据本技术实施例的用于机器人10的运动规划装置800的框图示意图。
168.如图10所示，根据本技术实施例的运动规划装置800，包括：如上所述的位姿估计装置700，用于确定所述机器人10在所述目标站点附近相对于所述目标站点的位姿；位姿差获取单元810，用于获得所述机器人10在所述目标站点附近相对于所述目标站点的位姿与所述机器人10的实时位姿之间的位姿差；以及，移动单元820，用于基于所述位姿差，移动所述机器人10。
169.这里，本领域技术人员可以理解，上述运动规划装置800中的各个单元和模块的具体功能和操作已经在上面参考图1至图6描述的运动规划方法中详细介绍，并因此，将省略其重复描述。
170.如上所述，根据本技术实施例的运动规划装置800可以实现在各种机器人10中，例如扫地机器人10。在一个示例中，根据本技术实施例的运动规划装置800可以作为一个软件模块和/或硬件模块而集成到所述机器人10中。例如，该运动规划装置800可以是该机器人10的操作系统中的一个软件模块，或者可以是针对于该机器人 10所开发的一个应用程序；当然，该运动规划装置800同样可以是该机器人10的众多硬件模块之一。
171.替换地，在另一示例中，该运动规划装置800与该机器人10也可以是分立的设备，并且该运动规划装置800可以通过有线和/或无线网络连接到该机器人10，并且按照约定的数据格式来传输交互信息。
172.以上结合具体实施例描述了本技术的基本原理，但是，需要指出的是，在本技术中提及的优点、优势、效果等仅是示例而非限制，不能认为这些优点、优势、效果等是本技术的各个实施例必须具备的。另外，上述公开的具体细节仅是为了示例的作用和便于理解的作用，而非限制，上述细节并不限制本技术为必须采用上述具体的细节来实现。
173.本技术中涉及的器件、装置、设备、系统的方框图仅作为例示性的例子并且不意图要求或暗示必须按照方框图示出的方式进行连接、布置、配置。如本领域技术人员将认识到的，可以按任意方式连接、布置、配置这些器件、装置、设备、系统。诸如“包括”、“包含”、“具有”等等的词语是开放性词汇，指“包括但不限于”，且可与其互换使用。这里所使用的词汇“或”和“和”指词汇“和/或”，且可与其互换使用，除非上下文明确指示不是如此。这里所使用的词汇“诸如”指词组“诸如但不限于”，且可与其互换使用。
174.还需要指出的是，在本技术的装置、设备和方法中，各部件或各步骤是可以分解和/或重新组合的。这些分解和/或重新组合应视为本技术的等效方案。
175.提供所公开的方面的以上描述以使本领域的任何技术人员能够做出或者使用本技术。对这些方面的各种修改对于本领域技术人员而言是非常显而易见的，并且在此定义的一般原理可以应用于其他方面而不脱离本技术的范围。因此，本技术不意图被限制到在此示出的方面，而是按照与在此公开的原理和新颖的特征一致的最宽范围。
176.为了例示和描述的优势已经给出了以上描述。此外，此描述不意图将本技术的实施例限制到在此公开的形式。尽管以上已经讨论了多个示例方面和实施例，但是本领域技术人员将认识到其某些变型、修改、改变、添加和子组合。