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

1 简介

—Recent advances in technology have increased thecapability of mobile platforms while decreasing the cost. It hasbecome more feasible to deploy a team of agents to cooperativelyaccomplish an objective. While multi-agent systems provideadvantages, including lower cost and robustness to failure, thereis a need for additional study of principles for the design andtest of these decentralized systems. The main contribution of thispaper is a novel estimation and path planning algorithm thatcan be used for improved estimation of uncertain environments.The estimation algorithm utilizes Bayesian fusion, measurementsharing on a graph, and belief consensus. One new componentof this approach is the reward-based path planning algorithmthat incentivizes agents to collect the best local informationas well as improve coverage of the environment. When agentsplan paths to collect more valuable information, the estimationerror is reduced. This approach is studied for the applicationof estimating the state of a forest fifire but can be applied inmany domains. Simulations were performed to demonstrate theeffectiveness of the algorithm compared to other approaches

2 部分代码

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
   

3 仿真结果