一种在未知封闭空间内自主建图的移动机器人及工作方法

本发明涉及一种在未知封闭空间内自主建图的移动机器人及工作方法，属于点云数据处理和环境建图。

背景技术：

1、随着人工智能和机器人技术的飞速发展，自主导航和决策系统在多个领域，特别是工业、矿业和交通运输中，正逐渐展现出其巨大的应用潜力。在未知环境下的建图和导航，充分利用3d雷达的点云信息至关重要，点云信息密度是指单位空间内所包含的点云信息量，它反映了环境的复杂性和丰富程度。在未知全盲受限空间场景中，使用激光雷达建图时，点云信息密度的分布往往是不均匀的，一些关键区域的点云信息密度可能较高，而一些普通区域的点云信息密度则相对较低。因此，如何准确评估和利用这些信息密度，对于机器人在该场景下的自主导航和决策至关重要。

2、现有的环境建图技术大多依赖全人工控制或半自主操作来实现。其中，全人工控制建图是指由操作人员手动控制移动设备，采集未知环境信息并完成建图；半自主建图则是通过设备自主规划移动路径，并结合人工干预用于提升地图的完整性和保障设备的移动安全。然而，这些方法存在一定的应用局限性，例如在地下封闭环境中，移动设备与控制端之间难以建立稳定可靠的通信，导致人工难以介入，从而无法顺利执行全人工或半自主条件下的环境建图。

3、综上所述，如何提供一种用于未知封闭环境中的移动智能体全自主建图方案，使其不依靠人工干预即可全自动对未知环境进行精确建图，是本发明所需研究的方向。

技术实现思路

1、针对上述现有技术存在的问题，本发明提供一种在未知封闭空间内自主建图的移动机器人及工作方法，能在完全不依靠人工干预的情况下实现在未知封闭环境中全自动路径规划及避障，从而对未知环境进行精确建图。

2、为了实现上述目的，本发明采用的技术方案是：一种在未知封闭空间内自主建图的移动机器人，包括机器人主体、三维激光雷达、二维激光雷达、惯性测量单元(imu)、轮式里程计和中心处理器；

3、所述三维激光雷达通过支架装在机器人主体顶部，且其能以支架为中心进行旋转，用于获取机器人主体所处环境的360°实时激光扫描数据；

4、所述二维激光雷达通过转盘装在机器人主体顶部、且处于三维激光雷达的支架前方，通过转盘能带动二维激光雷达360°旋转，用于在机器人主体移动过程中获取实时扫描数据，并反馈给中心处理器构建二维栅格地图进行自主路径规划及避障；

5、所述惯性测量单元装在机器人主体内部的重心位置，用于测量机器人主体的加速度信息和角速度信息；

6、所述轮式里程计装在机器人主体的驱动轮上，用于获取机器人主体的车轮转动信息和行进距离信息；

7、所述中心处理器装在机器人主体内部，用于获取三维激光雷达、二维激光雷达、惯性测量单元和轮速里程计的监测数据，进行分析处理后控制机器人主体的移动路径，直至完成自主建图工作。

8、进一步，所述机器人主体为轮式机器人，中心处理器通过控制机器人主体前后轮的转向角度和速度，实现机器人的前进、后退、转弯动作；所述三维激光雷达为256线激光雷达。

9、上述移动机器人在未知封闭空间内的自主建图方法，具体步骤为：

10、步骤一、确定机器人主体实时位姿：中心处理器利用三维激光雷达和二维激光雷达对环境信息进行采集，并通过惯性测量单元和轮速里程计反馈的数据进行融合定位，使用扩展卡尔曼滤波器(ekf)将惯性测量单元和里程计数据结合，基于运动模型预测和更新机器人主体的位姿；

11、步骤二、自主建图：从三维激光雷达采集的三维点云数据中提取特征点，特征点包括角点和平面点两类；通过三维激光雷达扫描每帧之间的特征点匹配，利用角点匹配边缘特征，平面点匹配平面特征，估计当前帧和上一帧之间的相对位姿变化；根据多个帧之间的特征点匹配结果，通过levenberg-marquardt优化算法优化三维激光雷达的全局位姿，将每帧三维激光雷达数据都插入到全局点云地图中，构建形成完整的三维地图，以较低频率更新地图；将每一帧局部点云数据转化为全局坐标系中的点云信息，并加入全局点云地图；

