矿用无人运输车辆精准停靠控制算法

张翔宇，徐国艳，夏 启，李 涵，徐全耀，蔡明祥

（1.北京航空航天大学 特种车辆无人运输技术工业和信息化部重点实验室，北京 100191；2.北京航空航天大学交通科学与工程学院，北京 100191；3.国能新疆准东能源有限责任公司，新疆 昌吉 831100）

自动驾驶技术逐渐成为智能交通系统的重要组成部分[1]，矿区无人运输作为自动驾驶落地典型，受到越来越多的关注。矿用无人运输车辆在工作面和卸矿平台装卸车时需要实现精准停靠以提高工作效率[2]，尤其要防止溜坡或二次停靠等现象的出现。

已有一些研究尝试提高停靠精度，从提高速度跟踪控制精度入手，多采用模型预测控制或改进PID 控制。朱敏等[3]提出了1 种模型预测速度跟踪算法，能够克服行驶阻力大等模型误差和不确定干扰；RAFFIN 等[4]给出了1 种基于模型参考自适应控制的纵向控制器，但需要考虑完整的轮胎性能、制动系数等因素进行速度跟踪；TOULOTTE 等[5]针对无人驾驶车辆跟踪控制过程模型的不稳定问题，提出了基于TS 的模糊逻辑模型建立的纵向跟踪控制器，具有较好的控制效果和实时性；裴晓飞等[6]设计了1 种基于车辆纵向动力学方程的前馈加反馈车速控制器，通过测试验证得到有关节气门开度-车速-减速度以及制动压力-车速-减速度的三维经验图对实际控制量进行补偿，减少了工况改变所引起的车速控制偏差；WANG 等[7]基于非参数车辆模型设计出的前馈加反馈控制器，考虑车辆实际运行中的复杂工况，设计了1 种比例内模控制器补偿路况变化和车辆本身变化所带来的干扰；李贻斌等[8]使用基于模糊逻辑和遗传算法，结合油门/刹车切换逻辑设计出纵向运动控制器。

根据矿区运输工况和运输车辆的工作特性，精准停靠算法需要建立全工况环境参数辨识模型，考虑路面不平度和坡度产生的扰动影响和制动系统本身存在的延时影响。为此，考虑外部扰动和底层控制逻辑切换策略，提出1 种矿用运输车辆精准停靠的前馈±反馈控制算法。

1 环境参数辨识

1.1 坡度估计

采用车辆动力学坡度测量法和GPS 识别法融合识别的方式，并参考文献［9］中的交互多模型卡尔曼滤波算法，通过多模型混合输入的联合估计来得到鲁棒性好、准确度高的坡度估计结果。基于车辆纵向动力学模型的道路坡度量测计算式推得车轮旋转动力学方程为：

式中：Jwj为绕轮心的转动惯量，kg·m2；ωj为旋转角速度，rad/s；Tdj为驱动力矩，N·m；Tbj为制动力矩，N·m；Fxj为轮胎与地面之间的纵向力，N；f 为轮胎滚动阻力系数；Fzj为垂向力，N；Rtj为车轮滚动半径，m。

考虑车辆因纵、侧向加速度引起的轮荷转移后，各车轮的垂向力表达式为：

式中：Fz1为前轮垂向力，N；Fz3为后轮垂向力，N；hg为车辆质心高度，m；L 为前后轴距，m；a 为车辆前轴距质心的距离，m；b 为车辆后轴距质心的距离，m；θ 为坡度角度值，（°）；m 为车辆质量，kg；ax为车辆的加速度，m/s2。

根据离散状态方程式，建立基于纵向动力学的定坡度模型，得到观测器的状态方程为：

式中：vx为速度，m/s，i为坡度；δ 为模拟驾驶员反应强度系数，δ＝0.9；Δt 为算法采样间隔时间，s；Fx为垂向力，N；Fw为垂向阻尼力，N；wk1为基于纵向动力学的过程噪声。

露天矿区环境空阔，无高大遮挡物，GPS 信号强，道路坡度一般不大于10%。通过提取导航坐标下纵向和垂向运动参数来推导坡度值，计算式为：

式中：Δs＝vxΔt；Δk＝hk-hk-1；hk为k 时刻的高程。

根据离散状态方程式，建立基于GPS 的坡度模型，得到观测器的状态方程为：

式中：wk2为基于GPS 的过程噪声。

1.2 路面不平度估计

采用基于悬架响应的路面不平度识别方法，根据无人运输车辆运过程中产生的动态响应间接获取路面高程。建立半车4 自由度动力学微分方程为：

式中：mf、mr分别为前后轮质量，kg；mH为簧上质量，kg；Ff、Fr分别为前后轴悬架与车身之间的动态作用力；Iy为绕y 轴的转动惯量，kg·m2；ktf、ktr分别为前后轮胎刚度，N/m；kf、kr分别为前后悬架刚度，N/m；cf、cr分别为前后悬架阻尼系数；xHf、xHr分别为前后悬架簧上质量的垂直位移，m；xf、xr分别为前后轮的垂直位移，m；xH为质心处的垂直位移，m；Lf、Lr分别为质心距前后轴的距离，m；h 为质心高度，m；qf、qr分别为前后轮的路面输入。

将上式转换成矩阵表达形式：

式中：M、C、K 分别为质量矩阵、阻尼矩阵和刚度矩阵；F、F＊分别为纵向加速度和前后轮路面激励对应的输入矩阵。

参考文献［10］中路面不平度的估计算法，结合传感器测量信息，将质心垂向加速度、垂向位移、角速度和角度作为观测向量，车身纵向加速度作为滤波器的已知输入，通过增广卡尔曼滤波迭代更新，与车辆纵向位移一起转换得到空间域路面不平度。

2 控制算法设计

精准停靠控制算法包括距离-速度规划层、停靠算法层以及运动控制层。其中，控制算法层是实现精准停靠控制的关键层，用于给出维持车辆期望运动状态、纠正车辆实时位置所需的执行器开度指令，并对实车平台反馈的状态信息进行滤波以降低系统噪声；运动控制层是实现车辆精准停靠的控制的基础层，主要用于对算法层指令进行合理的转化和分配，并作用与整车系统，保证车速控制响应速度，提升控制系统的准确性和鲁棒性。

2.1 距离及速度规划

停靠作业涉及前向停靠，如障碍物停车工况、任务停车工况与倒车停靠，如装卸载停车工况、泊车工况。车辆降速到达停靠区域后，获取停靠线的精确位置并摆正车辆位姿后即进入停靠状态，然后根据当下到停靠线的实际距离并结合环境信息做出距离规划。

停靠过程大致分为：接收停靠任务→减速至初始车速→到达控制点驱动退出→制动开度反馈控制→到达停靠点→驻车制动使能。由于矿区实际装、卸载区面积变化大，选取的直线停靠距离Sp为30 m≤Sp≤50 m，并将其分为匀速行驶路段和一级制动路段等7 段不同的作业线路。

当停靠作业处于匀速行驶路段时，如果车速过低，受路面不平度影响，可能导致车辆直接熄火，为此设置车辆匀速状态下的速度不小于3 km/h；当停靠作业处于制动投入路段时，若制动投入时机过早或过晚，会导致车辆停靠位置与目标停靠位置出现较大偏差，为此设定车辆到达0.7Sp的位置时投入制动；当停靠作业处于制动介入路段时，如果制动力投入过少，车辆可能会冲撞上挡墙，设计二级制动方案，到停靠线附近短时投入较大强度的制动，以确保停靠的精确度和可靠度。结合车辆停靠的线路情况和速度规划方案，最终规划出的车辆运行区间，车辆运行区间表见表1。

表1 车辆运行区间表

明确各个运行区间的调速位置和停靠速度后，根据加减速过程中加速度变化规律可以计算得到车辆的速度-时间信息，通过积分运算获取距离-时间信息，使得车辆在同一时刻的位置、速度信息显示在同一个坐标系下，得到速度-位置曲线。

2.2 控制器整体概述

设计的精准停靠算法支持车辆在空重载、不同路况条件下的停靠，通过初步拟合的参数和算法内部的自调节来满足多场景下的停靠要求。输入为目标停靠位置和当前实际位置、目标车速和实际车速、停靠工况等，停靠线距离规划模型根据预先设定好的停靠线距离规划方案输出车辆在当前距离下的目标车速，通过前馈开环控制和误差反馈修正联合控制的方法调节车速。

