【文献分享】基于感知质量的滚动优化无人机导航

论文题目：Perception-aware Receding Horizon Navigation for MAVs

作者：Zhang, Zichao ; Scaramuzza, Davide

作者机构：苏黎世大学

论文链接：https://files.ifi.uzh.ch/rpg/website/rpg.ifi.uzh.ch/html/docs/ICRA18_Zhang.pdf

研究目的：为了安全、准确地到达指定的目的地，同时最小化其状态估计的不确定性。

主要贡献：

• 利用前向单目相机用于状态估计和建图。
• 利用单目状态估计和地图系统的信息，生成一个候选轨迹库，并从感知质量、碰撞概率和到目标的距离三个方面对每条轨迹进行评分，选择得分最高的轨迹。
• 在一个实际的四旋翼机上通过仿真和实际实验证明，与纯反应式导航系统相比，文中的主动式导航方法能够提高目标到达任务的状态估计精度，特别是在困难场景（例如，弱纹理）下。

1 前言

微型飞行器（MAVs）既灵活又通用，适用于工业检测、农业和货物运输等各种任务。为了使无人机能够在未知环境中自主运行，需要可靠的定位和对自身位姿的估计。在用于状态估计的不同传感器中，摄像机重量轻且节能，非常适合于微型飞行器。

在对于基于视觉的状态估计中，摄像机的运动对估计精度有重要影响。因此，在规划无人机的运动时，既要考虑以上的任务，又要考虑感知质量。

本文的设计思路是一个典型的ActiveSLAM问题，作者选择了使状态估计精度最大化的运动，使得无人机在运行过程中不至于发生定位丢失，而且他们没有摒弃了优化全局地图中的运动轨迹的方法，而是用滚动的方式来解决主动SLAM问题，这样有效地降低了计算资源。

形象说明：他们算法的示意图如上所示，这个方法遵循图中蓝色轨迹运动，这个轨迹可以避开障碍物、到达指定的目的地并且最小化状态估计的不确定性。相比之下，纯反应式导航方案（红色线）虽然距离更近，但会进入无纹理区域，导致状态估计误差大，最终无法到达指定的目的地。

2 本文架构

该系统包括单目状态估计和建图系统以及运动规划系统。

• 单目状态估计和建图系统：首先使用SVO来估计摄像机的6自由度姿态。利用多传感器融合（MSF）软件将姿态估计与IMU测量进一步融合，得到正确的尺度和额外的速度估计。然后，将状态估计值和图像输入REMODE的变体，得到正面视图的密集三维地图。在使用稠密地图进行运动规划之前，利用OctoMap来降低地图中的噪声。

• 运动规划系统：由轨迹生成模块和轨迹评估模块组成。使用一种有效的轨迹生成方法，根据当前状态估计生成候选轨迹库。然后，我们根据感知质量、碰撞概率和到目标的距离三个指标评估每个轨迹。

3 单目状态估计与建图

文中的单目状态估计和建图采用SVO+MSF进行状态估计，通过REMODE生成稠密点云进行避障。SVO是一种非常有效的VO算法，适用于资源受限的系统(无人机)。

首先，为了评估感知质量，他们还从SVO中提取一个活动地图。在内部SVO维护一组稀疏点，这些点可以分为标志和种子两类。

• 标志点是从不同的帧多次观测到的三维点，它们的位置已经得到了很好的估计，对帧姿态估计的准确性贡献最大
• 种子是三维点，其位置尚未精确估计。

然后从与当前帧重叠的关键帧中提取可见的标志，我们将这些标志记为活动地图（图3）。

其次，使用Octomap来进一步降低REMODE输出中的噪声。如果密集点云包含过多的离群值，轨迹评估模块会错误估计碰撞概率，导致不必要的避碰机动。图4展示了活动地图和稠密地图，分别用MA和MD表示。

4 轨迹生成

轨迹生成函数：
$$f(t)\leftarrow g(p_0,v_0,p_f,t_f)$$

p0和v0是轨迹在世界坐标系中的初始位置和速度，来自当前状态估计。pf是轨迹的期望终点，tf是四旋翼跟随轨迹到达pf所需的时间。输出是函数f(t)。对于t∈[0,tf]，f(t)给出了时刻t时轨道上的状态（位置、方向、速度）。

因为使用了前视摄像机，希望在前方可见的方向移动，所以通过选择四旋翼前面一段弧上的端点来规划轨迹，如下图所示。这样只需要知道圆弧的半径r和角度θ即可，当半径r为一个设计参数时（固定值），θ的计算公式为：
$$\theta =max(k_{\theta }||v_0,{\theta }_{max})$$
其中kθ是常数。直观地说，θ随着速度的增加而增加，直到达到最大值θmax。然后使用θ和l，就可以均匀地采样弧上的端点。在生成N个候选轨迹后，以一个固定的时间间隔从每个轨迹上采样J个姿态。

然后根据以下公式，选择得分最高的轨迹Cbest驱动无人机运动：
$$C_{best}=argmax(1-p_{col})(R_{perc}+R_{goal})+p_{col}{R}_{col}$$
其中pcol是碰撞障碍物的概率（即密集地图）。Rcol、Rperc和Rgoal分别给出了与碰撞风险、感知质量和目标进度相关的奖励，使用D-opt标准的模拟形式来计算感知质量，其他具体推导在这里不展开，感兴趣可以阅读原文。

下图展示碰撞概率的计算。左栏显示模拟轨迹（黑色箭头）和障碍物（水平白带），右栏显示使用不同方法计算的碰撞概率。轨迹指数从左向右增加。

5 实验

5.1 仿真实验

作者使用Rotors模拟器搭建了仿真环境，在每个场景中，从稍微不同的起始位置启动四旋翼机，并命令它飞到指定的目的地10次，在每次运行中，一旦状态估计偏离地面真相超过5米，就终止执行并报告试验失败，一旦状态估计到给定目的地的距离小于3米，就报告试验成功。地图如下图所示：

下面这些图为实验结果，展示了所提出方法的优势：

• 第一个场景L形状由纹理强的区域（草和石头）和纹理较弱的区域（左下角的黑色区域）组成。可以观察到，纯反应式导航方法指令四旋翼机直接飞向目标，但轨迹经过视觉退化部分，导致状态估计误差较大。相比之下，文中的方法避免了四转子进入纹理较少的区域，并且能够保持合理的状态估计。
• 第二个场景转换在中间包含一个视觉退化区域，该区域位于起点和终点之间。虽然文中的方法能够使无人机走了更远的路才到达目的地，但反应式导航无法成功完成任务。
• 在第三场景障碍物中，地面几乎没有纹理，大部分视觉信息来自障碍物。在这个场景中，文中的方法和反应式导航的轨迹与前两个场景差别不大，但是文中的方法的状态估计误差仍然明显较小
• 第四个场景纹理完全，没有明显的视觉退化部分。反应式导航和文中的方法在这个场景中都表现得很好，文中的方法也有更好的性能。

5.2 真实世界的实验

下图显示了真实世界实验环境和实验结果，从实验结果可以看出，无人机并没有直线的前往目标，而是选择了一条距离稍长但纹理信息更丰富的路线。

6 总结

这篇文章出自苏黎世大学，我认为这篇文章的内容、配图以及公式推导都非常清晰明了，当时我读这篇文章的时候还是刚接触SLAM不就，但是看这篇文章也不会觉得困惑与无聊；作者提出的无人机ActiveSLAM解决方案也十分创新高效，通过文中的实验结果，可以清晰地看出他们所提出方法的优势！推荐大家阅读！