CN113282103A - 基于改进自适应调整阈值势场法的无人机冲突探测和解脱方法 - Google Patents

Abstract

本发明公开了一种基于改进自适应调整阈值势场法的无人机冲突探测和解脱方法，用于解决现有技术中无人机路线规划非动态调整障碍物躲避的范围导致飞行效率低下且安全性较差的问题。本发明利用势场法的及时性，避免采用基于启发函数的算法而由于计算量庞大等问题错过避让时机。同时采用动态避让半径规划，避免传统方法一刀切的设定避让半径，平衡飞行安全及空域利用率。保证无人机可以在安全的情况下少避让，在危险的时候提前采取避让措施。另外，本方法由于其时效性优越，基于不同的避让半径，可以在多障碍物环境下高效稳定的选择出避让实际及角度。

Description

基于改进自适应调整阈值势场法的无人机冲突探测和解脱 方法

技术领域

本发明涉及一种基于改进自适应调整阈值势场法的无人机冲突探测和解脱方法，属于飞行器路线规划技术领域。

背景技术

随着无人机应用潜力被发掘，无人机近年来在数量和种类上急剧增长。然而空域资源有限，导致无人机遇到冲突的概率也随之增加，所以合理平衡飞行安全及空域运行效率迫在眉睫。为实现这一目的，涌现了很多冲突解脱算法，可以使得无人机躲避飞行过程中的移动障碍物，并在短时间内进行合理避让，避让后回归目标方向，确保飞行安全。在众多无人机冲突解脱算法中，主要分为基于启发函数的解脱算法或基于势场的解脱算法。

基于启发函数的解脱算法以A*，粒子群算法等为主，他们的特点都是以当前位置为基础，将其所有可能到达的下一个位置记录在案作为集合。将集合中的位置分别取出，计算各位置距起点的固定成本与该位置和目标点的启发函数结果之和。选取所有位置的计算结果最小的位置为下一目标位置。由于固定成本计算简单且形式统一，该方法的效果则极大依赖于启发函数的建立，一个不好的函数不仅会增大计算量，增加计算时间，甚至将不好的位置方向作为接下来的位置选择，进而影响无人机的安全。一个缺乏考量的启发式函数虽然也能得到结果，但可能错过最优解，所以出现了另一种基于迭代的A*算法。该方法虽然可以避免错过最优解，但是仍不可避免当函数设计不好时，计算量增大，计算时间增长，甚至由于迭代的原因，会进行大量的重复计算，其时效性及其不好，更适用于理论层面。在现时层面，很可能因为计算时间的原因而无法及时做出决策。基于势场的解脱算法，以引力势场算法为主，势场法有别与启发函数法，其时效性尤其突出，可以及时得出下一运动位置，所有这种方法被广泛应用于对时间敏感的避障等问题。但是传统的势场法常常以经验为依据，一刀切的以固定距离作为势场范围，针对同一个避让模型，无论无人机与障碍物的速度，方向如何，其势场作用域均相同，针对相同距离的不同障碍物，无人机均采取相同避让策略。

由此，空域运行效率被大大牺牲，一些可不进行避让的情况会出现避让行为，增加了无人机的飞行距离，增长了总飞行时间，造成了空域效率下降。由于其时效性快，常规做法以固定时间间隔作为重新计算势场结果的触发条件，时间间隔设置过密，无人机运动轨迹过于曲折，时间间隔设置过疏，无人机可能避让不及时。

发明内容

本发明所要解决的技术问题是提供一种基于改进自适应调整阈值势场法的无人机冲突探测和解脱方法，用于解决现有技术中无人机路线规划非动态调整障碍物躲避的范围导致飞行效率低下且安全性较差的问题。

为解决上述问题，本发明所采取的技术方案是：

一种基于改进自适应调整阈值势场法的无人机冲突探测和解脱方法，包括以下步骤：S100：获取初始避让半径；

S200：建立以无人机为中心的坐标系，获取坐标系中障碍物的标记信息；

S300：根据障碍物的标记信息计算自适应避让半径；

S400：根据各障碍物的自适应避让半径，规划以障碍物为圆心、以自适应避让半径为半径的规划冲突解脱区域；

S500：如无人机进入冲突解脱区域，对有效障碍物计算其危险半径，有效障碍物为无人机进入冲突解脱区域内的障碍物；

