一种基于环视视觉和惯导融合的地下车库库位跟踪方法与流程

本发明涉及智能汽车自主泊车技术领域，尤其是涉及一种基于环视视觉和惯导融合的地下车库库位跟踪方法。

背景技术：

随着我国物质水平不断提高，汽车工业迎来了崭新的发展阶段。同时，与自动驾驶相关的技术已成为近年学术界和工业界的研究热点。作为自动驾驶研究的应用之一，自动泊车入位系统给驾驶者在停车入位方面带来了很大的便利，特别是对于新手驾驶者，该系统可帮助驾驶员更快的、自动地、正确地将汽车停入位泊车。该技术于2009年开始逐渐得到推广，2011年之后应用比例逐年快速增加。2016年上市的乘用车中，自动泊车技术应用比例达到了16.05％。2017年1-3月份上市的车型中，应用比例更是达到了19.99％。

然而，当前的智能汽车定位大都依赖gps全球定位系统，在gps信号缺失的地下车库无法用通常的手段进行定位。此外，目前地下车库基于无线信号的定位方案需要事先布置使用场景，成本较高、追踪精度较差，在数据处理上不具备良好的实时性。

技术实现要素：

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种硬件成本低、跟踪精度高的基于环视视觉和惯导融合的地下车库库位跟踪方法。

本发明的目的可以通过以下技术方案来实现：

一种基于环视视觉和惯导融合的地下车库库位跟踪方法，该方法包括下列步骤：

s1：启动车辆的环视相机和惯性测量单元，利用环视相机采集车辆行驶过程中的全景环视图像，利用惯性测量单元获取当前预积分。

优选地，所述的环视相机包括四个设于车辆前后左右四个方向的环视鱼眼相机。

s2：判断环视相机和惯性测量单元是否已完成初始化，若是，则执行步骤s3，否则，对环视相机和惯性测量单元进行初始化。

对环视相机和惯性测量单元进行初始化具体包括以下步骤：

201)启动环视相机和惯性测量单元，获取全景环视图像和当前预积分模型，选定一初始位置作为全局坐标系原点，用以建立库位角点地图；

202)采用棋盘格对环视相机进行标定，获得四个环视鱼眼相机的内参、畸变系数以及与车辆的相对位置；

203)通过五点法分解一个本质矩阵获取初始化环视相机运动，将初始化的平移作为单位尺度；

204)通过三角测量的方法恢复图像特征点深度，完成环视相机初始化；

205)依据初始化环视相机获取两图像帧之间的旋转，并将其与利用惯性测量单元的预积分模型获取的旋转进行对比，以偏差为节点变量，最小化二者的差值，获取惯性测量单元初始偏置值；

206)根据初始偏置值对惯性测量单元进行矫正，对每一个图像帧的速度、重力和尺度进行估计，完成环视相机和惯性测量单元初始化操作。

s3：根据全景环视图像提取库位角点，根据库位角点在观测时刻相对车辆的二维坐标，获取实际车辆坐标与库位角点坐标之间的相对位置。具体包括以下步骤：

301)利用机器视觉方法提取全景环视图像中的库位角点特征，并获取库位角点在观测时刻相对车辆的二维坐标；

302)根据库位角点在观测时刻相对车辆的二维坐标，使用单应矩阵描述处于共同平面上的一些点在两张图像之间的变换关系，对两张图像之间的特征点进行匹配；

假设图像i1和i2有一对匹配好的特征点p1和p2，则这些特征点落在某平面上，该平面满足方程：

p2＝hp1

式中，h为单应性矩阵。

303)根据匹配点获取单应性矩阵，并将其分解以计算旋转和平移；

304)根据步骤s2标定得到的环视鱼眼相机的内参、畸变系数以及与车辆的相对位置，结合旋转和平移获取实际车辆坐标与库位角点坐标之间的相对位置。

s4：根据惯性测量单元的当前预积分获取惯性姿态约束，对多个相邻数据执行相同处理并积分，获取相邻数据的相对位姿变换关系。

根据惯性测量单元的当前预积分获取惯性姿态约束具体包括以下步骤：

401)根据惯性测量单元的当前预积分获取加速度和角速度；

402)利用加速度推算速度和位移，利用角速度推算旋转，获取如下的积分关系：

其中，rwb为惯导元件在世界坐标系下的旋转姿态，vw为惯导元件在世界坐标系下的速度，pw为惯导元件在世界坐标系下的位移，为陀螺仪测量的角速度值，aw(τ)为加速度计测量的加速度值；

403)将积分形式转为差分形式；

404)考虑实际观测值，将偏差和噪声加入到差分形式，获取插入后的旋转姿态方程，即：

其中，b^g(t)和η^gd(t)分别为陀螺仪零偏和噪声，b^a(t)和η^ad(t)分别为加速度计的零偏和噪声。

