Velodyne 激光雷达每帧有十几万个三维数据点,对于有限大小的地图来说,大多数栅格内的实际三维点个数必然不止一个(近距离点数较多,越远越少) , 所以我们首先建立一张栅格地图,再将当前帧内的三维点投影到栅格地图上,每个栅格内有若干点,这些点保留高度、强度等信息,最后根据这些点计算该栅格的属性。首先,建立一张栅格地图大小为前方(FOV_FX)为4m,后方(FOV_BX)-10m,左方(FOV_LY)-10m,右方(FOV_RY) 4m的一张栅格地图,其中栅格的宽度(Grid_L)为 0.2m,高度或长度(Grid_H)为0.2m。
栅格属性的判定:经过投影之后,地图上每个栅格包含三维点个数,最大高度,最小高度等信息。最后遍历每个栅格,判断该栅格的属性。属性分为可通行(白色区域),不可通行区域(黑色区域)。其中可通行有平坦地面,坡面等,不可通行类指一般影响自主车安全行驶的障碍物,未知区域表示雷达检测盲点。具体算法如下表所示:
本文依据该方法利用Matlab编写程序仿真
其中LIDAR.m为主程序,Velodyne-HDL-32-Data1.csv为HDL-32雷达的输出数据,运行LIDAR.m既可以把Velodyne-HDL-32-Data1.csv文件中的三维雷达数据转化为障碍物图。
完整的程序包链接:https://download.csdn.net/download/qq_34911636/13741240
LIDAR.m代码如下所示:
clear;
clc;
fid=csvread('C:\Users\Administrator\Desktop\障碍物检测采用高度差-雷达数据\Velodyne-HDL-32-Data1.csv',1,3,[1,3,66000,5]);
%[C, position]=textscan(fid,'%f');
%fclose(fid);
%data = textscan(Fid,'float');
fid(:,1);
[x,y] = meshgrid(-10:0.2:4,-10:0.2:4);
z=ones(71)*-4;
%Grid_Pretreatment函数依据高度差法判定当前位置是否有障碍物,然后输出
%一幅二值化的图像,图像中的黑色表示障碍物,变色区域表示可通行区域
[Grid_Array,Grid_Struct] = Grid_Pretreatment(0.2,0.2,4,-10,-10,4);
figure
plot3(fid(:,1),fid(:,2),fid(:,3),'b.','MarkerSize',10);
% hold on;
% plot3(x,y,z,'r.','LineStyle','-');
% hold on
% plot3(fid(49570,1),fid(49570,2),fid(49570,3),'r.','MarkerSize',30);
% plot3(x,y,z,'r.','MarkerSize',2);
grid on
%disp(C);
Grid_Pretreatment.m代码如下所示:
function [Grid_Array,Grid_Struct] = Grid_Pretreatment(Grid_L,Grid_H,FOV_FX,FOV_BX,FOV_LY,FOV_RY)
%FOV_FX,表示视场前方的长度,为正数
%FOV_BX,表示视场后方的长度,为负数
%FOV_LY,表示视场左方的长度,为负数
%FOV_RY,表示视场右方的长度,为正数
%Grid_H,表示栅格的高度或长度
%Grid_L,表示栅格的宽度
fid = csvread('C:\Users\Administrator\Desktop\障碍物检测采用高度差-雷达数据\Velodyne-HDL-32-Data1.csv',1,3,[1,3,66000,5]);
disp(fid(3,2));
xcolumn = size(fid,1); %读取X轴数据的列数
ycolumn = size(fid,2); %读取Y轴数据的列数
zcolumn = size(fid,3); %读取Z轴数据的列数
middlecolumn = max(xcolumn,ycolumn); %读取xyz三轴的最小列数
maxcolumn = max(middlecolumn,zcolumn);
disp(maxcolumn);
%检查参数的合法性
if((Grid_L<=0)&&(Grid_H<=0))
end
if((FOV_FX<=Grid_H/2)&&(FOV_BX>=-Grid_H/2)&&(FOV_LY>=-Grid_L/2)&&(FOV_RY<=Grid_L/2))
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%结构的的初始化
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Grid_Property = struct(Obstacle_Property,Coordinate_XYZ,MAXH,MINH,Origin);
Grid_Property.Obstacle_Property = 0; %用于标识当前网格为障碍物的变量
Grid_Property.Coordinate_XYZ = 0; %用于存储当前网格点云的
Grid_Property.MAXH = 0; %存储当前网格z轴点云的最大值
Grid_Property.MINH = 0; %存储当前网格z轴点云的最小值
Grid_Property.Origin = 0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%障碍物检测边界的取整,从根据函数的传入值,设定要扫描雷达数据的区域,并求取网格
%的数目和边界,这边设定网格的原点位于中心位置
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Range_LY_Num = floor((FOV_LY+Grid_L/2)/Grid_L); %求取y左半轴的网格数目
Range_LY = (floor((FOV_LY+Grid_L/2)/Grid_L))*Grid_L-Grid_L/2; %求取y左半轴的边界
Range_RY_Num = ceil((FOV_RY-Grid_L/2)/Grid_L); %求取y右半轴的网格数目
Range_RY = (ceil((FOV_RY-Grid_L/2)/Grid_L))*Grid_L+Grid_L/2; %求取y右半轴的边界
Range_FX_Num = floor((FOV_FX-Grid_H/2)/Grid_H); %求取x后半轴的网格数目
Range_FX = (floor((FOV_FX-Grid_H/2)/Grid_H))*Grid_H+Grid_H/2; %求取x后左半轴的边界
Range_BX_Num = ceil((FOV_BX+Grid_H/2)/Grid_H); %求取x前右半轴的网格数目
Range_BX = (ceil((FOV_BX+Grid_H/2)/Grid_H))*Grid_H-Grid_H/2; %求取x前右半轴的边界
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Region_Array = []; %用于咱存放某个网格点云数据
Region_Array_Column = []; %用于暂记录落在当前栅格中点云数据的下标
Pseudo_Image = [];%zeros(-Range_BX_Num+Range_FX_Num,-Range_LY_Num+Range_RY_Num);
xa = 1; %这边的xa和ya变量用于对应栅格的坐标,这边的坐标用于把栅格的信息映射到最终输出的障碍物图像Pseudo_Image上
ya = 1;
for ystep = Range_LY:Grid_L:(Range_RY-Grid_L) %y轴方向的取值,以栅格的宽度步进
for xstep = Range_BX:Grid_H:(Range_FX-Grid_H) %x轴方向的取值,以栅格的宽度步进
% disp(ystep);
% disp(xstep);
Region_Array = [];
Region_Array_Column = [];
i = 1; %i参数统计落在当前栅格中的点云数
for Datacolumn = 1:maxcolumn %for循环遍历整个点云数据,通过遍历法搜索落在当前栅格中的点云数据
if((fid(Datacolumn,2)>=ystep)&&(fid(Datacolumn,1)>=xstep))
if((fid(Datacolumn,2)<(ystep+Grid_L))&&(fid(Datacolumn,1)<(xstep+Grid_H)))
Region_Array(i) = fid(Datacolumn,3); %数组Region_Array把落在当前栅格中的点云数据暂存起来
Region_Array_Column(i) = Datacolumn; %Region_Array_Column记录落在当前栅格中点云数据的下标
i = i+1; %这边的i变量记录了当前网格有多少个点云数据
end
end
end
i = i-1; %i减1表示没有点云数据落在相应的区域中,如果大于1表示有点云数据落在相应区域
if(i>1)
OutPutCoordinate = LengthToCoordinate(xstep,ystep,Grid_L,Grid_H,FOV_FX,FOV_BX,FOV_LY,FOV_RY);
if((abs(max(Region_Array)-min(Region_Array))>0.5)) %通过判断落在当前栅格中点云数据的最大与最小高度之差 来判断是否为障碍物,若高度差大于0.1米则归为障碍物
Grid_Property(OutPutCoordinate(1),OutPutCoordinate(2)).MAXH = max(Region_Array);%把当前最大的当云数据存到结构体中
Grid_Property(OutPutCoordinate(1),OutPutCoordinate(2)).MINH = min(Region_Array);%把当前最小的当云数据存到结构体中
Grid_Property(OutPutCoordinate(1),OutPutCoordinate(2)).Obstacle_Property = 255; %障碍物标识码,255表示障碍物,0表示非障碍物
Pseudo_Image(xa,ya) = 0; %把栅格转化为二值图像,Pseudo_Image存储二值图像,255表示障碍物,0表示非障碍物
% Pseudo_Image(xstep+abs(Range_BX)+Range_FX,ystep+abs(Range_LX)+Range_RX) = 255;
end
if((abs(max(Region_Array)-min(Region_Array))<=0.5)) %通过判断落在当前栅格中点云数据的最大与最小高度之差来判断是否为障碍物,若高度差大于0.1米则归为障碍物
%disp(Region_Array);
%disp(max(Region_Array));
%disp(min(Region_Array));
%disp(min(Region_Array));
Grid_Property(OutPutCoordinate(1),OutPutCoordinate(2)).MAXH = max(Region_Array);%把当前最大的当云数据存到结构体中
Grid_Property(OutPutCoordinate(1),OutPutCoordinate(2)).MINH = min(Region_Array);%把当前最小的当云数据存到结构体中
Grid_Property(OutPutCoordinate(1),OutPutCoordinate(2)).Obstacle_Property = 0; %障碍物标识码,255表示障碍物,0表示非障碍物
Pseudo_Image(xa,ya) = 255; %把栅格转化为二值图像,Pseudo_Image存储二值图像,255表示障碍物,0表示非障碍物
% Pseudo_Image(xstep+abs(Range_BX)+Range_FX,ystep+abs(Range_LX)+Range_RX) = 255;
end
for j = 1:1:i %把落在当前栅格中的点云数据存到结构体中
Grid_Property(OutPutCoordinate(1),OutPutCoordinate(2)).Coordinate_XYZ(j,1) = fid(Region_Array_Column(j),1);%x轴数据
Grid_Property(OutPutCoordinate(1),OutPutCoordinate(2)).Coordinate_XYZ(j,2) = fid(Region_Array_Column(j),2);%y轴数据
Grid_Property(OutPutCoordinate(1),OutPutCoordinate(2)).Coordinate_XYZ(j,3) = fid(Region_Array_Column(j),3);%z轴数据
end
end
if(i<=1)
Pseudo_Image(xa,ya) = 100;
%Pseudo_Image(xstep+abs(Range_BX)+Range_FX,ystep+abs(Range_LX)+Range_RX) = 0;
end
xa = xa+1; %xa ya相当于采用列扫描的形式,ya从左往右扫描,每循环一次xa重置为1开始从上往下扫描
end
xa = 1; %xa重置为1,列扫描从上往下
ya = ya+1;
end
Grid_Array = Pseudo_Image; %函数返回值 输出图像
Grid_Struct = Grid_Property; %函数返回值 输出网格信息
figure;
imshow(Pseudo_Image); %输出图像信息
disp(Pseudo_Image); %命令行中显示图像像素信息
end
LengthToCoordinate.m代码如下所示:
function [OutPutCoordinate] = LengthToCoordinate(x,y,Grid_L,Grid_H,FOV_FX,FOV_BX,FOV_LY,FOV_RY)
Now_Range_X_Num = 0;
Now_Range_Y_Num = 0;
%LengthToCoordinate求取出网格对应的像素点,我们把雷达数据变成在平面上网格化
%然后根据落在当前网格的三维雷达数据进行对比和分析,如果z轴上的雷达数据高度差
%大于某一个阈值则把当前网格判定为障碍物点。为了后期方便处理我们把上述的网格
%信息图像化,每一个网格代表一个像素点,然后绘制成一幅当前环境的二维图,二维图
%的黑色部分表示障碍物,白色部分表示可同行区域。LengthToCoordinate函数在这边的
%作用就是求取出网格对应像素点的坐标,返回值OutPutCoordinate为坐标
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%障碍物检测边界的取整
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Range_LY_Num = floor((FOV_LY+Grid_L/2)/Grid_L); %求取y左半轴的网格数目
Range_LY = (floor((FOV_LY+Grid_L/2)/Grid_L))*Grid_L-Grid_L/2; %求取y左半轴的边界
Range_RY_Num = ceil((FOV_RY-Grid_L/2)/Grid_L); %求取y右半轴的网格数目
Range_RY = (ceil((FOV_RY-Grid_L/2)/Grid_L))*Grid_L+Grid_L/2; %求取y右半轴的边界
Range_FX_Num = floor((FOV_FX-Grid_H/2)/Grid_H); %求取x后半轴的网格数目
Range_FX = (floor((FOV_FX-Grid_H/2)/Grid_H))*Grid_H+Grid_H/2; %求取x后左半轴的边界
Range_BX_Num = ceil((FOV_BX+Grid_H/2)/Grid_H); %求取x前右半轴的网格数目
Range_BX = (ceil((FOV_BX+Grid_H/2)/Grid_H))*Grid_H-Grid_H/2; %求取x前右半轴的边界
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if(y<-Grid_L/2)
if(x<-Grid_H/2)
Now_Range_Y_Num = Range_LY_Num-(Range_LY-y)/(Grid_L);
Now_Range_X_Num = Range_BX_Num-(Range_BX-x)/(Grid_H);
end
if(x==-Grid_H/2)
Now_Range_Y_Num = Range_LY_Num-(Range_LY-y)/(Grid_L);
Now_Range_X_Num = 0;
end
if(x>Grid_H/2)
Now_Range_Y_Num = Range_LY_Num-(Range_LY-y)/(Grid_L);
Now_Range_X_Num = Range_FX_Num-(Range_FX-x)/(Grid_H);
end
if(x==Grid_H/2)
Now_Range_Y_Num = Range_LY_Num-(Range_LY-y)/(Grid_L);
Now_Range_X_Num = 1;
end
end
if(y>=Grid_L/2)
if(x<-Grid_H/2)
Now_Range_Y_Num = Range_RY_Num-(Range_RY-y)/(Grid_L);
Now_Range_X_Num = Range_BX_Num-(Range_BX-x)/(Grid_H);
end
if(x==-Grid_H/2)
Now_Range_Y_Num = Range_LY_Num-(Range_LY-y)/(Grid_L);
Now_Range_X_Num = 0;
end
if(x>Grid_H/2)
Now_Range_Y_Num = Range_RY_Num-(Range_RY-y)/(Grid_L);
Now_Range_X_Num = Range_FX_Num-(Range_FX-x)/(Grid_H);
end
if(x==Grid_H/2)
Now_Range_Y_Num = Range_LY_Num-(Range_LY-y)/(Grid_L);
Now_Range_X_Num = 1;
end
end
if(y==-Grid_L/2)
if(x==-Grid_H/2)
Now_Range_Y_Num = 0;
Now_Range_X_Num = 0;
end
end
% disp(Now_Range_X_Num);
% disp(Now_Range_Y_Num);
OutPutCoordinate(1) = round(Now_Range_X_Num+(Range_FX_Num-Range_BX_Num)+1);
OutPutCoordinate(2) = round(Now_Range_Y_Num+(Range_RY_Num-Range_LY_Num)+1);
% if((OutPutCoordinate(1)<0)||(OutPutCoordinate(2)<0))
% disp(OutPutCoordinate(1));
% disp(OutPutCoordinate(2));
% end
end
雷达数据图:
识别的结果:
上图即采用高度差法识别出的障碍物图,图中黑色区域表示障碍物,白色区域表示可通行区域。雷达数据带有一定的噪声和在识别过程中因为设定阈值不同的因素会出现一些不规则的黑色区域和噪声,后期可对改图进行腐蚀和膨胀处理达到更好的识别效果。