一种融合事件与距离的视觉惯性里程计方法

1.本发明属于多传感器融合导航定位技术领域，尤其涉及一种融合事件与距离的视 觉惯性里程计方法。


背景技术：

2.微型无人机具有体积小、重量轻、操作性强、可单人携带、隐蔽性好、操作方便 等特点，具有巨大的军事价值和民用价值。与传统飞行器不同，微型无人机在高动态 环境下的定位是一项极具挑战性的研究课题。随着无人机应对的场景越来越复杂，在 室内、楼宇之间、丛林等全球卫星定位导航拒止环境，对于微小型无人机等轻小型设 备而言，既无外部定位源，也无法携带激光雷达等体积和重量较大的传感器，定位难 度大大增加。面对上述情况，相机和惯性测量单元(inertial measurement unit,imu) 这类轻小型传感器的单目视觉惯性里程计(visual-inertial odometry，vio)能够发挥 重要作用。
3.目标在高速运动或亮度变化剧烈的场景下会出现运动模糊，进而导致图像质量下 降，采用基于传统视觉的vio难以实现精确的定位。事件相机作为一种新型的神经 激发视觉传感器在高动态环境中具备潜在优势。与传统帧相机输出固定帧率的图像不 同，事件相机输出异步、高频的“事件流”，即当每个像素的亮度变化超过一定阈值 时输出一个事件。相比传统帧相机，事件相机具有低延时(微秒级)、动态范围广 (140db)的特性。事件相机这些特性在高动态场景中具有巨大的应用价值。
4.同时无人飞行器在高速运动过程中可能出现imu无法得到有效激励的情况，vio 会因无法获得有效的观测尺度信息从而产生严重的运动漂移，出现较大的定位误差。 距离传感器能够以厘米级的误差测量数十米范围内的距离，并以其轻小的重量体积能 够很好的作为视觉(惯性)模组的补充而不显著地增加系统的负载。距离传感器可以 在imu无法激励时，补充提供vio所需的视觉尺度信息，提升vio的定位精度与稳 定性。
5.基于上述现有技术中的不足，以及事件相机和距离传感器的特点，本发明提出了 一种融合事件与距离的视觉惯性里程计，在现有单目vio的基础上融合了事件相机 与距离传感器，提供更加精确的定位。


技术实现要素：

