什么是自动泊车系统？自动泊车路径规划和跟踪技术分析

一 前言

随着城镇化水平的不断提高以及经济水平的快速增长，人们对汽车的需求量日益旺盛，汽车保有量持续增多。汽车的出现满足了人们的出行需求，但现阶段汽车的行驶永远离不开驾驶员对汽车的操作，到达目的地后，停车成为每个驾驶员需要面对的问题。下班驾车回到小区停车场，在密密麻麻的停泊车辆中忽然发现了一个停车位，带着惊奇与庆幸将汽车开到停车位旁，这时却忽然发现旁边两个汽车停得与空置的车位靠较近，凭借笨拙的车技，根本无法将汽车停进去，只能眼看车位空瞪眼悔科二忘天边。

在各种环境下想要完美的将车停到车位中，是一件并不容易的事情。无论是侧方位停车，还是倒车入库，都是需要每个驾驶员熟练掌握的技巧，而实际情况却远不是想象的那么美好，“停车”这一简单操作，却成为了很多驾驶员的难题。对于驾驶新手来说，开车的过程中最困难的事情是什么？百分之八十的答案都是泊车入库。许多刚出“茅庐”的驾驶新手对于侧方位停车十分的头痛，是一种痛苦的经历，有时侧方车位前后距离太小、有时后方车位前面距离太窄，都让许多新手们为难不已。多倒几次车事小，有时还会出现刮蹭，让人十分恼火。

把又长又大又笨重的前驱车停好，也是一件有点困难的事。前轮驱动的车辆发动机一般是横置安装的，车宽的很大一部分都由发动机占据，所以就算把方向盘打到底，轮胎的转动幅度也不会很大，在低速情况下想要改变方向很难，车体越大，这一点感受得越明显，无论驾驶技术多好都没法避免。由于车辆和人均受自身条件影响而存在“视觉盲区”，泊车往往耗费大量的时间和精力，一直是新老司机的驾驶痛点。

随着城镇化发展的不断加快，交通拥堵现象愈发凸显，城市停车位资源紧张，停车位空间小等问题，停车难题不断发酵。由于城市停车位数有限而车辆数量众多，对于“老司机们”来说，想要找到合适的停车位已经越来越困难，在较大停车场停车容易产生找车位难、找车难等问题，驾车出行变得愈发受到挑战；与此同时大城市停车空间有限，特别是市中心拥挤的停车空间，狭小的停车空间时刻考验着他们的停车技术，将汽车倒入狭小的停车位成为一项必备技能，给驾驶员带来困扰。对于“新司机们”而言，很多情况下都需要颇费周折才能停好车，在这种环境下泊车容易引起局部交通堵塞、神经疲惫和保险杠被撞弯等剐蹭事故的发生。

在这样的背景下，为便捷人们出行、缓解交通压力，解决停车难题便成为了城市管理的重要任务。近年来世界各国从科技的角度不断探索，尝试找到破解城市停车难题的办法。在不同的国家，落实不同场景的自动驾驶的意愿也不相同，唯一确定的是，大家都不爱停车，确切的说是不爱找车位和停车，国外车企也意识到停车难的问题。随着高级辅助驾驶系统的出现，汽车的驾驶体验越发丰富，驾驶员在驾驶汽车的过程中也越来越轻松，驾驶安全性也由于高级辅助驾驶的搭载获得了巨大的提升，作为在驾驶汽车过程中不可避免的动作——停/泊车，也被汽车设计师和汽车制造商考虑到，技术的发展为之提供了解决之道，设计并研发了相关的高级辅助驾驶系统，它就是——自动泊车系统。 经过不懈努力，在自动驾驶技术的前哨应用之中，人们也是看到了问题解决的希望和曙光，那便是借助“自动泊车技术”，率先推出了自动泊车系统。虽然有些车型已经配备了自动泊车技术，但是依然需要驾驶员人为控制档位、油门，并不算真正的自动泊车。随着技术的发展自动化程度的提高，较先进的自动泊车辅助只需要轻轻启动按   钮、坐定、放松，就可以自动完成停车入位，这种功能进一步得到老司机们的喜爱，毕竟可以减少日常通勤停车的重复劳动。目前很多车企推出的新车都有自动泊车功能。 自动泊车技术的应用，对于解决人口密集城区的停车问题和交通问题也确有奇效。因为自动泊车技术可以将汽车停放在较小的空间内，这些空间比大多数驾驶员能自己停车的空间小得多，这使得车主能更容易地找到停车位。与此同时由于相同数量的汽车占用的空间也更小，也使得停车场的利用率也大幅提升。 现在越来越多的厂商都开始研发无需人工干预真正的自动泊车功能，而这就属于无人驾驶的范畴了。这样自动泊车无需驾驶员就可以实现泊车入库，不仅非常方便，同时非常炫酷。因此致力于运用新技术让泊车变得更智能、更安全、更便捷，自动泊车技术的出现和发展为解决泊车问题提供新思路，将有效解决驾驶员找位难、停车难等痛点。 二 概述

 

三 定义

1.自动泊车系统 自动泊车又称为自动泊车入位，顾名思义就是汽车不用人工干预，系统能够自动帮用户将车辆停入车位。当找到了一个理想的停车地点，只需轻轻启动按钮、坐定、放松，其它一切即可自动完成，彻底消除在停车中遇到的麻烦。自动车辆控制的一个重要目标是提高安全性和驾驶员的舒适性，APAS通过泊车操作来实现这个目标。

自动泊车是一个常见的低速操作场景，是解决自动驾驶最后一公里的核心技术，也是自动驾驶技术，相当于低速的自动驾驶，技术难度与自动驾驶相比较为容易，只不过是自动驾驶的一种低配版，就是自动驾驶降低要求后的一种衍生，也是实现自动驾驶的必经之路。 目前真正的自动驾驶虽然距离商业落地和规模普及还有一段距离，但如果降低自动驾驶应用的环境条件和实际限制，其同样具备重大使用价值。 自动泊车已经成了无人驾驶最先落地的应用场景，低速，封闭或者半封闭空间，场景相对固定，最容易落地。

自动泊车系统（Automated Parking System，APS）/自动泊车辅助系统（Auto Parkig Assist， APA）主要是利用遍布车辆自身和周边环境里的车载传感器，测量车辆自身与周边物体之间的相对距离、速度和角度：通过泊车雷达来实现自动识别可用车位，利用360°全景摄像头的图像信息，并结合超声波雷达传感器，实现对车位、 周边环境的感知和识别，然后通过车载处理器/车载计算平台或云计算平台计算出操作流程，控制器根据识别的信息计算泊车合适的路径以及控制执行机构实施车辆的转向和加减速，并自动正确地完成停车入车位动作的系统，以实现自动泊入、泊出及部分行驶功能，实现车辆安全、平顺的泊车。 搭载有自动泊车功能的汽车可以不需要人工干预，通过车载传感器、处理器和控制系统的帮助收集车辆的环境信息（包括障碍物的位置），就可以实现自动识别车位和找到一个停车位，并自动安全地完成泊车入位操作的过程。

2.自主泊车系统

随着自动驾驶技术的发展，自动泊车逐渐往自主泊车方向演进。自主泊车又称为代客泊车或一键泊车：指驾驶员可以在指定地点处召唤停车位上的车辆，或让当前驾驶的车辆停入指定或随机的停车位。整个过程正常状态下无需人员操作和监管，对应于SAE L3级别。

自主泊车系统包含两个功能，即泊车与唤车：

泊车功能：是指用户通过车载中控大屏或手机APP选定在园区、住宅区等半封闭区域内的停车位或者选定停车场（有高精地图覆盖），然后车辆通过获取园区、住宅区等半封闭道路上的车道线、道路交通标志、周围其他车辆等交通环境、参与者信息；控制车辆的油门、转向、制动来实现安全自动驾驶，并通过自动寻找可用停车位或识别用户选定停车位；实现自动泊入、自动停车、挂P档、熄火、锁车门，同时防止潜在的碰撞危险的功能。

唤车功能：是指用户通过手机APP选定园区、住宅区等半封闭区域内的某一唤车点，然后车辆从停车位自动泊出、低速自动驾驶到达唤车点，从而实现唤车，同时防止潜在的碰撞危险的功能。  

AVP自主泊车方案中的核心功能“记忆泊车”，就是支持“无人泊车”的。“记忆泊车”顾名思义，可以让车辆“记住”车位，让车根据“记忆”自己找车位。车就可按着记忆中的路线自己找到车位泊进去了。AVP的记忆泊车和召唤功能是支持地库停车场和露天停车场的，部分目前具备记忆泊车功能的还只能用于地库，露天停车场是用不了的。

AVP自主泊车可以在100米内记住10条行进路线，在用户使用记忆泊车时，系统车辆将优先选择最佳泊车路线。这些路线是可以在用户之间共享的。也就是说，随着使用“记忆泊车”的人越来越多，自主泊车数据库也会越来越完善，其记忆泊车能适应的场景也会越来越多。

四 组成

自动泊车系统组成及功能如下表所示：

要实现自动泊车这样一个复杂的功能，需要车辆的各个子系统协同工作。下图所示的系统示意图展示了CAN数据总线中的联网关系。

自动泊车APS系统架构一般包括一个环境数据采集系统、一个中央处理器和一个车辆策略控制系统。典型的系统架构如下：

泊车系统典型架构

自动泊车辅助系统主要由信息检测单元、电子控制单元和执行单元等组成，如下图

自动泊车的系统硬件架构可以分为四部分：传感器、执行器、主控制器、其它关联系统

自主泊车系统AVP

自主泊车系统方案如下图所示，主要采用智能化车端+智能化场端的方式。车端智能化主要依赖于融合式全自动泊车的传感器配置，外加前视摄像头、V2X设备等实现特定区域内的点到点自动驾驶、自动车位扫描、自动泊入泊出等功能。车辆自身具备车辆、行人等动态障碍物检测和识别功能，可实现自动紧急制动、避障等决策规划。场端智能化主要依托摄像头检测技术，实现停车场车位占用情况检测，并上传至停车场服务器，并实现为自主泊车车辆提前分配车位信息。

1.传感器     环境数据采集系统包括图像采集系统和车载距离探测系统（超声波雷达或者毫米波雷达系统），可采集图像数据及周围物体距车身的距离数据，并通过数据线传输给中央处理器；通过传感器系统感知环境信息，包括视频传感器（摄像头），毫米波雷达， 超声波雷达等。当这些传感器（它们共同组成了车辆的环境数据采集系统）采集到外部的数据信息之后，会将这些信息传输到车辆搭载的中央处理器中，并且在这里得到 车辆的位置、车 位的位置以及周围的环境参 数。

主要任务是探测环境信息，如寻找可用车位，在泊车过程中实时探测车辆的位置信息和车身状态信息。在车位探测阶段，采集车位的长度和宽度；在泊车阶段，监测汽车相对于目标停车位的位置坐标，进而用于计算车身的角度和转角等信息，确保泊车过 程的安全可靠。   不同的自动泊车系统采用不同的方法来检测汽车周围的物体。有些在汽车前后保险杠四周装上了感应器，它们既可以充当发送器，也可以充当接收器，这些感应器会发送信号，当信号碰到车身周边的障碍物时会反射回来，然后车上的计算机会利用其接收信号所需的时间来确定障碍物的位置；其它一些系统则使用安装在保险杠上的摄像头或雷达来检测障碍物。但最终结果都是一样的：汽车会检测到已停好的车辆、停车位 的大小以及与路边的距离，然后将车子驶入停车位。

其中传感器包括4颗远距离传感器（测距5米）和8颗近距离传感器（测距1.5米））， 分别实现车位扫描和距离侦测的功能。

传感器为超声波传感器及摄像头，方案如下图所示，包括12个超声波传感器，4个环视摄像头和1个前视摄像头。

车辆上标配共有8个摄像头，后面有一个倒车摄像头，还有一个毫米波雷达以及一个探测范围8米的超声波传感器。车身上搭载的8个摄像头，视角都会有相当区域的重

合，这就保证了传感器不会有视野盲区，这对实现自动泊车是最重要的一件事了。

▲ 各种传感器的位置和视角示意

其 它 传感器还包括轮速传感器、环境温度传感器、转向灯开关、自动泊车辅助功能按

钮和拖车钩传感器。

（1）超声波传感器为自动泊车系统必要组成，可以无摄像头，因为超声波雷达除了检测车位，另一个更重要的作用是障碍物检测，进行避障。从成本考虑通常选用性价比比较高的超声波传感器，是APS主流的技术方案，探测距离为5～8 m。它们发射超声波信号，然后接收从障碍物反射回来的信号，并根据从发射到接收信号的时间长短来评估与障碍物的距离。车辆保险杠正前方前雷达监测距离为100cm，后方监测的距离为120cm，其中左前和右前外侧距离传感器用于探测停车位的长度和宽度。

泊车雷达采用超声波测距原理，判断障碍物的位置和距离，当距离达到警报限值时，通过声音及模拟显示进行提示；声音警示通过IPK整合实现、泊车模拟显示通过AIC整合实现。

‍         远距超声波传感器外形如下图： ‍

主要参数如下表所示：

（2）车载毫米波雷达

‍

（3）摄像头为非必要组成部分，缺少摄像头仍可实现自动泊车，但是泊车应用场景会减少。

AVM（Around View Monitor）：全景环视系统/全景影像。在自动驾驶领域，AVM属于自动泊车系统的一部分，是一种实用性极高、可大幅提升用户体验和驾驶安全性的功能。360度环绕视频系统的使用， 对自动泊车系统的车位线的识别，驾驶员实时监测系统有了质的提高。

将装在车身前后左右四个方向的摄像头图像信息进行处理，形成一幅车辆周边360度的车身俯视图，并在屏幕上显示，帮助驾驶员轻松停泊车辆。系统自带畸变校正功能、无缝拼接、内部亮度平衡与颜色平衡算法，使输出的画面更加接近车身周围环境。根据车身信号自动切换视角，使得不同场景下给用户呈现出恰当的视角，有用的图像，使驾驶员能够及时观察到车身周围的障碍物，防止车辆发生碰撞。

比较先进的全自动泊车系统，会结合选用毫米波雷达系统，距离检测和抗干扰能力更强。比如为了支持功能强大的自动泊车技术，有的提供了多达12枚泊车雷达，同时360°摄像头也能让驾驶者在车内知晓车辆周围的情况，必要时也可以亲自介入停车动作。现在结合无人驾驶的传感器技术， 包括毫米波雷达和激光雷达的使用， 探测距离和探测精度可以得到很大提升。

自动驾驶——自动泊车之AVM环视系统算法框架

AVM已是一种较为成熟的技术，中高端车型均有部署，但详细讲述AVM系统算法的技术博文并不多。搭建了一套AVM算法框架，有一些效果还不错的demo，主要是想将AVM算法框架中每个算子讲述清楚，文的风格为理论与实践结合，含有部分代码，适合有一些计算机视觉基础的读者。

AVM系统概述

AVM汽车环视影像系统如图所示，由安装在前保险杠、后备箱、后视镜上的四个外置鱼眼相机构成。该系统包含的算子按照先后顺序：去畸变、四路鱼眼相机联合标定、投影变换、鸟瞰图微调、拼接融合、3D模型纹理映射等。四路鱼眼捕捉到的图像信息通过上述算子，生成一个2D、3D的全景图。AVM算法又分为离线阶段和在线阶段两部分，在线阶段是对离线阶段的简化，更加适合于工程实现。

avm系统示意图

离线阶段算法pipeline

先来粗略浏览下AVM算法Pipeline包含那些算子：

2D AVM

2D AVM Pipeline

3D AVM

3D AVM Pipeline

基于畸变表的鱼眼相机去畸变

1.1鱼眼相机畸变模型 普通相机和广角相机的投影方式一般为透视投影，即通过三角形相似原理，将相机坐标系下三维世界中的物体投影到平面上，这是基于理想的透视投影模型（无畸变）。但实际情况是得到的最终图像与理想的透视投影模型有一些区别，即径向畸变（桶形、枕型）、切向畸变。因此相机标定中都会对畸变做矫正。 鱼眼相机的投影方式有很多种假设，例如等距投影、等立体角投影、正交投影、体视投影、线性投影。但是真实的鱼眼相机镜头并不完全遵循上述的这些模型假设。因此Kannala-Brandt提出了一种一般形式的估计，适用于不同类型的鱼眼相机：  ，这个也是纳入opencv中的鱼眼相机畸变模型。对照下图： 为光线入射角，  为出射光线在相机归一化平面上或者在相机成像平面上与O之间的距离（在opencv中表示光线在相机归一化平面上的成像位置）。

鱼眼相机模型 相机去畸变通常使用棋盘格标定方法，首先通过矩阵推导得到一个比较好的初始解，然后通过非线性优化得到最优解，包括相机的内参、外参、畸变系数，然后对鱼眼图像做去畸变处理。内参即： 相机内参矩阵 然而，此标定法并不适用于这场景。 1.2基于厂家畸变表的鱼眼图像去畸变 由于棋盘格标定法是在图像的全局进行拟合得到一个全局的最优解，因此需要保证多次拍摄到的标定板的棋盘格可以覆盖整个图像区域。而假设的场景为要求汽车整车上流水线进行标定，即相机已经安装在车上。很显然由于车身遮挡的原因，很难保证上述条件。另外棋盘格标定法并不适用于批量生产。因此选择了基于厂家提供的畸变表对鱼眼相机图像进行去畸变。相机厂家都有专业的光学工程师，大厂提供的畸变表通常情况下比较准确。 当然也有一些在畸变表的基础上进行优化的方法，例如[采用最小重投影的方法计算出最优的相机主点位置，然后使用畸变表进行去畸变处理。在其它场景中，还有些先标定出相机的内参，然后将内参与畸变表联合使用。下面来讲述基于畸变表的去畸变方法：

厂家提供的畸变表 上面的表格展示了相机畸变表的一部分，厂家给出了入射角从  到  的光线在焦距为f的相机真实成像平面上成像点距离成像平面中心的真实距离，单位为mm。如果想用opencv提供的API做去畸变处理，需要使用厂家提供的焦距f，将换算到相机的归一化平面上去（即除以f）。然后通过多项式拟合的方法，计算出  这几个畸变参数，例如可以使用python的curve_fit库进行多项式拟合。调用Opencv API，m_distortion_coeffs即为多项式拟合的畸变参数。

 

fisheye::initUndistortRectifyMap(m_intrinsic_matrix, m_distortion_coeffs, R, NewCoeff, image_size*2, CV_32FC1, mapx, mapy);cv::remap(disImg, undisImg, mapx, mapy, INTER_LINEAR);

通俗讲：鱼眼相机去畸变的过程实际上就是遍历想要的无畸变图上的坐标点，通过mapx，mapy两个查找表，找到该坐标点在畸变图上的像素位置。通常这个像素的位置为浮点型，需要做双线性插值。否则在纹理边缘上会有锯齿状的问题，这个结论是实现了opencv remap函数验证过的，有兴趣的可以实现一下mapping的过程（查找+插值）。来看图：

鱼眼图 去畸变 右图为基于畸变表去畸变的结果，可以看出去畸变的效果大体上满足要求，例如柱子边、标定布边、车道线为直线。但是仍有部分区域的去畸变效果不好，直线不够直。这个问题会在鸟瞰图中看起来更加突出，也是导致覆盖区域拼接不齐的重要原因。 原因可能有几种：（1）相机光轴与成像平面的交点（主点）与图像平面的中心不重合，即内参矩阵中的 。（2）厂家给的焦距f不准（3）厂家给的畸变表有误差。 理论上相机的标定是一个计算全局最优解的过程，可以理解为：内参可以不那么准，拿到的畸变表也可以不那么准，但是只要优化目标重投影误差很小，或者畸变去的比较干净，那么这个全局最优解就是可以接受的。因此使用了最小化重投影误差的方法得到内参中的  ，然后再使用畸变表；在有的场景中，还有人用棋盘格标定出相机的内参，然后配合畸变表进行使用。这些内容后面都会陆续做优化。

四路鱼眼联合标定 鱼眼相机联合标定的目的是要得到四个鱼眼相机之间的位姿关系，然后将拍摄到的图像搞到同一个坐标系下得到一幅全景环视图。

相机联合标定示意图 如图所示，全景鸟瞰图的视野范围是人为给定的参数，可根据用户喜好进行调节。标定布上的棋盘格大小、黑格子尺寸、汽车与标定布之间的间距这些都是已知的先验信息。上述先验信息在现实世界中与在全景图上的尺度关系为1：1，即1个像素代表1cm（当然这个尺度也可以调节，你想让一个像素代表n厘米也没问题）。这样做联合标定的意义在于：可以知道前、后、左、右四个鱼眼相机去畸变后图像中棋盘格上角点，与前、后、左、右四个鸟瞰图中棋盘格角点之间对应的坐标关系。这样就可以根据投影变换，将整张图像投影到对应的鸟瞰图上去。又由于在联合标定中，四个鸟瞰图是刚好拼接到一块的，因此利用上述方法将四张图全部投影到鸟瞰图上，在不考虑误差的理想情况下，应该是刚好拼接在一起的。以上，就是联合标定的思路。

 

 

投影变换

3.1投影变换原理 投影变换的通俗理解就是：假设同一个相机分别在A、B两个不同位置，以不同的位姿拍摄同一个平面（重点是拍摄平面，例如桌面、墙面、地平面），生成了两张图象，这两张图象之间的关系就叫做投影变换。张正友老师的相机标定法使用的就是从标定板平面到图像平面之间的投影模型。

投影变换模型 图中相机从两个不同的角度拍摄同一个X平面，两个相机拍摄到的图像之间的投影变换矩阵H（单应矩阵）为：

其中K为相机内参矩阵，R、T为两个相机之间的外参。这个公式怎么推导的网上有很多，只需要知道，这个单应矩阵H内部实际是包含了两个相机之间的位姿关系即可。这也就解释了：为什么有的AVM pipeline的方法是需要标定相机的外参，然后通过厂家提供的相机安装参数将四路鱼眼全部统一到车身坐标系下，而不需要这个过程，只需要用标定布来做联合标定。其实两种方法内部都是相通的，都绕不开计算相机外参这件事情。 3.2 投影变换生成鸟瞰图 生成鸟瞰图的过程可以理解为：将鱼眼相机拍摄到的图像，投影到某个在汽车上方平行地面拍摄的相机的平面上去。这个单应矩阵H具体是多少，由去畸变图中检测到的棋盘格角点坐标和联合标定全景图中棋盘格角点坐标来决定。如图所示，以后置相机为例，联合标定已知图（2）中框出棋盘格的坐标，图（1）中的棋盘格坐标可通过opencv的函数进行检测，从而建立单应矩阵H的求解模型。

（1）去畸变图中棋盘格位置 （2）联合标定全景图中棋盘格位置 （3）瞰图 3.3一些经验之谈 3.3.1尽量选择更多的角点计算单应矩阵 单应矩阵的求解是一个拟合的过程，如果选用过少的点，容易陷入局部最优解。造成的结果是就是鸟瞰图上只有你选择的那些点可以正确的投影，其他像素的投影可能不正确。这一点有点类似于深度学习中训练样本太少，导致过拟合的问题。

单应矩阵三种形式（1）（2）（3） 上面公式可以看出，一对匹配点可以提供两组方程，理论上4对匹配点就可以求解出单应矩阵。Opencv求解单应矩阵提供了两个函数，findHomography和getPerspectiveTransform。 getPerspectiveTransform的输入是4对点，对（2）中矩阵求逆。理想情况下这种方法是可行的，但由于存在噪声，我们在图像上检测到的角点的误差、标定布棋盘格的误差，这种方法极其不准确。 findHomography求单应矩阵的方法输入点对很多，解一个超定方程（3）。经过一顿推导，单应矩阵为（3）中矩阵的奇异值分解中最小奇异值对应的特征向量。这种方法用于做拟合的样本更多，最终的效果更好。而且Opencv还有很多优化算法，例如基于ransac思想的单应矩阵求解方法。当然为了提高效果，可以对标定布进行DIY，某宝上很多这种DIY标定布，想搞多少格子就搞多少。 如1.2所述，由于畸变去除的不彻底，导致有些直线仍然是弯曲的。这一现象在投影到鸟瞰图上之后尤为明显，通过大量的棋盘格点进行投影变换，可以从一定程度上强制矫正这个问题。至少可以让车身附近的全景图效果更佳，而avm系统最在意的恰好就是车身周围这部分，距离车身远的部分也不会呈现出来。如图所示为某厂DIY的标定布示意图。

DIY标定布 3.3.2尽量让棋盘格处于相机拍摄图像的中心 鱼眼相机在中心部分畸变小，边缘位置畸变大。去畸变的结果通常也是中间的效果好，边缘残留的畸变多。因此为了使单应矩阵计算的更佳准确，要保证标定布摆放的时候棋盘格位于鱼眼相机中央。这也是为什么某宝上标定布使用的示意图通常是图（2）这种，而不是图（1）。很显然，图（2）中棋盘格位于左侧后视镜附近（左鱼眼相机就在左后视镜上），即相机图像的中间位置，而图1中棋盘格则在相机图像边缘上。

左侧鱼眼相机鸟瞰图（1）（2）  

拼接融合

经过3中的投影变换，得到4张包含重叠区域的鸟瞰图如图所示，需要将这些鸟瞰图进行拼接融合。

鸟瞰图 以左、前鱼眼相机俯视图为例，观察下它们的重叠区域重叠区域：

白色为重叠区域、AB为前鸟瞰图边界、CD为右鸟瞰图边界 通常的做法是分别以AB、CD为边界，计算白色区域像素点与AB、CD之间的距离，然后计算一个权重，距离CD越近的位置，前俯视图权重越大；距离AB越近的位置，左俯视图权重越大。但会出现边界效应如图所示：

前俯视图权重图 其原因也很容易理解：如图所示，将AB、CD延长至O点，在EAOCE这个区域内，使用上述方法计算权重图才是一个完整连续的模型，如果在EABFDCE这个区域内计算权重图相当于把一个完整连续的域强行截断，计算得到的权重图必然是有截断痕迹的。

完整、连续模型示意图 因此需要使用某种策略，让在一个连续的作用域上计算权重，这里提供一个思路：在EAFCE这个连续的作用域中计算权重，得到一个非常漂亮的权重图：

连续模型和权重图  

基于光流的鸟瞰图微调

在整个AVM系统中，厂家提供的畸变表、焦距、相机主点位置，联合标定使用的标定布都会引入误差。这些误差会导致生成的鸟瞰图在重叠区域有一些偏移。第4小节中的拼接融合模块是为了让鸟瞰图在覆盖区域过渡平滑，尽量避免伪影现象。但是不能将这个压力全部施加给拼接融合模块。因此需要在拼接融合模块之前，对鸟瞰图进行微调，这个微调功能是供客户或者4S店人员进行手动调节的。 在调研中发现，现在很多部署在车上的AVM系统都包含有微调功能。但大部分都存在一个问题：只能保证一边是对齐的，另外一边拼不齐。例如，前面对齐后面对不齐。 使用了，将前、后微调光流图进行融合，得到一个平缓过渡的光流图，兼顾了前后两侧的微调。以左俯视图为例，算法流程如下：

固定住前、后两个鸟瞰图

手动微调左鸟瞰图，使左鸟瞰图与前鸟瞰图之间的重叠区域贴合。记录微调矩阵 M1，并根据矩阵计算光流 map1

手动微调左鸟瞰图，使左鸟瞰图与后鸟瞰图之间的重叠区域贴合。记录微调矩阵 M2，并根据矩阵计算光流 map2

根据像素距离计算 map1 与 map2 的权重图 w，即距离前鸟瞰图越近，map1 的权重越大，反之则越小

使用 w 对 map1 和 map2 进行加权融合

当微调矩阵M1和M2方向正好相反时，这个基于光流的思想可以很好地将两者融合，因为矩阵变换是比较“硬”的一种数学方法，而光流却像水一样的“软”。看下前后对比的融合效果图吧：

左侧为普通的融合效果 右侧为基于光流思想微调后的效果 可以看出这个方法可以兼顾前、后的微调效果。可以理解为将拼接区域的误差均摊给中间的区域，而中间这部分区域不存在拼接融合的问题，而且在泊车的过程中更注重的是车与周围物体的相对位置而非周围物体的精确位置，因此实际看上去也没什么问题。   三维模型纹理映射 2D的AVM算法是基于投影变换，将鱼眼图像投影到鸟瞰图上。而单应变换有一个前提：就是平面假设。是把一个平面投影到另外一个平面。存在的问题是：图像中所有的三维物体，例如汽车、柱子、墙，全部被当成平面来处理。这些内容在鸟瞰图上会被拉的很长，如图所示：

                          2D图像中的三维物体左侧为墙，右侧为车（没错就是车的前杠被拉的好长） 安装在车身周围的鱼眼相机是单目相机，单目相机不能获取三维物体的深度。在图形学中有一种真实感增强的方法：制作一个三维模型，把二维的纹理贴图以某种方式映射到三维模型上，3D AVM正是使用了这个纹理映射技术，为驾驶员呈现出一个伪3D的效果。 6.1 AVM 3D模型构建 3D模型是由一个个小面片构成的，可以是三角面片、多边形面片等等。面片又是由多个顶点构成的，例如三角面片对应的就是3个顶点。先来直观地看一个三维模型：

3dsMax三维模型 放大看可以看到，3D模型是由很多个小的多边形面片构成。3D模型的文件形式有很多种，但大体上都是包含：模型顶点、面片、纹理坐标、法向量这些三维信息。具体如何使用3dsMax来制作3D模型，就不叙述了，不是专业的美工，方法可能不太聪明，领会精髓即可。 AVM 3D模型是一个碗状的三维模型。模拟驾驶员视角，即汽车周围附近是路面，这部分直接映射到碗底平面上；而距离汽车较远的位置可能是楼房、树木、墙等三维物体，这部分内容将使用某种方式映射到三维点上。下面展示的就是3d模型中的必要信息，包含顶点坐标、纹理坐标、法向量、三角面片索引。

 

//顶点坐标v 166.2457 190.1529 575.8246v 169.0261 192.6147 575.0482v 163.5212 194.2559 576.8094v 160.4214 177.1097 576.3941v 160.5880 183.6252 577.0156......//纹理坐标vt 0.227618 0.463987vt 0.254011 0.468448vt 0.251903 0.470549vt 0.248436 0.466586vt 0.267204 0.509296......//法向量信息vn 0.3556 -0.4772 -0.8036vn 0.3606 -0.4537 -0.8149vn 0.3145 -0.3999 -0.8609vn 0.3101 -0.3998 -0.8626vn 0.3170 -0.3811 -0.8685......//三角面片信息f 5825/5825/4368 5826/5826/4369 5827/5827/4370f 5828/5828/4371 5829/5829/4372 5830/5830/4373f 5831/5831/4374 5832/5832/4375 5833/5833/4376f 5834/5834/4377 5835/5835/4378 5836/5836/4379f 5837/5837/4380 5838/5838/4381 5839/5839/4382 6.2 三维模型纹理映射 这一小节讲述的是： （1）纹理映射是从哪里映射到哪里 （2）采用哪种策略进行映射 最终目标是：找到3d模型上每个顶点对应在鱼眼2d图像上的纹理坐标。 采用的是一种基于虚拟相机思想的3d纹理映射方法，如图所示：

基于虚拟相机思想的3d纹理映射模型 假设2D AVM的全景鸟瞰图是由汽车正上方某个虚拟相机拍摄到的图，将其当作2D纹理，以透视投影的方法映射到3d模型上面。图中Lw-Rw为全景俯视图，虚拟相机与顶点A的直线在鸟瞰图上的交点为A'，从而得到A顶点对应的2D纹理映射坐标A'。然后通过逆投影变换H_inverse、畸变mapx、mapy查找到3d模型的顶点A在鱼眼相机上的纹理坐标。遍历3d模型上的每一个点，即可得到三维模型与鱼眼相机纹理坐标之间的映射关系：

 

 

根据透视投影原理，计算顶点A对应的鸟瞰图纹理A'

使用矩阵变换和单应变换逆推A'在去畸变图上的坐标A1

通过去畸变的查找表map查找A1在鱼眼相机畸变图上的坐标

遍历上述过程，即可得到3D模型上所有顶点对应鱼眼图上的纹理坐标

具体流程如图所示：

3d模型纹理映射流程 看下效果：这个模型的法向量是反的，所以渲染的结果光线有问题，非常的暗。不过可以看到伪3D的真实感增强效果，领会精神即可。

右侧模型映射 6.3 三维融合 实际上使用的是前、后、左、右4个曲面模型，这4个曲面模型与4路鱼眼图像一一对应，这样做是为了增加OpenGl渲染的并行，避免在做拼接融合时用到if、else这些判断语句。上图中的3d模型就是左侧鱼眼相机对应的曲面模型。 先来回顾2D AVM的做法：生成鸟瞰图，然后做融合。当时生成的鸟瞰图大小为1080*1080，这个分辨率等同于现实世界中的1080cm*1080cm，足以显示车身周围。但是，这个范围最多只能映射到3d碗模型的底部附近区域，如下图：忽略边上的锯齿，那是渲染时插值的bug，先不去管它。从这图可以看出：如果鸟瞰图选的很小，会导致只映射到碗的底部。

小鸟瞰图映射到模型上 把鸟瞰图的尺寸放大，可以看到：

大鸟瞰图映射到模型上 图1为左侧鱼眼相机去畸变后的图像，图2是由图1做投影变换得到的鸟瞰图，图3为映射到左侧模型后的结果 这里要注意：在算法实现上不可以像2D AVM那样，去真正地生成一个鸟瞰图。从鸟瞰图不难看出，在远离棋盘格的部分被严重拉长，而图1中接近消失点、消失线的那些像素在鸟瞰图上将会被拉到无穷远。可以这样理解，图一中的消失点、消失线表示：在当前的相机位姿，某一个平面上（例如地面）的点全部在这条消失线以下。而鸟瞰图相当于把相机平行于地面进行拍摄，那么地平面无穷远处（即1中的消失点、消失线）成像在鸟瞰图中必然会被拉长到无穷远处，就像图2一样。有兴趣的可以看看消失点的解释。

消失点 如果想要将整个碗状模型填满纹理，需要生成一个特别大的鸟瞰图。即需要计算一个特别大的map，这个计算量是巨大的。因此在算法实现上，要选用遍历模型上的每个顶点，进行逆向纹理映射的方法计算纹理坐标（不再依赖于生成一个鸟瞰图），顶点之间的空缺将由渲染引擎通过插值的方法进行填充，这个是种成熟的技术。 讲了这么多终于说到三维的融合。2D的融合是对鸟瞰图的覆盖区域做形态学操作，得到下图，然后计算权重。而3D的算法强调的是离散点的思维，不会再生成一个超级大的鸟瞰图。换句话说，算法不会再计算一个像下图一样覆盖区域的图像。因此，要寻找其它的方式来解决3D融合的问题。

左上角重叠区域 如图所示：

右上方重叠区域示意图 大概思路就是计算3D顶点对应鸟瞰图的纹理坐标B。通过AB与m、l的夹角与计算权重。 当然，重叠区域不可能这么理想，这个论文中的示意图l和m正好交于A点。实际情况是它上面那个图的样子。需要使用某种专门针对3D AVM融合的策略来实现之。将3D模型顶点对应的权重图映射到二维的示意图：

放大看 总体来讲3D AVM算法就是先搞一个3维模型，然后通过纹理映射，将3维模型上的每一个顶点与二维的纹理图进行绑定。OpenGl利用上述数据进行渲染。最终的3D效果后续会发上来。 在线阶段工程实现pipeline 前面介绍的是离线阶段的算法流程，离线阶段只有在流水线上或者4s店才会用到，是一个初始化的过程。初始化的内容包括：畸变表、投影变换矩阵、纹理映射关系、拼接融合权重图等。最重要的是要将去畸变、投影变换、纹理映射这些过程写入一个查找表，存入内存，在线处理的时候直接调用即可。附上部分代码，对map做remap这块可能会稍微难理解一些。

 

//4个label是鸟瞰图在avm全景上的位置坐标for (int i = label1; i < label2; i++){ float *map2_x = map2_xR.ptr(i); float *map2_y = map2_yR.ptr(i); for (int j = label3; j < label4; j++) { Mat vec = (Mat_(3, 1) << j, i, 1);//AVM全景图的grid网格坐标 vec = matrix * (vec);//获取鸟瞰图坐标 Mat coor = Homo_inverse * vec;//从鸟瞰图反向投影到去畸变图 map2_x[j] = coor.at(0, 0); map2_y[j] = coor.at(1, 0); }}//map1（畸变） remap map2（投影+旋转）remap(map1_y, my, map2_xR, map2_yR, INTER_LINEAR);remap(map1_x, mx, map2_xR, map2_yR, INTER_LINEAR);//畸变+投影+旋转+finetuneif (finetune){ remap(mx, mx, m_finetune_l_blendX, m_finetune_l_blendY, INTER_LINEAR, BORDER_REPLICATE); remap(my, my, m_finetune_l_blendX, m_finetune_l_blendY, INTER_LINEAR, BORDER_REPLICATE);} 2D AVM

2D AVM Pipeline

 

 

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）模拟显示

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

编辑：黄飞