基于场景驱动与分层规划的自主代客泊车系统研究

邱少林 钱立军 陆建辉

合肥工业大学汽车与交通工程学院，合肥，230009

0 引言

近年来，高级驾驶辅助系统(advanced driver assistance systems, ADAS)和自动驾驶(autonomous driving, AD)技术一直是学术界和工业界研究和开发的热点。因为视距角度和距离误判的局限性以及停车位空间的限制，对驾驶员来说停车入位任务并不容易，所以泊车辅助系统是市场上最为必要的产品之一。几乎所有商业化的泊车辅助系统都基于各种传感器和基础设施，并且具有被动安全辅助功能，如在停车时警告驾驶员障碍物的存在，即使采用智能泊车辅助系统(intelligent parking assistance systems, IPAS)等主动泊车辅助系统，泊车位也需由驾驶员确定。为了提高驾驶员的便利性和停车场的安全性，自主代客泊车系统(autonomous valet parking systems, AVPS)已经得到了广泛的研究[1-4]。

AVPS系统的开发主要涉及两个重要的研究问题：一个是确定目标停车位和停车场中已停车辆的周围环境；另一个是生成和跟踪参考轨迹。通常使用车载传感器检测周围环境，如相机、激光雷达，而要实现泊车过程中车辆的准确定位和环境信息的正确表达，目前车载传感器的性能尚未达到令人满意的水平，故关于车辆定位的研究都集中在基于地图辅助的自动驾驶车辆定位[5-7]，也有许多研究涉及构建合适的数字地图[5-9]。在一种典型的自动驾驶场景——智能化停车场中，停车场是具备提供地图信息功能的基础设施。基于基础设施的停车位占用率检测系统的研究已经持续了近十年[10-12]，这些系统的优点是车辆不需要具备复杂检测系统。然而，当车辆具有自动驾驶系统时，因为车辆已经具有车载传感器，所以不需要对所有车位进行占用检测，因此，本文将占用检测算法与基础设施信息分开，并使用基于停车场几何结构的简单环境地图。

目前研究成果中，生成参考路径的方法主要有基于几何的方法和基于最优控制的实时路径规划方法。GMEZ-BRAVO等[13-14]提出基于连续曲率β-样条曲线的路径生成算法，为了保证参考路径的无碰撞和可用性，该算法采用具有曲率和边界约束的模糊控制逻辑。CHENG等[15]将路径生成问题分为两部分，分别采用不同的方法规划，基于感知结果，每个部分选择Reeds-Shepp曲线、连续曲率曲线或半连续曲率曲线，但没有提出环境感知模块的细节。YOON等[16]基于两种启发式算法提出A*算法，这两种启发式算法分别考虑有障碍物和无障碍物时车辆运动学特性。

本文从系统的角度对AVPS系统进行功能集成与仿真验证。

1 场景定义

场景是ADAS系统和自动驾驶系统开发和测评的关键因素，是ADAS系统和自动驾驶系统进行功能需求分析和测试评价的基础，同时也决定系统的工作范围，因此，如何定义功能开发和测试场景一直是ADAS系统及其他智能车技术的研究重点。鉴于本文的研究目的，在此只做驾驶场景的定义，并基于定义的驾驶场景进行系统设计与验证。

自主代客泊车(AVP)有望成为自动驾驶领域最早实现落地并转化商业价值的应用场景之一。简单来说，自主代客泊车就是指车辆到达商场、园区等某个相对封闭环境或分时租赁的还车范围以后，用户便可下车办事，同时只需在手机端下达停车指令，车辆就可自动从下车地点规划行驶路径，开进停车场的指定车位停好；需要取车时，用户也可通过手机端发出指令，一键召车。整个泊/取车过程没有人的参与，是一个典型的限定区域内低速无人驾驶应用场景。

AVPS系统的最终目标是通过智能基础设施和V2I通信提供AVP服务。AVP服务的驾驶场景由一系列驾驶操作组成，例如向前驾驶(图1中的A到B)、临时停车点(图1中的B)、平行或垂直停车(图1中B到C属于平行泊车，B到D属于垂直泊车)、完全停车(图1中的C或D)[17]。

智能基础设施由多台服务器、分布式感知传感器和V2I通信系统组成[18]。分布式感知传感器(如相机、激光雷达和超声波传感器)用于检测停放车辆并跟踪正在运动的车辆。相应的信息被发送到一个局部状态感知服务器，对车辆位置和车位占用进行估算并将结果发送到全局状态感知服务器，如图1所示。

