一种基于激光雷达的Frenet坐标系下的局部动态规划调度方法

本发明涉及车路协同，尤其涉及一种基于激光雷达的frenet坐标系下的局部动态规划调度方法。

背景技术：

1、近年来无人驾驶和车路协同技术的迅速发展对提高道路交通安全有着重大意义，具有良好的发展前景。车路协同和无人驾驶技术可以为运输调度提供更多数据，使得调度优化模型可以考虑的目标函数和约束条件更为丰富；但无人驾驶卡车运输调度问题仍是研究运输成本最小化、设备利用率最大化的问题。目前对露天矿卡车的调度研究仍集中于对传统露天矿人工驾驶卡车的调度理论研究当中，针对无人驾驶卡车背景下的全局运输调度研究鲜少，而且关于无人驾驶矿卡的运输调度多数还停留在静态调度，即出发前的调度，并不是实时根据运输状态动态规划的调度，之前关于无人驾驶矿卡调度并没有考虑全局调度出现突发情况时的化解。由于露天矿环境恶劣，路面状况复杂，弯路、岔路多且道路结构稳定性差，以及山体等地形遮挡的原因，无人驾驶矿卡在行驶时极易产生在会车或跟车过程中无法正确判断前方车辆和环境的态势。现行技术下当矿车途经露天矿区弯道交叉口时，弯道交叉区域视距不足可能会导致车辆碰撞、侧翻、冲出路面等事故的发生，影响车辆的行驶安全和运输效率。

技术实现思路

1、针对上述现有技术的不足，本发明提出了一种基于激光雷达的frenet坐标系下的局部动态规划调度方法，重点针对露天矿无人驾驶矿卡在局部特殊路段进行速度规划和轨迹规划。

2、本发明提出的一种基于激光雷达的frenet坐标系下的局部动态规划调度方法，该方法包括如下步骤：

3、步骤1：获取露天矿道路信息并构建弯道交叉路段冲突模型，涉及：露天矿道路、交叉路口、目标矿卡和若干个非目标矿卡；

4、步骤2：目标矿卡和非目标矿卡按照预设的初始轨迹行驶，在行驶过程中实时对目标矿卡进行安全车距检测、对向车辆冲突检测预测以及运动学和碰撞检测并调整行驶状态，再根据目标矿卡的行驶状态生成若干条候选轨迹；

5、步骤3：当目标矿卡进入交叉路口时，根据候选轨迹判断目标矿卡在该交叉路口是否存在车辆优先级冲突，若目标矿卡进入交叉路口后不存在车辆优先级冲突，则目标矿卡以小于交叉路口安全车速的车速驶离路口；反之，则目标矿卡按照车辆优先级的排序结果以交叉路口安全车速驶离路口，生成有效轨迹列表；

6、步骤4：定义轨迹的成本函数，计算有效轨迹列表中所有轨迹的成本并选择成本最小的轨迹为最优轨迹；

7、步骤1中所述露天矿道路信息包括车道信息和车辆信息；所述车道信息包括：露天矿道路、交叉路口、车辆速度限制和车道宽度；所述车辆信息包括：车型、车辆优先级、车辆行驶轨迹、车速、车辆的长宽高数据、车辆的三维模型、最小安全跟随车距以及车辆的空载满载状态；

8、步骤1中所述构建弯道交叉路段冲突模型的方法为：根据露天矿道路信息中的车道信息建立包含露天矿道路和交叉路口的可视化路网场景模型，根据露天矿道路信息中的车辆信息向可视化路网场景模型中添加目标矿卡和若干个非目标矿卡，对目标矿卡进行初始化，确定目标矿卡的初始位置和初始姿态；

9、步骤2中所述安全车距检测的方法为：在目标矿卡的行驶过程中，利用目标矿卡上配置的激光雷达获取目标矿卡与该目标矿卡前进方向上的非目标矿卡之间的实时车距，将获取的实时车距与最小安全跟随车距比较，若实时车距小于最小安全跟随车距，则通过调整目标矿卡的车速保证实时车距始终大于等于最小安全跟随车距；同时建立最小安全跟随车距模型，用于防止目标矿卡的行驶过程中发生紧急制动时发生追尾事故，建立目标矿卡最小安全跟随车距约束，以保证安全车距；

10、步骤2中所述对向车辆冲突检测预测的方法为：利用目标矿卡上配置的激光雷达实时获取与目标矿卡对向行驶的非目标矿卡的位置信息，并根据获取到的位置信息计算非目标矿卡的车速和行驶轨迹，基于非目标矿卡的车速和行驶轨迹，通过仿真模拟预测目标矿卡按照当前行驶状态继续行驶是否会与对向行驶的非目标矿卡发生碰撞，若否，则目标矿卡继续按当前行驶状态行驶；若是，则目标矿卡通过调整车速和行驶方向改变行驶轨迹，保证目标矿卡始终不与对向行驶的非目标矿卡发生碰撞；

11、步骤2中所述运动学和碰撞检测的方法为：判断目标矿卡的行驶状态是否符合运动学约束条件，若符合则保持当前行驶状态继续行驶；若不符合则通过调整目标矿卡的车速和行驶方向改变行驶状态，以保证目标矿卡的行驶状态始终符合运动学约束条件；

12、所述目标矿卡的车辆弯道安全车速模型：

13、vsafe＝min{vcp,vhb,vcq}

14、其中vsafe为目标矿卡的车辆弯道安全车速；min{vcp,vhb,vcq}表示目标矿卡的车辆弯道安全车速取目标矿卡的侧偏临界速度vcp、目标矿卡的横摆临界速度vhb和目标矿卡的侧倾临界速度vcq三者中的最小值；

15、步骤4所述最优轨迹为：

16、minc＝js+jd+1000pc+100(vδt-vl)2