12、步骤三、确定机器人主体移动的目标点：对二维激光雷达采集的点云数据进行半径滤波操作；构建当前点云数据的最小包围盒并将其分割为若干体素格；计算每个体素格的点云信息密度，选择未达到密度阈值的体素格，其中心投影点作为二维栅格地图中局部路径规划目标点；

13、步骤四、机器人移动及路径优化：通过运动学约束、时间约束、障碍物约束和平滑约束，确保步骤三规划的移动路径满足机器人运动学和避障要求；接着通过全局代价函数同时优化路径的几何形状和时间参数，最小化代价函数以获得最优路径；利用二维激光雷达进行环境感知和实时避障，直至机器人主体移动到步骤三计算出的目标点；

14、步骤五、重复步骤一至四持续更新全局地图，直至完成对当前环境的自主建图工作。

15、进一步，所述步骤一具体为：

16、步骤1.1：对数据预处理，轮式里程计从驱动轮的编码器获取数据，转换成线速度和角速度；惯性测量单元获取加速度和角速度数据，积分以估计速度和姿态变化；

17、步骤1.2：使用惯性测量单元的监测数据来预测机器人下一时刻的位置、速度和姿态，利用轮式里程计的测量值来校正预测状态；在状态空间模型中对轮式里程计和惯性测量单元各自数据进行融合，其中轮式里程计被视为观测值，更新卡尔曼滤波器的状态估计；

18、步骤1.3：基于贝叶斯推理，结合先验概率(预测)和观测概率(测量)来估计状态向量及其协方差矩阵；持续接收惯性测量单元和轮式里程计数据，不断进行预测和更新步骤，实时校正机器人的状态估计，实现基于运动模型预测和更新机器人主体的位姿。

19、进一步，所述步骤二具体为：

20、步骤2.1：在三维点云数据中提取两种特征点，角点和面点，曲率计算公式为其中k为一次扫描周期，l为三维激光雷达当前坐标系，i表示第k次扫描点云的第i个点，代表位置，第k次扫描的坐标系为l，点云中的i个点在l中的位置，s为点附近指定规则的点，包括前后左右点，0.25度间隔方向的点，为向量范数，刻画向量长度大小的一种度量，默认为2范数，为度量空间，表示两个向量之间的欧氏距离；

21、步骤2.2：基于步骤2.1中的曲率计算公式，筛选特征点，按照c在不同范围的阈值将数据中的点分类，分成曲率特别大的点、曲率较大的点和曲率较小的点；

22、步骤2.3：在分类后的点中选择曲率特别大的点为边缘点，曲率较小的点为平面点；

23、步骤2.4：对连续特征帧进行匹配，建立误差模型，利用点到线的距离和点到面的距离构造误差函数，问题变为求解即第k+1帧点云中的第i个点的变换点到参考点云的所有的点到线和点到面的距离的累加和为求解的问题模型；

24、步骤2.5：加入旋转r和平移t，第k+1帧原始数据点云的第i个点和变化后的点云之间的关系为：

25、步骤2.6：由步骤2.4和步骤2.5推导loss函数为其中为第k+1次扫描点云中第i个点的位置向量，为第k+1次扫描点云中第i个点的位置向量经过旋转和平移后的位置向量；

26、步骤2.7：扫描过程中，用恒定的角速度和线速度对三维激光雷达运动进行建模，在扫描范围内对不同时间接收到的点的姿态变换进行线性插值；

27、步骤2.8：设t为时间戳，记tk+1是扫描k+1的开始时间，设为激光雷达在[tk+1,t]之间进行的姿态变换，包含三维激光雷达在6自由度下的刚体运动，其中tx,ty,tz分别为沿l的x,y,z轴平移，θx,θy,θz为三个对应轴的旋转角，遵循右手法则，给定一个点i,i∈pk+1，设ti为其时间戳，并设为[tk+1,ti]之间的位姿变换，假设雷达为恒定的角速度和线速度运动，则能通过进行线性插值公式计算；

28、步骤2.9：使用非线性优化方法进行求解；

29、步骤2.10：累计一定数目点云数据开始建图，以较低频率将点云数据融入到世界地图中，精确估计激光在世界坐标系中的位姿。

30、进一步，所述步骤三具体为：

31、步骤3.1：对当前二维激光雷达采集的点云数据构造k-d树，建立点云拓扑关系；求点云中任意一点邻域范围内邻近点个数，判断邻近点个数是否小于判断阈值，若小于则认为噪声点并去除，重复上述步骤直至点云中所有的点都处理完毕；

