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

本发明属于自动驾驶，更具体的说是涉及一种基于车路协同的矿井下自动驾驶车辆协调规划方法。

背景技术：

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

2、因此，如何提供一种基于车路协同的矿井下自动驾驶车辆协调规划方法成为了本领域技术人员亟需解决的问题。

技术实现思路

1、针对矿井下自动驾驶会车场景存在的问题，本发明提供了一种基于车路协同的矿井下自动驾驶车辆协调规划方法，本发明所要解决的技术问题是如何在矿井下狭窄道路会车以及路口会车情形下，避免自动驾驶车辆决策错误或规划失败，实现高效地规划出多辆自动驾驶车辆的无冲突轨迹，提高自动驾驶车辆交互场景下的运行效率。

2、为了实现上述目的，本发明采用如下技术方案：

3、一种基于车路协同的矿井下自动驾驶车辆协调规划方法，包括如下步骤：

4、自动驾驶车路获取云平台提供的矿井下高精地图，基于高精地图中的道路中心线，进行全局平滑导航路径的规划，实现完整的参考线平滑；

5、根据规划出平滑的参考线，自动驾驶车辆底层规划器进行路径和速度决策规划，将底层规划器输出的轨迹信息作为输出发送给控制模块；

6、根据自车底层规划轨迹，自动驾驶车辆正常行驶，当遇到狭窄隧道会车以及路口会车场景时触发协调节点，并根据具体情况进行高级规划；

7、高级规划期的规划结果替代自动驾驶车辆原参考线，采用路径和速度解耦分别规划，并迭代求解可行的自车轨迹；

8、各自动驾驶车辆的自车轨迹作为输入给控制模块，控制模块执行自动驾驶车辆的横纵向控制通过矿井路口以完成会车动作。

9、进一步的，自动驾驶车路获取云平台提供的矿井下高精地图，基于高精地图中的道路中心线，进行全局平滑导航路径的规划，实现完整的参考线平滑，包括以下步骤：

10、自动驾驶车路获取云平台提供的矿井下高精地图，基于高精地图中的道路中心线，首先进行全局平滑导航路径的规划；其中道路中心线为离散点集进行平滑处理后作为参考线使用，道路中心线的离散点集之间采用三次多项式连接并均匀采样加密中心线离散点，多项式连接相邻的离散点(xi,yi)和(xi+1,yi+1)：

11、y＝f(x)＝a0+a1x+a2x2+a3x3

12、其中a0、a1、a2、a3分别表示三次多项式的0次项系数、1次项系数、2次项系数、3次项系数；

13、进行平滑参考线规划，在一个规划周期寻找离散点集中的自车投影点，基于投影点进行分段处理，分段后路径作为待平滑的路径段；基于由增密离散点组成的分段好的路径段将参考线平滑问题转化为二次规划问题，根据分段的中心线点集平滑的代价函数和约束条件进行求解得到平滑参考线点集；最后对不同周期的参考线段进行拼接，从而实现完整的参考线平滑。

14、进一步的，中心线点集平滑的代价函数为：

15、

16、其中w1、w2、w3为代价函数中每一项的权重，xi、yi和xref、yref分别为参考线和增密中心线的横纵坐标。

17、进一步的，矿井下高精度地图将道路数据和矿井下隧道的固定对向信息存储为结构化数据，道路中心线进行参考线平滑处理过程中，在每个自动驾驶车辆规划周期内对自车在道路中心线的投影点为起点，平滑起点前后一定范围内的点集，平滑后的点集作为参考线。

18、进一步的，根据规划出平滑的参考线，自动驾驶车辆底层规划器进行路径和速度决策规划，将底层规划器输出的轨迹信息作为输出发送给控制模块，包括以下步骤：

19、根据规划出的平滑的参考线，自动驾驶车辆底层规划器基于以导航路径为坐标轴的frenet坐标系进行路径和速度决策规划，将底层规划器输出的轨迹信息作为输出发送给控制模块；底层规划器采用slt降维方法决策规划过程如下：

20、(1)采用slt降维方法分成sl层面和st层面进行规划，然后在sl坐标系和st坐标系中构建路径和速度规划问题：

21、

22、其中，其中l表示自动驾驶车辆路径相对于道路中心线横向偏移量，s表示自动驾驶车辆路径沿着道路中心线纵向偏移量；t表示速度规划中对应纵向偏移的所在时刻；

23、(2)基于静态和低速障碍物投影，建立sl图并对状态空间离散化，采用启发式搜索方法以及数值优化的方法进行路径决策规划；

24、(3)基于动态障碍物轨迹预测，建立st图并对状态空间离散化,采用启发式搜索方法以及数值优化的方法进行速度决策规划。

25、进一步的，底层规划器规划过程中笛卡尔坐标系转换为frenet坐标系，轨迹信息发送给控制模块前将frenet坐标系转换为全局笛卡尔坐标系。

26、进一步的，sl图中路径规划和st图中速度规划根据隧道场景均设定不同的非均匀采样规模，首先进行状态空间离散化，依据代价函数分配各离散点的处的代价值，采用改进a*算法启发式搜索快速得到初解；初解作为决策方案开辟了安全空间，基于安全空间原问题转换为凸优化问题，在约束条件下采用凸优化求解方法得到最优轨迹解；sl图中进行速度规划的数值优化过程中，代价函数为：

27、

