一种基于改进TEB方法的露天矿无人矿卡动态路径规划方法

摘要

本发明提出了一种露天矿无人矿卡动态路径规划方法，解决当前无人矿卡动态路径规划问题，同时重点解决无人矿卡在露天矿场景下的动态路径规划中卡车运行安全问题。建立以满足运载时间最短为目标、以避开障碍物、朝局部目标前行、非完整性约束、运载时的速度、加速度、急动度及在重载下坡和空载上坡情况下的牵引力为约束的露天矿无人矿卡动态路径规划问题模型，用权重求和模型表示该优化问题，并使用分类权重法对权重求和模型的目标函数权重进行设定，再使用g2o通用图形优化方法，对露天矿无人矿卡动态路径规划问题进行解算，求出局部最优轨迹。

说明书

技术领域

本发明属于矿业系统工程、矿山优化及智能驾驶技术领域，特别涉及一种基于改进TEB(时间弹性带,Timed-Elastic-Band)方法的露天矿无人矿卡动态路径规划方法。

背景技术

随着5G通信技术的发展，无人驾驶领域发展迅速，无人车路径规划是无人驾驶的研究的关键，而目前大多数无人驾驶的路径规划研究基本集中在高速公路等城市道路下，少有针对复杂场景下的无人驾驶路径规划的方法研究。在露天矿山场景中，因为工作条件恶劣、安全事故频发，无人矿卡的落地运行是未来露天矿运载的主要趋势，而想要实现无人矿卡的实际落地，无人矿卡除了具有全局静态路径规划能力，还必须具备局部动态路径规划能力。因为全局路径规划下无人矿卡不能应对动态环境的变化，如对动态障碍物的避障行为，这为无人矿卡行驶的安全造成了威胁。目前还没有针对露天矿场景为无人矿卡设计的局部路径规划方法。

发明内容

为了克服上述现有技术的缺点，解决无人矿卡在露天矿场景下的动态路径规划中矿卡运行安全问题，满足当前露天矿对无人矿卡的动态路径规划实际需求，本发明的目的在于提供一种基于改进TEB方法的露天矿无人矿卡动态路径规划方法。

为了实现上述目的，本发明采用的技术方案是：

一种基于改进TEB方法的露天矿无人矿卡动态路径规划方法，包括：

步骤1，建立以满足运载时间最短为目标，以运载时的速度、加速度、急动度、避开障碍物、朝局部目标前行、非完整性约束及在重载和空载情况下上坡和下坡的牵引力为约束的露天矿无人矿卡动态路径规划问题模型；

步骤2，将该模型用权重求和模型表示，并使用分类权重法对权重求和模型的目标函数权重进行设定；

步骤3，使用g2o通用图形优化方法，对露天矿无人矿卡动态路径规划问题进行解算，求出局部最优轨迹。

与现有技术相比，本发明在现有TEB方法进行动态路径规划的基础上进一步考虑了矿卡在重载下坡和空载上坡情况下牵引力的约束，增加了牵引力的约束，为无人矿卡在相应的场景中规定安全范围内的行驶速度，防止无人矿卡上坡和下坡时溜车的情况出现，保障矿卡在坡道上的行驶安全。除此之外增加了急动度约束，将急动度限定在一个较小的范围内，从而提高矿卡运输的平顺性。以上述目标和约束条件构建动态路径规划的优化模型，使用权重求和模型表示模型，并使用分类权重法在保证矿卡安全行驶的前提下，按照其重要程度对每项目标函数的权重进行分类，最后采用g2o方法对动态路径规划模型求解。相较于原始TEB方法和其他局部路径规划方法求解动态路径规划问题更符合露天矿运输实际道路情况。本发明对无人矿卡在矿区内各个场景下安全行驶具有着重要的意义。

附图说明

图1是本发明中采用改进TEB方法进行动态路径规划的流程图。

图2是运动体在世界坐标系下的轨迹序列。

图3是无人矿卡在上坡和下坡时的受力分析图。

图4是根据问题模型所构建的超图。

具体实施方式

下面结合附图和实施例详细说明本发明的实施方式。

本发明为基于改进TEB方法的露天矿无人矿卡动态路径规划方法，该方法进行动态路径规划流程图如图1所示。其中包含有以下步骤：

(1)TEB动态路径规划模型构建。

