【路径规划】基于麻雀搜索算法的无人机三维路径规划matlab 源码

1. 麻雀搜索算法简介

(以下描述，均不是学术用语，仅供大家快乐的阅读)\ 　　麻雀搜索算法(sparrow search algorithm)是根据麻雀觅食并逃避捕食者的行为而提出的群智能优化算法。提出时间是2020年，也就是今年，这是一个新鲜热乎的新算法，相关的论文和研究还比较少，有可能还有一些正在发表中，受疫情影响需要论文的同学抓紧时间水论文了。\ 　　麻雀搜索算法主要模拟了麻雀群觅食的过程。麻雀群觅食过程也是发现者-跟随者模型的一种，同时还叠加了侦查预警机制。麻雀中找到食物较好的个体作为发现者，其他个体作为跟随者，同时种群中选取一定比例的个体进行侦查预警，如果发现危险则放弃食物，安全第一。\ 　　麻雀搜索算法的具体实现其实和人工蜂群算法非常相似，基本结构几乎一致，但是搜索算子有一定的差异，可以说是一种人工蜂群算法的改进算法。\ 　　麻雀搜索算法的相关论文比较少，只看了原始论文，算法的描述比较详细，不过可以看出论文编排的比较匆忙，有部分公式显得过于复杂，影响理解。下面我会根据自己的理解对其中的部分公式进行简化，如果有不对的地方，欢迎大家留言。

2. 算法流程

这次我们的主角是一群麻雀。

\ 　　麻雀虽小五脏俱全，每只麻雀只有一个属性：位置，代表它找到的食物的位置。每只麻雀有三种可能的行为：1.作为发现者，继续搜索食物，2，作为跟随者，跟随一个发现者觅食，3.警戒侦查，有危险则放弃食物。\ 　　在D维解空间内每只麻雀的位置为 ，适应度值 。\ 　　这群麻雀中有N只麻雀，每代选取种群中位置最好的PN只麻雀作为发现者，剩余的N-PN只麻雀作为跟随者。

2.1更新发现者位置

每代发现者的位置更新公式如下：

\ 　　上图为文中的公式描述，有些不准确，更加精确的描述如下：

\ 　　其中表示种群中第t代中第i个个体的第d维位置， 为(0，1]中的均匀随机数，Q为一个标准正态分布随机数。为[0,1]中的均匀随机数，ST为警戒阈值，取值范围为[0.5,1.0]。\ 　　可以看出，当大于ST时，该发现者将按正态分布随机移动到当前位置附近。(其值收敛于最优位置)。\ 　　当小于ST时是什么情况呢，我们先看看函数 的图像，其中取值为1000。

　　可以看出在其分布随着x的变大，取值范围逐渐由(0,1)慢慢缩小为约(0,0.4)。\ X值较小时，取值靠近1的概率较高，随着X的增大，取值的分布变得较为均匀。\ 所以当 小于ST，麻雀的每一维都在变小，当然，这不是一个好策略。(其值收敛于0)。

2.2更新跟随者位置

文中跟随者的位置更新公式及描述如下图：

\ 　　首先我们先看看是一个大小为1*D的矩阵(1行D列)，其每一维都随机从{-1,1}中选取 。\ 　　举个简单的例子，假设 ，X=(x1,x2,x3)=(1,2,3)，则

\ 　　由于是行向量与矩阵运算，所以文中公式两边不应该出现公式j,出现公式j则表示该变量是一个数值而非一个向量。\ 　　简化可得其位置更新公式如下：

　　其中xw为当前种群中麻雀的最差位置，xb则为种群中麻雀的最有位置。\ 　　从公式(2)中可以看出，若i>n/2时，其值为一个标准正态分布随机数与一个以自然对数为底数的指数函数的积，当种群收敛时其取值符合标准正态分布随机数。(其值会收敛于0)。\ 　　若i<=n/2时，其取值为当前最优的麻雀的位置加上该麻雀与最优位置每一维距离随机加减后，将总和均分到每一维上。解释有点绕口，请结合公式自行理解。该过程可以描述为在当前最优位置附近随机找一个位置，且每一维距最优位置的方差将会变得更小，即不会出现在某一维上与最优位置相差较大，而其他位置相差较小。(其值收敛于最优位置)。

2.3侦查预警行为

在麻雀觅食的同时他们中的部分会负责警戒，当危险靠近时，他们会放弃当前的食物，即无论该麻雀是发现者还是跟随者，都将放弃当前的食物而移动到一个新的位置。\ 　　每代将从种群中随机选择SD个个体进行预警行为。\ 其位置更新公式如下：

\ 　　其中为符合标准正态分布的随机数，K为[-1,1]的均匀随机数，为一个较小的数防止分母唯一。与原文中的公式相比，我去除了原绝对值符号，因为两个随机数均是关于原点中心对称，增加的分母绝对值，防止分母取值为0。其中为最差位置的麻雀的适应度值。\ 　　从公式(3)中可以看出，如果该预警的麻雀处于当前的最优位置时，它会逃离到自身附近的一个位置，具体有多近取决于自身距离最差位置与自身位置食物与最差食物的差别的比值；如果该麻雀不是处于最优位置的那一只，它讲逃到当前最优位置附近。(其值收敛于最优位置)。

麻雀搜索算法的流程图如下：

流程图

　　可以看出麻雀搜索算法的流程非常的简单。但是根据上面对这三个更新公式的分析可以看出，麻雀搜索算法的更新方式可大致分为两种：1.向当前最优位置靠近；2.向原点靠近。\ 　　使用这两种方式来更新麻雀的位置将会使算法容易陷入局部最优。直觉告诉我拥有这样的更新规则的算法不是一个优秀的算法。具体的问题下面实验中进行详述

``` close all; clear all; clc; addpath(genpath('./'));

%% Plan path disp('Planning ...'); map = loadmap('maps/map4.txt', 0.1, 0.5, 0.25); start = { [1 7 1]}; stop = {[0.1 17 3]}; %start = {[0 1 5]}; %stop = {[19 1 5]}; nquad = length(start); for qn = 1:nquad v = cputime; path{qn} = dijkstra(map, start{qn}, stop{qn}); c = cputime - v; fprintf('Algo Execution time = %d \n',c); end if nquad == 1 plotpath(map, path{1}); else % you could modify your plot_path to handle cell input for multiple robots end

%% Additional init script init_script;

%% Run trajectory trajectory = test_trajectory(start, stop, map, path, true); % with visualization ```

``` function [xtraj, ttraj, terminatecond] = testtrajectory(start, stop, map, path, vis) % TESTTRAJECTORY simulates the robot from START to STOP following a PATH % that's been planned for MAP. % start - a 3d vector or a cell contains multiple 3d vectors % stop - a 3d vector or a cell contains multiple 3d vectors % map - map generated by your loadmap % path - n x 3 matrix path planned by your dijkstra algorithm % vis - true for displaying visualization

%Controller and trajectory generator handles controlhandle = @controller; trajhandle = @trajectory_generator;

% Make cell if ~iscell(start), start = {start}; end if ~iscell(stop), stop = {stop}; end if ~iscell(path), path = {path} ;end

% Get nquad nquad = length(start);

% Make column vector for qn = 1:nquad start{qn} = start{qn}(:); stop{qn} = stop{qn}(:); end

% Quadrotor model params = nanoplus();

%% ******** FIGURES ********* % Environment figure if nargin < 5 vis = true; end

fprintf('Initializing figures...\n') if vis hfig = figure('Name', 'Environment'); else hfig = figure('Name', 'Environment', 'Visible', 'Off'); end if nquad == 1 plotpath(map, path{1}); else % for qn = 1:nquad % plotpath(map, path{qn}); % end % you could modify your plotpath to handle cell input for multiple robots end h3d = gca; drawnow; xlabel('x [m]'); ylabel('y [m]'); zlabel('z [m]') quadcolors = lines(nquad); set(gcf,'Renderer','OpenGL')

%% Trying Animation of Blocks NoofBlocks = size(map(:,1),1); x0 = map(1,4); x1 = map(2,4); y0 = map(1,5); y1 = map(2,5); z0 = map(1,6); z1 = map(2,6);

for i=1:2:NoofBlocks

    xb_0 = map(i,1);
    xb_1 = map(i+1,1);
    yb_0 = map(i,2);
    yb_1 = map(i+1,2);
    zb_0 = map(i,3);
    zb_1 = map(i+1,3);

    B_1 = [xb_0 yb_0 zb_0]';
    B_2 = [xb_1 yb_0 zb_0]';
    B_3 = [xb_0 yb_0 zb_1]';
    B_4 = [xb_1 yb_0 zb_1]';
    B_5 = [xb_0 yb_1 zb_0]';
    B_6 = [xb_1 yb_1 zb_0]';
    B_7 = [xb_0 yb_1 zb_1]';
    B_8 = [xb_1 yb_1 zb_1]';


%     BlockCoordinatesMatrix(j:j+7,:) = [B_1';B_2';B_3';B_4';B_5';B_6';B_7';B_8'];

% BlockCoordinatesMatrix(j:j+1,:) = [B1';B8']; % BlockCoordinates(i,:) = {B1 B2 B3 B4 B5 B6 B7 B8}; % j = j+2;

S_1 = [B_1 B_2 B_4 B_3];
    S_2 = [B_5 B_6 B_8 B_7];
    S_3 = [B_3 B_4 B_8 B_7];
    S_4 = [B_1 B_2 B_6 B_5];
    S_5 = [B_1 B_3 B_7 B_5];
    S_6 = [B_2 B_4 B_8 B_6];

    fill3([S_1(1,:)' S_2(1,:)' S_3(1,:)' S_4(1,:)' S_5(1,:)' S_6(1,:)'],[S_1(2,:)' S_2(2,:)' S_3(2,:)' S_4(2,:)' S_5(2,:)' S_6(2,:)'],[S_1(3,:)' S_2(3,:)' S_3(3,:)' S_4(3,:)' S_5(3,:)' S_6(3,:)'],[1 0 0]);%[cell2mat(Block(i,8))/255 cell2mat(Block(i,9))/255 cell2mat(Block(i,10))/255]);
    xlabel('x'); ylabel('y'); zlabel('z'); 
    axis([min(x_0,x_1) (max(x_0,x_1)) min(y_0,y_1) (max(y_0,y_1)) min(z_0,z_1) (max(z_0,z_1))])
    grid
    hold on
end

%% ****** INITIAL CONDITIONS ****** fprintf('Setting initial conditions...\n') % Maximum time that the quadrotor is allowed to fly timetol = 50; % maximum simulation time starttime = 0; % start of simulation in seconds tstep = 0.01; % this determines the time step at which the solution is given cstep = 0.05; % image capture time interval nstep = cstep/tstep; time = starttime; % current time maxiter = timetol / cstep; % max iteration for qn = 1:nquad % Get start and stop position x0{qn} = initstate(start{qn}, 0); xtraj{qn} = zeros(maxiter*nstep, length(x0{qn})); ttraj{qn} = zeros(maxiter*nstep, 1); end

% Maximum position error of the quadrotor at goal postol = 0.05; % m % Maximum speed of the quadrotor at goal veltol = 0.10; % m/s

x = x0; % state flag = 0; %% ******* RUN SIMULATION ******* fprintf('Simulation Running....\n') for iter = 1:maxiter timeint = time:tstep:time+cstep; tic; % Iterate over each quad for qn = 1:nquad % Initialize quad plot if iter == 1 QP{qn} = QuadPlot(qn, x0{qn}, 0.1, 0.04, quadcolors(qn,:), maxiter, h3d); desiredstate = trajhandle(time, qn); QP{qn}.UpdateQuadPlot(x{qn}, [desiredstate.pos; desiredstate.vel], time); h_title = title(sprintf('iteration: %d, time: %4.2f', iter, time)); end

% Run simulation
    [tsave, xsave] = ode45(@(t,s) quadEOM(t, s, qn, controlhandle, trajhandle, params), timeint, x{qn});
    x{qn} = xsave(end, :)';
    % Save to traj
    xtraj{qn}((iter-1)*nstep+1:iter*nstep,:) = xsave(1:end-1,:);
    ttraj{qn}((iter-1)*nstep+1:iter*nstep)   = tsave(1:end-1);

    % Update quad plot
    desired_state = trajhandle(time + cstep, qn);
    if flag == 1
        desired_state.vel = [0; 0 ;0];
        flag = 0;
    end
    QP{qn}.UpdateQuadPlot(x{qn}, [desired_state.pos; desired_state.vel], time + cstep);
end

set(h_title, 'String', sprintf('iteration: %d, time: %4.2f', iter, time + cstep))
time = time + cstep; % Update simulation time
t = toc;

% Pause to make real-time
if (t < cstep)
    pause(cstep - t);
end

% Check termination criteria
terminate_cond = terminate_check(x, time, stop, pos_tol, vel_tol, time_tol);
if terminate_cond == 3
    flag = 1;
end
if terminate_cond ~= 0 && terminate_cond ~= 3
    break
end

end

fprintf('Simulation Finished....\n')

%% ******* POST PROCESSING ******* % Truncate xtraj and ttraj for qn = 1:nquad xtraj{qn} = xtraj{qn}(1:iternstep,:); ttraj{qn} = ttraj{qn}(1:iternstep); end

% Plot the saved position and velocity of each robot if vis for qn = 1:nquad % Truncate saved variables QP{qn}.TruncateHist(); % Plot position for each quad hpos{qn} = figure('Name', ['Quad ' num2str(qn) ' : position']); plotstate(hpos{qn}, QP{qn}.statehist(1:3,:), QP{qn}.timehist, 'pos', 'vic'); plotstate(hpos{qn}, QP{qn}.statedeshist(1:3,:), QP{qn}.timehist, 'pos', 'des'); % Plot velocity for each quad hvel{qn} = figure('Name', ['Quad ' num2str(qn) ' : velocity']); plotstate(hvel{qn}, QP{qn}.statehist(4:6,:), QP{qn}.timehist, 'vel', 'vic'); plotstate(hvel{qn}, QP{qn}.statedeshist(4:6,:), QP{qn}.timehist, 'vel', 'des'); end end

end ```

​