RRT

• colormap 函数 创建栅格地图

clc
clear
close all%% 构建颜色MAP图
cmap = [1 1 1; ...       % 1-白色-空地0 0 0; ...           % 2-黑色-静态障碍1 0 0; ...           % 3-红色-动态障碍1 1 0;...            % 4-黄色-起始点 1 0 1;...            % 5-品红-目标点0 1 0; ...           % 6-绿色-到目标点的规划路径   0 1 1];              % 7-青色-动态规划的路径% 构建颜色MAP图
colormap(cmap);%% 构建栅格地图场景
% 栅格界面大小:行数和列数
rows = 10;
cols = 10; % 定义栅格地图全域，并初始化空白区域
field = ones(rows, cols);% 障碍物区域
obsRate = 0.3;
obsNum = floor(rows*cols*obsRate);
obsIndex = randi([1,rows*cols],obsNum,1);
field(obsIndex) = 2;% 起始点和目标点
startPos = 2;
goalPos = rows*cols-2;
field(startPos) = 4;
field(goalPos) = 5;%% 画栅格图
image(1.5,1.5,field);
grid on;
set(gca,'gridline','-','gridcolor','k','linewidth',2,'GridAlpha',0.5);
set(gca,'xtick',1:cols+1,'ytick',1:rows+1);
axis image;

RRT* （渐进最优）

算法原理

与RRT类似，通过采样来确定路径。优点：在每次步骤中会比较找出路径中最短的一条，通过不断优化迭代，找出路径最短的路线。核心为对生成的树重新剪枝得到最优的。

• 参数初始化

  clear all; close all; clc;
  %% 参数初始化
  x_I = 1; y_I = 1;           % 设置初始点
  x_G = 750; y_G = 750;       % 设置目标点
  GoalThreshold = 30;         % 设置目标点阈值
  Delta = 30;                 % 设置扩展步长 default:30
  RadiusForNeib = 80;         % rewire的范围,半径r
  MaxIterations = 1200;       % 最大迭代次数
  UpdateTime = 50;            % 更新路径的时间间隔
  DelayTime = 0.0;            % 绘图延迟时间%% 建树初始化:T是树,v是节点  结构体实现
  T.v(1).x = x_I;             % 把起始节点加入到T中
  T.v(1).y = y_I; 
  T.v(1).xPrev = x_I;         % 节点的父节点坐标:起点的父节点是其本身
  T.v(1).yPrev = y_I;
  T.v(1).totalCost = 0;       % 从起始节点开始的累计cost，这里取欧氏距离
  T.v(1).indPrev = 0;         % 父节点的索引%% 开始构建树
  figure(1);
  ImpRgb = imread('map.png');
  Imp = rgb2gray(ImpRgb);
  imshow(Imp)
  xL = size(Imp,1);   % 地图x轴长度
  yL = size(Imp,2);   % 地图y轴长度
  hold on
  plot(x_I, y_I, 'mo', 'MarkerSize',10, 'MarkerFaceColor','m');   % 绘制起点和目标点
  plot(x_G, y_G, 'go', 'MarkerSize',10, 'MarkerFaceColor','g');
  count = 1;pHandleList = [];
  lHandleList = [];
  resHandleList = [];
  findPath = 0;
  update_count = 0;
  path.pos = [];   % 最终回溯路径
  

• 环境中随机采样状态点，Xrand

%Step 1: 在地图中随机采样一个点x_rand (Sample)x_rand = [unifrnd(0,xL),unifrnd(0,yL)];	%产生随机点(x,y)

• 遍历树，找到最近的节点, xnear, 第一次遍历，最近节点为初始点

%Step 2: 遍历树，从树中找到最近邻近点x_near (Near)minDis = sqrt((x_rand(1) - T.v(1).x)^2 + (x_rand(2) - T.v(1).y)^2);minIndex = 1;for i = 2:size(T.v,2)	% T.v按行向量存储，size(T.v,2)获得节点总数  distance = sqrt((x_rand(1) - T.v(i).x)^2 + (x_rand(2) - T.v(i).y)^2);   %两节点间距离if(distance < minDis)        %% 找到最短距离minDis = distance;minIndex = i;            %%  index 赋值end     endx_near(1) = T.v(minIndex).x;    % 找到当前树中离x_rand最近的节点x_near(2) = T.v(minIndex).y;temp_parent = minIndex;         % 临时父节点的索引temp_cost = Delta + T.v(minIndex).totalCost;   % 临时累计代价  

