杜广泽， 张旭东， 邹渊， 郑壮壮

(1.北京理工大学 机械与车辆学院， 北京 100081；2.北京电动车辆协同创新中心， 北京 100081)

0 引言

地面无人车辆作为无人平台的一种，无论是在民用领域还是在军用领域，都承担着越来越重要的功能与任务[1]。无人车辆系统避障是重要的一项功能要求，该功能涉及车辆环境感知技术、规划技术与控制技术。以感知结果为基础实现轨迹规划，进而进行动力学控制是无人化行驶的主要思路。一般来说，避障功能的主要目的是寻找一条符合车辆约束的、无碰撞的安全轨迹，并控制车辆安全通过障碍区域[2]。对于非结构化道路下的轮式无人车辆而言，由于地形因素的影响，车辆运动多面临侧翻的风险。因此，车辆避障过程中的轨迹除了考虑车辆本身约束之外，还需要考虑车辆运动状态的安全性，也应考虑不同路面坡度的动力学特性[3]。

针对无人车辆的避障需求，国内外学者已提出许多算法。这些算法大多期望无人车辆在包含障碍物的环境中按照某些指标运动，这些指标通常与车辆运动的通过性、快速性、稳定性、安全性相关，且大多数规划出来的轨迹都满足连续和平滑的要求[4]。这些轨迹规划算法大致分为图搜索类算法[5]、虚拟场类算法[6]、启发类算法[7]、数学优化类算法[8]4类。其中，图搜索类算法与虚拟场类算法运算速度快，但难以考虑车辆特性，启发类算法需要额外的轨迹平滑手段，而数学优化类算法可以考虑车辆特性，但需要较多的计算时间。

在数学优化类算法中，基于模型预测控制(MPC)[9]的优化类算法被广泛应用。现阶段研究已经证明了MPC在无人车辆轨迹规划中的可行性。文献[10]提出了一种基于MPC的分层式车辆避障算法，利用车辆运动学模型进行车辆上层轨迹规划，并利用动力学模型进行车辆轨迹跟踪，证明了规划轨迹的可行性；文献[11]将凸近似的避障原理与MPC进行结合，提出一种改进的无人驾驶车辆轨迹规划方法，并验证了其合理性；文献[12]针对特定的换道问题提出一种基于MPC的换道轨迹规划算法，该算法使用车辆动力学模型，验证了轨迹规划在车辆高速运动情况下的可行性与稳定性；文献[13]研究了高速无人车辆避障问题，使用MPC算法保证车辆安全性与路径跟踪的准确性；文献[14]使用MPC实现地面无人车辆的避障功能，证明了基于MPC避障算法的可行性；文献[15]根据模型预测控制原理，提出一种考虑车辆安全性约束的轨迹规划方法；文献[16]提出一种适用于高速地面无人车辆的避障算法，使用了一种非线性MPC算法；文献[17]针对乘用车辆实时避障问题，提出一种基于速度预测的模型预测控制器，将规划与避障功能融合在一起。

上述研究大多以车辆动力学为基础，针对某一特定工况进行避障研究，而在离线地图与参考轨迹未知情况下进行的车辆避障研究并不多见。

本文将非结构化道路避障问题描述为分段最优控制问题，针对地面无人车辆的动力学特性，以得到车辆控制量序列为目的，提出在非结构化道路下的避障算法。本文所提算法综合了避障所需的规划与控制两部分，优化求解完毕后，直接得到车辆的规划轨迹、参考纵向加速度与前轮转角。所提算法保证了车辆尽可能以目标速度接近目标点，确保车辆在安全性限制的条件下运动，提升了轨迹规划与避障算法的环境适应性。本文的创新点与价值在于：

