动态障碍物轨迹预测方法、移动机器人轨迹规划方法、设备、介质及产品与流程

本技术涉及机器人导航，特别是涉及一种动态障碍物轨迹预测方法、移动机器人轨迹规划方法、设备、介质及产品。

背景技术：

1、近年来，移动机器人在商场导购、医院物资配送、智能制造车间物料转运等民生与工业场景的应用日益广泛。此类场景的典型特征在于，环境中既包含墙壁、固定货架、生产设备基座等位置长期不变的静态障碍物，又存在行人、自动导引车等运动状态具有显著不确定性的动态障碍物。并且这些动态障碍物并非孤立运动，在交互过程中（如行人之间的避让、搬运设备的路径交错），常出现临时变向、速度调整等不可预测的行为。这种复杂环境对移动机器人“精准预判动态障碍物运动趋势、规划出安全且连贯的导航轨迹”的核心能力提出了严峻挑战。

2、现有技术中，已有多种方案尝试解决上述问题。例如，清华大学jian等人开发了一种基于三维激光雷达的移动机器人避障方法，采用dbscan密度聚类算法对障碍物区域聚类，并将每个障碍物参数化为最小边界椭圆，基于卡尔曼滤波估计当前状态与方差，并外推未来n个时间步的障碍物状态。局部规划环节扩展传统控制屏障函数得到动态控制屏障函数，将安全集作为约束融入模型预测控制，输出速度指令控制机器人运动。又如，韩国庆北国立大学zhou和亚洲大学lee等人开发了一种基于优化的轨迹预测方法，采用带速度损失项的多项式拟合方法，通过二次规划求解带速度/加速度约束的优化问题以外推未来轨迹，并将预测位置回代卡尔曼滤波，同时随预测步数增加与障碍物速度增大动态扩展危险区域不确定性范围。再如，美国卡耐基梅隆大学zhang等人开发了一种基于概率似然最大化的局部路径规划方法，离线预生成轨迹库并建立体素与轨迹的邻接表，在线规划时仅需基于激光雷达感知数据快速标记遮挡轨迹，计算各组轨迹到达目标的概率似然，选择似然最高的轨迹组作为最终轨迹。

3、然而，上述现有技术仍存在以下不足：

4、第一，在动态障碍物轨迹预测环节，现有技术通常基于单一模型（如卡尔曼滤波、多项式拟合）和障碍物历史信息外推未来轨迹，忽略了动态障碍物之间的交互影响（如行人避让、多障碍物交错）。行人和agv等动态障碍物的运动往往受到周围其他移动物体的影响，例如迎面走来的行人会相互避让、多台agv在交叉口会相互等待。现有方法仅将每个障碍物独立建模，难以捕捉这种交互行为对轨迹的修正作用，当障碍物实际运动与模型假设不符时，预测性能显著降低，导致预测偏差大。

5、第二，现有轨迹预测方法通常仅输出单一预测轨迹，无法覆盖动态障碍物临时变向、速度骤变等行为不确定性。在实际场景中，行人可能突然改变方向、agv可能因临时任务调整速度，这些不确定性难以通过单一确定性轨迹表达。当机器人的局部规划基于单一的、可能错误的预测轨迹进行避障时，极易引发碰撞或导致机器人陷入频繁急停、重新规划的窘境，导航连贯性差。

6、第三，基于离线轨迹库的局部路径规划方法虽然在线计算效率高，但现有方案仅从空间维度删除与静态障碍物碰撞的轨迹，缺乏对时间维度的考量。例如，zhang等人的方法仅基于体素遮挡标记轨迹，未考虑机器人与动态障碍物在连续时间上的相对运动关系。当环境中存在多个快速运动的动态障碍物时，一条在空间上不与静态障碍物重叠的轨迹，仍可能在某个未来时刻与动态障碍物的预测位置发生碰撞。现有技术缺乏时空联合检测机制，导致避障不及时，安全隐患大。

7、第四，现有技术中轨迹预测与局部规划相互割裂，预测模块输出的单条轨迹难以被规划模块有效利用以进行时空协同避障。即使部分方法尝试将预测结果作为规划约束，也往往因预测不确定性大、规划计算量大而难以实现在线实时应用。特别是当动态障碍物数量增多时，基于优化的规划方法计算量激增，易出现规划失败；而离线轨迹库方法虽效率高，却缺少将动态障碍物预测轨迹与机器人候选轨迹进行时空匹配的机制，无法提前规避潜在碰撞。

技术实现思路

