目录
一、概述
泊车算法离不开感知、规划、控制，从目前行业技术发展的角度来看，其中涉及的每一个算法都不算完美，甚至可以说仍然不成熟。然而，小鹏P7采用优秀的系统方案设计，特别是通过引入同步建图与定位技术有效提升了泊车性能；并通过设计与算法配合默契的HMI，有效地发挥了各个算法的长处，规避了算法的不足。
视觉感知只在寻库过程中检测库位（泊入阶段不使用视觉感知，保证视觉感知算法可以简单），并与超声波雷达寻库的结果进行融合（业界比较成熟的方案），一旦锁定库位后进行一次规划（不会根据动态障碍物的出现调整规划路径路径，也无需根据泊入阶段再感知的库位位置调整规划，使得规划算法可以简单），随后通过航位推算进行泊入（对线控和航位推算要求较高，但这也是电动车的优势），泊入过程中通过超声波雷达进行避障。整个过程中，HMI优秀地配合算法规避了复杂问题：视觉感知仍然存在非常多的误检，通过人工选库的设置，可以让用户轻易地过滤掉那些明显错误的库位，给视觉感知兜底；当车辆与库位的相对角度和位置使得规划无法给出好的泊车路线时，HMI屏幕上显示的库位是灰色的，不给用户选择泊入的权利，这给规划兜了底；当有动态障碍物出现在泊车路径时，暂停泊车，屏幕让用户选择是否继续泊车，这可以防止超声波雷达误报障碍物，若用户判断没有障碍物是误报，可以选择继续泊车，否则退出泊车，这给超声波雷达避障兜底。
二、感知算法
1. 视觉库位检测
1.1. 视觉系统
算法首先依赖于图像质量。在地下停车场（有的地下停车场光线昏暗、有的地下停车场地面反光非常严重）、夜晚等场景，都可能严重降低算法性能。相机是被动成像，所以成像的效果依赖于光线和相机的sensor。
- 摄像头的sensor采用的是索尼019，在低照度画质在夜晚比较好。
- 环视相机安装在后视镜的支柱上，摄像头按照位置和光轴角度保证进出接近无盲区，在车辆泊入库位后仍然能看到库位线。荣威、蔚来等车型是安装在后视镜上的，安装在后视镜支柱上相比于安装在后视镜上的方案而言更稳定，因为后视镜是需要折叠的，布置在后视镜上弊端：a. 如果后视镜处于折叠状态，则泊车功能不可用，但自动泊车功能应该独立于给人使用的后视镜的；b. 如果后视镜每次展开位置存在误差，则会带来环视相机标定的误差；c. 后视镜来回折叠导致内部相机线束跟随运动，可能造成松动或其他线束损坏；d.后视镜在使用过程中易被损坏（刮擦等），更换相机可能性大。位置如下图：
- 小鹏P7的灯光设计很好地为视觉感知服务，包括后鱼眼环视摄像头附近的一排尾部补光灯、较宽视野的前大灯（及前大灯上根据方向盘等信号打开的左右补光灯）；
- 泊车控制器业界说是tda2，GPU算力50GFlops，决定了视觉检测的天花板，深度学习网络不会特别大；
1.2. 库位检测算法
左侧&右侧环视鱼眼进行库位检测，使用的算法是深度学习神经网络（下图中库位检测算法给出部分被车辆遮挡的库位、以及墙体后大部分区域被遮挡的库位，可以推算出使用的是神经网络）。具体而言，是把库位检测问题formulate成CNN目标检测问题（核心思想参见论文：A Deep-Learning Approach for Parking Slot Detection on
Surround-View Images），让CNN基于标注库位的全局特征自动检测库位。CNN的性能远优于传统的基于角点局部特征或线局部特征的检测。另外，CNN目标检测网络可以同时给出库位是否有车的信息，只需要在标注库位时增加是否有车的分类属性。
图片截取自视频链接：https://www.bilibili.com/video/BV1Tg4y1z7Ly?from=search&seid=15892368420444322913
从小鹏P7在2020年8月实车给出的库位结果表现来看，仍然存在较大改进空间，主要体现在两点：
1）覆盖的库位类型有限，例如，花砖型库位、两条直线（无角点）的库位、库位边线磨损的库位等支持较差，其实CNN目标网络是具有非常强的泛化能力的，用来检测特征丰富的车辆、行人等尚不在话下，何况检测特征明显的库位，这一点可通过增加标注数据解决。
2）对于支持的库位类型，库位检测召回率低，这也不应该是CNN网络的正常水平，跟上面视频中直观看到的召回率是不太相符的，或许为了保证最终的泊车性能，对检测到的库位进行了较严格的过滤，毕竟CNN的误检是在所难免的。直观上我能想到的过滤策略包括：
- a. 将目标检测给出的4个角点从图像坐标根据标定参数投影到世界坐标系，校验角点之间形成的4个边之间彼此的夹角是否满足真实世界的先验知识（垂直库位、水平库位的边之间夹角应该接近90度，斜库位一般是45度、60度等）；
- b. 如果连续观察的多帧中同一角点方差过大，则不输出。
Q1：CNN进行库位检测有多种方法，什么算法更好？
基于CNN的库位检测算法对比：
| 优点 | 缺点 | |
| 角点或线检测 | 给出的局部特征位置准确 | 泛化能力差，召回率低； 需要再将局部特征组合成库位 |
| 语义分割网络 | 语义分割网络运算量大，帧率相对底 需要从分割的像素中再次提取角点或拟合成线，后处理相对复杂 |
|
| 目标检测 |
|
角点位置偏差大，需要对CNN网络做特殊优化，兼顾局部特征（如论文End-to-End Trainable One-Stage Parking Slot Detection Integrating Global and Local Information） |
Q2：库位检测到底应该在俯视拼接图上做还是在原图上做？
现在主流的库位检测是在前、后、左、右四路鱼眼相机一起制作的俯视拼接图中进行操作的，但是，从视频中看小鹏P7的方案是在鱼眼相机的原图中选取了一个ROI区域后直接做库位检测的。库位检测到底应该在俯视拼接图做还是在原图上做，这是一个值得讨论的问题，因为各有优劣。
| 原图（选取ROI后去畸变） | 俯视拼接图（四路相机拼接） | |
| CNN网络寻找feature鲁棒性 |
|
|
| 算力要求 | 为了检测两侧库位，需要将左、右两路图像分别送进CNN网络推理 | 只要将单帧送进CNN网络，对算力要求较低，实时性更强 |
| 检测范围 | 覆盖范围远，可以看到4个角点 | 由于远处物体占像素少，需要插值填充，一般拼接图仅保留距离相机5m～10m范围内的部分，在寻库阶段，距离自车较远的两个角点可能不在视野范围内 |
IDEA（待探索）：如果两个视角都很重要，或许可以在CNN网络中直接结合两者的优势给出更好的结果，例如，3D-LaneNet: End-to-End 3D Multiple Lane Detection中提出的two-pathway网络同时在俯视图和原图上进行检测。
1.3. 同步建图与定位技术
根据感知的库位结果建一个局部地图，随后对于已经建在图中的库位不再依赖于视觉的结果（检测出库位后遮盖住摄像头仍然可以正常泊车，不影响最终泊车效果），完全靠航位推算给出车辆相对于库位的位置（地库下功能正常，说明不依赖于RTK）。之所以选用这样的方案，也是因为小鹏在原图视野中做库位检测，在泊入阶段无法再看到完整的库位；这样的好处是降低了视觉感知算法的难度，毕竟只需要在寻库阶段进行检测的场景相比于泊入阶段车辆与库位发生各种相对旋转和库位遮挡的场景而言更容易。
建图的过程也是实现角点相对定位Refine的过程。检测到库位只是第一步，为了保证泊车性能，必须确保检测到的库位位置的准确性，降低库位检测误差。对于CNN目标检测网络，即是降低CNN给出的目标检测框的角点空间位置的误差。主要方法是利用角点时空位置关联性：单帧测量存在较大误差，CNN目标检测网络对于同一个库位的同一个角点在不同帧中也不可能完全一致；结合多帧信息即可降低误差，以多帧检测的角点的空间位置作为输入，把确定角点的位置formulate成经典的概率问题，可以使用卡尔曼滤波等算法，或者更简单地，可以给出各个角点的期望和方差，根据DR（自车在检测库位的同时发生移动导致库位相对运动）不断推算历史帧中检测到的角点在当前时刻出现的空间位置，当给出多帧的结果后即可计算角点的期望和方差，如下图示意图所示。当方差小于一定阈值时即锁定库位位置。（至于使用几帧的结果最优则是需要实车调试的。）
另一方面，一旦锁定库位后不再使用感知结果，完全以来航位推算泊入，这就需要整车线控和航位推算比较准确才能保证泊入效果。小鹏P7的航位推算精度调校的非常高！例如：向前移动十米左右并带有方向盘左右调整后，再将车辆退回来，车辆仍然能够针对之前检测到并建在局部地图中的库位进行高精度泊车，泊车后在库位内居中。但航位推算毕竟也有其局限，即误差累计增加：通过在库位周围进行大范围地多次调整方向盘\转弯等操作引入较大航位推算误差后泊入效果会变差，泊车后在库位内会左偏或右偏； 在库位旁掉头，即便是紧挨着库位掉头，库位在HMI界面也直接消失，不再支持泊入，说明对航位推算的边界做了限制。
Q3：检测出库位并锁定后，在泊车的过程中是否有必要使用视觉感知结果修正DR引起的误差？
这取决于系统设计方案，从小鹏的设计方案看，采用了一次规划好路径即锁定的方法，因此，泊车的过程中也没有必要修正视觉感知结果。
1.3. 泊车停止线
后环视鱼眼摄像头给出泊车停止线（遮挡住后环视鱼眼摄像头则车在已经相对库位调正、但车头尚未完全泊入库位时停止泊车）。
2. 超声波库位检测
主要覆盖两侧有车或单侧有车的场景，tesla之前已经做到的比较成熟的技术，小鹏使用的方案基本一致，不算小鹏的独特亮点，不多赘述。
3. 视觉库位检测与超声波库位检测融合
三、路径规划与控制
规划的路径简洁，这点真正做到比人泊车好（因为人是强感知的生物，但涉及到做精确计算就不如计算机厉害了）。泊车平顺连续，几乎没有原地打方向盘“揉库”的感觉。
- 支持与库位不同横向和纵向距离初始条件的路径规划；
- 路径规划对车辆航向与库位方向的夹角有一定的限制：以垂直库位为例，正常寻库时车辆航向与垂直库位之间一般是垂直关系，即与库位入口线基本平行，但当人为调整航向到与库位入口线夹角较大时，HMI界面不再给出库位可泊车的操作，但仍然保留库位的信息，当倾角回到路径规划可覆盖的范围内，又允许用户选中库位进行泊车。有的大角度情况显示可以泊车，实际规划的路径会先把车调整到和库位入口线平行，随后再按照常规的路径规划。
不足：只支持一次规划，不支持根据场景动态调整。即：在开始泊车前进行路径规划，如果中途有障碍物挡住规划路径（靠超声波雷达感知），则自动暂停泊车，不会动态调整路径，用户点击继续泊车后，则按照原来规划的路径继续泊车。
使用A*算法应该可以调到小鹏P7这个程度，如果以后使用强化学习或许可以覆盖更广泛的车辆航向与库位方向范围。
四、HMI
泊车过程支持渲染界面、俯视拼接图、后环视鱼眼的ROI图；
a. 渲染界面简洁，支持人工查看检测到的库位，以及在多个检测到的库位中手动选择其中一个进行泊入。检测到的库位以灰色框绘制出来，对于可泊车的库位以蓝色框标注。渲染界面与泊车算法配合紧密，在路径规划、航位推算精度不满足可泊车的工况下是不给出可以泊车的蓝色框示意的。在泊车过程中，当超声波freespace检测到障碍物或人为刹车接管时，车暂停泊车，但不退出泊车，在HMI界面让人选择是否继续泊车。
渲染界面存在的缺点是在连续检测到多个库位时用户将渲染的库位和实际库位对应起来的感觉不够直观，而且往往是车辆行驶过库位才给出检测到的库位，是的对应描绘界面的库位和真实库位要“琢磨一段时间”；另外，根据超声波freespace给出的结果，渲染界面会绘制车辆障碍物，但超声波freespace没有给出类别，所以会将地库的柱子等障碍物误以车的形式渲染在界面上。
b. 俯视拼接图支持透明底盘功能，透明底盘对于灯光和阴影处理较好，让用户感觉舒适；
c. 后视ROI图仍然是最直观有效，缺点是夜晚受后小鹏P7尾红色灯带影响使得画面偏红。