CN111862672A - 基于顶视图的停车场车辆自定位及地图构建方法 - Google Patents

Abstract

一种基于顶视图的停车场车辆自定位及地图构建方法，包括实时建图和自定位两部分。该方法直接在四幅鱼眼相机投影拼接形成的顶视图中检测停车场特征，无需在原始鱼眼相机图像或者矫正后的图像上进行车位检测，计算成本低，能满足实时性需求；使用增强的车位信息作为特征描述，结合轮速计对多种场景下的车位追踪问题进行处理，具有普适性；引入车位之间的位置关系约束，保证类型相同且位置相近的车位的边界保持平行，增强优化约束，提升了建图效果。该方法普适于室内、室外停车场，并且以低成本的传感器系统，在低功耗的车载处理器上可以实时地实现车辆自定位及地图构建。本公开还提供了一种构建装置、构建系统、自动驾驶车辆及自主泊车系统。

Description

基于顶视图的停车场车辆自定位及地图构建方法

技术领域

本发明涉及无人驾驶行业中的自主泊车技术领域以及计算机视觉技术领域，具体涉及驾驶车辆在停车场范围内进行停车场地图构建以及车辆自定位的方法和装置。

背景技术

近年来，自动驾驶技术发展迅猛，而解决停车难的问题也是自动驾驶领域研发的重点之一。由以倒车雷达为代表的辅助泊车系统、以超声波雷达为基础的双边障碍自动泊车技术发展到现在，自动泊车技术已经不仅仅限于泊车入库这一操作，而是扩展成包含自主低速巡航、寻车位、泊车以及召唤响应的综合泊车系统。其中，对于停车场的地图构建、地图自定位等技术作为新增的技术要点，是自动泊车研究的重点之一。

现有的技术中，大部分技术仅针对或室内或室外一种场景进行研究；部分研究在室内停车场中部署WIFI或UWB等无线设备，需要大量的基站设备，成本昂贵，同时定位精度低，不满足自主泊车需求。在以SLAM算法为基础的地图构建与自定位技术中，按照传感器不同可分为激光雷达SLAM、双目SLAM、单目SLAM等技术：其中激光SLAM受到激光设备成本的制约，难以部署在量产车型中；双目SLAM无法解决室内场景纹理稀疏问题，无法保证精度与稳定性，且双目视差计算复杂度高，车载低功耗处理器难以达到实时处理；单目SLAM受到尺度漂移问题的影响，难以获得停车场环境的绝对尺度，无法辅助车辆进行自主泊车。

为了解现有技术的发展状况，本发明对已有的专利和论文进行了检索、比较和分析：

技术方案1：专利文献CN107180215A(“基于库位和二维码的停车场自动建图与高精度定位方法”)，通过对于布置在停车场内的二维码以及车库进行检测，同时利用惯性导航设备估算自车速度与航向角，以SLAM算法实现车库平面图的构建，利用建图结果实现高精度定位。然而，由于二维码的部署问题，该方法仅适用于室内停车场；此外，图优化算法计算复杂度高，车载嵌入式设备无法实时建图。

技术方案2：专利文献CN108959321A(“停车场地图构建方法、系统、移动终端及存储介质”)，提出了基于SLAM算法，通过深度学习算法对于五个车载摄像头(一个前视相机和四个环视相机)输出的视频进行语义分割，提取地标信息和可行驶区域；利用里程计计算自车速度与航向角，进一步生成地标地图和车辆行驶轨迹；并利用可行驶区域以及行驶轨迹生成栅格地图。但是，该方法在五路摄像头上分别进行语义分割等视觉感知任务，计算复杂程度高，很难适用于低功耗的车载处理器。

技术方案3：论文“Houben S,Neuhausen M,Michael M,et al.Park marking-based vehicle self-localization with a fisheye topview system[J].Journal ofReal-Time Image Processing,2019,16(2):289-304.”，采用四个鱼眼相机组成的环视系统，独立地将每个相机采集到的图像投影到鸟瞰图中，检测明亮的直线并聚类作为停车场特征，随后用卡尔曼滤波算法将初始的粗略位姿与特征进行融合，计算精确且置信程度高的位姿与车身运动。然而，论文中提出的算法独立地计算每个镜头中的特征，增大了计算量和功耗，难以部署在车载设备上；同时在估计自车的姿态与运动的算法中，以检测到的停车场特征为主，难以应对停车场中车位消失或者断裂带来的影响，实际应用意义受到影响。

可见，自主泊车中，现有的方法在定位精度、稳定性和实时性上难以获得满意的综合效果。因此，需要研究新的停车场车辆自定位及地图构建方法，既能保证定位精度，又能适应室内、室外的背景环境，又不增加额外的计算开销，可用于低功耗车载处理器，同时无需高成本传感器系统支持。

发明内容

本发明致力于使得自主泊车技术普适于室内、室外停车场，并以低成本的传感器系统，在低功耗的车载处理器上可以实时地完成高精度的车辆自定位与停车场地图构建工作，以辅助自主泊车任务的完成。

基于上述发明目的，本发明主要在以环视系统为基础的停车场车辆自定位及地图构建技术上提出了新的方法。

为解决上述技术问题，根据本发明的一个方面，提供了一种基于顶视图的停车场车辆自定位及地图构建方法，包括实时建图和自定位两部分，其中，所述实时建图部分包括如下步骤：S11)，采集相机图像，投影生成顶视图；S12)，在所述顶视图上进行车位检测；S13)，初始化车位地图和追踪匹配，估计车位的当前姿态，筛选关键帧；S14)，局部地图优化；S15)，回环检测与地图固化；所述自定位部分包括：在所述步骤S11)、S12)之后，进行如下步骤：S23)，初始化定位，在自定位系统启动时，确定车辆自身位于地图中的坐标；S24)，追踪与重定位，确定车辆位姿。

根据至少一个实施方式，所述相机为鱼眼相机。

根据至少一个实施方式，所述鱼眼相机有四个，分辨率在720P或以上，分别布置于汽车前后保险杠以及左右后视镜下边沿。

根据至少一个实施方式，在顶视图上进行车位的检测后，并将检测到的车位的信息进行增强，生成增强的车位特征，并依据停车场规范进行帧内的坐标优化。

根据至少一个实施方式，结合轮速计的里程计信息进行融合，来估计车位的当前姿态，筛选关键帧。

根据至少一个实施方式，包括：对所述鱼眼相机进行标定；依据所述鱼眼相机的内外参数计算投影矩阵，生成映射表。