S600：根据无人机最小转弯半径、有效障碍物危险半径以及飞行方向规划可飞行区域；S700：根据障碍物危险半径以及障碍物运动方向规划扩充危险区；

S800：根据过危险扩充区计算障碍物斥力、引力；

S900：无人机沿最小转弯半径运动直至离开冲突解脱区域。

作为本发明的进一步改进，步骤S100中，所述初始避让半径为计算动态自适应半径的初始避让半径r_init。

作为本发明的进一步改进，步骤S200中，所述坐标系为笛卡尔直角坐标系，无人机运动方向为y轴，垂直于y轴方向为x轴，并坐标系中标记空间中所有障碍物位置及速度大小及方向。

作为本发明的进一步改进，步骤S300中，自适应避让半径计算公式如下：

其中，|v|:飞机与障碍的相对速度；γ：角度风险系数；θ：为飞机与障碍的相对速度与飞机与障碍连线夹角所对应弧度值；DI：飞机与障碍间距离；σ：综合风险系数；DI_min：无人机与障碍物无避让情况下飞行全程中的最短距离；r_min:最小安全距离,即采取规避后无人机与障碍的最短距离，若采取规避后DI<r_min则会因为距离过近导致规避失败，小于r_init。

作为本发明的进一步改进，步骤S500中，有效障碍物的危险半径，按如下公式确定：

r_danger：障碍物危险半径，r_min:最小安全距离。

作为本发明的进一步改进，步骤S600中，根据无人机最小转弯半径、有效障碍物危险半径以及飞行方向规划可飞行区域，包括：

以无人机为圆心，以无人机到障碍物的危险区域的最远端距离为半径做圆，取无人机飞行方向左右各90度半圆区域，以无人机最小拐弯半径，绘制飞行受限区，飞行受限区起点与无人机当前飞行方向相切。

作为本发明的进一步改进，所述以无人机为圆心，以无人机到障碍物的危险区域的最远端距离为半径做圆，当障碍物为多个障碍物时，取多个障碍物中最远端距离最大值为半径做圆。

作为本发明的进一步改进，步骤S700中，根据障碍物危险半径以及障碍物运动方向规划扩充危险区，包括：

连结障碍物与无人机，交障碍物危险半径于点o，过点o做危险区域内接三角形，连结无人机至内接三角形另外两顶点M、N，过点O做两条连线的内切圆，将该圆以圆心移动至障碍物运动方向与障碍物危险半径交点处完成扩充。

采用上述技术方案所产生的有益效果在于：

本发明利用势场法的及时性，避免采用基于启发函数的算法而由于计算量庞大等问题错过避让时机。同时采用动态避让半径规划，避免传统方法一刀切的设定避让半径，平衡飞行安全及空域利用率。保证无人机可以在安全的情况下少避让，在危险的时候提前采取避让措施。另外，本方法由于其时效性优越，基于不同的避让半径，可以在多障碍物环境下高效稳定的选择出避让实际及角度。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是无人机和障碍物在同一时刻到达相同地点的空域示意图；

图2是潜在碰撞危险较大时，初始避让半径与自适应避让半径的关系示意图；

图3是图1空域情况下，当无人机尚未进入冲突解脱区域时，障碍物和无人机的位置关系示意图；

图4是无人机已进入r_resolution所确定的冲突解脱区域，而未进入传统方法r_init所确定的冲突解脱区域的示意图；

图5是当无人机到达障碍物运动轨迹线上时，障碍物已飞过路径交点的空域示意图；

图6是图5空域情况下，r_resolution与r_init的大小关系示意图；

图7是图5空域情况下，当无人机尚未进入冲突解脱区域时，障碍物和无人机的位置关系；

图8是无人机已进入r_init所确定的冲突解脱区域，而未进入r_resolution所确定的冲突解脱区域的示意图；

图9是r_danger和r_resolution的位置关系示意图；

图10是实施例中无人机飞行受限区域的示意图；

图11是实施例中无人机可飞行区域的示意图；

图12是实施例中扩充危险区域的第一图解示意图；

图13是实施例中扩充危险区域的第二图解示意图；

图14是实施例中计算障碍物斥力、引力的第一图解示意图；

图15是实施例中计算障碍物斥力、引力的第二图解示意图；

图16是实施例中计算障碍物斥力、引力的第三图解示意图；

图17是实施例中计算障碍物斥力、引力的第四图解示意图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本申请及其应用或使用的任何限制。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本申请的范围。同对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为授权说明书的一部分。在这里示出和讨论的所有示例中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。

因此，示例性实施例的其它示例可以具有不同的值。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。

本实施例中所用到的术语解释如下：

避让半径：一个以障碍物为圆心的范围半径。一个障碍物对于不同的无人机可有相同或不同的避让半径。本实施例常见两种避让半径，一种是在所有空域情况下，障碍物均具有相同的避让半径。另一种是基于本方法，针对不同的角度，速度等参数，由前者计算而得出的动态自适应避让半径。在下文中，前者统一称为r_init，后者称为r_resolution。

冲突解脱区域：以障碍物为圆心，初始避让半径r_init或自适应避让半径r_resolution为半径做圆，所覆盖区域。无人机进入该区域内，应以相应方法进行冲突解脱，避免碰撞发生。同时也作为斥力势场作用域。

冲突解脱区域半径r_resolution：用来判定障碍物是否对无人机产生斥力的距离限制参数，超出该阈值时，障碍物不对无人机运动产生斥力影响，无人机保持原有飞行状态。当距离小于该阈值时，说明无人机进入障碍物斥力作用域，将受障碍物产生的斥力更改运动轨迹。(该作用域阈值分为固定阈值及自适应阈值两种形式，前者为针对不同模型采用相同的作用域范围作为参数，后者则计算得出不同作用域范围。本文所述模型为自适应阈值形式，传统做法常见固定阈值形式。)

危险区域：以障碍物为圆心，r_danger为半径做圆，所覆盖区域。

可飞行区域半径：以无人机为圆心，半径为无人机到障碍物的危险区域的最远端距离。本实施例解决的技术问题是：

1)自适应避让半径规划

传统冲突解脱策略均基于固定常量来判断是否进行冲突解脱，而本方法则提供一种基于初始避让半径(前述固定常量)，针对不同速度，方向等影响因素，建立自适应的障碍物避让模型，从而进行合理避让半径规划。更进一步的该半径会随着冲突解脱的过程无人机与障碍物之间距离的变化而变化。

2)平衡飞行安全与空域运行效率

上述自适应避让半径可以对威胁性高的障碍物计算得出更大的危险半径，从而提早进行冲突解脱操作，而针对一些威胁性低的有效障碍物，可以稍晚进行冲突解脱。即使在复杂的动态空域环境下，无人机也可以以最佳的飞行效果接近目的地而无需在飞行过程中提前避让或过多避让。

3)冲突解脱规划

本方法基于引力势场提供一种新的冲突解脱方案，

以下通过图解的方式阐明该方法如何平衡飞行安全与空域运行效率。

图1展示了障碍物和无人机以90度姿势飞行，如保持现有状态，经过一段时间的飞行无人机和障碍物将在同一时刻到达相同地点。

图2的障碍物拥有两个圆圈，分别代表r_init,r_resolution,前者为前述固定常量——初始避让半径，后者为本方法提供的自适应避让半径，在当前情况下，由于潜在碰撞危险过大，本方法计算得出的自适应避让半径大于常规方法的避让半径。

图3展示了当无人机尚未进入冲突解脱区域经过一段时间飞行后，障碍物和无人机的位置关系。其中，虚线圆圈分别代表障碍物或无人机的起点位置。实线圆圈代表障碍物或无人机的当前位置。

图4展示了无人机已进入r_resolution所确定的冲突解脱区域，而未进入传统方法r_init所确定的冲突解脱区域，此时，按本方法已经开始进行冲突解脱行为，虚线表示无人机未受干预的运动轨迹。由图中可以看出无人机已偏离原有航线开始避让。若采用传统方法，此时无人机将更晚进行避让，可能由于r_init设置过小引发危险，无法及时完成避让。

图5展示了另一种空域情况，当无人机到达障碍物运动轨迹线上时，障碍物已飞过路径交点，可认为此为安全情况。

图6的r_init与图1中大小相同，而基于本方法的r_resolution大小则不同，甚至该结果下的r_resolution小于r_init。

图7展示了当无人机尚未进入冲突解脱区域经过一段时间飞行后，障碍物和无人机的位置关系。

图8展示了无人机已进入r_init所确定的冲突解脱区域，而未进入本方法r_resolution所确定的冲突解脱区域，此时，按传统方法已经开始进行冲突解脱行为，虚线表示无人机未受干预的运动轨迹。由图中可以看出如果采用本方法，无人机无需偏离原有航线进行避让。若采用传统方法，此时无人机已进行避让，降低了飞行效率。本实施例提供的一种基于改进自适应调整阈值势场法的无人机冲突探测和解脱方法，实现过程如下：

1)针对冲突解脱机制，将传统方法的避让半径设置为计算动态自适应半径的初始避让半径r_init。亦可以经验值或具体空域情况设置。(不同环境下，安全容忍度不同)

2)以无人机为坐标系原点，无人机运动方向为y轴，垂直于y轴方向为x轴建立笛卡尔直角坐标系。在该坐标系中标记空间中所有障碍物位置及速度大小及方向。

3)根据上述坐标系，计算自适应避让半径，公式如下：

其中：|v|:飞机与障碍的相对速度；γ：角度风险系数；θ：为飞机与障碍的相对速度与飞机与障碍连线夹角所对应弧度值；DI：飞机与障碍间距离；σ：综合风险系数；DI_min：(无人机与障碍物无避让情况下飞行全程中的)最短距离；r_min：最小安全距离,即采取规避后无人机与障碍的最短距离，若采取规避后DI<r_min则会因为距离过近导致规避失败，小于r_init。

4)根据各障碍物的自适应避让半径，以障碍物为圆心，r_resolution为半径规划冲突解脱区域。

5)重复步骤2-4，直至无人机进入任意冲突解脱区。

6)对有效障碍物计算其危险半径，用于后续计算斥力，公式如下：

r_danger：障碍物危险半径，r_min：最小安全距离；

本实施例中r_danger中的“_”均表示下角标。

7)可飞行区域

当满足步骤5时，无人机刚刚进入以r_resolution为半径的冲突解脱区域。以无人机运动方向为y轴，无人机，障碍物运动方向如图虚实线所示。r_danger，r_resolution如图9所示。

以无人机为圆心，作可飞行区域，以无人机到障碍物的危险区域的最远端距离为半径做圆(多个障碍物取最大值)，取无人机飞行方向左右各90度半圆区域。图10中阴影部分为无人机飞行受限区域(其半径受限于无人机最小拐弯半径，由于飞行特性，该曲线起点与无人机当前飞行方向相切)。

A，B两点为无人机水平解脱的最快情况。一般而言，无人机避让过程中无需达到与避让前相垂直的水平方向即可完成解脱。为简化模型，现连结并延长无人机与A点，无人机与B点，得到图11，点虚线内即为无人机可飞行区域。该区域角度范围为π/2，即无人机与A点无人机与B点夹角范围为π/2，其中无人机与A点相连的点虚线对应π/4，无人机与B点相连的点虚线对应-π/4。点虚线半径为当前可飞行阈值。

8)扩充危险区域

如图12所示，连结障碍物与无人机，交r_danger于点O，过点O做危险区域内接三角形，连结无人机至内接三角形另外两顶点M，N。过点O做两条连线的内切圆。

如图13所示，该圆即为危险扩充区，用于将障碍物运行方向设为危险区域的补充，即，将该圆以圆心移动至障碍物运动方向与r_danger交点处完成扩充。

9)计算障碍物斥力、引力

如图14所示，为了将斥力能够更好地与无人机和障碍的运动状态联系，连接无人机与M，其连线延长线记作α，以无人机为起点过危险扩充区做切线，记作β。

此时取α与β中线，将α与β所成夹角一分为二，即为α-β/2，图15中，v_u：无人机速度；v_o：障碍速度；v_uo是相对速度；lv为相对速度v_uo在中线垂直方向分量的长度，即向量v_uo⊥的长度。Lv为v_uo落在锥形内部速度域的宽度。则可得到Lv-lv的长度。Lv-lv越大表示需要的转弯需求越高。

K_v表示速度斥力系数，e_uo⊥表示与α与β中线垂直的单位向量(与v_uo⊥同方向，大小为1)。其他参数与步骤3中相同。Lv-lv越大表示需要的转弯需求越高。F_turn方向为v_uo⊥方向。

图16中相同情况(与图15的无人机与障碍的相同运动情况)下实线a为障碍圆的相切线构造的锥形区域。短实线b为实线a形成锥形对应的Lv-lv值；虚线c是上图葫芦状障碍圆构造的锥形区域。短实线d为黑色虚线锥形对应的Lv-lv值；可以看出短实线d的比短实线b长，所以能够获得更大的转向力。

K_p为位置斥力系数，p_obs是障碍位置；p_u无人机位置，||v_uo||是无人机与障碍的相对速度；DI为无人机与障碍的相对距离。

引力：

K_a1，K_a2：引力系数；θ：相对速度与无人机、障碍连线夹角；p_t：目标位置；p_u：无人机位置。

合力：

F_sum＝F_turn+F_rep+F_att

如图17所示，飞行方向为

的方向。

10)当无人机在F_sum下沿最小转弯半径运动至上一步骤中所得方向后，重复步骤3计算r_resolution后，重复步骤6，7，8，9，直至冲突解脱完成。

Claims (8)

1.一种基于改进自适应调整阈值势场法的无人机冲突探测和解脱方法，其特征在于，包括以下步骤：

S100：获取初始避让半径；

S200：建立以无人机为中心的坐标系，获取坐标系中障碍物的标记信息；

S300：根据障碍物的标记信息计算自适应避让半径；

S400：根据各障碍物的自适应避让半径，规划以障碍物为圆心、以自适应避让半径为半径的规划冲突解脱区域；

S500：如无人机进入冲突解脱区域，对有效障碍物计算其危险半径，有效障碍物为无人机进入冲突解脱区域内的障碍物；

S600：根据无人机最小转弯半径、有效障碍物危险半径以及飞行方向规划可飞行区域；

S700：根据障碍物危险半径以及障碍物运动方向规划扩充危险区；

S800：根据过危险扩充区计算障碍物斥力、引力；

S900：无人机沿最小转弯半径运动直至离开冲突解脱区域。

2.根据权利要求1所述的基于改进自适应调整阈值势场法的无人机冲突探测和解脱方法，其特征在于：步骤S100中，所述初始避让半径为计算动态自适应半径的初始避让半径r_init。

3.根据权利要求1所述的基于改进自适应调整阈值势场法的无人机冲突探测和解脱方法，其特征在于：步骤S200中，所述坐标系为笛卡尔直角坐标系，无人机运动方向为y轴，垂直于y轴方向为x轴，并坐标系中标记空间中所有障碍物位置及速度大小及方向。

4.根据权利要求1所述的基于改进自适应调整阈值势场法的无人机冲突探测和解脱方法，其特征在于：步骤S300中，自适应避让半径计算公式如下：

其中，|v|:飞机与障碍的相对速度；γ：角度风险系数；θ：为飞机与障碍的相对速度与飞机与障碍连线夹角所对应弧度值；DI：飞机与障碍间距离；σ：综合风险系数；DI_min：无人机与障碍物无避让情况下飞行全程中的最短距离；r_min:最小安全距离,即采取规避后无人机与障碍的最短距离，若采取规避后DI<r_min则会因为距离过近导致规避失败，小于r_init。

5.根据权利要求1所述的基于改进自适应调整阈值势场法的无人机冲突探测和解脱方法，其特征在于：步骤S500中，有效障碍物的危险半径，按如下公式确定：

r_danger：障碍物危险半径，r_min:最小安全距离。

6.根据权利要求5所述的基于改进自适应调整阈值势场法的无人机冲突探测和解脱方法，其特征在于：步骤S600中，根据无人机最小转弯半径、有效障碍物危险半径以及飞行方向规划可飞行区域，包括：

以无人机为圆心，以无人机到障碍物的危险区域的最远端距离为半径做圆，取无人机飞行方向左右各90度半圆区域，以无人机最小拐弯半径，绘制飞行受限区，飞行受限区起点与无人机当前飞行方向相切。

7.根据权利要求6所述的基于改进自适应调整阈值势场法的无人机冲突探测和解脱方法，其特征在于，所述以无人机为圆心，以无人机到障碍物的危险区域的最远端距离为半径做圆，当障碍物为多个障碍物时，取多个障碍物中最远端距离最大值为半径做圆。

8.根据权利要求7所述的基于改进自适应调整阈值势场法的无人机冲突探测和解脱方法，其特征在于，步骤S700中，根据障碍物危险半径以及障碍物运动方向规划扩充危险区，包括：连结障碍物与无人机，交障碍物危险半径于点o，过点o做危险区域内接三角形，连结无人机至内接三角形另外两顶点M、N，过点O做两条连线的内切圆，将该圆以圆心移动至障碍物运动方向与障碍物危险半径交点处完成扩充。

Images