CN112519783A - 一种智能驾驶的自底向上平滑轨迹生成方法及系统 - Google Patents

Abstract

本发明公开了一种智能驾驶的自底向上平滑轨迹生成方法及系统，构建可行驶区域，对矿井巷道的壁沿进行特征提取，以此来辨别当前时刻掘进机行进路段；本发明通过在不同路段采用不同的参考点生成方法，保证了在直线路线参考点处于道路中间，在弯道路段参考点处于弯道内侧，大大提高了掘进机行驶过程中的安全性；后续再对生成的参考点进行预处理，得到参考点数量充足且便于跟踪的参考点序列。

Description

一种智能驾驶的自底向上平滑轨迹生成方法及系统

技术领域

本发明涉及智能驾驶矿下掘进机路径规划领域，特别是一种智能驾驶的自底向上平滑轨迹生成方法及系统。

背景技术

掘进机矿下参考线生成及平滑方法是矿下无人驾驶技术研究的重要内容之一，生成平滑的参考中心线有利于减小矿下掘进机的跟踪误差与能量损耗，同时也兼顾车辆的安全性与舒适性，具有重要研究价值。实现无人驾驶掘进机矿下路径规划首先需要对激光数据预处理得到原始巷道壁点云数据，然后生成参考中心线。然而，在实际场景中，仍有以下问题制约生成矿下车辆参考中心线的应用。

(1)地下矿用掘进机由于单一作业方式，一般在巷道内预设好的路径上反复运行，一旦参考中心线生成后，路径规划出的参考路径可以使用较长时间，但是需要综合考虑障碍物，巷道壁凹凸不平及矿用掘进机的运动特性，只有按照实际巷道特点规划出的参考中心线才是更加可靠、安全的。

(2)在矿下掘进机跟踪参考中心线行驶过程中，传统参考路径与巷道两侧沿壁的距离不平均，由于直线与弯道路径的跟踪方法不同，传统方法在主目标规划后再进行一次局部避障规划，二次规划后的参考路径对于车辆左右外廓特征点与左右巷道沿壁的距离无法得到很好的平均效果，车辆运行时相对某一侧沿壁有碰撞风险。

(3)传统方法对于生成矿下参考线主要考虑车辆当前位置与左右巷道沿壁的距离、与障碍物的距离以及巷道壁的道路曲率，但是并未考虑该参考轨迹是否适合掘进机跟踪，平滑后的路径可能不满足车辆运动特性与车辆执行器的饱和特性，使车辆在跟踪时误差较大甚至失控。

发明内容

本发明所要解决的技术问题是，针对现有技术不足，提供一种智能驾驶的自底向上平滑轨迹生成方法及系统，保证在直线路线参考点处于道路中间，在弯道路段参考点处于弯道内侧，提高掘进机行驶过程中的安全性。

为解决上述技术问题，本发明所采用的技术方案是：一种智能驾驶的自底向上平滑轨迹生成方法，包括以下步骤：

1)获取当前时刻掘进机行进路段参考点，对所有参考点进行预处理，得到参考点序列；

2)获取所述参考点序列中距离铰接车最近的路径参考点，并根据所述最近的路径参考点在所述参考点序列中的索引确定参考路径上的预瞄点；

3)计算掘进机当前位置与所述预瞄点之间的航向偏差角α；

4)令掘进机当前转向半径R等于期望转向半径R_desire，得到控制律：

其中ω_γ为铰接角角速度，单位为rad·s^-1；-ω_max≤ω_γ≤ω_max；ω_max为转向机构所能提供的最大铰接角角速度的绝对值，单位为rad·s^-1；v_f为前桥纵向速度，单位m·s^-1；l_f是前桥轮心到铰接点的距离，单位为m；θ_f为前桥横摆角速度，单位rad·s^-1；

5)利用下式更新车辆前桥中心坐标位置(x_f,y_f)、前桥横摆角θ_f、车辆铰接角γ：

为掘进机前桥纵向速度、掘进机前桥横向速度；其中，以前桥中心为参考点，纵向为车辆前进方向，横向为垂直于纵向，采用右手系建立车身坐标系；

6)根据重复步骤2)～步骤5)，当参考点序列遍历完成或掘进机停车时，输出平滑后的参考点序列，结束。

本发明首先对所有参考点进行预处理生成参考点序列，可以保证车辆在直线巷道路段跟踪中心线；在弯道路段跟踪巷道壁内侧，首先保证了车辆的安全性。然后对参考点序列进行自底向上的轨迹平滑，使得规划出的轨迹不仅满足车辆运动学约束，还考虑了车辆执行机构的饱和特性，更适合车辆进行底层跟踪控制。

步骤1)的实现过程包括：

A)采集掘进机在矿井作业过程中的环境信息，构建可行驶区域，提取所述可行驶区域的巷道壁沿并分辨左、右壁沿；

B)根据所述左、右壁沿判断行驶路段类别；所述行驶路段类别包括直道、弯道且可计算参考距离、弯道且不可计算参考距离；

