矿用卡车无人驾驶车载决策系统

王 佳，黄佳德，刘智聪，黄 鹏，吕 亮

（株洲中车时代电气股份有限公司，湖南 株洲 412001）

0 引言

采矿业是我国国民经济的基础和支柱产业之一，其工作环境复杂、危险，且传统采矿业存在生产效率低、成本高等诸多问题［1-2］，因此矿山智慧化是当前采矿业发展的重要趋势之一。2020年，国家发展和改革委员会、国家能源局及应急管理部等8部门联合发布了《关于加快煤矿智能化发展的指导意见》，提出到2025年露天煤矿实现智能连续作业和无人化运输的目标。

车载决策系统是矿山无人运输生产作业中车载端的一个重要环节，国内外学者在城市交通自动驾驶技术基础上结合矿山环境对矿山无人驾驶技术进行了大量研究。文献［2-3］提到了可以将相关技术运用到矿山、港口场景并实现尽快落地。文献［4］提出了基于道路信息的速度优化策略，对混合动力采矿卡车的运输时间和燃料消耗进行最优决策。文献［5］提出了一种高效、节能的从装载点到卸载点的轨迹规划算法框架。文献［6］采用安全的规划方法，从感知、决策和控制故障安全导向方面对决策规划进行探讨。这些文献主要针对矿山场景，研究如何利用中央式决策架构并结合地面系统全局调度规划能力，提高运输系统效率；但这牺牲了单车的灵活性，在特定场景（如装卸载、泊车）下会降低系统运输效率。

本文在全局规划调度的基础上重点研究车载决策系统单车自主决策方法，采用基于双层有限状态机技术实现单车自主决策逻辑判断，保证车辆行驶安全；采用基于时间最短、能耗最优的二次规划算法求解速度规划，提高运输效率；采用混合A*与曲线拟合算法求解局部规划，提高单车灵活性。文章明确了车载决策系统与地面系统规划调度功能边界，以及在特定场景与约束下车载决策系统所应具备的自主决策与规划能力。车载决策系统可以根据地面系统全局的任务调度信息以及自身状态决定何时开始任务调度及何时结束任务调度，并根据地面系统的全局规划以及自身运行状态，在特定场景下进行局部路径规划，进一步提升调度和规划基础上的单车运行效率，从而提升整体运行效率。

1 车载决策系统功能组成

车载决策系统属于车载系统的核心处理环节，是连接地面系统与车载感知定位系统、车载控制系统信息交互与处理的纽带。其综合感知定位系统的周边环境信息、地面系统的全局运营信息以及自车底盘、故障等信息进行推理决策，指导运动控制系统对车辆进行控制，使无人车产生安全、合理的驾驶行为，最终实现车辆的"装、运、卸"全流程无人运输。车载决策系统架构功能如图1所示。

图1 车载决策系统架构示意Fig.1 Architecture of onboard decision system

车载决策系统核心功能包含行为决策与运动规划。车载决策系统通过通信适配中间件与外部远程接管系统、LED显示屏、显示器、安保系统、控制系统、感知定位设备及地面系统进行数据交互，实现对外界信息最大限度的获取，然后在数据池完成数据的统一筛选与管理，最终将经数据池处理的数据作为输入传输给行为决策与运动规划两大核心功能系统，同时车载决策系统将生成的轨迹序列与行为动作信息输出给控制系统以指导控车。

远程接管系统与决策系统通过以太网连接。在出现无人驾驶系统无法处理的场景时，安全员可以通过驾驶舱完成远程控车；安装在驾驶室外的LED屏通过以太网与决策系统连接，用于显示车辆此时的驾驶模式，提醒周边行人车辆注意安全；安装在驾驶室内的车载显示器通过以太网与决策系统连接，用于安全员在车上时的必要车载信息提示；控制系统通过车载内部以太网与决策系统连接，以提供车辆线控底盘信息。感知定位系统通过车载内部以太网与决策系统连接，以提供车辆定位信息以及周边环节信息。地面系统通过4G/5G以太网与车载决策系统连接，以提供全局规划信息。

2 行为决策

行为决策的目标主要是保证车辆可以产生安全的驾驶行为，遵守矿区生产运行规则，满足车辆安全性能要求。矿卡决策系统的行为决策主要包含驾驶与任务模式决策、碰撞预警与防护决策、故障分级与安全导向决策。

