从激光雷达数据构建地图
这个例子展示了如何在惯性测量单元(IMU)读数的帮助下，处理来自安装在车辆上的传感器的3-D激光雷达数据，逐步构建地图。这样的地图可以方便车辆导航的路径规划，也可以用于定位。为了评估生成的地图，这个示例还展示了如何将车辆的轨迹与全球定位系统(GPS)记录进行比较。
概述
高清晰度(HD)地图是一种地图服务，可以提供精确的道路几何形状，精度可达几厘米。这种精度水平使得高清地图适用于自动驾驶的工作流程，如定位和导航。这种高清地图是通过三维激光雷达扫描，结合高精度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 =数据文件夹+“lidarPointClouds.mat”；imuFileName =数据文件夹+“imuOrientations.mat”；gpsFileName =数据文件夹+“gpsSequence.mat”；folderExists = exist(数据文件夹“dir”）;matfilesExist =存在(lidarFileName，“文件”) && exist(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。垫(3kb)…
首先，加载从Velodyne®HDL32E激光雷达保存的点云数据。激光雷达数据的每次扫描都存储为三维点云pointCloud对象。该对象使用K-d树数据结构在内部组织数据，以便更快地搜索。与每次激光雷达扫描相关联的时间戳记录在时间可变的时间表。
从mat文件中加载激光雷达数据data = load(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文件中加载GPS数据。的纬度，经度,高度的变量时间表用于存储车载GPS设备记录的地理坐标。
从mat文件加载GPS序列data = load(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:17.4573 37.4 -122.11 -42.5 23:46:18.4656 37.4 -122.11 -122.11 -42.5 23:46:18.4656 37.4 -122.11 -122.11 -42.5 23:46:18.4656 37.4 -122.11 -42.5 23:46:15.4455 37.4
从MAT-file中加载IMU数据。IMU通常由报告车辆运动信息的单个传感器组成。它们结合了多个传感器，包括加速度计、陀螺仪和磁力计。的取向变量存储IMU传感器报告的方向。这些读数以四元数报告。每个读数都指定为包含四个四元数部分的1乘4向量。将1 × 4向量转换为a四元数对象。
从mat文件加载IMU录音data = load(imuFileName);imuOrientations = data.imuOrientations;将IMU记录转换为四元数类型imuOrientations = convertars (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 = median(diff(lidarPointClouds.Time));gpsFrameDuration = median(diff(gpsSequence.Time));imuFrameDuration = median(diff(imuorientation . time));调整显示格式为秒lidarFrameDuration。格式=“年代”；gpsFrameDuration。格式=“年代”；imuFrameDuration。格式=“年代”；计算帧速率lidarRate = 1/秒(lidarFrameDuration);gpsRate = 1/seconds(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。激光雷达是第二慢的，速度接近10赫兹，其次是IMU，速度接近400赫兹。
可视化驾驶数据
为了理解场景包含什么，使用流媒体播放器可视化记录的数据。要可视化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)-1从时间表中提取地理坐标纬度= gpsSequence.Latitude(g);经度= gpsSequence.Longitude(g);在GPS显示中更新当前位置plotPosition(gpsPlayer，纬度，经度);计算当前和下一个GPS读数之间的时间跨度timeSpan = timerrange (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有关每个算法的详细信息。本例使用无损检测，因为它通常更准确，特别是在考虑旋转时。的pcregisterndt函数返回将移动点云与参考点云对齐的刚性转换。通过依次组合这些变换，每个点云被变换回第一个点云的参考系。合并对齐扫描:注册一个新的点云扫描并转换回第一个点云的参考框架后，可以使用该点云与第一个点云合并
pcmerge．
首先取两个点云对应附近的激光雷达扫描。为了加快处理速度，并在扫描之间积累足够的运动，请每10次扫描一次。
skipFrames = 10;frameNum = 100;fixed = lidarPointClouds.PointCloud(frameNum);moving = lidarPointClouds。PointCloud(frameNum + skipFrames);
在注册之前，处理点云，以保留点云中独特的结构。这些预处理步骤包括:
检测并拆除接地平面
侦测并移除自我载具
中更详细地描述了这些步骤地面平面和障碍物检测使用激光雷达的例子。在本例中，helperProcessPointCloudHelper函数完成这些步骤。
fixedProcessed = helpprocesspointcloud (fixed);movingProcessed = helpprocesspointcloud(移动);
在顶视图中显示原始和处理过的点云。洋红色点在处理过程中被去除。这些点对应于地平面和自我载具。
hFigFixed =图;pcshowpair(fixed, fixedProcessed);调整视图以显示俯视图helperMakeFigurePublishFriendly (hFigFixed);在注册前对点云进行下采样。将采样提高配准精度和算法速度。downsamplePercent = 0.1;fixeddownsampling = pcdownsample(fixedProcessed，“随机”, downsamplePercent);movingdownsampling = pcdownsample(movingProcessed，“随机”, downsamplePercent);
对点云进行预处理后，采用无损检测对点云进行配准。可视化注册前后的对齐。
regGridStep = 5;tform = pcregisterndt(movingDownsampled, fixedDownsampled, regGridStep);movingReg = pctransform(movingProcessed, tform);在注册前后的俯视图中可视化对齐。hFigAlign =图;subplot(121) pcshowpair(movingProcessed, fixedProcessed)之前注册的pcshowpair(movingReg, fixedProcessed)注册后的视图(2)helperMakeFigurePublishFriendly(hFigAlign);
请注意，点云在配准后是很好的对齐的。即使点云紧密地对齐，但对齐仍然不完美。
接下来，合并点云使用pcmerge．
mergeGridStep = 0.5;ptCloudAccum = pcmerge(fixedProcessed, movingReg, mergeGridStep);hFigAccum =图;pcshow (ptCloudAccum)标题(“累积点云”视图(2)helperMakeFigurePublishFriendly(hFigAccum);
现在，我们已经很好地理解了单个点云对的处理管道，将它们放在整个记录数据序列的循环中。的helperLidarMapBuilderClass把所有这些放在一起。的updateMap类的方法采用一个新的点云，并执行前面详细介绍的步骤:
通过删除地平面和自我车辆来处理点云，使用
processPointCloud方法。下采样点云。
估计将之前的点云与当前点云合并所需的刚性转换。
将点云变换回第一帧。
将点云与累积的点云图合并。
此外,updateMap方法还接受初始转换估计，该估计用于初始化注册。良好的初始化可以显著提高注册结果。相反，糟糕的初始化会对注册产生不利影响。提供良好的初始化也可以提高算法的执行时间。
为配准提供初始估计的一种常用方法是使用恒定速度假设。使用来自前一个迭代的转换作为初始估计。
的updateDisplay方法还创建并更新2-D顶视图流点云显示。
创建一个映射生成器对象mapBuilder = helperLidarMapBuilder(“DownsamplePercent”, downsamplePercent);设置随机数种子rng (0);closeDisplay = false;numFrames = height(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 . height (1)];将GPS读数转换为本地东-北-上坐标系统。[xEast, yNorth, zUp] = latlon2local(gpsSequence. txt)。纬度,gpsSequence。经度,.．.gpsSequence。高度，origin);%估计轨迹开始时的粗略方向，以对准局部ENU%系统与激光雷达坐标系theta =中位数(atan2d(yNorth(1:15)， xEast(1:15)));R = [cosd(90-theta) sind(90-theta) 0;-sind(90-theta) cosd(90-theta) 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);imureads = imuOrientations(timeSinceScan，“定位”）;使用IMU读数形成初步估计initTform = helpcomputeinitialestimatefromimu (imureads, tform);结束使用点云更新地图tform = updateMap(mapBuilder, ptCloud, initTform);%更新地图显示updateDisplay (mapBuilder closeDisplay);结束在新地图上叠加地面真相轨迹(mapBuilder。轴,“上”)散射(mapBuilder。轴,groundTruthTrajectory(:,1), groundTruthTrajectory(:,2),.．.“绿色”，“填充”）;helperAddLegend (mapBuilder。轴,.．.｛“地图点”，“估计轨迹”，“地面真相轨迹”});捕获用于发布的快照snapnow;%关闭未打开数字close([hFigFixed, hFigAlign, hFigAccum]);updateDisplay (mapBuilder,真实);
使用IMU的方向估计显著改善了配准，导致更接近的轨迹和更小的漂移。
金宝app支持功能
helperAlignPlayers对齐流媒体播放器的单元格数组，使它们在屏幕上从左到右排列。
函数helperAlignPlayers(玩家)validateattributes(玩家，{“细胞”},{“向量”});hasAxes = cellfun(@(p)isprop(p，“轴”),球员,“UniformOutput”,真正的);如果~ (hasAxes)错误(“期望所有查看器都具有Axes属性”）;结束screenSize = get(格鲁特，“拉”）;screenMargin = [50,100];= cellfun(@getPlayerSize，玩家，“UniformOutput”、假);playerSizes = cell2mat(playerSizes);maxhightinset = max(playerSizes(1:3:end));垂直排列玩家，使最高的玩家距离100像素%顶端。location = round([screenMargin(1)， screenSize(4)-screenMargin(2)- maxhightinset]);为N = 1: number (players) player = players{N};hFig =祖先(播放器。轴,“图”）;hFig.OuterPosition(1:2) = location;右转设置下一个位置location = location + [50+ hfig outerposition (3)， 0];结束函数sz = getPlayerSize(查看器)获取父图形容器H =祖先(查看器。轴,“图”）;sz = h.OuterPosition(3:4);结束结束
helperVisualizeEgoView通过围绕中心旋转来可视化自我视角中的点云数据。
函数player = helperVisualizeEgoView(ptCloud)创建一个pcplayer对象xlimits = ptCloud.XLimits;ylimits = ptCloud.YLimits;zlimits = ptCloud.ZLimits;玩家= pcplayer(xlimits, ylimits, zlimits);关闭轴线轴(球员。轴,“关闭”）;设置相机显示自我视图camproj(球员。轴,“视角”）;camva(球员。轴,90);坎波斯(球员。轴,[0 0 0]); camtarget(player.Axes, [-1 0 0]);设置一个转换旋转5度Theta = 5;R = [cosd(theta) sind(theta) 0 0 -sind(theta) cosd(theta) 0 0 0 0 1 0 0 0 0 1];rotateByTheta = rigid3d(R);为N = 0: theta: 359将点云旋转ptCloud = pctransform(ptCloud, rotateByTheta);%显示点云视图(球员,ptCloud);暂停(0.05)结束结束
helperProcessPointCloud通过移除属于地平面或自我载体的点来处理点云。
函数ptCloudProcessed = helpprocesspointcloud (ptCloud)检查点云是否有组织isOrganized = ~ismatrix(ptCloud.Location);如果点云是有组织的，则使用基于范围的洪水填充算法% (segmentGroundFromLidarData)。否则，使用平面拟合。groundSegmentationMethods = [“planefit”，“rangefloodfill”];method = groundSegmentationMethods(isOrganized+1);如果方法= =“planefit”以地面为主导平面，取参考法向量%指向正z方向，使用pcfitplane。为组织%点云，考虑使用segmentGroundFromLidarData代替。maxDistance = 0.4;%米maxAngDistance = 5;%度refVector = [0,0,1];% z方向[~，groundIndices] = pcfitplane(ptCloud, maxDistance, refVector, maxAngDistance);elseif方法= =“rangefloodfill”%分段地面使用基于范围的洪水填充。grounddindices = segmentGroundFromLidarData(ptCloud);其他的错误("期望方法为'planefit'或'rangeflood '"）结束在传感器的给定半径内，将自我车辆分割为点sensorLocation = [0,0,0];半径= 3.5;egoIndices = findNeighborsInRadius(ptCloud, sensorLocation, radius);移除属于地面或自我载具的点ptsToKeep = true(ptCloud. txt)统计,1);ptsToKeep(grounddindices) = false;ptsToKeep(egoIndices) = false;%如果点云是有组织的，则保留有组织的结构如果isOrganized ptCloudProcessed = select(ptCloud, find(ptsToKeep)，“OutputSize”，“全部”）;其他的ptCloudProcessed = select(ptCloud, find(ptsToKeep));结束结束
helperComputeInitialEstimateFromIMU使用IMU定向读数和先前估计的转换估计NDT的初始转换。
函数tform = helpcomputeinitialestimatefromimu (imureads, prevTform)使用先前估计的转换初始化转换tform = prevTform;%如果没有可用的IMU读数，返回如果height(imureads) <= 1返回；结束% IMU方向读数以四元数表示%到车身框架的旋转偏移量。计算方向变化间隔内第一次和最后一次报告IMU方向之间的%%的激光雷达扫描。q1 = imureads . orientation (1);q2 = imureads . orientation (end);计算第一个和最后一个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，标签{:});设置文本颜色和字体粗细hLegend。TextColor = [1 1 1];hLegend。FontWeight =“大胆”；结束
helperMakeFigurePublishFriendly调整数字，以便发布捕获的屏幕截图是正确的。
函数helperMakeFigurePublishFriendly (hFig)如果~isempty(hFig) && isvalid(hFig)HandleVisibility =“回调”；结束结束
另请参阅
功能
对象
相关的话题
外部网站
您也可以从以下列表中选择一个网站: