面向无人清扫车的覆盖保持型分级动态避障方法及系统

本发明涉及无人清扫车，尤其涉及一种面向无人清扫车的覆盖保持型分级动态避障方法及系统。

背景技术：

1、无人清扫车作为一种典型的服务型移动机器人，广泛应用于校园、园区、广场、道路等开放或半开放场景中。无人清扫车在执行清扫任务时，通常需要按照预先规划的全覆盖路径进行往复行驶，以保证目标区域能够被尽可能完整地清扫。

2、在无人清扫车的自主导航过程中，路径规划和动态避障是影响其作业效率、安全性和覆盖完整性的重要因素。现有无人清扫车通常采用全覆盖路径规划方法生成清扫路线，并结合局部避障算法应对行人、自行车、电动车等动态障碍物；其中，动态窗口法（dwa）是移动机器人局部避障中较为常用的方法，该算法通过在速度空间中采样线速度和角速度，并依据评价函数选择最优运动轨迹，具有实时性较强、计算复杂度较低、易于工程实现等优点。

3、然而，传统dwa在无人清扫车全覆盖作业场景中仍存在以下不足：

4、首先，传统dwa仅依赖障碍物距离评估碰撞风险，未考虑障碍物的运动速度和运动趋势，导致对快速接近的远端障碍物响应迟钝，对静止的近端障碍物过度避让，产生不必要的绕行。其次，传统dwa仅以目标点方向为引导，避障过程中大幅偏离预设覆盖路径，导致漏扫区域累积。现有清扫车避障方案虽引入漏扫补偿，但采用事后面积计算方式，在障碍物频繁出现的场景中漏扫积累问题仍然显著。然后，传统dwa对所有障碍物采用统一的绕行策略，对瞬时穿越型障碍物采取绕行会打断覆盖路径的连续性；对长时间停留的阻塞型障碍物，等待策略则会造成任务中断，缺乏分级差异化的响应机制。

5、现有技术虽在预测碰撞时间（ttc）应用和风险分级方面有所探索，但方法大多未将ttc风险分级与全覆盖路径保持机制进行统一建模，难以同时满足动态避障安全性和全覆盖清扫完整性的要求。

技术实现思路

1、本发明的目的在于提供一种面向无人清扫车的覆盖保持型分级动态避障方法及系统，以解决现有技术中风险评估不足、覆盖连续性差、策略响应单一的技术问题，实现无人清扫车在动态环境下的安全避障与高覆盖率作业的有机统一。

2、本发明为解决上述技术问题所采用的技术方案是：一种面向无人清扫车的覆盖保持型分级动态避障方法，包括以下步骤：

3、s1、获取无人清扫车的全覆盖路径、当前位姿状态及运动状态；

4、s2、通过传感器获取周围障碍物的位置、速度、运动方向与类别信息，构建障碍物状态集合；

5、s3、根据s2获取的障碍物信息，计算无人清扫车与各障碍物之间的预测碰撞时间，并依据所述预测碰撞时间与预设阈值的关系将障碍物划分为无风险、低风险、中风险、高风险和阻塞五个风险等级；所述风险等级在障碍物停留时长超过占据时间阈值时触发；

6、s4、依据s3确定的风险等级，生成对应避障策略：

7、无风险等级保持覆盖路径行驶；低风险等级执行减速行驶；中风险等级执行持续降速；高风险等级执行停车或进行动态窗口法局部避障；阻塞等级切换a*绕行模式并同步实现漏扫记录机制；

8、s5、对于低风险、中风险、高风险、阻塞风险等级，以当前预测碰撞时间为驱动参数，动态调整改进动态窗口法评价函数中各项权重；

9、s6、判断当前轨迹是否会导致无人清扫车偏离全覆盖路径，未偏离路径则继续运行；如果偏离全覆盖路径则记录未清扫区域，并在障碍物风险降低或消失后引导无人清扫车回归全覆盖路径或对未清扫区域执行补扫。

10、s7、对于风险等级采用a*算法规划绕行路径，并在绕行期间实时记录所有偏离全覆盖路径的未清扫航迹点至漏扫区域，合并至漏扫队列；

11、s8、判断全覆盖路径及漏扫队列中所有航迹点是否均已清扫完毕，若是全部完成则任务结束，否则返回s2继续执行。

12、进一步地，步骤s5中，所述改进动态窗口法评价函数为：

13、

14、其中，表示方向一致性评价项，表示障碍物距离评价项，表示速度评价项，表示轨迹平滑性评价项；表示全覆盖路径保持项；表示预测碰撞时间风险惩罚项；ttc表示预测碰撞时间；表示各项对应权重，其中均表示以预测碰撞时间为自变量的动态权重函数。

15、更进一步地，全覆盖路径保持项的表达式为：

16、

17、其中，dpath表示候选轨迹末端点到原全覆盖路径的最小距离；

18、预测碰撞时间风险惩罚的表达式为：

19、

20、其中，表示防零除小量。

21、进一步地，步骤s1中，所述全覆盖路径表示为：

22、

23、其中，表示第个覆盖航迹点，表示覆盖航迹点总数；

24、所述当前位姿状态及运动状态表示为：