2.1 驾驶模式与任务模式决策

车载决策系统采用双有限状态机进行不同驾驶模式与任务模式切换的决策，其中驾驶模式构成第一层状态机，任务模式构成了第二层状态机。

车载决策系统有无人驾驶、人工驾驶和远程接管3种驾驶模式［7］。无人驾驶模式是指当前车辆执行机构完全听从无人驾驶系统控制；人工驾驶模式指当前车辆执行机构完全听从安全员控制；远程接管模式指当前车辆执行机构完全听从远程驾驶舱的控制［4］。无人驾驶模式和人工驾驶模式可以相互切换；人工驾驶模式和远程接管模式可以相互切换；无人驾驶模式和远程遥控模式可以相互切换。状态转移图如图2所示。

图2 车载驾驶模式状态转移Fig.2 State transfer of on-board driving modes

进入无人驾驶模式之后，车辆接收来自地面系统的全局调度任务以及自身的任务决策，完成“装、运、卸”任务流程，其状态转移如图3所示。

图3 车载任务模式状态转移Fig.3 State transfer of on-board task modes

2.2 碰撞预警与防护决策

在循迹过程中，车辆基于当前车辆运行轨迹与实时的周围障碍物信息判断是否存在碰撞风险并基于车速判断车辆何时何地会发生碰撞风险；在确定以上信息后，决策车辆能否跨障、绕障或者原路径安全停车。规划的路径点可以近似看成一个有角度的矩形，同时障碍物也是有角度的矩形。图4为车辆轨迹与障碍物示意，其中白色矩形表示车，红色矩形表示障碍物。

图4 车辆轨迹与障碍物示意Fig.4 Schematic diagram of vehicle track and obstacle

采用有向包围盒（oriented bounding box，OBB）算法判断车辆是否会与障碍物碰撞。根据几何原理：两个矩形，当存在一条直线，使这两个矩形在直线的投影不相交，则这两个矩形不相交。这条线被称为分割线。若存在分割线，则一定有一条分割线和矩形中的一条边平行。分割原理如图5所示。

图5 分割原理Fig.5 Segmentation principle

将矿卡和障碍物近似成2个矩形。其中，Lo为障碍物长度1/2，Wo为障碍物宽度的1/2，Lv为矿卡长度1/2，Wv为矿卡宽度的1/2，T为两矩形的中心连线。建立xv、yv、xo和yo4条分割轴，然后将Lo、Lv、Wo、Wv和T分别向分割轴做投影：

式中：P(Wo)xv——Wo向xv轴所做投影的长度；P(Lo)xv——Lo向xv轴所做投影的长度；P(Wv)xv——Wv向xv轴所做投影的长度；P(Lv)xv——Lv向xv轴所做投影的长度。

如果在4个分割轴投影都满足式（2）~式（5）所示条件，则认为矿卡不会与障碍物发生碰撞；否则，认为会发生碰撞。

式中：P(T)xv——T向xv轴做投影的长度。

已知车辆有n个规划的轨迹点，每个轨迹点表示为(xi，yi，pi)，其中xi和yi为轨迹点位置，pi为坡道角的正切（上坡为正值），i=1，2，…，n；有m个障碍物(oj，yj)，其中j=1，2，…，m。

分别循环遍历所有轨迹点对应的车辆包围盒与所有障碍物对应的包围盒进行碰撞检测，得到离轨迹起点(x0，y0，p0)最近、发生碰撞的车辆轨迹点(xs，ys，ps)。假设(x0，y0，p0)到(xs，ys，ps)的距离为ds，则当前车辆位置与障碍物位置的安全距离为ds。已知车辆的初速度为v1，即为在第1个路点时的速度。

假设矿卡不会与障碍物发生碰撞，且车速度未超过防护速度，则进行绕障决策；若车辆速度已经超过防护速度，则应该立即安全停车。

将整个制动过程设定为电涡流制动和液压制动2个阶段。当车辆行驶速度不低于阈值vT时，采用电涡流制动；当车辆行驶速度低于阈值vT时，采用液压制动。同时，假设电涡流制动阶段，制动力矩恒定；液压制动阶段，制动力矩恒定。

设电涡流制动减速度为aC，液压制动减速度为aH，则对于第k个路点，其实际制动减速度ak为

式中：ik——第i个路点坡度角正切值；g——重力加速度。

对电涡流制动段，有

对液压制动段，有

式中：Δdk——第k个路点和第（k+1）个路点的距离。

于是，将电涡流制动段的速度-加速度-距离公式相加，可得

将液压制动段的速度-加速度-距离公式相加，可得

式中：第n个路点为电涡流制动和液压制动的分界点，vn≅vT。

将aH设置为平路半载工况下采用液压制动以50%制动强度制动时的减速度。于是，根据式（10），可计算出分界点的路点序号n；再根据式（9），计算出电涡流制动段的制动减速度aC；最后根据式（6），计算各路点k的防护速度vk。

2.3 故障分级与安全导向决策

车载决策系统接收感知系统、控制系统以及自身系统的故障诊断信息，基于驾驶场景统一进行分级故障安全导向，以保证无人驾驶矿卡安全、可靠的运行。根据安全导向，故障分为4个等级，即轻微故障、中等故障、较严重故障和严重故障。

对于轻微故障，车载决策系统向地面系统上报警告故障，采用文本提示、语音警告方式，提醒调度操作员，且车载系统不采取其他安全动作。对于中等故障，车辆以固定限速完成本次驾驶任务。对于较严重故障，当车辆高速行驶时，施加适当电制动力；当车速低速运行时，采用液压制动方式平稳停车。对于严重故障，车载系统施加电制动力与液压制动力并控制车辆根据轨迹停车；停车后，车载系统处于紧急锁定状态，除非地面系统下发紧急制动缓解命令。

3 局部运动规划

车辆运动规划是指生成衔接车辆起点与终点的几何路径，同时给出车辆沿该路径运动的速度信息，并使车辆在整个运动过程中满足运动学/动力学约束、碰撞躲避约束以及其他源自内部系统或外部环境的时间或空间约束条件要求。矿卡无人驾驶决策系统将运动规划多维优化问题进行解耦，拆分为二维平面的路径规划与在所选的轨迹上进行速度规划这两个子问题。

3.1 速度规划算法

矿山车辆无人驾驶技术更注重安全性、高效性和节能性，舒适性反而是次之考虑的问题。速度规划的目标是生成一条从起点到终点运行时间最短、能量最少的序列集合，以指导控制系统控制车辆速度。规划过程中，控制系统跟踪的目标速度源自于地面系统的限速要求。为了达到时间最短、能量最少的目标，车辆在满足基本运动学要求、最大加速度、最大减速度及速度上限约束的基础上尽量贴近目标速度来规划速度。速度规划算法具体如下：

（1）构建待优化问题模型的标准模型

（2）根据时间最短、能量最少原则，代价函数为

式中：vi——车辆在路点（xi，yi）的速度；ai——加速度。

（3）已知初始速度vo、终点速度vf、两点间距离Δs、最大速度vmax、最大加速度amax和最大减速度amin，轨迹总共有n个路点，根据车辆运动速度、加速度和距离关系，推导n行n列H矩阵、1行n列f矩阵、n行n列A矩阵、n行1列b矩阵、1行n列Aeq和beq矩阵，见式（13）~式（18）。

根据amax与amin，得到n行1列lb和ub矩阵：

采用C++OSQP库求解带约束的二次规划，得到最优的加速度曲线；然后反算出最优速度曲线；最后，根据实际车速对规划的速度曲线进行实时滚动优化，获取每一时刻的最佳速度规划。

3.2 轨迹规划算法

本文在地面系统多车调度全局轨迹规划的基础上完成车载端局部规划。矿山生产作业环境变化频繁，使得全局规划终点变得不可靠，导致泊车终点不可到达，因此重点考虑泊车规划。此时车辆决策系统需要根据环境信息，在原来终点附近自动寻找一个合适的终点进行泊车，规划出一条能到达终点的安全、合理的路径。

3.2.1 泊车终点决策

如果泊车终点不可达，需要重新选择一个泊车点。评价规划的泊车点是否合理，主要从以下两方面考虑：

（1）泊车点角度与环境土挡尽量垂直，使车辆靠边；

