地面平面和障碍物检测使用激光雷达

打开脚本

这个例子展示了如何通过分割地平面和寻找附近的障碍物来处理来自安装在车辆上的传感器的3-D激光雷达数据。这有利于车辆导航的可行驶路径规划。该实例还展示了如何可视化流激光雷达数据。

创建一个Velodyne文件阅读器

本例中使用的激光雷达数据是使用安装在车辆上的Velodyne®HDL32E传感器记录的。建立一个velodyneFileReader对象来读取记录的PCAP文件。

文件名=“lidarData_ConstructionRoad.pcap”；deviceModel =“HDL32E”；veloReader = velodyneFileReader(文件名，deviceModel);

读取激光雷达扫描

激光雷达数据的每次扫描都存储为三维点云。使用快速索引和搜索有效地处理这些数据是传感器处理管道性能的关键。这种效率是通过使用pointCloud对象，它使用K-d树数据结构在内部组织数据。

的veloReader构建一个有组织的pointCloud每次激光雷达扫描。的位置的属性pointCloud是m × n × 3矩阵，包含点的XYZ坐标，单位为米。点强度存储在强度．

读取激光雷达扫描数据ptCloud = readFrame(veloReader)% #好< NOPTS >

ptCloud = pointCloud with properties: Location: [32×1083×3 single] Count: 34656 XLimits: [-80.0444 87.1780] YLimits: [-85.6287 92.8721] ZLimits:[-21.6060 14.3558]颜色:[]正常:[]强度:[32×1083 uint8]

设置流点云显示

的pcplayer可用于可视化流点云数据。通过配置设置车辆周围要显示的区域pcplayer．

指定点云显示限制Xlimits = [-25 45];%米Ylimits = [-25 45];Zlimits = [-20 20];创建一个pc播放器lidarViewer = 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];%障碍点为每个标签定义索引颜色。Unlabeled = 1;颜色。地面= 2;颜色。自我= 3;颜色。障碍= 4;设置颜色colormap (lidarViewer。轴,colorLabels)

细分自我车辆

激光雷达安装在车辆顶部，点云可以包含属于车辆本身的点，例如在车顶或引擎盖上。知道了车辆的尺寸，我们可以分割出离车辆最近的点。

创建一个vehicleDimensions对象，用于存储车辆的尺寸。

vehicleDims = vehicleDimensions();典型车辆4.7米乘1.8米乘1.4米

指定激光雷达在车辆坐标系中的安装位置。车辆坐标系以后轴中心为中心，在地面上，正X方向指向前方，正Y方向向左，正Z方向向上。在这个例子中，激光雷达安装在车辆的顶部中心，与地面平行。

mountLocation = [.．.vehicleDims。长度/2 - vehicledim。RearOverhang,.．.% x0,.．.y %vehicleDims.Height];% z

使用helper函数分割自我车辆helperSegmentEgoFromLidarData．这个函数分割了自我载体定义的长方体中的所有点。将分割的点存储在结构体中点．

Points = struct();点。egpoints = helperSegmentEgoFromLidarData(ptCloud, vehicleDims, mountLocation);

用分段自我载体可视化点云。使用helperUpdateViewhelper函数。

closePlayer = false;helperUpdateView(lidarViewer, ptCloud，点，颜色，closePlayer);

分割地平面和附近障碍物

为了从激光雷达数据中识别障碍物，首先使用地平面分割segmentGroundFromLidarData函数来完成此任务。该函数从有组织的激光雷达数据中分割出属于地面的点。

elevationDelta = 10;点。GroundPoints = segmentGroundFromLidarData(ptCloud，“ElevationAngleDelta”, elevationDelta);可视化分割的地平面。helperUpdateView(lidarViewer, ptCloud，点，颜色，closePlayer);

删除属于自我载具和地平面的点选择函数在点云上。指定“OutputSize”作为“全部”保持点云的组织性。

nonEgoGroundPoints = ~points。egpoints & ~points.GroundPoints;ptCloudSegmented = select(ptCloud, nonEgoGroundPoints，“OutputSize”，“全部”）;

接下来，通过寻找在自我载具半径范围内不属于地面或自我载具的所有点来分割附近的障碍物。这个半径可以根据激光雷达的范围和感兴趣的区域来确定，以便进一步处理。

sensorLocation = [0,0,0];传感器位于坐标系统的中心半径= 40;%米点。ObstaclePoints = findNeighborsInRadius(ptcloudsegented，.．.sensorLocation,半径);可视化分割的障碍helperUpdateView(lidarViewer, ptCloud，点，颜色，closePlayer);

过程激光雷达序列

现在，用于单个激光雷达扫描的点云处理管道已经布置好，将所有这些放在一起，从记录的数据序列中处理30秒。下面的代码被缩短了，因为关键参数已经在前面的步骤中定义了。这里使用的参数没有进一步解释。

%倒带|veloReader|从序列的开头开始重置(veloReader);30秒后停止处理stopTime = velereader。开始时间+秒(30);isPlayerOpen = true;而hasFrame(velereader) && velereader。CurrentTime < stopTime && isPlayerOpen抓取下一个激光雷达扫描。ptCloud = readFrame(velereader);%属于自我载具的分段点数点。egpoints = helperSegmentEgoFromLidarData(ptCloud, vehicleDims, mountLocation);%属于地平面的段点点。GroundPoints = segmentGroundFromLidarData(ptCloud，“ElevationAngleDelta”, elevationDelta);移除属于自我载具和地平面的点nonEgoGroundPoints = ~points。egpoints & ~points.GroundPoints;ptCloudSegmented = select(ptCloud, nonEgoGroundPoints，“OutputSize”，“全部”）;%分段障碍点。ObstaclePoints = findNeighborsInRadius(ptCloudSegmented, sensorLocation, radius);closePlayer = ~hasFrame(velereader);更新激光雷达显示isPlayerOpen = helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer);结束snapnow

金宝app支持功能

helperSegmentEgoFromLidarData给定车辆的尺寸和安装位置，分段属于自我车辆的点。

函数egpoints = helperSegmentEgoFromLidarData(ptCloud, vehicleDims, mountLocation)%helperSegmentEgoFromLidarData段从激光雷达数据中获取车辆点% egpoints = helperSegmentEgoFromLidarData(ptCloud,vehicleDims,mountLocation)属于维度vehicledim的自我车辆的分段点%来自激光雷达扫描ptCloud。激光雷达安装在指定的位置%由装载位置在车辆坐标系。ptCloud是一个% pointCloud对象。vehicleDimensions是一个vehicleDimensions对象。% mountLocation是一个3元素向量，指定对象的XYZ位置%激光雷达在车辆坐标系。％这个函数假设激光雷达与地面平行安装%平面，正X方向指向车辆前方，%正Y方向指向车辆左侧a%右手系统。自我车辆周围有缓冲区bufferZone = [0.1, 0.1, 0.1];米%在车辆坐标中定义自我车辆限制egoXMin = -vehicleDims。RearOverhang - bufferZone(1);egoXMax = egoXMin + vehicledim。长度+ bufferZone(1);egoYMin = - vehicledim。宽度/2 - bufferZone(2);egoYMax = egoYMin + vehicledim。宽度+ bufferZone(2);egoZMin = 0 - bufferZone(3);egoZMax = egoZMin + vehicledim。高度+缓冲区(3);egoXLimits = [egoXMin, egoXMax];egoYLimits = [egoYMin, egoYMax]; egoZLimits = [egoZMin, egoZMax];转换为激光雷达坐标egoXLimits = egoXLimits - mountLocation(1);egoYLimits = egoYLimits - mountLocation(2);egoZLimits = egoZLimits - mountLocation(3);使用逻辑索引选择自我车辆立方体内的点egpoints = 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，点，颜色，closePlayer)%helperUpdateView更新流点云显示% isPlayerOpen = helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayers)用一个新的点更新lidarViewer中指定的pcplayer对象% cloud ptCloud。在结构点中指定的点是彩色的根据lidarViewer的色图，使用指定的标签%结构颜色。closePlayer是指示是否关闭的标志lidarViewer。如果closePlayer隐藏(lidarViewer);isPlayerOpen = false;返回；结束scanSize = size(ptCloud.Location);scanSize = scanSize(1:2);初始化色图colormapValues = ones(scanSize，“喜欢”， ptCloud.Location) * colors.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);结束