矿井下狭窄通道自动驾驶车辆的掉头规划方法技术

本发明专利技术公开了一种矿井下狭窄通道自动驾驶车辆的掉头规划方法，包括：S1，确定掉头规划起点区域；S2，进行隧道区域规划，生成区间导航路径和局部路径；S3，进行硐室区域规划：自动驾驶车辆根据隧道区域规划结果行驶至掉头规划起点区域内，建立硐室规划区域离散地图并设定步长Sn，进行节点拓展，直至拓展到目标点，生成无碰撞路径；S4，对步骤S3生成的无碰撞路径进行后处理：构造的目标函数，对所述无碰撞路径离散点采用非线性优化规划问题进行建模，并采用相应求解器，得到最终平滑路径解。该方法能快速、有效地在狭窄区域内借助硐室空间完成掉头轨迹规划，实现较为连贯的掉头动作，保证了规划算法的实时性，提高了车辆通行效率。提高了车辆通行效率。提高了车辆通行效率。

【技术实现步骤摘要】
矿井下狭窄通道自动驾驶车辆的掉头规划方法


[0001]本专利技术涉及自动驾驶领域，尤其涉及一种矿井下狭窄通道自动驾驶车辆的掉头规划方法。

技术介绍

[0002]区别于地面交通领域的自动驾驶，矿井下隧道道路狭窄且路面宽度不均匀。传统矿井下车辆进行掉头时限制于隧道路况以及道路宽度，并依赖驾驶员的主观意图判断矿井隧道内的掉头位置，这给矿井下的自动驾驶车辆实现掉头带来了严峻的挑战。井下自动驾驶车辆在该场景下掉头较为复杂，规划失败的概率较大。
[0003]硐室是一种未直通地表出口的、横断面较大而长度较短的水平坑道。其作用是安装各种设备、机器，存放材料和工具，或用作其它专门用途。与一般巷道相比，硐室施工具有以下特点：1.硐室的断面大、变化多，长度则比较短；2.硐室往往与其他硐室、巷道、井筒相连；3.硐室的工程规格和质量高。
[0004]自动驾驶车辆如能利用矿井下设置的硐室来实现自主掉头规划，则有利于降低规划难度、加快掉头规划速度并提升掉头规划的成功率。目前未见利用硐室进行井下狭窄通道自动驾驶车辆掉头的规划方法。

技术实现思路

