一种基于二维码的SLAM定位方法及装置制造方法及图纸

本发明专利技术公开了一种基于二维码的SLAM定位方法及装置，该方法包括：步骤1，利用车载相机采集车辆周围的图像，从图像中获取二维码和自然特征点；步骤2，解析二维码得到二维码位姿，并获取自然特征点的位置信息；步骤3，根据二维码位姿和自然特征点的位置信息确定相机位姿；该步骤3包括：构建残差函数，其中包括自然特征点投影误差和二维码定位误差，根据残差函数构建最小二乘问题，求解残差函数的最小值，根据与最小值对应的相机位姿确定相机位姿。本发明专利技术所采用的方案采用视觉SLAM融合二维码定位的形式，实现更精确稳定的定位，具有更好的精度、稳定性和鲁棒性。稳定性和鲁棒性。稳定性和鲁棒性。

【技术实现步骤摘要】
一种基于二维码的SLAM定位方法及装置


[0001]本专利技术涉及自动驾驶
，特别是关于一种基于二维码的SLAM定位方法及装置。

技术介绍

[0002]随着自动驾驶技术的发展，越来越多企业及研究机构开始关注自动驾驶技术的落地实现。其中，自主代客泊车(AVP，AutomatedValet Parking)用于实现从停车场入口/出口到停车位之间这一特定区域内完全自主的车辆自动驾驶，因其行驶速度低以及特定运营场地能够最大程度保证安全的特性，被业内公认为是最先实现落地的L4级自动驾驶场景。定位作为自主代客泊车重要的一环，为车辆的控制、决策提供了先决条件。目前，停车场环境下的定位技术主要采用基于摄像头、激光雷达等传感器的SLAM(simultaneous localization andmapping)即同步定位与地图构建技术，可以解决车辆在停车场环境运动时的定位与地图构建问题。
[0003]目前SLAM根据传感器配置的不同主要分为两类：1)基于激光雷达的激光SLAM(Lidar SLAM)；2)基于视觉摄像头的VSLAM(Visual SLAM，视觉SLAM)。
[0004]但是，采用激光SLAM的方式进行自主代客泊车的定位建图，对激光雷达在车辆上的安装部署有要求，且激光雷达价格昂贵，激光SLAM无法利用停车场中丰富的停车位语义纹理信息，无法区分各车位相对位置。视觉SLAM采用摄像头提取的图像信息，受环境光影响较大，在光线较弱，纹理较差区域无法工作，而室内停车场环境可能灯光较暗，且大部分为白墙等无纹理区域，因此基于特征点的纯视觉SLAM效果不佳。而且，视觉SLAM由于标定误差，传感器误差，噪声等的影响，随着运行时间和距离的增大，累积误差也会逐渐增大，影响定位精度。
[0005]因此，亟待提供一种新的SLAM定位方案。

技术实现思路

