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

Posted 2021-06-16 MatlabQQ1575304183

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

一、简介

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

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

 

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

引力场：

常用的引力函数：

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

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

 Fig .引力场模型

斥力场：

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

斥力就是斥力场的梯度

                                                                         Fig 斥力场模型

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

二、存在的问题

（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
make_movie = false;
make_pot     = 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( make_movie )
    writerObj = VideoWriter('3D_test3.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
ind_c = -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
end_loc = [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 = [];
dims_o = [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).*( dims_o(:,2)-dims_o(:,1) ) + dims_o(:,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

完整代码添加QQ1575304183