徐静　苑征　梁寅博　甄泽安

关键词：向量场直方图；动态避障；速度规划；无人机；局部路径规划

中图分类号：V249.1 文献标识码：A

文章编号：1009-3044（2023）12-0095-05

0 引言

随着人工智能技术的发展，无人机的自主路径规划能力也变得越来越高，其中避障问题一直是无人机领域的热点研究问题。由于多旋翼无人机成本较为低廉，飞行震动小，操作较为简单本文实验中采用的是以四旋翼无人机作为对象，进行动态避障算法的应用研究。

路径规划算法是无人机在执行任务时，首先需要解决的问题。它需要考虑在三维地理空间以及自身动力学约束的条件下，规划出从起始点到目标的安全线路，保证无人机不与三维地理空间中的障碍物进行碰撞，同时还需要保证飞行姿态的平稳，以及飞行轨迹的平滑。目前路径规划算法可以分为两类即全局路径规划和局部路径规划，全局路径规划算法有Dijk?stra[1]，A*算法[2-3]，D*算法[4]，D* Lite算法[5]等，由于全路路径规划需要掌握所处三维地理空间的所有信息，或者在运行时通过传感器数据构建一个全局三维地理空间地图，该三维空间地图是由数个大小一致的正方体组成，随后对地图进行图搜索找出可通行轨迹，它随着空间维度的上升计算量也随之陡增，全局路径规划比较适用于已知的三维地理空间；局部路径规划也有很多成熟的算法，如人工势场法[6]、虚拟力场法[7]、VFH[8]，模糊混合势场法[9]等，局部路径规划无须对三维空间进行全局建图，它是通过深度相机、激光雷达等传感器实时感知无人机周围的飞行环境进行局部建图，解析出障碍物位置距离等信息，找到一条当前位置到下一目标位置无碰撞的最优轨迹[10]，局部路径规划更加适合于未知的三维地理空间。本文选用局部路径规划算法来进行研究。

1 系统的总体设计

无人机自主避障系统总体设计如图1所示，它是由四旋翼无人机飞行平台、基于ROS导航与避障分系统、飞行控制分系统、数据传输分系统以及任务载荷分系统五部分组成。

1.1 动态避障算法设计

动态避障算法属于导航与避障分系统，它是基于ROS机器人操作系统平台。由于无人机在室内或室外飞行面临着动态环境变化，本文选用局部路径规划算法，其中向量场直方图法（VFH）在局部避障算法中表现出良好的性能[11]。VFH是Borenstein J.提出的理论，该算法的提出主要是解决人工势场法在实际应用中数据突变导致环境信息丢失的问题[12]。若无人机具备自主避障能力，对于存在障碍物的飞行区域或者飞行过程中遇到突然出现障碍的情况，能够保障无人机继续执行飞行任务，从而有效保障无人机的飞行安全和任务执行能力，提高了无人机对飞行环境的适应能力。

本文针对无人机在未知三维地理空间下局部路径避障问题，采用二维激光雷达建立局部直方图地图，并进行二值化处理，通过代价函数求出代价最小的可通行区域，得出无人机飞行方向，为了在避障的同时解决过冲震荡、姿态不稳以及飞行效率等问题，提出一种多场景自适应的速度规划控制器，通过条件判定是否遇到障碍物调用相应的速度规划控制器规划出无人机飞行的速度，在板外控制的模式下，实时发送给无人机的飞行控制系统，实现对无人机实时水平速度控制以及垂直位置控制，保证无人机在避障同时提高飞行效率和飞行姿态的稳定性。

1.2 激光雷达

单线激光雷达主要用于障碍物避障，多线激光雷达主要应用于雷达成像，相对单线激光雷达多线激光雷达可以识别物体的高度信息，多线激光雷达需要由多个发射器和接收器组成，通过转动可以获取多条线束，处理的数据量越大，硬件要求也更高。由于成本问题在实机飞行实验的激光雷达采用的是上海思岚科技公司RPLIDAR S1，它是单线的二维激光雷达，它的测距核心是顺时针旋转水平扫描范围0°～360°，测量分辨率可达到3cm左右，最大测距白色物体40m，黑色物体10m，角度分辨率为0.5°，扫描一圈得到720 个点。它的基本原理是激光雷达传感器首先通过发射器发出激光雷达信号，发出去的激光雷达遇到障碍物时就会被接收器检测到信号，从而根据三角测距原理测出距离[13]。TOF 激光雷达测距示意图如图2 所示：

RPLIDAR S1通过USB接入板载计算单元作为通信接口，内部处理器接收激光雷达信号，给出相應测距数据[14]，板载计算单元需要安装rplidar_ros功能包驱动，获取激光雷达扫描障碍物距离的实时数据，该数据可以用于建立无人机周围环境的局部地图。激光雷达安装在无人机的正前方，使得无人机在飞行过程中实时探测障碍物并且进行动态避障。

2 VFH 算法

2.1 数据预处理

在无人机飞行过程中，利用激光雷达探测360度障碍物信息，本文按照每个扇区是30度，划分为12个扇区，由于激光雷达的精度是0.5，每个扇区有60条激光雷达的测距信息，设定检测距离范围[dmin，dmax]，若测试距离di不在测距范围内，认为该点未探测到障碍物，同时标记距离di为零，若测试距离di在测距范围内，认为该点探测到障碍物，同时标记距离di为雷达探测最大距离dmax减去扫描距离di，依次累加计算每个扇区的距离总和作为该区的柱状高度，每个扇区的值越小，代表越安全。

利用传感器探测障碍物示意图如图3所示，无人机在飞往目标点的过程中，探测到障碍1和障碍物2；二维直方图示意图如图4所示其中横坐标表示是以无人机为中心感知到的周围各个角度范围（顺时针方向为正），纵坐标表示在该方向存在障碍物的密度值：障碍物距离越近密度越大，障碍物距离越远密度越小。

2.2 二维直方图二值化

上一小节中得到二维直方图的数值较为杂乱，没有一个确定的标准来判定直方图中哪些方向是安全的，哪些方向是危险的，不利于算法选择可通行方向。因此需要对直方图进行二值化，从而简化信息，并最终区分出二维直方图中的安全方向和不安全方向。之后依据无人机飞行的安全半径等影响因素，从二维直方图中找出连续的安全区域，并作为备选的可行方向。本算法通过设定两个阈值T_high和T_low与二维直方图中的每个扇区内的密度值H_k进行比较，若权重H_k<T_high，则H_k设为0，表示该方向安全；若H_k>T_high，则H_k设为2，表示该扇区危险，不可通行;若权重满足T_low≤H_k≤T_high ，则表示当前扇区方向危险还不确定[15]，H_k设为4。二值化每个扇区内的密度值H_k表示为（1） ：

2.4 确定飞行速度

在确定可通行飞行方向后，需要确定无人机的飞行速度，避障时候通常采用较低的恒定速度Vg，将无人机当前的飞行速度Vg进行x和y轴的速度分解，赋值给MAVROS系统话题-/mavros/setpoint_raw/local发送给PX4飞行控制系统，此话题支持发送期望位置，期望速度，加速度，航向角以及航向角速率给飞控从而进行控制，如果同时有位置，速度，加速度中两项指令给出，那么后者是前馈项。比如同时给出位置和加速度制指令，由于PX4是属于串级PID控制，那么加速度是前馈项，本文水平方向采用的是速度控制，垂直方向采用的位置控制方式。

2.5 多场景自适应速度规划控制器

当采用较低的恒定速度，在没有障碍物的时候飞行效率就非常低，为了解决在避障的同时提高飞行效率以及飞行速度过快容易引起过冲震荡以及姿态不稳等问题，本文提出一种多场景自适应速度规划控制器，分为无人机检测到障碍物和没有检测到障碍物两种情形分别进行速度规划。

为了解决速度过快引起过冲震荡和姿态不稳等问题，不再采用单一的速度控制，而是采用三段式的梯形速度规划算法，设定加速和减速距离，（3） 和（4） 中的参数可以根据无人机本身的加速度，最大飞行速度等参数性能以及实际飞行场景进行调整。此次实验中加速距离和减速距离分别设定20m，飞行最大速度设定Vg为5m/s，避障速度设置为0.5m/s，k值取0.3避免在刚开始起飞的时候速度过小，a取值0.8为了使无人机减速到预设速度。

3 仿真实验

3.1 实验平台搭建

为了能够精准而高效地模拟在复杂室内和室外环境无人机飞行，Meyer J 等人使用ROS与Gazebo 环境进行了四旋翼无人机综合仿真[18]。本文实验平台搭建需要在VMWARE虚拟机中安装Ubuntu 18.04，并下载PX4的源码然后运行ubuntu.sh用于安装所有的PX4所需要的工具链，工具链安装完成后运行仿真测试验证环境是否安装成功，若运行make px4_sitl_de?fault gazebo成功，则PX4和gazebo仿真搭建成功；继续在Ubuntu 18.04下安装ROS Melodic机器人操作系统以及MAVROS软件功能包，安装完可以进行ROS以及MAVROS启动测试是否安装成功，若运行roslaunchposix_sitl.launch成功，表示ROS仿真环境安装成功。最后下载QGC地面站安装包进行仿真测试验证，PX4 和gazebo 仿真环境运行成功后，运行地面站控制系统，地面站将自动连接上飞控系统。

3.2 仿真平台架构

在Gazebo仿真平台中，搭建了一套无人机飞行仿真环境，主要完成了仿真环境内无人机和激光雷达的搭建和配置，为了使得仿真环境中无人机和激光雷达在对外接口、输出数据等方面和现实的硬件基本一致，本实验采用PX4+Gazebo+ROS+MAVROS+QGC 的软件在环（SITL）仿真测试，其中PX4+Gazebo+QGC 仿真架构图[19]如图5。

