写在前面

今年参加了Mathorcup数学建模挑战杯比赛，参加这个比赛也是我和我的两个小伙伴在聊天时突然想要冲一冲。我们找到了很优秀也很尽责的指导老师，教导了我们很多知识，无论是老师的指导，在解题过程中的思考还是查阅参考文献，都给我带来了非常多的建模与算法知识。今天像写一些我在解决2022年MathorcupC题的思路，仅个人思路，尚不成熟也不完美，不一定是“全局最优解”。

在最后我会贴上我解题的代码，我是找到网上的代码，并根据自己的需求做出相应的改变，如果遇到我一样的问题的话，可以查看我的代码，如果只需解决单目标路径最优问题，我会在最后面附上一个详解传统A*算法代码的博客。再附上一篇详解A*原理的博客，这两篇博客在比赛期间帮助了我很多，强烈推荐看看。

题目

本次Mmathorcup挑战杯C题的第三问是一个解决自动泊车的最优路径问题，具体问题如下：

问题分析

问题的核心在于，车辆如何搜索到最优的最佳停车位，代价最小的停车位，路程最短的停车位，时间最短的停车位.....随便怎么解释最优这个名词，为了方便理解A*算法，以下的最优停车位我们统一指“代价最小”的停车位，并我们人为设定：“路程最短=代价最小”。

由此，我们可以在网上搜到各种各样的路径搜索算法，包括BFS，DFS，Dijkstra算法，贪婪最佳算法等等，我们在众多算法中挑选了A*算法来解决本问题。

为什么挑选A*呢？因为A*从原理上比较容易理解，其次，A*是具有启发性的算法，能够避免无效，代价大的路径的搜索，这样我们可以大大缩短搜索的时间。

本问题中的停车场其实已经帮我们简化了很多问题，尤其是设计地图以及搜索时的判断。其实本质上我们并不是寻找不同路径上的最优决策。

而是在一条路径下，在多个目标中，寻找到一个离我当前位置代价最小的一个目标。为什么要强调这一点呢，是因为传统A*算法解决的是“寻找在多条路径下，代价最小的一条路径到达一个目的地”，其起始点只有1个，终点只有1个，路径有多条。

而我们的问题中，起始点有1个，但可行终点有多个，路径只有1条，这就导致传统A*算法不能很好匹配本问题，所以我们就需要对它进行修改。

传统A*算法

A*算法原理

A*算法的原理其实很简单：

1、 制作地图，将地图信息存放在N×N（不一定是方阵）的矩阵中

我们在找代价最小路径时，必须要在规定的场景中查找，若场景中没有任何障碍物，那么问题就会退化成一个简单的两点间线段长度的问题了。地图是A*算法的基础。地图中应该包含以下信息：

起始位置：我们在Matlab中用元胞数组，以'S'代表我们在地图中的起始位置。
目的节点：同样的，我们也用元胞数组，以'G'代表我们在地图中的终点位置，在field矩阵中，我们用0来表示目的节点。
可行路径：在field矩阵中，我们用1来表示可行路径。
障碍物：在field矩阵中，我们用Inf（正无穷）来表示障碍物。

这样，我们就可以构建出一个N×N的矩阵，这个矩阵中，包含了0，1，Inf，可以用来计算轨迹以及判断障碍物。

2、 A*的核心：计算节点代价

首先给出A*算法中的核心公式：

$F(n)=G(n)+H(n)$

这个公式表示了当前节点到目的节点估计所需要的代价，其中：

G表示从起始节点到当前节点所需要花费的代价，这个代价是实际的，可以是上下左右移动的叠加，当然可以可以考虑斜方向移动。

H表示从当前节点到目的节点的预估代价，这个代价是预计的，不考虑障碍物碰撞，可以理解为两点之间最短距离，只可以上下左右移动。

F表示预估从起始点到终点的最短距离。这个F就是我们判断该往那个方向行走的重中之重。

3、拓展节点（判断哪些节点是下一个可行的节点，哪些节点不必再考虑了）

在这里我们引入几个存放信息的向量：

open向量：这个向量里存放着所有被考虑寻找最小代价的节点。

close向量：记录不会再被考虑的节点（已经被探索过的节点）。

