[发明专利]基于双层声呐传感器的四旋翼无人机避障飞行装置及方法

权利要求书

1.一种基于双层声呐传感器的四旋翼无人机避障飞行装置的控制方法，该控制方法所采用的无人机避障飞行装置包括双层动态障碍物检测模块、避障飞行决策模块和避障飞行微控制器，所述双层动态障碍物检测模块上下两层，每层装有一个舵机，每个舵机上安装有四个均匀分布的声呐传感器；所述避障飞行决策模块包括顺序连接的避障速度矢量生成模块、飞控输入转换模块和飞控通信模块，声呐传感器的数据通过AD转换接口传输给避障飞行微控制器，避障飞行微控制器和避障速度矢量生成模块连接；所述声呐传感器固定在圆环上，每两个传感器之间间隔90°；所述上下两组传感器的初始位置错开30°；其特征在于，该控制方法包括如下步骤：

(1)通过双层共8个声呐传感器读取0°,+30°,+60°,+90°,+120°,+150°,+180°,+210°,+240°,+270°,+300°,+330°共12个角度θ₀-θ₁₁的障碍物距离d₀-d₁₁；安装在上层舵机的上层传感器S1，S2，S3，S4的初始放置角度为0°,+90°,+180°,+270°，安装在下层舵机的下层的传感器S5，S6，S7，S8的初始放置角度为30°,+120°,+210°,+300°，每个检测周期通过AD转换接口读取一层共4个声呐传感器的数据，另外一层声呐传感器通过舵机带动顺时针转动60°，这样在3个检测周期共300ms内得到12个角度θ₀-θ₁₁的障碍物距离d₀-d₁₁；

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

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

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

2.根据权利要求1所述的基于双层声呐传感器的四旋翼无人机避障飞行装置的控制方法，其特征在于，步骤(2)中候选飞行方向β_i，i＝0-10和障碍物等效距离d’_i,i＝0-10依据θ_i和θ_i+1,i＝0-10角度方向的障碍物距离d_i和d_i+1，i＝0-10确定,β₁₁和d’₁₁依据θ₁₁和θ₀角度方向的障碍物距离d₁₁和d₀确定，具体分为四种情况：其中d_safe是飞行安全距离，当d_i为d₁₁时，d_i+1为d₀，当θ_i为θ₁₁时，θ_i+1为θ₀+360°，当β_i＞+360°时，β_i＝β_i-360°，

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

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

3)d_i＜d_safe,d_i+1＞d_safe，也就是θ_i+1角度方向没有障碍物，θ_i角度方向有障碍物,

4)d_i＜d_safe,d_i+1＜d_safe，也就是θ_i和θ_i+1角度方向均有障碍物,