基于双层声呐传感器的四旋翼无人机避障飞行装置及方法与流程

本发明涉及一种基于双层声呐传感器的四旋翼无人机避障飞行装置及方法，属于四旋翼无人机智能飞行控制领域。

背景技术：

四旋翼无人机(quadrotor)是一种新型的旋翼类无人机，四旋翼无人机具有操纵简单、起降要求低、成本低廉等优点。在过去十几年中，随着其他领域，如传感器、微处理器、材料科学技术、飞行控制理论的发展，四旋翼无人机更是迎来了迅猛的发展。当前，四旋翼无人机的飞行控制理论已经基本成熟，简单的飞行控制问题已经得到了解决，四旋翼无人机的应用范围越来越广泛，但是飞行安全性问题一直没能得到很好的解决，实现避障飞行就是提高飞行安全性的一种方法。目前四旋翼无人机一般都是通过操作者遥控控制的，在距离操作者很远，未知的飞行环境下，很容易碰撞到障碍物，从而坠毁，对公共财产和行人的安全造成威胁；当四旋翼无人机在室内飞行时，由于没有gps信号，或者操作者反应不及时，很容易飞行失去控制，对室内人员造成伤害。当四旋翼无人机没有解决避障飞行问题时，即使它的用途十分广泛，也会极大地限制四旋翼无人机的使用。

避障飞行是保证四旋翼无人机安全飞行的重要一步，它在最近几年随着四旋翼无人机的迅猛发展也成为了研究热点。当前的避障飞行技术很多，例如基于光流、基于目标特征和基于双目视觉的避障飞行技术，这些技术很多都是处于理论研究阶段，成本比较高，实用性不强。避障的关键是检测障碍物的距离，声呐传感器已经成功应用于移动机器人上的避障行走，该传感器硬件接口简单，工作频率可以满足实时性的要求，也可以用于四旋翼无人机的避障飞行。

技术实现要素：

本发明提出了一种基于双层声呐传感器的四旋翼无人机避障飞行装置及方法。

本发明为解决其技术问题采用如下技术方案：

一种基于双层声呐传感器的四旋翼无人机避障飞行装置及方法，包括双层动态障碍物检测模块、避障飞行决策模块和避障飞行微控制器，所述双层动态障碍物检测模块上下两层，每层装有一个舵机，每个舵机上安装有四个均匀分布的声呐传感器；所述避障飞行决策模块包括顺序连接的避障速度矢量生成模块、飞控输入转换模块和飞控通信模块，声呐传感器的数据通过ad转换接口传输给避障飞行微控制器，避障飞行微控制器和避障速度矢量生成模块连接。

所述声呐传感器固定在圆环上，每两个传感器之间间隔90°。

所述上下两组传感器的初始位置错开30°。

基于双层声呐传感器的四旋翼无人机避障飞行装置的控制方法，包括如下步骤：

(1)通过双层共8个声呐传感器读取0°,+30°,+60°,+90°,+120°,+150°,+180°,+210°,+240°,+270°,+300°,+330°共12个角度θ0-θ11的障碍物距离d0-d11；安装在上层舵机的上层传感器s1，s2，s3，s4的初始放置角度为0°,+90°,+180°,+270°，安装在下层舵机的下层的传感器s5，s6，s7，s8的初始放置角度为30°,+120°,+210°,+300°，每个检测周期通过ad转换接口读取一层共4个声呐传感器的数据，另外一层声呐传感器通过舵机带动顺时针转动60°，这样在3个检测周期共300ms内得到12个角度θ0-θ11的障碍物距离d0-d11；

(2)双层动态障碍物检测模块得到的d0-d11提供给避障飞行决策模块中的避障速度矢量生成子模块，避障速度矢量生成子模块首先在每两个相邻角度θi-θi+1，i＝0-10和θ11-θ0之间依据di和di+1，i＝0-10或者d11和d0确定一条新的候选飞行方向βi，i＝0-11,同时得到新的候选飞行方向的障碍物等效距离d’i,i＝0-11；避障速度矢量生成子模块得到了βi和d’i后从βi中依据代价估计函数f(βi,d’i)选择最佳飞行方向，k1,k2为权重系数，将代价估计函数值最小的飞行方向作为最佳飞行方向然后将最佳飞行方向转换为期望偏航角ψ，最后依据方向的障碍物等效距离d’i得到避障飞行速度vavoid，其中vaverage为巡航飞行速度，k3为安全系数；

