【无人机路径规划】基于人工势场实现无人机编队路径规划matlab源码

人工势场法是局部路径规划的一种比较常用的方法。这种方法假设机器人在一种虚拟力场下运动。

一、简介

如图所示，机器人在一个二维环境下运动，图中指出了机器人，障碍和目标之间的相对位置。 

这个图比较清晰的说明了人工势场法的作用，物体的初始点在一个较高的“山头”上，要到达的目标点在“山脚”下，这就形成了一种势场，物体在这种势的引导下，避开障碍物，到达目标点。

人工势场包括引力场合斥力场，其中目标点对物体产生引力，引导物体朝向其运动(这一点有点类似于A*算法中的启发函数h)。障碍物对物体产生斥力，避免物体与之发生碰撞。物体在路径上每一点所受的合力等于这一点所有斥力和引力的和。这里的关键是如何构建引力场和斥力场。下面我们分别讨论一下：

引力场：

常用的引力函数：

这里的ε是尺度因子.ρ(q,q_goal)表示物体当前状态与目标的距离。引力场有了，那么引力就是引力场对距离的导数(类比物理里面W=FX)：

关于梯度的算法可以参考相关资料，简单提一下，二元函数梯度是酱紫的[δx,δy],这个符号是偏导数，不太对，见谅。

 Fig .引力场模型

斥力场：

公式(3)是传统的斥力场公式，现在还没有搞清楚是怎么推导出来的。公式中η是斥力尺度因子，ρ(q,q_obs)代表物体和障碍物之间的距离。ρ_0代表每个障碍物的影响半径。换言之，离开一定的距离，障碍物就对物体没有斥力影响。

斥力就是斥力场的梯度

                                                                         Fig 斥力场模型

总的场就是斥力场合引力场的叠加，也就是U=Uatt+Urep,总的力也是对对应的分力的叠加，如下图所示：

二、存在的问题

(a) 当物体离目标点比较远时，引力将变的特别大，相对较小的斥力在甚至可以忽略的情况下，物体路径上可能会碰到障碍物

(b)当目标点附近有障碍物时，斥力将非常大，引力相对较小，物体很难到达目标点

(c)在某个点，引力和斥力刚好大小相等，方向想反，则物体容易陷入局部最优解或震荡

三、各种改进版本的人工势场法

(a)对于可能会碰到障碍物的问题，可以通过修正引力函数来解决，避免由于离目标点太远导致引力过大

和(1)式相比，(5)式增加了范围限定。d*_goal 给定了一个阈值限定了目标和物体之间的距离。对应的梯度也就是引力相应变成：

(b)目标点附近有障碍物导致目标不可达的问题，引入一种新的斥力函数

这里在原有斥力场的基础上，加上了目标和物体距离的影响,(n是正数，我看到有篇文献上n=2)。直观上来说，物体靠近目标时，虽然斥力场要增大，但是距离在减少，所以在一定程度上可以起到对斥力场的拖拽作用

相应斥力变成：

所以可以看到这里引力分为两个部分，编程时要格外注意

(c)局部最优问题是一个人工势场法的一个大问题，这里可以通过加一个随机扰动，让物体跳出局部最优值。类似于梯度下降法局部最优值的解决方案。

``` %% Test Swarm Trajectory in 3-D % Author: Christian Howard % Date : 2/17/2015 % Info : This is a script developed to show the swarm navigation % algorithms in 3-D. This script also allows for the user to specify % various waypoints so that they can maneuver around a space

clear all; false = 0; true = 1;

%% Setup the movie stuff makemovie = false; makepot = false; % make video showing potential field | Takes a LONG time to make, so usually set to false writerObj = [];

%% Define the plane of points to evaluate potential function, if you are making video of potential function x = linspace(0, 50, 100); y = linspace(0, 50, 100); z = 5;

[X,Y] = meshgrid(x,y); [rx, cx] = size(X); Z = zeros(rx, cx);

%% Setup the movie file and frame rate if( makemovie ) writerObj = VideoWriter('3Dtest3.avi'); writerObj.FrameRate = 15; open(writerObj); end

% initial perspective in video az = -60; el = 20;

% final perspective in video azf = -60; elf = 20;

%% Parameter specifications for drones Nd = 3; % number of drones in swarm indc = -1; % index for center/lead drone radius = 2; % radius for drones and possibly obstacles dims = [0, 50; 0, 50; 0, 50]; % first row is lower and upper x bounds % second row is lower and upper y bounds % third row is lower and upper z bounds xc = [40, 40, 40]'; % initial location for central/lead drone endloc = [5, 5, 5]'; % Desired end location

%% Setup the waypoint object dist_thresh = 2; % The distance threshold the swarm must be within of the waypoint % to make the waypoint change to the new one droneWaypoints = []; copyWaypoints = [];

for i = 1:Nd wypt = Waypoints( dist_thresh );

% Add waypoint structure to drone waypoint structure
% The drone waypoint structure represents individual waypoints for each
% drone, so they don't have to move in the same direction. 
droneWaypoints(i).w = wypt;

wypt2 = Waypoints( dist_thresh ); % copy waypoints for use in plotting waypoints

% Add waypoint structure to copy waypoint structure
copyWaypoints(i).w = wypt2;

end

% Initialize the waypoints % For simplicity in this example, will initilize waypoint paths to be the % same thing for each drone. pt1 = [15;50;20]; pt2 = [0; 50; 0]; pt3 = end_loc; pt4 = [20;20;20]; pt5 = [30; 30; 30]; pt6 = [10; 40; 20]; pt7 = [10;10;10]; pt8 = [15; 15; 15]; pt9 = [10; 20; 20];

i = 1;

droneWaypoints(i).w.addPoint( pt1 ); droneWaypoints(i).w.addPoint( pt2 ); droneWaypoints(i).w.addPoint( pt3 );

copyWaypoints(i).w.addPoint( pt1 ); copyWaypoints(i).w.addPoint( pt2 ); copyWaypoints(i).w.addPoint( pt3 );

i = 2;

droneWaypoints(i).w.addPoint( pt4 ); droneWaypoints(i).w.addPoint( pt5 ); droneWaypoints(i).w.addPoint( pt6 );

copyWaypoints(i).w.addPoint( pt4 ); copyWaypoints(i).w.addPoint( pt5 ); copyWaypoints(i).w.addPoint( pt6 );

i = 3;

droneWaypoints(i).w.addPoint( pt7 ); droneWaypoints(i).w.addPoint( pt8 ); droneWaypoints(i).w.addPoint( pt9 );

copyWaypoints(i).w.addPoint( pt7 ); copyWaypoints(i).w.addPoint( pt8 ); copyWaypoints(i).w.addPoint( pt9 );

%% Parameter specifications for the obstacles No = 4; i = 1; obst = []; dimso = [0, 10; 0, 10; 0, 10]; % first row is lower and upper x bounds % second row is lower and upper y bounds while( i <= No ) %randomly generate position of obstacle within bounds pos = rand(3,1).*( dimso(:,2)-dimso(:,1) ) + dimso(:,1);

% Add new obstacle to obstacle array obst
obst = [obst, Obstacle(pos,radius)];
i = i + 1;

end

%% Create drones and obstacle arrays i = 1; drones = []; while( i <= Nd ) if( i == ind_c ) % Add lead drone to drone array drones = [drones, Drone(radius, xc , 2)]; else % Add a follower drone to drone array DEL = [xc - 10, xc + 10]; % make swarm initialize around lead drone randomly pos = rand(3,1).*( DEL(:,2)-DEL(:,1) ) + DEL(:,1); drones = [drones, Drone(radius, pos, 1)]; end i = i + 1; end

%% Setup figure traits for the movie close all; h = figure('Position', [10, 10, 1000, 800]); set(gca,'nextplot','replacechildren'); set(gcf,'Renderer','zbuffer'); dims2 = dims';

%% Do the initial drawing i = 1;

figure(1) N = 1; if( make_pot ) N = 2; end

subplot(1,N,1) while( i <=Nd ) % loop through drones drawObject(drones(i)); if( i == 1 ) hold on end i = i + 1; end

i = 1; while( i <= No ) % loop through obstacles drawObject(obst(i)); i = i + 1; end hold off grid on view(az, el) axis(dims2(:)) xpos = [];

%% Do iterations of the drone moving to final location it = 1; count = 0; done = 0; while( done == 0 ) i = 1;

% Compute new locations
while( i <= Nd )
    pt = droneWaypoints(i).w.getWaypoint( drones(i).pos );
    drones(i).pos = drones(i).pos + GradientDescentUpdate(drones(i).pos, i, drones, obst, pt );

    if( count > 20 || it >= 150 )
        done = 1;
    end
    i = i + 1;
end

% Draw drones in new position
i = 1;

figure(1)
subplot(1,N,1)

while( i <= Nd ) % loop through drones
    drawObject(drones(i));
    if( i == 1 )
        hold on
    end
    i = i + 1;
end

i = 1;
while( i <= Nd )
    drawWaypoints( copyWaypoints(i).w );
    i = i + 1;
end

i = 1;
while( i <= No ) % loop through obstacles
    drawObject(obst(i));
    i = i + 1;
end
hold off
coef = it/100;

%zoom(2)
grid on
axis(dims2(:))
view(az + coef*(azf - az) , el + coef*(elf - el) )
%rotate(h, [0, 0, 1], 1)
pause(.01)




    frame = getframe(gcf);
    writeVideo(writerObj,frame);
end

it = it + 1;

end

%% Close movie file, if you are recording a movie if( make_movie ) close(writerObj); end ```