用激光雷达数据构建地图
这个例子展示了如何处理安装在车辆上的传感器的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,“drive_segment_11_18_16”, filesep);选择= weboptions (“超时”、正);lidarFileName = dataFolder +“lidarPointClouds.mat”；imuFileName = dataFolder +“imuOrientations.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 +“imuOrientations.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)
ans = 8×3时间表时间纬度经度海拔_____________ ________ _________ ________ 23:46:11.4563 37.4 -122.11 -42.5 23:46:12.4563 37.4 -122.11 -42.5 23:46:14.4455 37.4 -122.11 -42.5 23:46:15.4455 37.4 -122.11 -42.5 23:46:16.4567 37.4 -122.11 -42.5 23:46:16.4567 37.4 -122.11 -42.5 23:46:17.4573 37.4 -122.11 -42.5 23:46:17.4573-122.11 - -42.5
从mat -文件加载IMU数据。IMU通常由单个传感器组成，报告有关车辆运动的信息。它们结合了多个传感器，包括加速度计、陀螺仪和磁力计。的取向变量存储IMU传感器的报告方向。这些读数以四元数报告。每个读数被指定为包含四个四元数部分的1乘4向量。把1 × 4向量转换成a四元数对象。
从mat文件加载IMU录音数据=加载（imuFileName）；imuOrientations=数据。imuOrientations；%将IMU录音转换为四元数类型imuOrientations = convertvars (imuOrientations,“定位”，“四元数”);%显示IMU数据的前几行头(imuOrientations)
ans = 8×1时间表时间取向 _____________ ______________ 23:46: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.4670 1×1四元数23:46:11.4705 1×1四元数23:46:11.4720 1×1四元数23:46:11.4755 1×1四元数
为了理解传感器读数是如何传入的，对每个传感器计算近似的帧持续时间。
lidarFrameDuration=中值（diff（lidarPointClouds.Time））；gpsFrameDuration=中值（diff（gpsSequence.Time））；imuFrameDuration=中值（diff（imuOrientations.Time））；%调整显示格式为秒lidarFrameDuration。格式=“年代”；gpsFrameDuration。格式=“年代”；imuFrameDuration。格式=“年代”；计算帧速率lidarRate = 1 /秒(lidarFrameDuration);gpsRate = 1 /秒(gpsFrameDuration);imuRate = 1 /秒(imuFrameDuration);%显示帧持续时间和速率流('激光雷达:%s， %3.1f Hz\n'char (lidarFrameDuration), lidarRate);流('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];%创建一个pc播放器从激光雷达传感器可视化流点云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.Latitude (g);经度= gpsSequence.Longitude (g);%更新当前位置在GPS显示plotPosition (gpsPlayer,经度和纬度);%计算当前和下一次GPS读取之间的时间跨度时间跨度=时间范围(gpsSequence.Time(g)， gpsSequence.Time(g+1));%提取这段时间内记录的激光雷达帧lidarFrames = lidarPointClouds(timeSpan，:);激光雷达读数(更快的信号)为l = 1:高度(lidarFrames)提取点云ptCloud = lidarFrames.PointCloud (l);更新激光雷达显示视图(lidarPlayer ptCloud);%暂停以减慢显示速度暂停(0.01)结束结束
使用记录的激光雷达数据建立地图
激光雷达是一种功能强大的传感器，可以在其他传感器不起作用的环境中进行感知。它们提供了车辆环境的详细、完整的360度视图。
%隐藏球员隐藏(gpsPlayer)隐藏(lidarPlayer)%选择一帧激光雷达数据来演示注册流程frameNum = 600;ptCloud = lidarPointClouds.PointCloud (frameNum);%显示和旋转自我视图以显示激光雷达数据helperVisualizeEgoView (ptCloud);
激光雷达可以用来制作厘米精度的高清地图，包括整个城市的高清地图。这些地图以后可以用于车内定位。构建这种地图的一个典型方法是将从移动车辆获得的连续激光雷达扫描对齐，并将它们组合成单个大点云。本示例的其余部分将探讨这种构建映射的方法。
结合激光雷达扫描:使用点云配准技术，如迭代最近点(ICP)算法或正态分布变换(NDT)算法，对齐连续的激光雷达扫描。看到
pcregistericp和pcregisterndt有关每种算法的更多细节。本例使用NDT，因为它通常更准确，特别是在考虑旋转时。的pcregisterndt函数返回将移动点云与参考点云对齐的刚性变换。通过依次组合这些变换，每个点云将变换回第一个点云的参考坐标系。结合对齐扫描:一旦一个新的点云扫描被注册并转换回第一个点云的参考框架，点云可以与第一个点云合并使用
pcmerge。
首先取两个点云对应附近的激光雷达扫描。为了加快处理速度，并在两次扫描之间积累足够的移动，请每10次扫描一次。
skipFrames = 10;frameNum = 100;固定= lidarPointClouds.PointCloud (frameNum);= lidarPointClouds移动。PointCloud (frameNum + skipFrames);
在配准之前，对点云进行处理，以保留点云中独特的结构。这些预处理步骤包括:
检测并拆除接地面
检测并移除车辆
对这些步骤进行了更详细的描述用激光雷达探测地面和障碍物的例子。在这个例子中helperProcessPointCloudHelper函数完成这些步骤。
fixedProcessed = helperProcessPointCloud(固定);movingProcessed = helperProcessPointCloud(移动);
在俯视图中显示原始和处理过的点云。洋红色点在加工过程中被去除。这些点对应于地平面和自我飞行器。
hFigFixed =图;pcshowpair(固定,fixedProcessed)视图(2);%调整视图以显示顶部视图helperMakeFigurePublishFriendly (hFigFixed);在配准前对点云进行下采样。将采样提高%配准精度和算法速度。downsamplePercent = 0.1;fixedDownsampled = pcdownsample (fixedProcessed,“随机”, downsamplePercent);movingDownsampled = pcdownsample (movingProcessed,“随机”, downsamplePercent);
对点云进行预处理后，使用无损检测进行注册。在注册之前和之后可视化对齐。
regGridStep = 5;tform = pcregisterndt(movingdownsampling, fixeddownsampling, 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 vehicle
processPointCloud方法。对点云下采样。
估计合并先前点云和当前点云所需的刚性转换。
将点云转换回第一帧。
将点云与累积点云地图合并。
此外,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读数(纬度、经度、高度)转换为当地坐标系统。选择一个与序列中第一个点云的原点重合的局部坐标系。这个转换是通过两个转换计算的:
将GPS坐标转换为本地笛卡尔东-北-北坐标
latlon2local函数。用从轨迹开始的GPS位置作为参考点，定义了局部x、y、z坐标系的原点。旋转笛卡尔坐标，使局部坐标系统与第一个激光雷达传感器坐标对齐。由于激光雷达和GPS在车辆上的确切安装配置还不知道，它们是估计的。
%选择参考点作为首次GPS读数origin = [gpsSequence.Latitude(1)， gpsSequence.Longitude(1)， gpsSequence.Altitude(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,真实);
使用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,真实);
使用来自IMU的方向估计显著改善了配准，导致更近的轨迹和更小的漂移。
万博1manbetx支持功能
helperAlignPlayers对齐流播放器的单元阵列，使它们在屏幕上从左到右排列。
函数helperAlignPlayers(球员)validateattributes(球员,{“细胞”}, {“向量”}）;hasAxes = cellfun (@ (p) isprop (p,“斧头”),球员,“UniformOutput”,真正的);如果~ (hasAxes)错误(“希望所有观众都能拥有Axes属性”);结束拉=得到(大的,“拉”);screenMargin = [50,100];cellfun(@getPlayerSize, players，“UniformOutput”、假);playerSizes = cell2mat (playerSizes);maxHeightInSet = max (playerSizes(1:3:结束));垂直排列球员，使最高的球员是100像素%。位置=圆形（[屏幕边距（1），屏幕尺寸（4）-屏幕边距（2）-最大高度插入]）；为N = 1: numel(players) player = players{N};hFig =祖先(球员。轴,“图”);hFig.OuterPosition(1:2) =位置;向右设置下一个位置location = location + [50+hFig.OuterPosition(3)， 0]; / /当前位置结束函数深圳= getPlayerSize(观众)%获取父图形容器h =祖先(查看器。轴,“图”);深圳= h.OuterPosition (3:4);结束结束
helperVisualizeEgoView通过绕中心旋转，在自我视角中可视化点云数据。
函数球员= 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);显示点云视图(球员,ptCloud);暂停(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);elseif方法= =“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)如果~是空的(hFig) &&是有效的(hFig)HandleVisibility =“回调”；结束结束
另请参阅
功能
对象
相关的话题
外部网站
你也可以从以下列表中选择一个网站:
