什么是自动泊车系统？自动泊车路径规划和跟踪威廉希尔官方网站 分析

3D AVM

3D AVM Pipeline 其它 另外还有一种做法：通过厂家提供的相机安装参数计算鱼眼相机与同意坐标系（汽车中心）之间的外参，通过外参将3d模型上的顶点坐标转换到相机坐标系下，再通过相机内参转换到图像坐标系上。用此方法同样可以得到2d图像纹理与3d模型顶点之间的一一映射关系。这两种方法的基本思想其实是相通的，殊途同归。只不过这种方法相机安装参数会有一些误差，可能会导致最终的3d拼接效果不佳。

总结

AVM2D、3D全景环视是一个需要算法理论和实践强结合的自动驾驶系统，其中涉及到的领域为计算机视觉、图像增强、三维等。后续还会对标定、去畸变等算子进行优化，并加入自标定、车轮视角、广角、透明底盘等内容。

本文的每个章节都是首先讲述基础理论，再结合实验demo来进行呈现，尽量避免繁琐的公式推导，目的在于将AVM系统算法框架搭建的方法讲述清楚。

2.控制器

中央处理器/中央控制系统，为 APS的核心部分，首先根据车位监测传感器侦测到的距离和角度信息，分析处理后得出汽车的当前位置、目标位置以及周围的环境参数，建立自动泊车的坐标系，探测泊车位的位置和有效长度和宽度尺寸，确定本车与泊车位及前后车辆、周围环境的相对位置；然后对泊车行为进行预测，判断该车位是否可用和能否成功进入并根据停车位和汽车的相对位置，实时进行环境建模，计算出最优泊车路径，规划泊车路径及避障，并将其转换成电信号；最后控制汽车的执行系统（如方向盘、油门、刹车等），即在自动泊车辅助过程中通过电动转向助力系统EPS控制转向和刹车系统ABS、ESP控制制动，按照泊车算法预定的泊车路径进行自动泊车。在泊车过程中，实时监测并且通过蜂鸣器警示周边行人和车辆注意。

中央处理器则需要通过算法作两步骤：

第一步是将这些参数制作成三维动态模型，展示到车辆前排的屏幕上供驾驶员参考，第二步则是计算出自动泊车的路线，并转换成电信号，并传输给车辆策略控制系统。

之后的操作就相当简单了。控制系统按照这些电信号指令，对行驶角度、方向和速度 等做出控制，直到停车入位。 （1）硬件

两种硬件解决方案， TC297+TDA2 平台立足于自动泊车的应用，TC397+TDA4 平台提供更高的算力和更丰富的内存资源和接口，立足于代客泊车的应用。

‍‍ ‍‍       （2）软件  

为适应软件不断升级的需求，以及主机厂自主开发泊车控制算法的需求，在实现上述

功能场景需要的软件的基础上，自动泊车产品的解决方案有以下特点：

标准的软件架构

满足标准AutoSAR要求的MCU端软件，以及参考OpenVX 架构的SoC端软件。

‍‍‍

‍     MCU端软件架构   ‍

‍‍‍‍‍       SoC端软件架构     ‍     OTA远程升级 与Tbox等控制器配合，实现APA控制器软件的远程升级，TDA平台的OTA通过‍‍‍‍‍‍‍‍‍‍TCx97来管理和执行。‍‍‍‍‍ 开放的软 / 硬件接口 提供开放的软/硬接口，支持主机厂在自动泊车平台上自主开发全部/部分 泊车算法。

（3）自动泊车核心--算法模块

主要算法模块及算法模块之间数据传递，自动泊车算法的基本架构。自动泊车可以细化成：传感器融合、轨迹规划、车辆控制、避障模块、状态管理模块、定位模块。自动泊车算法框图如下：

自动泊车算法模块组成

1）定位模块

定位模块贯穿整个自动泊车，作用为保证所有算法模块处于一个“频道”，避免因“频道”不同引起的偏差。

定位模块接收信息为：ESP提供的轮脉冲信息、EPS提供的方向盘转角信息、ESP提供的横纵向加速度及IMU信息。其中IMU信息为非必项，但是仅依靠车辆提供的信息定位精度会有下降，特别是横向精度。

定位模块输出信息为：车位坐标、坡道、车速、加速度等。

定位精度的下降会导致三方面问题：

①对驾驶员找车位的要求提高，驾驶员需尽量沿直线行驶，沿曲线行驶会造成车位无法识别；

②对于车位识别更新精度及更新频率要求会增加。对于这条可以通过手动泊车入库的方式理解：人在倒车的时候，对于定位的信息很模糊，只知道车辆大概走了多远，处于什么位置。但是人可以通过后视镜、倒车影像等不断修正车辆，进而使车辆泊入车位。类比，当定位精度不高时，车位更新的要求会相应提升。

③无法记忆车辆行驶过距离较长的车位。此问题影响很小，正常使用基本是找到车位后立即泊入，很少会走很远距离泊入。

2）传感器融合模块

传感器融合模块相当于人的眼睛、耳朵，主要用于构建车辆周围环境信息。传感器融合模块输出信息为车辆周围环境地图，这张地图应包括：车位信息、可达空间信息、路沿信息、档杆信息、减速带信息等。

环境地图

环境地图信息的构建直接影响后续泊车的性能。

例如，车位位置精度存在偏差，可能会造成车辆泊入后位置不正，或是在泊入后发现不居中，此时需要出库重新调整。

例如，无档杆的信息，车辆会存在猛烈撞档杆问题。部分停车场中，车位较短，如提前知道档杆的信息，可以使车辆靠前一点停放。

例如，可达空间偏差，内缩会造成规划难度增加，外扩会造成泊车失败，或是产生安全风险。

3）轨迹规划模块

轨迹规划模块作用是根据地图和当前车辆坐标信息，对地图中的可泊车位进行规划形成路径。影响规划成功率的主要有两种，车位对向可达空间宽度与车辆参数。其它条件相同的情况下，对向空间越大越容易规划成功。

车位对向空间对规划的影响

其它条件相同，车辆的转弯半径、宽度、长度越大，越难规划成功。

车辆参数对规划的影响

规划模块也需负责进行重规划的触发：

①随着泊车的进行，目标位置发生变更；

②在进行轨迹跟踪使车辆与路径产生较大的偏差；

③可达空间发生变化，原路径无法继续行驶。

4）车辆控制模块

车辆控制模块，相当于路径的执行模块，主要作用是接收路径信息，将路径信息离散，进行跟踪。

车辆控制模块接收的信息为：路径信息、车辆坐标、当前车速、加速度、坡道等。

车控控制模块输出信息为：扭矩请求、方向盘转角请求、档位请求、减速度请求、EPB控制请求。

VCU(整车控制器)：在自动泊车功能中，VCU的主要功能有两个，响应挡位切换、响应扭矩输出实现车辆运动；

车辆控制模块性能直接影响驾驶员使用泊车时的体验，比如车辆加速度，低速情况下人可明显感知的加速度大小为0.8m/s2，如果超过这个加速度，人会感觉非常不舒服。因此除非紧急避障，其它情况应进行预判，使用比较小的减速度。

5）避障模块

避障模块作用是预测可能存在的风险，提前刹停车辆，防止碰撞。

避障模块需检测两种障碍物：泊车开始后可达空间的动态变化、和历史可达空间边界。可达空间动态变化可以是行驶到车辆行驶路径上的人或是车辆等动态障碍物；历史可达空间的边界为地图中可达空间的边界。

避障模块接收的信息为：车辆坐标、地图信息、超声波或视觉信息。

输出信息为：向车辆控制模块输入避障信息。

6）状态管理模块

状态管理模块的作用可分为三部分：HMI实现、外部及软件内部状态监控、安全模块。

HMI包括泊车动画显示、泊车过程文字提示、泊车开始按钮操作等等；状态监控主要是监控软件模块运行状态、监控车辆执行器、传感器、车身状态（车门、后视镜）等，出现异常情况时刹停车辆退出泊车；安全模块相当于车辆控制模块的备份，在车辆控制模块发生异常时接管车辆，保证车辆能够安全刹停并退出泊车。

产品形态不同、需求不同对于算法的架构都会有一定影响，但是如上所述模块基本可以覆盖到泊车系统所需的主要算法模块。

3.执行器

车辆策略控制系统接受电信号后，依据指令作出汽车的行驶如角度、方向等方面的操控，直至停车入位。

执行系统主要包括电动助力转向系统和汽车发动机电控系统。根据中央控制系统的决策信息，电动助力转向系统将数字控制量转化为方向盘的角度，控制汽车的转向。汽车发动机电控系统控制汽车油门开度等，从而控制汽车泊车速度。电动助力转向系统与汽车发动机电控系统协调配合，控制汽车按照指定命令完成泊车过程。

执行器主要由四个部件组成,全部必不可少：ESP车辆稳定系统控制单元/电子稳定程序：具有控制自适应制动、制动力分配EBD、防抱死制动ABS、起步加速防滑控制ASR、电子牵引辅助ETS、制动辅助BAS等功能，通过分析各传感器（如轮速传感器）传来的信号，然后向ABS、ASR发出纠偏指令/正确的控制指令，来帮助车辆维持动态平衡使车辆可以在各种状况下保持最佳的稳定性。在转向过度或转向不足的情形下，稳定效果更加明显。后轮驱动汽车常出现的转向过度情况，后轮失控而甩尾，ESP便会迅速轻微制动外侧的前轮来稳定车子（注意：此时制动不会使车轮抱死，旨在降低轮速）；在转向不足时，ESP则会迅速轻微制动内后轮，从而校正车辆行驶方向。 主要作用为接收减速度信息实现车辆制动。目前ESP支持两种制动接口，CDDS与CDDAP。CDDAP为泊车专用接口，通过发送剩余制动距离、紧急制动等信息可以实现更高精度的制动，其精度一般可以控制在10cm内。另外CDDAP也提供了例如二段泄压的接口用于在坡道上起步。

EPB电子刹车：泊车结束后使车辆驻车，另外EPB也可以作为制动冗余备份，EPB一般支持在3km以下直接响应，可以使车辆刹停。对于钥匙泊车和手机泊车，制动冗余备份是必要的。有部分车型，因成本考虑会把EPB控制器集成在ESP中，此时ESP出现异常，EPB无法起到制动备份的作用。

EPS电子转向助力/电动助力转向机构：由齿轮齿条式转向机、扭矩传感器A91b1、电动电动机A91m1和转向机构控制单元N68组成，N68读取A91b1的信号和来自ESP的轮速信号，据此促动A91m1，从而带动齿轮齿条式转向机运转，实现转向功能。 主要作用转动方向盘，实现车辆的横向控制。EPS的接口有两个，扭矩控制与角度控制。扭矩控制的方式一般用于车道保持系统。泊车主要使用角度控制，角度控制其控制精度更高，响应一致性更好，方便轨迹跟踪算法的实现。转向管柱模块控制单元读取方向盘转角和转向角速度，并通过Flex Ray总线与CAN R络通信。 

4.其它关联系统：

IP仪表：主要作用是用于显示车位信息，并且提示泊车进程

HUT导航主机：主要作用显示泊车动画，提供泊车按钮

BCM整车控制器：提供车辆车门状态。当车门开启时泊车系统应被禁用，车门开启会影响摄像头车位识别精度，另外车门开启，车辆边界改变，存在一定碰撞风险

PEPS无钥匙进入系统：转发钥匙信号，实现钥匙遥控泊车

BLE蓝牙模块：可以集成在大屏中，但是要满足一定范围的连接距离，主要用于手机操控泊车。为了保证系统安全，防止蓝牙通道恶意攻击劫持，可以使用报文加密及时间戳来保证。手机泊车不使用4G或是5G网络作为连接通道的原因为网络信号容易受到环境干扰及网络延迟。例如部分地下停车无网络信号，泊车将无法使用。另外关于网络延迟，手机泊车过程中如出现异常，需要保证车辆立即退出泊车。

驻车定位系统PTS控制单元：位于行李箱中左侧，主要有以下作用：读取各种电子元件输入信号，如车速、挡位状态、点火开关状态、电动方向机的状态等信号，促动车距传感器和警告元件，通过Flex Ray总线与CAN网络通信。 

警告元件：前部警告元件集成于仪表中，当车速低于16km/h时，驻车系统切换至测量模式。后部警告元件位于后风挡玻璃上方，在车速低于16km/h时，警告部分亮起向驾驶员发出视觉警告。

五 原理

自动泊车系统由多组传感器组成，遍布车辆周围的雷达探头、摄像头测量自身与周围物体之间的距离和角度，在采集好图像、距离等相关数据后，数据会传输给处理器，处理器将采集到的数据分析处理，然后通过处理器计算出操作流程，形成自动泊车策略再将其转换成信号，控制系统接受到信号后依据指令控制汽车做出诸如转向、倒车和制动等动作，配合车速调整方向盘的转动，直至泊车过程完成。

系统通过车辆两侧的超声波传感器和全景摄像头自动搜索车辆左右侧可用的停车位。在自选车位模式下，驾驶员在系统探测范围内自动选择合适的车位。基于超声波雷达的全自动泊车威廉希尔官方网站 ，主要包括：搜索停车位、自动泊入、自动泊出等功能,如下表。

经过在不同场景下的测试，在整个泊车的成功率包括了泊车的精度，六项最重要的泊车指标。 通过多媒体或自动泊车按键，就可以开启或者关闭自动泊车功能。当车位被选择后，驾驶员按照多媒体显示屏上的相关信息提示停车并点击开始泊车按键，系统可自动进入泊车模式。该模式下系统可以自动规划泊车轨迹，并控制车辆的挡位、转向、制动、车速等使其驶入规划的车位。   或在挂入倒挡后选择“自动泊车”，接着将车速保持在25km/h以下以便车辆主动识别停车位。最后选择自己想停的车位，剩下的就是松开刹车踏板，全程交由自动泊车功能独自完成。自动泊车时按下提示区域的“暂停”按键可以暂停泊车，再按下“开始”恢复泊车，在必要时还可以直接踩下刹车踏板，这时自动泊车过程便会立即终止。 具备APP遥控驾驶的功能，在狭小的空间里也能一键实现挪车神操作，赋予了自动泊车新玩法和新内涵。

2.工作步骤

（1）扫描车位

（2）建立坐标系

（3）规划泊车路径

（4）执行泊车动作

以右侧平行车位的自动泊车辅助功能泊车入位工作过程可以分为5个步骤。

1）笔直向后倒车至转向点

2）向右转向调整车尾指向

3）回正倒车让车尾进入车位

4）向左转向让车头进入车位

5）回正完成倒车

车尾到前车某个位置打死方向，车头到前车某个位置回死方向。自动泊车控制单元好比把老司机的精准直觉全都存进了车载电脑，转向时交给车辆自己就好。

顺列式驻车要求汽车沿路边平行停放，与其它停好的汽车排成一条直线。顺列式驻车，自动泊车系统遵循以下五个基本步骤：

1）驾驶员将汽车开到停车位的前面，停在前面一辆车的旁边，启动自动泊车系统

2）自动泊车系统向路边转动车轮，以大约45°将车向后切入停车位

3）当汽车进入车位后，自动泊车系统会拨直前轮，然后继续倒车

4）当通过后视境确保与后面车辆保持一定距离后。自动泊车系统会向从路边打车轮，这是驾驶员需要将汽车泊入行进档，自动泊车则会将汽车前端回转到停车位中

5）驾驶员需要在停车位前后移动汽车，直到汽车停在适当的位置(约0.3米为止)

3.威廉希尔官方网站 特点

（1）自动停车系统APS 的启用需要满足一定速度条件。APS 对于车辆行驶速度有限制，当驾驶车辆沿道路行驶时，只要车速低于36公里/小时（30km/h，每款车型时速的设定值会有不同），系统就会认为驾驶者有停车意图，从而进行车位探测，车辆便开始利用雷达探头自动检测周围是否有合适的停车位置。

（2）具备侧方向泊车、垂直方向泊车功能模式中的一种或两种，以侧方向（顺列式驻车）泊车居多，还可附带自动驶出功能。除了侧方向泊车外，一般也必须支持垂直方向泊车。

（3）车位识别时对所需车位的长度或宽度有最小要求。在车辆进行车位识别时，会根据执行自动泊车所需车位的最小长度或宽度来判断车位是否可用。侧方向泊车的情况下，一般要求车位最小长度是车身长度的 1.2 倍（约车长 +0.8 m）；垂直方向泊车的情况下，一般要求车位最小宽度是车身宽度的 1.5 倍（约车宽 +0.8 m），才可确认该区域属于可停范围。

4.威廉希尔官方网站 要求 自动泊车过程大致可包含以下五大环节：环境感知、停车位检测与识别、泊车路径规划、泊车路径跟随、控制模拟显示；也可以分为三个部分：车位探测、路径规划和路径追踪，就是通常理解的数据采集、路线生成和泊车执行。 （1）环境感知是自动泊车系统中比较重要的一个功能，通过对停车场环境的探测，如寻找可用的停车位、自动泊车过程中两侧车辆的监测、确认车辆的位置信息和车辆的车身状态信息等，从而确保车辆自动泊车过程中的安全可靠。自动泊车系统环境感知主要通过超声波雷达进行监测，在车辆四周安装超声波雷达，让车辆在自动泊车过程中实时监测周边环境，让车辆避免剐蹭。

如下图所示,整套系统由12个超声波传感器和1个控制器组成，一种典型的超声波自动

泊车系统的

环境感知方案，由12个超声波雷达/传感器（红色圆点）组成，主要安装在车辆前后保险杠，8个超声波雷达/高精度超声波距离传感器，泊车过程中检测车身周边的障碍物，避免剐蹭；4个超声波雷达/长距离超声波距离传感器，泊车开始前进行车位的探测及在泊车过程中提供侧向障碍物信息。有的还有摄像头（黄色圆点）安装在前后保险杠及左右外后视镜。

传感器位置示意

（2）停车位检测与识别

车位探测就是利用超声波传感器等监测本车与路边车辆的距离信息，判断车位的长度是否满足停车要求。自动泊车超声波车位探测系统主要是由布置在车身侧面的超声测距模块构成的， 通过超声传感器对车辆侧面的障碍物进行探测， 即可完成车位探测及定位。

超声波车位探测的过程如下图所示。在使用超声波传感器探测车位时，车辆以一定的恒定车速V平行行驶向泊车位：

1）当车辆驶过1号车停放的位置时，装在车身侧面的超声波传感器开始测量车辆与1号车的横向距离D。

2）当车辆通过1号车的上边缘时，超声波传感器测量的数值会有一个跳变，记录此时时刻。 3）车辆继续匀速前进，当行驶在1号车与2号车之间时，处理器可以求得车位的平均宽度W。 4）当通过2号车下边缘时，超声波传感器测量的数值又发生跳变，处理器记录当前时刻，算得最终的车位长度L。 5）处理器对测量的车位长度L和宽度W进行分析，判断车位是否符合泊车基本要求并判断车位类型。 在车位检测过程中，需要车辆速保持在一定范围以内（通常是15-25km/h之间），这个时候车身两侧的超声波雷达开始工作，连续扫描车位两侧的泊车空间。 如图所示，当车辆3行驶到接车辆1时，右测雷达探测到车辆1 ，随着车辆3继续向前行驶，雷达测距在小范围内不断更新侧向距离；当车辆3驶离车辆1时，雷达测距发生明显跳变；当车辆3探测到车辆2时，雷达测距又会在小范围内变化……很明显， 车辆行程 Xa3 与 Xa2 之间的差值就是泊车位的长度，而侧向距离 Ya2与 Ya1之间的差值就是泊车位的宽度，这就是基于超声波测距威廉希尔官方网站 的车位探测的基本原理。

车位探测与识别的精准度

超声波传感器近距范围内不受光线影响，数据处理简单快速，易于做到实时控制，在测量距离、精度方面能达到工业实用的要求，但是存在波束角太大、方向性差、分辨率低、作用距离短等缺点。而摄像头具有数据获取量大、图像信息量大、可探斜侧面物体的优点，但受环境因素影响较大、运算量大。总之当前的车位探测和识别手段各有优缺点，如何进一步提升探测与识别的精准度是 APS 推广的主要威廉希尔官方网站 难点之一。

障碍物坐标检测及多目标识别

超声波传感器单纯的距离检测能力在泊车预警辅助场景已可满足使用要求，但是在智能化泊车应用场景、及多传感器融合应用中还远远不够。为此开发了障碍物坐标检测威廉希尔官方网站 及多目标识别威廉希尔官方网站 ，如下两图：

高精度车位检测及车位融合

基于超声波传感器可实现空间车位的探测、360环视摄像头可实现线车位的检测，同时结合超声波传感器及环视摄像头的障碍物信息检测，对车位进行多层次的融合，实现泊车位的高精度检测，大大提升了泊车场景的覆盖范围。如下图：

基于视觉的停车场车位状态检测威廉希尔官方网站

如下图，通过停车场安装的监控摄像头，基于深度学习算法，实现车位占用状态的实时检测，并将此信息上传至停车场车位管理后台服务器，为自主泊车车辆提供可泊车位信息。

（3）泊车路径规划

路径规划是APS 的重要内容，主要通过控制算法来实现。该过程分为三个阶段，分别是车位外起始位置调整、泊车入位和车位内姿态调整。

车位外起始位置要在控制算法中设定相应的距离、位置等条件，使得车辆位置满足泊车条件；泊车入位阶段要建立模型，进行合理的路径规划；在调整阶段，应该针对车身相对于车位的位置和姿态进行系统分析，制定车辆在车位内调整的方案，确保车辆符合条件。以上控制策略的实现，均需要大量的实际停车数据分析，并结合系统采集到的具体车位条件，将理论和实际结合，才能顺利实现路径规划。

路径规划是中央处理器根据汽车与目标停车位的相对位置等数据，得出汽车的当前位置、目标位置及周围的环境参数，据此规划计算出最佳泊车路径和策略。考虑到自动泊车实现原理，泊车路径规划一般尽可能满足以下要求：

1）完成泊车路径所需要的动作必须尽可能少，因为每个动作的精度误差会传递到下一个动作，动作越多，精度越差。

2）在每个动作的实施过程中，车辆的转向轮（绝大部分为前轮）的角度需要保持一致。因为系统是通过嵌入式系统实现的，而嵌入式系统的性能有限，转向轮角度保持一致能够将运动轨迹的计算归结为几何问题，反之需要涉及复杂的积分问题，这对嵌入式系统的性能是一个挑战。