控制算法主要依赖于参数的设定与调整，需要针对目标车辆进行制动和开度和节气门开度的标定。在前馈开环控制中，基础控制项提供车辆在平路保持匀速行驶状态的基本控制量，坡度补偿项消除车辆上下坡时重力势能带来的影响，不平度补偿项减少车辆停靠过程中路面不平度导致速度跟随出现的干扰。

反馈控制为PID 控制，比例和微分控制项考虑速度误差和速度误差的变化量，积分项考虑带遗忘因子的速度误差累积量。公式表示为：

式中：utotal为执行器的开度，utotal∈［-1，1］；ubase为基础控制项；us为坡度补偿项；ur为不平度补偿项；uPD为比例微分项；ui为积分项。

2.3 前馈开环控制

利用实车试验数据总结出的“期望车速-坡度”二维标定表，拆分为基础补偿量、坡度补偿项及不平度补偿项。基础补偿项即坡度为0 时的控制参数，a、b、c 为多项式拟合系数，公式为：

以此确定基础控制项的参数，并确定坡度为0时的对应散点，经过多次整定后最终确定基础控制项公式为：

坡度补偿项和不平度补偿项均为比例控制，以坡度为0 的散点为初始点，确定坡度补偿项公式为：

不平度补偿项公式为：

2.4 误差反馈修正

以比例微分项以速度误差ev、速度误差变化量Δev作为输入，公式为：

式中：kP为速度误差比例项系数；kD为速度误差变化量微分系数。

利用实车试验数据总结出的“速度误差-速度误差变化率”二维标定表，将其从插值PD 控制调整为固定参数PD 控制。为使调整前后的算法输出量基本一致，需要基于标定结果进行拟合，首先以误差变化率为0.03 的散点为起始点，进行比例项系数kP的拟合。

基于上述kP的拟合结果，将速度误差变化率为0.03 拟合后所对应不同车速的点在二维表中确定，再根据原有二维表数据，进行微分项参数整定。

最终确定比例微分项公式为：

精准停靠算法中的积分项为带遗忘因子的积分控制，根据原有参数，暂定积分项参数为0.015，遗忘因子初步设置为0.95，确定积分项公式为：

式中：Iev，k-1为k-1 次速度误差累积量；ev，k为第k 次速度误差。

-1≤ui≤1，表示执行器开度百分比；当ui＝-1时，表示投入最大的制动踏板开度；当ui＝1 时，表示投入最大的节气门开度。

3 速度门限值与底层控制逻辑

3.1 速度门限值确定

由于车辆执行器存在较大延时，车辆在碎石道路上进行停靠时可能会导致急停或者速度骤降等问题，为避免信号波动引起节气门和制动器之间的频繁切换，控制算法优先保证车辆在各个调速区间内保持滑行状态或者缓慢降速行驶，并在调速位置设置切换裕度。当车辆到达各区间末端时，计算出实时速度ve与目标速度va之间的误差百分比Ea为：

若误差过大，则需要对执行器开度的大小进行调节。设置Ea＝0.3，调节策略如下：

当（1-Ea）va≤ve≤（Ea＋1）va时，正常进行下一阶段的调速；当ve≥（Ea＋1）va时，增大下一阶段的制动投入强度，调节因子kb预设为1.05；当ve≤（1-Ea）va时，增大下一阶段的节气门投入强度，调节因子kα预设为1.05。对于Ea的确定，需要经过测试做进一步调整。

3.2 底层控制逻辑

在矿用运输车辆中制动系统一般采用电-气冗余的机械制动或者再生制动与机械制动并存的复合制动方式。对于同时存在再生制动与机械制动的车辆，需要设计两者切换逻辑，充分利用电制动，并把握机械制动切入时机。

根据车辆所需的制动踏板开度，设计串联控制策略，对电机制动力和机械制动力进行调整。当车辆处于轻度制动模式下，仅进行再生制动；当车辆处于中度制动模式下，气压制动与电机制动共同作用，一旦制动强度达到设定的数值，先提高前轮的气压制动力，后轮气压制动力保持为0，待前后轮制动力分配符合理想的制动投入曲线时，前后轮制动力才会根据拟合曲线同步增加；当制动踏板开度超过0.7 时，车辆进行紧急制动，电机不再参与制动，此时需要通过切换逻辑规划出再生制动退出曲线与机械制动弥补曲线使得制动力分配依然符合制动投入曲线。

