CN111862672B - 基于顶视图的停车场车辆自定位及地图构建方法 - Google Patents
基于顶视图的停车场车辆自定位及地图构建方法 Download PDFInfo
- Publication number
- CN111862672B CN111862672B CN202010587090.4A CN202010587090A CN111862672B CN 111862672 B CN111862672 B CN 111862672B CN 202010587090 A CN202010587090 A CN 202010587090A CN 111862672 B CN111862672 B CN 111862672B
- Authority
- CN
- China
- Prior art keywords
- parking space
- top view
- parking
- positioning
- vehicle
- 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.)
- Active
Links
- 238000010276 construction Methods 0.000 title abstract description 12
- 238000000034 method Methods 0.000 claims abstract description 102
- 238000001514 detection method Methods 0.000 claims abstract description 77
- 238000005457 optimization Methods 0.000 claims abstract description 66
- 238000004364 calculation method Methods 0.000 claims abstract description 40
- 238000013507 mapping Methods 0.000 claims description 104
- 238000004422 calculation algorithm Methods 0.000 claims description 92
- 239000013598 vector Substances 0.000 claims description 76
- 239000011159 matrix material Substances 0.000 claims description 37
- 238000000605 extraction Methods 0.000 claims description 28
- 238000005070 sampling Methods 0.000 claims description 15
- 230000004927 fusion Effects 0.000 claims description 13
- 238000013135 deep learning Methods 0.000 claims description 12
- 238000005516 engineering process Methods 0.000 claims description 12
- 238000005259 measurement Methods 0.000 claims description 12
- 238000001914 filtration Methods 0.000 claims description 11
- 238000012216 screening Methods 0.000 claims description 11
- 238000006073 displacement reaction Methods 0.000 claims description 10
- 230000003068 static effect Effects 0.000 claims description 10
- 238000013519 translation Methods 0.000 claims description 10
- 238000012897 Levenberg–Marquardt algorithm Methods 0.000 claims description 5
- 238000006243 chemical reaction Methods 0.000 claims description 5
- 238000007635 classification algorithm Methods 0.000 claims description 5
- 230000004438 eyesight Effects 0.000 claims description 5
- 238000007711 solidification Methods 0.000 claims description 5
- 230000008023 solidification Effects 0.000 claims description 5
- 239000000284 extract Substances 0.000 claims description 3
- 230000000694 effects Effects 0.000 abstract description 6
- 238000010586 diagram Methods 0.000 description 5
- 238000012545 processing Methods 0.000 description 5
- 230000005540 biological transmission Effects 0.000 description 3
- 239000000470 constituent Substances 0.000 description 3
- 230000000007 visual effect Effects 0.000 description 3
- 238000007796 conventional method Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000011218 segmentation Effects 0.000 description 2
- 238000003860 storage Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 235000004522 Pentaglottis sempervirens Nutrition 0.000 description 1
- 240000004050 Pentaglottis sempervirens Species 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000012512 characterization method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 230000008034 disappearance Effects 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 230000002708 enhancing effect Effects 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012827 research and development Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 230000016776 visual perception Effects 0.000 description 1
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
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Multimedia (AREA)
- Theoretical Computer Science (AREA)
- Signal Processing (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Traffic Control Systems (AREA)
- Image Analysis (AREA)
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={(ui,vi)|i=1,2,3…n},其中(ui,vi)代表第i个角点坐标,n代表角点数量;利用目标检测算法得到每个车位编号的位置框Rect={(ui,vi,wi,hi)|i=1,2,3…m},其中,(ui,vi)代表检测框的中心位置,(wi,hi)代表检测的框的宽和高,m代表单帧图像中检测到的车位数量;
首先,分成三个部分进行:
第一部分,利用车位检测算法获取车位是否为有效车位以及车位类别Classm,m∈[0,6];
第二部分,利用车位的出口角点与车位号的位置关系,生成相对坐标向量;
第三部分,利用特征工程的算法提取车位编号特征向量,生成唯一车位编号向量N;
然后,将上述三个部分的特征结合,生成增强的车位特征;最后,利用车位线的平行关系,优化帧内的角点坐标。
根据至少一个实施方式,所述角点提取算法为Harris角点检测+SVM分类算法。
根据至少一个实施方式,所述角点提取算法为深度学习网络算法。
根据至少一个实施方式,所述目标检测算法为深度学习网络算法。
根据至少一个实施方式,所述目标检测算法为模板匹配算法。
根据至少一个实施方式,所述车位类型包括非法车位、左水平车位、右水平车位、左垂直车位、右垂直车位、左斜车位和右斜车位共七个类别。
根据至少一个实施方式,采用OCR技术提取图像矩阵中的编号标志。
根据至少一个实施方式,采用ORB或SIFT特征描述子生成唯一车位号特征向量。
根据至少一个实施方式,所述相对坐标向量是指,经过任一车位的两出口角点坐标,与车位编号检测框的中点坐标,且依据标定和投影过程中计算得到的投影矩阵,以及计算得到的对应的车身坐标,所确定的坐标向量;
其中,
所述任一车位的两出口角点坐标如公式(1)所示:
P2m=(u2m,v2m,1)T,P2m+1=(u2m+1,v2m+1,1)T (1)
其中P2m和P2m+1分别代表第m个车位出口线的左右角点坐标,以齐次坐标的形式表示,u2m、,v2m、u2m+1、v2m+1均为以顶视图左上角为原点的像素偏移;
所述车位编号检测框的中点坐标如公式(2)所示:
Rectm=(um,vm,1)T (2)
其中Rectm代表第m个车位的中点坐标,um、vm均为以顶视图左上角为原点的像素偏移;
所述计算得到对应的车身坐标如公式(3)所示:
计算公式如公式(4)所示:
根据至少一个实施方式,所述优化帧内的角点坐标是指依据停车场中类型一致、位置相近且入库角度的差值小于一定程度的的车位应当符合入库线平行这一规范,对图中的停车场队列进行优化。
根据至少一个实施方式,所述优化方法为:在计算得到增强的车位特征后,遍历车位特征列表,依据车位类型划分聚类中心,并以特征中对应角点坐标距离和入库角度的差值作为距离度量,进行聚类算法,计算出k个聚类中心;对每个聚类簇中的车位特征,取其中的车位出库角点坐标,进行直线拟合,并计算投影误差;当投影误差总和大于第一阈值时,将重新进行角点检测工作;否则,将每个角点坐标投影到该直线上,生成新的角点坐标。
根据至少一个实施方式,轮速计姿态估计的表达式如公式(5)所示:
其中,[xt,yt]T代表t时刻时,当前车辆在世界坐标系下的坐标,θt代表t时刻时车辆的航向角;[xt+1,yt+1]T代表t+1时刻时,当前车辆在世界坐标系下的坐标,θt+1代表t+1时刻时车辆的航向角; Δsl和Δsr分别代表两帧之间左轮和右轮移动的距离,B代表轮距。
根据至少一个实施方式,车辆的t时刻的速度vt和车辆坐标ct,两帧间时间间隔ΔT,记t+1时刻的车辆坐标为ct+1=ct+vt·ΔT,并依次计算局部地图中的车位特征角点坐标与车辆坐标ct+1的距离,超出顶视图视景范围的车位特征予以筛除;然后将当前帧中检测得到的车位特征列表与局部地图中映射而来的车位特征列表进行匹配追踪。
根据至少一个实施方式,所述顶视图视景范围为以车为中心,5米为半径的区域。
根据至少一个实施方式,根据所述匹配追踪的状态进行如下三种操作:
1)全部追踪成功:证明车辆处于静止状态或位移过小,利用卡尔曼系滤波将轮速计推理的位姿与观测估计的位姿进行融合优化;
2)部分追踪成功:证明车辆发生了一定的位移,用卡尔曼滤波将轮速计推理的位姿与观测估计的位姿进行融合优化,之后更新本帧内全部的车位坐标,加入关键帧候选队列;
3)全部追踪失败:仅利用轮速计信息推理位姿,加入关键帧候选队列。
根据至少一个实施方式,所述置信度指代车位号提取过程中的车位编号为真值的概率。
根据至少一个实施方式,所述实时建图过程中,局部地图由关键帧组成,关键帧的筛选标准为:地面平坦条件下,当前帧全部追踪失败且车位检测数量不为0,或者为本帧中部分车位追踪成功且追踪失败的车位占当前帧车位检测数量的比例大于第三阈值,车辆姿态变化大于第四阈值。
根据至少一个实施方式,所述地面平坦条件是指,利用IMU或者视觉相关算法检测车身俯仰角变化,变化幅度小于第五阈值,则认为此时地面平坦。
根据至少一个实施方式,对局部地图进行优化操作,剔除冗余关键帧,并对相同的车位特征进行融合。
根据至少一个实施方式,所述对局部地图进行优化操作包括:
采用车辆的平移和旋转描述车辆的运动,i时刻的车辆位姿描述为公式(6)所示的形式:
ci=[ti,θi]T=[xi,yi,θi]T (6)
则i时刻到j时刻的位姿变化描述为公式(7)所示的形式:
其中Ri为2*2的旋转矩阵,ti和θi,分别代表i时刻的世界坐标和航向角,即ti=[xi,yi]T,h(ci,cj)为运动方程;设定从j时刻观察到的车位点的坐标与从i时刻观察到的车位点坐标的差值为zij,则误差方程如公式(8)和(9)所示:
eij=zij-h(ci,cj) (8)
其中eij代表对于同一车位在不同时刻由位姿估计和车位测量两方面影响带来的误差项,Λ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={(ui,vi)|i=1,2,3…n},其中(ui,vi)代表第i个角点坐标,n代表角点数量;利用目标检测算法得到每个车位编号的位置框Rect={(ui,vi,wi,hi)|i=1,2,3…m},其中,(ui,vi)代表检测框的中心位置,(wi,hi)代表检测的框的宽和高,m代表单帧图像中检测到的车位数量;
首先,分成三个部分进行:
第一部分,利用车位检测算法获取车位是否为有效车位以及车位类别Classm,m∈[0,6];
第二部分,利用车位的出口角点与车位号的位置关系,生成相对坐标向量;
第三部分,利用特征工程的算法提取车位编号特征向量,生成唯一车位编号向量N;
然后,将上述三个部分的特征结合,生成增强的车位特征;最后,利用车位线的平行关系,优化帧内的角点坐标。
根据至少一个实施方式,所述角点提取算法为Harris角点检测+SVM分类算法。
根据至少一个实施方式,所述角点提取算法为深度学习网络算法。
根据至少一个实施方式,所述目标检测算法为深度学习网络算法。
根据至少一个实施方式,所述目标检测算法为模板匹配算法。
根据至少一个实施方式,所述车位类型包括非法车位、左水平车位、右水平车位、左垂直车位、右垂直车位、左斜车位和右斜车位共七个类别。
根据至少一个实施方式,采用OCR技术提取图像矩阵中的编号标志。
根据至少一个实施方式,采用ORB或SIFT特征描述子生成唯一车位号特征向量。
根据至少一个实施方式,所述相对坐标向量是指,经过任一车位的两出口角点归一化坐标,与车位编号检测框的中点坐标,且依据标定和投影过程中计算得到的投影矩阵,以及计算得到的对应的车身坐标,所确定的坐标向量;
其中,
所述任一车位的两出口角点归一化坐标如公式(1)所示:
P2m=(u2m,v2m,1)T,P2m+1=(u2m+1,v2m+1,1)T (1)
其中P2m和P2m+1分别代表第m个车位出口线的左右角点坐标,以齐次坐标的形式表示,u2m、,v2m、u2m+1、v2m+1均为以顶视图左上角为原点的像素偏移;
所述车位编号检测框的中点坐标如公式(2)所示:
Rectm=(um,vm,1)T (2)
其中Rectm代表第m个车位的中点坐标,um、vm均为以顶视图左上角为原点的像素偏移;
所述计算得到对应的车身坐标如公式(3)所示:
计算公式如公式(4)所示:
根据至少一个实施方式,所述优化帧内的角点坐标是指依据停车场中类型一致、位置相近且入库角度的差值小于一定程度的的车位应当符合入库线平行这一规范,对图中的停车场队列进行优化。
根据至少一个实施方式,所述优化的方法为:在计算得到增强的车位特征后,遍历车位特征列表,依据车位类型划分聚类中心,并以特征中对应角点坐标距离和入库角度的差值作为距离度量,进行聚类算法,计算出k个聚类中心;对每个聚类簇中的车位特征,取其中的车位出库角点坐标,进行直线拟合,并计算投影误差;当投影误差总和大于第一阈值时,将重新进行角点检测工作;否则,将每个角点坐标投影到该直线上,生成新的角点坐标。
根据至少一个实施方式,轮速计姿态估计的表达式如公式(5)所示:
其中,[xt,yt]T代表t时刻时,当前车辆在世界坐标系下的坐标,θt代表t时刻时车辆的航向角;[xt+1,yt+1]T代表t+1时刻时,当前车辆在世界坐标系下的坐标,θt+1代表t+1时刻时车辆的航向角; Δsl和Δsr分别代表两帧之间左轮和右轮移动的距离,B代表轮距。
根据至少一个实施方式,车辆的t时刻的速度vt和车辆坐标ct,两帧间时间间隔ΔT,记t+1时刻的车辆坐标为ct+1=ct+vt·ΔT,并依次计算局部地图中的车位特征角点坐标与车辆坐标ct+1的距离,超出顶视图视景范围的车位特征予以筛除;然后将当前帧中检测得到的车位特征列表与局部地图中映射而来的车位特征列表进行匹配追踪。
根据至少一个实施方式,所述顶视图视景范围为以车为中心,5米为半径的区域。
根据至少一个实施方式,根据所述匹配追踪的状态进行如下三种操作:
1)全部追踪成功:证明车辆处于静止状态或位移过小,利用卡尔曼系滤波将轮速计推理的位姿与观测估计的位姿进行融合优化;
2)部分追踪成功:证明车辆发生了一定的位移,用卡尔曼滤波将轮速计推理的位姿与观测估计的位姿进行融合优化,之后更新本帧内全部的车位坐标,加入关键帧候选队列;
3)全部追踪失败:仅利用轮速计信息推理位姿,加入关键帧候选队列。
根据至少一个实施方式,所述置信度指代车位号提取过程中的车位编号为真值的概率。
根据至少一个实施方式,所述实时建图过程中,局部地图由关键帧组成,关键帧的筛选标准为:地面平坦条件下,当前帧全部追踪失败且车位检测数量不为0,或者为本帧中部分车位追踪成功且追踪失败的车位占当前帧车位检测数量的比例大于第三阈值,车辆姿态变化大于第四阈值。
根据至少一个实施方式,所述地面平坦条件是指,利用IMU或者视觉相关算法检测车身俯仰角变化,变化幅度小于第五阈值,则认为此时地面平坦。
根据至少一个实施方式,对局部地图进行优化操作,剔除冗余关键帧,并对相同的车位特征进行融合。
根据至少一个实施方式,所述对局部地图进行优化操作包括:
采用车辆的平移和旋转描述车辆的运动,i时刻的车辆位姿描述为公式(6)所示的形式:
ci=[ti,θi]T=[xi,yi,θi]T (6)
则i时刻到j时刻的位姿变化描述为公式(7)所示的形式:
其中Ri为2*2的旋转矩阵,ti和θi,分别代表i时刻的世界坐标和航向角,即ti=[xi,yi]T,h(ci,cj)为运动方程;设定从j时刻观察到的车位点的坐标与从i时刻观察到的车位点坐标的差值为zij,则误差方程如公式(8)和(9)所示:
eij=zij-h(ci,cj) (8)
其中eij代表对于同一车位在不同时刻由位姿估计和车位测量两方面影响带来的误差项,Λ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={(ui,vi)|i=1,2,3…n+,其中(ui,vi)代表第i个角点坐标,并利用目标检测算法得到每个车位编号的位置框Rect={(ui,vi,wi,hi)|i=1,2,3…m+,其中,(ui,vi)代表检测框的中心位置,(wi,hi)代表检测的框的宽和高,后续分成三个部分进行:第一部分利用车位检测算法获取车位是否为有效车位以及车位类型Classm={0,1,2,3,4,5,6};第二部分利用车位的出口角点与车位号的位置关系,生成相对坐标向量;第三部分利用特征工程的相关算法提取车位编号特征向量,生成唯一车位编号向量N。然后,将上述三部分的特征结合,生成增强的车位特征;最后,利用车位线的平行关系,优化帧内的角点坐标。
上述角点提取算法包括但不限于Harris角点检测+SVM分类算法,深度学习网络算法等;
上述车位检测算法包括但不限于深度学习网络算法,模板匹配算法等;
上述车位类型主要包括非法车位、水平车位(左,右)、垂直车位(左,右)和斜车位(左,右)共七个类别。顶视图上检测出的车位角点、车位号区域的如图5所示。
上述特征工程算法的目的为提取唯一的车位标号特征向量,重点在于唯一性。既可以是利用OCR技术提取图像矩阵中的编号标志,也可以利用ORB、SIFT等特征描述子生成唯一车位号特征向量;
上述相对坐标向量是指:经过相关技术获取的任一车位的两出口角点归一化坐标
P2m=(u2m,v2m,1)T,P2m+1=(u2m+1,v2m+1,1)T (1)
与车位编号检测框的中点坐标
Rectm=(um,vm,1)T (2)
且依据标定和投影过程中计算得到的投影矩阵R,可计算得到对应的车身坐标
计算公式如下:
上述帧内坐标优化是指依据停车场中类型一致、位置相近且入库角度的差值小于一定程度的的车位应当符合入库线平行这一规范,对图中的停车场队列进行优化。
具体的优化方法为:在计算得到增强的车位特征后,遍历车位特征列表,依据车位类型划分聚类中心,并以特征中对应角点坐标距离和入库角度的差值作为距离度量,进行聚类算法,计算出k个聚类中心。对每个聚类簇中的车位特征,取其中的车位出库角点坐标,进行直线拟合,并计算投影误差;当投影误差总和大于一定阈值时,将重新进行角点检测工作;否则,将每个角点坐标投影到该直线上,生成新的角点坐标。
相比于其他SLAM***通过提取SIFT、ORB等特征描述子进行特征的提取匹配,本发明采用具有高级语义特性的增强车位特征进行特征的匹配工作,相比于上述SLAM***的特征描述子,增强车位特征具有更明确的语义,更容易完成匹配工作;同时,本发明建立车位语义拓扑地图,仅保留停车场内全部车位的增强特征,数据量少,优化、计算速度更快。
3、在建图和自定位阶段
本发明引入了轮速计作为车辆的姿态估计的补充优化,其姿态估计模型如下:相比于无人机需要估算三个维度的欧拉角和空间上三个维度的运动进行姿态观测,自动泊车***仅需要观测空间中两个维度的运动以及一个维度的欧拉角即可完成姿态的估计,即平行于地面的[x,y]T坐标与偏航角θ,更一般地,使用[x,y,θ]T作为指代。则有轮速计姿态估计的表达式如下:
如图6所示,给定车辆的t时刻的速度vt和车辆坐标ct,两帧间时间间隔ΔT,可以粗略得计算出t+1时刻的车辆坐标ct+1=ct+vt·ΔT,并依次计算局部地图中的车位特征角点坐标与车辆坐标ct+1的距离,超出顶视图视景范围(5米)的车位特征予以筛除。后将当前帧中检测得到的车位特征列表与局部地图中映射而来的车位特征列表进行匹配追踪,会有三种状态:1)全部追踪成功:则证明车辆处于静止状态或位移过小,利用卡尔曼系滤波将轮速计推理的位姿与观测估计的位姿进行融合优化;2)部分追踪成功:证明车辆发生了一定的位移,用卡尔曼滤波将轮速计推理的位姿与观测估计的位姿进行融合优化,之后更新本帧内全部的车位坐标,加入关键帧候选队列;3)全部追踪失败:仅利用轮速计信息推理位姿,加入关键帧候选队列。
建图过程中,局部地图由关键帧组成,关键帧的筛选标准为:地面平坦条件下,当前帧全部追踪失败且车位检测数量不为0,或者为本帧中部分车位追踪成功且追踪失败的车位占当前帧车位检测数量的比例大于一定阈值,车辆姿态变化大于一定阈值。
上述地面平坦条件是指,利用IMU或者视觉相关算法检测车身俯仰角变化,变化幅度小于一定阈值,则认为此时地面平坦。
***关键帧时,需要对局部地图进行优化操作,剔除冗余关键帧,并对相同的车位特征进行融合。
优化策略:由于仅考虑车辆的地面运动且车位基本位于地面,需要考虑的运动变量仅为车辆的平移和旋转,即i时刻的车辆位姿可描述为:
ci=[ti,θi]T=[xi,yi,θi]T (6)
则i时刻到j时刻的位姿变化描述为公式(7)所示的形式:
其中Ri为2*2的旋转矩阵,ti和θi,分别代表i时刻的世界坐标和航向角,即ti=[xi,yi]T,h(ci,cj)为运动方程;设定从j时刻观察到的车位点的坐标与从i时刻观察到的车位点坐标的差值为zij,则误差方程如公式(8)和(9)所示:
eij=zij-h(ci,cj) (8)
其中eij代表对于同一车位在不同时刻由位姿估计和车位测量两方面影响带来的误差项,Λ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 (77)
1.一种基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,包括实时建图和自定位两部分,其中,
所述实时建图部分包括如下步骤:
S11),采集相机图像,投影生成顶视图;
S12),在所述顶视图上进行车位检测;
S13),初始化车位地图和追踪匹配,估计车位的当前姿态,筛选关键帧;
S14),局部地图优化;
S15),回环检测与地图固化;
所述自定位部分包括:
在所述步骤S11)、S12)之后,进行如下步骤:
S23),初始化定位,在自定位***启动时,确定车辆自身位于地图中的坐标;
S24),追踪与重定位,确定车辆位姿;
所述相机为鱼眼相机;
对所述鱼眼相机进行标定;
分别对四个所述鱼眼相机采样生成顶视图,根据四个鱼眼相机标定后的内参以及相对于汽车后轴中心的外参,计算得到新的参数矩阵以生成各鱼眼相机的原始图像中地面像素对应于顶视图像素的映射关系,得到映射表,并通过采样生成顶视图;
在采样生成的顶视图上,利用角点提取算法检测得到车身周边的车位出口线的车位角点集合P={(ui,vi)|i=1,2,3...n},其中(ui,vi)代表第i个角点坐标,n代表角点数量;利用目标检测算法得到每个车位编号的位置框Rect={(ui,vi,wi,hi)|i=1,2,3...m},其中,(ui,vi)代表检测框的中心位置,(wi,hi)代表检测框的宽和高,m代表单帧图像中检测到的车位数量;
首先,分成三个部分进行:
第一部分,利用车位检测算法获取车位是否为有效车位以及车位类别Classm,m∈[0,6];
第二部分,利用车位的出口角点与车位编号的位置关系,生成相对坐标向量;
第三部分,利用特征工程的算法提取车位编号特征向量,生成唯一车位编号特征向量Nm;
然后,将上述三个部分的特征结合,生成增强的车位特征;最后,利用车位线的平行关系,优化帧内的角点坐标。
2.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,所述鱼眼相机有四个,分辨率在720P或以上,分别布置于汽车前后保险杠以及左右后视镜下边沿。
3.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,在顶视图上进行车位的检测后,并将检测到的车位的信息进行增强,生成增强的车位特征,并依据停车场规范进行帧内的坐标优化。
4.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,结合轮速计的里程计信息进行融合,来估计车位的当前姿态,筛选关键帧。
5.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,包括:
所述内参包括焦距和畸变参数;
所述外参包括相对于车身的旋转平移矩阵;
所述顶视图为10米*10米。
6.根据权利要求5所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,包括:
通过所述内参和外参计算图像中任意一地面像素点相对于车辆后轴中心的相对坐标,完成像素坐标系到车身坐标系的转换。
7.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,包括:
在顶视图的视角,对于车身周围10米*10米的范围采样实现栅格化,确定每个栅格内对应于原始图像像素的坐标信息。
8.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,包括:
所述映射表不是一对一的关系,而是一对多或多对一的关系;
对于一对多关系的像素值取均值,对于多对一关系的像素值取插值后的结果值。
9.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
引入车位编号特征向量、车位类别和车位出口三角关系信息,进一步生成增强的车位特征,用于后续的特征匹配与追踪工作。
10.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
所述角点提取算法为Harris角点检测+SVM分类算法。
11.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
所述角点提取算法为深度学习网络算法。
12.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
所述目标检测算法为深度学习网络算法。
13.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
所述目标检测算法为模板匹配算法。
14.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
所述车位类别包括非法车位、左水平车位、右水平车位、左垂直车位、右垂直车位、左斜车位和右斜车位共七个类别。
15.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
采用OCR技术提取图像矩阵中的编号标志。
16.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
采用ORB或SIFT特征描述子生成唯一车位编号特征向量。
17.根据权利要求10-16中任一项所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
所述相对坐标向量是指,经过任一车位的两出口角点坐标,与车位编号检测框的中点坐标,且依据标定和投影过程中计算得到的投影矩阵,以及计算得到的对应的车身坐标,所确定的坐标向量;
其中,
所述任一车位的两出口角点坐标如公式(1)所示:
P2m=(u2m,v2m,1)T,P2m+1=(u2m+1,v2m+1,1)T (1)
其中P2m和P2m+1分别代表第m个车位出口线的左右角点坐标,以齐次坐标的形式表示,u2m、v2m、u2m+1、v2m+1均为以顶视图左上角为原点的像素偏移;
所述车位编号检测框的中点坐标如公式(2)所示:
Rectm=(um,vm,1)T (2)
其中Rectm代表第m个车位的中点坐标,um、vm均为以顶视图左上角为原点的像素偏移;
所述计算得到对应的车身坐标如公式(3)所示:
计算公式如公式(4)所示:
19.根据权利要求10-16中任一项所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
所述优化帧内的角点坐标是指依据停车场中类型一致、位置相近且入库角度的差值小于一定程度的的车位应当符合入库线平行这一规范,对图中的停车场队列进行优化。
20.根据权利要求19所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
所述优化方法为:在计算得到增强的车位特征后,遍历车位特征列表,依据车位类别划分聚类中心,并以特征中对应角点坐标距离和入库角度的差值作为距离度量,进行聚类算法,计算出k个聚类中心;对每个聚类簇中的车位特征,取其中的车位出库角点坐标,进行直线拟合,并计算投影误差;当投影误差总和大于第一阈值时,将重新进行角点检测工作;否则,将每个角点坐标投影到该直线上,生成新的角点坐标。
22.根据权利要求21所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
车辆的t时刻的速度vt和车辆坐标ct,两帧间时间间隔ΔT,记t+1时刻的车辆坐标为ct+1=ct+vt·ΔT,并依次计算局部地图中的车位特征角点坐标与车辆坐标ct+1的距离,超出顶视图视景范围的车位特征予以筛除;然后将当前帧中检测得到的车位特征列表与局部地图中映射而来的车位特征列表进行匹配追踪。
23.根据权利要求22所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
所述顶视图视景范围为以车为中心,5米为半径的区域。
24.根据权利要求22所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
根据所述匹配追踪的状态进行如下三种操作:
1)全部追踪成功:证明车辆处于静止状态或位移过小,利用卡尔曼滤波将轮速计推理的位姿与观测估计的位姿进行融合优化;
2)部分追踪成功:证明车辆发生了一定的位移,用卡尔曼滤波将轮速计推理的位姿与观测估计的位姿进行融合优化,之后更新本帧内全部的车位坐标,加入关键帧候选队列;
3)全部追踪失败:仅利用轮速计信息推理位姿,加入关键帧候选队列。
26.根据权利要求25所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
所述置信度指代车位编号提取过程中的车位编号为真值的概率。
27.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
所述实时建图过程中,局部地图由关键帧组成,关键帧的筛选标准为:地面平坦条件下,当前帧全部追踪失败且车位检测数量不为0,或者为本帧中部分车位追踪成功且追踪失败的车位占当前帧车位检测数量的比例大于第三阈值,车辆姿态变化大于第四阈值。
28.根据权利要求27所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
所述地面平坦条件是指,利用IMU或者视觉相关算法检测车身俯仰角变化,变化幅度小于第五阈值,则认为此时地面平坦。
29.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
对局部地图进行优化操作,剔除冗余关键帧,并对相同的车位特征进行融合。
30.根据权利要求29所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
所述对局部地图进行优化操作包括:
采用车辆的平移和旋转描述车辆的运动,i时刻的车辆位姿描述为公式(6)所示的形式:
ci=[ti,θi]T=[xi,yi,θi]T (6)
则i时刻到j时刻的位姿变化描述为公式(7)所示的形式:
其中Ri为2*2的旋转矩阵,ti和θi,分别代表i时刻的世界坐标和航向角,即ti=[xi,yi]T,h(ci,cj)为运动方程;设定从j时刻观察到的车位点的坐标与从i时刻观察到的车位点坐标的差值为zij,则误差方程如公式(8)和(9)所示:
eij=zij-h(ci,cj) (8)
其中eij代表对于同一车位在不同时刻由位姿估计和车位测量两方面影响带来的误差项,Λij代表信息矩阵,F(c)为优化方程,优化目的即为最小化F(c)。
31.根据权利要求29所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
采用Gauss-Newton算法或Levenberg-Marquardt算法进行优化求解。
32.根据权利要求29-31中任一项所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
当新加入的关键帧与地图早期的关键帧有重合车位时,即为检测到了回环,并将地图中的全部车位加入优化队列,进行整体优化。
33.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
当所述实时建图的工作完成后,将全部关键帧删除,只保留车位特征向量构建地图,在删除关键帧的同时,建立车位特征的词典,用于后续定位过程中的快速查找。
34.根据权利要求1、4、9中任一项所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
所述增强的车位特征的词典建立方法包括以下步骤:
首先,依据已建成的图,将图的坐标轴旋转至与车位线平行;
其次,按照x轴的值,进行第一层聚类,然后按照第二层的值,进行第二次聚类,
最后,生成三层的树的结构以供快速查询。
35.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
所述初始化定位完成车辆位姿的初始化,以确定车辆自身处于车位地图中的位置。
36.根据权利要求35所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
所述初始化定位包括如下步骤:
首先,保持车辆处于静止状态,将k帧作为初始化帧序列,并逐帧进行顶视图投影和车位特征提取算法,设定初始序列帧中,实际共有m个车位,则应共提取出n≈k*m个车位特征;其中k表示初始化中需要的最少帧数,为预设的阈值;
其次,利用特征匹配方法对任意两帧的车位特征进行两两匹配,成功匹配一次则两帧内对应车位特征的置信权重增加1/k一1,而后在帧内优化时,加入每个角点的权重进行直线拟合,将每个角点投影到直线上后再进行聚类,取聚类中心为最终的车位特征角点;
然后,遍历地图中的车位列表,与候选车位列表的车位特征列表进行追踪匹配,此时由于车身坐标未知,因此,仅依靠车位特征中的车位编号特征向量进行匹配;
最后,匹配完成后,计算当前车辆相对于地图的坐标关系,并优化位姿,最小化投影误差,最终计算出当前车辆位姿坐标。
37.根据权利要求1所述的基于顶视图的停车场车辆自定位及地图构建方法,其特征在于,
所述追踪与重定位包括,任意时刻相机采集的原始图像都被采样生成顶视图,并提取出待匹配车位特征列表,依据匀速运动模型,估计此时刻车辆的姿态,从而将本帧的车位坐标从车身坐标系转换至世界坐标系,然后通过x轴坐标和y轴坐标的两次查询,快速查找出地图的车位特征列表中与当前待搜索车位特征最邻近的特征,继而进行匹配追踪;
如果追踪成功,则计算当前车辆位姿并结合轮速计信息利用滤波器进行优化,不断更新车辆的位姿状态;
如果连续追踪失败时,则进行重定位,所述重定位对地图内的全部车位特征进行逐一匹配,获得匹配队列后,计算当前车辆位姿,计算车辆相对于地图的位姿坐标。
38.一种基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,包括实时建图装置和自定位装置,其中,
所述实时建图装置包括:
图像采集装置,采集相机图像,投影生成顶视图;
车位检测装置,在所述顶视图上进行车位检测;
初始化与追踪匹配装置,初始化车位地图和追踪匹配,估计车位的当前姿态,筛选关键帧;
优化装置,对局部地图进行优化;
回环检测装置,进行回环检测与地图固化;
所述自定位装置包括:
初始化装置,进行初始化定位,在自定位***刚启动时,确定车辆自身位于地图中的坐标;
定位装置,进行追踪与重定位操作,确定车辆位姿;
所述相机为鱼眼相机;
对所述鱼眼相机进行标定;
分别对四个所述鱼眼相机采样生成顶视图,根据四个鱼眼相机标定后的内参以及相对于汽车后轴中心的外参,计算得到新的参数矩阵以生成各鱼眼相机的原始图像中地面像素对应于顶视图像素的映射关系,得到映射表,并通过采样生成顶视图;
在采样生成的顶视图上,利用角点提取算法检测得到车身周边的车位出口线的车位角点集合P={(ui,vi)|i=1,2,3...n},其中(ui,vi)代表第i个角点坐标,n代表角点数量;利用目标检测算法得到每个车位编号的位置框Rect={(ui,vi,wi,hi)|i=1,2,3...m},其中,(ui,vi)代表检测框的中心位置,(wi,hi)代表检测框的宽和高,m代表单帧图像中检测到的车位数量;
首先,分成三个部分进行:
第一部分,利用车位检测算法获取车位是否为有效车位以及车位类别Classm,m∈[0,6];
第二部分,利用车位的出口角点与车位编号的位置关系,生成相对坐标向量;
第三部分,利用特征工程的算法提取车位编号特征向量,生成唯一车位编号特征向量Nm;
然后,将上述三个部分的特征结合,生成增强的车位特征;最后,利用车位线的平行关系,优化帧内的角点坐标。
39.根据权利要求38所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,所述鱼眼相机有四个,分辨率在720P或以上,分别布置于汽车前后保险杠以及左右后视镜下边沿。
40.根据权利要求38所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,在顶视图上进行车位的检测后,并将检测到的车位的信息进行增强,生成增强的车位特征,并依据停车场规范进行帧内的坐标优化。
41.根据权利要求38所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,结合轮速计的里程计信息进行融合,来估计车位的当前姿态,筛选关键帧。
42.根据权利要求38所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,包括:
所述内参包括焦距和畸变参数;
所述外参包括相对于车身的旋转平移矩阵;
所述顶视图为10米*10米。
43.根据权利要求42所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,包括:
通过所述内参和外参计算图像中任意一地面像素点相对于车辆后轴中心的相对坐标,完成像素坐标系到车身坐标系的转换。
44.根据权利要求38所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,包括:
在顶视图的视角,对于车身周围10米*10米的范围采样实现栅格化,确定每个栅格内对应于原始图像像素的坐标信息。
45.根据权利要求38所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,包括:
所述映射表不是一对一的关系,而是一对多或多对一的关系;
对于一对多关系的像素值取均值,对于多对一关系的像素值取插值后的结果值。
46.根据权利要求38所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
引入车位编号特征向量、车位类别和车位出口三角关系信息,进一步生成增强的车位特征,用于后续的特征匹配与追踪工作。
47.根据权利要求38所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
所述角点提取算法为Harris角点检测+SVM分类算法。
48.根据权利要求38所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
所述角点提取算法为深度学习网络算法。
49.根据权利要求38所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
所述目标检测算法为深度学习网络算法。
50.根据权利要求38所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
所述目标检测算法为模板匹配算法。
51.根据权利要求38所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
所述车位类别包括非法车位、左水平车位、右水平车位、左垂直车位、右垂直车位、左斜车位和右斜车位共七个类别。
52.根据权利要求38所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
采用OCR技术提取图像矩阵中的编号标志。
53.根据权利要求38所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
采用ORB或SIFT特征描述子生成唯一车位编号特征向量。
54.根据权利要求47-53中任一项所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
所述相对坐标向量是指,经过任一车位的两出口角点归一化坐标,与车位编号检测框的中点坐标,且依据标定和投影过程中计算得到的投影矩阵,以及计算得到的对应的车身坐标,所确定的坐标向量;
其中,
所述任一车位的两出口角点归一化坐标如公式(1)所示:
P2m=(u2m,v2m,1)T,P2m+1=(u2m+1,v2m+1,1)T (1)
其中P2m和P2m+1分别代表第m个车位出口线的左右角点坐标,以齐次坐标的形式表示,u2m、v2m、u2m+1、v2m+1均为以顶视图左上角为原点的像素偏移;
所述车位编号检测框的中点坐标如公式(2)所示:
Rectm=(um,vm,1)T (2)
其中Rectm代表第m个车位的中点坐标,um、vm均为以顶视图左上角为原点的像素偏移;
所述计算得到对应的车身坐标如公式(3)所示:
计算公式如公式(4)所示:
56.根据权利要求47-53中任一项所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
所述优化帧内的角点坐标是指依据停车场中类型一致、位置相近且入库角度的差值小于一定程度的的车位应当符合入库线平行这一规范,对图中的停车场队列进行优化。
57.根据权利要求56所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
所述优化的方法为:在计算得到增强的车位特征后,遍历车位特征列表,依据车位类别划分聚类中心,并以特征中对应角点坐标距离和入库角度的差值作为距离度量,进行聚类算法,计算出k个聚类中心;对每个聚类簇中的车位特征,取其中的车位出库角点坐标,进行直线拟合,并计算投影误差;当投影误差总和大于第一阈值时,将重新进行角点检测工作;否则,将每个角点坐标投影到该直线上,生成新的角点坐标。
59.根据权利要求58所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
车辆的t时刻的速度vt和车辆坐标ct,两帧间时间间隔ΔT,记t+1时刻的车辆坐标为ct+1=ct+vt·ΔT,并依次计算局部地图中的车位特征角点坐标与车辆坐标ct+1的距离,超出顶视图视景范围的车位特征予以筛除;然后将当前帧中检测得到的车位特征列表与局部地图中映射而来的车位特征列表进行匹配追踪。
60.根据权利要求59所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
所述顶视图视景范围为以车为中心,5米为半径的区域。
61.根据权利要求59所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
根据所述匹配追踪的状态进行如下三种操作:
1)全部追踪成功:证明车辆处于静止状态或位移过小,利用卡尔曼滤波将轮速计推理的位姿与观测估计的位姿进行融合优化;
2)部分追踪成功:证明车辆发生了一定的位移,用卡尔曼滤波将轮速计推理的位姿与观测估计的位姿进行融合优化,之后更新本帧内全部的车位坐标,加入关键帧候选队列;
3)全部追踪失败:仅利用轮速计信息推理位姿,加入关键帧候选队列。
63.根据权利要求62所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
所述置信度指代车位编号提取过程中的车位编号为真值的概率。
64.根据权利要求38所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
所述实时建图过程中,局部地图由关键帧组成,关键帧的筛选标准为:地面平坦条件下,当前帧全部追踪失败且车位检测数量不为0,或者为本帧中部分车位追踪成功且追踪失败的车位占当前帧车位检测数量的比例大于第三阈值,车辆姿态变化大于第四阈值。
65.根据权利要求64所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
所述地面平坦条件是指,利用IMU或者视觉相关算法检测车身俯仰角变化,变化幅度小于第五阈值,则认为此时地面平坦。
66.根据权利要求38所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
对局部地图进行优化操作,剔除冗余关键帧,并对相同的车位特征进行融合。
67.根据权利要求66所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
所述对局部地图进行优化操作包括:
采用车辆的平移和旋转描述车辆的运动,i时刻的车辆位姿描述为公式(6)所示的形式:
ci=[ti,θi]T=[xi,yi,θi]T (6)
则i时刻到j时刻的位姿变化描述为公式(7)所示的形式:
其中Ri为2*2的旋转矩阵,ti和θi,分别代表i时刻的世界坐标和航向角,即ti=[xi,yi]T,h(ci,cj)为运动方程;设定从j时刻观察到的车位点的坐标与从i时刻观察到的车位点坐标的差值为zij,则误差方程如公式(8)和(9)所示:
eij=zij-h(ci,cj) (8)
其中eij代表对于同一车位在不同时刻由位姿估计和车位测量两方面影响带来的误差项,Λij代表信息矩阵,F(c)为优化方程,优化目的即为最小化F(c)。
68.根据权利要求66所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
采用Gauss-Newton算法或Levenberg-Marquardt算法进行优化求解。
69.根据权利要求66-68中任一项所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
当新加入的关键帧与地图早期的关键帧有重合车位时,即为检测到了回环,并将地图中的全部车位加入优化队列,进行整体优化。
70.根据权利要求38所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
当所述实时建图的工作完成后,将全部关键帧删除,只保留车位特征向量构建地图,在删除关键帧的同时,建立车位特征的词典,用于后续定位过程中的快速查找。
71.根据权利要求38、40、46中任一项所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
所述增强的车位特征的词典建立方法包括以下步骤:
首先,依据已建成的图,将图的坐标轴旋转至与车位线平行;
其次,按照x轴的值,进行第一层聚类,然后按照第二层的值,进行第二次聚类,
最后,生成三层的树的结构以供快速查询。
72.根据权利要求38所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
所述初始化定位完成车辆位姿的初始化,以确定车辆自身处于车位地图中的位置。
73.根据权利要求72所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
所述初始化定位包括如下步骤:
首先,保持车辆处于静止状态,将k帧作为初始化帧序列,并逐帧进行顶视图投影和车位特征提取算法,设定初始序列帧中,实际共有m个车位,则应共提取出n≈k*m个车位特征;其中k表示初始化中需要的最少帧数,为预设的阈值;
其次,利用特征匹配方法对任意两帧的车位特征进行两两匹配,成功匹配一次则两帧内对应车位特征的置信权重增加1/k一1,而后在帧内优化时,加入每个角点的权重进行直线拟合,将每个角点投影到直线上后再进行聚类,取聚类中心为最终的车位特征角点;
然后,遍历地图中的车位列表,与候选车位列表的车位特征列表进行追踪匹配,此时由于车身坐标未知,因此,仅依靠车位特征中的车位编号特征向量进行匹配;
最后,匹配完成后,计算当前车辆相对于地图的坐标关系,并优化位姿,最小化投影误差,最终计算出当前车辆位姿坐标。
74.根据权利要求38所述的基于顶视图的停车场车辆自定位及地图构建装置,其特征在于,
所述追踪与重定位包括,任意时刻相机采集的原始图像都被采样生成顶视图,并提取出待匹配车位特征列表,依据匀速运动模型,估计此时刻车辆的姿态,从而将本帧的车位坐标从车身坐标系转换至世界坐标系,然后通过x轴坐标和y轴坐标的两次查询,快速查找出地图的车位特征列表中与当前待搜索车位特征最邻近的特征,继而进行匹配追踪;
如果追踪成功,则计算当前车辆位姿并结合轮速计信息利用滤波器进行优化,不断更新车辆的位姿状态;
如果连续追踪失败时,则进行重定位,所述重定位对地图内的全部车位特征进行逐一匹配,获得匹配队列后,计算当前车辆位姿,计算车辆相对于地图的位姿坐标。
75.一种基于顶视图的停车场车辆自定位及地图构建***,其特征在于,包括:
存储器,所述存储器存储执行如权利要求1所述方法的程序;
处理器;所述处理器执行所述程序。
76.一种自动驾驶车辆,其特征在于,包括:
所述自动驾驶车辆上搭载如权利要求38所述的基于顶视图的停车场车辆自定位及地图构建装置或权利要求75所述的***。
77.一种自主泊车***,其特征在于,包括:
如权利要求38所述的基于顶视图的停车场车辆自定位及地图构建装置或如权利要求75所述的***。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010587090.4A CN111862672B (zh) | 2020-06-24 | 2020-06-24 | 基于顶视图的停车场车辆自定位及地图构建方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010587090.4A CN111862672B (zh) | 2020-06-24 | 2020-06-24 | 基于顶视图的停车场车辆自定位及地图构建方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111862672A CN111862672A (zh) | 2020-10-30 |
CN111862672B true CN111862672B (zh) | 2021-11-23 |
Family
ID=72989776
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010587090.4A Active CN111862672B (zh) | 2020-06-24 | 2020-06-24 | 基于顶视图的停车场车辆自定位及地图构建方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111862672B (zh) |
Families Citing this family (22)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20220019810A1 (en) * | 2020-07-14 | 2022-01-20 | The Chamberlain Group, Inc. | Object Monitoring System and Methods |
CN112802346B (zh) * | 2020-12-28 | 2022-04-15 | 苏州易航远智智能科技有限公司 | 基于云端共享和地图融合的自主泊车***和方法 |
CN113096187B (zh) * | 2021-05-03 | 2022-05-17 | 湖北汽车工业学院 | 一种车辆与障碍物相对位置自动获取方法 |
CN113256681B (zh) * | 2021-05-26 | 2022-05-13 | 北京易航远智科技有限公司 | 基于时空注意力机制的行人轨迹预测方法 |
CN113483755B (zh) * | 2021-07-09 | 2023-11-07 | 北京易航远智科技有限公司 | 一种基于非全局一致地图的多传感器组合定位方法及*** |
CN113619569A (zh) * | 2021-08-02 | 2021-11-09 | 蔚来汽车科技(安徽)有限公司 | 用于固定泊车场景的车辆定位方法及其*** |
CN113483769B (zh) * | 2021-08-17 | 2024-03-29 | 清华大学 | 基于粒子滤波器的车辆自定位方法、***、设备和介质 |
CN113705474B (zh) * | 2021-08-30 | 2022-04-15 | 北京易航远智科技有限公司 | 车位检测方法和装置 |
CN113781645B (zh) * | 2021-08-31 | 2024-03-26 | 同济大学 | 一种面向室内泊车环境的定位和建图方法 |
CN113762272B (zh) * | 2021-09-10 | 2024-06-14 | 北京精英路通科技有限公司 | 道路信息的确定方法、装置和电子设备 |
CN113865580B (zh) * | 2021-09-15 | 2024-03-22 | 北京易航远智科技有限公司 | 构建地图的方法、装置、电子设备及计算机可读存储介质 |
CN113807288A (zh) * | 2021-09-24 | 2021-12-17 | 上海欧菲智能车联科技有限公司 | 库位检测方法、车辆定位方法、装置、设备和介质 |
CN114088083B (zh) * | 2021-11-09 | 2023-10-31 | 北京易航远智科技有限公司 | 一种基于顶视图语义对象的建图方法 |
CN114255584B (zh) * | 2021-12-20 | 2023-04-07 | 济南博观智能科技有限公司 | 一种停放车辆的定位方法、***、存储介质和电子设备 |
CN114758318A (zh) * | 2022-02-21 | 2022-07-15 | 北京航空航天大学 | 一种基于全景视图的任意角度车位检测方法 |
CN114863096B (zh) * | 2022-04-02 | 2024-04-16 | 合众新能源汽车股份有限公司 | 室内停车场的语义地图构建及定位方法和装置 |
CN114862945A (zh) * | 2022-05-20 | 2022-08-05 | 远峰科技股份有限公司 | 一种构建车位地图方法与装置 |
CN114822216B (zh) * | 2022-06-23 | 2022-09-16 | 小米汽车科技有限公司 | 生成车位地图的方法、装置、车辆、存储介质及芯片 |
CN115388880B (zh) * | 2022-10-27 | 2023-02-03 | 联友智连科技有限公司 | 一种低成本记忆泊车建图与定位方法、装置及电子设备 |
CN115690261B (zh) * | 2022-12-29 | 2023-04-14 | 安徽蔚来智驾科技有限公司 | 基于多传感器融合的车位建图方法、车辆及存储介质 |
CN115984417B (zh) * | 2023-03-20 | 2023-10-24 | 知行汽车科技(苏州)股份有限公司 | 一种语义建图方法、装置及存储介质 |
CN116358533B (zh) * | 2023-05-31 | 2023-08-29 | 小米汽车科技有限公司 | 回环检测数据的处理方法、装置、存储介质以及车辆 |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
DE4320918C2 (de) * | 1993-06-24 | 1997-02-06 | Intertraffic Ges Fuer Integrie | Verkehrsleitsystem |
CN105469405A (zh) * | 2015-11-26 | 2016-04-06 | 清华大学 | 基于视觉测程的同时定位与地图构建方法 |
CN108717710A (zh) * | 2018-05-18 | 2018-10-30 | 京东方科技集团股份有限公司 | 室内环境下的定位方法、装置及*** |
CN109685000A (zh) * | 2018-12-21 | 2019-04-26 | 广州小鹏汽车科技有限公司 | 一种基于视觉的车位检测方法及装置 |
CN109859260A (zh) * | 2017-11-30 | 2019-06-07 | 华为技术有限公司 | 确定车位位置的方法、装置及计算机可读存储介质 |
CN110136058A (zh) * | 2018-10-25 | 2019-08-16 | 北京初速度科技有限公司 | 一种基于俯视拼接图的建图方法及车载终端 |
CN110136199A (zh) * | 2018-11-13 | 2019-08-16 | 北京初速度科技有限公司 | 一种基于摄像头的车辆定位、建图的方法和装置 |
CN110415550A (zh) * | 2019-07-31 | 2019-11-05 | 北京智行者科技有限公司 | 基于视觉的自动泊车方法 |
Family Cites Families (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR100380906B1 (ko) * | 2002-05-29 | 2003-04-26 | 주식회사 성일종합건축사사무소 | 주차차량 위치안내 시스템 및 방법 |
CN107180215B (zh) * | 2017-05-31 | 2020-01-31 | 同济大学 | 基于库位和二维码的停车场自动建图与高精度定位方法 |
CN109631896B (zh) * | 2018-07-23 | 2020-07-28 | 同济大学 | 一种基于车辆视觉和运动信息的停车场自主泊车定位方法 |
FR3087041B1 (fr) * | 2018-10-04 | 2021-01-08 | Bull Sas | Procede de localisation d'un emplacement de stationnement d'un vehicule dans un parking |
CN109345860A (zh) * | 2018-10-15 | 2019-02-15 | 平安科技(深圳)有限公司 | 室内停车场导航方法、装置、计算机设备及存储介质 |
CN109131318B (zh) * | 2018-10-19 | 2020-03-27 | 清华大学 | 一种基于拓扑地图的自主泊车路径协调方法 |
CN109798911B (zh) * | 2019-02-28 | 2020-12-25 | 北京智行者科技有限公司 | 一种用于代客泊车的全局路径规划方法 |
CN110775052B (zh) * | 2019-08-29 | 2021-01-29 | 浙江零跑科技有限公司 | 一种基于视觉与超声波感知融合的自动泊车方法 |
CN110796063B (zh) * | 2019-10-24 | 2022-09-09 | 百度在线网络技术(北京)有限公司 | 用于检测车位的方法、装置、设备、存储介质以及车辆 |
-
2020
- 2020-06-24 CN CN202010587090.4A patent/CN111862672B/zh active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
DE4320918C2 (de) * | 1993-06-24 | 1997-02-06 | Intertraffic Ges Fuer Integrie | Verkehrsleitsystem |
CN105469405A (zh) * | 2015-11-26 | 2016-04-06 | 清华大学 | 基于视觉测程的同时定位与地图构建方法 |
CN109859260A (zh) * | 2017-11-30 | 2019-06-07 | 华为技术有限公司 | 确定车位位置的方法、装置及计算机可读存储介质 |
CN108717710A (zh) * | 2018-05-18 | 2018-10-30 | 京东方科技集团股份有限公司 | 室内环境下的定位方法、装置及*** |
CN110136058A (zh) * | 2018-10-25 | 2019-08-16 | 北京初速度科技有限公司 | 一种基于俯视拼接图的建图方法及车载终端 |
CN110136199A (zh) * | 2018-11-13 | 2019-08-16 | 北京初速度科技有限公司 | 一种基于摄像头的车辆定位、建图的方法和装置 |
CN109685000A (zh) * | 2018-12-21 | 2019-04-26 | 广州小鹏汽车科技有限公司 | 一种基于视觉的车位检测方法及装置 |
CN110415550A (zh) * | 2019-07-31 | 2019-11-05 | 北京智行者科技有限公司 | 基于视觉的自动泊车方法 |
Non-Patent Citations (2)
Title |
---|
一种基于环视***的车位检测方法;王晋疆 等;《分析仪器》;20190131(第1期);全文 * |
基于全景环视***的车位检测技术的研究;王鹏飞;《基于全景环视***的车位检测技术的研究》;20200615;第2-3章 * |
Also Published As
Publication number | Publication date |
---|---|
CN111862672A (zh) | 2020-10-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111862672B (zh) | 基于顶视图的停车场车辆自定位及地图构建方法 | |
CN111862673B (zh) | 基于顶视图的停车场车辆自定位及地图构建方法 | |
CN111882612B (zh) | 一种基于三维激光检测车道线的车辆多尺度定位方法 | |
CN111986506B (zh) | 基于多视觉***的机械式停车位泊车方法 | |
CN108647646B (zh) | 基于低线束雷达的低矮障碍物的优化检测方法及装置 | |
AU2018282302B2 (en) | Integrated sensor calibration in natural scenes | |
Qu et al. | Vehicle localization using mono-camera and geo-referenced traffic signs | |
Alonso et al. | Accurate global localization using visual odometry and digital maps on urban environments | |
CN112767490B (zh) | 一种基于激光雷达的室外三维同步定位与建图方法 | |
Yoneda et al. | Urban road localization by using multiple layer map matching and line segment matching | |
Senlet et al. | Satellite image based precise robot localization on sidewalks | |
JP2023021098A (ja) | マップ構築方法、装置及び記憶媒体 | |
CN112669354B (zh) | 一种基于车辆非完整约束的多相机运动状态估计方法 | |
CN115407357A (zh) | 基于大场景的低线束激光雷达-imu-rtk定位建图算法 | |
JP2008065088A (ja) | 静止物地図生成装置 | |
CN114459467B (zh) | 一种未知救援环境中基于vi-slam的目标定位方法 | |
CN113252051A (zh) | 一种地图构建方法及装置 | |
CN115564865A (zh) | 一种众包高精地图的构建方法、***、电子设备及车辆 | |
Yabuuchi et al. | Visual localization for autonomous driving using pre-built point cloud maps | |
CN117367427A (zh) | 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法 | |
Hara et al. | Vehicle localization based on the detection of line segments from multi-camera images | |
Gálai et al. | Crossmodal point cloud registration in the Hough space for mobile laser scanning data | |
CN112258391B (zh) | 一种基于道路交通标线的碎片化地图拼接方法 | |
CN117635721A (zh) | 目标定位方法及相关***、存储介质 | |
CN114565669A (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 |