point向量：记录地图中的障碍物，即Inf。

下面是遍历寻找下一节点的循环过程：

1. 从起始点S开始，将S放入open向量中。

2.并将S点周围可行的点也放入open向量中。

3.将S从open向量中删除，放入close向量中，”S点周围的点已经被检查了“

4.计算在open向量里的点的F值（从起始点，经过该点，然后到目的地的预估代价）。H值的计算为：将前一最小节点的花费cost保留下来，再加上走这一步花费就是H了。

5.将F值最低的节点设为a。把F、H等值保存下来。

6.将a周围可行的点放入open向量中。（障碍物，已经被探索放在close的点就是不可移动的点，无需再探索了）

7.将a当作S，重复第2个步骤。

8.当open向量中出现了G（目标节点），说明路径已经找到。退出循环。

9.当open向量为空，说明把能找的都找了，还没有出现G，没有路径能够到达G（目标节点）。

A*算法的思路就是这样啦，下面写我在这题中的解题过程。

A*算法的应用与优化

在上面的A*算法原理中，我们提到了G点表示目的节点，而在本题中，G点不止一个，（停车场中也许有一个也许有多个空闲停车位，有些极端时候说不定没有停车位。）

所以，我们在在构建地图时，必须将多个G点坐标都记录进地图，也就是说，field矩阵存在多个”1“值。

在后面的过程中，算法中的核心部分，计算F（预计从起始节点到目的节点的代价）时，我们需要计算open向量中每一个点到所有目的节点的预计代价，并从中选出最小的一个。路径代价与当前节点都记录下来。

当open向量中碰到了第一个G节点，就退出，返回路线以及代价。

如果open向量为空，则没有一条路径能到达目标节点。

Matlab代码：

下面的Matlab代码只要贴到Matlab中就可以直接运行得出结果，（直接运行AStar函数或者创建参数调用AStar函数都可以，若调用AStar函数，则将AStar函数中默认参数删除）

我做了5个函数：其实大部分函数都与网络上的相似，只做了部分改动，以满足题目的需求，整理起来比较麻烦，可能存在纰漏或者注释不清晰的地方，需要帮助的话可以私信。

AStar函数，A*算法的核心函数，这其中调用了其它的函数，本身具有判断当前节点的最优性的功能：