根据至少一个实施方式，包括：分别对四个所述鱼眼相机采样生成顶视图，根据四个鱼眼相机标定后的内参以及相对于汽车后轴中心的外参，计算得到新的参数矩阵以生成各鱼眼相机的原始图像中地面像素对应于顶视图像素的映射关系，得到映射表，并通过采样生成顶视图。

根据至少一个实施方式，包括：所述内参包括焦距和畸变参数；所述外参包括相对于车身的旋转平移矩阵；所述顶视图为10米*10米。

根据至少一个实施方式，包括：通过所述内参和外参计算图像中任意一地面像素点相对于车辆后轴中心的相对坐标，完成像素坐标系到车身坐标系的转换。

根据至少一个实施方式，包括：在顶视图的视角，对于车身周围10米*10*的范围采样实现栅格化，确定每个栅格内对应于原始图像像素的坐标信息。

根据至少一个实施方式，包括：所述映射表不是一对一的关系，而是一对多或多对一的关系；对于一对多关系的像素值取均值，对于多对一关系的像素值取插值后的结果值。

根据至少一个实施方式，引入车位号特征向量、车位类别和/或车位出口三角关系信息，进一步生成增强的车位特征，用于后续的特征匹配与追踪工作。

根据至少一个实施方式，在采样生成的顶视图上，利用角点提取算法检测得到车身周边的车位出口线的车位角点集合P＝{(u_i,v_i)|i＝1,2,3…n}，其中(u_i,v_i)代表第i个角点坐标，n代表角点数量；利用目标检测算法得到每个车位编号的位置框Rect＝{(uⁱ,vⁱ,wⁱ,hⁱ)|i＝1,2,3…m}，其中，(uⁱ,vⁱ)代表检测框的中心位置，(wⁱ,hⁱ)代表检测的框的宽和高，m代表单帧图像中检测到的车位数量；

首先，分成三个部分进行：

第一部分，利用车位检测算法获取车位是否为有效车位以及车位类别Class_m，m∈[0,6]；

第二部分，利用车位的出口角点与车位号的位置关系，生成相对坐标向量；

第三部分，利用特征工程的算法提取车位编号特征向量，生成唯一车位编号向量N；

然后，将上述三个部分的特征结合，生成增强的车位特征；最后，利用车位线的平行关系，优化帧内的角点坐标。

根据至少一个实施方式，所述角点提取算法为Harris角点检测+SVM分类算法。

根据至少一个实施方式，所述角点提取算法为深度学习网络算法。

根据至少一个实施方式，所述目标检测算法为深度学习网络算法。

根据至少一个实施方式，所述目标检测算法为模板匹配算法。

根据至少一个实施方式，所述车位类型包括非法车位、左水平车位、右水平车位、左垂直车位、右垂直车位、左斜车位和右斜车位共七个类别。

根据至少一个实施方式，采用OCR技术提取图像矩阵中的编号标志。

根据至少一个实施方式，采用ORB或SIFT特征描述子生成唯一车位号特征向量。

根据至少一个实施方式，所述相对坐标向量是指，经过任一车位的两出口角点坐标，与车位编号检测框的中点坐标，且依据标定和投影过程中计算得到的投影矩阵，以及计算得到的对应的车身坐标，所确定的坐标向量；

其中，

所述任一车位的两出口角点坐标如公式(1)所示：

P_2m＝(u_2m,v_2m,1)^T，P_2m+1＝(u_2m+1,v_2m+1,1)^T (1)

其中P_2m和P_2m+1分别代表第m个车位出口线的左右角点坐标，以齐次坐标的形式表示，u_2m、,v_2m、u_2m+1、v_2m+1均为以顶视图左上角为原点的像素偏移；

所述车位编号检测框的中点坐标如公式(2)所示：

Rect_m＝(u_m,v_m,1)^T (2)

其中Rect_m代表第m个车位的中点坐标，u_m、v_m均为以顶视图左上角为原点的像素偏移；

所述计算得到对应的车身坐标如公式(3)所示：

其中，

和

分别代表车身坐标系下的第m个车位出口线的左右角点坐标，

代表车身坐标系下的第m个车位的中点坐标，均以齐次坐标的形式表达，单位为米；x_2m、y_2m、x_2m+1、y_2m+1、x_m、y_m均为坐标值；

计算公式如公式(4)所示：

其中，R代表从顶视图u-v坐标系到车身坐标系的投影矩阵，其与顶视图中一个像素代表的实际距离相关，由预先定义得到；所述相对坐标向量即为车身坐标向量

在后续的追踪匹配过程中，

将被更新为地图坐标向量

根据至少一个实施方式，所述增强的车位特征E_m是由车位类别Class_m,车位相对坐标向量

以及车位号特征向量N_m组合而成，用作地图中任一车位的唯一特征代表。

根据至少一个实施方式，所述优化帧内的角点坐标是指依据停车场中类型一致、位置相近且入库角度的差值小于一定程度的的车位应当符合入库线平行这一规范，对图中的停车场队列进行优化。

根据至少一个实施方式，所述优化方法为：在计算得到增强的车位特征后，遍历车位特征列表，依据车位类型划分聚类中心，并以特征中对应角点坐标距离和入库角度的差值作为距离度量，进行聚类算法，计算出k个聚类中心；对每个聚类簇中的车位特征，取其中的车位出库角点坐标，进行直线拟合，并计算投影误差；当投影误差总和大于第一阈值时，将重新进行角点检测工作；否则，将每个角点坐标投影到该直线上，生成新的角点坐标。

根据至少一个实施方式，轮速计姿态估计的表达式如公式(5)所示：

其中，[x_t,y_t]^T代表t时刻时，当前车辆在世界坐标系下的坐标，θ_t代表t时刻时车辆的航向角；[x_t+1,y_t+1]^T代表t+1时刻时，当前车辆在世界坐标系下的坐标，θ_t+1代表t+1时刻时车辆的航向角；

Δs_l和Δs_r分别代表两帧之间左轮和右轮移动的距离，B代表轮距。

根据至少一个实施方式，车辆的t时刻的速度v_t和车辆坐标c_t，两帧间时间间隔ΔT，记t+1时刻的车辆坐标为c_t+1＝c_t+v_t·ΔT，并依次计算局部地图中的车位特征角点坐标与车辆坐标c_t+1的距离，超出顶视图视景范围的车位特征予以筛除；然后将当前帧中检测得到的车位特征列表与局部地图中映射而来的车位特征列表进行匹配追踪。

根据至少一个实施方式，所述顶视图视景范围为以车为中心，5米为半径的区域。

根据至少一个实施方式，根据所述匹配追踪的状态进行如下三种操作：

1)全部追踪成功：证明车辆处于静止状态或位移过小，利用卡尔曼系滤波将轮速计推理的位姿与观测估计的位姿进行融合优化；

2)部分追踪成功：证明车辆发生了一定的位移，用卡尔曼滤波将轮速计推理的位姿与观测估计的位姿进行融合优化，之后更新本帧内全部的车位坐标，加入关键帧候选队列；

3)全部追踪失败：仅利用轮速计信息推理位姿，加入关键帧候选队列。

根据至少一个实施方式，所述匹配追踪的判断依据为：车位类别Class_m一致，两个车位的车位相对坐标向量

以及车位号特征向量N_m之间的距离度量小于第二阈值且置信度应高于90％。

根据至少一个实施方式，所述置信度指代车位号提取过程中的车位编号为真值的概率。

根据至少一个实施方式，所述实时建图过程中，局部地图由关键帧组成，关键帧的筛选标准为：地面平坦条件下，当前帧全部追踪失败且车位检测数量不为0，或者为本帧中部分车位追踪成功且追踪失败的车位占当前帧车位检测数量的比例大于第三阈值，车辆姿态变化大于第四阈值。

根据至少一个实施方式，所述地面平坦条件是指，利用IMU或者视觉相关算法检测车身俯仰角变化，变化幅度小于第五阈值，则认为此时地面平坦。

根据至少一个实施方式，对局部地图进行优化操作，剔除冗余关键帧，并对相同的车位特征进行融合。

根据至少一个实施方式，所述对局部地图进行优化操作包括：

采用车辆的平移和旋转描述车辆的运动，i时刻的车辆位姿描述为公式(6)所示的形式：

c_i＝[t_i,θ_i]^T＝[x_i,y_i,θ_i]^T (6)

则i时刻到j时刻的位姿变化描述为公式(7)所示的形式：

其中Rⁱ为2*2的旋转矩阵，t_i和θ_i，分别代表i时刻的世界坐标和航向角，即t_i＝[x_i,y_i]^T，h(c_i,c_j)为运动方程；设定从j时刻观察到的车位点的坐标与从i时刻观察到的车位点坐标的差值为z_ij，则误差方程如公式(8)和(9)所示：

e_ij＝z_ij-h(c_i,c_j) (8)

其中e_ij代表对于同一车位在不同时刻由位姿估计和车位测量两方面影响带来的误差项，Λ_ij代表信息矩阵，F(c)为优化方程，优化目的即为最小化F(c)。

根据至少一个实施方式，采用Gauss-Newton算法或Levenberg-Marquardt算法进行优化求解。

根据至少一个实施方式，当新加入的关键帧与地图早期的关键帧有重合车位时，即为检测到了回环，并将地图中的全部车位加入优化队列，进行整体优化。

根据至少一个实施方式，当所述实时建图的工作完成后，将全部关键帧删除，只保留车位特征向量构建地图，在删除关键帧的同时，建立车位特征的词典，用于后续定位过程中的快速查找。

根据至少一个实施方式，所述增强的车位特征的词典建立方法包括以下步骤：

首先，依据已建成的图，将图的坐标轴旋转至与车位线平行；

其次，按照x轴的值，进行第一层聚类，然后按照第二层的值，进行第二次聚类，

最后，生成三层的树的结构以供快速查询。

根据至少一个实施方式，所述初始化定位完成车辆位姿的初始化，以确定车辆自身处于车位地图中的位置。

根据至少一个实施方式，所述初始化定位包括如下步骤：

首先，保持车辆处于静止状态，将k帧作为初始化帧序列，并逐帧进行顶视图投影和车位特征提取算法，设定初始序列帧中，实际共有m个车位，则应共提取出n≈k*m个车位特征；其中k表示初始化中需要的最少帧数，为预设的阈值；

其次，利用特征匹配方法对任意两帧的车位特征进行两两匹配，成功匹配一次则两帧内对应车位特征的置信权重增加1/k-1，而后在帧内优化时，加入每个角点的权重进行直线拟合，将每个角点投影到直线上后再进行聚类，取聚类中心为最终的车位特征角点；

然后，遍历地图中的车位列表，与候选车位列表的车位特征列表进行追踪匹配，此时由于车身坐标未知，因此，仅依靠车位特征中的车位号特征向量进行匹配；

最后，匹配完成后，计算当前车辆相对于地图的坐标关系，并优化位姿，最小化投影误差，最终计算出当前车辆位姿坐标。

根据至少一个实施方式，所述追踪与重定位包括，任意时刻相机采集的原始图像都被采样生成顶视图，并提取出待匹配车位特征列表，依据匀速运动模型，估计此时刻车辆的姿态，从而将本帧的车位坐标从车身坐标系转换至世界坐标系，然后通过x轴坐标和y轴坐标的两次查询，快速查找出地图的车位特征列表中与当前待搜索车位特征最邻近的特征，继而进行匹配追踪；

如果追踪成功，则计算当前车辆位姿并结合轮速计信息利用滤波器进行优化，不断更新车辆的位姿状态；

如果连续追踪失败时，则进行重定位，所述重定位对地图内的全部车位特征进行逐一匹配，获得匹配队列后，计算当前车辆位姿，计算车辆相对于地图的位姿坐标。

根据本公开的另一方面，一种基于顶视图的停车场车辆自定位及地图构建装置，包括实时建图装置和自定位装置，其中，所述实时建图装置包括：图像采集装置，采集相机图像，投影生成顶视图；车位检测装置，在所述顶视图上进行车位检测；初始化与追踪匹配装置，初始化车位地图和追踪匹配，估计车位的当前姿态，筛选关键帧；优化装置，对局部地图进行优化；回环检测装置，进行回环检测与地图固化；所述自定位装置包括：初始化装置，进行初始化定位，在自定位系统刚启动时，确定车辆自身位于地图中的坐标；定位装置，进行追踪与重定位操作，确定车辆位姿。

根据至少一个实施方式，所述相机为鱼眼相机。

根据至少一个实施方式，所述鱼眼相机有四个，分辨率在720P或以上，分别布置于汽车前后保险杠以及左右后视镜下边沿。

根据至少一个实施方式，在顶视图上进行车位的检测后，并将检测到的车位的信息进行增强，生成增强的车位特征，并依据停车场规范进行帧内的坐标优化。

