在三维未知动态环境中无人机避障导航混合方法的研究

赵其定，汪夏荣

（1.江西经济管理干部学院飞行技术学院，江西 南昌 330088；2.深圳市大疆创新科技有限公司，上海 200120）

引言

无人驾驶飞行器（Unmanned Aerial Vehicle,UAV），也称为空中无人机，已经出现在许多需要在特定环境中执行一些环境复杂重复性的任务中。在这些应用中需要自主操作，这增加了无人机导航到任务区域的更多要求。导航方法一般可分为全局路径规划、局部路径规划和混合路径规划[1]。

全局路径规划需要对环境有全面的了解，以生成由无人机控制系统跟踪的最佳有效路径。有许多不同的技术来解决全局规划问题，包括路线图方法、单元分解、势场、概率路线图、快速搜索随机树和基于优化的技术[2-3]。在未知环境中使用的经典反应方法是动态窗口和曲率速度[4]。一类反应式方法采用边界跟随模式来规避障碍物[5]，这种方法的低计算成本是以容易陷入陷阱为代价，将随机行为与边界跟随方法相结合[6]，以规避这种情况。

混合策略倾向于通过结合判断和反应方法来解决上述缺点，从而在未知和动态环境中实现更有效的导航。本文提出了一种混合3D 导航策略，以允许在部分未知/动态环境中进行有效导航。建议的策略将全局路径规划层与基于通3D 运动学模型开发的反应性避障控制方法相结合，基于快速搜索随机树混合算法(Rapidly Exploring Random Tree Connecting，RRT-C)的全局路径规划层可以根据环境状况生成路径。采用滑模技术在反应层中实现边界跟随，与基于搜索和基于优化的本地规划器相比，这种以低廉的计算成本提供了对障碍物的快速反应[5]。为了适应混合导航策略，提出了一种切换机制来实现两个控制方法之间的转换。总体而言，所提出的方法可以克服纯粹依靠判断或被动方法的缺点。

本文组织如下，第2 节提供了解决避障导航问题的公式，然后在第3 节中介绍了混合策略，第4 节运用混合策略这种方法的性能通过不同的模拟场景得到证实，最后在第5 节中作出结论。

1 问题描述

这里考虑了一个3D 导航问题，其中需要无人机在已知的环境ε⊂R3 中安全导航。环境ε 包含n 个障碍物O={O1，O2，…，On}⊂ε。这些障碍物可以是已知静态Ok或未知动态Ou使得O=Ok∪Ou。主要目标是引导无人机安全地到达，坐标系W 中表示的某个目标位置pgoal∈ε{O}。UAV 从环境的初始位置开始，该地图仅包含基于先前关于Ok的信息。也可以考虑未知环境，无人机在穿越环境时可以开始构建地图。在这种情况下，最初Ok=Ø。另外设p(t)=[x(t), y(t), z(t)]T为以W 表示无人机的笛卡尔坐标。注意将向量表示为列向量，写成行向量[·]T 的转置以简化书写。无人机的安全要求定义为与所有障碍物保持安全距离dsafeε＞0，具体如公式（1）所示：

其中d(t)是到最近障碍物的距离，||·||是R3 中向量的标准的欧几里得范数。

考虑适用于不同类型的无人机通用3D 非完整运动学模型，该模型的描述如公式（2）-（4）：

其中V(t)∈R+是线速度，u(t)∈R3是与角速度相关的二自由度控制输入, 任何给定时刻t 的运动方向由单位矢量r(t)表征。公式(4)表明输入u(t)始终垂直于r(t)，这会在3D 中产生类似转向的行为。由于物理限制，无人机速度V(t)和u(t)被视为具有一些上限的控制输入, 由Vmax，umax＞0 表示，这些约束可以表示如公式(5)：

由于只考虑向前运动，不允许V(t)为负值，对于恒速应用，V(t)保持恒定在某个值V＞0。做出以下假设。

假设1:无人机通过传感器感知部分环境，可以确定到最近障碍物的距离，作为感知系统的一部分。考虑抽象传感模型，其中可以检测到距无人机dsensing＞0距离内的障碍物，使用边界几何图来表示物体的传感部分。

假设2:估计无人机的位置p(t)和方向矢量r(t)，以下陈述总结了这项工作中考虑的问题。

问题1:考虑无人机其运动可以用公式(2)-(4)来描述。在假设1 和2 下，设计V(t)和u(t)的控制方法，确保通过未知部分或已知环境ε 无碰撞导航，通过满足公式(1)中的安全要求到达目标位置pgoal和公式(5)中的约束。

备注1:公式(2)-(4)可适用于固定翼无人机、多旋翼无人机。

2 混合导航策略

自主避障导航方法采用的是模块化结构，从软件的角度来看，整个系统结构包括感知、导航和控制子系统。感知子系统负责处理机载传感器测量，提供有关环境信息。可以生成环境的地图信息，并根据反应控制方法提供最近障碍物的距离和方向。

高级导航子系统的设计是这项工作的主要贡献，所提出的设计结合了几个主要组件，即全局路径规划、运动监控器、路径跟踪控制、反应控制和切换机制。显示这些组件的整体混合导航策略的架构见图1。