function [goalposind,flag] = AStar( start,emptycarports )
%下面两行代码可以不需要，因为我已经将AStar封装成函数，你可以在其它模块中调用该函数，并把你需要的空闲车位编号以及其实位置输入到AStar函数中即可
emptycarports=[2,5,9,13,45,47,49,52,53,54,64,67,78,81,82];%给定空闲车位编号
start=[2,100];%给定起始位置坐标goalposind = inputcarsports(emptycarports);
startposind= sub2ind([6,124],start(1),start(2));
%方格以及障碍物的创建
[field, startposind, goalposind, costchart, fieldpointers] =initializeField(goalposind,startposind); %生成包含障碍物，起始点，终止点等信息的矩阵
% 这个函数用来生成环境，障碍物，起点，终点axishandle = createFigure(field,costchart,startposind,goalposind);   % 路径规划中用到的一些矩阵的初始化
%setOpen存放待选的子节点的矩阵，初始化为索引值
setOpen = [startposind];
%setOpenCosts存放拓展出来的最优值距离起始点的代价，初始化为0
setOpenCosts = [0];
%setOpenHeuristics中存放待选的子节点距离终止点的代价
setOpenHeuristics = [Inf];
setClosed = []; setClosedCosts = [];
%movementdirections存放四个移动方向的代号
movementdirections = {'R','L','D','U'};  %移动方向%%% 这个while循环是本程序的核心，利用循环进行迭代来寻找终止点
while ~max(ismember(setOpen,goalposind)) && ~isempty(setOpen)[temp, ii] = min(setOpenCosts + setOpenHeuristics);     %寻找拓展出来的最小值 for i=1:length(goalposind)[costs,heuristics,posinds] = findFValue(setOpen(ii),setOpenCosts(ii), field,goalposind(i),'euclidean');endsetClosed = [setClosed; setOpen(ii)];     % 将找出来的拓展出来的点中代价最小的那个点串到矩阵setClosed 中 setClosedCosts = [setClosedCosts; setOpenCosts(ii)];    % 将拓展出来的点中代价最小的那个点的代价串到矩阵setClosedCosts 中% 从setOpen中删除刚才放到矩阵setClosed中的那个点%如果这个点位于矩阵的内部if (ii > 1 && ii < length(setOpen))setOpen = [setOpen(1:ii-1); setOpen(ii+1:end)];setOpenCosts = [setOpenCosts(1:ii-1); setOpenCosts(ii+1:end)];setOpenHeuristics = [setOpenHeuristics(1:ii-1); setOpenHeuristics(ii+1:end)];%如果这个点位于矩阵第一行elseif (ii == 1)setOpen = setOpen(2:end);setOpenCosts = setOpenCosts(2:end);setOpenHeuristics = setOpenHeuristics(2:end);%如果这个点位于矩阵的最后一行elsesetOpen = setOpen(1:end-1);setOpenCosts = setOpenCosts(1:end-1);setOpenHeuristics = setOpenHeuristics(1:end-1);end%%  % 把拓展出来的点中符合要求的点放到setOpen 矩阵中，作为待选点for jj=1:length(posinds)if ~isinf(costs(jj))   % 判断该点（方格）处没有障碍物% 判断一下该点是否 已经存在于setOpen 矩阵或者setClosed 矩阵中% 如果我们要处理的拓展点既不在setOpen 矩阵，也不在setClosed 矩阵中if ~max([setClosed; setOpen] == posinds(jj))fieldpointers(posinds(jj)) = movementdirections(jj);costchart(posinds(jj)) = costs(jj);setOpen = [setOpen; posinds(jj)];setOpenCosts = [setOpenCosts; costs(jj)];setOpenHeuristics = [setOpenHeuristics; heuristics(jj)];% 如果我们要处理的拓展点已经在setOpen 矩阵中elseif max(setOpen == posinds(jj))I = find(setOpen == posinds(jj));% 如果通过目前的方法找到的这个点，比之前的方法好（代价小）就更新这个点if setOpenCosts(I) > costs(jj)costchart(setOpen(I)) = costs(jj);setOpenCosts(I) = costs(jj);setOpenHeuristics(I) = heuristics(jj);fieldpointers(setOpen(I)) = movementdirections(jj);end% 如果我们要处理的拓展点已经在setClosed 矩阵中elseI = find(setClosed == posinds(jj));% 如果通过目前的方法找到的这个点，比之前的方法好（代价小）就更新这个点if setClosedCosts(I) > costs(jj)costchart(setClosed(I)) = costs(jj);setClosedCosts(I) = costs(jj);fieldpointers(setClosed(I)) = movementdirections(jj);endendendend%% if isempty(setOpen) break; endset(axishandle,'CData',[costchart costchart(:,end); costchart(end,:) costchart(end,end)]);set(gca,'CLim',[0 1.1*max(costchart(find(costchart < Inf)))]);drawnow;
end%%%调用findWayBack函数进行路径回溯，并绘制出路径曲线for i=1:length(emptycarports)j = ismember(setOpen,goalposind(i));if(~all(j==0))flag = i;end
end
if max(ismember(setOpen,goalposind(flag)))disp('Solution found!');p = findWayBack(goalposind(flag),fieldpointers); % 调用findWayBack函数进行路径回溯，将回溯结果放于矩阵P中plot(p(:,2)+0.5,p(:,1)+0.5,'Color',0.2*ones(3,1),'LineWidth',4);  %用 plot函数绘制路径曲线drawnow;drawnow;elseif isempty(setOpen)disp('No Solution!'); end
end

inputcarsports函数，用来为A*算法提供目标节点的，根据本题，这个函数将停车场中停车位的编号转换成地图索引存放到goalpoints向量输出，AStar函数就直接拿着goalpoints向量开始操作：

function [ goalpoints ] = inputcarsports( emptycarports )
% 提供停车场中的空停车位
%emptycatpoints为空停车位
goalpoints = [];
for i=1:length(emptycarports)if(emptycarports(i)>0 && emptycarports(i)<=29)goalpoints(i)=sub2ind([6,124],1,(emptycarports(i)-1)*4+3);elseif(emptycarports(i)>=30 && emptycarports(i)<=32)goalpoints(i)=sub2ind([6,124],mod(emptycarports(i),27),118);elseif(emptycarports(i)>=33 && emptycarports(i)<=61)goalpoints(i)=sub2ind([6,124],6,(61-emptycarports(i))*4+1);elseif(emptycarports(i)>=62 && emptycarports(i)<=73)goalpoints(i)=sub2ind([6,124],3,(emptycarports(i)-62)*9+8);elseif(emptycarports(i)>=74 && emptycarports(i)<=85)goalpoints(i)=sub2ind([6,124],4,(147-emptycarports(i)-62)*9+1);else printf('不存在该停车位！');end
endend

initializeField函数，用来设定field矩阵，这个矩阵就是地图矩阵，Inf表示障碍物，S表示起始点，G表示终点，根据输入的goalposind(就是上面函数的goalpoints，当初在写inputcarsports函数的时候的时候意外发现写错了，但比赛太累了就没改回来)。

%%
%这个矩阵的作用就是生成环境，障碍物，起始点，终止点等
function [field, startposind, goalposind, costchart, fieldpointers] = initializeField(goalposind,startposind)field = ones(6,124);field(1,:) = Inf;field(6,:) = Inf;field(3,1:116)=Inf;field(4,1:116)=Inf;field(3,118:124)=Inf;field(4,118:124)=Inf;field(:,118:124)=Inf;field(startposind) = 0; field(goalposind) = 0;  %把矩阵中起始点和终止点处的值设为0costchart = NaN*ones(6,124);%生成一个6*124的矩阵costchart，每个元素都设为NaN。就是矩阵初始NaN无效数据costchart(startposind) = 0;%在矩阵costchart中将起始点位置处的值设为0% 生成元胞数组fieldpointers = cell(6,124);%生成元胞数组6*124fieldpointers{startposind} = 'S'; for i=1:length(goalposind)fieldpointers{goalposind(i)} = 'G'; %将元胞数组的起始点的位置处设为 'S'，终止点处设为'G'endfieldpointers(field==inf)={0};end
% end of this function

createFigure函数，专门将field矩阵生成成可视化地图出来，并标号起始点和终点。

      function axishandle = createFigure(field,costchart,startposind,goalposind)if isempty(gcbf)                                       %gcbf是当前返回图像的句柄，isempty(gcbf)假如gcbf为空的话，返回的值是1，假如gcbf为非空的话，返回的值是0figure('Position',[0 50 2000 120]);axes('position', [0.01 0.01 0.99 0.99]);               %设置坐标轴的位置，左下角的坐标设为0.01,0.01   右上角的坐标设为0.99 0.99  （可以认为figure图的左下角坐标为0 0   ，右上角坐标为1 1 ）elsegcf; cla;  endfield(field < Inf) = 0; %将fieid矩阵中没有障碍物的位置处设为0pcolor([field field(:,end); field(end,:) field(end,end)]);cmap = flipud(colormap('jet'));  %生成的cmap是一个256X3的矩阵，每一行的3个值都为0-1之间数，分别代表颜色组成的rgb值cmap(1,:) = zeros(3,1); cmap(end,:) = ones(3,1); %将矩阵cmap的第一行设为0 ，最后一行设为1colormap(flipud(cmap)); %进行颜色的倒转 hold on;axishandle = pcolor([costchart costchart(:,end); costchart(end,:) costchart(end,end)]);[goalposy,goalposx] = ind2sub([6,124],goalposind);[startposy,startposx] = ind2sub([6,124],startposind);plot(goalposx+0.5,goalposy+0.5,'ys','MarkerSize',10,'LineWidth',6);plot(startposx+0.5,startposy+0.5,'bo','MarkerSize',10,'LineWidth',6);end

finFValue函数：用来找拓展点的，就是A*算法步骤中的寻找当前节点的周围的可行节点，并计算F。也是相当重要的一个函数，在这个函数中，我加入了一些小小的判断，因为根据在题目中，停车场是单行道，所以在下面的车道上不用往左判断，在上面的车道中不用往右判断，故我加入了if(currentpos(1)~=2)的判断，让汽车在下面车道时不需要往左边搜索，currentpos（1）是存放当前节点的纵坐标，我做的地图是6×124，第2行就是下层单行道，这段代码的意思为，当横坐标不为2时，才需要往左搜索，当横坐标为2时，不需要回头搜索。