s5：将步骤s3中提取的库位角点进行三角化并插入到局部映射中，剔除两帧图像特征匹配的非库位角点，将可见的所有库位角点保存在本地地图中。具体包括以下步骤：

501)从步骤2)提取的库位角点数据集中随机抽出四个不共线的样本数据计算变换矩阵，记为模型1；

502)计算库位角点数据集中所有库位角点数据与模型1的投影误差，若误差小于设定阈值，则将对应的库位角点数据加入内点集；

503)若当前内点集中的元素个数大于最优内点集个数，则更新当前内点集并同时更新迭代次数；

504)若内点置信度大于95％，则退出操作，否则，将迭代次数加一并重复上述步骤。

s6：对读取的环视图像进行分类，将不同类型的环视图像加入不同地图中进行优化，结合相邻数据的相对位姿变换关系，优化目标库位角点的环境地图全局坐标，进而完成库位跟踪。具体包括以下步骤：

601)对读取的图像帧进行分类，将当前图像帧的n个之前帧作为近邻帧，根据两帧图像间匹配的库位角点数量判断当前图像帧是否为关键帧；

602)将近邻帧的惯性单元积分处理得到的惯性姿态约束和两帧图像之间提取的库位角点的重投影误差约束作为边，将边加入到图优化模型中并对图优化模型进行优化；

603)若当前图像帧被判断为关键帧，则将其加入到全局优化地图检测回环，否则，将其加入到局部优化地图；

604)若局部优化地图中图像数量大于某一设定阈值m，则采用边缘化方法剔除旧的近邻帧，将局部地图中的近邻帧数量维持在m个；

605)循环步骤601)～604)，优化目标库位角点的环境地图全局坐标，直到车辆在地下车库中完成库位跟踪过程。

优选地，若两帧图像间匹配的库位角点数量少于2个，则将当前图像帧标记为关键帧。

优选地，采用levenberg-marquardt方法对图优化模型进行优化。

与现有技术相比，本发明具有以下优点：

一、本发明基于环视视觉的定位方法对车库库位进行追踪，该方法所需硬件成本低廉，且无需预先布置场景，可适应无gps信号环境；

二、本发明融合了视觉信息和惯性测量元件的运动信息，对于小场景范围的追踪精度较高，通过采用边缘化方法将局部优化变量数量控制在一定范围之内，具有较好的实时性；

三、本发明通过低成本的惯性测量元件与环视视觉测量特性互相补充，可有效解决视觉定位中的尺度问题，进一步提高整体的追踪效果。

附图说明

图1为本发明基于环视视觉和惯性测量单元的地下车库库位跟踪方法的流程示意图；

图2为环视相机安装位置示意图；

图3为环视视觉提取库位角点示意图；

图4为从环视鱼眼图像坐标系到车辆俯视图坐标系映射关系图；

图5为图优化节点和边关系示意图；

图6为边缘化示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

本发明涉及一种基于环视视觉和惯导融合的地下车库库位跟踪方法，如图1所示，包括下列步骤：

步骤1：启动车辆的环视相机和惯性测量单元，判断环视相机和惯性测量单元是否已完成初始化，若是，则执行步骤2，否则，对环视相机和惯性测量单元进行初始化后再执行步骤2。初始化步骤主要内容包括：

1.1、在车辆车体的前后左右四个方向通过3d打印特制的高分子外壳安装固定四个环视鱼眼相机，如图2所示；

1.2、采用棋盘格对相机进行标定，获得4个环视鱼眼相机内参、畸变系数以及与车辆车体的相对位置；

1.3、通过五点法分解一个本质矩阵来得到初始化环视相机运动，把初始化的平移当作单位尺度；

1.4、通过三角测量的方法恢复图像特征点深度，完成环视相机初始化；

1.5、根据初始化相机求得两帧之间的旋转，对比利用惯性测量单元预积分模型求得的旋转，以偏差为节点变量，最小化两者的差值，得到惯性测量单元初始偏置值；

1.6、根据初始偏置值对惯性测量单元进行矫正，对每一个图像帧的速度、重力和尺度进行估计，完成环视相机和惯性测量单元初始化操作。

步骤2：利用环视相机采集的全景图像数据，得到库位角点在观测时刻相对车辆的二维坐标，获取实际车辆坐标与库位角点坐标之间的相对位置。具体步骤为：

2.1、利用adaboost方法构建二分类器，依次判断某一个像素点是否为库位点，提取环视图像中库位角点特征，如图3所示。

2.2、使用单应矩阵描述处于共同平面上的一些点在两张图像i1和i2之间的变换关系。考虑在图像i1和i2有一对匹配好的特征点p1和p2。这些特征点落在某平面上，设这个平面满足方程：

p2＝hp1

式中，h为单应性矩阵。

2.3、根据匹配点计算得到3×3的单应性矩阵h，然后将它分解以计算旋转和平移；

2.4、通过步骤1的标定过程得到环视图像的不同坐标系的映射关系，将相机内参矩阵与单应性矩阵相乘可得到相应矩阵，某个像素点在图像坐标系下坐标乘以该矩阵可以得到在车辆坐标系下的坐标，如图4所示。

步骤3：读入惯性测量单元数据，计算相邻两帧数据之间的相对位姿，对多个相邻数据执行相同处理并积分，得到两个关键帧之间的相对位姿变换关系；具体内容包括：

3.1、从惯性测量单元获取的加速度推算速度和位移，以及根据角速度推算旋转，得到惯性测量单元运动学关系式，并计算旋转积分传播方程，公式如下：

式中，rwb是惯导元件在世界坐标系下的旋转姿态，为陀螺仪测量的角速度值。

3.2、计算得到速度积分传播方程，公式如下：

式中，vw是惯导元件在世界坐标系下的速度，aw(τ)为加速度计测量的加速度值。

3.3、计算位移积分传播方程，公式如下：

式中，pw是惯导元件在世界坐标系下的位移。

3.4、将上述公式的积分形式转为差分形式以匹配获取的离散的差分数据；

3.5、考虑到实际的观测值，将偏差和噪声加入到差分形式计算公式中，计算含有偏差和噪声的差分旋转传播方程，即：

式中，b^g(t)和η^gd(t)分别为陀螺仪零偏和噪声。

3.6、计算含有偏差和噪声的差分速度传播方程，公式如下：

式中，b^a(t)和η^ad(t)分别为加速度计零偏和噪声，g为重力加速度。

3.7、计算含有偏差和噪声的差分位移传播方程：

步骤4：将环视图像提取的库位角点三角化并插入到局部映射中。通过ransac方法排除两帧图像特征匹配的非库位角点，在这些图像中可见的所有库位角点都保存在本地地图中。具体操作包括：

4.1、从数据集中随机抽出4个不共线的样本数据计算出变换矩阵，记为模型1；

4.2、计算数据集中所有数据与模型1的投影误差，若误差小于阈值，加入内点集2；

4.3、如果当前内点集元素个数大于最优内点集个数，则更新当前内点集并同时更新迭代次数k；

4.4、如果内点置信度大于95％，则退出操作，否则，将迭代次数加一并重复上述步骤。

步骤5：对读取的图像帧进行分类，对不同类型的环视图像加入到不同的地图中进行局部或者全局非线性优化。随着车辆不断行驶，读取的环视图像越来越多，采用边缘化的方法剔除图优化模型中多余的图像节点和惯性测量节点，以维持较高的计算效率。

5.1、对读取的图像帧进行分类，包括当前帧图像的n个最近帧作为一类，被称为近邻帧；

5.2、如果两帧图像间匹配的库位角点数量少于2个，则当前帧被标记为关键帧；

5.3、将之前近邻帧的惯性单元积分处理得到的惯性姿态约束加入到图优化模型中作为运动边；

5.4、根据运动预积分的结果预测下一帧的车辆姿态，计算得到当前观测库位点坐标和协方差矩阵在下一帧的估计；

5.5、得到下一帧时刻实际观测到的库位点坐标和协方差矩阵，计算与上一帧预测得到的库位点之间的马氏距离；

5.6、如果马氏距离小于设定阈值，则判断当前帧观测的库位点与上一帧库位点是同一库位点，两帧图像数据关联；

5.7、根据数据关联情况，将两帧图像之间提取的库位角点的重投影误差约束作为边加入到图优化模型中作为视觉边；

5.8、把得到视觉帧时刻的车辆位姿和观测到的库位角点作为优化节点加入到图优化模型中，构成了如图5所示的结构，图中x代表定位位置，y表示观测到的库位点，每一个位置或库位点表示一个节点，位置和位置之间的连线表示运动边，位置与库位点之间的连线表示视觉边。

5.9、最小化目标函数如下式：

其中，er和es分别表示视觉残差项和惯性运动残差项，wr和ws分别表示环视相机和惯性测量元件的信息矩阵，它们由传感器噪声协方差矩阵的逆矩阵唯一确定。

5.10、采用levenberg-marquardt方法使局部地图的图优化模型的残差项误差达到最低；

5.11、如果当前图像被判断为关键帧，则将之加入到数据库中用以检测回环。

5.12、如果局部优化地图中图像数量大于某一设定阈值m(m>n)，则采用边缘化方法将旧的近邻帧剔除掉，使局部地图中的近邻帧数量维持在m个，如图6所示，x表示位置，z表示观测的库位角点，若x+z>m，将虚线框中的节点剔除。

5.13、将上述过程不断循环，直到车辆在地下车库中完成库位跟踪过程。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的工作人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。