基于双维度搜索的地下自主铲运机最优转弯轨迹规划

顾 青，刘 立，白国星，孟 宇✉

1) 北京科技大学机械工程学院，北京 100083 2) 北京科技大学顺德研究生院，佛山 528300

✉通信作者，E-mail: myu@ustb.edu.cn

地下铲运机是一种铰接式结构的地下无轨移动采矿装备，具有转弯半径小，地形适应能力强的特点，广泛应用于矿产开采和运输. 随着采矿行业的发展，采矿深度不断增加，地下高温高湿环境对驾驶员身体有极大伤害，因此，自主行驶成为新一代智能移动采矿装备的必备功能.

为了实现铲运机的自主行驶，轨迹规划与控制方法被广泛的研究[1-11]. 文献[1]～[8]研究了地下铲运机的导航控制方法，进而实现铲运机可以在地下巷道内采用“沿壁法”安全避障行驶. 文献[8]在无约束小范围内，可在铲装点和卸载点之间生成平滑行驶路径. 文献[9]提出了一种基于蚁群算法的路径规划方法，在地下巷道环境内，可生成全局最优路径. 文献[10]提出一种自主导航控制框架：首先人工示教生成导航地图和驾驶提示信息，然后在自主行驶时依照驾驶提示信息行驶. 基于这些研究，目前铲运机自主行驶技术可分为两类：一类是路径规划和跟踪，此类方法没有考虑到速度因素，因此对于行铲运机工作效率无法考虑，同时行驶速度较慢；另一类是先人工示教，然后自主行驶. 此类方法受到驾驶员经验影响较大，一般在直道中可以以较高的速度行驶，而接近转弯区域时，为了避免与巷道壁碰撞，驾驶员通常会提前减速，以很低的速度通过转弯区域，也就是说，通过转弯区域的行驶时间较长. 因为铲运机的地下金属开采工作主要是出矿，通常每个“铲—运—返”工作循环都要转弯，所以，转弯区域通过时间长会影响铲运机工作效率. 总的来说，目前铲运机的自主行驶水平还处于初级阶段.

对于采用跟踪目标要来完成自主行驶功能的控制框架来说，除了研究高精度的跟踪控制算法[11-12]，进行高质量的轨迹规划也是提高自主行驶水平的有效途径. 目前针对乘用车轨迹规划法方法的研究很多，总体可以分为直接法[13-19]、基于解耦的方法[20-32]，较好的解决了乘用车在边界曲率平缓的公路上的轨迹规划问题. 文献[31]研究了铲运机在地下巷道内典型循环工况的轨迹规划问题，可视为一种转弯轨迹规划问题. 其中，纵向轨迹规划是一个二次规划问题，易于求解. 然而横向轨迹还是一个非线性优化模型，因此求解时间较长（而且有可能得不到最优解或可行解）；此外，研究发现，当转弯角度过大时，现有的约束并不能约束铰接角及其角速度. 究其原因，还是没有将铰接角及其角速度作为决策变量加入到优化模型之中，无法对其直接约束. 然而，若是直接将铰接角及其角速度作为决策变量，则根据其运动状态方程，轨迹优化模型一定是一个复杂的非线性优化模型，能否得到最优解或可行解都没有保障.

为了解决上述问题，提高自主铲运机的弯道行驶性能，根据铲运机及其行驶环境特点，本文提出了一种基于双维度搜索的地下铲运机最优转弯轨迹规划方法. 本文的贡献及新颖之处在于设计了一种基于行驶里程和时间的双维度搜索策略，在轨迹计算阶段，建立了一种以平稳行驶为目标的纵横向解耦二次规划模型，其中，铰接角及铰接角速度约束并不在优化模型中直接考虑，而是通过对生成轨迹进行约束条件检查来筛选得到满足约束的最优轨迹. 与现有规划方法不同的是，使用本文提出的方法可以快速得到在纵向横向两个维度都具有最优性保证的轨迹，不需要后备策略，保障了导航控制系统中轨迹规划层的可靠性，提高了生成轨迹的质量和速度，使铲运机在转弯时可以合理调整行驶速度和位置，实现高效转弯，提高生产效率.

本文提出的方法具有以下特点：能够生成平稳的转弯轨迹，模型简单易于实时求解，所得到的轨迹具有最优性保证，不需要后备策略.

