一种基于车路协同的矿井下自动驾驶车辆协调规划方法技术

本发明专利技术公开了一种基于车路协同的矿井下自动驾驶车辆协调规划方法，包括自动驾驶车路获取云平台提供的矿井下高精地图，基于高精地图中的道路中心线，进行全局平滑导航路径的规划，实现完整的参考线平滑；自动驾驶车辆底层规划器进行路径和速度决策规划，将底层规划器输出的轨迹信息作为输出发送给控制模块；根据自车底层规划轨迹，自动驾驶车辆正常行驶，根据具体情况进行高级规划；采用路径和速度解耦分别规划，迭代求解可行的自车轨迹；各自动驾驶车辆的自车轨迹作为输入给控制模块，执行自动驾驶车辆的横纵向控制通过矿井路口以完成会车动作。本发明专利技术实现高效地规划出多辆自动驾驶车辆的无冲突轨迹，提高自动驾驶车辆交互场景下的运行效率。景下的运行效率。景下的运行效率。

【技术实现步骤摘要】
一种基于车路协同的矿井下自动驾驶车辆协调规划方法


[0001]本专利技术属于自动驾驶
，更具体的说是涉及一种基于车路协同的矿井下自动驾驶车辆协调规划方法。

技术介绍

[0002]伴随自动驾驶技术的快速发展，基于自动驾驶车辆的安全性和可靠性，有力推动了煤矿“机械化换人、自动化减人”专项计划有效推动落实。自动驾驶技术依托车联网(vehicle to everything，V2X)来实现，当车联网的基础设施越来越完善，自动驾驶技术也将向更加成熟的趋势发展。V2X通过装载在车辆上的传感器、车载终端及电子标签提供车辆信息，采用各种通信技术实现车与车(vehicle to vehicle，V2V)、车与人(vehicle to pedestrian，V2P)、车与路(即基础设施)(vehicle to infrastructure，V2I)的互连互通，并在信息网络平台上对车辆信息进行提取、共享等有效利用，以实现对车辆进行有效的管控和提供综合服务。区别于地面交通领域的自动驾驶，矿井下隧道道路狭窄且路宽并不均一，一般情况下最宽处可达7米，狭窄处仅有4米左右。传统井下会车基于驾驶员和井下隧道设置的凹形避让区来实现在巷道照度低、路况不佳的条件下完成会车动作，且隧道路口区域会车时无高效的交通通行策略。井下自动驾驶车辆之间在该场景下会车情况较为复杂，决策错误或规划失败的概率较大。
[0003]因此，如何提供一种基于车路协同的矿井下自动驾驶车辆协调规划方法成为了本领域技术人员亟需解决的问题。

技术实现思路