28、其中w1、w2、w3、w4、w5为代价函数中每一项的权重，li、lcentre分别表示sl图中的路径和参考线的纵向偏移量；

29、st图中进行速度规划的数值优化过程中，代价函数为：

30、

31、其中w1、w2、w3、w4为代价函数中每一项的权重，si、vref分别表示st图中的路径和的横向位移量以及参考速度。

32、进一步的，当遇到狭窄隧道会车以及路口会车场景时触发协调节点，并根据具体情况进行高级规划，包括以下步骤：

33、根据自车底层规划轨迹，自动驾驶车辆正常行驶，当遇到狭窄隧道会车以及路口会车场景时触发协调节点，首先判断自动驾驶车辆原轨迹是否冲突，无冲突则按原轨迹行驶；如有冲突，在车辆交互区形成冲突区域，在矿井狭窄隧道会车以及路口会车场景的冲突区域前形成缓冲协调区域；矿井隧道中的一辆或者多辆自动驾驶车辆驶入缓冲协调区域进行减速或者停车，依次等待协调；

34、协调节点通过v2i通信接收路口缓冲协调区域内的所有自动驾驶车辆的驾驶机动状态，其中高级规划器为所有缓冲协调区域内车辆进行协调规划，生成通行冲突区域的协调参考轨迹，且协调参考轨迹仅考虑缓冲协调区域内的自动驾驶车辆，不考虑其他静态或动态障碍物；其中高级规划器的路径生成采用基于直线和圆弧的平滑优化方法，首先对直线和圆进行采样：

35、knots:{(xk,m,yk,m,sk,m)m＝0,1,…,nk}

36、anchorpoints:{(xa,j,ya,j,sa,j)j＝0,1,…,na}

37、其中knots和anchorpoints表示分割直线和圆的节点和锚点，m和j表示对应节点和描点的编号，(xk,m,yk,m,sk,m)中分别表示节点的横纵坐标和所分割直线的总长度，(xa,j,ya,j,sa,j)中分别表示锚点的横纵坐标和所分割圆的总长度；

38、每两个相邻节点之间的参考路径使用五次多项式连接，然后采用优化的方法在直线和圆路径附近搜索平滑可行路径；

39、高级规划器基于各车路径规划结果在st图中对范围内所有自动驾驶车辆进行速度规划，首先将各车与冲突区域的交互投影到st图中，然后进行状态空间离散化并确定自动驾驶车辆通行顺序，依次进行速度的初解搜索和优化；且高级规划器的速度规划满足冲突区同一时刻仅被同一车辆占据的约束条件；

40、自动驾驶车辆进入缓冲协调区域，协调节点通过c-v2x的pc5直连通信接口与区域内的自动驾驶车辆实现v2i通信，利用v2i通信技术将路侧感知与车载感知连接在一起，实现数据传输低时延、高可靠的要求，建立可靠信息传输通道，实现多维度、全方面的感知信息共享和协同调度控制；

41、当协调节点判断各路口的自动驾驶车辆轨迹与其他自动驾驶车辆的轨迹存在冲突关系，高级规划器将基于自动驾驶车的当前机动状态确定规划起点，重新规划协调缓冲协调区域内的所有车辆。

42、进一步的，高级规划期的规划结果替代自动驾驶车辆原参考线，采用路径和速度解耦分别规划，并迭代求解可行的自车轨迹，包括以下步骤：

43、高级规划期的规划结果替代自动驾驶车辆原参考线，各自动驾驶车辆依据协调参考线建立frenet坐标系，将车载传感器感知到的路口障碍物信息投影到sl图和st图中，自车底层规划器进行再规划，采用路径和速度解耦分别规划，并迭代求解可行的自车轨迹；

44、高级规划器输出的各自动驾驶车辆进出冲突区时间作为在底层规划过程中st图的限制区域，以保证自车再规划的输出轨迹与其他自动驾驶车辆的输出轨迹无冲突：

45、

46、其中tsl、tel表示高级规划器速度规划结果中对自车的通行冲突区域的时间域边界，tin、tout表示自车底层规划器速度规划结果与冲突区域的交互时间；

47、冲突区域通行速度应满足速度约束条件：

48、

49、其中表示底层规划器的速度规划结果，且小于冲突区域限速vl；

50、高级规划器规划出相应场景下的各自动驾驶车辆协调参考轨迹，各自动驾驶车辆底层规划器以协调参考轨迹为输入进行自车重规划，以避免通行冲突区域时遇到各种障碍物，高级规划器和底层规划器共同保证各车辆依次安全协调通过冲突区域。

51、进一步的，各自动驾驶车辆的自车轨迹作为输入给控制模块，控制模块执行自动驾驶车辆的横纵向控制通过矿井路口以完成会车动作，自动驾驶车辆的自车轨迹经过坐标系转换后作为输入给控制模块，其中横向控制采用模型预测控制方法，纵向控制采用pid控制方法。

52、本发明的有益效果在于：

53、1.针对矿井下的自动驾驶场景，采用基于车路协同系统的高级规划器考虑矿井下路口和非路口会车特点，为缓冲协调区域内所有车辆提供无冲突参考轨迹；

54、2.考虑矿井下会车冲突区域的协调策略以及自车通行时的障碍物避让，自动驾驶车辆基于高级规划器的参考轨迹，结合自身底层规划器重规划得到安全可行的无冲突轨迹；

55、3.综合多层规划器，实现协同调度控制，提高矿井下自动驾驶车辆通行效率。