机械制动下节气门开度退出曲线为：

式中：yk为节气门开度；ak为各车型不同的配置参数，ak＞0，使不同车型在节气门退出过程中行驶的距离基本保持一致。kk为制动投入位置与实际位置误差ep之间的补偿系数，kk＝0.2×ep；bk为保持5 km/h 车速时的初始开度。

机械制动介入曲线为：

式中：yz为制动开度；λ 为停靠误差ed的反馈调节量，λ＝1-0.2ed；cz为机械制动投入变化率；dz为死区；yzmax为机械制动投入上限值。

最终的距离dis＝μ×dis，μ 为停靠距离系数，根据实际情况调整。若惯导的俯仰角变化量超过一定周期，μ 应增大；若坡度为正，μ 应减小，反之μ 应增加；考虑上次停靠误差，若停靠距离靠前，μ 应减小，反之μ 应增加。

等到车辆到达二级制动介入点后，立即投入驻车制动，再缓慢退出机械制动，利用驻车制动的快速响应特性保证车辆在缓坡上可靠停稳。

4 测试验证

测试验证分为仿真测试和实车测试，用于验证矿用运输车精准停靠算法的有效性和实用性，并为改进控制算法、调整拟合参数提供方向。

仿真测试使用PreScan 与Matlab 进行联合仿真，搭建通信传输模块，根据GPS、组合导航系统、执行控制机构等设备的通信方式进行仿真模拟，通过串口通信及CAN 通信等实现仿真平台与矿用无人驾驶车辆实车的车载计算单元的信息交互，形成闭环控制。电制动／机械制动开度曲线如图1。

图1 电制动/机械制动开度曲线

车辆采用精准停靠控制策略时，速度波动在可控范围内，在各个速度调整区间均能完成速度规划任务，并且在对应的调整节点均能较快响应减速请求。在完成减速后，能持续根据路面前馈信息进行调整，从而使车辆在路面不平度较高的工况下能保证速度的相对平稳，显著提高了一次停靠的成功率。

在停靠的前半段主要是依靠电制动进行减速，而在调整节点会利用机械制动进行短暂的快速降速，实现对不同制动方式的有效利用以达到良好的减速效果。由于速度及距离规划方案，误差明显变化均出现在速度调整节点附近。除此之外，误差普遍均在±0.2 的范围内变化，满足车辆停靠过程中的平稳性要求。

实车测试在排土场进行。依据矿区卸载作业操作规范，测试条件停靠限速为5 km/h。排土场长200 m、宽50 m，直线停靠距离大概50 m，符合规划距离要求。实车测试速度跟踪效果如图2，实车测试执行开度曲线如图3。

图2 实车测试速度跟踪效果

图3 实车测试执行开度曲线

空载工况下实际车速在速度调整节点对期望车速的相应更快，跟踪效果更理想。而在降速调整区间，空重载更多时候均在高于期望车速20%范围内进行波动，以达到更好的抗颠簸效果。在最后的调整区间内，速度在1 km/h 上下频繁波动，受道路不平度影响剧烈，驾驶模式在制动和牵引模式两者间来回切换，但控制算法依然能保持速度的相对平稳，在崎岖道路上行进至停靠终点。

制动开度跳动较大的节点与减速调整请求节点一致，符合精准停靠规划方案。在速度明显低于当前门限值时，油门开度能迅速响应，立马加速将速度纠正回期望速度附近，保证了停靠作业的连贯性，并有效避免了来回溜车蠕行和二次停靠现象的发生。

5 结语

针对矿用无人运输车辆的精准停靠问题，提出了1 种前馈加反馈控制并考虑外部扰动的矿用运输车辆精准停靠算法，包括前馈开环控制器和误差反馈控制器，两者分别根据速度信息和速度误差信息进行控制量的计算，实现对目标速度平稳、快速的跟随，从而达到精准停靠对实时位置精度的要求。通过上述测试验证表明，设计的控制算法对制动逻辑做出进一步优化，能够有效降低路面不平度对车速波动的干扰，较好的适应矿区停靠的作业要求，最终实现车辆的精准停靠。但由于卸载区路面坡度和不平度受到诸多因素影响，不同车辆的每1 次停靠都存在差异性，这对参数的实时调整和速度补偿提出了更高的要求，需要进行进一步优化。