三维激光雷达点云的路缘检测与跟踪

打开生活的脚本

这个例子展示了如何在激光雷达点云中检测和跟踪约束。一个抑制是一条连接道路和人行道的石头或混凝土线。路沿作为道路可通行面积的分隔线。

在没有GPS信号的情况下，检测路缘位置对自动驾驶的路径规划和安全导航具有重要意义。

类的一个子集PandaSet数据集来自Hesai和Scale。pandset包含使用Pandar64传感器捕获的各种城市场景的激光雷达点云扫描。

简介

激光雷达点云路缘检测涉及到道路左右路缘相对于自我车辆的检测。激光雷达传感器安装在车辆顶部。

使用以下步骤检测激光雷达传感器捕获的点云数据中的约束:

提取一个感兴趣的区域(ROI)
分类道路上和非道路上的点
使用越野点识别道路角度
使用道路点检测候选路沿点

下载激光雷达数据集

数据集大小为109 MB，包含50个预处理过的有组织点云。每个点云都指定为64 × 1856 × 3数组。ground truth数据包含13个类的语义分割标签。点云以PCD格式存储，地面真实数据以PNG格式存储。

下载pandset数据集helperDownloadPandasetDataHelper函数，在本例末尾定义。

设置随机种子生成可重复的结果rng (“默认”）;[ptCloudData, labelsData] = helperDownloadPandasetData;

选择数据集的第一帧进行进一步处理。想象点云。

ptCloud = ptCloudData {1};标签= labelsData {1};可视化输入点云与相应的地面真值标签图pcshow (ptCloud.Location、标签)轴在视图(2)标题(“输入激光雷达点云”）

图中包含一个axes对象。标题为Input Lidar Point Cloud的axis对象包含一个类型为scatter的对象。

数据进行预处理

作为检测路缘点的预处理步骤，首先从点云中提取一个感兴趣的区域，并将该区域内的点分类为道路上或越野点。

根据激光雷达传感器的安装位置，超过一定距离的点云数据是稀疏的。为了确保提取的点云足够密集，以便进行进一步处理，请在距离传感器的有限距离内指定一个ROI。

%以米定义ROIxLim = [-35 20];yLim = [-25 25];roiIdx = ptCloud.Location(:，:，1) >= xLim(1) &.．.ptCloud.Location(:，:，1) <= xLim(2) &.．.ptCloud.Location(:，:，2) >= yLim(1) &.．.ptCloud.Location (:: 2) < = yLim (2);

使用以下标签将点云分为道路上和道路外点:

1 -无标号
2 -植被
3 -地面
4 -路
5 -道路标记
6边走
7 -汽车
8 -卡车
9 -其他车辆
10 -行人
11 -道路屏障
12 -迹象
13 -建筑

标签(~ roiIdx) =南;道路点由地面、道路和人行道组成。onRoadLabelsIdx = (labels == 3 | labels == 4 | labels == 6);越野点由建筑物和其他物体组成。offRoadLabelsIdx = (labels == 2 | labels == 11 |.．.|标签== 13);形象化点云，以及公路和公路上的点。图subplot(1,3,1) pcshow(select(ptCloud,roiIdx))轴在视图(2)标题(“裁剪输入点云”) subplot(1,3,2) pcshow(select(ptCloud,onRoadLabelsIdx))轴在视图(2)标题(“路点云”) subplot(1,3,3) pcshow(select(ptCloud,offRoadLabelsIdx))轴在视图(2)标题(“越野点云”）

图中包含3个轴对象。标题为裁剪输入点云的axis对象1包含一个散点类型的对象。标题为On-road Point Cloud的Axes对象2包含一个类型为scatter的对象。标题为“越野点云”的Axes对象3包含一个类型为scatter的对象。

如果您的数据集中没有语义标签，您可以使用深度学习网络计算它们。有关更多信息，请参见基于SqueezeSegV2深度学习网络的激光雷达点云语义分割的例子。

检测道路形状

使用detectRoadAngles函数用于检测越野点云中的道路角度。

该函数将梁模型应用于越野点。有关梁模型的更多信息，请参见[1]而且[２]．

然后，它应用自定义版本的寻脚趾算法[3]归一化梁长度，求出道路角度。

选择越野点云offRoadPtCloud =选择(ptCloud offRoadLabelsIdx);使用detectRoadAngles函数来检测道路的角度roadAngles = detectRoadAngles (offRoadPtCloud);

检测道路限制

使用segmentCurbPoints函数用于从道路上的点云中分割路缘点。该函数使用以下步骤计算约束点:-

1)从路面点中提取特征路缘点。

为了识别道路上点云中的路缘点，利用以下空间特征对道路路缘进行建模:

平滑的特性[２]-这个功能检查一个点周围区域的平滑度。平滑度值越高，表示该点为边点，平滑度值越低，表示该点为平面点。
高度差特性[２]-该功能检查一个点周围的标准偏差和高度的最大差值。
水平和垂直连续性特征[1]-该功能检查一个点相对于其相邻点的水平和垂直连续性。

该函数为每条扫描线上的每个点计算这些特征。满足上述特征特征阈值的点称为特征抑制点。

2)根据特征约束点计算候选约束点。

特征约束点可能包含假阳性。为了去除假阳性，该函数进一步对特征抑制点进行基于RANSAC的二次多项式拟合，提取候选抑制点。

选择越野点云onRoadPtCloud =选择(ptCloud onRoadLabelsIdx OutputSize =“全部”）;使用segmentCurbPoints函数在道路上的点云中分割路缘[~, candidateCurbPoints] = segmentCurbPoints (onRoadPtCloud roadAngles,.．.HorizontalAngularResolution = 0.24);可视化点云覆盖了候选约束点。图pcshow (select (ptCloud roiIdx)。的位置,“w”(2)持有)视图在pcshow (candidateCurbPoints。的位置,“r”MarkerSize = 200)视图(2)从轴在标题(“检测到候选抑制点”）

图中包含一个axes对象。标题为“候选抑制点检测到”的axis对象包含两个散点类型的对象。

跟踪控制分

循环处理激光雷达数据，提取和跟踪候选路沿点。跟踪路沿点提高了路沿检测的鲁棒性。你可以在分段道路上停止路缘跟踪，当自我车辆离开分段道路时重新启动路缘跟踪。约束跟踪涉及对xy点的多项式拟合，使用2次多项式表示为 $y ＝一个 x^{2} + bx + c$ ,在那里一个，b,c是多项式参数。路沿跟踪分为两个步骤:

轨迹抑制多项式参数一个和b来控制多项式的曲率。
轨迹抑制多项式参数c来控制多项式的漂移。

使用具有恒定速度运动模型的卡尔曼滤波器更新这些参数。使用configureKalmanFilter函数初始化卡尔曼滤波器。

rng (“默认”) player = pcplayer([-35 20]，[-25 25]，[-10 10]，MarkerSize=6);candidateCurbQueue = [];candidateCurbQueueSize = 3;candidateCurbQueueCount = 1;prevOffRoadPtCloud = [];prevCandidateCurbPoints = [];prevAngle = [];rmse = 1;%卡尔曼滤波器的初始值initParams。initialEstimateError = [1 1 1]*1e-1;initParams。motionNoise = [1 1 1]*1e-7;initParams。measurementNoise = 1 e3;leftTracker。漂移= [];leftTracker。filteredDrift = [];leftTracker。curveSmoothness = []; leftTracker.filteredCurveSmoothness = []; rightTracker.drift = []; rightTracker.filteredDrift = []; rightTracker.curveSmoothness = []; rightTracker.filteredCurveSmoothness = []; isTracking = false; isLeftCurbTrackingInitialised = false; isRightCurbTrackingInitialised = false;找出ROI内的点xLim = [-35 20];yLim = [-25 25];[ptCloudData, labelsData] = helperDownloadPandasetData;为fileIdx = 1:元素个数(ptCloudData)载入点云和它的地面真相标签。ptCloud = ptCloudData {fileIdx};标签= labelsData {fileIdx};找出ROI内的点roiIdx = ptCloud.Location(:，:，1) >= xLim(1) &.．.ptCloud.Location(:，:，1) <= xLim(2) &.．.ptCloud.Location(:，:，2) >= yLim(1) &.．.ptCloud.Location (:: 2) < = yLim (2);标签(~ roiIdx) =南;将点分为公路上点和公路外点。onRoadLabelsIdx = (labels == 3| labels == 4 | labels == 6);offRoadLabelsIdx = (labels == 2 | labels == 11 | labels == 12 |.．.标签= = 13);offRoadPtCloud =选择(ptCloud offRoadLabelsIdx);onRoadPtCloud =选择(ptCloud onRoadLabelsIdx,“OutputSize”，“全部”）;使用越野点计算道路角度roadAngles = detectRoadAngles (offRoadPtCloud);计算前面和后面的路段数%自我车辆numFrontRoadSegments = sum(roadAngles < 110 | roadAngles > 250);numRearRoadSegments = sum(~(roadAngles < 110 | roadAngles > 250));检查是否应该启用或禁用抑制跟踪如果~ isTracking如果(numFrontRoadSegments < numRearRoadSegments) ||.．.(numel(roadAngles) == 2) isTracking = true;结束其他的如果numFrontRoadSegments > numRearRoadSegments isTracking = false;isLeftCurbTrackingInitialised = false;isRightCurbTrackingInitialised = false;结束结束将前一帧的候选点转换为%当前帧如果~isempty(prevOffRoadPtCloud) gridStep = 0.5;[tform, rmse] = pcregistericp (prevOffRoadPtCloud offRoadPtCloud);prevCandidateCurbPoints = pctransform (pccat (candidateCurbQueue),.．.tform);结束使用道路点计算路缘点。如果rmse > 0.7 [~，candidateCurbPoints] = segmentCurbPoints(onRoadPtCloud，.．.roadAngles HorizontalAngularResolution = 0.24);其他的[~, candidateCurbPoints] = segmentCurbPoints (onRoadPtCloud,.．.roadAngles prevCandidateCurbPoints,.．.HorizontalAngularResolution = 0.24);结束如果isempty(candidateCurbPoints) prevOffRoadPtCloud = offRoadPtCloud;prevAngle = roadAngles;继续；结束curbPoints = candidateCurbPoints.Location;xVal = linspace (0, xLim (2), 80);如果isTracking leftrein = [];rightCurb = [];模型= pcfitplane(选择(ptCloud onRoadLabelsIdx), 0.1,.．.(0 0 1);modelParams = model.Parameters;candidateAngles = atan2d(curbPoints(:，2)， curbPoints(:，1));candidateAngles(candidateAngles < 0) = candidateAngles(.．.candidateAngles < 0) + 360;将候选路沿点划分为左路沿点和右路沿点如果(roadAngles(1) >= 270 || roadAngles(1) < 90) &&.．.(roadAngles(2) >= 90 && roadAngles(2) < 270) idx = candidateAngles >= roadAngles(1) &.．.candidateAngles < roadAngles (2);其他的idx = candidateAngles >= roadAngles(2) |.．.candidateAngles < roadAngles (1);结束leftCandidateCurbPoints = curbPoints (idx:);rightCandidateCurbPoints = curbPoints (~ idx:);计算多项式来跟踪左右限制。leftPolynomial = fitPolynomialRANSAC (.．.leftCandidateCurbPoints (: 1:2), 2, 0.1);rightPolynomial = fitPolynomialRANSAC (.．.rightCandidateCurbPoints (: 1:2), 2, 0.1);如果~ isempty (leftPolynomial)如果~ isleftcurbtrackinginitialized [leftFilter,leftCurb] = helperTrackCandidateCurbs(.．.leftPolynomial、xVal modelParams initParams);isLeftCurbTrackingInitialised = true;其他的[leftPolynomial, leftCurb leftTracker] = helperTrackCandidateCurbs (.．.leftPolynomial、xVal modelParams,.．.leftFilter leftTracker);结束结束如果~ isempty (rightPolynomial)如果~ isrightcurbtrackinginitializing [rightFilter,rightCurb] = helperTrackCandidateCurbs(.．.rightPolynomial、xVal modelParams initParams);isRightCurbTrackingInitialised = true;其他的[rightPolynomial, rightCurb rightTracker] = helperTrackCandidateCurbs (.．.rightPolynomial、xVal modelParams,.．.rightFilter rightTracker);结束结束ptCloud =选择(ptCloud roiIdx);使用附加到它的helper函数% example作为支持文件，以可视化最终输出outputPtCloud = helperVisualiseOutput (ptCloud leftCurb rightCurb,.．.[]);其他的ptCloud =选择(ptCloud roiIdx);使用附加到它的helper函数% example作为支持文件，以可视化最终输出outputPtCloud = helperVisualiseOutput (ptCloud curbPoints);结束candidateCurbQueue = [candidateCurbQueue; candidateCurbPoints];如果<= candidateCurbQueueCount <= candidateCurbQueueCount + 1;其他的candidateCurbQueue = candidateCurbQueue(2:结束);结束prevOffRoadPtCloud = offRoadPtCloud;prevAngle = roadAngles;如果isOpen(球员)视图(球员,outputPtCloud);视图(球员。一个xes,[0 90]) camva(player.Axes,0)结束结束

分析路缘跟踪的漂移和平稳性

为了分析路沿检测结果，将其与跟踪的路沿多项式进行比较，绘制在图中。每个图比较使用和不使用卡尔曼滤波的参数。第一个图比较了路缘沿y轴的漂移，第二个图比较了路缘多项式的平滑性。平滑度是斜率的变化率。

图ax = subplot(2,1,1);情节(leftTracker.drift)在情节(leftTracker.filteredDrift)从标题(“左路缘沿y轴漂移”) ax.Position(2) = ax.Position(2) - 0.08;ax =次要情节(2,1,2);情节(rightTracker.drift)在情节(rightTracker.filteredDrift)从标题(“右路沿y轴漂移”) ax.Position(2) = ax.Position(2) - 0.06;Lgnd =传奇(“漂移价值观”，“过滤漂移价值观”，.．.“定位”，“水平”）;Lgnd.Position(1:2) = [0.29 0.9];

图中包含2个轴对象。标题为Left Curb Drift Along y轴的Axes对象1包含2个类型为line的对象。标题为右遏制沿y轴漂移的Axes对象2包含2个类型为line的对象。这些对象表示漂移值，经过过滤的漂移值。

图ax = subplot(2,1,1);情节(leftTracker.curveSmoothness);持有在情节(leftTracker.filteredCurveSmoothness)从标题(“左曲线平滑度”) ax.Position(2) = ax.Position(2) - 0.08;ax =次要情节(2,1,2);情节(rightTracker.curveSmoothness);持有在情节(rightTracker.filteredCurveSmoothness)从标题(“正确的曲线光滑”) ax.Position(2) = ax.Position(2) - 0.06;Lgnd =传奇(“曲线平滑度”，“曲线平滑过滤”，.．.“定位”，“水平”）;Lgnd.Position(1:2) = [0.23 0.9];

图中包含2个轴对象。标题为Left Curve Smoothness的Axes对象1包含2个line类型的对象。标题为Right Curve Smoothness的Axes对象2包含两个类型为line的对象。这些对象表示曲线平滑度，经过过滤的曲线平滑度。

辅助函数

的helperDownloadPandasetData助手函数将激光雷达数据集加载到MATLAB工作空间中。

函数[ptCloudData,labelsData] = helperDownloadPandasetData() lidarDataTarFile = matlab.internal.examples.downloadSupportFile(激光雷达的，.．.“数据/ CurbPointCloudData.tar”）;filepath = fileparts (lidarDataTarFile);outputFolder = fullfile (filepath,“CurbPointCloudData”）;检查是否下载了tar文件，但未解压。如果(~存在(fullfile (outputFolder“激光雷达”)，“文件”）).．.& & (~ (fullfile (outputFolder,存在“semanticLabels”)，“文件”)解压(lidarDataTarFile outputFolder);结束lidarFiles = dir (fullfile (outputFolder激光雷达的，‘* .pcd‘));labelFiles = dir (fullfile (outputFolder“semanticLabels”，‘* . png”));ptCloudData =细胞(元素个数(lidarFiles), 1);labelsData =细胞(元素个数(labelFiles), 1);为fileIdx = 1:numel(lidarFiles) ptCloud = pcread(fullfile(lidarFiles(fileIdx).folder,lidarFiles(fileIdx).name));这个例子遵循自我载体运动的惯例。%的正x轴，因此，变换点云θ= -90;Trans = [0 0 0];Tform = rigidtform3d([0 0 theta]，trans);ptCloudData {fileIdx} = pctransform (ptCloud tform);labelsData {fileIdx} = imread (fullfile (labelFiles (fileIdx) .folder labelFiles (fileIdx) . name));结束结束

的helperTrackCandidateCurbs助手函数使用卡尔曼滤波器跟踪候选约束点。

函数varargout = helperTrackCandidateCurbs(varargin)多项式= varargin{1};xVal =变长度输入宗量{2};modelParams =变长度输入宗量{3};如果numel(varargin) == 4 initParams = varargin{4};initParams。initialEstimateError = initParams.initialEstimateError (1:2);initParams。motionNoise = initParams.motionNoise (1:2);curveInitialParameters =多项式(1:2);driftInitialParameters =多项式(3);配置卡尔曼滤波过滤器。curveFilter = configureKalmanFilter(.．.“ConstantVelocity”curveInitialParameters,.．.initParams.initialEstimateError,.．.initParams.motionNoise,.．.initParams.measurementNoise);过滤器。driftFilter = configureKalmanFilter (.．.“ConstantVelocity”driftInitialParameters,.．.initParams.initialEstimateError,.．.initParams.motionNoise,.．.initParams.measurementNoise);varargout{1} =过滤器;其他的过滤器=变长度输入宗量{4};追踪=变长度输入宗量{5};预测(filter.curveFilter);预测(filter.driftFilter);跟踪器。漂移= [tracker.drift;多项式(3)];跟踪器。curveSmoothness = [tracker.curveSmoothness; polynomial(1)];%使用卡尔曼滤波校正多项式updatedCurveParams =正确(filter.curveFilter,.．.多项式(1:2));updatedDriftParams =正确(filter.driftFilter,.．.多项式(3));%更新多项式多项式= [updatedCurveParams, updatedDriftParams];跟踪器。filteredDrift = [tracker.filteredDrift;多项式(3)];跟踪器。filteredCurveSmoothness = [tracker.filteredCurveSmoothness;多项式(1)];varargout{1} =多项式;varargout{3} =追踪;结束%坐标估计yVal = polyval(多项式,xVal);% z坐标估计zVal = (-modelParams(1)*xVal -modelParams(2)*yVal -modelParams(4)).．./ modelParams (3);%的最终控制遏止= [xVal' yVal' zVal'];varargout{2} =抑制;结束

参考文献

[1]张，一欢，王军，王小年，约翰·m·多兰。基于道路分割的3d激光雷达自动驾驶路缘检测方法。IEEE智能交通系统汇刊19日,没有。12(2018年12月):3981-91。https://doi.org/10.1109/TITS.2018.2789462．

王国军，吴健，何睿，田斌。基于激光雷达数据的道路边界检测的速度和准确性权衡。自动化学报8,不。6(2021年6月):1210-20。https://doi.org/10.1109/JAS.2020.1003414．

[3]胡，九翔，Anshuman Razdan, John C. Femiani, Ming Cui, Peter Wonka。基于道路足迹跟踪的航拍图像路网提取与交叉口检测。《IEEE地球科学与遥感汇刊》45岁的没有。12(2007年12月):4144-57。https://doi.org/10.1109/TGRS.2007.906107．