Vision-Only Robot Navigation in a Neural Radiance World

Paper name

Paper Reading Note

URL: https://arxiv.org/abs/2110.00168

TL;DR

RA-L 2022 文章，提出在 NeRF 表达的环境中进行机器人导航及循迹任务的方法

Introduction

背景

基于 on-board 传感器进行轨迹规划与循迹对于机器人应用很关键，目前机器人导航依赖 3d 环境表达方式，比如 voxel grid、point cloud、mesh model、 Signed Distance Field（SDF）等 3d 表示，目前流行的 NeRFs 精确编码详细的3D几何和颜色，本文拟研究在给定环境的预训练 NeRF 模型以及 on-board 的单目 rgb 摄像头传感器的情况下，如何在环境中进行机器人导航及循迹任务
在机器人导航中使用 NeRF 作为环境表达的优势
- 与体素模型、点云或网格模型不同，NeRF 直接基于图像输入进行训练，而不需要传统的特征提取、匹配和对齐流程
- 将几何图形表示为连续的密度场，因此非常适合机器人运动规划和轨迹优化
- NeRF 在新的位姿上可以渲染该位姿的图像，可以用来给机器人的定位做一些反馈校正
FastNeRF 等研究将 NeRF 的推理速度提升到 200 FPS，足够在实时机器人上应用

本文方案

在这里插入图片描述

本文提出以下方案来实现在 NeRF 表达的环境中进行机器人导航及循迹任务
- 一种新的轨迹规划方法，基于微分平面度规划完整的、动态可行的轨迹，以避免与NeRF环境的碰撞
- 基于优化的滤波器，以获得完整动态状态的后验估计，平衡动态预测损失和基于nerf的光度损失
- 一种在线重规划控制器，该控制器结合了上述两种反馈算法，使用NeRF模型在其环境中鲁棒地导航机器人

Dataset/Algorithm/Model/Experiment Detail

实现方式

在这里插入图片描述

任务定义

在仿真环境中，一个带有单目摄像头的机器人，给定初始位置 x0 与目标位置 xf，以及环境的 NeRF 模型，机器人需要基于单目摄像头的感知无碰撞地从 x0 移动到 xf
碰撞定义：在某一点上NeRF密度与它的占用率之间的关系并不明显。具体来说，NeRF密度表示给定空间点阻止光线的差分概率。本文假设终止光线的概率是终止质量粒子的概率的一个强有力依据。因此，t 时刻的碰撞概率为

其中 Xcoll 为碰撞集，s(bt) 为时间步 t 中机器人上的点 b 走过的距离，边界由布尔不等式得到。本文将碰撞概率作为轨迹优化过程中要最小化的 cost
任务实现流程：
- 机器人规划出最小化碰撞概率的轨迹
- 执行控制操作
- 基于机器人感知到的图片，更新对当前状态的置信度
- 重复以上操作直到机器人到达目标位置

轨迹规划

考虑到 NeRF 没有显示表示 occupancy，这里使用基于一阶优化（Adam optimizeroptimizer）的 differential flatness 规划轨迹，优化的损失包含碰撞概率和控制量

其中 W = {σ0, . . . , σh} 是希望通过优化得到的轨迹，Rτ 是机器人到世界坐标系的旋转矩阵，s(·) 是得到机器人上的点的运行举例的函数

初始轨迹：
- 初始位置与目标位置的直线，或者基于 A* 在粗网格上计算得到的轨迹
基于初始轨迹的优化过程示意如下，红色为初始轨迹，蓝色是优化中间状态，绿色为最终规划的轨迹

基于 NeRF 的机器人位姿滤波

每个时间步上，观测器会提供一个相机采集的图片 It、之前采取的控制量 ut，a state prior μt−1，之前时间步上的covariance Σt−1，各个状态的推导如下

其中 Qt 是 process noise covariance
本文的位姿估计基本参考之前的 iNeRF，即基于 ORB 等特征提取器挑选出图像中梯度变化大的像素点来进行 photometric loss 优化，由 μt 的位置和旋转可以构造出机器人 Tt 的姿态，优化目标如下

其中St是测量噪声协方差，最小化这个方程得到更新后的均值μt
最后，利用高斯损失函数的Hessian和协方差之间的已知关系来产生后验协方差
本文与 iNeRF 相比的优化点在于
- iNeRF 通过对参考姿态采取梯度步骤并在优化后投影到SE(3)流形上以恢复状态估计来优化状态
- 本文在每个梯度步骤后都将投影回流形上
  这两种方法在数学上是不同的，因为指数映射中斜对称矩阵的乘法不交换，公式为
  
  其中T为参考齐次变换矩阵，exp(·)为切空间与流形之间的指数映射。
  定性地观察到递归SE(3)梯度下降收敛比 iNeRF 的方法更快，因为在SE(3)流形上的 noisy photometric loss landscape。

