基于几何图形搭建障碍物地图的方法(MATLAB)

讲解一下，我的算法，基于几何图形搭建的障碍物地图的算法，一般使用在移动机器人路径规划或者地图构建的领域，算法是由matlab编写的，代码会提供下载链接，关于代码的讲解，现在开始。

1   基于几何图形搭建的障碍物地图的算法构建的障碍物图形如下图1所示：

                                           

                                                                                                图1

其中黑色线条围成的蓝色区域，即为障碍物区域，这些基本的几何图形构成了障碍物区域，本算法主要采用的是线性规划的原理构建的障碍物信息。如下所示，

                              

由这四条 直线构成的图形所围成的区域可以通过我写的Barrier函数完成障碍物区域的构建，你只需要先计算出构成你的障碍物图形的每一条边的直线方程即可，代码如下，

function [numbarrier,quadrantBarrier] = Barrier( x,y,confine) %构建障碍物区域 %输入参数 % x----x轴坐标 % y----y轴坐标 % confine----地图边界 % 输出参数 % orderBarrier----返回改点在第几象限 % numbarrier----该象限第几个障碍物,若numbarrier == 32代表到达地图边界,若numbarrier==0代表不在障碍物区域,numbarrier = 16,代表坐标轴 %%quadrant = 0; numbarrier = 0; quadrantBarrier = 0; %判断点在哪一个象限 if x >= 0 && y >= 0quadrant = 1; elseif x < 0 && y > 0quadrant = 2; elseif x < 0 && y < 0quadrant = 3; elsequadrant = 4; endswitch (quadrant)case 1quadrantBarrier = 1; %第一象限if x <= 0||y <= 0numbarrier = 16;endif x >= confine || y >= confinenumbarrier = 32; endif x-2-y <= 0 && 0.5-y <= 0 && -x+2-y <= 0 && y-x-1 <= 0 && y-2.5 <= 0 && y+x-5 <= 0%第一象限障碍物区，这些方程即为构成该象限的障碍物区域方程，你需要在这里做变动定制成你的直线方程numbarrier = 1;%返回1表示在障碍物区域endcase 2quadrantBarrier = 2; %第二象限if x >= 0||y <= 0numbarrier = 16;endif x <= -confine || y >= confinenumbarrier = 32; end %% % 对于组合图形 1 的描述if x+2.5-y <= 0 && -x-1.5-y <= 0 && y-x/3-13/6 <= 0numbarrier = 1;end % if y+x+1.5 <= 0 && x+4.5-y <= 0 && y-2*x-8.5 <= 0 % numbarrier = 1; % endif -x-1.5-y <= 0 && y-2*x-8.5 <= 0 && y+x+0.5 <= 0 && x/3+13/6-y <= 0numbarrier = 1;end %case 3quadrantBarrier = 3; %第三象限if x >= 0||y >= 0numbarrier = 16;endif x <= -confine || y <= -confinenumbarrier = 32; end %% % 对于组合图形 1 的描述 if x+1 <= 0 && -3.5-x <= 0 && y+1 <= 0 && -1.5-y <= 0numbarrier = 1;endif x+1.5 <= 0 && -3.5-x <= 0 && y+1.5 <= 0 && -2-y <= 0numbarrier = 1;endif x+1.5 <= 0 && -2.5-x <= 0 && y+2 <= 0 && -2.5-y <= 0 numbarrier = 1;end %case 4quadrantBarrier = 4; %第四象限if x <= 0||y >= 0numbarrier = 16;endif x >= confine || y <= -confinenumbarrier = 32; endif 1.5*x-5-y <= 0 && -x-y <=0 && y+0.5 <=0numbarrier = 1;end endend

由以上算法可知，所有在numbarrier=1处的判断条件中的直线方程，你都因该改成你自己构建障碍物的方程。

2    障碍物区域搭建好了，还需要将该区域画出来

通过我写的drawMap函数，输入地图的边界就能画出第1节中的障碍物区域图，和图1的效果一样。代码如下所示，

