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

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

【技术实现步骤摘要】
一种基于改进TEB方法的露天矿无人矿卡动态路径规划方法
本专利技术属于矿业系统工程、矿山优化及智能驾驶
，特别涉及一种基于改进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个位姿状态可以表示为si＝[xi，yi，θi]，其中xi、yi为位置信息，θi为方向角。使用TEB方法表示矿卡轨迹序列如图2所示。矿卡在世界坐标系XOY下的位姿序列表示为：C＝{xi}，i＝0，1，…，n；(1)n为位姿个数，n∈N，(N为整数)TEB描述的轨迹模型的位姿si到si+1的过渡时间间隔为ΔTi，n个位姿间隔的时间序列为：Δτ＝{ΔTi}，i＝0，1，…，n-1(2)每个时间间隔表示机器人从一个位姿运动到另一个位姿的时间。TEB轨迹模型包含位姿序列信息和时间间隔序列信息，用TEB方法表示的轨迹模型T由下式表示：T＝(C,Δτ)(3)其中C为矿卡的位姿序列信息，Δτ为时间间隔序列信息，将位姿和时间间隔设为待优化的变量，通过设定目标和相应的约束，求解出当前时刻最优的局部路径轨迹。模型目标为：时间间隔最优函数：约束条件包括：①速度与加速度约束ωi＝(θi+1-θi)/ΔTi(6)ai＝2(vi+1-vi)/(ΔTi+ΔTi+1)(7)αi＝2(ωi+1-ωi)/(ΔTi+ΔTi+1)(8)根据露天矿现场矿卡具体情况，指定速度和加速度限制：vmin≤vi≤vmax(9)amin≤ai≤amax(10)ωmin≤ωi≤ωmax(11)αmin≤αi≤αmax(12)②局部目标点及障碍物约束fpath＝g(dmin,k,rPmax)(13)fob＝g(-dmin,j,-rOmax)(14)④非完整性约束连续的两个位姿需要位于具有恒定曲率的公共圆弧上：θi＝θi+1(15)即位姿状态si和方向之间的角度θi等于下一时刻位姿状态si+1对应的角度θi+1以上为原始TEB方法中包含的目标和约束，在为露天矿矿卡设计动态路径规划模型中除了要考虑上述目标和约束外，还应该考虑露天矿矿卡运载过程中的实际情况，参考图3，本专利技术将矿卡在重载下坡和空载上坡两种情况下牵引力约束引入模型，并考虑了矿卡在运输过程中的平顺性，引入矿卡急动度约束，改进的TEB方法所增加的约束条件包括：⑤牵引力约束若要保证矿卡在上坡过程中有足够的牵引力，在空载上坡过程中，牵引力的约束Fupmin为：假设矿卡发动机功率恒定，可以将牵引力的约束转换为速度的约束：在重载下坡过程中，牵引力的约束Fdownmin为：将牵引力的约束转换为速度的约束：⑥急动度约束急动度表现为加速度的变化率，在矿卡运输的过程中如果急动度过大会对矿卡的运输造成冲击，影响运输的平顺性，故本专利技术增加了急动度约束，将急动度限定在一个较小的范围内，从而提高矿卡运输的平顺性。其表示如下：j＝ai+1-ai/(0.25ΔTi+0.5ΔTi+1+0.25ΔTi+2)(20)指定速度和急动度限制：jmin≤j≤jmax(22)式中，ftime为时间最短目标函数，vi为线速度，ωi为角速度，ai为线加速度，αi为角加速度，vmin为最小线速度，vmax为最大线速度，amin为最小线加速度，amax为最大线加速度，ωmin为最小角速度，ωmax为最大角速度，αmin为最小角本文档来自技高网...

【技术保护点】
1.一种基于改进TEB方法的露天矿无人矿卡动态路径规划方法，其特征在于，包括：/n步骤1，建立以满足运载时间最短为目标，以运载时的速度、加速度、急动度、避开障碍物、朝局部目标前行、非完整性约束及在重载和空载情况下上坡和下坡的牵引力为约束的露天矿无人矿卡动态路径规划问题模型；/n步骤2，将该模型用权重求和模型表示，并使用分类权重法对权重求和模型的目标函数权重进行设定；/n步骤3，使用g2o通用图形优化方法，对露天矿无人矿卡动态路径规划问题进行解算，求出局部最优轨迹。/n