• 开始树的生长过程 xnew. 连接xnear和xrand，方向即为生长方向。根据步长生成形成新的节点，xnew. 同时检测节点是否在障碍物内。（到这里就完成了RRT，只要不断重复采样直到生成到达目标点的路径即可。但路径并不优化，而且对于narrow 很难找到）

%Step 3: 扩展得到x_new节点 (Steer)theta = atan2((x_rand(2) - x_near(2)),(x_rand(1) - x_near(1)));x_new(1) = x_near(1) + cos(theta) * Delta;x_new(2) = x_near(2) + sin(theta) * Delta;  %plot(x_rand(1), x_rand(2), 'ro', 'MarkerSize',10, 'MarkerFaceColor','r');%plot(x_new(1), x_new(2), 'bo', 'MarkerSize',10, 'MarkerFaceColor','b');% 检查节点是否是collision-free 点估计if ~collisionChecking(x_near,x_new,Imp)  continue;   %有障碍物end

• 以生成的xnew为圆心，在半径内重新搜索节点是否有到达此处更近的路径

%Step 4: 在以x_new为圆心,半径为R的圆内搜索节点 (NearC)disToNewList = [];    % 距离 每次循环要把队列清空  xnew改变nearIndexList = [];   % indexfor index_near = 1:count  %% count是实际的节点多少 每插入一个 count+1disTonew = sqrt((x_new(1) - T.v(index_near).x)^2 + (x_new(2) - T.v(index_near).y)^2);if(disTonew < RadiusForNeib)    % 满足条件:欧氏距离小于RdisToNewList = [disToNewList disTonew];     % 满足条件的所有节点到x_new的costnearIndexList = [nearIndexList index_near];     % 满足条件的所有节点基于树T的索引endend

• 搜索完成后，重新选择xnew的父节点，是否有更近的路径使得xnew的累计cost更小

%Step 5: 选择x_new的父节点,使x_new的累计cost最小 (ChooseParent)for cost_index = 1:length(nearIndexList)    % 在上个step中寻找到的节点 cost_index是基于disToNewList的索引,不是整棵树的索引costToNew = disToNewList(cost_index) + T.v(nearIndexList(cost_index)).totalCost;if(costToNew < temp_cost)    % temp_cost为通过minDist节点的路径的costx_mincost(1) = T.v(nearIndexList(cost_index)).x;     % 符合剪枝条件节点的坐标x_mincost(2) = T.v(nearIndexList(cost_index)).y;if ~collisionChecking(x_mincost,x_new,Imp) continue;   %有障碍物endtemp_cost = costToNew;  % 重新赋值 cost mintemp_parent = nearIndexList(cost_index); %% xnew新的父节点 indexendend

• 新的父节点已经生成，现在需要将xnew插入到树中

 %Step 6: 将x_new插入树T (AddNodeEdge)count = count+1;    %最新节点的索引T.v(count).x = x_new(1);          T.v(count).y = x_new(2); T.v(count).xPrev = T.v(temp_parent).x;     T.v(count).yPrev = T.v(temp_parent).y;T.v(count).totalCost = temp_cost;   %  到达当前点的 costT.v(count).indPrev = temp_parent;     %其父节点x_near的indexl_handle = plot([T.v(count).xPrev, x_new(1)], [T.v(count).yPrev, x_new(2)], 'b', 'Linewidth', 2); %% 线句柄p_handle = plot(x_new(1), x_new(2), 'ko', 'MarkerSize', 4, 'MarkerFaceColor','k'); %% 点句柄pHandleList = [pHandleList p_handle];    %绘图的句柄索引即为countlHandleList = [lHandleList l_handle];pause(DelayTime);

• 最核心的部分，把多余的节点 进行剪枝

