用激光雷达探测地面和障碍物

打开脚本

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

创建Velodyne文件读取器

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

文件名=“lidarData_ConstructionRoad.pcap”；设备模型=“HDL32E”；veloReader=velodyneFileReader（文件名，设备模型）；

读取激光雷达扫描

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

的veloReader构建一个有组织的点云对于每个激光雷达扫描地方财产的点云是一个m × n × 3的矩阵，包含点的XYZ坐标，单位为米。点强度存储在强烈．

%读取激光雷达数据扫描ptCloud=readFrame（veloReader）%#好的

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] Color: [] Normal: [] Intensity: [32×1083 uint8]

设置流点云显示

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

%指定点云显示的限制xlimits=[-25 45]；%米Ylimits = [-25 45];Zlimits = [-20 20];%创建一个播放器lidarViewer = pcplayer(xlimits, ylimits, zlimits);%自定义播放器轴标签包含(lidarViewer。轴,“X (m)”)ylabel（lidarViewer.Axes，“Y (m)”) zlabel (lidarViewer。轴,“Z (m)”）%显示原始激光雷达扫描视图（lidarViewer、ptCloud）

在这个例子中，我们将分割属于地平面、自我飞行器和附近障碍物的点。设置颜色图来标记这些点。

定义用于分段点的标签颜色标签=[．..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；%设置颜色映射彩色地图（lidarViewer.Axes、彩色标签）

分割自我载体

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

创建一个车辆尺寸存放车辆尺寸的对象。

vehicleDims=车辆尺寸（）；%典型车辆4.7米乘1.8米乘1.4米

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

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

使用辅助功能分割自我载体helperSegmentEgoFromLidarData．这个函数将ego载具定义的长方体内的所有点分割。将分段的点存储在结构体中点．

点=结构();点。egpoints = helperSegmentEgoFromLidarData(ptCloud, vehicledimms, mountLocation);

使用分段的ego车辆可视化点云。使用helperUpdateViewhelper函数。

closePlayer=false；HelperUpdate视图（lidarViewer、ptCloud、点、颜色、closePlayer）；

分段地平面和附近障碍物

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

高程delta=10；points.GroundPoints=sectiongroundfromLidardata（ptCloud，“升降角度增量”，高程三角洲）；%可视化分割的地平面。HelperUpdate视图（lidarViewer、ptCloud、点、颜色、closePlayer）；

使用的方法移除属于ego车辆和地平面的点选择函数在点云上执行。指定“OutputSize”作为“全部”要保留点云的组织性质，请执行以下操作。

nonEgoGroundPoints=~points.EgoPoints&~points.GroundPoints；ptCloudSegmented=select（ptCloud，nonEgoGroundPoints，“OutputSize”，“全部”);

接下来，分割附近的障碍物，在距离“自我”车辆半径内寻找所有不属于地面或“自我”车辆的点。这个半径可以根据激光雷达的距离和感兴趣的区域来确定，以供进一步处理。

传感器位置=[0,0,0]；传感器位于坐标系统的中心半径= 40;%米点。ObstaclePoints = findNeighborsInRadius (ptCloudSegmented,．..sensorLocation,半径);想象分割的障碍HelperUpdate视图（lidarViewer、ptCloud、点、颜色、closePlayer）；

激光雷达序列处理

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

%倒带| veloReader |以从序列的开头开始复位（速度读卡器）；%30秒后停止处理stopTime = veloReader。开始时间+秒(30);isPlayerOpen = true;而hasFrame (veloReader) & & veloReader。CurrentTime < stopTime && isPlayerOpen获取下一次激光雷达扫描ptCloud=readFrame（veloReader）；%属于ego车辆的分段点点。egpoints = helperSegmentEgoFromLidarData(ptCloud, vehicledimms, mountLocation);属于接地面的线段点点。GroundPoints = segmentGroundFromLidarData (ptCloud,“升降角度增量”，高程三角洲）；%移除属于自我战车和地平面的点nonEgoGroundPoints=~points.EgoPoints&~points.GroundPoints；ptCloudSegmented=select（ptCloud，nonEgoGroundPoints，“OutputSize”，“全部”);%段障碍点。ObstaclePoints = findNeighborsInRadius(ptcloudsegments, sensorLocation, radius);closePlayer = ~ hasFrame (veloReader);更新激光雷达显示isPlayerOpen = helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer);终止snapnow