根据至少一个实施方式，结合轮速计的里程计信息进行融合，来估计车位的当前姿态，筛选关键帧。

根据至少一个实施方式，包括：对所述鱼眼相机进行标定；依据所述鱼眼相机的内外参数计算投影矩阵，生成映射表。

根据至少一个实施方式，分别对四个所述鱼眼相机采样生成顶视图，根据四个鱼眼相机标定后的内参以及相对于汽车后轴中心的外参，计算得到新的参数矩阵以生成各鱼眼相机的原始图像中地面像素对应于顶视图像素的映射关系，得到映射表，并通过采样生成顶视图。

根据至少一个实施方式，所述内参包括焦距和畸变参数；所述外参包括相对于车身的旋转平移矩阵；所述顶视图为10米*10米。

根据至少一个实施方式，通过所述内参和外参计算图像中任意一地面像素点相对于车辆后轴中心的相对坐标，完成像素坐标系到车身坐标系的转换。

根据至少一个实施方式，在顶视图的视角，对于车身周围10米*10*的范围采样实现栅格化，确定每个栅格内对应于原始图像像素的坐标信息。

根据至少一个实施方式，所述映射表不是一对一的关系，而是一对多或多对一的关系；对于一对多关系的像素值取均值，对于多对一关系的像素值取插值后的结果值。

根据至少一个实施方式，引入车位号特征向量、车位类别和/或车位出口三角关系信息，进一步生成增强的车位特征，用于后续的特征匹配与追踪工作。

根据至少一个实施方式，在采样生成的顶视图上，利用角点提取算法检测得到车身周边的车位出口线的车位角点集合P＝{(u_i,v_i)|i＝1,2,3…n}，其中(u_i,v_i)代表第i个角点坐标，n代表角点数量；利用目标检测算法得到每个车位编号的位置框Rect＝{(uⁱ,vⁱ,wⁱ,hⁱ)|i＝1,2,3…m}，其中，(uⁱ,vⁱ)代表检测框的中心位置，(wⁱ,hⁱ)代表检测的框的宽和高，m代表单帧图像中检测到的车位数量；

首先，分成三个部分进行：

第一部分，利用车位检测算法获取车位是否为有效车位以及车位类别Class_m，m∈[0,6]；

第二部分，利用车位的出口角点与车位号的位置关系，生成相对坐标向量；

第三部分，利用特征工程的算法提取车位编号特征向量，生成唯一车位编号向量N；

然后，将上述三个部分的特征结合，生成增强的车位特征；最后，利用车位线的平行关系，优化帧内的角点坐标。

根据至少一个实施方式，所述角点提取算法为Harris角点检测+SVM分类算法。

根据至少一个实施方式，所述角点提取算法为深度学习网络算法。

根据至少一个实施方式，所述目标检测算法为深度学习网络算法。

根据至少一个实施方式，所述目标检测算法为模板匹配算法。

根据至少一个实施方式，所述车位类型包括非法车位、左水平车位、右水平车位、左垂直车位、右垂直车位、左斜车位和右斜车位共七个类别。

根据至少一个实施方式，采用OCR技术提取图像矩阵中的编号标志。

根据至少一个实施方式，采用ORB或SIFT特征描述子生成唯一车位号特征向量。

根据至少一个实施方式，所述相对坐标向量是指，经过任一车位的两出口角点归一化坐标，与车位编号检测框的中点坐标，且依据标定和投影过程中计算得到的投影矩阵，以及计算得到的对应的车身坐标，所确定的坐标向量；

其中，

所述任一车位的两出口角点归一化坐标如公式(1)所示：

P_2m＝(u_2m,v_2m,1)^T，P_2m+1＝(u_2m+1,v_2m+1,1)^T (1)

其中P_2m和P_2m+1分别代表第m个车位出口线的左右角点坐标，以齐次坐标的形式表示，u_2m、,v_2m、u_2m+1、v_2m+1均为以顶视图左上角为原点的像素偏移；

所述车位编号检测框的中点坐标如公式(2)所示：

Rect_m＝(u_m,v_m,1)^T (2)

其中Rect_m代表第m个车位的中点坐标，u_m、v_m均为以顶视图左上角为原点的像素偏移；

所述计算得到对应的车身坐标如公式(3)所示：

其中，

和

分别代表车身坐标系下的第m个车位出口线的左右角点坐标，

代表车身坐标系下的第m个车位的中点坐标，均以齐次坐标的形式表达，单位为米；x_2m、y_2m、x_2m+1、y_2m+1、x_m、y_m均为坐标值；

计算公式如公式(4)所示：

其中，R代表从顶视图u-v坐标系到车身坐标系的投影矩阵，其与顶视图中一个像素代表的实际距离相关，由预先定义得到；所述相对坐标向量即为车身坐标向量

在后续的追踪匹配过程中，

将被更新为地图坐标向量

根据至少一个实施方式，所述增强的车位特征E_m是由车位类别Class_m,车位相对坐标向量

以及车位号特征向量N_m组合而成，用作地图中任一车位的唯一特征代表。

根据至少一个实施方式，所述优化帧内的角点坐标是指依据停车场中类型一致、位置相近且入库角度的差值小于一定程度的的车位应当符合入库线平行这一规范，对图中的停车场队列进行优化。

根据至少一个实施方式，所述优化的方法为：在计算得到增强的车位特征后，遍历车位特征列表，依据车位类型划分聚类中心，并以特征中对应角点坐标距离和入库角度的差值作为距离度量，进行聚类算法，计算出k个聚类中心；对每个聚类簇中的车位特征，取其中的车位出库角点坐标，进行直线拟合，并计算投影误差；当投影误差总和大于第一阈值时，将重新进行角点检测工作；否则，将每个角点坐标投影到该直线上，生成新的角点坐标。

根据至少一个实施方式，轮速计姿态估计的表达式如公式(5)所示：

其中，[x_t,y_t]^T代表t时刻时，当前车辆在世界坐标系下的坐标，θ_t代表t时刻时车辆的航向角；[x_t+1,y_t+1]^T代表t+1时刻时，当前车辆在世界坐标系下的坐标，θ_t+1代表t+1时刻时车辆的航向角；

Δs_l和Δs_r分别代表两帧之间左轮和右轮移动的距离，B代表轮距。