function drawMap(x_neg,x_pos,y_neg,y_pos) %输入参数： % x_neg----x轴负界 % x_pos----x轴正界 % y_neg----y轴负界 % y_pos----x轴正界%画出y = 0曲线 x0_1 = x_neg:1:x_pos; y0_1 = 0.*x0_1; plot(x0_1,y0_1,'k--');hold on;%画出x = 0曲线 y0_2 = y_neg:1:y_pos; x0_2 = 0.*y0_2; plot(x0_2,y0_2,'k--'); hold on;% scatter(0,0,'MarkerEdgeColor',[1 0 0],'MarkerFaceColor',[1 0 0], 'LineWidth',1);%画出(0,0)%% %绘制障碍物区域 syms x y%描述障碍物不等式组 %第一象限 %% %这也是一种画法，但是很诡异 % v1_1 = cat(6,x-2-y,0.5-y,-x+2-y,y-x-1,y-2.5,y+x-5);%全部化成<=0的不等式 % v1_1 = max(v1_1,[],6); % contourf(x,y,-v1_1,[0,0]); % colormap(display_color); %% [x1_1,y1_1] = solve(x-2-y == 0,0.5-y == 0,x,y); [x1_2,y1_2] = solve(0.5-y == 0,-x+2-y == 0,x,y); [x1_3,y1_3] = solve(-x+2-y == 0,y-x-1 == 0,x,y); [x1_4,y1_4] = solve(y-x-1 == 0,y-2.5 == 0,x,y); [x1_5,y1_5] = solve(y-2.5 == 0,y+x-5 == 0,x,y); [x1_6,y1_6] = solve(x-2-y == 0,y+x-5 == 0,x,y);plot(x1_1,y1_1,x1_2,y1_2,x1_3,y1_3,x1_4,y1_4,'-b',x1_5,y1_5,x1_6,y1_6,'-b');X1_1 = [x1_1,x1_2,x1_3,x1_4,x1_5,x1_6,x1_1]; Y1_1 = [y1_1,y1_2,y1_3,y1_4,y1_5,y1_6,y1_1]; h = fill(X1_1,Y1_1,'c');hold on;%第二象限 %---------------------障碍物2_1的第一部分-------------------------- [x2_1,y2_1] = solve(x+2.5-y== 0,-x-1.5-y == 0,x,y); [x2_2,y2_2] = solve(-x-1.5-y == 0,y-x/3-13/6 == 0,x,y); [x2_3,y2_3] = solve(y-x/3-13/6 == 0,x+2.5-y == 0,x,y);plot(x2_1,y2_1,x2_2,y2_2,x2_3,y2_3);X2_1_1= [x2_1,x2_2,x2_3,x2_1]; Y2_1_1 = [y2_1,y2_2,y2_3,y2_1]; h = fill(X2_1_1,Y2_1_1,'c');%---------------------障碍物2_1的第二部分-------------------------- % [x2_4,y2_4] = solve(y+x+1.5 == 0,x+4.5-y == 0,x,y); % [x2_5,y2_5] = solve(x+4.5-y == 0,y-2*x-8.5 == 0,x,y); % [x2_6,y2_6] = solve(y-2*x-8.5 == 0,y+x+1.5 == 0,x,y); % % plot(x2_4,y2_4,x2_5,y2_5,x2_6,y2_6); % % X2_1_2 = [x2_4,x2_5,x2_6,x2_4]; % Y2_1_2 = [y2_4,y2_5,y2_6,y2_4]; % h = fill(X2_1_2,Y2_1_2,'c'); %---------------------障碍物2_1的第三部分-------------------------- [x2_7,y2_7] = solve(-x-1.5-y == 0,y-2*x-8.5 == 0,x,y); [x2_8,y2_8] = solve(y-2*x-8.5 == 0,y+x+0.5 == 0,x,y); [x2_9,y2_9] = solve(y+x+0.5 == 0,x/3+13/6-y == 0,x,y); [x2_10,y2_10] = solve(x/3+13/6-y == 0,-x-1.5-y == 0,x,y);plot(x2_7,y2_7,x2_8,y2_8,x2_9,y2_9,x2_10,y2_10);X2_1_3 = [x2_7,x2_8,x2_9,x2_10,x2_7]; Y2_1_3 = [y2_7,y2_8,y2_9,y2_10,y2_7]; h = fill(X2_1_3,Y2_1_3,'c');% %第三象限% -----------------障碍物3_1第一部分---------- [x3_1,y3_1] = solve(x+1 == 0,y+1 == 0,x,y); [x3_2,y3_2] = solve(y+1 == 0,3.5+x == 0,x,y); [x3_3,y3_3] = solve(3.5+x == 0,1.5+y == 0,x,y); [x3_4,y3_4] = solve(1.5+y == 0,x+1 == 0,x,y);plot(x3_1,y3_1,x3_2,y3_2,x3_3,y3_3,x3_4,y3_4);X3_1_1 = [x3_1,x3_2,x3_3,x3_4,x3_1]; Y3_1_1 = [y3_1,y3_2,y3_3,y3_4,y3_1]; h = fill(X3_1_1,Y3_1_1,'c');% -----------------障碍物3_1第二部分---------- [x3_5,y3_5] = solve(x+1.5 == 0,y+1.5 == 0,x,y); [x3_6,y3_6] = solve(y+1.5 == 0,3.5+x == 0,x,y); [x3_7,y3_7] = solve(3.5+x == 0,2+y == 0,x,y); [x3_8,y3_8] = solve(2+y == 0,x+1.5 == 0,x,y);plot(x3_5,y3_5,x3_6,y3_6,x3_7,y3_7,x3_8,y3_8);X3_1_2 = [x3_5,x3_6,x3_7,x3_8,x3_5]; Y3_1_2 = [y3_5,y3_6,y3_7,y3_8,y3_5]; h = fill(X3_1_2,Y3_1_2,'c');% -----------------障碍物3_1第三部分---------- [x3_9,y3_9] = solve(x+1.5 == 0,y+2 == 0,x,y); [x3_10,y3_10] = solve(y+2 == 0,2.5+x == 0,x,y); [x3_11,y3_11] = solve(2.5+x == 0,2.5+y == 0,x,y); [x3_12,y3_12] = solve(2.5+y == 0,x+1.5 == 0,x,y);plot(x3_9,y3_9,x3_10,y3_10,x3_11,y3_11,x3_12,y3_12);X3_1_3 = [x3_9,x3_10,x3_11,x3_12,x3_9]; Y3_1_3 = [y3_9,y3_10,y3_11,y3_12,y3_9]; h = fill(X3_1_3,Y3_1_3,'c');% 第四象限 %4-1代表第四象限第一个障碍物，以此类推 [x4_1,y4_1] = solve(1.5*x-5-y == 0,-x-y == 0,x,y); [x4_2,y4_2] = solve(-x-y == 0,y+0.5 == 0,x,y); [x4_3,y4_3] = solve(1.5*x-5-y == 0,y+0.5 == 0,x,y);plot(x4_1,y4_1,x4_2,y4_2,x4_3,y4_3);X4_1 = [x4_1,x4_2,x4_3,x4_1]; Y4_1 = [y4_1,y4_2,y4_3,y4_1]; h = fill(X4_1,Y4_1,'c'); axis([x_neg x_pos y_neg y_pos]);%画出象限图 end

