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

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

Info

Publication number
CN111862673A
CN111862673A CN202010588699.3A CN202010588699A CN111862673A CN 111862673 A CN111862673 A CN 111862673A CN 202010588699 A CN202010588699 A CN 202010588699A CN 111862673 A CN111862673 A CN 111862673A
Authority
CN
China
Prior art keywords
vehicle
map
top view
line
angular
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202010588699.3A
Other languages
English (en)
Other versions
CN111862673B (zh
Inventor
李一龙
陈禹行
范圣印
李雪
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Yihang Yuanzhi Technology Co Ltd
Original Assignee
Beijing Yihang Yuanzhi Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Yihang Yuanzhi Technology Co Ltd filed Critical Beijing Yihang Yuanzhi Technology Co Ltd
Priority to CN202010588699.3A priority Critical patent/CN111862673B/zh
Publication of CN111862673A publication Critical patent/CN111862673A/zh
Application granted granted Critical
Publication of CN111862673B publication Critical patent/CN111862673B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G1/00—Traffic control systems for road vehicles
    • G08G1/14—Traffic control systems for road vehicles indicating individual free spaces in parking areas
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T3/00—Geometric image transformation in the plane of the image
    • G06T3/40—Scaling the whole image or part thereof
    • G06T3/4038—Scaling the whole image or part thereof for image mosaicing, i.e. plane images composed of plane sub-images
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/50—Context or environment of the image
    • G06V20/56—Context or environment of the image exterior to a vehicle by using sensors mounted on the vehicle
    • G06V20/58—Recognition of moving objects or obstacles, e.g. vehicles or pedestrians; Recognition of traffic objects, e.g. traffic signs, traffic lights or roads
    • G06V20/586—Recognition of moving objects or obstacles, e.g. vehicles or pedestrians; Recognition of traffic objects, e.g. traffic signs, traffic lights or roads of parking space
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04N—PICTORIAL COMMUNICATION, e.g. TELEVISION
    • H04N7/00—Television systems
    • H04N7/18—Closed-circuit television [CCTV] systems, i.e. systems in which the video signal is not broadcast
    • H04N7/181—Closed-circuit television [CCTV] systems, i.e. systems in which the video signal is not broadcast for receiving images from a plurality of remote sources
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10004—Still image; Photographic image
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior

Abstract

一种基于顶视图的停车场车辆自定位及地图构建方法，以低成本的鱼眼相机组成的环视系统作为信息采集系统，基于所述环视系统生成顶视图，鲁棒性地提取车辆附近车位线的角线特征，进行跟踪和建图；利用图匹配技术实时生成角线地图，并利用局部地图优化和全局地图优化，对车辆进行实时高精度定位和地图构建。所述方法从原理上避免了单目SLAM的尺度漂移问题，并利用现有的车载系统，以低成本传感器、在低功耗的车载处理器上完成实时、高精度车辆自定位和地图构建工作，来辅助自主泊车任务的完成。本公开还提供了一种构建装置、构建系统、自动驾驶车辆及自主泊车系统。

Description

基于顶视图的停车场车辆自定位及地图构建方法
技术领域
本发明涉及无人驾驶行业中的自主泊车技术领域以及计算机视觉技术领域，具体涉及驾驶车辆在停车场范围内进行停车场地图构建以及车辆自定位的方法和装置。
背景技术
近年来，自动驾驶技术发展迅猛，而解决停车难的问题也是自动驾驶领域研发的重点之一。由以倒车雷达为代表的辅助泊车系统、以超声波雷达为基础的双边障碍自动泊车技术发展到现在，自动泊车技术已经不仅仅限于泊车入库这一操作，而是扩展成包含自主低速巡航、寻车位、泊车以及召唤响应的综合泊车系统。其中，对于停车场的地图构建、地图自定位等技术作为新增的技术要点，是自动泊车研究的重点之一。
现有的技术中，大部分技术仅针对或室内或室外一种场景进行研究；部分研究在室内停车场中部署WIFI或UWB等无线设备，需要大量的基站设备，成本昂贵，同时定位精度低，不满足自主泊车需求。在以SLAM算法为基础的地图构建与自定位技术中，按照传感器不同可分为激光雷达SLAM、双目SLAM、单目SLAM等技术：其中激光SLAM受到激光设备成本的制约，难以部署在量产车型中；双目SLAM无法解决室内场景纹理稀疏问题，无法保证精度与稳定性，且双目视差计算复杂度高，车载低功耗处理器难以达到实时处理；单目SLAM受到尺度漂移问题的影响，难以获得停车场环境的绝对尺度，无法辅助车辆进行自主泊车。
为了解现有技术的发展状况，本发明对已有的专利和论文进行了检索、比较和分析：
技术方案1：专利文献专利CN104077809B(“基于结构性线条的视觉SLAM方法”)利用建筑的结构型线条作为特征线条来实现实时定位和地图的实时构建；利用特征线条与其对应的参考平面的从属关系，将特征线条参数化，并使用卡尔曼滤波器更新特征线条、摄像设备的位姿状态。该方法对建筑的结构性线条进行描述，能够在缺乏点特征的室内场景完成建图。但是，该方法仅适用于室内场景，对室外停车场的适用性不强，不能满足室外使用需求。
技术方案2：专利文献CN107180215B(“基于库位和二维码的停车场自动建图与高精度定位方法”)，使用车辆的前视相机与四周的鱼眼相机组成的环视系统，对预先布置好的二维码进行鲁棒性识别，并使用深度学习的方法对车位、车位号进行检测，同时利用惯性导航设备估算自车速度与航向角，以SLAM算法实现车库平面图的构建，利用建图结果实现高精度定位。然而，由于深度学习对车位、车位号的定位与其真实位置存在一定角度的偏差，使得重建地图与真实地图有着一定的差异，需要通过二维码来辅助实现重定位，而由于二维码的部署问题，该方法仅适用于室内停车场；此外，图优化算法计算复杂度高，低功耗的车载嵌入式设备无法实时建图。
技术方案3：专利文献EP3057061B1“Method and device for the estimation ofcar egomotion from surround view images”对车辆前后左右四个相机获取图像进行处理，生成顶视图。在顶视图上，利用光流，确定车辆的自我状态。使用阿克曼转向模型从车轮速度和转向角得出车辆围绕顺势曲率中心的角速度。将光流和车辆角速度得出的运动姿态进行融合，更新、预测车辆的运动状态。然而，无论室内停车场还是室外停车场，光照对顶视图的影响是不可避免的，而在这种情况下，在顶视图上使用光流的方法对车辆位姿的估计是不准确的。
技术方案4：论文“Houben S,Neuhausen M,Michael M,et al.Park marking-based vehicle self-localization with a fisheye topview system[J].Journal ofReal-Time Image Proc 16,289–304(2019).”，采用四个鱼眼相机组成的环视系统，独立地将每个相机采集到的图像投影到鸟瞰图中，检测明亮的直线并聚类作为停车场特征，随后用卡尔曼滤波算法将初始的粗略位姿与特征进行融合，计算精确且置信程度高的位姿与车身运动。然而，论文中提出的算法独立地计算每个镜头中的特征，增大了计算量和功耗，难以部署在车载设备上；并且该方法依赖于其它方法给出的一个较好的初始位姿，并不能单独完成重定位。
技术方案5：论文“J.Ma,X.Wang,Y.He,X.Mei and J.Zhao,"Line-Based StereoSLAM by Junction Matching and Vanishing Point Alignment,"in IEEE Access,vol.7,pp.181800-181811(2019).”以平面上相交直线作为基本结构，在交点处提取描述子用于线关系的匹配，通过建立线段和消失点的观测模型，优化相机位姿和线的位置，构建了可以在便携式电脑上实时运行的双目slam系统。但是，双目图像计算像素距离消耗计算力，难以适用于低功耗的车载处理器。
可见，自主泊车中，现有的方法在定位精度、稳定性和实时性上难以获得满意的综合效果。因此，需要研究新的停车场车辆自定位及地图构建方法，既能保证定位精度，又能适应室内、室外的背景环境，又不增加额外的计算开销，可用于低功耗车载处理器，同时无需高成本传感器系统支持。
发明内容
本发明致力于使得自主泊车技术普适于室内、室外停车场，并以低成本的传感器系统，在低功耗的车载处理器上可以实时地完成高精度的车辆自定位与停车场地图构建工作，以辅助自主泊车任务的完成。
基于上述发明目的，本发明主要在以环视系统为基础的停车场车辆自定位及地图构建技术上提出了新的方法，基于利用车载环视系统生成顶视图，在顶视图中使用角线特征作为基本特征，通过对车位线、车位角点的识别和定位，进行高精度的车辆自定位和地图构建工作。该方法从原理上避免了单目slam的尺度漂移问题，并利用现有的车载系统，以低成本传感器、在低功耗的车载处理器上完成实时、高精度车辆自定位和地图构建工作，来辅助自主泊车任务的完成。
为解决上述技术问题，根据本发明的一个方面，提供了一种基于顶视图的停车场车辆自定位及地图构建方法，包括如下步骤：
步骤1)、采集相机图像；
步骤2)、根据所述相机图像进行车辆顶视图的拼接；
步骤3)、对拼接的所述车辆顶视图提取并匹配角线特征；
步骤4)、基于所述角线特征计算车辆位姿，生成角线地图；
步骤5)、根据生成的所述角线地图匹配所述角线特征；
步骤6)、输出车辆位姿。
优选地，所述相机为鱼眼相机或所述相机为多个工业小孔相机组合而成。
优选地，所述鱼眼相机有四个，分辨率在720P或以上，分别布置于汽车前后保险杠以及左右后视镜下边沿。
优选地，对所述鱼眼相机进行标定，得到相机的内参和相机到地面靶标的外参。
优选地，标定时，车辆静止在平整路面上，采用标定板直接获取从鱼眼相机图像到顶视图坐标系的对应关系，计算生成各相机的原始图像与顶视图像素的映射表。
优选地，所述顶视图坐标系位于车底平面上，以车辆后轴中心为原点，水平向前和水平向右分别为坐标轴正方向。
优选地，所述顶视图坐标系与地面平行。
优选地，所述映射表表示所述顶视图像素到所述原始图像像素坐标的映射关系。
优选地，生成所述映射表包括：
在静止平面上，通过标定，获取每个鱼眼相机的内参和相机相对于顶视图坐标系原点的外参；
将车身周围采样，实现栅格化，利用相机的内参和外参，计算顶视图上的采样点到原始图像像素的坐标信息。
优选地，所述顶视图的尺寸为510像素*425像素，其表达的实际物理尺寸为12米*10米。
优选地，采用均值法计算得到一对多的像素的像素值；采用插值法计算得到多对一的像素的像素值。
优选地，使用线特征作为基本的特征，获取车位线的边缘。
优选地，将端点相近、存在一定角度差异的线与它们的交点组合形成角线结构，用来描述车位线角点附近的特征。
优选地，提取角线结构中交点附近的梯度直方图作为角线结构的描述子。
优选地，在采样生成的顶视图上，利用线特征检测器，检测车身周边的线特征集合L＝{li＝(si,ei)|i＝1,2,3…n}，其中li表示第i条线段，si、ei是二维平面上的点坐标，表示li的两个端点；
在第m帧中，车身周边的角线特征集合为：
Fm＝{fi,m＝(li,1,m,li,2,m,pi,m,θi,m,di,m)|i＝1,2,3…N} (1)
其中N为第m帧中提取到的特征数量，fi,m表示第m帧中第i个角线特征，li,1,m、li,2,m分别是组成角线特征fi,m的两条线段，pi,m为li,1,m、li,2,m所在直线的交点，θi,m为从线段li,1,m按顺时针绕交叉点到li,2,m的有向角，di,m是角线特征fi,m的描述子；为满足表达的唯一性，li,1,m、li,2,m的选取满足0<θi<π，对于li,1,m(si,1,m,ei,1,m)，si,1,m为li,1,m中靠近pi,m的端点，ei,1,m为远离pi,m的端点；描述子以交叉点pi,m为中心、分别以li,1,m、li,2,m为主、副方向进行提取。
优选地，所述线特征检测器为LSD、EDLine、FSD或DP线特征检测器。
优选地，采用类SIFT描述子提取方法提取所述描述子，描述角线交叉点附近的特性。
优选地，采用BRIEF描述子描述角线交叉点附近的特性。
优选地，采用LBP或梯度直方图信息作为描述子描述角线交叉点附近的特性。
优选地，计算以交叉点为中心，分别沿着两条线的方向取4*2个方格，每个方格的长度是12像素*12像素，共48像素*24像素；在每个方格中以45度为间隔统计8维梯度直方图；在每个交叉点共统计2*4*2*8＝128维描述子；在地平面设定下，48像素*24像素代表1.13米*0.56米的范围。
优选地，当车位位置由直线来描述时，则使用深度学习方法对车位、车位角点进行检测；根据检测出的车位角点位置和车位的开口方向，添加虚拟线，提取角线特征。
优选地，特征提取时，使用语义分割对车位角点附近的角线特征进行筛选。
优选地，所述匹配角线特征包括将当前帧图像与角线地图中的数据进行匹配。
优选地，所述角线地图由关键帧和地图角线特征组成。
优选地，当已知当前帧图像在角线地图中的参考帧时，进行局部地图的匹配；当不能够确定当前图像在角线地图中的参考帧时，进行全局地图的匹配。
优选地，对于匹配的角线特征fi＝(li,1,li,2,pi,θi,di)，fj＝(lj,1,lj,2,pj,θj,dj)，要满足公式(2)的条件：
Figure BDA0002555597780000061
其中，τθ、τd分别表示角度、描述子的阈值，距离函数dist()根据描述子的提取方式来确定；li,1、li,2分别是组成角线特征fi的两条线段，pi为li,1、li,2所在直线的交点，θi为从线段li,1按顺时针绕交叉点到li,2的有向角，di是角线特征fi的描述子；li,1、li,2分别是组成角线特征fi的两条线段，pi为li,1、li,2所在直线的交点，θi为从线段li,1按顺时针绕交叉点到li,2的有向角，di是角线特征fi的描述子。
优选地，所述距离函数包括但不限于欧氏距离、汉明距离或余弦距离。
优选地，所述局部地图的匹配包括：
首先，将当前帧图像与参考帧可以观察到的地图角线特征进行匹配；
然后，对于匹配上的地图角线特征，找到与之存在可视关系的关键帧序列，将这个关键帧序列作为局部地图；
最后，将当前帧图像提取到的角线特征与局部地图可以观察到的地图角线特征进行匹配。
优选地，所述全局地图的匹配包括：
当获取一个帧序列与地图中的唯一一个关键帧序列存在匹配关系时，表明全局地图匹配成功。
优选地，所述匹配关系是指当两帧图像通过提取到角线特征进行匹配，且根据匹配结果能够计算出满足平面上旋转平移的位姿。
优选地，设第m帧与第n帧匹配的角线特征的集合为MFm,n，角线特征fi,m与fj,n相匹配记为(fi,m,fj,n)∈MFm,n，fi,m在第n帧的投影特征记为
Figure BDA0002555597780000071
在位姿计算中关注特征点的位置，因此不考虑夹角θ和描述子d；
对于集合Fm，在第m帧中的第i个角线特征fi,m∈Fm，则有：
fi,m＝(li,m,1(si,m,1,ei,m,1),li,m,2(si,m,2,ei,m,2),pi,m(xi,m,yi,m))
其在第n帧的投影
Figure BDA0002555597780000072
为：
Figure BDA0002555597780000073
其中，li,m,1(si,m,1,ei,m,1)、li,m,2(si,m,2,ei,m,2)是fi,m的两条线段；pi,m(xi,m,yi,m)是li,m,1(si,m,1,ei,m,1)、li,m,2(si,m,2,ei,m,2)所在直线的交点；
Figure BDA0002555597780000074
是
Figure BDA0002555597780000075
的两条线段；
Figure BDA0002555597780000076
为li,m,1(si,m,1,ei,m,1)的两个端点si,m,1、ei,m,1在第n帧的投影
Figure BDA0002555597780000077
所形成的线段，
Figure BDA0002555597780000078
同理，投影点
Figure BDA0002555597780000079
为
Figure BDA00025555977800000710
所在直线的交叉点；xi,m,yi,m和
Figure BDA00025555977800000711
分别表示交叉点的坐标；
则投影点
Figure BDA00025555977800000712
如公式(3)所示：
Figure BDA00025555977800000713
其中Tm,n(txm,n,tym,n,θm,n)∈SE(2)为第m帧到第n帧的转换关系；SE(2)表示平面上的平移、旋转变换；(txm,n,tym,n)为位移分量，θm,n为旋转分量；
第m帧与第n帧的匹配误差如公式(4)所示：
Figure BDA00025555977800000714
xj,n、yj,n表示第n帧中第j个特征fj,m的交叉点的坐标；
则所述车辆位姿表示为公式(5)所示：
(tx,ty,θ)＝argminReporj(tx,ty,θ) (5)
优选地，通过角线特征进行位姿估计，包括：
选取两对匹配的角线特征作为种子，记为(fi,m,fj,n)、(fp,m,fq,n)∈MFm,n；令fi,m的交叉点pi,m(xi,m,yi,m)在第n帧上的投影
Figure BDA0002555597780000081
与fj,n的交叉点pj,n(xj,n,yj,n)重合，即
Figure BDA0002555597780000082
有
Figure BDA0002555597780000083
同理，对于(fp,m,fq,n)∈MFm,n有
Figure BDA0002555597780000084
cos2θm,n+sin2θm,n＝1 (8)
其中Tm,n(txm,n,tym,n,θm,n)∈SE(2)为第m帧到第n帧的转换关系；SE(2)表示平面上的平移、旋转变换；(txm,n,tym,n)为位移分量，θm,n为旋转分量；
联立公式(6)、(7)和(8)表示的方程，求解出第m帧中点到第n帧转换关系Tm,n(txm,n,tym,n,θm,n)；
在随机抽样一致性算法(RANSAC)框架下，通过多次计算，按照公式(4)和(5)选择重投影误差最小的位姿Tmn作为第m帧到第n帧的位姿。
优选地，通过重投影误差进行位姿估计，包括：
在进行车辆t时刻的姿态进行估计时，采用t-1时刻图像的位姿作为初始值，根据角线特征的匹配结果，如公式(4)和(5)以当前位姿作为变量、最小投影误差为目标建立损失函数，使用Levenberg-Marquardt方法对当前位姿进行求解，得到所述车辆位姿。
优选地，当特征匹配数量骤减、计算出的与前一帧的相对位姿不满足车辆运动模型时，则进行顶视图的矫正。
优选地，当使用投影变换模型表示两个图像间的操作时，用平面单应矩阵，如公式(9)所示，来表示两帧图像的变换关系，
Figure BDA0002555597780000091
其中，hij,(i,j＝1,2,3)表示单应矩阵的矩阵元素，将单应性矩阵分解得到旋转分量和平移分量，此时旋转分量与平移分量中包含三维信息；将旋转分量分解为欧拉角，当欧拉角中俯仰角或翻滚角超过一定阈值时，表明车辆遇到颠簸。
优选地，当车辆遇到颠簸时，将当前帧图像舍弃掉，或使用前两帧图像位姿的偏差当作当前图像到上一帧图像的位姿偏差。
优选地，当车辆遇到颠簸时，获得平面单应矩阵以及对应的欧拉角、平移距离；将去掉位姿的俯仰角、翻滚角以及去掉平移向量中的高度分量作为矫正过的车辆位姿，将当前帧图像由矫正前的相机坐标系映射到矫正过的相机坐标系，以实现当前帧图像的矫正。
优选地，所述地图角线特征分为角结构和线结构两层结构。
优选地，所述线结构是地图角线特征的原子结构，角线特征由线特征和描述子组成。
优选地，当进入建图模式且地图为空时，需要进行角线地图的初始化；或者，当从两帧图像恢复出的运动满足初始化条件时，进行角线地图的初始化。
优选地，所述初始化条件为位移超过第一阈值或者匹配的角线数量超过第二阈值。
优选地，所述初始化的步骤包括：
1)将两帧图像加入到角线地图当中；
2)对于两帧中的每一对匹配的角线特征，在地图中生成一个新的地图角线特征，用mfi来表示：
mfi＝(fi,S＝{fj,k,…})
其中fi表示地图角线特征中的位置、描述子的值，S表示与mfi存在可视关系的角线特征的集合，fj,k为第k帧中第j个角线特征；
3)首先根据可视关系，更新集合S，再根据S中的每一个角线特征，融合生成地图角线特征的位置、描述子的属性。
优选地，当当前帧图像与最后一个关键帧发生一段距离的移动时，将当前帧图像加入到角线地图中；
向地图中加入新的地图角线特征时，依照匹配关系，首先更新角线特征、线特征的可视关系，然后根据可视关系，依次对地图线特征的位姿、角线特征的描述子进行更新。
优选地，角线地图的优化包括角线地图的局部优化或角线地图的全局优化。
优选地，所述角线地图的局部优化是指以最后一帧及其局部地图的位姿以及局部地图可以观察到的角线特征的位置作为变量，以最小重投影误差作为目标函数，利用光束法平差(Bundle Adjustment)进行优化，使得获得的关键帧位姿、地图角线特征的位置更加精确。
优选地，所述角线地图的全局优化是指在建图过程中，如果发现获取的一个帧序列与地图中的唯一一个帧序列匹配时，认定为发生了回环，在进入角线地图的全局优化；
在全局优化过程中，将关键帧的位姿、地图角线特征中线的位姿作为变量，以最小重投影误差作为损失函数，进行优化；根据地图角线特征中线位姿的调整结果重新计算地图角线特征的描述子。
为解决上述技术问题，根据本发明的另一个方面，提供了一种基于顶视图的停车场车辆自定位及地图构建装置，包括
图像采集装置，采集相机图像；
顶视图拼接装置，根据所述相机图像进行车辆顶视图的拼接；
特征提取匹配装置，对拼接的所述车辆顶视图提取并匹配角线特征；
地图生成装置，基于所述角线特征计算车辆位姿，生成角线地图；
匹配装置，根据生成的所述角线地图匹配所述角线特征；
输出装置，输出车辆位姿。
优选地，所述相机为鱼眼相机或所述相机为多个工业小孔相机组合而成。
优选地，所述鱼眼相机有四个，分辨率在720P或以上，分别布置于汽车前后保险杠以及左右后视镜下边沿。
优选地，对所述鱼眼相机进行标定，得到相机的内参和相机到地面靶标的外参。
优选地，标定时，车辆静止在平整路面上，采用标定板直接获取从鱼眼相机图像到顶视图坐标系的对应关系，计算生成各相机的原始图像与顶视图像素的映射表。
优选地，所述顶视图坐标系位于车底平面上，以车辆后轴中心为原点，水平向前和水平向右分别为坐标轴正方向。
优选地，所述顶视图坐标系与地面平行。
优选地，所述映射表表示所述顶视图像素到所述原始图像像素坐标的映射关系。
优选地，生成所述映射表包括：
在静止平面上，通过标定，获取每个鱼眼相机的内参和相机相对于顶视图坐标系原点的外参；
将车身周围采样，实现栅格化，利用相机的内参和外参，计算顶视图上的采样点到原始图像像素的坐标信息。
优选地，所述顶视图的尺寸为510像素*425像素，其表达的实际物理尺寸为12米*10米。
优选地，采用均值法计算得到一对多的像素的像素值；采用插值法计算得到多对一的像素的像素值。
优选地，使用线特征作为基本的特征，获取车位线的边缘。
优选地，将端点相近、存在一定角度差异的线与它们的交点组合形成角线结构，用来描述车位线角点附近的特征。
优选地，提取角线结构中交点附近的梯度直方图作为角线结构的描述子。
优选地，在采样生成的顶视图上，利用线特征检测器，检测车身周边的线特征集合L＝{li＝(si,ei)|i＝1,2,3…n}，其中li表示第i条线段，si、ei是二维平面上的点坐标，表示li的两个端点；
在第m帧中，车身周边的角线特征集合为：
Fm＝{fi,m＝(li,1,m,li,2,m,pi,m,θi,m,di,m)|i＝1,2,3…N} (1)
其中N为第m帧中提取到的特征数量，fi,m表示第m帧中第i个角线特征，li,1,m、li,2,m分别是组成角线特征fi,m的两条线段，pi,m为li,1,m、li,2,m所在直线的交点，θi,m为从线段li,1,m按顺时针绕交叉点到li,2,m的有向角，di,m是角线特征fi,m的描述子；为满足表达的唯一性，li,1,m、li,2,m的选取满足0<θi<π，对于li,1,m(si,1,m,ei,1,m)，si,1,m为li,1,m中靠近pi,m的端点，ei,1,m为远离pi,m的端点；描述子以交叉点pi,m为中心、分别以li,1,m、li,2,m为主、副方向进行提取。
优选地，所述线特征检测器为LSD、EDLine、FSD或DP线特征检测器。
优选地，采用类SIFT描述子提取方法提取所述描述子，描述角线交叉点附近的特性。
优选地，采用BRIEF描述子描述角线交叉点附近的特性。
优选地，采用LBP或梯度直方图信息作为描述子描述角线交叉点附近的特性。
优选地，计算以交叉点为中心，分别沿着两条线的方向取4*2个方格，每个方格的长度是12像素*12像素，共48像素*24像素；在每个方格中以45度为间隔统计8维梯度直方图；在每个交叉点共统计2*4*2*8＝128维描述子；在地平面设定下，48像素*24像素代表1.13米*0.56米的范围。
优选地，当车位位置由直线来描述时，则使用深度学习方法对车位、车位角点进行检测；根据检测出的车位角点位置和车位的开口方向，添加虚拟线，提取角线特征。
优选地，特征提取时，使用语义分割对车位角点附近的角线特征进行筛选。
优选地，所述匹配角线特征包括将当前帧图像与角线地图中的数据进行匹配。
优选地，所述角线地图由关键帧和地图角线特征组成。
优选地，当已知当前帧图像在角线地图中的参考帧时，进行局部地图的匹配；当不能够确定当前图像在角线地图中的参考帧时，进行全局地图的匹配。
优选地，对于匹配的角线特征fi＝(li,1,li,2,pi,θi,di)，fj＝(lj,1,lj,2,pj,θj,dj)，要满足公式(2)的条件：
Figure BDA0002555597780000132
其中，τθ、τd分别表示角度、描述子的阈值，距离函数dist()根据描述子的提取方式来确定；li,1、li,2分别是组成角线特征fi的两条线段，pi为li,1、li,2所在直线的交点，θi为从线段li,1按顺时针绕交叉点到li,2的有向角，di是角线特征fi的描述子；li,1、li,2分别是组成角线特征fi的两条线段，pi为li,1、li,2所在直线的交点，θi为从线段li,1按顺时针绕交叉点到li,2的有向角，di是角线特征fi的描述子。
优选地，所述距离函数为欧氏距离、汉明距离或余弦距离。
优选地，所述局部地图的匹配包括：
首先，将当前帧图像与参考帧可以观察到的地图角线特征进行匹配；
然后，对于匹配上的地图角线特征，找到与之存在可视关系的关键帧序列，将这个关键帧序列作为局部地图；
最后，将当前帧图像提取到的角线特征与局部地图可以观察到的地图角线特征进行匹配。
优选地，所述全局地图的匹配包括：
当获取一个帧序列与地图中的唯一一个关键帧序列存在匹配关系时，表明全局地图匹配成功。
优选地，所述匹配关系是指当两帧图像通过提取到角线特征进行匹配，且根据匹配结果能够计算出满足平面上旋转平移的位姿。
优选地，设第m帧与第n帧匹配的角线特征的集合为MFm,n，角线特征fi,m与fj,n相匹配记为(fi,m,fj,n)∈MFm,n，fi,m在第n帧的投影特征记为
Figure BDA0002555597780000131
在位姿计算中关注特征点的位置，因此不考虑夹角θ和描述子d；
对于集合Fm，在第m帧中的第i个角线特征fi,m∈Fm，则有：
fi,m＝(li,m,1(si,m,1,ei,m,1),li,m,2(si,m,2,ei,m,2),pi,m(xi,m,yi,m))
其在第n帧的投影
Figure BDA0002555597780000141
为：
Figure BDA0002555597780000142
其中，li,m,1(si,m,1,ei,m,1)、li,m,2(si,m,2,ei,m,2)是fi,m的两条线段；pi,m(xi,m,yi,m)是li,m,1(si,m,1,ei,m,1)、li,m,2(si,m,2,ei,m,2)所在直线的交点；
Figure BDA0002555597780000143
是
Figure BDA0002555597780000144
的两条线段；
Figure BDA0002555597780000145
为li,m,1(si,m,1,ei,m,1)的两个端点si,m,1、ei,m,1在第n帧的投影
Figure BDA0002555597780000146
所形成的线段，
Figure BDA0002555597780000147
同理，投影点
Figure BDA0002555597780000148
为
Figure BDA0002555597780000149
所在直线的交叉点；xi,m,yi,m和
Figure BDA00025555977800001410
分别表示交叉点的坐标；
则投影点
Figure BDA00025555977800001411
如公式(3)所示：
Figure BDA00025555977800001412
其中Tm,n(txm,n,tym,n,θm,n)∈SE(2)为第m帧到第n帧的转换关系；SE(2)表示平面上的平移、旋转变换；(txm,n,tym,n)为位移分量，θm,n为旋转分量；
第m帧与第n帧的匹配误差如公式(4)所示：
Figure BDA00025555977800001413
xj,n、yj,n表示第n帧中第j个特征fj,m的交叉点的坐标；
则所述车辆位姿表示为公式(5)所示：
(tx,ty,θ)＝argminReporj(tx,ty,θ) (5)
优选地，通过角线特征进行位姿估计，包括：
选取两对匹配的角线特征作为种子，记为(fi,m,fj,n)、(fp,m,fq,n)∈MFm,n；令fi,m的交叉点pi,m(xi,m,yi,m)在第n帧上的投影
Figure BDA00025555977800001414
与fj,n的交叉点pj,n(xj,n,yj,n)重合，即
Figure BDA00025555977800001415
有
Figure BDA0002555597780000151
同理，对于(fp,m,fq,n)∈MFm,n有
Figure BDA0002555597780000152
cos2θm,n+sin2θm,n＝1 (8)
其中Tm,n(txm,n,tym,n,θm,n)∈SE(2)为第m帧到第n帧的转换关系；SE(2)表示平面上的平移、旋转变换；(txm,n,tym,n)为位移分量，θm,n为旋转分量；
联立公式(6)、(7)和(8)表示的方程，求解出第m帧中点到第n帧转换关系Tm,n(txm,n,tym,n,θm,n)；
在随机抽样一致性算法(RANSAC)框架下，通过多次计算，按照公式(4)和(5)选择重投影误差最小的位姿Tmn作为第m帧到第n帧的位姿。
优选地，通过重投影误差进行位姿估计，包括：
在进行车辆t时刻的姿态进行估计时，采用t-1时刻图像的位姿作为初始值，根据角线特征的匹配结果，如公式(4)和(5)以当前位姿作为变量、最小投影误差为目标建立损失函数，使用Levenberg-Marquardt方法对当前位姿进行求解，得到所述车辆位姿。
优选地，当特征匹配数量骤减、计算出的与前一帧的相对位姿不满足车辆运动模型时，则进行顶视图的矫正。
优选地，当使用投影变换模型表示两个图像间的操作时，用平面单应矩阵，如公式(9)所示，来表示两帧图像的变换关系，
Figure BDA0002555597780000153
其中，hij,(i,j＝1,2,3)表示单应矩阵的矩阵元素，将单应性矩阵分解得到旋转分量和平移分量，此时旋转分量与平移分量中包含三维信息；将旋转分量分解为欧拉角，当欧拉角中俯仰角或翻滚角超过一定阈值时，表明车辆遇到颠簸。
优选地，当车辆遇到颠簸时，将当前帧图像舍弃掉，或使用前两帧图像位姿的偏差当作当前图像到上一帧图像的位姿偏差。
优选地，当车辆遇到颠簸时，获得平面单应矩阵以及对应的欧拉角、平移距离；将去掉位姿的俯仰角、翻滚角以及去掉平移向量中的高度分量作为矫正过的车辆位姿，将当前帧图像由矫正前的相机坐标系映射到矫正过的相机坐标系，以实现当前帧图像的矫正。
优选地，所述地图角线特征分为角结构和线结构两层结构。
优选地，所述线结构是地图角线特征的原子结构，角线特征由线特征和描述子组成。
优选地，当进入建图模式且地图为空时，需要进行角线地图的初始化；或者，当从两帧图像恢复出的运动满足初始化条件时，进行角线地图的初始化。
优选地，所述初始化条件为位移超过第一阈值或者匹配的角线数量超过第二阈值。
优选地，所述初始化的步骤包括：
1)将两帧图像加入到角线地图当中；
2)对于两帧中的每一对匹配的角线特征，在地图中生成一个新的地图角线特征，用mfi来表示：
mfi＝(fi,S＝{fj,k,…})
其中fi表示地图角线特征中的位置、描述子的值，S表示与mfi存在可视关系的角线特征的集合，fj,k为第k帧中第j个角线特征；3)首先根据可视关系，更新集合S，再根据S中的每一个角线特征，融合生成地图角线特征的位置、描述子的属性。
优选地，当当前帧图像与最后一个关键帧发生一段距离的移动时，将当前帧图像加入到角线地图中；
向地图中加入新的地图角线特征时，依照匹配关系，首先更新角线特征、线特征的可视关系，然后根据可视关系，依次对地图线特征的位姿、角线特征的描述子进行更新。
优选地，角线地图的优化包括角线地图的局部优化或角线地图的全局优化。
优选地，所述角线地图的局部优化是指以最后一帧及其局部地图的位姿以及局部地图可以观察到的角线特征的位置作为变量，以最小重投影误差作为目标函数，利用光束法平差(Bundle Adjustment)进行优化，使得获得的关键帧位姿、地图角线特征的位置更加精确。
优选地，所述角线地图的全局优化是指在建图过程中，如果发现获取的一个帧序列与地图中的唯一一个帧序列匹配时，认定为发生了回环，在进入角线地图的全局优化；
在全局优化过程中，将关键帧的位姿、地图角线特征中线的位姿作为变量，以最小重投影误差作为损失函数，进行优化；根据地图角线特征中线位姿的调整结果重新计算地图角线特征的描述子。
为解决上述技术问题，根据本发明的再一个方面，提供了一种基于顶视图的停车场车辆自定位及地图构建系统，包括：
存储器，所述存储器存储执行所述基于顶视图的停车场车辆自定位及地图构建方法的程序；
处理器；所述处理器执行所述程序。
为解决上述技术问题，根据本发明的再一个方面，提供了一种自动驾驶车辆，包括：
所述自动驾驶车辆上搭载所述基于顶视图的停车场车辆自定位及地图构建装置或所述基于顶视图的停车场车辆自定位及地图构建系统。
为解决上述技术问题，根据本发明的再一个方面，提供了一种自主泊车系统，包括：
所述基于顶视图的停车场车辆自定位及地图构建装置或所述基于顶视图的停车场车辆自定位及地图构建系统。
本发明提出了基于顶视图上的角线特征的实时建图与自定位技术，以低成本的鱼眼相机组成的环视系统作为信息采集系统，鲁棒性地提取车辆附近车位线的角线特征，进行跟踪和建图；利用图匹配技术实时生成角线地图，并利用局部地图优化和全局地图优化，对车辆进行实时高精度定位和地图构建。
本发明的有益效果：
1、建立二维平面设定，使用车载环视系统生成基于平行投影的顶视图，顶视图具有与物理空间相同的确定尺度，在顶视图上进行SLAM，只需要三个自由度，相比于传统slam的六个自由度，降低了很多计算量，且尺度明确，无需考虑尺度漂移问题，从原理上解决了单目slam的尺度问题；
2、基于二维平面设定，利用仿射变换模型，检测车辆的颠簸情况，避免车辆颠簸、上下坡时，因二维平面设定不成立导致跟踪失效的问题；
3、使用角线特征对车位线进行描述，降低了三维空间中基于点特征的SLAM算法的特征使用量，提高了地面上这种弱纹理平面上线特征表达能力的不足，同时解决了线特征描述能力的问题，降低计算量的同时保证定位精度，能够满足在低功耗处理器上实时需求；
4、使用角、线的二层结构建立角线地图，增加对地图角线特征的约束，能够进行更准确的位姿估计，计算结果精度高；
5、该方法仅通过车载环视系统实现在停车场环境下的自定位和地图构建，成本低，性价比高。
附图说明
附图示出了本发明的示例性实施方式，并与其说明一起用于解释本发明的原理，其中包括了这些附图以提供对本发明的进一步理解，并且附图包括在本说明书中并构成本说明书的一部分。通过结合附图对本发明的实施例进行详细描述，本发明的上述和其它目的、特征、优点将更加清楚。
图1是整体流程图；
图2是顶视图坐标系；
图3是描述子的组成；
图4是车辆右前轮经过减速带时，顶视图发生扭曲示意图；
图5是角线地图的组成：关键帧到角结构表示可视关系；线结构到角结构表示组成关系。
具体实施方式
下面结合附图和实施方式对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施方式仅用于解释相关内容，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分。
需要说明的是，在不冲突的情况下，本发明中的实施方式及实施方式中的特征可以相互组合。下面将参考附图并结合实施方式来详细说明本发明。
本发明的目的是提供一种基于顶视图的停车场车辆自定位及地图构建方法。图1描述了本发明的整体流程图。如图1所示，整体流程包括五个主要步骤：一、车辆顶视图的拼接与角线特征提取；二、角线特征的匹配；三、基于角线特征的位姿计算；四、角线地图的生成；五、角线地图的局部和全局优化。
本发明以低成本的鱼眼相机组成的环视系统作为信息采集系统，基于所述环视系统生成顶视图，鲁棒性地提取车辆附近车位线的角线特征，进行跟踪和建图；利用图匹配技术实时生成角线地图，并利用局部地图优化和全局地图优化，对车辆进行实时高精度定位和地图构建。所述方法可以从原理上避免了单目slam的尺度漂移问题，并利用现有的车载系统，能够以低成本传感器、在低功耗的车载处理器上完成实时、高精度车辆自定位和地图构建工作，来辅助自主泊车任务的完成。具体包括如下过程：
1、车辆顶视图的拼接与角线特征提取
1.1车辆顶视图的拼接
本发明的数据采集系统由四个鱼眼相机组成，分别布置于汽车前后保险杠以及左右后视镜下边沿，以采集车身周围的视觉数据。相比于针孔相机，鱼眼相机的视场角更大，基本一般大于180°，四个相机基本可以完全覆盖掉车身周围的场景；由于鱼眼相机与传统针眼相机的原理上存在区别，且不适用于透视投影模型，需要对相机进行精准的标定工作，得到相机的内参和相机到地面靶标的外参。标定方法包括但不限于Camodocal(http://www.github.com/hengli/camodocal)、OCamCalib(https://sites.google.com/site/scarabotix/ocamcalib-toolbox)。
顶视图可以看作为以车底平面的法向量为方向，地面在车底平面的平行投影。为满足平行投影模型，标定在车辆静止在平整路面上进行。使用标定板，直接获取从鱼眼相机图像到顶视图坐标系的对应关系，进而计算生成各相机的原始图像与顶视图像素的映射表。顶视图坐标系为在车底平面上，以车辆后轴中心为原点，水平向前和水平向右分别为轴的坐标系。这里提出一个地平面设定：设定顶视图坐标系与地面是平行的，这样根据平行投影模型，才能够保证顶视图上以真实尺度反应地面情况，进而车辆的运动可以用二维平面中的运动来描述。
映射表时顶视图像素到原始图像像素坐标的映射关系，具体生成方法为：在静止平面上，通过标定方法，获取每个鱼眼相机的内参和相机相对于顶视图坐标系原点的外参。将车身周围采样，实现栅格化，利用相机的内参和外参，计算顶视图上的采样点到原始图像像素的坐标信息。在实施例中，选取顶视图的尺寸为510像素*425像素，表达的实际物理尺寸为12米*10米。由于鱼眼相机存在畸变，距离信息在原始图像中的分布是不均匀的，存在这一对多和多对一的问题。最终生成的顶视图时，一对多的像素需要进行均值运算，多对一的像素值需通过插值计算结果。
在以下各个模块中，均采用顶视图坐标系，进行特征和位姿的描述。
1.2角线特征提取
在停车场场景下，车位线、车位角点、车位号是较为明显的特征。我们使用线特征作为基本的特征，获取车位线的边缘。为了方便线特征的匹配，将端点相近、存在一定角度差异的线与他们的交点组合形成角线结构，用来描述车位线角点附近的特征。提取角线结构中交点附近的梯度直方图作为角线结构的描述子，方便角线特征的匹配。
在采样生成的顶视图上，利用线特征检测器，如LSD、EDLine、FSD、DP等，检测车身周边的线特征集合L＝{li＝(si,ei)|i＝1,2,3…n}，其中li表示第i条线段，si、ei是二维平面上的点坐标，表示li的两个端点；
在第m帧中，车身周边的角线特征集合为：
Fm＝{fi,m＝(li,1,m,li,2,m,pi,m,θi,m,di,m)|i＝1,2,3…N} (1)
其中N为第m帧中提取到的特征数量，fi,m表示第m帧中第i个角线特征，li,1,m、li,2,m分别是组成角线特征fi,m的两条线段，pi,m为li,1,m、li,2,m所在直线的交点，θi,m为从线段li,1,m按顺时针绕交叉点到li,2,m的有向角，di,m是角线特征fi,m的描述子；为满足表达的唯一性，li,1,m、li,2,m的选取满足0<θi<π，对于li,1,m(si,1,m,ei,1,m)，si,1,m为li,1,m中靠近pi,m的端点，ei,1,m为远离pi,m的端点；描述子以交叉点pi,m为中心、分别以li,1,m、li,2,m为主、副方向进行提取。
对于描述子有多种形式提取方法，如：类SIFT描述子，BRIEF等，用来描述角线交叉点附近的特性。由于角线特征落在车位角点附近，且车位角点附近不同方向的车位线数量不同，直接导致了每个车位角点的不同属性。在本实施例中，计算以交叉点为中心，分别沿着两条线的方向取4*2个方格，如图3所示，每个方格的长度是12像素*12像素，共48像素*24像素。在每个方格中以45度为间隔统计8维梯度直方图。在每个交叉点共统计2*4*2*8＝128维描述子。在地平面设定下，48x24像素代表1.13米*0.56米的范围，能够覆盖到车位角点附近的车位线。
需要说明的是，对于一些仅用直线来描述位置的车位，也可以使用角线特征进行描述。首先使用深度学习的方法对车位、车位角点进行检测。根据检测出的车位角点位置和车位的开口方向，添加虚拟线，使得角线特征可以正常提取。
特征提取时，使用语义分割对车位角点附近的角线特征进行筛选，以便提高精度。
2、角线特征的匹配
角线特征的匹配是将当前帧图像与角线地图中的数据进行匹配。角线地图中包括关键帧、地图角线特征。具体有两种情况：与局部地图进行匹配、与全局地图进行匹配。当已知当前帧图像在角线地图中的参考帧时，进行局部地图的匹配；当不能够确定当前图像在角线地图中的参考帧时，进行全局地图的匹配。
2.1角线特征的匹配
根据角线特征的构造方法，对角线特征进行更严格的表示。
对于匹配的角线特征fi＝(li,1,li,2,pi,θi,di)，fj＝(lj,1,lj,2,pj,θj,dj)，要满足公式(2)的条件：
Figure BDA0002555597780000221
其中，τθ、τd分别表示角度、描述子的阈值，距离函数dist()根据描述子的提取方式来确定；li,1、li,2分别是组成角线特征fi的两条线段，pi为li,1、li,2所在直线的交点，θi为从线段li,1按顺时针绕交叉点到li,2的有向角，di是角线特征fi的描述子；li,1、li,2分别是组成角线特征fi的两条线段，pi为li,1、li,2所在直线的交点，θi为从线段li,1按顺时针绕交叉点到li,2的有向角，di是角线特征fi的描述子。
在这里，距离函数dist()包括但不限于欧氏距离、汉明距离等。在这个实施例中使用欧氏距离。
2.2局部地图的匹配
在已知当前图像在角线地图中的参考帧时，进行局部地图的匹配。首先将当前帧图像与参考帧可以观察到的地图角线特征进行匹配，然后对于匹配上的地图角线特征，找到与之存在可视关系的关键帧序列。将这个关键帧序列成为局部地图。最后，将当前帧图像提取到的角线特征与局部地图可以观察到的地图角线特征进行匹配。以求找到更多的与当前帧图像提取到的角线特征匹配的地图点线特征。
2.3全局地图的匹配
当两帧图像通过提取到角线特征进行匹配，且根据匹配结果能够计算出满足平面上旋转平移的位姿，则称这两帧图像是匹配的。在全局地图匹配时，当我们获取到一个帧序列与地图中的唯一一个关键帧序列存在匹配关系时，表明全局地图匹配成功。找到与当前帧图像匹配的关键帧，进行局部地图匹配，为位姿计算做准备。
3、位姿的计算
3.1基于地平面设定的位姿计算
根据地平面设定，匹配的线特征应满足于平面上的旋转、平移变换，即SE(2)，包含三个自由度(tx,ty,θ)。位姿计算主要有如下方法：1)通过角线特征进行位姿估计；2)通过投影误差进行位姿估计。
3.1.1相关定义
设第m帧与第n帧匹配的角线特征的集合为MFm,n，角线特征fi,m与fj,n相匹配记为(fi,m,fj,n)∈MFm,n，fi,m在第n帧的投影特征记为
Figure BDA0002555597780000231
在位姿计算中关注特征点的位置，因此不考虑夹角θ和描述子d；
对于集合Fm，在第m帧中的第i个角线特征fi,m∈Fm，则有：
fi,m＝(li,m,1(si,m,1,ei,m,1),li,m,2(si,m,2,ei,m,2),pi,m(xi,m,yi,m))
其在第n帧的投影
Figure BDA0002555597780000232
为：
Figure BDA0002555597780000233
其中，li,m,1(si,m,1,ei,m,1)、li,m,2(si,m,2,ei,m,2)是fi,m的两条线段；pi,m(xi,m,yi,m)是li,m,1(si,m,1,ei,m,1)、li,m,2(si,m,2,ei,m,2)所在直线的交点；
Figure BDA0002555597780000234
是
Figure BDA0002555597780000235
的两条线段；
Figure BDA0002555597780000236
为li,m,1(si,m,1,ei,m,1)的两个端点si,m,1、ei,m,1在第n帧的投影
Figure BDA0002555597780000237
所形成的线段，
Figure BDA0002555597780000238
同理，投影点
Figure BDA0002555597780000239
为
Figure BDA00025555977800002310
所在直线的交叉点；xi,m,yi,m和
Figure BDA00025555977800002311
分别表示交叉点的坐标；
则投影点
Figure BDA00025555977800002312
如公式(3)所示：
Figure BDA00025555977800002313
其中Tm,n(txm,n,tym,n,θm,n)∈SE(2)为第m帧到第n帧的转换关系；SE(2)表示平面上的平移、旋转变换；(txm,n,tym,n)为位移分量，θm,n为旋转分量；
第m帧与第n帧的匹配误差如公式(4)所示：
Figure BDA0002555597780000241
xj,n、yj,n表示第n帧中第j个特征fj,m的交叉点的坐标；
则所述车辆位姿表示为公式(5)所示：
(tx,ty,θ)＝argminReporj(tx,ty,θ) (5)
3.1.2RANSAC框架下通过角线特征进行位姿估计
选取两对匹配的角线特征作为种子，记为(fi,m,fj,n)、(fp,m,fq,n)∈MFm,n；令fi,m的交叉点pi,m(xi,m,yi,m)在第n帧上的投影
Figure BDA0002555597780000242
与fj,n的交叉点pj,n(xj,n,yj,n)重合，即
Figure BDA0002555597780000243
有
Figure BDA0002555597780000244
同理，对于(fp,m,fq,n)∈MFm,n有
Figure BDA0002555597780000245
cos2θm,n+sin2θm,n＝1 (8)
其中Tm,n(txm,n,tym,n,θm,n)∈SE(2)为第m帧到第n帧的转换关系；SE(2)表示平面上的平移、旋转变换；(txm,n,tym,n)为位移分量，θm,n为旋转分量；
联立公式(6)、(7)和(8)表示的方程，求解出第m帧中点到第n帧转换关系Tm,n(txm,n,tym,n,θm,n)；
在随机抽样一致性算法(RANSAC)框架下，通过多次计算，按照公式(4)和(5)选择重投影误差最小的位姿Tmn作为第m帧到第n帧的位姿。
3.1.3通过重投影误差进行位姿估计
通过投影误差进行计算时：在进行车辆t时刻的姿态进行估计时，采用t-1时刻图像的位姿作为初始值，根据角线特征的匹配结果，以当前位姿作为变量、最小投影误差为目标建立损失函数，如公式(4)(5)，使用Levenberg-Marquardt方法对当前位姿进行求解，使用到的工具是g2o。
3.2异常检测和矫正
实际的跟踪的过程中不能实时保证地平面设定。即顶视图坐标系与地面不重合，如车辆经过减速带等情况，如图4所示。此时会造成生成的顶视图不能够很好的表明路面上真实的信息，在这里引入了异常检测模块，以解决由于地平面设定不成立，导致车辆位姿计算不准确而引起跟踪失效的问题。
一方面，当特征匹配数量骤减、计算出的与前一帧的相对位姿不满足车辆运动模型时(求解出的位姿是车辆不能够做出的运动，如横向移动。)，就有足够的理由相信当前车辆没有在地平面上运动，需要进行顶视图的矫正。
另一方面，当使用投影变换模型表示两个图像间的操作时，用平面单应矩阵，如公式(9)所示，来表示两帧图像的变换关系，
Figure BDA0002555597780000251
其中，hij,(i,j＝1,2,3)表示单应矩阵的矩阵元素，将单应性矩阵分解得到旋转分量和平移分量，此时旋转分量与平移分量中包含三维信息；将旋转分量分解为欧拉角，当欧拉角中俯仰角或翻滚角超过一定阈值时，表明车辆遇到颠簸。
当车辆遇到颠簸时，可以将当前帧图像舍弃掉，或使用前两帧图像位姿的偏差当作当前图像到上一帧图像的位姿偏差，也可以将当前帧图像进行调整。
调整时获得平面单应矩阵，以及对应的欧拉角、平移距离。将去掉位姿的俯仰角、翻滚角以及去掉平移向量中的高度分量作为矫正过的车辆位姿，将当前帧图像由矫正前的相机坐标系映射到矫正过的相机坐标系，以实现当前帧图像的矫正。
4、角线地图的生成
4.1角线地图的组成
角线地图由关键帧和地图角线特征组成。地图角线特征分为两层结构：角结构、线结构。线结构是地图角线特征的原子结构，角特征由线特征和描述子组成。这是由于在某些帧中，同一条线有组成两个角线结构的可能，如图5所示。如果将包含同一条直线的两个角线特征当作原子结构进行优化时，缺少了由同一条直线组成的约束。
4.2角线地图的初始化
当进入建图模式且为地图为空时，需要进行角线地图的初始化。当从两帧图像恢复出的运动满足有一定条件，如位移超过某个小量、匹配的角线数量超过某个量值等，进行角线地图的初始化。
初始化的步骤是：1)将这两帧图像加入到角线地图当中；2)对于两帧中的每一对匹配的角线特征，在地图中生成一个新的地图角线特征，用mfi来表示：
mfi＝(fi,S＝{fj,k,…})
其中fi表示地图角线特征中的位置、描述子的值，S表示与mfi存在可视关系的角线特征的集合，fj,k为第k帧中第j个角线特征。首先根据可视关系，更新集合S，再根据S中的每一个角线特征，融合生成地图角线特征的位置、描述子的属性。
4.3角线地图的更新
当当前帧图像与最后一个关键帧发生一段距离的移动时，就可以将当前帧图像加入到角线地图中。
向地图中加入新的地图角线特征时，依照匹配关系，首先，更新角线特征、线特征的可视关系，然后根据可视关系，依次对地图线特征的位姿、角线特征的描述子进行更新。
5、角线地图的局部和全局优化
5.1角线地图的局部优化
以最后一帧及其局部地图的位姿以及局部地图可以观察到的角线特征的位置作为变量，以最小重投影误差作为目标函数，利用光束法平差(Bundle Adjustment)进行优化，使得获得的关键帧位姿、地图角线特征的位置更加精确。
5.2角线地图的全局优化
在建图过程中，如果发现获取的一个帧序列与地图中的唯一一个帧序列匹配时，认定为发生了回环，进入角线地图的全局优化。在全局优化过程中，将关键帧的位姿、地图角线特征中线的位姿作为变量，以最小重投影误差作为损失函数，进行优化。根据地图角线特征中线位姿的调整结果重新计算地图角线特征的描述子。
自主泊车中，现有技术的方法在定位精度、稳定性和实时性上难以获得满意的综合效果。
相比于现有技术方法在直接获取到的原始图像中进行特征检测工作，本发明直接在四幅鱼眼相机投影拼接形成的顶视图中检测停车场特征，仅特征提取部分的计算量即可降低约3/4，降低了计算成本，能够满足实时性要求；本发明建立的二维平面设定，使用车载环视系统生成基于平行投影的顶视图，顶视图具有与物理空间相同的确定尺度，在顶视图上进行SLAM，只需要三个自由度，相比于传统slam的六个自由度，降低了很多计算量，且尺度明确，无需考虑尺度漂移问题，从而从原理上解决了单目slam的尺度问题；本发明利用仿射变换模型，检测车辆的颠簸情况，避免车辆颠簸、上下坡时，因二维平面设定不成立导致跟踪失效的问题；由于使用角线特征对车位线进行描述，降低了三维空间中基于点特征的SLAM算法的特征使用量，提高了地面上这种弱纹理平面上线特征表达能力的不足，同时也解决了线特征描述能力的问题。降低计算量的同时保证定位精度，能够满足在低功耗处理器上实时需求；本发明使用角、线的二层结构建立角线地图，增加对地图角线特征的约束，能够进行更准确的位姿估计，计算结果精度高；本发明所述方法仅通过车载环视系统实现在停车场环境下的自定位和地图构建，成本低，性价比高。
可见，本发明的基于顶视图的停车场车辆自定位及地图构建方法，以低成本的鱼眼相机组成的环视系统作为信息采集系统，基于所述环视系统生成顶视图，鲁棒性地提取车辆附近车位线的角线特征，进行跟踪和建图；利用图匹配技术实时生成角线地图，并利用局部地图优化和全局地图优化，对车辆进行实时高精度定位和地图构建。所述方法从原理上避免了单目slam的尺度漂移问题，并利用现有的车载系统，以低成本传感器、在低功耗的车载处理器上完成实时、高精度车辆自定位和地图构建工作，来辅助自主泊车任务的完成。
综上所述，本发明提出的新的停车场车辆自定位及地图构建方法，既能保证定位精度，又能适应室内、室外的背景环境，又不增加额外的计算开销，可用于低功耗车载处理器，同时也无需高成本传感器系统支持。采用本发明的新的停车场车辆自定位及地图构建方法的自主泊车技术普适于室内、室外停车场，并能以低成本的传感器系统、低功耗的车载处理器系统获得高效的辅助自主泊车效果，具有广泛的应用前景。
至此，已经结合附图所示的优选实施方法描述了本发明的技术方案，但是，本领域技术人员应当理解，上述实施方式仅仅是为了清楚地说明本发明，而并非是对本发明的范围进行限定，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征作出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。

