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

最新推荐文章于 2024-08-07 20:51:33 发布

SimileciWH

最新推荐文章于 2024-08-07 20:51:33 发布

阅读量1.3w

点赞数 18

分类专栏： algorithm 文章标签： matlab 移动机器人构建障碍物图

本文链接：https://blog.csdn.net/SimileciWH/article/details/83958617

版权

讲解一下，我的算法，基于几何图形搭建的障碍物地图的算法，一般使用在移动机器人路径规划或者地图构建的领域，算法是由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