近年来，无人驾驶技术在各个领域的发展十分迅猛，例如露天矿卡的无人化、地下采矿装备的智能化和地面交通工具的无人化等。对于无人驾驶车辆，环境建图、即时定位、轨迹规划和车辆控制是其自主导航的必要环节，其中路径规划是其自主行走的核心要素（陈盟等，2013）。地下移动装备在复杂多变的巷道或采场环境中运行，运输路线上可能临时出现人员或设备等障碍物，而且巷道会随着采掘推进而动态延伸，这时地下装备通常无法准确获取真实的环境信息，这就需要进行局部路径规划，生成动态的可实际运行路线，以达到主动避障、安全行车的目的（Larsson et al.，2006）。

然而，当前地下移动装备的路径规划主要是靠先前建立的全局地图进行预先规划路线（姜勇等，2018），然后通过控制算法使车辆跟随这条静态路径运行，该路径一般是在道路的中心线上，这使得装备的维护成本过高，而且安全性也得不到保障（杨超等，2011）。因此，地下移动装备必须具备在没有全局地图的情况下，依靠自身传感器识别周围环境并通过中央处理器决策行驶路线的能力，即以全局规划路线作为引导和参考，局部通过环境信息修正参考路线（罗维东等，2020），并产生符合地下移动装备运动学特性的轨迹，如图1所示。

前人针对地下装备路径规划问题开展了若干研究。白国星等（2020）开发了一种铰接式地下车辆的引导系统，通过路径跟踪方法能够在隧道环境下进行自主导航。Ma et al.（2020）将传统的利用欧几里得距离的D*算法改进为更适合地下矿山环境的曼哈顿距离，提高了路径规划的计算效率。Wang et al.（2022）改进了RRT*全局路径规划算法，解决了地下铰接结构车辆在巷道环境的路径规划问题，实现了地下装备的运动学特性。鲍久圣等（2022）提出基于改进A*和人工势场算法用于井下无轨胶轮车的路径规划，通过全局与局部路径规划算法，规划出可供无人驾驶无轨胶轮车会车或避障行驶的合理路径。综上可知，目前针对地下装备路径规划的研究较少，且提出的算法没有考虑在环境更新时如何动态更新路径，也无法在遇到动态障碍物时产生可行性较高的轨迹。

为了应对上述问题，研究人员提出了一种在线轨迹规划算法，即时间弹性带（TEB）方法（Rösmann et al.，2012，2013，2017），该算法已推广到一种有效的动态系统时间最优模型预测控制方法。此外，为了克服传统的本地规划器无法穿越动态障碍物的缺点，研究人员提出了在候选轨迹中切换到当前的最优轨迹，并通过构建运动模型对机器人未来状态进行预测，采用二次规划来进一步优化轨迹。陈纪廷等（2021）在传统TEB算法的基础上将障碍物聚类为凸多边形，降低了障碍物约束的计算量，提高了导航的实时性。陈奕梅等（2022）提出多机器人环境下TEB算法在动态避障时轨迹欠佳且速度输出不稳定的问题，将TEB算法与速度障碍法（VO）进行了融合，使得TEB算法在应对动态障碍物时效率更高。Smith et al.（2020）提出了egoTEB算法，通过使用以自我为中心的感知空间来表示局部代价地图，使得TEB算法在不确定环境中具有更高的稳定性。Zhang et al.（2018）提出将自动导航和自动泊车无缝集成的两阶段TEB规划算法，生成了自动驾驶汽车自动泊车所需的轨迹。

然而，上述改进算法无法应对地下巷道特殊场景，且在遇到动态障碍物时速度的跳变幅度较大，未能从能耗的角度考虑移动装备的轨迹规划问题，缺乏应用性。因此，在巷道环境发生变化时，为了增加移动装备运动过程的平滑性和高效性，本研究采用满足机器人非完整约束且具有实时性的TEB算法，在原有基础上考虑地下场景中巷道位置的动态障碍物、加速度、速度、急动度、曲率和能耗等关键因素，从而使移动装备在遇到动态障碍物时降低运动代价，行驶更加安全平滑且能够有效避障。

