今年参加了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*算法中的核心公式：
这个公式表示了当前节点到目的节点估计所需要的代价，其中：
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');
end
setClosed = [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);
%如果这个点位于矩阵的最后一行
else
setOpen = 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 矩阵中
else
I = find(setClosed == posinds(jj));
% 如果通过目前的方法找到的这个点，比之前的方法好（代价小）就更新这个点
if setClosedCosts(I) > costs(jj)
costchart(setClosed(I)) = costs(jj);
setClosedCosts(I) = costs(jj);
fieldpointers(setClosed(I)) = movementdirections(jj);
end
end
end
end
%%
if isempty(setOpen) break; end
set(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
endinputcarsports函数，用来为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
end
end
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; %把矩阵中起始点和终止点处的值设为0
costchart = NaN*ones(6,124);%生成一个6*124的矩阵costchart，每个元素都设为NaN。就是矩阵初始NaN无效数据
costchart(startposind) = 0;%在矩阵costchart中将起始点位置处的值设为0
% 生成元胞数组
fieldpointers = cell(6,124);%生成元胞数组6*124
fieldpointers{startposind} = 'S';
for i=1:length(goalposind)
fieldpointers{goalposind(i)} = 'G'; %将元胞数组的起始点的位置处设为 'S'，终止点处设为'G'
end
fieldpointers(field==inf)={0};
end
% end of this functioncreateFigure函数，专门将field矩阵生成成可视化地图出来，并标号起始点和终点。
function axishandle = createFigure(field,costchart,startposind,goalposind)
if isempty(gcbf) %gcbf是当前返回图像的句柄，isempty(gcbf)假如gcbf为空的话，返回的值是1，假如gcbf为非空的话，返回的值是0
figure('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 ）
else
gcf; cla;
end
field(field < Inf) = 0; %将fieid矩阵中没有障碍物的位置处设为0
pcolor([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 ，最后一行设为1
colormap(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);
end
cost(1) = costsofar + field(newy,newx);%计算出拓展出来的点距离起始点的代价
end
end
% 拓展方向二（右）
newx = currentpos(2) + 1; newy = currentpos(1);
if newx <= 124
pos(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);
end
cost(2) = costsofar + field(newy,newx);
end
% 拓展方向三（下）
newx = currentpos(2); newy = currentpos(1)-1;
if newy > 0
pos(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);
end
cost(3) = costsofar + field(newy,newx);
end
% 拓展方向四（上）
newx = currentpos(2); newy = currentpos(1)+1;
if newy <= 6
pos(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);
end
cost(4) = costsofar + field(newy,newx);
end
posinds = 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;
end
p = [p; py px];
posind = sub2ind([6,124],py,px);% 将坐标值转换为索引值
end
end
将以上5个函数全部创建到你的Matlab中，就可以得到如下最佳停车轨迹图：
若还有什么问题欢迎留言或者私信，有时间可以一起探讨学习编程与建模。
下面附上我参考的两篇大神的博客，一个把A*算法的思路写的很清晰，一个把A*的Matlab代码逐条逐句地讲解，都是很帮的文章！
A*算法思路博客：
A*算法（超级详细讲解，附有举例的详细手写步骤）
A*算法Matlab代码讲解博客：详细介绍用MATLAB实现基于A*算法的路径规划（附完整的代码，代码逐行进行解释）（一）--------A*算法简介和环境的创建
欢迎分享，转载请注明来源：内存溢出
微信扫一扫
支付宝扫一扫
评论列表（0条）