图1 AVPS场景定义Fig.1 AVPS scenario definition

2 环境模型层

停车场的优点之一是车辆可以使用来自基础设施的地图信息。地图数据可以通过许多不同的方式使用。环境模型代表环境地图，对于AVPS系统，该地图包括可用和被占用的停车位、道路标记以及诸如行人或其他车辆的障碍物信息。占用概率地图是这种环境模型的常见表示形式。环境地图通常通过集成来自激光雷达和相机传感器的观测数据来实现即时定位与地图创建(simultaneous localization and mapping, SLAM)。本文旨在得到一个更简单的场景，如车辆通过基础设施(V2I)系统或俯瞰整个停车位的相机提供地图。使用停车场的静态地图，并假定车辆的自定位是准确的。本文所建停车场环境模型主要包含3个占用栅格层(图2)：①静止障碍层包含静止障碍物，如墙壁、障碍物和停车场的边界；②道路标记层包含道路标记信息，包括停车位的道路标记；③停放车辆层包含已被占用停车位的信息。

(a)静止障碍层 (b)道路标记层 (c)停放车辆层图2 停车场环境分层模型Fig.2 Layered environment model of the parking lot

每一个地图层都包含不同种类的障碍物，它们代表不同程度的危险。通过这种结构，每个层都可以独立地处理、更新和维护。在每一层中，黑色单元代表已占用，而白亮单元代表可用。为简单起见，将3个图层合并为一个代价地图，如图3所示。代价地图将车辆环境表示为二维占用网格。单元格中的每个网格都具有介于0和1之间的值，表示单元格的成本。障碍物成本较高，而可用空间成本较低。如果一个单元的成本高于占用阈值，则该单元被视为障碍物，如果成本低于可用阈值，则该单元是可用的。

图3 停车场环境组合模型Fig.3 Combined environment model of the parking lot

3 路径规划层

自主代客泊车的路径规划问题可以看作是一个简单的2D路径规划，因为停车场的坡度是可以忽略不计的(与道路行驶相比，泊车总路径长度相对较短，可以忽略道路坡度而产生的误差)，因此，2D路径足以用于AVPS系统的参考路径设计。为了生成参考路径，应该考虑车辆运动的特性，而不是设计不可跟踪的不合理路径。如果转向角是恒定的，并且车辆的速度足够低而不使轮胎力饱和，则车辆的运动可近似看成沿后轴中心行驶的圆周运动。从而，停车过程中，车辆可以被视为具有离散控制输入的Reeds-Shepp Car。控制输入包括：由恒定半径向左、向右转向输入组成的横向运动，由向前、向后的恒定速度输入组成的纵向运动。事实证明，48个不同的组合足以完全描述Reeds-Shepp Car的最短路径[19]。停车场区域使用简单的路径生成算法，如Reeds-Sepp Car。因为停车场的大小和每个停车位之间的间隔是规范化的，这意味着使用标准停车场定义的路径可以被用于几乎所有停车场。

在48个Reeds-Shepp曲线中，曲线-曲线-直线(CCS)用于生成垂直停车的参考路径，如图4所示。在二维平面上，除了目标点外，还需要定义3个点，本文中，曲线与直线之间的点被定义为切点，两条曲线之间的点被定义为中间点，连接在自由行驶路径和曲线之间的点被定义为转折点。通过这3个标准点，可完全描述参考路径。为了定义这些点，必须计算半径R1和R2、中心弧角θ和直线的距离dt。基于停车场环境地图和目标停车位的位置、航向信息生成参考路径。为避免与已停放车辆发生碰撞，应以最低安全边界cmgn计算dt，计算公式如下：

(1)

(2)

dt=Lpf-dm

(3)

式中，WP为车位宽度；dm为纵向边界；Vw为车辆宽度；R1为切点与中间点之间的圆弧半径；Lpf为停车位前端与目标点之间的长度。

图4 路径生成示意图Fig.4 Path generating schematic

dt定义后，基于停车位的尺寸计算θ，计算公式如下：

(4)

(5)

式中，cpath为图4中停车位与预行驶路径之间的安全边界。

为了简单起见，假设R1和R2是相同的，并由Ackerman的角度公式计算如下：

