基于改进型人工势场法的多无人机三维编队路径规划

1、人工势场法的定义及优缺点

      将无人机的运动设计成人造引力场中的运动，目标点对无人机产生引力，障碍物对移动无人机产生斥”，最后通过求合力控制无人机的运动。应用势场法规划出来的路径一般是比较平滑并且安全、容易实现。
      障碍物目标不可达、容易陷入局部极小点、障碍物附近震荡
      障碍物与目标点距离很近，无人机逼近目标的同时，也将向障碍物靠近，所受引力不断减小，斥力不断增大，目标不可达；所受斥力和引力大小相等方向相反，陷入局部极小点；航迹上存在狭窄区域，即四周都有障碍物，无人机发生震荡现象，无人机来回移动，不能前行。

2、传统人工势场法

设n架无人机，m个障碍物
无人机坐标 (,, )，障碍物坐标 (,, )
无人机目标位置 (,, )
第i架UAV与第j障碍物距离
第 i 架UAV与第 k 架之间的距离
第 i 架UAV与对应目标距离 

障碍物对无人机斥力增益系数；
无人机对无人机斥力增益系数; 
????目标点对无人机引力增益系数; 
障碍物对无人机产生的斥力场；
其他UAV对目标UAV产生的斥力场；
​​​​​​​人为设定的障碍物影响距离; 
​​​​​​​无人机对应目标点对其产生的引力场。

为无人机i所收到的合势力场​​​​​​​

3、改进势场函数

引入无人机与目标之间相对距离  ，将原来的斥力场函数乘以一个  (p是一个大于 0 的任意实数)，使得目标位置斥力为0

(距离目的地越近越小，斥力势场越小）

4、局部极小点判断与逃离

某一时刻无人机i位置 (,, ),
上一时刻位置 (,, )，
‖− ‖<????且‖− ‖>????，则无人机陷入了局部极小点

在无人机指向目标点方向的法平面上附加一个势场，势场方向随机，使其沿着目标方向90度的方向移动
附加力垂直于于目标方向，这样更容易逃离局部极小点

代表极小点位置，  附加势场最大影响距离

5、实验仿真

程序设计思路

传统人工势场法（由于斥力场发生抖动无法到达目的地）

改进势场函数（陷入极小点无法达到目的地，且在极小点位置发生抖动）

加入极小点判断机制（顺利到达目的点）