一种井下狭窄巷道双层安全冗余避障路径规划方法与流程

本发明涉及无人驾驶，特别是涉及一种井下狭窄巷道双层安全冗余避障路径规划方法。

背景技术：

1、在当今的工业自动化进程中，人工智能与无人驾驶技术的融合正逐步成为提升生产效率的关键驱动力。特别是在矿业领域，面对极端的工作环境与人力短缺的双重挑战，无人驾驶运输车辆的应用显得尤为迫切。煤矿辅运大巷作为矿井内部物资流通的主要通道，其狭窄的空间与密集的车辆活动使得行车安全成为亟需解决的问题。据统计，刮擦事故在总车辆事故中占比高达40%，严重影响了矿井作业效率与安全性。

2、传统的人工驾驶方式在狭小巷道内不仅容易引发事故，而且难以应对突发情况，尤其是错车时巷道宽度不足以容纳两车安全通过的情况。因此，研发一种能适应井下特殊环境，具备高度智能避障能力的无人驾驶路径规划技术，对提升矿区智能化水平、保障作业安全、提高运输效率具有重大意义。

3、当前，针对井工矿环境的无人驾驶车辆研究中，避障路径规划仍面临诸多挑战，如巷道复杂、动态障碍物频繁、路径规划实时性与准确性要求高等。传统的路径规划方法往往忽视了实时避障的灵活性和路径的安全冗余度，难以满足井下特殊作业需求。因此，设计一种能够实时感知环境、动态规划安全绕障路径，并具备第二层靠边停靠策略的双层安全冗余路径规划技术，成为行业内的迫切需求，以确保无人驾驶车辆能在复杂环境下安全、高效地完成运输任务。

技术实现思路

1、为了解决传统的路径规划方法往往忽视了实时避障的灵活性和路径的安全冗余度，难以满足井下特殊作业需求的问题，本发明的目的是提供一种井下狭窄巷道双层安全冗余避障路径规划方法。

2、为了实现上述目的，本发明采用如下技术方案：一种井下狭窄巷道双层安全冗余避障路径规划方法，包括有障碍物感知模块、激光雷达系统、避障局部规划模块、轨迹跟踪控制模块和靠边停靠路径生成模块，具体步骤为：

3、步骤一，障碍物感知模块利用激光雷达系统实时监测前方障碍物；

4、步骤二，监测结果为无障碍物存在时，车辆沿全局路径行驶，不启用避障局部规划模块；

5、步骤三，监测到前方障碍物时，避障局部规划模块生成绕障路径，并由轨迹跟踪控制模块执行；

6、步骤四，实时监测局部绕障路径有效性，路径失效，靠边停靠路径生成模块生成靠边停靠路径，且由轨迹跟踪控制模块执行靠边；

7、步骤五，实时监测局部绕障路径持续有效，且由轨迹跟踪控制模块执行持续绕行。

8、优选的，所述避障局部规划模块执行步骤为：

9、a、采用撒点采样算法，基于参考轨迹进行路径规划，当障碍物与车辆的最小距离值dmin小于预设阈值时，实施绕行规划；

10、b、在参考轨迹上根据最小距离值dmin及其后续等间距撒点，生成多个候选纵向坐标s，用于后续轨迹规划；

11、c、计算每个s处的道路边界偏差，确定横向坐标d的撒点范围，确保车辆宽度及安全间隙；

12、d、利用frenet坐标系下的五次多项式函数描述期望轨迹，确保轨迹的连续性和可行驶性；

13、e、 最优轨迹选取，排除无法绕过障碍的轨迹，保障车辆行驶安全和效率。

14、优选的，所述a中，参考轨迹是从全局轨迹上截取一部分，截取的长度为固定值t，当轨迹接近全局轨迹终点且小于t时，则截取到全局轨迹的终点，障碍物位置包括：a、障碍物在参考轨迹上；b、障碍物在参考轨迹一侧；参考轨迹上最近障碍物确定方法为：

15、对于a，障碍物影响车辆行驶；对于b，障碍物四个角点到轨迹的最近距离imin，如果imin<wvehicle/2+ith, wvehicle为车辆宽度，ith为安全距离阈值，则判定为影响车辆行驶。

16、优选的，在纵向上，以参考轨迹的第一个点作为s坐标的0点s0,设定障碍物距离阈值dobth；

17、当dmin≦dobth时，进行绕行轨迹规划；

18、当dmin>dobth时，进行非绕行轨迹规划；

19、在进行绕行轨迹规划时，纵向轨迹规划终点s1撒点如下：

20、dmin，dmin+5，dmin+10，dmin+15，……，min轨迹终点距离；

21、在进行非绕行轨迹规划时，纵向轨迹规划终点s1撒点如下：

22、dobth，dobth+5，dobth+10，dobth+15，……，min轨迹终点距离；

23、根据纵向撒点，在每一个纵向位置进行横向撒点，每一s1处道路边界到期望轨迹的偏差[dboundmin,dboundmax]，边界左侧偏差为正值，右侧偏差为负值，撒点的取值范围为[dboundmin+0.5+wvehicle/2,dboundmax-0.5-wvehicle/2]；