32、步骤3.2：以map坐标系为参考，利用pcl遍历点云数据中的每一个点，并依次进行比较x，y，z三个值大小；

33、步骤3.3：由步骤3.2得出当前局部点云数据x,y,z三个方向上的最大值记为xmax,ymax和zmax，最小值记为xmin,ymin和zmin；

34、步骤3.4：根据步骤3.3得到各坐标轴上的最大及最小值，计算最小包围盒的边长lx,ly,lz，计算公式为

35、步骤3.5：将三个坐标轴方向上的边长划分为m1,m2,m3份，实现将最小包围盒分割成n个体素格，令每个体素格的体积为v＝a·b·c，式中a,b,c为x方向，y方向，z方向体素格的边长；

36、步骤3.6：设点云数据中第p个点的坐标为(xp,yp,zp)，对每个体素格进行标号，令q＝i·4+j+k，其中表示向下取整符号，点(xp,yp,zp)位于第q个体素格中；

37、步骤3.7：遍历点云数据中的每一个点，按步骤3.6将其分配到对应的体素格中，设第q个体素格中的点的总个数为wq，第q个体素格的点云密度为其中v＝a·b·c；

38、步骤3.8：通过与设定的点云密度阈值比较，假设点云密度最小的第q个区域的点云密度未达到阈值要求，则用第q个体素格的中心作为本次局部路径规划的目标点。

39、进一步，所述步骤四具体为：

40、步骤4.1：通过二维激光雷达检测到障碍物的位置，并将其映射到地图中，将障碍物所在的栅格赋予高代价值；

41、步骤4.2：障碍物周围的一定范围会被标记为膨胀区域；利用局部代价地图实时更新障碍物的位置信息；

42、步骤4.3：将全局路径规划器生成的初始轨迹作为输入，将初始运动路线离散为一系列的路径点，为每个路径点分配一个时间分段；

43、步骤4.4：每个路径点由xi＝(xi,yi,θi)t表示，其中xi和yi是路径点的空间位置，ti是时间信息；运动空间内机器人主体所在点的位置姿态坐标序列由q＝{xi}i＝0,…,n,n∈n表示；

44、步骤4.5：相邻两个路径点之间的时间间隔δti＝ti+1-ti，代表机器人在这两个路径点之间的移动时间，时间序列由τ＝{δti}i＝0,…,n,n∈n表示，n表示控制点的个数；

45、步骤4.6：将上述控制点的时间序列和位置序列合并得到b＝(q,τ)，n表示控制点的个数，在时间序列和控制点序列两个方面利用加权的方式进行多目标优化；

46、步骤4.7：通过代价函数最小化路径点之间的角度变化，约束路径点之间的平滑性，其中i＝0,…,n,n∈n，n表示控制点的个数；

47、步骤4.8：通过代价函数最小化路径的总时间，保持时间的均匀分布，其中是路径点之间的平均时间间隔，i＝0,…,n,n∈n，n表示控制点的个数；

48、步骤4.9：通过最小路径点到障碍物的距离代价函数确保路径点远离障碍物，其中d(xi,o)是路径点xi到障碍物的距离，i＝0,…,n,n∈n，n表示控制点的个数；

49、步骤4.10：通过代价函数保持机器人在路径上的速度和加速度满足其运动学限制，其中i＝0,…,n,n∈n，n表示控制点的个数；

50、步骤4.11：利用获得b的最优解，找到最优的路径点位置和时间分布，式中b*表示最优结果，f(b)为综合考虑多种目标的总代价，γk表示每一个优化目标的代价函数的权值，k＝1,2,3,4；

51、步骤4.12：机器人主体按照上述确定的路径、速度及加速度数据进行移动，直至机器人主体移动到步骤三计算出的目标点。

52、与现有技术相比，本发明具有如下优点：

53、1、本发明通过构建场景信息密度的数学模型，得到了信息密度与机器人主体的导航和地图构建性能之间的关系，实现了对未知环境空间点云的稀疏程度的量化，实现移动机器人在未知环境下的自主导航和决策。

54、2、本发明基于点云信息密度确定目标点，并利用三维激光雷达进行未知封闭环境的三维点云地图的构建，基于二维激光雷达在二维栅格地图上进行路径规划和导航，实现移动机器人在未知环境自主建图，能在完全不依靠人工干预的情况下实现在未知封闭环境中全自动路径规划及避障，从而对未知环境进行精确建图，且效率较高。