Claims (10)

1.一种基于顶视图的停车场车辆自定位及地图构建方法，其特征在于，包括如下步骤：
步骤1)、采集相机图像；
步骤2)、根据所述相机图像进行车辆顶视图的拼接；
步骤3)、对拼接的所述车辆顶视图提取并匹配角线特征；
步骤4)、基于所述角线特征计算车辆位姿，生成角线地图；
步骤5)、根据生成的所述角线地图匹配所述角线特征；
步骤6)、输出车辆位姿。
2.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法，其特征在于，所述相机为鱼眼相机或所述相机为多个工业小孔相机组合而成。
3.根据权利要求2所述的基于顶视图的停车场车辆自定位及地图构建方法，其特征在于，所述鱼眼相机有四个，分辨率在720P或以上，分别布置于汽车前后保险杠以及左右后视镜下边沿。
4.根据权利要求2或3所述的基于顶视图的停车场车辆自定位及地图构建方法，其特征在于，
对所述鱼眼相机进行标定，得到相机的内参和相机到地面靶标的外参。
5.根据权利要求4所述的基于顶视图的停车场车辆自定位及地图构建方法，其特征在于，
标定时，车辆静止在平整路面上，采用标定板直接获取从鱼眼相机图像到顶视图坐标系的对应关系，计算生成各相机的原始图像与顶视图像素的映射表。
6.根据权利要求5所述的基于顶视图的停车场车辆自定位及地图构建方法，其特征在于，
所述顶视图坐标系位于车底平面上，以车辆后轴中心为原点，水平向前和水平向右分别为坐标轴正方向。
7.一种基于顶视图的停车场车辆自定位及地图构建装置，其特征在于，包括如下步骤：
图像采集装置，采集相机图像；
顶视图拼接装置，根据所述相机图像进行车辆顶视图的拼接；
特征提取匹配装置，对拼接的所述车辆顶视图提取并匹配角线特征；
地图生成装置，基于所述角线特征计算车辆位姿，生成角线地图；
匹配装置，根据生成的所述角线地图匹配所述角线特征；
输出装置，输出车辆位姿。
8.一种基于顶视图的停车场车辆自定位及地图构建系统，其特征在于，包括：
存储器，所述存储器存储执行如权利要求1所述方法的程序；
处理器；所述处理器执行所述程序。
9.一种自动驾驶车辆，其特征在于，包括：
所述自动驾驶车辆上搭载如权利要求7所述的装置或权利要求8所述的系统。
10.一种自主泊车系统，其特征在于，包括：
如权利要求7所述的装置或如权利要求8所述的系统。
CN202010588699.3A 2020-06-24 2020-06-24 基于顶视图的停车场车辆自定位及地图构建方法 Active CN111862673B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010588699.3A CN111862673B (zh) 2020-06-24 2020-06-24 基于顶视图的停车场车辆自定位及地图构建方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010588699.3A CN111862673B (zh) 2020-06-24 2020-06-24 基于顶视图的停车场车辆自定位及地图构建方法