金宝app支持功能

helperSegmentEgoFromLidarData根据车辆尺寸和安装位置，对属于ego车辆的点进行分段。

函数egpoints = helperSegmentEgoFromLidarData(ptCloud, vehicledimms, mountLocation)%helperSegmentEgoFromLidarData从lidar数据中分割EgoVehicle点%egoPoints=helperSegmentEgoFromLidarData（云端、车辆信息、安装位置）%分段属于尺寸为vehicleDims的ego车辆的点%从激光雷达扫描ptCloud。激光雷达安装在指定的位置%通过车辆坐标系中的安装位置。ptCloud是一个%pointCloud对象。vehicleDimensions是一个vehicleDimensions对象。% mountLocation是一个由3个元素组成的向量，指定XYZ位置%激光雷达在车辆坐标系中。％%此功能假定激光雷达安装在与地面平行的位置%平面，正X方向指向前方，正Y方向，指向车辆左侧%右手系统。%汽车周围的缓冲器缓冲区=[0.1,0.1,0.1]；%在米%在车辆坐标中定义车辆限制egoXMin=-vehicleDims.Rearhanding-缓冲区（1）；egoXMax=egoXMin+车辆长度+缓冲区（1）；egoYMin=-vehicleDims.Width/2-缓冲区（2）；egoYMax=egoYMin+车辆最小宽度+缓冲区（2）；egoZMin=0-缓冲区（3）；egoZMax=egoZMin+车辆高度+缓冲区（3）；egoXLimits=[egoXMin，egoXMax]；egoYLimits=[egoYMin，egoYMax]；egoZLimits=[egoZMin，egoZMax]；%转换为激光雷达坐标egoXLimits = egoXLimits -挂载位置(1);egoYLimits = egoYLimits -挂载位置(2);egoZLimits = egoZLimits -挂载位置(3);%使用逻辑索引来选择ego车辆立方体内的点egpoints = ptCloud.Location(:，:，1) > egoXLimits(1)．..&ptCloud.Location（：，：，1）．..&ptCloud.Location（：，：，2）>egoYLimits（1）．..& ptCloud.Location(:，:，2) < egoYLimits(2)．..&ptCloud.Location（：，：，3）>egoZLimits（1）．..&位置（：，：，3）终止

helperUpdateView用最新的点云和相关的颜色标签更新流点云显示。

函数isPlayerOpen = helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer)%HelperUpdate视图更新流式点云显示% isPlayerOpen = helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayers)%更新在lidarViewer中指定的pcplayer对象一个新点云ptCloud %。在结构点中指定的点是有颜色的%根据lidarViewer的颜色图，使用%结构颜色。closePlayer是指示是否关闭的标志% lidarViewer。如果closePlayer隐藏(lidarViewer);isPlayerOpen = false;回来；终止扫描大小=大小（ptCloud.Location）；扫描大小=扫描大小（1:2）；%初始化colormapcolormapValues=个（扫描大小，“喜欢”，ptCloud.Location）*颜色。未标记；如果isfield(点,“GroundPoints”) colormapValues(points.GroundPoints) = colors.Ground;终止如果isfield(点,“EgoPoints”) colormapValues(points. egpoints) = colors.Ego;终止如果isfield(点,“ObstaclePoints”) colormapValues(points.ObstaclePoints) = colors.Obstacle;终止%更新视图视图（lidarViewer、ptCloud.Location、colormapValues）%检查播放机是否打开isPlayerOpen = isOpen (lidarViewer);终止