一种基于降维映射的泳池清洁机器人全覆盖路径规划方法及系统与流程

本技术涉及路径规划与自动控制，特别是涉及一种基于降维映射的泳池清洁机器人全覆盖路径规划方法及系统。

背景技术：

1、泳池清洁机器人通常需要在池底、池壁、台阶、斜坡等三维内表面上执行清洁作业。与仅在单一二维地面上运行的扫地机器人不同，泳池清洁机器人面对的是由多个表面相互连接形成的三维工作环境。该类环境既要求机器人覆盖所有有效清洁表面，又要求机器人尽量减少转弯、停顿、跨面重定位和无效转移，以降低水下运动能耗并延长单次作业续航。

2、现有技术中，常见路径规划方式包括以下几类。

3、第一类方式是分面独立规划。该方式通常将泳池三维内表面拆分为池底、前壁、后壁、左壁、右壁等多个独立二维地图，并分别生成覆盖路径。机器人在池底与池壁交界处需要暂停当前任务，切换局部地图和姿态控制模式，再进入另一表面继续作业。该方式破坏了实际物理表面的拓扑连通关系，容易在底面与侧壁交界处以及多个面交汇的角落处产生漏扫、重复清扫或控制死锁。

4、第二类方式是在三维点云、三维网格、体素地图或八叉树地图中直接执行三维路径搜索和覆盖规划。该方式需要处理大量三维空间节点，并可能涉及复杂的三维测地线计算，计算量和存储量较大，不适合算力和功耗受限的泳池清洁机器人实时重规划。

5、第三类方式是在离散栅格地图中使用a星（a*）、迪杰斯特拉（dijkstra）等算法生成跨区域转移路径。该方式受栅格分辨率和栅格方向约束，生成的路径容易呈阶梯状或锯齿状。泳池机器人在水下沿锯齿路径运动时需要频繁进行差速转向，导致非工作能耗增加。

技术实现思路

1、基于此，有必要针对上述技术问题，提供一种能够保持泳池三维内表面拓扑连通关系、降低路径规划计算量并减少水下无效转向的全覆盖路径规划方法和系统。

2、第一方面，本技术提供了一种基于降维映射的泳池清洁机器人全覆盖路径规划方法。方法包括：

3、获取泳池清洁机器人的工作环境三维感知数据，并基于三维感知数据识别泳池内表面的底面流形、侧壁流形以及相邻流形之间的交线；

4、根据底面流形、侧壁流形以及交线，确定底面多边形轮廓、侧壁尺寸和侧壁结构倾角；

5、以底面多边形轮廓所在平面为基准面，计算底面多边形轮廓的边界外向法向量，并将与对应边界相邻的侧壁流形沿边界外向法向量等距展开至基准面，生成全局物理连通的二维展开地图；

6、在二维展开地图中确定扫掠方向，并基于扫掠方向对二维展开地图进行细胞分解，得到多个待覆盖子区域；

7、在各待覆盖子区域内生成往复覆盖路径，并在相邻待覆盖子区域之间生成无碰撞转移路径，从而得到二维全覆盖路径；

8、将二维全覆盖路径中的路径点逆向映射至泳池内表面的三维物理表面，生成供泳池清洁机器人执行的三维全覆盖路径。

9、在其中一个实施例中，获取泳池清洁机器人的工作环境三维感知数据，并基于三维感知数据识别泳池内表面的底面流形、侧壁流形以及相邻流形之间的交线，包括：

10、通过设置于泳池清洁机器人上的三维感知系统对泳池内表面进行扫描，以获取高密度点云数据，其中，三维感知系统包括三维激光雷达、rgb-d深度相机、超声波阵列传感器中的一种或多种；

11、对高密度点云数据进行滤波、下采样和离群点去除；

12、对处理后的点云数据进行点云分割，得到多个候选表面点云集合；

13、对各候选表面点云集合执行随机抽样一致平面拟合，得到底面流形方程和多个侧壁流形方程；

14、根据底面流形方程与各侧壁流形方程之间的交线，确定相邻流形交线的空间非共线向量。

15、在其中一个实施例中，根据底面流形、侧壁流形以及交线，确定底面多边形轮廓、侧壁尺寸和侧壁结构倾角，包括：

16、根据多个交线在底面流形上的投影和交点关系，解算底面多边形轮廓的顶点序列；

17、根据各侧壁流形相对于底面流形的法向夹角，确定对应侧壁的结构倾角；

18、根据交线长度、侧壁边界点云范围和侧壁流形法向，确定对应侧壁的宽度、高度或有效清洁区域尺寸。

19、在其中一个实施例中，将与对应边界相邻的侧壁流形沿边界外向法向量等距展开至基准面，包括：

20、对于底面多边形轮廓中的任一边界线段，确定该边界线段的单位切向量和二维外向法向量；

21、将连接该边界线段的侧壁流形上的点，以沿边界线段方向的距离作为第一展开坐标，以该点相对于边界线段并沿侧壁流形延伸方向的表面距离作为第二展开坐标；

22、将第二展开坐标沿二维外向法向量方向布置于基准面，以保持底面与侧壁在边界线段处的拓扑连通关系。

23、在其中一个实施例中，二维展开地图为包含底面展开区域和多个侧壁展开区域的二维非凸多边形地图，底面展开区域与多个侧壁展开区域在对应边界处连续拼接；

24、在二维展开地图中确定扫掠方向，包括：

25、提取二维展开地图的边界线段，并将边界线段的平行方向作为候选扫掠方向集；

26、对于任一候选扫掠方向，计算二维展开地图的顶点集合在该候选扫掠方向的正交法向量上的投影高度；

27、从候选扫掠方向集中选择使投影高度最小的方向作为最优扫掠方向。