1、本技术的目的是提供一种动态环境中移动机器人轨迹预测方法、轨迹规划方法、设备、介质及产品，可实现将多动态障碍物间的避让、交错等交互行为量化为可计算的速度采样约束，为覆盖行为不确定性提供了合理的采样空间；实现在多障碍物交互场景下生成多条覆盖不确定性的候选轨迹，并通过偏离度与安全距离的双维度加权评分筛选出最贴合真实运动趋势的预测轨迹，显著提升了轨迹预测的鲁棒性和准确性；实现对静态障碍物的快速空间碰撞检测，结合离线预计算的邻接表结构大幅提升了在线规划效率；实现通过时间邻接表和时间尺度适配机制，精确检测机器人与动态障碍物在连续时间上的潜在碰撞，并在保证规划效率的同时输出既安全又贴近全局路径的局部轨迹，提升了动态环境下移动机器人的导航连贯性与避障成功率。

2、为实现上述目的，本技术提供了如下方案：

3、第一方面，本技术提供了一种动态障碍物轨迹预测方法，包括：

4、获取动态环境中动态障碍物的运动状态信息以及静态障碍物位置，所述运动状态信息至少包括动态障碍物的当前位置和当前速度矢量；

5、基于所述动态障碍物的运动状态信息，通过速度障碍法rvo构建动态障碍物之间的交互约束区域，并根据所述交互约束区域确定动态障碍物的速度采样范围；

6、在所述速度采样范围内进行速度采样，得到多个采样速度值，并以所述当前位置为起点，根据每个采样速度值计算动态障碍物的未来位置，生成多条候选预测轨迹；

7、基于所述当前速度矢量与采样速度值之间的偏离度，以及所述候选预测轨迹上各预测位置与静态障碍物位置之间的安全距离，计算每条候选预测轨迹的综合评分，并将综合评分最高的轨迹作为动态障碍物的最终预测轨迹。

8、可选地，所述通过速度障碍法rvo构建动态障碍物之间的交互约束区域，具体包括：

9、将第一动态障碍物的最小碰撞圆半径与第二动态障碍物的最小碰撞圆半径相加，并以相加之和为半径构建扩展碰撞圆；

10、以第一动态障碍物为参考系，将第二动态障碍物的速度矢量平移至第一动态障碍物处，并取两个速度矢量终点的连线中点作为交互约束区域的顶点；

11、以所述扩展碰撞圆的圆心为参考点，生成两条与所述扩展碰撞圆相切的侧边，并结合时间阈值形成三角形区域；

12、将所述三角形区域平移至所述顶点处，得到最终的交互约束区域。

13、可选地，所述基于当前速度矢量与采样速度值之间的偏离度，以及所述候选预测轨迹上各预测位置与静态障碍物位置之间的距离，计算每条候选预测轨迹的综合评分，具体包括：

14、计算当前速度矢量与采样速度值之间的偏离度得分，计算公式为：

15、；

16、式中，表示偏离度得分，表示当前速度矢量，表示采样速度值，表示衰减系数，表示方向偏离权重因子，表示速度大小偏离权重因子，表示极小正数；

17、计算所述候选预测轨迹上各预测位置与静态障碍物位置之间的安全距离得分，计算公式为：

18、；

19、式中，表示安全距离得分，表示总时间步数，表示时间步索引，表示第个时间步，动态障碍物的预测位置与最近静态障碍物位置之间的欧氏距离，表示最小碰撞圆半径，表示预设的最大安全距离阈值，表示取最大值函数；

20、计算每条候选预测轨迹的综合评分，计算公式为：

21、；

22、式中，表示综合评分，表示用于平衡速度连续性与避障安全性的权重系数。

23、第二方面，本技术提供了一种移动机器人轨迹规划方法，包括：

24、获取第一方面所述的动态障碍物轨迹预测方法输出的动态障碍物的最终预测轨迹，以及机器人的当前速度矢量和静态障碍物的位置信息；

25、加载预先构建的离线轨迹库，所述离线轨迹库基于体素网格构建，且包括多条候选机器人轨迹，并为每个体素网格建立位置邻接表，以及为机器人以基准速度运动时的每个时间步建立时间邻接表；

26、基于所述静态障碍物的位置信息查询所述位置邻接表，删除所有与静态障碍物占据的体素网格相关联的候选机器人轨迹；

27、根据所述机器人的当前速度矢量和所述基准速度计算时间尺度；

28、基于所述时间尺度，将所述动态障碍物的最终预测轨迹在各时间步的预测位置映射到所述时间邻接表的对应时间步，并进行匹配判定；

29、当匹配成功时，删除体素网格在所述位置邻接表中对应的候选机器人轨迹；

