基于库位和二维码的停车场自动建图与高精度定位方法技术

本发明专利技术提出了一种基于库位和二维码识别的即时定位与建图(SLAM)方法，基于低成本的前视单目相机以及由鱼眼相机组成的环视系统，对环境中的库位、库位编号以及预先布设的二维码进行鲁棒识别；利用低成本的惯性导航设备对车辆航向以及速度进行观测；利用SLAM算法实现对环境中的库位以及二维码标记的位置的优化估计，实现车库平面分布图自动构建，并利用建图结果实现无人车的实时高精度分米级定位。

Parking area automatic construction drawing and high precision positioning method based on storehouse and two-dimensional code

The invention provides a real-time positioning and mapping based on location and two-dimensional code recognition (SLAM) method, low cost and forward-looking monocular camera consists of fisheye camera surveying system based on the environment of the library, a library number one and two-dimensional deployed code for robust recognition; observation the vehicle heading and speed using inertial navigation equipment and low cost; estimation of environment in the library and the two-dimensional code marked position optimization using the SLAM algorithm, realize the automatic garage construction plane distribution map, and use the mapping results achieved without vehicle real-time high-precision decimeter level positioning.

【技术实现步骤摘要】
基于库位和二维码的停车场自动建图与高精度定位方法
本专利技术是一种应用于停车场以及类似无GNSS环境下的自动建图与高精度定位方法，适用无人车在停车场环境中的自主无人驾驶。
技术介绍
在室外，全球卫星导航系统(GNSS)技术已经可以提供准确可靠的定位导航服务，但在室内空间，由于建筑墙体遮挡，无法接受到GNSS信号。当前实现室内停车场定位的方法主要依靠无线设备例如WiFi或UWB等，其一方面需要布设大量基站设备，成本高昂；另一方面定位精度(米级)难以满足无人驾驶需求。另外，采用激光雷达的机器人室内SLAM受到激光雷达成本的制约，难以在量产车型中装备；采用纯视觉系统的机器人SLAM方法受到停车场环境中缺乏丰富纹理的限制，无法保证精度和可用性。
技术实现思路
本专利技术提出了一种基于库位和二维码识别的即时定位与建图(SLAM)方法，基于低成本的前视单目相机以及由鱼眼相机组成的环视系统，对环境中的库位、库位编号以及预先布设的二维码进行鲁棒识别；利用低成本的惯性导航设备对车辆航向以及速度进行观测；利用SLAM算法实现对环境中的库位以及二维码标记的位置的优化估计，实现车库平面分布图自动构建，并利用建图结果实现无人车的实时高精度分米级定位。本专利技术所采用的技术方案是：1、通过低成本前视单目相机以及鱼眼环视相机获得车辆前方以及车辆四周地面的图像信息；2、从前视图像中识别并提取二维码标志的类型和二维码的四个角点，利用二维码的平面假设计算车辆与二维码之间的相对位置关系；3、从环视图像中提取库位的边界线，计算库位边界在车辆坐标系中的坐标，同时对库位的编号进行识别；4、利用二维码标志的编号、库位位置和编号，对重复观测的库位以及二维码进行闭环优化，消除累积噪声；5、实时通过扩展卡尔曼滤波器优化融合基于库位和二维码分别求解出的车辆位置信息，以及惯性导航观测得到的车辆位移信息，实现优化建图；6、利用图优化方法对建图结果进行多次优化，提高建图的精度；7、将建图结果进行保存，生成停车场库位平面地图；在线定位时通过已生成的建图结果，通过对库位边界以及二维码的实时识别优化求解当前车辆的位置，实现分米级的高精度定位。本专利技术的有益效果是：1、在停车场等无GNSS信号以及在视觉特征稀疏的环境中提供高精度的实时定位结果；2、使用低成本传感器，和少数纸质二维码标记，实用性强；3、自动构建的定位地图能够同时支持无人车自主无人驾驶的定位需求，以及人类在停车场内的导航需求。附图说明图1各相机传感器获得的影像图2二维码布置效果图图3单目相机影像中视觉标记与三维空间坐标系中视觉标记对应关系图4俯视图中检测得的库位图5本方法中使用的图模型及基于视觉标记的导航示例具体实施方式基于本专利技术技术方案，以下结合附图对本专利技术技术方案做更为详细的介绍。各个步骤依次为：1、通过低成本前视单目相机以及鱼眼环视相机获得车辆前方以及车辆四周地面的图像信息视觉传感器布置与标定：1.1布置本专利技术采用1个前视相机和4个鱼眼环视相机，前视相机分辨率为1080p或以上，水平视场角75-90度之间，帧率为25Hz，延迟小于5ms；鱼眼环视相机分辨率为720p或以上，水平视场角180度，帧率为25Hz，延迟小于5ms。其中，前视相机布置于汽车前挡风玻璃与后视镜之间；鱼眼环视相机布置于车辆前后保险杠，以及左右后视镜。1.2标定通过棋盘格内参标定法对前视相机和鱼眼相机的内参数进行标定，通过地面标志点对鱼眼相机的外参数进行标定，获得拼接后的俯视图如图1所示。2、从前视图像中识别并提取二维码标志的类型和二维码的四个角点，利用二维码的平面假设计算车辆与二维码之间的相对位置关系二维码布置及检测方法说明2.1二维码布置二维码生成采用Apriltag方案(https://april.eecs.umich.edu/wiki/AprilTags)，将二维码打印于A2以上尺寸的雾面防水纸，然后将其张贴在停车场的立柱与墙面之上。张贴的密度约为沿停车场道路每1张/10m，在转弯处增加张贴密度至1张/5m。张贴高度约为1.7m-2.5m，保证车辆前视相机能够观测到二维码，并避免其他车辆的遮挡。二维码布置效果图如图2所示。2.2检测并计算车辆与二维码之间的相对位置关系利用前视单目相机对二维码进行检测，其中首先使用AprilTagSDK检测二维码的角点坐标。由于图像中四个角点在世界坐标系中位于同一平面上，因此对于每一个角点，可列出方程：·K-1·x+t＝X，其中，K为相机内参数，x为角点在像空间坐标系中的坐标，R、t为相机在这一时刻的外参数，分别代表旋转和平移，X为角点在三维空间中的坐标。如图3所示，其中s为人为二维码标记边长的一半。由于四个角点之间的位置关系已知，根据其位置关系，可以建立一个以二维码标记中心为原点，x轴沿标记边缘方向右，y轴沿标记边缘方向向上，z轴垂直于标记平面向内的三维空间直角坐标系。在此坐标系中，X已知，此外，x已经检测确定，K通过相机标定得出，R、t自由度为6Dof，则仅需三个或以上点，即可迭代求解出R、t。对R进行罗德里格变换，即得单目相机到二维码标记中心连线与车辆航向的夹角a，对t取二范数即为单目相机到二维码标记中心的距离d。则构建视觉标记在车身坐标系，坐标为：x＝sin(a)·dy＝cos(a)·d。3、从环视图像中提取库位的边界线，计算库位边界在车辆坐标系中的坐标，同时对库位的编号进行识别库位布置及检测方法说明3.1库位布置库位需为停车场标准库位，其包括平直的库位线以及库位编号(英文字母与阿拉伯数字)。3.2检测并计算在获得环视相机俯视图后，利用机器视觉方法，检测俯视图中的停车库位线与库位编号。由于俯视图标定在步骤2构建的车身坐标系(x轴方向垂直航向向右，y轴方向沿航向指向车辆行驶方向)下，因此库位近端两个角点坐标可直接读出，远端两个角点利用库位规则(所述库位规则，为每个车库固有的规则，非本专利技术技术方案需要披露内容)估算得到。图4俯视图中检测得的库位。4、利用二维码标志的编号、库位位置和编号，对重复观测的库位以及二维码进行离线闭环优化，消除累积噪声该离线优化建图过程说明：由于基于惯性导航进行航位推算存在累积误差，因此需要通过不断重复观测二维码标记或库位位置来修正累积误差。在离线建图过程中，由于无需考虑效率等问题，使用图优化方法建立地图。所述图优化方法，是以图的方式来建立优化模型，图由顶点和边组成。如图5(左)所示，本方法中，所述顶点是指每个时刻车辆所在位置、各视觉标记(二维码与库位)所在位置，所述边是指两时刻间基于惯性导航的推算结果，或每个时刻车辆与视觉标记的相对坐标差。每观测到一个新的视觉标记或车辆发生了位移，则在图模型中增加一个新的结点和一条边，在添加所有数据后，根据图模型中边和结点的关系，列出边误差方程，使用高斯牛顿法迭代进行优化。进一步公开，所述图模型可以用下列误差方程描述：其中，k表示图模型中任意一个结点(视觉标记或车辆)；C表示图模型中全体结点集合；xk表示第k个结点的位置信息矩阵，若该结点为视觉标记，则存储视觉标记的位置坐标；若该结点为车辆，则存储车辆的位置坐标及航向；zk表示由与第k个结点相关的观测获得的第k个结点的位置信息矩阵；Ωk表示由与第k个结点相关的观测获得的协方差矩阵；e本文档来自技高网...

【技术保护点】
基于库位和二维码的停车场自动建图与高精度定位方法，其特征在于，包括如下过程：步骤1、通过前视单目相机以及鱼眼环视相机获得车辆前方以及车辆四周地面的图像信息，所述图像信息是单目相机采集的车库中张贴的二维码标志和环视相机采集的俯视图；包括视觉传感器布置与标定：步骤1.1视觉传感器布置采用1个前视相机和4个鱼眼环视相机；所述前视相机布置于汽车前挡风玻璃与后视镜之间；所述鱼眼环视相机布置于车辆前后保险杠，以及左右后视镜；步骤1.2标定通过棋盘格内参标定法(为现有技术)分别对前视相机和鱼眼相机的内参数进行标定，通过地面标志点对鱼眼相机的外参数进行标定，获得拼接后的俯视图。步骤2、从前视图像中识别并提取二维码标志的类型和二维码的四个角点，利用二维码的平面假设计算车辆与二维码之间的相对位置关系；步骤2.1二维码布置将二维码打印，然后将其张贴在停车场的立柱与墙面之上；步骤2.2检测并计算车辆与二维码之间的相对位置关系；步骤3、从环视图像中提取库位的边界线，计算库位边界在车辆坐标系中的坐标，同时对库位的编号进行识别；步骤3.1库位布置库位需为停车场标准库位，其包括平直的库位线以及库位编号(英文字母与阿拉伯数字)。3.2检测并计算在获得环视相机俯视图后，利用机器视觉方法，检测俯视图中的停车库位线与库位编号；由于俯视图标定在步骤2构建的车身坐标系(x轴方向垂直航向向右，y轴方向沿航向指向车辆行驶方向)下，因此库位近端两个角点坐标可直接读出，远端两个角点利用库位规则估算得到；步骤4、利用二维码标志的编号、库位位置和编号，对重复观测的库位以及二维码进行离线闭环优化，消除累积噪声；通过不断重复观测二维码标记或库位位置来修正累积误差，在离线建图过程中，使用图优化方法建立地图；步骤5、实时通过扩展卡尔曼滤波器优化融合基于库位和二维码分别求解出的车辆位置信息，以及通过惯性导航观测得到的车辆位移信息，实现优化建图；步骤6、利用图优化方法对建图结果进行多次优化，提高建图的精度：首先对结点和边进行均匀采样，进行全局图优化，再以全局图中的点为基准，进行局部图优化；步骤7、将建图结果进行保存，生成停车场库位平面地图；在线定位时通过已生成的建图结果，通过对库位边界以及二维码的实时识别优化求解当前车辆的位置，实现分米级的高精度定位。...

【技术特征摘要】
1.基于库位和二维码的停车场自动建图与高精度定位方法，其特征在于，包括如下过程：步骤1、通过前视单目相机以及鱼眼环视相机获得车辆前方以及车辆四周地面的图像信息，所述图像信息是单目相机采集的车库中张贴的二维码标志和环视相机采集的俯视图；包括视觉传感器布置与标定：步骤1.1视觉传感器布置采用1个前视相机和4个鱼眼环视相机；所述前视相机布置于汽车前挡风玻璃与后视镜之间；所述鱼眼环视相机布置于车辆前后保险杠，以及左右后视镜；步骤1.2标定通过棋盘格内参标定法(为现有技术)分别对前视相机和鱼眼相机的内参数进行标定，通过地面标志点对鱼眼相机的外参数进行标定，获得拼接后的俯视图。步骤2、从前视图像中识别并提取二维码标志的类型和二维码的四个角点，利用二维码的平面假设计算车辆与二维码之间的相对位置关系；步骤2.1二维码布置将二维码打印，然后将其张贴在停车场的立柱与墙面之上；步骤2.2检测并计算车辆与二维码之间的相对位置关系；步骤3、从环视图像中提取库位的边界线，计算库位边界在车辆坐标系中的坐标，同时对库位的编号进行识别；步骤3.1库位布置库位需为停车场标准库位，其包括平直的库位线以及库位编号(英文字母与阿拉伯数字)。3.2检测并计算在获得环视相机俯视图后，利用机器视觉方法，检测俯视图中的停车库位线与库位编号；由于俯视图标定在步骤2构建的车身坐标系(x轴方向垂直航向向右，y轴方向沿航向指向车辆行驶方向)下，因此库位近端两个角点坐标可直接读出，远端两个角点利用库位规则估算得到；步骤4、利用二维码标志的编号、库位位置和编号，对重复观测的库位以及二维码进行离线闭环优化，消除累积噪声；通过不断重复观测二维码标记或库位位置来修正累积误差，在离线建图过程中，使用图优化方法建立地图；步骤5、实时通过扩展卡尔曼滤波器优化融合基于库位和二维码分别求解出的车辆位置信息，以及通过惯性导航观测得到的车辆位移信息，实现优化建图；步骤6、利用图优化方法对建图结果进行多次优化，提高建图的精度：首先对结点和边进行均匀采样，进行全局图优化，再以全局图中的点为基准，进行局部图优化；步骤7、将建图结果进行保存，生成停车场库位平面地图；在线定位时通过已生成的建图结果，通过对库位边界以及二维码的实时识别优化求解当前车辆的位置，实现分米级的高精度定位。2.如权利要求1所述的方法，其特征在于，所述步骤2.2，具体为：先检测二维码的角点坐标；由于图像中四个角点在世界坐标系中位于同一平面上，对于每一个角点可列出方程：R·K-1·x+t＝X，其中，K为相机内参数，x为角点在像空间坐标系中的坐标，R、t为相机在这一时刻的外参数，分别代表旋转和平移，X为角点在三维空间中的坐标；由于四个角点之间的位置关系已知，根据其位置关系，可以建立一个以二维码标记中心为原点，x轴沿标记边缘方向右，y轴沿标记边缘方向向上，z轴垂直于标记平面向内的三维空间直角坐标系。在此坐标系中，X已知，此外，x已经检测确定，K通过相机标定得出，R、t自由度为6Dof，则仅需三个或以上点，即可迭代求解出R、t；对R进行罗德里格变换，即得单目相机到二维码标记中心连线与车辆航向的夹角a，对t取二范数即为单目相机到二维码标记中心的距离d。则构建视觉标记在车身坐标系，坐标为：x＝sin(a)·dy＝cos(a)·d。3.如权利要求1所述的方法，其特征在于，步骤4中，所述图优化方法，是以图的方式来建立优化模型，图由顶点和边组成，所述顶点是指每个时刻车辆所在位置、各视觉标记(二维码与库位)所...

【专利技术属性】
技术研发人员：赵君峤，张绍明，黄业韡，贺旭东，张涛，孙路，
申请(专利权)人：同济大学，
类型：发明
国别省市：上海,31