(6)

式中，L为车的轴距；δ为前轮转向角。

则定义参考路径所需的点确定如下：

(7)

式中，xGoal、yGoal为目标点的横纵坐标；xtan、ytan为切点的横纵坐标；xmid、ymid为中间点的横纵坐标；xturn、yturn为转折点的横纵坐标。

4 行为层

规划是将所有相关信息组织到分层结构中，每个连续的层负责更细分的任务。行为层位于顶部，负责通过提供一系列导航任务来激活和管理任务的不同部分[20]。行为层汇集来自系统所有相关部分的信息,包括以下各部分的信息：

(1)定位。行为层检查定位模块以估算车辆的当前位置。

(2)环境模型。感知和传感器融合系统反映车辆周围环境的地图。

(3)车位检测。分析地图以确定最近的可用停车位。

(4)全局路径搜索。路径模块基于地图服务或从V2I基础设施获得的路网计算全局路径。将全局路径分解为一系列道路段，可以对每个连接的路径段进行不同的规划，例如，不同的泊车方式，其最后的泊车行为需要不同的速度曲线。推而广之，分段规划对不同车速限制、不同车道数量和不同道路标志的街道的导航也至关重要。

本文不是依靠车辆传感器来构建环境地图，而是通过车辆与基础设施V2I通信从智能停车场获取地图。为简单起见，假设地图采用占据网格的形式，由V2I提供道路连接和可用停车位的位置。全局路径规划被描述为遍历一系列路径段以到达停车位。全局路径规划由路径规划列表构成，主要包含起始姿态(x0,y0,θ0)、终止姿态(xn,yn,θn)以及路段属性(如限速信息)。以当前姿态并根据路径规划中关键点的创建行为来规划辅助目标。行为请求用于从行为规划器请求一系列导航任务，直至到达目的地。每个路径段的车辆导航步骤如下：

(1)运动规划。通过环境地图规划满足车辆约束的可行路径。

(2)路径平滑。通过拟合样条曲线来平滑参考路径。

(3)轨迹生成。通过生成速度曲线将平滑路径转换为轨迹。

(4)车辆控制。基于所设计的横向控制器执行轨迹。

(5)终点检测。检查车辆是否到达目的地。

4.1 运动规划

给定全局路径的情况下，运动规划用于规划通过环境的路径以到达每个辅助目标点，直到车辆到达最终目的地。每个路径段的规划路径必须可行并且无碰撞。一条可行的路径是指车辆在运动学和动力学约束条件下实现的路径。AVPS系统涉及低速和低加速，可忽略由惯性效应引起的动力学约束。

路径规划算法通过构建连续且无碰撞车辆姿态树来找到路径。使用Reeds-Shepp转向连接姿态，确保生成的路径在运动学上可行。除规划的参考路径外，还应考虑车辆行驶的无碰撞区域，这些区域表示车辆的参考点不得越过代价地图区域，以避免碰撞任何障碍物。路径规划器检查并确保产生的车辆姿态不在这些区域上，规划避开障碍物的路径。

4.2 路径平滑

路径规划器生成的参考路径由Reeds-Shepp线段组成。两条线段连接处的曲率可能不连续，导致转向角度突然改变。为了避免这种不自然的运动以确保乘客的舒适度，路径需要连续可微并且平滑[21]。本文采用的平滑路径的方法是拟合三次样条曲线，通过拟合参考路径中所有辅助目标点的三次样条曲线进行路径平滑，生成控制器可以执行的平滑路径。样条曲线的起始和结束方向与车辆的起始和结束方位角近似匹配。

4.3 轨迹生成

轨迹生成是将平滑路径转换为可基于速度曲线执行的轨迹。按照三个阶段的顺序计算每个路径的速度曲线：加速到设定的最大速度、保持最大速度和减速到终点速度。定义初始速度、最大速度和终点速度，得到包含平滑路径上每个点参考速度的轨迹曲线。

4.4 车辆控制

AVPS系统最具挑战性的问题之一是在给定的驾驶条件下对车辆进行建模。用于横向控制的自行车模型已经被广泛用于公路驾驶[22-23]。假定侧向力与滑移角成正比，并且所有的车轮转角都很小，但停车时会产生较大的车轮转角，而在现实中不容易识别出不同车辆的刚度系数等车辆参数。

对于AVPS系统，车辆以低速行驶，因此滑移角在本文中可被忽略。运动学模型可以用于前向运动和后向平行/垂直泊车，并且可以被写成如下形式[24-25]：

(8)

式中，(x，y)为车辆的质心坐标；ψ为车辆的航向角；v为纵向速度。

此外，考虑转向执行机构的运动约束，即转向盘转角与前轮转向角成正比，前轮转向角和前轮转向角速度有界，运动学约束如下：

(9)

式中，γ为转向盘转角；Rj为转向系统的传动比；ω为前轮转向角速度。

基于式(8)和式(9)中的车辆模型和动态面控制(dynamic surface control,DSC)设计横向控制器[17,26]。由于横向控制器应该具有足够的鲁棒性，以补偿由各种车辆类型和驾驶操纵引起的模型不确定性和干扰，且计算出的转向角应该足够平滑以满足式(9)中的运动学约束，所以应用DSC技术设计横向控制器。横向控制器的设计流程如下。

定义误差曲面为

Si=ey+dieψ=ey+di(ψd-ψ)

(10)

式中，下标i表示驾驶动作，即f是前进行驶，b是后退行驶；Si为误差曲面；di为预瞄距离；eψ为横摆角误差；ψd为期望的横摆角。

其中，横向误差ey定义为

图5中，di和ψd的定义如下：

di=‖epd‖

ψd=∠epd

图5 预瞄算法原理图Fig.5 Preview algorithm schematic

应该注意的是，预瞄控制思想通过考虑等式(10)中的di和ψd(图5)，被纳入动态面控制中[25]。图5中Pd(xd,yd)不是将ψd定义为切线角度，而是通过使用Pd和Ppd之间的角度以及预瞄距离di来定义。

对式(10)中Si取导数，结合式(8)有

(11)

(12)

为了满足式(9)中的第二运动约束，即转向执行器的角速度极限，通过如下的低通滤波器对式(11)中与期望转向角相关的项进行滤波：

(13)

式中，τ为滤波时间常数。

然后，基于式(9)，可得转向盘转角

γ=Rjarctangdes

(14)

5 仿真验证

PreScan是一款可用于ADAS和智能车辆(IV)系统仿真测试的软件工具。在 PreScan软件中可以很方便地搭建用于验证和评估ADAS和IV系统的车辆虚拟行驶环境以及相关传感器，如毫米波雷达、激光雷达、超声波雷达、相机、GPS和V2X通信。本文基于PreScan-MATLAB联合仿真环境实现对AVPS系统的功能集成与有效性验证。整个自主泊车过程可以分成以下步骤。

(1)基于智能停车场的V2I通信服务，AVPS系统接收停车场的环境地图以及目标可用车位信息。

(2)基于环境地图与目标车位信息生成辅助目标点，规划全局路径，并生成轨迹。

(3)车辆向前行驶跟踪全局轨迹，同时超声波雷达实时检测，一旦超声波检测结果与V2I发送结果一致，则确认车位检测成功。

(4)发现车位后车辆减速，建立大地直角坐标系，同时实现路径规划。全部计算完毕后，车辆开始倒行泊车。

(5)通过实时横向控制器计算转向角度，车辆自动泊车，泊车修正，泊车完成。

PreScan场景模型、联合仿真模型及仿真结果如图6～图8所示。

(a)二维场景 (b)三维场景图6 PreScan场景模型Fig.6 PreScan scenario model

图7 PreScan-MATLAB联合仿真模型Fig.7 PreScan-MATLAB co-simulation model

(a)创建环境模型 (b)生成辅助目标点

(c)路径规划 (d) 轨迹生成

(e)轨迹跟踪 (f)泊车操作图8 AVPS联合仿真结果Fig.8 AVPS co-simulation results

6 结语

为了提高驾驶员泊车的便利性和停车场的安全性，结合V2I通信服务实现特定场景下的环境建模，利用Reeds-Shepp Car曲线进行路径规划并生成运动轨迹，基于预瞄控制理论与动态面控制技术实现车辆前向驾驶与后退泊车的控制，在PreScan-MATLAB联合仿真环境下实现对所设计系统的功能集成与有效性验证。结果表明该系统可在特定场景下实现自主代客泊车功能，后续将在功能样车上做更加充分的验证，同时，场景的普适性与动态避障也是重点研究的方面。