一种露天矿山无人驾驶矿卡规划方法及系统与流程

摘要: 本发明涉及无人驾驶,特别涉及一种露天矿山无人驾驶矿卡规划方法及系统。背景技术:1、随着矿山智能化建设的不断推进,露天矿山无人驾驶技术已成为降低人工成本、提高作业...
一种露天矿山无人驾驶矿卡规划方法及系统与流程

本发明涉及无人驾驶,特别涉及一种露天矿山无人驾驶矿卡规划方法及系统。

背景技术:

1、随着矿山智能化建设的不断推进,露天矿山无人驾驶技术已成为降低人工成本、提高作业安全和生产效率的重要发展方向。在无人驾驶矿卡系统中,轨迹规划模块是连接调度决策与底层控制的桥梁,其规划质量直接决定了矿卡行驶的安全性、平顺性以及作业效率。

2、露天矿山的作业场景具有高度非结构化、环境复杂多变的特点,矿卡的典型作业循环通常包括停车场出车、驶向装载区、等待装载点分配、进入装载点、装载完成后驶向排土区、进入排土点、排土完成后返回装载区、进入停车场或进入充换电站等多个连续阶段。在此过程中,车辆需要根据调度指令、车辆定位、车辆状态、作业流程状态、路网数据和环境约束生成可供控制模块跟踪的行驶轨迹。不同阶段(如空载行驶、重载行驶、泊车对位、倒车入位)对规划目标、速度约束、档位要求、终点姿态要求和异常处理方式存在明显差异。尤其在装载区和卸载区等复杂终端场景下,矿卡需要与电铲、推土机等设备进行精确的空间对位配合,对轨迹的安全性、精确性和连续性要求极为严苛。

3、然而,现有方法不能保障露天矿山无人驾驶矿卡在复杂终端场景下的作业安全性与连续性,主要体现在以下方面:

4、(1)调度与规划脱节,导致复杂终端场景下的规划目标与调度意图不一致。

5、现有的方法往往将业务状态机、全局路径搜索、轨迹生成分散在不同模块中独立实现,缺乏统一的任务输入架构。在新增业务状态或恢复中断任务时,常出现状态与路径信息传递不完整、字段遗漏、状态跳转错误或规划目标不清晰等问题。不同业务阶段对应着特定的速度约束、档位约束和终点姿态要求,而现有系统缺乏将这些业务约束与规划任务进行统一封装和传递的机制。这导致在装载区、卸载区等复杂终端场景下,规划目标往往偏离调度意图,矿卡无法准确执行对位操作,严重影响作业安全性与连续性。

6、(2)常规轨迹生成在复杂终端场景下适应能力不足,动态增强规划与主链路耦合度低,导致轨迹生成中断或偏离作业位。

7、在轨迹生成层面,常规方法多采用固定算法在全局路径上截取窗口来生成轨迹。然而,露天矿山的装载区和卸载区空间狭窄,需与电铲、推土机等设备进行精确的空间对位配合。传统的固定窗口截取方式无法根据车辆状态、障碍物信息及严格的终点姿态约束灵活生成轨迹。当遇到突发障碍物、复杂地形或需满足严苛终点位姿要求时,常规轨迹往往无法生成可行解,或导致矿卡偏离作业位,甚至触发安全急停,严重破坏作业连续性。

8、此外,虽然现有技术引入了dubins曲线、hybrid a、reeds-shepp曲线等算法处理复杂场景,但这些动态增强规划模块常以独立形式存在。一旦常规轨迹生成在复杂终端场景下失效,动态增强规划无法及时、无缝地接替,系统容易陷入无轨迹可循的停滞状态。上述轨迹生成适应能力不足与动态增强规划耦合度低的问题相互叠加,共同导致现有方法难以保障矿卡在复杂终端场景下的作业安全性与连续性。

9、综上所述,现有的无人驾驶矿卡规划方法,难以保障露天矿山无人驾驶矿卡在复杂终端场景下的作业安全性与连续性。

技术实现思路

1、本发明所要解决的技术问题是针对现有技术的上述不足,提出一种露天矿山无人驾驶矿卡规划方法及系统。该方法能够保障露天矿山无人驾驶矿卡在复杂终端场景下的作业安全性与连续性。

2、第一方面,本发明提供了一种露天矿山无人驾驶矿卡规划方法,该方法包括以下步骤:

3、基于包括主状态层和子状态层的双层业务状态机,根据矿山调度平台的任务指令、到达事件及作业完成事件,确定车辆当前的主状态和与主状态对应的子状态;

4、根据主状态和子状态的组合确定业务类型,并根据业务类型匹配速度约束和档位约束;结合车辆位姿、任务起点及任务终点,构建包括状态字段、目标字段、车辆字段、业务字段、运动约束字段及控制标志字段的标准任务上下文;

5、根据标准任务上下文,确定规划起点及规划终点,并结合与矿区作业场景对应的业务路径策略、速度约束和档位约束,生成全局路径;

6、根据车辆位姿在全局路径上截取局部窗口生成常规局部轨迹;

7、根据常规局部轨迹,确定统一轨迹点序列:若常规局部轨迹满足预设可用性条件,则将常规局部轨迹转换为统一轨迹点序列;若常规局部轨迹不满足预设可用性条件,则根据车辆位姿、目标位姿、障碍物、车辆运动学约束和装卸区对位所需的终点姿态约束,生成动态增强轨迹,并将动态增强轨迹转换为统一轨迹点序列;

8、将统一轨迹点序列输出,以完成露天矿山无人驾驶矿卡规划。

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、若存在任一检查项的结果不满足对应的预设安全标准,则判定安全检查不通过,生成包含故障类型的检查结果,阻断当前轨迹的发布,并将检查结果发送至异常恢复层以触发对应的异常恢复策略;

40、将统一轨迹点序列输出,具体包括:在安全检查通过后,将统一轨迹点序列输出至车辆控制单元。

41、进一步地,异常恢复策略,具体包括:

42、根据预设的恢复状态编码重建主状态和子状态;

43、基于重建后的主状态和子状态重新生成标准任务上下文,并执行保持已发布的有效轨迹、重规划、降级或安全停车操作,以形成断点恢复闭环。

44、第二方面,本发明提供一种露天矿山无人驾驶矿卡规划系统,该系统包括:

45、状态确定模块,用于基于包括主状态层和子状态层的双层业务状态机,根据矿山调度平台的任务指令、到达事件及作业完成事件,确定车辆当前的主状态和与主状态对应的子状态;

46、任务封装模块,与状态确定模块连接,用于根据主状态和子状态的组合确定业务类型,并根据业务类型匹配速度约束和档位约束;结合车辆位姿、任务起点及任务终点,构建包括状态字段、目标字段、车辆字段、业务字段、运动约束字段及控制标志字段的标准任务上下文;

47、全局规划模块,与任务封装模块连接,用于根据标准任务上下文确定规划起点及规划终点,并结合与矿区作业场景对应的业务路径策略、速度约束和档位约束,生成全局路径;

48、局部生成模块,与全局规划模块连接,用于根据车辆位姿在全局路径上截取局部窗口生成常规局部轨迹;

49、动态规划模块,与局部生成模块连接,用于在局部轨迹生成模块生成的常规局部轨迹不满足预设可用性条件时,根据车辆位姿、目标位姿、障碍物、车辆运动学约束和装卸区对位所需的终点姿态约束,生成动态增强轨迹;

50、轨迹输出模块,与局部生成模块和动态规划模块分别连接,用于在常规局部轨迹满足预设可用性条件时,将常规局部轨迹转换为统一轨迹点序列;以及在常规局部轨迹不满足预设可用性条件时,将动态增强轨迹转换为统一轨迹点序列并输出;以及,用于将统一轨迹点序列输出,以完成露天矿山无人驾驶矿卡规划。

51、进一步地,该系统还包括:

52、安全检查模块,与轨迹输出模块连接,用于在输出统一轨迹点序列之前,执行轨迹点数量与间距连续性检查、航向角异常突变检查、曲率阈值检查、速度跳变检查、速度曲率匹配检查、作业区域边界检查、障碍物冲突检查、定位状态有效性检查及轨迹发布频率检查;若每一项检查项均满足对应的预设安全标准,则生成发布许可标志;若任意一项检查项不满足对应的预设安全标准,则生成包含故障类型的检查结果,并禁止轨迹发布;

53、异常恢复模块,与安全检查模块连接,用于在接收到未通过安全检查的检查结果时,触发异常恢复策略。

54、本发明通过基于双层业务状态机构建标准任务上下文统一传递调度意图与运动约束,并在常规局部轨迹不满足可用性条件时触发动态增强轨迹进行多级容错与统一输出,能够保障复杂终端场景下规划目标与调度意图的一致性,提升轨迹连续性及下行控制执行的安全稳定性。具体的有益效果如下:

55、1)本发明基于主状态层和子状态层的双层业务状态机,准确确定车辆当前的主状态、子状态及对应的业务类型,并匹配相应的速度约束和档位约束。通过将状态、目标、车辆位姿、起终点、业务及运动约束等关键信息统一封装为包含多字段的标准任务上下文,打破了业务状态机与轨迹规划之间的信息壁垒。这一机制确保了调度任务意图与运动约束能够无损耗地传递至规划环节,避免了任务切换或中断恢复时因状态跳转或字段遗漏导致的规划目标偏离,保障了矿卡能够准确理解并执行调度意图,显著提升了作业流程的衔接顺畅度与安全性。

56、2)针对复杂终端场景,本发明设计了常规局部轨迹优先、动态增强轨迹兜底的多级规划容错策略。在根据全局路径截取局部窗口生成常规局部轨迹后,若其不满足预设可用性条件,则自动根据车辆位姿、目标位姿、障碍物、车辆运动学约束和装卸区对位所需的终点姿态约束,生成动态增强轨迹。随后,将常规局部轨迹或动态增强轨迹均统一转换为统一轨迹点序列输出。该方法有效避免了常规规划失效导致的运行停滞,同时屏蔽了底层规划算法的差异,为控制环节提供了标准化的输入依据,极大提升了无人驾驶矿卡在复杂终端场景下的适应能力、作业连续性与执行安全性。

57、应当理解,本部分所描述的内容并非旨在标识本发明的实施例的关键或重要特征,也不用于限制本发明的范围。本发明的其它特征将通过以下的说明书而变得容易理解。