根据不同的行驶路段类别生成行进路段参考点，并对所述参考点进行预处理，预处理后的参考点构成参考点序列。本发明对于不同行驶路段类别有着不同的参考点生成方法，这样生成的参考点可以尽可能的保证掘进机行驶过程中的安全性。

步骤A)的具体实现过程包括：

A1)建立车身右手坐标系，所述车身右手坐标系原点为掘进机质心位置(0,0)，激光雷达采集到的数据点相对掘进机的坐标为(x,y)；

A2)将数据点按照x<0和x≥0分类为S_l，S_r，并且每一类按照y进行升序排列，将S_l，S_r合并为一类S；

A3)寻找S中与掘进机距离小于r的数据点；

A4)在步骤A3)寻找到的数据点中寻找y值最接近于0的坐标点，其中x<0记录为左巷道壁起始点(x_l0,y_l0)，x＞0记录为右巷道壁起始点(x_r0,y_r0)；

A5)在S中寻找与左巷道壁起始点(x_l0,y_l0)距离小于r₁的数据点(x_l1,y_l1)，存储(x_l1,y_l1)至C_l，并在下一循环寻找与点(x_l1,y_l1)距离小于r₁的数据点(x_l2,y_l2)，以此循环直到遍历所有数据点，记录C_l为左边巷道壁沿；在S中寻找与右巷道壁起始点(x_r0,y_r0)距离小于r₁的数据点(x_r1,y_r1)，存储(x_r1,y_r1)至C_r，并在下一循环寻找与点(x_r1,y_r1)距离小于r₁的数据点(x_r2,y_r2)，以此循环直到遍历所有数据点，记录C_r为右边巷道壁沿。

本发明将巷道壁沿分为左壁沿和右壁沿后，在后续进行参考点生成时不必要每一次进行数据处理。因为在采用激光雷达采样时得到的数据点排列是散乱的，有可能某一个点时左壁沿的，而下一个点就变成右壁沿了。本发明的方法可以精确识别左壁沿和右壁沿。

步骤B)的具体实现过程包括：

B1)判断是否满足以下条件：C_l＝M1，numel(C_l)＜M2/C_r＝M1，numel(C_r)＜M2；若满足，则表示当前行驶路段为弯道，并标记z＝1，r＝0；其中，z＝1说明是弯道，r＝0说明参考点距离弯道内侧距离d需要事先给定；若不满足，标记z＝0，并进行如下步骤B2)的判断；

B2)分别找到C_l、C_r两类数据中y的最小值，并且在两个最小值中找到最大值y_mmin；分别找到C_l、C_r两类数据中y的最大值，并且在两个最大值中找到最小值y_mmax；基于y_mmin、y_mmax分别对左巷道壁和右巷道壁进行三次样条插值，y方向上间隔lm，生成一系列横坐标数据；l为设定的阈值；

计算从y_mmin开始，在y方向上每相隔k₁e的横坐标之差Δx_i＝x_r-x_l，k₁为设定的系数值，e＝y_mmax-y_mmin；下标r表示掘进机右侧巷道壁的数据点，下标l表示掘进机左侧巷道壁的数据点；

若：

|Δx_i-Δx_j|＞1&&|Δx_i-Δx_k|＞1，i＝1,2,3,4，j＝1,2,3,4，k＝1,2,3,4，i≠j≠k，则z＝1，r＝1；否则z＝0，r＝1；r＝1说明在弯道行驶时弯道内侧与参考线的距离可以计算得出。

通过本发明的方法，如果是直线，可以保证在后续参考点生成时参考点在行驶道路中间，安全性高。如果是弯道，可以保证在弯道中生成的参考点靠近弯道内侧，因为掘进机在弯道转弯时如果是在弯道中间进行转弯，车身外廓与外侧巷道壁沿距离过小，有剐蹭或碰撞危险，同样安全性要比在弯道中间转弯要高。

步骤C)中，对直道生成行进路段参考点的具体实现过程包括：

C11)分别在C_l、C_r两类数据中在y方向上每相隔k₂米找到最靠近x＝0的数据点并记录为C_nl、C_nr；

C12)分别对数据点C_nl、C_nr进行三次样条插值，在y方向上生成一系列等间隔的数据点C_sl、C_sr；

C13)分别找出C_sl、C_sr中y的最小值，并找出两个最小值中的最大值y_rmin；

C14)以y_rmin为起始纵坐标值，分别找到在C_sl、C_sr中与起始纵坐标值最接近y_rmin的数据点(x_cl,y_cl)、(x_cr,y_cr)，计算点(x_cl,y_cl)、(x_cr,y_cr)的中点(x₀,y₀)，(x₀,y₀)即为直线行驶的参考点，并记录C₀；

C15)将初始纵坐标值加上步长，重复步骤C14)，当纵坐标值加上步长后接近(等于)y_mmax时，生成最后一个参考点，结束。