上面的代码区域，关于我注释掉的代码区域，可以不予理会，其中主要用到两个主要的matlab函数完成的绘图，solve函数和fill函数，solve函数是用来求解两条直线方程的，fill函数是用来完成直线函数围成区域之间的颜色填充的，我使用的是蓝色。你可以使用doc solve和doc fill来查看matlab对于这两个函数的详细解释。另外plot，和scatter函数也是必不可少的，plot用来画图，scatter函数我用来描绘具体的点。

你需要结合你的障碍物图形，将solve函数中求解的两条直线方程替换掉，注意之两条直线方程必须要有交点才行！！！

3    障碍物搜索的算法实现

现在障碍物的环境已经构建完成，我们需要让算法具备搜索障碍物的能力，我把它写成了 searchBarrier函数，这个函数的主要功能可以理解成超声波传感器，会在可见的视野区域内反馈障碍物的点集，代码如下，

function [ Bexist,crosspiont,num,other_crosspoint ] = searchBarrier( xnext,ynext,beta1,beta2,r,confine ) %输入参数： % xnext,ynext----机器人下一时刻位置 % beta1,beta2----机器人视野范围大小 % r----机器人视野长度，半径 % confine----地图边界值 % 输出参数： % Bexist----当前位置是否检测到障碍物,0--没有，1--有 % crosspiont----搜索区域与障碍物区域的边界交点坐标 %%测试时使用 % quaBarrier----返回改点在第几象限 % num----该象限第几个障碍物,若numbarrier == 32代表到达地图边界,若numbarrier ==0代表不在障碍物区域 %% %机器人探测区域 %将圆形区域与所有该坐标系的曲线求解，若解在Barrier区域且同时在搜索区域则，表示探测到障碍物 Bexist = 0; crosspiont = []; count = 1; other_crosspoint = []; count_other = 1; quadrant = 0; quaBarrier = 0; num = 0;%判断点在哪一个象限 if xnext >= 0 && ynext >= 0quadrant = 1; elseif xnext < 0 && ynext > 0quadrant = 2; elseif xnext < 0 && ynext < 0quadrant = 3; elsequadrant = 4; endsyms x y eqn = (x-xnext)^2+(y-ynext)^2 == r^2; %% %第一象限所有边界曲线 if quadrant == 1eqn1_1(1) = x-confine == 0; %地图边界1eqn1_1(2) = y-confine == 0; %地图边界2eqn1_1(3) = x-2-y == 0; eqn1_1(4) = 0.5-y == 0; eqn1_1(5) = -x+2-y ==0; eqn1_1(6) = y-x-1 == 0; eqn1_1(7) = y-2.5 == 0; eqn1_1(8) = y+x-5 == 0; eqn1_1(9) = y == 0; eqn1_1(10) = x == 0; end %第二象限所有边界曲线 if quadrant == 2eqn2_1(1) = x+confine == 0; %地图边界1eqn2_1(2) = y-confine == 0; %地图边界2eqn2_1(3) = x+2.5-y == 0; eqn2_1(4) = -x-1.5-y == 0; eqn2_1(5) = y-x/3-13/6 ==0; % eqn2_1(6) = x+4.5-y == 0; eqn2_1(6) = y-2*x-8.5 == 0; eqn2_1(7) = y+x+0.5 == 0; eqn2_1(8) = y == 0; eqn2_1(9) = x == 0; end %第三象限所有边界曲线 if quadrant == 3eqn3_1(1) = x+confine == 0; %地图边界1eqn3_1(2) = y+confine == 0; %地图边界2eqn3_1(3) = x+1 == 0; eqn3_1(4) = x+3.5 == 0; eqn3_1(5) = y+1 ==0; eqn3_1(6) = y+1.5 == 0; eqn3_1(7) = x+1.5 == 0; eqn3_1(8) = y+2 == 0; eqn3_1(9) = x+2.5 == 0; eqn3_1(10) = y+2.5 == 0; eqn3_1(11) = y == 0; eqn3_1(12) = x == 0; end %第四象限所有边界曲线 if quadrant == 4eqn4_1(1) = x-confine == 0; %地图边界1eqn4_1(2) = y+confine == 0; %地图边界2eqn4_1(3) = 1.5*x-5-y == 0; eqn4_1(4) = -x-y == 0; eqn4_1(5) = y+0.5 ==0; eqn4_1(6) = y == 0; eqn4_1(7) = x == 0; end %% %按象限匹配 switch quadrantcase 1%==========================================================================n = length(eqn1_1);for i = 1:1:n[sx,sy] = solve(eqn,eqn1_1(i),x,y,'Real',true);%只求实数解if ~isempty(sx)%有交点%判断交点是否在Barrier中n1 = length(sx);%确认有几个交点for j = 1:1:n1[numbarrier,quadrantBarrier] = Barrier(sx(j),sy(j),confine);% if sx(j) < 0 || sy(j) < 0 || sx(j) > confine || sy(j) > confine%说明点不在第一象限地图区域内,那么结束当前循环 % continue; % endif sx(j) < -confine || sy(j) < -confine || sx(j) > confine || sy(j) > confine%说明点不在地图区域内,那么结束当前循环continue;end if numbarrier ~=0%在障碍物空间中%还得判断是否在视野范围中beta = asin((sy(j)-ynext)/sqrt((sx(j)-xnext)^2+(sy(j)-ynext)^2));%sin(beta) = y-ynext/(sqrt(x-xnext)^2+(y-ynext)^2) 得到的是弧度值%因为asin只能得出 -90 <= beta <= 90之间的角度，所以还需要细化判断if sx(j)-xnext <0 && sy(j)-ynext > 0%说明在第二象限beta = beta+pi/2;endif sx(j)-xnext < 0 && sy(j)-ynext < 0%说明在第三象限beta = beta-pi/2;end%%%当x=0或y=0时，会出现奇异性要单独讨论if sy(j)-ynext ==0if sx(j)-xnext > 0beta = 0;elseif sx(j)-xnext < 0beta = pi;endendif (beta >= beta1 && beta <= beta2) || ((beta-2*pi) >= beta1 && (beta-2*pi) <= beta2) || ( (2*pi+beta) >= beta1 && (2*pi+beta) <= beta2 )%因为一个角度有两种表达方式，在-270 <= beta <= 270范围中Bexist = 1;if sx(j) < 0 || sy(j) < 0 || sx(j) > confine || sy(j) > confine%说明点不在第一象限地图区域内,那么结束当前循环other_crosspoint(:,count_other) = [sx(j);sy(j)];count_other = count_other+1;continue;endcrosspiont(:,count) = [sx(j);sy(j)];num(count) = numbarrier;count = count+1;quaBarrier = quadrant; endendendendend %=========================================================================== %===========================================================================case 2n = length(eqn2_1);for i = 1:1:n[sx,sy] = solve(eqn,eqn2_1(i),x,y,'Real',true);%只求实数解if ~isempty(sx)%有交点%判断交点是否在Barrier中n1 = length(sx);%确认有几个交点for j = 1:1:n1[numbarrier,quadrantBarrier] = Barrier(sx(j),sy(j),confine);if sx(j) > 0 || sy(j) < 0 || sx(j) < -confine || sy(j) > confine%说明点不在第一象限,那么结束当前循环continue;endif numbarrier ~=0%在障碍物空间中%还得判断是否在视野范围中beta = asin((sy(j)-ynext)/sqrt((sx(j)-xnext)^2+(sy(j)-ynext)^2));%sin(beta) = y-ynext/(sqrt(x-xnext)^2+(y-ynext)^2) 得到的是弧度值%因为asin只能得出 -90 <= beta <= 90之间的角度，所以还需要细化判断if sx(j)-xnext <0 && sy(j)-ynext > 0%说明在第二象限beta = beta+pi/2;endif sx(j)-xnext < 0 && sy(j)-ynext < 0%说明在第三象限beta = beta-pi/2;end%%%当x=0或y=0时，会出现奇异性要单独讨论if sy(j)-ynext ==0if sx(j)-xnext > 0beta = 0;elseif sx(j)-xnext < 0beta = pi;endendif beta >= beta1 && beta <= beta2 || ((beta-2*pi) >= beta1 && (beta-2*pi) <= beta2) || ( (2*pi+beta) >= beta1 && (2*pi+beta) <= beta2 )%因为一个角度有两种表达方式，在-270 <= beta <= 270范围中Bexist = 1;crosspiont(:,count) = [sx(j);sy(j)];num(count) = numbarrier;count = count+1;quaBarrier = quadrant;endendendendendcase 3n = length(eqn3_1);for i = 1:1:n[sx,sy] = solve(eqn,eqn3_1(i),x,y,'Real',true);%只求实数解if ~isempty(sx)%有交点%判断交点是否在Barrier中n1 = length(sx);%确认有几个交点for j = 1:1:n1[numbarrier,quadrantBarrier] = Barrier(sx(j),sy(j),confine);if sx(j) > 0 || sy(j) > 0 || sx(j) < -confine || sy(j) < -confine%说明点不在第一象限,那么结束当前循环continue;endif numbarrier ~=0%在障碍物空间中%还得判断是否在视野范围中beta = asin((sy(j)-ynext)/sqrt((sx(j)-xnext)^2+(sy(j)-ynext)^2));%sin(beta) = y-ynext/(sqrt(x-xnext)^2+(y-ynext)^2) 得到的是弧度值%因为asin只能得出 -90 <= beta <= 90之间的角度，所以还需要细化判断if sx(j)-xnext < 0 && sy(j)-ynext > 0%说明在第二象限beta = beta+pi/2;endif sx(j)-xnext < 0 && sy(j)-ynext < 0%说明在第三象限beta = beta-pi/2;end%%%当x=0或y=0时，会出现奇异性要单独讨论if sy(j)-ynext ==0if sx(j)-xnext > 0beta = 0;elseif sx(j)-xnext < 0beta = pi;endendif beta >= beta1 && beta <= beta2 || ((beta-2*pi) >= beta1 && (beta-2*pi) <= beta2) || ( (2*pi+beta) >= beta1 && (2*pi+beta) <= beta2 )%因为一个角度有两种表达方式，在-270 <= beta <= 270范围中Bexist = 1;crosspiont(:,count) = [sx(j);sy(j)];num(count) = numbarrier;count = count+1;quaBarrier = quadrant;endendendendendcase 4n = length(eqn4_1);for i = 1:1:n[sx,sy] = solve(eqn,eqn4_1(i),x,y,'Real',true);%只求实数解if ~isempty(sx)%有交点%判断交点是否在Barrier中n1 = length(sx);%确认有几个交点for j = 1:1:n1[numbarrier,quadrantBarrier] = Barrier(sx(j),sy(j),confine);if sx(j) < 0 || sy(j) > 0 || sx(j) > confine || sy(j) < -confine%说明点不在第四象限,那么结束当前循环continue;endif numbarrier ~=0%在障碍物空间中%还得判断是否在视野范围中beta = asin((sy(j)-ynext)/sqrt((sx(j)-xnext)^2+(sy(j)-ynext)^2));%sin(beta) = y-ynext/(sqrt(x-xnext)^2+(y-ynext)^2) 得到的是弧度值%因为asin只能得出 -90 <= beta <= 90之间的角度，所以还需要细化判断if sx(j)-xnext < 0 && sy(j)-ynext > 0%说明在第二象限beta = beta+pi/2;endif sx(j)-xnext < 0 && sy(j)-ynext < 0%说明在第三象限beta = beta-pi/2;end%%%当x=0或y=0时，会出现奇异性要单独讨论if sy(j)-ynext ==0if sx(j)-xnext > 0beta = 0;elseif sx(j)-xnext < 0beta = pi;endendif ( beta >= beta1 && beta <= beta2 ) || ( (beta-2*pi) >= beta1 && (beta-2*pi) <= beta2 ) || ( (2*pi+beta) >= beta1 && (2*pi+beta) <= beta2 ) %因为一个角度有两种表达方式，在-270 <= beta <= 270范围中Bexist = 1;crosspiont(:,count) = [sx(j);sy(j)];num(count) = numbarrier;count = count+1;quaBarrier = quadrant;endendendendendendend

 这个函数相对更加复杂一些，然而对于你的障碍物信息，你只需要关注于每一个quadrant后的eqn1_1，eqn2_1，eqn3_1，eqn4_1这几个数组中存放的地图边界，虚拟xoy坐标系和你的障碍物构成的直线方程即可，其他各种运动过程中的转角，角度变换等等的逻辑我都在switch-case语句中处理好了，你只需要关注于设计你的障碍物直线方程即可。

