Research on the obstacle avoidance path planning of unmanned truck in open-pit mine with improved time-elastic-bands
-
摘要: 针对露天矿非结构化道路上因障碍具有突发性和多样性等特点，极大影响无人卡车路径规划和安全行驶等问题，提出一种基于改进时间弹性带(timed elastic bands, TEB)的矿区无人卡车避障路径规划算法。该算法通过调节融合位姿轨迹点，减少卡车位姿计算量和路径转弯幅度，并针对复杂环境下易陷入局部最优的问题，在障碍物膨胀层与安全行驶区域间引入缓冲层，完成静态代价地图中最优安全路径规划。实验结果表明，在静态障碍物复杂场景下，该算法较传统TEB算法在安全绕障能力上有较大提升，平均导航速度更快，可有效保障露天矿无人卡车的安全行驶和高效运输。Abstract: To address the sudden and diverse obstacles on unstructured roads in open-pit mines, which greatly affect the path planning and safe operation of unmanned trucks, a path-planning algorithm based on improved timed elastic bands (TEB) is proposed. This algorithm streamlines the number of truck pose calculations and minimizes path-turning amplitude by adjusting and fusing pose trajectory points. It also introduces a buffer layer between the obstacle expansion layer and the safe driving area to achieve optimal safe path planning in the static cost map, preventing local optimization in complex environments. Experimental results show that, when dealing with static obstacles, the proposed algorithm significantly improves the ability to safely navigate around obstacles compared with the traditional TEB algorithm. Furthermore, it increases the average navigation speed, effectively ensuring the safe and efficient transportation of unmanned trucks in open-pit mines.
-
近年来，随着智慧矿山建设和无人驾驶技术的快速发展，露天矿无人驾驶也逐渐成熟并落地[1]，但由于真实的矿区作业环节非常恶劣，面临极端天气、颠簸道路、落石、狭窄工作区、重载陡坡等诸多挑战，对硬件可靠性、算法适配性、定位精准性有严格要求，根据矿山运输的工作需求，较差作业环境给无人卡车安全行驶带来了重大安全隐患，因此亟待对露天矿区无人卡车的路径规划与智能绕障进行深入研究[2]。
根据已知环境信息和规划重心的不同，路径规划分为全局路径规划和局部路径规划[3]。其中，局部路径规划在全局路径的基础上进行动态优化，局部规划器有A*算法[4-5]、人工势场法（artificial potential field, APF）[6-7]、动态窗口法（dynamic window approaches, DWA）[8-9]、时间弹性带（timed elastic bands, TEB）、模块预测控制算法（model-based predictive control, MPC）[10]等，最常用的是DWA算法与TEB算法。DWA算法通过评价函数对采样速度生成的轨迹进行评价，路径规划结果过度依赖评价函数，导航避障效果一般[11]；TEB算法作为时间最短的路径规划器，加入了时间约束，还综合考虑特定应用场景下的约束条件，特别是车辆的运动学特征，但算法无法考虑各种车辆模型，故在躲避障碍物时，易因不符合运动位姿及几何约束，导致行驶震荡，不能引导完成避障任务[12]。林洁等[13]针对局部规划算法容易陷入局部极小值的问题，提出了“沿边走”的详细策略进行路径求解，跳出局部极小点，路径长度和平滑度都得到明显提升；Wang等[14]针对复杂车间路径规划任务中障碍物密集导致最优轨迹难规划、轨迹不平滑的问题，提出一种新的动态窗口算法，该方法通过方向角变化因子改进评价函数提高算法的效率和灵活性，在保障机器人安全的同时具有良好的避障效果；郑凯林等[15]针对TEB方法缺少加速度约束导致机器人底盘受到冲击震荡的问题，提出了加加速度约束的方法，该方法是在轨迹优化过程中构建了具体的加加速度的约束，规划轨迹满足动力学要求且平滑。上述方法主要是在相对稳定的环境中对机器人进行路径规划，由于多露天矿山复杂环境中，作业环境和作业对象都具有复杂性和随机性[16]，而现有车辆动态避障方法对复杂环境处理能力不够，易陷入局部最小值，实时性不满足应用要求[17]，因此本文提出一种以无人卡车及其运输道路为基础，提高避障路径安全性及导航效率的局部避障方法，以适应矿区复杂环境，为无人卡车的安全运输提供可靠的安全保障。
1. 露天矿卡车运输道路问题分析
由于露天矿山生产条件的日趋复杂，多目标动态规划配矿优化是大型露天矿山的趋势，编制科学合理的规划可以有效提高配矿效率，保证矿山企业可持续发展[18]。矿山运输卡车的路径规划，最基本的要求是在特定空间内自动规划出一条避开所有障碍物的端到端的安全路径[19]。不同的是，露天矿区路网是非结构化的，装载点到卸载点之间的运输路线情况复杂，且路线随时可能因爆破塌陷改道；此外，卡车运输具有较高的机动性，在复杂的采矿场作业中有其独特优势，然而，卡车较大的立方体车架结构导致其在矿区道路行驶时面临更为严格的限制，这些限制增加了路径规划的复杂性。
1.1 露天矿卡车运输道路特点分析
露天矿山基本为缺乏交通标识、道路与工作区频繁变化的非结构化道路，不仅坡度陡、断面形状复杂、弯度大、车辆载重大，还需面对落石、颠簸、翻浆、泥泞等问题[20]。露天矿区全局、局部道路如图1所示。
图 1 露天矿矿区运输路线Fig. 1 Open-pit mine transportation road
下载:
全尺寸图片
本文主要考虑无人卡车运输道路及卡车运输特点设计局部路径规划算法，综合以上因素，其主要具备以下特点：
1）非结构化。露天矿山道路形状不规则，没有明确的边缘线，路面主要由土块构成，具有非结构化特点。矿区道路及通行规则自成体系，不存在公开道路的交通法规问题[21]；非结构化的特点导致道路边缘不清晰，无法快速、精准地对道路进行识别，检测安全行驶区域的难度较大。
2）临时性、多变性。随着采掘面的不断延伸，露天矿山的采场运输道路也将随之改线或直接废弃，因此其中的大部分道路是属于临时性的[22]；此外，随着矿山生产的推进，矿车的装载、卸货点以及矿区地貌还会发生变化，路段上的阶梯结构和气候也会导致道路边界变化。
3）崎岖性、坡度大、宽窄多变。露天矿区道路运输系统以环形向上的方式布置在一个比较固定的山体范围内，所以矿石输送路线往往较为集中，这样的布局导致矿区运输道路崎岖、宽窄多变；环形路网设计降低了卡车运输的爬坡压力[23]，保障卡车安全行驶，虽方便爆破改道，但随之形成了高度差较大的斜坡道。
4）路面障碍突发性。露天矿区卡车运输道路宽度相对较窄，使得大体积的卡车在遇到大块落石等障碍物或者道路边缘损坏时很难绕行。
由于这些复杂的道路环境，矿区无人驾驶的实现面临极大的安全隐患。
1.2 露天矿运输道路特殊路段分析
露天矿区运输道路中存在一些特殊路段，增加了通行困难程度，如图2所示，包括狭窄路段、多路交叉路口、急弯路段、障碍物阻碍路段等。由于山区地形地势和矿区道路布局的限制，矿区路网难以避免地出现特殊路段，这给运输车辆带来了极大的安全隐患。为了降低无人卡车行驶风险，需要分析这些路段的特点，并采取相应的改进措施。
图 2 金属矿山特殊运输路段Fig. 2 Special transport sections for metal mines下载:
全尺寸图片
1）狭窄路段，如图2(a)所示。矿区的狭窄路段是运输卡车事故高发路段，其主要原因是运输卡车本身体积大，在狭窄区域行驶时极易陷入危险边缘。以洛阳某露天矿区为例，运输卡车的宽度大约3.5 m，但矿区狭窄路段的宽度一般仅有5 m，这时卡车通行时可行驶区域就会变小，驾驶员需要时刻注意卡车与道路两边的距离，在长时间紧张驾驶下，应对紧急状况能力降低，使得行车安全性大大降低，可能造成侧翻甚至翻车等运输事故[24]。对于无人卡车来说，运输的难度很高。
2）多路交叉路口，如图2(b)所示。由于矿区道路分布复杂，生产干线与多条生产支线相连接造成多交叉路口，在该路段中路径选择则更加重要。通常，路径规划算法在搜索路径时会考虑当前位置到目标点的最短距离，当出现多条路径时，算法容易出现跳路问题，陷入局部最短路径，因此无人卡车在交叉路口做路径选择规划算法尤其困难。
3）急弯路段，如图2(c)所示。矿区道路布局以弯道为主，弯道的线型设计、设置的频率都是造成安全事故的原因。常见事故原因是卡车经过急弯路段的运行速度差较大，不满足车辆的减速要求时极易发生侧翻；通常，无人卡车在经过转弯时会进行减速慢行，在转弯半径过小的路段中导航系统偏向规划最短距离，在未知或已知环境下，由于传感器视距较短、规划算法性能不足，在该路段极易陷入局部最优、程序卡死，车辆紧急制动，无法通行。
4）障碍物阻碍路段，如图2(d)所示。露天矿区道路路面多为小石块和土块，在卡车运输时可以为轮胎增加摩擦，由于生产路线上运输量巨大，道路内侧和卡车运输时极易掉落矿石和大土块，大体积障碍物会使卡车剧烈颠簸，所以行驶时避开障碍物也是解决矿区运输安全隐患的一个有效措施。矿区道路上的障碍物位置、大小对卡车避障的难度有很大的影响。
2. TEB算法模型构造与改进
为适应矿区运输卡车行驶轨迹，提升行驶效率和安全性，对TEB算法模型进行优化改进，对全局路径上插入的位姿轨迹点进行融合，调整轨迹分辨率，并对障碍物膨胀层进行改进。
2.1 TEB算法模型构造
TEB算法从当前位置起截取一段全局路径，在这段全局路径中插入N个离散时间的位姿轨迹点，这些离散的位姿轨迹点形成一条弹性带[25]。弹性带在i时刻的位姿序列及轨迹位姿序列描述为
$$ {\boldsymbol{X}}_i = {\left( {x_i,y_i,\theta_i} \right)^{\mathrm{T}}} \in {R^2} \times {S^{{1}}} $$ (1) 式中：xi、yi表示对应车辆在世界坐标系下的坐标位置，θi表示该车辆当前航向角。
所有的轨迹点序列描述为
$$ {\boldsymbol{X}}{{:}} = \left\{ {{\boldsymbol{X}}_i} \right\},\;\; i = 0,1, \cdots ,n $$ (2) 位姿点xi到xi+1间的时间定义为时间分辨率
${\boldsymbol{T}}_i $ ，记录时间序列T，TEB轨迹模型中包括位姿序列信息及时间序列信息，故可将其进行合并，表示为B:，弹性带相邻3个轨迹点运动位姿如图3所示。
图 3 TEB轨迹位姿序列Fig. 3 TEB trajectory pose sequence下载:
全尺寸图片
$$ {\boldsymbol{T}} = \left\{ {{\boldsymbol{T}}_i} \right\},\;\; i = 0,1, \cdots ,n - 1 $$ (3) $$ {\boldsymbol{B}}{{:}} = \left( {{\boldsymbol{X}},{\boldsymbol{T}}} \right) = \left( {{\boldsymbol{X}}_1,{\boldsymbol{T}}_1,{\boldsymbol{X}}_2, \cdots ,{\boldsymbol{X}}_{n-1},{\boldsymbol{T}}_{n-1},{\boldsymbol{X}}_n} \right) $$ (4) TEB算法通过对位姿序列和时间间隔进行加权多目标优化来获取最优的轨迹序列，根据具体约束目标建立m个不同的约束目标函数fk(B) [26]。每个约束目标函数可表示为f(B)，将各个函数进行优化，求解位姿序列X:，得到最优轨迹集合B*:，
$$ f\left( {\boldsymbol{B}} \right) = \sum {\gamma_kf_k\left( {\boldsymbol{B}} \right)} ,\;\; k = 1,2, \cdots ,m $$ (5) $$ \boldsymbol{B}^*={{\mathrm{arg}}_{{\boldsymbol{B}}}\min}f\left(\boldsymbol{B}\right) $$ (6) 式中γk为该目标函数的权重。
因为TEB算法是对多个约束目标函数进行组合优化，故在配合全局路径规划算法进行特定场景下的轨迹规划与动态避障时，常因考虑约束过多使得规划出的轨迹不满足约束条件。这样的轨迹在车辆行驶时易出现速度波动大、走走停停或者因轨迹拐点较多而导致行驶“摇摆”甚至穿过碰撞层发生碰撞的情况，该情况下需根据特定对象底盘特征及工作环境适时调整对应约束目标函数。
2.2 融合位姿轨迹点
在一段局部路径中插入大量的位姿轨迹点可以更准确地描述车辆的运动，以逼近其动力学模型。然而，计算和优化每个位姿点的过程需要很多时间，可能导致生成轨迹输出有一定的延迟；同时，过多的轨迹点会引入更多的不连续性和曲率变化导致路径变得不够平滑。因此，在插入位姿控制点时需要考虑动力学模型逼近的准确性、时间成本、路径平滑度以及实际应用需求。针对露天矿区卡车局部路径规划时，生成轨迹拐点较多导致行驶“摇摆”的情况，使用融合位姿轨迹点对冗余路径点进行去除，减少轨迹中的拐点个数。
对全局路径的轨迹片段进行优化时，首先在不同时间间隔内插入相对应的位姿轨迹点，如图4中的序列1。融合位姿轨迹点通过计算相邻2个位姿点之间的距离来确定融合后的路径相邻2个位姿点之间的时间间隔，也称分辨率，在去除冗余路径点的同时，保证轨迹流畅度。
图 4 TEB融合位姿序列轨迹Fig. 4 TEB fusion pose sequence trajectories下载:
全尺寸图片
算法1 改进TEB算法融合位姿点（单次）
输入 _start：初始点；_goal：目标点；globalpath：全局路径
输出 localpath：融合位姿点后的局部路径
1) function extraLocalPath(globalpath, _start, _goal ):
2) //从全局路径中截取路径
3) localpath = []
4) if _start >= 0 and _goal < length(globalpath)
5) and _start <= _goal:
6) for i from _start to _goal:
7) localpath.append(globalpath[i] )
8) end for
9) end if
10) end function
11) function insertPoseInPath( Pose, localpath,index ):
12) //在局部路径中插入位姿点
13) if index >= 0 and index < length(localpath ):
14) Pose.insert( index,pose )
15) end if
16) endfunction
17) //根据位姿分辨率更新出新的位姿轨迹集合
18) for Pose.insert in localpath:
19) Pose0 = _start
20) if Pose0 == _goal
21) newPoses[] = Pose0
22) else
23) newPoses[]=alculatebyref(Pose.insert ):
24) new_goal = _start
25) return newPoses[]
26) end if
27) end for
2.3 膨胀层处理
由于露天矿区运输卡车在行驶过程中存在很多不确定性，所以尽管原本规划出来的局部路径是安全无碰撞的，也极有可能由于执行偏差，行驶到距离落石等不规则障碍物较近的区域，出现车辆计算量激增、速度失稳侧翻，发生危险。改进TEB算法在传统算法膨胀层的基础上进行二次改进，提出一种对膨胀层处理及避障目标函数进行优化的方法。
膨胀层的处理模型如图5所示，中心位置为一个不规则的障碍物，以障碍物形状的最长宽度构成一个最大的圆，圆内作为碰撞层；在碰撞层外设置合适的膨胀半径对障碍物作膨胀处理，这2个圆之间的区域称为膨胀层；膨胀操作可以有效减少算法的计算量，增加车辆与障碍物的距离，得到更加平稳安全的避障轨迹；同时，考虑到矿区运输卡车的体积较大且底盘结构在转弯处可能会进入膨胀层导致安全事故，故在膨胀层外设置小单元的缓冲半径为卡车运动作膨胀缓冲处理，增加局部路径与障碍物的距离。
图 5 膨胀层处理示意Fig. 5 Schematic diagram of the expansion layer treatment下载:
全尺寸图片
算法避障目标函数的优化针对约束位姿点距障碍物的最小距离omin。考虑到矿区环境的不确定性，应对落石等不规则障碍物，增大与障碍物的最短距离及该目标函数的避障惩罚系数fobstacle2，避障惩罚函数表示为
$$ f_{{\mathrm{obstacle2}}} = \sum\limits_0^n {\frac{1}{{\left[ {d\left( {x_i,o_{{\mathrm{min}}}} \right)} \right]}}} ,i = 0,1, \cdots ,n;d < d_{{\mathrm{min}}} $$ (7) 式中位姿序列或障碍物的最近距离为dmin。
3. 仿真实验与分析
为验证改进TEB算法的可行性和合理性，根据露天矿区无人卡车动力学特征，实验中采用一款阿克曼小车（长宽比为0.4∶0.15=8∶3）模拟无人卡车行驶于矿区环境地图，进行优化融合算法验证，实验平台装有Ubuntu系统和ROS的Melodic机器人操作系统。同时在小车上搭载视觉相机、深度相机、激光雷达、惯性测量单元（inertial measurement unit, IMU）等传感器用于感知监测。
实验中使用Gazebo搭建道路边界和落石模型，模拟矿区道路及障碍物。根据某露天矿区的无人卡车道路现状，使用Gazebo的BuildingEditor构建2D赛道环境设计草图，车路比的设置根据不同测试场景进行相应调整，至此，矿区无人卡车仿真环境搭建完成。
A*算法与改进TEB算法融合结束后需根据实际运行效果在ROS功能包中完成相应配置，仿真小车所用的参数通过在绘制的矿区路线地图中多次调试获得，相关参数如表1所示。
表 1 关键参数Table 1 Key parameters参数 数值 轨迹时间分辨率 0.3 最大线速度/(m/s) 1.5 最大后退线速度/(m/s) 0.5 最大角速度/(rad/s) 2.0 最大线加速度/(m/s2) 1.5 最大角加速度/(rad/s2) 2.0 最小转弯半径/(°) 45 小车与障碍物的安全缓冲距离/m 0.05 小车与障碍物的膨胀距离/m 0.04 小车与障碍物的碰撞距离/m 0.02 3.1 改进TEB算法仿真对比测试
将预先建立的仿真环境加载至导航节点中，初始化小车位姿，在RVIZ可视化界面进行仿真测试，设置小车的初始坐标（−18.496，1.171，0.070），目标点（−13.631，1.063，0.147），在相同全局路径规划算法的基础上，使用TEB算法与改进TEB算法分别控制小车进行导航避障测试，比较2种算法控制下的运动规划、对障碍物的规避情况。
避障轨迹如图6所示。结果显示：传统TEB算法规划的轨迹穿过膨胀层紧贴障碍物，改进TEB算法由于改进了位姿轨迹点与膨胀缓冲层，生成的避障轨迹路径拐点较少使得路径长度明显减少，且规划轨迹位置处于障碍物膨胀层外，能有效避开障碍物，但在路径平滑度上会牺牲一定的代价。
图 6 导航避障仿真①表示小车规划起点，轨迹②表示改进TEB算法规划出的避障轨迹，③表示规划长度（实验中设置为3 m），箭头④表示目标点位姿，红色圆形表示障碍物，周围的紫色、蓝色光圈为其膨胀层及缓冲层。Fig. 6 Navigation obstacle avoidance simulation下载:
全尺寸图片
3.2 矿区特殊路段路径规划仿真结果及分析
矿区特殊路段路径规划仿真采用A*算法和改进TEB算法作为路径规划算法，仿真场景主要分为4种：1)狭窄路段仿真环境；2)多路交叉路口仿真环境；3)急弯路段仿真环境；4)障碍物阻碍路段仿真环境。规划过程中，黑色较细路径为全局路径，红色较粗路径为局部路径。
1）狭窄路段仿真环境
改进TEB算法在双车道及狭窄静态道路环境的路径规划过程见图7。该测试在车路比为1∶1.5及1∶2.5单双车道地图的基础上，模拟矿区运输卡车在卸载区（图7(a)①）与装载区（图7(a)③）之间的短距离运输。
图 7 狭窄路段仿真环境导航过程①表示起点，②表示狭长路段，箭头③确定目标点位姿。Fig. 7 Navigation process of the narrow sections simulate environment下载:
全尺寸图片
确定好目标点后，系统根据小车位姿规划全局路线，生成初始的局部路径，从图7(a)中①处可以看出，在初始无障碍物的道路上，局部路径跟随全局路径行驶，算法根据更新频率对行驶过的轨迹进行清除；图7(b)②处的矿区狭长道路中显示，改进TEB算法结合静态代价地图依次截取全局路径进行优化，使得优化后的轨迹远离边缘位于安全区域，满足无人卡车行驶要求，虽可能牺牲一定的运动时间，但可保证小车整体避开道路边缘，安全通过途中狭窄路段，直到所有的轨迹被清除，到达终点，如图7(c)。该实验表明无人卡车使用改进TEB算法能在短途运输中安全通过狭窄路段。
2）多路交叉路口仿真环境
为验证改进TEB算法在多路交叉路口的路径选择性能，本次实验搭建双车道地图进行仿真，路径选择过程如图8所示。
图 8 多路交叉路口仿真环境导航过程①表示起点，②表示原规划路线上的落石堆，③表示规划目标点位置及方向，④表示新规划路线转角。Fig. 8 Multi-intersection simulates the environment navigation process下载:
全尺寸图片
由图8(a)可以看出，在已知多路口栅格地图上，给出目标点③，系统自动选取路线较短的路径，小车开始沿着规划方向路口行驶，靠近路口时，系统中雷达等感知传感器检测到②处有大量落石障碍物阻挡行驶轨迹，见图8(b)，此时在已知栅格地图中搜寻其他路线，选择一条无障碍路径进行规划；图8(c)、(d)中，小车行驶到拐角④与③处时， 改进TEB算法通过位姿点融合与膨胀缓冲操作规划一段远离道路边缘且符合卡车行驶需求的轨迹，安全到达目标点。该实验表明无人卡车使用改进TEB算法能够在原规划路径中有障碍物无法逾越时，通过选择其他可通行路线安全到达目标点。
3）急弯路段仿真环境
矿区急弯道路的仿真使用车路比为1∶2的道路，如图9所示，图9(a)中可以看出从①处出发，全局路径紧贴道路边缘且拐角较多，而经过局部路径规划算法对静态道路边缘进行膨胀缓冲处理后，红色局部轨迹远离边界，更符合无人卡车几何运动学特征；图9(b)表明，改进TEB算法在过弯前②处规划出的过弯轨迹处于道路中央且线型相对流畅；图9(c)无人卡车跟随局部轨迹通过弯道；图9(d)完成弯道测试到达目标点④。该实验表明无人卡车使用改进TEB算法能安全通过急弯路段。
图 9 急弯路段仿真环境导航过程Fig. 9 Sharp curves of simulate environment navigation process下载:
全尺寸图片
4）障碍物阻碍路段仿真环境
为了进一步验证改进TEB算法在避障环境中生成轨迹的准确性，仿真实验环境使用车路比为1∶2.5的栅格地图。
避障过程如图10、11所示，在行进的路径边缘分别放置半径为0.1、0.15 m的小球。
图 10 半径0.1 m障碍物仿真路段导航过程①表示小车起点，②表示障碍物位置，③表示目标点位姿。Fig. 10 Radius 0.1 m obstacle simulation road section navigation process下载:
全尺寸图片
图 11 半径0.15 m障碍物仿真路段导航过程①表示小车起点，②表示障碍物位置，③表示目标点位姿。Fig. 11 Radius 0.15 m obstacle simulation road section navigation process下载:
全尺寸图片
图10、11(a)中，系统未感知到障碍物，故A*算法按照原规划正确规划出全局路径，同时在全局路径上放置小球；图10(b)中，由于障碍物与道路宽度相比体积较小，故小车的行驶路线仅需向无障碍一侧偏移少量，便可跟随全局路径安全通过障碍物位置，见图10(c)；而图11(b)中，障碍物较大使得小车可通行区域变小，当小车行驶至位置②时，改进TEB算法优化靠近小球的部分全局路径，在小球与道路边缘的膨胀层之间寻找安全通行的无障碍轨迹，行驶时与障碍物保持足够的安全距离，如图11(c)、(d)；图10(d)、(e)表明，在全局路径冗余时，局部算法可及时优化更新出高效的行驶轨迹。该实验表明无人卡车使用改进TEB算法能安全绕开半径分别0.1、0.15 m的障碍物。
图12(a)中从①出发，全局算法规划无障碍时的轨迹，在全局路径中②处放置半径为0.2 m的障碍物；图12(b)中，系统感知传感器检测出障碍物位置大小信息，小车运动到障碍物②前分析是否可通行；若无法通行，无人卡车及时制动于障碍物前的安全区域，如图12(c)；移除障碍物后，系统继续沿全局路径行驶至③处，如图12(d)、(e)。该实验表明无人卡车能在障碍物较大时及时制动，检测清除障碍物后，按照原轨迹继续完成导航任务。
图 12 半径0.2 m障碍物仿真路段导航过程Fig. 12 Radius 0.2 m obstacle simulation road section navigation process下载:
全尺寸图片
为验证改进算法在矿区无人卡车上避障效率及避障安全性方面的表现，在ROS仿真系统中进行了实验，比较传统TEB算法与改进TEB算法在相同避障场景下的性能。通过进行半径为0.1、0.15 m障碍物的2种避障场景下避障路径规划和执行，将执行时间、速度变化、进入碰撞层出现危险距离次数进行对比统计，得到了如图13及表2所示的结果。从图表中可以看出，在本文实验场景下，提出的改进算法相较于传统算法平均减少了34.37%的导航时间，基本能与障碍物保持安全距离，提高了算法的导航速度。
图 13 无人车执行避障时间、速度对比Fig. 13 Comparison of obstacle avoidance time and speed of unmanned vehicle下载:
全尺寸图片
表 2 传统TEB算法与改进TEB算法对比Table 2 Comparison between traditional TEB algorithm and improved TEB algorithm测试场景 安全性 效率 碰撞层/次 优化效果 耗时/ms 优化效果/% 场景1 传统TEB 3 提升2倍 6758.22 提升31.25 改进TEB 1 5149.12 场景2 传统TEB 7 提升1.3倍 7286.92 提升37.50 改进TEB 3 5299.58 4. 结束语
针对露天矿区复杂环境下，无人卡车导航时局部路径规划算法出现路径拐点多、路线规划不平滑、车速规划不平稳、避障距离过近等问题，提出了一种基于目标优化的改进TEB卡车路径规划算法。该算法通过对路径规划目标函数优化，实现了缩短导航时间、提高运输效率、规划轨迹安全有效的目标。选用矿区特殊路段对改进算法进行对比实验分析，实验结果表明，在本文静态场景下，改进TEB算法的平均导航时间减少约34.37%，可提高矿区无人卡车的导航速度，所规划的局部路径更加贴近卡车几何运动特征，实现了导航避障效率和安全性的优化。由于实验是在仿真环境中测试的，系统在复杂矿区环境下的鲁棒性仍有改进空间，后续需进一步优化无人卡车实车检测模块位置，在矿区现场环境中部署滚动落石、工人等移动障碍物甚至负障碍，以适应更复杂的矿区现场环境。
-
图 1 露天矿矿区运输路线
Fig. 1 Open-pit mine transportation road
下载:
全尺寸图片
图 2 金属矿山特殊运输路段
Fig. 2 Special transport sections for metal mines
下载:
全尺寸图片
图 3 TEB轨迹位姿序列
Fig. 3 TEB trajectory pose sequence
下载:
全尺寸图片
图 4 TEB融合位姿序列轨迹
Fig. 4 TEB fusion pose sequence trajectories
下载:
全尺寸图片
图 5 膨胀层处理示意
Fig. 5 Schematic diagram of the expansion layer treatment
下载:
全尺寸图片
图 6 导航避障仿真
①表示小车规划起点，轨迹②表示改进TEB算法规划出的避障轨迹，③表示规划长度（实验中设置为3 m），箭头④表示目标点位姿，红色圆形表示障碍物，周围的紫色、蓝色光圈为其膨胀层及缓冲层。
Fig. 6 Navigation obstacle avoidance simulation
下载:
全尺寸图片
图 7 狭窄路段仿真环境导航过程
①表示起点，②表示狭长路段，箭头③确定目标点位姿。
Fig. 7 Navigation process of the narrow sections simulate environment
下载:
全尺寸图片
图 8 多路交叉路口仿真环境导航过程
①表示起点，②表示原规划路线上的落石堆，③表示规划目标点位置及方向，④表示新规划路线转角。
Fig. 8 Multi-intersection simulates the environment navigation process
下载:
全尺寸图片
图 9 急弯路段仿真环境导航过程
Fig. 9 Sharp curves of simulate environment navigation process
下载:
全尺寸图片
图 10 半径0.1 m障碍物仿真路段导航过程
①表示小车起点，②表示障碍物位置，③表示目标点位姿。
Fig. 10 Radius 0.1 m obstacle simulation road section navigation process
下载:
全尺寸图片
图 11 半径0.15 m障碍物仿真路段导航过程
①表示小车起点，②表示障碍物位置，③表示目标点位姿。
Fig. 11 Radius 0.15 m obstacle simulation road section navigation process
下载:
全尺寸图片
图 12 半径0.2 m障碍物仿真路段导航过程
Fig. 12 Radius 0.2 m obstacle simulation road section navigation process
下载:
全尺寸图片
图 13 无人车执行避障时间、速度对比
Fig. 13 Comparison of obstacle avoidance time and speed of unmanned vehicle
下载:
全尺寸图片
表 1 关键参数
Table 1 Key parameters
参数 数值 轨迹时间分辨率 0.3 最大线速度/(m/s) 1.5 最大后退线速度/(m/s) 0.5 最大角速度/(rad/s) 2.0 最大线加速度/(m/s2) 1.5 最大角加速度/(rad/s2) 2.0 最小转弯半径/(°) 45 小车与障碍物的安全缓冲距离/m 0.05 小车与障碍物的膨胀距离/m 0.04 小车与障碍物的碰撞距离/m 0.02 表 2 传统TEB算法与改进TEB算法对比
Table 2 Comparison between traditional TEB algorithm and improved TEB algorithm
测试场景 安全性 效率 碰撞层/次 优化效果 耗时/ms 优化效果/% 场景1 传统TEB 3 提升2倍 6758.22 提升31.25 改进TEB 1 5149.12 场景2 传统TEB 7 提升1.3倍 7286.92 提升37.50 改进TEB 3 5299.58 -
[1] 顾清华, 江松, 李学现, 等. 人工智能背景下采矿系统工程发展现状与展望[J]. 金属矿山, 2022(5): 10−25. GU Qinghua, JIANG Song, LI Xuexian, et al. Development status and prospect of mining system engineering under the background of artificial intelligence[J]. Metal mine, 2022(5): 10−25. [2] ZHANG Wei, CHENG Hongtai, HAO Lina, et al. An obstacle avoidance algorithm for robot manipulators based on decision-making force[J]. Robotics and computer-integrated manufacturing, 2021, 71: 102114. doi: 10.1016/j.rcim.2020.102114 [3] ZHANG Lei, LI Yuan. Mobile robot path planning algorithm based on improved A star[J]. Journal of physics: conference series, 2021, 1848(1): 012013. doi: 10.1088/1742-6596/1848/1/012013 [4] 华洪, 张志安, 施振稳, 等. 动态环境下多重A*算法的机器人路径规划方法[J]. 计算机工程与应用, 2021, 57(10): 173−180. doi: 10.3778/j.issn.1002-8331.2007-0102 HUA Hong, ZHANG Zhian, SHI Zhenwen, et al. Robot path planning method of multiple A* algorithm in dynamic environment[J]. Computer engineering and applications, 2021, 57(10): 173−180. doi: 10.3778/j.issn.1002-8331.2007-0102 [5] 何朝伟, 田应仲, 贾亚飞, 等. 基于改进A~*算法的家居服务机器人路径规划[J]. 计量与测试技术, 2019, 46(8): 32−35. HE Chaowei, TIAN Yingzhong, JIA Yafei, et al. Home service robot path planning based on improved A~* algorithm[J]. Metrology & measurement technique, 2019, 46(8): 32−35. [6] OUSSAMA K. Real-time obstacle avoidance for manipulators and mobile robots[J]. The international journal of robotics research, 1985: 90-98. [7] 修彩靖, 陈慧. 基于改进人工势场法的无人驾驶车辆局部路径规划的研究[J]. 汽车工程, 2013, 35(9): 808−811. doi: 10.3969/j.issn.1000-680X.2013.09.011 XIU Caijing, CHEN Hui. A research on local path planning for autonomous vehicles based on improved APF method[J]. Automotive engineering, 2013, 35(9): 808−811. doi: 10.3969/j.issn.1000-680X.2013.09.011 [8] 张瑜, 宋荆洲, 张琪祁. 基于改进动态窗口法的户外清扫机器人局部路径规划[J]. 机器人, 2020, 42(5): 617−625. ZHANG Yu, SONG Jingzhou, ZHANG Qiqi. Local path planning of outdoor cleaning robot based on an improved DWA[J]. Robot, 2020, 42(5): 617−625. [9] WANG Y, HE T, SILVA P, et al. Wide-band high-accuracy ΔΣ ADC using segmented DAC with DWA and mismatch shaping[J]. Electronics letters, 2017, 53(11): 713−714. doi: 10.1049/el.2017.1068 [10] 姚莉娟, 李丽娟, 胡蓓蓓. 基于大规模化工过程的快速分布式MPC算法研究[J]. 现代化工, 2016, 36(3): 164−168. YAO Lijuan, LI Lijuan, HU Beibei. Fast distributed MPC algorithm based on large-scale chemical process[J]. Modern chemical industry, 2016, 36(3): 164−168. [11] BAI Xiong, JIANG Haikun, CUI Junjie, et al. UAV path planning based on improved A and DWA algorithms[J]. International journal of aerospace engineering, 2021, 2021: 4511252. [12] FOX D, BURGARD W, THRUN S. The dynamic window approach to collision avoidance[J]. IEEE robotics & automation magazine, 1997, 4(1): 23−33. [13] 林洁, 张志安. 改进人工势场法的路径规划研究[J]. 机械与电子, 2022, 40(3): 65−70, 75. doi: 10.3969/j.issn.1001-2257.2022.03.014 LIN Jie, ZHANG Zhian. Research on path planning of improved artificial potential field[J]. Machinery & electronics, 2022, 40(3): 65−70, 75. doi: 10.3969/j.issn.1001-2257.2022.03.014 [14] WANG Haojie, MA Xianghua, ZHU Li. Obstacle avoidance path planning of mobile robot based on improved DWA[J]. Journal of physics: conference series, 2022, 2383(1): 012098. doi: 10.1088/1742-6596/2383/1/012098 [15] 郑凯林, 韩宝玲, 王新达. 基于改进TEB算法的阿克曼机器人运动规划系统[J]. 科学技术与工程, 2020, 20(10): 3997−4003. doi: 10.3969/j.issn.1671-1815.2020.10.030 ZHENG Kailin, HAN Baoling, WANG Xinda. Ackerman robot motion planning system based on improved TEB algorithm[J]. Science technology and engineering, 2020, 20(10): 3997−4003. doi: 10.3969/j.issn.1671-1815.2020.10.030 [16] PONOMARENKO M R, KUTEPOV Y I. Mining complexity assessment to substantiate deformation monitoring at open pit mines[J]. Journal of mining science, 2021, 57(6): 986−994. doi: 10.1134/S1062739121060119 [17] ZHANG Hongxin, Li Jiaming, SHU R, et al. Research on dynamic obstacle avoidance method of manipulator based on binocular vision[J]. Recent patents on engineering, 2022, 16(6): e201221199157. [18] 顾清华, 刘思鲁, 张金龙. 基于进化算法MOEA/D-AU的露天矿多金属多目标智能配矿研究[J]. 有色金属(矿山部分), 2021, 73(6): 1−8. GU Qinghua, LIU Silu, ZHANG Jinlong. Research on multi-metal and multi-objective intelligent ore blending in open-pit mine based on evolutionary algorithm MOEA/D-AU[J]. Nonferrous metals(mining section), 2021, 73(6): 1−8. [19] ZHAO Ziyu, BI Lin. 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 [20] 邱锦标. 露天矿山道路设计及施工[J]. 采矿技术, 2012, 12(3): 42−46. doi: 10.3969/j.issn.1671-2900.2012.03.017 QIU Jinbiao. Design and construction of open-pit mine roads[J]. Mining technology, 2012, 12(3): 42−46. doi: 10.3969/j.issn.1671-2900.2012.03.017 [21] 高睿. 露天矿区无人驾驶矿车的安全通行辅助决策研究[D]. 重庆: 西南大学, 2021: 1-2. GAO Rui. Research on aid decision-making for driving safety of uncrewed mining trucks in open-pit mine[D]. Chongqing: Southwest University, 2021: 1-2. [22] 唐龙. 露天矿矿山采场道路的修筑和养护[J]. 中国设备工程, 2020(23): 217−219. doi: 10.3969/j.issn.1671-0711.2020.23.117 TANG Long. Construction and maintenance of stope road in open pit mine[J]. China plant engineering, 2020(23): 217−219. doi: 10.3969/j.issn.1671-0711.2020.23.117 [23] 孙盛, 闫永富, 王文才, 等. 露天矿斜坡路精细化爆破方法[J]. 内蒙古煤炭经济, 2020(19): 1−3. doi: 10.3969/j.issn.1008-0155.2020.19.002 SUN Sheng, YAN Yongfu, WANG Wencai, et al. Fine blasting method of slop road in open pit mine[J]. Inner mongolia coal economy, 2020(19): 1−3. doi: 10.3969/j.issn.1008-0155.2020.19.002 [24] 张彩杰. 露天矿山运输系统风险分析及路径优化研究[D]. 广州: 华南理工大学, 2016: 23-26. Zhang Caijie. Transportation system risk analysis and path optimization of open-pit mine[D]. Guangzhou: South China University of Technology, 2016: 23−26. [25] WU Jiafeng, MA Xianghua, PENG Tongrui, et al. An improved timed elastic band (TEB) algorithm of autonomous ground vehicle (AGV) in complex environment[J]. Sensors, 2021, 21(24): 8312. doi: 10.3390/s21248312 [26] ROESMANN C, FEITEN W, WOESCH T, et al. Trajectory modification considering dynamic constraints of autonomous robots[C]//7th German Conference on Robotics. Berlin: [s. n.], 2012: 1−6.
