CN111638727B - 一种基于深度图像的多旋翼飞行器安全导航控制方法 - Google Patents

Abstract

本发明涉及一种基于深度图像的多旋翼无人机自主避障方案，该方案能实现无人机在未知环境自主飞行并实现障碍物规避，对于凹形障碍物场景造成的局部困境，该方案具有良好的局部困境逃逸能力；针对单个视觉传感器的视野盲区问题，本发明设计了轻量化的障碍物存在状态推断方案，有效实现了传感器感知范围外的障碍物规避，确保了无人机安全飞行，此外，该避障方案具有体积小，重量轻、功耗低、实时性高等优势。

Description

一种基于深度图像的多旋翼飞行器安全导航控制方法

技术领域

本发明属于无人机控制领域，尤指多旋翼飞行器自动避障方法。

背景技术

近年来，随着无人机控制技术的日趋成熟，以多旋翼飞行器为首的微小型无人机逐渐进入普通人的生活，其具有结构简单、控制灵活、垂直起降和悬停等优点，在航拍、监控和侦察等常见应用场景中受到广泛使用，低空场景中存在如灌木、汽车、电缆和建筑物等不同类型的障碍物常对无人机造成潜在威胁，而碰撞的发生可能导致无人机损毁，甚至造成人员伤亡等不可挽回的损失。从长远来看，安全问题必须放在首位，自主避障是无人机自主安全飞行的必要技术。

当前的无人机避障技术主要基于视觉传感器，主要手段包括虚拟航点法、光流法、人工智能算法和SLAM算法，上述方案在特殊场景下能实现避障，但缺陷在于计算资源消耗大或者对场景实验场景要求高等；此外，基于多传感器的避障方案主要以超声波测距传感器为代表，该方案可以直接获取无人机各方向上障碍物的距离，但缺陷在于传感器数量增加了无人机任务载重负担。

发明内容

要解决的技术问题

使无人机具备在未知场景中自主避障能力，凭借单个立体视觉传感器完成对环境中障碍物的感知、评估和规避，从而实现自主飞行，最终设计一套基于深度图像的多旋翼无人机自主飞行控制方法。该方案旨在克服传统避障方案对传感器数量、机载处理器性能等约束，使得无人机利用单个视觉传感器完成自主导航任务。

技术方案

一种基于深度图像的多旋翼飞行器安全导航控制方法，其特征在于步骤如下：

1、基于深度图像的飞行器环境评估方案

基于机体水平坐标系建立球状安全模型表示多旋翼的碰撞模型，其半径为R_safe，，引入参数

Depth＝x_v，有区域1的空间表达式为：Φ₁＝{P_v(x_v，y_v，z_v)|R_safe＜Depth＜D_set，D_v＜R_safe}；区域2的表达式为Φ₂＝{P_v(x_v，y_v，z_v)|0＜Depth＜D_turn，R_safe＜D_v＜2*R_safe}；区域3的表达式为：Φ₃＝{P_v(x_v，y_v，z_v)|D_turn＜Depth＜D_set，R_safe＜D_v＜2*R_safe}；其中P_v(x_v，y_v，z_v)表示障碍物在机体水平坐标系内的位置信息；D_set为立体视觉相机的最大有效感知距离；D_turn为飞行器转向的安全距离，需要大于无人机安全半径R_safe；区域1为飞行器保持当前速度下，机身的期望轨迹所在空间；区域2和区域3所示的空间为飞行器转向过程的期望轨迹，其所在空间内障碍物的存在情况需要结合飞行器运动进行推理，推理方案为：以飞行器水平直线运动为模型，无人机与障碍物之间的相对运动规律计算视野内的障碍物离开该区域需要的时间，其计算过程如公式(1)和(2)所示：

t₂＝max(Depth/v_x，t₂) (1)

t₃＝max(Depth/v_x，t₃) (2)

式中：v_x为无人机水平速度分量；t₂和t₃分别表示区域2和3中障碍物对飞行器的期望威胁时间，采用公式t₂＝t₂-Δt和t₃＝t₃-Δt对该时间进行更新，其中Δt为图像处理间隔时间；

因此有区域1内存在障碍物的判别条件为：R_safe＜Depth＜D_set，D_v＜R_safe；

区域2内存在障碍物的判别条件为：t₂＞0；

区域3内存在障碍物的判别条件为：t₃＞D_turn/v_xmax；其中v_xmax为无人机的最大水平速度；

2、基于评估模型的多旋翼无人机自主避障方案设计

根据任务过程中无人机所处状态不同将其控制模式划分为：起飞、降落、悬停、导航、过渡和避障；

相关参数声明：无人机当前位置：P_current(x，y，z)；目标航点：P_target(x，y，z)；无人机的偏航角为：ψ；Δψ为期望偏航角误差；

为两个点的空间距离；

2.1控制模式的选择

设计任务最低高度H_safe作为无人机起飞模式的极限，即当P_current.z＜H_safe，无人机处于起飞模式；

假设D_safe为无人机在目标航点附近的悬停最大误差极限，即当

无人机切换至悬停模式；

当区域1内存在障碍物，即无人机正前方空间存在障碍物，则应进入避障模式；避障模式的结束判据为：区域1、区域2和区域3内均无障碍物；

避障结束后，进入过渡模式，过渡模式的结束条件为：|Δψ|＜π/2，即目标点位于无人机前方；

其他情况下无人机处于导航模式；发布降落指令后无人机进入降落模式回到地面；

2.2不同控制模式的控制指令

2.2.1起飞、过渡和悬停模式

无人机所在环境被假设为绝对安全，采用PID控制器获得控制指令：

式中：v_x，v_y，v_z为飞行器的速度控制指令，

为比例系数；

2.2.2导航模式

受到传感器感知范围限制，保持飞行器速度方向与传感器安装方向一致的方式确保无人机在安全空间内运动，其控制指令的计算过程如下：

偏航角速度：

x方向水平速度：

竖直方向速度(P_target.z-P_current.z＞0时)：

或(P_target.z-P_current.z＜0时)

式中：ω_zmax，v_xmax，v_ymax，v_zmax为受飞行器硬件限制的最大控制指令；

2.2.3避障模式

当避障模式触发时，即区域1内存在障碍物，根据当前视觉传感器感知到的障碍物分布情况，选择障碍物最少的方向作为最佳避障方向，分为：水平向左和水平向右；通过转向的避障实质为控制其偏航角速度，其水平速度和竖直速度不变；

当区域1中存在障碍物，并且区域2中存在障碍物：ω_z＝0；

当区域1中不存在障碍物，并且区域2中存在障碍物：ω_z＝0；

当区域1中存在障碍物，并且区域2中不存在障碍物：ω_z＝ω_zmax；

当区域1中不存在障碍物，并且区域2中不存在障碍物：ω_z＝0；

2.2.4过渡模式

根据避障方向对飞行器转向进行控制，当避障方向是向左转向时，过渡模式下的偏航角速度为：ω_z＝-ω_zmax；当避障方向是向右转向时，过渡模式下的偏航角速度为：ω_z＝ω_zmax°

采用基于Robot Operating System的geometry_msgs/Twis类型规范控制指令对无人机进行控制。

有益效果

本发明提出的一种基于深度图像的多旋翼飞行器安全导航控制方法，该方法使得飞行器在未知场景中具有自主飞行的能力。在环境感知方面，该方案依赖的原始数据为飞行器正前方环境的深度图像，减少了对传感器数量的依赖；在环境评估方面，采用障碍物的存在状态对飞行器周围的环境进行评估，减少了虚拟场景构建所占用的内存；该方法综合考虑了飞行器的各种复杂运动形式，用计算量较小的方式推导了视野盲区内的障碍物存在情况，实现无人机在未知环境中安全飞行；此外，该方案有别于传统反应式避障方案，对凹形障碍物具有良好的逃逸能力，确保飞行器在未知障碍物场景中顺利完成导航任务，具有十分重要的工程应用价值。

附图说明

图1是本发明用于环境评估的模型；

图2是本发明用于无人机导航角计算示意图；

图3是本发明用于仿真的四旋翼无人机模型和传感器数据示意图；

图4是本发明的自主避障方案在单个障碍物场景中的飞行轨迹；

图5是本发明的自主避障方案在凹形障碍物场景中的飞行轨迹。

具体实施方式

现结合实施例、附图对本发明作进一步描述：

为了实现基于深度图像的多旋翼无人机避障，本发明采用立体相机作为传感器为无人机提供深度图像，实现对无人机安全评估，最后根据无人机当前环境周围障碍物存在情况生成相应的控制指令，从而避免碰撞并实现安全飞行。因此本发明包括基于深度图像的无人机安全评估和自主避障控制方案设计。

1、基于深度图像的飞行器环境评估方案

为了保证飞行器在任务过程中的飞行安全，本发明采用球状安全模型表示多旋翼的碰撞模型，其半径为R_safe，基于此设计了如图1所示的无人机环境评估模型。该模型基于机体水平坐标系建立，引入参数

Depth＝x_v，有区域1的空间表达式为：Φ₁＝{P_v(x_v，y_v，z_v)|R_safe＜Depth＜D_set，D_v＜R_safe}；区域2的表达式为Φ₂＝{P_v(x_v，y_v，z_v)|0＜Depth＜D_turn，R_safe＜D_v＜2*R_safe}；区域3的表达式为：Φ₃＝{P_v(x_v，y_v，z_v)|D_turn＜Depth＜D_set，R_safe＜D_v＜2*R_safe}；其中P_v(x_v，y_v，z_v)表示障碍物在机体水平坐标系内的位置信息；D_set为立体视觉相机的最大有效感知距离；D_turn为飞行器转向的安全距离，需要大于无人机安全半径R_safe。从而有：区域1为飞行器保持当前速度下，机身的期望轨迹所在空间；区域2和区域3所示的空间为飞行器转向过程的期望轨迹，其所在空间内障碍物的存在情况需要结合飞行器运动进行推理，推理方案为：以飞行器水平直线运动为模型，无人机与障碍物之间的相对运动规律可以计算视野内的障碍物离开该区域需要的时间，其计算过程如公式(1)和(2)所示。

t₂＝max(Depth/v_x，t₂) (1)

t₃＝max(Depth/v_x，t₃) (2)

式中：v_x为无人机水平速度分量；t₂和t₃分别表示区域2和3中障碍物对飞行器的期望威胁时间，采用公式t₂＝t₂-Δt和t₃＝t₃-Δt可以对该时间进行更新，其中Δt为图像处理间隔时间；

因此有区域1内存在障碍物的判别条件为：R_safe＜Depth＜D_set，D_v＜R_safe；

区域2内存在障碍物的判别条件为：t₂＞0；

区域3内存在障碍物的判别条件为：t₃＞D_turn/v_xmax；其中v_xmax为无人机的最大水平速度。

2、基于评估模型的多旋翼无人机自主避障方案设计

基于此环境评估模型，本发明将设计自主避障方案实现障碍物规避，为了便于描述无人机控制方案，本项目根据任务过程中无人机所处状态不同将其控制模式划分为：起飞、降落、悬停、导航、过渡和避障。采用基于ROS(Robot Operating System)的geometry_msgs/Twis类型规范控制指令对无人机进行控制。

相关参数声明：无人机当前位置：P_current(x，y，z)；目标航点：P_target(x，y，z)；无人机的偏航角(如图2所示)为：ψ；Δψ为期望偏航角误差(如图2所示)；

为两个点的空间距离。

2.1控制模式的选择

设计任务最低高度H_safe作为无人机起飞模式的极限，即当P_current.z＜H_safe，无人机处于起飞模式。

假设D_safe为无人机在目标航点附近的悬停最大误差极限，即当

无人机切换至悬停模式。

当区域1内存在障碍物，无人机正前方空间存在障碍物，则应进入避障模式；避障模式的结束判据为：区域1、区域2和区域3内均无障碍物。

避障结束后，进入过渡模式，过渡模式的结束条件为：|Δψ|＜π/2，即目标点位于无人机前方。

其他情况下无人机处于导航模式；发布降落指令后无人机进入降落模式回到地面。

2.2不同控制模式的控制指令

2.2.1起飞、过渡和悬停模式

无人机所在环境被假设为绝对安全，可以采用常规PID控制器获得控制指令。

式中：v_x，v_v，v_z为飞行器的速度控制指令，

为比例系数。

2.2.2导航模式

受到传感器感知范围限制，保持飞行器速度方向与传感器安装方向一致的方式确保无人机在安全空间内运动，其控制指令的计算过程如下：

偏航角速度：

x方向水平速度：

竖直方向速度(P_target.z-P_current.z＞0时)：

或(P_target.z-P_current.z＜0时)

式中：ω_zmax，v_xmax，v_ymax，v_zmax为受飞行器硬件限制的最大控制指令。

2.2.3避障模式

当避障模式触发时，即区域1内存在障碍物，因此可以根据当前视觉传感器感知到的障碍物分布情况，选择障碍物最少的方向作为最佳避障方向，一般可以分为：水平向左和水平向右。通过转向的避障实质为控制其偏航角速度，其水平速度和竖直速度不变。以水平面左转向为例(偏航角速度大于零，飞行器左转向，反之右转向)，对避障过程中的控制指令进行简单叙述。

当区域1中存在障碍物，并且区域2中存在障碍物：ω_z＝0；

当区域1中不存在障碍物，并且区域2中存在障碍物：ω_z＝0；

当区域1中存在障碍物，并且区域2中不存在障碍物：ω_z＝ω_zmax；

当区域1中不存在障碍物，并且区域2中不存在障碍物：ω_z＝0；

2.2.4过渡模式

根据避障方向对飞行器转向进行控制，当避障方向是向左转向时，过渡模式下的偏航角速度为：ω_z＝--ω_zmax；当避障方向是向右转向时，过渡模式下的偏航角速度为：ω_z＝ω_zmax。

以如图3所示携带立体相机的四旋翼飞行器为实验对象，在单个障碍物场景和凹形障碍物场景中飞行为例，实验样例将在ROS+GAZEBO仿真平台下进行。具体的实现主要包括环境评估和自主飞行控制程序编写。

1、环境评估

图3中所示为通过仿真软件获取的深度图像，类型为CV_32FCl，大小为480*640，每一个像素点的值为障碍物到相机的距离，因此可以通过公式8计算得到障碍物在相机坐标系内的位置P_c(x_c，y_c，z_c)信息。

式中：u，v分别为深度图像中像素点的横坐标和中坐标；u₀＝240；v₀＝320；

考虑实际情况中无人机俯仰角较小，因此有：P_v(x_v，y_v，z_v)＝P_c(x_c，y_c，z_c)。

根据关系：

和Depth＝x_v可以将障碍物坐标转化为飞行器环境评估参数，从而实现对图1所示的环境模型进行评估，得到区域1、区域2和区域3中是否存在障碍物。

2无人机自主飞行控制

基于ROS系统，飞行器的控制指令可以被封装为geometry_msgs/Twist类型的消息，该消息由三轴速度和角速度组成，对于多旋翼飞行器而言，可以由v_x、v_y、v_z和ω_z作为飞行器飞行过程中的控制输入。

2.1起飞、过渡和悬停模式

飞行器所在环境被假设为绝对安全，可以采用常规PID控制器获得控制指令。

相关参数设定为：

2.2导航模式

受到传感器感知范围限制，保持飞行器速度方向与传感器安装方向一致的方式确保无人机在安全空间内运动，其控制指令的计算过程如下：

偏航角速度：

x方向水平速度：

竖直方向速度(P_target·z-P_current·z＞0时)：

或(P_target·z-P_current·z＜0时)

式中：ω_zmax＝0.5；v_xmax＝1；v_ymax＝0.6；；

β_c＝π/2；

D_safe＝0.5。

2.3避障模式

以水平面左转避障过程中的控制指令计算方式如下：

当区域1中存在障碍物，并且区域2中存在障碍物：ω_z＝0；

当区域1中不存在障碍物，并且区域2中存在障碍物：ω_z＝0；

当区域1中存在障碍物，并且区域2中不存在障碍物：ω_z＝ω_zmax；

当区域1中不存在障碍物，并且区域2中不存在障碍物：ω_z＝0；

以水平面右转避障过程中的控制指令计算方式如下：

当区域1中存在障碍物，并且区域2中存在障碍物：ω_z＝0；

当区域1中不存在障碍物，并且区域2中存在障碍物：ω_z＝0；

当区域1中存在障碍物，并且区域2中不存在障碍物：ω_z＝-ω_zmax；

当区域1中不存在障碍物，并且区域2中不存在障碍物：ω_z＝0；

2.4过渡模式

根据避障方向对飞行器转向进行控制，当避障方向是向左转向时，过渡模式下的偏航角速度为：ω_z＝-ω_zmax；当避障方向是向右转向时，过渡模式下的偏航角速度为：ω_z＝ω_zmax。

实验过程中所用的参数大小分别如下：u₀＝240；v₀＝320；

ω_zmax＝0.5；v_xmax＝1；v_ymax＝0.6；v_zmax＝0.5；

R_safe＝0.5；D_safe＝0.5；H_safe＝2；D_turn＝2*R_safe；

β_c＝π/2；

D_set＝5；D_turn＝1。

为了对上述算法进行实验验证，搭建了单个障碍物和凹形障碍物两个仿真场景，并在其中进行如下仿真实验：1、控制飞行器起飞，并在起飞后的初始航点为P_target0(0 06)；2、发布第一个航点P_target1(20 0 6)；3、当飞行器抵达第一个目标航点后，发布第二个航点P_target2(0 0 6)；4、发布降落指令。从而得到仿真轨迹如图4和图5所示(为了绘图清晰，未绘制降落段轨迹)，结果表明该方案使得飞行器对未知环境具有较强的适应性，且对于凹形障碍物造成的局部困境具有良好的逃逸能力。

Claims (2)

1.一种基于深度图像的多旋翼飞行器安全导航控制方法，采用立体相机作为传感器为无人机提供深度图像；其特征在于步骤如下：

(1)基于深度图像的飞行器环境评估方案

基于机体水平坐标系建立球状安全模型表示多旋翼的碰撞模型，其半径为R_safe，引入参数

Depth＝x_v，有区域1的空间表达式为：Φ₁＝{P_v(x_v,y_v,z_v)|R_safe＜Depth＜D_set,D_v＜R_safe}；区域2的表达式为Φ₂＝{P_v(x_v,y_v,z_v)|0＜Depth＜D_turn,R_safe＜D_v＜2*R_safe}；区域3的表达式为：Φ₃＝{P_v(x_v,y_v,z_v)|D_turn＜Depth＜D_set,R_safe＜D_v＜2*R_safe}；其中P_v(x_v,y_v,z_v)表示障碍物在机体水平坐标系内的位置信息；D_set为立体视觉相机的最大有效感知距离；D_turn为飞行器转向的安全距离，需要大于无人机安全半径R_safe；区域1为飞行器保持当前速度下，机身的期望轨迹所在空间；区域2和区域3所示的空间为飞行器转向过程的期望轨迹，其所在空间内障碍物的存在情况需要结合飞行器运动进行推理，推理方案为：以飞行器水平直线运动为模型，无人机与障碍物之间的相对运动规律计算视野内的障碍物离开该区域需要的时间，其计算过程如公式(1)和(2)所示：

t₂＝max(Depth/v_x,t₂) (1)

t₃＝max(Depth/v_x,t₃) (2)

式中：v_x为无人机水平速度分量；t₂和t₃分别表示区域2和3中障碍物对飞行器的期望威胁时间，采用公式t₂＝t₂-Δt和t₃＝t₃-Δt对该时间进行更新，其中Δt为图像处理间隔时间；

因此有区域1内存在障碍物的判别条件为：R_safe＜Depth＜D_set，D_v＜R_safe；

区域2内存在障碍物的判别条件为：t₂＞0；

区域3内存在障碍物的判别条件为：t₃＞D_turn/v_xmax；其中v_xmax为无人机的最大水平速度；

(2)基于评估模型的多旋翼无人机自主避障方案设计

根据任务过程中无人机所处状态不同将其控制模式划分为：起飞、降落、悬停、导航、过渡和避障；

相关参数声明：无人机当前位置：P_current(x,y,z)；目标航点：P_target(x,y,z)；无人机的偏航角为：ψ；Δψ为期望偏航角误差；

为两个点的空间距离；

2.1控制模式的选择

设计任务最低高度H_safe作为无人机起飞模式的极限，即当P_current.z＜H_safe，无人机处于起飞模式；

假设D_safe为无人机在目标航点附近的悬停最大误差极限，即当

无人机切换至悬停模式；

当区域1内存在障碍物，即无人机正前方空间存在障碍物，则应进入避障模式；避障模式的结束判据为：区域1、区域2和区域3内均无障碍物；

避障结束后，进入过渡模式，过渡模式的结束条件为：|Δψ|＜π/2，即目标点位于无人机前方；

其他情况下无人机处于导航模式；发布降落指令后无人机进入降落模式回到地面；

2.2不同控制模式的控制指令

2.2.1起飞、过渡和悬停模式

无人机所在环境被假设为绝对安全，采用PID控制器获得控制指令：

式中：v_x,v_y,v_z为飞行器的速度控制指令，

为比例系数；

2.2.2导航模式

受到传感器感知范围限制，保持飞行器速度方向与传感器安装方向一致的方式确保无人机在安全空间内运动，其控制指令的计算过程如下：

偏航角速度：

x方向水平速度：

竖直方向速度P_target.z-P_current.z＞0时：

或P_target.z-P_current.z＜0时

式中：ω_zmax,v_xmax,v_zmax为受飞行器硬件限制的最大控制指令；

2.2.3避障模式

当避障模式触发时，即区域1内存在障碍物，根据当前视觉传感器感知到的障碍物分布情况，选择障碍物最少的方向作为最佳避障方向，分为：水平向左和水平向右；通过转向的避障实质为控制其偏航角速度，其水平速度和竖直速度不变；

当区域1中存在障碍物，并且区域2中存在障碍物：ω_z＝0；

当区域1中不存在障碍物，并且区域2中存在障碍物：ω_z＝0；

当区域1中存在障碍物，并且区域2中不存在障碍物：ω_z＝ω_zmax；

当区域1中不存在障碍物，并且区域2中不存在障碍物：ω_z＝0；

2.2.4过渡模式

根据避障方向对飞行器转向进行控制，当避障方向是向左转向时，过渡模式下的偏航角速度为：ω_z＝-ω_zmax；当避障方向是向右转向时，过渡模式下的偏航角速度为：ω_z＝ω_zmax。

2.根据权利要求1所述的一种基于深度图像的多旋翼飞行器安全导航控制方法，其特征在于采用基于Robot Operating System的geometry_msgs/Twis类型规范控制指令对无人机进行控制。

Images