function [cost,heuristic,posinds] = findFValue(posind,costsofar,field,goalind,heuristicmethod)%currentpos(1)为父节点的横坐标，currentpos(2)为父节点的纵坐标[currentpos(1) currentpos(2)] = ind2sub([6 124],posind);   %将要进行拓展的点（也就是父节点）的索引值拓展成坐标值%goalpos(1)为目标点的横坐标，goalpos(2)为目标点的纵坐标[goalpos(1) goalpos(2)] = ind2sub([6 124],goalind);        %将终止点的索引值拓展成坐标值%pos中粗南方找到的子节点的坐标值，一个横坐标，一个纵坐标cost = Inf*ones(4,1); heuristic = Inf*ones(4,1); pos = ones(4,2); %将矩阵cost和heuristic初始化为4x1的无穷大值的矩阵，pos初始化为4x2的值为1的矩阵%拓展方向即上下左右，方向一为下，方向二为上，方向三为左，方向四为右% 拓展方向一（左）if(currentpos(1)~=2)newx = currentpos(2) - 1; newy = currentpos(1);if newx > 0%判断边界pos(1,:) = [newy newx];%存放找到的拓展点switch lower(heuristicmethod)%计算终止点点距离拓展出来的点的代价的方法case 'euclidean'heuristic(1) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy);case 'taxicab'heuristic(1) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy);endcost(1) = costsofar + field(newy,newx);%计算出拓展出来的点距离起始点的代价endend% 拓展方向二（右）newx = currentpos(2) + 1; newy = currentpos(1);if newx <= 124pos(2,:) = [newy newx];switch lower(heuristicmethod)case 'euclidean'heuristic(2) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy);case 'taxicab'heuristic(2) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy);endcost(2) = costsofar + field(newy,newx);end% 拓展方向三（下）  newx = currentpos(2); newy = currentpos(1)-1;if newy > 0pos(3,:) = [newy newx];switch lower(heuristicmethod)case 'euclidean'heuristic(3) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy);case 'taxicab'heuristic(3) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy);endcost(3) = costsofar + field(newy,newx);end% 拓展方向四（上）newx = currentpos(2); newy = currentpos(1)+1;if newy <= 6pos(4,:) = [newy newx];switch lower(heuristicmethod)case 'euclidean'heuristic(4) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy);case 'taxicab'heuristic(4) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy);endcost(4) = costsofar + field(newy,newx);endposinds = sub2ind([6 124],pos(:,1),pos(:,2)); % 将拓展出来的子节点的坐标值转换为索引值
end

findWayBack函数是当找到路径后，通过回溯找到起始点，将轨迹图像画出来。

%findWayBack函数用来进行路径回溯，这个函数的输入参数是终止点goalposind和矩阵fieldpointers，输出参数是P
function p = findWayBack(goalposind,fieldpointers)posind = goalposind;[py,px] = ind2sub([6,124],posind); % 将索引值posind转换为坐标值 [py,px]p = [py px];%利用while循环进行回溯，当我们回溯到起始点的时候停止，也就是在矩阵fieldpointers中找到S时停止while ~strcmp(fieldpointers{posind},'S')switch fieldpointers{posind}case 'L' % ’L’ 表示当前的点是由左边的点拓展出来的px = px - 1;case 'R' % ’R’ 表示当前的点是由右边的点拓展出来的px = px + 1;case 'U' % ’U’ 表示当前的点是由上面的点拓展出来的py = py - 1;case 'D' % ’D’ 表示当前的点是由下边的点拓展出来的py = py + 1;endp = [p; py px];posind = sub2ind([6,124],py,px);% 将坐标值转换为索引值end
end

将以上5个函数全部创建到你的Matlab中，就可以得到如下最佳停车轨迹图：

若还有什么问题欢迎留言或者私信，有时间可以一起探讨学习编程与建模。

下面附上我参考的两篇大神的博客，一个把A*算法的思路写的很清晰，一个把A*的Matlab代码逐条逐句地讲解，都是很帮的文章！

