自主飞行无人机地理围栏算法设计与实现

2019-04-29付其喜梁晓龙张佳强何吕龙周万银

西安交通大学学报 2019年5期

付其喜,梁晓龙,张佳强,何吕龙,周万银

(1.空军工程大学国家空管防相撞技术重点实验室,710051,西安;2.空军工程大学陕西省电子信息系统综合集成重点实验室,710051,西安;3.空军工程大学空管领航学院,710051,西安)

随着无人机(unmanned aircraft system,UAS)产业的飞速发展,无人机数量激增,这对地面人员的人身财产安全和空中的航空器飞行安全构成了严重的威胁[1],因此人们迫切需要发展无人机交通管理系统(UAS traffic management system,UTM)来保证无人机在空域中的安全有序飞行。无人机的机载地理围栏是UTM的重要组成部分[2]。在获得由权威来源发布和共享的地理围栏数据后,地理围栏将空域分为可用空域(禁出地理围栏)与禁飞区(禁入地理围栏),由垂直方向上的高度限制与水平方向上的边界组成[3]。无人机在水平方向上的边界可以视为由若干顶点构成的多边形。飞行过程中,无人机的飞行空域可以视为由一个禁出地理围栏与任意数量的禁入地理围栏组成。为了避免无人机地理围栏越界,需要在无人机飞行时实时监控无人机与地理围栏边界的位置关系,判断无人机是否越界或存在越界的危险,在必要时根据无人机的越界状态来触发边界保持控制。

近年来,在无人机地理围栏的应用与研究方面,国内外已取得了一定的进展。在一些现有无人机商业产品中,大疆[4]、Ardupilot[5]公司已经应用了地理围栏系统,但这些地理围栏系统不能完全可靠地防止无人机地理围栏越界。文献[6]提出,地理围栏系统可以作为感知避让系统的安全替代系统,认为安装了机载地理围栏系统的低空无人机对地面的人、财、物几乎不会再构成任何威胁。当该系统被动监视到无人机可能的地理围栏越界后,通过夺取无人机的导航控制权以避免越界。不同于直接夺取无人机的导航控制权,文献[7]利用优化的方法来生成在避免地理围栏越界的前提下的最小幅度机动,实现了以尽可能微小的飞行指令变动生成平滑的躲避轨迹,但该方法仅将地理围栏简化为圆形,难以适应无人机复杂的飞行环境。文献[8]提出了一种在UTM环境下基于无人机性能和风的影响的代数几何地理围栏算法,将地理围栏垂直和水平方向上的最低尺寸由30 m降低到5 m以下。文献[9-10]中提出了一种无人机“安全网”技术——安全卫士,并介绍了对既定的规则(如地理围栏)进行验证测试的结果。安全卫士中地理围栏边界分为三层,即警告边界、飞行终止边界及硬边界,但并没有提出地理围栏边界的生成算法。文献[11]通过对植保无人机的凸禁出地理围栏进行内缩处理和越界检测,能够在出现越界危险时为植保无人机操作人员发出越界预警。

本文在国内外无人机地理围栏相关研究的基础上,针对水平方向上的地理围栏边界问题,首先提出了对凹凸地理围栏普遍适用的地理围栏预控制层生成算法,包括地理围栏缩放子算法、顶角平滑子算法及自相交检测处理子算法,然后利用改进的射线法进行地理围栏越界探测。对于地理围栏的边界保持控制问题,设计了针对自主飞行无人机的越界航点重规划方法及边界保持控制控制律,并进行了仿真及实验验证。

1 地理围栏预控制层生成

无人机在飞行过程中一旦出现越界危险,无人机上的地理围栏系统将会利用相应的反应机制来避免地理围栏的越界,例如强制终止无人机飞行或者返回起降点等控制机制[12]。但是,如果在无人机越界之后再进行警告或者对无人机的控制进行干预,无人机将不能被地理围栏严格包含而只是停留在地理围栏边界附近,如图1a所示,其中xoy为平面直角坐标。这种看似微小的地理围栏越界在复杂危险的环境下(如空中禁飞区、空中危险区等)可能会造成毁灭性的后果。针对以上问题,本文提出了一种地理围栏预控制层自主生成算法,生成了分层地理围栏,以确保无人机足够早地触发地理围栏的边界保持控制来避免越界,从而保证无人机的飞行安全,如图1b所示。地理围栏预控制层生成算法包括地理围栏缩放子算法、地理围栏顶角平滑子算法以及地理围栏自相交检测处理子算法。

(a)未处理地理围栏 (b)处理后地理围栏图1 地理围栏对比

1.1 地理围栏缩放子算法

地理围栏缩放指在原始地理围栏的基础上产生一个新的地理围栏,新生成的地理围栏边与原始地理围栏的对应边间隔为δb。δb的取值与无人机的强制停车飞行距离、转弯半径、风的影响及状态估计误差等因素有关。当δb<0时,地理围栏向内缩放,即地理围栏为禁出地理围栏;当δb>0时,地理围栏向外缩放,即地理围栏为禁入地理围栏。

地理围栏可以通过简单缩放法来实现,公式为

g′=gsb

(1)

式中:g′为缩放地理围栏;g为原始地理围栏;sb为地理围栏缩放系数且0

(a)简单缩放法 (b)等距离缩放法图2 地理围栏缩放方法对比

等距离缩放法具体步骤如下,设计过程示意图为图3。

步骤1 设置缩放距离δb,获取地理围栏顶点集合g=(p1,…,pn)=[(x1,y1),(x2,y2),…,(xn,yn)],其中pi为地理围栏的第i个顶点。缩放地理围栏进行初始化,g′=g。

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

(2)

各边的截距为

bi=yi-ximi

(3)

每个内角的半角为

(4)

步骤3 计算使得缩放后新旧边间隔为δb的各角平分线上的距离为

(5)

将θi转换到0～2π之间得到

(6)

步骤4 若δb<0(向内缩放),则有

(7)

(8)

若δb>0(向外缩放),则有

(9)

(10)

图3 等距离缩放法的设计过程示意图

虽然等距离缩放算法已经能够初步达到地理围栏预控制层生成的目的,但当地理围栏为凹多边形时,简单缩放法无法解决地理围栏缩放问题,等距离缩放法会出现过度缩放导致地理围栏中可用空域浪费的现象,如图4所示。为此,需要用顶角平滑子算法或自相交处理子算法来辅助进行处理。

(a)简单缩放法 (b)等距离缩放法图4 凹地理围栏缩放方法对比

1.2 地理围栏顶角平滑子算法

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

(11)

(12)

将β转换到0～2π之间得到

(13)

(14)

(15)

否则得到

(16)

(17)

(18)

(19)

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

图5 顶角平滑算法的设计过程示意图

1.3 地理围栏自相交检测处理子算法

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

图6 缩放地理围栏自相交现象

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

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

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

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

图6所示的缩放地理围栏经过自相交检测处理子算法修正后生成了图7所示的缩放地理围栏,避免了自相矛盾的情况,确保了生成的缩放地理围栏的有效性,且在紧急态势下无人机可以忽略越界警告而通过手动控制穿过该狭窄区域。

2 地理围栏越界探测

无人机飞行过程中需要实时探测无人机位置与地理围栏的关系,以判断是否存在越界危险[13]。水平方向上地理围栏的越界探测可以视为一种点包容性检测问题[14]。在点包容性检测问题中广泛使用的射线法能够很好地解决水平地理围栏探测问题[15]。射线法通过以r点(坐标为(rx,ry))为端点引出的一条射线s来判断r点是否在地理围栏g′内,如图8所示。

图8 射线法示意图

在原始的射线法中,射线由端点开始向y轴的正半轴方向投射,如果射线与地理围栏的交点个数为奇数,则判定点在地理围栏内,否则点在地理围栏外。但射线与地理围栏的顶点重合时,则会错误地将两条边记为一条,从而错误判断交点个数。为了避免上述问题导致的探测错误,在原始的射线法的基础上定义了缓冲距离b,从而提出改进的射线法。若地理围栏某个顶点的横坐标在rx的缓冲距离b以内,则给该顶点的横坐标施加一个扰动-2b,以避免射线与地理围栏顶点重合。地理围栏越界探测的具体流程如图9所示,其中,pi点的横坐标为pix,pi点经过缓冲距离b处理后的横坐标为pix,b。

图9 地理围栏越界探测算法流程图

3 地理围栏边界保持自主控制

地理围栏预控制层生成之后,无人机在飞行中会利用地理围栏越界探测算法实时判断自己与地理围栏预控制层的位置关系。当没有越界危险时,无人机会根据预先规划的路径飞行完成相应的任务。一旦地理围栏探测到无人机存在越界的危险,机载地理围栏系统将会立即夺取无人机的导航控制权,并按照算法设定的导航控制方法进行反应以避免无人机越界,从而保证无人机的飞行安全[16]。由此可见,地理围栏边界保持自主控制的导航控制方法尤为重要。

