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

本发明是一种应用于停车场以及类似无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)·d

y＝cos(a)·d。

3、从环视图像中提取库位的边界线，计算库位边界在车辆坐标系中的坐标，同时对库位的编号进行识别

库位布置及检测方法说明

3.1库位布置

库位需为停车场标准库位，其包括平直的库位线以及库位编号(英文字母与阿拉伯数字)。

3.2检测并计算

在获得环视相机俯视图后，利用机器视觉方法，检测俯视图中的停车库位线与库位编号。由于俯视图标定在步骤2构建的车身坐标系(x轴方向垂直航向向右，y轴方向沿航向指向车辆行驶方向)下，因此库位近端两个角点坐标可直接读出，远端两个角点利用库位规则(所述库位规则，为每个车库固有的规则，非本发明技术方案需要披露内容)估算得到。图4俯视图中检测得的库位。

4、利用二维码标志的编号、库位位置和编号，对重复观测的库位以及二维码进行离线闭环优化，消除累积噪声

该离线优化建图过程说明：

由于基于惯性导航进行航位推算存在累积误差，因此需要通过不断重复观测二维码标记或库位位置来修正累积误差。在离线建图过程中，由于无需考虑效率等问题，使用图优化方法建立地图。

所述图优化方法，是以图的方式来建立优化模型，图由顶点和边组成。如图5(左)所示，本方法中，所述顶点是指每个时刻车辆所在位置、各视觉标记(二维码与库位)所在位置，所述边是指两时刻间基于惯性导航的推算结果，或每个时刻车辆与视觉标记的相对坐标差。每观测到一个新的视觉标记或车辆发生了位移，则在图模型中增加一个新的结点和一条边，在添加所有数据后，根据图模型中边和结点的关系，列出边误差方程，使用高斯牛顿法迭代进行优化。

进一步公开，所述图模型可以用下列误差方程描述：

其中，

k表示图模型中任意一个结点(视觉标记或车辆)；

c表示图模型中全体结点集合；

xk表示第k个结点的位置信息矩阵，若该结点为视觉标记，则存储视觉标记的位置坐标；若该结点为车辆，则存储车辆的位置坐标及航向；

zk表示由与第k个结点相关的观测获得的第k个结点的位置信息矩阵；

ωk表示由与第k个结点相关的观测获得的协方差矩阵；

ek(xk，zk)表示xk与zk的误差函数；

则f(x)是整个图模型的误差函数，x^*表示图模型的全局最优解；

为了求出全局最优解x^*，用高斯牛顿法对上式进行迭代求解：

首先，对f(x)进行一阶泰勒展开：

其中，

表示结点信息矩阵x的初始值；

δx表示x的修正值；

jk表示将初始值代入ek中计算，得到的雅可比矩阵；

于是，该问题转化为对求解线性方程：

hδx^*＝-b

解得新的全局最优解并作为初始值代入f(x)进行新一轮迭代。

5、实时通过扩展卡尔曼滤波器优化融合基于库位和二维码分别求解出的车辆位置信息，以及通过步骤4惯性导航观测得到的车辆位移信息，实现优化建图

在线定位及建图过程中的优化

在获得构建的停车场地图后，即可进行即时定位。为达到实时性的需求，在定位过程中使用扩展卡尔曼滤波同时对定位结果及导航地图中视觉标记的位置进行优化，系统的处理速度为5-10hz。

所述扩展卡尔曼滤波是适用于非线性系统的卡尔曼滤波，它基于联合概率分布和贝叶斯理论，可根据一系列具有噪声的时序数据，对未知量进行估计，在本方法中，每一时刻获得的视觉标记观测和车辆位移观测均含有误差，而车辆所处的位置即为待求的未知量，则可用扩展卡尔曼滤波公式描述这一问题：

其中，

k为当前时刻，k-1为上一时刻，

x是包含车辆位置、航向及视觉标记位置的未知量矩阵，

z中包含了车辆对视觉标记的观测，

u表示惯性导航数据，

函数f()表示了上一时刻x、u对这一时刻x值的影响，

f为函数f()的雅可比矩阵，

q和r表示观测信息和经验模型中包含的噪声。

经过上式即可增量式地得到车辆当前位置，并即时更新地图中视觉标记的位置。

6、利用图优化方法对建图结果进行多次优化，提高建图的精度

离线地图的局部细化与全局优化相结合

为了提高优化的效率，本方法采取全局图优化与局部图优化相结合的模式，首先对结点和边进行均匀采样，进行全局图优化，再以全局图中的点为基准，进行局部图优化。建图步骤的结果如图5(右)所示，其中点为二维码标记，框为库位。该地图自动生成完全无需人工干预，可用于实现在线高精度定位和行人导航。

7、将建图结果进行保存，生成停车场库位平面地图；在线定位时通过已生成的建图结果，通过对库位边界以及二维码的实时识别优化求解当前车辆的位置，实现分米级的高精度定位。

7.1离线地图的保存

在完成建图优化工作后，将地图中的二维码和库位位置坐标及属性信息(库位的编号、长度、宽度、方向等)存储为文本信息，供在线定位使用。

7.2在线定位中离线地图的使用

在线定位时，首先从文本中读取离线地图信息，进行地图初始化，再用步骤2、3中的检测方法识别车库中的二维码及库位信息、利用步骤5进行在线优化，最终实时获得车辆在离线地图中的位置，并进一步对离线地图进行精化，达到分米级定位效果。

创新点：

1.利用低成本视觉传感器与惯性导航设备结合二维码和库位信息进行高精度室内定位；

2.定位与建图的速度能够满足实际应用中的实时性需求；

3.导航地图在被使用的同时可以不断迭代优化，最终趋于稳定；

4.使用二维码和库位作为定位地图中的地标，既可用于车辆的自主定位，又符合人类对地图的认知规律。