%Step 7: 剪枝 (rewire)  核心  %% 不是最小cost的节点for rewire_index = 1:length(nearIndexList)  %%  半径内剪枝if(nearIndexList(rewire_index) ~= temp_parent)    % 若不是之前计算的最小cost的节点newCost = temp_cost + disToNewList(rewire_index);    % 计算neib经过x_new再到起点的代价          if(newCost < T.v(nearIndexList(rewire_index)).totalCost)    % 计算neib经过x_new再到起点的代价小于该点回到起点 需要剪枝x_neib(1) = T.v(nearIndexList(rewire_index)).x;     % 符合剪枝条件节点的坐标x_neib(2) = T.v(nearIndexList(rewire_index)).y;if ~collisionChecking(x_neib,x_new,Imp) continue;   %有障碍物endT.v(nearIndexList(rewire_index)).xPrev = x_new(1);      % 父节点改变为 xnew 对该neighbor信息进行更新T.v(nearIndexList(rewire_index)).yPrev = x_new(2);T.v(nearIndexList(rewire_index)).totalCost = newCost;T.v(nearIndexList(rewire_index)).indPrev = count;       % x_new的索引%delete(pHandleList());%delete(lHandleList(nearIndexList(rewire_index)));lHandleList(nearIndexList(rewire_index)) = plot([T.v(nearIndexList(rewire_index)).x, x_new(1)], [T.v(nearIndexList(rewire_index)).y, x_new(2)], 'r', 'Linewidth', 2);%pHandleList = [pHandleList p_handle];    %绘图的句柄索引即为count%lHandleList = [lHandleList l_handle];endendend

• 终点检测

%Step 8:检查是否到达目标点附近 disToGoal = sqrt((x_new(1) - x_G)^2 + (x_new(2) - y_G)^2);if(disToGoal < GoalThreshold && ~findPath)    % 找到目标点，此条件只进入一次findPath = 1;count = count+1;    %手动将Goal加入到树中Goal_index = count;T.v(count).x = x_G;          T.v(count).y = y_G; T.v(count).xPrev = x_new(1);     T.v(count).yPrev = x_new(2);T.v(count).totalCost = T.v(count - 1).totalCost + disToGoal;T.v(count).indPrev = count - 1;     %其父节点x_near的indexend

• 沿着终点回溯到起点

 if(findPath == 1)update_count = update_count + 1;if(update_count == UpdateTime)  %%  更新路径的间隔update_count = 0;j = 2;path.pos(1).x = x_G; path.pos(1).y = y_G;pathIndex = T.v(Goal_index).indPrev;while 1     path.pos(j).x = T.v(pathIndex).x;  % 回溯path.pos(j).y = T.v(pathIndex).y;pathIndex = T.v(pathIndex).indPrev;    % 沿终点回溯到起点if pathIndex == 0breakendj=j+1;end  for delete_index = 1:length(resHandleList)delete(resHandleList(delete_index));endfor j = 2:length(path.pos)res_handle = plot([path.pos(j).x; path.pos(j-1).x;], [path.pos(j).y; path.pos(j-1).y], 'g', 'Linewidth', 4);resHandleList = [resHandleList res_handle];endendend  pause(DelayTime); %暂停DelayTime s,使得RRT*扩展过程容易观察

• 碰撞检测 确保生成的点 和 生成的路径不在障碍物内

%%  每次迭代一个小步长来判断是否碰撞
function feasible=collisionChecking(startPose,goalPose,map)
feasible=true;
%dir=atan2(goalPose(1)-startPose(1),goalPose(2)-startPose(2));
dir = atan2(goalPose(2)-startPose(2),goalPose(1)-startPose(1));
for r = 0:0.5:sqrt(sum((startPose-goalPose).^2))      %以0.5为步长,从startPose开始递增的检查是否有障碍%posCheck = startPose + r.*[sin(dir) cos(dir)];      %直线距离增加0.5后的坐标posCheck = startPose + r.*[cos(dir) sin(dir)];      %直线距离增加0.5后的坐标%将一个小数(x,y)向4个方向取整,确保该点没有触碰障碍if ~(feasiblePoint(ceil(posCheck),map) && feasiblePoint(floor(posCheck),map) ...&& feasiblePoint([ceil(posCheck(1)) floor(posCheck(2))],map) ...&& feasiblePoint([floor(posCheck(1)) ceil(posCheck(2))],map))feasible = false;break;endif ~feasiblePoint([floor(goalPose(1)),ceil(goalPose(2))],map)feasible = false; end
endfunction feasible = feasiblePoint(point,map)
feasible = true;
if ~(point(1)>=1 && point(1)<=size(map,1) && point(2)>=1 ...&& point(2)<=size(map,2) && map(point(2),point(1))==255)  %% 225  是否是白的feasible = false;   %有障碍
end

• 针对采样方法也可以做出调整，比如在一定区域内采样而不是直接在整个地图内进行均匀采样