文章结构如下，第一章介绍铲运机运动学模型、转弯区域模型及所研究的具体问题，第二章介绍轨迹规划方法框架，第三章介绍轨迹规划方法数学模型，第四章和第五章是算例分析与样机试验，验证本文提出的方法有效性和优越性，第五章总结结论.

1 对象、场景模型与问题描述

1.1 铲运机运动学模型

由于铲运机（Load haul dump，LHD）具有铰接结构，对其运动姿态的描述与刚性车辆不同，在运动规划时需要同时考虑其航向角和铰接角. LHD的整体结构如图1 所示，车体由两部分组成. PA是前后车体的交接点， Pf(xf,yf)是前桥中心点，Pr(xr,yr)是 后桥中心点， θf和 θr分别是前后车体的航向角. Pf和 Pr与 PA之 间的距离分别为 Lf和 Lr. 定义铰接角 γ 为 θf与 θr之差

图 1 LHD 结构Fig.1 Structure of an LHD

PfPr和 之间的位置关系可以表示为

铰接车辆的运动学模型[30]可以描述为

其中， vf为前车体速度， γ˙ 为铰接角 γ的角速度. 可知，前车体航向角的角速度为

铰接角及其角速度的物理约束为

其中，γmin为铰接角最小值，γmax为 铰接角最大值，γ˙min为铰接角速度最小值， γ˙max为铰接角速度最大值.

1.2 弯道区域模型

转弯区域如图2 所示. 图2（a）中，巷道A 与B 相交，铲运机从巷道B 向巷道A 转弯. 在路口建立直角坐标系， O为转弯区域起点位置，位于巷道A，弯道区域直角坐标系Y 轴垂直于巷道B 的边界；红色虚线为转弯区域结束的位置，垂直于巷道A 的边界. Lsafe是安全距离长度，黑色虚线之间的蓝色阴影部分是除去安全边界的可行区域； α为两巷道夹角； LA和 LB分别为巷道A 和B 在转弯区域的长度. (xout,yout)是弯道出口处左边界点位置坐标. 地下巷道转弯处都会根据铰接式采矿装备的转弯半径进行有磨角，目的是为了使这类移动采矿装备能够安全通过路口. 磨角后，转弯处可行区域增大，如图2（b）所示， L′A和 LB′是磨角操作后巷道A 和巷道B 的剩余长度.

1.3 问题描述

当铲运机进入转弯区域时，根据铲运机当前速度、位置，实时生成一条平稳轨迹，使铲运机能够在安全转弯的情况下，平稳的行驶出转弯区域.在轨迹规划时，要满足下列约束条件：

（1）前后车体都不与巷道壁发生碰撞；

（2）铲运机速度不超过限速；

（3）铰接角和铰接角速度不超过设计约束值；

（4）在驶出转弯区域时，前车体方向与巷道壁平行.

2 轨迹规划算法框架

2.1 最优轨迹存在性分析

在矿山建设中，巷道的结构和尺寸与矿产种类、分布、采矿工艺相关. 也就是说，在建设之初，巷道尺寸和拟选用的采矿装备是相互匹配的. 而且，在地下巷道的路口处，都要根据采矿装备的转弯半径进行磨角，这是一种标准操作，目的就是为了能使铰接式的移动采矿装备可以安全无碰撞通过路口. 因此，对于给定铲运机以及符合其标准的路口，铲运机一定能够顺利通过.

图 2 地下巷道转弯区域. （a）磨角之前的转弯区域；（b）磨角之后的转弯区域Fig.2 Roadway tuning area: (a) before grinding; (b) after grinding

轨迹同时包含速度和位置信息，通常来说，可以通过路口的轨迹不止一条. 当铲运机采用不同的速度，沿不同曲率的路径曲线，都可以安全通过路口. 例如：对于一条比较平缓的位置曲线，采用不同的速度都可以很好的沿此位置曲线行驶，但是每种速度对应了不同的轨迹. 因此，当对轨迹设置一个的评价指标后，则在可行轨迹之中，存在一个最优指标值最小的最优解，本文的目标正是寻找这一最优轨迹.

2.2 算法思想及框架