[0005]本专利技术提供一种矿井下狭窄通道自动驾驶车辆的掉头规划方法，基于矿井下狭窄通道、利用硐室来保证足够大的安全空间，解决自动驾驶车辆在狭窄隧道场景下车辆掉头问题。
[0006]为此，本专利技术采用以下技术方案：
[0007]一种矿井下狭窄通道自动驾驶车辆的掉头规划方法，包括以下步骤：
[0008]S1，确定掉头规划起点区域：自动驾驶车辆沿着矿井下的原始导航路线行驶，当接收到掉头指令时，依据高精地图搜索隧道内最近的硐室位置，并根据隧道和硐室布局确定掉头规划起点区域；
[0009]S2，进行隧道区域规划，生成区间导航路径和局部路径：自动驾驶车辆确定当前所在位置，将前所在位置和上述掉头规划起点区域分别作为路线规划的起点和终点，在此区间内生成全局导航路径；然后，基于所述全局导航路径，将自动驾驶车辆在参考线上的投影点作为坐标原点，建立Frenet坐标系，为行驶至掉头规划起点区域过程中的局部规划构建规划空间；
[0010]S3，进行硐室区域规划：自动驾驶车辆根据隧道区域规划结果行驶至掉头规划起点区域内，建立硐室规划区域离散地图并设定步长Sn，进行节点拓展，直至拓展到目标点，生成无碰撞路径；
[0011]S4，对步骤S3生成的无碰撞路径进行后处理：构造的目标函数，对所述无碰撞路径离散点采用非线性优化规划问题进行建模，并采用相应求解器，得到最终平滑路径解。
[0012]其中，步骤S1所述掉头规划起点区域靠近矿井内的硐室区域与狭窄隧道路口处，且处于狭窄隧道内。
[0013]所述步骤S2包括以下分步骤：
[0014]S21，找到自动驾驶车辆的坐标在导航参考线上的匹配点，根据Cartesian坐标系和Frenet坐标系的转换关系得到Frenet坐标系下相应坐标点的信息；
[0015]S22，将Frenet坐标系中的SLT三维问题解耦成SL层面和ST层面的两个二维问题，基于SL图以及静态障碍物的投影信息，采用空间离散化方法结合二次规划进行路径决策和规划；有了路径结果，再建立ST图以及进行动态障碍物投影，采用空间离散化方法结合二次规划进行速度决策和规划；将最后的导航路径规划结果和速度规划结果合并，得到轨迹信息。
[0016]所述步骤S22的具体过程如下：
[0017]自动驾驶车辆基于导航路径建立Frenet坐标系，并依次建立SL图和ST图，分别构建路径规划和速度规划空间：
[0018][0019]基于静态和低速障碍物投影建立SL图，并对状态空间离散化，采用启发式搜索方法以及数值优化方法进行路径决策规划；基于动态障碍物轨迹预测，建立ST图并对状态空间离散化，采用启发式搜索方法以及数值优化的方法进行速度决策规划；
[0020]其中，SL图中路径规划和ST图中速度规划均根据隧道场景设定不同的非均匀采样规模，首先进行状态空间离散化，依据代价函数分配各离散点的处的代价值，采用改进的A*算法启发式搜索快速得到初解；初解作为决策方案开辟了安全空间，基于安全空间原问题转换为凸优化问题，在约束条件下采用凸优化求解方法得到最优轨迹解；SL图中进行速度规划的数值优化过程中，代价函数为：
[0021][0022]其中w
i
为代价函数中每一项的权重，l
i
、l
centre
分别表示SL图中的路径和参考线的纵向偏移量；代价函数的各项分别表示一阶导数代价、二阶导数代价、三阶导数代价、横向偏移代价、中心线代价，每一项前均有相应的代价权重，约束条件为：
[0023]A
eq
x＝b
eq
[0024]Ax≤b
[0025]lb≤x≤ub
[0026]x＝(l1,l
′1,l
″1,
…
l
n
,l
′
n
,l
″
n
)
T
[0027]式中x为状态量，A
eq
,b
eq
用于描述状态等式约束方程，Ax≤b描述的是状态不等式约束，lb,ub是状态x的取值范围下界与上界(lower bound与upper bound)。
[0028]对应的约束条件分别为连续性等式约束、碰撞不等式约束和上下界约束，经过特定的求解器(OSQP等行业内开源常用的对应问题类型的求解器)得到l1,l
′1,l
″1,
…
l
n
,l
′
n
,l
″
n
与s1,s2,s3,
…
s
n
结合的二次规划路径结果；
[0029]ST图中进行速度规划的数值优化过程中，代价函数为：
[0030][0031]A
eq
x＝b
eq
[0032]Ax≤b
[0033]lb≤x≤ub
[0034]其中w
i
为代价函数中每一项的权重，s
i
、v
ref
分别表示ST图中的路径和的横向位移量以及参考速度；式中对应的约束条件分别为连续性约束、速度和加速度不等式约束，经过求解器得到速度规划结果，合并路径规划结果完成轨迹规划；
[0035]车辆根据局部规划结果沿着导航路径实现矿井下静态障碍物和动态障碍物的简单避让，并安全行驶至掉头规划起点区域。
[0036]所术述步骤S3包括以下分步骤：
[0037]S31，进行硐室规划区域离散化：离散化方法采用栅格地图来描述掉头规划起点区域和硐室区域的环境信息，其中障碍物以及边界在栅格地图中进行膨胀处理，离散化规模依据硐室区域动态调整；
[0038]S32，基于离散化状态空间进行节点变步长拓展：
[0039]在隧道口掉头规本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种矿井下狭窄通道自动驾驶车辆的掉头规划方法，包括以下步骤：S1，确定掉头规划起点区域：自动驾驶车辆沿着矿井下的原始导航路线行驶，当接收到掉头指令时，依据高精地图搜索隧道内最近的硐室位置，并根据隧道和硐室布局确定掉头规划起点区域；S2，进行隧道区域规划，生成区间导航路径和局部路径：自动驾驶车辆确定当前所在位置，将前所在位置和上述掉头规划起点区域分别作为路线规划的起点和终点，在此区间内生成全局导航路径；然后，基于所述全局导航路径，将自动驾驶车辆在参考线上的投影点作为坐标原点，建立Frenet坐标系，为行驶至掉头规划起点区域过程中的局部规划构建规划空间；S3，进行硐室区域规划：自动驾驶车辆根据隧道区域规划结果行驶至掉头规划起点区域内，建立硐室规划区域离散地图并设定步长Sn，进行节点拓展，直至拓展到目标点，生成无碰撞路径；S4，对步骤S3生成的无碰撞路径进行后处理：构造的目标函数，对所述无碰撞路径离散点采用非线性优化规划问题进行建模，并采用相应求解器，得到最终平滑路径解。2.根据权利要求1所述的掉头规划方法，其于特征在于：步骤S1所述掉头规划起点区域靠近矿井内的硐室区域与狭窄隧道路口处，且处于狭窄隧道内。3.根据权利要求1所述的掉头规划方法，其于特征在于，步骤S2包括以下分步骤：S21，找到自动驾驶车辆的坐标在导航参考线上的匹配点，根据Cartesian坐标系和Frenet坐标系的转换关系得到Frenet坐标系下相应坐标点的信息；S22，将Frenet坐标系中的SLT三维问题解耦成SL层面和ST层面的两个二维问题，基于SL图以及静态障碍物的投影信息，采用空间离散化方法结合二次规划进行路径决策和规划；有了路径结果，再建立ST图以及进行动态障碍物投影，采用空间离散化方法结合二次规划进行速度决策和规划；将最后的导航路径规划结果和速度规划结果合并，得到轨迹信息。4.根据权利要求3所述的掉头规划方法，其于特征在于，步骤S22的具体过程如下：自动驾驶车辆基于导航路径建立Frenet坐标系，并依次建立SL图和ST图，分别构建路径规划和速度规划空间：基于静态和低速障碍物投影建立SL图，并对状态空间离散化，采用启发式搜索方法以及数值优化方法进行路径决策规划；基于动态障碍物轨迹预测，建立ST图并对状态空间离散化，采用启发式搜索方法以及数值优化的方法进行速度决策规划；其中，SL图中路径规划和ST图中速度规划均根据隧道场景设定不同的非均匀采样规模，首先进行状态空间离散化，依据代价函数分配各离散点的处的代价值，采用改进的A*算法启发式搜索快速得到初解；初解作为决策方案开辟了安全空间，基于安全空间原问题转换为凸优化问题，在约束条件下采用凸优化求解方法得到最优轨迹解；SL图中进行速度规划的数值优化过程中，代价函数为：
其中w
i
为代价函数中每一项的权重，l
i
、l
centre
分别表示SL图中的路径和参考线的纵向偏移量；代价函数的各项分别表示一阶导数代价、二阶导数代价、三阶导数代价、横向偏移代价、中心线代价，每一项前均有相应的代价权重，约束条件为：A
eq
x＝b
eq
Ax≤blb≤x≤ubx＝(l1,l
′1,l
″1,
…
l
n
,l
′
n
,l
″
n
)
T
式中x为状态量，A
eq
,b
eq
用于描述状态等式约束方程，Ax≤b描述的是状态不等式约束，lb,ub是状态x的取值范围下界与上界；对应的约束条件分别为连续性等式约束、碰撞不等式约束和上下界约束，通过求解器得到l1,l
′1,l
″1,
…
l
n
,l
′
n
,l
″
n
与s1,s2,s3,
…
s
n
结合的二次规划路径结果；ST图中进行速度规划的数值优化过程中，代价函数为：A
eq
x＝b
eq
Ax≤blb≤x≤ub式中x为状态量，A
eq
,b
eq
用于描述状态等式约束方程，Ax≤b描述的是状态不等式约束，lb,ub是状态x的取值范围下界与上界；w
i
为代价函数中每一项的权重，s
i
、v
ref
分别表示ST图中的路径和的横向位移量以及参考速度；式中对应的约束条件分别为连续性约束、速度和加速度不等式约束，经过求解器得到速度规划结果，合并路径规划结果完成轨迹规划；车辆根据局部规划结果沿着导航路径实现矿井下静态障碍物和动态障碍物的简单避让，并安全行驶至掉头规划起点区域。5.根据权利要求1所述的掉头规划方法，其于特征在于，步骤S3包括以下分步骤：S31，进行硐室规划区域离散化：离散化方法采用栅格地图来描述掉头规划起...

【专利技术属性】
技术研发人员：汪先伟，张祖锋，殷嘉伦，
申请(专利权)人：畅加风行苏州智能科技有限公司，
类型：发明
国别省市：