根据至少一个实施方式，车辆的t时刻的速度v_t和车辆坐标c_t，两帧间时间间隔ΔT，记t+1时刻的车辆坐标为c_t+1＝c_t+v_t·ΔT，并依次计算局部地图中的车位特征角点坐标与车辆坐标c_t+1的距离，超出顶视图视景范围的车位特征予以筛除；然后将当前帧中检测得到的车位特征列表与局部地图中映射而来的车位特征列表进行匹配追踪。

根据至少一个实施方式，所述顶视图视景范围为以车为中心，5米为半径的区域。

根据至少一个实施方式，根据所述匹配追踪的状态进行如下三种操作：

1)全部追踪成功：证明车辆处于静止状态或位移过小，利用卡尔曼系滤波将轮速计推理的位姿与观测估计的位姿进行融合优化；

2)部分追踪成功：证明车辆发生了一定的位移，用卡尔曼滤波将轮速计推理的位姿与观测估计的位姿进行融合优化，之后更新本帧内全部的车位坐标，加入关键帧候选队列；

3)全部追踪失败：仅利用轮速计信息推理位姿，加入关键帧候选队列。

根据至少一个实施方式，所述匹配追踪的判断依据为：车位类别Class_m一致，两个车位的车位相对坐标向量

以及车位号特征向量N_m之间的距离度量小于第二阈值且置信度应高于90％。

根据至少一个实施方式，所述置信度指代车位号提取过程中的车位编号为真值的概率。

根据至少一个实施方式，所述实时建图过程中，局部地图由关键帧组成，关键帧的筛选标准为：地面平坦条件下，当前帧全部追踪失败且车位检测数量不为0，或者为本帧中部分车位追踪成功且追踪失败的车位占当前帧车位检测数量的比例大于第三阈值，车辆姿态变化大于第四阈值。

根据至少一个实施方式，所述地面平坦条件是指，利用IMU或者视觉相关算法检测车身俯仰角变化，变化幅度小于第五阈值，则认为此时地面平坦。

根据至少一个实施方式，对局部地图进行优化操作，剔除冗余关键帧，并对相同的车位特征进行融合。

根据至少一个实施方式，所述对局部地图进行优化操作包括：

采用车辆的平移和旋转描述车辆的运动，i时刻的车辆位姿描述为公式(6)所示的形式：

c_i＝[t_i,θ_i]^T＝[x_i,y_i,θ_i]^T (6)

则i时刻到j时刻的位姿变化描述为公式(7)所示的形式：

其中Rⁱ为2*2的旋转矩阵，t_i和θ_i，分别代表i时刻的世界坐标和航向角，即t_i＝[x_i,y_i]^T，h(c_i,c_j)为运动方程；设定从j时刻观察到的车位点的坐标与从i时刻观察到的车位点坐标的差值为z_ij，则误差方程如公式(8)和(9)所示：

e_ij＝z_ij-h(c_i,c_j) (8)

其中e_ij代表对于同一车位在不同时刻由位姿估计和车位测量两方面影响带来的误差项，Λ_ij代表信息矩阵，F(c)为优化方程，优化目的即为最小化F(c)。

根据至少一个实施方式，采用Gauss-Newton算法或Levenberg-Marquardt算法进行优化求解。

根据至少一个实施方式，当新加入的关键帧与地图早期的关键帧有重合车位时，即为检测到了回环，并将地图中的全部车位加入优化队列，进行整体优化。

根据至少一个实施方式，当所述实时建图的工作完成后，将全部关键帧删除，只保留车位特征向量构建地图，在删除关键帧的同时，建立车位特征的词典，用于后续定位过程中的快速查找。

根据至少一个实施方式，所述增强的车位特征的词典建立方法包括以下步骤：

首先，依据已建成的图，将图的坐标轴旋转至与车位线平行；

其次，按照x轴的值，进行第一层聚类，然后按照第二层的值，进行第二次聚类，

最后，生成三层的树的结构以供快速查询。

根据至少一个实施方式，所述初始化定位完成车辆位姿的初始化，以确定车辆自身处于车位地图中的位置。

根据至少一个实施方式，所述初始化定位包括如下步骤：

首先，保持车辆处于静止状态，将k帧作为初始化帧序列，并逐帧进行顶视图投影和车位特征提取算法，设定初始序列帧中，实际共有m个车位，则应共提取出n≈k*m个车位特征；其中k表示初始化中需要的最少帧数，为预设的阈值。

其次，利用特征匹配方法对任意两帧的车位特征进行两两匹配，成功匹配一次则两帧内对应车位特征的置信权重增加1/k-1，而后在帧内优化时，加入每个角点的权重进行直线拟合，将每个角点投影到直线上后再进行聚类，取聚类中心为最终的车位特征角点；

然后，遍历地图中的车位列表，与候选车位列表的车位特征列表进行追踪匹配，此时由于车身坐标未知，因此，仅依靠车位特征中的车位号特征向量进行匹配；

最后，匹配完成后，计算当前车辆相对于地图的坐标关系，并优化位姿，最小化投影误差，最终计算出当前车辆位姿坐标。

根据至少一个实施方式，所述追踪与重定位包括，任意时刻相机采集的原始图像都被采样生成顶视图，并提取出待匹配车位特征列表，依据匀速运动模型，估计此时刻车辆的姿态，从而将本帧的车位坐标从车身坐标系转换至世界坐标系，然后通过x轴坐标和y轴坐标的两次查询，快速查找出地图的车位特征列表中与当前待搜索车位特征最邻近的特征，继而进行匹配追踪；

如果追踪成功，则计算当前车辆位姿并结合轮速计信息利用滤波器进行优化，不断更新车辆的位姿状态；

如果连续追踪失败时，则进行重定位，所述重定位对地图内的全部车位特征进行逐一匹配，获得匹配队列后，计算当前车辆位姿，计算车辆相对于地图的位姿坐标。

根据本公开的另一方面，一种基于顶视图的停车场车辆自定位及地图构建系统，包括：

存储器，所述存储器存储执行如上所述方法的程序；

处理器；所述处理器执行所述程序。

根据本公开的再一方面，一种自动驾驶车辆，包括：

所述自动驾驶车辆上搭载如上所述的装置或如上所述的系统。

根据本公开的再一方面，一种自主泊车系统，包括：

如上所述的装置或如上所述的系统。

本发明的有益效果：

1、相比于现有技术方法在直接获取到的原始图像中进行特征检测工作，本发明直接在四幅鱼眼相机投影拼接形成的顶视图中检测停车场特征，仅特征提取部分的计算量即可降低约3/4，降低了计算成本，能够满足实时性要求；

