?About the author: A Matlab simulation developer who loves scientific research. He cultivates his mind and improves his technology simultaneously. For cooperation on MATLAB projects, please send a private message.
Personal homepage: Matlab Research Studio
Personal credo: Investigate things to gain knowledge.
For more complete Matlab code and simulation customization content, click
Intelligent optimization algorithm Neural network prediction Radar communication Wireless sensor Power system
Signal processing Image processing Path planning Cellular automaton Drone
Content introduction
Path planning is a key technology with wide applications in many fields, including aviation, navigation, robotics, etc. In ship motion planning, the COLREG ship motion planning algorithm is a commonly used method considering complex encounter scenarios. This article will introduce a path planning algorithm based on Model Predictive Artificial Potential Field (MPAPF) to solve the COLREG ship motion planning problem.
COLREG (International Code of Navigation) is an international convention that guides ships’ collision avoidance behavior and aims to ensure that ships can pass safely in the event of an encounter. Compliance with the COLREG Code is essential to ensure the safety of ship transportation. However, in complex encounter scenarios, it is not always easy to comply with COLREG rules because ships need to comprehensively consider multiple factors, such as the movement of the target ship, wind, currents, etc.
In order to solve the COLREG ship motion planning problem, researchers have proposed various algorithms. Among them, the method based on model prediction artificial potential field (MPAPF) is a common and effective method. This method uses model predictive control (MPC) technology to generate artificial potential fields to guide the movement of the ship by predicting the ship’s movement. The MPAPF method can automatically plan the ship’s movement path taking into account complex encounter scenarios to ensure compliance with COLREG rules and avoid collisions.
The steps of the MPAPF algorithm are as follows:
- Collect environmental information: First, it is necessary to obtain environmental information related to ship movement, including the position, speed, heading, etc. of the target ship. This information can be obtained through radar, AIS (automatic identification system) and other equipment.
- Predict ship movement: Use model predictive control technology to predict the future movement of the target ship. This can be achieved by building dynamic models and environmental models. The dynamic model describes the movement of the ship, while the environmental model describes the impact of the external environment on the ship.
- Generate artificial potential field: Generate artificial potential field based on the predicted movement of the target ship. The artificial potential field is a virtual force field used to guide the movement of the ship. In the MPAPF algorithm, the artificial potential field consists of two parts: the static potential field and the dynamic potential field. The static potential field is related to obstacles in the environment and is used to avoid collisions between ships and obstacles; the dynamic potential field is related to the movement of the target ship and is used to guide the ship to maintain a safe distance from the target ship.
- Path planning: Carry out path planning based on the generated artificial potential field. The goal of path planning is to find a safe route that complies with COLREG rules to ensure that the ship can safely pass through complex encounter scenarios.
- Motion control: Carry out motion control according to the planned route. Motion control can be achieved by adjusting parameters such as the ship’s course and speed. In COLREG ship motion planning, motion control needs to take into account the requirements of COLREG rules to ensure that the ship’s motion complies with the rules and avoids collisions.
Through the above steps, the COLREG ship motion planning algorithm based on model prediction of artificial potential fields can automatically plan the ship’s motion path in complex encounter scenarios to ensure the safe passage of ships. This algorithm combines model predictive control technology and artificial potential field methods to effectively solve the COLREG ship motion planning problem. In practical applications, the algorithm can be optimized and improved according to specific situations to improve the accuracy and efficiency of path planning.
In short, path planning is an important technology, especially critical in complex encounter scenarios. The COLREG ship motion planning algorithm based on model prediction of artificial potential fields provides an effective solution to ensure that ships can pass safely in encounter situations. With the continuous development of technology, path planning algorithms will be further improved and applied in more fields, bringing more convenience and safety to people’s lives and work.
Part of the code
clear all, clc, close all
addpath('./code')
ver_num_reconfig_dec = 2
%%basic setting
tic
fprintf('decrease_reconfig_84_tabu.m \\
')
warning('off')
addpath(pathdef)
mpopt = mpoption;
mpopt.out.all = 0; % do not print anything
mpopt.verbose = 0;
version_LODF = 0 % 1: use decrease_reconfig_algo_LODF.m
% 0: use decrease_reconfig_algo.m
distancePara = 6
candi_brch_bus = []; % candidate branch i added to bus j
casei=4
d84_v2
substation_node = 84; n_bus = 84;
combine3 = 0
n1 = 3
n2 = 2
n1_down_substation = n1 + 1; n2_up_ending = n2;
Branch0 = Branch;
brch_idx_in_loop0 = unique(brch_idx_in_loop(:));
%% original network's power flow (not radial)
show_biograph1 = 0
show_biograph = 0
from_to = show_biograph_not_sorted(Branch, substation_node, show_biograph1);
mpc = generate_mpc(Bus, Branch, n_bus);
res_orig = runpf(mpc, mpopt);
losses = get_losses(res_orig.baseMVA, res_orig.bus, res_orig.branch);
loss0 = sum(real(losses));
fprintf('case84_tabu: original loop network''s loss is %.5f \\
\\
', loss0)
% for each branch in a loop,
% if open that branch does not cause isolation, check the two ending buses
% of that branch for connectivity, realized by shortestpath or conncomp
% calculate the lowest loss increase, print out the sorted loss increase
% open the branch with lowest loss increase
% stop criterion: number of buses - number of branches = 1
%% -------------------------- Core algorithm -------------------------- --%%
ff0 = Branch(:, 1); ff = ff0;
tt0 = Branch(:, 2); tt = tt0;
t1 = toc;
if version_LODF
[Branch] = decrease_reconfig_algo_LODF(Bus, Branch, brch_idx_in_loop, ...
ff0, tt0, substation_node, n_bus, loss0, distancePara); %%% core algorithm
else
[Branch] = decrease_reconfig_algo(Bus, Branch, brch_idx_in_loop, ff0, tt0, ...
substation_node, n_bus, loss0); %%% core algorithm
end
t2 = toc;
time_consumption.core = t2 - t1
% output of core algorithm
from_to = show_biograph_not_sorted(Branch(:, [1 2]), substation_node, ...
show_biograph1); %%% show figure, take time
mpc = generate_mpc(Bus, Branch, n_bus);
t1 = toc;
res_pf_dec = runpf(mpc, mpopt);
t2 = toc;
time_consumption.runpf = t2-t1;
losses = get_losses(res_pf_dec.baseMVA, res_pf_dec.bus, res_pf_dec.branch);
loss0_dec = sum(real(losses)) % loss =
fprintf('case84_tabu: radial network obtained by my core algorithm''s loss is %.5f \\
\\
', ...
loss0_dec);
yij_dec = generate_yij_from_Branch(Branch, Branch0);
Branch_loss_record = [];
% record branch and loss
Branch_loss_record.core.Branch = Branch;
Branch_loss_record.core.loss = loss0_dec;
%% prepare force open branches for tabu: branch_idx_focused
% nodes_focused = []
% [branch_idx_focused] = get_branch_idx_focused_for_tabu_v2( ...
% from_to, Branch0, Branch, substation_node, brch_idx_in_loop0, n_bus, ...
% n1_down_substation, n2_up_ending); % to answer reviewer 5-5's question
[branch_idx_focused] = get_branch_idx_focused_for_tabu( ...
from_to, Branch0, Branch, substation_node, brch_idx_in_loop0, n_bus, ...
n1_down_substation, n2_up_ending);
%% -------------------------- Tabu algorithm -------------------------- --%%
% run the core program for each upstream branch connected to the idx_force_open
for iter = 1:length(branch_idx_focused) % idx_force_open)
fprintf('iter=%d/%d\\
', iter, length(branch_idx_focused)); % idx_force_open));
Branch = Branch0;
Branch(branch_idx_focused(iter), :) = [];
% % Branch(idx_force_open(iter), :) = [];
ff0 = Branch(:, 1); ff = ff0;
tt0 = Branch(:, 2); tt = tt0;
brch_idx_in_loop = brch_idx_in_loop0;
idx_tmp = find(brch_idx_in_loop == branch_idx_focused(iter));
% % idx_tmp = find(brch_idx_in_loop == idx_force_open(iter));
if isempty(idx_tmp)
else
brch_idx_in_loop(idx_tmp) = [];
brch_idx_in_loop(idx_tmp:end) = brch_idx_in_loop(idx_tmp:end)-1;
end
t1 = toc;
%%------------------- core algorithm in Tabu loop--------------------%%
if version_LODF
[Branch] = decrease_reconfig_algo_LODF(Bus, Branch, brch_idx_in_loop, ...
ff0, tt0, substation_node, n_bus, loss0, distancePara); %%% core algorithm
else
[Branch] = decrease_reconfig_algo(Bus, Branch, brch_idx_in_loop, ff0, tt0, ...
substation_node, n_bus, loss0); %%% core algorithm
end
t2 = toc;
time_consumption.tabu(iter) = t2-t1;
from_to = show_biograph_not_sorted(Branch(:, [1 2]), substation_node, ...
show_biograph); %%% show figure, take time
mpc = generate_mpc(Bus, Branch, n_bus);
t1 = toc;
res_pf = runpf(mpc, mpopt);
t2 = toc;
time_consumption.runpf(iter) = t2-t1;
losses = get_losses(res_pf.baseMVA, res_pf.bus, res_pf.branch);
lossi = sum(real(losses)) % loss = 0.2960
loss_tabu(iter,1) = lossi;
yij_dec = generate_yij_from_Branch(Branch, Branch0);
% record branch and loss
Branch_loss_record.tabu(iter,1).Branch = Branch;
Branch_loss_record.tabu(iter,1).loss = lossi;
[PQ, PV, REF, NONE, BUS_I, BUS_TYPE, PD, QD, GS, BS, BUS_AREA, VM, ...
VA, BASE_KV, ZONE, VMAX, VMIN, LAM_P, LAM_Q, MU_VMAX, MU_VMIN] = idx_bus;
% Vm = res_pf.bus(:, VM)';
% Va = res_pf.bus(:, VA)';
% ending_bus = find_ending_node(Branch, substation_node);
% [ending_bus'; Vm(ending_bus)];
% % ending_bus = [6 14 72 22 24 83 41 42 32 33 37 55 63 8 9 10 62 71 13 82]';
% % [ending_bus'; Vm(ending_bus)]
%%---------------------one open and one close---------------------% %
% prepare nodes_focused for one_open_one_close
t1 = toc;
[nodes_focused] = get_nodes_focused_o1c1( ...
from_to, Branch, Branch0, substation_node, brch_idx_in_loop, ...
n1_down_substation, n2_up_ending);
loss_before_switch0 = lossi;
if combine3
[record_o1c1_loss_dec, loss_after_switch_combine_two_o1c1, Branch_loss] = ...
one_open_one_close_combine3(nodes_focused, Bus, Branch0, Branch, from_to, ...
substation_node, n_bus, loss_before_switch0);
else
[record_o1c1_loss_dec, loss_after_switch_combine_two_o1c1, Branch_loss] = ...
one_open_one_close(nodes_focused, Bus, Branch0, Branch, from_to, ...
substation_node, n_bus, loss_before_switch0);
end
t2 = toc;
time_consumption.tabu_o1c1(iter) = t2-t1;
% record branch and loss
Branch_loss_record.tabu_o1c1_dec{iter}.Branch = Branch_loss.Branch_o1c1_dec;
% Branch_loss_record.tabu_o1c1_dec(iter,1).Branch = Branch_loss.Branch_o1c1_dec;
Branch_loss_record.tabu_o1c1_dec{iter}.loss = Branch_loss.loss_o1c1_dec;
Branch_loss_record.tabu_combine_2_o1c1_dec{iter}.Branch = ...
Branch_loss.Branch_after_switch_combine_two_o1c1;
Branch_loss_record.tabu_combine_2_o1c1_dec{iter}.loss = ...
Branch_loss.loss_after_switch_combine_two_o1c1;
min_loss_o1c1 = min(record_o1c1_loss_dec(:,1));
fprintf('case84_tabu: minimum loss obtained after ''one open and one close'': %.5f\\
', ...
min_loss_o1c1);
min_loss_combine_two_o1c1 = 1e9;
fprintf('case84_tabu: loss obtained after combine two ''one open and one close'': \\
')
for i = 1:length(loss_after_switch_combine_two_o1c1)
temp = min(loss_after_switch_combine_two_o1c1{i});
if temp<min_loss_combine_two_o1c1
min_loss_combine_two_o1c1 = temp;
end
fprintf(' %.5f \\
', temp);
end
fprintf('case84_tabu: minimum loss obtained after combine two ''one open and one close'': %.5f \\
', ...
min_loss_combine_two_o1c1)
%%---------------------two open and two close---------------------% %
flag_2o2c = 0
if flag_2o2c == 1
t1 = toc;
loss_before_switch0 = lossi;
[record_o2c2_loss_dec, loss_after_switch_combine_two_o2c2] = ...
two_open_two_close(nodes_focused, Bus, Branch0, Branch, from_to, ...
substation_node, n_bus, loss_before_switch0);
t2 = toc;
time_consumption.tabu_o2c2(iter) = t2-t1;
min_loss_o2c2 = min(record_o2c2_loss_dec(:,1));
fprintf('case84_tabu: minimum loss obtained after ''two open and two close'': %.5f\\
', ...
min_loss_o2c2);
min_loss_combine_two_o2c2 = 1e9;
fprintf('case84_tabu: loss obtained after combine two ''two open and two close'': \\
')
for i = 1:length(loss_after_switch_combine_two_o2c2)
temp = min(loss_after_switch_combine_two_o2c2{i});
if temp<min_loss_combine_two_o2c2
min_loss_combine_two_o2c2 = temp;
end
fprintf(' %.5f \\
', temp);
end
fprintf('case84_tabu: minimum loss obtained after combine two ''two open and two close'': %.5f \\
', ...
min_loss_combine_two_o2c2)
res_save{iter}.min_loss_o2c2 = min_loss_o2c2;
res_save{iter}.min_loss_combine_two_o2c2 = min_loss_combine_two_o2c2;
end
res_save{iter}.yij_dec = yij_dec;
res_save{iter}.Branch = Branch;
res_save{iter}.lossi = lossi;
res_save{iter}.record_o1c1_loss_dec = record_o1c1_loss_dec;
res_save{iter}.min_loss_o1c1 = min_loss_o1c1;
res_save{iter}.min_loss_combine_two_o1c1 = min_loss_combine_two_o1c1;
% save(file_name, 'yij_dec', 'Branch', 'lossi');
file_name = ['id', num2str(ver_num_reconfig_dec), ...
'_case84_yij_Branch', '.mat'];
save(file_name, 'res_save', 'branch_idx_focused', 'Branch_loss_record', ...
'time_consumption');
end
file_name = ['id', num2str(ver_num_reconfig_dec), ...
'_case84_yij_Branch', '.mat'];
save(file_name, 'res_save', 'branch_idx_focused', 'Branch_loss_record', ...
'time_consumption');
% find_all_losses(Branch_loss_record);
fprintf('case84_tabu: losses obtained after applying tabu strategy: \\
') % 0.28343 zjp 2018-1-18
fprintf('%.5f \\
', loss_tabu)
fprintf('----- min: %.5f -----\\
', min(loss_tabu))
min_loss = 1e9;
for i = 1:length(res_save)
if min_loss>res_save{i}.min_loss_o1c1
min_loss = res_save{i}.min_loss_o1c1;
end
if min_loss>res_save{i}.min_loss_combine_two_o1c1
min_loss = res_save{i}.min_loss_combine_two_o1c1;
end
end
min_loss_o1c1 = min_loss
if flag_2o2c == 1
min_loss = 1e9;
for i = 1:length(res_save)
if min_loss>res_save{i}.min_loss_o2c2
min_loss = res_save{i}.min_loss_o2c2;
end
if min_loss>res_save{i}.min_loss_combine_two_o2c2
min_loss = res_save{i}.min_loss_combine_two_o2c2;
end
end
min_loss_o2c2 = min_loss
end
fprintf('\\
')
ver_num_reconfig_dec
Running results
edit