RRT

• colormap 函数 创建栅格地图

clc
clear
close all%% 构建颜色MAP图
cmap = [1 1 1; ...       % 1-白色-空地0 0 0; ...           % 2-黑色-静态障碍1 0 0; ...           % 3-红色-动态障碍1 1 0;...            % 4-黄色-起始点 1 0 1;...            % 5-品红-目标点0 1 0; ...           % 6-绿色-到目标点的规划路径   0 1 1];              % 7-青色-动态规划的路径% 构建颜色MAP图
colormap(cmap);%% 构建栅格地图场景
% 栅格界面大小:行数和列数
rows = 10;
cols = 10; % 定义栅格地图全域，并初始化空白区域
field = ones(rows, cols);% 障碍物区域
obsRate = 0.3;
obsNum = floor(rows*cols*obsRate);
obsIndex = randi([1,rows*cols],obsNum,1);
field(obsIndex) = 2;% 起始点和目标点
startPos = 2;
goalPos = rows*cols-2;
field(startPos) = 4;
field(goalPos) = 5;%% 画栅格图
image(1.5,1.5,field);
grid on;
set(gca,'gridline','-','gridcolor','k','linewidth',2,'GridAlpha',0.5);
set(gca,'xtick',1:cols+1,'ytick',1:rows+1);
axis image;

RRT* （渐进最优）

算法原理

与RRT类似，通过采样来确定路径。优点：在每次步骤中会比较找出路径中最短的一条，通过不断优化迭代，找出路径最短的路线。核心为对生成的树重新剪枝得到最优的。

• 参数初始化

  clear all; close all; clc;
  %% 参数初始化
  x_I = 1; y_I = 1;           % 设置初始点
  x_G = 750; y_G = 750;       % 设置目标点
  GoalThreshold = 30;         % 设置目标点阈值
  Delta = 30;                 % 设置扩展步长 default:30
  RadiusForNeib = 80;         % rewire的范围,半径r
  MaxIterations = 1200;       % 最大迭代次数
  UpdateTime = 50;            % 更新路径的时间间隔
  DelayTime = 0.0;            % 绘图延迟时间%% 建树初始化:T是树,v是节点  结构体实现
  T.v(1).x = x_I;             % 把起始节点加入到T中
  T.v(1).y = y_I; 
  T.v(1).xPrev = x_I;         % 节点的父节点坐标:起点的父节点是其本身
  T.v(1).yPrev = y_I;
  T.v(1).totalCost = 0;       % 从起始节点开始的累计cost，这里取欧氏距离
  T.v(1).indPrev = 0;         % 父节点的索引%% 开始构建树
  figure(1);
  ImpRgb = imread('map.png');
  Imp = rgb2gray(ImpRgb);
  imshow(Imp)
  xL = size(Imp,1);   % 地图x轴长度
  yL = size(Imp,2);   % 地图y轴长度
  hold on
  plot(x_I, y_I, 'mo', 'MarkerSize',10, 'MarkerFaceColor','m');   % 绘制起点和目标点
  plot(x_G, y_G, 'go', 'MarkerSize',10, 'MarkerFaceColor','g');
  count = 1;pHandleList = [];
  lHandleList = [];
  resHandleList = [];
  findPath = 0;
  update_count = 0;
  path.pos = [];   % 最终回溯路径
  

• 环境中随机采样状态点，Xrand

%Step 1: 在地图中随机采样一个点x_rand (Sample)x_rand = [unifrnd(0,xL),unifrnd(0,yL)];	%产生随机点(x,y)

• 遍历树，找到最近的节点, xnear, 第一次遍历，最近节点为初始点

%Step 2: 遍历树，从树中找到最近邻近点x_near (Near)minDis = sqrt((x_rand(1) - T.v(1).x)^2 + (x_rand(2) - T.v(1).y)^2);minIndex = 1;for i = 2:size(T.v,2)	% T.v按行向量存储，size(T.v,2)获得节点总数  distance = sqrt((x_rand(1) - T.v(i).x)^2 + (x_rand(2) - T.v(i).y)^2);   %两节点间距离if(distance < minDis)        %% 找到最短距离minDis = distance;minIndex = i;            %%  index 赋值end     endx_near(1) = T.v(minIndex).x;    % 找到当前树中离x_rand最近的节点x_near(2) = T.v(minIndex).y;temp_parent = minIndex;         % 临时父节点的索引temp_cost = Delta + T.v(minIndex).totalCost;   % 临时累计代价  