本质上，弯道区域中每条可行轨迹可以通过不同行驶里程和行驶时间表征. 因此，为了能够根据入口速度和位置快速的生成最优轨迹，本文提出一种基于双维度搜索策略的轨迹优化方法. 此方法核心思想是在弯道行驶区域内，通过调整轨迹的里程和时间，找到最优轨迹. 本方法采用最优化算法进行构建，总体过程是：根据里程和时间迭代策略给出的终端位置和行驶时间，计算轨迹并检查是否符合铲运机铰接角物理约束，最终得到最优轨迹. 因此，算法主要包含三步：

（1）确定行驶里程和行驶时间.

此步骤的作用是确定当前轨迹规划问题的行驶里程与时间. 设计了一种双维度搜索策略，每次在轨迹计算前，首先通过搜索策略，从终端位置和行驶时间两个维度进行调整，确定当前迭代的终端位置和行驶时间.

（2）轨迹生成.

在转弯区域直角坐标系下，将轨迹分解为纵向和横向两个维度，分别采用二次规划生成最优轨迹（此时并不考虑铰接角约束）. 本算法中，横纵向轨迹都建模为二次规划问题，因此，所得到的轨迹在纵向和横向两个维度都具有最优性保证，而且求解速度快.

（3）轨迹铰接角约束检查及最优轨迹确定.

对于生成的轨迹，根据运动学方程计算铰接角和铰接角速度序列，若满足约束，且防碰撞检查通过，则最优轨迹找到；否则，回到第一个步骤进入下一次迭代.

使用本算法，针对一个转弯路口，基于铲运机进入转弯区域的位置和速度，本文提出的方法能够快速的找到一条纵向和横向都具有最优性保证的最优轨迹，不需要后备策略. 第三章介绍规划算法的具体细节.

3 轨迹规划算法

3.1 基于双维度搜索的终端位置和行驶时间确定

本算法中利用二次规划构建轨迹生成模型，设计基于双维度搜索的策略来确定优化问题的具体条件. 其中，外循环调整轨迹的行驶时间，内循环调整轨迹终端位置. 在每次循环中，首先计算纵向轨迹，然后计算横向轨迹.

记为i外循环中的循环变量， j为内循环中的循环变量，每次循环中行驶时间 Tij通过方程（8）设置，

其中： vin是铲运机进入转弯区域的入口速度； Δv为速度递减间隔.

每次循环中轨迹终端位置 (xendij,yendij)，通过如下方程设置，

其中： WB为 巷道B 宽度； Δd=(WB-2Lsafe)/m为位置递增间隔，即将巷道B 的可行宽度进行等间隔离散后的离散长度， m为间隔离散个数.

3.2 基于二次规划的转弯轨迹计算

本算法中轨迹计算是基于纵向横向解耦和二次规划完成的. 首先生成纵向行驶轨迹，然后生成横向行驶轨迹.

（1）纵向轨迹规划模型.

将转弯轨迹离散为 N 段，纵向轨迹规划建模为一个标准的二次规划问题，如公式（11-1）到（11-9）所示.

其中： Jlo是 纵向轨迹目标函数； Δt=Ti/N ， Ti是第i 次外循环时铲运机行驶时间； k是轨迹点序号；λ1和 λ2为 权重系数， ε1是 松弛变量， xi是铲运机前桥中心点纵向位置； vxi是前桥中心点纵向速度， axi和Δaxi分别是前桥中心点纵向加速度和加速度变化量， x0=0， vx0=vxin， ax0=axin；公式（11-5）表示的约束是为了令轨迹终端姿态与巷道壁平行. vxin和axin分别是铲运机进入转弯区域的入口纵向速度和加速度.

在本算法中，在目标函数是最小化加速度及其变化量. 没有将目标速度考虑在优化模型之中，这是因为在弯道区域，理想的目标速度很难设定，设置不好会造成实际速度曲线发生突变，难以控制.

（2）横向轨迹规划模型.

横向轨迹规划也采用二次规划模型，与文献[30]中方法不同的是，此时并不考虑铰接角相关约束，因此可以将横向轨迹规划模型也建立为一个标准二次规划模型，如公式（12-1）到（12-9）所示.

其中： Jla是横向轨迹目标函数； ρ1和 ρ2为权重系数，ε2是松弛变量， yi是铲运机前桥中心点横向位置；vyi是前桥中心点横向速度， ayi和 Δayi分别是前桥中心点纵向加速度和加速度变化量， y0=yin， vy0=vyin，ay0=ayin；公式（12-5）表示的约束是为了令轨迹终端姿态与巷道壁平行. vyin和 ayin分别是铲运机进入转弯区域的入口横向速度和加速度. 与纵向轨迹规划模型设计思想相同的是，目标函数是最小化加速度及其变化量.