24、其中，wvehicle为车辆宽度；在取值范围内，每隔0.5米取一个点，得到若干组s_1相对应的d_1的取值。

25、优选的，通过纵向和横向撒点生成期望轨迹集合，期望轨迹的原型用frenet坐标系表示，表示为横向偏差d相对于纵向偏差s的函数，为五次多项式形式，如下：

26、d=f(s)=a0+a1s+a2s^2+a3s^3+a4s^4+a5s^5；

27、在起点处的约束条件：

28、d0=车辆当前相对于参考轨迹的横向偏差；

29、d0'=tan(yawvehice-yawroad)；

30、d0''=0；

31、在终点处的约束条件：

32、d1=横向撒点的偏差；

33、d1'=0；

34、d1''=0；

35、在s1之后，d的取值为恒定的d1；

36、将参考轨迹上每个点的s值，带入轨迹方程，计算出每个点的横向偏差；

37、当s≦s1时，d=f(s)；

38、当s>s1时，d=d1；

39、得出系列(s, d)点序列，根据坐标关系，将点序列变换到笛卡尔坐标系下，即可获得一条绕行轨迹；

40、对轨迹上每个点的横向偏差的平方求和得到横向偏差代价参数costd：

41、；

42、对轨迹上每个点的曲率的平方求和得到轨迹曲率代价参数costc:

43、；

44、轨迹变化代价参数costl用来表示当前规划的轨迹与上一个周期的轨迹的偏差:

45、；

46、则；

47、最优轨迹选取：

48、剔除无法绕过障碍物的轨迹，在dmin≦dobth时，进行剔除操作；对未被剔除的轨迹的cost进行对比，取cost最小的轨迹作为最优轨迹，并输出给后续节点。

49、优选的，所述靠边停靠路径生成模块执行步骤为：

50、s1、当车辆无法直接绕行通过障碍物时，规划靠边停靠路径，停靠点位于障碍物前方一定距离，确保车辆与墙体间有足够的安全间隙；

51、s2、使用五次多项式模型规划纵向位移s与时间t的关系，确保车辆平稳停止，同时考虑车辆当前状态约束；

52、s3、 确定横向停靠位置d，确保车辆右侧停靠时与巷道墙壁保持安全距离，不阻碍交通。

53、优选的，所述靠边停靠路径生成模块执行停车状态中，期望轨迹的原型用frenet坐标系表示，表示为纵向位移s相对于t的函数，为五次多项式形式，如下：

54、s=f(t)=a0+a1t+a2t^2+a3t^3+a4t^4+a5t^5；

55、在起点处的约束条件：

56、s0=车辆当前纵向位移；

57、s0'=当前纵向速度；

58、s0''=0；

59、在终点处的约束条件：

60、s1=车辆停靠点纵向位移；

61、s1'=0；

62、s1''=0；

63、根据纵向和横向撒点生成期望轨迹集合，期望轨迹的原型用frenet坐标系表示，表示为横向偏差d相对于纵向偏差s的函数，为五次多项式形式，如下：

64、d=f(s)=a0+a1s+a2s^2+a3s^3+a4s^4+a5s^5；

65、在起点处的约束条件：

66、d0=车辆当前相对于参考轨迹的横向偏差；

67、d0'=tan(yawvehice-yawroad)；

68、d0''=0；

69、在终点处的约束条件：

70、d1=道路宽度/2-车辆离墙体安全距离+车宽/2；

71、d1'=0；

72、d1''=0；

73、将此路径替换为当前全局参考路径，实现局部路径规划。

74、与现有技术相比，本发明实现的有益效果：

75、1、增强安全性与可靠性：通过集成的障碍物感知模块实时监控周围环境，及时发现潜在碰撞风险，结合双层路径规划策略，确保车辆在遇到障碍时既能灵活绕行也能安全停靠，显著降低碰撞风险。

76、2、提升通行效率：在没有障碍物的情况下，车辆能沿预定的全局路径高效行驶，而在障碍物存在时，快速切换至局部避障路径规划，避免了不必要的停顿，保障了交通流畅通。

77、3、路径规划灵活性：采用撒点采样算法与五次多项式轨迹拟合技术，生成精确的绕行轨迹，同时考虑了车辆宽度和安全距离，实现了路径规划的高精度与动态适应性。

78、4、资源优化利用：在避障路径不可行时，自动触发靠边停靠策略，避免了无谓的能耗与时间浪费，待条件允许时再继续行驶，优化了资源利用。

79、5、成本效益：通过综合考虑横向偏差、曲率及轨迹变化的cost值，选择最优路径，既减少了对车辆硬件的要求，也降低了运行成本，同时提升了整体的经济效益。

80、6、智能化水平提升：该技术推动了井下无人驾驶运输的智能化进程，减轻了对人工驾驶的依赖，为解决矿工招工难问题提供了科技支撑，符合未来智能化发展趋势。