认知机器人：3D环境表示方法

本文章收录在黑鲸智能系统知识库-黑鲸智能系统知识库成立于2021年，致力于建立一个完整的智能系统知识库体系。我们的工作：收集和整理世界范围内的学习资源，系统地建立一个内容全面、结构合理的知识库。

作者博客：途中的树

动机

• 机器人肯定是在3d世界活动的
• 避免碰撞、运动规划和定位需要准确的三维世界模型
• 已知：来自已知机器人和传感器位置的3D点云数据
• 问题：如何构建环境的三维结构？

传感器

构建3D世界需要的传感器

• 激光扫描
  • 范围扫描仪测量到最近的障碍物的距离

• kinect相机

  Kinect是由微软开发，应用于Xbox 360和Xbox One主机的周边设备。它让玩家不需要手持或踩踏控制器，而是使用语音指令或手势来操作Xbox 360和Xbox One的系统界面。它也能捕捉玩家全身上下的动作，用身体来进行游戏，带给玩家“免控制器的游戏与娱乐体验”。此设备是微软研究院的研究成果之一。

3D环境比较流行的表现方法

• 点云 Point clouds
• 体素网格 Voxel grids
• 高度地图 Height maps
• 表面地图 Surface maps
• 网状结构 Meshes
• 距离场 Distance fields
• …

点云 Point clouds

• 世界框架中的一组三维数据点

• 由激光扫描仪或深度相机等获得

• 优点

  • 数据无离散性
  • 绘图区域无限制

• 缺点

  • 内存不受限制
  • 位置查询没有确定的访问时间
  • 对自由空间或未知空间不作区分
    

• 使用点云高效的位置查询方法

  • 原始的实现（列表、数组）对位置查询有线性的复杂性
  • 通过kd-trees获得更有效的解决方案
  • kd-trees 在k维度上运行
  • 空间分割数据结构用于组织k维的点
    • 有右图为二进制空间划分
  • 搜索/插入/删除平均为 logarithmic 对数时间

3D体素网格

体素voxel网格是用固定大小的立方块作为最小单元，来表示三维物体的一种数据结构。
体素可以看成粗略版的点云。三维空间分割上的最小单位，参考二维空间的像素pixel

• 优点：
  • 体积表示法
  • 恒定的访问时间
  • 概率性更新
• 缺点
  • 内存要求。完整的网格被分配在内存中
  • 地图的范围必须是已知的/猜测的
  • 分散化错误

2.5D 地图绘制：高度地图

• 对落入一个二维单元格的所有点进行平均，并将其视为高度值
• 优点
  • 内存效率高（二维）。
  • 恒定时间访问
• 缺点
  • 没有纵向的物体
  • 只展示了一个级别的信息

多层平面地图Multi-level surface maps (MLS)

• 在多层级平面地图中MLS，每个2D的单元都储存着一组“补丁”包括
  • 平均高度$\mu$
  • 高度的方差 $\sigma$
  • 深度值 $d$
• 补丁也可能不包含深度信息（平面物体）
• 一个单元可能包含一个或者多个补丁（垂直间隔，例如桥）

从点云转换到到MLS地图