• 开始树的生长过程 xnew. 连接xnear和xrand，方向即为生长方向。根据步长生成形成新的节点，xnew. 同时检测节点是否在障碍物内。（到这里就完成了RRT，只要不断重复采样直到生成到达目标点的路径即可。但路径并不优化，而且对于narrow 很难找到）

%Step 3: 扩展得到x_new节点 (Steer)theta = atan2((x_rand(2) - x_near(2)),(x_rand(1) - x_near(1)));x_new(1) = x_near(1) + cos(theta) * Delta;x_new(2) = x_near(2) + sin(theta) * Delta;  %plot(x_rand(1), x_rand(2), 'ro', 'MarkerSize',10, 'MarkerFaceColor','r');%plot(x_new(1), x_new(2), 'bo', 'MarkerSize',10, 'MarkerFaceColor','b');% 检查节点是否是collision-free 点估计if ~collisionChecking(x_near,x_new,Imp)  continue;   %有障碍物end

• 以生成的xnew为圆心，在半径内重新搜索节点是否有到达此处更近的路径

%Step 4: 在以x_new为圆心,半径为R的圆内搜索节点 (NearC)disToNewList = [];    % 距离 每次循环要把队列清空  xnew改变nearIndexList = [];   % indexfor index_near = 1:count  %% count是实际的节点多少 每插入一个 count+1disTonew = sqrt((x_new(1) - T.v(index_near).x)^2 + (x_new(2) - T.v(index_near).y)^2);if(disTonew < RadiusForNeib)    % 满足条件:欧氏距离小于RdisToNewList = [disToNewList disTonew];     % 满足条件的所有节点到x_new的costnearIndexList = [nearIndexList index_near];     % 满足条件的所有节点基于树T的索引endend

• 搜索完成后，重新选择xnew的父节点，是否有更近的路径使得xnew的累计cost更小

%Step 5: 选择x_new的父节点,使x_new的累计cost最小 (ChooseParent)for cost_index = 1:length(nearIndexList)    % 在上个step中寻找到的节点 cost_index是基于disToNewList的索引,不是整棵树的索引costToNew = disToNewList(cost_index) + T.v(nearIndexList(cost_index)).totalCost;if(costToNew < temp_cost)    % temp_cost为通过minDist节点的路径的costx_mincost(1) = T.v(nearIndexList(cost_index)).x;     % 符合剪枝条件节点的坐标x_mincost(2) = T.v(nearIndexList(cost_index)).y;if ~collisionChecking(x_mincost,x_new,Imp) continue;   %有障碍物endtemp_cost = costToNew;  % 重新赋值 cost mintemp_parent = nearIndexList(cost_index); %% xnew新的父节点 indexendend

• 新的父节点已经生成，现在需要将xnew插入到树中

 %Step 6: 将x_new插入树T (AddNodeEdge)count = count+1;    %最新节点的索引T.v(count).x = x_new(1);          T.v(count).y = x_new(2); T.v(count).xPrev = T.v(temp_parent).x;     T.v(count).yPrev = T.v(temp_parent).y;T.v(count).totalCost = temp_cost;   %  到达当前点的 costT.v(count).indPrev = temp_parent;     %其父节点x_near的indexl_handle = plot([T.v(count).xPrev, x_new(1)], [T.v(count).yPrev, x_new(2)], 'b', 'Linewidth', 2); %% 线句柄p_handle = plot(x_new(1), x_new(2), 'ko', 'MarkerSize', 4, 'MarkerFaceColor','k'); %% 点句柄pHandleList = [pHandleList p_handle];    %绘图的句柄索引即为countlHandleList = [lHandleList l_handle];pause(DelayTime);

• 最核心的部分，把多余的节点 进行剪枝