A*算法是经典的启发式搜索方法，其适应范围广且路径搜索效率较高，但A*算法是在栅格空间中进行搜索，其状态空间具有离散性，将其运用在井下实际空间中时规划路径不能被移动装备很好的运行。因此，在进行实际路径规划时，需要把移动装备的连续运动状态分配到相应的离散栅格中，提出将改进的混合A*算法运用到地下采场环境当中。

混合A*算法是A*算法的改进，增加了移动装备当前朝向和前后运动模式2个维度。4个维度连接起来的栅格状态可被井下移动装备实际执行。其栅格状态的目标函数为

式中：g(x)为路径起点到当前点的代价和，可以通过递归公式得到：

式中：g(xparent)为父节点；cost(xparent,x)为路径点的代价函数，cost(xparent,x)由上一路径点到当前路径点的长度以及是否有倒车、换挡等决定。cost(xparent,x)计算公式如下：

式中：dist(xparent,x)为父节点到当前节点的欧式距离长度；backcost为父节点到当前节点的运动模式，如果移动装备是向前行驶，此变量的值为0，向后倒车时值为1，即在倒车时代价值会迅速增加；switchcost为移动装备在行进过程当中的挡位频繁切换代价，该代价旨在限制车辆在前进和后退状态之间频繁换档；a和b为权重参数。

移动装备在世界坐标系的离散位姿状态可用sk=[xk,yk,βk]T表示，其中xk,yk,βk分别表示在k时刻的坐标(xk,yk)和方位角βk。TEB算法在经典弹性算法（EB）的基础上，增加了全局路径上2个位姿之间严格的时间间隔，ΔTk表示从位姿sk移动到sk+1所运动的时间，n个位姿生成n-1个等时长的位姿，其时间序列表示为

由于TEB轨迹严格按照时间顺序进行位姿移动，故TEB轨迹可由位姿序列和时间间隔序列组成，表示为

TEB算法的关键是对获得的全局路径进行实时调整和多目标非线性优化得到一条局部最优轨迹。其目标函数f(B)和优化后轨迹B*可表示为

传统TEB约束主要有障碍物约束、速度与加速度约束以及时间最优的目标函数约束，但对于具有特殊性的地下移动装备，传统约束无法适应地下场景。本文在传统TEB约束的基础上增加曲率约束、目标点约束、角急动度约束、线急动度约束和末端平滑约束，以适用地下复杂多变的动态环境。

曲率约束表示移动装备的最大转弯半径，由于在地下受限空间，移动装备的最大转弯半径必须约束在一定范围内，以保证地下装备行驶路径的平滑性（Duff et al.，2003），若其在转角位置或遇到动态障碍物时转角过大将会导致失稳。为了确保可驾驶性，曲率约束在每个顶点处都限制了路径的瞬时曲率，当ki=Δφixi>kmax时，其优化函数可表示为

式中：Δsi=si-si-1，为si位姿处的位移矢量；Δφi为顶点处切向角的变化量；kmax为最大容许曲率，最大容许曲率的偏离用二次惩罚函数σcur进行优化，曲率权重ωcur控制曲率对路径变化的影响。

目标点约束表示移动装备的最终停靠位置与设定目标点的欧式距离以及偏角达到最小状态，由于地下装备运行在地下受限空间，对装备位姿要求较高，所以规划的路径必须满足到达目标点时精准度的要求。此约束只会在末端平滑阶段才会被触发，可表示为

式中：Δφgoal表示当前节点速度方向与目标点的偏向角；σgoal表示超越目标点时的偏离惩罚；ωgoal表示目标点对路径变化的影响程度；buff为末端阈值。

角急动度表示移动装备在转向时的来回调整频率，当移动装备的角急动度过大时会造成车身振动，行驶轨迹不平滑，也容易造成移动装备在运行过程当中的侧翻。角急动度可表示为

式中：β⃗为角加速度。

线急动度表示移动装备在直线方向的加速度变化率，可表示为

式中：α⃗为线加速度。

本文改进的TEB算法是当移动装备离设定目标点的欧式距离在阈值范围内时，调整装备的行驶速度使其缓慢平稳地到达目标点，同时实现了更为精确的速度控制，其末端行驶速度可表示为

式中：vi为移动装备刚达到距离阈值时的速度；dlim为离目标点的末端阈值；d为移动装备到目标点的距离。

地下移动装备在运行时通常以电池作为驱动能源，电池容量的大小限制了地下移动装备的工作时长和运行效率，因此，通过轨迹规划加入能耗约束的方法能够实现最大化的降能（高志伟等，2020；杨怡婷，2022）。本文在图优化框架中加入能耗约束边产生性价比更高的轨迹。本研究只考虑小车在状态变化时的能量消耗，不考虑维持小车自身运行和运动摩擦所带来的能量消耗，即每2个局部目标点间能耗最优轨迹。由于机器人移动速度变化量Δv、角速度变化量Δω和运动方向的变化量为固定值，所以移动装备变状态能耗表达式为

式中：Ts为变化时间；Rm为电机内阻；ul-,ur-分别为在静止状态时的左右轮电压；Δul,Δur分别为左右轮电压波动。试验部分主要记录小车在一定时间内的电压波动来反映小车在轨迹规划后的能耗性能，通过寻找最优轨迹改变量和运行时间来使系统的总能耗降低。

图优化是把优化问题表现成图的形式。在改进TEB路径规划模块中，核心是通过迭代器求解出带有时间信息的优化位姿点序列，即算出最小代价下目标函数的位姿序列，但目标函数中的位姿是局部的，这就导致稀疏系统矩阵的产生，而且引入的非线性函数会加大求解难度。

本文采用图优化框架G2O（Kummerle et al.，2011）变步长迭代求解，通过只接受使得目标函数代价下降的步长加快求解效率。其中，顶点为移动装备状态、相邻位姿的时间间隔、障碍物以及目标点，边为相应的约束，而且路径点与障碍以及时间的优化求解正好可以用超图的形式表现，从而更直观地描述了求解的问题。改进的超图如图2所示。

全局规划器采用A*静态搜索算法，局部规划器采用改进的TEB算法，通过高分辨率的地图对障碍物信息进行精准定位，达到安全避障的效果。路径规划整体框架如图3所示。

为验证改进TEB算法在封闭地下环境中的可行性，在ROS框架中的Stage仿真平台进行试验，在rqt中进行参数调节，在rviz中进行可视化。针对地下移动装备在动态受限空间中的路径规划来说，面临最主要的问题是地下装备运行环境是复杂多变的巷道或采场环境，其位置高差随时都发生改变。本文采用的改进TEB算法是一种三维空间局部路径规划算法，选择仿真试验环境为二维空间，简化了模型，减少了计算量，所使用的传感器为激光雷达。当移动装备上坡或下坡时，其感知空间与小车处在同一个二维平面，故本文使用二维平面空间进行仿真试验。仿真试验主要是在巷道断面支持双线的二维环境下运行：正常情况下各行其道、有对侧车辆换道、交叉环境下多装备同时运行且随机出现障碍物的场景，仿真参数见表1。

在地下巷道动态受限空间内进行路径规划的基础上，进一步对地下移动装备完成整个装矿与卸矿过程的路径规划进行研究。该场景包含双线直线巷道和巷道交叉口等多个场景的综合，因此选择的井下出矿场景为分支鲱鱼骨式或分支人字形的出矿巷道布置形式，如图4所示。

正常情况下，地下移动装备在支持双线运行的巷道中运行时，各自行走在自车的路面中央，此时，当前车辆只需在自己的路线上平稳行驶，仿真试验如图5所示。由图5可以看出，自车在与对向车辆会车时有一个轻微的避障动作，这是自车为了降低总体代价值而产生的优化路径结果。

在双车道情况下，自车在遇到对向来车时，应具备避让对向来车的能力。根据井下行车规则，对向来车应减速行驶，自车在第二车道无障碍的条件下提前换道，为了提高行驶效率，两车不必停车，路径规划仿真结果如图6所示。由图6可以看出，自车产生了迅速换道的轨迹，安全性较高，且符合车辆的运动学规律。

地下移动装备在交叉巷道中会车的情况比较复杂。如图7所示，自车在出交叉口过程中遇到来车，根据岔道车辆先行的原则，只要岔道中有来车，其他车辆先停止，等待岔道车辆通过之后再按原路径进行行驶。若在本车行驶车道突然遇到随机障碍物，则自车轻微绕过障碍物，不必完全进入其他车道，待绕过之后再按原来车道继续行驶。

根据行人优先的规则，自车对前方的行人主动避障，并设置一定的安全阈值。可以看出，自车灵活地避开了来车，而且在遇到突然出现的随机障碍物时能够迅速躲避。

