行人过街场景下车辆避障路径规划与控制方法

李文礼，肖凯文，任勇鹏，李 超，易 帆

（重庆理工大学 汽车零部件先进制造技术教育部重点实验室，重庆400054，中国）

行人违章过街导致行人伤亡的现象常年发生，世界卫生组织 (World Health Organization，WHO)指出，全世界每年约有75万行人和自行车骑行者死于交通事故[1]；国家市场监督管理总局的统计数据显示：我国的行人致命事故中，68 %是行人违章横穿道路时发生的[2]。由于行人的运动具有突变性和复杂性，导致驾驶员难以在短时间内正确操控车辆避让违规过街的行人，因此自动驾驶车辆在行人过街场景下的安全避障控制受到很大挑战。

自动驾驶车辆安全避障控制分为路径规划和跟踪控制。现有路径规划方法主要包括人工势场法、基于图搜索、基于采样和基于离散优化的方法[3]：目前应用比较广泛且成熟的图搜索方法主要有文献[4-5]及其各种相关变体算法，此类算法在小地图场景下有较高的搜索效率，但随着地图增大、场景复杂化，使其难以在动态环境中规划出良好的路径，甚至陷入局部最小值；基于随机采样的快速扩展随机树 (rapidly exploring random tree，RRT)算法[6]适用于复杂场景下的路径规划，但存在路径规划结果曲折、抖动大等不足；基于离散优化[7]的规划方法通常将Frenet 坐标系下的参数空间离散化，同时考虑车辆动力学、道路等约束，规划出多条待选轨迹，由于每个规划周期都须对轨迹进行采样计算，导致计算时间较长，对动态障碍物的避障规划轨迹不佳。人工势场法[8](artificial potential field，APF)由于模型简单、实时性高等优点，早期被广泛应用于机器人路径规划领域，随着研究不断深入，许多学者将其应用到智能驾驶领域，取得了较好的成果。文献[9]较早地用行车风险场描述主车周围环境中的潜在风险，提出一种考虑人-车-路相互作用的行车安全场模型，设计一种基于安全场模型的车辆碰撞预警算法并应用于实车试验，验证了碰撞预警算法的有效性。文献[10]基于自车与周车运动状态、位置和速度建立虚拟力模型，实现了车辆轨迹、速度规划，解决了传统势能场的规划路径振荡问题。文献[11]建立基于人工势场的动、静态风险场以对主车周围进行风险评估，基于评估结果实现了个性化的换道触发。文献[12]提出一种动态驾驶风险场模型，很好地描述了车辆跟驰行为以及解释驾驶员类型与换道安全之间的影响因素。文献[13]使用人工势场法对交通环境和驾驶风格进行建模并集成到模型预测控制设计过程中以优化轨迹和控制输出。

现有车辆跟踪控制方法主要包括比例—积分—微分(proportion-integral-differential,PID)控制、最优控制、鲁棒控制、自适应控制、滑模变结构控制、模型预测控制(model predictive control,MPC)等经典控制方法和模糊控制、神经网络控制等智能控制方法[14]。其中模型预测控制(MPC)能在多约束下对车辆进行控制，具有较高的控制精度。文献[15]考虑到传统MPC适应动态工况较差的问题，提出基于模糊控制的权重自适应MPC 控制器，提高了舒适性和控制性能。文献[16]将智能车辆控制器设计为分层结构，利用粒子群算法在线优化控制器参数，提高了控制器的鲁棒性和实时性，减小了车辆跟踪误差。文献[17]设计自适应双时域MPC 路径跟踪器得到了最优预测、控制时域参数，基于综合性能评价指标，提高了行驶稳定性和跟踪精确度。文献[18]设计双层模型预测控制器，对低、中、高速下的主车避障性能进行实验，建立多个典型车车交互场景，验证了控制器的控制稳定性。

为解决车辆对动态行人的避障问题，本文选择行人违规横穿过街的场景，设计一种基于行车风险场的车辆避障控制模型，考虑行人加速时的潜在风险，提出一种基于行人斥力场重心的新型避障函数以优化人车横向避障距离。基于行车风险场理论构建全局路径规划层，并利用MPC 控制方法建立局部路径规划和轨迹跟踪层进行仿真研究。

