一种基于预设航线的地表近距离自主探查无人机的三维避障方法与流程

本发明涉及无人机自主避障领域，尤其涉及一种基于预设航线的地表近距离自主探查无人机的三维避障方法。

背景技术：

随着传感技术和嵌入式系统的快速发展，无人机自主性日益提高，无人机导航日益成熟，但是在实际应用中，环境复杂，无人机难免会遇到障碍物，是否具有自主避障能力就成了衡量无人机自主性的最主要因素，因此无人机避障成为了当前的研究热点。

目前国内外学者们关于无人机避障的研究主要是基于二维空间的无人机避障，避障方法层出不穷。此类避障方法旨在消除无人机与障碍物之间的碰撞，进行避障轨迹优化。但是在实际应用中为提高无人机的自主性，避障往往伴随着航线跟随，因此目前大多避障方法无法完成基于预设航线的避障；通常高空避障时，绕过障碍物基本能避开所有障碍物，目前大部分避障方法研究的是绕开障碍物的二维避障，针对地面近距离自主探查无人机的任务需求，无人机需要在地表执行任务，地表障碍物情况复杂，采用绕过障碍物(二维避障)会增大无人机的探查盲区。

总之，目前的避障技术在无人机执行地表近距离自主探查时存在弊端有：无法结合预设航线，自主性较低；避障多为二维，探查盲区增大。

技术实现要素：

本发明要解决的技术问题在于针对现有技术中的缺陷，提供一种基于预设航线的地表近距离自主探查无人机的三维避障方法。

本发明解决其技术问题所采用的技术方案是：

本发明提供一种基于预设航线的地表近距离自主探查无人机的三维避障方法，该方法包括以下步骤：

步骤1、无人机进行地表近距离自主探查任务时，根据任务需求，选取人工势场法作为避障算法；

步骤2、对无人机预设航线，并建立以预设航线中出发点到目标点方向上的坐标系；

步骤3、对人工势场法进行改进并提出避障原则：引入线势场，使无人机在有效规避障碍物后重新回到预设航线；对斥力势场进行改进，使无人机自主逃离局部极小点；

步骤4、根据灾后场景，搭建无人机执行任务的避障场景，并在避障场景中，将障碍物近似为球型，构建障碍物场景；

步骤5、在避障场景内，对无人机执行任务时的轨迹和速度进行三维仿真分析，验证方法的可行性。

进一步地，本发明的步骤2中建立坐标系的方法具体为：

根据预设航线，以出发点到目标点方向在大地平面投影为x轴方向，y轴在大地平面与x轴垂直，z轴方向与x轴、y轴均垂直，且z轴方向朝向大地平面上方。

进一步地，本发明的步骤3中引入线势场的方法具体为：

基于预设航线时，目标点变为目标线，点势场变为线势场，引力大小不变，方向变为指向预设航线；

当预设航线为直线时，引力为：

当预设航线为曲线时，单纯的位置反馈控制无法消除曲线跟踪的误差，这里引进前馈和速度控制，引力为：

其中，为无人机所受引力，ρline(g)为无人机距离预设直线航线的距离，▽ρ(g)为无人机指向预设航线的单位矢量；ρcurve(g)为无人机距离预设曲线航线的距离，vx为无人机沿x轴方向的速度，δt为控制周期，δvy为预设轨迹和实际轨迹的y向速度差，θ为速度差系数；q为无人机当前位置，ρ(g)为无人机距离目标点距离，ρ(q)为无人机距离障碍物表面距离，ρ0为障碍物有效影响范围，ξ为引力场系数，η为斥力场系数，▽ρ(q)是单位矢量，方向为斥力场方向。

进一步地，本发明的步骤3中斥力场改进的方法具体为：

对斥力场进行改进，调整斥力方向为与垂直于航线方向，经过改进之后的人工势场法能自主逃离局部最小点，局部最小点是无人机所受引力和斥力合力为零的点，在该点处，无人机有沿x轴的速度，下一周期，无人机x轴的位置发生变化，所受合力不再为零，即逃离局部最小点；斥力为：

其中，为无人机所受斥力，ρ(g)为无人机距离目标点距离，ρ(q)为无人机距离障碍物表面距离，ρ0为障碍物有效影响范围，η为斥力场系数，▽ρ(q)是单位矢量，方向为斥力场方向。

进一步地，本发明的步骤3中无人机按照预设航迹进行飞行时采用的避障原则为：

设定一个避障阈值h，h为无人机定高最大稳定高度，如果无人机检测到障碍物高度大于h，则绕过，若小于h，则从障碍物上方越过；将改进后的人工势场法分别运用在无人机绕过y轴方向或越过z轴方向的避障上。

进一步地，本发明的步骤4中进行避障场景构建时采用的软件为机械系统动力学自动分析软件adams。

进一步地，本发明的步骤5中进行三维仿真分析的方法具体为：

设置仿真参数并进行避障仿真及分析，无人机三维避障采用仿真控制周期为t，进行改进方法验证以及结果的轨迹分析和无人机各向速度分析，验证方法的可行性。

本发明产生的有益效果是：本发明的基于预设航线的地表近距离自主探查无人机的三维避障方法，1、对人工势场法进行改进，将点引力势场改进为线引力势场，并针对预设航线为曲线时引入了速度控制和前馈控制，并改进斥力势场，优化了无人机的避障轨迹，无差跟踪预设航线，实现了基于预设航线的无人机避障，提高了自主性；2、将改进后的人工势场法运用到无人机的y轴和z轴避障上，并给出了如何权衡越过或者绕过的问题，实现了无人机的近地面三维避障，并能降低无人机的探查盲区。因此，本发明实现了基于预设航线的地表近距离自主探查无人机的三维避障，提高了无人机的自主性，并降低了其探查的盲区，提高了探查效率。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1为线势场引入下无人机所受引力；

图2为基于曲线航线的无人机受力情况；

图3为为泥石流灾后场景及障碍物形状；

图4为地震灾后场景及障碍物形状；

图5为基于直线航线的无人机三维避障仿真；

图6为基于直线航线无人机三维避障仿真xy轴投影；

图7为基于直线航线无人机三维避障仿真xz轴投影；

图8为基于直线航线三维避障无人机三轴速度；

图9为线势场改进前后无人机跟踪轨迹对比；

图10为线势场改进前后无人机速度对比；

图11为基于曲线航线的无人机三维避障仿真；

图12为基于曲线航线的无人机三维避障仿真xy轴投影；

图13为基于曲线航线的无人机三维避障仿真xz轴投影；

图14为基于曲线航线三维避障无人机三轴速度。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

实施例1：

如图1所示，本发明实施例的基于预设航线的旋翼无人机三维避障方法，旋翼无人机三维避障方法的步骤为：

步骤1、根据任务需求，选取合适的避障算法：无人机任务为地表近距离自主探查，预设航线为直线航线，对避障实时性要求较高，选用人工势场法；

步骤2、基于预设航线的无人机避障坐标系建立：任务为地表近距离自主探查，预设航线为直线航线，建立坐标系为以出发点到目标点方向在大地平面投影为x轴方向，y轴在大地平面与x轴垂直，z轴方向向上。

步骤3、针对传统人工势场法在任务中的不足进行改进并提出避障原则：

线势场引入：传统人工势场法的引力场由目标点产生，目的是引导无人机到达目标点；任务要求为基于直线预设航线时，目标点变为目标直线，点势场变为线势场，引力大小不变，方向变为指向预设航线。当预设航线为直线时，引力为：

斥力场改进：

由于传统人工势场法存在局部最小点，引力场改进之后这个问题也没有得到解决，本发明对斥力场进行改进，调整斥力方向为与垂直于航线方向。经过改进之后的人工势场法能够自主逃离局部最小点。局部最小点是无人机所受引力和斥力合力为零的点，在该点处，无人机有沿x轴的速度，下一周期，无人机x轴的位置发生变化，所受合力不再为零，即逃离局部最小点。斥力为：

公式中出现的参数包括：为无人机所受引力，为无人机所受斥力，ρline(g)为无人机距离预设直线航线的距离，▽ρ(g)为无人机指向预设航线的单位矢量；ρcurve(g)为无人机距离预设曲线航线的距离，vx为无人机沿x轴方向的速度，δt为控制周期，δvy为预设轨迹和实际轨迹的y向速度差，θ为速度差系数；q为无人机当前位置，ρ(g)为无人机距离目标点距离，ρ(q)为无人机距离障碍物表面距离，ρ0为障碍物有效影响范围，ξ为引力场系数，η为斥力场系数，▽ρ(q)是单位矢量，方向为斥力场方向。

基于无人机任务为自主探查，其前提是无人机定高飞行。本发明为了使无人机尽量跟踪预设航线，减少探查盲区，采用“尽量越过，否则绕过”的避障原则。设定一个避障阈值h(h为无人机定高最大稳定高度)，如果无人机检测到障碍物高度大于h，则绕过，若小于h，则从障碍物上方越过。将改进后的人工势场法分别运用在无人机y轴(绕过)z轴(越过)避障上。

步骤4、搭建避障场景：无人机任务区域为灾后场景，灾后场景如图3和4所示，图中障碍物为多边形障碍物，可近似为球，避障场景构造时，以球型作为障碍物，依照障碍物的大小分别设置低矮型障碍物和较高障碍物，仿真场景如图。

步骤5、设置仿真参数并进行避障仿真及分析：无人机三维避障采用仿真控制周期为δt＝0.1s，进行改进方法验证以及结果的轨迹分析和无人机各向速度分析，验证方法的可行性。

仿真参数设置：

a)无人机和障碍物信息设置：无人机用半径0.5m的单位质量球代替，出发点(-30,0,0)，低矮障碍物半径2m，位于(-25,0,0)，三个较高障碍物半径依次为5m，5m，6m，位置依次为(-15,1,0)，(1,3,0)，(15,7,0)。

b)预设航线信息设置：预设航线为航点(-30,0,0)到航点(30,0,0)之间的直线航线。

c)改进算法参数设置：引力为斥力为：

避障阈值h＝2m，无人机初始速度vx＝1m/s。

仿真分析：

为了分析本发明避障方法是否适用，将分析无人机的避障轨迹是否平滑，是否有较大不可控速度出现。无人机避障曲线三维视图和其分别在xy平面和xz平面投影如图5,6,7所示，无人机避障过程中三维速度如图8所示。从图中得无人机避障轨迹平滑，且各向速度均处于较小水平(<＝2m/s)，本发明能够完成基于直线航线的无人机三维避。

实施例2：

本发明实施例的基于预设航线的旋翼无人机三维避障方法，旋翼无人机三维避障方法的步骤为：

步骤1、根据任务需求，选取合适的避障算法：无人机任务为地表近距离自主探查，预设航线为曲线航线，对避障实时性要求较高，选用人工势场法；

步骤2、基于预设航线的无人机避障坐标系建立：任务为地表近距离自主探查，预设航线为曲线航线，建立坐标系为以出发点到目标点方向在大地平面投影为x轴方向，y轴在大地平面与x轴垂直，z轴方向向上。

步骤3、针对传统人工势场法在任务中的不足进行改进并提出避障原则：

线势场引入：传统人工势场法的引力场由目标点产生，目的是引导无人机到达目标点；当预设航线为直线时，目标点变为目标直线，点势场变为线势场，引力大小不变，方向变为指向预设航线。任务要求预设航线为曲线时，单纯的位置反馈控制无法消除曲线跟踪的误差，这里引进前馈和速度控制，引力为：

斥力场改进：

由于传统人工势场法存在局部最小点，引力场改进之后这个问题也没有得到解决，本发明对斥力场进行改进，调整斥力方向为与垂直于航线方向。经过改进之后的人工势场法能够自主逃离局部最小点。局部最小点是无人机所受引力和斥力合力为零的点，在该点处，无人机有沿x轴的速度，下一周期，无人机x轴的位置发生变化，所受合力不再为零，即逃离局部最小点。斥力为：

基于无人机任务为自主探查，其前提是无人机定高飞行。本发明为了使无人机尽量跟踪预设航线，减少探查盲区，采用“尽量越过，否则绕过”的避障原则。设定一个避障阈值h(h为无人机定高最大稳定高度)，如果无人机检测到障碍物高度大于h，则绕过，若小于h，则从障碍物上方越过。将改进后的人工势场法分别运用在无人机y轴(绕过)z轴(越过)避障上。

步骤4、搭建避障场景并进行曲线跟踪验证：无人机任务区域为灾后场景，灾后场景如图3和4所示，图中障碍物为多边形障碍物，可近似为球，避障场景构造时，以球型作为障碍物，依照障碍物的大小分别设置低矮型障碍物和较高障碍物，仿真场景如图。在adams中构建障碍物场景以验证曲线航线的跟踪效果。

仿真场景设置三个单位质量的小球代替无人机，分别为预设轨迹球，前馈+反馈的线势场跟踪球和单纯反馈的跟踪球。三球均给定x向的恒定速度vx＝1m/s，预设轨迹球给定y方向的正弦位移y＝5sin(0.1×π×t)(以正弦曲线为例)，两跟踪球受到线势场作用力，分别为：

控制周期δt＝1s。

分析两种方法的跟踪轨迹和各向速度。由图9和10可以得到，线势场改进前，轨迹跟踪有抖动，且速度抖动太大，无法严格跟踪预设航线；线势场改进后，轨迹跟踪误差基本消除，速度也与预设航线保持一致，无人机能够快速消除位置误差，跟踪效果良好，满足曲线轨迹跟踪需求。

步骤5、设置仿真参数并进行避障仿真及分析：无人机三维避障采用仿真控制周期为δt＝0.1s，进行改进方法验证以及结果的轨迹分析和无人机各向速度分析，验证方法的可行性。

仿真参数设置：

a)无人机和障碍物信息设置：无人机用半径0.5m的单位质量球代替，出发点(-30,0,0)，低矮障碍物半径2m，位于(-15,-7,0)，三个较高障碍物半径依次为5m，5m，6m，位置依次为(-15,1,0)，(1,3,0)，(15,7,0)。

b)预设航线信息设置：预设航线为航点(-30,0,0)到航点(30,0,0)之间的正弦曲线，轨迹为y＝8sin(0.1×π×t)。

c)改进算法参数设置：引力为斥力为：

避障阈值h＝2m，无人机初始速度vx＝1m/s。

仿真分析：

为了分析本发明避障方法是否适用，将分析无人机的避障轨迹是否平滑，是否有较大不可控速度出现。无人机避障曲线三维视图和其分别在xy平面和xz平面投影如图11,12,13所示，无人机避障过程中三维速度如图14所示。从图中得无人机避障轨迹平滑，且各向速度均处于较小水平(<＝7m/s)，本发明能够完成基于曲线航线的无人机三维避障。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。