目录
1 移动机器人寻路算法概述
移动机器人寻路算法主要可分为基于图搜索的算法和基于概率搜索的算法，基于图搜索的有Dijstra、A*、Hybrid A*算法等，基于概率搜索的算法有PRM、RRT、RRT*等。
需要注意的是二者并不是割裂开来的。基于图搜索的算法通过在栅格地图上离散控制量，得到所有可能的下一步的状态（即父节点的所有的相邻节点），不断迭代查找最优状态（代价值最小），直到找到目标点，这种方法所需的图的节点数非常多，在地图维数较大时，非常耗时。
为了解决这个问题，可以利用概率搜索的算法先构造一张稀疏的图，然后再利用基于图搜索的算法寻找起始点到目标点的最优路径。
基于概率搜索的算法，其思想就是先以一定的概率在地图范围内随机撒点，剔除掉在障碍物上的点，剩下的点我们用来构造图的节点，节点构造完以后，遍历所有节点，按照一定的规则构造边，根据上述方法构造出来的图用于图搜索算法，查找最优路径。
2 算法流程
第一步：基于PRM构造图
step1：按照一定概率在地图上撒点，剔除掉落在障碍物上的点，将剩余的N个点存储起来
step2：遍历所有节点
for i=1:N
找第i个节点相邻的k个节点
for j=1:k
判断其i节点与其相邻的节点j连线是否与障碍物发生碰撞，如果不碰撞，则i节点与该相邻节点可通行，边的权值设为1，保存在二维数组G（i，j）中
end
end
第二步：基于图G调用A*算法
具体算法流程见【MATLAB】二维栅格地图A star（A*）路径搜索算法matlab实现_一只小白白的博客-CSDN博客
3 算法讲解
PRM的主要部分代码注释比较详细，就直接给出来了：
function [allV,graph]= build_PRM(n,k,QLIST,MAX_X,MAX_Y)
% 随机生成节点数组
V=[];
% 初始化图的二维数组（n * n）
G=zeros(n);
% 障碍物个数
Obstacle_Num = size(QLIST,1);
%% 绘制障碍物模型
for i=1:Obstacle_Num
Q = [QLIST(i,1:4);QLIST(i,5:8)];
fill(Q(1,:),Q(2,:),'k');
hold on
end
%% 初始化图G的相邻节点间边的权值
for i=1:n
for j=1:n
if ~isequal(i,j)
G(i,j)=inf;
end
end
end
%% 随机生成n个节点，判断是否落在障碍物上，如果不在障碍物中，将其加入节点数组V中
if Obstacle_Num==0
while size(V,2)<n
qrand=[MAX_X*rand;MAX_Y*rand];
V=[V,qrand]; % 由于没有障碍物，直接填充即可
end
else
while size(V,2)<n
qrand=[MAX_X*rand;MAX_Y*rand];
% 存在障碍物时，需要剔除随机生成在障碍物上的点，保留符合条件的点
for m=1:Obstacle_Num
Q = [QLIST(m,1:4);QLIST(m,5:8)];
Q = [Q,Q(:,1)];
[in,on] = inpolygon(qrand(1),qrand(2),Q(1,:),Q(2,:));
if (in==1 || on==1)
plot(qrand(1),qrand(2),'r*'); hold on;
break;
elseif m==Obstacle_Num
V=[V,qrand];
plot(qrand(1),qrand(2),'k*'); hold on;
end
end
end
end
%% 根据上面生成的n个节点，填充图G的数据
if Obstacle_Num==0
for j=1:size(V,2)
% 找出当前节点j邻近的k个节点，找出的邻近点包括自己，且为第一个索引值，即ind(1)
ind= knnsearch( V', V(:,j)', 'k',k+1);
ind_temp = [];
for i=2:size(ind,2)
% 保证邻近节点Nq不包括自己
ind_temp = [ind_temp,ind(i)];
Nq(:,i-1)= V(:,ind(i));
end
ind = ind_temp;
% 遍历这k个节点，因为没有障碍物，所以直接把当前节点与其邻近节点相连即可
for i=1:size(Nq,2)
G(j,ind(i))=1; % 如果一直到最后一个障碍物都没有交叉，则此邻近节点与j节点可通行，权值设为1
G(ind(i),j)=1;
plot([V(1,j),Nq(1,i)],[V(2,j),Nq(2,i)],'g--');
pause(0.01);
hold on
end
end % 结束 for 循环
else
for j=1:size(V,2)
plot(V(1,j),V(2,j),'rs'); hold on;
% 找出当前节点j邻近的k个节点，找出的邻近点包括自己，且为第一个索引值，即ind(1)
ind= knnsearch( V', V(:,j)', 'k',k+1);
ind_temp = [];
% 保证邻近节点Nq不包括自己
for i=2:size(ind,2)
ind_temp = [ind_temp,ind(i)];
Nq(:,i-1)= V(:,ind(i));
plot(Nq(1,i-1),Nq(2,i-1),'bs'); hold on;
end
ind = ind_temp;
% 遍历这k个节点，如果与父节点连线不与障碍物碰撞，则G中相应权值为1，代表两节点之间可通行
for i=1:size(Nq,2)
% 遍历所有障碍物，判断第i个邻近节点与j节点的连线与障碍物是否有交叉
for m=1:Obstacle_Num
Q = [QLIST(m,1:4);QLIST(m,5:8)];
if isIntersection( V(:,j),Nq(:,i),Q )==1
% plot([V(1,j),Nq(1,i)],[V(2,j),Nq(2,i)],'r--');
break; % 如果有交叉，直接break，进入下一个邻近节点的判断
elseif m==Obstacle_Num
G(j,ind(i))=1; % 如果一直到最后一个障碍物都没有交叉，则此邻近节点与j节点可通行，权值设为1
G(ind(i),j)=1;
plot([V(1,j),Nq(1,i)],[V(2,j),Nq(2,i)],'g--');
% pause(0.01);
hold on
end
end
end
end % 结束 for 循环
end % 结束 if和else 判断
% 输出图G和节点信息V
allV=V;
graph=G;
end
上面PRM的代码主要参考的是【github】PRM+Astar MATLAB Code
(源代码是用Dijstra寻路的，我是用A*寻路的，除此之外，我还改进了其PRM算法中节点连线与障碍物碰撞检测的函数，大家有兴趣的话可以把他的代码下载下来，和我的代码对比一下，看看改进后的效果）
可以注意到在整个PRM中，障碍物的检测是非常重要的，判断随机生成的点是否落在障碍物内，可直接调用MATLAB函数inpolygon()，难点就在于如何判断连线是否经过障碍物，利用上面多给的github代码并不能很好的检测出来，在网上搜了很多也没有现成的代码，于是就写了一个比较简单的检测代码，检测原理很简单，就是判断给定两点P0和P1的连线是否与Q描述的多边形的边有交叉，如果有交叉，判断交点是否位于P0P1线段上以及是否位于Q多边形的边上，如果都位于，则说明P0P1线段与Q相交，也就说明此条路径行不通，返回0，否则返回1，具体代码如下：
function IS_INTERSECT_FLAG = isIntersection( P0,P1,Q )
%ISINTERSECTION 此处显示有关此函数的摘要
% 此处显示详细说明
% P0，P1：起始点和终点
% Q：四边形
% IS_INTERSECT_FLAG：为1则有交叉，为0则没有交叉
IS_INTERSECT_FLAG = 0;
% 判断Q为边数
N = size(Q,2);
% 扩充Q，方便后续相关参数的计算
QQ = [Q,Q(:,1)];
% 对P0，P1两个点的x坐标和y坐标分别排序
x_P_sort = sort([P0(1),P1(1)]);
y_P_sort = sort([P0(2),P1(2)]);
% 判断P0和P1是否在Q中
[P0_in,P0_on] = inpolygon(P0(1),P0(2),QQ(1,:),QQ(2,:));
[P1_in,P1_on] = inpolygon(P1(1),P1(2),QQ(1,:),QQ(2,:));
if (P0_in||P0_on) || (P1_in||P1_on)
IS_INTERSECT_FLAG=1;
disp('xxx1');
elseif (isequal(P0,P1))
% 如果两个点都不在Q中，且两个点重合，则不可能和Q相交
IS_INTERSECT_FLAG=0;
disp('xxx2');
else
% 两个点不重合且都不在Q中，计算其与Q交点
% 计算P0-P1连线参数
if (P1(1)-P0(1)==0)
k_P = inf;
b_P = 0;
else
k_P = (P1(2)-P0(2))/(P1(1)-P0(1));
b_P = P0(2)-k_P*P0(1);
end
% 循环四条边判断是否有交叉
for i=1:N
% 计算Q的第i条边的参数 y = k_Q*x+b_Q
if (QQ(1,i+1)-QQ(1,i)==0)
k_Q(i) = inf;
b_Q(i) = 0;
else
k_Q(i) = (QQ(2,i+1)-QQ(2,i))/(QQ(1,i+1)-QQ(1,i));
b_Q(i) = QQ(2,i)-k_Q(i)*QQ(1,i);
end
% 如果平行则不交叉，但不代表不重合
if (k_Q(i)==k_P)
% 如果四点共线，仍然视为相交
if k_P==inf
if (Q(1,i)==P0(1))
IS_INTERSECT_FLAG=1;
break;
end
else
if (b_P==b_Q(i))
IS_INTERSECT_FLAG=1;
break;
end
end
else
if k_P==inf
% 如果不平行则一定交叉，计算交点
X_INTERSECT = P0(1);
Y_INTERSECT = k_Q(i)*X_INTERSECT+b_Q(i);
% 判断交点是否在两条线段之间
x_Q_sort = sort([QQ(1,i),QQ(1,i+1)]);
y_Q_sort = sort([QQ(2,i),QQ(2,i+1)]);
if (X_INTERSECT>=x_Q_sort(1)&&X_INTERSECT<=x_Q_sort(2)&&Y_INTERSECT>=y_Q_sort(1)&&Y_INTERSECT<=y_Q_sort(2))
if (X_INTERSECT>=x_P_sort(1)&&X_INTERSECT<=x_P_sort(2)&&Y_INTERSECT>=y_P_sort(1)&&Y_INTERSECT<=y_P_sort(2))
IS_INTERSECT_FLAG=1;
plot(X_INTERSECT,Y_INTERSECT,'c+');
disp('xxx3');
break;
end
end
elseif k_Q(i)==inf
disp('xxx4');
% 如果不平行则一定交叉，计算交点
X_INTERSECT = Q(1,i);
Y_INTERSECT = k_P*X_INTERSECT+b_P;
% 判断交点是否在两条线段之间
x_Q_sort = sort([QQ(1,i),QQ(1,i+1)]);
y_Q_sort = sort([QQ(2,i),QQ(2,i+1)]);
if (X_INTERSECT>=x_Q_sort(1)&&X_INTERSECT<=x_Q_sort(2)&&Y_INTERSECT>=y_Q_sort(1)&&Y_INTERSECT<=y_Q_sort(2))
if (X_INTERSECT>=x_P_sort(1)&&X_INTERSECT<=x_P_sort(2)&&Y_INTERSECT>=y_P_sort(1)&&Y_INTERSECT<=y_P_sort(2))
IS_INTERSECT_FLAG=1;
plot(X_INTERSECT,Y_INTERSECT,'c+');
break;
end
end
else
% 如果不平行则一定交叉，计算交点
X_INTERSECT = (b_P-b_Q(i))/(k_Q(i)-k_P);
Y_INTERSECT = k_Q(i)*X_INTERSECT+b_Q(i);
% 判断交点是否在两条线段之间
x_Q_sort = sort([QQ(1,i),QQ(1,i+1)]);
y_Q_sort = sort([QQ(2,i),QQ(2,i+1)]);
if (X_INTERSECT>=x_Q_sort(1)&&X_INTERSECT<=x_Q_sort(2)&&Y_INTERSECT>=y_Q_sort(1)&&Y_INTERSECT<=y_Q_sort(2))
if (X_INTERSECT>=x_P_sort(1)&&X_INTERSECT<=x_P_sort(2)&&Y_INTERSECT>=y_P_sort(1)&&Y_INTERSECT<=y_P_sort(2))
IS_INTERSECT_FLAG=1;
plot(X_INTERSECT,Y_INTERSECT,'c+');
disp('xxx5');
break;
end
end
end
end
end
end上面的图算是构建完了，在给出A*寻路之前，我们需要找到图中需要搜索的起始点和终止点，很简单，直接找到给定的起始点和终止点在图中查找最近可能的点即可，（注意这里给定的起始点和终止点与图中需要搜索的起始点和终止点不一样，看代码就理解了）具体代码如下：
function [Index_startInPRM, Index_endInPRM] = findStartAndEndInPRM( Vertex, qset, QLIST)
%FINDSTART 此处显示有关此函数的摘要
% 障碍物个数
Obstacle_Num = size(QLIST,1);
% 此处显示详细说明
V = Vertex;
%% 找到起始点和终止点的k个邻近点
k=5;
Index_AllNearStart= knnsearch( V', qset(:,1)', 'k',k);
Index_AllNearEnd = knnsearch(V',qset(:,2)','k',k);
for i=1:k
plot(V(1,Index_AllNearStart(i)),V(2,Index_AllNearStart(i)),'ro'); hold on;
plot(V(1,Index_AllNearEnd(i)),V(2,Index_AllNearEnd(i)),'bo'); hold on;
end
%% 从k个邻近点中找到满足要求的起始点
IS_FIND_START_FLAG=0;
IS_FIND_END_FLAG=0;
if Obstacle_Num==0
% 由于没有障碍物，所以一定存在最近点
Index_startInPRM = Index_AllNearStart(1);
else
for i=1:k % this loop is find nearest nodes to qset
% 检测起始点和邻近点的连线是否经过障碍物
for m=1:Obstacle_Num
Q = [QLIST(m,1:4);QLIST(m,5:8)];
if isequal( 1, isIntersection( V(:,Index_AllNearStart(i)),qset(:,1),Q ) )
break;
elseif m==Obstacle_Num
% 如果不经过障碍物直接返回当前起始点邻近节点的索引值
Index_startInPRM = Index_AllNearStart(i);
IS_FIND_START_FLAG = 1;
plot(V(1,Index_startInPRM),V(2,Index_startInPRM),'b*'); hold on;
end
end
% 如果找到符合条件的邻近起点，直接break
if IS_FIND_START_FLAG
break;
end
% 未找到则给出提示
if i==k && IS_FIND_START_FLAG==0
msgbox('未搜索到起始点！');
end
end
end
%% 从k个邻近点中找到满足要求的终止点
if Obstacle_Num==0
% 由于没有障碍物，所以一定存在最近点
Index_endInPRM = Index_AllNearEnd(1);
else
for i=1:k % this loop is find nearest nodes to qset
% 检测起始点和邻近点的连线是否经过障碍物
for m=1:Obstacle_Num
Q = [QLIST(m,1:4);QLIST(m,5:8)];
if isequal( 1, isIntersection( V(:,Index_AllNearEnd(i)),qset(:,2),Q ) )
break;
elseif m==Obstacle_Num
% 如果不经过障碍物直接返回当前起始点邻近节点的索引值
Index_endInPRM = Index_AllNearEnd(i);
IS_FIND_END_FLAG = 1;
plot(V(1,Index_endInPRM),V(2,Index_endInPRM),'b*'); hold on;
end
end
% 如果找到符合条件的邻近起点，直接break
if IS_FIND_END_FLAG
break;
end
% 未找到则给出提示
if i==k && IS_FIND_END_FLAG==0
msgbox('未搜索到终点！');
end
end
end
end
上面的地图给出了，而且图中需要搜索的起始点和终止点也找到了，下面给出A*算法代码用于寻路：
function path = A_star_search( graph, Vertex,Index_start,Index_end )
%A_STAR_SEARCH 此处显示有关此函数的摘要
% 此处显示详细说明
% 节点总个数
N = size(graph,2);
% 定义节点的数据结构
%--------------------------------------------------------------------------
% Node |Index |Parent Index |h(n) |g(n) |f(n)|
%--------------------------------------------------------------------------
Node = zeros(1,5);
% OPEN LIST STRUCTURE
%--------------------------------------------------------------------------
% |Index |Parent Index |h(n) |g(n) |f(n)|
%--------------------------------------------------------------------------
OPEN=repmat(Node,N*N,1);
OPEN_COUNT=0;
% CLOSED LIST STRUCTURE
%--------------------------------------------------------------------------
% |Index |Parent Index |h(n) |g(n) |f(n)|
%--------------------------------------------------------------------------
CLOSED=repmat(Node,N*N,1);
CLOSED_COUNT=0;
% 设置启发函数的距离计算公式，DISTANCE_FLAG=0：曼哈顿距离，DISTANCE_FLAG=1：欧式距离
DISTANCE_FLAG = 1;
% 搜索起始点
xStart = Vertex(1,Index_start);
yStart = Vertex(2,Index_start);
% 搜索终点
xTarget = Vertex(1,Index_end);
yTarget = Vertex(2,Index_end);
% 初始化父节点
xFatherNode=xStart;
yFatherNode=yStart;
goal_cost= DISTANCE(xFatherNode,yFatherNode,xTarget,yTarget,DISTANCE_FLAG);
path_cost=0;
total_cost = goal_cost+path_cost;
Node_Father = [Index_start,Index_start,goal_cost,path_cost,total_cost];
% 将父节点加入CLOSED中
CLOSED_COUNT=CLOSED_COUNT+1;
CLOSED(CLOSED_COUNT,:)= Node_Father;
% 搜索最优路径
while(1) %you have to dicide the Conditions for while loop exit
%
%finish the while loop
%
% 获取当前父节点坐标
xFatherNode = Vertex(1,Node_Father(1)); yFatherNode = Vertex(2,Node_Father(1));
% 判断当前父节点的所有子节点个数
Node_Child_Num = 0;
Index_Child = [];
for i=1:N
if(graph(Node_Father(1),i)==1)
Node_Child_Num = Node_Child_Num+1;
Index_Child(Node_Child_Num) = i;
end
end
% 初始化子节点List
Node_Child_List = repmat(Node,Node_Child_Num,1);
% 将当前父节点的所有子节点压入Node_Child_List（初次循环的起始点为父节点）
for i=1:Node_Child_Num
Node_Child_List(i,1) = Index_Child(i);
end
% 遍历Node_Child_Num个子节点，填充满足要求的子节点信息,并将其填充至OPEN中
for i=1:Node_Child_Num
% 当前子节点坐标
xChildNode = Vertex(1,Node_Child_List(i,1));
yChildNode = Vertex(2,Node_Child_List(i,1));
% 如果子节点在CLOSED中，则不满足要求
CHILD_IS_IN_CLOSED = 0;
for j=1:CLOSED_COUNT
if(CLOSED(j,1)==Node_Child_List(i,1))
CHILD_IS_IN_CLOSED = 1;
break;
end
end
if (CHILD_IS_IN_CLOSED == 1)
continue;
end
% 计算满足要求的子节点信息
goal_cost=distance(xChildNode,yChildNode,xTarget,yTarget);
path_cost = DISTANCE(xFatherNode,yFatherNode,xChildNode,yChildNode,DISTANCE_FLAG);
total_cost = path_cost+goal_cost;
Node_Child_List(i,2) = Node_Father(1);
Node_Child_List(i,3) = goal_cost;
Node_Child_List(i,4) = path_cost;
Node_Child_List(i,5) = total_cost;
% 判断当前节点是否在OPEN中
CHILDE_IS_IN_OPEN_FLAG = 0;
INDEX_CHILD_IN_OPEN = 0;
for j=1:OPEN_COUNT
if(OPEN(j,1)==Node_Child_List(i,1))
CHILDE_IS_IN_OPEN_FLAG = 1;
INDEX_CHILD_IN_OPEN = j;
break;
end
end
% 如果不在OPEN中加入，如果不在OPEN中则判断当前路径total_cost是否比之前的小，
% 如果更小，则更新之前节点在OPEN中的信息
if (CHILDE_IS_IN_OPEN_FLAG)
if (total_cost<OPEN(INDEX_CHILD_IN_OPEN,5))
OPEN(INDEX_CHILD_IN_OPEN,2) = Node_Father(1);
OPEN(INDEX_CHILD_IN_OPEN,4) = path_cost;
OPEN(INDEX_CHILD_IN_OPEN,5) = total_cost;
end
else
OPEN_COUNT = OPEN_COUNT+1;
OPEN(OPEN_COUNT,1) = Node_Child_List(i,1);
OPEN(OPEN_COUNT,2) = Node_Father(1);
OPEN(OPEN_COUNT,3) = goal_cost;
OPEN(OPEN_COUNT,4) = path_cost;
OPEN(OPEN_COUNT,5) = total_cost;
end
end
% 判断OPEN中是否已经出现的目标节点
TARGET_IS_IN_OPEN_FLAG = 0;
for j=1:OPEN_COUNT
if(OPEN(j,1)== Index_end)
TARGET_IS_IN_OPEN_FLAG=1;
% 将目标节点放入CLOSED中
CLOSED_COUNT = CLOSED_COUNT+1;
CLOSED(CLOSED_COUNT,:) = OPEN(j,:);
end
end
% 如果找到目标节点，则终止while循环
if (TARGET_IS_IN_OPEN_FLAG==1)
break;
end
% 判断OPEN是否为空，OPEN为空则代表没找到路径
IS_OPEN_EMPTY_FLAG = false;
if OPEN_COUNT == 0
IS_OPEN_EMPTY_FLAG = true;
end
if IS_OPEN_EMPTY_FLAG
break;
end
% 如果没找到，找出当前代价最小的节点
temp_cost=OPEN(1,5);
INDEX_MINCOST_IN_OPEN = 1;
for j=2:OPEN_COUNT
if (temp_cost>OPEN(j,5))
temp_cost = OPEN(j,5);
INDEX_MINCOST_IN_OPEN = j;
end
end
% 将其作为父节点
Node_Father = OPEN(INDEX_MINCOST_IN_OPEN,:);
% 将其加入CLOSED
CLOSED_COUNT = CLOSED_COUNT+1;
CLOSED(CLOSED_COUNT,:) = OPEN(INDEX_MINCOST_IN_OPEN,:);
% 将其弹出OPEN，并更新OPEN
for j=INDEX_MINCOST_IN_OPEN:OPEN_COUNT
OPEN(j,:) = OPEN(j+1,:);
end
OPEN_COUNT = OPEN_COUNT-1;
end %End of While Loop
%Once algorithm has run The optimal path is generated by starting of at the
%last node(if it is the target node) and then identifying its parent node
%until it reaches the start node.This is the optimal path
%
%How to get the optimal path after A_star search?
%please finish it
%
%% 循环完成后处理结果
if TARGET_IS_IN_OPEN_FLAG %OpenList中如果出现目标点则代表搜索完成找到路径,根据CloseList中的数据画出图像
PATH = [xTarget yTarget];
Index = Index_end;
while 1
for i = 1:CLOSED_COUNT % 寻找当前节点的父节点
if Index == CLOSED(i,1)
Index = CLOSED(i,2);
PositionX = Vertex(1,Index);
PositionY = Vertex(2,Index);
Pos = [PositionX PositionY];
PATH = [PATH;Pos];
break;
end
end
if Index == Index_start
break;
end
end
elseif IS_OPEN_EMPTY_FLAG %OpenList为空则代表没找到路径
msgbox('未搜索到路径！');
end
path = PATH;
end
% 启发函数的距离计算，可根据flag选择欧氏距离或是曼哈顿距离
function d = DISTANCE(xTarget,yTarget,xGoal,yGoal,flag)
% flag==0：曼哈顿距离，flag==1：欧氏距离
if (flag==0)
d = abs(xTarget-xGoal)+abs(yTarget-yGoal);
elseif(flag==1)
d = distance(xTarget,yTarget,xGoal,yGoal);
end
end
% 欧式距离计算
function dist = distance(x1,y1,x2,y2)
%This function calculates the distance between any two cartesian
%coordinates.
% Copyright 2009-2010 The MathWorks, Inc.
dist=sqrt((x1-x2)^2 + (y1-y2)^2);
end上面几个算法函数全部写完，下面给出主函数，主函数就很简单了，先调用PRM生成节点V和图G，然后找到图中待搜索的起始点和终止点，最后调用A*寻路函数查找最短路径，代码如下：
clc
clear all
close all
%% 1. Run PRM
%get the graph by running PRM and map lot
%V is all the qs'
qset=[9 9;12 3]; % 1st column: q_initial, 2nd: q_goal
n=50; % 随机生成的节点数
k=20; % 每个节点最大允许的邻近子节点数量
% 多边形表示的障碍物
Q1=[1 2 2 1;1 1 2 2]; % 障碍物
Q2=[10 15 15 10;
10 10 15 15]; % 障碍物
Q3=[5 10 10 5;
5 5 10 10]; % 障碍物
Q4=[2 4 4 2;14 14 16 16]; % 障碍物
Q5=[4 6 6 4;16 16 18 18]; % 障碍物
Q6=[16 18 18 16;4 4 6 6]; % 障碍物
Q7=[12 14 14 12;2 2 4 4]; % 障碍物
Q8=[6 8 8 6;12 12 14 14]; % 障碍物
% 障碍物列表
QLIST = [Q1(1,1:4),Q1(2,1:4);
Q2(1,1:4),Q2(2,1:4);
Q3(1,1:4),Q3(2,1:4);
Q4(1,1:4),Q4(2,1:4);
Q5(1,1:4),Q5(2,1:4);
Q6(1,1:4),Q6(2,1:4);
Q7(1,1:4),Q7(2,1:4);
Q8(1,1:4),Q8(2,1:4);];
% QLIST = [];
% QLIST = [Q2(1,1:4),Q2(2,1:4);Q3(1,1:4),Q3(2,1:4)];
% 设置地图范围
MAX_X = 20;
MAX_Y = 20;
% 绘制地图，起始点和终止点
figure(1)
set(gcf, 'Renderer', 'painters');
set(gcf, 'Position', [500, 50, 700, 700]);
set(gca, 'XTick', 0:1:MAX_X);
set(gca, 'YTick', 0:1:MAX_Y);
grid on;
axis equal;
axis ([0 MAX_X 0 MAX_Y ]);
hold on;
LL = 0.3;
fill([qset(1,1)-LL,qset(1,1)+LL,qset(1,1)+LL,qset(1,1)-LL],[qset(2,1)-LL,qset(2,1)-LL,qset(2,1)+LL,qset(2,1)+LL],'r'); % 起始点
fill([qset(1,2)-LL,qset(1,2)+LL,qset(1,2)+LL,qset(1,2)-LL],[qset(2,2)-LL,qset(2,2)-LL,qset(2,2)+LL,qset(2,2)+LL],'b'); % 终点
% 创建随机路图
[V,G]=build_PRM(n,k,QLIST,MAX_X,MAX_Y);
%% 2. 找到随机路图中距离起点和终点最近的点
[Index_startInPRM, Index_endInPRM] = findStartAndEndInPRM( V, qset, QLIST);
%% 3. 利用A star算法寻路
path = A_star_search( G, V, Index_startInPRM,Index_endInPRM );
repath = [];
repath = [repath;qset(:,1)'];
kk = size(path,1);
for i=1:size(path,1)
repath = [repath;path(kk,:)];
kk = kk-1;
end
repath = [repath;qset(:,2)'];
plot(repath(:,1)',repath(:,2)','m-','LineWidth',2)4 运行结果
470
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