由于巷道壁沿的不平整性，如果直接找两边巷道壁沿的中点作为参考点，会使得生成的参考点极度不平顺。这样在后续掘进机跟踪时是不利的。而采用本发明的方法生成的参考点在平顺性上要更好，并且参考点同样靠近巷道中间，安全性也有所保障。

步骤C)中，对弯道生成行进路段参考点的具体实现过程包括：

C21)判断是否满足以下条件：r＝1；若满足，则进入步骤C22)；若不满足，则给定参考距离

进入步骤C24)；

C22)分别找到C_l、C_r两类数据中y的最小值，并且在两个最小值中找到最大值y_mmin；

C23)分别在C_l、C_r两类数据中寻找y＝y_mmin，并找到对应的数据点(x_rl,y_mmin)、(x_rr,y_mmin)，则

C24)判断是否满足以下条件：k₁＝∞，若满足，则x₀＝x_m+d，y₀＝y_m；

若不满足，则：

y₀＝k₂·x₀+b₂

其中，若k₁≤0，则

否则

c＝y_m-k₁·x_m；

b₂＝y_m-k₂·x_m；(x_i,y_i)为同一边巷道壁沿连续两个数据点中其中一个数据点的坐标，(x_i+1,y_i+1)为(x_i,y_i)的下一个数据点的坐标；

C25)重复步骤C24)，直至循环到弯道内侧最后一个点。

本发明对弯道生成行进路段参考点的优势在于生成的参考点靠近弯道内侧，因为掘进机在弯道转弯时如果是在弯道中间进行转弯，车身外廓与外侧巷道壁沿距离过小，有剐蹭或碰撞危险。而靠近内侧可以大大降低该危险性，安全性有所提高。

对参考点进行预处理的方法包括：

i)若参考点距离巷道壁沿距离小于0.8d，则删除该参考点，否则保留；其中，d为参考点距离巷道壁沿的参考距离；

ii)对经步骤i)处理后的参考点进行三次样条插值，得到参考线；

iii)将所述参考线平移至车身参考系原点(0,0)，车身参考系是指掘进机前进方向为y轴正方向，前进方向朝右为x轴正方向；

iv)对平移后的参考点序列进行旋转变换，顺时针旋转90°。

虽然上述方法生成的参考点是平顺的，但是在弯道路段上生成的参考点中有小部分参考点距离弯道内侧过近，这样去用掘进机跟踪也会导致掘进机与巷道壁沿发生碰撞，这是弯道参考点生成方法的BUG，是由于巷道壁沿的不平顺导致的。因此本发明删除与巷道壁沿过近的参考点，降低碰撞的可能性，提高掘进机在弯道转弯时的安全性。

距离铰接车最近的路径参考点的更新过程包括：

a)选取所述参考点序列traj的第一个点作为初始最近参考点，将初始最近参考点在traj的位置索引index初始化为1；

b)确定局部参考路径点范围N_local，选取traj(index,index+N_local-1)的点序列作为掘进机局部参考轨迹local_traj；

c)计算车辆当前坐标位置与局部参考轨迹local_traj所有点的距离D_k：

d)选取欧式距离序列中最小的位置索引index_min：index_min＝D_k(min(D_k))；

e)利用下式更新最近参考点位置索引，得到更新后的位置索引index^*：index^*＝index+index_min-1。

本发明每次更新最近的路径参考点的优势是可以更快的找到当前车辆位姿与参考轨迹的最短距离，并且有利于找到新的预瞄点并计算跟踪误差。

根据所述最近的路径参考点在所述参考点序列中的索引确定参考路径上的预瞄点的具体实现过程包括：

I)确定预瞄距离l_pre的取值；

II)计算最近参考点后相邻两点的欧式距离D_i，i＝index_min+1...n；

III)根据以下公式寻找预瞄点的位置索引index_p：

其中index_p为预瞄点在局部参考轨迹local_traj中的位置索引；local_traj(index_p)即为更新后的预瞄点位置。

确定参考路径上的预瞄点对于跟踪控制十分重要，若车辆以最近的路径点作为参考点进行跟踪，会使得轨迹连续振荡，因为此时的航向误差过大；而在最近的路径点基础上往前预瞄一段距离，并将预瞄点作为参考点跟踪会使得控制器输出的控制量更平顺且跟踪更稳定。

作为一个发明构思，本发明还提供了一种矿井下掘进机智能驾驶的自底向上平滑轨迹生成系统，包括计算机设备；所述计算机设备被配置或编程为用于上述方法的步骤。

与现有技术相比，本发明所具有的有益效果为：本发明通过激光雷达采集环境信息，构建可行驶区域，对矿井巷道的壁沿进行特征提取，以此来辨别当前时刻掘进机行进路段；通过在不同路段采用不同的参考点生成方法，保证了在直线路线参考点处于道路中间，在弯道路段参考点处于弯道内侧，大大提高了掘进机行驶过程中的安全性；后续再对生成的参考点进行预处理，得到参考点数量充足且便于跟踪的参考点序列；本发明采用一种自底向上的平滑轨迹生成方法，通过将待平滑参考点序列输入给底层控制系统，输出满足掘进机运动学约束以及执行器饱和特性的平滑点序列作为上层规划出的参考轨迹，在后续采用其他高级控制器跟踪该轨迹时跟踪效果更好，并且平滑后的参考轨迹与原参考轨迹在行驶路段上的位置基本一致，在保持轨迹平滑的同时拥有更高的安全性。