（2）泊车点尽量靠近原始终点，但又不会使车辆发生碰撞。

满足以上两个条件，则可认为车辆选择了一个合理的泊车点。

构建泊车点附近的2维平面坐标系，见图6。其中，紫色的栅格为障碍物；红色的箭头线为地面轨迹L，其终点为C。作射线B0，其过C且为L的切线。B0上有两点Aa和Ab，且，其中lab为车后轮两外侧轮胎宽度；射线B0在地图坐标系下与x轴的夹角为δ。

图6 泊车点二维坐标系Fig.6 Parking location in planar coordinate system

已知存在直线序列{Xa，Xa+1，…，Xi…，Xb-1，Xb}，这些直线都与x轴垂直，线段AaAb与直线序列的交点分别 为pa，g，p(a+1)，g，…，px，g，…，X(b-1)，g，Xb，g；直线Xb与射线B0和挡墙边界的交点分别为pb，g和pb，s，两点间距离为lb，0，如图7所示。

图7 直线序列与射线B0示意Fig.7 Schematic diagram of line sequence and ray B0

记li，0表示直线Xi与射线B0的交点和Xi与栅格地图土挡边界的交点之间的距离，则

式中：lB0——射线为B0方向的车后轮两外侧轮胎连线的各离散点到挡墙边沿的最大距离。

射线B0以C为圆心进行顺时针与逆时针旋转，每次旋转角度为α；逆时针旋转到的最大位置Bn，顺时针旋转最大位置B-n，对应的旋转角度为最大旋转角，记为β，如图8所示。

图8 旋转示意Fig.8 Rotation diagram

已知射线Bj，令

则应考虑如下两个因素：

（1）最优泊车角度计算。泊车时车辆尽量贴近道路内侧，需要寻找满足最小lBj的j值，具体要求为

当满足-n≤j≤n时，得到求得最优泊车角度yaw对应射线Bj与x轴夹角θ。

此时根据几何关系，车辆泊车角度应该与射线Bj垂直，得到最优泊车角度yaw为

（2）泊车点位置。泊车点应尽量靠近土挡但车辆又不会与障碍物碰撞。在原来泊车终点C附近考虑坡度因素。首先考虑在穿过点C的射线上寻找最优泊车点。根据公式（22）求得最优泊车角度对应射线Bj和原始泊车点C，作射线M与射线Bj垂直且射线M经过C点，射线M与土挡边沿交点为E，图中E与C点恰好重合，如图9所示。

图9 最优泊车点G的选取Fig.9 Optimal stopping point selection diagram

可以在线段GE中找到点S坐标，使得S坐标既不与障碍物碰撞，又接近全局终点C。至此，得到了最优泊车点。

3.2.2 局部路径规划

车辆路径规划是指生成衔接车辆起点与终点的几何路径，并使车辆在整个运动过程中满足运动学/动力学约束、碰撞躲避约束以及其他源自内部系统或外部环境的时间和/或空间约束条件。面向自动驾驶车辆的路径规划方法主要包括曲线插值方法、采样搜索方法以及最优控制方法［8-14］。考虑到实际工程中赋予车载决策系统的运算资源有限，因此首先采用曲线拟合和采样搜索的方法生成次优的初始路径，然后经过优化生成一条舒适、合理、无碰撞的最优轨迹。

矿山场景泊车倒车一般距离在20~50 m范围内，因此首先尝试高效率的曲线拟合；如果生成的路径不满足约束，则再尝试采样搜索。矿卡路径生成、信息逻辑处理过程如图10所示。

图10 路径生成处理过程Fig.10 Path generation process

曲线拟合算法采用三次样条插值方法实现。根据规划起点(xs，ys，φs)和终点(xg，yg，φg)，首先求解三次样条曲线函数各系数，然后根据采样距离，求得各个轨迹点(xi，yi，φi)位置和角度信息［8］。函数公式如下：

混合A*算法作为车辆自动驾驶常用启发式搜索算法，其包含了一种考虑车辆运动能力的更精准模型，适合在起点、终点相距不远的局部规划任务中提供近似最优或近似可行的初始解［15-17］。