1)提出一种不明确区分避障路径规划与避障路径跟踪的算法应用，无需进行地图重建，提高了避障功能实现的准确程度。非结构道路包含更多复杂形状的障碍物、树木等元素，障碍物尺寸形状都不易表达，受限于栅格形状以及激光雷达在远处分辨率低的特点，在地图重建过程中容易产生失真，影响无人车辆避障效果。本文基于MPC的避障方法，无需明确的路径规划部分，因此无需明确的地图重建过程，仅需要根据激光雷达点云数据提取扇形边界，这样就减少了由于地图重建造成的误差。相比之下，在解决无人车辆非结构化道路下的避障问题时，具有更少的步骤、更高的准确程度。

2)针对非结构化道路车辆行驶目的，结合无参考轨迹与离线地图的工况，提出了合理的评价指标与约束条件，保证车辆最大程度按照期望车速到达目标点，并保证车辆运动的安全性。考虑到没有参考轨迹与离线地图，设计了基于车辆航向、运行时间、车速以及距目标距离综合在内的评价指标，保证车辆运行的高效性。针对非结构化道路车辆行驶安全性特点，综合考虑车辆动力学约束与车辆载荷转移下的安全性约束，将车辆横纵向控制结合，达到无需后续设计控制率即可实现车辆纵向控制的目的。在安全性约束中，硬约束考虑了车辆车轮在实际运动中不可离地，在MPC的评价指标中亦通过约束车辆横纵向加速度与质心侧偏角保证车辆安全性。

3)采用变预测时域的方法来解决固定距离上的模型预测控制问题。受限于激光雷达探测范围，车辆模型预测范围最多到达激光雷达探测边界，而由于车辆车速在该问题中不是定值，因此预测时域亦作为模型的状态量参与优化。这更符合非结构道路下车辆避障的要求，也使得车辆在行进中更好地保持连续性。在求解中，使用伪谱法将分段非线性最优控制问题转换为包含优化变量的非线性问题，然后使用基于内点法的非线性求解器进行问题求解，从而达到变预测时域的目的。

1 基于MPC的避障算法设计

在MPC算法中，优化问题通常需要定义好预测时域与控制时域，确定优化问题的约束与评价函数，在实际使用中根据被控对象的反馈进行滚动优化与校正。

然而，在非结构化道路的避障问题中，在没有离线地图与参考轨迹的情况下，车辆运动仅依赖于动态局部地图确定的可通行区域，因此，车辆在避障轨迹终点处的状态无法确定。由于车辆横纵向运动的耦合性，无法明确定义预测时域与控制时域的长度。并且由于动态局部地图的实时性，车辆在不同位置具有不同数学表达形式、不同数量的约束条件。本文提出基于MPC的分段优化避障算法，通过构建分段最优控制问题，准确地描述车辆在避障过程不同阶段应满足的动力学特性与状态约束。因此，在综合考虑车辆横纵向动力学时，车辆避障问题可以转换为分段最优控制问题。本文按照车辆在局部地图中运动的不同区域，对问题进行划分。

根据模型预测控制原理与轨迹规划问题，设计基于MPC的轨迹避障算法，图1所示为基于MPC的避障算法框架。

图1 基于MPC的避障算法框架Fig.1 Framework of MPC-based obstacle avoidance algorithm

该框架分被控对象和避障算法两部分。其中被控对象指的是实际车辆和车载传感器；避障算法分为感知、规划、控制3层，感知层为算法的基础，包括状态估计与局部地图的创建，规划层包括了基于MPC的优化问题描述、优化问题分段、添加优化问题约束、优化问题的转换与求解4步，规划算法直接生成横向控制量，即前轮转角，并生成纵向参考加速度，纵向参考加速度由纵向控制算法进行跟踪，继而生成纵向控制量。本文主要研究避障算法中的规划层与控制层，实际应用中避障算法根据车辆实际控制状态进行优化求解，从而对车辆进行修正，达到在非结构化道路下避障的目的。

2 分段最优控制问题构建与求解

