主要内容

用激光雷达数据构建地图

这个例子展示了如何处理安装在车辆上的传感器的3-D激光雷达数据,并借助惯性测量单元(IMU)读数逐步构建地图。这样的地图可以方便的路径规划车辆导航或可用于定位。为了评估生成的地图,本示例还演示了如何将车辆的轨迹与全球定位系统(GPS)记录进行比较。

概述

高清晰度(HD)地图是一种提供精确道路几何图形的地图服务,精确度可达几厘米。这种精度水平使高清地图适用于自动驾驶流程,如定位和导航。这种高清地图是由3-D激光雷达扫描生成的,结合高精度GPS和或IMU传感器,可以用于定位车辆在几厘米内。这个示例实现了构建这样一个系统所需的特性子集。

在这个例子中,您将学习如何:

  • 加载,探索和可视化记录的驾驶数据

  • 用激光雷达扫描建立一张地图

  • 使用IMU读数改进地图

加载并浏览记录的驾驶数据

本例中使用的数据来自此GitHub®存储库,表示约100秒的激光雷达、GPS和IMU数据。数据以mat文件的形式保存,每个文件包含一个时间表.从库中下载mat -文件,并将它们加载到MATLAB®工作空间中。

注意:这个下载可能需要几分钟。

baseDownloadURL=“https://github.com/mathworks/udacity-self-driving-data-subset/raw/master/drive_segment_11_18_16/”; dataFolder=fullfile(tempdir,“驾驶室段11、18、16”, filesep);选择= weboptions (“超时”,Inf);lidarFileName=dataFolder+“lidarPointClouds.mat”;imuFileName = dataFolder +“iMuoOrientations.mat”;gpsFileName = dataFolder +“gpsSequence.mat”;folderExists =存在(dataFolder,“dir”);matfilesExist =存在(lidarFileName,“文件”) & &存在(imuFileName“文件”...& &存在(gpsFileName“文件”);如果~ folderExists mkdir (dataFolder);结束如果~ matfilesExist disp (“下载lidarPointClouds。垫(613 MB)…') websave(lidarFileName, baseDownloadURL +“lidarPointClouds.mat”、选择);disp (“下载imuOrientations。垫(1.2 MB)……”) websave(imuFileName, baseDownloadURL +“iMuoOrientations.mat”、选择);disp (“下载gpsSequence。垫(3 KB)……”) websave(gpsFileName, baseDownloadURL +“gpsSequence.mat”、选择);结束
下载lidarPointClouds。垫(613 MB)…下载imuOrientations。垫(1.2 MB)…下载gpsSequence。垫(3 KB)……

首先,加载Velodyne®HDL32E激光雷达保存的点云数据。激光雷达的每次扫描数据都被存储为三维点云pointCloud对象。该对象使用K-d树数据结构在内部组织数据,以加快搜索速度。与每次激光雷达扫描相关的时间戳记录在时间时间表的变化。

从MAT-file加载激光雷达数据data =负载(lidarFileName);lidarPointClouds = data.lidarPointClouds;%显示激光雷达数据的前几行头(lidarPointClouds)
ans = 8×1 PointCloud时间表的时间  _____________ ______________ 23:46:10.5115 1×1 pointCloud 23:46:10.6115 1×1 pointCloud 23:46:10.7116 1×1 pointCloud 23:46:10.8117 1×1 pointCloud 23:46:10.9118 1×1 pointCloud 23:46:11.0119 1×1 pointCloud 23:46:11.1120 1×1 pointCloud 23:46:11.2120 1×1 pointCloud

从MAT-file加载GPS数据。的纬度经度,高度变量时间表用于存储由车载GPS设备记录的地理坐标。

载入GPS序列从MAT-filedata =负载(gpsFileName);gpsSequence = data.gpsSequence;%显示前几行GPS数据头(gpsSequence)
4.4.4-4.4 -4.4 4-4.4-4.4-4.4-11-11.11-4 4.5 23:46:12.5 5 4.4 4-4 4-4 4 4.11 11:46:12.4 4 4:12.5 10 10 10:12.4 4 4 4:12.4 4 4 4 4.4 4.4 4 4.4 4-4 4 4 4 4-4-12.4 4 4-12.4 4-12.4 4 4 4 4 4 4 4 4-12.11 11 11-11 11 11 11-11 11 11 11 11-4 4 4.5 5 5 5 5 5 5 5 5 5 5 11 11 11 11 11 11 11 11 11 11 11 11 11 11 11 11 11 11 11-5 5 5 5 5 5 5 23:13:10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 10 16.456737.4-122.11-42.523:46:17.457337.4-122.11-42.523:46:18.465637.4-122.11 -42.5

从mat -文件加载IMU数据。IMU通常由单个传感器组成,报告有关车辆运动的信息。它们结合了多个传感器,包括加速度计、陀螺仪和磁力计。的取向变量存储IMU传感器的报告方向。这些读数以四元数报告。每个读数被指定为包含四个四元数部分的1乘4向量。把1 × 4向量转换成a四元数对象。

从mat文件加载IMU录音data = load(imufilename);Imuorientations = data.imuorientation;%将IMU录音转换为四元数类型imuOrientations = convertvars (imuOrientations,“定位”“四元数”);%显示IMU数据的前几行头(imuOrientations)
ans=8×1时间表时间方向:11.4570 1×1四元数23:46:11.4605 1×1四元数23:46:11.4620 1×1四元数23:46:11.4655 1×1四元数23:46:11.4655 1×1四元数23:11.4670 1×1四元数23:46:11.4670 1×1四元数23:46:11.4670 1×1四元数23:46:11.4705 1×1四元数23:11.471四元数

为了理解传感器读数是如何传入的,对每个传感器计算近似的帧持续时间

