基于安全飞行走廊的无人机密集障碍规避算法

范力元,张浩哲,徐钊,吕明伟,胡劲文,赵春晖,刘晓斌

(1.西北工业大学 自动化学院,陕西 西安 710072;2.西北工业大学 电子信息学院,陕西 西安 710072;3.沈阳飞机设计研究所,辽宁 沈阳 110035;4.西安现代控制技术研究所,陕西 西安 710065)

近年来,随着无人机技术的快速发展,无人机在许多方面取代了人工劳作,比如在地形勘探、灾难搜寻、边界巡检等方面[1]。与旋翼无人机相比,固定翼无人机在载荷、飞行距离等方面有明显优势,在作战、运输等领域具有更广泛的应用。无人机的路径规划是无人机自主控制中的一项关键技术,由于固定翼飞行约束较强,目前成熟的固定翼无人机路径规划通常是起飞前进行的预规划,中途遇到突发情况往往需要人为干预,缺乏威胁规避的自主性,难以适应低空复杂多障碍环境下的高动态敏捷控制需求,例如对鸟群、树木、建筑物等的防撞规避,以及机场附近密集空域下的无人机自主安全飞行。为了提升未来民用无人机空域集成安全与自主飞行安全能力,需要研究固定翼无人机在复杂多障碍环境下的自主“感知与规避技术”[2]。

传统的障碍规避算法多用于无人车、无人船等智能体上。避障算法大致可以分为全局路径规划、局部路径规划以及与近年来发展迅猛的人工智能方法相结合的路径规划算法。全局路径规划方面,基于A*发展出了更加高效的迭代加深A*算法(iterative deepening A*,IDA*)、终身规划A*算法(lifelong planning A*,LPA*)、双向A*算法(bidirectional A*)[3-5];适用于动态障碍物环境的D*,Focused D*、D*Lite算法[6-8];基于概率的概率地图法(probabilistic road map,PRM)、快速搜索随机树法(rapidly-exploring random tree,RRT)、快速搜索随机树优化算法(RRT*)[9-10]、双向RRT*算法[11]。局部路径规划方面人工势场法(artificial potential field,APF)[12]简单有效,被广泛应用于实际工程当中;速度障碍法(velocity obstacle,VO)[13]选择速度场中合适的避障行为来保证机器人的安全性;向量场直方图算法(vector field histogram,VFH)、VFH+[14]利用障碍物信息对移动机器人运动方向产生的影响达到避障效果;人工智能算法方面,应用较早的引力搜索树法(gravitational search algorithm,GSA)、模拟退火法(simulated annealing,SA)、遗传算法(genetic algorithm,GA)、粒子群优化算法(particle swarm optimization,PSO)[15-18]在路径规划算法方面取得了不错的应用效果,人工神经网络(artificial neural network,ANN)、监督学习(supervised learning,SL)、强化学习(reinforcement learning,RL)也在路径规划方面取得了不错的效果[19-21]。固定翼无人机相比无人车、旋翼无人机等智能体有更多的运动学限制,上述避障算法不能简单应用于固定翼无人机的避障。

针对固定翼无人机的避障问题,研究人员也做了许多相关研究。Vanegas等[22]使用与贝塞尔曲线近似的3D回旋曲线生成平滑曲线,引导无人机跟踪实现障碍物规避。Lee等[23]基于三次贝塞尔曲线的样条方法扩展提出的样条RRT*算法的树结构,生成三维曲线，实现固定翼无人机三维空间中的障碍物规避。Keller等[24]将图搜索算法和样条采样算法相结合,应用于多固定翼无人机的障碍物规避。Benders等[25]在考虑固定翼无人机多飞行约束条件下,利用A*算法实现了有风干扰环境中的3D路径规划。Song等[26]综合A*和Dubins曲线实现了固定翼无人机的避障,相比传统A*算法,此方法飞行效率更高。另外智能算法在固定翼无人机的避障方面也有应用。Sanders等[27]结合无人机的动力学特性和油耗、油门推力等,利用非支配排序遗传算法实现了固定翼无人机到目标点的最优路径规划。Roberge等[28]使用遗传算法(GA)和粒子群优化算法(PSO),考虑无人机动态特性实现无人机在复杂3D环境中的准最优路径规划,该团队在2018年[29]利用遗传算法在GPU上运动生成适合于固定翼无人机的平滑路径,实现固定翼无人机在满足战场高动态环境中的障碍物规避。

相比上述提到的固定翼无人机障碍规避算法,本文所提算法能够实现复杂低空密集障碍物环境的障碍物规避。本算法考虑并满足无人机机动性的要求,不仅基于无人机当前位置与障碍物位置,还考虑无人机未来航迹与障碍物的关系,实现障碍物规避,另外能够通过调整障碍规避评价函数的权重向量,对避障算法进行有倾向性的调节。以上功能对于安全性要求更严苛的固定翼无人机而言更加安全。

1 问题描述

1.1 固定翼无人机飞行建模

(1)

pk=pk-1+uk-1

(2)

式中：pk=[xk,yk,θk]分别表示k时刻航迹的坐标点以及朝向;uk表示k时刻无人机的输入控制量。

1.2 基于Dubins曲线的安全飞行走廊

1.2.1 Dubins曲线

对于固定翼无人机的航路规划问题,首先要解决满足诸如最小转弯半径等约束条件时,2个航路点之间的最短路径规划问题。航路点包含的信息有二维平面坐标点、偏航角,即Dubins曲线模型中包含的信息。Dubins[30]于1957年给出完备的Dubins路径集,解决了有界曲率下2个点之间的最短路径问题,最短路径就包含在Dubins完备路径集中,路径集中的每条路径由线段和圆弧组成。

图1 Dubins轨迹示意图(RSL)

为了减少最短路径计算时间,Shkel等[31]对Dubins完备路径集进行启发式改进,根据起点和终点的运动方向将Dubins路径集分成几种等价组,通过构建等价组逻辑分类判断,就能快速求得最短路径。包含6种路径类型的Dubins路径集可以表示为S={LSL,LSR,LRL,RSR,RSL,RLR},最短路径可以表示为

P(ps,pe)=

(3)

式中：ps=[xs,ys,θs]表示Dubins曲线的起始点;pe=[xe,ye,θe]表示Dubins曲线的终止点；L为左旋转圆弧；R为右旋转圆弧；S表示直线段。RSL类型的路径示意图如图1所示。

1.2.2 固定翼无人机安全飞行走廊

在民航领域,国家统一规划了飞机的航路图,客机需要依据地面和机载的导航定位设备,按照预先设定的航路飞行。航路是具有上下限高度和宽度,类似于走廊的飞行通路,本文借鉴民用领域的相关规定,提出了基于Dubins曲线的飞行走廊定义方法。

如图2所示,假设飞行轨迹起点为ps,局部终点为pe′,连接ps与pe′,定义飞行走廊的左宽度为Dl,飞行走廊的右宽度为Dr(飞行走廊的左右宽度如图中蓝色线段所示),另外根据任务需求,在飞行走廊的两侧增加安全阈值δ,飞行走廊的宽度最终可以定义为

Dw=Dl+Dr+2δ

(4)

图2 基于Dubins曲线的飞行走廊示意图

1.3 安全飞行评价函数

一般地,为躲避障碍物而进行飞行航迹重规划时,障碍物规避路径偏离原始路径的程度越大,任务执行效率越低而飞行的安全性越高;相反,任务执行效率提高而飞行安全性却降低了。本节的目的是设计一个能在任务效率和飞行安全性之间进行权衡的多参数评价函数,在保证飞行安全的前提下尽可能地靠近原始路径。评价函数由障碍物与飞行走廊距离、局部终点偏移距离、未来航迹与障碍物距离三部分构成。

1) 障碍物与飞行走廊距离:假设计算得到的路径局部终点为pe′=(xe′,ye′,θe′),连接起点ps和局部终点pe′,得到一条满足(5)式的直线。

(ye′-ys)x+(xe′-xs)+(xe′ys-ye′xs)=0

(5)

障碍物与飞行走廊之间的距离Di可以由(6)式计算得到

Di=

(6)

2) 局部终点偏移距离:飞行航迹的局部终点与原始终点的距离可以表示为

Dpe′,e=‖pe′-pe‖

(7)

3) 未来航迹与障碍物距离:未来路径与障碍物的最小距离可以由(8)式计算得到

(8)

综上所述,基于Dubins飞行走廊的避障优化函数可以设计为

(9)

式中：[η1η2η3]为权重向量,用户可以根据具体的任务场景及需求设定相应的权重；Ro,i表示下标为i障碍物的半径。

基于以上定义,无人机的障碍物规避问题就可以描述为

(10)

2 基于蒙特卡洛采样的数值优化算法

2.1 DBSCAN聚类算法

为了生成无碰撞的路径,要先确定感知范围内的可飞行区域和不可飞行区域。考虑到空间中的障碍物可能很多,为了减小蒙特卡洛采样法的搜索空间,本文采用DSBCAN算法对感知范围内的所有障碍物进行空间聚类处理。DSBCAN算法根据障碍物之间的距离等评价指标来定义障碍物之间的密度,当感知范围内的障碍物满足相应的密度要求时,将相应的障碍物区域视为空间障碍簇,即为不可飞行区域。