• 确定点云中的每个3D点对应的2D单元
• 根据threshold计算每个单元每个点（补丁）的垂直间隔
• 确定每个垂直补丁的相关信息
  • 高度和方差
  • 深度：最高和最低测量值之差

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-sUpmuC6G-1627291912654)(https://raw.githubusercontent.com/Uzizkp/image-host/master/img/picgo/20210720154323.png)]

MLS Maps

• 优点
  • 每个二维单元可以代表多个表面
• 缺点
  • 没有办法表示体积，在垂直维度上进行了离散。
  • 在MLS地图中，有几项任务并不是直接就能实现的

八叉树表示 Octree-Based Representation

八叉树（英语：octree）是一种树形数据结构，每个内部节点都正好有八个子节点。八叉树常用于分割三维空间，将其递归细分为八个卦限。八叉树是四叉树在三维空间中的对应，在三维图形、三维游戏引擎等领域有很多应用。

• 树形结构

• 递归地将空间细分为八个空间 octants

  • 实体几何中的octant是欧几里得三维坐标系中由坐标符号定义的八个分部之一。它类似于二维象限

• 根据需要分配空间体积

• 一种更智能的3d网格

• 优点

  • 全部都是3D模型
  • 本身就适应多分辨率
  • 内存效率高，只在需要时分配空间
  • 可以进行概率更新

• 缺点

  • 实现起来很困难（内存分配、更新、地图文件…）

例子：多分辨率查询

• $P(m_i)=ma{x}_{j=1...8}P(m_{i_j})$ with $m_{i_j}\in Children(m_i)$
• 这个分布函数指出了不同分辨率下的3D环境表示

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-I1EUizkx-1627291912665)(https://raw.githubusercontent.com/Uzizkp/image-host/master/img/picgo/20210720182212.png)]

利用光线追踪进行地图更新

• 从传感器原点到地图中的端点沿光束投射的射线
• 将最后一个体素标记为被占用，射线上的所有其他体素为空。
• 考虑到机器人的姿势，测量结果以概率方式整合（递归二元贝叶斯过滤器）。

概率地图更新

• 占用概率被建模为递归二进制贝叶斯过滤器

• 使用对数log-odds notation高效更新

例子(youtube)： Freiburg computer science campus (292 x 167 x 28 m3, 20 cm resolution)

https://youtu.be/GaX4vOM16EE

Octree in memory: 130 MB
3D Grid: 649 MB
Octree file: 2 MB (bit stream)

3D环境比较流行的表现方法：符号距离函数

下面再介绍一种3D环境表示方法——距离场

符号距离函数Signed Distance Function是某度量空间X中的一个集合Ω的函数，决定X中任一点到 Ω边界∂Ω的距离，并且由x是在Ω内还是Ω外确定其SDF的正负号：当x在Ω内时，SDF为正；当x在Ω外时，SDF为负。

Key idea

• 不表示占用值，而表示每个单元到最近的测量表面的距离
• 可以在亚体素精度下提取表面

Grid map VS SDF

• 网格地图：明确表示网格是否占用

• SDF ： 隐含表示网格占用状态

  [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-2JIIW0SE-1627291912703)(https://raw.githubusercontent.com/Uzizkp/image-host/master/img/picgo/20210720201540.png)]

SDF 方法

1. 计算符号距离值
2. 使用内插法提取曲面
3. 物体表面位于零点交叉点的

SDF性质

• 噪声在多次测量中可以被抵消
• 可以在亚体素精度下提取零点交叉点

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Q5LL7hfN-1627291912713)(https://raw.githubusercontent.com/Uzizkp/image-host/master/img/picgo/20210720202421.png)]

多重测量的加权函数

• 对于沿着光束的每个体素$x$，根据其置信度$w(x)$来衡量观察结果
• 小权重确保在有新的观察结果时可以更新数值
• 对于沿着光束的每个体素，存储
  • 到下一个表面的距离 $D$
  • 权重 $W$

截断SDF （TSDF）

• 从深度图像计算SDF
• 沿着光束计算体素到观察表面的距离
• 为了提高效率，只更新端点周围的一个小区域（截断）

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-SMKpP2cu-1627291912724)(https://raw.githubusercontent.com/Uzizkp/image-host/master/img/picgo/20210720212705.png)]

权重更新

• 对于每个体素，计算其所有测量值的加权平均值

  [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-aZPIwUHF-1627291912732)(https://raw.githubusercontent.com/Uzizkp/image-host/master/img/picgo/20210720212847.png)]

  • 当新的数据到达时，根据以下原则更新每个体素的值
    • $D\leftarrow \frac{WD+w_t{d}_t}{W+w_t}$,$W\leftarrow W+w_t$

  例子：通过真实场景的3D符号距离函数的截面图

  

  平面渲染方法

  • 光线投射 Ray Casting （GPU渲染，快）

    • 对于每个相机像素，拍摄一条射线并搜索零点交叉点
    • 当相机像素远离物体表面时，SDF中的值可以沿射线跳过（只更新截断）。

    [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-a865RjWQ-1627291912749)(https://raw.githubusercontent.com/Uzizkp/image-host/master/img/picgo/20210720214941.png)]

    光线投射 ，光线追踪与路径追踪的区别

• 多边形化（CPU渲染，慢）

  • 使用marching cubes算法来生成三角形网格Mesh extraction
    • 需要处理整个网格
    • 通过内插法找到符号距离函数

• 2D Marching square
  • 分别评估每个单元
  • 检查哪些顶点是内侧/外侧
  • 根据16个查找表生成三角形

• 3D Marching cube

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ckQFKPov-1627291912768)(https://raw.githubusercontent.com/Uzizkp/image-host/master/img/picgo/20210720215844.png)]

http://users.polytech.unice.fr/~lingrand/MarchingCubes/algo.html

Pros VS Cons

• SDF的优点
  • 可以构建整个3D模型
  • 亚体素的精准度
  • 支持实施GPU加速
• 缺点
  • 体素网格空间消耗比较大
  • 多边形化很慢

总结

• 存在大量3D环境表示法
• 要根据具体的应用场景选取建模方法
• 体素（Voxel）表示法可以构建起完整的3D场景
• 八叉树Octrees是紧凑的、具有多分辨率、概率性的三维表示法
• 表面模型支持可穿行性分析
• 符号距离函数也可以用来表示3D网格，但是仅支持表面亚体素级的精确度

参考

• Multi-Level Surface Maps for Outdoor Terrain Mapping and Loop Closing, R. Triebel, P. Pfaff, and W. Burgard, IEEE/RSJ Int. Conf. on Int. Robots and Systems (IROS), 2006
• OctoMap: An Efficient Probabilistic 3D Mapping Framework Based on Octrees, A. Hornung,. K.M. Wurm, M. Bennewitz, C. Stachniss, and W. Burgard, Autonomous Robots, 2013
• World Modeling, W. Burgard, M. Herbert, and M. Bennewitz. Handbook of Robotics (2nd edition), Chapter 45, Springer, 2016.
• Real-Time Camera Tracking and 3D Reconstruction Using Signe Distance Functions, E. Bylow, J. Sturm, C. Kerl, F. Kahl, and D. Cremers, Robotics: Science and Systems (RSS), 2013
• Continuous Humanoid Locomotion over Uneven Terrain using Stereo Fusion, M. F. Fallon, P. Marion, R. Deits, T. Whelan, M. Antone, J. McDonald, and R. Tedrake, Humanoids 2015