图1 避障导航策略架构

全局路径规划组件根据当前可用的全局地图生成可行且安全的几何路径。每当分配新的目标位置或发现环境的新部分时检测到诱捕情况（即无人机卡住）时，运动监控器就会被触发生成新的路径。在未知和动态的环境中，无人机可能会因为检测到新的障碍物而发现规划的全局路径不安全。为了快速响应，反应控制组件用于将命令速度直接耦合到当前传感器观测值来生成围绕未知/动态障碍物的局部安全运动，与重新规划新路径相比，反应控制对障碍物的快速反应是由于其计算成本低，然后使用一种机制根据传感器测量在路径跟踪和反应式导航之间切换。

这些组件的详细描述在以下小节中给出，任务计算机通常连接到所有电机的飞行控制单元，然后使用低级控制子系统生成执行器/电机命令，以执行高级导航子系统生成的高级快速命令。

2.1 全局路径规划

全局路径规划算法需要环境的地图来确定，规划路径的最优性和整体算法的复杂性可能是决定不同应用的关键因素。每个规划器在特定条件下使用，基于优化算法来生成最佳的路径，同时在需要重新规划时实施计算效率高的算法来修改初始路径[7]。

混合避障导航策略不限于特定的路径规划算法，将基于抽样的方法视为实现全局规划器的关键步骤，规划器在三维空间中进行规划时可以很好地扩展。使用了快速搜索随机树（Rapidly Exploring Random Tree，RRT）算法的一种变体[8]，称为快速搜索随机树混合算法(RRT-C)。由于算法的流行和低计算成本，每当检测到陷阱时，需要在运动期间进行在线规划。

RRT 算法是基于随机抽样的路径规划方法，其基本概念总结如下，G 是用初始配置初始化的搜索树，使用迭代方法在配置空间中扩展搜索树，直到找到可行的解决方案。在每次迭代中，随机或使用一些启发式对配置进行采样，这有助于使搜索树的增长。发现偏向抽样过程选择具有一定概率(0＜pbgoal＜1)的目标配置以增强RRT 增长，然后该算法通过将搜索树G 连接到树中最近的配置来将其扩展到采样配置[8]。一种方法是使用直线连接两个配置，在处理时考虑的欧几里得空间，然后使用碰撞检查器根据可用的环境图检查每个扩展的可行性，每个可行的扩展通过添加新配置导致搜索树的增长。抽样过程不断重复，直到找到初始配置和目标配置之间的可行路径或满足停止标准（例如超过预定义的计划时间）[9]。

RRT-C 遵循同样的思路，通过维护两棵树，来自初始配置和目标配置。在每次迭代中，规划器尝试扩展其中一棵树，然后尝试找到与另一棵树中一个顶点的无冲突连接。与标准RRT 算法相比，这为复杂环境中解决方案提供快速收敛。

由于RRT 算法的采样性质，生成的路径在总长度方面不是最优。在扩展搜索树时考虑直线，这些路径不满足非完整约束，因此通过两个后处理阶段（即修剪和平滑）细化获得路径[10]。

在剪枝阶段，从规划路径中去除多余的航路点，以提高整体路径质量。令W={w1,w2,···,wk}为一组节点，表示RRT-C 生成的路径，令Wp为经过此阶段后得到的剪枝路径，然后用算法1 删除冗余航路点。

在修剪后的路径旁边应用平滑算法，以确保最终路径满足无人机的非完整约束（最小曲率半径），使用参数贝塞尔曲线生成连续曲率平滑路径。

2.2 追踪路径跟随控制

为了确保无人机准确跟踪规划路径需要进行路径跟随控制设计，采用纯追踪算法（Pure Pursuit Tracking，PP），该算法以稳定简单[9]。假设几何路径τ⊂R3 可用，PP 算法引导无人机跟随虚拟目标pv=(xv，yv，zv)∈τ 沿路径移动。这个目标被选择在距离最近的路径点pc∈τ 到无人机当前位置的某个前沿距离L 处。当无人机远离此处的计划路径时，PP 算法提供更高的稳定性，因为无人机有时会在使用反应控制组件避开障碍物时偏离计划路径，公式(6)-(7)控制算法用于路径跟踪：

其中γ＞0。映射函数F(w1，w2):R3×R3→R3作为一个转向函数，它产生一个在w2方向上垂直于w1的向量，具体定义见公式(8)：

2.3 反应式控制方法

在未知动态环境中导航需要更多的安全措施，如果障碍物是动态的，全局规划器的规划路径可能会变得不安全。反应控制法用于通过在检测到的障碍物周围导航来对检测到的障碍物产生类似反射的反应，直到可以安全地遵循先前计划的路径。

采用基于滑动模式控制技术的反应控制算法[10]，到最近障碍物的距离d (t) 是实现该控制器所需的信息，且可以从无人机传感器获得，这种反应控制基于边界跟随模式，如公式(9)-(10)所示：

考虑恒定的前进速度，d0＞dsafe＞0 是所需 距离，Γ=±1 确定回避机动方向，sgn(α)为符号函数。此外χ（β）是一个饱和函数，公式(12)定义为：

对于某些设计参数γ，δ＞0。ia(t*)表示与某个障碍物的回避平面法线，这个法线对于每个障碍物可能是不同的，并且可以在检测到新障碍物的时刻t*确定。反应式导航方法确保无人机保持固定距离d0，同时在某些假设下绕过最近的障碍。

2.4 切换规则

切换机制对于混合避障导航非常重要，负责遵循计划的路径是安全的还是被动地避开新的障碍物。一般使用两种导航模式，并采用切换机制在两种模式之间切换。这两种模式是路径跟随模式M1 和避障/反应模式M2。

假设无人机最初以模式M1 启动，考虑以下切换规则：

规则一：当与最近障碍物Oi的距离低于某个阈值距离C 即i.e.,d (t)=C 和d (t)＜0 时，切换到反应模式M2。

规则二：当|d(t)-d0|＜ε0时从M2 切换到路径跟随模式M1 对于某个小的值ε0＞0 并且无人机的航向是针对规划路径上的虚拟目标pv的，可以根据公式(13)确定：

2.5 运动管理

每当无人机新发现的环境而陷入局部最小值时，运动监控器负责检测陷阱情况。当在类似迷宫的环境、凹形障碍物中导航时，解决此问题的方法是向全局路径规划器发出重新规划命令，以根据有关环境的更新信息获取新路径[6]。

3 仿真结果与讨论

混合导航方法使用两种不同情况的模拟进行测试，即静态环境和动态环境。假设有关这些环境的一些特征是已知的，以显示全局规划的作用。RRT-C 算法是基于三维路径规划实现的，用于规划全局路径。

在仿真中对于第一种和第二种情况，分别考虑了V=0.75 m/s 的线速度和umax=1.75 rad/s 和2.5 rad/s 的角速度。设计参数也选择如下：L=0.5,δ=0.5,γ=1,d0=1和C=2。对公式(11)中使用的ia(t*) 进行了任意选择，每个选项都不同障碍。此外控制的更新速率设置为0.01 s，接下来介绍仿真结果。

3.1 未知的静态障碍

在第一个模拟案例中，考虑无人机执行一些重复性任务，关于环境的特点是已知的，如墙壁位置。无人机不知道的静态障碍物可以在不同时间添加到环境中。这种场景的一个真实示例是在建筑物内操作时，建筑布局是提前知道的。

图2 显示了该仿真案例所考虑的环境，无人机可用的初始地图仅包括一堵墙，这堵墙有两个方形缺口。有9 个未知圆柱形障碍物。在实践中，通常可以通过无人机的感知系统估计边界形状以确定障碍物。

图2 混合导航策略规划的路径和执行的运动（静态环境）

图2 是在静态环境中混合导航策略规划的路径和执行的运动，可以看出所提出的混合导航策略安全地引导无人机从某个初始位置到达目标位置。该图显示了全局规划器基于已知环境初始生成的路径以及无人机实际执行的路径。只要无人机与障碍物有良好的间隙，就可以成功地执行计划路径。一旦无人机检测到障碍物，无人机就会切换到反应模式M2 以绕过障碍物。图3 给出了运动过程中到最近障碍物的距离，表明无人机可以通过与所有障碍物保持适当的间隙来满足安全要求。无人机的线速度如图4 所示，结果证实了所提出的方法可以在未知的静态障碍物之间安全地引导无人机。

图3 无人机到障碍物的距离

图4 无人机运动中的线速度

3.2 未知的动态障碍

初始规划路径和执行运动在动态环境中如图5所示，其中A、B、C、D 为四个动态障碍，红色虚线为初始路径。全局规划器使用RRT-C 找到一条安全路径，如图6 中的黑线所示。无人机开始以M1 模式移动以跟踪规划（参考）路径，由于环境的动态性，只要在运动过程中有障碍物接近无人机，这条路径就会变得不安全。每次检测到有威胁时，无人机根据切换规则切换到规则二的导航。可以在图6 中看到，这表明无人机的实际执行路径在某些位置偏离了规划路径，以避开动态障碍物。这在图7 中得到验证，与最近障碍物的距离保持在安全阈值之上。无人机的线速度如图8所示。从结果可以看出，所提出的策略在动态环境中也能很好地工作，需要说明的是，无人机的最大速度必须大于障碍物的速度才能保证安全。

图5 初始规划路径和执行运动（动态环境）

图6 初始规划路径和执行运动（动态环境）

图7 无人机到障碍物的距离

图8 无人机运动过程中的线速度

4 结论

本文提出了一种用于无人机的混合三维导航策略。问题制定考虑了适用于不同无人机类型和自主水下航行器的通用3D 非完整运动学模型。使用了基于RRT-C 算法的全局规划器，该算法在三维空间中运行良好，采用基于滑模控制的反应控制方法来避开未知和动态障碍物。仿真结果证实，所提出的混合导航方法在未知静态和动态障碍物的3D 环境中运行良好。