附图说明

图1为无人驾驶掘进机的参考线生成以及运动学平滑流程图；

图2为弯道参考点计算示意图；

图3为纯跟踪控制器框图；

图4为直线参考线采用运动学平滑前后效果对比图；

图5为弯道参考线采用运动学平滑前后效果对比图。

具体实施方式

下面对本发明的实施例作详细说明，本实施例以本发明的技术方案为依据开展，给出了详细的实施方式和具体的操作过程，对本发明的技术方案作进一步解释说明。

本发明实施例提供的一种矿井下掘进机智能驾驶的自底向上平滑轨迹生成方法，如图1所示。包括以下步骤：

步骤一：装配有激光雷达的掘进机在矿井内自主行驶时，采集矿井作业过程中激光雷达环境信息，构建可行驶区域，并对矿井巷道的壁沿进行特征提取：

步骤1，根据上层得到的感知信息建立巷道壁，构建掘进机可行驶区域，并提取区域内矿井巷道的壁沿；

步骤2，建立车身右手坐标系，原点为掘进机质心位置(0,0)，激光雷达采集到的数据点相对掘进机的坐标为(x,y)；

步骤3，将数据点按照x<0和x＞0分类为S_l，S_r，并且每一类按照y进行升序排列，将合并为一类S；

步骤4，寻找与掘进机距离小于r的数据点并记录，

步骤5，在记录的数据点中寻找y值最接近于0的坐标点，其中x<0记录为左巷道壁起始点(x_l0,y_l0)，x＞0记录为右巷道壁起始点(x_r0,y_r0)；

步骤6，对数据点进行分类分为左巷道壁数据点和右巷道壁数据点，具体过程如下：

步骤6.1，在S中寻找与左巷道壁起始点(x_l0,y_l0)距离小于r₁的数据点(x_l1,y_l1)，存储(x_l1,y_l1)至C_l，并在下一循环寻找与点(x_l1,y_l1)距离小于r₁的数据点(x_l2,y_l2)，以此循环直到遍历所有数据点，记录C_l为左边巷道壁沿；

步骤6.2，在S中寻找与右巷道壁起始点(x_r0,y_r0)距离小于r₁的数据点(x_r1,y_r1)，存储(x_r1,y_r1)至C_r，并在下一循环寻找与点(x_r1,y_r1)距离小于r₁的数据点(x_r2,y_r2)，以此循环直到遍历所有数据点，记录C_r为右边巷道壁沿；

步骤7，对C_l、C_r中的数据点进行计数，当数据点数少于50个则舍去，默认无该边界，并且记录为C_l＝111，numel(C_l)＜50/C_r＝111，numel(C_r)＜50。

其中，numel(C_l)、numel(C_r)分别表示对C_l、C_r进行计数。

步骤二：根据提取的壁沿进行特征分析，辨别当前时刻掘进机行进路段：

步骤1，判断是否满足以下条件：C_l＝111，numel(C_l)＜50/C_r＝111，numel(C_r)＜50；若满足，则表示当前行驶路段为弯道，并标记z＝1，r＝0；

其中，z＝1说明是弯道，r＝0说明参考线距离弯道内侧距离d需要事先给定：

其中，L_vw为掘进机车身宽度；

若不满足，标记z＝0，并且进行下一个判断；

步骤2，判断步骤如下：

步骤2.1，分别找到C_l、C_r两类数据中y的最小值，并且在两个最小值中找到最大值：

y_mmin＝max{min(y_l),min(y_r)}

其中，y_l、y_r分别为C_l、C_r两类数据中的y值；

步骤2.2，分别找到C_l、C_r两类数据中y的最大值，并且在两个最大值中找到最小值：

y_mmax＝min{max(y_l),max(y_r)}

步骤2.3，基于y_mmin、y_mmax分别对左巷道壁和右巷道壁进行三次样条插值，y方向上间隔0.01m，生成一系列横坐标数据；

步骤2.4，计算从y_mmin开始，在y方向上每相隔0.25e的横坐标之差，分别为Δx₁、Δx₂、Δx₃、Δx₄；

e＝y_mmax-y_mmin

Δx_i＝x_r-x_l

步骤2.5，若Δx_i、Δx_j、Δx_k满足下列式子：

|Δx_i-Δx_j|＞1&&|Δx_i-Δx_k|＞1，i＝1,2,3,4，j＝1,2,3,4，k＝1,2,3,4，i≠j≠k则z＝1，r＝1；若不满足，则z＝0，r＝1；

其中，r＝1说明在弯道行驶时弯道内侧与参考线的距离可以计算得出。

