基于矿卡和挖机之间精准定向需求的双层轨迹规划技术研究

熊焱飞,梅贵周,奚小虎,王亚飞,汪贵冬

(1.安徽海博智能科技有限责任公司, 安徽 芜湖市 241200;2.上海交通大学 机械与动力学院, 上海 200240)

0 引言

根据挖机所在的位置和挖机附近所在山体形状,露天矿山作业平台协作场景一般可分为一般场景和复杂场景。一般场景是指在矿车视线范围内,且挖机周围无遮挡,挖机周围无特殊山体形状环绕,在一般场景中,矿车从平台主路到挖机装料点都能很顺利地规划出泊车轨迹,并能很顺利地到达挖机铲下进行装料作业。而在复杂场景中,挖机在不同位置时,高精地图虽然能解决此类路径规划问题,但是考虑到矿山场景以小时的频率在变动,而高精地图的制作流程相当复杂,周期和成本都非常高,满足不了矿山实时生产需求,因而通过高精地图的路径规划算法不适用于矿山自动驾驶场景。在无高精地图的情况下,不同的复杂场景都会导致路径规划与山体有碰撞或规划路径不合理,从而导致装载功能失效[1]。

针对矿山复杂场景应用的需要以及轨迹规划功能需求,矿山自动驾驶技术的难点主要概括如下。

(1)作业平台协作场景的时变性和复杂性。作业平台协作场景的时变特性使得无人驾驶矿卡需要实时更新全局地图信息,提升矿卡轨迹规划的安全性和准确性。

(2)矿卡挖机精准定向需求。装载作业需要挖机和矿卡的协作,为了满足挖机工作需求,矿卡不仅需要行驶到装载点,还需满足矿卡和挖机的精准定向。

针对上述矿山自动驾驶技术的难点,为了实现复杂场景下的矿卡精准定位,本文提出了一种基于自感知全局地图、考虑矿卡挖机精准定向需求的双层轨迹规划技术,以此满足复杂矿山作业平台协作场景的路径规划需要,该技术可为其他矿山的自动驾驶提供参考。

1 作业平台协作场景的路径规划方法

路径规划框架主要由三部分组成,基于矿车多源感知数据的地图构建、基于混合A*算法的轨迹规划关键点选取以及面向矿卡挖机精准定向需求的DWA局部规划,如图1所示。

图1 路径规划框架

1.1 基于矿车多源感知数据的地图构建

为了提高铲装区泊车规划算法的成功率,需要在矿车作业前提供铲装点附近的栅格地图。铲装区场景具有场景开阔、纹理较少、路面颠簸等特点,在这样的场景中,仅使用激光雷达传感器建立点云地图的方案(即激光SLAM)的数据匹配环节鲁棒性差,整体建图效果不佳。融合车辆组合导航、毫米波雷达等其他传感器的信息能够有效提升建图精度和建图效果,为轨迹规划提供车辆的全局感知地图。

考虑到在特殊路况下,矿车在行驶过程中会发生较大颠簸,导致车辆的控制结果与预想的结果产生不可预料的偏差;另一方面受限于车辆底层的控制精度,矿车在实际控制中无法理想地跟踪到混合A*轨迹中的每个点。与此同时,由于在大地图情况下,混合A*轨迹规划效率低、时间长,并不能满足矿卡实时轨迹规划的应用需求。因此,本研究采用了基于混合A*算法的轨迹规划关键点选取和面向矿卡挖机精准定向需求的DWA局部规划组成的双层轨迹规划框架。基于矿车多源感知数据的全局栅格地图[2]如图2所示。

图2 基于矿车轨多源感知数据的全局地图

1.2 基于混合A*算法的轨迹规划关键点选取

混合A*考虑了车辆动力学模型,使得规划出的路径符合车辆动力学约束,且输出的路径为一系列带有坐标和朝向信息的离散点。基于一些准则,从完整的混合A*路径中选取某些点作为关键点,如图3所示,红色点为当前状态选取的关键点。将这些点作为局部规划的目标点,这样可以有效降低因为控制问题无法到达目标点从而出现原地转圈的现象,同时提高系统规划效率,满足在复杂矿山作业平台协作场景的应用需要。

图3 混合A*关键点

1.3 面向矿卡挖机精准定向需求的DWA局部规划

