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

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

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

2、传统人工势场法

设n架无人机，m个障碍物
无人机坐标 $P_{i}$ ( $x_{i}$ , $y_{i}$ , $z_{i}$ )，障碍物坐标 $P_{oj}$ ( $x_{oj}$ , $y_{oj}$ , $z_{oj}$ )
无人机目标位置 $P_{gi}$ ( $x_{gi}$ , $y_{gi}$ , $z_{gi}$ )
第i架UAV与第j障碍物距离
第 i 架UAV与第 k 架之间的距离
第 i 架UAV与对应目标距离

$k_{1}$ 障碍物对无人机斥力增益系数；
$k_{2}$ 无人机对无人机斥力增益系数;
?目标点对无人机引力增益系数;
$U_{rep1}$ 障碍物对无人机产生的斥力场；
$U_{rep2}$ 其他UAV对目标UAV产生的斥力场；
$l_{max}$ 人为设定的障碍物影响距离;
$U_{atti}$ 无人机对应目标点对其产生的引力场。

$U_{i}$ 为无人机i所收到的合势力场

3、改进势场函数

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

(距离目的地越近 $l_{g}^{p}$ 越小，斥力势场越小）

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

某一时刻无人机i位置 $P_{ik}$ ( $x_{ik}$ , $y_{ik}$ , $z_{ik}$ ),
上一时刻位置 $P_{ik-1}$ ( $x_{ik-1}$ , $y_{ik-1}$ , $z_{ik-1}$ )，
‖ $P_{ik}$ − $P_{ik-1}$ ‖<?且‖ $P_{ik}$ − $P_{ik-1}$ ‖>?，则无人机陷入了局部极小点