Voxblox阅读笔记

🚗Voxblox: Incremental 3D Euclidean Signed Distance Fields for On-Board MAV Planning

Contributions

• 介绍在动态增长的maps中从TSDFs增量构建ESDFs的第一种方法。
• 分析建立TSDF的不同方法，以在大体素尺寸下最大化重建速度和表面精度
• 提供对最终ESDF误差的分析和实验分析，并提出克服这些误差的安全裕度。
• 使用MAV上的这些地图进行在线重新规划，从而验证整个系统

voxblox的系统图，展示了多个地图层(TSDF、ESDF和mesh)如何相互作用，并通过集成商与传入的传感器数据交互。

System

TSDFs

选择大体素的TSDF，同时采用二次加权 quadratic weighting（新测量值如何与现有测量值平均）和合并merging（传感器数据点如何分组）策略 $grouped $ $raycasting$来提高精度和速度

ESDFs

基于$wavefronts$思想从TSDF更新ESDF，mesh也根据TSDF的最新状态来构建以实现可视化目的。

使用两个$wavefronts$：raise和lower。

• 当一个体素与TSDF的新距离值高于存储在ESDF体素中的前一个值时，它将被添加到raise队列中。这意味着体素及其所有子体素必须无效。$wavefronts$传播直到没有任何体素留下无效的父体素为止。

• 当新的固定体素进入地图或先前观察到的体素降低其值时，lower的$wavefronts$开始。 相邻体素的距离会根据相邻体素及其到当前体素的距离进行更新。 当没有剩余的体素与邻居之间的距离会减小时，$wavefronts$结束。

  [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-w7IsqSTV-1616676298160)(https://gitee.com/wang-youai/image/raw/master/img/20210324224336.png)]

Experimental Results

与将新的TSDF更新传播到EuRoC数据集上的ESDF相比，将新数据集成到TSDF中的时序结果。 在体素较小的情况下，TSDF集成速度更快，但是在体素较大的情况下，由于传感器数据量没有减少，而ESDF时序却不断减少，因此TSDF集成变得平坦。

Path Planning

使用voxblox生成的TSDF(显示为灰度网格)和ESDF(显示为3D网格的水平切片)进行规划实验，完全在MAV上实时运行，不使用任何外部感知。飞行器试图通过使用基于轨迹优化的方法[2]来规划运动到人体模型后面的一点，该方法依赖于平滑的距离成本和梯度

系统搭建：

• 一个与IMU同步的前置立体摄像机作为stereo matching的输入到mapping过程和视觉惯性状态估计器visual-inertial state estimator的的输入
• 使用Intel i7 2.1 GHz CPU的一个线程
• 以0.05m大小体素更新TSDF，生成时间为0.08s
• 每次TSDF地图更新后使用0.20米的体素重新规划ESDF，时间为0.007s
• 以4 Hz的频率更新ESDF,保证即使在优化过程中随机重启，包括生成TSDF、更新ESDF和重新规划在内的整个系统也能够在250毫秒内运行良好

Planning method :

• 一个5米的包围球体，并将该球体中的所有未知体素标记为已被占用

• 为避免无法识别自身起始位置，在机器人的起始位置周围取一个1米的小球体，并将这个小球体中的所有未知体素标记为自由体素。

• 使用连续时间轨迹优化Replanning^[1]方法，该方法依赖于平滑的碰撞代价和来自欧几里得距离映射的梯度

  • 由全局规划器 Informed RRT* 获得一个直线路径
  • 沿全局规划的直线路径运行，逐步更新ESDF，同时局部规划器 Replanning 实时选择起点和终点：
    • 起点： 为当前轨迹上未来0.25秒（Replanning更新频率）的点
    • 终点： 选择全局轨迹上距离起点$h$米的点，其中$h$是规划视界。如果没有被占领，我们接受这个点作为目标，否则我们尝试在ESDF中逐渐缩短规划视界$h$，直到找到一个离该被占领点最近的未被占领(free)的邻点，作为最后的目标点。
  • 在这两点之间运行局部优化过程。1）优化成功地找到无碰撞路径，否则： 2）尝试随机重启，直到找到无碰撞轨迹，最后：3）飞行器停下来等待全局规划者选择新的路径。

  形成一条动态可行的多项式轨迹。
  

  实验结果显示了我们的在线本地重新规划系统

  UAV从大床垫后面开始，床垫挡住了它看到后面的铁障碍物(左侧)。规划目标设置在铁障碍内，但一旦UAV发现它occupied,，它就会停在前方，重新规划路径(黑色显示的规划路径)。这表明我们的系统能够避免新发现的障碍物，并在线找到可行的目标位置。

  

  随着时间的推移，为现实世界的实验制定本地计划

  最初的目标点嵌在第二个障碍(粉色)中，从一开始的位置是看不到的。随着时间的推移，这些路径在未与障碍物碰撞时就停止了，最终的路径(蓝色)比实验早期的许多计划更短，曲率更低

References

[1] H. Oleynikova, M. Burri, Z. Taylor, J. Nieto, R. Siegwart, and E. Galceran, “Continuous-time trajectory optimization for online uav replanning,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2016.