由于挖机位置和朝向随挖机工作状态动态变化,矿卡在工作区进入装料点时需进行动态局部轨迹规划,为满足挖机对矿卡的装载作业要求,矿卡和挖机不仅需要到达矿机作业的装载点,同时还需要保证矿卡和挖机的精准定向。所谓精准定向,指的是矿卡停在挖机铲斗的正前方,矿卡的航向和挖机臂展方向的朝向角误差控制在一定范围内,这样才能满足矿卡装料需求。面向矿卡挖机精准定向需求的DWA局部规划在轨迹规划关键点的指导下,实现无人驾驶矿卡的局部轨迹规划和精准定向。

2 关键技术

2.1 基于混合A*算法的轨迹规划关键点选取技术

混合A*考虑了车辆动力学模型,使得规划出的路径符合车辆的动力学约束,且输出的路径为一系列带有坐标和朝向信息的离散点,可用于DWA等局部路径规划。因此,本方案采用混合A*算法进行全局规划。考虑到混合A*算法应用的广泛性,在此不对其进行详细介绍,重点阐述轨迹规划关键点选取技术。

在关键点的选取过程中,需将起点和终点作为关键点,剩下的关键点则应出现在车辆状态发生较大变化的位置,例如由直线行驶突然切换为转弯行驶,以防止车辆因为突然的转向而无法及时调整,所以在遍历所有A*的路径点时会比较其与上一个关键点车辆朝向的变化幅度,如果大于设置的阈值就将其作为关键点;同时,由于混合A*的路径中经常出现“人”字形拐弯,这意味着车辆必须由切换档位改变行驶方向,而在这样的位置往往是车辆调整位姿的重要时刻,所以本研究会在遍历A*路径点的同时检测每个路径点速度方向与上一时刻是否发生变化,如若变化,则加入关键点序列,如图4所示,红色箭头处表示“人”字形倒车转折点,因此,可选为本方案的关键点[3]。

图4 普通关键点与“人”字形关键点

此外,为了保证矿车在最后的倒车阶段能更好地与目标朝向保持一致,混合A*规划出的轨迹终点是实际目标终点前移15 m后的结果,即车辆在到达A*终点后还需倒退一段直线距离才会到达真正的终点,所以在关键点选取时需计算出真正的终点位置并将其补入关键点序列[4]。

2.2 面向矿卡和挖机精准定向需求的DWA局部规划技术

基于上述关键点选取技术,局部规划模块可以得到轨迹规划的关键点,DWA算法(Dynamic window approach)的基本流程包括采样、预测路径生成、代价评估与最优路径选取4个部分。图5为DWA算法流程。

图5 DWA算法流程

基于矿车基本参数,系统可以对矿车前轮转角进行采样,然后基于匀速运动假设与车辆运动学模型对所有前轮转角进行预测轨迹生成。通过轨迹的代价评估,能够得出局部轨迹规划的最后路径作为局部轨迹规划的结果。在无人驾驶矿卡应用中,考虑矿卡挖机精准定向需求,设计合理的代价评估目标函数是本研究局部轨迹规划技术的难点。

在图6中,蓝线矿卡局部轨迹规划端点e的期望方向;o是后轮轴心。而oa、ob和oc是不同集合（δf,V f）的预测轨迹。因为朝向的代价较小,基于传统DWA的目标函数,DWA算法倾向于轨迹oa。如果没有障碍物,矿车将沿着ae停放。即使矿车可以到达终点,方向也会有很大的偏差。

图6 传统DWA的规划策略

2.2.1 代价函数的设计

考虑无人矿卡的精准定向需求,卡车选择ob是合理的,可以快速缩短到蓝线的距离。然后卡车可以反向到达期望方向。为满足精准定向需求,本研究在现有代价函数设计基础上定义了垂直距离代价和期望朝向代价。

2.2.1 .1 垂直距离代价

从预测轨迹的末端到预期线的垂直距离为d l(δf,v r),基于该代价,如果权重比其他代价大,卡车将有移动到预期线路的趋势。

归一化d l(δf,v r),垂直距离代价为:

2.2.1 .2 期望的方向代价

为了快速将车辆的方向调节到所需的方向,方向代价定义为:

式中,yaw0是目的地的期望方向;yaw(δf,v r)是预测轨迹中端点的偏航角。

将本方案的代价函数与传统DWA的代价函数相结合,最终的目标函数可以表示为:

式中,heading(δf,v r)表示朝向代价,即挖机臂展方向的期望朝向和矿车当前位置朝向的差值;dist obs(δf,v r)表示到障碍物处的误差权值;velocity(δf,v r)表示速度代价;w1、w2、w3、w4是各个代价函数的权重值,可用模糊控制逻辑来计算各个代价函数的权重值。

2.2.2 基于模糊逻辑的代价权值

