CN109857133A - 基于双目视觉的多旋翼无人机选择性避障控制方法 - Google Patents

Abstract

本发明公开了一种基于双目视觉的多旋翼无人机选择性避障控制方法，包括双目测距模块、无人机姿态识别模块、飞行控制模块以及传输模块。双目摄像机垂直设于无人机前方，双目摄像机的两个摄像头左右设置且相互平行，用于拍摄障碍物图像。根据障碍物的图像与无人机姿态识别模块获取的俯仰角综合分析出障碍物的真实方位以及障碍物与无人机的真实距离；根据障碍物的真实方位，障碍物与无人机的真实距离和俯仰角构建无人机在像素坐标系的矩形框尺寸；无人机按照该矩形框内是否存在障碍物规划飞行路径；同时允许用户操纵遥控器控制无人机。本发明减少了无人机倾斜对双目测距的影响，使得无人机避障更准确。

Description

基于双目视觉的多旋翼无人机选择性避障控制方法

技术领域

本发明涉及无人机避障技术领域，尤其涉及一种基于双目视觉的多旋翼无人机选择性避障控制方法。

背景技术

随着无人机相关技术的发展以及其应用复杂场景，对周围感知环境的能力有了更高的要求，其中避障功能的强弱对无人机自适应飞行起到了决定性的影响。

目前，无人机避障系统大多采用超声波、激光、雷达以及双目视觉等。超声波测距较短且对外界条件比较敏感；激光对光照等外界条件比较敏感；雷达对外部条件不太敏感，但技术不太成熟，存在误判问题；而双目测距具有测量范围广，信息量大，成本低等优势，尤其是能迅速捕获周围环境。因此本专利选取双目视觉进行避障。具体而言，双目摄像机采集障碍物的第一图像和第二图像，根据视差图和产生的立体视觉信息可以直接恢复障碍物的三维空间坐标，从而获取障碍物的深度值，使无人机规避障碍物。

当多旋翼无人机向前飞行时，无人机为倾斜状态，双目视觉所测出的距离并不是无人机与地面或障碍物之间的水平实际距离，会出现不必要的避障或悬停，使得无人机不能按照我们预期的规则进行避障，将使无人机发生碰撞的可能性大大提升，从而降低了双目避障系统的实用性以及可靠性。

发明内容

鉴于现有技术的上述缺陷，本发明提供一种基于双目视觉的多旋翼无人机选择性避障控制方法，克服无人机自身倾斜带来的误差，解决了无人机复杂环境下的避障问题。

本发明专利解决其技术问题所采用的技术方案是：

基于双目视觉的多旋翼无人机选择性避障控制方法，其特征在于，所述避障系统包括双目测距模块，无人机姿态识别模块，飞行控制模块以及传输模块。

所述双目测距模块包括一个带USB接口的双目摄像机，一个树梅派。双目摄像机垂直设于无人机前方，双目摄像机的两个摄像头左右设置且相互平行；树梅派为双目摄像机采集数据的处理系统，通过USB接口连接双目摄像机，用于处理视差图，并且通过俯仰角的补偿重新构建三维，计算深度，同时通过IO口连接飞控，控制无人机的俯仰角、横滚角和偏航角等飞行姿态，从而实现选择性避障。

所述姿态识别模块包括加速度计，陀螺仪，磁力计，气压计，用于获得无人机姿态角与高度。

所述飞行控制模块主要为嵌入式单片机和地面站，用于控制无人机的飞行姿态。

所述传输模块主要为数传模块，图传模块，遥控器。数传模块与图传模块用于传输数据和视频信息，遥控器用于人为控制无人机。

进一步的，基于双目视觉的多旋翼无人机选择性避障控制方法步骤如下：

1.通过双目摄像机获取无人机前方的环境图像；所述环境图像为双目摄像机的两个摄像头在同一时刻分别所拍摄的第一图像和第二图像；

2.根据获取的环境图像识别无人机前方是否有障碍物；

3.若存在障碍物，则基于无人机当前姿态信息以及根据图像获取的障碍物深度值；所述无人机姿态信息为设于无人机的姿态传感器所获取的无人机俯仰角，姿态传感器为加速度计、陀螺仪及磁力计；所述障碍物的深度值通过以下步骤获得：

3.1所述双目摄像机获取无人机前方的环境图像包括第一图像和第二图像；

3.2所述双目摄像机的两个摄像头通过张氏标定法，获取摄像头的内外参数，再通过摄像头的外部参数和畸变系数对第一图像和第二图像进行矫正；将矫正后的第一图像和第二图像使用SGAM算法进行立体匹配，得到两个图像的视差图；

3.3基于所述视差图获取深度图像，根据所述深度图像提取障碍物深度值；

3.4设定避障阈值l_max，识别小于l_max的障碍物并保留，去除l_max的障碍物。

4.若图像存在小于l_max的障碍物，树梅派进行判断是否避障，否则无人机保持原先路线继续飞行；

4.1基于图像存在小于l_max的障碍物，计算无人机与最近障碍物的水平距离d：

若v₀-v≠0时，

式中，z为最近障碍物的竖轴坐标，y为最近障碍物的纵轴坐标，θ为俯仰角，v₀为图像中心的像素纵坐标，v为最近障碍物的像素纵坐标。

若v₀-v＝0时，

d＝zcos|θ| (2)

4.2计算无人机实际矩形框宽度w_fact和长度I_fact，

w_fact＝h cos|θ|+bsin|θ| (3)

式中，h为无人机高度，b为无人机宽度，θ为俯仰角，θ∈(-15°，15°)，向下为正，向上为负；I_fact＝l，其中l为无人机长度。

4.3计算像素飞行框的长度Δu和宽度Δv：深度值d时的图像中心点的视差为

D＝Bf/d (4)

式中，B为两个摄像头之间的距离，f为双目摄像机的焦距；

Δx＝lD/B (5)

式中，Δx为l在深度值d时的图像坐标系下的对应尺寸。

式中，Δu₁为l在深度值d时的像素坐标系下的对应的尺寸，dx为单位像素在横轴的物理尺寸；

式中，Δv₁为w_fact在深度值d时的像素坐标系下的对应的尺寸，dy为单位像素在纵轴的物理尺寸；

将Δu₁和Δv₁都扩大为原先的1.2倍，能减小外界因素影响和及时处理突发情况，则有

Δu＝1.2Δu₁,Δv＝1.2Δv₁ (8)

4.4当无人机倾斜时，获取像素飞行安全框的位子。设像素飞行安全框中心与像素飞行框中心连线的间距Δc：

Δe＝d tan|θ| (9)

式中，Δe为无人机实际框中心偏移量；

Δy_c＝DΔe/B (10)

式中，Δy_c为无人机在图像坐标系下的矩形框中心点的偏移量；

Δc＝Δy_c/dy＝ftan|θ|/dy (11)

4.5在像素坐标系下，若θ≥0°，将像素飞行框向正上方平移Δc得到像素飞行安全框；若θ＜0°，将像素飞行框向正下方平移Δc得到像素飞行安全框；

4.6遍历像素飞行安全框：(1)若像素飞行安全框内不存在障碍物，无人机向像素飞行安全框方向飞行；(2)若像素飞行安全框内存在障碍物，无人机进行相应选择性避障，具体步骤为：

4.6.1无人机开始减速慢行，记录像素飞行安全框的最近障碍物的深度值deep，进行选择相应的避障模式1：

无人机向上飞行，如果在向上飞行过程中像素飞行安全框内不存在障碍物，无人机按原先规划的航线飞行；如果向上飞行5米之后像素飞行安全框内存在障碍物，无人机立即减速悬停，并通过数传模块和图传模块发送信息，提示人为遥控；

4.6.2 deep与设定的阈值set_d比较，无人机进行选择相应的避障模式2：

①deep大于set_d时进入缓行区，无人机调整俯仰角适当降低速度；

②deep小于或等于set_d时进入警报区，无人机立即减速悬停，并通过数传模块和图传模块发送信息，提示人为遥控。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

(1)基于树梅派的双目测距模块小巧轻便，安装简单，同时减小了嵌入式处理器的操作量，便于无人机姿态控制及时调整；

(2)通过角度修正了无人机倾斜对双目测距的干扰，并将在矩形框内检测到的范围根据距离及无人机速度分为三部分分别进行处理，使无人机的回避路线规划更加合理、准确。

附图说明

图1是基于双目视觉的多旋无人机选择性避障控制方法的结构图；

图2是基于双目视觉的多旋翼无人机选择性避障控制方法的流程图；

图3是无人机与最近障碍物的水平距离；

图4是无人机实际矩形框尺寸；

图5是无人机实际矩形转化为像素飞行框；

图6是像素飞行框转化为像素飞行安全框；

图7是无人机与障碍物之间距离处理方式的示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚，以下结合附图1-7，对本发明进一步详细描述。

参照图1，本发明所提供的基于双目视觉的多旋翼无人机选择性避障控制方法，其特征在于，所述避障系统包括双目测距模块，无人机姿态识别模块，飞行控制模块以及传输模块。

所述双目测距模块包括一个带USB接口的双目摄像机，一个树梅派。双目摄像机垂直设于无人机前方，双目摄像机的两个摄像头左右设置且相互平行；树梅派为双目摄像机采集数据的处理系统，通过USB接口连接双目摄像机，用于处理视差图，并且对三维进行重新构建，计算深度，同时通过IO口连接飞控，控制无人机的俯仰角、横滚角和偏航角等飞行姿态，从而实现选择性避障。

所述姿态识别模块包括加速度计，陀螺仪，磁力计，气压计，用于获得无人机姿态角与高度。

所述飞行控制模块主要为嵌入式单片机和地面站，用于控制无人机的飞行姿态。

所述传输模块主要为数传模块，图传模块，遥控器。数传模块与图传模块用于传输数据和视频信息，遥控器用于人为控制无人机。

如图2所示，基于双目视觉的多旋翼无人机选择性避障控制方法步骤如下：

1.通过双目摄像机获取无人机前方的环境图像；所述环境图像为双目摄像机的两个摄像头在同一时刻分别所拍摄的第一图像和第二图像；

2.根据获取的环境图像识别无人机前方是否有障碍物；

3.若存在障碍物，则基于无人机当前姿态信息以及根据图像获取的障碍物深度值；所述无人机姿态信息为设于无人机的姿态传感器所获取的无人机俯仰角，姿态传感器为加速度计、陀螺仪及磁力计；所述障碍物的深度值通过以下步骤获得：

3.1所述双目摄像机获取无人机前方的环境图像包括第一图像和第二图像；

3.2所述双目摄像机的两个摄像头通过张氏标定法，获取摄像头的内外参数，再通过摄像头的外部参数和畸变系数对第一图像和第二图像进行矫正；将矫正后的第一图像和第二图像使用SGAM算法进行立体匹配，得到两个图像的视差图；

3.3基于所述视差图获取深度图像，根据所述深度图像提取障碍物深度值；

3.4设定避障阈值l_max，识别小于l_max的障碍物并保留，去除l_max的障碍物。

4.若图像存在小于l_max的障碍物，树梅派进行判断是否避障，否则无人机保持原先路线继续飞行；

4.1基于图像存在小于l_max的障碍物，计算无人机与最近障碍物的水平距离d(见图3)：

若v₀-v≠0时，

式中，z为最近障碍物的竖轴坐标，y为最近障碍物的纵轴坐标，θ为俯仰角，v₀为图像中心的像素纵坐标，v为最近障碍物的像素纵坐标。

若v₀-v＝0时，

d＝zcos|θ| (2)

4.2如图4所示，计算无人机实际矩形框宽度w_fact和长度I_fact，