由于用曲线拟合和混合A*算法产生的路径不平滑，不利于后期控制系统控车。为了达到更高的舒适度与控制效果，本文采用梯度下降平滑器来平滑轨迹。给定一个定点序列xi=(xi，yi)，i=1，2，…，N；定义Δxi=Δxi-Δxi-1表示顶点间位移，Δφi表示顶点切向角变化，oi表示距离xi点最近的障碍物距离；优化目标以最小化变量P为目标函数，表示优化后的路径平滑、曲率不超限、不与障碍物发生碰撞。P主要由Pcur、Psmo和Pobs组成，其中Pcur用于限制轨迹曲率，Psmo用于保证路径平滑，Pobs用于惩罚与障碍物的碰撞。

式中：σcur——距离障碍物的惩罚函数；δobs——曲率的惩罚函数；kmax——最大曲率；ωcur、ωsmo和ω0——权重系数［18］。

优化目标函数如下：

文献［19］给出了Pcur、Psmo和Pobs的求导过程，通过梯度下降方法实现路径平滑。优化算法过程为

输入：原始轨迹点集合P；

输出：优化轨迹点集合P，；

1.iterations=0；

2.cor=（0，0）

3.while iterations<200：

4. forxi∈P：

5. if（IsCusp（xi）==True）continue；

6. cor-=ObstacleGC（xi）；

7. cor-=SmoothnessGC（xi）；

8. cor-=CurvatureGC（xi）；

9.xi+=cor；

10. i+=1；

11. iterations+=1；

12.P，=P

其中：IsCusp（xi）表示xi为前进转后退轨迹段附件的尖点或者无需优化的固定点；ObstacleGC（xi）表示在xi处Pobs的梯度；SmoothnessGC（xi）表示在xi处Psmo的梯度；CurvatureGC（xi）表示在xi处Pcur的梯度。

4 仿真效果与分析

为验证本文所提方法的正确性和有效性，本文对速度规划和局部路径规划进行仿真分析。

4.1 速度规划分析

已知矿用自卸车车辆型号为XDE240，满载220 t，初始速度0 m/s，终点速度0 m/s，终点速度0 m/s，运行距离2 500 m；设置速度规划参数，最大速度vmax=5.55 m/s，最大加速度amax=0.8 m/s2，最大减速度amin=-0.4 m/s2。在北方某露天矿从生产作业从起点（装载点）到终点（卸载点）运行，已知全局路径规划如图11所示。

图11 全局路径Fig.11 Global path

采用3.1节所述速度规划算法，全局路径下的实时滚动速度规划效果如图12所示。可以看出，车辆在规划速度满足约束的前提下尽量贴近限速曲线行驶，实现了使运行时间最短的目标。

图12 速度规划结果Fig.12 Speed plan results

4.2 局部路径规划分析

XDE240型矿用自卸车车身长14.22 m、宽7.41 m，车辆轮胎半径R=1.8 m，最大线路曲率kmax=0.08 m-1。根据泊车点二维坐标系，采用3.2.1节所述泊车点决策算法得到终点坐标goal(23.0，54.2，46°)；已知全局规划进入泊车区域的起点坐标start(60.0，60.2，0°)，采用3.2.2节所述的路径规划算法生成轨迹序列，如图13所示。可以看出，车载决策系统基于全局规划调度与周边的环境信息，自主选择了合适的终点，成功规划了一条满足运动学约束的停靠挡墙路径。

图13 路径规划结果Fig.13 Path plan results

5 结语

本文针对矿卡无人驾驶车载决策系统提出一种基于双层有限状态机的行为决策方案，通过驾驶模式与任务模式决策、碰撞预警与防护决策、故障分级与安全导向决策，保证车辆在满足车辆安全性能的前提下产生安全的驾驶行为并遵守矿区生产运行原则；基于地面系统全局路径、全局调度信息，感知定位模块环境信息，实现车载的速度规划与局部路径规划，完成车辆的“装、运、卸”全流程运输，在装、卸载泊车场景可以提升10%~20%的运行效率。该系统在中国北方某矿区历经了半年实车生产运营，结果表明，该系统可有效提高车辆运行效率。后续，在此决策系统的基础上，将结合车载高精度地图与车对外界的信息交换（vehicle-toeverything，V2X）技术，开展在更多场景下自主决策、自主规划等更深层次的研究。