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

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

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

🍊个人信条：格物致知。

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

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

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

🔥 内容介绍

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

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

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

MPAPF算法的步骤如下：

1. 收集环境信息：首先，需要获取与船舶运动相关的环境信息，包括目标船舶的位置、速度、航向等。这些信息可以通过雷达、AIS（自动识别系统）等设备获取。

2. 预测船舶运动：利用模型预测控制技术，对目标船舶的未来运动进行预测。这可以通过建立动力学模型和环境模型来实现。动力学模型描述了船舶的运动规律，而环境模型描述了外部环境对船舶的影响。

3. 生成人工势场：根据预测的目标船舶运动，生成人工势场。人工势场是一种虚拟的力场，用于引导船舶的运动。在MPAPF算法中，人工势场由两部分组成：静态势场和动态势场。静态势场与环境中的障碍物有关，用于避免船舶与障碍物的碰撞；动态势场与目标船舶的运动有关，用于引导船舶与目标船舶保持安全距离。

4. 路径规划：根据生成的人工势场，进行路径规划。路径规划的目标是找到一条安全且符合COLREG规则的航线，以确保船舶能够安全通过复杂的遭遇场景。

5. 运动控制：根据规划的航线，进行运动控制。运动控制可以通过调整船舶的航向、速度等参数来实现。在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 = 6​candi_brch_bus = []; % candidate branch i added to bus jcasei=4d84_v2substation_node = 84;        n_bus = 84;combine3 = 0​n1 = 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_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_lossend​​fprintf('\n')ver_num_reconfig_dec

⛳️ 运行结果

🔗 参考文献

🎈 部分理论引用网络文献，若有侵权联系博主删除

🎁  关注我领取海量matlab电子书和数学建模资料

👇  私信完整代码、论文复现、期刊合作、论文辅导及科研仿真定制

1 各类智能优化算法改进及应用

生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化

2 机器学习和深度学习方面

卷积神经网络（CNN）、LSTM、支持向量机（SVM）、最小二乘支持向量机（LSSVM）、极限学习机（ELM）、核极限学习机（KELM）、BP、RBF、宽度学习、DBN、RF、RBF、DELM、XGBOOST、TCN实现风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断

2.图像处理方面

图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知

3 路径规划方面

旅行商问题（TSP）、车辆路径问题（VRP、MVRP、CVRP、VRPTW等）、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、车辆协同无人机路径规划、天线线性阵列分布优化、车间布局优化

4 无人机应用方面

无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配、无人机安全通信轨迹在线优化

5 无线传感器定位及布局方面

传感器部署优化、通信协议优化、路由优化、目标定位优化、Dv-Hop定位优化、Leach协议优化、WSN覆盖优化、组播优化、RSSI定位优化

6 信号处理方面

信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号、信号配时优化

7 电力系统方面

微电网优化、无功优化、配电网重构、储能配置

8 元胞自动机方面

交通流 人群疏散 病毒扩散 晶体生长

9 雷达方面

卡尔曼滤波跟踪、航迹关联、航迹融合