图2所示为分段最优控制问题原理，被控对象在不同的时间段内，有不同的评价指标、目标值与约束，分段最优控制问题的构建要针对具体问题，选定某种规则进行分段，红色圆圈标注区域为分段连接区域，Tp为控制时域。在车辆运动过程中，车辆运动的可通行区域范围随着车辆位置改变而改变，以车辆在局部地图中的位置作为分段因素，可以将避障问题构建为分段最优控制问题。

图2 分段最优控制问题描述Fig.2 Problem description of multi-phrases optimal control

按照车辆运动的位置，将轨迹规划问题构建为分段最优控制问题，由(1)式～(9)式描述。

(1)

(2)

(3)

(4)

S(i)[X(i)(t)]≤0,

(5)

R(i)[X(i)(t)]≤0,

(6)

X(i)(T(i))=X(i+1)(T(i)),

(7)

G[X(N)(T(N))]≥0,

(8)

T(0)=0 s,X(1)(T(0))=X(T(0)),T(N)≤Tmax，

(9)

2.1 代价函数

(1)式表示最优控制问题的评价指标，设定为代价函数J(参数示意图见图3)，如(10)式所示：

(10)

式中：w1、w2、w3、w4、w5为权重系数，w1对应的指标是在保证避障功能的前提下，期望规划终点的车辆位置更靠近目标点，w2对应的指标是期望规划终点的车辆航向尽可能朝向目标点，w3对应的指标是期望车辆以恒定的速度行驶，w4对应的指标是使得车辆尽可能贴近规划起点与目标点的连线，从而保持车辆不失控，w5对应的指标是期望车辆保持平稳行驶，减少发生危险的概率；s0、s1分别为规划起点和规划终点距离目标点的距离，

(11)

(12)

在局部地图坐标系OSXSYS下，x和y分别表示车辆横纵坐标，xg和yg分别表示目标点横纵坐标；γ0为目标点方位角；φ为车辆航向；u为车辆纵向速度；ut为车辆行驶的期望速度；xref和yref分别为参考点横纵轴方向坐标；ω为车辆横摆角速度。图3中v为车辆横向速度。

图3 代价函数参数示意图Fig.3 Cost function parameter diagram

2.2 车辆动力学模型

(2)式表示轮式无人地面车辆的动力学模型，如图4所示，车辆运动的动力学状态由(13)式描述：

(13)

图4 轮式无人车辆动力学模型Fig.4 Dynamic model of wheeled unmanned vehicle

式中：ax为车辆纵向加速度；Ffy前轴车轮侧向力；Fry为后轴车轮侧向力；m为整车整备质量；Lf为质心到前轴的水平距离；Lr为质心到后轴的水平距离；Iz为车辆z轴方向转动惯量；kf为前轴车轮侧偏刚度；kr为后轴车轮侧偏刚度；δ为转角；β为侧偏角；FRN为右侧车轮接地垂向力；FLN为左侧车轮接地垂向力；b为车辆轮距；θ为车辆侧倾角；h为质心位置距离地面高度。图4中OXVYVZV为车辆坐标系，O为车辆质心，RT为转弯半径，af、ar分别为前、后轴车轮侧偏角，FLT、FRT分别为左侧和右侧车轮接地切向力。选取状态量X=[x,y,φ,u,v,ω]T,选取控制量μ=[ax,δ]T。

2.3 状态量约束

对于本文针对的避障问题，只考虑车辆前进方向半圆周内的情形，记车辆最高纵向速度约束为umax，车辆最高横向速度约束为vmax，车辆最高横摆角速度为ωmax，则：

(14)

(15)

(14)式与(15)式表示了车辆状态量约束，无穷作为约束界限表示该状态量不需要进行约束。

2.4 控制量约束

(4)式表示车辆控制量约束，对于该避障问题，车辆纵向加速度由驱动系统与完成任务的需求共同决定，根据需求选定上下边界值，记车辆最大纵向加速度为axmax. 车辆前轮转角应在极限转角范围内，极限转角由车辆机械结构决定，记为δmax，最优控制问题控制量约束表示为