路径规划就更加复杂了，它需要一套严谨的算法和建模过程，最终形成最佳路径和控制策略，才能进入到下一个执行环节--路径追踪。例如，几何路径规划法能够根据车辆与车位之间的相对位置，准确规划出无碰撞且完全考虑约束空间的泊车最佳路径，保障自动泊车的准确性和安全性。路径规划运算量庞大，并且运算过程极为复杂。

使用自动泊车辅助系统倒车入位的过程可分为如下几个阶段：测量泊车位长度、启动自动泊车辅助系统、在自动泊车辅助系统帮助下进行泊车。 自动泊车辅助系统在向驾驶人提供转向帮助之前，必须先对泊车位进行测量，并识别车辆相对于泊车位的位置。 即使自动泊车辅助系统未开启，传感器（G568和G569）仍保持工作状态。这样在车辆前行过程中，当车速低于40 km/h（平行泊车位）或低于20 km/h（垂直泊车位）时，两个位于车前端的传感器便会测量车两侧所有可停入的泊车位。这两个传感器的探测距离在4.5m左右。 通过上述方法还能找到并识别出在转弯处或弯道上的泊车位，与在笔直道路上没有差别。除车辆以外，系统还能识别到其它物体以及某一物体后的或是两个物体之间的泊车位。如果PLA没有识别出泊车位前面较小的物体，当车辆靠近这些物体时，会由泊车距离控制系统发出警告声。 无论泊车位在道路的左边还是右边，最后一个被测泊车位的数据会临时存储在自动泊车辅助系统的控制单元中。当发现新的泊车位或车辆已远离上一个泊车位（驶离平行泊车位超过15m，垂直泊车位超过8m）时，关于上一个泊车位的数据就会被删除。 若在有效范围内按下PLA按键开启了自动泊车辅助系统，记录在控制单元中的泊车位就会在组合仪表显示器上显示为在长方形阴影中的一段空白区域。下图可以帮助理解在道路右边测量泊车位的过程。

平行泊车测量如下图，符合要求的平行泊车位长度应大于车身长度加上机动距离与安全距离（前、后至少各留有0. 4 m ）。驶过泊车位时的车速要低于40km/h。车辆的最佳起始位置应在平行泊车位旁，处于行驶方向，且车辆侧面与已停放好的车辆之间的距离为0. 5～2. 0m。

测量垂直泊车位如下图，符合要求的垂直泊车位长度应大于车身长度加上机动距离与安全距离（左、右至少各留有0. 35m）。驶过泊车位时的车速要低于20km/h车辆所在的最佳位置应在垂直泊车位旁，处于行驶方向，且车辆侧面与已停放好的车辆之间的距离为0. 5～2. 0m。

一般平行泊车和垂直泊车采用如下图所示路径。

平行泊车分为单次和多次：单次为如下图所示路径一次泊车完成；

多次则为当车位长度比较小时，可采用多次“揉库”的方法泊车。                       

轨迹动态规划威廉希尔官方网站                                                                                             泊车过程中有诸多不可控因素，如转向系统执行速度与精度问题、参考障碍物位置变动问题等，导致在泊车过程中出现泊车轨迹偏离路径规划轨迹现象。为此开发了泊车轨迹动态规划威廉希尔官方网站 ，可实现泊车过程中的轨迹实时修正甚至轨迹重规划，如下图：

室内定位威廉希尔官方网站 如下图，通过采用视觉SLAM+标签辅助定位方式，解决地下停车场无GPS的问题，同时通过多源信息融合，提升定位精度。

全局与局部路径规划威廉希尔官方网站

如下图，基于A*算法实现任意两点间的全局路径规划，支持路径规划重置、选路以及速度规划功能，同时结合实时环境感知状态，进行局部路径规划，实现紧急制动、 跟车巡航以及换道避让、 换道超车等自主决策。

基于改进深度强化学习的自动泊车路径规划

提出一种基于深度强化学习的运动规划方法，以车辆位姿、方向盘转角和与障碍物的最小距离作为状态，以目标方向盘转角作为动作，通过Pytorch搭建了基于深度强化学习的泊车算法框架。设计基于引导的奖励函数以避免奖励稀疏问题；以回合平均奖励作为优先级，将经验池改进为基于优先队列对样本进行存储和淘汰；针对泊车问题，提出了基于课程学习的分阶段训练方法，加速算法收敛。仿真结果表明：提出的算法较原始算法收敛速度提高25%，完成训练的智能体具有较强的规划能力和健壮性，规划成功率达到90.6%，同时具有良好的舒适性和安全性。

0 引言

自动泊车的运动规划采用的方法可以分为基于规则的方法和基于学习的方法。与上述规划方法相比，深度强化学习具有求解能力强、能够自主探索等优点，许多研究学者和机构利用深度强化学习解决控制问题，收到了良好的效果。虽然基于深度强化学习的自动泊车路径规划有着良好的应用前景，但是对其在理论和应用层次研究较少，尚未有投入产业化的成功案例，要想大规模的应用和推广，仍存在探索效率低下、收敛困难等问题亟待解决。

强化学习算法能够自主探索获得样本，但由于训练初期策略随机，无效样本众多，成功样本对神经网络权值改变的贡献容易被淹没，导致样本的利用率不高，甚至无法收敛。针对固定位置，不同航向角的起始位姿，利用人工泊车的控制序列，对智能体（本文中车辆、智能体、算法模型等名词视语境具有相同含义）进行预训练，让智能体在初期不需探索就获得高回报值的样本。将失败和成功的探索经验分别存储，并设置随训练回合数变化的采样比例，让智能体始终能从成功样本学习经验。借鉴AlphaGo中的蒙特卡罗树搜索方法，生成泊车数据，并用奖励函数评价数据质量，筛选出最优数据训练智能体，避免了随机探索时低质量数据对智能体的影响。以TD误差作为样本的优先级，利用SumTree数据结构存储样本，基于优先级采样，使对梯度计算贡献较大的样本更容易被采样。在研究车辆高速智能驾驶的决策控制时，将探索策略分为车道保持探索策略和超车避障探索策略，在原始动作的基础上，加上基于改进策略的修正值，从而减少无效探索。

在强化学习中，奖励函数的设计直接关系着模型能否收敛。针对机器人路径规划，设计了仅包含碰撞惩罚和到达终点的奖励函数，属于奖励稀疏问题。分别用稀疏奖励和稠密奖励训练智能体，结果表明用稠密奖励训练得到的智能体有着更高的泊车成功率。针对室内移动机器人的路径规划问题，将奖励设计成超出规定时间－0.05，防止机器人因为胆怯而待在原地不动。将奖励设置为车辆当前位置与目标位置的距离的负数，在引导车辆向目标位置靠近的同时，催促智能体尽快到达。另外有一些学者，从改进训练方法的角度改善深度强化学习算法的收敛性。基于课程学习，通过逐步加上障碍物的训练方法，加速收敛。课程学习由机器学习领军人物提出，其精髓是根据先验知识，为模型设置一系列从易到难的课程，加快收敛速度。采用固定航向角离散化训练的方式，先训练航向角为30°的工况，待收敛后再逐步扩展到初始航向角为0°～90°，这与课程学习先易后难的思想不谋而合。

综上，到目前为止，基于深度强化学习的自动泊车路径规划算法仍存在某些不足。在训练过程中，智能体的学习效率不高，收敛速度较慢。强化学习需要通过智能体基于当前策略与环境交互获得学习所需的样本，而样本的质量又会影响策略更新，二者相互依赖，算法容易陷入局部最优。与机器人相比，汽车为非完整系统，横向和纵向耦合，且泊车可行使空间狭小，对于给定初始条件，泊车路径和控制序列十分稀疏。为了降低学习难度，常用手段就是固定起始位姿训练、放宽泊车空间限制，但这也导致训练得到的智能体相比传统规划方法规划能力不强，无法满足自动泊车的实际应用需求。若能对上述不足做出有效改进，将对基于深度强化学习的自动泊车方法有积极推动作用。首先介绍基于深度强化学习的自动泊车运动规划方法，在考虑收敛性和稳定性的基础上做出改进，通过搭建仿真平台训练智能体，然后从健壮性、规划能力、安全性等多角度分析评价智能体表现。

1 建立车辆动力学模型

1.1 深度强化学习算法模型

强化学习是马尔科夫决策过程。智能体（agent）基于当前状态s，选择动作a、环境返回奖励r和下一状态s′，通过不断地尝试，智能体学习到最优策略。基于Ator-Critic框架的深度确定性策略梯度（deep deterministic policy gradient，DDPG），在确定性策略梯度（deterministic policy gradient，DPG）的基础上，结合DQN（deep q-learning）算法的优点，包括双网络、经验回放池等，在很多问题上有着不俗的效果。

在传统的强化学习中，基于价值的方法用表格记录所有动作价值，但对于连续状态空间，状态数量庞大，若使用表方法会导致维度灾难。因此，采用神经网络对动作价值Qπ（s，a）近似：

式中，w代表了神经网络的权重。

同理，车辆在泊车时对控制精度要求很高，离散动作无法满足，故对策略也用神经网络（actor网络）近似，如式（2）。在网络输出基础上加上奥恩斯坦－乌伦贝克噪声（ornstein-uhlenbeck，OU噪声），在训练前期增加探索性。该式刻画了状态空间到动作空间的一种映射，给定某一状态，输出最佳动作。

在DQN中，critic网络的损失函数定义如下：

由式（3）可以看到，critic网络的梯度更新依赖actor网络计算的动作值和自身网络计算目标Q值，而actor网络的梯度更新又依赖critic网络计算的Q值，2个网络之间，以及目标值和当前值之间相关性太强，导致算法不稳定。为了降低此相关性，为actor网络和critic网络创造了一份副本，即目标critic网络和目标actor网络，用来计算目标动作和目标价值。改进后的当前critic网络和当前actor网络损失函数如式（4）所示，其中当前critic网络梯度更新改为依赖于目标actor网络计算的动作值和目标critic网络计算的目标Q值。

目标网络以缓慢跟踪当前网络的方式进行更新（软更新），如式（5），α为当前网络权重，α′为目标网络权重。目标价值在短期内可看作恒定，类似于监督学习中的样本标签，大大改善了学习的稳定性。

1.2 车辆运动学模型

在泊车的低速工况下，不考虑轮胎的侧滑，车辆非线性状态空间模型如式（6）所示。其中x、y为车辆后轴中心坐标，航向角θ为车辆纵轴线与x轴夹角，（x，y，θ）为车辆位姿；v为车辆后轴中点的线速度，L为轴距，δ为前轮转角。

1.3 强化学习要素定义

车辆在某一时刻的状态，需要具有明显的区分性，能够表征车辆与环境的关系，最好与控制量相关。选择车辆的x、y坐标，航向角θ，方向盘转角sw，车身与周围障碍物的最小距离d作为状态：

在泊车时，与纵向控制相比，横向控制更加重要，因此将纵向速度设为恒定值。动作a定义为下一时刻的目标方向盘转角sw target，其区间为［－540，540］。同时为了保证运动舒适性，避免方向盘转角变化过大，在输入到车辆运动学模型时，将方向盘转角变化量限制在20（°）／Δt：

基于Pytorch框架，建立了自动泊车仿真平台算法框图，如图1所示。整个框架分三部分组成：一是智能体与环境的交互过程，智能体（也就是车辆）根据当前车辆状态s，确定方向盘目标转角a，加上OU噪声后输入到车辆运动学模型中，计算返回下一状态s′给智能体，循环往复直到车辆与障碍物碰撞或者完成泊车。二是经验池储存样本的过程，每次智能体与环境交互后，奖励函数根据下一状态s计算奖励r，然后将（s，a，r，s′）元组储存在经验池中。三是智能体的训练过程，从经验池中随机采样一批数据，计算当前critic网络和当前actor网络的损失函数并进行随机梯度下降，通过软更新的方式更新目标critic网络和目标actor网络的参数。

图1 泊车算法框图

2 泊车算法的三点重要改进

2.1 基于引导的奖励函数

对于泊车任务，智能体的唯一目标就是运动到目标位姿，此时应给予智能体较大的奖励，防止被其他部分奖励掩盖。为保证泊车的准确性，仅当车辆运动到目标区域时，认为泊车完成，获得最终位姿奖励R target，包括基础奖励R base和航向角偏差奖励。

为满足泊车的安全性，避免与其他障碍物碰撞，需要设置碰撞惩罚R collision。在无碰撞时，给智能体一个较小但为正的奖励值Rc max，鼓励智能体探索未知区域。当车身与边界的最小距离d小于安全距离d safe时，则会受到相应的惩罚。

在很多简单任务（如OpenAI的倒立摆、登山车任务）中，仅有上述主线奖励和碰撞惩罚（稀疏奖励），智能体就能够在足够的探索中发现任务的目标。但泊车路径规划本就是高难度探索问题，即使随机探索一百万次，也未必有一次能顺利到达终点，稀疏奖励显然无法适用。为了使智能体时刻获得反馈（稠密奖励），引导智能体朝着目标位置运动，增加引导奖励。基于距离的引导奖励可以考虑以下两种形式：

上述2种基于距离的奖励，尽管通过系数k进行缩放，在距离较远处和距离较近处的奖励仍会相差过大（1～2个量级），导致智能体过于“贪婪”（在原地打转以获得更多收益），或者“鲁莽”（为了不再持续受到负奖励而选择碰撞障碍物以结束回合）。智能体需要更加平稳的引导奖励，因此，将引导奖励R guide设置成由车辆后轴中心与目标位置连线和航向角的夹角计算得出，如式（12）所示，任何夹角都能获得正的奖励，且夹角越小（车辆靠近目标位置的速度越快），奖励越大。使智能体既能学会靠近目标位置，又能长远考虑，为了到达目标的奖励而选择绕远路（避免碰撞）。

2.2 基于优先队列的经验池设计

在大规模训练时，大部分无效探索会产生数量众多的失败样本，将成功样本对网络权重的更新淹没，影响智能体学习。以回合平均奖励作为优先级，设计基于优先队列的经验回放池Arr。随着训练的进行，经验池不断更新，失败样本的数量将逐渐减少，更高比例的成功样本用于网络训练，加速模型收敛。优先队列本质是完全二叉树结构，能自动按优先级排序，其首元素的优先级总是最小，能够方便地淘汰掉低优先级的样本。经验池更新流程如图2所示，先将智能体与环境互动产生的样本存储在临时数组T中，在完成一个回合的互动后，按式（14）计算回合平均奖励，作为该批次样本的优先级P，将T中所有样本以（P，（st，at，rt，st＋1））的格式储存到经验池中。若经验池满，则通过比较Arr中最小优先级与P的大小，淘汰其中一个样本。

图2 经验池更新流程框图

2.3 基于课程学习的多阶段训练

泊车训练时的成功样本稀少，随机的初始位姿得到的样本序列会对智能体学习到泊车策略造成干扰，但对初始位姿进行随机，又是规划能力和泛化性的必然要求。因此，参考课程学习的思想，按照表1所示的随机范围，将智能体的训练过程分为4个阶段，先从固定起始位姿训练智能体；然后在区域内随机生成起始位姿，并逐阶段扩大初始位姿的随机区域。如此，智能体在学习到从固定位姿到达目标位姿后，在这基础上再挑战从不同的起始位姿运动到目标位姿，减少探索的盲目性。

表1 随机范围

改进算法的伪代码如下：

1：随机初始化当前critic网络Q（s，a｜w）和actor网络μ（s｜β），通过w′←w和β′←β初始化目标critic网络和actor网络

2：初始化基于优先队列的经验池B

3：for（K，A）∈｛（K0，A0），（K1，A1），（K2，A2），（K3，A3）｝do

4：for episode＝1，K do

5：从区域A中随机生成初始位姿并赋予初始状态s0

6：初始化临时数组temp，用来存放该回合的所有样本

7：for t＝1，T do

8：利用式（12）生成动作at，观测奖励rt和新的状态st＋1

9：将（st，at，rt，st＋1）存储到temp中

10：从经验池B中采样一批训练数据

11：利用式（13）分别计算当前critic网络和actor网络的损失函数，并进行梯度更新

12：利用式（14）更新目标critic网络和actor网络

13：end for

14：利用式（14）计算temp数组中所有样本的优先级P，并转移到经验池B中

15：end for 16：end for

3 泊车算法的收敛性、泛化能力、健壮性和规划能力分析

3.1 收敛性分析

为对比改进方法与原始方法的收敛性，进行10次测试，每100个回合作为一个轮次，测试当前模型的规划能力，平均成功率和平均回合回报的变化曲线如图3、4所示。

图3 成功率

由于是在范围内随机生成初始位姿，无法保证生成的每个位姿都“确实”有满足要求的泊车路径，因此成功率只能逼近100%。由图3可以看出，改进方法在训练到第8个轮次时（第800回合），已经有成功泊车样例，相比原始方法，提前400个回合（提高33%）；相同的训练回合数，改进方法的成功率始终比原始方法高出不少；最终二者成功率都收敛到略高于80%，但改进方法要提前1 000个回合（提高25%）。图4和图3所示的规律基本一样，训练相同回合数，改进方法的平均回合回报要高于原始方法，说明改进方法泊车成功的次数更多。

图4 平均回合回报

初始位姿为（6，7.5，0）时的泊车运动轨迹如图5所示，方向盘转角曲线如图6所示。一开始车辆先往左转方向盘，往远离障碍物的方向行驶一段距离，从而使泊车空间更加充裕，然后再往右转方向盘到最大，待后轴中心越过角点连线后，便开始回正方向盘，最后进行微调。由于方向盘转角变化量的限制，单位时间车辆的方向盘转角变化量为200（°）／s，不会令人感到不舒适。

图5 泊车轨迹

图6 方向盘转角

3.2 泛化能力分析

在深度学习中，泛化能力指机器学习算法对新样本的适应能力。在自动泊车中，智能体学习的目的是学习到泊车问题的目标、注意点和解决策略，即从任意初始位姿开始，无碰撞地倒车进入车位中。泛化能力不好的算法，只是“记住”从某一初始位置到目标位置的固定路线，当将初始位置稍微移动一小段距离时，则无法规划出路径。而本文算法在训练后期对初始位姿进行随机，让智能体“见识”了更加丰富的样本，其泛化能力得到保证。

为展示经过训练的智能体的泛化能力，在表1的最终随机范围A3之外，选取4个有代表性的初始位姿（如表2）进行泊车测试，泊车轨迹如图7～10所示。

图7 组1（5.0，7.2，－0.2）的泊车轨迹和方向盘转角

表2 初始位姿

由泊车轨迹图可知，4个测试均泊车成功，没有与障碍物碰撞。上述4组起始位姿均位于最终随机范围A3之外，属于智能体未曾“见过”的数据，但得益于神经网络强大的近似能力，智能体仍然能够在初始阶段选择正确的动作执行。可以认为智能体确实学习到了合理的泊车策略，能够感知当前车辆处于何种境地；根据当前状态采取合适的动作；在越过角点连线后，修正航向角，最终以较小的误差停在目标位置。

图8 组2（4.5，8.0，0）的泊车轨迹和方向盘转角

图9 组3（7.6，8.5，0）的泊车轨迹和方向盘转角

图10 组4（7，10.2，0.3）的泊车轨迹和方向盘转角

3.3 健壮性分析

在实际驾驶时，传感器（如惯性导航、方向盘传感器、距离传感器等）的状态感知和控制器对车辆的速度控制和方向盘转角控制都存在一定误差。为了验证智能体具有较强的健壮性，能够抵抗感知和控制误差的干扰，用高斯噪声代表随机扰动，施加在状态s（x，y，θ，sw，d）和动作a上，如式（15）所示。X代表变量值。由高斯分布可知，扰动值位于区间［－2σ，2σ］的概率为95.45%。

如表3，参考实车感知系统和控制系统的精度，分别设置4组不同的随机误差进行泊车实验，得到泊车轨迹和方向盘转角曲线，如图11、12所示。可知，随机高斯噪声的方差不同以及生成扰动值的不确定性，导致同一方差不同次测试和不同方差测试的车辆泊车轨迹和方向盘转角变化都不尽相同。方差越大，轨迹偏移的最大值越大，车辆需要采取更大的方向盘转角以对抗偏移，尤其是在进入车位微调位置时，该特征尤为明显。尽管受到了较大的随机干扰，导致车辆在相同时间步的状态不同，采取动作后的状态不如预期，智能体仍然能根据当前状态实时调整方向盘转角，最终运动到目标位置。

表3 随机噪声方差设置

图11 不同噪声的泊车轨迹

图12 不同噪声的方向盘转角

为进一步分析随机噪声对模型的泊车能力是否造成影响，在A3区域内随机生成10 000组初始位姿，统计不同组别误差的泊车表现，见表4（其中

按式（16）方法计算）。在随机扰动较小时，泊车成功数和x方向误差基本不受影响，但航向角误差逐渐增大。当后轴中心y方向超过目标位置，泊车完成，因此y方向误差均小于最小步长0.1 m且相差不大。组4的成功率为67.3%，下降比较明显，x方向误差和航向角误差增大，说明随机噪声的干扰对智能体造成了明显影响。结合噪声方差设置可知，航向角噪声方差增加幅度很小，说明智能体的泊车性能受航向角噪声的影响最大。这是因为航向角的单步变化量最大仅为1.2°（在方向盘转角达到最值540°时由式（6）计算得到），而组4的航向角噪声方差为3°，相当于每个时间步车辆的实际航向角变化都远小于随机扰动。

表4 加上噪声与未加噪声的规划性能指标

3.4 规划能力分析

在分析规划算法的规划能力时，可从耗时、成功率、路径长度、容差、泊车误差、路径平滑度等方面考虑。其中容差指在泊车过程中车辆与周围障碍物的最小距离，容差越大，安全性越高。路径平滑度可用曲率变化衡量，曲率变化越大甚至趋于无穷，则路径越不平滑。对于离散点表示的路径，用式（17）计算连续两点曲线的曲率。

Reeds-shepp规划算法［］（RS）作为经典的基于几何的路径规划方法，具有实现简单、求解速度快等优点，许多专家学者对RS进行了研究和改进，并应用在实际的泊车系统上。RS曲线是在车辆能够前进和后退时，满足几何约束和固定曲率的条件下，连接起始位姿和终止位姿的最短路径。其分为5大类，共48种可能的解，每条路径最多由5段组成。图13为RS曲线中的CCC（curvecurve-curve，3段圆弧）情况，原点S为起点，G（x，y，φ）为终点。由几何关系可得方程组（20），其中t、u、w为每段圆弧的圆心角，从而求得每段圆弧的长度。

图13 RS曲线的CCC类示意图

RS算法的应用逻辑就是给定起始位姿和终止位姿，依次求解48种情况（可能无解），对可行解进行碰撞检查，然后利用代价函数从无碰撞的路径中择优选取一条路径输出。由于RS曲线仅由半径相同的圆弧和直线组成，对于一组起始位姿和终点位姿，解只有48种可能，考虑障碍物的话情况更加糟糕，这导致RS算法规划能力不强。与之相对的是，基于深度强化学习的路径规划算法，其路径由许多小片段组成，每个片段的曲率都在变化，解的数量可以认为是无限个，因此具有更加强大的规划能力和泛化能力。因此，对二者进行规划能力上的对比，以此说明基于深度强化学习的路径规划算法的优越性。

在区间x∈［2，7.5］，y∈［7，10］，θ∈［－0.2，0.2］内，随机生成一组起始位姿，对改进算法训练得到的智能体和RS算法进行测试，各指标结果如表5所示。可以看到RS算法的平均耗时为4.3 ms，最终位姿误差为0，但成功率仅有3.0%，而本文算法成功率高达54.2%，耗时和误差并未相差多少。由于RS曲线在切点处曲率不连续，相比本文基于车辆运动学模型得到的规划器，平滑度要高很多。

表5 规划能力指标

将2种算法泊车成功的起始位姿绘制成散点图（图14）。

图14 可泊区域起始位姿散点图

可以看到，在A3区域内，改进算法的可泊起始位姿均匀且密集地覆盖整个区域，而在A3之外，起始位置的分布相对稀疏，因为在这些区域单凭一次倒车很难完成泊车，需要至少揉库1次，而本文设计的车辆暂未考虑前进后退的切换功能；能够泊车成功得益于初始航向角的偏移。即使如此，改进算法的可泊区域仍要比RS算法大得多，且可泊起始位姿更加密集。这也说明RS算法的泛化能力不足，某个位姿可以泊车，但在该位姿的邻域内，则无法泊车。

4 结论

1）研究了深度强化学习算法模型的原理和特点；基于阿克曼转向几何建立了车辆运动学模型；以车辆位姿、方向盘转角和与障碍物的最小距离作为状态，以目标方向盘转角作为动作；搭建了基于深度强化学习的自动泊车算法框架，分析了框架中智能体与环境交互过程、经验池储存样本过程和智能体训练过程的运行逻辑。

2）以车辆后轴中心与目标位置连线和航向角的夹角作为衡量，设计了基于引导的奖励函数，避免奖励稀疏带来的收敛困难问题；为切断样本之间的时间相关性，以回合平均奖励作为优先级，设计了基于优先队列的经验池结构对样本进行存储和淘汰，并阐述了相应的经验池更新流程；针对泊车问题，提出了基于课程学习的多阶段训练方法，将智能体的训练分为4个阶段，逐渐扩大初始位姿的随机区域，加速模型收敛。

3）结合实例，研究了改进算法的收敛性、泛化能力、健壮性和规划能力。仿真结果表明，相较于原始算法，改进算法收敛速度加快了25%，二者收敛后的成功率基本相同。经过训练得到的规划器具有较强的健壮性，随着高斯噪声的方差增大，智能体的泊车轨迹偏移增大，泊车成功率逐渐减小，但最终位姿几乎不受影响，即使在x、y方向噪声极值达到10cm，距离误差达到20cm时，泊车成功率仍达67.3%。与传统的基于几何规划的RS曲线方法相比，基于改进算法训练得到的智能体有着更强的规划能力和泛化能力，有着更大的可泊区域和更加密集的可泊初始位姿。

（4）泊车路径跟随控制

路径追踪主要是执行路径规划，将相关策略转化为电信号传达给执行器，依据指令引导汽车按照规划好的路径泊车。该过程为通过车载传感器不断探测环境，实时估算车辆位置，实际运行路径与理想路径对比，必要时做局部校正。

泊车入位过程控制

泊车入位是APS执行机构按照路径规划控制车辆进入车位，是APS的重要环节。路径规划是在传感器测量的距离信息的基础上制定的，但是其测量结果受环境影响较大，容易形成误差。因此，在泊车入位的过程中，应该注重对车辆入位过程的实时控制和调整，确保对环境数据的及时更新和对路径的及时调整。

自动泊车路径规划和跟踪

展示了自动泊车系统（APAS）的两个主要组成部分。APAS对周围的环境建模并检测车辆可以停车的车位。APAS的两个最主要任务是：1. APAS必须计算可行的路径 2. 基于驾驶员输入的车速和路径通过电子转向助力系统（EPAS）控制转向角度以闭环的跟随这一路径，展示了连续曲线路径规划问题和基于时标跟随控制的解决方案。这些部件是实现乘用车APAS商业化应用的一部分。

关键词：连续曲线路径规划，跟随控制，时间标记

1. 介绍

不讨论停车位探测步骤，距离测量相关的信号处理，和车辆位置及方向计算问题。停车位和可行路径的计算由超声波雷达来实现，而路径的跟随则使用车上ABS传感器的速度和角度数据（这里有个问题：角度应该是EPS或者方向盘转角传感器发出，不是ABS。

要设计一个APAS，需要车辆的数学模型。文献中给出了许多车辆运动学和动力学模型，进行泊车操作时的低速车辆运动学模型是一个完善的方法。

如果车辆模型已知，那么路径规划方法和跟随控制算法可以依据运动等式设计。运动规划可以使用确定性或概率性方法。为了获取连续曲率路径，需要使用特殊曲线。基于软计算法的算法也可以计算参考路径。

半自动系统的跟随控制会比全自动系统更复杂。全自动模式的跟随控制使用两个输入影响车辆行为而半自动模式少一个输入，即半自动模式的车速由驾驶员控制，其纵向速度和用于路径规划的参考速度有很大不同。这个问题通过时间坐标解决。

目标是开发一个可以同时以半自动和全自动模式运行的泊车辅助系统，并需要满足三种不同的泊车情形。（侧方位泊车，垂直车位泊车和斜向车位泊车）

首先给出了系统的部件（第二章）。第三章为车辆动力学模型，路径规划和跟随控制算法的细节在第四-六章。第七章给出了实车基于测试的结果和全文的简要总结。

2. 系统部件

为了保证自动控制，需要执行很多任务：系统应当能检测到环境中的障碍物；检测或估算它的位置和朝向；规划参考动作；最后尽可能高精度的跟随参考动作。这些任务由图一中独立且互相连接的子系统共同实现。

车辆的ABS传感器探测车轮的位移。基于这些数据计算和假设车辆在世界坐标系中的实际位置和朝向。该假设状态用于环境建模和控制模型。为了建立环境模型，还需要环境的数据。超声波传感器用于测量周边障碍物的距离。基于这些距离，可以建立起环境模型。然后使用简单的算法来寻找环境模型中的可泊车区域（如果有的话）。

图1：自动泊车辅助系统部件

运动规划会先计算出一条参考路径，参考路径是连接初始位置和终点的曲线。在规划阶段需要考虑存在的约束（如：模型描述的非法规特性，碰撞避免，执行器信号的最大值等）最终，使用跟随控制算法来跟随参考路径。

3. 车辆动力学模型

路径规划和跟随控制算法都需要用到车辆动力学模型。为了计算路径几何，需要稍微扩展模型以确保连续曲线的特性。

如文献中常见的，车辆的参考点是后轴中心点，用R表示。车辆配置（q）由四个状态变量描述：参考点位置R(x,y)，车辆的航向ψ，曲率κ（转向半径的倒数）。假定阿克曼转向理论成立，则车辆的动态可以用位于车辆中轴的二自由度模型来描述（见图2）；

车辆的纵向速度由v表示，曲率的变化速度为σ，轴距用b表示。这样我们可以得到曲率及其导数的以下关系。

图2 x-y平面上的车辆动力学模型

δ是二自由度模型前轮方向和车辆纵轴之间的夹角。曲率和曲率导数都有限制，如：

为了设计跟随控制，我们使用公式（1）的简化版本，其中减少了曲率。

由于纵向速度由驾驶员产生，不受控制器影响。因此半自动系统的（跟随）控制没有使用车辆的纵向速度作为系统输入。使用vd表示驾驶员控制的车速。（我们假定速度vd可以被测量或估计。）

4. 路径规划

路径规划方法的任务是决定参考路径的几何参数，目标是获得连续曲率的曲线，以避免车辆在车轮转动的时候必须停车。其它的约束包括曲率的上限和对时间的导数。由于车辆无法以任意小的转弯半径转向，且转向半径（或曲率）变化的速度也受到电控转向系统的限制。

为了规划一条满足以上约束的路径，使用三种不同的路径原型，即直线，圆弧线段和连续曲率曲线（CC turns）。直线的曲率为0，圆弧线段的曲率是非零固定值，且不超过给定的上限，连续曲率曲线的曲率则线性变化。

如果参数已知的话（如长度，半径），直线或者圆弧线段的几何很容易描述。连续曲率曲线的参数计算要复杂的多。如果运动的初始状态时q0 = [0, 0, 0, 0]T，速度曲线时固定值（如v = 1），曲率变化小于允许的最大值σmax，那么连续曲率弯的特征由以下等式描述：

CF 和SF 表示菲涅尔积分，其不可以用近似形式表达。求微分后可以看到这些等式满足（1）所给出的车辆动力学模型。

对公式（6-9）进行简单的数学运算，可以得到不同初始位置和任意固定速度的运动方程。如果（6-7）中菲涅尔积分的值可以预先计算，那么剩余的计算可以实时进行。

现在来讨论如何使用这三种路径原型来获得平行泊车车位的参考路径：使用七段线段来组成整个路径（见图3）。在保证普遍性的前提下，假设车辆从qs = [0, 0, 0, κs]T 初始状态向后移动。首先它以连续曲率曲线移动，直到达到最大曲率（或者最小转弯半径）。然后以最小转弯半径沿着圆弧转动，转动的角度以ψ1 表示。再经过一段连续曲率曲线，曲率变为0。接着车辆通过连续曲率曲线转向一个相反的方向，再经过一段转动角度为ψ2 的圆弧和另一段连续曲率曲线，曲率再次回到0。最后路径以一条长度为l的直线结束。

这样一个路径包含三个参数：圆弧线段的转动角度（ψ1，ψ2）和直线的长度（l）。如果这些参数已知，则可以计算路径的几何和目标位置。

图3：平行车位泊车的参考路径

路径参数由目标位置状态（qd）确定，可以通过解等式qd = qg得到。所以完整的参考路径可以计算得出，并且状态(qref = [xref ,yref ,ψref ,κref ]T )的参考值和时间导数也可以确定。

5. 时间缩放

路径规划时假设速度是固定值（如 v=1）以避免复杂的计算。即使驾驶员会产生另一个速度（vd ≠ v )。也足以跟随参考路径的几何。路径的时间分布会实时的适应驾驶员产生的车速。

在上面的等式中状态q 是时间t 的函数，t 的导数是1。通用的表达等式如下

u 是输入的向量，w 表示外部信号，在当前的例子中，由公式（5）给出，u = δ 而 w = vd。

引入一个新的时间缩放（单位），由τ 表示，τ 可以用来修改沿着路径的时间分布。建议t 和τ 的关系不仅仅取决于车辆的状态，也同时取决于一个新的外部输入，由us表示，称为坐标输入：

使用时间缩放，公式（11）可以表示为时间τ相关：

上标表示对τ的微分，τ‘=1。

（12）中时间坐标的定义需要满足以下条件：

- τ(0) = t(0) = 0，原始和缩放后的轨迹要从同样的初始状态开始；

- τ’ > 0，因为时间不可以停止和回朔。

由于时间缩放我们修改了沿着路径的时间分布，使其按照时间τ分配：

更高阶的导数和其他状态变量可以用类似的方法计算。通过（14）可见，当τ‘ ≠ 1时，时间缩放不会改变参考路径的几何形状，仅仅是速度和更高阶导数会变化。

6. 跟随控制

仅讨论跟随控制的关键特征。给出了一些两输入车辆动力学（4）控制解决方案。这些方法确保了参考路径的指数跟踪。在单输入情况下，算法需要修改才可以使用，因为控制器无法影响车辆速度。想法是通过时间缩放输入来补偿缺失的速度输入。

可以使用以下的时间缩放方程。

（5）中给出的模型等式随着t的演变可以变形为使用缩放时间，这样我们得到：

这个缩放模型现在有两个输入（us和δ），选择参考文献中的一个控制器做跟踪，它会依据缩放的参考路径和实际路径的跟随误差计算δ 和us。公式（14-16）要求的时间缩放函数及其导数，可以使用以下基于（17）的关系式计算：

如果信号us 和ud 是一样的，那么时间缩放函数满足τ‘  > 0。若其中一个信号等于0，车辆是不可控的。这种情况一般发生在运动最开始和结束的时候。

闭环控制机制如图4。首先，路径规划模块计算以τ 为参数的参考路径。第二步，基于车辆的纵向速度vd 缩放该参考路径。vd 由驾驶员输入，且和由控制器计算的缩放输入us相关。时间缩放后我们得到一个基于t 的缩放参考路径。控制器根据实际和缩放参考路径的误差确定它的输出。这样车辆的输入是驾驶员产生的纵向速度vd 和控制器计算的δ，输出是车辆的位置，包括参考点坐标和朝向。

图4：带时间缩放的跟随控制机制

7. 结果

在某轿车上应用了所展示的方法（见图5），该车使用蒂森克虏伯的EPAS来实现前轮转向。路径规划和跟随控制在一个固定在车上，且连接到EPAS执行器和CAN总线的dSPACE硬件（专用Autobox）中运行。

示例的运动是一个向前泊入的操作，结果如图6。系统能够跟随参考路径，车速是非固定的。

图6：左图 -  XY平面的车辆路径

右图 - 车辆vd和δ对时间的曲线

8. 结论

讨论了一个APAS系统的两个部分，所示的路径规划方法可以实时计算连续曲率曲线。时间缩放跟随控制在驾驶员输入车速时跟随参考路径。时间缩放函数由车速和一个缩放参数计算得出，缩放参数由控制器基于闭环特性计算。展示的算法在车上做了测试，取得了令人鼓舞的结果。

（5）模拟显示

在自动泊车过程中，还会由传感器反馈构建泊车环境模拟，给驾驶员进行提示与交互，提示用户处理器意图，让驾驶员在车辆自动泊车过程中可以进行必要的操作，并可以在车辆自动泊车过程中发现问题时立刻介入自动泊车过程，从而避免危险的发生。另外路径规划后进行泊车时为了知晓处理器定位和计算路径运行情况，需要将这些处理器信息反馈给用户。如果处理器获取环境信息或者处理过程中出现重大错误，用户可以及时知晓与停止。

编辑：黄飞