lidarframeduration =中位数(diff(lidarpointclouds.time));gpsframedrion =中值(diff(gpssequence.time));imuframeduration =中位数(差异(Imuorientation.time));%将显示格式调整为秒lidarFrameDuration。格式=“年代”;gpsFrameDuration。格式=“年代”;imuFrameDuration。格式=“年代”计算帧速率lidarRate = 1 /秒(lidarFrameDuration);gpsRate = 1 /秒(gpsFrameDuration);imuRate = 1 /秒(imuFrameDuration);%显示帧持续时间和速率流('激光雷达:%s, %3.1f Hz\n',char(lidarFrameDuration),lidarRate;fprintf('GPS: %s, %3.1f Hz\n'char (gpsFrameDuration), gpsRate);流('IMU: %s, %3.1f Hz\n'char (imuFrameDuration), imuRate);
激光雷达:0.10008秒,10.0 Hz GPS: 1.0001秒,1.0 Hz IMU: 0.002493秒,401.1 Hz

GPS传感器是最慢的,运行速度接近1hz。激光雷达是下一个最慢的,以接近10hz的速率运行,其次是IMU以接近400hz的速率运行。

数据可视化驾驶

要了解场景包含的内容,请使用流媒体播放器可视化记录的数据。要可视化GPS读数,请使用geoplayer.使用可视化激光雷达读数pcplayer

%创建地质图层以可视化流式地理坐标latCenter = gpsSequence.Latitude (1);lonCenter = gpsSequence.Longitude (1);zoomLevel = 17;gpsPlayer = geoplayer(latCenter, lonCenter, zoomLevel);绘制完整的路线plotRoute (gpsPlayer gpsSequence。纬度,gpsSequence.Longitude);确定玩家的极限Xlimits = [-45 45];%仪表Ylimits = [-45 45];Zlimits = [-10 20];%创建一个pcplayer以可视化来自激光雷达传感器的流点云lidarPlayer=pcplayer(xlimits、ylimits、zlimits);%自定义播放器轴标签包含(lidarPlayer。轴,“X (m)”) ylabel (lidarPlayer。轴,“Y (m)”) zlabel (lidarPlayer。轴,‘Z(m)’)标题(lidarPlayer。轴,“激光雷达传感器数据”%在屏幕上对齐玩家helperAlignPlayers ({gpsPlayer, lidarPlayer});通过GPS读数外回路(信号变慢)g = 1:高度(gpsSequence从时间表中提取地理坐标纬度=gpsSequence.纬度(g);经度=gpsSequence.经度(g);%更新当前位置在GPS显示plotPosition (gpsPlayer,经度和纬度);%计算当前和下一次GPS读取之间的时间跨度时间跨度=时间范围(gpsSequence.Time(g), gpsSequence.Time(g+1));%提取这段时间内记录的激光雷达帧lidarFrames=lidarPointClouds(时间跨度:);%内环激光雷达读数(更快的信号)l = 1:高度(lidarFrames)%提取点云ptCloud = lidarFrames.PointCloud (l);%更新激光雷达显示器视图(lidarPlayer、ptCloud);%暂停以减慢显示速度暂停(0.01)结束结束

使用记录的激光雷达数据建立地图

激光雷达是一种功能强大的传感器,可以在其他传感器不起作用的环境中进行感知。它们提供了车辆环境的详细、完整的360度视图。

%隐藏玩家隐藏(gpsPlayer)隐藏(lidarPlayer)%选择一帧激光雷达数据来演示注册流程frameNum = 600;ptCloud = lidarPointClouds.PointCloud (frameNum);%显示和旋转自我视图以显示激光雷达数据helperVisualizeEgoView (ptCloud);

激光雷达可以用来制作厘米精度的高清地图,包括整个城市的高清地图。这些地图以后可以用于车内定位。构建这种地图的一个典型方法是将从移动车辆获得的连续激光雷达扫描对齐,并将它们组合成单个大点云。本示例的其余部分将探讨这种构建映射的方法。

  1. 结合激光雷达扫描:使用点云配准技术,如迭代最近点(ICP)算法或正态分布变换(NDT)算法,对齐连续的激光雷达扫描。看到pcregistericpPcregisterndt.有关每种算法的更多详细信息,请参阅。本例使用NDT,因为它通常更精确,尤其是在考虑旋转时Pcregisterndt.函数返回刚性变换,使移动点云相对于参考点云对齐。通过连续地构成这些转换,每个点云被转换回第一点云的参考帧。

  2. 结合对齐扫描:一旦一个新的点云扫描被注册并转换回第一个点云的参考框架,点云可以与第一个点云合并使用pcmerge

首先,获取与附近激光雷达扫描相对应的两点云。要加快处理速度,并在扫描之间积累足够的运动,请使用每十次扫描。

skipFrames = 10;frameNum = 100;固定= lidarPointClouds.PointCloud (frameNum);= lidarPointClouds移动。PointCloud (frameNum + skipFrames);

在注册之前,处理点云,以便在点云中保留与众不同的结构。这些预处理步骤包括以下步骤:

  • 检测并拆除接地面

  • 侦测并移除自我载具

对这些步骤进行了更详细的描述用激光雷达探测地面和障碍物的例子。在这个例子中helperProcessPointCloudHelper函数完成这些步骤。

fixedProcessed = helperProcessPointCloud(固定);movingProcessed = helperProcessPointCloud(移动);

在俯视图中显示原始和处理过的点云。在加工过程中去除了品红点。这些点对应于地平面和车辆。

hFigFixed=图;pcshowpair(固定,固定处理)视图(2);%调整视图以显示顶部视图helperMakeFigurePublishFriendly (hFigFixed);在配准前对点云进行下采样。将采样提高%配准精度和算法速度。downsamplePercent = 0.1;fixedDownsampled = pcdownsample (fixedProcessed,“随机”, downsamplePercent);movingDownsampled = pcdownsample (movingProcessed,“随机”, downsamplePercent);

对点云进行预处理后,使用无损检测进行注册。在注册之前和之后可视化对齐。

regGridStep=5;tform=pcregisterndt(移动下采样、固定下采样、regGridStep);movingReg=pctransform(movingProcessed,tform);%在注册前和注册后的顶部视图中可视化对齐hFigAlign =图;subplot(121) pcshowpair(movingProcessed, fixedProcessed) title(之前注册的) view(2) subplot(122) pcshowpair(movingReg, fixedProcessed) title(注册后的)视图(2)helperMakeFigurePublishFriendly (hFigAlign);

注意点云在配准后对齐良好。即使点云是紧密对齐的,对齐仍然不是完美的。

接下来,合并点云使用pcmerge

mergeGridStep = 0.5;ptCloudAccum = pcmerge(fixedProcessed, movingReg, mergeGridStep); / /将数据合并hFigAccum =图;pcshow (ptCloudAccum)标题(“累积点云”)视图(2)helperMakeFigurePublishFriendly(hFigAccum);

现在,对单个点云的处理管道已经很好地理解了,将它们放在一个循环中,覆盖整个记录的数据序列。的helperLidarMapBuilderClass将所有这些放在一起。的updateMap类的方法采用一个新的点云,并执行前面详细介绍的步骤:

  • 对点云进行处理,去除地平面和ego vehicleprocessPointCloud方法。

  • 对点云下采样。

  • 估计合并先前点云和当前点云所需的刚性转换。

  • 将点云转换回第一帧。

  • 将点云与累积点云映射合并。

此外,updateMap方法还接受一个初始转换估计,该估计用于初始化注册。良好的初始化可以显著改善注册的结果。相反,糟糕的初始化会对注册产生不利影响。提供良好的初始化还可以提高算法的执行时间。

为注册提供初始估计的一种常见方法是使用恒定速度假设。使用来自先前迭代的转换作为初始估计。

updateDisplay方法另外创建和更新一个二维俯视图流点云显示。

创建一个地图构建器对象mapBuilder=helperLidarMapBuilder(“DownsamplePercent”, downsamplePercent);%设置随机数种子rng (0);closeDisplay = false;numFrames =身高(lidarPointClouds);tform = rigid3d;n=1:skipFrames:numFrames-skipFrames得到第n个点云ptcloud = lidarpointclouds.pointCloud(n);使用来自先前迭代的转换作为初始估计点云配准的当前迭代。(恒定速度)initTform = tform;%使用点云更新地图tform = updateMap(mapBuilder, ptCloud, initTform);%更新地图显示updateDisplay (mapBuilder closeDisplay);结束

仅点云注册就可以构建车辆所经过环境的地图。虽然地图可能在局部上看起来是一致的,但它可能在整个序列上产生了显著的漂移。

使用记录的GPS读数作为地面真实轨迹,以视觉评估已建成地图的质量。首先将GPS读数(纬度、经度、高度)转换为当地坐标系统。选择一个与序列中第一个点云的原点重合的局部坐标系。这个转换是通过两个转换计算的:

  1. 将GPS坐标转换为本地笛卡尔东-北-北坐标latlon2local作用从轨迹起点开始的GPS位置用作参考点,并定义局部x、y、z坐标系的原点。

  2. 旋转笛卡尔坐标,使局部坐标系与第一个激光雷达传感器坐标对齐。由于不知道车辆上激光雷达和GPS的确切安装配置,因此会对其进行估计。

%选择参考点作为首次GPS读数原点=[gpsSequence.纬度(1)、gpsSequence.经度(1)、gpsSequence.海拔(1)];%将GPS读数转换为当地的东-北-北坐标系统[xEast, yNorth, zUp] = latlon2local(gpsSequence。纬度,gpsSequence。经度,...gpsSequence。高度,origin);在轨迹开始时估计粗方向以对齐局部ENU%系统与激光雷达坐标系统theta =中值(atan2d(yNorth(1:15), xEast(1:15)));R = [cosd(90-theta) sind(90-theta) 0;信德(90 -θ)cosd(90 -θ)0;0 0 1);旋转ENU坐标与激光雷达坐标系统对齐groundTruthTrajectory = [xEast, yNorth, zUp] * R;

在建立的地图上叠加地面真相轨迹。

(mapBuilder。轴,“上”)散射(mapBuilder。轴,groundTruthTrajectory(:,1), groundTruthTrajectory(:,2),...“绿色”“填充”);helperAddLegend (mapBuilder。轴,...{“地图点”“估计轨迹”“地面真相轨道”});

初始转弯后,估计轨迹明显偏离地面真轨迹。仅使用点云配准估计的轨迹可能会漂移,原因有以下几个:

  • 传感器的噪声扫描没有足够的重叠

  • 例如,缺乏足够强大的功能,近长道路

  • 不准确的初始转换,特别是当旋转是重要的。

%关闭地图显示updateDisplay(mapBuilder,true);

使用IMU方向改进已建地图

IMU是安装在平台上的电子设备。imu包含多个传感器,可以报告有关车辆运动的各种信息。典型的imu包括加速度计、陀螺仪和磁力计。IMU可以提供可靠的方位测量。

使用IMU读数为注册提供一个更好的初始估计。本例中使用的imu报告的传感器读数已经在设备上进行了过滤。

重置地图生成器以清除先前生成的地图重置(mapBuilder);%设置随机数种子rng (0);initTform = rigid3d;n=1:skipFrames:numFrames-skipFrames得到第n个点云ptcloud = lidarpointclouds.pointCloud(n);如果n > 1由于IMU传感器以更快的速度报告读数,请收集%自上次激光雷达扫描以来报告的IMU读数。prevTime = lidarPointClouds。时间(n - skipFrames);currTime = lidarPointClouds.Time (n);timeSinceScan = timerange(prevTime, currTime);imuReadings = imuOrientations (timeSinceScan,“定位”);%使用IMU读数形成初步估计initTform = helperComputeInitialEstimateFromIMU(imureads, tform);结束%使用点云更新地图tform = updateMap(mapBuilder, ptCloud, initTform);%更新地图显示updateDisplay (mapBuilder closeDisplay);结束在新地图上%叠加地面真理轨迹(mapBuilder。轴,“上”)散射(mapBuilder。轴,groundTruthTrajectory(:,1), groundTruthTrajectory(:,2),...“绿色”“填充”);helperAddLegend (mapBuilder。轴,...{“地图点”“估计轨迹”“地面真相轨道”});%捕获用于发布的快照snapnow;%关闭打开数字关闭([hFigFixed、hFigAlign hFigAccum]);updateDisplay(mapBuilder,true);

使用来自IMU的方向估计显著改进了配准,导致更接近的轨迹和更小的漂移。

金宝app支持功能

helperAlignPlayers对齐流播放器的单元阵列,使它们在屏幕上从左到右排列。

函数helperAlignPlayers(球员)validateattributes(球员,{“细胞”}, {“向量”});hasAxes = cellfun (@ (p) isprop (p,'轴'),球员,“UniformOutput”,对);如果~ (hasAxes)错误(“希望所有观众都能拥有Axes属性”);结束拉=得到(大的,“拉”); 屏幕边距=[50100];PlayerSize=cellfun(@getPlayerSize,玩家,“UniformOutput”、假);playerSizes = cell2mat (playerSizes);maxHeightInSet = max (playerSizes(1:3:结束));垂直排列球员,使最高的球员是100像素%。位置=圆形([ScreenMargin(1),屏幕大小(4)-ScreenMargin(2)-MaxheightInset]);n=1:numel(players)player=players{n};hFig=祖先(player.Axes,“图”);hFig.OuterPosition(1:2) =位置;向右设置下一个位置location = location + [50+hFig.OuterPosition(3), 0]; / /当前位置结束函数深圳= getPlayerSize(观众)%获取父图形容器h =祖先(查看器。轴,“图”);深圳= h.OuterPosition (3:4);结束结束

helperVisualizeEgoView通过围绕中心旋转,在ego透视图中可视化点云数据。

函数球员= helperVisualizeEgoView (ptCloud)%创建一个pcplayer对象xlimits = ptCloud.XLimits;ylimits = ptCloud.YLimits;zlimits = ptCloud.ZLimits;Player = pcplayer(xlimits, ylimits, zlimits);关闭轴线轴(球员。轴,“关闭”);设置摄像机显示自我视图camproj(球员。轴,“视角”);camva(球员。轴,90);坎波斯(球员。轴,[0 0 0]); camtarget(player.Axes, [-1 0 0]);%设置一个变换,旋转5度θ= 5;R = [cosd(theta) sind(theta) 0 0 -sind(theta) cosd(theta) 0 0 0 1 0 0 0 1];rotateByTheta = rigid3d (R);N = 0: theta: 359%将点云旋转为ptCloud = pctransform(ptCloud, rotateByTheta);显示点云查看(播放器、云端);暂停(0.05)结束结束

helperProcessPointCloud通过删除属于地平面或车辆的点来处理点云。

函数ptCloudProcessed = helperProcessPointCloud (ptCloud)%检查点云是否有组织isOrganized = ~ ismatrix (ptCloud.Location);%如果有组织点云,请使用基于范围的洪水填充算法% (segmentGroundFromLidarData)。否则,使用平面配件。groundSegmentationMethods = [“planefit”“rangefloodfill”];方法= groundSegmentationMethods (isOrganized + 1);如果方法= =“planefit”%线段地面作为主导平面,参考法向量%指向正z方向,使用pcfitplane。为组织%点云,考虑使用SegmentGroundFromLidardata。maxDistance = 0.4;%仪表maxAngDistance = 5;%度refVector = [0,0,1];% z方向[~, grounddinices] = pcfitplane(ptCloud, maxDistance, refVector, maxAngDistance);埃尔塞夫方法= =“rangefloodfill”使用基于范围的洪水填充分割地面。groundIndices = segmentGroundFromLidarData (ptCloud);其他的错误(“预期的方法是‘planefit’或‘rangefloodfill’”结束在给定的传感器半径内,以点的形式分割自我车辆传感器位置= [0,0,0];半径= 3.5;egoIndices = findNeighborsInRadius(ptCloud, sensorLocation, radius);%移除属于地面或车辆的点ptsToKeep = true (ptCloud。统计,1);ptsToKeep (groundIndices) = false;ptsToKeep (egoIndices) = false;%如果点云已组织,请保留组织结构如果isOrganized ptCloudProcessed = select(ptCloud, find(ptsToKeep)),“OutputSize”“全部”);其他的ptCloudProcessed = select(ptCloud, find(ptsToKeep));结束结束

helperComputeInitialEstimateFromIMU使用IMU方向读数和先前估计的变换,估计NDT的初始变换。

函数tform = helperComputeInitialEstimateFromIMU(imureads, prevTform)%使用先前估计的转换初始化转换tform = prevTform;%如果没有IMU读数,返回如果高度(imuReadings)<=1返回结束% IMU方向读数报告为四元数表示到车身框架的旋转偏移%。计算方向变化%间隔期间第一次和最后一次报告的IMU方向之间%的激光雷达扫描。q1 = imuReadings.Orientation (1);q2 = imuReadings.Orientation(结束);计算第一次和最后一次IMU读取之间的旋转偏移% -从q2帧旋转到体帧%-从车身框架旋转到q1框架Q = q1 * conj(q2);%转换为欧拉角yawPitchRoll =欧拉(q,“ZYX股票”“点”);放弃俯仰和滚转角度估计。只使用方向角估计%从IMU方向。yawPitchRoll (2:3) = 0;转换回旋转矩阵q =四元数(yawPitchRoll,“欧拉”“ZYX股票”“点”);R = rotmat (q,“点”);使用计算旋转tform。T(1:3, 1:3) = r ';结束

helperAddLegend将图例添加到坐标轴上。

函数helperAddLegend (hAx、标签)给坐标轴添加一个图例hLegend = legend(hAx, labels{:});设置文本颜色和字体权重hLegend。TextColor = [1 1 1];hLegend。FontWeight =“大胆的”结束

helperMakeFigurePublishFriendly调整数字,以便发布捕获的屏幕截图是正确的。

函数helperMakeFigurePublishFriendly (hFig)如果~isempty(hFig)和&isvalid(hFig)hFig.Handleviability=“回调”结束结束

另请参阅

功能

对象

相关的话题

外部网站