[0004]针对矿井下自动驾驶会车场景存在的问题，本专利技术提供了一种基于车路协同的矿井下自动驾驶车辆协调规划方法，本专利技术所要解决的技术问题是如何在矿井下狭窄道路会车以及路口会车情形下，避免自动驾驶车辆决策错误或规划失败，实现高效地规划出多辆自动驾驶车辆的无冲突轨迹，提高自动驾驶车辆交互场景下的运行效率。
[0005]为了实现上述目的，本专利技术采用如下技术方案：
[0006]一种基于车路协同的矿井下自动驾驶车辆协调规划方法，包括如下步骤：
[0007]自动驾驶车路获取云平台提供的矿井下高精地图，基于高精地图中的道路中心线，进行全局平滑导航路径的规划，实现完整的参考线平滑；
[0008]根据规划出平滑的参考线，自动驾驶车辆底层规划器进行路径和速度决策规划，将底层规划器输出的轨迹信息作为输出发送给控制模块；
[0009]根据自车底层规划轨迹，自动驾驶车辆正常行驶，当遇到狭窄隧道会车以及路口会车场景时触发协调节点，并根据具体情况进行高级规划；
[0010]高级规划期的规划结果替代自动驾驶车辆原参考线，采用路径和速度解耦分别规划，并迭代求解可行的自车轨迹；
[0011]各自动驾驶车辆的自车轨迹作为输入给控制模块，控制模块执行自动驾驶车辆的
横纵向控制通过矿井路口以完成会车动作。
[0012]进一步的，自动驾驶车路获取云平台提供的矿井下高精地图，基于高精地图中的道路中心线，进行全局平滑导航路径的规划，实现完整的参考线平滑，包括以下步骤：
[0013]自动驾驶车路获取云平台提供的矿井下高精地图，基于高精地图中的道路中心线，首先进行全局平滑导航路径的规划；其中道路中心线为离散点集进行平滑处理后作为参考线使用，道路中心线的离散点集之间采用三次多项式连接并均匀采样加密中心线离散点，多项式连接相邻的离散点(x
i
,y
i
)和(x
i+1
,y
i+1
)：
[0014]y＝f(x)＝a0+a1x+a2x2+a3x3[0015]其中a0、a1、a2、a3分别表示三次多项式的0次项系数、1次项系数、2次项系数、3次项系数；
[0016]进行平滑参考线规划，在一个规划周期寻找离散点集中的自车投影点，基于投影点进行分段处理，分段后路径作为待平滑的路径段；基于由增密离散点组成的分段好的路径段将参考线平滑问题转化为二次规划问题，根据分段的中心线点集平滑的代价函数和约束条件进行求解得到平滑参考线点集；最后对不同周期的参考线段进行拼接，从而实现完整的参考线平滑。
[0017]进一步的，中心线点集平滑的代价函数为：
[0018][0019]其中w1、w2、w3为代价函数中每一项的权重，x
i
、y
i
和x
ref
、y
ref
分别为参考线和增密中心线的横纵坐标。
[0020]进一步的，矿井下高精度地图将道路数据和矿井下隧道的固定对向信息存储为结构化数据，道路中心线进行参考线平滑处理过程中，在每个自动驾驶车辆规划周期内对自车在道路中心线的投影点为起点，平滑起点前后一定范围内的点集，平滑后的点集作为参考线。
[0021]进一步的，根据规划出平滑的参考线，自动驾驶车辆底层规划器进行路径和速度决策规划，将底层规划器输出的轨迹信息作为输出发送给控制模块，包括以下步骤：
[0022]根据规划出的平滑的参考线，自动驾驶车辆底层规划器基于以导航路径为坐标轴的Frenet坐标系进行路径和速度决策规划，将底层规划器输出的轨迹信息作为输出发送给控制模块；底层规划器采用SLT降维方法决策规划过程如下：
[0023](1)采用SLT降维方法分成SL层面和ST层面进行规划，然后在SL坐标系和ST坐标系中构建路径和速度规划问题：
[0024][0025]其中，其中l表示自动驾驶车辆路径相对于道路中心线横向偏移量，s表示自动驾驶车辆路径沿着道路中心线纵向偏移量；t表示速度规划中对应纵向偏移的所在时刻；
[0026](2)基于静态和低速障碍物投影，建立SL图并对状态空间离散化，采用启发式搜索
方法以及数值优化的方法进行路径决策规划；
[0027](3)基于动态障碍物轨迹预测，建立ST图并对状态空间离散化,采用启发式搜索方法以及数值优化的方法进行速度决策规划。
[0028]进一步的，底层规划器规划过程中笛卡尔坐标系转换为Frenet坐标系，轨迹信息发送给控制模块前将Frenet坐标系转换为全局笛卡尔坐标系。
[0029]进一步的，SL图中路径规划和ST图中速度规划根据隧道场景均设定不同的非均匀采样规模，首先进行状态空间离散化，依据代价函数分配各离散点的处的代价值，采用改进A*算法启发式搜索快速得到初解；初解作为决策方案开辟了安全空间，基于安全空间原问题转换为凸优化问题，在约束条件下采用凸优化求解方法得到最优轨迹解；SL图中进行速度规划的数值优化过程中，代价函数为：
[0030][0031]其中w1、w2、w3、w4、w5为代价函数中每一项的权重，l
i
、l
centre
分别表示SL图中的路径和参考线的纵向偏移量；
[0032]ST图中进行速度规划的数值优化过程中，代价函数为：
[0033][0034]其中w1、w2、w3、w4为代价函数中每一项本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于车路协同的矿井下自动驾驶车辆协调规划方法，其特征在于，包括如下步骤：自动驾驶车路获取云平台提供的矿井下高精地图，基于高精地图中的道路中心线，进行全局平滑导航路径的规划，实现完整的参考线平滑；根据规划出平滑的参考线，自动驾驶车辆底层规划器进行路径和速度决策规划，将底层规划器输出的轨迹信息作为输出发送给控制模块；根据自车底层规划轨迹，自动驾驶车辆正常行驶，当遇到狭窄隧道会车以及路口会车场景时触发协调节点，并根据具体情况进行高级规划；高级规划期的规划结果替代自动驾驶车辆原参考线，采用路径和速度解耦分别规划，并迭代求解可行的自车轨迹；各自动驾驶车辆的自车轨迹作为输入给控制模块，控制模块执行自动驾驶车辆的横纵向控制通过矿井路口以完成会车动作。2.根据权利要求1所述的一种基于车路协同的矿井下自动驾驶车辆协调规划方法，其特征在于，自动驾驶车路获取云平台提供的矿井下高精地图，基于高精地图中的道路中心线，进行全局平滑导航路径的规划，实现完整的参考线平滑，包括以下步骤：自动驾驶车路获取云平台提供的矿井下高精地图，基于高精地图中的道路中心线，首先进行全局平滑导航路径的规划；其中道路中心线为离散点集进行平滑处理后作为参考线使用，道路中心线的离散点集之间采用三次多项式连接并均匀采样加密中心线离散点，多项式连接相邻的离散点(x
i
,y
i
)和(x
i+1
,y
i+1
)：y＝f(x)＝a0+a1x+a2x2+a3x3其中a0、a1、a2、a3分别表示三次多项式的0次项系数、1次项系数、2次项系数、3次项系数；进行平滑参考线规划，在一个规划周期寻找离散点集中的自车投影点，基于投影点进行分段处理，分段后路径作为待平滑的路径段；基于由增密离散点组成的分段好的路径段将参考线平滑问题转化为二次规划问题，根据分段的中心线点集平滑的代价函数和约束条件进行求解得到平滑参考线点集；最后对不同周期的参考线段进行拼接，从而实现完整的参考线平滑。3.根据权利要求2所述的一种基于车路协同的矿井下自动驾驶车辆协调规划方法，其特征在于，中心线点集平滑的代价函数为：其中w1、w2、w3为代价函数中每一项的权重，x
i
、y
i
和x
ref
、y
ref
分别为参考线和增密中心线的横纵坐标。4.根据权利要求3所述的一种基于车路协同的矿井下自动驾驶车辆协调规划方法，其特征在于，矿井下高精度地图将道路数据和矿井下隧道的固定对向信息存储为结构化数据，道路中心线进行参考线平滑处理过程中，在每个自动驾驶车辆规划周期内对自车在道路中心线的投影点为起点，平滑起点前后一定范围内的点集，平滑后的点集作为参考线。
5.根据权利要求1所述的一种基于车路协同的矿井下自动驾驶车辆协调规划方法，其特征在于，根据规划出平滑的参考线，自动驾驶车辆底层规划器进行路径和速度决策规划，将底层规划器输出的轨迹信息作为输出发送给控制模块，包括以下步骤：根据规划出的平滑的参考线，自动驾驶车辆底层规划器基于以导航路径为坐标轴的Frenet坐标系进行路径和速度决策规划，将底层规划器输出的轨迹信息作为输出发送给控制模块；底层规划器采用SLT降维方法决策规划过程如下：(1)采用SLT降维方法分成SL层面和ST层面进行规划，然后在SL坐标系和ST坐标系中构建路径和速度规划问题：其中，其中l表示自动驾驶车辆路径相对于道路中心线横向偏移量，s表示自动驾驶车辆路径沿着道路中心线纵向偏移量；t表示速度规划中对应纵向偏移的所在时刻；(2)基于静态和低速障碍物投影，建立SL图并对状态空间离散化，采用启发式搜索方法以及数值优化的方法进行路径决策规划；(3)基于动态障碍物轨迹预测，建立ST图并对状态空间离散化,采用启发式搜索方法以及数值优化的方法进行速度决策规划。6.根据权利要求5所述的一种基于车路协同的矿井下自动驾驶车辆协调规划方法，其特征在于，底层规划器规划过程中笛卡尔坐标系转换为Frenet坐标系，轨迹信息发送给控制模块前将Frenet坐标系转换为全局笛卡尔坐标系。7.根据权利要求5所述的一种基于车路协同的矿井下自动驾驶车辆协调规划方法，其特征在于，SL图中路径规划和ST图中速度规划根据隧道场景均设定不同的非均匀采样规模，首先进行状态空间离散化，依据代价函数分配各离散点的处的代价值，采用改进A*算法启发式搜索快速得到初解；初解作为决策方案开辟了安全空间，基于安全空间原问题转换为凸优化问题，在约束条件下采用凸优化求解方法得到最优轨迹解；SL图中进行速度规划的数值优化过程中，代价函数为：其中w1、w2、w3、w4、w5为代价函数中每一项的权重，l
i
、l
centre
分别表示SL图中的路径和参考线的纵向偏移量；ST图中进行速度规划的数值优化过程中，代价函数为：其中w1、w2、w3、...

【专利技术属性】
技术研发人员：吴宏涛，张林梁，牛秉青，李朝霞，孟颖，李臻，吕永萍，樊恩成，
申请(专利权)人：山西省智慧交通研究院有限公司，
类型：发明
国别省市：