步骤三：生成行进路段参考点，对参考点进行预处理：

步骤1，若行进路线为直线，则给定标志：z＝0；若行进路线为弯道并且可计算参考距离，即为参考点距离弯道内侧壁沿的最近距离，给定标志：z＝1&&r＝1；若行进路段为弯道并且不可计算参考距离，则给定标志：z＝1&&r＝0。

其中，z＝1说明是行进路段为弯道，z＝0说明行进路线为直线；r＝1说明参考点距离巷道壁沿的参考距离d可计算，r＝0说明参考距离不可计算，需要直接给定。

步骤2，对直线参考点的生成方法如下：

步骤2.1，分别在C_l、C_r两类数据中在y方向上每相隔1米找到最靠近x＝0的数据点并记录为C_nl、C_nr；

其中，C_l表示左边巷道壁沿，C_r表示右边巷道壁沿；

步骤2.2，分别对数据点C_nl、C_nr进行三次样条插值，在y方向上生成一系列等间隔的数据点C_sl、C_sr；

步骤2.3，分别找出C_sl、C_sr中y的最小值，并找出两个最小值中的最大值；

y_rmin＝max{min(y_sl),min(y_sr)}

y_sl、y_sr分别为C_sl、C_sr中的y值；

步骤2.4，以y_rmin为起始y值，分别找到在C_sl、C_sr中y值最接近y_rmin的数据点(x_cl,y_cl)、(x_cr,y_cr)，求出它们的中点

y₀＝y_rmin，(x₀,y₀)即为直线行驶的参考点，并记录C₀；

步骤2.5，重复步骤4，每一次的y值均为上一次循环后的y值加上0.1，即y_i+1＝y_i+0.1，(y₁＝y_rmin)；

步骤3，对弯道参考点的生成方法如下：

步骤3.1，判断是否满足以下条件：r＝1；若满足，则计算参考距离；若不满足，则给定参考距离

其中，L_vw为掘进机车身宽度；

参考距离计算步骤如下：

步骤3.1.1，分别找到C_l、C_r两类数据中y的最小值，并且在两个最小值中找到最大值：

y_mmin＝max{min(y_l),min(y_r)}

步骤3.1.2，分别在C_l、C_r两类数据中寻找y＝y_mmin，并找到对应的数据点(x_rl,y_mmin)、(x_rr,y_mmin)；

步骤3.1.3，计算参考距离d。

步骤3.2，计算同一边巷道壁沿连续两个数据点的斜率和中点坐标：

其中，(x_i,y_i)为其中一个数据点的坐标，(x_i+1,y_i+1)为下一个数据点的坐标；

步骤3.3，根据直线方程y＝k·x+c，计算出c的值c＝y_m-k₁·x_m；

步骤3.4，判断是否满足以下条件：k₁≤0，若满足，则

若不满足，则

步骤3.5，计算

b₂＝y_m-k₂·x_m；

步骤3.6，判断是否满足以下条件：k₁＝∞，若满足，则x₀＝x_m+d，y₀＝y_m；若不满足，则

y₀＝k₂·x₀+b₂

其中(x₀,y₀)为参考点；

步骤3.7，重复步骤3.2到步骤3.6，并记录(x₀,y₀)为S₀。

步骤4，步骤3.6中弯道参考点的计算示意图如图2所示，计算算法如下：

已知两点(x_i,y_i)和(x_i+1,y_i+1)，设过两点的直线方程l₁：y＝k₁x+c，两点的中点(x_m,y_m)

过中点并且垂直l₁的直线方程设为l₂：y＝k₂x+b₂，平行l₁且过参考点的直线方程设为l₃：y＝k₁x+c+b₁；

计算k₁、c：

计算k₂、b₂：

计算b₁：

计算(x₀,y₀)：

y₀＝k₂·x₀+b₂

其中(x₀,y₀)为参考点；

步骤5，对生成后的参考点预处理方法如下：

步骤5.1，若参考点距离巷道壁沿距离小于0.8d，则删除该参考点，否则保留；

其中，d为参考点距离巷道壁沿的参考距离；

步骤5.2，对处理后的参考点进行三次样条插值，得到参考点足够多、便于跟踪的参考点序列；

步骤5.3，对生成的参考线平移至车身参考系原点(0,0)，车身参考系是指掘进机前进方向为y轴正方向，前进方向朝右为x轴正方向；

步骤5.4，对平移后的参考点序列进行旋转变换，顺时针旋转90°。

x_i+1＝x_i·cos(-90°)-y_i·sin(-90°)

y_i+1＝x_i·sin(-90°)+y_i·cos(-90°)

步骤四：设计底层控制系统，自底向上生成满足掘进机运动学约束及执行器饱和特性的平滑参考点：

步骤1，底层控制系统框图如图3所示，对底层控制系统的设计方法如下：

步骤1.1，以掘进机前桥轮轴中心为参考点，选取掘进机全局坐标位置、航向角、铰接角作为状态变量：