在线重规划

在线重规划算法：机器人首先基于 A* 规划初始轨迹，然后执行第一个动作，状态滤波器基于采集的图像更新 belief 信息，该后验的平均值在轨迹规划器中用作新的起始状态，并与热启动时的其他航路点一起，在考虑外部干扰的情况下重新优化轨迹。这将一直持续到机器人达到目标状态。这个流程能够让机器人的规划抵抗一部分外部扰动影响
规划示意：蓝色轨迹是优化器返回的初始轨迹。然后，为许多时间步(白色)推出动态扰动，并重新优化轨迹(绿色)。规划器对垂直干扰的反应是选择飞到拱门上方，而不是像最初计划的那样飞到拱门下方。

实验结果

仿真环境

使用各种高保真模拟网格环境（Sketchfab生成的场景网格）演示了算法性能，首先渲染一批该环境中的图片，离线训练 NeRF 模型，基于 Habitat2.0 渲染了一批图像数据作为评测数据集
机器人的传感器图像类似地从真实环境中渲染，但轨迹不同，因此对NeRF模型的任何查询都不同于训练数据

Planner - Ground truth comparison

为了使用NeRF来推理避免碰撞，需要证明 NeRF 所学习的光密度是真实世界碰撞的一个很好的代理。图 a 为轨迹规划器生成的优化轨迹的可视化。提供的初始化显示为红色，部分优化的轨迹显示为蓝色。可以看到优化器收敛到一个既避免碰撞又平滑的轨迹。图 b 展示了 NeRF 碰撞损失图（实线）和 gt 网格的相交体积图（虚线），可以看到实线和虚线相关性很大，也即优化 NeRF 碰撞损失是能真的避免碰撞的。其中 iteration49 的规划轨迹（虚线）是完全能避免和 gt 障碍物网格的碰撞的。

在这里插入图片描述

Planner - Comparison to prior work

在 NeRF 规划器上实现了两个被广泛应用的方法：
- minimum-snap trajectory planning ：使用差分平坦度来计算通过一系列航路点的轨迹。然而，该方法通常使用手动放置的航路点，而本文的方法能够基于 NeRF 优化这些航路点的位置。在这个比较中，使用与本文方法使用的相同 A* 算法为最小捕捉规划器生成路线点
- Rapidly-exploring Random Trees (RRT)：基于采样的方法生成用于查找无碰撞轨迹的空间填充树。由于它需要二进制碰撞度量，首先使用 marching cubes 将 NeRF 转换为网格。当生成RRT时，使用球形碰撞模型，因为该规划器在规划阶段只有位置信息，不知道机器人的方向。最后，为了提取跟随RRT轨迹所需的控制动作，使用基于差分平坦度的控制器。
在 Stonehenge 场景中测试了 10 条不同障碍物、速度和复杂度的轨迹，实验对比如下

RRT 和本文方法无碰撞，本文方法的轨迹更平滑

颜色匹配轨迹的起点和终点，以及是否对给定规划轨迹是否成功的指示，本文方法失败率最低

每个规划器的平均NeRF碰撞度量和每个时间步的控制量，在 20 次初始化中取平均值。可以看到，本文方法产生的轨迹具有低控制量、低 NeRF 碰撞成本以及低失败率

Planner - Omnidirectional robot in tight space

piano movers 问题也能进行规划

Estimator - Comparison to prior work

与 iNeRF 相比，观测误差更低（红色是本文方法，界限表示平均误差之上和之下的一个标准偏差）

Online Replanning

抗扰动能力强。最初计划的轨迹是红色的。然而，当状态估计与计划的轨迹显著偏离时，机器人会重新规划并执行一条无碰撞的路径以到达目标，如蓝色和绿色的重新规划轨迹所示。

Performance and Timing

测试环境：AMD Ryzen 9 5900X @ 3.7 GHz CPU and Nvidia RTX 3090 GPU
初始轨迹需要经过20次以上2500次迭代才能优化。在在线重新规划实验中（∆t=0.1s），随后的轨迹更新在250次迭代的2s内发生。状态估计器通常在300个梯度步长上运行4s，其中0.25s是Hessian计算
作者任务上面的速度慢问题可以由未来工作中 NeRF 的加速来缓解