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

权利要求书

1.一种基于避障系统的消防无人机航迹规划方法，其特征在于：在无人机的前后左右四个方向设有超声波测距传感器，无人机飞行过程中的规划步骤如下：

步骤1：在无人机起飞后，无人机按照规划路径进行飞行，同时四个超声波测距传感器监测前方飞行中的突现障碍物距离信息，以PWM波的形式输出给Arduino单片机；

步骤2：安全距离确定，安全距离在初始设定的安全距离基础上，根据当前无人机飞行速度进行调整：

所述安全距离＝初始安全距离+k₀×当前飞行速度；

系数k₀为正系数，值取值范围为0.001～0.005；

步骤3：判断测量距离是否为安全距离：当测量距离不小于安全距离，则输出原始控制信号，飞行器按计划正常飞行；

测量距离小于安全距离，则油门输出信号和方向输出信号分别重构：

油门输出信号＝原始油门控制信号-k₁×(安全距离-测量距离)

方向输出信号＝原始方向控制信号-k₂×(安全距离-测量距离)

系数k₁和k₂为正系数，物理意义为避障效果强度，即k₁和k₂值越大则避障的反应越强烈，k₁和k₂的取值范围为0.05～0.1；

当发现无人机某一方向出现障碍物，判断即将碰撞，飞控系统将根据测量距离采取收油门、向障碍物反方向倾侧飞行动作避开障碍物；

所述规划路径的建立步骤：

步骤a1、建立起点至终点的原始数字地形；从中科院数据云中查找起点至终点数字地形数据，获取数字高程数据；在三维空间中构建单个高程值和它位于水平面的坐标x,y的对应的关系：

z1(x,y)＝f(x,y)

其中z1(x,y)为(x,y)的点的高程值，f是坐标和高程值的关系函数；

采用函数模拟法来模拟任务中已经确认的地形，使用的数学模型如下：

其中，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的规划模型，得到起点至终点的规划路径。