3.3 基于约束检查的最优轨迹确定

上面的模型并未考虑铰接角约束，为了保证铰接角及其角速度符合约束，本算法设计了约束检查环节. 根据公式（4）可以计算得到此轨迹对应的铰接角和铰接角速度序列. 记本条轨迹中最大铰接角为 γi-max、最大铰接角速度为 γ˙i-max，对于可行的轨迹，有

同时，对于最优轨迹来说，松弛变量应该为0，即

此外，还需进行碰撞检查，本文采用的是最为简单的膨胀法，此处不赘述.

若公式（13）、（14）和（15）同成立，且铲运机与边界没有碰撞，则迭代停止，当前轨迹为最优轨迹；否则 j= j+1，返回公式（9）和（10）进行内循环；若 j=m 时都没有获得最优解，则 i=i+1，返回公式（8）进行外循环. 由于外循环相当于降低行驶速度，所以在延长转弯行驶时间的情况下，一定能找到符合约束的轨迹，此时迭代停止，当前轨迹为最优轨迹. 具体流程如图3 所示.

图 3 双维度搜索轨迹规划方法流程图Fig.3 Flow chart for the two-dimensional search-based trajectory planning method

4 算例分析与讨论

为了验证本文提出的轨迹规划方法，设计了三组算例. 设置了一种难度较高的转弯场景，转弯角度为直角，且铲运机从宽巷道向窄巷道转弯行驶，描述了一种铲运机从运输巷道向开采巷道内转弯的场景. 算例参数如表1 所示. 三组算例分别设计为：以相同的入口位置，不同的入口速度进入转弯区域. 将入口位置分为三个等级，由于铲运机在地下平直巷道工作时限速一般为15 km·h-1，也就是4 m·s-1左右，因此将入口速度分为四个等级，入口方向都与巷道B 保持平行. 然后，针对每个入口速度，求解其最优轨迹，求解采用Matlab quadprog函数，并将得到最优轨迹时内循环次数i、外循环的次数j 、最优轨迹对应的行驶时间 Tbest，以及此条轨迹中的最大铰接角 γmax和 最大角速度 γ˙max列出.

表 1 算例参数表Table 1 Parameters for case studies

（1）第一组算例，入口位置为（0,1.5）, 不同入口速度的最优轨迹计算结果列表如表2 所示.

表 2 第一组算例结果Table 2 Results of the first group

（2）第二组算例，入口位置为（0,2.5）, 不同入口速度的最优轨迹计算结果列表如表3 所示.

表 3 第二组算例结果Table 3 Results of the second group

（3）第二组算例，入口位置为（0,3.5）, 不同入口速度的最优轨迹计算结果列表如表4 所示.

表 4 第三组算例结果Table 4 Results of the third group

从上面的算例可以看出，针对不同的入口速度和位置，采用所提出的规划方法都可以得到最优轨迹.

在相同的位置，入口速度越高，迭代次数越多. 根据本算例中的问题规模，单次轨迹生成时间为0.005 s 左右，若是迭代上百次，则有可能超过控制周期（例如，当入口速度为4 m·s-1时，迭代次数为17×3×2=102 次，则得到轨迹的时间超过了0.5 s），这就会影响轨迹跟跟踪性能. 因此，可以通过调整速度减小间隔 Δv这一迭代关键参数的方法来减少迭代次数. 例如，当 Δv=0.2 m·s-1时，迭代次数减少一半. 因此，本方法可以根据控制器性能，调整间隔，满足实际控制需求.

为了更直观显示轨迹，以入口坐标为（0, 2.5），入口速度为2 m·s-1的情况为例，图4～6 给出了最优轨迹的位置曲线，速度曲线、铰接角、航向角和相应的角速度曲线. 图6 中， vγ、 vθf和 vθr分别是铰接角和前后车体的航向角速度. 可以看出，沿行驶方向速度、铰接角和铰接角速度变化平缓，适合跟踪控制.

图 4 位置曲线（y in=2.5,vxin =2）Fig.4 Position trajectory (y in=2.5,vxin =2)

图 5 行驶方向速度曲线（y in=2.5,vxin =2）Fig.5 Heading velocity trajectory (y in=2.5,vxin =2)

