[发明专利]一种基于避障系统的消防无人机航迹规划方法

摘要

本发明涉及一种基于避障系统的消防无人机航迹规划方法，在无人机的前后左右四个方向设有超声波测距传感器，当测量距离不小于安全距离，则输出原始控制信号，飞行器按计划正常飞行；当发现无人机某一方向出现障碍物，判断即将碰撞，飞控系统将根据测量距离采取收油门、向障碍物反方向倾侧飞行等动作避开障碍物。本发明中所设计的避障算法同传统的局部避障规划算法(人工势场法、栅格法)相比较具有兼容性好，执行效率高的优点；基于超声波避障系统，采用A*搜索算法能够有效的降低机载控制器的计算量，并且能够保证无人机在突发情况下能够快速避障，并及时规划出新的航迹。

主权项

一种基于避障系统的消防无人机航迹规划方法，其特征在于：在无人机的前后左右四个方向设有超声波测距传感器，无人机飞行过程中的规划步骤如下：步骤1：在无人机起飞后，无人机按照规划路径进行飞行，同时四个超声波测距传感器监测前方飞行中的突现障碍物距离信息，以PWM波的形式输出给Arduino单片机；步骤2：安全距离确定，安全距离在初始设定的安全距离基础上，根据当前无人机飞行速度进行调整：所述安全距离＝初始安全距离+k0×当前飞行速度；系数k0为正系数，值取值范围为0.001～0.005；步骤3：判断测量距离是否为安全距离：当测量距离不小于安全距离，则输出原始控制信号，飞行器按计划正常飞行；测量距离小于安全距离，则油门输出信号和方向输出信号分别重构：油门输出信号＝原始油门控制信号‑k1×(安全距离‑测量距离)方向输出信号＝原始方向控制信号‑k2×(安全距离‑测量距离)系数k1和k2为正系数，物理意义为避障效果强度，即k1和k2值越大则避障的反应越强烈，k1和k2的取值范围为0.05～0.1；当发现无人机某一方向出现障碍物，判断即将碰撞，飞控系统将根据测量距离采取收油门、向障碍物反方向倾侧飞行等动作避开障碍物；所述规划路径的建立步骤：步骤a1、建立起点至终点的原始数字地形；从中科院数据云中查找起点至终点数字地形数据，获取数字高程数据；在三维空间中构建单个高程值和它位于水平面的坐标x,y的对应的关系：z1(x,y)＝f(x,y)其中z1(x,y)为(x,y)的点的高程值，f是坐标和高程值的关系函数；采用函数模拟法来模拟任务中已经确认的地形，使用的数学模型如下：z1(x,y)=sin(y+a)+b·sin(x)+cos(d·x2+y2)+e·cos(y)+f·sin(f·x2+y2)+g·cos(y)]]>其中，x,y为点坐标，z1是对应坐标点的地形高程值，a，b，c，d，e，f，g为相应常系数；步骤a2、构建威胁等效山峰模型：其中，x，y为威胁投在水平面之上的坐标，z2相对应的高程值；x0，y0为第i个威胁的坐标，xi(i)和yi(i)为第i个威胁朝x轴和v轴方向的衰减度，h(i)表示威胁的作用强度，N是山峰个数；步骤a3、数字地形图与威胁等效山峰图信息融合：z(x,y)＝max(z1(x,y),z2(x,y))其中，z1函数表示的是原地图里面对应点的高程值，z2函数是等效的山峰地图里面对应点的高程值；步骤a4、航迹规划模型的建模：fitness＝min(a1×f1+a2×f2+a3×f3+a4×f4)其中ai为性能指标函数fi的权系数；f1是为航迹长度的指标限制函数，f2为最小转弯半径的性能约束函数，f3是飞行高度的性能函数，f4是与已知威胁源的中心点的距离的约束函数；步骤a5：采用A*算法求解步骤4的规划模型，得到起点至终点的规划路径。