[path planning] based on firefly uav 3d path planning MATLAB source code

Over time, fireflies have evolved to control light emission in a variety of ways to produce different mating signals. They produce different signals by changing the following parameters:

• Glowing colors;
• Luminosity;
• Male flare and female flare – phase difference;
• Duration of each flash;
• Number of flashes per cycle;
• Flash time;
• A sequence of continuous luminous or flashing pulses.

Kaipa and Ghose (2017) used different examples to describe these flashing patterns, such as using the Lampyrus firefly, a common firefly in Europe, where only females have the ability to emit light. She writhes in the grass, sweeping light from one direction to the other to attract the attention of wandering male fireflies. For the lamorustenebrosus fireflies, both males and females are luminescent, but the female is wingless and, similar to Lampyrus, uses light to attract mates. In some species, females use different patterns, such as long flashes of light, that are not completely extinguished during the interval. When the males can sense the pattern from 10 feet away, they fly toward the female. These flashing patterns are species-specific. In the species Photinus, for example, males initiate mating by crawling on the ground, emitting a variety of lights and observing signals from nearby females. In Photinus Consanguineus, the male emits two flashes, the male pauses, then emits two more flashes, and he repeats the pattern. After a second flash from the male, the female responds within a second. In Photinus Castus, the males emit long flashes of light, which the females immediately respond to. Although p. Consanguineus and P. Castus are very similar in structure, they are considered different species based on their luminous patterns. Although they often fly together, they don’t interbreed, but they are considered different species based on their luminous patterns, but they often fly together.

One thing all of the species mentioned above have in common is that whoever is attracting the other, male or female, needs a constant line of sight so they can pick up the signal and respond. In areas of visual clutter, such as mangrove swamps in Southeast Asia, it’s not easy to get such uninterrupted sight. Thus, species found in these areas do not select individuals for mating. Instead, the fireflies stay in groups in trees or caves, so that the wandering fireflies can easily find mates. If there is no initial group, fireflies may form a nuclear cluster through mutual light attraction. There is competition in the formation of these groups, which leads to more than one larger group, and these larger groups further attract smaller nuclei nearby because they have a higher average light emission, meaning a higher likelihood of finding a mate.

For the behavior of fireflies and fireflies, Krishnanand and Ghose (2005) [1] proposed Glowworm Swarm Optimization (GSO), which has been applied in many applications. Originally, GSO was developed to provide solutions to numerical optimization problems rather than to determine global optimality, but thanks to GSO’s decentralized decision making and mobility protocols, it has made more contributions in areas such as robotics.

Initially inspired by fireflies, GSO randomly distributes a group or group of agents in a random search space, and the agents interact with each other through other behavioral mechanisms that do not exist in their natural counterparts. The basic work of the algorithm is based on the following three mechanisms:

1. Fitness Broadcast Fireflies have a pigment called luciferin that makes them glow. The content of luciferin in fireflies determines their fitness for location in the target space.
2. Positive trending fireflies are attracted to their brighter neighbors, so they start moving toward them. When there are multiple such neighbors, it uses a probabilistic mechanism to select one.
3. Adaptive neighborhood Each firefly uses an adaptive neighborhood to identify its neighbors, defined by a local decision domain with variable range RDI, bounded by a hard-limited perceptual range RS (0< RDI < RS). An appropriate heuristic can be used here to tune THE RDI. The movement of fireflies depends entirely on local information, and each firefly will choose a neighbor whose luciferin value is greater than its own and move toward it. These movements, based on available local information and selective neighbor interactions, enable firefly populations to aggregate into disjoint subgroups that move towards and meet on multiple optimal values of a given multi-peak function.

Although the algorithm is interpreted to find multiple optimal solutions for multimodal functions, it can be used to minimize the problem with simple modifications. Initially, GSO placed fireflies randomly in the search space so that they were well distributed. Each firefly starts out with zero luciferin. The unit period of the algorithm includes fluorescein update stage, movement stage and neighborhood update stage, as shown in Figure 1.

FIG. 1 GSO algorithm flow

7.2.2 code

function [xtraj, ttraj, terminate_cond] = test_trajectory(start, stop, map, path, vis)
% TEST_TRAJECTORY 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 load_map
% 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
    h_fig = figure('Name', 'Environment');
else
    h_fig = figure('Name', 'Environment', 'Visible', 'Off');
end
if nquad == 1
    plot_path(map, path{1});
else
%     for qn = 1:nquad
%          plot_path(map, path{qn});
%     end
    % you could modify your plot_path to handle cell input for multiple robots
end
h_3d = 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);
x_0 = map(1,4);
x_1 = map(2,4);
y_0 = map(1,5);
y_1 = map(2,5);
z_0 = map(1,6);
z_1 = 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,:) = [B_1';B_8'];
%         BlockCoordinates(i,:) = {B_1 B_2 B_3 B_4 B_5 B_6 B_7 B_8};
%         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
time_tol = 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
max_iter  = time_tol / cstep;      % max iteration
for qn = 1:nquad
    % Get start and stop position
    x0{qn}    = init_state(start{qn}, 0);
    xtraj{qn} = zeros(max_iter*nstep, length(x0{qn}));
    ttraj{qn} = zeros(max_iter*nstep, 1);
end

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

x = x0;        % state
flag = 0;
%% ************************* RUN SIMULATION *************************
fprintf('Simulation Running....\n')
for iter = 1:max_iter
    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,:), max_iter, h_3d);
            desired_state = trajhandle(time, qn);
            QP{qn}.UpdateQuadPlot(x{qn}, [desired_state.pos; desired_state.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:iter*nstep,:);
    ttraj{qn} = ttraj{qn}(1:iter*nstep);
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
        h_pos{qn} = figure('Name', ['Quad ' num2str(qn) ' : position']);
        plot_state(h_pos{qn}, QP{qn}.state_hist(1:3,:), QP{qn}.time_hist, 'pos', 'vic');
        plot_state(h_pos{qn}, QP{qn}.state_des_hist(1:3,:), QP{qn}.time_hist, 'pos', 'des');
        % Plot velocity for each quad
        h_vel{qn} = figure('Name', ['Quad ' num2str(qn) ' : velocity']);
        plot_state(h_vel{qn}, QP{qn}.state_hist(4:6,:), QP{qn}.time_hist, 'vel', 'vic');
        plot_state(h_vel{qn}, QP{qn}.state_des_hist(4:6,:), QP{qn}.time_hist, 'vel', 'des');
    end
end

end
Copy the code

Complete code added QQ1575304183

7.3 Mathematical proof of GSO algorithm

See text [1].

1. Krishnanand, K.N. and D. Ghose. Detection of multiple source locations using a glowworm metaphor with applications to collective robotics. in Proceedings 2005 IEEE Swarm Intelligence Symposium, 2005. SIS 2005. 2005

2. Previous review >>>>>>