Research on path planning methods for autonomous dump trucks in open-pit mines
-
摘要:
针对矿用自卸车在露天矿场环境下的路径左行要求以及长距离运输路径的平滑效率问题,提出了基于Clothoid曲线拓展的左行混合A*算法和离散点对角向量模最小化平滑算法相结合的路径规划方法。首先,在混合A*算法节点拓展搜索过程中,采用Clothoid曲线代替传统圆弧进行拓展,以保证混合A*搜索路径的曲率连续性和曲率变化率限制要求。然后,利用左行规则改进混合A*算法累加代价和启发代价,在累加代价中加入左转代价和碰撞吸引代价保证路径向左和沿地图边界左侧进行规划,在启发代价中根据拓展方向与地图边界最近点的左右位置关系调整拓展代价得到利于左行的启发代价图,以生成左行全局粗略路径。最后,结合二次规划技术,以离散点对角向量模最小化为目标函数,坐标轴方向移动量为约束,构建平滑优化模型,对全局路径进行平滑,同时为防止平滑过程中曲率超限超出车辆转向响应能力,利用大曲率点可行域隧道缩减技术,限制大曲率点平滑可移动量。试验结果表明:所提方法可以生成适应矿区左行规则的沿地图左侧边界行驶的全局路径;启发代价的改进有效地减少了混合A*算法拓展的节点数量,有利于更好实现左行;离散点路径平滑方法有效提高了全局路径的平滑度,利于车辆控制跟踪;Clothoid曲线拓展与可行域隧道缩减技术可有效应对装载、卸载等大曲率路径,不存在曲率超限情况;对比不同长度路径规划耗时数据,路径平滑算法有效降低了路径规划的总耗时,4 km路径平滑耗时只需76 ms;左行规划的实现和总耗时的减少提高了路径对于矿区场景的适应性。
Abstract:To address the requirements for left-side driving and the issue of smoothing efficiency for long-distance transportation routes of mining dump trucks in open-pit mine environments, a path planning method that combines a left-side driving hybrid A* algorithm based on Clothoid curve expansion with a smoothing algorithm that minimizes the diagonal vector norm of discrete points is proposed. Initially, Clothoid curves are used instead of traditional circular arcs in the hybrid A* node expansion search process, ensuring the continuity of curvature and meeting the requirements for curvature change rate limitations in the hybrid A* search path. The left-side driving rules are then applied to improve both the cumulative cost and heuristic cost within the hybrid A* framework, incorporating left-turn costs and collision attraction costs to ensure the path veers leftward and follows along the left side of the map border. The heuristic cost is adjusted based on the relation of the expansion direction to the nearest point on the map border, facilitating a heuristic cost map that favors left-side driving and generates a rough global path for leftward travel. Finally, by integrating quadratic programming techniques with the objective of minimizing the diagonal vector norm of discrete points under movement constraints along the coordinate axes, a smoothing optimization model is constructed to smooth the global path. To prevent the curvature from exceeding the vehicle's steering response capability during the smoothing process, a tunnel reduction technique for the feasible region of high-curvature points is employed, limiting the smooth movement of high-curvature points. The results show that the proposed method can generate a global path suitable for left-side driving rules in mining areas, which follows the left side boundary of the map. The heuristic cost improvement significantly reduces the number of nodes expanded by the hybrid A*, favoring the implementation of left-side driving. The discrete point path smoothing method effectively enhances the smoothness of the global path, aiding in vehicle control and tracking. Moreover, thanks to Clothoid curve expansion and the feasible region tunnel reduction technique, the method can effectively handle high-curvature paths, such as loading and unloading, without curvature exceeding limits. Comparing the planning times for paths of different lengths, the path smoothing algorithm significantly reduces the total planning time, with a 4-kilometer path smoothing process requiring only 76 milliseconds. The execution of left-side planning and the reduction in planning time improve the adaptability of the path planning for mining area scenarios.
-
0. 引 言
2020年2月,国家八部委联合印发的《关于加快煤矿智能化发展的指导意见》中提到:到2025年,露天煤矿实现智能连续作业和无人化运输[1-2]。研究可提高露天矿山智能性、安全性和运行效率的无人驾驶系统成为实现无人化运输的关键[3]。无人驾驶系统主要包括环境感知、行为决策、路径规划、运动控制等几大部分,路径规划作为其中的关键一环,其任务是根据环境感知模块接收的矿山环境信息以及采集的数字地图信息,为矿卡规划一条安全、可行驶的路径[4-5]。因此,研究适应露天矿场环境的无人驾驶自卸车路径规划方法具有重要的现实意义。
面向智能车辆的路径规划方法主要包括采样方法、人工势场方法、曲线插值方法、数值优化方法、图搜索方法等[6-8]。各方法都有其优缺点及适用场景,需要结合矿区场景特性进行分析。基于采样的路径规划算法通常通过在配置空间内随机撒点进行路径拓展[9],随机撒点采样方式很难充分利用矿区的全局地图静态信息,如地图边界、作业区域等信息。人工势场方法通过对终点建立引力场,障碍物建立斥力场的方式进行规划[10],终点引力场大小通常通过距离进行衡量,难以适应露天矿场左行、非距离最短路径的情况。曲线插值方法[11]和数值优化方法[12]依赖于全局参考路径点进行规划,矿区场景道路变化快,参考路径点需要频繁采集和编辑,增加了维护难度。图搜索方法基于预先建立的状态空间图,从当前节点向四周拓展搜索,搜索分辨率高,能够很好地利用矿区地图的全局静态信息。图搜索方法中的A*算法相比于贪婪最佳优先搜索、Dijkstra等同类算法,通过累加代价和启发代价兼顾路径最优性和效率[6]。混合A*算法在A*算法的基础上进一步考虑了车辆运动学约束,适合具有非完整约束的矿用自卸车规划[13-14]。同时启发代价和累加代价计算过程中可以充分利用矿区地图边界等静态信息,适合矿区场景的规划。
基于混合A*算法搜索出来的粗略路径并不平滑[15],需要利用路径平滑算法对路径进行进一步平滑,而矿区的排队路径通常较长,以运煤为例单程可达4 ~ 5 km,高效的路径平滑尤其重要。文献[14]利用混合A*算法进行矿区场景路径规划,主要对终点拟合曲线及其筛选进行了改进,并没有解决算法适应矿区左行规则要求的问题。同时在现场实际应用过程中发现由于样条曲线平滑算法对曲率的限制效果较差,导致不能对连续曲率RS (Continuous Curvature Reeds-Shepp,CCRS)曲线[16]进行平滑,影响了路径的整体平滑度。随着矿区运煤长距离场景的应用,平滑效率也需要进一步提升。
针对上述问题,结合矿区场景的特性,进一步对混合A*算法进行适应性改进:①提出基于Clothoid曲线[17]拓展的左行混合A*算法,可生成起点到终点的符合露天矿区左行规则的全局路径。②提出基于二次规划的离散点平滑技术,平滑模型简单,效率高,适合长距离路径的平滑,同时为保证大曲率路径处平滑后曲率不超限,其对大曲率点进行检测并控制平滑效果。
1. Clothoid曲线拓展及左行混合A*算法
传统混合A*算法主要用于规划起点到终点的最短路径[18],而矿区要求车辆遵守左行规则,规划的路径需要尽快的回归到道路左侧,沿着道路的左侧进行规划。通过改进混合A*算法的代价计算方式实现左行,同时为了提高路径拓展的曲率连续性和确保曲率变化率限制,将传统混合A*算法的圆弧形式节点拓展改为Clothoid曲线节点拓展。
1.1 Clothoid曲线拓展
传统混合A*算法子节点的拓展结合了满足阿克曼转向约束的自行车模型,车辆在状态空间中的状态由后轴中心位置$(x,y)$、车身横摆角$\theta $以及相应的曲率$k$表示,即$(x,y,\theta ,k)$。如图1所示,将车辆前轮转角离散为9个固定角度,车辆前轮转角确定后,以一定的步长沿圆弧向前拓展可得到0 ~ 8序号子节点[19]。该方式虽然考虑到了车辆运动学,但在节点处车轮转角可能有较大的突变,从而导致生成的路径有较大的曲率突变。
为了改善此情况,采用Clothoid曲线进行节点拓展,Clothoid曲线曲率随弧长线性变化,变化快慢可由曲率变化率$\sigma $进行控制。假设${{\boldsymbol{q}}_{n - 1}}$为父节点n−1的状态,子节点n的状态${{\boldsymbol{q}}_n}$可由式(1)计算[16]:
$$ \begin{gathered} {{\boldsymbol{q}}_n} = \left[ {\begin{array}{*{20}{c}} {{x_n}} \\ {{y_n}} \\ {{\theta _n}} \\ {{k_n}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{x_{n - 1}}} \\ {{y_{n - 1}}} \\ {{\theta _{n - 1}}} \\ {{k_{n - 1}}} \end{array}} \right] + \\ \left[ {\begin{array}{*{20}{c}} {\mathop \int_0^L {d_{n - 1}}\;\cos ({\theta _{n - 1}} + {d_{n - 1}}{k_{n - 1}}l + {d_{n - 1}}{\sigma _{{\text{dif}}}}{l^2}/2){\text{d}}l} \\ {\mathop \int_0^L {d_{n - 1}}\;\sin ({\theta _{n - 1}} + {d_{n - 1}}{k_{n - 1}}l + {d_{n - 1}}{\sigma _{{\text{dif}}}}{l^2}/2){\text{d}}l} \\ {{d_{n - 1}}{k_{n - 1}}L + {d_{n - 1}}{\sigma _{{\text{dif}}}}{L^2}/2} \\ {L{\sigma _{{\text{dif}}}}} \end{array}} \right] \\ \end{gathered} $$ (1) 式中:积分可用菲涅尔函数进行计算;L为拓展的步长;$d_n$为车辆前进后退,$d_n \in \{ 1, - 1\} $(前进,后退);l为长度积分变量;${\sigma _{{\text{dif}}}} = ({k_n} - {k_{n - 1}})/L$代表相邻父子节点的曲率变化率。
${\sigma _{{\text{dif}}}}$为预先计算的,如果${\sigma _{{\text{dif}}}}$大于最大曲率变化率$ {\sigma _{\max }} $则放弃该拓展子节点(有碰撞子节点也会一并去除),采用该方法进行拓展可以有效地限制父子节点间的曲率变化率,且可保证曲率的线性连续性。如图2所示,采用Clothoid曲线进行拓展,路径掉头处转弯更为柔和,曲率连续性更好,没有突变。
1.2 左行混合A*算法
混合A*算法主要通过占据栅格地图进行代价计算,任意无碰撞节点n的代价可表示为[20]
$$ F(n) = g(n) + h(n) $$ (2) 代价函数$F(n)$由距目标点的预估距离代价(也称启发代价) $h(n)$以及距离起始点的累加代价$g(n)$组成。为了实现路径左行规划,需要对这2个代价进行适应性改进。
1.2.1 累加代价改进
距离起始点的累加代价$g(n)$在节点n拓展过程中实时计算得到,对其进行改进,使得规划路径满足左行要求。此代价由5项代价组成:
$$ \begin{gathered} g(n) = {\text{angdif}}(n) + {\text{back}}(n) + {\text{dist}}(n) + {\text{shift}}(n) + \\ {\text{left}}(n) + g(n - 1) \\ \end{gathered} $$ (3) 1)${\text{angdif}}(n)$为父子节点角度差代价,计算公式为
$$ {\text{angdif}}(n) = {\text{abs}}[\text{mod} ({\theta _{n - 1}} - {\theta _n} + 3{\text{π }},2{\text{π }}) - {\text{π }}] $$ (4) 式中:$ {{\mathrm{abs}}} ( \cdot ) $为取绝对值;$ \text{mod} ( \cdot ) $为取余操作。
利用式(4)可以计算任意2点方向所形成的较小夹角的角度,避免出现$ {0^\circ } $和$ {360^\circ } $的角度突变问题,此代价关注路径的平滑度。
2)倒车代价${\text{back}}(n)$计算公式为
$$ {\text{back}}(n) = \left\{ {\begin{array}{*{20}{l}} {0,{\text{ }}{d_n} = 1} \\ {a,{\text{ }}{d_n} = - 1} \end{array}} \right. $$ (5) 式中,a为惩罚常数,惩罚后退路径,因为矿区场景下,车辆后退风险更大。
3)父子节点间的距离代价${\text{dist}}(n)$为固定常数,对应了A*算法拓展的步长。
4)${\text{shift}}(n)$为换挡代价,其计算公式为
$$ {\text{shift}}(n) = \left\{ {\begin{array}{*{20}{c}} {b,{\text{ }}{d_n}{d_{n - 1}} \lt 0} \\ {0,{\text{ }}{d_n}{d_{n - 1}} \gt 0} \end{array}} \right. $$ (6) 式中,b为惩罚常数,代表了前进后退切换时代价惩罚的大小,避免频繁换挡影响效率。
5)左行代价$ {\text{left}}(n) $由左行节点拓展代价left_expand(n)和边界碰撞吸引代价left_collision(n)组成:
$$ {\text{left}}(n) = {{{\mathrm{left}}\_{\mathrm{expand}}}}(n) + {{{\mathrm{left}}\_{\mathrm{collision}}}}(n) $$ (7) ${{{\mathrm{left}}\_{\mathrm{expand}}}}(n)$计算公式为
$$ {{{\mathrm{left}}\_{\mathrm{expand}}}}(n) = \left\{ {\begin{array}{*{20}{l}} {c,{\text{ expandid}}(n) = 0} \\ {c,{\text{ expandid}}(n) = 1} \\ {c,{\text{ expandid}}(n) = 2} \\ {0,{\text{ other}}} \end{array}} \right. $$ (8) 式中:c为惩罚常数;${\text{expandid}}(n)$为当前节点由父节点拓展的节点序号(图1中序号)。
对右拐的拓展节点进行适当惩罚,让路径优先选择左行节点,从而快速回归到地图左侧边界。
${{{\mathrm{left}}\_{\mathrm{collision}}}}(n)$主要用于混合A*算法搜索到边界后产生吸引,让其后续沿着道路边界进行节点拓展。此代价可由式(9)计算:
$$ {{{\mathrm{left}}\_{\mathrm{collision}}}}(n) = \left\{ {\begin{array}{*{20}{l}} {2w,{\text{ collies}}(n) = 1} \\ {0,{\text{ collieb}}(n) = 1{\text{ and}}} \\ {w,{\text{ other}}} \end{array}} \right.{\text{nocollies}}(n) = 1 $$ (9) 式中:w为惩罚常数;${\text{collies}}(n)$、${\text{collieb}}(n)$、${\text{nocollies}}(n)$为车辆加入代价包络后是否和地图边界发生碰撞的判断函数。
如图3所示,车辆前、后、左侧分别加入距离为e1和e2的包络,形成2个判断车辆是否和边界发生碰撞的矩形,当地图边界和车辆小包络矩形发生碰撞(${\text{collies}}(n)$=1)就施加惩罚,此时路径过于靠近边界;地图边界与大包络矩形发生碰撞(${\text{collieb}}(n)$=1)并且没有与小包络矩形发生碰撞(${\text{nocollies}}(n)$=1)就不奖不惩,此时路径距离边界刚好在合适范围内;其余情况施加较小的惩罚。此项代价可以让路径贴近地图左侧边界进行规划。
1.2.2 启发代价改进
在传统混合A*算法中,栅格地图中各栅格距目标点的启发代价$h(n)$由Dijkstra算法[21]以目标点为起始位置预先计算得到,后期在路径搜索过程中可通过查表得到。由于Dijkstra算法求解的是从终点到起点的最短路径,某些场景下存在抄近道的现象。如果不对其进行改进,会使得启发代价和累加代价的左行趋势发生冲突,在冲突区域启发代价和累加代价此消彼长,导致路径规划拓展节点变多。为此对启发代价进行以下改进:
1)假设子节点坐标为$ ({x_{\text{r}}},\;{y_{\text{r}}},\;{\alpha _{\text{r}}}) $,在栅格地图中采用八邻域拓展方式进行子节点拓展,取x轴方向为$ {0^\circ } $,按逆时针方向,子节点方向$ {\alpha _{\text{r}}} $则为相应的8个拓展方向角$ \beta\text{π }/4 $之一,其中$\beta = 0,1,2, \cdots ,7$。
2)计算子节点和最近地图边界点$({x_{\text{s}}},\;{y_{\text{s}}})$的距离,如果距离在车辆半宽和安全余量之和的一定范围空间内,则利用式(10)判断最近地图边界点与拓展方向向量的位置关系。
$$ C_{\text{ross}} = \cos ({\alpha _{\text{r}}})({y_{\text{s}}} - {y_{\text{r}}}) - \sin ({\alpha _{\text{r}}})({x_{\text{s}}} - {x_{\text{r}}}) $$ (10) 3)因为是从终点向起点拓展,如果$C_{\text{ross}} \lt 0$,则证明最近地图边界点在子节点拓展方向右侧,启发代价为欧式距离的一半,反之则保持原有欧式距离。利用此方法可以让拓展的启发代价,沿地图左侧边界形成较低的代价。
如图4a所示,取起点在右上侧,终点在左下侧,利用Dijkstra算法得到启发代价在拐弯处存在明显的右侧值低的现象,和左行规划的要求相违背。而采用优化后的启发代价计算方法(图4b),在地图左侧形成代价较低的区域,且靠近终点代价变小。改进后的启发代价更加符合矿区的左侧规划要求,与累加代价配合即可生成沿地图左侧边界规划的路径。
2. 离散点路径平滑
利用Clothoid曲线拓展及左行混合A*算法生成的初始路径虽然曲率连续,但并不平滑,且路点间距较大,为此对其作进一步的插值与平滑,主要分为可行域隧道构建、离散点平滑2大步骤。
2.1 可行域隧道构建
在构造可行域隧道之前,首先利用Clothoid曲线将初始路径进行插值,生成间隔为l m的均匀路径点,然后对每个点生成局部矩形可行域$ \left[ {x_{\min }},\; {y_{\min }}, \right. \left.{x_{\max }},\;{y_{\max }} \right] $,其代表每个路径点在x、y坐标方向上可以移动的最大距离,具体生成方式可以参照文献[14],惟一需要改变的是将移动的方向由车辆坐标系的前后左右变为全局坐标系的x、y轴正负方向。
由于离散点平滑模型没有显式地对曲率、曲率变化率进行限制,为了避免平滑后出现大曲率路径点曲率超限,超出车辆转向响应能力,不利于车辆控制跟踪,利用式(11)对大曲率点进行可行域缩减:
$$ \begin{split} & {\theta_{i + g_{\text{num}}} - \theta_i} \geqslant C_{\text{of}} g_{\text{num}},\\ & \left\{ \begin{gathered}{x_{\min }}\left( {i + I_{\text{nd}}} \right) = B_{\text{ound}} \\ {y_{\min }}\left( {i + I_{\text{nd}}} \right) = B_{\text{ound}} \\ {x_{\max }}\left( {i + I_{\text{nd}}} \right) = B_{\text{ound}} \\ {y_{\max }}\left( {i + I_{\text{nd}}} \right) = B_{\text{ound}} \\ \end{gathered} \right. \end{split}$$ (11) 式中:$ i = 0,1,2, \cdots ,N - g_{\text{num}} $表示对所有的路径点遍历大曲率检查;$ N $为路径总点数;$g_{\text{num}}$为大曲率检测区间;Cof为角度系数;$I_{\text{nd}} = 0,1,2, \cdots ,g_{\text{num}}$表示这一区间内的路径点的矩形可行域都进行调整;Bound为调整后的矩形可行域大小。
对每个路径点进行大曲率路径点可行域缩减检查后,即可得到由全部路径点矩形约束构成的可行域隧道,如图5所示。
2.2 对角向量优化模型
如图6所示,取离散路径上的3个点$ {p_0} $、$ {p_1} $、$ {p_2} $,组成向量$ \overrightarrow {{p_0}{p_1}} $、$\overrightarrow {{p_1}{p_2}} $,2个向量的差得到对角向量$\overrightarrow {{p_1}v} $,显然对角向量的模越小意味着$ {p_0} $、$ {p_1} $、$ {p_2} $的连线更加平直。
因此,路径离散点平滑性Smooth可用所有路径点对角向量模的和表示[22]:
$$S_\text{mooth}=\sum\limits_{j=1}^{N-1}{\sqrt{{{\left( \Delta {{{\boldsymbol{X}}}_{j+1}}-\Delta {{{\boldsymbol{X}}}_{j}} \right)}^{2}}}} $$ (12) 式中,$\Delta {{{\boldsymbol{X}}}_{j}}=\overrightarrow{{{p}_{j-1}}{p}_{j}}$。
以式(12)为目标函数进行最小化,结合可行域隧道,建立数值优化二次规划模型,表示为
$$ \begin{gathered} \mathop {\min }\limits_{{\boldsymbol{x}} \in {\mathbb{R}^{2N}}} {\text{ }}f({\boldsymbol{x}}) = \frac{1}{2}{{\boldsymbol{x}}^{\text{T}}}{\boldsymbol{Hx}} + {{\text{ω}} ^{\text{T}}}{\boldsymbol{x}} \\ {\text{s.t. }}{{\boldsymbol{A}}_{\text{I}}}{\boldsymbol{x}} = {{\boldsymbol{b}}_{\text{I}}} \\ \qquad{{\boldsymbol{A}}_{{\text{ε}}}}{\boldsymbol{x}} \lt {{\boldsymbol{b}}_{{\text{ε}}}} \\ \end{gathered} $$ (13) 式中:$ {\boldsymbol{x}} $为待求的路径点向量;$ {\boldsymbol{H}} $为目标函数系数矩阵;${\text{ω}}$为防止过拟合的正则化项;$ {{\boldsymbol{A}}_{\text{I}}} $、$ {{\boldsymbol{b}}_{\text{I}}} $为等式系数矩阵;$ {{\boldsymbol{A}}_{{\text{ε}}}} $、$ {{\boldsymbol{b}}_{{\text{ε}}}} $为不等式系数矩阵。
可行域隧道构建主要对离散路径点的x、y坐标值进行最大最小值约束。相应的不等式约束可表示为
$$ {x_{{\text{min}}}}(n) \leqslant x(n) \leqslant {x_{{\text{max}}}}(n) $$ (14) $$ {y_{{\text{min}}}}(n) \leqslant y(n) \leqslant {y_{{\text{max}}}}(n) $$ (15) 模型构建后,可通过OSQP求解器[23]快速求解得到平滑后离散路径点,平滑后的点位置发生变化,点距不再均匀,进行线性插值和点的方向角重新计算后即可得到最终的路径。
3. 试验验证
为验证提出的无人驾驶自卸车路径规划方法在左行路径、归划效率和平滑度等方面的优势,进行仿真和实车试验。采用C++编写算法,程序在装有Linux操作系统的笔记本电脑平台上运行(处理器Intel(R) Core(TM) i7 2.7 GHz,运行内存8 GB)。基于采集的露天煤矿地图,将本文改进方法与文献[14]所提针对矿山的混合A*算法进行仿真对比试验。然后,结合实际“装−运−卸”全流程场景进行实车路径规划对比试验。试验参数见表1。
表 1 试验相关参数Table 1. Related parameters of experiment参数 数值 倒车代价a 4.0 换挡代价b 15.0 左行代价c 1.5 边界跟随代价w 1.0 大曲率检测区间gnum 12 角度系数Cof 0.08 矩形边界大小Bound /m 0.1 车辆前轮最大转角 /(°) 30 车辆轴距 /m 6.25 搜索步长L/m 4 3.1 仿真对比试验
在采集的露天煤矿地图中(图7),对2种方法生成路径的启发值改进效果、左行效果、平滑优化时间及曲率控制效果进行对比试验。
1)验证对比启发代价改进前后的效果,如图8所示。从图8a可以看出改进前拓展节点在累加代价的影响下有往地图边界左侧规划的趋势,但受启发代价最短路径原则影响,在前部分没有实现左行。而改进启发代价后(图8b),拓展节点能够较快地回到道路左侧,沿地图边界左侧搜索到终点。启发代价改进前拓展节点数量达到12 318个,改进后拓展节点数量只有5 985,减少了51%。
2)进行左行效果对比,如图9所示。改进方法采用左行规则生成的路径符合矿区的左行要求,主要得益于搜索代价的左行调整;而原有混合A*代价规则方法生成的路径没有靠道路左侧,不符合矿区左行要求。左行规划的实现,从符合矿区作业规范的角度,提高了混合A*路径规划方法对矿区场景的适应性。
3)进行路径平滑效率对比(表2),统计不同路径长度下的规划总耗时和平滑耗时。从平滑耗时上看,离散点平滑方法比原有样条曲线方法效率平均提高5倍以上,路径越长平滑耗时优势越明显;从总耗时上看,在不同路径长度上均比原有方法更有优势,且随着路径长度增加,耗时优势更为明显,这主要得益于平滑耗时的减少。
表 2 总耗时和平滑耗时数据对比Table 2. Comparison of total computation time and smoothing computation time路径长度/m 平滑耗时/ms 总耗时/ms 改进方法 原有方法 改进方法 原有方法 200 4 21 123 141 1 000 16 60 217 245 2 000 34 301 325 582 3 000 51 524 436 875 4 000 76 803 531 1 225 矿区无人驾驶系统从接收到动车指令到车辆实际动车,需要经历地面路径规划、车地数据传输和车辆换挡启动等3个主要环节。通常5G车地数据传输耗时15 ms左右,车辆换挡启动耗时30 ms左右,地面路径规划耗时占比较大。路径规划总耗时的减少,从提高系统响应速度的角度,增强了对矿区运煤等长运距场景的适应性。
4)对路径曲率控制效果进行了对比,如图10所示。在采集的露天煤矿地图上规划1条多段路径,为了方便比较,2种方法均对整条路径进行平滑。从图10a中看出,2种方法均能将粗略路径的摆动拉直,达到平滑效果。从图10b中可以看出改进方法曲率控制效果更加优异,保证平滑效果的同时也控制了最大曲率不超限(最大曲率限制为0.092 4),而样条曲线平滑方法则存在曲率超限现象。
3.2 实车对比试验
结合矿区的实际“装−运−卸”全流程场景,选取排队路径、卸载路径和装载路径,进行实车规划对比试验,对比规划路径的曲率、曲率变化率、限速和路径跟踪效果。如图11a所示,采用线控改造过后的XD240矿用自卸车进行实车试验,该车配备有工控机、4G/5G CPE、相机以及雷达等设备。采用非线性模型预测控制(NMPC)控制策略[24]进行路径跟踪。图11b为测试的露天煤矿实际场景。
规划1条前往装载区排队点的排队路径,图12a中下侧为装载区,可以看出改进方法和原有方法规划路径存在较大差异,改进方法路径沿地图左侧边界进行规划,而原有方法直接抄近道沿地图右侧规划到了终点,不符合矿区的左行要求。从图12b、图12c、图12d中可以看出,改进方法曲率有着与样条曲线平滑方法相当的曲率平滑性,均具有较好的平滑效果,不存在曲率超限情况。限速方面2种方法均没有曲率大幅振荡变化导致的限速跳动情况。
图12e、图12f反映了车辆路径跟踪的横向和横摆角偏差,改进方法横向偏差为−0.16 ~ 0.13 m,横摆角偏差为−0.5° ~ 1.3°;原有方法横向偏差为−0.14 ~ 0.35 m,横摆角偏差为−2.8° ~ 3.9°,综合来看改进方法变化幅度小,实车跟踪效果更佳。
规划1条前往卸载点的卸载路径,从图13a中可以看出,采用Clothoid曲线进行拓展并结合离散点平滑,规划路径更为平直。图13b、图13c中可以看出,由于采用A*搜索段和CCRS段一起合并平滑,改进方法得到的路径平滑度更优,而原有样条曲线方法对曲率控制效果不佳,只对A*搜索段进行平滑,平滑效果差一些。限速方面(图13d),改进方法和原有方法相差不大,优势主要体现在A*搜索段和CCRS段的连接处以及后退段。
从图13e、图13f中可以看出,改进方法车辆路径跟踪横向偏差为−0.12 ~ 0.23 m,横摆角偏差为−4.3° ~ 3.5°;原有方法横向偏差为−0.73 ~ 0.12 m,横摆角偏差为−5.8° ~ 11.8°,综合来看改进方法路径有着更好的实车跟踪效果。
规划1条前往装载点的装载路径,从图14a可以看出改进方法在接近后退段处比原有方法更为平直。图14b、图14c中可以看出,在路径曲率上改进方法更为平滑。尤其是后退段,由于原有方法直接采用CCRS曲线路径未做平滑,其存在尖峰曲率情况,而改进方法对CCRS段进行了平滑,有效地缓解了后退段存在的曲率尖峰情况。限速方面(图14d),改进方法虽然在起点和换挡处限速更为明显,但是由于在这2种工况转换场景下速度本身需要很低,所以影响不大。在A*搜索段和CCRS段的连接处、后退段,改进方法更具限速优势。
图14e、图14f可以看出改进方法横向偏差为−0.09 ~ 0.17 m,横摆角偏差为−4.7° ~ 1.8°;原有方法横向偏差为−0.1 ~ 0.2 m,横摆角偏差为−4.2° ~ 3.6°,改进方法横向偏差变化范围更小,横摆角偏差变化范围稍大,后退段横向和横摆角偏差变化幅度更小。综合来看改进方法路径有着较好的实车跟踪效果。
4. 结 论
1)为提升混合A*算法粗略解的质量,提出利用Clothoid曲线拓展代替以固定前轮转角进行节点拓展的方法。试验表明:该方法提高了粗略路径的曲率连续性,且可满足曲率变化率限制要求。
2)为使生成路径满足矿区左行规则,提出利用左行规则改进混合A*算法累加代价和启发代价,在累加代价中加入左行代价和碰撞吸引代价,对启发代价加入基于位置关系的动态拓展代价调整。改进后的混合A*算法可以生成符合矿区左行规则的路径,且有效减少了算法拓展的节点数量。
3)为提高路径对运煤等长距离运输场景的适应性,提出利用大曲率点可行域隧道缩减技术限制大曲率点的移动,结合二次规划对角向量模最小化模型,对路径进行平滑,提高了效率的同时保证了曲率限制要求。试验表明:高效的路径平滑算法保证了矿区运煤等长路径场景的规划效率,且在“装−运−卸”全流程场景中生成的路径具有良好的平滑性和可行驶性。
-
表 1 试验相关参数
Table 1 Related parameters of experiment
参数 数值 倒车代价a 4.0 换挡代价b 15.0 左行代价c 1.5 边界跟随代价w 1.0 大曲率检测区间gnum 12 角度系数Cof 0.08 矩形边界大小Bound /m 0.1 车辆前轮最大转角 /(°) 30 车辆轴距 /m 6.25 搜索步长L/m 4 表 2 总耗时和平滑耗时数据对比
Table 2 Comparison of total computation time and smoothing computation time
路径长度/m 平滑耗时/ms 总耗时/ms 改进方法 原有方法 改进方法 原有方法 200 4 21 123 141 1 000 16 60 217 245 2 000 34 301 325 582 3 000 51 524 436 875 4 000 76 803 531 1 225 -
[1] 王星烨. 矿用卡车无人驾驶系统路径规划方案研究[J]. 工矿自动化,2023,49(S1):99−102. WANG Xingye. Research on path planning scheme of mine truck unmanned driving system[J]. Journal of Mine Automation,2023,49(S1):99−102.
[2] 王国法. 煤矿智能化最新技术进展与问题探讨[J]. 煤炭科学技术,2022,50(1):1−27. doi: 10.3969/j.issn.0253-2336.2022.1.mtkxjs202201001 WANG Guofa. New technological progress of coal mine intelligence and its problems[J]. Coal Science and Technology,2022,50(1):1−27. doi: 10.3969/j.issn.0253-2336.2022.1.mtkxjs202201001
[3] 闫凌,黄佳德. 矿用卡车无人驾驶系统研究[J]. 工矿自动化,2021,47(4):19−29. YAN Ling,HUANG Jiade. Research on unmanned driving system of mine-used truck[J]. Journal of Mine Automation,2021,47(4):19−29.
[4] 于海旭,杜志勇,魏志丹,等. 我国矿区无人驾驶技术现状与发展趋势分析[J]. 工矿自动化,2022,48(S2):82−87. DU Zhiyong,YU Haixu,WEI Zhidan,et al. Analysis on the current situation and development trend of unmanned driving technology in mining areas in China[J]. Journal of Mine Automation,2022,48(S2):82−87.
[5] 薛光辉,刘爽,王梓杰,等. 基于改进概率路线图算法的煤矿机器人路径规划方法[J]. 工矿自动化,2023,49(6):175−181. XUE Guanghui,LIU Shuang,WANG Zitie,et al. A path-planning method for coal mine robot based on improved probability roadmap algorithm[J]. Journal of Mine Automation,2023,49(6):175−181.
[6] BADUE C,GUIDOLINI R,CARNEIRO R V,et al. Self-driving cars:a survey[J]. Expert Systems with Applications,2021,165:113816.
[7] GONZÁLEZ D,PÉREZ J,MILANÉS V,et al. A review of motion planning techniques for automated vehicles[J]. IEEE Transactions on Intelligent Transportation Systems,2016,17( 4):1135−1144. doi: 10.1109/TITS.2015.2498841
[8] RAFAI A N A,ADZHAR N,JAINI N I. A review on path planning and obstacle avoidance algorithms for autonomous mobile robots[J]. Journal of Robotics,2022:2538220.
[9] NASIR J,ISLAM F,MALIK U,et al. RRT*-SMART:a rapid convergence implementation of RRT*[J]. International Journal of Advanced Robotic Systems,2013,10(7):1651−1656.
[10] KOVÁCS B,SZAYER G,TAJTI F,et al. A novel potential feld method for path planning of mobilerobots by adapting animal motion attributes[J]. Robotics and Autonomous Systems,2016,82:24−34. doi: 10.1016/j.robot.2016.04.007
[11] ELBANHAWI M,SIMIC M,JAZAR R. N. Continuous path smoothing for car-like robots using B-spline curves[J]. Journal of Intelligent & Robotic Systems,2015,80(1):23−56.
[12] PEK C,ALTHOFF M. Fail-safe motion planning for online verification of autonomous vehicles using convex optimization[J]. IEEE Transactions on Robotics,2021,37(3):798−814. doi: 10.1109/TRO.2020.3036624
[13] 鲍久圣,张牧野,葛世荣,等. 基于改进A*和人工势场算法的无轨胶轮车井下无人驾驶路径规划[J]. 煤炭学报,2022,47(3):1347−1360. BAO Jiusheng,ZHANG Muye,GE Shirong,et al. Underground driverless path planning of trackless rubber tyredvehicle based on improved A* and artificial potential field algorithm[J]. Jounal of China Coal Society,2022,47(3):1347−1360
[14] 邓穆坤,刘勇,黄佳德,等. 基于混合A*算法的无人驾驶矿用卡车路径优化研究[J]. 控制与信息技术,2022(5):60−67. DENG Mukun,LIU Yong,HUANG Jiade,et al. Research on hybrid A* based path optimization of unmanned mine truck[J]. Control and Information Technology,2022(5):60−67.
[15] DOLGOV D,THRUN S,MONTEMERLO M,et al. Path planning for autonomous vehicles in unknown semi-structured environments[J]. The International Journal of Robotics Research,2010,29(5):485−501. doi: 10.1177/0278364909359210
[16] FRAICHARD T,SCHEUER A. From Reeds and Shepp’s to continuous curvature paths[J]. IEEE Transactions on Robotics,2004,20(6):1025−1034. doi: 10.1109/TRO.2004.833789
[17] GIM S,ADOUANE L,LEE S,et al. Clothoids composition method for smooth path generation of car-like vehicle navigation[J]. Journal of Intelligent & Robotic Systems. 2017,88:129−146.
[18] ZHAO Ziyu,LIN Bi. A new challenge:path planning for autonomous truck of open-pit mines in the last transport section[J]. Applied Sciences,2020,10(18):6622. doi: 10.3390/app10186622
[19] 陈鑫鹏,徐彪,胡满江,等. 一种基于等步长分层拓展的混合A*路径规划方法[J]. 控制与信息技术,2021(1):17−22, 29. CHEN Xinpeng,XU Biao,HU Manjiang,et al. A hybrid-A* path planning method based on equal step hierarchical expansion[J]. Control and Information Technology,2021(1):17−22, 29.
[20] SEDIGHI S,NGUYEN D V,KUHNERT K D. Guided hybrid A-star path planning algorithm for valet parking applications[C]// 2019 5th International Conference on Control,Automation and Robotics (ICCAR),Beijing,China. IEEE,2019:570−575.
[21] CANDRA A,BUDIMAN M A,HARTANTO K. Dijkstra's and A-star in finding the shortest path:a tutorial[C]//2020 International Conference on Data Science,Artificial Intelligence,and Business Analytics (DATABIA),Medan,Indonesia. IEEE,2020:28−32.
[22] LI X,SUN Z,CAO D,et al. Real-time trajectory planning for autonomous urban driving:framework,algorithms,and verifications[J]. IEEE/ASME Transactions on Mechatronics,2016,21(2):740−753. doi: 10.1109/TMECH.2015.2493980
[23] STELLATO B,BANJAC G,GOULART P. OSQP:an operator splitting solver for quadratic programs[J]. Mathematical Programming Computation,2020,12(4):637−672. doi: 10.1007/s12532-020-00179-2
[24] 王斌,袁希文,张乾,等. 露天矿山运输车辆横向非线性模型预测控制研究[J]. 控制与信息技术,2022(5):30−37. WANG Bin,YUAN Xiwen,ZHANG Qian,et al. Research on lateral nonlinear model predictive control of transportation vehicles in open-pit mines[J]. Control and Information Technology,2022(5):30−37.
-
期刊类型引用(1)
1. 高明宇, 鲍久圣, 阴妍, 胡德平, 张可琨, 朱晨钟, 王茂森, 王凯. 基于优化蚁群算法的露天矿无人矿卡绕跨并行类三维路径规划. 煤炭科学技术. 2025(S1) 本站查看
其他类型引用(1)