室内机器人的定位与模型构建方法及系统与流程

1.本发明涉及导航与定位领域，具体地，涉及一种室内机器人的定位与模型构建方法及系统。


背景技术：

2.随着机器人技术的发展，人们对于机器人功能的多样性复杂性需求与日俱增。如重复性高、工作量大的工作或者在恶劣艰巨环境下的作业都可以用机器人来代替。为了满足以上需求，人们通常在可自主移动的机器人身体上安装如操作杆或者机械臂等可以与环境交互的装备或系统。
3.室内环境通常比较固定，对于室内作业机器人来说，如何充分理解场景中物体、对象的空间位置，结构对于和它们的交互来说尤为重要。因此，对室内场景的三维建模以及地图的建立可以帮助机器人更好的完成任务。传统的同步地图构建与定位系统，相机或激光雷达和机器人的空间相对位置通常是固定的。由于空间中物体间通常会互相遮蔽，机器人虽然能自我定位以及移动，但对三维环境的细节可能理解不充分。同时，用二维栅格地图导航对于三维空间物体避障问题上也有局限性。针对现有技术中的缺陷，本发明要解决的技术问题体现在以下几点：机器人建图时缺乏引导，盲目性较大，对空间信息理解不充分；二维栅格地图导航时不能对空间中物体避障；对于空间中场景或目标进行交互时对模型精度要求很高，普通建模方式很难达到要求。
4.专利文献cn105354875a(申请号：cn201510622546.5)公开了一种室内环境二维与三维联合模型的构建方法和系统，包括：获得所述水平方向设置的二维激光扫描仪输出的二维激光扫描数据，构建二维栅格地图；采用扩展卡尔曼滤波方法，根据惯性传感单元输出的数据与二维栅格地图实现平台六自由度姿态估计；获得两个其底部沿竖直方向安装的二维激光扫描仪输出的二维激光扫描数据，构建室内环境的三维点云数据；将平台六自由度位姿估计的结果作为初始旋转平移矩阵，将所述三维点云数据配准为三维地图；采用二维栅格地图构建中的闭合环路检测原理，进行二维栅格地图的闭环检测，并通过平台位姿调整，获得全局优化的二维栅格地图和三维地图。该发明虽然能对场景中的三维信息有一个整体的描述，但对于细节的构建受限于建图机器人的结构以及建图方式。对于场景中需要细致交互的目标理解有局限性。
5.专利文献cn106970395a(申请号：cn201710317929.0)公开了一种确定障碍物位置的方法和装置，属于计算机技术领域。所述方法包括：根据每束第二雷达激光确定每束第二雷达激光对应反射点的三维坐标信息；根据每个反射点的横坐标信息和纵坐标信息，分别确定每个反射点在二维栅格地图中对应的栅格，二维栅格地图包括多个分区，每个分区包括多个栅格；根据第一栅格对应的最大高度信息和最小高度信息以及第二栅格对应的最小高度信息确定第一栅格是否包括障碍物，第一栅格为二维栅格地图中任一个栅格，第二栅格与第一栅格在同一分区内；如果包括障碍物，则根据第一栅格的位置和车辆的位置确定该障碍物相对于车辆的位置。该发明用两层激光确定障碍物，虽然能得到障碍物在底层栅
格地图的位置，但依旧缺乏对之的细节描述。
6.专利文献cn110842926a(申请号：cn201911192862.8)公开了一种机器人精准定位系统，包括：建筑物设计图纸录入模块，用于录入对应的建筑物设计图纸；建筑物三维模型构建模块，用于根据录入的建筑物设计图纸实现建筑物三维模型的构建；坐标系生成模块，用于在构建的建筑物三维模型中标定机器人当前所在位置，以该位置为圆心构建三维坐标系；机器人运动轨迹采集模块，用于采集机器人的运动轨迹；坐标计算模块，用于根据采集到的机器人运动轨迹计算当前机器人所在坐标；定位信息输出模块，用于根据所得的坐标输出建筑物三维模型中对应的位置信息，通过三维坐标系的构建，基于起点坐标以及运动轨迹进行机器人当前位置定位。但该发明对于空间中场景或目标进行交互时对模型精度不高，达不到需要的要求。


