基于激光雷达感知的无人机自主避障3DVFH改进算法.pdf

2023 年 8 月第 4 期 现代导航 297 基于激光雷达感知的无人机自主避障 3DVFH 改进算法 吉彦蓉，袁子雄（中国电子科技集团公司第二十研究所，西安 710068）摘 要：传统无人机避障算法通过视觉传感器基于八叉树地图（Octomap）构建全局地图，会产生较大存储量，难以满足工程需要。基于三维向量直方图（3DVFH）做出改进，只需构建局部地图，但该方法不考虑先前的数据或操作，易造成行为不稳定和局部最小值。针对以上问题，提出基于增强 3DVFH的自主局部避障算法，将全局地图直接替换为使用激光雷达提供的三维点云地图，同时设计一种价格较低的计算内存策略来减轻局部方法的固有问题。经试验验证，有效地提升了环境感知的效率与存储量，有助于更好地进行无人机自主避障。关键词：激光雷达；无人机；三维向量直方图；环境感知；自主避障 中图分类号：TN974 文献标志码：A 文章编号：1674-7976-(2023)-04-297-06 Improved 3DVFH Algorithm for Autonomous Obstacle Avoidance of UAV Based on LiDAR Perception JI Yanrong,YUAN Zixiong Abstract:Traditional obstacle avoidance algorithm of Unmanned Aerial Vehicle(UAV)constructs the global map based on Octomap by visual sensor,which will produce large storage and is difficult to meet the engineering needs.The improved algorithmbased onThree-Dimensional Vector Histogram(3DVFH)only needs to construct a local map,but this method does not consider previous data or operations,which can easily lead to unstable behavior and local minima.Regarding the above issues,an autonomous local obstacle avoidance algorithm based on enhanced 3DVFH is proposed,which directly replaces the global map with a 3D point cloud map provided by LiDAR,and a low-cost computational memory strategy to alleviate the inherent problems of local methods is designed.Through experimental verification,it has effectively improved the efficiency and storage capacity of environmental perception,which is conducive to better autonomous obstacle avoidance for UAV.Key words:LiDAR;Unmanned Aerial Vehicle;Three-Dimensional Vector Histogram;Situational Awareness;Autonomous Obstacle Avoidance 0 引言 随着无人机应用越来越广泛，作为保证无人机 收稿日期：2023-04-21。吉彦蓉（1994.07），甘肃兰州人，硕士，主要研究方向为雷达、无人机避障、信息化弹药。安全飞行的核心，无人机避障技术受到广泛的 研究1。无人机在飞行过程中，由于传感器的处理、机载芯片/板卡的计算和飞控的响应等均存在相应的时延，因此快速地对环境进行感知，获取场景中障碍物的信息，提升自主避障技术的算法效率与存储效率成为研究的重点。现在许多无人机采用视觉传感器进行感知避 298 现代导航 2023 年 障，如大疆的精灵 4 等2，视觉传感器能快速识别障碍物，但受光照影响容易被干扰，同时作用距离较短，难以满足作用环境复杂时无人机的需求。激光雷达能够实时扫描环境特征，绘制三维点云地图等功能，且激光雷达探测距离及角度分辨率都比较高，相较于视觉传感器，有着更远的感知距离与更稳定的感知效率，随着激光雷达传感器越来越普及，关于激光雷达的避障研究也越来越多3-4。1 三维向量直方图算法 目前主流的无人机避障算法有 A*算法5和快速扩展随机树（Rapidly-exploring Random Tree，RRT）算法6，这两种算法均需要构建全局地图，对存储性能要求较高。但当无人机无法提前获取全局地图时，这些算法均不适用。三维向量直方图（Three-Dimensional Vector Histogram，3DVFH）自主避障算法由 Vanneste 等人引进7，Vanneste 以八叉树栅格地图（Octomap）8形式构建场景的三维全局地图，判断这些栅格点与无人机自身的位置关系，选取无人机周围需要进行避障的栅格点，建立二维直方图，使用直方图进行无人机自主避障。3DVFH 算法使用无人机周围边界框内的八叉树栅格绘制一个边界球，如图 1 所示。对于栅格地图中的每个 3D 点，计算其相对于无人机位置的距离、方位角和俯仰角，使用该距离、方位角与俯仰角构建二维极坐标直方图，边界球内的 3D点均被置于相对应的直方图的单元格中。通过试验得到 3D 点数量的阈值，将每个直方图单元格中的点数与阈值进行比较，把直方图转换为二进制极坐标直方图。最后在直方图中搜索可用路径，根据避开障碍物的方位角、俯仰角和无人机自身旋转角度，设定权重系数，路径权重值最低的路径为避障路径，即可绕开障碍物9-10。二维极坐标直方图方位角俯仰角图 1 构建二维极坐标直方图 2 改进 3DVFH 算法 3DVFH 算法可以访问全局地图的主要优势是该算法会记住以前存在但现在可能不在视野范围内的障碍物，但构建全局地图也会引入大量计算，与此同时若仅依赖局部避障，不考虑先前时间步长中的任何数据或操作，沿着较大的障碍物（例如墙壁）滑行等情況会造成避障行为不稳定等问题。因此，在本文中，将 3DVFH 方法改进为纯局部算法而无需构建全局地图。全局地图被直接替换为使用激光雷达提供的 3D 点云，同时建立局部直方图存储器，与新直方图组合来提升原算法避障不稳定和存储量过大的问题。该策略存储每个时间步长内计算的最终组合直方图，以便在随后的时间步长中可用，如图 2 所示，共包括四个步骤：第一步，将这个旧直方图的数据重投影到 3D 点中；第二步，从那些重新投影的 3D 点以一半的分辨率构建记忆直方图，然后，此记忆直方图将被上采样到常规分辨率；第三步，从新的激光雷达 3D 点云数据计算当前时间步长的新直方图；最后，将新的直方图和记忆直方图融合在一起，成为当前时间步长的组合直方图。该组合的直方图可用于确定可能的行进方向并在障碍物周围导航。年龄层距离层二进制层年龄层距离层二进制层年龄层距离层二进制层年龄层距离层二进制层年龄层距离层二进制层旧的直方图重投影的3D点激光雷达3D点云记忆直方图上采样记忆直方图新的直方图组合直方图 图 2 改进 3DVFH算法策略 年龄层距离层二进制层年龄层距离层二进制层上采样年龄层年龄层 距离层距离层 二进制层二进制层二进制层距离层年龄层二维极坐标直方图 方位角 俯仰角 第 4 期 吉彦蓉等：基于激光雷达感知的无人机自主避障 3DVFH 改进算法 299 2.1 直方图重投影 通过使用来自上一个时间步长的数据来增强建立当前时间步长的直方图，存储的旧数据是以直方图形式显示的，其中旧直方图是以上一个时间步长观测到的障碍物的极坐标形式表示。由于障碍物的角度方向取决于无人机的位置，因此将旧的直方图与无人机先前的位置进行匹配，直至使用旧直方图的数据进行当前时间步长的计算时，才需要将其转换为当前无人机位置的直方图。本文通过将旧直方图中的已占用单元格重新投影到 3D 点中，然后从这些 3D 点构建记忆直方图来实现转换。为了获得重新投影的 3D 点，将扫描旧的直方图以查找被占用的单元格。这些被占用的单元格都包含一个二进制层，二进制层存储历史二维极坐标直方图，该二维直方图通过方位角与俯仰角构建。除二进制层外，与无人机的距离信息还存储在旧直方图的距离层中，直方图对应的历史时间信息存储在年龄层中。如果占用的单元格的历史时间未超过定义的限制，则将历史单元格重新投影到与单元格角点相对应的四个 3D 点),(4321PPPP中。此过程如图 3所示。重投影点旧的位置Poldd 图 3 旧直方图已占用像元对应重投影点 通过加/减被占用单元格的俯仰角和方位角中直方图的角分辨率的一半来计算四个角点的方位角与俯仰角，然后可以将这四个点重新投影到 3D 空间中，如式（1）所示 2222222244332211 （1）2.2 从 3D 点建立直方图 为了获得环境的直方图，首先需要将 3D 点转换为二维极坐标直方图，其中每个单元格都包含位于相应扇区中的 3D 点的数量。对于每个 3D 点，根据式（2）和式（3）计算从无人机当前位置看到的方位角与俯仰角),(2tan180yyxxpospposp （2）22)()(2tan180yyxxzzposppospposp（3）式中：xp、yp与zp分别代表记忆直方图中存储的x、y、z 坐标；xpos、ypos、zpos表示当前无人机 位置坐标。确定 3D 点的角度后，需要将其转换为直方图索引。计算的方位角和俯仰角的角度范围分别为180,180和90,90。首先，这些角度需要转换为正范围360,0和180,0。从这些正角度，可以根据式（4）和式（5）计算 1)(index （4）1)(index （5）式中：index和index表示直方图索引对应的正角度；是直方图的分辨率。在获取用于避障的组合直方图的过程中，此过程被两次使用：根据重新投影的 3D 点获取记忆直方图，并从激光雷达提供的 3D 点云中计算新的直方图。在这两种情况下，二维极坐标直方图都以相同的方式计算。一方面，对于新的直方图，如果至少 1 个 3D 点落入二进制图层中，则将二进制图层中的单元格设置为已占用，距离层中的相应值设置为落入该单元格中的所有点的距离的平均值。年龄层中的值始终设置为 0，因为此直方图是根据新数据生成的。另一方面，以所有其他直方图的一半分辨率生成记忆直方图，然后进行上采样。如果至少有 6 个点落入二进制层，则该单元将被视为已占用。重投影点 旧的位置 Pold d P1P2P3 P4 300 现代导航 2023 年 如果将旧直方图的每个单元划分为 4 个 3D 点，则6 是唯一允许的值。如果选择的阈值低于 6，则记忆直方图的占用单元数将多于旧直方图。这导致组合直方图收敛到完全占据的直方图。相反，如果选择的阈值太大，则记忆直方图将比旧直方图占用更少的单元。记忆效应将被最小化，尤其是障碍物的边界将被忽略。最小点数的阈值如图 4 所示。旧的位置Pold新的位置Pnew 图 4 最小点数的阈值 因此，阈值的最佳选择是最大数量 6，它不会导致收敛到完整的直方图。对于距离层，使用与新直方图相同的方法。因此，距离值被定义为单元格箱中所有点的平均距离。年龄值也被确定为占据单元格中所有点的平均年龄。以一半的分辨率创建记忆直方图后，将对其进行上采样以匹配新直方图的完整分辨率。分辨率的变化使得该方法不易受到离散误差的影响。2.3 结合记忆直方图和新直方图 在上一步中，记忆直方图和新直方图都需要合并为最终直方图。最终的直方图存储用于下一次计算。为了组合两个直方图，区分了两个不同的区域：位于无人机当前视场角（Field of View，FOV）内部的单元格与 FOV 外部的单元格。从无人机的方位角和俯仰角以及传感器作用范围可以计算视角内的直方图区域。使用的激光雷达的垂直视角为46，水平 FOV 为 120。因为是以非常保守的方式选择 FOV，新的直方图有可能占据了 FOV 之外的单元格。它标记了新数据受信任的区域，不仅包含障碍物的占用的单元，而且包含空的单元，另一方面，FOV 之外未被占用的单元格通常是由于缺乏数据而不是有缺乏空闲单元的知识所致。对于 FOV内的直方图单元，将复制新的直方图，优先于记忆直方图。对于位于 FOV 之外的所有单元，将“或”运算用于二进制层。在当前直方图中，如果相应的单元格标记为已占用，则年龄和距离层将从新的直方图中获取。当前直方图中单元格未被占用而记忆直方图中标记为已占用的的情况下，年龄和距离层的值会从记忆直方图中被复制。组合新直方图和内存直方图的示例如图5所示，黄色区域标记了FOV。新的直方图记忆直方图二进制层距离层年龄层二进制层距离层年龄层组合直方图 图 5 建立组合直方图 2.4 权重路径选择 最后需要在组合直方图中搜索可用的路径，并选择权重最小的路径。为了确定可用路径，通过二维二进制直方图设置一个滑动窗口来检测直方图中的开放路径。如果滑动窗口中所元素均设置为0，则该窗口为可通过路径。在实现该路径搜索算法时，需要在极坐标直方图内考虑极性属性。由于三维世界的连续性，直方图的边界也需要进行判定，如图 6 所示。图 6 滑动窗口设置 确定了所有可用路径后，通过权重来计算最佳路径。具体而言，设定三个路径权重：第一路径旧的位置 Pold 旧的位置 Pnew 第 4 期 权重设置为设置为无人重设置为第方向的角度式中，1、用场景设置的旋转运动目标位置，将3 试验验为验证人机上进行光雷达传感卡集成本文具体参数如传无激光TX 进行飞生成的 Octo感知生成的图 8 中椭圆存储显示很为基于目标方人机所需旋转第 i 时刻的选度差。如式1 tk2和3作置为不同的值动而非攀爬运将1、2和验证 证本文算法的行试飞验证，感器用于感知文算法，板卡如表 1 所示。图 7 表 1 无传感器 无人机 光雷达 X2 板卡 飞行测试，感omap 全局地的局部障碍地圆区域。如图很少的场景信图 8 Octom吉彦蓉等：方向的角度tk转的角度差择方向与第式（6）所示32为权重系数可值，例如有时更运动（本文未更3分别设置有效性与适用如图 7 所示场景 3D 点云卡操作系统为 卓翼无人机 无人机参数 型卓翼镭神Ubunt感知场景信息地图如图 8 所示地图如图 9 所示8 和图 9 易得息。局部局部区区map全局感知地基于激光雷达；第二路径权；第三路径i+1 时刻的候 （可以根据不同更倾向与无人更快速地移动置为 5、2 和 2用性，在卓翼。无人机配备云，搭载 TX为 Ubuntu18.0 型号 翼 680 神 C16 tu18.04 息，3DVFH 感示，增强 3DV示，局域位置得本文算法仅区区域域 地图 达感知的无人机权重径权候选（6）同应人机动至）。翼无备激2 板04。感知VFH置在仅需图头表图程中性，局地时地升。全机自主避障 3DV图同时需要对10 所示，图表示无人机的（b）为避障中，依旧可以图随后，为了在有效飞行地图的算法效地图自主避障 表 2算法 本算法 全局地图VFH 改进算法 图 9 改进 3DV对增强算法中10 中红色曲的移动方向，障途中的某一以存储局部障（a）初始（b）避障途中的图 10 局部障碍了进一步验证行 30 s 的情况效率，如表 2障方面存储量 局部障碍地存储量/MB 100 1 044.48 VFH感知地图 中的存储器进曲线为无人机图（a）为一时刻，在无障碍物信息。始时刻 的某一时刻 碍物存储器 证本算法自主况下，对比了2 所示。易得量与计算量地图算法效率计算0.00.3301 进行验证，如机的航迹，箭为初始时刻，无人机运动过 主避障的有效了本算法与全得本算法在实均有显著提率 算速率/s 45 194 41 412 如箭过效全实提 302 现代导航 2023 年 4 结语 经试验验证，采用本文提出的改进算法，通过建立局部直方图记忆存储器，并使用激光雷达感知的 3D 点云作为全局地图，能有效地生成无人机自主感知的局部障碍物地图，满足无人机飞行过程中自主避障的需求，并在无人机运行的过程中有效地减少了存储量与计算量，有利于提高无人机自主避障与路径规划效率。参考文献：1 Bachrach A，He R J，Roy N.Autonomous flight in unknown indoor environmentsJ.International Journal of Micro Air Vehicles，2009，1（4）：217-228.2 赵航，王立峰.基于 pixahwk 的多旋翼无人机避障飞行系统研发J.动力系统与控制，2017，6（3）：11.3 章志诚，杜昌平.基于激光雷达的多旋翼飞行器实时避障系统J.计算机测量与控制，2016，24（9）：117-121.4 张大伟，王佳，孟森森，等.基于单目视频路径移动机器人导航方法研究J.传感器与微系统，2018，37（2）：4.5 Yang F，Fang X，Gao F，et al.Obstacle Avoidance Path Planning for UAV Based on Improved RRT AlgorithmJ.Discrete Dynamics in Nature and Society，2022.6 潘富强，曾成，马国红，等.一种融合改进 A*算法与改进动态窗口法的 AGV 路径规划J.传感技术学报，2023，36（1）：10.7 Vanneste S，Bellekens B，Weyn M.3DVFH+：Real-Time Three-Dimensional Obstacle Avoidance Using an OctomapC.Morse，2014：1319.8 Hornung A，Kai M W，Bennewitz M，et al.OctoMap：an efficient probabilistic 3D mapping framework based on octreesJ.Autonomous Robots，2013.9 Xin G，Jia Q，Sun H，et al.Real-time dynamic system to path tracking and collision avoidance for redundant robotic armsJ.The Journal of China Universities of Posts and Telecommunications，2016（1）：73-85+96.10 Tsai，C.P，Chuang，et al.Machine-vision based obstacle avoidance system for robot systemC.IEEE International Conference on System Science&Engineering，2013：273-277.