25、

26、其中，xr和yr为无人清扫车当前位置，θr为航向角，vr为当前线速度，ωr为当前角速度，上标t表示矩阵或向量的转置运算。

27、进一步地，步骤s2中，所述障碍物状态集合表示为：

28、

29、其中，k表示第k个障碍物，和表示该障碍物当前位置坐标，表示该障碍物运动速度，表示该障碍物运动方向，表示该障碍物类别；所述障碍物类别包括行人、机动车、非机动车和临时静态障碍物中的至少一种。

30、进一步地，步骤s3中，所述预测碰撞时间的计算函数为：

31、

32、其中，为第k个障碍物相对于无人清扫车的位置向量，为第k个障碍物相对于无人清扫车的速度向量。

33、更进一步地，当则说明两者距离正在减小，存在潜在碰撞风险；当则说明两者距离正在增大或不再接近，认为无即时碰撞风险；当存在多个动态障碍物时，取最小预测碰撞时间作为当前时刻的风险判断依据，其表达式为：

34、

35、其中，表示当前检测到的动态障碍物数量。

36、进一步地，步骤s6中，当无人清扫车执行绕行动作并偏离原全覆盖路径时，将被跳过的覆盖路径段记录至漏扫队列：

37、

38、其中，表示漏扫区域对应的覆盖航迹点集合；

39、记录漏扫区域后，在原全覆盖路径中搜索距离无人清扫车当前位置最近且尚未清扫的覆盖航迹点作为回归点，其表达式为：

40、

41、其中，表示尚未完成清扫的覆盖航迹点集合，pi表示全覆盖路径中第个覆盖航迹点，表示无人清扫车当前位置；

42、确定回归点后，根据无人清扫车当前位置与回归点之间的可通行区域生成回归路径：

43、

44、其中，表示回归路径中的第个路径点，无人清扫车沿回归路径返回原全覆盖路径；

45、当漏扫队列不为空时，无人清扫车根据漏扫航迹点生成补扫路径：

46、

47、其中，表示补扫路径，表示补扫路径中的第个补扫路径点。

48、本发明还提出一种面向无人清扫车的覆盖保持型分级动态避障系统，包括：

49、信息获取模块，用于获取无人清扫车的全覆盖路径、当前位姿状态及运动状态，以及通过传感器获取周围障碍物的位置、速度、运动方向与类别信息；

50、风险分级模块，用于根据障碍物信息计算无人清扫车与各障碍物之间的预测碰撞时间，并依据预测碰撞时间与预设阈值的关系将障碍物划分为无风险、低风险、中风险、高风险和阻塞五个风险等级；

51、策略生成模块，用于依据风险等级生成对应避障策略，所述策略包括保持覆盖路径、减速行驶、持续降速、停车或动态窗口法局部避障以及a*绕行；

52、轨迹规划模块，用于以预测碰撞时间为驱动参数，动态调整改进动态窗口法评价函数中的各项权重，所述改进动态窗口法评价函数中包含覆盖路径保持项和预测碰撞时间风险惩罚项，并通过动态窗口采样生成候选轨迹并输出局部避障轨迹；

53、漏扫补偿模块，用于判断当前轨迹是否会导致无人清扫车偏离全覆盖路径，在偏离时记录未清扫区域，并在障碍物风险降低或消失后引导无人清扫车回归全覆盖路径或对未清扫区域执行补扫；

54、任务判断模块，用于判断全覆盖路径及漏扫队列中所有航迹点是否均已清扫完毕，若是则结束任务，否则继续执行避障清扫流程。

55、根据上述技术方案，本发明的有益效果是：

56、1.本发明通过引入ttc对动态障碍物进行风险分级，综合考虑障碍物与无人清扫车之间的相对位置、相对速度和运动趋势，相较于仅依赖障碍物距离的传统dwa，风险判断更加准确，能够避免无谓绕行，提高无人清扫车的作业效率。

57、2.本发明根据不同风险等级采取差异化避障策略，包括保持覆盖路径、减速行驶、停车等待、局部绕行、a*绕行等方式，使无人清扫车在保证安全性的同时，充分利用动态障碍物的主动避让行为，降低路径偏移和停顿次数。

58、3.本发明在dwa评价函数中引入覆盖路径保持项，使无人清扫车在低风险或中风险情况下优先保持原全覆盖清扫路径，减少因局部避障造成的轨迹偏离，从而提高全覆盖路径的连续性和清扫完整性。

59、4.本发明在dwa评价函数中引入ttc风险惩罚项，并结合动态权重调整，使无人清扫车能够根据障碍物风险程度自适应调整运动速度和避障轨迹，提高动态场景下的安全性和鲁棒性。

60、5.本发明设计了漏扫区域记录、回归点生成和补扫机制，当无人清扫车因绕行偏离原覆盖路径时，能够记录未清扫区域，并在障碍物风险降低或消失后自动回归覆盖路径或执行补扫任务，从而降低漏扫率，提高覆盖率。

61、6.本发明适用于校园道路、园区道路、广场、人行通道等存在行人、自行车、电动车等动态障碍物的无人清扫场景，具有较强的实用性和推广价值。