17、其中minc表示成本函数；js是有效轨迹列表中任意有效轨迹的加速度变化率的累计平方和；s是沿该有效轨迹方向的弧长；jd是垂直于该有效轨迹方向的加速度变化率的累计平方和，作为用于评估轨迹平滑性和安全稳定性的指标；pc是碰撞概率；vδt为单位时间内目标矿卡的行驶速度；vl为无人驾驶矿卡行驶限制车速。

18、采用上述技术方案所产生的有益效果在于：

19、本发明方法提出了一种基于激光雷达的frenet坐标格网局部动态规划调度算法(lidar-based frenet gridwise local dynamic planning scheduling algorithm，lfg-ldpsa)，采用无人矿卡搭载的车载系统激光雷达传感器合成数据，使用frenet坐标系与笛卡尔全局坐标系转换，将无人驾驶规划调度轨迹这一高纬度的优化问题分割成了沿参考路径横向和纵向两个独立的优化问题，通过使用五阶多项式连接frenet坐标中的初始状态和最终状态生成一系列样本轨迹，对这些候选轨迹进行动力学可行性分析和碰撞检测，通过目标函数选出综合性能最优的轨迹，实现了更快速的获取平稳可靠的运行轨迹，实现车辆在弯道交叉口安全平稳的无冲突的运输行驶。

技术特征：

1.一种基于激光雷达的frenet坐标系下的局部动态规划调度方法，其特征在于，该方法包括如下步骤：

2.根据权利要求1所述一种基于激光雷达的frenet坐标系下的局部动态规划调度方法，其特征在于，步骤1中所述露天矿道路信息包括车道信息和车辆信息；所述车道信息包括：露天矿道路、交叉路口、车辆速度限制和车道宽度；所述车辆信息包括：车型、车辆优先级、车辆行驶轨迹、车速、车辆的长宽高数据、车辆的三维模型、最小安全跟随车距以及车辆的空载满载状态。

3.根据权利要求2所述一种基于激光雷达的frenet坐标系下的局部动态规划调度方法，其特征在于，步骤1中所述构建弯道交叉路段冲突模型的方法为：根据露天矿道路信息中的车道信息建立包含露天矿道路和交叉路口的可视化路网场景模型，根据露天矿道路信息中的车辆信息向可视化路网场景模型中添加目标矿卡和若干个非目标矿卡，对目标矿卡进行初始化，确定目标矿卡的初始位置和初始姿态。

4.根据权利要求2所述一种基于激光雷达的frenet坐标系下的局部动态规划调度方法，其特征在于，步骤2中所述安全车距检测的方法为：在目标矿卡的行驶过程中，利用目标矿卡上配置的激光雷达获取目标矿卡与该目标矿卡前进方向上的非目标矿卡之间的实时车距，将获取的实时车距与最小安全跟随车距比较，若实时车距小于最小安全跟随车距，则通过调整目标矿卡的车速保证实时车距始终大于等于最小安全跟随车距；同时建立最小安全跟随车距模型，用于防止目标矿卡的行驶过程中发生紧急制动时发生追尾事故，建立目标矿卡最小安全跟随车距约束，以保证安全车距。

5.根据权利要求2所述一种基于激光雷达的frenet坐标系下的局部动态规划调度方法，其特征在于，步骤2中所述对向车辆冲突检测预测的方法为：利用目标矿卡上配置的激光雷达实时获取与目标矿卡对向行驶的非目标矿卡的位置信息，并根据获取到的位置信息计算非目标矿卡的车速和行驶轨迹，基于非目标矿卡的车速和行驶轨迹，通过仿真模拟预测目标矿卡按照当前行驶状态继续行驶是否会与对向行驶的非目标矿卡发生碰撞，若否，则目标矿卡继续按当前行驶状态行驶；若是，则目标矿卡通过调整车速和行驶方向改变行驶轨迹，保证目标矿卡始终不与对向行驶的非目标矿卡发生碰撞。

6.根据权利要求5所述一种基于激光雷达的frenet坐标系下的局部动态规划调度方法，其特征在于，步骤2中所述运动学和碰撞检测的方法为：判断目标矿卡的行驶状态是否符合运动学约束条件，若符合则保持当前行驶状态继续行驶；若不符合则通过调整目标矿卡的车速和行驶方向改变行驶状态，以保证目标矿卡的行驶状态始终符合运动学约束条件。

7.根据权利要求6所述一种基于激光雷达的frenet坐标系下的局部动态规划调度方法，其特征在于，所述目标矿卡的车辆弯道安全车速模型：

8.根据权利要求7所述一种基于激光雷达的frenet坐标系下的局部动态规划调度方法，其特征在于，步骤4所述最优轨迹为：

技术总结本发明提供一种基于激光雷达的Frenet坐标系下的局部动态规划调度方法，涉及车路协同技术领域。该方法具体包括：获取露天矿道路信息并构建弯道交叉路段冲突模型；该模型中的目标矿卡和非目标矿卡均按照预设的初始轨迹行驶，在行驶过程中实时对目标矿卡进行安全车距检测、对向车辆冲突检测预测以及运动学和碰撞检测并调整行驶状态，再根据目标矿卡的行驶状态生成若干条候选轨迹；当目标矿卡进入交叉路口时，根据候选轨迹判断目标矿卡在该交叉路口是否存在车辆优先级冲突，通过设定消解冲突的约束生成有效轨迹列表；计算有效轨迹列表中所有轨迹的成本并选择成本最小的轨迹为最优轨迹，从而实现车辆在弯道交叉口安全平稳的无冲突的运输行驶。技术研发人员：张林丛,张文波,魏可峰,王冬旭,梁晓宇受保护的技术使用者：沈阳理工大学技术研发日：技术公布日：2024/7/29