1 基于行车风险场的全局路径规划层

本文描述的行车风险场由解释行人、道路边界风险和目的地前进倾向的行人斥力势场、道路边界斥力势场和道路引力势场叠加构成。行车风险场主要用于衡量道路环境中影响驾驶安全的潜在危险分布情况，并沿着潜在危险较低区域为主车规划一条驶向目标位置附近的参考路径。如图1是车辆面对违规横穿的行人时，利用行车风险场规划参考路径的示意图。

图1 行车风险场示意图

1.1 行人斥力势场

行人势场是由行人位置向四周均匀发散的一种“虚拟势力场”，用于衡量行人作为障碍物时，对主车的行车风险威胁程度，该势力场在道路环境中表现出“近大远小”的特征，本文使用近似二元Gauss 分布来描述行人势场。设行人作为障碍物时的坐标为（x0，y0），行人中心位置周围任意坐标为（x，y），记行人势场周围任意位置（x，y）与行人位置（x0，y0）的距离为｜P｜，则主车在行人势场周围任意位置的势场方向向量为eP，如式(1)所示，其中e是单位向量。

行人作为交互场景中的障碍物时具有运动状态突变性、复杂性和不确定性，要建立合理的行人势场需要研究行人运动时的诸多因素。因此，本文设计的行人势场模型考虑到行人运动和静止时势场分布的差异，以及其速度、加速度和航向角因素对势场分布的影响。设主车在行人周围任意位置（x，y）处的行人斥力势场为UP，如式(2)所示。

式中：kP为行人势场因数；α为行人加速度的数值(α=a/(m·s-2))；v为行人速度(m·s-2)；θ为行人航向角(rad)，以y轴正向为起始位置，值域为[-π/2,π/2]，逆时针为正；xmax、ymax、vmax分别是纵、横向距离及速度的最大正值，其中λ1=vmax/xmax，λ2=vmax/ymax。图2 展示了行人航向角θ为0 时，沿y轴负方向加速行走的行人斥力场三维图。

图2 行人斥力势场三维图

对于行人航向角θ，当时θ≠ 0，行人斥力势场UP应当随着航向角θ偏转。设偏转后的行人斥力势场周围任意位置（x'，y'）处的势场为偏转过程如式(3)所示。

图3 展示了行人在不同运动状态下的斥力势场映射图。在图3a 中，行人以1 m·s-1的速度，从(10,10)位置匀速向y轴正方向移动，斥力场向y轴正负两端延伸；图3b 中的行人速度比3(a)中的速度更快，斥力势场在y轴正负两端延伸程度更大；图3c 中的行人在3a 中的基础上做匀加速运动，此时的斥力势场更倾向于往y轴正向延伸；图3d 中的行人在3c 的基础上改变了运动航向角，行人在x轴方向获得速度，使斥力场分布变宽且整体逆时针旋转30°。

图3 不同状态下的行人斥力势场xoy 平面映射图

1.2 道路引力势场

道路引力势场的作用是“吸引”主车前往目标位置附近，通常认为当前时刻主车所要前往的目的地位置附近引力势场值最低。设主车欲前往的目的地坐标为（xa，yb），与(1)式同理，记主车在坐标系任意位置（x，y）处的道路引力势场方向向量为ea，则主车的道路引力势场Ua定义如下：

式(4)中：ka为道路引力势场因数，该因数值越大则势场下降梯度越大，道路引力势场越强；图4 描述了主车目的地位置为(50,20)时的道路引力势场分布。

图4 道路引力势场三维图

1.3 道路边界斥力势场

道路边界斥力势场的作用是引导主车行驶在规定的道路范围内，避免其驶出道路边界。设主车在任意位置（x，y）处的道路边界斥力势场为Ur，斥力势场方向向量为er，由于本文选择的道路类型为直行道，则er始终沿竖直方向(y轴)垂直于x轴，故er=(0,±1)。Ur的定义如下：

式中：kr为道路斥力势场因数，该数值越大，则道路边界对主车的排斥程度越强；y1和yr是左右车道边界线的横向位置；L是道路横向宽度；图5 是道路边界斥力势场Ur的三维图，道路中间位置势场大小为0，越靠近道路两侧边界的势场值越大。

