讲解一下，我的算法，基于几何图形搭建的障碍物地图的算法，一般使用在移动机器人路径规划或者地图构建的领域，算法是由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 >= 0
quadrant = 1;
elseif x < 0 && y > 0
quadrant = 2;
elseif x < 0 && y < 0
quadrant = 3;
else
quadrant = 4;
end
switch (quadrant)
case 1
quadrantBarrier = 1; %第一象限
if x <= 0||y <= 0
numbarrier = 16;
end
if x >= confine || y >= confine
numbarrier = 32;
end
if 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表示在障碍物区域
end
case 2
quadrantBarrier = 2; %第二象限
if x >= 0||y <= 0
numbarrier = 16;
end
if x <= -confine || y >= confine
numbarrier = 32;
end
%%
% 对于组合图形 1 的描述
if x+2.5-y <= 0 && -x-1.5-y <= 0 && y-x/3-13/6 <= 0
numbarrier = 1;
end
% if y+x+1.5 <= 0 && x+4.5-y <= 0 && y-2*x-8.5 <= 0
% numbarrier = 1;
% end
if -x-1.5-y <= 0 && y-2*x-8.5 <= 0 && y+x+0.5 <= 0 && x/3+13/6-y <= 0
numbarrier = 1;
end
%
case 3
quadrantBarrier = 3; %第三象限
if x >= 0||y >= 0
numbarrier = 16;
end
if x <= -confine || y <= -confine
numbarrier = 32;
end
%%
% 对于组合图形 1 的描述
if x+1 <= 0 && -3.5-x <= 0 && y+1 <= 0 && -1.5-y <= 0
numbarrier = 1;
end
if x+1.5 <= 0 && -3.5-x <= 0 && y+1.5 <= 0 && -2-y <= 0
numbarrier = 1;
end
if x+1.5 <= 0 && -2.5-x <= 0 && y+2 <= 0 && -2.5-y <= 0
numbarrier = 1;
end
%
case 4
quadrantBarrier = 4; %第四象限
if x <= 0||y >= 0
numbarrier = 16;
end
if x >= confine || y <= -confine
numbarrier = 32;
end
if 1.5*x-5-y <= 0 && -x-y <=0 && y+0.5 <=0
numbarrier = 1;
end
end
end
由以上算法可知，所有在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 >= 0
quadrant = 1;
elseif xnext < 0 && ynext > 0
quadrant = 2;
elseif xnext < 0 && ynext < 0
quadrant = 3;
else
quadrant = 4;
end
syms x y
eqn = (x-xnext)^2+(y-ynext)^2 == r^2;
%%
%第一象限所有边界曲线
if quadrant == 1
eqn1_1(1) = x-confine == 0; %地图边界1
eqn1_1(2) = y-confine == 0; %地图边界2
eqn1_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 == 2
eqn2_1(1) = x+confine == 0; %地图边界1
eqn2_1(2) = y-confine == 0; %地图边界2
eqn2_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 == 3
eqn3_1(1) = x+confine == 0; %地图边界1
eqn3_1(2) = y+confine == 0; %地图边界2
eqn3_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 == 4
eqn4_1(1) = x-confine == 0; %地图边界1
eqn4_1(2) = y+confine == 0; %地图边界2
eqn4_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 quadrant
case 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;
% end
if 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;
end
if sx(j)-xnext < 0 && sy(j)-ynext < 0%说明在第三象限
beta = beta-pi/2;
end
%%
%当x=0或y=0时，会出现奇异性要单独讨论
if sy(j)-ynext ==0
if sx(j)-xnext > 0
beta = 0;
elseif sx(j)-xnext < 0
beta = pi;
end
end
if (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;
end
crosspiont(:,count) = [sx(j);sy(j)];
num(count) = numbarrier;
count = count+1;
quaBarrier = quadrant;
end
end
end
end
end
%===========================================================================
%===========================================================================
case 2
n = 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;
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;
end
if sx(j)-xnext < 0 && sy(j)-ynext < 0%说明在第三象限
beta = beta-pi/2;
end
%%
%当x=0或y=0时，会出现奇异性要单独讨论
if sy(j)-ynext ==0
if sx(j)-xnext > 0
beta = 0;
elseif sx(j)-xnext < 0
beta = pi;
end
end
if 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;
end
end
end
end
end
case 3
n = 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;
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;
end
if sx(j)-xnext < 0 && sy(j)-ynext < 0%说明在第三象限
beta = beta-pi/2;
end
%%
%当x=0或y=0时，会出现奇异性要单独讨论
if sy(j)-ynext ==0
if sx(j)-xnext > 0
beta = 0;
elseif sx(j)-xnext < 0
beta = pi;
end
end
if 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;
end
end
end
end
end
case 4
n = 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;
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;
end
if sx(j)-xnext < 0 && sy(j)-ynext < 0%说明在第三象限
beta = beta-pi/2;
end
%%
%当x=0或y=0时，会出现奇异性要单独讨论
if sy(j)-ynext ==0
if sx(j)-xnext > 0
beta = 0;
elseif sx(j)-xnext < 0
beta = pi;
end
end
if ( 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;
end
end
end
end
end
end
end
这个函数相对更加复杂一些，然而对于你的障碍物信息，你只需要关注于每一个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----dangrous
IsMysafe = 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;
end
if abs(ynext - 0) < 0.0001
ynext = 0;
end
xnext = xnext+safe_level*cos(seta);
ynext = ynext+safe_level*sin(seta);
%将无限接近0的数据归零化
if abs(xnext - 0) < 0.0001
xnext = 0;
end
if abs(ynext - 0) < 0.0001
ynext = 0;
end
%%
%================================================================
%判断预测位置是否在障碍物区域
[numbarrier,quadrantBarrier] = Barrier( xnext,ynext,confine);
%================================================================
%判断点在哪一个象限
if xnext >= 0 && ynext >= 0
flag_quadrant = 1;
elseif xnext < 0 && ynext > 0
flag_quadrant = 2;
elseif xnext < 0 && ynext < 0
flag_quadrant = 3;
else
flag_quadrant = 4;
end
%超过象限区域
if flag_quadrant ~= quadrant
over_quadrant = 1;
end
%==============================================================
%判断该位置是否走过
if numbarrier ~= 0 || over_quadrant ~= 0
IsMysafe = 1;
end
end
在函数的注释中，所有的输入/输出参数，解释的很清楚，这里就不再赘述了。
关于算法的具体实现，你可以在这里下载,
link:https://download.csdn.net/download/simileciwh/10777849
至此你可以，应用自己的算法到这个障碍物环境中去，验证自己设计的路径规划算法或者地图构建算法的性能和效果了。
9194
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
