汽车自动泊车辅助系统APA(上)

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

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

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

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

在这样的背景下，为便捷人们出行、缓解交通压力，解决停车难题便成为了城市管理的重要任务。近年来世界各国从科技的角度不断探索，尝试找到破解城市停车难题的办法。在不同的国家，落实不同场景的自动驾驶的意愿也不相同，唯一确定的是，大家都不爱停车，确切的说是不爱找车位和停车，国外车企也意识到停车难的问题。随着高级辅助驾驶系统的出现，汽车的驾驶体验越发丰富，驾驶员在驾驶汽车的过程中也越来越轻松，驾驶安全性也由于高级辅助驾驶的搭载获得了巨大的提升，作为在驾驶汽车过程中不可避免的动作——停/泊车，也被汽车设计师和汽车制造商考虑到，技术的发展为之提供了解决之道，设计并研发了相关的高级辅助驾驶系统，它就是——自动泊车系统。
经过不懈努力，在自动驾驶技术的前哨应用之中，人们也是看到了问题解决的希望和曙光，那便是借助“自动泊车技术”，率先推出了自动泊车系统。虽然有些车型已经配备了自动泊车技术，但是依然需要驾驶员人为控制档位、油门，并不算真正的自动泊车。随着技术的发展自动化程度的提高，较先进的自动泊车辅助只需要轻轻启动按

钮、坐定、放松，就可以自动完成停车入位，这种功能进一步得到老司机们的喜爱，毕竟可以减少日常通勤停车的重复劳动。目前很多车企推出的新车都有自动泊车功能。
自动泊车技术的应用，对于解决人口密集城区的停车问题和交通问题也确有奇效。因为自动泊车技术可以将汽车停放在较小的空间内，这些空间比大多数驾驶员能自己停车的空间小得多，这使得车主能更容易地找到停车位。与此同时由于相同数量的汽车占用的空间也更小，也使得停车场的利用率也大幅提升。
现在越来越多的厂商都开始研发无需人工干预真正的自动泊车功能，而这就属于无人驾驶的范畴了。这样自动泊车无需驾驶员就可以实现泊车入库，不仅非常方便，同时非常炫酷。因此致力于运用新技术让泊车变得更智能、更安全、更便捷，自动泊车技术的出现和发展为解决泊车问题提供新思路，将有效解决驾驶员找位难、停车难等痛点。
二概述

三定义

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的做法：生成鸟瞰图，然后做融合。当时生成的鸟瞰图大小为10801080，这个分辨率等同于现实世界中的1080cm1080cm，足以显示车身周围。但是，这个范围最多只能映射到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<float>(i);    float *map2_y = map2_yR.ptr<float>(i);  for (int j = label3; j < label4; j++){Mat vec = (Mat_<double>(3, 1) << j, i, 1);//AVM全景图的grid网格坐标        vec = matrix * (vec);//获取鸟瞰图坐标        Mat coor = Homo_inverse * vec;//从鸟瞰图反向投影到去畸变图        map2_x[j] = coor.at<double>(0, 0);map2_y[j] = coor.at<double>(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时，警告部分亮起向驾驶员发出视觉警告。

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

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

![图片](https://img-blog.csdnimg.cn/6f2b4fdad2f6423eaa54e2354e53eff0.jpeg

经过在不同场景下的测试，在整个泊车的成功率包括了泊车的精度，六项最重要的泊车指标。
通过多媒体或自动泊车按键，就可以开启或者关闭自动泊车功能。当车位被选择后，驾驶员按照多媒体显示屏上的相关信息提示停车并点击开始泊车按键，系统可自动进入泊车模式。该模式下系统可以自动规划泊车轨迹，并控制车辆的挡位、转向、制动、车速等使其驶入规划的车位。

或在挂入倒挡后选择“自动泊车”，接着将车速保持在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*算法实现任意两点间的全局路径规划，支持路径规划重置、选路以及速度规划功能，同时结合实时环境感知状态，进行局部路径规划，实现紧急制动、跟车巡航以及换道避让、换道超车等自主决策。

下半部分见公众号原文，包含两篇论文及安全协议不粘到CSDN了