利用光线跟踪加速欧几里德符号距离场的地图构建

唐嘉宁 刘志聪 李孟霜 彭志祥 谢翠娟 陈云浩

摘 要：针对现有小型无人飞行器在复杂未知环境中执行自主探索任务时，存在机载端GPU计算资源不足，在线建图速度慢、探索效率低的问题.本文在传统欧几里德符号距离场（ESDF）方法的基础上，融合光线跟踪原理，加速构建ESDF地图，以提高飞行器在复杂未知环境中的探索效率.首先使用整数运算提高光线跟踪遍历体素的速度，从而加速体素占用概率的更新;然后通过调整哈希数据结构，减少地图占用内存;最后使用广度优先搜索算法（BFS）实现地图更新与融合.为验证本文方法的有效性，分别在公开数据集、仿真环境和真实环境下，与当前前沿的建图方法进行对比，实验结果表明，本文所提出的方法在三种不同情况下的地图平均更新时间分别减少了72.37%、60.80%和56.79%，显著提高了建图速度，为小型无人飞行器在线探索奠定基础.

关键词：地图构建；光线跟踪；符号距离场；梯度优化

中图分类号：V249

文献标志码： A

文章编号：2096-398X（2023）04-0158-08

Abstract：For the existing small unmanned aerial vehicles （UAVs） performing autonomous exploration tasks in complex and unknown environments，there are problems such as insufficient computing resources of the airborne GPU，slow online mapping and low exploration efficiency.Based on the traditional Euclidean signed distance field （ESDF） method，this paper integrates the ray tracing principle and accelerates the construction of ESDF maps to improve the exploration efficiency of aircraft in complex and unknown environments.Firstly，integer operation is used to improve the speed of ray tracing voxel traversal，thus speeding up the update of voxel occupancy probability; Then adjust the hash data structure to reduce the memory occupied by the map; Finally，the breadth first search algorithm （BFS） is used to update and fuse the map.In order to verify the effectiveness of the method proposed in this paper，it is compared with the current cutting-edge mapping methods in the open dataset，simulation environment and real environment respectively.The experimental results show that the average map update time of the method proposed in this paper in three different situations is reduced by 72.37%，60.80% and 56.79% respectively，which significantly improves the mapping speed and lays a foundation for online exploration of small unmanned aerial vehicles.

Key words：mapping; ray tracing; signed distance fields; gradient optimization

0 引言

小型无人飞行器因其体积小和灵活性，已经成为最广泛的无人自主系统研究平台之一，并且多应用于未知环境的自主探索及自主导航等方面［1-3］.由于小型飞行器自身的低载荷及有限的机载计算能力，使小型飞行器在进行自主探索的过程中需要快速、轻量级的算法进行环境感知、地图构建和航迹规划.

地图是无人机进行导航的基础，无人机航迹规划过程中有很多不同类型的地图框架可以使用.其中，占用栅格地图是规划算法中常用的地图形式之一，它可以用于快速碰撞检测以及占用概率的评估，Octomap基于八叉树的数据结构，用来表示紧凑的内存，进行地图查询和存储占用概率［4］.Chen等［5］利用八叉树的数据结构，将空闲空间进行划分，生成安全飞行走廊，用以生成无碰撞的安全轨迹.Ji等［6］提出了一个mapless-planner系统，它利用原始的点云地图进行规划，通过最近邻查询算法，查找空间中最近的障碍物，以此生成球形飞行走廊而保证高质量的轨迹.Florence等［7］提出一种新的地图结构，它能够在规划时有效查询地图构建时位姿不确定性的局部3D几何信息，从而实现提高规划的效率以及鲁棒性，支持无人机安全快速飞行的目的.但是对于无人机自主导航来说，轨迹规划需要的是空闲空间的信息，而不是障碍物的信息，用于轨迹规划［8］的理想地图要求能够快速查询地图中每个体素的空闲、占用状态，或者能够方便的获取当前体素到障碍物的距离，例如欧几里德符号距离场（ESDF，Euclidean Signed Distance Field）地圖.ESDF地图中的每个体素都储存着当前体素距离其最近障碍物的直线距离和梯度信息，因此广泛的应用于基于梯度的无人自主导航规划［9-14］.基于梯度信息的轨迹规划方法能够将生成的轨迹推离障碍物，以保证轨迹的安全性.Zhou等[15]中利用局部ESDF地图中的梯度信息和动态约束，通过B样条来优化初始路径，保证路径的安全性和动态可行性，以此用于三维复杂环境中的快速飞行.

最近，众多机构在增量构建三维规划的ESDF地图方面做了大量工作.Oleynikova等［16］提出的Voxblox框架，专注于从截断符号距离场（TSDF，Truncated Signed Distance Fields）增量构建ESDF地图.该框架首先将传感器数据集成到TSDF地图，它包含曲面截断半径内地距离信息，然后使用广度优先搜索算法从TSDF增量计算ESDF.与之相似的是Han等［17］提出的Fiesta框架，该方法以增量方式从占用栅格地图构建全局ESDF地图，通过引入两个独立更新队列分别插入和删除障碍，并使用索引数据结构和双链表对地图进行维护更新.Chen等［18］提出了一种基于GPU的地图构建系统，该系统在GPU上并行构建占用栅格地图和ESDF，利用并行的线程同时处理删除和插入障碍物操作.

满足小型无人飞行器在未知环境进行在线探索的主要挑战是以高效和准确的方式维护ESDF地图更新，虽然有不同的方法能动态构建ESDF地图，但是只有少数方法可以在仅有CPU平台式运行.针对机载端计算资源不足，导致在线建图速度慢、探索效率低的问题.本文在传统欧几里德符号距离场（ESDF）方法的基础上，融合光线跟踪原理，加速构建ESDF地图，在仅有CPU的计算资源下保证小型无人飞行器在复杂未知环境中的探索效率.首先使用整数运算提高光线跟踪遍历体素的速度，从而加速体素占用概率的更新；然后通过调整哈希数据结构，减少地图占用内存；最后使用广度优先搜索算法（BFS）实现地图更新与融合.通过数据集、仿真环境以及真实环境的测试实验，验证了该算法的可行性及适用性.本文的主要贡献如下：

（1） 提出了一种利用光线跟踪快速体素遍历的方法.

（2） 将快速体素遍历算法集成到小型无人机平台，提出了一种能够在CPU平台上快速构建ESDF地图的框架.

1 系统框架

构建ESDF地图的系统框架如图1所示，虚线部分为本文的工作范围.在地图构建过程中，系统首先从传感器中获取环境数据，其中深度图像信息可以通过单目深度估计或者机载传感器（如R-GBD深度相机、激光雷达）获得.位姿信息可以从外部设备GPS或者Vicon动作捕捉系统获得，也可以从内部位姿估计如VIO（视觉惯性里程计）获得.然后使用改进的光线跟踪方法将这些数据加速集成到占用栅格地图中，将占用栅格地图作为储存占用信息的数据结构.通过使用体素哈希方法来处理持续增长的地图［17］，最后用BFS算法将新的地图数据融合到现有地图，实现地图快速更新.

2 基于光线跟踪快速体素遍历方法

当前有两种主流的方法构建ESDF地图，一种是从TSDF表示的基础上逐步构建ESDF地图如voxblox；另一种是从占用栅格地图中增量构建ESDF地图，对于占用栅格地图，需将环境信息映射到一组由空闲或者占用状态表示的体素中，如Fiesta.其中voxblox基于TSDF采用的是投影距离，即沿传感器射线到测量表面的距离，容易高估到最近表面的实际欧几里德距离，而Fiesta使用占据栅格图，计算的是传感器到体素中心的真实欧几里德距离，地图构建精度更高.因此本文专注直接从占用栅格地图构建ESDF地图，其构建的步骤如下：

（1） 通过传感器观测获取深度点云信息.

（2） 传感器原点和点云中的每个点构成一条具有固定长度的单独光线.

