激光雷达的检测仿真代码详解（附Matlab源码详解）

最新推荐文章于 2024-07-26 18:11:15 发布

努力把公司干倒闭

最新推荐文章于 2024-07-26 18:11:15 发布

阅读量5.4k

点赞数 28

分类专栏：多传感器融合文章标签： matlab 开发语言

本文链接：https://blog.csdn.net/m0_64346597/article/details/126441020

版权

多传感器融合专栏收录该内容

14 篇文章 6 订阅

订阅专栏

一、创建velodyneFileReader

本示例中的激光雷达数据来自安装在车辆上的激光雷达Velodyne HDL32E。创建velodyneFileReader对象以读取录制的PCAP文件。

fileName = 'lidarData_ConstructionRoad.pcap';   
deviceModel = 'HDL32e';   
veloReader = velodyneFileReader(fileName,deviceModel);

二、读取激光雷达扫描数据

激光雷达每次扫描的数据都存储为三维点云。为了能够高效地处理这些数据，需要使用快速索引和搜索，通过K-d tree数据结构处理数据。

veloReader为激光雷达每个扫描构建一个有组织的点云。点云的位置属性是M×N×3的矩阵，包含点的X，Y，Z坐标（m）。

ptCloud=readFrame(veloReader);

三、设置点云显示

pcplayer用于点云数据的可视化，通过配置pcplayer设置车辆周围区域。

%设置点云显示的区域
xlimits = [-25,45];
ylimits = [-25,45];
zlimits = [-20,20]; 
%创建pcplayer
lidarViewer = pcplayer(xlimits,ylimits,zlimits);  
%定义坐标轴标签
xlabel(lidarViewer.Axes,'X(m)')   
ylabel(lidarViewer.Axes,'Y(m)')   
zlabel(lidarViewer.Axes,'Z(m)') 
%显示激光雷达点云
view(lidarViewer,ptCloud)

输出结果如图所示

分割属于地平面、主车和附近障碍物的点；设置用于标记这些点的颜色映射。

%定义用于分段点的标签
colorLabels=[...
    0      0.4470 0.7410;...%未标记点
    0.4660 0.6740 0.1880;...%地平面点
    0.9290 0.6940 0.1250;...%主车点
    0.635,0.078,0.1840];%障碍点
%为每个标签定义索引
colors.Unlabeled=1;
colors.Ground=2;
colors.Ego=3;
colors.Obstacle=4;
%设置颜色映射
colormap(lidarViewer.Axes, colorLabels)

输出结果如下图所示

四、分割主车

激光雷达安装在车辆顶部，点云可能包含属于车辆本身的点，例如车顶或发动机罩上的点。了解车辆的尺寸，可以分割出离车辆最近的点。

创建车辆尺寸对象以存储车辆尺寸，典型车辆尺寸为4.7m×1.8m×1.4m。

vehicleDims=vehicleDimensions ();

在车辆坐标系中指定激光雷达的安装位置。在该示例中，车辆坐标系原点位于后轴中心，正X方向指向前方，正Y方向指向左侧，正Z方向指向上方，激光雷达安装在车辆的顶部中心，与地面平行。

mountLocation= [...
    vehicleDims.Length/2-vehicleDims.RearOverhang,...%设置X方向
    0,...                                            %设置Y方向
    vehicleDims.Height];                             %设置Z方向

使用helper函数helperSegmentEgoFromLidarData分割主车，该函数分割主车定义的长方体内的所有点；将分段点存储在结构points中。

points=struct();
points.EgoPoints=helperSegmentEgoFromLidarData(ptCloud,vehicleDims, mountLocation);

使用helperUpdateView函数对主车分段点云可视化

closePlayer=false;
helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer);

helperSegmentEgoFromLidarData函数程序如下：

%helperSegmentEgoFromLidarData函数
function egoPoints=helperSegmentEgoFromLidarData(ptCloud, vehicleDims,mountLocation)
bufferZone= [0.1,0.1,0.1];
%在车辆坐标系中定义主车限值
egoXMin=-vehicleDims.RearOverhang-bufferZone (1);
egoXMax=egoXMin+vehicleDims.Length+bufferZone (1);
egoYMin=-vehicleDims.Width/2-bufferZone (2);
egoYMax=egoYMin+vehicleDims.Width+bufferZone (2);
egoZMin=0-bufferZone (3);
egoZMax=egoZMin+vehicleDims. Height+bufferZone (3);
egoXLimits= [egoXMin, egoXMax];
egoYLimits= [egoYMin, egoYMax];
egoZLimits= [egoZMin, egoZMax];
%转换为激光雷达坐标系
egoXLimits=egoXLimits-mountLocation(1);
egoYLimits=egoYLimits-mountLocation(2);
egoZLimits=egoZLimits-mountLocation(3);
%使用逻辑索引选择主车立方体内的点
egoPoints=ptCloud.Location(:,:,1)>egoXLimits(1)...
   & ptCloud. Location(:,:,1) <egoXLimits(2)...
   & ptCloud. Location(:,:,2) >egoYLimits(1)...
   & ptCloud. Location(:,:,2) <egoYLimits(2)...
   & ptCloud. Location(:,:,3) >egoZLimits(1)...
   & ptCloud. Location(:,:,3) <egoZLimits(2);
end

helperUpdateView函数程序如下

%helperUpdateView函数程序
function isPlayerOpen=helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer)
if closePlayer
     hide (lidarViewer);
     isPlayerOpen=false;
     return;
end
scanSize=size (ptCloud.Location);
scanSize=scanSize (1:2);
%初始化颜色映射
colormapValues=ones (scanSize,'like',ptCloud.Location) * colors.Unlabeled;
if isfield(points,'GroundPoints') 
    colormapValues (points.GroundPoints)=colors.Ground;
end
if isfield(points, 'EgoPoints')
    colormapValues (points.EgoPoints)=colors.Ego;
end
if isfield (points, 'ObstaclePoints')
    colormapValues (points.ObstaclePoints)=colors.Obstacle;   
end
%更新视图
view (lidarViewer,ptCloud. Location, colormapValues)
%检查播放器是否打开
isPlayerOpen=isOpen (lidarViewer);
end

输出结果如下所示

五、分割地平面和周围的障碍物

为了从激光雷达数据中识别障碍物，首先使用segmentGroundFromLidarData函数对地面进行分段，从有组织的激光雷达数据中分割出属于地平面的点。

elevationDelta = 10;   
points.GroundPoints=segmentGroundFromLidarData(ptCloud,'ElevationAngleDelta', elevationDelta); 
%分割地平面的可视化
helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer);

输出结果如图所示

使用点云上的选择功能删除属于主车和地平面的点；将“Outputsize”指定为“full”，以保留点云的组织性质。

nonEgoGroundPoints=~points.EgoPoints &~points.GroundPoints;
ptCloudSegmented=select(ptCloud,nonEgoGroundPoints,'OutputSize','full');

通过寻找距离主车一定半径内所有不属于地平面和主车的点来分割附近的障碍物。这个半径可以根据激光雷达的范围和感兴趣的区域来确定。

sensorLocation=[0,0,0];%传感器位于坐标系的原点
radius=40;
points.ObstaclePoints=findNeighborsInRadius(ptCloudSegmented,sensorLocation,radius);
%分割障碍物的可视化
helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer);

输出结果如下

六、激光雷达数据处理

从激光雷达记录的数据序列中处理30s。

reset(veloReader);
stopTime=veloReader.StartTime+seconds(20);
isPlayerOpen=true;
while hasFrame(veloReader)&&veloReader.CurrentTime<stopTime&&isPlayerOpen
%获取下一次激光雷达扫描
    ptCloud=readFrame(veloReader);
%属于主车的分段点
    points.EgoPoints=helperSegmentEgoFromLidarData(ptCloud,vehicleDims,mountLocation);
%属于地平面的分段点
    points.GroundPoints=segmentGroundFromLidarData(ptCloud,'ElevationAngleDelta',elevationDelta);
%删除属于主车和地平面的点
    nonEgoGroundPoints=~points.EgoPoints&~points.GroundPoints;
    ptCloudSegmented=select(ptCloud,nonEgoGroundPoints,'OutputSize','full');
%分段障碍物
    points.ObstaclePoints=findNeighborsInRadius(ptCloudSegmented,sensorLocation,radius);
    closePlayer=~hasFrame(veloReader);
%更新激光雷达显示
    isPlayerOpen=helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer);
end
snapnow

点击“Run”按钮，可以看到激光雷达点云30s点云的变化，如下图所示

完整代码如下图所示

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%     一
fileName = 'lidarData_ConstructionRoad.pcap';   
deviceModel = 'HDL32e';   
veloReader = velodyneFileReader(fileName,deviceModel); 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%     二
ptCloud=readFrame(veloReader);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%     三
%设置点云显示的区域
xlimits = [-25,45];
ylimits = [-25,45];
zlimits = [-20,20]; 
%创建pcplayer
lidarViewer = pcplayer(xlimits,ylimits,zlimits);  
%定义坐标轴标签
xlabel(lidarViewer.Axes,'X(m)')   
ylabel(lidarViewer.Axes,'Y(m)')   
zlabel(lidarViewer.Axes,'Z(m)') 
%显示激光雷达点云
view(lidarViewer,ptCloud)  

%定义用于分段点的标签
colorLabels=[...
    0      0.4470 0.7410;...%未标记点
    0.4660 0.6740 0.1880;...%地平面点
    0.9290 0.6940 0.1250;...%主车点
    0.635,0.078,0.1840];%障碍点
%为每个标签定义索引
colors.Unlabeled=1;
colors.Ground=2;
colors.Ego=3;
colors.Obstacle=4;
%设置颜色映射
colormap(lidarViewer.Axes, colorLabels)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    四
%分割主车
vehicleDims=vehicleDimensions ();
mountLocation= [...
    vehicleDims.Length/2-vehicleDims.RearOverhang,...%设置X方向
    0,...                                            %设置Y方向
    vehicleDims.Height];                             %设置Z方向
points=struct();
points.EgoPoints=helperSegmentEgoFromLidarData(ptCloud,vehicleDims, mountLocation);
closePlayer=false;
helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%      五
%%%%分割地平面和周围障碍物
elevationDelta = 10;   
points.GroundPoints=segmentGroundFromLidarData(ptCloud,'ElevationAngleDelta', elevationDelta);   
helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer); 

nonEgoGroundPoints=~points.EgoPoints &~points.GroundPoints;
ptCloudSegmented=select(ptCloud,nonEgoGroundPoints,'OutputSize','full');

sensorLocation=[0,0,0];%传感器位于坐标系的原点
radius=40;
points.ObstaclePoints=findNeighborsInRadius(ptCloudSegmented,sensorLocation,radius);
%分割障碍物的可视化
helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%       六
reset(veloReader);
stopTime=veloReader.StartTime+seconds(20);
isPlayerOpen=true;
while hasFrame(veloReader)&&veloReader.CurrentTime<stopTime&&isPlayerOpen
%获取下一次激光雷达扫描
    ptCloud=readFrame(veloReader);
%属于主车的分段点
    points.EgoPoints=helperSegmentEgoFromLidarData(ptCloud,vehicleDims,mountLocation);
%属于地平面的分段点
    points.GroundPoints=segmentGroundFromLidarData(ptCloud,'ElevationAngleDelta',elevationDelta);
%删除属于主车和地平面的点
    nonEgoGroundPoints=~points.EgoPoints&~points.GroundPoints;
    ptCloudSegmented=select(ptCloud,nonEgoGroundPoints,'OutputSize','full');
%分段障碍物
    points.ObstaclePoints=findNeighborsInRadius(ptCloudSegmented,sensorLocation,radius);
    closePlayer=~hasFrame(veloReader);
%更新激光雷达显示
    isPlayerOpen=helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer);
end
snapnow

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%下列函数属于第四部分


%helperSegmentEgoFromLidarData函数
function egoPoints=helperSegmentEgoFromLidarData(ptCloud, vehicleDims,mountLocation)
bufferZone= [0.1,0.1,0.1];
%在车辆坐标系中定义主车限值
egoXMin=-vehicleDims.RearOverhang-bufferZone (1);
egoXMax=egoXMin+vehicleDims.Length+bufferZone (1);
egoYMin=-vehicleDims.Width/2-bufferZone (2);
egoYMax=egoYMin+vehicleDims.Width+bufferZone (2);
egoZMin=0-bufferZone (3);
egoZMax=egoZMin+vehicleDims. Height+bufferZone (3);
egoXLimits= [egoXMin, egoXMax];
egoYLimits= [egoYMin, egoYMax];
egoZLimits= [egoZMin, egoZMax];
%转换为激光雷达坐标系
egoXLimits=egoXLimits-mountLocation(1);
egoYLimits=egoYLimits-mountLocation(2);
egoZLimits=egoZLimits-mountLocation(3);
%使用逻辑索引选择主车立方体内的点
egoPoints=ptCloud.Location(:,:,1)>egoXLimits(1)...
   & ptCloud. Location(:,:,1) <egoXLimits(2)...
   & ptCloud. Location(:,:,2) >egoYLimits(1)...
   & ptCloud. Location(:,:,2) <egoYLimits(2)...
   & ptCloud. Location(:,:,3) >egoZLimits(1)...
   & ptCloud. Location(:,:,3) <egoZLimits(2);
end

%helperUpdateView函数程序
function isPlayerOpen=helperUpdateView(lidarViewer,ptCloud,points,colors,closePlayer)
if closePlayer
     hide (lidarViewer);
     isPlayerOpen=false;
     return;
end
scanSize=size (ptCloud.Location);
scanSize=scanSize (1:2);
%初始化颜色映射
colormapValues=ones (scanSize,'like',ptCloud.Location) * colors.Unlabeled;
if isfield(points,'GroundPoints') 
    colormapValues (points.GroundPoints)=colors.Ground;
end
if isfield(points, 'EgoPoints')
    colormapValues (points.EgoPoints)=colors.Ego;
end
if isfield (points, 'ObstaclePoints')
    colormapValues (points.ObstaclePoints)=colors.Obstacle;   
end
%更新视图
view (lidarViewer,ptCloud. Location, colormapValues)
%检查播放器是否打开
isPlayerOpen=isOpen (lidarViewer);
end

码字不易，点个赞再走吧QAQ

转载请附加转载链接说明！！！！！

努力把公司干倒闭

关注

28
点赞
踩
89

收藏

觉得还不错? 一键收藏
打赏
5
评论
激光雷达的检测仿真代码详解（附Matlab源码详解）

在该示例中，车辆坐标系原点位于后轴中心，正X方向指向前方，正Y方向指向左侧，正Z方向指向上方，激光雷达安装在车辆的顶部中心，与地面平行。为了从激光雷达数据中识别障碍物，首先使用segmentGroundFromLidarData函数对地面进行分段，从有组织的激光雷达数据中分割出属于地平面的点。点云的位置属性是M×N×3的矩阵，包含点的X，Y，Z坐标（m）。激光雷达安装在车辆顶部，点云可能包含属于车辆本身的点，例如车顶或发动机罩上的点。了解车辆的尺寸，可以分割出离车辆最近的点。转载请附加转载链接说明！
复制链接

扫一扫