χ＝(x_f,y_f,θ_f,γ)^T

其中，(x_f,y_f)为前桥坐标位置，(m,m)，θ_f为前桥横摆角速度，rad·s^-1，γ为掘进机铰接角，rad。

步骤1.2，选取前桥纵向车速、铰接角角速度作为掘进机控制输入变量：

u＝(v_f,ω_γ)^T

其中，v_f为前桥纵向速度，m·s^-1，ω_γ为车辆铰接角速度，也即车辆转向角速度，即为

rad·s^-1。

步骤1.3，建立掘进机运动学状态空间模型：

其中，l_f,l_r分别是前后桥轮心到铰接点的距离，m,m。

步骤1.4，设计纯跟踪控制器，首先建立预瞄模型，计算距离铰接车最近的路径参考点min_dis(x_mind,y_mind)。

步骤1.5，选取预瞄距离l_pre，公式如下：

l_pre＝K·v_f

其中，K为比例系数，v_f为前桥纵向速度，m·s^-1。

步骤1.6，根据预瞄距离l_pre确定参考路径上的预瞄点pre(x_p,y_p)，

步骤1.7，计算当前车辆位置与预瞄点间的航向偏差角α：

其中α为航向偏差角，rad，(x_p,y_p)为预瞄点坐标，(x,y)为车辆当前位置，θ_f为掘进机当前航向角，rad。

步骤1.8，根据掘进机角速度线速度关系计算掘进机当前转向半径R：

其中

为车辆横摆角速度，rad·s^-1，v_f为前桥纵向速度，m·s^-1，R为车辆当前转向半径，m。

步骤1.9，根据几何关系计算掘进机期望转向半径R_desire：

步骤1.10，根据步骤1.3,1.5,1.7,1.8,1.9计算横向控制输入w_γ，令掘进机当前转向半径R等于期望转向半径R_desire，可得控制律：

其中ω_γ为铰接角角速度，rad·s^-1。

步骤1.11，限定横向控制输入w_γ的取值范围，使平滑轨迹满足掘进机执行器饱和特性约束：

-ω_max≤ω_γ≤ω_max

其中，ω_max为转向机构所能提供的最大铰接角角速度的绝对值，rad·s^-1。

步骤2，自底向上的参考线平滑方法如下：

步骤2.1，根据设计的底层控制系统，选取控制器为权利要求所述纯跟踪控制器，被控对象为掘进机运动学模型。

步骤2.2，将控制器输出与被控对象输入建立连接，被控对象输出与控制器输入建立连接，构成反馈闭环系统。

步骤2.3，将待平滑的参考轨迹输入给步骤2.2所述闭环系统，让掘进机跟踪待平滑的参考轨迹。

步骤2.4，记录掘进机跟踪结果，获取其实际走过的坐标点构成平滑后的参考点序列。

步骤五：运行闭环控制系统，更新平滑参考点，生成平滑后的参考点序列：

步骤1，平滑参考点更新方法如下：

步骤1.1，更新距离铰接机最近的路径参考点min_dis(x_mind,y_mind)。

步骤1.2，更新预瞄点位置(x_p，y_p)。

步骤1.3，计算控制器输出，并作用于掘进机运动学模型，输出车辆下一时刻的位姿即为平滑后的参考点信息。

步骤2，更新最近路径参考点的方法如下：

步骤2.1，选取平滑前的参考路径序列traj的第一个点作为初始最近参考点，将最近参考点在traj的位置索引index初始化为1。

步骤2.2，确定局部参考路径点范围N_local，选取traj(index,index+N_local-1)的点序列作为掘进机局部参考轨迹local_traj。

步骤2.3，计算车辆当前坐标位置与局部参考轨迹local_traj所有点的距离：

其中D_k为欧式距离序列，(x,y)为车辆当前坐标位置，(x_k,y_k)为参考点坐标位置，n为局部参考轨迹local_traj的长度。

步骤2.4，选取欧式距离序列中最小的位置索引index_min：

index_min＝D_k(min(D_k))

步骤2.5，更新最近参考点位置索引：

index＝index+index_min-1

步骤3，预瞄点位置更新方法如下：

步骤3.1，首先确定预瞄距离l_pre的取值。

步骤3.2，计算最近参考点后相邻两点的欧式距离：

步骤3.3，根据以下公式寻找预瞄点的位置索引index_p：

其中index_p为预瞄点在局部参考轨迹local_traj中的位置索引。通过累加参考点后相邻点的距离，直到累加和大于预瞄距离l_pre，便找到了预瞄点在参考轨迹中的位置索引，local_traj(index_p)即为更新后的预瞄点位置。

最后，分别对直线参考线以及弯道参考线进行平滑，平滑后的参考线如图4、图5所示。

本发明通过激光雷达采集环境信息，构建可行驶区域，对矿井巷道的壁沿进行特征提取，以此来辨别当前时刻掘进机行进路段；通过在不同路段采用不同的参考点生成方法，保证了在直线路线参考点处于道路中间，在弯道路段参考点处于弯道内侧，大大提高了掘进机行驶过程中的安全性；后续再对生成的参考点进行预处理，得到参考点数量充足且便于跟踪的参考点序列；