（3） 对于所有光线，使用光线跟踪遍历体素，用以发现空闲和占用的体素.

（4） 将观测到的体素合并到占用栅格图中，并根据体素概率进行地图更新.

其中，在每次光线跟踪处理中，上述步骤2、3占其处理时间90%以上.针对该问题本文将利用加速体素遍历来缩短其光线跟踪的处理时间.

占用栅格地图通常将三维空间均匀分割为大小相等的正方体遍历三维空间中的体素，其中每个正方体即为一个体素.在三维空间中，一个体素与相邻的六个体素都有一个公共面，这种连接方式称为6连接.与之相似的是26连接，因为每个体素包含26个邻居（6个面连接，12个边连接和8个顶点连接）.光线跟踪的目的是为了遍历这些体素并且确定这些体素哪些是占用哪些是空闲.地图构建过程示意图如图2 所示，当相机发射一条光线，能穿过的中间体素标记为空闲（图中米黄色格），穿不过的即为占用（图中黑色格）.当进行体素遍历时，找到每条光线所在直线经过的体素，后续只需要对这些体素所包含的对象与直线进行求交点操作从而进行体素遍历.

对于体素遍历算法，首先考虑二维情况，为了正确遍历网格，算法必须按照如图3所示的顺序（a，b，c，d，e，f，j）访问体素，光线方程为：

式（1）中：v光线方向，p0和p1分别为光线的起点和终点，t为遍历一个体素的时间间隔.

遍历算法包括两个阶段：初始化和增量遍历.初始化阶段从光线原点开始，如果光线原点在网格之外，则找到光线进入网格的交点，并获取相邻的体素.二维体素遍历如图4所示.

在循環过程中，加入距离限制，dist为当前体素到光线原点的欧式距离，当这个距离大于maxDist（起点与终点的距离）时，跳出循环.从二维算法扩展到三维只需要适当添加Z分量即tIntZ，ΔtZ的值即可.

3 ESDF地图构建方法

3.1 占用栅格地图

本文通过占用栅格地图构建ESDF地图，占用栅格地图来存储体素的占用概率，当从传感器获取到新的深度信息时，占用栅格地图不断更新从光线跟踪获得的新的占用信息数据，对于每个体素，用p（s=1）表示为其空闲状态的概率，p（s=0）表示为其占用状态的概率.则两者的比值可以表示为该体素的状态.

3.2 地图数据结构

对于构建三维栅格地图如果内存够大且已知地图尺寸和分辨率大小，通常利用数组直接进行索引工作.但是在自主探索过程中，无人机要探索的区域往往是未知的，通常采用哈希表来进行体素索引工作，以减小内存消耗.但是单纯使用哈希表进行体素索引，由于其所需查找索引的数量巨大，导致其性能比使用数组的方式差.参考文献［19］利用哈希表和数组结合的方式索引一个体素坐标，通过生成一个8×8×8个体素组成的体素块，采用哈希表用于块管理.从体坐标计算得到块坐标后，哈希表用于查找相应的块，该块中所有体素的索引储存在相对于块的数组中.由于块哈希表的数量远少于体素哈希表的数量，因此哈希表用于块管理可以节省更多的内存，用于提高地图构建的性能.

3.3 地图更新

ESDF地图中每个地图都存储着距离最近障碍物的欧几里德距离，即对于空闲空间中的点坐标p（x，y，z），确定到所有障碍物b（i，j，k）的最近距离为：

ESDF地图更新示意图如图5所示，ESDF地图更新算法基于广度优先搜索算法.在图5（a）中，当obs1未插入时，体素V1的最近障碍物为obs0，此时插入一个新的障碍物obs1，则从该障碍物点的邻居开始搜索，当这个邻居V1与obs1的距离小于之前存储的最近距离（体素V1与其最近障碍物obs0的距离），则将存储的距离信息更新为新的最近距离，并且把该邻居作为生长点，搜索其下一个邻居并进行比较.如图5（b）中，当删除一个障碍物时，即障碍物obs1删除，把该点的距离设为无穷大，查找所有以obs1為最近障碍物的体素.若删除前V2的最近障碍物为obs1，则查找V2邻居的最近障碍物，用该邻居的最近障碍物作为体素V2的最近障碍物.例如，图5（b）中V2邻居中nbr的最近障碍物为obs0，即把obs0作为V2的最近障碍物，以此来更新最近欧几里德距离minDist（x，y，z）.

4 实验结果与分析

4.1 实验设置

为了验证所提算法的可行性与适用性，本文在机器人操作系统（ROS，Robot Operating System）中进行实验，该系统包含了视觉图像模块与地图构建模块之间的通信，并采用Rviz作为实时地图构建的显示平台.在同等测试条件下本文分别在公开RGB-D数据集、仿真环境和真实场景中进行实验，并将提出的方法与先进的ESDF地图构建方法Fiesta框架进行比较［17］.该系统运行在配备Ubuntu 18.04 64位操作系统，Intel Core i7-8750 （8核@2.20 GHz）和32 GB RAM的笔记本电脑上运行.实验所涉及到的基本仿真参数如表1所示.

4.2 实验结果

4.2.1 RGD-B数据集分析

本次实验采用的是苏黎世联邦理工学院自主系统实验室（ASL）的公开数据集cow_and_lady，该数据集是一个具有地面实况的小型数据集，拍摄的是一个室内办公室环境，包含一头奶牛，一个人体模型和一些其他典型的办公配件.数据集包含彩色RGB-D点云数据和位姿信息.cow_and_lady数据集实验的原始场景及两种方法实时构建的ESDF地图如图6所示，地图更新时间对比如图9所示，在同等条件测试下Fiesta的方法平均地图更新时间为87.63 ms，而本文的方法单次平均地图更新时间仅为23.89 ms，本文地图构建速度比Fiesta的方法快了3倍左右.

4.2.2 仿真环境实验

本文采用Rotors仿真环境，其中包含一个搭载双目深度相机的六旋翼无人机.仿真环境由Gazebo构建，提供对外接口，能够通过ROS获取深度图像数据和无人机位姿信息.本次实验利用该仿真环境，在Gazebo构建尺寸为8 m×8 m×3 m的室内地图，通过键盘控制无人机飞行.Rotors仿真环境试实验环境及ESDF地图对比图如图7所示，其中图7（a）为仿真环境图，图7（b）、（c）分别为本文算法实时构建的ESDF地图和Fiesta构建的地图，地图更新时间对比如图9所示.在相同条件测试下Fiesta的方法单次平均地图更新时间为35.44 ms，而本文的方法平均地图更新时间仅为13.89 ms，本文地图构建速度比Fiesta的方法快2倍左右.

4.2.3 真实环境场景实验

对于真实场景实验，实验采用英特尔RealSense d435i立体视觉深度相机，用以采集地图构建所需的深度图像，相机采集频率为30 Hz，深度测量范围5 m，所获深度图像分辨率为640×480，并且通过vins_fusion［20］框架进行视觉定位，用来获取相机的位姿.本次实验的场地为一条尺寸为40 m×5 m×3 m的走廊.真实场景实验及构建的ESDF地图对比图如图8所示，其中图8（a）为真实环境如，图8（b）、（c）分别为本文实时构建的ESDF地图和Fiesta构建的地图.地图更新时间对比如图9所示，在同等条件测试下Fiesta的方法平均地图更新时间为12.61 ms，而本文的方法单次平均地图更新时间仅为5.42 ms，本文地图构建速度比Fiesta的方法快2倍左右.

图9为对三次不同场景、两种算法下ESDF地图平均更新时间的性能对比结果，图中簇状图为地图平均更新时间，折线图为本文所提算法与Fiesta方法在相同环境下地图平均更新时间的降低比率.从图9可以看出，在数据集、仿真环境和真实场景中，本文所提出算法的平均地图更新时间分别比Fiesta方法减少了72.37%、60.80%和56.79%.多个场景的实验结果表明，本文所提出的算法可以显著降低实时构建ESDF地图的时间，具有一定的可行性和高效性.