A*算法思路博客：

A*算法（超级详细讲解，附有举例的详细手写步骤）

A*算法Matlab代码讲解博客：详细介绍用MATLAB实现基于A*算法的路径规划（附完整的代码，代码逐行进行解释）（一）--------A*算法简介和环境的创建

2022年Mathorcup数学建模挑战杯C题比赛心得总结（1）——A*算法的应用与优化（含Matlab代码）相关推荐

2022国赛数学建模思路汇总A题B题C题D题高教社杯
一.2022 高教社杯数学建模竞赛 2022国赛数学建模竞赛的时间确定为9月15日(周四)18时至9月18日(周日)20时. A君为大家准备了一系列的学习资料,本次比赛A君也会不断提供赛题资料和思路给 ...
2022年MathorCup数学建模C题自动泊车问题解题全过程文档加程序
2022年第十二届MathorCup高校数学建模 C题自动泊车问题原题再现自动泊车是自动驾驶技术中落地最多的场景之一,自动泊车指在停车场内实现汽车的自动泊车入位过程,在停车空间有限的大城市, ...
2022年MathorCup数学建模B题无人仓的搬运机器人调度问题解题全过程文档加程序
2022年第十二届MathorCup高校数学建模 B题无人仓的搬运机器人调度问题原题再现本题考虑在无人仓内的仓库管理问题之一,搬运机器人 AGV 的调度问题.更多的背景介绍请参看附件-背景介 ...
2022年MathorCup数学建模A题大规模指纹图像检索的模型与实现解题全过程文档加程序
2022年第十二届MathorCup高校数学建模 A题大规模指纹图像检索的模型与实现原题再现在生物特征识别领域,指纹作为最具独特性与持久性的生物特征之一,被广泛应用于身份识别. 指纹识别 ...
2022年Mathorcup数学建模-大数据竞赛A题二手车估价论文（仅供参考）
图4.1.1.1箱线图图4.1.1.2热力图图4.1.1.5 各日期特征与二手车价格的线性图图4.1.1.6 数据分布直方图由以上分析初步可知影响二手车价格的主要因素有新车价.排量,以及车的使 ...
2021年数学建模中青杯A题论文、思路、代码！！
目背景:汽车制造冲压.焊接.涂装和总装四大关键工艺.总装过程中物料的配送补充,是本次题目的核心.物料补充工作称为运输,一般由拖车完成.拖车接收到任务指令后,需要进行如下操作:1)取料:前往目标物料的存 ...
【江西省研究生数学建模竞赛】第一题蒸汽发生器倒U型管内液体流动详细的数学模型及参考文献
[江西省研究生数学建模竞赛]题目之一蒸汽发生器倒U型管内液体流动相关链接 [江西省研究生数学建模竞赛]第一题蒸汽发生器倒U型管内液体流动 70页论文及MATLAB代码 [江西省研究生数学建模竞赛 ...
2022数学建模高教杯国家一等奖经验分享
此篇投稿者即是粉丝,也是我2022年带得最早的一批徒弟之一.能拿到国一我也非常开心,于是让他写了以下经验分享,希望对大家有所帮助. 很荣幸能在大学最后的时光中拿到国一,自此之后也可以算正式退役了,在这 ...
2022国赛数学建模A题B题C题思路分析高教社杯
一.思路分析 A题思路分析: (比赛开始后第一时间发布) B题思路分析: (比赛开始后第一时间发布) C题思路分析: (比赛开始后第一时间发布) 二.建模资料免费领取数模君多次参加国赛.美赛.深圳杯 ...
Mathorcup数学建模竞赛第六届-【妈妈杯】A题：淡水养殖池塘水华发生及池水自净化研究（附一等奖获奖论文和matlab代码）
赛题描述文中涉及附件数据详见第六届Mathorcup数学建模竞赛A题淡水养殖池塘水华发生及池水净化处理数据目前在我国水产养殖中,池塘养殖产量约占淡水养殖的70%.近年来,随着淡水生态系统水体污染 ...

2022年Mathorcup数学建模挑战杯C题比赛心得总结（1）——A*算法的应用与优化（含Matlab代码）