本发明采用一种自底向上的平滑轨迹生成方法，通过将待平滑参考点序列输入给底层控制系统，输出满足掘进机运动学约束以及执行器饱和特性的平滑点序列作为上层规划出的参考轨迹，在后续采用其他高级控制器跟踪该轨迹时跟踪效果更好，并且平滑后的参考轨迹与原参考轨迹在行驶路段上的位置基本一致，在保持轨迹平滑的同时拥有更高的安全性。

Claims (10)

1.一种智能驾驶的自底向上平滑轨迹生成方法，其特征在于，包括以下步骤：

1)获取当前时刻掘进机行进路段参考点，对所有参考点进行预处理，得到参考点序列；

2)获取所述参考点序列中距离铰接车最近的路径参考点，并根据所述最近的路径参考点在所述参考点序列中的索引确定参考路径上的预瞄点；

3)计算掘进机当前位置与所述预瞄点之间的航向偏差角α；

4)令掘进机当前转向半径R等于期望转向半径R_desire，得到控制律：

其中ω_γ为铰接角角速度，单位为rad·s^-1；-ω_max≤ω_γ≤ω_max；ω_max为转向机构所能提供的最大铰接角角速度的绝对值，单位为rad·s^-1；v_f为前桥纵向速度，单位m·s^-1；l_f是前桥轮心到铰接点的距离，单位为m；θ_f为前桥横摆角速度，单位rad·s^-1；

5)利用下式更新车辆前桥中心坐标位置(x_f,y_f)、前桥横摆角θ_f、车辆铰接角γ：

为掘进机前桥纵向速度、掘进机前桥横向速度；

6)根据重复步骤2)～步骤5)，当参考点序列遍历完成或掘进机停车时，输出平滑后的参考点序列，结束。

2.根据权利要求1所述的智能驾驶的自底向上平滑轨迹生成方法，其特征在于，步骤1)的实现过程包括：

A)采集掘进机在矿井作业过程中的环境信息，构建可行驶区域，提取所述可行驶区域的巷道壁沿并分辨左、右壁沿；

B)根据所述左、右壁沿判断行驶路段类别；所述行驶路段类别包括直道、弯道且可计算参考距离、弯道且不可计算参考距离；

C)根据不同的行驶路段类别生成行进路段参考点，并对所述参考点进行预处理，预处理后的参考点构成参考点序列。

3.根据权利要求2所述的智能驾驶的自底向上平滑轨迹生成方法，其特征在于，步骤A)的具体实现过程包括：

A1)建立车身右手坐标系，所述车身右手坐标系原点为掘进机质心位置(0,0)，激光雷达采集到的数据点相对掘进机的坐标为(x,y)；

A2)将数据点按照x<0和x≥0分类为S_l，S_r，并且每一类按照y进行升序排列，将S_l，S_r合并为一类S；

A3)寻找S中与掘进机距离小于r的数据点；

A4)在步骤A3)寻找到的数据点中寻找y值最接近于0的坐标点，其中x<0记录为左巷道壁起始点(x_l0,y_l0)，x＞0记录为右巷道壁起始点(x_r0,y_r0)；

A5)在S中寻找与左巷道壁起始点(x_l0,y_l0)距离小于r₁的数据点(x_l1,y_l1)，存储(x_l1,y_l1)至C_l，并在下一循环寻找与点(x_l1,y_l1)距离小于r₁的数据点(x_l2,y_l2)，以此循环直到遍历所有数据点，记录C_l为左边巷道壁沿；在S中寻找与右巷道壁起始点(x_r0,y_r0)距离小于r₁的数据点(x_r1,y_r1)，存储(x_r1,y_r1)至C_r，并在下一循环寻找与点(x_r1,y_r1)距离小于r₁的数据点(x_r2,y_r2)，以此循环直到遍历所有数据点，记录C_r为右边巷道壁沿。

4.根据权利要求3所述的智能驾驶的自底向上平滑轨迹生成方法，其特征在于，步骤B)的具体实现过程包括：

B1)判断是否满足以下条件：C_l＝M1，numel(C_l)＜M2/C_r＝M1，numel(C_r)＜M2；若满足，则表示当前行驶路段为弯道，并标记z＝1，r＝0；其中，z＝1说明是弯道，r＝0说明参考点距离弯道内侧距离d需要事先给定；若不满足，标记z＝0，并进行如下步骤B2)的判断；

B2)分别找到C_l、C_r两类数据中y的最小值，并且在两个最小值中找到最大值y_mmin；分别找到C_l、C_r两类数据中y的最大值，并且在两个最大值中找到最小值y_mmax；基于y_mmin、y_mmax分别对左巷道壁和右巷道壁进行三次样条插值，y方向上间隔lm，生成一系列横坐标数据；l为设定的阈值；