w_fact＝h cos|θ|+b sin|θ| (3)

式中，h为无人机高度，b为无人机宽度，θ为俯仰角，θ∈(-15°，15°)，向下为正，向上为负；I_fact＝l，其中l为无人机长度。

4.3如图5所示，计算像素飞行框的长度Δu和宽度Δv：深度值d时的图像中心点的视差为

D＝Bf/d (4)

式中，B为两个摄像头之间的距离，f为双目摄像机的焦距；

Δx＝lD/B (5)

式中，Δx为l在深度值d时的图像坐标系下的对应尺寸。

式中，Δu₁为l在深度值d时的像素坐标系下的对应的尺寸，dx为单位像素在横轴的物理尺寸；

式中，Δv₁为w_fact在深度值d时的像素坐标系下的对应的尺寸，dy为单位像素在纵轴的物理尺寸；

将Δu₁和Δv₁都扩大为原先的1.2倍，能减小外界因素影响和及时处理突发情况，则有

Δu＝1.2Δu₁,Δv＝1.2Δv₁ (8)

4.4如图6所示，当无人机倾斜时，获取像素飞行安全框的位子。设像素飞行安全框中心与像素飞行框中心连线的间距Δc：

Δe＝dtan|θ| (9)

式中，Δe为无人机实际框中心偏移量；

Δy_c＝DΔe/B (10)

式中，Δy_c为无人机在图像坐标系下的矩形框中心点的偏移量；

Δc＝Δy_c/dy＝f tan|θ|/dy (11)

4.5在像素坐标系下，若θ≥0°，将像素飞行框向正上方平移Δc得到像素飞行安全框；若θ＜0°，将像素飞行框向正下方平移Δc得到像素飞行安全框；

4.6遍历像素飞行安全框：(1)若像素飞行安全框内不存在障碍物，无人机向像素飞行安全框方向飞行；(2)若像素飞行安全框内存在障碍物，无人机进行相应选择性避障，具体步骤为：

4.6.1无人机开始减速慢行，记录像素飞行安全框的最近障碍物的深度值deep，进行选择相应的避障模式1：

无人机向上飞行，如果在向上飞行过程中像素飞行安全框内不存在障碍物，无人机按原先规划的航线飞行；如果向上飞行5米之后像素飞行安全框内存在障碍物，无人机立即减速悬停，并通过数传模块和图传模块发送信息，提示人为遥控；

4.6.2deep与设定的阈值set_d比较，无人机进行选择相应的避障模式2：

①deep大于set_d时进入缓行区，无人机调整俯仰角适当降低速度；

②deep小于或等于set_d时进入警报区，无人机立即减速悬停，并通过数传模块和图传模块发送信息，提示人为遥控。

上述的具体实施方式，对本发明的目的、技术方案和有益效果进行了详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.基于双目视觉的多旋翼无人机选择性避障控制方法，其特征在于，包括以下步骤：

步骤1：通过双目摄像机获取无人机前方的环境图像；所述环境图像为双目摄像机的两个摄像头在同一时刻分别所拍摄的第一图像和第二图像；

步骤2：根据获取的环境图像识别无人机前方是否有障碍物；

步骤3：若存在障碍物，则基于无人机当前姿态信息以及根据图像获取的障碍物深度值；所述无人机姿态信息为设于无人机的姿态传感器所获取的无人机俯仰角，姿态传感器为加速度计、陀螺仪及磁力计；

步骤4：设定避障阈值l_max，若图像存在小于l_max的障碍物，树梅派进行判断是否避障，否则无人机保持原先路线继续飞行。

2.如权力要求1所述的基于双目视觉的多旋翼无人机选择性避障控制方法，其特征在于，所述障碍物的深度值通过以下步骤获得：

1)所述双目摄像机获取无人机前方的环境图像包括第一图像和第二图像；

2)所述双目摄像机的两个摄像头通过张氏标定法，获取摄像头的内外参数，再通过摄像头的外部参数和畸变系数对第一图像和第二图像进行矫正；将矫正后的第一图像和第二图像使用SGAM算法进行立体匹配，得到两个图像的视差图；

3)基于所述视差图获取深度图像，根据所述深度图像提取障碍物深度值；

4)设定避障阈值l_max，识别小于l_max的障碍物并保留，去除大于或等于l_max的障碍物。

3.如权力要求1和权力要求2所述的基于双目视觉的多旋翼无人机选择性避障控制方法，其特征在于，若图像存在小于l_max的障碍物，树梅派通过如下几个步骤来判断是进行避障，还是保持原先路线继续飞行：

1)基于图像存在小于l_max的障碍物，计算无人机与最近障碍物的水平距离d：

若v₀-v≠0时，

式中，z为最近障碍物的竖轴坐标，y为最近障碍物的纵轴坐标，θ为俯仰角，v₀为图像中心的像素纵坐标，v为最近障碍物的像素纵坐标；

若v₀-v＝0时，

d＝zcos|θ| (2)

2)计算无人机实际矩形框宽度w_fact和长度I_fact：

w_fact＝hcos|θ|+bsin|θ| (3)

式中，h为无人机高度，b为无人机宽度，θ为俯仰角，θ∈(-15°，15°)，向下为正，向上为负；I_fact＝l，其中l为无人机长度；

3)计算像素飞行框的长度Δu和宽度Δv：深度值d时的图像中心点的视差为

D＝Bf/d (4)

式中，B为两个摄像头之间的距离，f为双目摄像机的焦距；

Δx＝lD/B (5)

式中，Δx为l在深度值d时的图像坐标系下的对应尺寸；

式中，Δu₁为l在深度值d时的像素坐标系下的对应的尺寸，dx为单位像素在横轴的物理尺寸；

式中，Δv₁为w_fact在深度值d时的像素坐标系下的对应的尺寸，dy为单位像素在纵轴的物理尺寸；

将Δu₁和Δv₁都扩大为原先的1.2倍，能减小外界因素影响和及时处理突发情况，则有

Δu＝1.2Δu₁,Δv＝1.2Δv₁ (8)

4)当无人机倾斜时，获取像素飞行安全框的位子；设像素飞行安全框中心与像素飞行框中心连线的间距Δc：

Δe＝dtan|θ| (9)

式中，Δe为无人机实际框中心偏移量；

Δy_c＝DΔe/B (10)

式中，Δy_c为无人机在图像坐标系下的矩形框中心点的偏移量；

Δc＝Δy_c/dy＝ftan|θ|/dy (11)

5)在像素坐标系下，若θ≥0°，将像素飞行框向正上方平移Δc得到像素飞行安全框；若θ＜0°，将像素飞行框向正下方平移Δc得到像素飞行安全框；

6)遍历像素飞行安全框：(1)若像素飞行安全框内不存在障碍物，无人机向像素飞行安全框方向飞行；(2)若像素飞行安全框内存在障碍物，无人机进行相应选择性避障。

4.如权力要求3所述的基于双目视觉的多旋翼无人机选择性避障控制方法，其特征在于，若像素飞行安全框内存在障碍物，无人机进行2种选择性避障模式；

1)无人机开始减速慢行，记录像素飞行安全框的最近障碍物的深度值deep，进行选择如下避障模式1：

无人机向上飞行，如果在向上飞行过程中像素飞行安全框内不存在障碍物，无人机按原先规划的航线飞行；如果向上飞行5米之后像素飞行安全框内存在障碍物，无人机立即减速悬停，并通过数传模块和图传模块发送信息，提示人为遥控；

2)deep与设定的阈值set_d比较，无人机进行选择如下避障模式2：

①deep大于set_d时进入缓行区，无人机调整俯仰角适当降低速度；

②deep小于或等于set_d时进入警报区，无人机立即减速悬停，并通过数传模块和图传模块发送信息，提示人为遥控。