matlab解决多车辆路径规划问题

Question

我现在有一段路径规划的matlab代码能够求解点到点的最短路径，需要根据实际问题进行修改。
问题描述如下：现有小车N辆，依次从同一起点出发，直至停满所有网格。先停进去的小车视为障碍物，使总的路径最短，并输出总的路径值
单车辆路径规划matlab代码如下，请问该如何修改呢：

clc
clear
close all

%%画地图
%行列定义
m=6;
n=6;
start_node = [1,1];
target_node = [6,6];
obs = [4,2; 4,3; 4,4;];

%画直线
for i = 1:m
    plot([0, n], [i,i], 'k');
    hold on
end

for j = 1: n
    plot([j, j], [0, m], 'k');
end

axis equal   %横纵坐标一致
xlim([0, n]);  %限制坐标轴范围
ylim([0, m]);

%绘制起止点和障碍物的颜色
fill([start_node(1)-1, start_node(1), start_node(1), start_node(1)-1],...  %四个顶点的x坐标
    [start_node(2)-1, start_node(2)-1, start_node(2), start_node(2)],'g');  %四个顶点的y坐标

fill([target_node(1)-1, target_node(1), target_node(1), target_node(1)-1],...
    [target_node(2)-1, target_node(2)-1, target_node(2), target_node(2)],'g');

for i = 1:size(obs,1) %返回矩阵的第1维尺寸
    temp = obs(i,:);  %矩阵的第i行
    fill([ temp(1)-1, temp(1), temp(1), temp(1)-1],...
    [ temp(2)-1, temp(2)-1, temp(2), temp(2)],'b');
end

%%预处理
%初始化closeList
closeList = start_node;
closeList_path = {start_node,start_node};
closeList_cost = 0;
child_nodes = child_nodes_cal(start_node, m, n, obs, closeList);

%初始化openList
openList = child_nodes;
%路径
for i =1:size(openList,1)
    openList_path{i,1} = openList(i,:);
    openList_path{i,2} = [start_node;openList(i,:)];
end

%代价
for i = 1:size(openList, 1)
    g = norm(start_node - openList(i,1:2)); %模，实际距离
    h = abs(target_node(1) - openList(i,1)) + abs(target_node(2) - openList(i,2)); %只考虑横向和纵向，不考虑斜的距离
    f = g + h; %移动代价=实际代价+估计代价
    openList_cost(i,:) = [g, h, f];
end

%%开始搜索
%从openList开始搜索移动代价最小的节点
[~, min_idx] = min(openList_cost(:,3)); %f值最小
parent_node = openList(min_idx,:);

%%进入循环
flag = 1;
while flag
    
    %找出父节点的忽略closeList的子节点
    child_nodes = child_nodes_cal(parent_node, m, n, obs, closeList);
    
    %判断这些子节点是否在openList中，若在，则比较更新；没在则追加到openList中
    for i = 1:size(child_nodes,1)
        child_node = child_nodes(i,:);
        [in_flag,openList_idx] = ismember(child_node, openList, 'rows');
        g = openList_cost(min_idx, 1) + norm(parent_node - child_node);
        h = abs(child_node(1) - target_node(1)) + abs(child_node(2) - target_node(2));
        f = g + h;
        
        if in_flag   %若在，比较更新g和f
            if g < openList_cost(openList_idx,1)
                openList_cost(openList_idx, 1) = g;
                openList_cost(openList_idx, 3) = f;
                openList_path{openList_idx, 2} = [openList_path{min_idx, 2};child_node];
            end
        else       %若不在，则追加到openList
            openList(end+1,:) = child_node;
            openList_cost(end+1, :) = [g, h, f];
            openList_path{end+1, 1} = child_node;
            openList_path{end, 2} = [openList_path{min_idx, 2};child_node];
        end
    end
    
    %从openList移除移动代价最小的节点到closeList
    closeList(end+1,:) = openList(min_idx,:);
    closeList_cost(end+1,1) = openList_cost(min_idx,3);
    closeList_path(end+1,:) = openList_path(min_idx,:);
    openList(min_idx,:) = [];
    openList_cost(min_idx,:) = [];
    openList_path(min_idx,:) = [];
    
    %重新搜索：从openList搜索移动代价最小的节点
    [~, min_idx] = min(openList_cost(:,3));
    parent_node = openList(min_idx,:);
    
    %判断是否到终点
    if parent_node == target_node
        closeList(end+1,:) = openList(min_idx,:);
        closeList_cost(end+1,1) = openList_cost(min_idx,1);
        closeList_path(end+1,:) = openList_path(min_idx,:);
        flag = 0;
    end
end

%%画路径
path_opt = closeList_path{end,2};
path_opt(:,1) = path_opt(:,1)-0.5;
path_opt(:,2) = path_opt(:,2)-0.5;
scatter(path_opt(:,1),path_opt(:,2),'k');
plot(path_opt(:,1),path_opt(:,2),'k');
        
    
   
    
                

Answer 1

你这个代码没有用的