[论文]Real-Time Visual-Inertial Mapping, Re-localization and Planning Onboard MA...

[论文]Real-Time Visual-Inertial Mapping, Re-localization and Planning Onboard MAVs in Unknown Environments

• 将通过视觉惯性里程计构造的local map 融合进global map
• 在地图上，使用在线重定位(online relocalization)补偿视觉惯性里程计漂移的问题
• 提出从稀疏的姿态图(pose graph) 构建稠密3D occupancy grid的方法，并用octomap八叉树地图表示
• 提出改进3D路径规划算法“Polynomial Trajectory Planning for Aggressive Quadrotor Flight in Dense Indoor Environments”
• 能够适应控制状态约束，提升稳定性及减少计算时间

分层规划，

AscTecFly + stereo camera + IMU

平台概述如下图：

1. VI模块在local drift坐标下，返回pose消息，变换为Mission坐标（M）,该坐标系保证控制器层能够确保飞机的安全及避障
2. 构建局部地图，以进行局部避障(不是本文的工作)
3. sparse mapping/relocalization层，并行构建全局地图（G），并进行高效的存储以便复用
4. 在全局地图上构建全局三维路径规划
5. Mission controller被用来作为MAV和规划器之间的桥梁，手动控制时，速度控制指令直接给到MAV，轨迹跟踪时，全局规划信息需要转换为Mission坐标，给到MAV

3.Mapping和Localization

使用基于关键帧的视觉惯性里程计，通过在后台基于里程计的输出，来构建局部地图，进而使用局部地图进行relocation，以减少视觉惯性里程计带来的漂移误差。该方法还支持在后端跑BA进而减少误差。

3.1 构建Local map

基于关键帧的视觉里程计使用的苏黎世他们自己提的“Keyframe-based visual–inertial odometry using nonlinear optimization”这篇论文的方法。由于计算能力限制，导致只有很少的历史关键帧能够被用来进行优化。文中使用了使用关键帧及关键特征点构建局部地图，来充分利用图优化过程，以此生成一个以关键帧为顶点，以IMU测量值为边的稀疏位姿图。顶点中主要包含帧图像关键点，关键点描述子，3D三角测量landmark。

本文所提出的姿势图的一个重要特征是，使用具有独立base帧的任务（子地图），使任务与global帧保持一致，所有顶点位姿和landmark都在local mission 帧中表示。这允许通过改变它们之间的基础帧变换来使得几个子图之间可以相对的对齐，而不必更新地图中的每个顶点或landmarks。使用时固定参考地图，并在新信息可用时不断更新局部地图和参考地图之间的转换。

3.2 重定位

通过在BRISK特征描述子空间中检测最近邻，然后用RANSAC方案使用几何验证进行异常值排除。此过程中的任何内点都将作为约束添加到参考映射中的当前顶点和三角形3D关键点之间。

3.3 全局稠密模型

在构建local map时，还记录了立体视差图像，将它们存储在MAV上的磁盘上，并将它们与稀疏重定位图相关联。只存储视觉测距的关键帧（位姿图中的顶点）的视差图，从而最大限度地减少对磁盘的访问。由于视觉惯性测距中的关键帧已经被选中并且其间具有足够的运动，这将产生均匀且有效的覆盖。在中断参考任务并运行BA后，迭代参考任务中的所有过去顶点，并使用更新的顶点位姿将视差图像重新投影到全局地图中。从而建立环境的3D occupancy grid。

4. 路径规划

牺牲最优，来换取更少的计算时间，使用首先仅采样位置点使用直线路径，然后将得到的位置顶点用作支撑点以计算平滑的分段多项式轨迹，同时迭代地处理直线路径中不存在的碰撞，而不是在高维的状态空间中进行随机的采样，并生成多项式轨迹。

4.1 无约束线性初始解决方案

轨迹规划参考Minimum snap,这里面提到的方法使用的是将多项式轨迹的n阶导构建成一个带约束的QP问题，本文使用的方法，将其转化为一个不带约束的QP问题(适用于只有等式约束的闭式求解)。

4.2 非线性轨迹细化

通过无约束初始方案得到分段的多项式轨迹，现在需要增加约束，使得无人机能够在每一个段中的速度及加速度最大，时间尽可能短。

(1) 构建非线性优化，在cost 函数中加入了每段轨迹行驶时间的平方和

(2) 解最大速度及加速度：如果使用在轨迹上进行离散采样，则会带来很多不等式约束的求解，对于较长的路径来说会造成计算缓慢。文中的处理方式是对速度的导数进行处理，将其变形为两个多项式方程的卷积的形式，然后使用JenkinsTraub 方法求解实数解

(3)将状态约束结合到非线性优化问题中:增加软约束

(4)使用轨迹插值进行轨迹优化防止由于速度过快，在轨迹拐点处产生过度冲击，通过插值的方式，对拐点处进行降速处理

5. 实验结果

5.1 轨迹优化结果

左图是使用初始线性解决方案的轨迹，右图是加入了非线性轨迹细化后的轨迹，可以看出其飞行时间更短，轨迹更接近于保持直线连接

下表列出了轨迹分段下，线性初始化时间，优化时间，以及成功率的对比

5.2 地图生成及返航

1. 初始环境未知，飞行员给定high-level的位置指令对环境进行探索(下图的绿色线)。在探索过程中，构建稀疏位姿图以及由立体相机构建局部3D occupancy map，此时视觉里程计会产生一定的漂移，
2. 返航阶段，经过BA的几次迭代后，通过将视差图从优化的关键帧位置投影到3D空间中来生成全局3D occupancy grid。
3. 下图以粗体显示了reference路径，飞行轨迹使用的漂移的视觉里程计坐标。为了克服漂移，重定位模块尝试在每个关键帧（大约4Hz）对局部地图和经过BA后的参考地图进行调整，并在必要时校正参考轨迹，调整的地方在下图中用蓝色垂直线表示。可以看出，由于视点与先前看到的轨迹的较大偏差，在轨迹的开始和结束处仅存在闭环。结果还表明，控制器可以处理参考路径中的小跳跃，这表面了重定位单独运行的优势。在未来的工作中，可以触发规划器，以便在新的闭环情况下重新规划当前段。