为验证改进算法在动态受限空间中的避障性能，将改进的TEB算法移植到小车上面。小车长为26 cm，宽为22 cm，搭载的系统为ubuntu 16.04，CPU为ARM Cortex-A57 （quad-core）@1.43GHz，运行环境为ROS kinetic。

由于井下巷道多为单车道巷道与双车道巷道相交叉，因此建立一个十字交叉巷道即可模拟真实巷道场景。考虑试验安全性与成本问题，选择在实验室场景下按大约9∶1的比例缩放巷道模型，模拟双向巷道的宽度为0.6 m，单车道宽度为0.3 m。试验分为2个阶段：第一阶段为对向存在车辆的换道避让场景；第二个阶段为交叉路口与对向存在多移动装备的场景。每个阶段为120组试验，即3个密度，每个密度场景下完成40组，不同密度代表随机出现的障碍物数，其中传统算法20组，改进算法20组。

第一阶段试验为小车在对向来车时的路径规划，结果如图8（a）所示。小车在一开始没有识别到隐藏在虚拟巷道内的障碍物，小车的起点坐标为（0，0），如图8（b）所示，随机出现障碍物坐标为（-25，240），（25，240），（0，180），（0，300），开始方向为y正轴方向，在行进过程中当激光雷达探测到障碍物时重新规划路径。记录小车的参数为穿越障碍物时间、路径长度、转弯次数和最大转向角。对比结果如表2所示。相比传统TEB算法，改进后的TEB算法有效缩短了行驶距离，并降低了小车的转弯频率。

第二阶段试验为小车在动态交叉环境下运行，如图9所示。小车起始坐标为（0，0），在靠近障碍物的过程中，动态障碍物以0.2 m/s的速度向小车靠近，从而模拟巷道环境中对向来车和岔道口来车时的场景，测量小车在穿越移动小车和随机障碍物时的转弯次数、停止和倒退次数，并输出小车在运动过程中的总体代价值和速度曲线，运行性能见表3，对比试验结果见图10。

由图10（a）、10（b）可以看出，小车在行驶过程中的平均代价值从23.09下降至10.19，反映了改进算法增加了小车在模拟巷道场景下的安全性，提高了运行效率。由图10（c）、10（d）可知，在对TEB算法增加末端平滑约束之后，其末端的减速过程更加平缓，速度波动减小。由表3可知，在小车穿越动、静障碍物时，改进的TEB算法规划的轨迹能够有效减少停退次数和转弯次数，这是通过约束加速度的频率变化带来的结果。

针对地下矿巷道环境中的移动装备轨迹规划问题，改进了TEB局部路径规划算法。首先针对地下矿用移动装备的运动模型进行建模和分析，然后在传统TEB算法基础上增加急动度约束、末端平缓约束和能耗约束，使得移动装备在地下矿环境中遇到静、动态障碍物时规划出更加平滑、安全、高效的运动轨迹，且在变化的环境中及时更新地图和路径。通过仿真和模拟试验，验证了改进算法的优越性，主要是在移动机器人的行走效率方面有所体现，减少了由于加速频率过快而带来的速度跳变，且从能耗的角度考虑轨迹规划，得到的轨迹更加易于控制且成本低，其总体代价降低了55.87%。

然而，本文所采用的方法仍有一定的局限性，主要体现在：本文方法只适用于固定四轮装备的阿克曼运动学模型而不适用于铰接车辆的路径规划，目前尚未与工业智能装备进行现场调试，方法的实际运用与现实需求还存在一定的差距，且目前的能耗模型不够准确。为此，未来在地下矿环境中可针对铰接车辆的运动学模型进行全局与局部结合的路径规划，进一步考虑地下巷道环境中移动装备的规划轨迹与实际行驶轨迹的匹配程度，使得规划的轨迹更加便于控制，通过自适应调整优化参数，使得目标函数更具实用性，同时可进一步建立准确的移动装备的运动能耗模型并对其能耗进行约束，进而降低装备的运行成本。

在“海陆矿”供给侧——聚焦一季度资源要素保增长

自然资源部4月12日发布数据显示，2023年以来，在严守资源安全基础上，通过优化用地用海用矿审批，经济高质量发展的要素保障能力不断提高。