2、本发明使用增强的车位信息作为特征描述，结合轮速计对多种场景下的车位追踪问题进行处理，具有普适性；

3、引入车位之间的位置关系约束，保证类型相同且位置相近的车位的边界保持平行，增强优化约束，提升了建图效果；

4、采用顶视图作为数据处理的数据源，计算量低，且尺度明确，无需考虑尺度漂移问题；

5、本发明采用的增强的车位特征，语义明确，匹配精准度高、优化速度快，更容易满足实时需求；

6、提出了车位语义拓扑地图，仅保留车位的增强特征作为地图的组成元素，大大降低了地图的数据规模，缩短了建图与车辆自定位时的时间消耗，满足云端快速传输与车辆在地图内的精准定位。

附图说明

附图示出了本发明的示例性实施方式，并与其说明一起用于解释本发明的原理，其中包括了这些附图以提供对本发明的进一步理解，并且附图包括在本说明书中并构成本说明书的一部分。通过结合附图对本发明的实施例进行详细描述，本发明的上述和其它目的、特征、优点将更加清楚。

图1是整体流程图；

图2是顶视图生成流程图；

图3是生成的顶视图；

图4是车位检测与特征增强流程图；

图5是车位角点及车位号检测结果示意图；

图6是关键帧筛选流程图。

具体实施方式

下面结合附图和实施方式对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施方式仅用于解释相关内容，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分。

需要说明的是，在不冲突的情况下，本发明中的实施方式及实施方式中的特征可以相互组合。下面将参考附图并结合实施方式来详细说明本发明。

本发明的目的是提供一种基于顶视图的停车场车辆自定位及地图构建方法。图1描述了本发明的整体流程图。如图1所示，本发明共分为实时建图和自定位两项独立的任务。

对于实时建图任务来讲，整体流程共分为五个主要步骤：第一步，采集鱼眼相机图像，投影生成顶视图；第二步，在顶视图上进行车位的检测，并将检测到的车位的信息进行增强，生成增强的车位特征，并依据停车场规范进行帧内的坐标优化工作；第三步，车位地图的初始化和追踪匹配工作，并结合包括但不限于轮速计的里程计信息进行融合，估计车位的当前姿态，筛选关键帧；第四步，局部地图优化；第五步，回环检测与地图固化。

对于自定位任务，整体流程可分为四部分，第一步与第二步与建图任务的一、二步一致，第三步为初始化工作，在系统刚启动时，确定自身位于地图中的坐标；第四步为追踪与重定位。

1、投影生成顶视图

本发明的数据采集系统由四个鱼眼相机组成，分辨率在720P或以上，分别布置于汽车前后保险杠以及左右后视镜下边沿，以采集车身周围的视觉数据。相比于针孔相机，鱼眼相机的视场角更大，基本一般大于180°，四个相机基本可以完全覆盖掉车身周围的场景；但鱼眼相机的畸变更为严重，且不适用于透视投影模型，需要对相机进行精准的标定工作，得到相机的内外参数，标定方法包括但不限于斯卡拉标定法。

一般的车位检测算法通常是在原始的鱼眼相机图像或者矫正后的图像上，利用机器学习或者深度学习的算法进行车位检测，数据计算量大且计算复杂度高；相比于上述算法，本发明提出成本小且计算复杂度更低的解决方案，首先根据四个鱼眼相机标定后的内参以及相对于汽车后轴中心的外参，可以计算得到新的参数矩阵以生成各鱼眼相机的原始图像中地面像素对应于顶视图像素的映射关系，得到一张映射表，并最终通过采样生成一张10米*10米的顶视图，再在顶视图上进行后续的检测工作。具体的计算流程见图2，顶视图示意图见图3。

上述映射表的生成流程为：依据标定算法，可获得每个相机镜头的内参(通常为焦距和畸变参数)和外参(相对于车身的旋转平移矩阵)，继而可以计算图像中任意一地面像素点相对于车辆后轴中心的相对坐标，即完成了像素坐标系到车身坐标系的转换；从而可以在顶视图的视角，对于车身周围10米*10*的范围采样实现栅格化，最终可以确定每个栅格内对应于原始图像像素的坐标信息。需要注意的是，在原始图像中，距离信息在像素中分布的密度是不均匀的，距离相机中心越远，像素表征的距离范围则越广。因此，映射表并不是一对一的关系，而是一对多和多对一。在最终采样生成具体的顶视图时，需要对于一对多关系的像素值取均值，对于多对一关系的像素值取插值后的结果值。

2、车位检测与特征增强

相比于其他方法在4个原始鱼眼相机采集到的图像上进行特征提取、车位检测等消耗大量计算资源的工作，本发明提出在采样生成的顶视图上进行车位的检测工作，相比较来讲，计算资源仅占1/4；并在传统的车位特征上，引入车位号特征向量、车位类别、车位出口三角关系等信息，进一步生成增强的车位特征，用于后续的特征匹配与追踪工作。

如图4所示，在采样生成的顶视图上，利用角点提取算法检测得到车身周边的车位出口线的车位角点集合P＝{(u_i,v_i)|i＝1,2,3…n}，其中(u_i,v_i)代表第i个角点坐标，并利用目标检测算法得到每个车位编号的位置框Rect＝{(uⁱ,vⁱ,wⁱ,hⁱ)|i＝1,2,3…m}，其中，(uⁱ,vⁱ)代表检测框的中心位置，(wⁱ,hⁱ)代表检测的框的宽和高，后续分成三个部分进行：第一部分利用车位检测算法获取车位是否为有效车位以及车位类型Class_m＝{0,1,2,3,4,5,6}；第二部分利用车位的出口角点与车位号的位置关系，生成相对坐标向量；第三部分利用特征工程的相关算法提取车位编号特征向量，生成唯一车位编号向量N。然后，将上述三部分的特征结合，生成增强的车位特征；最后，利用车位线的平行关系，优化帧内的角点坐标。

上述角点提取算法包括但不限于Harris角点检测+SVM分类算法，深度学习网络算法等；

上述车位检测算法包括但不限于深度学习网络算法，模板匹配算法等；

上述车位类型主要包括非法车位、水平车位(左，右)、垂直车位(左，右)和斜车位(左，右)共七个类别。顶视图上检测出的车位角点、车位号区域的如图5所示。

上述特征工程算法的目的为提取唯一的车位标号特征向量，重点在于唯一性。既可以是利用OCR技术提取图像矩阵中的编号标志，也可以利用ORB、SIFT等特征描述子生成唯一车位号特征向量；

上述相对坐标向量是指：经过相关技术获取的任一车位的两出口角点归一化坐标

P_2m＝(u_2m,v_2m,1)^T，P_2m+1＝(u_2m+1,v_2m+1,1)^T (1)

与车位编号检测框的中点坐标

Rect_m＝(u_m,v_m,1)^T (2)

且依据标定和投影过程中计算得到的投影矩阵R，可计算得到对应的车身坐标

计算公式如下：

其中R代表从顶视图u-v坐标系到车身坐标系的投影矩阵，与顶视图中一个像素代表的实际距离相关，预先定义得到。所述相对坐标向量即为车身坐标向量

在后续的追踪匹配过程中，

将被更新为地图坐标向量

上述增强的车位特征E_m是由车位类别Class_m,车位相对坐标向量

以及车位号特征向量N_m组合而成，用作地图中任一车位的唯一特征代表。

上述帧内坐标优化是指依据停车场中类型一致、位置相近且入库角度的差值小于一定程度的的车位应当符合入库线平行这一规范，对图中的停车场队列进行优化。

具体的优化方法为：在计算得到增强的车位特征后，遍历车位特征列表，依据车位类型划分聚类中心，并以特征中对应角点坐标距离和入库角度的差值作为距离度量，进行聚类算法，计算出k个聚类中心。对每个聚类簇中的车位特征，取其中的车位出库角点坐标，进行直线拟合，并计算投影误差；当投影误差总和大于一定阈值时，将重新进行角点检测工作；否则，将每个角点坐标投影到该直线上，生成新的角点坐标。

相比于其他SLAM系统通过提取SIFT、ORB等特征描述子进行特征的提取匹配，本发明采用具有高级语义特性的增强车位特征进行特征的匹配工作，相比于上述SLAM系统的特征描述子，增强车位特征具有更明确的语义，更容易完成匹配工作；同时，本发明建立车位语义拓扑地图，仅保留停车场内全部车位的增强特征，数据量少，优化、计算速度更快。

3、在建图和自定位阶段

本发明引入了轮速计作为车辆的姿态估计的补充优化，其姿态估计模型如下：相比于无人机需要估算三个维度的欧拉角和空间上三个维度的运动进行姿态观测，自动泊车系统仅需要观测空间中两个维度的运动以及一个维度的欧拉角即可完成姿态的估计，即平行于地面的[x,y]^T坐标与偏航角θ，更一般地，使用[x,y,θ]^T作为指代。则有轮速计姿态估计的表达式如下：

其中，

Δs_l和Δs_r分别代表两帧之间左轮和右轮移动的距离，B代表轮距。

如图6所示，给定车辆的t时刻的速度v_t和车辆坐标c_t，两帧间时间间隔ΔT，可以粗略得计算出t+1时刻的车辆坐标c_t+1＝c_t+v_t·ΔT，并依次计算局部地图中的车位特征角点坐标与车辆坐标c_t+1的距离，超出顶视图视景范围(5米)的车位特征予以筛除。后将当前帧中检测得到的车位特征列表与局部地图中映射而来的车位特征列表进行匹配追踪，会有三种状态：1)全部追踪成功：则证明车辆处于静止状态或位移过小，利用卡尔曼系滤波将轮速计推理的位姿与观测估计的位姿进行融合优化；2)部分追踪成功：证明车辆发生了一定的位移，用卡尔曼滤波将轮速计推理的位姿与观测估计的位姿进行融合优化，之后更新本帧内全部的车位坐标，加入关键帧候选队列；3)全部追踪失败：仅利用轮速计信息推理位姿，加入关键帧候选队列。

上述的匹配追踪策略指：车位类别Class_m一致，两个车位的车位相对坐标向量

以及车位号特征向量N_m之间的距离度量小于一定阈值且置信度应高于90％；置信度指代车位号提取过程中的车位编号为真值的概率。

建图过程中，局部地图由关键帧组成，关键帧的筛选标准为：地面平坦条件下，当前帧全部追踪失败且车位检测数量不为0，或者为本帧中部分车位追踪成功且追踪失败的车位占当前帧车位检测数量的比例大于一定阈值，车辆姿态变化大于一定阈值。

上述地面平坦条件是指，利用IMU或者视觉相关算法检测车身俯仰角变化，变化幅度小于一定阈值，则认为此时地面平坦。

插入关键帧时，需要对局部地图进行优化操作，剔除冗余关键帧，并对相同的车位特征进行融合。

优化策略：由于仅考虑车辆的地面运动且车位基本位于地面，需要考虑的运动变量仅为车辆的平移和旋转，即i时刻的车辆位姿可描述为：

c_i＝[t_i,θ_i]^T＝[x_i,y_i,θ_i]^T (6)

则i时刻到j时刻的位姿变化描述为公式(7)所示的形式：

其中Rⁱ为2*2的旋转矩阵，t_i和θ_i，分别代表i时刻的世界坐标和航向角，即t_i＝[x_i,y_i]^T，h(c_i,c_j)为运动方程；设定从j时刻观察到的车位点的坐标与从i时刻观察到的车位点坐标的差值为z_ij，则误差方程如公式(8)和(9)所示：

e_ij＝z_ij-h(c_i,c_j) (8)

其中e_ij代表对于同一车位在不同时刻由位姿估计和车位测量两方面影响带来的误差项，Λ_ij代表信息矩阵。

其中F(c)为优化方程，上述优化目的即为最小化F(c)。解上述优化问题的算法包括但不限于Gauss-Newton算法和Levenberg-Marquardt算法。

4、回环检测与优化：

当新加入的关键帧与地图早期的关键帧有重合车位时，即为检测到了回环，并将地图中的全部车位加入优化队列，利用上述优化策略进行优化处理，进行整体优化。

5、网络固化：

当建图工作完成后，将全部关键帧删除，只保留车位特征向量构建地图，在删除关键帧的同时，建立车位特征的词典，方便后续定位过程中的快速查找。与其他SLAM算法采取关键帧作为地图的组成元素相比，本发明采用车位特征向量表征地图，其特点是关键帧内特征的距离更明确(不存在尺度问题)，语义级别更高且数量量级低，且具有强唯一性；因此，舍弃关键帧而采取车位特征向量进行构建地图可以有效的降低地图的存储和传输成本，提升匹配效率，同时保证特征地图的高精度。增强特征的词典建立方式如下：首先，依据已建成的图，将图的坐标轴旋转至与车位线平行；后续按照x轴的值，进行第一层聚类，然后按照第二层的值，进行第二次聚类，最终生成三层的树的结构以供快速查询。

6、初始化定位：

在进行车辆自定位过程中，需要完成车辆位姿的初始化，以确定自身处于车位地图中的位置。初始化定位的流程为：保持车辆处于静止状态，将k帧作为初始化帧序列，并逐帧进行顶视图投影和车位特征提取算法，假设初始序列帧中，实际共有m个车位，则应共提取出n≈k*m个车位特征；然后，利用上述的特征匹配算法，对任意两帧的车位特征进行两两匹配，成功匹配一次则两帧内对应车位特征的置信权重增加1/k-1，而后在上述的帧内优化策略中，加入每个角点的权重进行直线拟合，将每个角点投影到直线上后再进行聚类，取聚类中心为最终的车位特征角点。后续遍历地图中的车位列表，与候选车位列表的车位特征列表进行追踪匹配工作，此时由于车身坐标未知，仅依靠车位特征中的车位号特征向量进行匹配；匹配完成后，计算当前车辆相对于地图的坐标关系，并利用上述的优化策略优化位姿，最小化投影误差，最终计算出当前车辆位姿坐标。

7、追踪与重定位：

初始化定位工作完成后，定位工作将进入追踪优化阶段，任意时刻相机采集的原始图像都将被采样生成顶视图，并提取出待匹配车位特征列表，然后同样依据上述的匀速运动模型，估计此时刻车辆的姿态，从而将本帧的车位坐标从车身坐标系转换至世界坐标系，然后通过x轴坐标和y轴坐标的两次查询，快速查找出地图的车位特征列表中与当前待搜索车位特征最邻近的特征，继而进行匹配追踪算法。追踪成功，则计算当前车辆位姿并结合轮速计信息利用滤波器进行优化，不断更新车辆的位姿状态；连续追踪失败时，则需要启动重定位算法，重定位算法对地图内的全部车位特征进行逐一匹配，获得匹配队列后，计算当前车辆位姿，计算车辆相对于地图的位姿坐标。

自主泊车中，现有的方法在定位精度、稳定性和实时性上难以获得满意的综合效果。相比于现有技术方法在直接获取到的原始图像中进行特征检测工作，本发明直接在四幅鱼眼相机投影拼接形成的顶视图中检测停车场特征，仅特征提取部分的计算量即可降低约3/4，降低了计算成本，能够满足实时性要求；本发明使用增强的车位信息作为特征描述，结合轮速计对多种场景下的车位追踪问题进行处理，具有普适性；本发明引入车位之间的位置关系约束，保证类型相同且位置相近的车位的边界保持平行，增强优化约束，提升了建图效果；本发明采用顶视图作为数据处理的数据源，计算量低，且尺度明确，无需考虑尺度漂移问题；本发明采用的增强的车位特征，语义明确，匹配精准度高、优化速度快，更容易满足实时需求；本发明提出的车位语义拓扑地图，仅保留车位的增强特征作为地图的组成元素，大大降低了地图的数据规模，缩短了建图与车辆自定位时的时间消耗，满足云端快速传输与车辆在地图内的精准定位。

由此可见，本发明提出的新的停车场车辆自定位及地图构建方法，既能保证定位精度，又能适应室内、室外的背景环境，又不增加额外的计算开销，可用于低功耗车载处理器，同时也无需高成本传感器系统支持。采用本发明的新的停车场车辆自定位及地图构建方法的自主泊车技术普适于室内、室外停车场，并能以低成本的传感器系统、低功耗的车载处理器系统获得高效的辅助自主泊车效果，具有广泛的应用前景。

至此，已经结合附图所示的优选实施方法描述了本发明的技术方案，但是，本领域技术人员应当理解，上述实施方式仅仅是为了清楚地说明本发明，而并非是对本发明的范围进行限定，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征作出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。

Claims (10)

1.一种基于顶视图的停车场车辆自定位及地图构建方法，其特征在于，包括实时建图和自定位两部分，其中，

所述实时建图部分包括如下步骤：

S11)，采集相机图像，投影生成顶视图；

S12)，在所述顶视图上进行车位检测；

S13)，初始化车位地图和追踪匹配，估计车位的当前姿态，筛选关键帧；

S14)，局部地图优化；

S15)，回环检测与地图固化；

所述自定位部分包括：

在所述步骤S11)、S12)之后，进行如下步骤：

S23)，初始化定位，在自定位系统启动时，确定车辆自身位于地图中的坐标；

S24)，追踪与重定位，确定车辆位姿。

2.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法，其特征在于，所述相机为鱼眼相机。

3.根据权利要求2所述的基于顶视图的停车场车辆自定位及地图构建方法，其特征在于，所述鱼眼相机有四个，分辨率在720P或以上，分别布置于汽车前后保险杠以及左右后视镜下边沿。

4.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法，其特征在于，在顶视图上进行车位的检测后，并将检测到的车位的信息进行增强，生成增强的车位特征，并依据停车场规范进行帧内的坐标优化。

5.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法，其特征在于，结合轮速计的里程计信息进行融合，来估计车位的当前姿态，筛选关键帧。

6.根据权利要求2或3所述的基于顶视图的停车场车辆自定位及地图构建方法，其特征在于，包括：

对所述鱼眼相机进行标定；

依据所述鱼眼相机的内外参数计算投影矩阵，生成映射表。

7.一种基于顶视图的停车场车辆自定位及地图构建装置，其特征在于，包括实时建图装置和自定位装置，其中，

所述实时建图装置包括：

图像采集装置，采集相机图像，投影生成顶视图；

车位检测装置，在所述顶视图上进行车位检测；

初始化与追踪匹配装置，初始化车位地图和追踪匹配，估计车位的当前姿态，筛选关键帧；

优化装置，对局部地图进行优化；

回环检测装置，进行回环检测与地图固化；

所述自定位装置包括：

初始化装置，进行初始化定位，在自定位系统刚启动时，确定车辆自身位于地图中的坐标；

定位装置，进行追踪与重定位操作，确定车辆位姿。

8.一种基于顶视图的停车场车辆自定位及地图构建系统，其特征在于，包括：

存储器，所述存储器存储执行如权利要求1所述方法的程序；

处理器；所述处理器执行所述程序。

9.一种自动驾驶车辆，其特征在于，包括：

所述自动驾驶车辆上搭载如权利要求7所述的装置或权利要求8所述的系统。

10.一种自主泊车系统，其特征在于，包括：

如权利要求7所述的装置或如权利要求8所述的系统。

Images