(16)

(17)

2.5 安全指标约束

(5)式表示车辆安全指标约束，对于非结构化道路，安全约束定义为车辆具有足够大的垂向力且车辆质心侧偏角在一定范围之内，即双侧车轮接地垂向力均大于接地垂向力阈值。记车轮接地垂向力阈值为FNmin，记车辆最大质心侧偏角为βmax，路面附着系数为η，重力加速度为g，车辆最大质心侧偏角按照(18)式[18]确定：

βmax=arctan (0.02ηg).

(18)

结合(13)式，选定：

(19)

2.6 车辆位置约束

(6)式表示车辆位置约束，该约束体现了车辆应通过的区域序列，在本算法中将所有的区域处理成为三角形。

经过在线局部地图的处理过程，得到如图5所示局部地图。在该局部地图中，地图坐标系与车辆坐标系在规划起点处重合，车辆初始位置，即规划起点，处于圆心处，车辆航向为横轴正方向。同时进行地形评估，即局部地图包含了地形起伏信息在内，该地形起伏信息在优化问题中体现在车辆侧倾角中。

图5 传感器感知结果Fig.5 Results of sensor signal processing

将车辆可通行区域按形状划分，扇形区域表示该区域的边界到达了传感器可探知的最大范围，即在扇形区域中无障碍物存在；三角形区域表示该区域的边界未到达传感器可探知的最大范围，即在区域尽头存在障碍物，如图5(b)所示。

在避障问题中，车辆应在满足无碰撞需求的情况下，尽可能接近目标点，即车辆的规划终点应在扇形区域中，由此可知，按照距离最近的原则，选取距离目标点最近的扇形区域作为规划终点所在的范围。

使用数学方法描述车辆运动区域的边界，如图5(c)所示，圆弧用割线来代替，其中蓝色界线为车辆运动区域的约束，在局部地图坐标系下用不等式描述边界。

对于优化问题的每一段来说，位置约束为3条直线所包围的区域，选取：

R(i)[X(i)(t)]≤0⟺
P(i)x(i)(t)+Q(i)y(i)(t)

(20)

式中：P、Q、R均为长度为3的向量。在图5所示的例子中N=2，即优化问题分为两段，优化问题的第1段对应3号区域，优化问题的第2段对应4号区域。

2.7 连续性约束

(7)式表示在两段之间状态量应是连续的，如图2所示的虚线圆圈部分，这是车辆本身动力学所决定的。此项约束表述为：对于分段问题的某一段，上一段最后时刻的状态量与这一段初始时刻的控制量在连续状态空间中应保持一致。

2.8 端点约束

(8)式和(9)式表示车辆在规划起点与规划终点处的约束， MPC每次求解时，局部地图坐标系与规划起点的车辆坐标系重合，即在初始时刻：

X(T(0))=[0,0,0,u(T(0)),v(T(0)),ω(T(0))]T.

(21)

当目标点在局部地图范围之外时，选定规划终点的坐标尽可能贴近局部地图边界，即

(22)

式中：RL为局部地图半径；ε为半径缩放因子，0<ε<1. 当目标点在局部地图范围之内时，选定规划终点的坐标在目标点附近，即

(23)

式中：λ为到达终点的距离阈值。

2.9 求解算法

对于(1)式～(9)式所描述的连续状态空间非线性分段优化问题，本文分两步进行求解。

首先针对连续状态空间的分段优化问题，使用hp自适应伪谱法[19]将连续状态空间的优化问题转换为含优化变量的非线性问题，这就将避障过程这一分段优化问题转换为了含有车辆状态量的非线性优化问题。该方法使用Legendre-Gauss-Radau配点方法，融合有限元法与全局伪谱法的优点，同时自适应调节单元长度与拉格朗日基函数的阶次，能快速收敛到给定精度。经过这一步的非线性优化问题具有如(24)式～(26)式所示的形式：

(24)