技术实现要素：

7.针对现有技术中的缺陷，本发明的目的是提供一种室内机器人的定位与模型构建方法及系统。
8.根据本发明提供的一种室内机器人的定位与模型构建方法，包括：
9.步骤s1：建立二维栅格地图，设置机器人起始位置，在二维栅格地图边缘设置目标点；
10.步骤s2：调整机器人机械臂姿态，对目标点采集三维点云数据，投影到二维栅格地图平面，更新二维栅格地图；
11.步骤s3：检测目标点点云是否有待建模对象；
12.步骤s4：控制机器人行进至待建模对象前，更新机械臂姿态，对待建模对象建模；
13.重复触发步骤s2
‑
步骤s4直至遍历整个二维栅格地图。
14.优选地，在所述步骤s1中：
15.将二维栅格地图外围看作一个点阵形成的封闭图形：
16.封闭图形是凸形，则以封闭图形质心为原点，等距离间隔在图形上取点，并使得取得的点朝质心方向偏移预设值距离得到目标点；以凸形图形的质心为起始点；从起始点开始，机器人向每个目标点逐个前进，遍历目标点集合，重复直至所有起始点、目标点遍历完成；
17.封闭图形为凹形，则把图形分为多个凸形组成的凹形，对每个分割后的凸型以分割后的凸型图形质心为原点，等距离间隔在分割后的凸型图形上取点，并使得取得的点朝质心方向偏移预设值距离得到目标点，则得到多个起始点和目标点集合的组合；以最靠近门的质心为起始点，遍历质心点，每个质心点对应每个凸型的目标点的集合，每到一个质心，则遍历质心对应的目标点，对应目标点遍历完成后，到下一个质心，遍历下一个质心的对应目标点，重复直至所有起始点、目标点遍历完成；
18.遍历过程加入遍历最短路径计算。
19.优选地，在所述步骤s2中：
20.点云坐标从相机坐标系转换到机械臂末端坐标系，然后从机械臂末端坐标系转换到机器人基座坐标系；
21.点云坐标投影在二维栅格地图平面；投影边缘在二维栅格地图内测，则更新二维
栅格地图以及目标点。
22.优选地，在所述步骤s3中：
23.步骤s3.1：以预设速度向目标点行进，采集彩色、深度图像以及对应的点云；利用深度学习对基于深度信息的彩色图像检测是否存在待建模对象；检测特征点，对相邻两帧特征点进行icp匹配，得到旋转平移变换矩阵；对点云进行矩阵变换并融合，并重复触发步骤s2，直至朝目标点前进至机器人地盘坐标原点距离目标点固定距离处；
24.步骤s3.2：检测到待建模对象，则对该对象进行点云分割，计算点云质心，以点云中距离质心最远点到质心的距离为半径做一个球面，重复触发步骤s3.1，对分割的点云进行坐标变换以及融合，以新的点云更新球面球心以及半径，直至达到目标点。
25.优选地，在所述步骤s3中，还包括：
26.如未检测到待建模对象，则重复步骤s3.1直至达到目标点。
27.优选地，在所述步骤s4中：
28.步骤s4.1：对球面上的点以预设值体素距离进行降取样，降取样点a指向质心o的向量为oa，使得oa和机械臂末端z轴共轴作为机械臂姿态posea；机械臂通过夹爪夹取工具与场景交互，a点朝ao方向偏移预设值距离，该距离大于深度信息采集的最小距离，偏移后得到点a’，机械臂以posea走到a'拍照，得到点云pca。
29.步骤s4.2：对球面上的点重复触发步骤s4.1，对得到的点云重复触发步骤s3.1；进行点云融合，直至遍历完所有点，然后对点云分割，得到目标模型。
30.根据本发明提供的一种室内机器人的定位与模型构建系统，包括：
31.模块m1：建立二维栅格地图，设置机器人起始位置，在二维栅格地图边缘设置目标点；
32.模块m2：调整机器人机械臂姿态，对目标点采集三维点云数据，投影到二维栅格地图平面，更新二维栅格地图；
33.模块m3：检测目标点点云是否有待建模对象；
34.模块m4：控制机器人行进至待建模对象前，更新机械臂姿态，对待建模对象建模；
35.重复触发步骤s2
‑
步骤s4直至遍历整个二维栅格地图。
36.优选地，在所述模块m1中：
37.将二维栅格地图外围看作一个点阵形成的封闭图形：
38.封闭图形是凸形，则以封闭图形质心为原点，等距离间隔在图形上取点，并使得取得的点朝质心方向偏移预设值距离得到目标点；以凸形图形的质心为起始点；从起始点开始，机器人向每个目标点逐个前进，遍历目标点集合，重复直至所有起始点、目标点遍历完成；
39.封闭图形为凹形，则把图形分为多个凸形组成的凹形，对每个分割后的凸型以分割后的凸型图形质心为原点，等距离间隔在分割后的凸型图形上取点，并使得取得的点朝质心方向偏移预设值距离得到目标点，则得到多个起始点和目标点集合的组合；以最靠近门的质心为起始点，遍历质心点，每个质心点对应每个凸型的目标点的集合，每到一个质心，则遍历质心对应的目标点，对应目标点遍历完成后，到下一个质心，遍历下一个质心的对应目标点，重复直至所有起始点、目标点遍历完成；
40.优选地，在所述模块m2中：
41.点云坐标从相机坐标系转换到机械臂末端坐标系，然后从机械臂末端坐标系转换到机器人基座坐标系；
42.点云坐标投影在二维栅格地图平面；投影边缘在栅格地图内测，则更新栅格地图以及目标点。
43.优选地，在所述模块m3中：
44.模块m3.1：以预设速度向目标点行进，采集彩色、深度图像以及对应的点云；利用深度学习对基于深度信息的彩色图像检测是否存在待建模对象；检测特征点，对相邻两帧特征点进行icp匹配，得到旋转平移变换矩阵；对点云进行矩阵变换并融合，并重复触发模块m2，直至朝目标点前进至机器人地盘坐标原点距离目标点固定距离处；
45.模块m3.2：检测到待建模对象，则对该对象进行点云分割，计算点云质心，以点云中距离质心最远点到质心的距离为半径做一个球面，重复触发模块m3.1，对分割的点云进行坐标变换以及融合，以新的点云更新球面球心以及半径，直至达到目标点；
46.优选地，在所述模块m3中，还包括：
47.如未检测到待建模对象，则重复模块m3.1直至达到目标点。
48.优选地，在所述模块m4中：
49.模块m4.1：对球面上的点以预设值体素距离进行降取样，降取样点a指向质心o的向量为oa，使得oa和机械臂末端z轴共轴作为机械臂姿态posea；机械臂通过夹爪夹取工具与场景交互，a点朝ao方向偏移预设值距离，该距离大于深度信息采集的最小距离，偏移后得到点a’，机械臂以posea走到a'拍照，得到点云pca。
50.模块m4.2：对球面上的点重复触发模块m4.1，对得到的点云重复触发模块m3.1；进行点云融合，直至遍历完所有点，然后对点云分割，得到目标模型。
51.与现有技术相比，本发明具有如下的有益效果：
52.1、机器人可以自主行进，建立三维空间模型以及目标物体模型；
53.2、在二维栅格地图的基础上，可以对空间中的物体位置提前知晓，进行避障；
54.3、从多角度细致地对目标对象进行建模，可以支持后续对目标对象的复杂精细操作；
55.4、建立场景地图以及对场景内内目标对象细致建模，以便于对它们执行重复性交互操作。
附图说明
56.通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
57.图1为机器人侧面图；
58.图2为原始凹形状二维栅格地图；
59.图3为二维栅格地图通过距离变换获得的图像；
60.图4为依据灰度划分的分割后的栅格地图。
具体实施方式
61.下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术
人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
62.实施例1：
63.根据本发明提供的一种室内机器人的定位与模型构建方法，如图1
‑
图4，包括：
64.步骤s1：建立二维栅格地图，设置机器人起始位置，在二维栅格地图边缘设置目标点；
65.步骤s2：调整机器人机械臂姿态，对目标点采集三维点云数据，投影到二维栅格地图平面，更新二维栅格地图；
66.步骤s3：检测目标点点云是否有待建模对象；
67.步骤s4：控制机器人行进至待建模对象前，更新机械臂姿态，对待建模对象建模；
68.重复触发步骤s2
‑
步骤s4直至遍历整个二维栅格地图。
69.具体地，在所述步骤s1中：
70.将二维栅格地图外围看作一个点阵形成的封闭图形：
71.封闭图形是凸形，则以封闭图形质心为原点，等距离间隔在图形上取点，并使得取得的点朝质心方向偏移预设值距离得到目标点；以凸形图形的质心为起始点；从起始点开始，机器人向每个目标点逐个前进，遍历目标点集合，重复直至所有起始点、目标点遍历完成；
72.封闭图形为凹形，则把图形分为多个凸形组成的凹形，对每个分割后的凸型以分割后的凸型图形质心为原点，等距离间隔在分割后的凸型图形上取点，并使得取得的点朝质心方向偏移预设值距离得到目标点，则得到多个起始点和目标点集合的组合；以最靠近门的质心为起始点，遍历质心点，每个质心点对应每个凸型的目标点的集合，每到一个质心，则遍历质心对应的目标点，对应目标点遍历完成后，到下一个质心，遍历下一个质心的对应目标点，重复直至所有起始点、目标点遍历完成；
73.遍历过程加入遍历最短路径计算。
74.具体地，在所述步骤s2中：
75.点云坐标从相机坐标系转换到机械臂末端坐标系，然后从机械臂末端坐标系转换到机器人基座坐标系；
76.点云坐标投影在二维栅格地图平面；投影边缘在二维栅格地图内测，则更新二维栅格地图以及目标点。
77.具体地，在所述步骤s3中：
78.步骤s3.1：以预设速度向目标点行进，采集彩色、深度图像以及对应的点云；利用深度学习对基于深度信息的彩色图像检测是否存在待建模对象；检测特征点，对相邻两帧特征点进行icp匹配，得到旋转平移变换矩阵；对点云进行矩阵变换并融合，并重复触发步骤s2，直至朝目标点前进至机器人地盘坐标原点距离目标点固定距离处；
79.步骤s3.2：检测到待建模对象，则对该对象进行点云分割，计算点云质心，以点云中距离质心最远点到质心的距离为半径做一个球面，重复触发步骤s3.1，对分割的点云进行坐标变换以及融合，以新的点云更新球面球心以及半径，直至达到目标点。
80.具体地，在所述步骤s3中，还包括：
81.如未检测到待建模对象，则重复步骤s3.1直至达到目标点。
82.具体地，在所述步骤s4中：
83.步骤s4.1：对球面上的点以预设值体素距离进行降取样，降取样点a指向质心o的向量为oa，使得oa和机械臂末端z轴共轴作为机械臂姿态posea；机械臂通过夹爪夹取工具与场景交互，a点朝ao方向偏移预设值距离，该距离大于深度信息采集的最小距离，偏移后得到点a’，机械臂以posea走到a'拍照，得到点云pca。
84.步骤s4.2：对球面上的点重复触发步骤s4.1，对得到的点云重复触发步骤s3.1；进行点云融合，直至遍历完所有点，然后对点云分割，得到目标模型。
85.实施例2：
86.实施例2为实施例1的优选例，以更为具体地对本发明进行说明。
87.本领域技术人员可以将本发明提供的一种室内机器人的定位与模型构建方法，理解为室内机器人的定位与模型构建系统的具体实施方式，即所述室内机器人的定位与模型构建系统可以通过执行所述室内机器人的定位与模型构建方法的步骤流程予以实现。
88.根据本发明提供的一种室内机器人的定位与模型构建系统，包括：
89.模块m1：建立二维栅格地图，设置机器人起始位置，在二维栅格地图边缘设置目标点；
90.模块m2：调整机器人机械臂姿态，对目标点采集三维点云数据，投影到二维栅格地图平面，更新二维栅格地图；
91.模块m3：检测目标点点云是否有待建模对象；
92.模块m4：控制机器人行进至待建模对象前，更新机械臂姿态，对待建模对象建模；
93.重复触发步骤s2
‑
步骤s4直至遍历整个二维栅格地图。
94.具体地，在所述模块m1中：
95.将二维栅格地图外围看作一个点阵形成的封闭图形：
96.封闭图形是凸形，则以封闭图形质心为原点，等距离间隔在图形上取点，并使得取得的点朝质心方向偏移预设值距离得到目标点；以凸形图形的质心为起始点；从起始点开始，机器人向每个目标点逐个前进，遍历目标点集合，重复直至所有起始点、目标点遍历完成；
97.封闭图形为凹形，则把图形分为多个凸形组成的凹形，对每个分割后的凸型以分割后的凸型图形质心为原点，等距离间隔在分割后的凸型图形上取点，并使得取得的点朝质心方向偏移预设值距离得到目标点，则得到多个起始点和目标点集合的组合；以最靠近门的质心为起始点，遍历质心点，每个质心点对应每个凸型的目标点的集合，每到一个质心，则遍历质心对应的目标点，对应目标点遍历完成后，到下一个质心，遍历下一个质心的对应目标点，重复直至所有起始点、目标点遍历完成；
98.具体地，在所述模块m2中：
99.点云坐标从相机坐标系转换到机械臂末端坐标系，然后从机械臂末端坐标系转换到机器人基座坐标系；
100.点云坐标投影在二维栅格地图平面；投影边缘在栅格地图内测，则更新栅格地图以及目标点。
101.具体地，在所述模块m3中：
102.模块m3.1：以预设速度向目标点行进，采集彩色、深度图像以及对应的点云；利用深度学习对基于深度信息的彩色图像检测是否存在待建模对象；检测特征点，对相邻两帧特征点进行icp匹配，得到旋转平移变换矩阵；对点云进行矩阵变换并融合，并重复触发模块m2，直至朝目标点前进至机器人地盘坐标原点距离目标点固定距离处；
103.模块m3.2：检测到待建模对象，则对该对象进行点云分割，计算点云质心，以点云中距离质心最远点到质心的距离为半径做一个球面，重复触发模块m3.1，对分割的点云进行坐标变换以及融合，以新的点云更新球面球心以及半径，直至达到目标点；
104.具体地，在所述模块m3中，还包括：
105.如未检测到待建模对象，则重复模块m3.1直至达到目标点。
106.具体地，在所述模块m4中：
107.模块m4.1：对球面上的点以预设值体素距离进行降取样，降取样点a指向质心o的向量为oa，使得oa和机械臂末端z轴共轴作为机械臂姿态posea；机械臂通过夹爪夹取工具与场景交互，a点朝ao方向偏移预设值距离，该距离大于深度信息采集的最小距离，偏移后得到点a’，机械臂以posea走到a'拍照，得到点云pca。
108.模块m4.2：对球面上的点重复触发模块m4.1，对得到的点云重复触发模块m3.1；进行点云融合，直至遍历完所有点，然后对点云分割，得到目标模型。
109.实施例3：
110.实施例3为实施例1的优选例，以更为具体地对本发明进行说明。
111.机器人装配可活动机械臂，机械臂末端装配三维彩色深度相机。本发明基于此系统，属于一种对室内固定场景的三维建模，地图部署方法。目的在于建立场景地图以及对场景内内目标对象细致建模，以便于对它们执行重复性交互操作。本方法包括如下步骤：
112.步骤1：建立室内二维栅格地图，设置机器人起始位置，在栅格地图边缘设置目标点；
113.步骤2：调整机械臂姿态，对目标点采集三维点云数据，投影到二维栅格地图平面，更新栅格地图，检测目标点点云是否有待建模对象，有则进入下一步骤，没有则对下一个目标点重复步骤2；
114.步骤3：检测到目标对象，机器人行进至目标对象前，更新机械臂姿态，对目标对象建模；
115.步骤4：重复步骤2，3直至遍历整个地图。
116.其中，步骤1包括如下步骤：
117.1)室内二维栅格地图外围可看作一个点阵形成的封闭图形，如果此图形是凸形，则以其质心为原点。等距离间隔在图形上取点，并使得这些点朝质心方向偏移50厘米得到目标点。同时，以此质心为机器人运动起始点。
118.2)如果封闭图形为凹形，则用距离变换和分水岭算法可把图形分为几个凸形组成的凹形。对每个分割后的凸型重复步骤1)，则可得到多个起始点和目标点集合的组合。
119.3)从第一个起始点开始，机器人向每个目标点逐个前进，遍历目标点集合。重复以上步骤直至所有起始点，目标点遍历完成。可以计算最短路径遍历剩下的点，如二维路径规划算法dijkstra。
120.如封闭图形是凹形，以最靠近门的质心为第一个点，遍历质心点，每个质心点对应
一个凸型的目标点的集合，每到一个质心，就遍历这个质心对应的目标点。对应目标点遍历完成后，到下一个质心，遍历下一个质心的对应目标，然后遍历剩下的点。
121.步骤2包括如下步骤：
122.1)点云坐标从相机坐标系转换到机械臂末端坐标系，然后从机械臂末端坐标系转换到机器人基座坐标系。
123.2)点云坐标投影在二维栅格地图平面，如投影边缘在栅格地图内测，则更新栅格地图以及目标点。
124.3)以20厘米每秒速度向目标点行进，每秒5帧采集彩色，深度图像以及对应的点云。利用深度学习对基于深度信息的彩色图像检测是否存在待建模对象。利用orb算法检测特征点，对相邻两帧特征点进行icp匹配，得到旋转平移变换矩阵。对点云进行矩阵变换并融合，并重复步骤“1)”和步骤“2)”。朝目标点前进至机器人地盘坐标原点距离目标点50厘米处。
125.4)如检测到待建模对象，则对该对象进行点云分割。计算点云质心，以点云中距离质心最远点到质心的距离为半径做一个球面。重复步骤“3)”，对分割的点云进行坐标变换以及融合。以新的点云更新球面球心以及半径，直至达到目标点。
126.5)如未检测到待建模对象，则重复步骤3)直至达到目标点。
127.步骤3包括如下步骤：
128.1)对球面上的点以5厘米体素距离进行降取样。假设降取样点a指向质心o的向量为oa，使得oa和机械臂末端z轴共轴作为机械臂姿态posea。
129.如图一中所示，机械臂通过夹爪夹取工具与场景交互。设a点朝ao方向偏移一段略大于深度信息采集的最小距离。此处取20厘米，可得到最好的深度信息效果。假设偏移后得到点a’。则使得机械臂以posea走到a'拍照，得到点云pca。
130.2)对球面上的点重复步骤1)，对得到的点云重复“步骤2
‑
3)”。进行点云融合。直至遍历完所有点，然后对点云分割，得到目标模型。
131.本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
132.以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本技术的实施例和实施例中的特征可以任意相互组合。