【路径规划】基于模型预测人工势场MPAPF求解考虑复杂遭遇场景的 COLREG船舶运动规划附matlab代码

本文链接：https://blog.youkuaiyun.com/matlab_dingdang/article/details/134042353

✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。

🍎个人主页：Matlab科研工作室

🍊个人信条：格物致知。

更多Matlab完整代码及仿真定制内容点击👇

智能优化算法神经网络预测雷达通信无线传感器电力系统

信号处理图像处理路径规划元胞自动机无人机

🔥 内容介绍

路径规划是一项关键的技术，在许多领域都有广泛的应用，包括航空、航海、机器人等。在船舶运动规划中，考虑到复杂的遭遇场景，COLREG船舶运动规划算法是一种常用的方法。本文将介绍一种基于模型预测人工势场（MPAPF）的路径规划算法，用于解决COLREG船舶运动规划问题。

COLREG（国际航行规则）是指导船舶避碰行为的国际公约，旨在确保船舶在遭遇情况下能够安全地通过。COLREG规则的遵守对于保证船舶运输的安全至关重要。然而，在复杂的遭遇场景中，遵守COLREG规则并不总是容易的，因为船舶需要综合考虑多个因素，如目标船舶的运动、风、海流等。

为了解决COLREG船舶运动规划问题，研究者们提出了各种各样的算法。其中，基于模型预测人工势场（MPAPF）的方法是一种常见且有效的方法。该方法利用模型预测控制（MPC）技术，通过预测船舶运动，生成人工势场，以引导船舶的运动。MPAPF方法能够在考虑复杂的遭遇场景下，自动规划船舶的运动路径，以确保遵守COLREG规则并避免碰撞。

MPAPF算法的步骤如下：

收集环境信息：首先，需要获取与船舶运动相关的环境信息，包括目标船舶的位置、速度、航向等。这些信息可以通过雷达、AIS（自动识别系统）等设备获取。
预测船舶运动：利用模型预测控制技术，对目标船舶的未来运动进行预测。这可以通过建立动力学模型和环境模型来实现。动力学模型描述了船舶的运动规律，而环境模型描述了外部环境对船舶的影响。
生成人工势场：根据预测的目标船舶运动，生成人工势场。人工势场是一种虚拟的力场，用于引导船舶的运动。在MPAPF算法中，人工势场由两部分组成：静态势场和动态势场。静态势场与环境中的障碍物有关，用于避免船舶与障碍物的碰撞；动态势场与目标船舶的运动有关，用于引导船舶与目标船舶保持安全距离。
路径规划：根据生成的人工势场，进行路径规划。路径规划的目标是找到一条安全且符合COLREG规则的航线，以确保船舶能够安全通过复杂的遭遇场景。
运动控制：根据规划的航线，进行运动控制。运动控制可以通过调整船舶的航向、速度等参数来实现。在COLREG船舶运动规划中，运动控制需要考虑到COLREG规则的要求，以确保船舶的运动符合规则并避免碰撞。

通过以上步骤，基于模型预测人工势场的COLREG船舶运动规划算法能够在复杂的遭遇场景中，自动规划船舶的运动路径，以确保船舶的安全通行。该算法结合了模型预测控制技术和人工势场方法，能够有效地解决COLREG船舶运动规划问题。在实际应用中，可以根据具体情况对算法进行优化和改进，以提高路径规划的准确性和效率。

总之，路径规划是一项重要的技术，在复杂的遭遇场景下尤为关键。基于模型预测人工势场的COLREG船舶运动规划算法提供了一种有效的解决方案，能够确保船舶在遭遇情况下能够安全通过。随着技术的不断发展，路径规划算法将进一步完善和应用于更多领域，为人们的生活和工作带来更多便利和安全。

📣 部分代码

clear all, clc, close alladdpath('./code')ver_num_reconfig_dec = 2%% basic settingticfprintf('decrease_reconfig_84_tabu.m \n')warning('off')addpath(pathdef)mpopt = mpoption;mpopt.out.all = 0; % do not print anythingmpopt.verbose = 0;version_LODF = 0 % 1: use decrease_reconfig_algo_LODF.m                                  % 0: use decrease_reconfig_algo.mdistancePara = 6candi_brch_bus = []; % candidate branch i added to bus jcasei=4d84_v2substation_node = 84;        n_bus = 84;combine3 = 0n1 = 3n2 = 2n1_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 = 0show_biograph = 0from_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 \n\n', 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 algorithmelse    [Branch] = decrease_reconfig_algo(Bus, Branch, brch_idx_in_loop, ff0, tt0, ...        substation_node, n_bus, loss0); %%%  core algorithmendt2 = toc;time_consumption.core = t2 - t1% output of core algorithmfrom_to = show_biograph_not_sorted(Branch(:, [1 2]), substation_node, ...    show_biograph1); %%% show figure, take timempc = 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 \n\n', ...    loss0_dec);yij_dec = generate_yij_from_Branch(Branch, Branch0);Branch_loss_record = [];% record Branch and lossBranch_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_openfor iter = 1:length(branch_idx_focused) % idx_force_open)    fprintf('iter=%d/%d\n', 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\n', ...        min_loss_o1c1);        min_loss_combine_two_o1c1 = 1e9;    fprintf('case84_tabu: loss obtained after combine two ''one open and one close'': \n')    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 \n', temp);    end        fprintf('case84_tabu: minimum loss obtained after combine two ''one open and one close'': %.5f \n', ...        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\n', ...            min_loss_o2c2);        min_loss_combine_two_o2c2 = 1e9;        fprintf('case84_tabu: loss obtained after combine two ''two open and two close'': \n')        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 \n', temp);        end        fprintf('case84_tabu: minimum loss obtained after combine two ''two open and two close'': %.5f \n', ...            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');endfile_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: \n') % 0.28343  zjp 2018-1-18fprintf('%.5f \n', loss_tabu)fprintf('----- min: %.5f -----\n', 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 ;    endend  min_loss_o1c1 = min_lossif 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_lossendfprintf('\n')ver_num_reconfig_dec