s.t.bL≤g(X)≤bU,

(25)

XL≤X≤XU,

(26)

式中：f(X)是代价方程；bU和bL分别为约束方程g(X)的上、下边界；XU和XL分别为变量向量X的上、下边界。

其次使用非线性问题求解器IPOPT[20]对非线性问题进行求解，IPOPT求解器使用内点法求解非线性问题的局部最优解。该方法将等式与不等式约束分解为一些列等式约束，然后用迭代的方法求解只含有等式约束的非线性问题。该方法可以使用精确的雅可比矩阵与海森矩阵，从而保证求解结果的准确性，在求解完毕后即可得到车辆避障问题的优化结果。

3 实车实验验证

3.1 车辆控制算法

通过实时优化得到车辆在控制时域内的状态量与控制量序列，将求解得到的加速度序列作为控制层的参考加速度序列，实际纵向控制量为车辆油门开度，采用PID控制器跟踪该参考控制量，实现轨迹跟踪。(27)式表示PID控制器的控制规律，将求解得到的前轮转角序列作为车辆横向控制的实际控制量：

(27)

式中：pad(t)为车辆油门踏板开度；Kp、Ki、Kd为比例、积分、微分环节的增益系数；

e(t)=amx(t)-ax(t)，

(28)

amx(t)为车辆实际纵向加速度。

3.2 实验平台搭建

实车平台如图6所示，被控车辆为北京理工大学轮式车辆平台，在该实验车辆的基础上，增加激光雷达、上位机、CAN协议转以太网协议(CAN-NET)设备，进行实验验证。

图6 实车验证平台架构Fig.6 Real vehicle verification platform architecture

其中CAN-NET设备将以太网通信协议转换为CAN通信协议，上述规划与控制算法在上位机中运行，算法结果通过以太网转CAN设备下发到整车CAN总线上，从而控制制动执行器与电机，实现车辆控制。除了激光雷达之外，车辆还装有视觉传感器、前向毫米波雷达传感器，可按照上述步骤准确地得到相应的局部地图与划分区域。实车实验参数如表1所示。

表1 实验相关参数Tab.1 Parameters related to experiment

3.3 实验验证

实际实验中，选取封闭园区内一段道路作为测试路段，该路段是典型的非结构化道路。该道路没有车道线，不含明显道路边界，道路周围障碍物种类复杂、数目较多，包含动态行人与车辆。在此测试环境下可充分测试算法的可行性。

测试结果的车辆实际运动轨迹如图7所示，将车辆实验轨迹标注在三维点云地图中，图7(a)为俯视视角下的车辆轨迹，图7(b)是轴测视角下的车辆轨迹。

图7 车辆实际运行轨迹Fig.7 Track of vehicle in experiment

测试过程中，车辆从起点由静止出发，给定图7中所示的终点为目标点，在运动过程中记录车辆在大地坐标系下的位置。图7中红色区域为无障碍区域，绿色曲线为车辆实际运动轨迹，黑色区域为障碍物区域。由图7所示不同视角的车辆轨迹结果可知，车辆能到达目标点并实现避障功能。图7表明，车辆能够在满足无碰撞的条件下，在非结构化道路中到达目标点。图7亦说明该算法能够在障碍物大小与数目未知的环境中控制车辆到达期望位置。

图8～图13所示为在实际测试过程中，车辆在整个过程中的动力学参数。

图8 实验车辆纵向速度Fig.8 Longitudinal velocity of experimental vehicle

图9 实验车辆纵向加速度Fig.9 Longitudinal acceleration of experimental vehicle

图10 实验车辆前轮摆角Fig.10 Front wheel angle of experimental vehicle

图11 实验车辆横摆角速度Fig.11 Yaw rate of experimental vehicle

图12 实验车辆垂直载荷Fig.12 Vertical load of experimental vehicle

图13 实验车辆质心侧偏角Fig.13 Sideslip angle of experimental vehicle