计算从y_mmin开始，在y方向上每相隔k₁e的横坐标之差Δx_i＝x_r-x_l，k₁为设定的系数值，e＝y_mmax-y_mmin；下标r表示掘进机右侧巷道壁的数据点，下标l表示掘进机左侧巷道壁的数据点；

若：

|Δx_i-Δx_j|＞1&&|Δx_i-Δx_k|＞1，i＝1,2,3,4，j＝1,2,3,4，k＝1,2,3,4，i≠j≠k，则z＝1，r＝1；否则z＝0，r＝1；r＝1说明在弯道行驶时弯道内侧与参考线的距离可以计算得出。

5.根据权利要求3所述的智能驾驶的自底向上平滑轨迹生成方法，其特征在于，步骤C)中，对直道生成行进路段参考点的具体实现过程包括：

C11)分别在C_l、C_r两类数据中在y方向上每相隔k₂米找到最靠近x＝0的数据点并记录为C_nl、C_nr；

C12)分别对数据点C_nl、C_nr进行三次样条插值，在y方向上生成一系列等间隔的数据点C_sl、C_sr；

C13)分别找出C_sl、C_sr中y的最小值，并找出两个最小值中的最大值y_rmin；

C14)以y_rmin为起始纵坐标值，分别找到在C_sl、C_sr中与起始纵坐标值最接近y_rmin的数据点(x_cl,y_cl)、(x_cr,y_cr)，计算点(x_cl,y_cl)、(x_cr,y_cr)的中点(x₀,y₀)，(x₀,y₀)即为直线行驶的参考点，并记录C₀；

C15)将初始纵坐标值加上步长，重复步骤C14)，当纵坐标值加上步长后等于y_mmax时，生成最后一个参考点，结束。

6.根据权利要求3所述的智能驾驶的自底向上平滑轨迹生成方法，其特征在于，步骤C)中，对弯道生成行进路段参考点的具体实现过程包括：

C21)判断是否满足以下条件：r＝1；若满足，则进入步骤C22)；若不满足，则给定参考距离

进入步骤C24)；

C22)分别找到C_l、C_r两类数据中y的最小值，并且在两个最小值中找到最大值y_mmin；

C23)分别在C_l、C_r两类数据中寻找y＝y_mmin，并找到对应的数据点(x_rl,y_mmin)、(x_rr,y_mmin)，则

C24)判断是否满足以下条件：k₁＝∞，若满足，则x₀＝x_m+d，y₀＝y_m；若不满足，则：

其中，若k₁≤0，则

否则

c＝y_m-k₁·x_m；

b₂＝y_m-k₂·x_m；(x_i,y_i)为同一边巷道壁沿连续两个数据点中其中一个数据点的坐标，(x_i+1,y_i+1)为(x_i,y_i)的下一个数据点的坐标；

C25)重复步骤C24)，直至循环到弯道内侧最后一个点。

7.根据权利要求1所述的智能驾驶的自底向上平滑轨迹生成方法，其特征在于，对参考点进行预处理的方法包括：

i)若参考点距离巷道壁沿距离小于0.8d，则删除该参考点，否则保留；其中，d为参考点距离巷道壁沿的参考距离；

ii)对经步骤i)处理后的参考点进行三次样条插值，得到参考线；

iii)将所述参考线平移至车身参考系原点(0,0)，车身参考系是指掘进机前进方向为y轴正方向，前进方向朝右为x轴正方向；

iv)对平移后的参考点序列进行旋转变换，顺时针旋转90°。

8.根据权利要求1～7之一所述的智能驾驶的自底向上平滑轨迹生成方法，其特征在于，距离铰接车最近的路径参考点的更新过程包括：

a)选取所述参考点序列traj的第一个点作为初始最近参考点，将初始最近参考点在traj的位置索引index初始化为1；

b)确定局部参考路径点范围N_local，选取traj(index,index+N_local-1)的点序列作为掘进机局部参考轨迹local_traj；

c)计算车辆当前坐标位置与局部参考轨迹local_traj所有点的距离D_k：

d)选取欧式距离序列中最小的位置索引index_min：index_min＝D_k(min(D_k))；

e)利用下式更新最近参考点位置索引，得到更新后的位置索引index^*：index^*＝index+index_min-1。

9.根据权利要求8所述的智能驾驶的自底向上平滑轨迹生成方法，其特征在于，根据所述最近的路径参考点在所述参考点序列中的索引确定参考路径上的预瞄点的具体实现过程包括：

I)确定预瞄距离l_pre的取值；

II)计算最近参考点后相邻两点的欧式距离D_i，i＝index_min+1...n；

III)根据以下公式寻找预瞄点的位置索引index_p：

其中index_p为预瞄点在局部参考轨迹local_traj中的位置索引；local_traj(index_p)即为更新后的预瞄点位置。

10.一种智能驾驶的自底向上平滑轨迹生成系统，其特征在于，包括计算机设备；所述计算机设备被配置或编程为用于执行权利要求1～9之一所述方法的步骤。

Images