28、在其中一个实施例中，投影高度按照如下方式确定：

29、设候选扫掠方向角为θ，其正交法向量为nθ=(-sinθ,cosθ)，二维展开地图的顶点集合为v，则投影高度a(θ)满足：

30、a(θ)=max(p·nθ)-min(p·nθ)，p属于v；

31、选择使a(θ)最小的候选扫掠方向作为最优扫掠方向。

32、在其中一个实施例中，基于扫掠方向对二维展开地图进行细胞分解，包括：

33、沿扫掠方向的正交方向推进扫描线；

34、当扫描线经过二维展开地图的顶点时，根据该顶点与相邻两个顶点形成的向量叉积判断该顶点的凹凸性；

35、结合扫描线经过该顶点前后的可行区域横截面数量变化，识别拓扑分裂事件或拓扑合并事件；

36、基于拓扑分裂事件或拓扑合并事件，将二维展开地图分解为多个待覆盖子区域。

37、其中，向量叉积按照如下方式确定：

38、对于连续三个顶点 p{i-1}、pi、p{i+1}，计算：

39、δ=(pi-p{i-1})×(p{i+1}-pi)=(xi-x{i-1})*(y{i+1}-yi)-(x{i+1}-x_i)*(yi-y{i-1})；

40、当δ满足预设凹点判定条件且扫描线经过顶点 pi 后可行区域横截面增加时，判定为拓扑分裂事件；

41、当δ满足预设凹点判定条件且扫描线经过顶点 pi  后可行区域横截面减少时，判定为拓扑合并事件。

42、在其中一个实施例中，在各待覆盖子区域内生成往复覆盖路径，包括：

43、根据泳池清洁机器人的清扫幅宽、路径重叠率和边界安全裕量确定扫掠间距；

44、在各待覆盖子区域内按照扫掠方向和扫掠间距生成多条平行扫描线；

45、计算各平行扫描线与对应待覆盖子区域边界的交点，并截取位于待覆盖子区域内部的线段作为覆盖线段；

46、按照相邻覆盖线段首尾交替连接的方式生成往复覆盖路径；

47、其中，设扫描线参数方程为l(t)=p0+t*d，边界线段参数方程为 s(u)=q0+u*(q1-q0)，通过叉积方程求解交点参数u：

48、u=[(p0-q0)×d]/[(q1-q0)×d]；

49、当分母非零且0≤u≤1时，确定扫描线与边界线段存在合法交点。

50、在其中一个实施例中，在相邻待覆盖子区域之间生成无碰撞转移路径，包括：

51、提取二维展开地图的外边界凹顶点和内部障碍物凸顶点，构建候选节点集合；

52、对候选节点集合中的节点对进行视距无碰撞检测；

53、在通过视距无碰撞检测的节点对之间建立图边，以构建可见性图；

54、在可见性图中以欧几里得距离作为边代价和启发式函数执行路径搜索，得到由直线段构成的跨区域转移路径；

55、将二维全覆盖路径中的路径点逆向映射至泳池内表面的三维物理表面，包括：

56、根据路径点在二维展开地图中的位置，确定该路径点对应的底面展开区域或侧壁展开区域；

57、调用与底面展开区域或侧壁展开区域对应的齐次刚体变换矩阵；

58、将路径点的二维坐标扩展为齐次坐标，并通过齐次刚体变换矩阵转换为三维全局坐标；

59、在泳池清洁机器人执行三维全覆盖路径时，实时获取惯性测量单元输出的重力矢量和姿态角，并在跨越底面与侧壁交界或侧壁与侧壁交界时，根据重力矢量和姿态角对机器人底盘的运动控制量进行姿态补偿。

60、第二方面，本技术还提供了一种基于降维映射的泳池清洁机器人全覆盖路径规划系统，包括：

61、三维感知模块，用于获取泳池清洁机器人的工作环境三维感知数据；

62、流形识别模块，用于基于三维感知数据识别泳池内表面的底面流形、侧壁流形以及相邻流形之间的交线，并确定底面多边形轮廓、侧壁尺寸和侧壁结构倾角；

63、降维展开模块，用于以底面多边形轮廓所在平面为基准面，将与底面边界相邻的侧壁流形沿边界外向法向量等距展开至基准面，生成全局物理连通的二维展开地图；

64、覆盖规划模块，用于在二维展开地图中确定扫掠方向，对二维展开地图进行细胞分解，并生成待覆盖子区域内的往复覆盖路径；

65、转移规划模块，用于在相邻待覆盖子区域之间生成无碰撞转移路径；

66、逆映射执行模块，用于将二维全覆盖路径逆向映射至泳池内表面的三维物理表面，生成三维全覆盖路径。

67、与现有技术相比，本发明至少具有以下有益效果。

68、第一，本发明通过将泳池底面和侧壁展开为全局物理连通的二维展开地图，使机器人能够在同一规划空间内处理池底、池壁以及跨面交界区域，减少多面独立规划导致的漏扫和死锁。

69、第二，本发明将三维表面覆盖问题转换为二维几何规划问题，避免直接在三维体素或三维网格中进行高复杂度搜索，降低路径规划计算量，更适合嵌入式控制器实时运行。

70、第三，本发明通过投影高度最小化确定扫掠方向，并在子区域内生成往复覆盖路径，可减少覆盖线数量和掉头次数，提高单位时间清洁效率。

71、第四，本发明基于可见性图生成跨区域转移路径，使转移路径由连续直线段构成，减少离散栅格路径带来的锯齿转向，从而降低水下非清洁运动能耗。

72、第五，本发明通过齐次刚体变换矩阵和姿态补偿将二维规划结果映射回三维表面执行，增强了路径规划与机器人实际运动控制之间的一致性。