TEB将矿卡的全局规划后路径上的位姿轨迹模型抽象成带有时间信息的弹性带模型。具体地，将动态路径规划前已知的全局静态轨迹中插入多个控制橡皮筋(Elastic-Band)形状的控制点(即矿卡的位姿)，为了显示轨迹的运动学信息，在点与点之间定义时间信息，建立带有时间信息的橡皮筋模型，之后再引入目标和约束。在矿卡的全局静态轨迹中，

矿卡的第i个位姿状态可以表示为s

C＝{x

n为位姿个数，n∈N，(N为整数)

TEB描述的轨迹模型的位姿s

Δτ＝{ΔT

每个时间间隔表示机器人从一个位姿运动到另一个位姿的时间。TEB轨迹模型包含位姿序列信息和时间间隔序列信息，用TEB方法表示的轨迹模型T由下式表示：

T＝(C,Δτ) (3)

其中C为矿卡的位姿序列信息，Δτ为时间间隔序列信息，将位姿和时间间隔设为待优化的变量，通过设定目标和相应的约束，求解出当前时刻最优的局部路径轨迹。

模型目标为：

时间间隔最优函数：

约束条件包括：

①速度与加速度约束

ω

a

α

v

a

ω

α

②局部目标点及障碍物约束

f

f

④非完整性约束

连续的两个位姿需要位于具有恒定曲率的公共圆弧上：

θ

即位姿状态s

以上为原始TEB方法中包含的目标和约束，在为露天矿矿卡设计动态路径规划模型中除了要考虑上述目标和约束外，还应该考虑露天矿矿卡运载过程中的实际情况，参考图3，本发明将矿卡在重载下坡和空载上坡两种情况下牵引力约束引入模型，并考虑了矿卡在运输过程中的平顺性，引入矿卡急动度约束，改进的TEB方法所增加的约束条件包括：

⑤牵引力约束

若要保证矿卡在上坡过程中有足够的牵引力，在空载上坡过程中，牵引力的约束F

在重载下坡过程中，牵引力的约束F

⑥急动度约束

急动度表现为加速度的变化率，在矿卡运输的过程中如果急动度过大会对矿卡的运输造成冲击，影响运输的平顺性，故本发明增加了急动度约束，将急动度限定在一个较小的范围内，从而提高矿卡运输的平顺性。其表示如下：

j＝a

指定速度和急动度限制：

j

式中，f

(2)用权重求和模型表示该问题模型，并使用分类权重法对模型的目标函数权重进行设定。

用权重求和模型表示上述优化问题模型，将约束条件通过罚函数表示，最终将由约束条件转换的罚函数与目标函数通过加权得到最终的成本函数f(B)，表示如下：

通过最小化成本函数可以求得最优轨迹和时间间隔：

B

f

当问题用权重求和模型表示后，应对每一罚函数及目标函数设定权重，本发明使用分类权重法对权重求和模型的目标函数的权重进行设定，分类权重法的原理是“关键的少数和次要的多数”。首先是“排队阶段”：对各个影响因素进行分析，根据露天矿无人矿卡的运输特点和各因素对矿卡路径的影响及重要程度，将因素按照重要性程度进行排序。第二，“分类阶段”：将所有罚函数及目标函数划分为三类，即A类：主要因素，占10％左右，B类：次要因素，占20％左右，C类：一般因素，占70％左右。第三：权重设定阶段：根据分类结果，对A、B、C三类目标函数分类赋予不同权值，具体可如下表所示。

(3)上述TEB优化问题可以转化为以位姿和时间间隔为节点，目标函数以及约束的罚函数为边的超图问题，再使用g2o中非线性优化库进行求解，求出局部最优轨迹，上述模型可以表现为如图4所示的超图。

g2o是一个通用图优化的开源框架，这个框架主要解决如下形式的最小二乘法问题：

其中e

综上，本发明针对露天矿无人矿卡动态路径规划问题，以最优路径时间为目标，以运载时的速度、加速度避开障碍物、朝局部目标前进、非完整性约束及在重载下坡和空载上坡情况下的牵引力、急动度为约束条件，构建了露天矿无人矿卡动态路径规划模型，用权重求和模型表示该问题模型，并使用分类权重法对模型的目标函数权重进行设定。使用g2o框架求解，实现了露天矿无人矿卡动态路径规划的问题的科学合理求解。解决当前无人矿卡动态路径规划问题，同时解决了无人矿卡在露天矿运输场景下的动态路径规划中卡车运输安全问题。上面结合附图对本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种非实质性的改进，或未经改进将本发明的构思和技术方案直接应用于其它场合的，均在本发明的保护范围之内。