从激光雷达数据建立一个映射
这个例子展示了如何处理三维激光雷达数据从一个传感器安装在车辆逐步建立一个地图,在惯性测量单元(IMU)协助下读数。这样可以促进车辆路径规划导航地图或可用于本地化。对生成的地图,这个示例还展示了如何比较车辆的轨迹和全球定位系统(GPS)录音。
概述
高清晰度(HD)地图是地图服务提供精确的几何道路几厘米的精度。这种程度的准确性使高清地图适合自动驾驶工作流如定位和导航。这样高清地图构建地图生成的3 d激光雷达扫描,结合高精度GPS和IMU传感器和可用于本地化汽车在几厘米。这个例子实现所需功能的一个子集建立这样一个系统。
在这个例子中,您将了解如何:
负载,探索和可视化驾驶记录数据
使用激光雷达扫描构建地图
提高地图使用IMU读数
负载和探索驾驶记录数据
在这个例子中是使用的数据这GitHub®库,代表了大约100秒激光雷达、GPS和IMU数据。数据保存在mat文件的形式,每个包含一个时间表
注意:这个下载可以花几分钟。
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)……
首先,从调速发电机加载点云数据保存®HDL32E激光雷达。每个扫描激光雷达作为一个三维点云数据存储使用pointCloud时间变量的时间表。
%从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设备。
%从MAT-file装载GPS序列data =负载(gpsFileName);gpsSequence = data.gpsSequence;%显示前几排的GPS数据头(gpsSequence)
ans = 8×3时间表的时间经度纬度海拔_________________ ________ ________ ____ 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-file IMU数据。一个IMU通常由单个传感器报告的运动车辆的信息。他们把多个传感器,包括加速度计、陀螺仪和磁力计。的取向变量存储乌兹别克斯坦伊斯兰运动的报道方向传感器。这些数据报告为四元数。每个阅读被指定为一个1-by-4向量包含四个四元数的部分。1-by-4向量转换为一个四元数(自动驾驶工具箱)
%从MAT-file加载IMU录音data =负载(imuFileName);imuOrientations = data.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.1 f赫兹\ n”char (lidarFrameDuration), lidarRate);流(全球定位系统(GPS): % s, % 3.1 f赫兹\ n 'char (gpsFrameDuration), gpsRate);流(IMU: % s, % 3.1 f赫兹\ n”char (imuFrameDuration), imuRate);
激光雷达:0.10008秒,10.0赫兹全球定位系统(GPS): 1.0001秒,1.0赫兹IMU: 0.002493秒,401.1赫兹
GPS传感器是最慢的,运行速度接近1赫兹。激光雷达是未来慢,运行速度接近10 Hz,紧随其后的是乌兹别克斯坦伊斯兰运动的速度几乎400 Hz。
数据可视化驾驶
了解现场包含、可视化使用流媒体播放器的记录数据。可视化的GPS数据,使用geoplayer(自动驾驶工具箱)pcplayer
%创建一个geoplayer可视化流地理坐标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 = 20 [-10];%创建一个pcplayer可视化流点云从激光雷达传感器lidarPlayer = pcplayer (xlimits ylimits zlimits);%定制球员轴标签包含(lidarPlayer.Axes“X (m)”)ylabel (lidarPlayer.Axes“Y (m)”)zlabel (lidarPlayer.Axes“Z (m)”)标题(lidarPlayer.Axes“激光雷达传感器数据”)%使玩家在屏幕上helperAlignPlayers ({gpsPlayer, lidarPlayer});%外循环GPS数据(慢信号)为g = 1:高度(gpsSequence) 1%提取地理坐标的时间表纬度= gpsSequence.Latitude (g);经度= gpsSequence.Longitude (g);%更新当前位置的全球定位系统(GPS)显示plotPosition (gpsPlayer,经度和纬度);%计算当前和未来之间的时间跨度GPS阅读时间间隔= timerange (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);
激光雷达可用于构建centimeter-accurate高清地图,包括高清整个城市的地图。这些地图后可用于车载本地化。典型的方法建立这样一个地图是一致连续激光雷达扫描获得的移动车辆,组合成一个大点云。剩下的这个例子探讨了这种方法建立一个地图。
结合激光雷达扫描:登记使用点云对齐连续激光雷达扫描技术的迭代最近点(ICP)算法或正态分布变换(无损检测)算法。看到
pcregistericppcregisterndtpcregisterndt函数返回的刚性变换,将移动的点云对参考点云。通过先后创作这些转换,每个点云转换回第一个点云的参考系。结合对齐扫描:一旦新的点云扫描登记并转换回第一个点云的参考系,点云可以与第一个点云合并使用
pcmerge
首先把两个点云对应于附近的激光雷达扫描。加快处理,积累足够的运动之间的扫描,每十扫描使用。
skipFrames = 10;frameNum = 100;固定= lidarPointClouds.PointCloud (frameNum);= lidarPointClouds移动。PointCloud (frameNum + skipFrames);
在登记之前,处理点云,以保持结构独特的点云。这些预处理步骤包括以下:*检测和去除地面飞机*检测和去除ego-vehicle
详细描述这些步骤使用激光雷达地面飞机和障碍物检测(自动驾驶工具箱)的例子。在这个例子中,helperProcessPointCloudhelper函数完成这些步骤。
fixedProcessed = helperProcessPointCloud(固定);movingProcessed = helperProcessPointCloud(移动);
在俯视图显示原材料点云。红色的点在处理过程中被移除。这些点对应于地平面和自我。
hFigFixed =图;pcshowpair(固定,fixedProcessed)视图(2);%调整视图显示顶视图helperMakeFigurePublishFriendly (hFigFixed);在登记之前% Downsample点云。将采样提高%登记精度和算法的速度。downsamplePercent = 0.1;fixedDownsampled = pcdownsample (fixedProcessed,“随机”,downsamplePercent);movingDownsampled = pcdownsample (movingProcessed,“随机”,downsamplePercent);
预处理后的点云,注册使用无损检测。可视化前后对齐登记。
regGridStep = 5;tform = pcregisterndt (movingDownsampled fixedDownsampled regGridStep);movingReg = pctransform (movingProcessed tform);%可视化前后对齐在顶视图登记hFigAlign =图;次要情节(121)pcshowpair (movingProcessed fixedProcessed)标题(之前注册的)视图(2)次要情节(122)pcshowpair (movingReg fixedProcessed)标题(注册后的)视图(2)helperMakeFigurePublishFriendly (hFigAlign);
注意,点云注册后很好地结合。尽管点云是紧密,排列仍然是不完美的。
接下来,点云合并使用pcmerge。
mergeGridStep = 0.5;ptCloudAccum = pcmerge (fixedProcessed movingReg mergeGridStep);hFigAccum =图;pcshow (ptCloudAccum)标题(“累积点云”)视图(2)helperMakeFigurePublishFriendly (hFigAccum);
现在为单个的点云处理管道是很好理解的,把这个在一个循环在整个序列的记录数据。的helperLidarMapBuilder类将这一切放在一起。的updateMap类的方法需要在一个新的点云与之前经过详细的步骤:
处理点云的地平面与自我车辆,使用
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坐标转换为局部笛卡儿坐标East-North-Up使用
latlon2local(自动驾驶工具箱)旋转笛卡尔坐标,以便当地坐标系统与第一个激光雷达传感器坐标。因为准确的安装配置激光雷达和GPS的车辆并不知道,他们估计。
%选择参考点作为第一个GPS阅读起源= [gpsSequence.Latitude (1) gpsSequence.Longitude (1) gpsSequence.Altitude (1)];%将GPS数据转换为本地East-North-Up坐标系统[xEast, yNorth, z上]= latlon2local (gpsSequence。gpsSequence.Longitude纬度,…gpsSequence。高度,origin);%估计粗定位轨迹调整当地ENU表示的开始%系统与激光雷达坐标系θ=值(atan2d (yNorth (1:15), xEast (1:15)));R = [cosd(90 -θ)信德(90 -θ)0;信德(90 -θ)cosd(90 -θ)0;0 0 1);% ENU表示旋转坐标结合激光雷达坐标系groundTruthTrajectory = [xEast yNorth, z上]* R;
重叠的地面实况轨迹建立地图。
持有(mapBuilder.Axes“上”)散射(mapBuilder。轴,groundTruthTrajectory (: 1), groundTruthTrajectory (:, 2),…“绿色”,“填充”);helperAddLegend (mapBuilder.Axes…{“地图点”,“估计轨迹”,“地面实况轨迹”});
在最初,估计轨迹方向的显著离地面真理轨迹。轨迹估计使用点云注册就可以漂移的原因:
嘈杂的扫描传感器没有足够的重叠
缺乏足够强大的功能,例如,在漫长的道路
不准确的初始转换,尤其是旋转是显著的。
%关闭地图显示updateDisplay (mapBuilder,真实);
使用IMU取向来提高构建地图
一个IMU电子设备安装在一个平台。艾莫斯报告包含多个传感器,各种车辆的运动信息。典型的艾莫斯将加速度计、陀螺仪和磁力计。取向的IMU可以提供可靠的测量。
使用IMU数据提供更好的初始估计登记。IMU-reported传感器读数中使用这个例子已经过滤设备上的。
%重置地图生成器之前构建的地图重置(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 (imuReadings tform);结束%更新地图使用点云tform = updateMap (mapBuilder ptCloud initTform);%更新地图显示updateDisplay (mapBuilder closeDisplay);结束%安装地面实况轨迹在新地图持有(mapBuilder.Axes“上”)散射(mapBuilder。轴,groundTruthTrajectory (: 1), groundTruthTrajectory (:, 2),…“绿色”,“填充”);helperAddLegend (mapBuilder.Axes…{“地图点”,“估计轨迹”,“地面实况轨迹”});%为出版捕获快照snapnow;%关闭打开数据关闭([hFigFixed、hFigAlign hFigAccum]);updateDisplay (mapBuilder,真实);
利用IMU的方向估计显著提高登记,导致更小漂移轨迹。
万博1manbetx支持功能
helperAlignPlayers对齐的单元阵列流玩家所以他们在屏幕上从左到右排列。
函数helperAlignPlayers(球员)validateattributes(球员,{“细胞”},{“向量”});hasAxes = cellfun (@ (p) isprop (p,“轴”),球员,“UniformOutput”,真正的);如果~ (hasAxes)错误(“预期的所有观众都有一个轴属性”);结束拉=得到(大的,“拉”);screenMargin = (100);playerSizes = cellfun (@getPlayerSize球员,“UniformOutput”、假);playerSizes = cell2mat (playerSizes);maxHeightInSet = max (playerSizes(1:3:结束));%安排球员垂直这最高的球员是100像素的%。位置=圆([screenMargin(1)拉(4)-screenMargin (2) -maxHeightInSet]);为n = 1:元素个数(球员)球员= {n}的球员;hFig =祖先(player.Axes,“图”);hFig.OuterPosition(1:2) =位置;%建立下一个位置正确位置=位置+ (50 + hFig.OuterPosition (3), 0];结束函数深圳= getPlayerSize(观众)%得到父容器图h =祖先(viewer.Axes,“图”);深圳= h.OuterPosition (3:4);结束结束
helperVisualizeEgoView可视化在自我角度旋转点云数据中心。
函数球员= helperVisualizeEgoView (ptCloud)%创建一个pcplayer对象xlimits = ptCloud.XLimits;ylimits = ptCloud.YLimits;zlimits = ptCloud.ZLimits;球员= pcplayer (xlimits ylimits zlimits);%关闭轴线轴(player.Axes“关闭”);%设置相机展示自我的观点camproj (player.Axes“视角”);camva(球员。轴,90);坎波斯(球员。轴,[0 0 0]);camtarget(球员。轴,(1 0 0));%建立转换旋转5度θ= 5;R = [cosd(θ)信德(θ)0 0信德(θ)cosd(θ)0 0 0 0 1 0 0 0 0 1);rotateByTheta = rigid3d (R);为n = 0:θ:359%旋转θ点云ptCloud = pctransform (ptCloud rotateByTheta);%显示点云视图(球员,ptCloud);暂停(0.05)结束结束
helperProcessPointCloud处理点云通过移除点属于地平面或自我。
函数ptCloudProcessed = helperProcessPointCloud (ptCloud)%组织检查点云isOrganized = ~ ismatrix (ptCloud.Location);%如果点云组织,使用基于范围洪水填充算法% (segmentGroundFromLidarData)。否则,使用平面拟合。groundSegmentationMethods = [“planefit”,“rangefloodfill”];方法= groundSegmentationMethods (isOrganized + 1);如果方法= =“planefit”%段地面作为主要平面,与参考平面的法向量使用pcfitplane %指向积极z方向。为组织%点云,考虑使用segmentGroundFromLidarData代替。maxDistance = 0.4;%米maxAngDistance = 5;%度refVector = [0, 0, 1];% z方向[~,groundIndices] = pcfitplane (ptCloud、maxDistance refVector, maxAngDistance);elseif方法= =“rangefloodfill”%段地面使用基于范围洪水填充。groundIndices = segmentGroundFromLidarData (ptCloud);其他的错误(“planefit”或“预期方法rangefloodfill”)结束%段自我汽车传感器的分在一个给定的半径sensorLocation = (0, 0, 0);半径= 3.5;egoIndices = findNeighborsInRadius (ptCloud sensorLocation,半径);%去除点属于地面或自我ptsToKeep = true (ptCloud。统计,1);ptsToKeep (groundIndices) = false;ptsToKeep (egoIndices) = false;%如果点云组织,保留组织结构如果isOrganized ptCloudProcessed =选择(ptCloud,找到(ptsToKeep),“OutputSize”,“全部”);其他的ptCloudProcessed =选择(ptCloud,找到(ptsToKeep));结束结束
helperComputeInitialEstimateFromIMU估计一个初始变换无损检测使用IMU取向读数和之前估计的转换。
函数tform = helperComputeInitialEstimateFromIMU (imuReadings prevTform)%初始化转换使用之前估计的变换tform = prevTform;%如果没有IMU数据可用,回报如果高度(imuReadings) < = 1返回;结束% IMU取向数据报告为四元数表示%旋转抵消身体框架。计算方向变化%之间的第一个和最后一个报告间隔期间IMU取向%的激光雷达扫描。q1 = imuReadings.Orientation (1);q2 = imuReadings.Orientation(结束);%计算旋转抵消之间的第一个和最后一个IMU阅读%——旋转从q2框架车身骨架%——从车底架旋转q1框架q = q1 *连词(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 =传奇(hAx、标签{:});%设置文本颜色和字体粗细hLegend。输入TextColor = (1 1 1);hLegend。FontWeight =“大胆”;结束
helperMakeFigurePublishFriendly调整数据,这样截图被公布是正确的。
函数helperMakeFigurePublishFriendly (hFig)如果~ isempty (hFig) & & (hFig) hFig是否是可用的。HandleVisibility =“回调”;结束结束
另请参阅
功能
对象
pcplayer|pointCloud|geoplayer(自动驾驶工具箱)
相关的话题
- 建立一个地图使用大满贯从激光雷达数据(自动驾驶工具箱)
- 使用激光雷达地面飞机和障碍物检测(自动驾驶工具箱)