假设无人机与障碍物之间的最小安全距离为Dmin,当障碍物的距离满足

‖po,i-po,j‖≥2Dmin+Ro,i+Ro,j

(11)

则说明无人机能从下标为i和j的障碍物之间穿过,其中Ro,i和Ro,j表示下标为i和j障碍物的半径。因此,DBSCAN算法的参数可以设置为：

1)Rmin=2Dmin+Ro,i+Ro,j,Rmin为最短领域半径。

2)Pmin=1;Pmin表示领域内障碍物的最少个数。

2.2 蒙特卡洛采样算法

局部终点的计算,即评价函数的求解问题。由2.1节可知,评价函数是不可导且不具有解析形式的,评价优化函数的解析解也就不能显式求得,蒙特卡洛采样法是一种具有概率完备性的、基于统计采样的近似推理方法,它是求解近似解的有力方法,因此本文采用蒙特卡洛采样法来近似逼近评价优化函数的最优解。

图3 蒙特卡洛采样区间

如图3所示,蒙特卡洛采样法只能在传感器探测范围且满足Dubins曲线模型的限制区域内采样(即图中蓝色虚线部分),采样获得一组二维坐标随机采样点:{(x1,y1),(x2,y2),…,(xnmc,ynmc)},其中nmc是随机采样点的数量。由此,优化问题转化成

(12)

总结以上步骤,完整的避障路径规划算法执行步骤如表1所示。

表1 避障路径规划算法执行步骤

3 算法仿真验证

3.1 密集障碍环境中的无人机避障仿真

本节在MATLAB R2017b中总计进行了100次无人机避障仿真,其中前50次均采用同一环境和无人机参数进行重复实仿真,后50次采用另外一组环境和无人机参数进行对照,验证该算法在不同环境中的避障效果。在2个仿真环境中随机生成多个障碍物,并且在2个环境中无人机可飞行区域内随机确定无人机的初始位置与目标位置。首先人为设置无人机任务路径的点集,再基于任务路径点集,通过Dubins算法生成一条满足固定翼无人机飞行约束的光滑曲线,获得的曲线作为无人机的任务路径。接下来设置本文所提避障算法的参数,前50次仿真为一组,后50次仿真为另一组,参数如表2所示。

表2 仿真参数设置

每进行一次仿真就会进行若干次蒙特卡洛采样,每次蒙特卡洛采样会采集到若干个位于采样区域的点,然后根据这些采样点得到最优局部终点。将前50次仿真中所有相对应的局部最优点进行均值化,得到第一组数据;同理,另一组环境下将得到第二组数据。下面对这2组不同环境中的数据绘制图像进行具体分析。

第一组的仿真结果如图4所示。图中随机散落着一些障碍物点,红色曲线代表预设的任务路径,无人机在跟随任务路径飞行的过程当中,传感器不断对环境进行感知,若感知到障碍物,无人机则会依据本文提出的算法进行路径规划。规划的路径如图4a)中黑色曲线所示,原始任务路径中被替代的曲线段由图中绿色曲线所示。从图4a)中还可以看出,当规划的避障路线中仍有障碍物时,算法对路径进行了二次重规划。最终无人机完成飞行任务时的飞行路线如图4b)中蓝色曲线所示。

图4 第一组仿真结果

第二组仿真结果如图5所示。和第一组仿真一样,无人机在随机散落障碍物的地图中成功地规划出一条从起点到终点,规避任务路径中所有障碍物的避障路线。以上仿真结果都验证了本文所提出算法的有效性。

图5 第二组仿真结果

在上面2组实验中,无人机都能避开规划航迹上的障碍,若无人机的任务路径点上没有障碍物,则得到一个经过所有无人机任务路径点集的最短路径;若有个别路径点与障碍物重叠,则根据上述算法绕开障碍再向下一路径点飞行。

3.2 基于MPC轨迹跟踪的固定翼无人机障碍规避仿真

考虑到无人机在实际飞行中很难实现对航路的零误差跟踪,本节以模型预测控制为轨迹跟踪控制器,模拟无人机对目标路径的跟踪效果,以进一步验证本文所提出算法的实用性。

设置起始状态为:ps=[1 200 m,200 m,235°],目标状态为:pe=[800 m,2 700 m,235°],利用3.1节中描述的任务路径生成方法生成任务路径。设置无人机最小转弯半径ρ=250 m,传感器探测角度θ=30°,传感器探测距离为d=1 000 m,障碍威胁评价函数权重η=[η1,η2,η3],分别为航线偏离程度、局部终点偏移距离、未来航迹与障碍物距离的权重。