%Step 7: 剪枝 (rewire)  核心  %% 不是最小cost的节点for rewire_index = 1:length(nearIndexList)  %%  半径内剪枝if(nearIndexList(rewire_index) ~= temp_parent)    % 若不是之前计算的最小cost的节点newCost = temp_cost + disToNewList(rewire_index);    % 计算neib经过x_new再到起点的代价          if(newCost < T.v(nearIndexList(rewire_index)).totalCost)    % 计算neib经过x_new再到起点的代价小于该点回到起点 需要剪枝x_neib(1) = T.v(nearIndexList(rewire_index)).x;     % 符合剪枝条件节点的坐标x_neib(2) = T.v(nearIndexList(rewire_index)).y;if ~collisionChecking(x_neib,x_new,Imp) continue;   %有障碍物endT.v(nearIndexList(rewire_index)).xPrev = x_new(1);      % 父节点改变为 xnew 对该neighbor信息进行更新T.v(nearIndexList(rewire_index)).yPrev = x_new(2);T.v(nearIndexList(rewire_index)).totalCost = newCost;T.v(nearIndexList(rewire_index)).indPrev = count;       % x_new的索引%delete(pHandleList());%delete(lHandleList(nearIndexList(rewire_index)));lHandleList(nearIndexList(rewire_index)) = plot([T.v(nearIndexList(rewire_index)).x, x_new(1)], [T.v(nearIndexList(rewire_index)).y, x_new(2)], 'r', 'Linewidth', 2);%pHandleList = [pHandleList p_handle];    %绘图的句柄索引即为count%lHandleList = [lHandleList l_handle];endendend

• 终点检测

%Step 8:检查是否到达目标点附近 disToGoal = sqrt((x_new(1) - x_G)^2 + (x_new(2) - y_G)^2);if(disToGoal < GoalThreshold && ~findPath)    % 找到目标点，此条件只进入一次findPath = 1;count = count+1;    %手动将Goal加入到树中Goal_index = count;T.v(count).x = x_G;          T.v(count).y = y_G; T.v(count).xPrev = x_new(1);     T.v(count).yPrev = x_new(2);T.v(count).totalCost = T.v(count - 1).totalCost + disToGoal;T.v(count).indPrev = count - 1;     %其父节点x_near的indexend

• 沿着终点回溯到起点

 if(findPath == 1)update_count = update_count + 1;if(update_count == UpdateTime)  %%  更新路径的间隔update_count = 0;j = 2;path.pos(1).x = x_G; path.pos(1).y = y_G;pathIndex = T.v(Goal_index).indPrev;while 1     path.pos(j).x = T.v(pathIndex).x;  % 回溯path.pos(j).y = T.v(pathIndex).y;pathIndex = T.v(pathIndex).indPrev;    % 沿终点回溯到起点if pathIndex == 0breakendj=j+1;end  for delete_index = 1:length(resHandleList)delete(resHandleList(delete_index));endfor j = 2:length(path.pos)res_handle = plot([path.pos(j).x; path.pos(j-1).x;], [path.pos(j).y; path.pos(j-1).y], 'g', 'Linewidth', 4);resHandleList = [resHandleList res_handle];endendend  pause(DelayTime); %暂停DelayTime s,使得RRT*扩展过程容易观察

• 碰撞检测 确保生成的点 和 生成的路径不在障碍物内

%%  每次迭代一个小步长来判断是否碰撞
function feasible=collisionChecking(startPose,goalPose,map)
feasible=true;
%dir=atan2(goalPose(1)-startPose(1),goalPose(2)-startPose(2));
dir = atan2(goalPose(2)-startPose(2),goalPose(1)-startPose(1));
for r = 0:0.5:sqrt(sum((startPose-goalPose).^2))      %以0.5为步长,从startPose开始递增的检查是否有障碍%posCheck = startPose + r.*[sin(dir) cos(dir)];      %直线距离增加0.5后的坐标posCheck = startPose + r.*[cos(dir) sin(dir)];      %直线距离增加0.5后的坐标%将一个小数(x,y)向4个方向取整,确保该点没有触碰障碍if ~(feasiblePoint(ceil(posCheck),map) && feasiblePoint(floor(posCheck),map) ...&& feasiblePoint([ceil(posCheck(1)) floor(posCheck(2))],map) ...&& feasiblePoint([floor(posCheck(1)) ceil(posCheck(2))],map))feasible = false;break;endif ~feasiblePoint([floor(goalPose(1)),ceil(goalPose(2))],map)feasible = false; end
endfunction feasible = feasiblePoint(point,map)
feasible = true;
if ~(point(1)>=1 && point(1)<=size(map,1) && point(2)>=1 ...&& point(2)<=size(map,2) && map(point(2),point(1))==255)  %% 225  是否是白的feasible = false;   %有障碍
end

• 针对采样方法也可以做出调整，比如在一定区域内采样而不是直接在整个地图内进行均匀采样