尽管所提出的方法可以确保矿车以准确的方向到达终点,但权重w1通常应该比其他权重大一个数量级。如果卡车在预期的方向线上,偏航角在相反方向,则车辆将逆向行驶。因此,实时调整代价函数权重使车辆能够适应不同的工作情况非常重要。基于此,本研究提出了基于模糊逻辑的代价权值,提升代价函数在不同工况下的性能。

2.2.2 .1 隶属函数

权重w1受车辆位置和偏航角的影响,如当车辆远离方向线时,w1应该较大,而偏航角和预期方向的绝对值(表示为Δθ)较大,w1变小以调整方向。同时,如果车辆靠近障碍物,则应将w1修改得更小,以便车辆更安全地避开障碍物。图7为本方案设计的隶属函数。

图7 隶属函数

因此,状态(d l,Δθ,dist obs)是确定w1的输入变量。每个输入变量都与2个模糊集M和N相关。

模糊集M和N的度可以分别用μM和μN表示。μM按式(3)计算,μN=1-μM。在μM和μN中应用四次方,结果对输入敏感。例如,如果Δθ大于90°,则相关输出将迅速变小以调节车辆的方向[5]。

式中,k i是关键参数,取值范围[k1,k2,k3],由车辆状态(d l,Δθ,dist obs)决定。每个输入产生2个结果μM和μN。

模糊隶属度值的产生式μRi使用AND逻辑运算符计算。

2.2.2 .2 模糊逻辑

鉴于除w1之外的其他权重都处于相同的大小,将表示为模糊逻辑结构的输出u。模糊规则见表1,由于系统的对称性,表1中规则4,5,6的值是规则3,2,1的值的倒数,当Δθ很大而dist obs很小时,w1应该是比其他权重最低的阶数,而不管d l的值如何,这有助于规则1和6,如果d l或dist obs较大,则相对λi应该较大,以确保车辆尽快到达所需路线。因此,相对排列为:n1＜n2＜n3＜1。

表1 模糊规则

最重要的因素是Δθ,它决定了车辆的方向,而w4只是基于避障方法使车辆远离障碍物。表1中的每个规则都对最终输出u有贡献,使用每个规则的单个输出的权重平均值。

最终输出u的计算公式见式(5):

3 算法仿真验证

在本研究中,基于混合A*算法的轨迹规划关键点选取和面向矿卡挖机精准定向需求的DWA局部规划组成的双层轨迹规划框架为工作平台协作场景提供了一整套解决方案。DWA局部规划算法对于无人驾驶矿卡的功能实现和精准定向具有重要意义,故在此进行算法验证。

如图8所示,传统DWA算法在方向调整上性能很差,而本方案提议的方法使无人驾驶矿卡能够很好地调节其方向,完成车辆的轨迹规划和精准定向。

图8(a)中传统DWA挖机设定目标朝向为湖蓝色箭头方向,矿车起始方向为红色箭头方向,矿车起始方向和挖机设定朝向垂直,实际矿车达到目的地后为绿色箭头方向,绿色箭头方向和湖蓝色箭头方两者的朝向误差为Δθ=29.5667°,矿车在这个朝向下无法完成装料任务;图8(b)保持起始方向不变,基于模糊自适应的DWA仿真结果表明,矿车最终朝向和挖机设定目标朝向误差较小,Δθ=3.0092°,可以满足挖机装料要求;为了防止矿车起始朝向对最终停车时的朝向有影响,将矿车起始朝向设置成和挖机目标朝向相对方向(见图8(c)),即呈180°,在这种极端苛刻的朝向下泊车,矿车依然能精准地停在挖机正下方,朝向误差控制在合理范围内,Δθ=3.0982°。此仿真说明了模糊自适应DWA算法的有效性,满足矿车泊车装矿需求[6]。

图8 不同场景下传统DWA算法与模糊自适应DWA算法的比较

根据仿真结果,保持相同初始条件,将传统DWA和模糊自适应DWA做实车测试对比,首先设置挖机的目标朝向和记录矿车的起始朝向,然后记录矿车泊车到终点时的朝向。

传统DWA测试数据见表2。

表2 传统DWA测试结果

模糊自适应DWA测试数据见表3。

表3 模糊自适应DWA测试结果

通过表2和表3的对比表明,模糊自适应DWA能精准定向,满足自动驾驶装矿需求。

4 结论

本文结合矿山实际场景,分析了基于混合A*算法的全局路径关键点选取和面向矿卡挖机精准定向需求的模糊自适应DWA局部路径规划技术,实现了无人驾驶矿卡的局部轨迹规划和精准定向。并通过仿真和实车测试验证了该算法的有效性,提高了无人矿车装料的成功率,具有一定的推广和应用价值。