由图8和图9可知，在纵向动力学方面，在实验的第70 s、120 s、200 s附近时，车辆周围出现了动态障碍物，使得车辆减速避障，在其余时间中，车辆运动基本保持了车速在目标车速附近。

车辆加速度经由PID控制器控制之后，实际加速度与目标加速度能保持良好的一致性。

由图10和图11可知，在横向动力学方面，车辆前轮转角在运动过程中保持了在了约束范围之内，车辆横摆角速度变化趋势与前轮一致，且横摆角速度在合理的范围之内。

由图12和图13可知，在安全性方面，车辆前后轴接地垂向力满足了安全性约束，车辆质心侧偏角也在约束范围之内。

MPC算法的滚动优化部分保证了算法的闭环特性，在实际使用中，由于车辆是非线性系统，实时使用时需要将连续的状态空间进行离散化，进而使用非线性求解器进行求解，因此算法的实时性很大程度上依赖于非线性问题的求解速度。

在本文的实验中，hp自适应伪谱法给定误差精度为0.01，设置最大迭代次数为16次，最大迭代时间为0.05 s；内点法求解非线性问题给定精度为0.01，设置最大迭代次数为32次，最大迭代时间为0.15 s，离线求解好目标函数与约束条件的雅克比矩阵与海森矩阵的表达式，相比使用数值方法求解梯度，这样可以加快收敛速度。由于使用了配点法构建非线性问题，使得约束条件的光滑性更好，收敛性更好，特别是针对车辆动力学优化问题。

在实际使用中，选取控制时域为0.2 s，图14所示为车辆实际运行中的计算时间图，实验使用计算机硬件配置为中央处理器i7-6700K,8 GB内存，基于Ubuntu 16.04操作系统开发上位机系统。由图14可知在整个过程中，总计1 175次计算，均未达到0.2 s的控制时域上限，平均计算时间为114.5 ms，满足实时性要求。在车辆实际运动中，遇到动态障碍物需要避障时的计算时间要比只有静态障碍物时略长，但仍在控制时域要求内。实验结果显示，算法能在0.2 s的控制时域内更新完成，实现实时控制。

图14 算法运行时间Fig.14 Computation time of algorithm

在本文所提算法中，由于边界的提取直接在点云数据上进行操作，没有任何地图重建过程，该过程运算速度非常快，相对于求解非线性问题，边界提取的计算时间可以忽略不计。由于该算法没有障碍物划分、聚类等过程，在包含不同大小障碍物的非结构化道路中，每次计算仅仅是对应的边界个数不同。边界引发的车辆约束个数远远小于车辆状态量序列引发的约束个数，因此障碍物个数变化对于实际求解运算量影响不大，对于MPC控制器求解时间的影响可以忽略不计，对总体实验结果而言，多障碍物情况下依然可以实现车辆避障。

在实验过程中，第70 s、120 s、200 s附近时存在动态障碍物出现的情况，而其运算时间仍在给定的控制时域约束内。实验结果表明，本文提出的MPC算法能够保证车辆实时控制要求，并使得车辆在给定的约束下无碰撞行驶，使得车辆在运行中实现避障。

4 结论

本文针对轮式地面无人车辆在非结构化道路中，没有参考轨迹与离线地图情况下的避障问题，设计了一种基于模型预测控制的避障算法。该算法使用局部地图信息构建分段最优控制问题，并在构建最优控制问题的过程中依据车辆动力学添加约束，从而求解得到车辆前轮转角与纵向参考加速度，继而实现车辆控制，进行避障，最后通过实车实验进行验证。结果表明：

1) 本文所提算法可以依据局部地图数据，明确代价函数与约束，将避障问题转化，得到合理的最优控制问题表达形式。

2) 本文所提避障算法可以在包含不同大小与数目障碍物的环境中，有效地引导车辆运动到目标点，并在运动过程中实现避障。

本文后续研究将进一步扩展到越野环境下算法的适应性与鲁棒性。