图5 道路边界斥力势场三维图

定义行车风险场U为行人斥力势场UP、道路引力势场Ua和道路边界斥力势场Ur的叠加：

图6和图7分别是行车风险场U的三维图和映射图。

图6 行车风险场U 三维图

图7 行车风险场U 映射图

利用行车风险场U和目的地坐标（xa，yb）就可以计算主车在任意位置到（xa，yb）的全局路径：将U、（xa，yb）和τ(主车与目的地位置的距离阈值)作为函数输入；初始化全局路径变量Path、主车初始位置（x0，y0）、主车当前位置、搜索步长step、迭代次数k；随后进行搜索迭代，若主车当前位置与目的地位置距离超过阈值τ且当前位置的风险场梯度存在且不等于0，则主车在当前位置基础上的移动矢量为将每次循环更新后的当前位置保存到Path 中，最后便求得了主车在（x0，y0）位置时，欲前往（xa，yb）的全局路径。图8是主车位置在(10,10)、目的地位置为(100,10)、行人位置为(60,15)时的全局参考路径结果图，行人以0.25 m·s-2的加速度沿轴负方向运动，此时行人速度为1.25 m·s-1，τ为0.5 m。

图8 全局参考路径仿真结果

2 基于模型预测控制的局部路径规划层

基于行车风险场U规划出的全局路径能够避开行人到达预定位置，为主车行驶起到了一定的引导作用，但全局路径在规划时并未考虑车辆动力学约束，当规划出的全局路径出现局部曲率突变时，主车便无法根据全局路径行驶，因此，需要在全局路径的基础上再规划出一条满足车辆动力学和道路约束的局部路径。本文在车辆点质量模型基础上，构建基于模型预测控制的局部路径规划层。

2.1 车辆点质量模型

模型预测控制(MPC)应用到非线性的车辆动力学模型时，计算量较大且难以保障实时性，因此，为了向控制层预留更多计算空间，提高计算速度，选择点质量模型作为局部路径规划层的主体。如图10 所示，设x、y为车辆纵横向位移；为车辆纵横向速度；φ为车辆航向角；v为车辆质心速度。

图9 车辆点质量模型

设纵向为匀速，车辆点质量运动模型[19]为：

设局部路径规划层的预测时域和控制时域分别为Np和Nc且Np＞Nc，对式(8)作前向Euler 离散化，得t时刻下的离散状态方程[17]和输出为：

式(9)中：Tp为规划层的采样周期。

2.2 基于行人斥力场重心的避障功能函数

传统避障功能函数以主车与障碍物间的质心距离来确定目标函数值，距离越近则避障函数值越大。当主车面对加速穿行的行人时，避障功能函数还应当考虑动态障碍物的运动趋势，在行人质心位置基础上将避障点朝行人加速方向前移，使主车避障时机提前，避障横向距离增大，确保避障安全。通过行人斥力场(图3)可以清楚地看出行人此时刻的运动趋势，因此，本文提出一种基于行人斥力场的新型避障函数。如图10 是行人以航向角θ加速运动的行人斥力场映射图，设行人的大地坐标位置为P=（x0，y0），记行人斥力场在XOY平面上的映射区域为A，高度为h，宽度为w。若映射区域内存在一点P'=（xP，yP）满足

图10 行人斥力场重心示意图

则P'为在XOY平面上的映射重心位置，且有dy=yp-y0，dx=xP-x0，θ=arctan(dx/dy)。因此，当行人加速时，行人斥力场重心P'主要分布在加速度方向的前端，主车应当以行人斥力场重心位置P'作为新的障碍物质点，得到改进后的避障功能函数为

式中：Sobs为避障权重因数；（xc，yc）为主车质心位置；σ是一个极小的正数，防止分母为零。

2.3 局部路径规划层的目标函数和约束设计

局部路径规划层的作用是规划出可以安全避开障碍物、满足车辆动力学和道路边界等约束条件的可行驶轨迹，同时还应保证局部路径与全局参考路径的偏差较小。对于规划层控制量u=[ay]，考虑其对车辆的动力学约束：由于式(7)中已假设点质量模型纵向为匀速，则摩擦圆约束可以化简为｜ay｜≤μg，μ为路面附着因数，g为重力加速度。因此，考虑控制量、控制增量约束和摩擦圆约束，设计规划层目标函数和优化过程的约束条件为：

