技术领域
[0001] 本
发明属于无人驾驶汽车定位导航领域，涉及一种基于视觉的无人驾驶汽车的同时定位与地图创建方法。
背景技术
[0002] 随着无人驾驶汽车技术的发展，国内外各大
汽车制造商先后发布了其无人驾驶汽车战略计划，同时诸如百度、华为、乐视等互联网企业和NVIDIA等芯片厂商也纷纷涉足无人驾驶汽车领域。该领域目前也形成了“车企+高校科研机构”、“车企+互联网企业”的发展模式。未来，随着廉价
传感器的普及，无人驾驶汽车将会高速的发展。
[0003] 实现汽车无人驾驶的前提是其配备的
导航系统能够实时、准确地提供车辆的状态信息，但是并不是所有情况下车辆都能够从导航系统中获得这些信息，如地下
停车场等未知环境。在未知环境中，无人驾驶汽车完全没有或者只有很少、很不完善的环境信息，无人驾驶汽车只能通过自身携带的传感器获取环境信息，以获取环境地图和实现自身定位，这就是通常所说的未知环境下同时定位与地图创建(SLAM)。无人驾驶汽车在未知环境中进行同时定位与地图创建(SLAM)是无人驾驶汽车研究的热点问题。传统的SLAM主要是应用
激光雷达、声呐等距离传感器实现。但这些传感器通常
分辨率较低，很难应用于复杂环境。
[0004] 尺度不变特征转换(Scale-invariantfeature transform或SIFT)是一种电脑视觉的
算法用来侦测与描述影像中的局部性特征，它在空间尺度中寻找极值点，并提取出其
位置、尺度、旋转不变量。局部影像特征的描述与侦测可以帮助辨识物体，SIFT特征是基于物体上的一些局部外观的兴趣点而与影像的大小和旋转无关。对于光线、噪声、些微视
角改变的容忍度也相当高。基于这些特性，它们是高度显著而且相对容易撷取，在母数庞大的特征
数据库中，很容易辨识物体而且鲜有误认。使用SIFT特征描述对于部分物体遮蔽的侦测率也相当高，甚至只需要3个以上的SIFT物体特征就足以计算出位置与方位。在现今的电脑
硬件速度下和小型的特征数据库条件下，辨识速度可接近即时运算。SIFT特征的信息量大，适合在海量数据库中快速准确匹配。
[0005] 无迹卡尔曼滤波(UKF)是无损变换(UT)和标准Kalman滤波体系的结合，通过无损变换使非线性系统方程适用于线性假设下的标准Kalman滤波体系。UKF以UT变换为
基础，采用卡尔曼线性滤波
框架，对于一步预测方程，使用无迹(UT)变换来处理均值和协方差的非线性传递，就成为UKF算法。UKF是对非线性函数的概率
密度分布进行近似，用一系列确定样本来逼近状态的后验概率密度。
发明内容
[0006] 本发明的目的在针对未知环境中的无人驾驶汽车定位导航问题，提出了一种基于基于视觉的无人驾驶汽车的同时定位与地图创建方法，以期望解决上述问题。
[0007] 为了实现上述目的，本发明提供了一种基于基于视觉的无人驾驶汽车的同时定位与地图创建方法，包括以下操作步骤：
[0008] S1:进行双目摄像机参数标定；
[0009] S2:进行图像增强；
[0010] S3：对图像特征进行提取；
[0011] S4：进行左右图像特征进行匹配，得到匹配图像；
[0012] S5：将匹配图像与特征地图库进行数据关联；
[0013] 若当前匹配图像与特征地图库中的某个视觉特征匹配，则采用特征进行地图更新和无人驾驶汽车的
位姿更新；否则，作为一个新的特征加入特征地图库；
[0014] S6:在将匹配图像与特征地图库进行数据关联时，采用自适应重
采样UKF-Fast
SLAM算法得到观测模型，该观测模型的输入为匹配得到的特征图像，以得到特征地图，通过该特征地图得到当前环境地图；
[0015] 其中，对无人驾驶汽车的
里程计的参数通过采用自适应重采样UKF-FastSLAM算法得到无迹卡尔曼滤波运动模型，该无迹卡尔曼滤波运动模型的输入为里程计采集到的无人驾驶汽车位姿信息；通过无迹卡尔曼滤波的预测和更新过程，对无人驾驶汽车的位置和环境地图进行更新。
[0016] 本方法通过匹配双目摄像头采集的环境信息图像与特征地图库图像，结合里程计的无人驾驶汽车位姿信息，利用基于自适应重采样技术和无迹卡尔曼滤波的FastSLAM算法对无人驾驶汽车位姿和环境地图进行更新，从而实现无人驾驶汽车在未知环境中的定位与地图创建。
[0017] 本发明将
机器视觉应无人驾驶汽车，同时，区别于传统的卡尔曼
滤波器截断项所带来的误差，本发明采用了自适应重采样技术和无迹卡尔曼滤波，避免了由高阶项截断而产生的系统截断误差，提高了系统的
精度和车辆定位精度，建立的环境地图也更加准确。
[0018] 进一步的是，所述进行双目摄像机参数标定包括以下操作步骤：
[0019] S1.1.1：变量的初始化；借助于OpenCV的cvCreateMat()函数来为摄像机的内外参数和所有图像的最大可能数量的角点来分配存储空间；
[0020] S1.1.2：读取一副标定用的图像并且进行角点检测；借助于OpenCV的cvFindChessshoardCorners()函数进行检测并且提取角点位置；返回值为1，表示角点提取成功；否则返回0，表示角点提取失败，即此图像中提取的角点数要少于预设的数目；
[0021] S1.1.3：角点坐标细化并绘制；借助于OpenCV的cvFindCornerSubPix()函数对提取到的 角点进一步细化来得到更精确的角点
像素坐标；调用OpenCV的cvDrawChessshoardCorners()来对提取到的角点进行绘制；
[0022] S1.1.4：对于成功提取角点的图像，存储角点在世界
坐标系下的坐标值和在图像坐标系下亚像素级的坐标值；
[0023] S1.1.5：所有图像读取完后，根据提取成功的图片数来为它们的所有角点重新分配数据空间，并且释放原有的数据空间；
[0024] S1.1.6：进行标定；借助于OpenCV的cvCalibrateCamera2()函数求摄像机的内外参数；
[0025] S1.1.7：计算每幅角点提取成功图像的旋转矩阵、旋转向量和平移向量；
[0026] 进一步的是，所述进行图像增强包括以下操作步骤：
[0027] S2.1：基于OpenCV灰度级变换的图像增强；
[0028] S2.1.1：用OpenCV打开图像并在窗口中显示；
[0029] S2.1.2：设置并调整
阈值对图像进行二值化处理；
[0030] S2.1.3：设置并调整γ值对图像进行伽
马变换；
[0031] S2.1.4：设置并调整r值对图像进行对数变换；
[0032] S2.1.5：彩色图像的补色变换。
[0033] 进一步的是，所述对图像特征进行提取采用尺度不变特征变换算法，操作步骤如下：
[0034] S3.1：尺度空间极值检测：搜索整个尺度上的图像位置，通过高斯差分函数确定对尺度和方向具有不变性的兴趣点；初步确定关键点位置及所在尺度；
[0035] S3.2：关键点定位：在每一个候选的位置上，通过拟合精细的模型来确定位置和尺度；通过
稳定性进行关键点选择，通过拟和3维的二次函数以精确确定点的位置和尺度，同时去除低
对比度的关键点和不稳定的边缘响应点；
[0036] S3.3：方向确定：每个关键点根据局部图像梯度方向分配一个或多个方向，关键点位置确定后，确定其尺度和方向，算出关键点的幅度和方向；
[0037] S3.4：关键点描述符：在每个关键点附近，在
选定的尺度下测量局部图像梯度；为增强匹配稳健性，对每个关键点用多个
种子点描述，形成多维的
特征向量。
[0038] 进一步的是，左右图像特征匹配采用S3中提取得到的特征实现图像匹配；生成两幅图像的尺度不变特征变换特征后，采用特征向量的欧式距离作为两幅图像中关键点的相似性判断量。
[0039] 进一步的是，所述进行左右图像特征进行匹配包括以下步骤：
[0040] S4.1：设定比例阈值a，计算左图像中每个关键点的特征向量到右图像每个关键点的特征向量的欧式距离，得到距离矩阵L；
[0041] S4.2：将距离矩阵L的主对角线元素所在行，所在列的元素值与阈值a进行比较，若元素值小于阈值a，这匹配成功；否则匹配失败，之后进行地图匹配，即将S4得到的图像与特征地图库进行数据关联，若当前图像与特征地图库中的某个视觉特征匹配，则利用特征进行地图更新和无人驾驶汽车的位姿更新；否则，作为一个新的特征加入特征地图库。
[0042] S6：进一步的是，所述采用自适应重采样UKF-FastSLAM算法得到观测模型，以得到特征地图，通过该特征地图得到当前环境地图的操作包括以下步骤：
[0043] S6.2.1：自适应重采样：
[0044] 算法中粒子的形式为
[0045] 计算有效样本数Neff，确定粒子退化程度；
[0046]
[0047] 其中 为粒子权值；
[0048] 设置有效样本数阈值，进行自适应采样；
[0049] 设定有效样本数Nthreshoid＝0.8n(n为粒子个数)作为阈值，当Neff＜Ntfreshold时，需要进行重采样；
[0050] S6.2.2：UT变换计算k+1时刻各粒子的位姿向量预测值；
[0051] S6.2.3：获取观测并与地图中已有特征进行数据关联；
[0052] S6.2.4：采用无迹卡尔曼滤波对粒子的先验分布 进行观测更新，获取各粒子的重要性密度函数
[0053] S6.2.5：路径估计
[0054] 从重要性密度函数 中采样 计算各粒子的非正则权值进行归一化处理 通过计算有效粒子数
比较Neff与所给阈值Nthreshold＝0.8n的关系，当Neff＜Nthreshold，说明粒子退化严重，需要对粒子集 重采样，得到新的粒子集
[0055] 利用粒子滤波器估计无人驾驶汽车的运动路径；对粒子集合进行扩展，将新得到的粒子集加入其中
[0056] S6.2.6：每个粒子都有对应的地图，但并不是地图对应的所有特征都需要进行更新，对于已经与地图中某个特征关联匹配的观测量，根据数据关联的结果，利用观测信息更新地图中的相应坐标；对于没有关联上任何特征的观测量，根据数据关联的结果，作为新特征增加到地图中；
[0057] 环境特征的后验概率p(mt|st,zt,ut,nt)用无迹卡尔曼滤波进行更新。
[0058] 在本方法中的滤波算法方面，利用UT变换获取状态向量的预测值和观测预测值，运用无迹卡尔曼滤波(UKF)对无人驾驶汽车进行状态更新，根据重采样技术，自适应的对粒子进行采样，根据数据关联的结果对地图进行更新匹配。
[0059] 下面结合
附图和具体实施方式对本发明做进一步的说明。本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显。或通过本发明的实践了解到。
附图说明
[0060] 构成本发明的一部分的附图用来辅助对本发明的理解，附图中所提供的内容及其在本发明中有关的说明可用于解释本发明，但不构成对本发明的不当限定。在附图中：
[0061] 图1为本发明的基于视觉的无人驾驶汽车的同时定位与地图创建方法的
流程图；
[0062] 图2为本发明的无人驾驶汽车
运动学模型图；
[0063] 图3为本发明的无人驾驶汽车观测模型图；
[0064] 图4为本发明采用的自适应重采样UKF-FastSLAM算法流程图；
[0065] 图5为本发明采用的UKF算法流程图。
具体实施方式
[0066] 下面结合附图对本发明进行清楚、完整的说明。本领域普通技术人员在基于这些说明的情况下将能够实现本发明。在结合附图对本发明进行说明前，需要特别指出的是：
[0067] 本发明中在包括下述说明在内的各部分中所提供的技术方案和技术特征，在不冲突的情况下，这些技术方案和技术特征可以相互组合。
[0068] 此外，下述说明中涉及到的本发明的
实施例通常仅是本发明一分部的实施例，而不是全部的实施例。因此，基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
[0069] 关于本发明中术语和单位。本发明的
说明书和
权利要求书及有关的部分中的术语“包括”以及它的任何
变形，意图在于
覆盖不排他的包含。
[0070] 本发明提供了一种基于基于视觉的无人驾驶汽车的同时定位与地图创建方法，包括以下操作步骤：
[0071] S1：摄像机参数标定：摄像机参数标定采用基于OpenCV的摄像机标定技术。区别于传统的标定方法精度不高，基于OpenCV的摄像机标定技术。OpenCV求取摄像机的内外参数时，考虑到透镜的径向畸变和切向畸变，对新的归一化坐标值进行更加精确的定义，从而使得最终成像点的图像像素坐标更加准确。
[0072] S1.1：基于OpenCV的摄像机标定：
[0073] S1.1.1：变量的初始化。借助于OpenCV的cvCreateMat()函数来为摄像机的内外参数和所有图像的最大可能数量的角点来分配存储空间。
[0074] S1 .1 .2 ：读 取一副标定 用的图像并 且进行 角点 检测。借 助于OpenCVcvFindChessshoardCorners()函数进行检测并且提取角点位置。返回值为1，表示角点提取成功；否则返回0，表示角点提取失败，即此图像中提取的角点数要少于预设的数目。
[0075] S1.1.3：角点坐标细化并绘制。借助于OpenCV的cvFindCornerSubPix()函数对提取到的 角点进一步细化来得到更精确的角点像素坐标；调用OpenCV的cvDrawChessshoardCorners()来对提取到的角点进行绘制。
[0076] S1.1.4：对于成功提取角点的图像，存储角点在世界坐标系下的坐标值和在图像坐标系下亚像素级的坐标值。
[0077] S1.1.5：所有图像读取完后，根据提取成功的图片数来为它们的所有角点重新分配数据空间，并且释放原有的数据空间。
[0078] S1.1.6：进行标定。借助于OpenCV的cvCalibrateCamera2()函数求摄像机的内外参数。
[0079] S1.1.7：计算每幅角点提取成功图像的旋转矩阵、旋转向量和平移向量。
[0080] S2：图像增强：图像增强采用基于OpenCV的灰度级变换技术。灰度变换的目的是提高目标与背景的对比度，使得目标更容易被分割出来。设置调整阈值对图像进行二值化处理后，先设置并调整γ值进行伽马变换，再设置并调整r值对进行对数变换，最后再对彩色图像进行补色变换。
[0081] S2.1：基于OpenCV灰度级变换的图像增强：
[0082] S2.1.1：用OpenCV打开图像，并在窗口中显示。
[0083] S2.1.2：设置并调整阈值对图像进行二值化处理。
[0084] S2.1.3：设置并调整γ值对图像进行伽马变换。
[0085] S2.1.4：设置并调整r值对图像进行对数变换。
[0086] S2.1.5：彩色图像的补色变换。
[0087] S3：特征提取：尺度不变特征变换(SIFT)算法流程如下：
[0088] S3.1：尺度空间极值检测：搜索整个尺度上的图像位置，通过高斯差分函数确定对尺度和方向具有不变性的兴趣点。初步确定关键点位置及所在尺度。
[0089] S3.2：关键点定位：在每一个候选的位置上，通过拟合精细的模型来确定位置和尺度。通过稳定性进行关键点选择，通过拟和3维的二次函数以精确确定点的位置和尺度，同时去除低对比度的关键点和不稳定的边缘响应点，以增强匹配稳定性，提高噪声能
力。
[0090] S3.3：方向确定：每个关键点根据局部图像梯度方向分配一个或多个方向，关键点位置确定后，仍需确定其尺度和方向，算出关键点的幅度和方向。
[0091] S3.4：关键点描述符：在每个关键点附近，在选定的尺度下测量局部图像梯度。为增强匹配稳健性，用16个种子点描述对每个关键点，那么每个关键点就可以产生128个数据，最后形成128维的特征向量。
[0092] S4：左右图像特征匹配：左右图像特征匹配采用S3中提取得到的特征实现图像匹配。生成两幅图像的SIFT特征后，采用特征向量的欧式距离作为两幅图像中关键点的相似性判断量。
[0093] S4.1：设定比例阈值a，计算左图像中每个关键点的特征向量到右图像每个关键点的特征向量的欧式距离，得到距离矩阵L。
[0094] S4.2：将距离矩阵L的主对角线元素所在行，所在列的元素值与阈值a进行比较，若元素值小于阈值a，这匹配成功；否则匹配失败。
[0095] S5：地图匹配：将S4得到的图像与特征地图库进行数据关联，若当前图像与特征地图库中的某个视觉特征匹配，则利用特征进行地图更新和无人驾驶汽车的位姿更新；否则，作为一个新的特征加入特征地图库。
[0096] S6：自适应重采样UKF-FastSLAM算法。自适应重采样UKF-FastSLAM算法是这个系统的核心。采用自适应重采样技术对粒子样本进行重采样，能够有效的提高系统的鲁棒性，同时考虑到无人驾驶汽车运动模型的非线性性，采用了无迹卡尔曼滤波，避免了舍去高阶项截断而产生的系统截断误差，提高了系统的精度。无迹卡尔曼滤波运动模型的输入为里程计采集到的无人驾驶汽车位姿信息，观测模型的输入为匹配得到的特征图像，通过滤波器的预测和更新过程，对无人驾驶汽车的位置和环境地图进行更新。
[0097] S6.1：无人驾驶汽车运动模型和观测模型为标准FastSLAM算法的常用模型；
[0098] S6.2：自适应重采样UKF-FastSLAM算法(算法流程如图4所示)
[0099] S6.2.1：自适应重采样：
[0100] 算法中粒子的形式为
[0101] 计算有效样本数Neff，确定粒子退化程度；
[0102]
[0103] 其中 为粒子权值。
[0104] 设置有效样本数阈值，进行自适应采样；
[0105] 设定有效样本数Nthreshoid＝0.8n(n为粒子个数)作为阈值，当Neff＜Ntfreshold时，需要进行重采样，这样就能根据样本的具体情况，自适应的决定是否需要进行重采样。
[0106] S6.2.2：UT变换计算k+1时刻各粒子的位姿向量预测值；
[0107] S6.2.3：获取观测并与地图中已有特征进行数据关联
[0108] S6.2.4：采用UKF对粒子的先验分布 进行观测更新，获取各粒子的重要性密度函数
[0109] S6.2.5：路径估计
[0110] 从重要性密度函数 中采样 计算各粒子的非正则权值进行归一化处理 通过计算有效粒子数
比较Neff与所给阈值Nthreshold＝0.8n的关系，当Neff＜Nthreshold，说明粒子退化严重，需要对粒子集 重采样，得到新的粒子集
[0111] 利用粒子滤波器估计无人驾驶汽车的运动路径。对粒子集合进行扩展，将新得到的粒子集加入其中
[0112] S6.2.6：更新地图估计
[0113] 每个粒子都有对应的地图，但并不是地图对应的所有特征都需要进行更新，根据数据关联的结果，对于已经与地图中某个特征关联匹配的观测量，则利用观测信息更新地图中的相应坐标；对于没有关联上任何特征的观测量，作为新特征增加到地图中。
[0114] 环境特征的后验概率p(mt|st,zt,ut,nt)用UKF进行更新，假设第i个粒子的地图估计表示为 其中 分别为第j个特征k时刻的均值和方差，对地图估计的更新也就是对均值和方差的更新。
[0115] 以上对本发明的有关内容进行了说明。本领域普通技术人员在基于这些说明的情况下将能够实现本发明。基于本发明的上述内容，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
