基于激光雷达的地面和障碍物检测
这个例子展示了如何通过分割地平面和寻找附近的障碍物来处理安装在车辆上的传感器的3-D激光雷达数据。这可以方便车辆导航的行驶路径规划。该示例还展示了如何可视化流激光雷达数据。
创建一个Velodyne文件阅读器
本例中使用的激光雷达数据是使用安装在车辆上的Velodyne®HDL32E传感器记录的。建立了一个velodyneFileReader
文件名=“lidarData_ConstructionRoad.pcap”；deviceModel =“HDL32E”；velereader = velodyneFileReader(fileName, deviceModel);
读取激光雷达扫描
激光雷达的每一次扫描数据都被存储为一个三维点云。使用快速索引和搜索有效地处理这些数据是传感器处理管道性能的关键。实现此效率的方法pointCloud
的veloReader构建一个有组织的pointCloud每次激光雷达扫描。的位置财产的pointCloud为m × n × 3矩阵，包含点的XYZ坐标，单位为米。点强度存储在强度．
读取激光雷达数据的扫描ptCloud = readFrame (veloReader)% #好< NOPTS >
ptCloud = pointCloud属性:Location: [32x1083x3 single] Count: 34656 XLimits: [-80.0444 87.1780] YLimits: [-85.6287 92.8721] ZLimits: [-21.6060 14.3558] Color: [] Normal: [] Intensity: [32x1083 uint8]
设置流点云显示
的pcplayer可用于可视化流点云数据。通过配置设置车辆周围的区域来显示pcplayer
指定点云显示的限制Xlimits = [-25 45];%米Ylimits = [-25 45];Zlimits = [-20 20];创建一个pcplayerlidarViewer = pcplayer(xlimits, ylimits, zlimits);%自定义播放器轴标签包含(lidarViewer。轴,“X (m)”) ylabel (lidarViewer。轴,“Y (m)”) zlabel (lidarViewer。轴,“Z (m)”）%显示原始激光雷达扫描视图(lidarViewer ptCloud)
在这个例子中，我们将分割属于地平面、自我载具和附近障碍的点。为标记这些点设置颜色图。
定义用于分段点的标签colorLabels = [.．.0 0.4470 - 0.7410;.．.%未标记点，指定为[R,G,B]0.4660 0.6740 0.1880;.．.%地面点0.9290 0.6940 0.1250;.．.%自我点0.6350 0.0780 0.1840);%的障碍点为每个标签定义索引颜色。无标号= 1;颜色。地面= 2;颜色。自我= 3;颜色。障碍= 4;设置颜色图colormap (lidarViewer。轴,colorLabels)
分割自我载体
激光雷达安装在车辆顶部，点云可能包含属于车辆本身的点，如车顶或引擎盖上的点。知道了飞行器的尺寸，我们就能分割出离飞行器最近的点。
创建一个vehicleDimensions
vehicleDims = vehicleDimensions ();典型车辆470米乘180米乘1.4米
指定激光雷达在车辆坐标系中的安装位置。车辆坐标系以后轴中心为中心，在地面上，正X方向指向前方，正Y方向指向左侧，正Z方向指向上方。在这个例子中，激光雷达安装在车辆的顶部中心，与地面平行。
mountLocation = [.．.vehicleDims。/ 2 - vehicleDims长度。RearOverhang,.．.% x0，.．.y %vehicleDims.Height];% z
使用助手功能分割自我载体helperSegmentEgoFromLidarData．这个函数将自我载体定义的长方体中的所有点分割。将分段的点存储在结构中点．
点=结构();点。EgoPoints = helperSegmentEgoFromLidarData(ptCloud, vehicleDims, mountLocation);
用分段自我载体来可视化点云。使用helperUpdateViewhelper函数。
closePlayer = false;helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer);
分割地平面和附近的障碍物
为了从激光雷达的数据中识别障碍物，首先使用的分割地面平面segmentGroundFromLidarData
elevationDelta = 10;点。GroundPoints = segmentGroundFromLidarData (ptCloud,“ElevationAngleDelta”, elevationDelta);可视化分割的地平面。helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer);
移除属于自我载体和地平面的点选择“OutputSize”作为“全部”保持点云的有组织性质。
nonEgoGroundPoints = ~点。EgoPoints & ~ points.GroundPoints;ptcloudsegments = select(ptCloud, nonEgoGroundPoints，“OutputSize”，“全部”）;
接下来，通过寻找所有不属于地面或自我载具半径范围内的点来分割附近的障碍。这个半径可以根据激光雷达的范围和感兴趣的区域来确定，以便进一步处理。
sensorLocation = [0,0,0];传感器位于坐标系的中心半径= 40;%米点。ObstaclePoints = findNeighborsInRadius (ptCloudSegmented,.．.sensorLocation,半径);可视化分割的障碍helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer);
过程激光雷达序列
现在，单个激光雷达扫描的点云处理管道已经布置好了，把它们放在一起来处理记录的数据序列。下面的代码被缩短了，因为关键参数已经在前面的步骤中定义了。这里只使用参数，不作进一步解释。
%倒带| velereader |从序列的开始开始重置(veloReader);isPlayerOpen = true;而hasFrame (veloReader) & & isPlayerOpen拿下一个激光雷达扫描。ptCloud = readFrame (veloReader);属于自我载具的分段点数。点。EgoPoints = helperSegmentEgoFromLidarData(ptCloud, vehicleDims, mountLocation);属于地平面的分段点点。GroundPoints = segmentGroundFromLidarData (ptCloud,“ElevationAngleDelta”, elevationDelta);移除属于自我载具和地平面的点nonEgoGroundPoints = ~点。EgoPoints & ~ points.GroundPoints;ptcloudsegments = select(ptCloud, nonEgoGroundPoints，“OutputSize”，“全部”）;%段障碍点。ObstaclePoints = findNeighborsInRadius(ptcloudsegented, sensorLocation, radius);closePlayer = ~ hasFrame (veloReader);更新激光雷达显示isPlayerOpen = helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer);结束snapnow
支持功能
helperSegmentEgoFromLidarData分段点属于自我车辆给定的车辆的尺寸和安装位置。
函数egoPoints = helperSegmentEgoFromLidarData(ptCloud, vehicleDims, mountLocation)%helperSegmentEgoFromLidarData分段自我车辆点从激光雷达数据% egoPoints = helperSegmentEgoFromLidarData(ptCloud,vehicleDims,mountLocation)%分段点属于自我车辆的尺寸vehicle dims%从激光雷达扫描ptCloud。激光雷达安装在指定的位置% by mountLocation在车辆坐标系中。ptCloud是一个% pointCloud对象。vehicleDimensions是一个vehicleDimensions对象。% mountLocation是一个3元素的向量，指定的XYZ位置%的激光雷达在车辆坐标系。％这个函数假设激光雷达与地面平行安装%平面，正X方向指向车辆前方，%正Y方向指向车辆左侧的a%右撇子体系。自我车辆周围的缓冲器bufferZone = [0.1, 0.1, 0.1];%在米在车辆坐标中定义自我车辆限制egoXMin = -vehicleDims。RearOverhang bufferZone (1);egoXMax = egoXMin + vehicleDims。长度+ bufferZone (1);egoYMin = -vehicleDims。宽/ 2 - bufferZone (2);egoYMax = egoYMin + vehicleDims。宽度+ bufferZone (2);egoZMin = 0 - bufferZone(3);egoZMax = egoZMin + vehicleDims。身高+ 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);结束
helperUpdateView使用最新的点云和相关的颜色标签更新流点云显示。
函数isPlayerOpen = helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer)%helperUpdateView更新流点云显示% isPlayerOpen = helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayers)用一个新点更新lidarViewer中指定的pcplayer对象云ptCloud %。在结构点中指定的点是彩色的%根据lidarViewer的色彩图使用指定的标签%结构颜色。closePlayer是指示是否关闭的标志% lidarViewer。如果closePlayer隐藏(lidarViewer);isPlayerOpen = false;返回；结束scanSize =大小(ptCloud.Location);scanSize = scanSize (1:2);%初始化colormapcolormapValues = 1 (scanSize,“喜欢”， ptCloud.Location) * color . unlabeled;如果isfield(点,“GroundPoints”) colormapValues(points.GroundPoints) = colors.Ground;结束如果isfield(点,“EgoPoints”) colormapValues(points.EgoPoints) = colors.Ego;结束如果isfield(点,“ObstaclePoints”) colormapValues(points.ObstaclePoints) = colors.Obstacle;结束%更新视图视图(lidarViewer ptCloud。位置,colormapValues)检查玩家是否打开isPlayerOpen = isOpen (lidarViewer);结束
另请参阅
功能
对象
相关的话题
您也可以从以下列表中选择网站: