基于前馈控制人工势场法的无人机群系统设计

丁佳宇， 王茂森， 戴劲松

(南京理工大学 机械工程学院，江苏 南京210094)

0 引 言

当今，无人机(unmanned aerial vehicle,UAV)编队飞行在军用与民用领域有着广阔的应用背景。无论是在战场作战还是大型表演，无人机编队飞行都有着不可替代的地位。在实际应用环境中，实时避障是无人机编队飞行中的一大难题[1]。

针对无人机实时避障问题，国内外专家提出的控制方法有人工势场法[2]、神经网络[3]和最优化理论[4]等方法。其中人工势场法因计算量小，实现简单，实时性好而被广泛运用[5]。人工势场法是由Khatib提出的一种虚拟力法。它将被控对象在周围环境中的运动设计成一种抽象的人造力场的运动，目标点对被控对象产生“引力”，障碍物对被控对象产生“斥力”，最后通过计算被控对象所受的合力来控制它的运动方向[6]。人工势场法规划出来的路径一般是比较平滑且安全的，但是这种方法存在局部最优与路径振荡的问题。为解决局部最优问题，国内外学者提出了不同的改进方法。例如，文献[7]在机器人陷入局部极小点后，提出设置中间目标的方法，使机器人跳出局部极小点。文献[8]设计了一种分层势场法，引入回环力，实现了无人机的路径规划，并提出了一种路径规划评价机制，但是该方法随着分层的增加，规划速度越慢。文献[9]在当无人机陷入局部极小点时，系统在原目标点周围产生一个虚拟目标，从而使无人机所受合力不为零，跳出极小点。

但上述方法仅对部分局部最优问题有效，例如当在目标点处存在障碍物时，文献[7，9]并不能有效地解决此问题。此外，无人机路径规划中存在的振荡问题，一直不能得到有效的应对措施[10，11]。

本文对斥力场作分层处理，分为可逼近层与危险层，以解决不可到达目标点的问题。此外，对路径点作类似滤波处理的前馈控制策略，将已规划的路径点作为下次规划的参考值，并设定合适的权重值，从而使规划的路径更为平滑，也可一定程度上解决局部最优问题。

为避免由于机群与地面控制平台失去联系，导致无人机群失控，造成巨大的经济损失，本文提出在主机上搭载ARM板，收集主从机的位置与障碍信息，对无人机群作统一的路径规划，从而在地面控制系统失效时，无人机群仍能完成预设任务，提高了无人机群控制系统的安全性与稳定性[12，13]。

1 改进人工势场法

1.1 改进斥力场函数

当障碍物位于目标点附近时，会造成目标点不可到达的问题。本文提出对目标点附近障碍物的斥力场函数作分层处理，如图1所示，分为可逼近层与危险层。其中，可逼近层的距离为路径规划中的单位步长l,危险层的半径为无人机的最大半径ρ1。

图1 斥力场分层示意

传统的斥力场函数为

(1)

式中η为位置增益系数，ρ为无人机在空间的位置X与障碍物之间的最短距离，ρ0为一个常数，代表障碍物的影响距离。n为一个大于零的任意常数，Xgoal为目标点位置。

对目标点附近障碍的斥力场函数修改为

Fre(X)=

(2)

式中ρ1为无人机的最大半径距离，l为路径规划中的2个单位步长。当无人机处于危险层时，斥力函数不变，而当处于可逼近层时，斥力函数乘以一个关于ρ的函数，使得此时的斥力随着无人机与障碍物距离的增加而急速减小。从而解决了因目标点附近存在障碍物而导致不可逼近的问题。

图2为斥力场优化前后的路径规划仿真效果。

图2 斥力场函数优化效果

从仿真结果可以发现，优化前的路径规划算法由于目标点处存在障碍物，无人机所受合力的方向一直是远离目标点的方向，使得其在目标点处徘徊，始终到达不了目标。经优化后，无人机在满足目标点处避障的要求外，仍能选择较短到达目标点的路径。

1.2 前馈控制优化

每次路径规划中，在斥力场与引力场的作用下，无人机所受合力的方向即为路径规划的方向，在规划方向上移动一定的步长

Xnext=X+1×cos(Angle)

Ynext=Y+1×cos(Angle)

(3)

式中X，Y为上一路径点的X，Y坐标值，Angle为当前路径规划的方向角度值，Xnext，Ynext为当前路径点的X，Y坐标值。

在人工势场法仿真过程中发现，当无人机运动到障碍物密集处时，路径规划可能会出现振荡现象。如图3所示。

图3 人工势场振荡现象

为解决图3中的路径振荡问题，本文采用前馈控制来消除此问题。

当算法检测到路径点出现振荡时，计算出上一路径规划的方向角度Angle次路径规划方向角度Angle(i)的平均值，在此成为期望角度值ExpectAngle。

ExpectAngle=(Angle(i-1)+Angle(i))/2

(4)

计算出期望角度值ExpectAngle与当前路径规划的角度值Angle的偏差绝对值E

E=|ExpectAngle-Angle|

求出偏差值与当前路径规划角度值的百分比Er(Er≥0)

Er=|E/Angle|×100 %

(5)

根据Er的值来决定期望角度值ExpectAngle与本次路径规划计算的角度值在本次实际路径点中所占的权重值。

当Er等于0时，表明期望角度值与当前规划的角度值一致，则设定期望角度值的权重为0，当前角度值的权重为1。当Er处于0～1之间时，表明期望角度值与当前规划的角度值有一定的偏差，此时，期望角度值的权重由已连续振荡的次数决定,当前规划的角度值则是1与期望角度值的权重的差值。当Er大于1时，期望角度值的权重值与Er的二次方成反比关系。具体公式如下

(6)

式中Ca为期望角度值的权重，Cb为当前路径规划角度值的权重，c为振荡次数。

将计算得到的权重值代入公式求得实际路径规划的角度值

A=ExpectAngle×Ca+Angle×Cb

(7)

最后将求得的角度值代入式(3)，解算出实际路径规划的坐标值。用MATLAB仿真结果如图4所示。

图4 人工势场优化效果

从图4可以发现，在图3中出现的两个较大振荡得到明显改善。在振荡产生后，算法能够对当前的路径规划做出相应的调整，使路径更为平整光滑，同时，有不丧失大角度转向的能力。

1.3 无人机群路径规划设计

在满足单机路径规划避障要求后，无人机群的路径规划还需考虑无人机之间的防碰撞问题。

本文的设计思路是先对无人机群进行编号，确定无人机群路径规划的优先级，路径规划顺序按照无人机优先级的高低进行排序。1#机～3#机优先级由高到低排列，每次规划确定下一路径点时，1#机优先规划，并将2#机与3#机的上一路径点当作障碍点，得出1#机的下一路径点。接着，2#机将1#机已规划的路径点与3#机上一路径点当作障碍点进行路径规划，同理，3#机将1#机与2#机已规划的路径点作为障碍点[14]。

当1#机规划完当前路径点后，2#机将1#机已规划完的路径点当作障碍物，但同一时刻2#机的路径还不应与1#机的路径发生干涉。考虑到上述问题，对无人机自身成为障碍时的斥力场进行单独分层处理。如图5所示。

图5 无人机群路径规划斥力场分层示意

图5中将无人机作为障碍点时的斥力场分为两层，里面一层的半径为步长l，最外层的半径为ρ1，这种分层处理保证了同时刻无人机路径之间不会发生干涉，达到无人机群防碰撞的要求。以下为上述斥力场函数表达式

Fre(X)=

(8)

以三架无人机为例，用MATLAB仿真结果如图6所示。

图6 多无人机避障仿真结果

从图6中可以发现，在无人机路径不发生干涉时，路径规划的效果符合避障要求。考虑到无人机群在实际路径规划中很有可能会出现路径干涉的现象，现对路径出现干涉的情况进行仿真，仿真结果如图7所示。

图7 路径干涉下的多无人机避障仿真结果

图7中的无人机群路径规划曲线出现干涉现象,但在同一时刻,三架无人机之间的距离仍控制在安全距离内。此次仿真所选取的步长l与障碍物最大影响距离L都相对来说较小，这是为了令仿真结果尽可能凸显出路径规划的细节，为后续算法优化提供参考。而在实际运用中，考虑到无人机的响应速度与飞行姿态与位置的连贯性，通常将步长l选取为无人机的半径，障碍物最大影响距离L选取为步长l的5倍以上。

图8为选取实际路径规划中参数的仿真示意。

在选取较大的步长与障碍物最大影响距离后，可以发现无人机群路径路线更为平滑，距离更短，满足无人机群避障的要求。

2 基于ARM上位机的无人机群控制系统搭建

2.1 ARM上位机与无人机群之间的通信

ARM上位机与无人群之间采用无线通信连接，ARM板将传输的信息通过串口发送给ZigBee，ZigBee再将信息传输给整个无人机群，通信链路图如图9所示。

图9 ARM与无人机群通信链路

ZigBee具有自组网能力，在同一网络可容纳65 000个节点，满足大多数无人机群的数量要求，并且它的安全性与可靠性较高，适合用于无人机群的内部通信。

图10为实验现场搭建效果。

图10 无人机群通信搭建实验效果

由于目前主流通信协议MAVLink是在串口通信基础上设计的，因而可以直接将根据MAVLink通信协议打包而成的数据信息发送给ZigBee，再发送给无人机群。而MAVLink可根据不同固件号的无人机打包其对应的消息帧，避免了无人机群间通信的干扰。图11为在ARM板上收发消息帧的实验效果。

图11 ARM板收发消息帧实验效果

2.2 路径规划算法实现

在上文路径规划算法中，将每次解算出的路径点赋值给MAVLink中的航点命令打包函数，并通过串口发送给对应的无人机，完成路径规划的控制。如图12所示。

图12 人工势场法路径规划点效果

在确定无人机群各个单位的起始点与目标点后，将他们的经纬度值转换为XY坐标值，路径规划算法结算完实时路径点的坐标后，再将坐标值转换为经纬度发送给无人机。在无人机群规划距离较短时，利用投影变换法即可满足飞行要求。

3 结束语

针对传统人工势场法出现的不可到达问题，改进了目标点附近障碍点的斥力函数。提出了一种基于前馈控制的人工势场改进法，有效解决了规划过程中出现的振荡与停滞问题，并一定程度上优化了路径曲线。在此基础上，实现了改进后无人机群的人工势场路径规划算法，本无人机群在保证避障要求的基础上，优化了无人机群路径曲线，避免陷入局部最优。利用上述改进的无人机群避障算法，搭建了无人机群控制通信链路，并将算法与其融合，完成了无人机群的系统设计，为无人机群飞提供了一种新的搭建思路。