注意，对于移动机器人的视野范围beta1,beta2，要始终满足beta1<= beta2！！！

4    至此你所需要的MATLAB平台下的障碍物环境已经搭建好了，而且可以搜索障碍物信息了

你现在需要让你的移动机器人在运动的过程中，不断的预测下一时刻行走的位置是否处于障碍物之中，或者很靠近障碍物，这些位置都是十分危险的位置，很容易发生碰撞，因此，我设计了一个简单的评估移动机器人行走安全的评价函数EvaSafe，代码如下，

function [ IsMysafe ] = EvaSafe( robotPosition,ii,v,dt,seta,aifa,quadrant,confine,safe_level ) %输入参数： % robotPosition----机器人位置集合，用来记录走过的位置，防止重复行走 % robotPosition为2*n矩阵，第一行为x轴位置，第二行为对应的y轴数据 % ii----机器人第几个位置数据,即robotPosition中的列值 % v----机器人运动速度 % dt----采样时间 % seta----速度方向与世界坐标系夹角 -180 <= seta <= 180 % aifa----机器人视野范围 % quadrant----第几象限的agent % confine----地图边界值 % safe_level----机器人路线安全等级，对应预测的安全位置个数---- safe_level = n*v*dt=n*v; %输出参数： % IsMysafe----是否安全,在不跳出之前象限区域限制的前提下。0----safe,1----dangrousIsMysafe = 0; numbarrier = 0; over_quadrant = 0; % xnext = []; % ynext = []; % position = robotPosition(:,ii); xnext = robotPosition(1,ii); ynext = robotPosition(2,ii); % [ xnext,ynext,beta1,beta2 ] = RobotMoving( position,i,v,dt,seta,aifa ); %将无限接近0的数据归零化 if abs(xnext - 0) < 0.0001 xnext = 0; endif abs(ynext - 0) < 0.0001 ynext = 0; endxnext = xnext+safe_level*cos(seta); ynext = ynext+safe_level*sin(seta);%将无限接近0的数据归零化 if abs(xnext - 0) < 0.0001 xnext = 0; endif abs(ynext - 0) < 0.0001 ynext = 0; end %% %================================================================ %判断预测位置是否在障碍物区域 [numbarrier,quadrantBarrier] = Barrier( xnext,ynext,confine); %================================================================ %判断点在哪一个象限 if xnext >= 0 && ynext >= 0flag_quadrant = 1; elseif xnext < 0 && ynext > 0flag_quadrant = 2; elseif xnext < 0 && ynext < 0flag_quadrant = 3; elseflag_quadrant = 4; end %超过象限区域 if flag_quadrant ~= quadrantover_quadrant = 1; end %============================================================== %判断该位置是否走过if numbarrier ~= 0 || over_quadrant ~= 0IsMysafe = 1; endend

在函数的注释中，所有的输入/输出参数，解释的很清楚，这里就不再赘述了。

关于算法的具体实现，你可以在这里下载,

link:https://download.csdn.net/download/simileciwh/10777849

 

至此你可以，应用自己的算法到这个障碍物环境中去，验证自己设计的路径规划算法或者地图构建算法的性能和效果了。

总结

以上是生活随笔为你收集整理的基于几何图形搭建障碍物地图的方法(MATLAB)的全部内容，希望文章能够帮你解决所遇到的问题。

如果觉得生活随笔网站内容还不错，欢迎将生活随笔推荐给好友。