为了验证真实环境场景下不同相机采样频率对建图精度的影响，进行了不同相机频率下的真实环境建图实验.实验中，深度相机以2 m/s的速度匀速移动，采样频率分别设置为20 Hz、30 Hz、40 Hz、50 Hz和60 Hz，平均地图更新时间控制在6 ms内.通过在建图结果的长边和宽边分别随机选取9个测量点，计算平均值后与实际尺寸对比以评估建图精度.不同相机频率下，论文所提方法的地图构建精度对比结果如图10所示，其中，柱状图为不同相机采样频率下建图的长边长度，折线图为宽边的建图长度.实验结果与实际场景尺寸对比可知，相机的采样频率与建图误差之间没有明显的对应关系，论文所提出建图方法在实际场景下的建图误差较小，40 m真实环境的建图误差小于0.2 m，5 m真实环境的建图误差小于0.1 m.

对于本文方法构建的ESDF地图，本文还测量了在仿真环境及真实场景环境下地图的长宽距离来计算其建图精度.仿真与真实场景分别进行了7次实验，每次实验测量了地图的长宽距离来计算其建图精度，其中长宽的计算为每条边各取9个测量点所测算的平均距离.仿真环境与真实环境下地图精度误差对比图如图11所示.从图中比对结果可以看出，不论在仿真环境或者真实场景下本文方法与传统Fiesta方法在精度偏差分别能够控制在0.06 m及0.12 m以内，两个建图方法在精度上基本能保持一致.

从实验可以看出在保持地图分辨率相同时，本文方法在提高建图速度后对于建图的精度并没有明显降低.为了满足无人机自主探索实时性的要求，对于当前构建ESDF地图的精度误差是可以接受的.因为对于大部分自主规划算法来说，为了保证飞行过程中的安全性，都会适当的把障碍物进行膨胀处理.而且ESDF地图又适用于基于梯度优化的方法，该方法能够利用障碍物的距离信息，把飞行轨迹推离障碍物，从而保证了飞行安全性.

5 结论

针对机载端计算资源不足，导致在线建图速度慢、探索效率低的问题.本文在传统欧几里德符号距离场方法的基础上，融合光线跟踪原理，加速构建ESDF地图，在仅有CPU的计算资源下保证小型无人飞行器在复杂未知环境中的探索效率.首先使用整數运算提高光线跟踪遍历体素的速度，从而加速体素占用概率的更新；然后通过调整哈希数据结构，减少地图占用内存；最后使用广度优先搜索算法实现地图更新与融合.通过数据集、仿真环境以及真实环境的测试实验，证实本文所提出算法的平均地图更新时间分别比Fiesta方法减少了72.37%、60.80%和56.79%，通过精度对比实验也可以看出本文在减少地图构建时间后相较于传统Fiesta方法还能不降低的地图构建精度，充分验证了算法的可行性与高效性.

参考文献

［1］ Xu Z，Deng D，Shimada K.Autonomous UAV exploration of dynamic environments via incremental sampling and probabilistic roadmap［J］.IEEE Robotics and Automation Letters，2021，6（2）：2 729-2 736.

［2］ Krátky＇ V，Petrácˇek P，Bácˇa T，et al.An autonomous unmanned aerial vehicle system for fast exploration of large complex indoor environments［J］.Journal of Field Robotics，2021，38（8）：1 036-1 058.

［3］ Zhao Y，Yan L，Chen Y，et al.Robust and efficient trajectory replanning based on guiding path for quadrotor fast autonomous flight［J］.Remote Sensing，2021，13（5）：972.

［4］ Hornung A，Wurm K M，Bennewitz M，et al.OctoMap：An efficient probabilistic 3D mapping framework based on octrees［J］.Autonomous Robots，2013，34（3）：189-206.

［5］ Chen J，Liu T，Shen S.Online generation of collision-free trajectories for quadrotor flight in unknown cluttered environments［C］//2016 IEEE International Conference on Robotics and Automation （ICRA）.Stockholm，Sweden：IEEE，2016：1 476-1 483.

［6］ Ji J，Wang Z，Wang Y，et al.Mapless-planner：A robust and fast planning framework for aggressive autonomous flight without map fusion［C］//2021 IEEE International Conference on Robotics and Automation （ICRA）.Xi′an，China：IEEE，2021：6 315-6 321.

［7］ Florence P R，Carter J，Ware J，et al.Nanomap：Fast，uncertainty-aware proximity queries with lazy search over local 3d data［C］//2018 IEEE International Conference on Robotics and Automation （ICRA）.Brisbane，QLD，Australia：IEEE，2018：7 631-7 638.

［8］ Ratliff N，Zucker M，Bagnell J A，et al.CHOMP：Gradient optimization techniques for efficient motion planning［C］//2009 IEEE International Conference on Robotics and Automation.Kobe，Japan：IEEE，2009：489-494.

［9］ Wu Z，Meng Z.Safe polynomial trajectory generation for quadrotor flight in dense environments［M］.Singapore：Springer Singapore，2022.

［10］ Woods A C，La H M.A novel potential field controller for use on aerial robots［J］.IEEE Transactions on Systems，Man，and Cybernetics：Systems，2017，49（4）：665-676.

［11］ Klancˇar G，Seder M，Blazˇicˇ S，et al.Drivable path planning using hybrid search algorithm based on E* and bernstein-bézier motion primitives［J］.IEEE Transactions on Systems，Man，and Cybernetics：Systems，2019，51（8）：4 868-4 882.

［12］ Quan L，Zhang Z，Zhong X，et al.EVA-planner：Environmental adaptive quadrotor planning［C］//2021 IEEE International Conference on Robotics and Automation （ICRA）.Xi′an，China：IEEE，2021：398-404.

［13］ Zhou B，Gao F，Pan J，et al.Robust real-time uav replanning using guided gradient-based optimization and topological paths［C］//2020 IEEE International Conference on Robotics and Automation （ICRA）.Paris，France：IEEE，2020：1 208-1 214.

［14］ Tang L，Wang H，Li P，et al.Real-time trajectory generation for quadrotors using b-spline based non-uniform kinodynamic search［C］//2019 IEEE International Conference on Robotics and Biomimetics （ROBIO）.Dali，China：IEEE，2019：1 133-1 138.

［15］ Zhou B，Gao F，Wang L，et al.Robust and efficient quadrotor trajectory generation for fast autonomous flight［J］.IEEE Robotics and Automation Letters，2019，4（4）：3 529-3 536.

［16］ Oleynikova H，Taylor Z，Fehr M，et al.Voxblox：Incremental 3d euclidean signed distance fields for on-board mav planning［C］//2017 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）.Vancouver，BC，Canada：IEEE，2017：1 366-1 373.

［17］ Han L，Gao F，Zhou B，et al.Fiesta：Fast incremental euclidean distance fields for online motion planning of aerial robots［C］//2019 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）.Macau，China：IEEE，2019：4 423-4 430.

［18］ Chen Y，Lai S，Wang B，et al.A GPU mapping system for real-time robot motion planning［C］//2021 IEEE International Conference on Real-time Computing and Robotics （RCAR）.Xining，China：IEEE，2021：762-768.

［19］ Niener M，Zollhfer M，Izadi S，et al.Real-time 3D reconstruction at scale using voxel hashing［J］.ACM Transactions on Graphics （ToG），2013，32（6）：1-11.

［20］ Qin T，Li P，Shen S.Vins-mono：A robust and versatile monocular visual-inertial state estimator［J］.IEEE Transactions on Robotics，2018，34（4）：1 004-1 020.

【責任编辑：蒋亚儒】

基金项目：国家自然科学基金项目（61963038）

作者简介：唐嘉宁（1984—），女，云南昆明人，教授，博士，研究方向：多无人机协同、SLAM定位建图

通讯作者：陈云浩（1991—），男，山东济南人，讲师，博士，研究方向：视觉SLAM、路径规划，cyh_619@163.com