基于改进人工势场法的四旋翼飞行器航迹规划*

卢艳军，李月茹

（沈阳航空航天大学自动化学院，沈阳 110136）

0 引言

飞行器已经广泛应用于军事和民用领域，并成为国内外众多机构的研究热点。其中，航迹规划问题也是飞行器研究的一个重要方向。理想的航迹规划方式应根据各种不同的飞行环境，灵活地制定飞行航迹，以便实现飞行器的完全自主飞行功能。目前多数飞行器仍采用地面远程操控和跟踪规划的方式来进行航迹规划，所使用的航迹规划方法多数是在飞行环境完全已知，且飞行过程中环境条件不变的情况下，飞行器按照事先规划好的航迹飞行。常用的航迹规划方法主要有：人工势场法、粒子群算法、A*算法、遗传算法、蚁群算法等。但是这些方法在面对环境与已知情况不一致时，都很难快速解决规划问题。

基于人工势场的方法是机器人运动规划中较为常用的方法。该方法对障碍物建立排斥势场，对目标点建立引力势场，综合目标对机器人的吸引力以及障碍物对机器人的排斥力，通过选取合适的势能函数参数和移动步长，从而形成运动轨迹[1]。该法设计简单、计算量小，实时性较高。但是，人工势场法依然存在目标不可到达、局部极小点等问题，导致机器人无法达到目标点。为了获得更好的运动轨迹，很多学者对该算法进行了改进。文献[2]和文献[3]分别提出两种更改排斥场函数的方法，改善目标不可达到问题；文献[4]提出障碍物连接法，改善了分散的障碍物对移动机器人的影响，并且简化了机器人对势场的计算；文献[5]提出机器人始终向势能场最小的方向运动的方式；文献[6]通过改变机器人受到的合斥力的方向，极大的避免目标不可到达和极小点问题。采用人工势场法进行四旋翼飞行器航迹规划，可以解决四旋翼飞行器对飞行环境感知的局限性问题，同样可以取得较好的航迹规划效果。

1 人工势场法

1.1 人工势场法

人工势场法是Khatib于1986年提出的[7]，基本思想是构造目标位置引力场和障碍物周围斥力场共同作用的人工势场，搜索势函数的下降方向来寻找无碰撞路径[8]。目标点产生“引力场”，周围障碍物产生“斥力场”，共同作用于运动物体，如图1所示。人工势场法的核心是构造引力势场和斥力势场函数。

图1 人工势场法示意图

传统人工势场法引力势场函数为：

其中，ka为吸引增益系数，为运动物体坐标到目标点坐标的距离，为运动物体在运动过程中任意点的坐标，为目标点坐标。由式（1）可见：目标点对运动物体的吸引作用随着运动物体与目标点距离的增大而增强。

传统人工势场法斥力势场函数为：

其中，kr为排斥增益系数；为运动物体到障碍物的距离；ρo为障碍物的有效影响距离，当时，障碍物对运动物体产生排斥作用。从式（2）中可见，在障碍物的有效影响范围内，障碍物对运动物体的排斥作用随着运动物体与障碍物距离的减小而增强。

1.2 改进的人工势场法

针对人工势场法的目标不可到达问题，以往文献中提出了多种改进的算法。文献[10]在原有解决目标不可到达问题的方法基础上，进一步改进，有效地解决了人工势场法存在的目标不可到达问题，规划的避障路径也比较高效、平滑。其改进后的引力和斥力方程，如式（3）和式（4）。

改进的斥力方程，保证了目标点在障碍物的影响距离以内时，运动物体所受障碍物的排斥力不会导致运动物体无法在目标点保持静止，运动物体在目标点处所受合力为零，规划的路径得到明显改善。

1.3 参数约束方程

应用人工势场法进行航迹规划时，为保证移动物体朝向目标点运动，要求斥力在引力方向上的分量小于引力；且下一航迹点比上一航迹点更靠近目标点[11]。由这两个条件可推算出二维空间航迹规划参数的约束方程。

航迹规划参数应满足以下不等式：

设向量Fa为引力，向量Fr为斥力合力，δ为步长，θ为引力和斥力合力的向量夹角。

向量Fr在向量Fa方向上的分力为Fr·cosθ，为了保证飞行器向靠近目标点的方向飞行，当θ≤π/2时，飞行器能飞向目标点；当θ≥π/2时，则必须满足，才能保证飞行器飞向目标点，此时由式（3）可得

设飞行器当前位置（x，y）与目标点（xg，y）g的距离为L。保证飞行器每走一个步长，都更接近目标点，即Ln＜L。

代入公式 Ln＜L，得

满足以上两个条件，则斥力在x，y方向上的分量小于引力分量，并保证飞行器每规划一步，更接近目标点。

2 基于选择穿越法的航迹规划方法

四旋翼飞行器低空环境定高飞行时，周围环境实时信息不完全已知。采用人工势场法进行航迹规划时，实时性高，计算量较小且设计步骤简易，但由于人工势场法自身的局限性，航迹可能出现局部极小点及震荡等问题，这些问题如得不得妥善解决，将会影响到航迹规划快速性和准确性。

2.1 局部极小点及震荡问题

四旋翼飞行器很容易在简单的环境中找出一条无碰撞的路径到达目标点。而飞行器要在复杂易变的低空环境中获得环境的全局信息，所需的通信量将会非常大，所以飞行器不可能获得全局的实时环境信息，而人工势场法就是只在活动局部环境信息情况下进行航迹规划。由于环境中威胁物和飞行器某些特殊的位置，飞行器可能会出现局部极小及震荡问题，即飞行器在某一点出现引力和斥力相互抵消的情况，随之可能引起航迹在该点处作徘徊震荡运动。这时在环境条件容许的情况下，可以增大引力系数或者减小物体的斥力系数，使飞行器走出当前局部极小摆脱震荡区域，朝着目标点前进。

例如，在只获得环境的局部信息情况下，飞行器就有可能处在如图2所示的特殊环境中，飞行器的任务是经由点S运动到目标点G。P点是航迹中的一点，飞行器在该点附件受到的引力和斥力的反复作用，使其在P点作徘徊震荡运动无法到达目标点G。此时如果把飞行器的航迹规划看作通过梯度法求二元非线性函数极值的过程[12]，则图中的P点实际上类似于该函数的一个局部极小点。将P点称为该环境中的一个“局部极小”。

图2 飞行器陷入极小点仿真图

2.2 选择穿越法

旋翼飞行器主要应用于低空飞行，飞行环境中障碍物多，选用人工势场法进行航迹规划，可以满足平稳、高效、实时性要求，但应用改进的人工势场法虽然解决了目标不可到达问题，但依据局部环境信息进行航迹规划，仍不能避免当势场中出现极小点问题，而旋翼飞行器的低空飞行环境中，障碍物较多，极易出现极小点问题。文中针对人工势场法一种特殊极小点问题，提出一种较好的解决办法。

图3 判断障碍物距离示意图

2.3 选择穿越法的实现

改进人工势场法，可以改善航迹规划中的一些问题，如文献[13-14]，考虑飞行器和障碍物的速度、加速度等动态特性，对传统的人工势场进行调节；文献[15]将飞行器与目标点之间的相对距离考虑进去，建立新的势场函数，如果飞行器陷入局部极小点，则重新定义势函数，跳出局部极小点等。文中介绍一种解决特殊情况下极小点问题的方法：“选择穿越”方法。

所谓选择穿越法，就是以飞行器在传感器只能检测到局部环境信息为前提，利用人工势场法进行实时航迹规划，在图2情况下，遇到局部极小点时，一种解决航迹规划问题的方法。首先，在飞行器到达极小点时，利用传感器信息，得到两个障碍物之间的距离大小，并判断障碍物之间距离是否达到保证飞行器安全通过的安全距离，如图3所示，若满足飞行器穿越的安全距离，则利用参数约束方程增大引力函数的引力系数，加大目标对飞行器的吸引作用，使飞行器跳出极小点，向目标点前进，跳出极小点后及时将引力系数的数值恢复。

若不满足穿越条件，即飞行器无法穿越障碍物，则及时调整合斥力的方向，将合斥力沿逆时针方向增加π/2 rad，如图4所示，使飞行器所受合力重新合成，进而改变飞行器运动方向，逃离极小点。判断飞行器跳出隐患区域，停止改变合斥力方向的该行为。

图4 飞行器受力示意图

那么首先要确定何时激活选择穿越行为，何时选择穿越行为。因为选择穿越行为是解决飞行器运动规划中的局部极小问题的，所以应该通过判断飞行器是否处在局部极小点再决定何时激活该行为。图2中局部极小的特点是飞行器到达后停止不动，左右两边都很接近障碍物，因此，激活条件可以定为：判断飞行器航迹规划一步后是否更接近目标点。

3 四旋翼飞行器航迹规划仿真实验

为了验证算法的有效性，应用MATLAB软件进行了仿真实验，设定算法中参数：起点位置（x，y）=（0，0），引力的增益系数ka=25，斥力的增益系数kr=4，障碍个数 n=4，步长 θ=0.1。

假设1：障碍物威胁使用圆形来近似，其几何中心和半径作为威胁物的主要参数，障碍物威胁作用可以相互叠加，对飞行器产生的斥力使其避开威胁。

假设2：飞行器用质点来表示，其与威胁物或目标点的距离为质点到圆心质心之间的距离。

针对仿真图2飞行器陷入局部极小点情况，采用选择穿越行为后，仿真结果如图5～图6所示。当飞行器到达极小点后，选择穿越行为被激活，飞行器判断障碍物是否能通过。若能通过，则穿过障碍，否则，更改斥力方向，继续前进。

图5 可穿越时matlab仿真图

图6 不可穿越时matlab仿真图

如图5、图6所示，当障碍物件距离大于安全距离时，飞行器穿越通过；当障碍物间距离小于安全距离时，改变斥力方向后继续前进。

4 结论

针对四旋翼飞行器一种特殊环境下航迹规划问题，提出了一种新型的基于人工势场的二维航迹规划方法。给出了目标和威胁物的虚拟力函数，并推导出了二维空间参数约束方程，使参数选择有了理论支撑；基于假定平坦空间进行规划；人工势场法在给定局部环境信息陷入局部极小的情况，提出一种选择穿越行为，通过判断移动飞行器进入局部极小点激活该行为，使飞行器沿着障碍物的边缘运动，从而跳出局部极小，并进行了仿真实验，证明此方法是有效的。