【技术特征摘要】
1.一种基于改进TEB方法的露天矿无人矿卡动态路径规划方法，其特征在于，包括：
步骤1，建立以满足运载时间最短为目标，以运载时的速度、加速度、急动度、避开障碍物、朝局部目标前行、非完整性约束及在重载和空载情况下上坡和下坡的牵引力为约束的露天矿无人矿卡动态路径规划问题模型；
步骤2，将该模型用权重求和模型表示，并使用分类权重法对权重求和模型的目标函数权重进行设定；
步骤3，使用g2o通用图形优化方法，对露天矿无人矿卡动态路径规划问题进行解算，求出局部最优轨迹。


2.根据权利要求1所述基于改进TEB方法的露天矿无人矿卡动态路径规划方法，其特征在于，所述步骤1中，将动态路径规划前已知的全局静态轨迹中插入多个控制橡皮筋(Elastic-Band)形状的控制点，即矿卡的位姿，为了显示轨迹的运动学信息，在点与点之间定义时间信息，建立带有时间信息的橡皮筋模型，之后再引入目标和约束。


3.根据权利要求2所述基于改进TEB方法的露天矿无人矿卡动态路径规划方法，其特征在于，所述矿卡的全局静态轨迹中，矿卡的第i个位姿状态表示为si＝[xi，yi，θi]，其中xi、yi为位置信息，θi为方向角，矿卡在世界坐标系XOY下的位姿序列表示为：
C＝{xi}，i＝0，1，…，n；(1)
n为位姿个数，n∈N，N为整数；
si到si+1的过渡时间间隔为ΔTi，n个位姿间隔的时间序列为：
Δτ＝{ΔTi}，i＝0，1，…，n-1(2)
每个时间间隔表示矿卡从一个位姿运动到另一个位姿的时间，用TEB方法表示的轨迹模型T由下式表示：
T＝(C,Δτ)(3)
其中C为矿卡的位姿序列信息，Δτ为时间间隔序列信息，将位姿和时间间隔设为待优化的变量，通过设定目标和相应的约束，求解出当前时刻最优的局部路径轨迹。


4.根据权利要求3所述基于改进TEB方法的露天矿无人矿卡动态路径规划方法，其特征在于，所述以满足运载时间最短为目标，即时间间隔最优函数：



约束条件包括：
①速度与加速度约束



ωi＝(θi+1-θi)/ΔTi(6)
ai＝2(vi+1-vi)/(ΔTi+ΔTi+1)(7)
αi＝2(ωi+1-ωi)/(ΔTi+ΔTi+1)(8)
根据露天矿现场矿卡具体情况，指定速度和加速度限制：
vmin≤vi≤vmax(9)
amin≤ai≤amax(10)
ωmin≤ωi≤ωmax(11)
αmin≤αi≤αmax(12)
②局部目标点及障碍物约束
fpath＝g(dmin,k,rPmax)(13)
fob＝g(-dmin,j,-rOmax)(14)
④非完整性约束
连续的两个位姿需要位于具有恒定曲率的公共圆弧上：
θi＝θi+1(15)
即位姿状态si和方向之间的角度θi等于下一时刻位姿状态si+1对应的角度θi+1；
⑤牵引力约束
若要保证矿卡在上坡过程中有足够的牵引力，在空载上坡过程中，牵引力的约束Fupmin为：



假设矿卡发动机功率恒定，将牵引力的约束转换为速度的约束：



在重载下坡过程中，牵引力的约束Fdownmin为：



将牵引力的约束转换为速度的约束：



⑥急动度约束
j＝ai+1-ai/(0.25ΔTi+0.5ΔTi+1+0.25ΔTi+2)(20)



指定速度和急动度限制：
jmin≤j≤jmax(22)



式中，ftime为时间最短目标函数，vi为线速度，ωi为角速度，ai为线加速度，αi为角加速度，vmin为最小线速度，vmax为最大线速度，amin为最小线加速度...

【专利技术属性】
技术研发人员：顾清华，张晓玥，冯治东，阮顺领，李学现，王倩，陈露，
申请(专利权)人：西安建筑科技大学，
类型：发明
国别省市：陕西;61