(3)避障飞行决策模块中的飞控输入转换子模块将避障速度矢量中的期望偏航角ψ转换为期望偏航角速度计算公式为：d’i是方向的障碍物等效距离，vavoid为避障飞行速度，ψ为期望偏航角；

(4)避障飞行决策模块中的飞控通信子模块将vavoid和通过串口依据mavlink通信协议发送给飞控板，飞控板接收到避障飞行指令后会改变四个电机转速，改变飞行速度大小和偏航角速度，避开障碍物飞行。

步骤(2)中候选飞行方向βi，i＝0-10和障碍物等效距离d’i,i＝0-10依据θi和θi+1,i＝0-10角度方向的障碍物距离di和di+1，i＝0-10确定,β11和d’11依据θ11和θ0角度方向的障碍物距离d11和d0确定，具体分为四种情况：其中dsafe是飞行安全距离，当di为d11时，di+1为d0，当θi为θ11时，θi+1为θ0+360°，当βi＞+360°时，βi＝βi-360°，

1)di＞dsafe,di+1＞dsafe，也就是θi和θi+1角度方向均没有障碍物，βi＝θi，d’i＝di；

2)di＞dsafe,di+1＜dsafe，也就是θi角度方向没有障碍物，θi+1角度方向有障碍物，βi＝θi，d’i＝di；

3)di＜dsafe,di+1＞dsafe，也就是θi+1角度方向没有障碍物，θi角度方向有障碍物,

4)di＜dsafe,di+1＜dsafe，也就是θi和θi+1角度方向均有障碍物,

本发明的有益效果如下：

1、避障飞行控制系统硬件成本低，装置安装简单，易于实现。

2、在满足了声呐传感器数据读取最高频率要求的前提下，尽可能在最短的时间内得到12个角度的障碍物信息，实时性得到了保证。

3、通过8个声呐传感器和舵机的转动可以得到12个均匀间隔的角度的障碍物信息，减少了传感器的个数，节省了成本；仅仅使用了8路ad转换接口和2路pwm输出接口，避免了占用避障飞行微控制器过多的外围接口资源。

4、避障飞行决策算法原理简单，cpu占用时间不长，可以满足四旋翼无人机避障飞行的实时性要求。

5、避障飞行决策模块最终传输给飞控板的是避障飞行速度和期望偏航角速度，一般的飞控系统都提供了对应接口，可以将vavoid和作为飞控系统输入，故本发明可适用于不同飞控系统。

附图说明

图1是本发明的系统组成框图。

图2(a)是本发明的基于双层声呐传感器的上层障碍物检测装置，图2(b)是本发明的基于双层声呐传感器的下层障碍物检测装置。

图3是本发明的障碍物检测装置与四旋翼无人机的连接结构图。

图4是双层动态障碍物检测模块100ms定时中断服务例程流程图。

图5是避障飞行决策模块300ms定时中断服务例程流程图。

图中：1.上层声呐传感器s1，2.上层声呐传感器s2，3.上层声呐传感器s3，4.上层声呐传感器s4，5.下层声呐传感器s5，6.下层声呐传感器s6，7.下层声呐传感器s7，8.下层声呐传感器s8，9.上层传感器固定圆环，10.下层传感器固定圆环，11.上层舵机1，12.下层舵机2，13.下层连接杆1,14.上层连接杆2,15.下层固定件1,16.上层固定件2,17.四旋翼无人机机体。

具体实施方式

下面结合附图对本发明创造作进一步详细说明。

图1为本发明的系统组成框图，分为两个模块：双层动态障碍物检测模块以及避障飞行决策模块。

双层动态障碍物检测模块分为上下两层，每层安装有四个均匀分布的声呐传感器，读取后的数据通过ad转换接口传输给避障飞行微控制器。通过合理分布两层传感器的初始角度和舵机的转动可以实现在三个检测周期(300ms)内检测到12个均匀角度(分别为0°，+30°,+60°,+90°,+120°,+150°,+180°,+210°，+240°,+270°,+300°,+330°)的障碍物距离，每个传感器数据检测的持续有效时间为100ms。