由图5软件在环仿真架构图可以看出PX4飞控软件由仿真器中获取传感器数据如IMU数据、磁罗盘、GPS、气压计以及激光雷达等，通过飞控软件解算得到控制力矩给仿真器，仿真器根据无人机动力模型再次解算出仿真数据反馈给飞控软件，这样使得PX4与仿真器形成了输入与反馈的闭环回路。地面站软件控制系统由PX4飞控软件系统中获取飞行信息、传感器信息以及无人机的姿态信息，同时地面站软件系统可以给飞控软件系统下发无人机控制信息，包括起飞、解锁、着陆、航迹规划、飞行模式变换、飞行速度以及地理围栏控制等指令。版外模式可以直接与飞控系统通过MAVLINK通信协议进行通信，从而对无人机进行实时控制以及信息监控。

由于本文避障算法是基于ROS系统在板载计算单元进行板外控制，在利用ROS开发环境进行编程时，由于各ROS节点（外部控制程序）之间通过ROS话题、服务进行交互，还需要利用MAVROS功能包进行MAVLINK消息与ROS话题、服务之间的转换[20]。由于VFH算法是建立在ROS系统的一个包上，因此它可以通过发布/订阅消息与其他节点通信。VFH节点所需的数据包括激光雷达传感器，IMU（航向）、GPS （当前位置）和制导控制，输入通过订阅MAROS话题和激光雷达。基于ROS避障系统发布与订阅体系结构如图6所示。

VFH算法主要是通过/scan话题实时订阅激光雷达传感器信息，并判定是否遇到障碍物，再调用速度规划控制器，对无人机的速度进行规划，通过发布话题/setpoint_raw/local话题对无人机进行实时的速度和位置控制，此话题支持发送期望位置、期望速度、加速度、航向角以及航向角速率给飞控从而进行控制，同时通过订阅/compass_hdg 获取航向信息，订阅/lo?cal_position/pose获取仿真实时的位置信息以及订阅/mission/waypoints话题获取通过地面站規划的任务航点信息等。

3.3 仿真实验

本实验采用的四旋翼的无人机模型，其中以sdf 结尾为无人机模型文件，world结尾为环境模型文件，默认的四旋翼无人机模型iris.sdf，没有搭载激光雷达传感器模型，需要打开posix_sitl.launch，把iris模型修改为iris_rplidar 模型，运行roslaunch posix_sitl.launch 启动之后gazebo将加载激光雷达传感器模型，环境模型默认为没有任何障碍物的空环境，首先需要为无人机建立多个圆柱体类型的障碍物环境模型，打开ga?zebo选择insert，选择圆柱体，调整圆柱体的大小、位置以及颜色等信息，然后另存为obstacles.world文件，修改posix_sitl.launch 中的empty.world 文件为obstacles.world，重新运行启动roslaunch posix_sitl.launch，安装有激光雷达的四旋翼无人机以及障碍物均已经建立并运行正常。

基于搭建好PX4+ROS+gazebo仿真环境之后并启动，之后启动地面站控制系统，地面站控制系统将自动连接上仿真飞控系统，通过地面站设置航点1和航点2信息，并上传至飞控系统。航点1到航点2的水平速度规划如图7所示，在遇到障碍物的过程保持避障低速运行，在没有障碍物的时候，由于限制了最大速度，主要分为三段式梯形速度规划。设定值、估计值与真实俯仰角的示意图如图8所示，实际俯仰角的范围在[-5，5]度之间，姿态角平稳。

由图9障碍物示意图可以看到在gazebo仿真环境中分布着多个柱状的障碍物，蓝色的光束代表着无人机激光雷达扫描发射的光束，由于在Home点到航点1 的途中设置密集柱状障碍物，由图10避障轨迹示意图中可以发现Home 点到航点1 的过程中有明显的避障，最终顺利穿过障碍物林，到达目标点航点2，之后升高返航到Home点。

4 结束语

本文重点介绍了基于向量场直方图的局部路径规划算法，根据收到的飞控的定位数据、激光雷达数据、航向信息以及目标航点位置等，按照一定频率向飞控模块实时发布规划后的无人机飞行方向和速度。在无人机执行飞行任务过程中，实时检测前方是否有障碍物，若遇到障碍物，根据代价函数找到能避开障碍物的最近的可通行区域，从而确定无人机飞行航向，再根据速度规划控制器规划出无人机的飞行速度；若无障碍物则根据剩余距离进行对无人机的速度进行规划，同时保持目标航向，从而保证无人机姿态平衡并以较优的路径完成避障的同时快速到达目标位置。通过gazebo仿真实验结果表明：该算法能快速穿越密集柱状障碍物林的同时保持飞行姿态的平稳，进而达到目标航点。然而对于还未确定的扇区，本文看作不可通行区域，后续研究将进一步优化算法查看未确定扇区周围区域的可通行性进一步判定，从而提高避障算法的抗干扰性能和健壮性。