将式(12)求得的预测时域内最优控制序列输入(9)式中，得到预测时域内局部(local,L)路径规划的离散点i=1,2,…,Np。最后采用四次多项式拟合离散的局部路径避障点，获得局部路径规划层输出的局部避障路径。

3 基于模型预测的轨迹跟踪控制器

3.1 车辆动力学模型

车辆作为非线性系统，其运动状态下的动力学过程极其复杂，考虑到过于复杂的模型系统不利于仿真模型的建立以及提高算法控制的实时性，因此，本文在尽可能充分描述动力学特性的条件下采用简化的整车非线性二自由度动力学模型，并进行了线性化处理。假定前轮偏角足够小且车辆直接用前轮转角控制车辆转向，构建如图11 所示的车身横摆与横向二自由度动力学模型，图中：δf为前轮转角；Fcf(Fcr)和Flf(Flr)为前(后)轮受到的侧向与纵向力；la(lb)为前(后)轮中心位置到车辆质心的距离；ω为横摆角速率；v为质心速度。

图11 车辆二自由度动力学模型

根据文献[20]，当主车前轮偏角δf和轮胎侧偏角足够小时，车辆动力学非线性模型如下：

式中：m为车身质量；Sf(Sr)为前(后)轮滑移率；Ccf(Ccr)和Clf(Clr)分别为前(后)轮的纵向和横向侧偏刚度；Iz为整车绕轴的转动惯量；分别为整车质心的横纵向速度；φ为横摆角；Y和X是大地坐标系下的纵横向坐标。

式中：A(t)和B(t)分别为线性化过程中对t时刻的状态量和控制量求得的Jacobi 矩阵。

设T为控制层采样周期，I为单位矩阵。对式(15)第2 行作前向Euler 离散化：

其中：

式(18)中：

3.2 轨迹跟踪层的目标函数和约束设计

轨迹跟踪层的作用是控制主车参考全局路径重规划后的局部路径行驶，为保证主车实际行驶路径与局部路径规划层输出的重规划路径偏差尽可能小，设计轨迹跟踪层的优化目标函数为

式中：ΔU(t)为每一控制周期下所求的控制增量序列，Q和R皆为权重矩阵，分别用于控制输出量偏差和控制量大小；ρ是松弛因子权重矩阵；ε为松弛因子。

为使主车行驶稳定，需要在设置控制器软约束(控制量约束和控制增量约束)的基础上考虑车辆的动力学约束：对于影响行驶稳定性的质心侧偏角约束β，极限范围设置为 ±12°[20]；对于影响舒适性的车辆附着条件约束：本文设路面附着因数μ=0.8，纵向速度恒定，在横向上应满足[20]：

同时，为满足控制器求解过程和舒适性指标，在式(21)添加软约束ε以动态调整附着条件约束：

其中：ay,min和ay,max为横向加速度的极限范围；对于车辆行驶安全性，在3.1 节的小角度假设前提下，对前轮侧偏角αf的约束范围设置为±12°[20]。

结合式(19)-(21)，轨迹跟踪控制问题即为如下优化问题：

式 中：Umax(Umin)为最大(小)控制量集合；ΔUmax(ΔUmin)为最大(小)控制增量集合；ηmin、ηmax为输出量上下极限约束；yhc(ysc)为硬(软)约束输出，yhc,max(min)为硬约束输出的极限值，ysc,max(min)为软约束的极限值；约束条件的第2 行中A=D⊗Im，D为Nc行的下三角单位矩阵，⊗为Kronecker 积，Im为m维单位矩阵。

将式(19)中的目标函数转化为标准二次型后，结合约束条件求解控制时域内每一控制周期下的控制增量序列ΔU(t)，再将该控制序列首个元素作为控制增量输入，每个控制周期都重复此步骤，从而实现主车的轨迹跟踪。

4 路径规划跟踪算法仿真实验

