一种自主飞行的无人机地理围栏算法的制作方法

本发明属于空中交通管制技术领域，涉及低空无人机交通管理系统，具体涉及一种自主飞行的无人机地理围栏算法。

背景技术：

随着无人机(uas,unmannedaircraftsystem)产业的飞速发展，无人机数量激增，这对地面人员的人身财产安全和空中的航空器构成了严重的威胁。人们迫切需要发展无人机交通管理系统(utm,uastrafficmanagementsystem)来保证无人机在空域中的安全有序飞行。

无人机的机载地理围栏是utm的重要组成部分，在获得由权威来源发布和共享的地理围栏数据后，地理围栏将空域分为可用空域(禁出地理围栏)与禁飞区(禁入地理围栏)，由垂直方向上的高度限制与水平方向上的边界组成。无人机在水平方向上的边界可以视为由若干顶点构成的多边形。飞行过程中，无人机的飞行空域可以视为由一个禁出地理围栏与任意数量的禁入地理围栏组成。为了避免无人机地理围栏越界，需要在无人机飞行时实时监控无人机与地理围栏边界的位置关系，判断无人机是否越界或存在越界的危险，在必要时根据无人机的越界状态来触发边界保持自主控制。

无人机在飞行过程中一旦出现越界危险之后，无人机上的地理围栏系统将会利用相应的控制机制来避免地理围栏的越界，例如强制终止无人机飞行或者rtl(returntolaunch)等控制机制。但是，如果无人机越界之后再进行警告或者对无人机的控制进行干预的话，无人机将不能被地理围栏严格包含而只是停留在地理围栏边界附近。这种看似微小的地理围栏越界在复杂危险的环境下(如空中禁飞区、空中危险区等)可能会造成毁灭性的后果。

技术实现要素：

针对上述问题中存在的不足之处，本发明提供一种自主飞行的无人机地理围栏算法。

本发明公开了一种自主飞行的无人机地理围栏算法，包括：

在原始地理围栏的基础上等距离缩放生成地理围栏预控制层；

无人机飞行前，利用射线法对规划飞行路径进行地理围栏越界探测；

若规划飞行路径上的航点相对所述地理围栏预控制层出现越界，则进行越界航点重规划，得到修正后的飞行路径；

基于地理围栏自主控制控制律，无人机按照修正后的飞行路径完成相应的任务。

作为本发明的进一步改进：

当所述原始地理围栏为禁出地理围栏时，所述原始地理围栏向内缩放；

当所述原始地理围栏为禁入地理围栏时，所述原始地理围栏向外缩放。

作为本发明的进一步改进，所述在原始地理围栏的基础上等距离缩放生成地理围栏预控制层，包括：

通过地理围栏缩放算法对所述原始地理围栏进行等距离缩放，得到缩放地理围栏；

通过地理围栏顶角平滑算法对所述缩放地理围栏中大于180°的内角进行处理，产生两个新的顶点来替换原内角大于180°的顶点；

通过地理围栏自相交检测处理算法对所述缩放地理围栏中自相交区域进行处理，得到若干未冲突的缩放地理围栏。

作为本发明的进一步改进，所述利用射线法对规划飞行路径进行地理围栏越界探测，包括：

以所述规划飞行路径上的航点为端点向y轴的正半轴方向投射，若射线与所述地理围栏预控制层的交点个数为奇数，则判定端点在所述地理围栏预控制层内；若射线与所述地理围栏预控制层的交点个数为偶数，则判定端点在所述地理围栏预控制层外，出现越界。

作为本发明的进一步改进，定义缓冲距离buff，若所述地理围栏预控制层的某个顶点的x坐标在所述端面的缓冲距离buff内，则给该所述顶点的x坐标施加一个扰动-buff*2。

作为本发明的进一步改进，所述越界航点重规划采用在原始航点间插入新航点的方式。

作为本发明的进一步改进，所述地理围栏预控制层设计独立于飞行控制器的自主控制控制律。

与现有技术相比，本发明的有益效果为：

本发明通过生成地理围栏预控制层，以确保无人机在飞行过程中可足够早地触发地理围栏的边界；保持自主控制控制律来避免地理围栏越界，从而保证无人机的飞行安全。

附图说明

图1为本发明一种实施例公开的自主飞行的无人机地理围栏算法的流程图；

图2为本发明一种实施例公开的等距离缩放算法示意图；

图3为本发明一种实施例公开的顶角平滑算法示意图；

图4为本发明一种实施例公开的缩放地理围栏自相交现象示意图；

图5为本发明一种实施例公开的自相交检测处理后的缩放地理围栏示意图；

图6为本发明一种实施例公开的射线法示意图；

图7为本发明一种实施例公开的地理围栏越界探测算法流程图；

图8为本发明一种实施例公开的禁出地理围栏越界航点重规划示意图；

图9为本发明一种实施例公开的穿越禁入地理围栏越界航点重规划示意图；

图10为本发明一种实施例公开的穿越禁入地理围栏越界航点重规划航点过渡中的相交情况示意图；

图11为本发明一种实施例公开的穿越禁入地理围栏越界航点重规划航点过渡的不相交情况示意图；

图12为本发明一种实施例公开的无人机飞行轨迹示意图；

图13为本发明一种实施例公开的无人机的速度变化示意图；

图14为本发明一种实施例公开的无人机与地理围栏边界的距离示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

下面结合附图对本发明做进一步的详细描述：

如图1所示，本发明提供一种自主飞行的无人机地理围栏算法，包括：

s1、在原始地理围栏的基础上等距离缩放生成地理围栏预控制层；其中：

当原始地理围栏为禁出地理围栏时，原始地理围栏向内缩放；

当原始地理围栏为禁入地理围栏时，原始地理围栏向外缩放；

生成地理围栏预控制层包括地理围栏缩放算法、地理围栏顶角平滑算法和地理围栏自相交检测处理算法；具体的：

通过地理围栏缩放算法对原始地理围栏进行等距离缩放，得到缩放地理围栏；上述缩放原理为：

地理围栏缩放指在原始地理围栏的基础上产生一个新的地理围栏，新生成的地理围栏边与原始地理围栏的对应边间隔为δb，δb的取值与无人机的强制停车飞行距离、转弯半径、风的影响及状态估计误差等因素有关。δb<0时，地理围栏向内缩放，即地理围栏为禁出地理围栏；反之δb>0时，地理围栏向外缩放，即地理围栏为禁入地理围栏。地理围栏也可以通过一种简单缩放方法来达到缩放的效果。

通过地理围栏顶角平滑算法对缩放地理围栏中大于180°的内角进行处理，产生两个新的顶点来替换原内角大于180°的顶点；上述顶角平滑设计原理为：

地理围栏缩放算法已经能够初步达到地理围栏预控制层生成的目的。但是当地理围栏存在大于180°的内角时，缩放算法会出现过度缩放导致地理围栏中可用空域浪费乃至无人机任务无法完成的问题。本发明采用地理围栏顶角平滑算法来重新释放由于凹地理围栏的特殊内角导致的不可用空域。

通过地理围栏自相交检测处理算法对缩放地理围栏中自相交区域进行处理，得到若干未冲突的缩放地理围栏；上述自相交检测处理设计原理为：

由于环境的限制，比如飞行空域中存在隧道、桥梁等，划设的地理围栏两端部分较大、中间部分较窄，地理围栏经过缩放后会出现地理围栏边的自相交现象。针对这种异常情况，本发明提出了地理围栏的自相交检测处理算法。地理围栏缩放算法与自相交检测处理算法能够快速提供与原始边界保持一定安全间隔的可飞空域范围，并有助于判断无人机是否可穿过该狭窄区域。

s2、无人机飞行前，利用射线法对规划飞行路径进行地理围栏越界探测；其中：

以规划飞行路径上的航点为端点向y轴的正半轴方向投射，若射线与地理围栏预控制层的交点个数为奇数，则判定端点在地理围栏预控制层内；若射线与地理围栏预控制层的交点个数为偶数，则判定端点在地理围栏预控制层外，出现越界。

进一步，为了避免射线与地理围栏的顶点重合而导致探测错误，本发明定义缓冲距离buff，若地理围栏预控制层的某个顶点的x坐标在端面的缓冲距离buff内，则给该顶点的x坐标施加一个扰动-buff*2。

s3、若规划飞行路径上的航点相对地理围栏预控制层出现越界，则进行越界航点重规划，得到修正后的飞行路径；

s4、无人机按照修正后的飞行路径完成相应的任务；其中：

由于不同的飞行控制器的控制律设计并不尽相同，无人机仍然会有越界的危险。所以地理围栏需要设计独立于飞行控制器的控制律。

实施例：

本发明提供一种自主飞行的无人机地理围栏算法，包括：

步骤一、生成地理围栏预控制层：

1、地理围栏缩放算法

等距离缩放算法具体步骤如下：

步骤1、设置缩放距离δb，获取地理围栏顶点集合g＝(p1,...,pn)＝[(x1,y1),(x2,y2),...,(xn,yn)]，缩放地理围栏进行初始化：

g'＝g(1)

步骤2、计算地理围栏各边的斜率：

各边的截距：

bi＝yi-xi*mi(3)

以及每个内角的半角大小θi；

步骤3、如图2所示，计算使得缩放后新旧边间隔为δb的各角平分线上的距离：

将θi转换到全局坐标系：

步骤4若δb<0(向内缩放)，则：

x′i＝cosφi*hi+xi(6)

y′i＝sinφi*hi+yi(7)

若δb>0(向外缩放)，则：

x′i＝-cosφi*hi+xi(8)

y′i＝-sinφi*hi+yi(9)

则g'＝(p′1,...,p'n)＝[(x'1,y'1),(x'2,y'2)...,(x'n,y'n)]。

2、地理围栏顶角平滑算法

顶角平滑算法将会产生两个新的顶点来替换原内角大于180°的顶点，如图3所示。原地理围栏该顶点到两个新产生的顶点构成的边的距离为δb。顶角平滑算法的具体步骤如下：

步骤1、获取缩放距离δb，缩放地理围栏顶点集合g'＝(p′1,...,p'n)＝[(x'1,y'1),(x'2,y'2)...,(x'n,y'n)]；

步骤2、利用(2)、(3)计算地理围栏各边的斜率mi和截距bi；

步骤3、若∠p′i＞180°∩δb＜0，或者∠p′i<180°∩δb＞0，则对于∠p′i，计算β：

计算顶点p′i到新生成的边之间的距离：

将β转换到全局坐标系：

步骤4、计算点坐标：

若

否则：

并计算新边的斜率c与截距

步骤5、计算新生成的两个顶点坐标：

更新缩放地理围栏的坐标。

3、地理围栏自相交检测处理算法

自相交检测处理算法的主要步骤如下：

步骤1、获取缩放地理围栏顶点集合g'＝(p′1,...,p'n)＝[(x'1,y'1),(x'2,y'2)...,(x'n,y'n)]，检测缩放地理围栏的任意两条不相邻边界是否相交，若相交，记录缩放地理围栏的交叉边与交叉点；

步骤2、对缩放地理围栏交叉边与交叉点进行处理，将缩放地理围栏分割为若干条边界；

步骤3、按原缩放地理围栏的顶点顺序将分割后的边界组合成若干多边形，如图4所示；

步骤4检测组合后的若干多边形的顶点排列顺序，删除与原缩放地理围栏顶点顺序不一致的多边形，保留下来的部分即为没有冲突的缩放地理围栏g'，如图5所示。

步骤二、地理围栏越界探测：

在点包容性检测问题中广泛使用的射线法能够很好地解决水平地理围栏探测问题。射线法通过以r点为端点引出的一条射线s来判断点r是否在地理围栏g’内，如图6所示。

射线法中射线由端点开始向y轴的正半轴方向投射，如果射线与地理围栏的交点个数为奇数，则判定点在地理围栏内，否则点在地理围栏外；为了避免射线与地理围栏的顶点重合而导致探测错误，本发明的射线法定义了缓冲距离buff。若地理围栏某个顶点的x坐标在rx的缓冲距离buff以内，则给该顶点的x坐标施加一个扰动-buff*2，地理围栏越界探测的具体流程如图7所示。

步骤三、地理围栏边界保持自主控制：

1、越界航点重规划

地理围栏边界保持控制过程中为了避免出现转弯机动过于剧烈导致航迹偏移过大的情况，无人机在边界保持控制中采用直线段匀速，转弯段采用先减速再转弯的策略飞行。航点重规划算法采用在原始航点间插入新航点的方式来尽可能增加无人机地理围栏边界保持控制中的匀速飞行的距离，以减少飞行时间。

禁出地理围栏越界航点重规划如图8所示，若地理围栏越界探测算法探测到航点出现越界情况，即进行航点重规划。首先，在越界航线的两原始航点间以等距离d插入新的航点，生成航点集wpinsert。利用地理围栏越界探测算法判断航点集wpinsert中航点是否越界。若越界，则将该航点映射到地理围栏预控制层最近边界上，得到修正航点并替换越界航点，最终得到修正航点集wpcorrrect。

当无人机预先规划的航线贯穿禁入地理围栏时，越界航点重规划与未穿越时有很大不同，此时的越界航点重规划如图9所示。为避免无人机相向飞行时进行穿越禁入地理围栏航点重规划时可能的冲突情况，本发明按照飞行规则进行穿越禁入地理围栏的航点重规划，即沿禁入地理围栏预控制层边界逆时针插入修正航点，也就是右转。

贯穿禁入地理围栏航点重规划主要步骤如下：

步骤1、判断无人机原始航点生成的航线(以下简称贯穿航线，其可能包含多个原始航点)是否贯穿禁入地理围栏预控制层g’，若贯穿，则进行贯穿禁入地理围栏的航点重规划；

步骤2、在贯穿航线的两端的原始航点之间以等距离d插入航点，生成航点集wpinsert；

步骤3、利用地理围栏越界探测算法判断航点集wpinsert中航点是否越界。若越界，则删除越界航点；

步骤4、沿禁入地理围栏预控制层边界逆时针以等距离d插入修正航点后，得到修正航点集wpcorrect，其中禁入地理围栏预控制层顶点固定为修正航点；

航点重规划过程中转弯前的两航点之间需要保留一定的安全距离ds：

式中：us为无人机地理围栏触发边界保持控制时的标准速度，方向为当前位置指向下一航点。amax为无人机的最大加速度。

为了保证如图10所示的wp4等始端修正航点之间的安全距离，需要对不同情况进行讨论。对航点重规划的始端修正航点讨论步骤如下：

步骤1、计算以wp3为圆心，以d为半径的圆与禁入地理围栏预控制层边界所在直线的交点，得两交点a与b，由飞行规则得下一个修正航点方向为交点b的方向。

步骤2、若禁入地理围栏预控制层边界所在线段与航点wp3与交点b所在线段相交，如图10所示，则转向步骤3，若不相交如图11所示，则转向步骤4。

步骤3、若点b与顶点的距离大于ds，则点b即为修正航点wp4，否则顶点为wp4。

步骤4、若wp3与顶点的距离大于ds，则顶点为修正航点wp4，否则，删除航点wp3，且顶点替代航点wp3。

对航点重规划的末端修正航点讨论与始端类似。地理围栏航点修正以后，无人机在合理设置参数δb的基础上按照修正航点飞行就能够有效避免地理围栏越界。

边界保持控制控制律设计：

无人机被视为质点，其动力学模型采用一阶积分模型：

式中：x(t)、u(t)为分别为无人机的位置与速度，同时u(t)为对应的控制输入。

当无人机进行直线飞行时，设计无人机的控制律为：

式中：δt为无人机状态更新间隔，a(t)为无人机的加速度。

当无人机进行转弯飞行时，设计无人机的控制律为：

式中：xw(i)为无人机当前飞向的航点。

仿真相关参数设置为：缩放距离δb＝±3m，||u0||＝0m/s，||us||＝3m/s，d＝12m。根据上述算法流程及参数设置，仿真获得的无人机飞行轨迹如图12所示，无人机的速度变化如图13所示，无人机与地理围栏边界的距离如图14所示。

以上仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。