6.本发明针对高动态场景下无人机导航定位困难的问题，提出了一种融合事件、距 离与惯性测量单元imu的导航定位方法，提升无人飞行器在快速运动环境下的定位 精度和稳定性。该方法的主要内容为：首先在前端利用imu状态预测、距离观测和 后端估计的地图点对事件相机输出的事件流进行运动补偿，合成清晰的事件图像；然 后后端将视觉几何约束、imu预积分约束和距离观测的共面约束联合起来做非线性 滑动窗口优化，最终构建代价函数优化求解估计系统状态。具体技术方案如下：
7.一种融合事件与距离的视觉惯性里程计方法，包含以下步骤：
8.s1：搭建视觉惯性里程计系统：定义世界坐标系为w，相机坐标系为c，imu 坐标系
分成m
×
n个区域，每个区域维持有限个特征点，特征点在各区域内均匀分布；
27.s32：对于当前帧图像，先做从上一帧到当前帧的前向光流，同时为上一帧图像 的每个特征点提供当前帧图像上坐标的预测值；
28.s33：得到各特征点在当前帧图像的跟踪值后，做从当前帧到上一帧的反向光流， 得到图像帧之间的特征匹配；
29.s34：基于ransac算法求解上一帧到当前帧的基础矩阵，去除错误匹配。
30.优选的，所述步骤s4中采用舒尔补的方式构建边缘化先验约束，具体为：
31.假设残差为r，残差关于优化变量的雅克比为j，求解以下方程：
32.hδx＝b
33.其中h＝j
t
j，b＝j
t
r，δx为变量x在本次迭代中的增量；
34.采用舒尔补的方式将变量x分成需要被边缘化的部分δx1和不被边缘化的部分 δx2：
[0035][0036]
相应地，h和b也分成：
[0037][0038]
采用高斯消元法将δx2边缘化，得到与δx1不相关的约束如下：
[0039][0040]
优选的，所述步骤s4中的imu预积分约束构建方式如下：
[0041]
根据步骤s1中的imu动力学模型得到：
[0042][0043][0044][0045]
其中为预积分量，分别为
[0046][0047][0048][0049]
预积分提供了连续帧之间的位置、速度和姿态约束，对于两个相邻帧i与i+1构 建预积分误差为：
[0050][0051]
其中是误差函数，χ是优化变量，是变量观测值；
[0052]
优选的，所述步骤s4中的视觉重投影约束构建方式如下：
[0053]
对于每一个特征点，利用其逆深度和关键帧的位姿将其投影到其他关键帧上，并 与其他关键帧的对应观测坐标作差，以计算重投影误差。
[0054]
优选的，所述步骤s4中的共面约束构建方式如下：
[0055]
假设事件相机视野内的点都处于同一平面，通过距离观测获得imu与水平面之 间的距离，同时利用特征点获得imu与水平面之间的距离，构建距离观测的共面约 束。
[0056]
优选的，所述步骤s5具体包括：
[0057]
s51：根据s4中的非线性约束条件构建代价函数如下：
[0058][0059]
其中，r
p
表示残差，h
p
是信息矩阵。是imu预积分误差函数，是重投影误差函数，是共面约束误差函数。表示优化变量的 观测值。ρ是核函数，表示imu坐标系下第i帧与第i+1帧之间预积分的误差协 方差，表示相机坐标系下第l帧与第j帧之间重投影的误差协方差，表示相 机坐标系下基于距离测量点r与特征点k的在第l帧与第j帧之间的共面约束误差协 方差；
[0060]
s52：通过ceres优化器求解得到位姿估计。
[0061]
相比于现有技术，本发明提出的一种融合事件与距离的视觉惯性里程计具有以下 优势：
[0062]
本发明融合了距离、视觉与惯导等信息实现多源共同定位，克服了无人机高动态 场景定位技术的不足，解决了无人飞行器在高速运动下定位不准以及难以定位的问 题，减小了传统技术的漂移。该方法在多种场景下均能使用，受环境影响较少，具有 良好的定位精度、稳定性与鲁棒性，在无卫星定位环境中发挥重要作用。
附图说明
[0063]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所 需要使用的附图作简单地介绍，通过参考附图会更加清楚的理解本发明的特征和优 点，附图是示意性的而不应理解为对本发明进行任何限制，对于本领域普通技术人员 来讲，在不付出创造性劳动的前提下，可以根据这些附图获得其他的附图。其中：
[0064]
图1是本发明融合事件与距离的视觉惯性里程计的流程图；
[0065]
图2是本发明的事件运动补偿示意图；
[0066]
图3是本发明的多点共面约束示意图；
[0067]
图4为本发明的位姿估计示意图；
[0068]
具体实施方式
[0069]
为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实 施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本发明 的实施例及实施例中的特征可以相互组合。
[0070]
在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可 以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面 公开的具体实施例的限制。
[0071]
一种融合事件与距离的视觉惯性里程计方法，实施方式包括以下步骤：
[0072]
s1：搭建视觉惯性里程计系统：定义世界坐标系为w，相机坐标系为c，imu 坐标系为b，距离传感器坐标系为r，确定imu动力学模型。
[0073]
imu动力学模型为：
[0074][0075][0076][0077]
其中在a坐标系下的点p定义为pa，该点坐标由a坐标系变换到b坐标系的转 换矩阵为对应旋转矩阵为平移矩阵为gw是世界坐标系下的重力向量， ti是第i帧的时间，δt是两帧之间的间隔，和是第i帧imu坐标系相对世 界坐标系的平移、速度和旋转矩阵。是对应的四元数，表示四元数相乘，a
t
和ω
t
是加速度与角速度的测量值，与是加速度计与陀螺仪的偏置。
[0078]
s2：生成事件运动补偿图像：固定事件数量生成事件帧图像，利用imu和距离 传感器获得的位姿与图像深度信息通过运动补偿得到更为清晰的事件运动补偿图像。
[0079]
按照时间戳顺序将事件流分为包含相同事件数量的窗格，每个窗格wi合成一张 事件图像，其中图片上每个像素的强度和窗格内在该像素坐标上的事件数量成正相 关。运动补偿后的图像表示为：
[0080]
[0081][0082]
其中x是事件原始位置，xi是运动补偿后事件位置，f(x)是事件帧图像中x像素 位置的亮度。
[0083]
由于每个事件都对应着不同时间戳，当相对运动较快时，直接累积会产生严重的 运动模糊，不利于后续的特征提取与跟踪。故对事件进行运动补偿，再累积为事件图 像。选取时间窗口内事件流某一时刻作为参考时刻t
ref
，对于发生在时刻tk的事件， 将其投影至t
ref
，投影后的新坐标为
[0084][0085]
其中pk是投影前坐标，k为相机的内参矩阵，tk和t
ref
分别为tk和t
ref
时刻相机 相对参考系的位姿，通过imu的角速度和加速度积分得到，sk和s
ref
分别是该事件投 影前和投影后在对应相机坐标系上的深度信息，通过距离测量与平面约束获得。
[0086]
最后，基于运动补偿后的新的归一化坐标p
′k，进行事件的累积生成更为清晰的 运动补偿事件图像。
[0087]
s3：特征提取、检测与跟踪：提取harris角点，进而通过光流法进行跟踪，得到 图像帧之间的特征匹配，并基于ransac去除少量的错误匹配得到较为准确的特征 点帧间关联。
[0088]
首先将运动补偿图像划分为m
×
n个区域提取harris角点，然后对当前帧图像， 进行从上一帧到当前帧的前向光流，并为上一帧的每个特征点提供一个在当前帧的坐 标预测值。对于已三角化的特征点k，在上一帧的i归一化坐标为利用imu预 测当前帧i+1的位姿t
i+1
将该点投影至当前帧上
[0089][0090]
对于未三角化的特征点，计算平均光流得到特征点在当前帧的预测坐标，至此前 向光流完成。
[0091]
在得到特征点在当前帧的跟踪值后，为保证跟踪的质量，再进行一次从当前帧到 上一帧的反向光流，逆向计算特征点在上一帧的坐标，只有当两次的坐标误差小于设 定阈值时才认为是一次正确的跟踪。
[0092]
最后，基于ransac算法求解上一帧到当前帧的基础矩阵来去除少量的错误匹 配，获得了较为准确的帧间关联。
[0093]
s4：构建非线性约束条件：固定滑动窗口大小，构建边缘化先验约束、imu预 积分约束、视觉重投影约束以及距离传感器与特征点的共面约束。
[0094]
s4-1：采用滑动窗口进行后端优化求解，窗口大小为n+1，构建优化变量如下
[0095]
χ＝[x0,x1,...,xn,ρ0,ρ1,...,ρm]
[0096][0097]
其中ρk是特征点k在起始帧的逆深度。是imu与相机之间的外参， 是相机与距离传感器之间的外参，可在优化求解时作为变量在线校准。
[0098]
根据步骤s1中的imu动力学模型得到
[0099][0100][0101][0102]
其中为预积分量，分别为
[0103][0104][0105][0106]
预积分提供了连续帧之间的位置、速度和姿态约束，对于两个相邻帧i与i+1构 建预积分误差为：
[0107][0108]
其中是误差函数，χ是优化变量，是变量观测值。
[0109]
s4-2：对于每一个特征点，利用其逆深度和关键帧的位姿将其投影到其他关键帧 上，并与它在其他关键帧的对应观测坐标作差来计算重投影误差。对于特征点k，从 第i帧投影到第j帧上的坐标为：
[0110][0111]
其中是第i帧和第j帧上的归一化坐标，λk是特征点在起始帧的逆深度。
[0112]
重投影误差表示为
[0113][0114]
s4-3：当新的关键帧到来时，为了维持滑动窗口的大小不变，需要将窗口内最旧 的关键帧边缘化出去。为了在控制规模的同时保证旧的关键帧所携带的观测或约束信 息不丢失，采用舒尔补的方式将过去的状态和观测转化为仍在窗口内的状态的先验约 束。
[0115]
假设残差为r，残差关于优化变量的雅克比为j，以高斯牛顿法为例，最终将求 解以下方程
[0116]
hδx＝b
[0117]
其中h＝j
t
j，b＝j
t
r，δx为变量x在这次迭代中的增量。
[0118]
采用舒尔补的方式将变量x分成需要被边缘化的部分和不被边缘化的部分：
[0119][0120]
相应地，h矩阵和b也分成：
[0121][0122]
采用高斯消元法将δx2边缘化，得到与δx1不相关的约束如下
[0123][0124]
s4-4：假设相机视野内的点都在同一平面上，通过距离观测可以获得imu与水 平面之间的距离，同时利用特征点也能获得该时刻imu与水平面的距离，利用这一 点构建距离观测的共面约束。
[0125]
距离测量点在imu坐标系下的坐标可表示为：
[0126][0127]
其中rj是第j帧的距离观测，和分别为相机与imu的旋转和平移外参，和分别为相机与距离传感器的旋转和平移外参。
[0128]
第j帧imu到平面的距离可表示为：
[0129][0130]
其中rj为第j帧imu相对世界坐标系的旋转矩阵，n是该平面在世界坐标系下 的单位法向量。
[0131]
对于第j帧上的特征点k，该点在世界坐标系下的坐标可表示为
[0132][0133]
其中λk是起始帧i的逆深度，ri和ti为第i帧imu相对世界坐标系的旋转与平移。
[0134]
由于该特征点也在平面上，因此第j帧imu到平面的距离也可以表示为第j帧 imu位置与该特征点的连线与平面法线的内积：
[0135][0136]
与dj两者应该相等，即表示该特征点与距离观测点的连线属于该平面，垂直 于法线：
[0137][0138]
由于每一帧相机都有对应的距离传感器数据，因此对于每一个特征点可以和它所 有被观测帧之间建立约束，与视觉的重投影误差在形式上保持一致。
[0139]
s4-5：相机视野多数是占据大部分区域的平面，但可能存在一些不同的平面，即 视野内的特征点可能不在同一平面，需要判断特征点是否和距离测量点处于同一个平 面。假设特征点在平面上，可以基于共面约束得到该特征点在当前帧j上的深度
[0140][0141]
其中是特征点k在第j帧的归一化坐标。
[0142]
该特征点在起始帧的估计深度为判断特征点是否和距离测量点在同一平面 上可以转换为基于此假设计算出来的深度是否合理。分别计算在这两个深度下的重投 影误差，通过对比这两个重投影误差，来判断特征点是否在同一平面上。
[0143]
首先基于估计深度计算特征点从第i帧到第j帧的重投影误差，投影得到特征 点在第j帧相机坐标系下的坐标：
[0144][0145]
其中，表示基于特征点k的在第i帧下的坐标；
[0146]
坐标归一化后得到重投影误差为
[0147][0148]
进而基于共面约束深度计算重投影误差
[0149][0150]
重投影误差为
[0151][0152]
比较两个重投影误差，如果|e2|≤|e1|，表示比更符合当前的位姿约束，即共 面约束的深度是合理的，该特征点与距离测量点属于同一个平面，因此将该平面约束 加入到优化求解当中，否则不加入。
[0153]
s5：构建代价函数，求解位姿：约束误差项构建代价函数，优化求解位姿。
[0154]
根据s4中的非线性约束条件构建代价函数如下：
[0155][0156]
其中从左向右依次是边缘化先验约束，imu预积分约束，视觉重投影，距离共 面约束，ρ是核函数。最后通过ceres优化器求解即可得到位姿估计。
[0157]
以上所述仅为本发明的实施按例而已，并不用于限制本发明，对于本领域的技术 人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任 何修改、等同替换、改进等，均应包含在本发明的保护范围之内。