本文设计双规划层生成参考轨迹，对于全局路径规划层，将传统静态全局路径设计为行车风险场实时规划的动态路径供局部路径规划层参考；对于局部路径规划层，设计基于行人斥力场重心的新型避障功能函数来优化局部路径；采用模型预测控制算法跟踪局部路径，实现主车对动态行人的避障。考虑到Carsim建立车辆动力学模型和PreScan 构建场景、传感器的优势，本文在PreScan-Carsim 联合仿真平台上进行仿真实验，验证算法的有效性，仿真实验平台结构原理如图12 所示。

图12 联合仿真控制原理结构图

4.1 动态风险场全局路径规划层下的主车跟踪避障仿真

为了测试行车风险场下主车对加速横穿行人的避撞效果，设计如图13 所示的场景。

图13 试验仿真场景

主车与行人的坐标分别为(0,5)、(23,2)；人行道宽3 m，车道为两车道，单车道宽3.8 m，长度100 m；主车车速为恒定值，分别设为30、45、60 km·h-1，沿纵向直线行驶；行人初始速度为0.75 m·s-1，沿横向直线加速运动，加速度为0.25 m·s-2；另外，在车辆顶部质心位置设置PreScan 自带的AIR 传感器，用于检测主车与行人的相对位置关系，检测距离最大为50 m，扫描角度为180°，即车辆质心横坐标位置大于行人时(主车在行人面前驶过)，行人不再被检测到。仿真实验中的主车参数如表1 所示，基于动态风险场的全局参考路径规划层参数如表2 所示，局部路径规划层和轨迹跟踪层的参数分别如表3 所示。

表1 主车参数

表2 基于行车风险场的全局参考路径规划层参数

表3 模型预测控制器参数

图14 展示了动态风险场全局参考路径下的主车在不同车速下的仿真对比结果。数据表明：主车在不同车速下完成行人避障的同时，其前轮转角(｜δf｜＜4.82°)、横摆角(｜φ｜＜7.98°)、横摆角速率(｜ω｜＜12.88 (°)/s)、横向加速度(｜ay｜＜0.26g)、质心侧偏角(｜β｜＜1.5°)、前轮侧偏角(｜αf｜＜2.88°)、前轮转角增量(｜Δδf｜＜0.35°)皆在约束范围内，不同车速下各参数的最大值参考表4。

从图14a 可以看出，主车在各车速下均能完成对行人的避障并逐渐返回原始车道，由于避障函数与车速成正比，当主车速度越快，避障函数值越大，局部路径规划层输出的避障路径越早呈现出避障趋势。在图14b中，主车速度越大则局部路径规划层中的避障函数会反馈较大的函数值，使局部路径较早显现出避障形态，因此，由于高速车辆能够提前按照避障姿态下的局部路径行驶，当逐渐接近行人时，主车不再需要像低速下行驶那样，需要在短时间内获得较大的前轮转角。

图14b-f 中所有车速为45、60 km·h-1的数据曲线在仿真开始时刻附近都呈现出较大的梯度，这是由于该场景设计的主车初始位置与行人间的纵向距离较近且车速较高导致的避障函数初始值在一开始就处于较大的区间，一旦开始仿真，局部路径规划层输出的局部路径就有了避障趋势，从而使轨迹跟踪层输出的前轮转角增量一开始就具备避障功能，促使横摆角、横摆角速率、横向加速度和质心侧偏角在仿真起始位置急剧上升，但根据图14a 显示，此种情况并不影响后续主车对行人的避障，且根据表4 数据统计结果，各项参数均在在合理的约束范围内。

从图14g 中可以看出随着车速的增大，前轮侧偏角｜αf｜的最大值依次减小，这是由于主车车速V的增大导致避障功能函数(式(11))值变大，从而让局部规划层输出的避障轨迹较早地呈现出避障趋势，使得主车提前做出避障行为，因此，主车车速越大则避障时的前轮转角峰值越小(图14b)，前轮侧偏角｜αf｜峰值越小(图14g)，前轮转角增量｜Δδf｜峰值越小(图14h)。

图14 主车在不同车速下跟踪动态风险场全局路径的仿真结果

表4 为主车在道路附着因数为0.8 时，各车速下的避障仿真数据最大值统计，从表中结果可知：在动态风险场全局路径下，主车能够实现动态行人避障功能，各项参数的最大值皆处于合理的约束范围内。

