一种面向封闭园区自动驾驶的单目视觉惯性定位方法技术

本发明专利技术公开了一种面向封闭园区自动驾驶的单目视觉惯性定位方法，方法包括：S0，读入多帧连续单目图像和IMU测量信息，初始化处理；S1，跟踪上一帧的特征点，在当前帧提取特征点，处理当前帧与上一帧的IMU测量信息；S2，求取当前帧的位姿；S3，将平移向量和特征点深度与旋转向量解耦，确定当前帧的特征点中的异常点，剔除异常点；S4，在滑动窗口中选择视觉重投影误差和IMU残差为代价函数，构建最小二乘问题，求解车辆位姿；S5，根据IMU测量信息的不确定性设置优化模式；S6，在滑动窗口中，若当前帧的上一帧为非关键帧，则去掉上一帧；否则去掉滑动窗口的初始帧；去帧后转S1。本发明专利技术能够保证车辆在园区的各种工况下实现实时稳定准确的定位。位。位。

【技术实现步骤摘要】
一种面向封闭园区自动驾驶的单目视觉惯性定位方法


[0001]本专利技术涉及自动驾驶
，特别为涉及一种面向封闭园区自动驾驶的单目视觉惯性定位方法。

技术介绍

[0002]近年来，L3级别以上的自动驾驶受到了广大企业和高校的关注，而封闭园区因车速基本在30km/h以下、线路固定和动态物体少等特点被认为为当前自动驾驶技术应用的优先场景。为了保证在园区中安全可靠地到达目的地，解决无人车“我在哪”、“怎么去”的问题相关重要，因此需要一种精确且鲁棒的定位技术。
[0003]自动驾驶的常用定位方法为由全球卫星导航定位单元(GNSS)和惯性单元(INS)组成的组合惯导系统。车辆大多采用安装双天线的卫星定位方式获取车辆的绝对三维坐标，在开阔地带定位精度可达厘米级，但在树木和建筑遮挡、天气不良的情况下GNSS信号不稳定，定位误差大。而IMU(英文全称为“Inertial Measuring Unit”，中文全称为“惯性测量单元”)不受外部测量干扰，通过自身高频率积分得到的位姿与低频率的GNSS位姿共同协作，可以完成高频率和高精度的定位。如果GNSS信号长时间较弱，单纯地依靠惯导系统，会因为IMU的噪声和偏差产生漂移。近年来不少专利采用同步定位与建图(SLAM)方法，通过搭载激光雷达或摄像头，能够在未知的环境中进行自主定位和导航。激光SLAM定位较为准确和稳定，但激光雷达成本高；视觉SLAM定位具有丰富的信息且成本低，但在纯旋转运动和急速转弯等情况下定位不准确。
[0004]当前自动驾驶定位系统的相关专利提出将上述定位方法结合以解决彼此弊端的技术。江铃汽车股份有限公司提出的商用车自动驾驶定位方法通过摄像头判断车辆前方为否为隧道、廊桥场景，如果为，则使用激光SLAM定位，否则使用组合惯导定位。安徽酷哇机器人有限公司提出的自动驾驶定位方法能够根据需求自动在组合惯导、激光SLAM和视觉SLAM定位方法间转换。杭州电子科技大学提出的基于激光SLAM和视觉SLAM地图融合的方法可以克服单种SLAM的缺陷，使融合后的地图更加适用于导航。
[0005]通过上述专利可以看出多传感器融合定位已经为一种趋势，相比单传感器定位，其定位的精度更高、稳定性更好。但除了定位的效果，同时也要考虑系统的易用性、传感器的价格等。

技术实现思路

[0006]针对上述问题，本专利技术的目的为提供一种面向封闭园区自动驾驶的单目视觉惯性定位系统，该系统不需要环境的先验信息，通过融合IMU的绝对尺度、快速响应的特点以及单目视觉标定简单、位姿准确的特点，保证车辆在园区的各种工况下实现实时稳定准确的定位，且其成本低、体积小，经济性好。
[0007]为实现上述目的，本专利技术提供一种面向封闭园区自动驾驶的单目视觉惯性定位方法，所述方法包括以下步骤：
[0008]S0，读入多帧连续图像和相应的IMU测量信息，并进行初始化处理，确定IMU参数，并将优化模式初始设置为mode A模式，其中，在mode A模式下，以IMU旋转积分和平移积分得到的旋转向量和平移向量作为图像帧的位姿；
[0009]S1，读取当前帧对应的IMU测量信息，并跟踪上一帧的特征点，并在当前帧提取预设数量的特征点，同时采取预积分的方法处理当前帧与上一帧间的IMU测量信息；
[0010]S2，基于优化模式，采用当前帧的特征点或IMU测量值积分求取当前帧的位姿；
[0011]S3，采用将平移向量和特征点深度与旋转向量解耦并求取归一化重投影误的方法，确定当前帧的特征点中的异常点，并剔除所述异常点；
[0012]S4，根据设置的优化模式，在滑动窗口中选择相应的视觉重投影误差和IMU残差为代价函数，构建最小二乘问题，通过非线性优化求解得到车辆位姿；
[0013]S5，根据IMU测量信息的不确定性大小设置优化模式，即选择mode I、mode G和mode A中的一种，其中，在mode I模式下，以IMU旋转积分得到的旋转向量作为初始值，经过视觉PnP求解图像帧的位姿，在mode G模式下，以IMU平移积分得到的平移向量作为初始值，经过视觉PnP求解图像帧的位姿；
[0014]S6，在滑动窗口中，若当前帧的上一帧为非关键帧，则去掉上一帧；否则去掉滑动窗口的初始帧；去帧后转S1。
[0015]进一步地，所述初始化步骤S0具体包括下述步骤：
[0016]S01，读入一帧图像作为当前帧以及读入对应的IMU测量信息，若滑动窗口中仅有当前帧一帧图像，从当前帧提取M个特征点；否则使用LK光流跟踪上一帧的特征点，并在当前帧提取合适的特征点以保证当前帧的特征点总数为M；
[0017]若当前帧为滑动窗口中的前两帧，设其为关键帧；否则基于上一帧与上一帧的上一帧之间的视差判断上一帧为否为关键帧；具体地，若上一帧与上一帧的上一帧之间的视差大于等于设定阈值，则将上一帧设置为关键帧；否则将上一帧设置为普通帧；
[0018]S02，若滑动窗口中图像的数量达到滑动窗口大小，则进入S03；否则返回S01；
[0019]S03，利用滑动窗口中图像的视觉特征求解各帧的位姿，从最大后验估计的角度出发，构建初始化代价函数，自适应地估计代价函数状态量的初始值并结合各帧位姿进行优化，若优化的结果收敛，则初始化成功，求得IMU参数，结束初始化步骤S0；否则转至S01。
[0020]进一步地，在步骤S03中，通过下述步骤来利用视觉特征求解位姿：
[0021]S031，计算滑动窗口中的第零帧与最后一帧的视差，若该视差大于第二预设阈值，使用对极几何方法求出第零帧和最后一帧的位姿变换，转步骤S032；若该视差不大于所述第二预设阈值或大于所述第二预设阈值但无法求出第零帧和最后一帧之间的位姿变换，则在滑动窗口内去帧后返回步骤S01；
[0022]S032，设第零帧所在坐标系为世界坐标系，位姿为单位矩阵，利用滑动窗口内各帧间的特征匹配关系求出各帧的位姿，此时位姿表示相机坐标系到世界坐标系的转换，使用三角化方法恢复3D点，以滑动窗口中各帧的位姿和3D点坐标为优化状态量的重投影误差构建代价函数，进行优化，若优化后不收敛，则在滑动窗口内去帧后返回步骤S01。
[0023]进一步地，在步骤S03中，通过以下步骤构建初始化代价函数，自适应地估计代价函数状态量的初始值并结合位姿进行优化获取IMU参数值：
[0024]S033，计算滑动窗口中各帧间的IMU预积分及协方差；
[0025]S034，假设相机和IMU之间的旋转外参已知，将滑动窗口中各帧的位姿由当前相机坐标系转为IMU的Body坐标系；
[0026]S035，初始化优化的状态量为尺度、重力方向、速度、加速度计偏差和陀螺仪偏差，速度的初始值由图像帧的平移计算得到，加速度计偏差和陀螺仪偏差默认为零，重力方向及尺度的初始值通过假设速度、位移预积分的初始噪声为零估计；
[0027]S036，以IMU预积分残差构建初始化代价函数，为加速度计添加先验，加速度计先验协方差对角线值的数量级与加速度计偏本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种面向封闭园区自动驾驶的单目视觉惯性定位方法，所述方法融合多帧连续的单目图像和IMU测量信息来输出车辆位姿，其特征在于，所述方法包括以下步骤：S0，读入多帧连续单目图像和相应的IMU测量信息，并进行初始化处理，确定IMU参数，并将优化模式初始设置为mode A模式，其中，在mode A模式下，以IMU旋转积分和平移积分得到的旋转向量和平移向量作为图像帧的位姿；S1，读取当前帧对应的IMU测量信息，并跟踪上一帧的特征点，并在当前帧提取预设数量的特征点，同时采取预积分的方法处理当前帧与上一帧间的IMU测量信息；S2，基于优化模式，采用当前帧的特征点或IMU测量值积分求取当前帧的位姿；S3，采用将平移向量和特征点深度与旋转向量解耦并求取归一化重投影误的方法，确定当前帧的特征点中的异常点，并剔除所述异常点；S4，根据设置的优化模式，在滑动窗口中选择相应的视觉重投影误差和IMU残差为代价函数，构建最小二乘问题，通过非线性优化求解得到车辆位姿；S5，根据IMU测量信息的不确定性大小设置优化模式，即选择mode I、mode G和mode A中的一种，其中，在mode I模式下，以IMU旋转积分得到的旋转向量作为初始值，经过视觉PnP求解图像帧的位姿，在mode G模式下，以IMU平移积分得到的平移向量作为初始值，经过视觉PnP求解图像帧的位姿；S6，在滑动窗口中，若当前帧的上一帧为非关键帧，则去掉上一帧；否则去掉滑动窗口的初始帧；去帧后转S1。2.如权利要求1所述的面向封闭园区自动驾驶的单目视觉惯性定位方法，其特征在于，所述初始化步骤S0具体包括下述步骤：S01，读入一帧图像作为当前帧以及读入对应的IMU测量信息，若滑动窗口中仅有当前帧一帧图像，从当前帧提取M个特征点；否则使用LK光流跟踪上一帧的特征点，并在当前帧提取合适的特征点以保证当前帧的特征点总数为M；若当前帧为滑动窗口中的前两帧，设其为关键帧；否则基于上一帧与上一帧的上一帧之间的视差判断上一帧为否为关键帧；具体地，若上一帧与上一帧的上一帧之间的视差大于等于设定阈值，则将上一帧设置为关键帧；否则将上一帧设置为普通帧；S02，若滑动窗口中图像的数量达到滑动窗口大小，则进入S03；否则返回S01；S03，利用滑动窗口中图像的视觉特征求解各帧的位姿，从最大后验估计的角度出发，构建初始化代价函数，自适应地估计代价函数状态量的初始值并结合各帧位姿进行优化，若优化的结果收敛，则初始化成功，求得IMU参数，结束初始化步骤S0；否则转至S01。3.如权利要求2所述的面向封闭园区自动驾驶的单目视觉惯性定位方法，其特征在于，在步骤S03中，通过下述步骤来利用视觉特征求解位姿：S031，计算滑动窗口中的第零帧与最后一帧的视差，若该视差大于第二预设阈值，使用对极几何方法求出第零帧和最后一帧的位姿变换，转步骤S032；若该视差不大于所述第二预设阈值或大于所述第二预设阈值但无法求出第零帧和最后一帧之间的位姿变换，则在滑动窗口内去帧后返回步骤S01；S032，设第零帧所在坐标系为世界坐标系，位姿为单位矩阵，利用滑动窗口内各帧间的特征匹配关系求出各帧的位姿，此时位姿表示相机坐标系到世界坐标系的转换，使用三角化方法恢复3D点，以滑动窗口中各帧的位姿和3D点坐标为优化状态量的重投影误差构建代
价函数，进行优化，若优化后不收敛，则在滑动窗口内去帧后返回步骤S01。4.如权利要求3所述的面向封闭园区自动驾驶的单目视觉惯性定位方法，其特征在于，在步骤S03中，通过以下步骤构建初始化代价函数，自适应地估计代价函数状态量的初始值并结合位姿进行优化获取IMU参数值：S033，计算滑动窗口中各帧间的IMU预积分及协方差；S034，假设相机和IMU之间的旋转外参已知，将滑动窗口中各帧的位姿由当前相机坐标系转为IMU的Body坐标系；S035，初始化优化的状态量为尺度、重力方向、速度、加速度计偏差和陀螺仪偏差，速度的初始值由图像帧的平移计算得到，加速度计偏差和陀螺仪偏差默认为零，重力方向及尺度的初始值通过假设速度、位移预积分的初始噪声为零估计；S036，以IMU预积分残差构建初始化代价函数，为加速度计添加先验，加速度计先验协方差对角线值的数量级与加速度计偏差间的协方差矩阵对角线值的数量级相同，后者直接从S033中的IMU协方差矩阵中读出；S037，优化获得IMU参数值，并将滑动窗口中位姿的世界坐标系转换为东北天坐标系，此时位姿表示IMU的Body坐标系到东北天坐标系的转换，即最终输出的车辆位姿。5.如权利要求4所述的面向封闭园区自动驾驶的单目视觉惯性定位方法，其特征在于，在步骤S035中，定义IMU参数为状态量在步骤S035中，定义IMU参数为状态量将传统预积分残差公式描述为r
I
＝[r
ΔR
，r
Δv
，r
Δp
]：式中，为窗口的大小；为尺度因子；为矩阵R
BI
的李代数，而为东北天的重力坐标g
I
＝(0，0，G)到IMU的Body坐标系g
B
的旋转矩阵，为重力的大小；b
a
为加速计的偏差；b
g
为陀螺仪的偏差；为滑动窗口中第i帧无尺度的速度；R
i
为第i帧的旋转矩阵，R
j
第为j帧的旋转矩阵，ΔR
ij
为第i和第j帧间的旋转预积分，(
·
)
vec
指旋转矩阵对应四元数的李代数；为滑动窗口中第j帧无尺度的速度，Δv
ij
为第i帧和第j帧间的速度预积分；为视觉结构求取的无尺度平移，Δp
ij
为第i帧和第j帧问的平移预积分；Δt
ij
为第i帧到第j帧所经历的时间。6.如权利要求5所述的面向封闭园区自动驾驶的单目视觉惯性定位方法，其特征在于，在步骤S036中，从最大后验估计问题...

【专利技术属性】
技术研发人员：秦晓辉，马珅，刘海涛，尚敬，胡云卿，王晓伟，边有钢，徐彪，谢国涛，秦兆博，秦洪懋，胡满江，丁荣军，
申请(专利权)人：中车株洲电力机车研究所有限公司，
类型：发明
国别省市：