CN107577241A - 一种基于避障系统的消防无人机航迹规划方法 - Google Patents

Abstract

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

Description

一种基于避障系统的消防无人机航迹规划方法

技术领域

本发明属于消防无人机领域，涉及一种基于避障系统的消防无人机航迹规划方法。

背景技术

无人机在城市高层建筑火灾的侦查、救援任务中具有很强的优势，为了能够快速准确的到达火灾现场，必须保证消防无人机能够以最少时间、燃料消耗以及环境威胁等条件飞往目的地，并能够保证无人机对突然出现的障碍物具有快速响应能力。

常规的无人机规划算法多采用动态规划法、A*搜索法、Voronoi图算法、人工势能法、蚁群算法、遗传算法等。它们之间各有优缺点，在一定条件下都能实现无人机的航迹规划，但是受复杂的飞行环境影响，无人机航迹规划问题的目标函数过于复杂，机载控制器需要处理大量的信息，在面临突发事件情况下，难以满足无人机避障系统的快速响应要求。

发明内容

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种基于避障系统的消防无人机航迹规划方法，针对现有无人机航迹规划中存在的问题做出改进。

技术方案

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

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

有益效果

本发明提出的一种基于避障系统的消防无人机航迹规划方法，在无人机的前后左右四个方向设有超声波测距传感器，当测量距离不小于安全距离，则输出原始控制信号，飞行器按计划正常飞行；当发现无人机某一方向出现障碍物，判断即将碰撞，飞控系统将根据测量距离采取收油门、向障碍物反方向倾侧飞行等动作避开障碍物。

本发明中所设计的避障算法同传统的局部避障规划算法(人工势场法、栅格法)相比较具有兼容性好，执行效率高的优点；

本发明提出的超声波避障系统采用结构较为简单的超声波模块传感器，相比于常规的视觉传感器具有更加紧凑的结构布局形式和更小的结构质量，同时还能减小无人机的功耗。

采用超声波模块距离传感器带宽大，响应速度快的特点保证了系统响应的快速性，同时其测量距离具有较高的分辨率，保证了系统的高精度；

基于超声波避障系统，采用A*搜索算法能够有效的降低机载控制器的计算量，并且能够保证无人机在突发情况下能够快速避障，并及时规划出新的航迹。

附图说明

图1为本发明采用的一种超声波发射接收模块原理图；

图2为本发明采用的Arduino单片机控制板原理图；

图3为本发明公开的一种避障算法流程图；

图4为原始数字地图示例；

图5为威胁等效山峰示例图；

图6为融合后的示例图；

图7为航迹规划算法流程图；

图8为航迹规划后的仿真显示结果。

具体实施方式

现结合实施例、附图对本发明作进一步描述：

为了实现消防无人机快速避障，本发明专利将综合超声波模块传感器测量数据与Arduino单片机控制系统，提出一种快速避障方法，将超声波模块传感器与Arduino单片机配合使用实现消防无人机在复杂飞行环境中的快速避障功能。

为了实现消防无人机按照预定轨迹自主飞行时能够快速避障的功能，本发明专利将基于超声波避障系统，采用A*搜索算法来进行无人机的航迹规划。

下面结合实施例并参照附图对本发明作进一步描述。

本发明设计了一套避障方案，执行步骤为：

步骤1：在无人机起飞后，无人机按照规划路径进行飞行，如图1所示，基于超声波模块工作原理，四个超声波测距传感器得到的飞行中的突现障碍物距离信息，以PWM波的形式输出给Arduino单片机；

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

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

系数k₀为正系数，反映了无人机飞行的安全级别，即k₀值越大则安全级别越高，k₀的取值需要结合实验数据进行确定，本专利通过测试，k₀值取值范围为0.001～0.005。初始安全距离本专利取值为1.3m。

步骤3：参照3程序流程图，判断测量距离是否为安全距离：

当测量距离不小于安全距离，则输出原始控制信号，飞行器按计划正常飞行；

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

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

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

系数k₁和k₂为正系数，物理意义为避障效果强度，即k₁和k₂值越大则避障的反应越强烈，k₁和k₂的取值需要结合实验数据进行确定，本专利通过测试，k₁和k₂值取值范围为0.05～0.1。

如图2所示，单片机控制板根据采集的距离信号和PWM标准信号进行信号判断处理后，重构出控制信号并转发为对应的PPM信号输出给飞行控制系统；当发现无人机某一方向出现障碍物，判断即将碰撞，飞控系统将根据测量距离采取收油门、向障碍物反方向倾侧飞行等动作避开障碍物。

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

步骤a1：建立起点至终点的原始数字地形；

步骤a2：构建威胁等效山峰模型；

步骤a3：数字地形图与威胁等效山峰图信息融合；

步骤a4：航迹规划模型的建模；

步骤a5：采用A*算法求解步骤4的规划模型，得到起点至终点的规划路径。

进一步的，步骤a1具体为，使用的数字地形数据能够从中科院数据云中查找，可以从中下载需要的DEM数字高程数据(STRM数据)。

获取中科院数据云中DEM数字高程数据后，在三维空间中单个高程值和它位于水平面的坐标x,y都有着相对应的关系：

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

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

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

其中，x,y为点坐标，z1是对应坐标点的地形高程值，a，b，c，d，e，f，g为相应常系数。

把常系数取不同的值能够模拟出各种形状的山坡地形，来当成已探明的任务区的地形情况。若取a＝10，b＝0.2，c＝0.1，d＝0.6，e＝1，f＝0.1，g＝0.1，则原始数字地图效果图如图4所示。

进一步的，步骤a2具体为，本发明采取下面公式来构建威胁等效山峰模型。

其中，x，y为威胁投在水平面之上的坐标，z2相对应的高程值。x0，y0为第i个威胁的坐标，xi(i)和yi(i)为第i个威胁朝x轴和y轴方向的衰减度，h(i)表示威胁的作用强度，N是山峰个数，示例图像如图5所示。

进一步的，步骤a3具体为，把原始数字地形图与威胁等效山峰图相融合之后得出综合等效数字高程图，所采用的数学模型为：

z(x,y)＝max(z1(x,y),z2(x,y)) (3)

其中，z1函数表示的是原地图里面对应点的高程值，z2函数是等效的山峰地图里面对应点的高程值，经过融合后的示例图像如图6所示。

进一步的，步骤a4具体为，在等效数字地形中，在给定起始点和终点时进行航迹规划，以无人机性能及生存概率为主要考虑因素作为约束条件，构建目标函数如下：

其中ai为性能指标函数fi的权系数，ai的取值根据所关注的指标重要程度来取值；f1是为航迹长度的指标限制函数，f2为最小转弯半径的性能约束函数，f3是飞行高度的性能函数，f4是与已知威胁源的中心点的距离的约束函数；

进一步的，步骤a5具体为，为了能够缩短搜索所用的时间，搜索的空间定义为初始点至目标点间的空间。数字地图由规划空间表示，把航迹的搜索转化成二维节点的寻优。

使用公式(5)给出的评价函数，给出初始点和终点之后，在给定的规划空间中，对下一个航迹点进行评估，从而找到最小代价点，然后再当成当前节点，继续进行下一个航迹点的寻找，直到到达目标点为止。

按照图7所示的航迹规划算法的程序流程，仿真计算。距离代价、最小转弯半径的性能代价、飞行高度的性能代价等的权系数分别取0.001、0.01、0.1、0.8；离威胁中心的安全距离r_safe＝10m；安全离地高度设为：50m；起点坐标为(20,250)，终点坐标为(450,450)。规划出的轨迹如图8所示。

本发明公开的基于避障系统的消防无人机航迹规划方法具有以下有益效果：

本发明中所设计的避障算法同传统的局部避障规划算法(人工势场法、栅格法)相比较具有兼容性好，执行效率高的优点；

本发明提出的超声波避障系统采用结构较为简单的超声波模块传感器，相比于常规的视觉传感器具有更加紧凑的结构布局形式和更小的结构质量，同时还能减小无人机的功耗。

采用超声波模块距离传感器带宽大，响应速度快的特点保证了系统响应的快速性，同时其测量距离具有较高的分辨率，保证了系统的高精度；

基于超声波避障系统，采用A*搜索算法能够有效的降低机载控制器的计算量，并且能够保证无人机在突发情况下能够快速避障，并及时规划出新的航迹。

Claims (1)

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是坐标和高程值的关系函数；

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

<mfenced open = "" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>(</mo> <mrow> <mi>x</mi> <mo>,</mo> <mi>y</mi> </mrow> <mo>)</mo> <mo>=</mo> <mi>sin</mi> <mo>(</mo> <mrow> <mi>y</mi> <mo>+</mo> <mi>a</mi> </mrow> <mo>)</mo> <mo>+</mo> <mi>b</mi> <mo>&amp;CenterDot;</mo> <mi>sin</mi> <mo>(</mo> <mi>x</mi> <mo>)</mo> <mo>+</mo> <mi>cos</mi> <mrow> <mo>(</mo> <mi>d</mi> <mo>&amp;CenterDot;</mo> <msqrt> <mrow> <msup> <mi>x</mi> <mn>2</mn> </msup> <mo>+</mo> <msup> <mi>y</mi> <mn>2</mn> </msup> </mrow> </msqrt> <mo>)</mo> </mrow> <mo>+</mo> <mi>e</mi> <mo>&amp;CenterDot;</mo> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mrow> <mo>(</mo> <mi>y</mi> <mo>)</mo> </mrow> <mo>+</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>f</mi> <mo>&amp;CenterDot;</mo> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mrow> <mo>(</mo> <mi>f</mi> <mo>&amp;CenterDot;</mo> <msqrt> <mrow> <msup> <mi>x</mi> <mn>2</mn> </msup> <mo>+</mo> <msup> <mi>y</mi> <mn>2</mn> </msup> </mrow> </msqrt> <mo>)</mo> </mrow> <mo>+</mo> <mi>g</mi> <mo>&amp;CenterDot;</mo> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mrow> <mo>(</mo> <mi>y</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced>

其中，x,y为点坐标，z1是对应坐标点的地形高程值，a，b，c，d，e，f，g为相应常系数；

步骤a2、构建威胁等效山峰模型：

<mrow> <msub> <mi>z</mi> <mn>2</mn> </msub> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <mi>y</mi> <mo>)</mo> </mrow> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <mi>h</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> <mo>*</mo> <mi>exp</mi> <mo>(</mo> <mo>-</mo> <mo>(</mo> <mrow> <mi>x</mi> <mo>-</mo> <msub> <mi>x</mi> <mn>0</mn> </msub> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </mrow> <mo>)</mo> <mo>/</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>(</mo> <mi>i</mi> <mo>)</mo> <mo>-</mo> <mo>(</mo> <mrow> <mi>y</mi> <mo>-</mo> <msub> <mi>y</mi> <mn>0</mn> </msub> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> <mo>/</mo> <msub> <mi>y</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </mrow> <mo>)</mo> </mrow>

其中，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)

<mrow> <mi>s</mi> <mo>.</mo> <mi>t</mi> <mo>.</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>h</mi> <mo>&amp;Element;</mo> <mi>z</mi> <mrow> <mo>(</mo> <mi>x</mi> <mi>i</mi> <mo>,</mo> <mi>y</mi> <mi>i</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>r</mi> <mi>i</mi> <mo>&gt;</mo> <msub> <mi>r</mi> <mrow> <mi>m</mi> <mi>i</mi> <mi>n</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>h</mi> <mo>&lt;</mo> <msub> <mi>h</mi> <mi>min</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>d</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>)</mo> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>&gt;</mo> <msub> <mi>r</mi> <mrow> <mi>s</mi> <mi>a</mi> <mi>f</mi> <mi>e</mi> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>

其中ai为性能指标函数fi的权系数；f1是为航迹长度的指标限制函数，f2为最小转弯半径的性能约束函数，f3是飞行高度的性能函数，f4是与已知威胁源的中心点的距离的约束函数；

步骤a5：采用A*算法求解步骤4的规划模型，得到起点至终点的规划路径。