基于割草机搜索策略(LM)实现森林火灾无人机的路径规划

 

import java.util.LinkedList;
tic
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ENV_SIZE = 20;                      % Size of environment

formation = 1;                      % 0 -> strong, 1 -> ring, 2 -> random
rand_comm = 3;                      % Num of random UAVs to connect with
depth = 3;                          % Depth of our tree
branch_factor = 4;                  % Number of childs per node (N,S,E,W)
num_of_agents = 5;                  % Number of UAVs simulated
fire_rate = 0.007;                  % Fire spread rate (0 <= rate <= 1)
burn_out_rate = 0;                  % Fire burnout rate (0 <= rate <= 1)
duration = 1000;                    % Duration of simulation
start_display = 0;               % Display maps after "n" timesteps
fusion_interval = 10;               % Fuse est maps after "n" timesteps
cost_power = 2;                     % Used to tune cost of proximity of UAV 
                                    % to others
UAV_range = 5;                      % Range of UAV sight (inclusive)
gamma = 3/10;                       % 0 < gamma < 1/max(num_connected)

alpha = 0.7;                        % Controls weight of reward function

false_pos = 0.10;                   % UAV Measurement inaccuracy rates
false_neg = 0.10;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% True Positive and False Positive.
true_pos = 1 - false_pos;
true_neg = 1 - false_neg;

% Laplacian matrix for communication between agents. (Ring formation)
L = get_laplacian(num_of_agents,formation,rand_comm);

% Estimated matrix initialized to 50% probability of fire in each location.
% (Centralized Map)
est_state = 0.5*ones(ENV_SIZE);

% Estimated matrix for each UAV within the environment. 
est_state_UAVs = 0.5*ones(ENV_SIZE,ENV_SIZE,num_of_agents);

% Initialized ENV_SIZExENV_SIZE matrix as our true state environment 
% (binary representation).
true_state = zeros(ENV_SIZE);

% Used to keep track of coordinates of "burned out" squares.
fire_out_x = LinkedList();
fire_out_y = LinkedList();

% Generate and place random location for the initial fire.
x = randi(ENV_SIZE);    % vertical
y = randi(ENV_SIZE);    % horizontal
true_state(x,y) = 1;
fire_out_x.add(x);
fire_out_y.add(y);

% Coordinates of randomly places UAVs. 
robX = randperm(ENV_SIZE,num_of_agents);
robY = randperm(ENV_SIZE,num_of_agents);

% Pre-allocate to calculate error in our model.
total_err = zeros(duration,1);

total_err_UAVs = zeros(duration,1);
UAV_error = zeros(num_of_agents,1);

% Used to calculate "best path". Calculated beforehand to optimize 
% runtime.
paths = zeros(depth,2,branch_factor^depth);
second_value = get_directions(paths,branch_factor^depth,1,0);

% Keep spreading fire and running algorithm until time duration is up.
for index = 1:duration
    % Spread the fire throughout the environment based on the fire_rate.
    [true_state,fire_out_x,fire_out_y] = spread_fire(true_state,ENV_SIZE,fire_out_x,fire_out_y,fire_rate);
    
    % The fire spread is updated. Now perform step 2 and update
    % est_state by using prediction model. (Centralized Map)
    est_state = update_est_state(est_state,ENV_SIZE,fire_rate);
    
    % Update the estimated mapping for each UAV.
    for i = 1:size(est_state_UAVs,3)
        est_state_UAVs(:,:,i) = update_est_state(est_state_UAVs(:,:,i),ENV_SIZE,fire_rate);
    end

    for loc = 1:size(robX,2)
        % Sensor "error" chance of having incorrect information (STEP 3)
        M = true_state(robX(loc),robY(loc));
        if M == 1
            % The true state is 1 and we have a false negative
            if rand() < false_neg
                M = 0;
            end
        else
            % The true state is 0 and we have a false positive
            if rand() < false_pos
                M = 1;
            end
        end
        
        % Now that we updated est_state, we want to update based on our
        % "sensor readings" or measurements. (Step 3 & 4) (Centralized Map)
        est_state = update_est_state_2(est_state,M,robX(loc),robY(loc), ...
                                       false_pos,false_neg,true_pos,true_neg);
        
        % Update estimated state for each UAV.
        est_state_UAVs(:,:,loc) = update_est_state_2(est_state_UAVs(:,:,loc),M, ...
                                              robX(loc),robY(loc),false_pos,false_neg,true_pos,true_neg);  

        % Update connected UAV's mapping based on current measurement.
        for j = 1:size(L(:,loc),1)
            if L(j,loc) == -1
                % j is UAV connected to loc
                est_state_UAVs(:,:,j) = update_est_state_2(est_state_UAVs(:,:,j),M, ...
                                        robX(loc),robY(loc),false_pos,false_neg,true_pos,true_neg);
            end
        end
    end
    
    % Only display maps after reaching timestep "start_display". 
    if (index >= start_display)
        display(num_of_agents,ENV_SIZE,true_state,est_state,est_state_UAVs,robY,robX);
        pause(0.02);
    end
    
    % Perform estimated mapping fusion for connected agents after specified
    % simulation iterations
    if (~mod(index,fusion_interval))
        for agent = 1:num_of_agents
            est_state_UAVs(:,:,agent) = est_map_fusion(L(agent,:),est_state_UAVs,...
                                        est_state_UAVs(:,:,agent),gamma);
        end
    end
    

完整代码添加QQ1575304183