30、将剩余的候选机器人轨迹划分为多个轨迹组，并计算每个轨迹组的综合得分，选择综合得分最高的轨迹组作为局部规划轨迹。

31、可选地，所述移动机器人轨迹规划方法在每个控制周期内周期性执行，将当前控制周期输出的局部规划轨迹用于驱动机器人移动后，在下一个控制周期返回获取动态障碍物的最终预测轨迹的步骤，形成循环往复的轨迹规划与执行闭环。

32、可选地，所述根据机器人的当前速度矢量和所述基准速度计算时间尺度，具体包括：

33、将所述机器人的当前速度矢量与所述基准速度的比值作为时间尺度，公式表达为：

34、；

35、式中，表示时间尺度，表示机器人的当前速度矢量，表示基准速度。

36、可选地，所述计算每个轨迹组的综合得分，具体包括：

37、将同一个轨迹组内所有单条轨迹的得分相加，以总和作为该轨迹组的综合得分，公式表达为：

38、；

39、式中，表示轨迹组的综合得分，表示轨迹组内包含的候选机器人轨迹总数，表示权重系数，表示第n条轨迹末端点与全局路径目标点之间的欧氏距离。

40、第三方面，本技术提供了一种计算机设备，包括：存储器、处理器以存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序以实现上述中任一项所述的动态障碍物轨迹预测方法及移动机器人轨迹规划方法的步骤。

41、第四方面，本技术提供了一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现上述中任一项所述的动态障碍物轨迹预测方法及移动机器人轨迹规划方法的步骤。

42、第五方面，本技术提供了一种计算机程序产品，包括计算机程序，该计算机程序被处理器执行时实现上述中任一项所述的动态障碍物轨迹预测方法及移动机器人轨迹规划方法的步骤。

43、根据本技术提供的具体实施例，本技术公开了以下技术效果：

44、本技术提供了一种动态障碍物轨迹预测方法、移动机器人轨迹规划方法、设备、介质及产品，通过步骤“基于动态障碍物的运动状态信息，通过速度障碍法rvo构建动态障碍物之间的交互约束区域，并根据交互约束区域确定动态障碍物的速度采样范围”，解决了现有技术中忽略动态障碍物之间交互影响、仅依赖单一固定模型导致轨迹预测偏差大的问题，实现了将多动态障碍物间的避让、交错等交互行为量化为可计算的速度采样约束，为覆盖行为不确定性提供了合理的采样空间；通过步骤“在速度采样范围内进行速度采样，得到多个采样速度值，并生成多条候选预测轨迹”以及“基于当前速度矢量与采样速度值之间的偏离度，以及候选预测轨迹上各预测位置与静态障碍物位置之间的安全距离，计算每条候选预测轨迹的综合评分，并将综合评分最高的轨迹作为动态障碍物的最终预测轨迹”，解决了单一预测轨迹无法覆盖动态障碍物临时变向、速度骤变等行为不确定性的问题，实现了在多障碍物交互场景下生成多条覆盖不确定性的候选轨迹，并通过偏离度与安全距离的双维度加权评分筛选出最贴合真实运动趋势的预测轨迹，显著提升了轨迹预测的鲁棒性和准确性；通过步骤“加载预先构建的离线轨迹库，离线轨迹库基于体素网格构建，且包括多条候选机器人轨迹，并为每个体素网格建立位置邻接表，以及为机器人以基准速度运动时的每个时间步建立时间邻接表”以及“基于静态障碍物的位置信息查询位置邻接表，删除所有与静态障碍物占据的体素网格相关联的候选机器人轨迹”，解决了现有离线轨迹库方法仅从空间维度删除碰撞轨迹、无法应对动态障碍物的问题，实现了对静态障碍物的快速空间碰撞检测，结合离线预计算的邻接表结构大幅提升了在线规划效率；通过步骤“根据机器人的当前速度矢量和基准速度计算时间尺度；基于时间尺度，将动态障碍物的最终预测轨迹在各时间步的预测位置映射到时间邻接表的对应时间步，并进行匹配判定；当匹配成功时，删除体素网格在位置邻接表中对应的候选机器人轨迹”以及“将剩余的候选机器人轨迹划分为多个轨迹组，并计算每个轨迹组的综合得分，选择综合得分最高的轨迹组作为局部规划轨迹”，解决了现有技术中局部轨迹规划缺乏时空协同、无法提前规避动态障碍物未来位置导致避障不及时的问题，实现了通过时间邻接表和时间尺度适配机制，精确检测机器人与动态障碍物在连续时间上的潜在碰撞，并在保证规划效率的同时输出既安全又贴近全局路径的局部轨迹，提升了动态环境下移动机器人的导航连贯性与避障成功率。