为了实现在三个检测周期内读取间隔十二个角度的障碍物距离，两组声呐传感器的转动角度如表1所示：表1(其中s1-s8分别代表传感器1到传感器8；角度值代表该传感器在每个周期结束时所处的角度，这里机头对应角度为0°,正左方对应角度为+270°，正右方对应角度为+90°，机尾对应角度为+180°；上标为1代表该传感器已经转动到了指定位置，清空原有数据，开始读取新的障碍物距离数据；上标为2代表该传感器已经在指定位置读取了100ms的障碍物距离数据，结束数据读取，并且在接下来的100ms内转动到下一个位置)。

表1

在0ms时，上层的传感器s1，s2，s3，s4的初始放置角度为0°，+90°,+180°,+270o，开始读取数据；在100ms时，上层的传感器结束数据读取，开始顺时针转动60°，下层的传感器s5，s6，s7，s8的初始放置角度为+30°,+120°,+210°,+300°，开始读取数据；在200ms时，上层的传感器转动至指定角度，开始读取新的数据，下层的传感器结束数据读取，开始顺时针转动60°；在300ms时，下层的传感器转动至指定角度，开始读取新的数据，上层的传感器结束数据读取，开始顺时针转动60°。这样，就可以实现在100，200，300ms时读取第一组12个均匀角度的障碍物信息，在400，500，600ms时读取第二组12个均匀角度的障碍物信息，以后每300ms交替读取每组12个均匀角度的障碍物信息。

避障飞行决策模块分为以下三个子模块：避障速度矢量生成模块、飞控输入转换模块、飞控通信模块。

通过障碍物检测模块，得到了θ0-θ11角度方向的障碍物距离d0-d11，当di＞dsafe(i＝0-11)时，认为θi(i＝0-11)角度方向没有障碍物，di为θi角度方向的障碍物距离，dsafe是飞行安全距离。

避障速度矢量包括期望偏航角和避障飞行速度。避障速度矢量生成步骤如下：

(1)得到候选飞行方向βi(i＝0-11)和障碍物等效距离d’i(i＝0-11)。候选飞行方向βi(i＝0-10)和障碍物等效距离d’i(i＝0-10)依据θi和θi+1(i＝0-10)角度方向的障碍物距离di和di+1确定,β11和d11依据θ11和θ0角度方向的障碍物距离d11和d0确定,具体分为四种情况：当di为d11时，di+1为d0，当θi为θ11时，θi+1为θ0+360°，当βi＞+360°时，βi＝βi-360°，

1)di＞dsafe,di+1＞dsafe，也就是θi和θi+1角度方向均没有障碍物，βi＝θi，d’i＝di；

2)di＞dsafe,di+1＜dsafe，也就是θi角度方向没有障碍物，θi+1角度方向有障碍物，βi＝θi，d’i＝di；

3)di＜dsafe,di+1＞dsafe，也就是θi+1角度方向没有障碍物，θi角度方向有障碍物,

4)di＜dsafe,di+1＜dsafe，也就是θi和θi+1角度方向均有障碍物,

(2)得到最佳飞行方向建立代价估计函数f(βi,d’i)，从βi(i＝0-11)中选择最佳飞行方向。这里的第一项考虑的是尽量不改变当前飞行方向，第二项考虑的是尽量向远离障碍物的方向飞行，k1,k2为这两项的权重系数，可以依据具体飞行环境设定合适数值。将代价估计函数值最小的飞行方向作为最佳飞行方向。

(3)将最佳飞行方向转换为期望偏航角ψ，ψ和本质上是等价的，只不过的单位是角度，数据范围为0-360，ψ的单位是弧度，数据范围是-π-+π，具体转换关系是：

(4)依据方向的障碍物等效距离d’i得到避障飞行速度vavoid。避障飞行速度是由方向的障碍物等效距离d’i决定的，计算方法为：其中vaverage为巡航飞行速度，k3为安全系数，和当前的飞行速度有关，飞行速度越大时，k3值越大；

避障速度矢量中的避障飞行速度指的是前向期望飞行速度，四旋翼无人机的当前飞行速度是通过遥控器或者地面站控制的，当前飞行速度一般不等于期望飞行速度，为了使四旋翼无人机达到期望飞行速度，同时改变偏航角，需要飞控板上的飞控系统改变系统输入，一般的飞控系统都可以将期望飞行速度和期望偏航角速度作为系统输入，飞控输入转换模块需要将避障速度矢量中的期望偏航角ψ转换为期望偏航角速度首先需要得到避障飞行时间tavoid，是方向的障碍物等效距离，vavoid为避障飞行速度，然后就可以得到期望偏航角速度ψ为期望偏航角。

飞控输入转换模块只是得到了vavoid和最终需要飞控板改变四旋翼无人机的四个电机的转速实现避障飞行。当改变了飞控系统的输入后，飞控系统会改变四个电机转速，避障飞行微控制器仅仅需要将vavoid和发送给飞控板，一般都基于某种通信协议，目前比较常见的是mavlink通信协议，本发明也是使用的这种通信协议。

图2(a)、图2(b)分别为的基于双层声呐传感器的障碍物检测装置的上下两层，上下两层是对称的，每层均包含四个声呐传感器、一个舵机、一个传感器固定圆环、一个套杆、一个固定件。四个声呐传感器固定在传感器固定圆环的四周，每两个传感器之间间隔90°，舵机安装在传感器固定圆环的中央下方，通过舵机的转动可以带动传感器固定圆环的转动，从而带动四个传感器转动。连接杆用于连接舵机和固定件，方便安装。

图3是本发明的障碍物检测装置与四旋翼无人机的连接结构图。固定件固定在四旋翼机体的上下两侧中央，连接杆用于连接舵机和固定件，舵机安装在传感器固定圆环的中央下方，通过舵机的转动可以实现上下两层传感器的转动。上下两层的两组传感器初始位置需要错开，s1-s8的具体初始角度为：0°,+90°,+180°,+270°,+30°,+120°,+210°,+300°，这样可以实现在三个检测周期内检测到十二个角度的障碍物距离。

图4是双层动态障碍物检测模块100ms定时中断服务例程流程图，这里的a组传感器是指到达了指定角度还未开始读取数据的传感器组，b组传感器是指已经读取了100ms数据的传感器组。避障飞行微控制器输出不同占空比的pwm波至障碍物检测模块，读取障碍物检测模块的数据，需要通过周期为100ms中断服务例程来实现。每一个定时中断到来时，已经到达了指定角度的a组传感器清空原有数据并开始读取新的数据，已经读取了100ms的b组传感器停止读取数据，并且开始顺时针转动60°，在下一个100ms内转动至新的位置。这样每100ms读取四个角度的障碍物距离信息，那么300ms就可以读取一周十二个角度的障碍物距离信息。

图5是避障飞行决策模块300ms定时中断服务例程流程图。双层动态障碍物检测模块的工作周期是100ms，每三个工作周期才能得到完整的十二个角度的障碍物信息，所以避障飞行决策模块的工作周期为300ms，同样通过避障飞行微控制器的中断服务例程来实现。每一个定时中断到来时，依据θ0-θ11角度方向的的障碍物距离d0-d11，在每两个相邻角度θi-θi+1(i＝0-10)和θ11-θ0之间依据di和di+1或者d11和d0可以确定一条新的候选飞行方向βi(i＝0-11)，同时可以得到新的候选飞行方向的障碍物等效距离d’i(i＝0-11)；然后依据代价估计函数f(βi,d’i)，从βi中选择最佳飞行方向将βi转换为期望偏航角ψ，依据方向的障碍物等效距离di'得到避障飞行速度vavoid；然后可以得到避障飞行时间tavoid，最后依据tavoid可以得到期望偏航角速度也就是将避障速度矢量转换为飞控系统输入；最后避障飞行微控制器将vavoid和通过串口，依据mavlink协议传输给飞控板，飞控板改变飞行速度大小和偏航角速度，避开障碍物飞行。