Publications (2)

Publication Number Publication Date
CN111862673A true CN111862673A (zh) 2020-10-30
CN111862673B CN111862673B (zh) 2021-10-15

Family

ID=72989831

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010588699.3A Active CN111862673B (zh) 2020-06-24 2020-06-24 基于顶视图的停车场车辆自定位及地图构建方法

Country Status (1)

Country Link
CN (1) CN111862673B (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113030960A (zh) * 2021-04-06 2021-06-25 陕西国防工业职业技术学院 一种基于单目视觉slam的车辆定位方法
CN113353067A (zh) * 2021-07-14 2021-09-07 重庆大学 一种基于全景摄像头的多环境检测与多模态匹配的平行泊车路径规划系统
CN113483755A (zh) * 2021-07-09 2021-10-08 北京易航远智科技有限公司 一种基于非全局一致地图的多传感器组合定位方法及系统
CN113535868A (zh) * 2021-06-11 2021-10-22 上海追势科技有限公司 一种基于公开导航地图的自主泊车高精地图生成方法
CN113705474A (zh) * 2021-08-30 2021-11-26 北京易航远智科技有限公司 车位检测方法和装置
CN114088083A (zh) * 2021-11-09 2022-02-25 北京易航远智科技有限公司 一种基于顶视图语义对象的建图方法
CN115841766A (zh) * 2023-02-22 2023-03-24 青岛慧拓智能机器有限公司 一种矿区作业区域停靠点车位推荐方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109443348A (zh) * 2018-09-25 2019-03-08 同济大学 一种基于环视视觉和惯导融合的地下车库库位跟踪方法
CN109631855A (zh) * 2019-01-25 2019-04-16 西安电子科技大学 基于orb-slam的高精度车辆定位方法
CN110132278A (zh) * 2019-05-14 2019-08-16 驭势科技（北京）有限公司 一种即时定位与建图的方法及装置
CN111058664A (zh) * 2018-10-16 2020-04-24 现代自动车株式会社 自动停车设备及方法
US20200193633A1 (en) * 2018-12-13 2020-06-18 Denso Ten Limited Image processing device and image processing method

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109443348A (zh) * 2018-09-25 2019-03-08 同济大学 一种基于环视视觉和惯导融合的地下车库库位跟踪方法
CN111058664A (zh) * 2018-10-16 2020-04-24 现代自动车株式会社 自动停车设备及方法
US20200193633A1 (en) * 2018-12-13 2020-06-18 Denso Ten Limited Image processing device and image processing method
CN109631855A (zh) * 2019-01-25 2019-04-16 西安电子科技大学 基于orb-slam的高精度车辆定位方法
CN110132278A (zh) * 2019-05-14 2019-08-16 驭势科技（北京）有限公司 一种即时定位与建图的方法及装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
王鹏飞: "基于全景环视系统的车位检测技术的研究", 《中国优秀硕士学位论文全文数据库（电子期刊），工程科技Ⅱ辑》 *

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113030960A (zh) * 2021-04-06 2021-06-25 陕西国防工业职业技术学院 一种基于单目视觉slam的车辆定位方法
CN113535868A (zh) * 2021-06-11 2021-10-22 上海追势科技有限公司 一种基于公开导航地图的自主泊车高精地图生成方法
CN113483755A (zh) * 2021-07-09 2021-10-08 北京易航远智科技有限公司 一种基于非全局一致地图的多传感器组合定位方法及系统
CN113483755B (zh) * 2021-07-09 2023-11-07 北京易航远智科技有限公司 一种基于非全局一致地图的多传感器组合定位方法及系统
CN113353067A (zh) * 2021-07-14 2021-09-07 重庆大学 一种基于全景摄像头的多环境检测与多模态匹配的平行泊车路径规划系统
CN113705474A (zh) * 2021-08-30 2021-11-26 北京易航远智科技有限公司 车位检测方法和装置
CN113705474B (zh) * 2021-08-30 2022-04-15 北京易航远智科技有限公司 车位检测方法和装置
CN114088083A (zh) * 2021-11-09 2022-02-25 北京易航远智科技有限公司 一种基于顶视图语义对象的建图方法
CN114088083B (zh) * 2021-11-09 2023-10-31 北京易航远智科技有限公司 一种基于顶视图语义对象的建图方法
CN115841766A (zh) * 2023-02-22 2023-03-24 青岛慧拓智能机器有限公司 一种矿区作业区域停靠点车位推荐方法

Also Published As

Publication number Publication date
CN111862673B (zh) 2021-10-15

Similar Documents

Publication Publication Date Title
CN111862672B (zh) 基于顶视图的停车场车辆自定位及地图构建方法
CN111862673B (zh) 基于顶视图的停车场车辆自定位及地图构建方法
Heng et al. Project autovision: Localization and 3d scene perception for an autonomous vehicle with a multi-camera system
Paya et al. A state-of-the-art review on mapping and localization of mobile robots using omnidirectional vision sensors
Konolige et al. Large-scale visual odometry for rough terrain
CN111986506B (zh) 基于多视觉系统的机械式停车位泊车方法
Zhou et al. Ground-plane-based absolute scale estimation for monocular visual odometry
CN111983639A (zh) 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法
CN109443348B (zh) 一种基于环视视觉和惯导融合的地下车库库位跟踪方法
Parra et al. Robust visual odometry for vehicle localization in urban environments
Goecke et al. Visual vehicle egomotion estimation using the fourier-mellin transform
Gao et al. Ground and aerial meta-data integration for localization and reconstruction: A review
CN113658337B (zh) 一种基于车辙线的多模态里程计方法
CN113781562B (zh) 一种基于道路模型的车道线虚实配准和自车定位方法
CN112669354A (zh) 一种基于车辆非完整约束的多相机运动状态估计方法
CN114325634A (zh) 一种基于激光雷达的高鲁棒性野外环境下可通行区域提取方法
Muresan et al. Real-time object detection using a sparse 4-layer LIDAR
Lui et al. A pure vision-based approach to topological SLAM
Rehder et al. Submap-based SLAM for road markings
Guizilini et al. Semi-parametric models for visual odometry
Hara et al. Vehicle localization based on the detection of line segments from multi-camera images
Zhang LILO: A Novel Lidar–IMU SLAM System With Loop Optimization
Beauvisage et al. Robust multispectral visual-inertial navigation with visual odometry failure recovery
Qian et al. Survey on fish-eye cameras and their applications in intelligent vehicles
CN112731503A (zh) 一种基于前端紧耦合的位姿估计方法及系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant