CN111780781B - 基于滑动窗口优化的模板匹配视觉和惯性组合里程计 - Google Patents

Abstract

本发明公开了一种基于滑动窗口优化的模板匹配视觉和惯性组合里程计，在载体上安装VO和IMU，根据IMU测量到的米制轨迹对VO测量到的非米制轨迹进行校正，得到单目VO的尺度因子；对VO采集到的图像进行仿射变换和模板匹配，得到基于VO观测量的视觉残差；对IMU测量值进行预积分，得到基于VO观测量的惯性残差；建立滑动窗口，判断当前帧是否发生了足够的运动，若是，将当前帧加入滑动窗口进行优化，并将窗口内最早的一帧边缘化，否则，将当前帧直接从滑动窗口中剔除；解算滑动窗口的非线性最小二乘方程，得到载***置、速度、姿态的最优估计值。本发明有效提高了组合里程计的定位实时性，且具有更好的鲁棒性。

Description

基于滑动窗口优化的模板匹配视觉和惯性组合里程计

技术领域

本发明属于导航技术领域。

背景技术

由于视觉里程计(VO)和惯性测量单元(IMU)具有良好的互补特性，所以越来越多的导航***采用视觉和惯性融合的方案进行定位，在大量场景中取得了很好的效果。视觉前端是VO/IMU组合定位***中最关键的模块，如果视觉前端无法很好的跟踪图像，定位效果会下降很多。目前的视觉前端非常依赖于周围环境，很容易受到光照变化、遮挡物、快速运动等因素影响，因此提高视觉前端的鲁棒性是组合导航***能够准确定位的关键。

目前常用的视觉前端通常采用特征点法或光流法进行位姿估计。特征点法首先要对每一帧图像进行特征检测，然后计算每一个特征点的描述子，根据描述子信息与上一帧的特征点进行匹配。在得到匹配好的特征点对后，通过PnP(Perspective-n-Point)算法计算得到相机的位姿估计。由于这种方法需要在每一帧图像都提取特征点并计算描述子，耗时较长，而且特征点法对于图像中的特征点数量要求较高，很难在特征点较少的场景下工作。光流法首先假设相邻图像帧间每个点的灰度值保持不变，在这个假设的基础上根据灰度值对特征点进行跟踪。相比于特征点法，光流法由于不需要在每一帧都提取特征点，也不用进行特征点的匹配工作，所有具有更好的实时性。但是光流法由于基于灰度值不变的假设，对***的光照变化非常敏感，很难在光照不稳定的场景下工作。此外，通常VO/IMU组合定位***的后端需要处理大量的三维空间点，会导致计算量很大，影响组合定位***的实时性。

发明内容

发明目的：为解决现有技术存在计算量大以及无法在关照不稳定的场景下工作等问题，本发明提供了一种基于滑动窗口优化的模板匹配视觉和惯性组合里程计。

技术方案：本发明提供了一种基于滑动窗口优化的模板匹配视觉和惯性组合里程计，具体包括如下步骤：

步骤一：将俯视单目相机VO和惯性传感器IMU安装在载体上，并将IMU的轨迹和VO的轨迹进行同步；

步骤二：设置基准时间，VO和IMU根据基准时间在采集的数据上加上时间戳信息，从而得到时间同步的图像和惯性测量值；

步骤三：基于模板匹配和图像仿射变换，得到第k时刻的图像和第k+1时刻的图像之间的非米制像素位移增量

步骤四：根据第k时刻IMU测得的数据和第k+1时刻IMU测得的数据，得到第k时刻和第k+1时刻之间载体的米制平移量

根据

和

得到VO的尺度因子，所述米制为以米为单位；

步骤五：IMU测得相邻两帧图像之间的原始数据，所述原始数据为相邻两帧图像之间VO的加速度和角度速，分别对原始数据进行预积分，从而推导出IMU的误差状态方程，利用IMU的误差状态方程处理预积分中的噪声，从而建立IMU惯性残差模型；

步骤六：构建滑动窗口，将当前时刻的图像放入滑动窗口中，基于模板匹配和图像仿射变换，得到当前时刻图像与窗口中上一个关键帧之间的非米制像素位移增量

将该

与VO的尺度因子相乘，得到当前时刻图像与窗口中上一个关键帧之间的米制像素位移增量p；根据上一次优化时建立的先验约束、IMU惯性残差和当前时刻图像与窗口中上一个关键帧之间的米制像素位移增量p，建立基于窗口的非线性优化函数；判断p是否大于预设的阈值，若是，则对最早的一个关键帧进行边缘化，基于非线性优化函数，对窗口进行优化，得到当前时刻的载体的位置、速度和姿态的最优估计值，并将当前时刻的图像作为用于下次优化计算的一个关键帧，根据边缘化的关键帧和窗口中剩余的关键帧，建立用于下一次优化计算的先验约束，并继续将下一个时刻的图像放入窗口中，计算下一个时刻载体的位置、速度和姿态的最优估计值；否则，在窗口中删除当前时刻的图像，并将下一个时刻的图像放入窗口中，继续计算下一个时刻载体的位置、速度和姿态的最优估计值。

进一步的，所述步骤一中将IMU的轨迹和VO的轨迹进行同步，具体为：根据IMU安装方向计算出VO和IMU之间的旋转外参数，并测量得到VO和IMU之间的平移量；将载体在IMU坐标系下的坐标与旋转外参数相乘，再加上VO和IMU传感器之间的平移量，从而将IMU的轨迹转换到VO坐标系下，实现IMU和VO的轨迹同步。

进一步的，所述步骤三具体为：

步骤3.1：在第k时刻的图像的中心选择一个大小为A×A的矩阵区域作为模板，按照如下公式进行图像仿射变换，得到图像仿射变换后的模板B：

其中，W(·)为图像仿射变换，(x,y)为图像中像素点的坐标值，p₁,p₂,p₃,p₄,p₅,p₆均为在该次仿射变换中的仿射变换参数；在该次仿射变换中p₁,p₂,p₃,p₄,p₅,p₆初始值均为0，该六个参数的确定值通过超定方程法计算得到；

步骤3.2：根据标准平方差匹配计算得到模板B和第k+1时刻的图像的相似度，从而在第k+1时刻的图像中找到与模板B相似度最高的一块区域，将该区域作为第k+1时刻的图像的模板；

步骤3.3：将第k时刻的模板的中心点像素坐标设为(u_k,v_k)，第k+1时刻的模板的中心点像素坐标设为(u_k+1,v_k+1)，则

为：

T为空间转置。

进一步的，

其中，K为第k时刻的图像的长，L为第k时刻的图像的宽，min(·)表示取最小值。

进一步的，所述VO的尺度因子s为：

进一步的，所述步骤六中的非线性优化函数χ*为:

χ^*＝argmin(e_prior+∑e_IMU+∑e_VO)

其中，e_prior为由边缘化计算得到的先验残差；e_IMU为根据IMU的误差状态方程计算得到的IMU惯性残差，e_VO为根据模板匹配计算得到的上一个关键帧点与当前时刻图像之间的米制像素位移增量。

进一步的，所述步骤六中采用列文伯格-马夸尔特算法求解非线性优化函数，从而对窗口进行迭代优化。

有益效果：本发明相比于传统视觉前端所采用的特征点法与光流法，模板匹配VO不需要提取特征点，摆脱了视觉前端对特征点的依赖，使组合里程计具有更好的鲁棒性。本发明还在模板匹配算法中加入了图像仿射变换，使模板匹配算法对旋转进一步的提高了鲁棒性，可以适用于快速运动或坡道场景等相机图像可能发生旋转变换的环境中。相比于传统的视觉惯性里程计，本发明提出的组合里程计在后端不用对大量路标点的深度值进行优化，有效提高了组合里程计的定位实时性。

附图说明

图1是本发明的***框图。

图2是IMU和VO同步时间处理示意图。

图3是加入仿射变换的模板匹配示意图。

具体实施方式

构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

如图1所示，本实施例提供了一种基于滑动窗口优化的模板匹配视觉和惯性组合里程计，视觉部分通过模板匹配算法计算得到视觉残差，IMU部分通过预积分计算得到惯性残差。后端通过L-M算法求出一个最优的位姿估计值，使视觉残差和惯性残差之和最小。具体为：

步骤一：安装传感器并初始化外参。在载体平台上安装俯视单目相机VO和惯性传感器IMU。根据VO和IMU坐标系之间的旋转约束计算得到单目相机和惯性传感器之间的旋转外参数，并通过人工测量直接得到VO和IMU之间的平移量。将载体在IMU坐标系下的坐标与旋转外参数相乘，再加上VO和IMU传感器之间的平移量，从而将IMU的轨迹转换到VO坐标系下，实现IMU和VO的轨迹同步。

步骤二：采集时间同步的传感器数据。由于IMU通常以一个较高的频率采集数据，而VO采集图像的频率较低，所以在采集传感器数据的时候要做好硬件同步和软件同步。组合里程计的同步时间处理示意图如图2所示。

给视觉传感器和惯性传感器一个基准时间，VO和IMU根据给出的基准时间为它们各自采集到的数据加上时间戳信息，从而得到时间同步的VO数据和IMU数据；

步骤三：计算单目相机的尺度因子。考虑k时刻和k+1时刻之间载体的位置变化，根据IMU加速度计测量出的数据计算得到位置变化的平移量

为米制，即以米为单位，计算单目VO的尺度因子：

其中，s为单目相机的尺度因子；

为VO测量出的k时刻和k+1时刻之间载体的平移量，这个平移量是非米制的。根据计算出的尺度因子将整个相机轨迹恢复为米制：

如此就可以将视觉轨迹和IMU轨迹对齐，完成了组合里程计的初始化。

步骤四：确定模板尺寸。假设k时刻，相机采集到的图像大小为K×L，那么以图像最中心的像素点为模板中心，选择一块大小为A×A的矩阵区域作为模板，A的选择方法为：

其中，min(·)表示取最小值。

步骤五：加入图像仿射变换。选择模板匹配并进行仿射变换的示意图如图3。对所选模板中的像素点进行图像仿射变换，所用的仿射变换表示为：

其中，W(·)为图像仿射变换；(x,y)为像素点的坐标值；p₁,p₂,p₃,p₄,p₅,p₆为仿射变换的参数，这六个参数值需要实时在线估计，本实施例采用超定方程法去估算，使图像在发生一定的旋转之后还能准确进行模板匹配。通过调整仿射变换的六个参数，可以得到相邻图像间的旋转和平移从而更好的进行模板匹配，使模板匹配对旋转具有更好的鲁棒性，克服了因为运动过快或者坡道场景导致的模板误匹配问题。

采用超定方程法叠代的终止条件为：

其中，F为模板在仿射变换后的误差函数。当误差函数最小时，通过这6个放射变换的参数可以得到相邻图像间的旋转和平移，从而使图像之间可以更好的进行模板匹配，使模板匹配对旋转具有更好的鲁棒性，克服了因为运动过快或者坡道场景导致的模板误匹配问题。

考虑在k时刻和k+1时刻进行模板匹配。根据标准平方差匹配计算得到k时刻模板和k+1时刻的图像之间的相似度；在k+1时刻的图像中找到和k时刻经图像仿射变换模板相似程度最高的一块图像作为k+1时刻的模板。将k时刻模板的中心点像素坐标设为(u_k,v_k)，k+1时刻模板的中心点像素坐标设为(u_k+1,v_k+1)，则k时刻到k+1时刻的像素坐标增量，即通过相机测量得到的非米制位移增量为：

其中T为空间转置。

将得到的相机位移增量与尺度因子相乘，即可得到相邻时刻两个图像之间米制的像素平移距离。

步骤六：计算IMU预积分和误差传递。对读取到的IMU数据进行预积分，得到相邻两图像帧之间相机的平移距离、速度变化和旋转变化量。

由于IMU会产生累积误差，所以要计算IMU的误差传递。IMU读取到的测量数据受到偏置和高斯白噪声的影响，IMU测量到的载体的角速度ω和加速度a可以表示为：

其中，

和

分别表示角速度和加速度的真实值；b_gyro和b_acc分别表示陀螺仪和加速度计偏置的随机游走，会随着运行时间缓慢变化，其导数服从高斯分布；n_gyro和n_acc分别表示陀螺仪和加速度计受到的高斯白噪声。根据IMU的运动方程，可以推导出IMU的误差状态方程为：

其中，χ_B为IMU误差状态向量；F为***传递矩阵；G为噪声观测矩阵。

步骤七：构建滑动窗口，将当前时刻的图像放入滑动窗口中，基于模板匹配和图像仿射变换，得到当前时刻图像与窗口中上一个关键帧之间的非米制像素位移增量

将该

与VO的尺度因子相乘，得到当前时刻图像与窗口中上一个关键帧之间的米制像素位移增量p；根据视觉里程计的状态变量、IMU的误差状态方程和当前时刻图像与窗口中上一个关键帧之间的米制像素位移增量p，建立基于窗口的非线性优化函数；判断该p是否大于预设的阈值，若是，则对最早的一个关键帧进行边缘化，基于非线性优化函数和上一次优化时建立的先验约束，对窗口进行优化，得到当前时刻的载体的位置、速度和姿态的最优估计值，并将当前时刻的图像作为用于下次优化计算的一个关键帧，根据边缘化的关键帧和窗口中剩余的关键帧，建立用于下一次优化计算的先验约束，并继续将下一个时刻的图像放入窗口中，计算下一个时刻载体的位置、速度和姿态的最优估计值；当前帧与上一个关键帧间的位置距离没有超过预设阈值，说明相机还没有发生足够大的运动，不必对滑动窗口进行更新，窗口中删除当前时刻的图像，并将下一个时刻的图像放入窗口中，继续计算下一个时刻载体的位置、速度和姿态的最优估计值若。

步骤八：求解***状态变量的最优估计值。视觉和惯性组合里程计的***状态变量χ为：

χ＝[x₀,x₁,…,x_n]

其中，x₀,x₁,…,x_n为每一个时刻载体的位姿估计值，包括位置、速度、姿态、陀螺仪偏置和加速度计偏置。构建的非线性优化函数表示为：

χ^*＝argmin(e_prior+∑e_IMU+∑e_VO)

其中，χ*为***状态变量的最优估计值；e_prior为由边缘化计算得到的先验残差；e_IMU为根据运动约束计算得到的IMU残差(根据IMU的误差状态方程计算得到的IMU残差)；e_VO为根据模板匹配计算得到的视觉残差(根据模板匹配计算得到的相邻时刻的两个图像之间的米制的像素位移增量)。

通过列文伯格-马夸尔特(L-M)算法求解构造的非线性最小二乘问题，求解的增量方程表示为：

(J^TJ+μI)Δx＝-J^Tf

其中，J为残差对状态变量求导的雅克比矩阵；μ为阻尼因子，需要根据***误差的真实下降值和按照近似模型得到的下降值之间的比例关系实时调整；Δx为用于状态变量更新的增量；f为***误差。根据该增量方程迭代对滑动窗口内的状态变量进行更新，直到增量Δx足够小，则输出载***置、速度和姿态的最优估计值。

本发明基于滑动窗口优化的模板匹配视觉和惯性组合里程计的技术方案为：在载体上安装单目摄像头和IMU并计算测量两个传感器之间的外参。其中，单目相机垂直于地面安装，镜头俯视地面。采集时间同步的传感器测量数据，并根据IMU测量到的米制轨迹对VO测量到的非米制轨迹进行校正，得到单目VO的尺度因子，完成***初始化。对单目相机采集到的图像进行仿射变换和模板匹配，得到基于VO观测量的视觉残差。对IMU测量值进行预积分，并推导其噪声模型，得到基于VO观测量的惯性残差。建立某个固定大小的滑动窗口，判断当前帧是否发生了足够的运动。若当前帧发生了足够的运动，则将当前帧加入滑动窗口进行非线性优化，同时将窗口内时间最早的一帧边缘化，否则，将当前帧直接从滑动窗口中剔除。通过L-M算法对滑动窗口内的所有状态变量进行迭代优化，得到载***置、速度、姿态的最优估计值并实时输出。

上面结合附图对本发明的实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。

Claims (5)

1.基于滑动窗口优化的模板匹配视觉和惯性组合里程计，其特征在于，具体包括如下步骤：

步骤一：将俯视单目相机VO和惯性传感器IMU安装在载体上，并将IMU的轨迹和VO的轨迹进行同步；

步骤二：设置基准时间，VO和IMU根据基准时间在采集的数据上加上时间戳信息，从而得到时间同步的图像和惯性测量值；

步骤三：基于模板匹配和图像仿射变换，得到第k时刻的图像和第k+1时刻的图像之间的非米制像素位移增量

步骤四：根据第k时刻IMU测得的数据和第k+1时刻IMU测得的数据，得到第k时刻和第k+1时刻之间载体的米制平移量

根据

和

得到VO的尺度因子，所述米制为以米为单位；

步骤五：IMU测得相邻两帧图像之间的原始数据，所述原始数据为相邻两帧图像之间VO的加速度和角度速，分别对原始数据进行预积分，从而推导出IMU的误差状态方程，利用IMU的误差状态方程处理预积分中的噪声，从而建立IMU惯性残差模型；

步骤六：构建滑动窗口，将当前时刻的图像放入滑动窗口中，基于模板匹配和图像仿射变换，得到当前时刻图像与窗口中上一个关键帧之间的非米制像素位移增量

将

与VO的尺度因子相乘，得到当前时刻图像与窗口中上一个关键帧之间的米制像素位移增量p；根据上一次优化时建立的先验约束、根据步骤5中的IMU惯性残差模型计算当前时刻图像与上一个关键帧之间的IMU惯性残差和当前时刻图像与窗口中上一个关键帧之间的米制像素位移增量p，建立基于窗口的非线性优化函数；判断p是否大于预设的阈值，若是，则对最早的一个关键帧进行边缘化，基于非线性优化函数，对窗口进行优化，得到当前时刻的载体的位置、速度和姿态的最优估计值，并将当前时刻的图像作为用于下次优化计算的一个关键帧，根据边缘化的关键帧和窗口中剩余的关键帧，建立用于下一次优化计算的先验约束，并继续将下一个时刻的图像放入窗口中，计算下一个时刻载体的位置、速度和姿态的最优估计值；否则，在窗口中删除当前时刻的图像，并将下一个时刻的图像放入窗口中，继续计算下一个时刻载体的位置、速度和姿态的最优估计值；

所述步骤五具体为：计算IMU预积分和误差传递，对读取到的IMU数据进行预积分，得到相邻两图像帧之间相机的平移距离、速度变化和旋转变化量；

由于IMU会产生累积误差，所以要计算IMU的误差传递，IMU读取到的测量数据受到偏置和高斯白噪声的影响，IMU测量到的载体的角速度ω和加速度a表示为：

其中，

和

分别表示角速度和加速度的真实值；b_gyro和b_acc分别表示陀螺仪和加速度计偏置的随机游走，会随着运行时间缓慢变化，其导数服从高斯分布；n_gyro和n_acc分别表示陀螺仪和加速度计受到的高斯白噪声，根据IMU的运动方程，可以推导出IMU的误差状态方程为：

其中，χ_B为IMU误差状态向量；F为***传递矩阵；G为噪声观测矩阵；

所述步骤六中求解得到的最优估计值的表达式为：

χ＝[x₀,x₁,…,x_n]

其中，x₀,x₁,…,x_n为每一个时刻载体的位姿估计值，包括位置、速度、姿态、陀螺仪偏置和加速度计偏置；

所述步骤六中的非线性优化函数χ^*为:

χ^*＝arg min(e_prior+∑e_IMU+∑e_VO)

其中，e_prior为由边缘化计算得到的先验残差；e_IMU为IMU惯性残差，e_VO为根据模板匹配计算得到的上一个关键帧点与当前时刻图像之间的米制像素位移增量；

所述步骤六中采用列文伯格-马夸尔特算法求解非线性优化函数，从而对窗口进行迭代优化，求解的增量方程表示为：

(J^TJ+μI)Δx＝-J^Tf

其中，J为残差对状态变量求导的雅克比矩阵；μ为阻尼因子；Δx为用于状态变量更新的增量；f为***误差，根据该增量方程迭代对滑动窗口内的状态变量进行更新，直到增量Δx足够小，则输出载***置、速度和姿态的最优估计值。

2.根据权利要求1所述的基于滑动窗口优化的模板匹配视觉和惯性组合里程计，其特征在于，所述步骤一中将IMU的轨迹和VO的轨迹进行同步，具体为：根据IMU安装方向计算出VO和IMU之间的旋转外参数，并测量得到VO和IMU之间的平移量；将载体在IMU坐标系下的坐标与旋转外参数相乘，再加上VO和IMU传感器之间的平移量，从而将IMU的轨迹转换到VO坐标系下，实现IMU和VO的轨迹同步。

3.根据权利要求1所述的基于滑动窗口优化的模板匹配视觉和惯性组合里程计，其特征在于，所述步骤三具体为：

步骤3.1：在第k时刻的图像的中心选择一个大小为A×A的矩阵区域作为模板，按照如下公式进行图像仿射变换，得到图像仿射变换后的模板B：

其中，W(·)为图像仿射变换，(x,y)为图像中像素点的坐标值，p₁,p₂,p₃,p₄,p₅,p₆均为在该次仿射变换中的仿射变换参数；在该次仿射变换中p₁,p₂,p₃,p₄,p₅,p₆初始值均为0，该六个参数的确定值通过超定方程法计算得到；

步骤3.2：根据标准平方差匹配计算得到模板B和第k+1时刻的图像的相似度，从而在第k+1时刻的图像中找到与模板B相似度最高的一块区域，将该区域作为第k+1时刻的图像的模板；

步骤3.3：将第k时刻的模板的中心点像素坐标设为(u_k,v_k)，第k+1时刻的模板的中心点像素坐标设为(u_k+1,v_k+1)，则

为：

T为空间转置。

4.根据权利要求3所述的基于滑动窗口优化的模板匹配视觉和惯性组合里程计，其特征在于，

其中，K为第k时刻的图像的长，L为第k时刻的图像的宽，min(·)表示取最小值。

5.根据权利要求1所述的基于滑动窗口优化的模板匹配视觉和惯性组合里程计，其特征在于，所述步骤四中VO的尺度因子s为：

Images