[0006]本专利技术的目的在于提供一种基于二维码的SLAM定位方法及装置，来克服或至少减轻现有技术的上述缺陷中的至少一个。
[0007]为实现上述目的，本专利技术提供一种基于二维码的SLAM定位方法，应用于自动泊车车辆，包括：
[0008]步骤1，利用车载相机采集车辆周围的图像，从图像中获取二维码和自然特征点；
[0009]步骤2，解析二维码得到二维码位姿，并获取自然特征点的位置信息；
[0010]步骤3，根据二维码位姿和自然特征点的位置信息确定相机位姿；该步骤包括：
[0011]构建残差函数，其中包括自然特征点投影误差和二维码定位误差，用下式表示残差函数：
[0012][0013]其中，f(x)表示残差函数，i表示当前第i帧图像，k表示共有k个关键帧图像，j表示当前第j个地图点，n表示共有n个地图点；s表示第s个二维码，t表示当前共检测到t个二维码；
[0014]e
P,i,j
表示自然特征点投影误差，表达式为其中P
j
表示世界坐标系下的第j个地图点，P
j
＝[X
j
,Y
j
,Z
j
]T
,X
j
,Y
j
,Z
j
为世界坐标系下的坐标，T
i
为第i帧图像对应相机在世界坐标系下的位姿，l
ij
＝[u
ij
,v
ij
]T
表示第i帧图像时点P
j
在图像中的实际像素坐标，为第i帧图像时点P
j
在图像中的投影像素坐标，s
i
表示第i帧图像对应的相机的尺度深度，K表示相机的内参矩阵；
[0015]表示信息矩阵，为预设的视觉观测的协方差矩阵的逆；
[0016]e
M,i,s
表示二维码定位误差，表达式为e
M,I,s
＝T
WMis
T
MCis
‑
T
i
，其中，T
WMis
表示第i帧图像时第s个二维码在世界坐标系下的位姿，T
MCis
表示第i帧图像时基于第s个二维码定位恢复的相机位姿，第i帧图像对应相机在世界坐标系下的位姿为T
i
；
[0017]表示信息矩阵，为预设的二维码与相机位姿变量的协方差矩阵的逆；
[0018]根据残差函数构建最小二乘问题，求解残差函数的最小值，将与最小值对应的相机位姿T
i
确定为第i帧图像时在世界坐标系下的相机位姿。
[0019]优选的，步骤2包括：
[0020]获取二维码的标识符和四个角点的坐标。
[0021]优选的，该方法还包括：当检测到之前出现过的二维码时，判断出现回环，执行回环检测；
[0022]回环检测包括：
[0023]构建基于二维码定位约束、自然特征点投影约束和回环检测约束的第二残差函数，以下式表示：
[0024][0025]其中，x
i
表示第i个地图点对应的当前帧下的像素坐标；
[0026]s
i
表示第i个地图点的深度；
[0027]P
i
表示回环帧下第i个地图点的世界坐标；其中，回环帧为二维码上次出现时对应的帧；
[0028]对从回环帧到当前帧的每一帧图像，对第二残差函数求解最小值，分别得到与最小值对应的相机位姿，利用得到的相机位姿更新从回环帧到当前帧的每一帧的相机位姿。
[0029]本专利技术实施例还提供一种基于二维码的SLAM定位装置，应用于自动泊车车辆，包括：
[0030]采集模块，用于利用车载相机采集车辆周围的图像，从图像中获取二维码和自然特征点；
[0031]处理模块，用于解析二维码得到二维码位姿，并获取自然特征点的位置信息；根据二维码位姿和自然特征点的位置信息确定相机位姿；
[0032]其中，处理模块用于通过下述方式根据二维码位姿和自然特征点的位置信息确定相机位姿：
[0033]构建残差函数，其中包括自然特征点投影误差和二维码定位误差，用下式表示残差函数：
[0034][0035]其中，f(x)表示残差函数，i表示当前第i帧图像，k表示共有k个关键帧图像，j表示当前第j个地图点，n表示共有n个地图点；s表示第s个二维码，t表示当前共检测到t个二维码；
[0036]e
P,i,j
表示自然特征点投影误差，表达式为其中P
j
表示世界坐标系下的第j个地图点，P
j
＝[X
j
,Y
j
,Z
j
]T
,X
j
,Y
j
,Z
j
为世界坐标系下的坐标，T
i
为第i帧图像对应相机在世界坐标系下的位姿，l
ij
＝[u
ij
,v
...

【技术保护点】

【技术特征摘要】
1.一种基于二维码的SLAM定位方法，应用于自动泊车车辆，其特征在于，包括：步骤1，利用车载相机采集车辆周围的图像，从图像中获取二维码和自然特征点；步骤2，解析二维码得到二维码位姿，并获取自然特征点的位置信息；步骤3，根据二维码位姿和自然特征点的位置信息确定相机位姿；该步骤包括：构建残差函数，其中包括自然特征点投影误差和二维码定位误差，用下式表示残差函数：其中，f(x)表示残差函数，i表示当前第i帧图像，k表示共有k个关键帧图像，j表示当前第j个地图点，n表示共有n个地图点；s表示第s个二维码，t表示当前共检测到t个二维码；e
P,i,j
表示自然特征点投影误差，表达式为其中P
j
表示世界坐标系下的第j个地图点，P
j
＝[X
j
,Y
j,
Z
j
]
T
,X
j
,Y
j,
Z
j
为世界坐标系下的坐标，T
i
为第i帧图像对应相机在世界坐标系下的位姿，l
ij
＝[u
ij
,v
ij
]
T
表示第i帧图像时点P
j
在图像中的实际像素坐标，为第i帧图像时点P
j
在图像中的投影像素坐标，s
i
表示第i帧图像对应的相机的尺度深度，K表示相机的内参矩阵；表示信息矩阵，为预设的视觉观测的协方差矩阵的逆；e
M,i,s
表示二维码定位误差，表达式为e
M,I,s
＝T
WMis
T
MCis
‑
T
i
，其中，T
WMis
表示第i帧图像时第s个二维码在世界坐标系下的位姿，T
MCis
表示第i帧图像时基于第s个二维码定位恢复的相机位姿，第i帧图像对应相机在世界坐标系下的位姿为T
i
；表示信息矩阵，为预设的二维码与相机位姿变量的协方差矩阵的逆；根据残差函数构建最小二乘问题，求解残差函数的最小值，将与最小值对应的相机位姿T
i
确定为第i帧图像时在世界坐标系下的相机位姿。2.根据权利要求1所述的基于二维码的SLAM定位方法，其特征在于，步骤2包括：获取二维码的标识符和四个角点的坐标。3.根据权利要求1或2所述的基于二维码的SLAM定位方法，其特征在于，还包括：当检测到之前出现过的二维码时，判断出现回环，执行回环检测；回环检测包括：构建基于二维码定位约束、自然特征点投影约束和回环检测约束的第二残差函数，以下式表示：其中，x
i
表示第i个地图点对应的当前帧下的像素坐标；s
i
表示第i个地图点的深度；
P
i
表示回环帧下第i个地图点的世界坐标；其中，回环帧为二维码上次出现时对应的帧；对从回环帧到当前帧的每一帧图像，对第二残差函数求解最小值，分别得到与最小值对应的相机位姿，利用得到的相机位姿更新从回环帧到当前帧的每一帧的相机位姿。4.一种基于二维码的SLAM定位装置，应用于自动...

【专利技术属性】
技术研发人员：秦兆博，李琦，谢国涛，王晓伟，秦洪懋，秦晓辉，徐彪，丁荣军，
申请(专利权)人：湖南大学无锡智能控制研究院，
类型：发明
国别省市：