图 6 铰接角、前后车体航向角及角速度（y in=2.5,vxin =2） Fig.6 Angle and angular velocity for γ , θf and θr(y in=2.5,vxin =2)

通过本文提出的方法可知， Tbest可视为以当前初始状态及平稳目标下铲运机通过弯道的最短时间. 因此，当找到最优轨迹之后，若将行驶时间继续延长，也就是继续外循环，也能找到满足新的行驶时间的解. 同理，对于一个固定的终端位置，只采用外循环延长时间，也可得到满足要求的最优轨迹，下面分别举例.

对于入口位置为（0, 2.5），入口速度为2 m·s-1的情况，得到最优轨迹时，i=7，j=4，Tbest=42.84 s，轨迹在出口的终端位置为（33, 35）. 此时，若保持此终端位置，将行驶时间设置为70 s，通过本文提出的方法得到最优转弯轨迹如图7～9 所示.

图 7 位置曲线（T i=70 s）Fig.7 Position trajectory (T i=70 s)

图 8 行驶方向速度曲线（T i=70 s ）Fig.8 Heading velocity trajectory (T i=70 s )

图 9 铰接角、前后车体航向角及角速度（T i=70 s）Fig.9 Angle and angular velocity for γ , θf , and θr(T i=70 s)

还是以入口位置为（0, 2.5），入口速度为2 m·s-1的情况为例，当要求铲运机行驶出转弯区域时，位置位于巷道中心线，也就是出口位置为（32.25, 35），采用本文提出的方法，只进行外循环，得到的最优轨迹如图10～12 所示. 此时i=9，弯道行驶时间为46.15 s.

图 10 位置曲线（出口位置为（32.25, 35））Fig.10 Position trajectory (exit position is (32.25, 35))

图 11 行驶方向速度曲线（出口位置为（32.25, 35））Fig.11 Heading velocity trajectory (exit position is (32.25, 35))

图 12 铰接角、前后车体航向角及角速度（出口位置为（32.25, 35））Fig.12 Angle and angular velocity for γ , θf , and θr (exit position is( 32.25, 35))

5 样机试验

为了表明算法的可行性，进行了样机试验. 样机如图13 所示. 采用激光雷达和电子地图进行定位. 工控机为研华ARK-3500，Intel Core i5-3610ME，2.7 GHz. 试验环境为楼道内的一个直角转弯处，楼道宽度为2.2 m. 由于是楼道试验，为了保证安全，最高速度设置为1 m·s-1. 试验参数设置如表5所示. 样机从转弯区域外开始启动并加速到1 m·s-1，保持此速度进入转弯区域，车辆控制周期为0.2 s.要求行驶出转弯区域时车辆位于巷道中线，基于本文提出的轨迹规划方法，得到最优解时，i=1，因此轨迹生成时间在一个控制周期之内. 所生成的轨迹时间为7.2 s，速度和角度曲线如图14 所示.路径采用LTV-MPC 进行控制，速度采用PID 控制，样机也平稳安全的通过了直角弯道，图15 显示了速度和航向角误差，其中， Eθf为前车体航向角跟踪误差， Ev为铲运机速度跟踪误差. 可以看出，误差都保持在一个很小的水平.

图 13 铰接车试验平台Fig.13 Articulated vehicle

6 结论

（1）本文提出的方法可以生成纵向和横向都具有最优性保证的轨迹，理论上保证了生成轨迹的最优性.

（2）本文提出的方法优化得到的轨迹，其纵向速度、铰接角及角速度变化平缓，易于跟踪控制.

（3）本文提出的算法可根据底层控制器的控制周期灵活调整迭代关键参数，进而调整轨迹生成时间，满足控制器对目标轨迹生成时间的要求.

（4）本文提出的算法适用于宽窄不同的、角度不同的转弯路口，不需要复杂的边界处理与坐标转换，因此便于实际部署实施.

（5）本文提出的方法既可以根据入口速度和位置生成安全平稳的最优转弯参考轨迹；也可用于根据具体条件确定弯道最短行驶时间，因此，不仅适用于实时轨迹规划，也可为多装备协同运行，甚至未来矿山的智能管控和调度等问题提供系统约束.

表 5 试验参数表Table 5 Parameters for experiments

图 14 参考速度和航向角曲线Fig.14 Reference trajectory for velocity and heading

图 15 速度和航向角跟踪误差Fig.15 Tracking Errors trajectory for velocity and heading