主要内容

根据激光雷达数据构建地图

这个例子使用了:

打开脚本

这个例子展示了如何在惯性测量单元(IMU)读数的帮助下，处理安装在车辆上的传感器的3-D激光雷达数据，逐步构建地图。这样的地图可以促进车辆导航的路径规划，也可以用于定位。为了评估生成的地图，这个例子还展示了如何将车辆的轨迹与全球定位系统(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 = exists (dataFolder，“dir”）;matfilesExist = exist(lidarFileName，“文件”) && exist(imuFileName，“文件”）.．.& &存在(gpsFileName“文件”）;如果~ folderExists mkdir (dataFolder);结束如果~ matfilesExist disp (“下载lidarPointClouds。mat (613 MB)…”(lidarFileName, baseDownloadURL + .“lidarPointClouds.mat”、选择);disp (“下载imuOrientations。mat (1.2 MB)…”websave(imuFileName, baseDownloadURL +。“imuOrientations.mat”、选择);disp (“下载gpsSequence。mat (3 KB)…'websave(gpsFileName, baseDownloadURL + .“gpsSequence.mat”、选择);结束

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

从mat文件加载激光雷达数据data = load(lidarFileName);lidarPointClouds = data.lidarPointClouds;显示前几行激光雷达数据头(lidarPointClouds)
时间点云_____________ ______________ 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)
的时间经度纬度海拔  _____________ ________ _________ ________ 23:46:11.4563 23:46:12.4563 37.4 -122.11 -42.5 37.4 -122.11 -42.5 37.4 -122.11 -42.5 23:46:13.4565 23:46:14.4455 23:46:15.4455 37.4 -122.11 -42.5 37.4 -122.11 -42.5 37.4 -122.11 -42.5 23:46:16.4567 23:46:17.4573 23:46:18.4656 37.4 -122.11 -42.5 37.4 -122.11 -42.5

从mat文件中加载IMU数据。IMU通常由报告车辆运动信息的独立传感器组成。它们结合了多种传感器，包括加速计、陀螺仪和磁力计。的取向变量存储IMU传感器报告的方向。这些读数报告为四元数。每个读数都指定为包含四个四元数部分的1 × 4向量。将1 × 4向量转换为a四元数(自动驾驶工具箱)对象。

从mat文件加载IMU录音data = load(imuFileName);imuoridings = data. imuoridings;将IMU记录转换为四元数类型imuoridings = convertvars(imuoridings，“定位”，“四元数”）;显示IMU数据的前几行头(imuOrientations)
时间方向_____________ ______________ 23:46:11.4570 1×1 quaternion 23:46:11.4605 1×1 quaternion 23:46:11.4620 1×1 quaternion 23:46:11.4655 1×1 quaternion 23:46:11.4670 1×1 quaternion 23:46:11.4705 1×1 quaternion 23:46:11.4720 1×1 quaternion 23:46:11.4755 1×1 quaternion

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

lidarFrameDuration = median(diff(lidarPointClouds.Time));gpsFrameDuration = median(diff(gpsSequence.Time));imuFrameDuration = median(diff(imuorientation . time));调整显示格式为秒lidarFrameDuration。格式=“年代”；gpsFrameDuration。格式=“年代”；imuFrameDuration。格式=“年代”；计算帧率lidarRate = 1/seconds(lidarFrameDuration);gpsRate = 1/seconds(gpsFrameDuration);imuRate = 1/seconds(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)-1从时间表中提取地理坐标latitude = gpsSequence.Latitude(g);longitude = gpsSequence.Longitude(g);更新GPS显示的当前位置plotPosition(gpsPlayer，纬度，经度);计算当前和下一个GPS读数之间的时间跨度timeSpan = timerange(gpsSequence.Time(g)， gpsSequence.Time(g+1));提取在此时间跨度内记录的激光雷达帧lidarFrames = lidarPointClouds(timeSpan，:);%内循环超过激光雷达读数(更快的信号)为l = 1: height(lidarFrames)%提取点云ptCloud = lidarFrames.PointCloud(l);更新激光雷达显示视图(lidarPlayer ptCloud);暂停以减慢显示速度。暂停(0.01)结束结束

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

激光雷达是一种功能强大的传感器，可以在其他传感器无法发挥作用的挑战性环境中进行感知。他们提供了一个详细的，完整的360度视角的车辆环境。

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

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

  1. 对齐激光雷达扫描:使用点云配准技术(如迭代最近点(ICP)算法或正态分布变换(NDT)算法)校准连续的激光雷达扫描。看到pcregistericp而且pcregisterndt有关每个算法的详细信息。本例使用NDT，因为它通常更准确，特别是在考虑旋转时。的pcregisterndt函数返回将移动点云相对于参考点云对齐的刚性变换。通过依次组合这些转换，每个点云被转换回第一个点云的参考框架。

  2. 合并对齐扫描:注册一个新的点云扫描并将其转换回第一个点云的参考框架后，可以使用将该点云与第一个点云合并pcmerge．

首先取两个点云对应于附近的激光雷达扫描。为了加快处理速度，并在扫描之间积累足够的运动，请每10次扫描一次。

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

在配准之前，处理点云以保留点云中独特的结构。这些预处理步骤包括:

  • 检测并拆除接地面

  • 发现并移除自驾车辆

中更详细地描述了这些步骤基于激光雷达的地面和障碍物检测(自动驾驶工具箱)的例子。在本例中，helperProcessPointCloudHelper函数完成了这些步骤。

fixedProcessed = helperProcessPointCloud(fixed);movingProcessed = helperProcessPointCloud(移动);

在顶部视图中显示原始和处理过的点云。洋红色的点在处理过程中被去除。这些点对应于地平面和自我载体。

hFigFixed = figure;pcshowpair(fixed, fixedProcessed)视图;调整视图以显示顶视图helperMakeFigurePublishFriendly (hFigFixed);%在注册前对点云进行下采样。将采样提高%配准精度和算法速度。downsamplePercent = 0.1;fixeddownsample = pcdownsample(fixedProcessed，“随机”, downsamplePercent);movingdownsample = pcdownsample(movingProcessed，“随机”, downsamplePercent);

在对点云进行预处理后，使用NDT对点云进行注册。可视化注册前后的对齐情况。

regGridStep = 5;tform = pcregisterndt(movingdown采样，fixeddown采样，regGridStep);movingReg = pctransform(movingProcessed, tform);在注册前和注册后在顶部视图中可视化对齐hFigAlign = figure;subplot(121) pcshowpair(movingProcessed, fixedProcessed)之前注册的pcshowpair(movingReg, fixedProcessed)注册后的helperMakeFigurePublishFriendly(hFigAlign);

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

接下来，使用合并点云pcmerge．

mergeGridStep = 0.5;ptCloudAccum = pcmerge(fixedProcessed, movingReg, mergeGridStep);hFigAccum = figure;pcshow (ptCloudAccum)标题(“累积点云”helperMakeFigurePublishFriendly(hFigAccum);

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

  • 通过删除地平面和自我载体来处理点云，使用processPointCloud方法。

  • 下采样点云。

  • 估计将以前的点云与当前点云合并所需的刚性转换。

  • 将点云转换回第一帧。

  • 将点云与累积的点云图合并。

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

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

的updateDisplay方法还会创建并更新二维顶视图流点云显示。

创建一个地图构建器对象mapBuilder = helperLidarMapBuilder(“DownsamplePercent”, downsamplePercent);设置随机数种子rng (0);closeDisplay = false;numFrames = height(lidarPointClouds);Tform = rigidtform3d;为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读数origin = [gpsSequence.Latitude(1)， gpsSequence.Longitude(1)， gpssequence . height (1)];将GPS读数转换为本地的东、北、上坐标系。[xEast, yNorth, zUp] = latlon2local(gpsSequence. gpsSequence.)纬度,gpsSequence。经度,.．.gpsSequence。高度，origin);估计轨迹开始时的粗略方向，以对齐局部ENU%系统与激光雷达坐标系theta = median(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 = rigidtform3d;为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 = imuoridings (timeSinceScan，“定位”）;使用IMU读数形成初步估计initTform = helperComputeInitialEstimateFromIMU(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(players) validateattributes(players， {)“细胞”}, {“向量”});hasAxes = cellfun(@(p)isprop(p，“轴”),球员,“UniformOutput”,真正的);如果~ (hasAxes)错误(“希望所有观看者都有一个Axes属性”）;结束screenSize = get(格鲁特，“拉”）;screenMargin = [50,100];(@getPlayerSize, players，“UniformOutput”、假);playerSizes = cell2mat(playerSizes);maxhightinset = max(playerSizes(1:3:end));垂直排列玩家，使最高的玩家距离100像素顶端。location = round([screenMargin(1)， screenSize(4)-screenMargin(2)- maxhightinset]);为N = 1: numel(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通过围绕中心旋转，在自我视角中可视化点云数据。

函数播放器= 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;eulerAngles = [0 0 theta];翻译= [0 0 0];rotateByTheta = rigidtform3d(eulerAngles, translation);为N = 0: = 359旋转点云ptCloud = pctransform(ptCloud, rotateByTheta);%显示点云视图(球员,ptCloud);暂停(0.05)结束结束

helperProcessPointCloud通过删除属于地平面或自我载体的点来处理点云。

函数ptCloudProcessed = helperProcessPointCloud(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'或'rangefloodfill'"）结束将自我车辆分割为传感器给定半径内的点。sensorLocation = [0,0,0];半径= 3.5;egoindexes = findNeighborsInRadius(ptCloud, sensorLocation, radius);移除属于地面或自我载具的点ptsToKeep = true统计,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读数，返回如果height(imureads) <= 1返回；结束% IMU方向读数报告为四元数，表示%旋转偏移到主体框架。计算方向变化%之间的第一个和最后一个报告的IMU方向之间的间隔%的激光雷达扫描。q1 = imureads . orientation (1);q2 = imureads . orientation (end);计算第一次和最后一次IMU读取之间的转动偏移量% -从q2帧旋转到主体帧% -从车身帧旋转到q1帧Q = q1 * conj(q2);%转换为欧拉角yawPitchRoll = euler(q，“ZYX股票”，“点”）;放弃俯仰和横摇角度估算。只使用头部角度估算%从IMU方向。yawPitchRoll(2:3) = 0;%转换回旋转矩阵q = quaternion(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 =“回调”；结束结束

另请参阅

功能

对象

相关的话题

外部网站