3.1 越界航点重规划

无人机飞行前会进行航点规划,在实际飞行过程中根据规划的航点飞行以完成任务。无人机在航点飞行过程中可能会出现地理围栏越界的情况,需要进行无人机的航点重规划。

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

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

图10 禁出地理围栏越界航点重规划

图11 穿越禁入地理围栏越界航点重规划

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

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

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

步骤2 在穿越航线的两端的原始航点之间以等距离D插入航点,生成插入航点集Winsert。

步骤3 利用地理围栏越界探测算法判断插入航点集Winsert中航点是否越界。若越界,则删除越界航点。

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

航点重规划过程中转弯前的两航点之间需要保留一定的安全距离Ds,其公式为

(20)

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

图12是穿越禁入地理围栏越界航点的重规划航点设计过程,为了保证如图中w4航点等始端修正航点之间的安全距离,需要对不同情况进行讨论。对航点重规划的始端修正航点讨论步骤如下。

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

步骤2 若禁入地理围栏预控制层边界所在线段与航点w3和交点B连接构成的线段相交,如图12a所示,则转向Step3,若不相交,如图12b所示,则转向Step4。

步骤3 若点B与顶点的距离大于Ds,则点B即为修正航点w4,否则顶点为w4。

步骤4 若航点w3与顶点的距离大于Ds,则顶点为修正航点w4,否则,删除航点w3,且顶点替代航点w3。

(a)相交

(b)不相交图12 穿越禁入地理围栏越界航点的重规划航点设计过程

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

3.2 边界保持控制律设计

地理围栏在探测到越界危险并对航点进行重规划之后,无人机就可以按照修正航点飞行以避免地理围栏越界。然而,不同的飞行控制器的控制律并不尽相同,无人机仍然会有越界的危险。所以,地理围栏需要设计独立于飞行控制器的控制律。

若将无人机视为质点,其采用一阶积分模型的动力学模型为

(21)

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

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

(22)

式中Δt为无人机状态更新间隔。

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

(23)

式中xw(i)为无人机当前正在飞向的第i个航点。

4 仿真及实验结果分析

针对提出的地理围栏算法在Matlab R2016a环境下对地理围栏算法进行仿真,并利用搭建的实验验证系统进行实验,最后进行仿真及实验数据分析。

4.1 实验系统介绍

实验验证系统由四旋翼无人机平台、数据链与地面站共同组成,如图13所示。

图13 实验验证系统

四旋翼无人机采用PIX自驾仪作为飞行控制系统,自驾仪由用于估计无人机运动状态的3组陀螺仪、3组加速度计、2组电子罗盘和用来记录实验数据的8 GB反式(trans flash,TF)存储卡组成。系统的定位模块为全球定位系统(global position system,GPS),频率为10 Hz,采用串口通信进行数据传输。实验系统数传部分采用基于Mesh组网技术的Microhard P400-840模块,通信频率为840 MHz。由于无人机进行地理围栏边界保持控制时需要一定的计算量和较高的实时性,选用STM32F4系列微处理器进行实时运算。系统硬件结构如图14所示。

图14 系统硬件结构图

4.2 仿真及实验数据分析

仿真及实验相关参数设置为:缩放距离δb=±3m,|u0|=0m/s,|us|=3m/s,D=12m。仿真结果如图15所示:从图15a可以看出,无人机能够避免原始地理围栏越界;从图15b可以看出,无人机能够按照设计的地理围栏边界保持自主控制控制律飞行,即直线段匀速飞行,转弯点先减速再加速;从图15c可以看出,无人机能够始终与地理围栏边界保持至少δb的间隔,说明了本文设计的地理围栏算法的有效性。

实验结果如图16所示:从图16a可以看出,无人机出现了地理围栏预控制层越界的情况,但却并未出现原始地理围栏越界;从图16b可以看出,无人机的飞行速度与仿真结果存在一定的误差,这是由于风的影响、无人机状态估计误差等共同的作用;从图16c可以看出,无人机出现了与原始地理围栏的边界间隔小于δb的情况,这主要是由于无人机惯性、控制响应时延以及定位误差等引起的,但并不会造成原始地理围栏越界。实验视频见文献[17]。