表4 μ 为0.8 下的各车速行车风险场避障仿真数据最大值

4.2 动态行车风险场全局路径与静态全局路径避障仿真对比

基于图13 的场景参数，控制主车速度变量为30 km·h-1，将全局路径规划层输入分别设置为传统静态全局参考路径(y=5 m)与基于动态行车风险场的全局参考路径，对比主车参考2 种不同全局路径时的避撞横向安全性与稳定性。

图15a 是2 种全局参考路径下的行人距主车轮廓最短横向距离对比结果：主车在仿真前1.4 s 与行人的横向距离不断接近，且静态全局参考路径下的主车在横向上更快地靠近行人；1.5 s 后，静态全局参考路径下的人车横向距离减小幅度变缓，而动态风险场全局参考路径下的主车为避免与行人在横向上距离过近，缓慢地增加与行人之间的横向间距，直到2.6 s 左右，车辆驶过，行人消失在传感器的检测范围内，传感器数值置为零。在图15a 中，计算2 条曲线在0～2.6 s 之间的积分结果，其中静态全局路径方法下的最短人车横向距离曲线积分结果为2.611，而动态风险场全局路径方法的曲线积分结果为2.732，与前者相比提升了4.63%，因此，利用动态风险场全局路径作为主车避障输入提高了人车之间的横向安全性。

图15b 是主车分别在2 种全局参考路径下驶过的前40 m 实际路径，从图中可以看出：主车在动态风险场全局路径下做出避让的行为早于静态全局路径且在横向上具有更大的偏移量。

图15c 是主车在2 种全局参考路径下前50 m 的避障函数值变化对比结果。从图15c 可知：当主车在动态风险场全局路径中，其避障函数值增长速度和峰值皆低于静态全局路径，这是由于在动态风险场全局路径规划层下的局部路径规划层中构建了基于行人斥力场重心的新型避障函数，当行人加速时，车辆对行人的避障点并非在行人质点位置，而是在行人加速方向的前一段位置(行人斥力场重心位置)，因此，车辆避障时机和避障距离都会相应地提前和增加，从而导致Jobs值减小。

图15d 是主车在2 种全局路径下的质心侧偏角β对比数据，根据表4 的约束条件，结果表明：2 种方法皆满足车辆行驶稳定性约束条件，其中静态全局参考路径下的质心侧偏角最大绝对值为1.34°，而动态风险场全局参考路径下为1.08°，因此，车辆在动态风险场全局路径下的避障稳定性更高。

图15e 是随仿真时间推移下的轨迹跟踪控制器迭代时间，可以看出，控制器在2 种全局参考路径下的计算时间处于40～50 ms 的区间内。

图15 静态全局路径与动态行车风险场全局路径仿真结果

图16 为前3 s 内主车对加速横穿的行人避撞过程。由图16 可知：2 种全局参考路径下的主车都可以避让加速横穿的行人，而主车在静态全局路径下避让行人的同时更看重车道的保持，相反，在动态风险场全局路径下的主车更看重与行人在横向上保持安全距离。

图16 静态全局路径与动态风险场全局路径仿真过程对比

5 结论

本文将动态行人作为主车在交通场景的避障对象，建立基于动态行车风险场的全局参考路径规划层，设计基于模型预测控制的局部路径规划层和轨迹跟踪层，实现行人障碍物加速横穿场景下的主车路径实时规划与避障。提出基于行人斥力场重心的新型避障函数，保证主车与行人避障过程中的安全横向距离。搭建PreScan-Carsim 联合仿真平台，设计多车速避障场景对提出的方法进行验证。

结果表明：主车能在合理的横向位移下对加速横穿的行人完成避障，且能保持行驶稳定性(｜β｜＜1.5°)，在一定范围内，车速越高，局部参考路径越早呈现出避障姿态，主车避障行为越早，符合现实场景下的避障安全性；设计静态全局参考路径与基于动态行车风险场的全局参考路径对比实验，结果表明：将全局路径规划层替换为本文设计的动态行车风险场规划层，主车仍能够完成对动态行人的避障，且避障稳定性提高了7.21%，横向安全性提高了4.63%。