无人机在按任务路径飞行时,如果没有探测到障碍物,则无人机通过模型预测控制器以任务路径为参考路径实行跟踪控制;如无人机探测到障碍物,则由本文提出的算法进行路径规划以躲避障碍物,此时无人机通过模型预测控制器以规划的避障路径为参考路径进行跟踪。仿真结果如图6所示,图中蓝色路径为无人机的任务路径,黑色路径为无人机实时参考的路径,红色路径为无人机最终飞行的路径。

图6 无人机跟踪轨迹

由图6可以看出无人机在任务起初阶段未探测到障碍物,以目标路径为参考路径进行跟踪飞行,当探测到障碍物时对障碍物进行聚类归并(归并的障碍物由相同的颜色表示),并由上文提出的路径规划算法计算出一条避障路径,此时无人机以避障路径为参考路径进行实时跟踪,当无人机实现对障碍物的规避之后,无人机又以任务路径为参考路径进行跟踪飞行。由仿真结果还可以看出在跟踪目标路径时,模型预测控制器的跟踪误差较小,但是在跟踪避障路径时,由于无人机机动性的约束,存在较大的跟踪误差。从飞行过程整体上看,无人机实现了对任务路径的跟随,并且成功规避了任务路径上的障碍物。仿真结果进一步证明了本文所提出算法的有效性。

在图7的仿真结果中展现了蒙特卡洛采样的过程。蒙特卡洛采样次数设置为100(采样点在图中用黑色圆圈表示),其余参数设置与本节仿真参数设置类似。

图7 蒙特卡洛采样示意图

上述2次仿真设置了不同的障碍威胁评价函数权重向量,仿真结果图7a)中权重向量η=[0.3,0.3,0.4],仿真结果图7b)中权重向量η=[0.2,0.6,0.2],可以看出蒙特卡洛采样点通常位于采样区域边缘的位置,通过改变权重向量,可以改变最优路径点在采样区域中的分布趋势。多次仿真实验后,设置不同的权重向量影响无人机避障过程中对于航线偏离程度、局部终点偏移距离、未来航迹与障碍物距离3个部分。取场景1中的一个避障过程,分析不同权值对局部终点偏移距离、未来航迹与障碍物距离的影响，结果如表3所示。

表3 权值向量设置

上述仿真中,航线偏离程度随着局部终点偏移距离的减小而减小,未来航迹与障碍物距离的变化与之相同。即当权重值η2越大,局部最优路径点倾向于靠近障碍物,这与局部终点偏移距离越小,飞行成本越低的事实相吻合;当η1和η3参数占比越大,局部最优路径点倾向于远离障碍物,这与障碍物与飞行走廊距离、未来航迹与障碍物距离越远,飞行安全越高的事实相吻合。通过以上仿真结果与分析说明了障碍威胁评价函数设置的有效性。针对危险系数较高的障碍物,需要尽可能地远离,并且要提前进行规避,这种情况下通常将权重值η2设置小一点,使得无人机避障轨迹的局部最优路径点远离障碍物;对于危险系数低或者半径较小的障碍物,出于减少飞行航程节省燃料等因素,趋向于将权值η2设置大一点,使得无人机尽可能靠近障碍物绕行。另外仿真中蒙特卡洛的采样细节如图8所示。

图8 蒙特卡洛采样细节图

4 结 论

针对固定翼无人机避障问题,提出了一种基于安全飞行走廊的密集障碍避障算法。首先建立了固定翼无人机的运动学模型与轨迹模型、给出了无人机安全飞行走廊的定义、构建了考虑无人机飞行成本、飞行安全,针对安全飞行走廊的障碍威胁评价函数,并将无人机的避障问题转化为目标点的求解问题;然后针对障碍物密集造成计算复杂的问题,通过DBSCAN算法将满足条件的障碍物聚类,减小计算的复杂程度,再通过蒙特卡洛采样法逼近无人机避障路径局部最优终点,基于无人机的当前位置与避障路径的局部最优终点通过Dubins曲线规划出固定翼无人机障碍规避路径,最终使得避障路径尽可能靠近任务路径,将飞行轨迹限制在一个飞行走廊当中。

本文所设计的算法能够让无人机在复杂密集多障碍环境中规划出一条满足无人机机动性约束的、综合考虑无人机飞行安全与飞行效率的飞行路径,对于安全性要求严苛的固定翼无人机而言更加安全。另外能够通过调节障碍威胁评价函数中的权重,对无人机避障路径进行有偏向性的调节,使规划的避障路径更好地与任务目的相匹配。