1. 为什么要做sensor fusion
单种类型的sensor均存在一定的缺陷:
-
Visual odometry:
- 需要适度的照明条件条件
- 需要差异化的视觉特征
-
Lidar odometrty:
- 运动雷达涉及aggressive motion问题(扫描率低)
- 在平坦环境下会扫描匹配失误
==> Sensor fusion,探索两者优势,互补融合。
2. DEMO & LOAM(Ji Zhang)
2.1 DEMO(Visual odometry)
针对单纯的视觉里程计,可以采用RGB-D图像+深度的解决方案。
深度可以采用红外测距获取,但由于sensor使用范围的限制,大部分测量范围为10m以内,超出这个范围,尤其是室外,会出现大量面积有图像但没有深度的情况,即存在数据稀疏/深度信息不足的情况。
DEMO的深度来自RGB-D相机/3D lidar,文章提出通过深度图or三角测量(针对先前的运动估计)两种方法将深度信息叠加在image上,此外采用Bundle Adjustment非线性优化方法,以细化帧对帧的运动估计。实验结果表明室内运动估计效果好,室外效果一般。
存在的问题:色彩均匀的地方很难跟踪特征,需改进feature detection and tracking
DEMO实验结果
2.2 LOAM(Lidar odometry)
若仅使用3D Lidar,与相机相比,分辨率比较差,激光扫描比较远的时候,点云非常稀疏,所以在比较远的情况下,无法进行正常工作。而针对Lidar的畸变,可以基于lidar扫描,用其他sensor来校正,解决方案包括:
- LOAM,相机+IMU
- 多个相机+scan matching(成本高,硬件复杂)
- V-LOAM,用一个相机+model drift as linear motion
LOAM依赖激光点云进行运动估计和构图,因为激光自身的扫描率很低,数据率低,所以需要激光器扫描比较平缓,让它有足够的时间得到充分的扫描结果,同时这里将IMU叠加上去,对点云结果进行修正。
存在的问题:运动平缓smooth,高频运动用IMU补偿,未解决aggressive motion问题
LOAM实验结果
3. V-LOAM
3.1 Overview
**目标/优势:**6-DOF,precision,low-drift,real-time,ligh-frequence,robost to aggressive motion
**解决方案:**DEMO + LOAM,漂移匀速假设,无回环
**大体思路:**VO结合雷达深度信息估计相机位姿,利用这个位姿进行激光点云的畸变矫正,后将矫正后的点云映射到局部地图中,用于后续的位姿优化。
3.2 Visual odometry
视觉里程计结合lidar的深度信息粗略估计相机运动位姿,在高频估计ego-motion。主要包括三大模块,即the feature tracking block、the depth map registration block和the frame to frame motion estimation block。
(1) The feature tracking block
Function:提取并匹配连续图像之间的视觉特征,利用Kanade Lucas Tomasi (KLT)方法跟踪Harris corners
Input:图像
Output:视觉特征
(2) The depth map registration block
Function:深度图处理,将雷达点云映射到局部深度图,并将深度关联至视觉特征
Input:视觉特征,深度图,雷达点云
Output:视觉特征(带/不带深度)
Method:
-
在KD-tree中,所有激光点都投射到相机中心周围的一个单位球面上(r,theta,fai);
-
当关联深度信息时,将特征投射到球面上,并为每个特征找到三个最近的激光点;
-
三个点构成局部平面,深度是通过将相机中心的光线投射到平面上来插值计算;
-
若无激光射程覆盖,使用图像序列对其进行三角测量。
深度来源: 相机/lidar、三角测量、KD-Tree(深度映射存储在2D KD-tree中,用于快速索引)
(3) The frame to frame motion estimation block
Function:粗略估计相机位姿
Input:相邻两帧的视觉特征(带/不带深度)
Output:相机位姿R和t
Method:构建3个约束方程,即上一帧深度已知和未知两个运动约束方程(归一化齐次坐标系),和一个重投影误差约束方程,即转化为3D-2D、2D-2D的重投影误差非线性优化方程,利用LM求解。具体推导如下所示:
3.3 Lidar odometry
通过激光雷达里程计在低频率细化运动估计和纠正漂移,即利用视觉里程计估算出来的位姿进行激光点云的畸变矫正,后将矫正后的点云映射到局部地图中。
(1) A sweep to sweep refinement step
Function:利用VO估计出来的位姿对相邻两帧的激光点云进行配准,畸变校正,并估计运动位姿
Input:激光点云,R,t
Output:优化后的位姿R,t
Method:计算曲率、提取边角点和平面点、计算相应点的距离、基于姿态估计做点插值、优化修正位姿(LM)
(2) A sweep to map registration step
Function:建图,多线程估计,以1Hz更新地图点云
-
局部地图存储在kd-tree
-
匹配新旧点云特征,合并,保证点云密度
-
Feature points are extracted 10 times more than odometry step
(3) Transfrom integration
Function:视觉里程计信息帧率高,BA优化帧率较低,将二者结合,从视觉里程计到帧运动变换,利用BA优化的结果修正视觉里程计,可以得到图像帧率下的高频综合位姿结果。
3.4 实验
实验设备:60Hz帧率uEye单目相机(广角/鱼眼镜头)+基于北越UTM-30LX激光扫描仪
评估设备:单个摄像机+Velodyne激光雷达
A. 精度测试
- 相机精度测试: 采用广角镜头(76水平视野)和鱼眼镜头(185水平视野)两种相机设置进行精度测试。传感器都由一个以0.7米/秒速度行走的人手持,激光雷达测程法测得的轨迹(蓝、黑曲线)相差不大,结果表明激光雷达测程法无论测程偏移量大小,都能校正视觉测程偏移。
- 室内和室外环境: 虽然鱼眼相机的视觉测程不如广角相机精确,但是激光雷达测程可以将精度提高到相同的水平。
B. 鲁棒性测试
- 楼梯环境和廊道环境: 使用广角相机进行快速运动时,即在快速转弯过程中视觉跟踪不准确,导致运动估计失败,鱼眼相机的精度略有下降,但在快速运动中具有较强的鲁棒性。
- 光照剧烈变化环境:
该实验中该灯被关闭了四次,当灯熄灭时视觉里程计停止工作,恒速预测,每扫一次激光雷达测程都可以校正漂移,能处理短暂性的光曝光。