基于传感器融合的三维激光雷达点云运动补偿
这个例子展示了如何通过融合全球定位系统(GPS)和惯性测量单元(IMU)传感器的数据来补偿由于车辆运动引起的点云失真。本例的目标是补偿点云数据中的失真，并准确地重建周围环境。
概述
自我车辆运动导致从附加激光雷达传感器收集的点云数据失真。畸变的程度取决于车速和激光雷达传感器的扫描速度。机械激光雷达传感器通过旋转反射激光脉冲的镜子来扫描环境，并生成周围环境的点云数据。反射镜的转速决定了传感器的扫描速率。激光雷达传感器生成点云数据，假设每个测量都是从同一个视点捕获的，但车辆运动改变了镜子旋转，从而改变了传感器捕获数据的视点。假设视点与实际视点之间的差异导致生成的点云失真。
该图显示了在自我载体移动时如何发生扭曲的俯视图，以及如何通过在点云中的每个点上使用自我载体姿态来补偿它。
现有的运动补偿算法要么使用点云数据，要么使用GPS和IMU等专用传感器来估计车辆的运动。本例使用GPS和IMU传感器方法。该算法假设GPS和IMU传感器的数据是准确的，并将其融合以获得车辆的里程数。然后，算法通过插值车辆里程数来调整点云中的每个点。
本例使用了Udacity®使用GPS、IMU、摄像头和激光雷达传感器记录的数据。本例遵循以下步骤来补偿记录的点云数据中的运动失真。
预处理和对齐数据。
融合GPS和IMU传感器数据。
在ENU (east-north-up)框架中对齐融合数据。
修正点云数据失真。
下载数据
以本例为例，记录自我载体数据已从Udacity数据集中收集并存储为.mat文件。记录的数据包括:
GPS数据-包含在每个GPS时间戳的自我车辆的纬度、经度、高度和速度。
IMU数据-包含自我车辆在每个IMU时间戳的线性加速度和角速度。
激光雷达数据-包含环境中每个激光雷达时间戳的点云数据，保存为
pointCloud对象。
注意:资料的下载时间视乎你的互联网连接而定。在执行此代码块期间，MATLAB将暂时无响应。
将GPS数据加载到工作空间gpsZipFile = matlab.internal.examples.downloadSupportFile(“开车”，.．.“数据/ UdacityHighway / gps.zip”）;outputFolder = fileparts(gpsZipFile);gpsFile = fullfile(输出文件夹，“gps / gps.mat”）;如果~存在(gpsFile“文件”)解压缩(gpsZipFile outputFolder)结束加载(gpsFile)将IMU数据加载到工作区imuZipFile = matlab.internal.examples.downloadSupportFile(“开车”，.．.“数据/ UdacityHighway / imu.zip”）;outputFolder = fileparts(imuZipFile);imuFile = fullfile(输出文件夹，“imu / imu.mat”）;如果~存在(imuFile“文件”)解压缩(imuZipFile outputFolder)结束加载(imuFile)将激光雷达数据加载到工作空间lidarZipFile = matlab.internal.examples.downloadSupportFile(“开车”，.．.“数据/ UdacityHighway / lidar.zip”）;outputFolder = fileparts(lidarZipFile);lidarFile = fullfile(输出文件夹，“激光雷达/ lidar.mat”）;如果~存在(lidarFile“文件”)解压缩(lidarZipFile outputFolder)结束加载(lidarFile)
坐标系统
在本例中，GPS和激光雷达传感器数据在ENU参考系中，IMU传感器数据在NED(东北向下)参考系中。
在激光雷达框架中，y轴指向自我-车辆运动方向，x轴指向右侧，z轴指向地面向上。
本例将所有传感器数据转换为自我-车辆坐标系，以调整点云数据。车辆坐标系统被锚定在自我车辆上，并遵循ISO 8855公约旋转。这个坐标系在ENU坐标系中。车辆坐标系的原点是自我车辆的车顶中心。
本例假设所有传感器的原始数据已经转换到自我车辆中心。这就是传感器如何与车辆坐标系对齐。
GPS数据在车辆坐标系中。
IMU数据在NED坐标系下的车辆坐标系中。
激光雷达数据在车辆坐标系中，偏航角为逆时针90度。
如果您使用的数据不符合这些假设，那么在继续本例之前，必须转换数据以满足这些假设。
预处理激光雷达数据
选择点云帧进行运动补偿，并将其从激光雷达ENU帧转换为车辆ENU帧。
%从激光雷达ENU转换为车辆ENU腐= [0 0 -90];Trans = [0 0 0];Tform = rigidtform3d(rot,trans);选择点云帧进行运动补偿lidarFrames = 45:54;定义变量来保存选定的点云帧lidarData = lidar.PointCloud;lidarDataAlign = lidarData(lidarFrames);对齐选定的点云帧为frameId = 1:numel(lidarDataAlign) lidarDataAlign(frameId) = pctransform(lidarDataAlign(frameId)，tform);结束
GPS数据预处理
使用latlon2local(自动驾驶工具箱)函数将原始GPS坐标转换为车辆ENU帧。这些坐标给出了自我飞行器轨迹的路径点。
将原始GPS坐标转换为车辆ENU帧gpsStartLocation = [gps.纬度(1,1)gps.经度(1,1)gps.海拔(1,1)];[currentEast,currentNorth,currentUp] = latlon2local(gps.Latitude,gps.Longitude，.．.gps.Altitude gpsStartLocation);waypointsGPS = [currentEast currentNorth];
为了融合GPS和IMU传感器数据，需要将路径点转换为IMU帧。
waypoints = [waypointsGPS(:，2) waypointsGPS(:，1) -1*waypointsGPS(:，3)];
由于方向数据在IMU传感器数据中不可用，您可以使用lookupPose(导航工具箱)函数。
使用GPS数据相对于IMU帧速率估计方向wp = waypointTrajectory(Waypoints= Waypoints,TimeOfArrival=seconds(gps.Time)，.．.ReferenceFrame =“内德”）;[~，orientationGPS] = lookupPose(wp,seconds(imu.Time));
结合GPS、IMU和激光雷达数据
GPS、IMU和激光雷达数据在时间表格式。把数据组合成一个矩阵，inputDataMatrix，用于核聚变。
创建一个同步GPS, IMU和Lidar传感器数据的表gpsTable =时间表(gps. time，[gps. time])纬度gps。经度gps.Altitude], gps.Velocity);gpsTable.Properties.VariableNames (1) =“latLonAlt”；gpsTable.Properties.VariableNames (2) =“gpsVelocity”；imuTable =时间表(imu.Time,imu.LinearAcceleration,imu.AngularVelocity，.．.紧凑(orientationGPS));imuTable.Properties.VariableNames (1) =“linearAcceleration”；imuTable.Properties.VariableNames (2) =“angularVelocity”；imuTable.Properties.VariableNames (3) =“定位”；lidarTable =时间表(lidar.Time,ones(size(lidar.Time,1)，1));lidarTable.Properties.VariableNames (1) =“lidarFlag”；inputDataMatrix =同步(gpsTable,imuTable,lidarTable);
融合GPS和IMU传感器数据
本例使用卡尔曼滤波器来异步融合GPS和IMU数据insfilterAsync(导航工具箱)对象。
创建INS过滤器来融合异步GPS和IMU数据filt = insfilterAsync(ReferenceFrame=“内德”）;
定义每个传感器的测量协方差误差。这个例子通过实验和自动调优得到误差参数insfilterAsync(导航工具箱)对象．有关更多信息，请参见insfilterAsync Filter的自动调优(导航工具箱)．
%位置协方差误差Rpos = [1 1 1e+5].^2;%速度协方差误差Rvel = 1;%加速度协方差误差Racc = 1e+5;%陀螺仪协方差误差Rgyro = 1e+5;%测量协方差误差Rcorr = 1;
根据传感器数据，设置参数的初始值状态INS滤波器的属性。假设飞行器运动是平面的，角速度最高的位置在偏航分量。俯仰和横摇分量几乎为零。你可以从GPS数据估计自我飞行器的初始偏航分量。
initialYaw = atan2d(median(waypoints(1:12,2))，median(waypoints(1:12,1)));initialPitch = 0;initialRoll = 0;%初始自我车辆方向initialOrientationRad = deg2rad([initialYaw initialPitch initialRoll]);initialEgoVehicleOrientationNED = eul2quat(initialOrientationRad);设置过滤器初始状态filt。reference elocation = gpsStartLocation;filt。State = [initialEgoVehicleOrientationNED，[0 0 0]，.．.[路径点(1,1)路径点(1,2)路径点(1,3)]，[0 0 0 0]，imu.线性加速度(1，:)，.．.[0 0 0]，[0 0 0]，[0 0 0]，[0 0 0]，[0 0 0]]';
定义自我车辆位置和方向的变量。
egoPositionLidar = 0 (size(lidar.Time,1)，3);egoOrientationLidar = 0 (size(lidar.Time,1)，1，“四元数”）;
融合GPS和IMU传感器数据来估计自我车辆的位置和方向。
prevTime = seconds(inputDataMatrix.Time(1));lidarStep = 1;%融合从GPS数据开始startRow = find(~isnan(inputDataMatrix.latLonAlt)，1，“第一”）;为row = startRow:size(inputDataMatrix,1) currTime = seconds(inputDataMatrix. time (row));%预测滤波器前向时间差预测(filt currTime-prevTime)如果任何(~ isnan (inputDataMatrix.latLonAlt(行,:)))将GPS与速度读数融合fusegps (filt inputDataMatrix.latLonAlt(行,:),rpo,.．.inputDataMatrix.gpsVelocity(行,:),Rvel);结束如果任何(~ isnan (inputDataMatrix.angularVelocity(行,:)))熔断器加速度计和陀螺仪读数fuseaccel (filt inputDataMatrix.linearAcceleration(行,:),Racc);fusegyro (filt inputDataMatrix.angularVelocity(行,:),Rgyro);如果任何(~ isnan (inputDataMatrix.orientation(行,:)))%基于从GPS获得的方向的正确方向%数据在IMU时间戳正确的(1:4,filt inputDataMatrix.orientation(行,:),Rcorr)结束结束在每个激光雷达时间戳获取自我车辆的姿态如果~isnan(inputDataMatrix.lidarFlag(row)) [egoPositionLidar(lidarStep，:)，egoOrientationLidar(lidarStep)，~] = pose(filt);lidarStep = lidarStep+1;结束prevTime = currTime;结束
将融合数据对齐到ENU帧
将估计的自我车辆里程计从NED框架转换为ENU框架，以使其与车辆坐标系对齐。
在激光雷达时间戳处摆姿势lidarFrames = [lidarFrames lidarFrames(end)+1];从NED帧转换为ENU帧egoPositionLidarENU = [egoPositionLidar(lidarFrames,2)].．.egoPositionLidar (lidarFrames, 1) 1 * egoPositionLidar (lidarFrames, 3)];从NED帧转换为ENU帧egoOrientationLidarNED = egoOrientationLidar(lidarFrames，:);egoTformLidarENU = [];为i = 1:size(egoOrientationLidarNED,1) yawPitchRollNED = quat2eul(egoOrientationLidarNED(i，:));yawPitchRollENU = yawPitchRollNED - [pi/2 0 pi];rotmENU = eul2rotm(yawPitchRollENU);egoTformLidarENU = [egoTformLidarENU rigidtform3d(rotmENU,egoPositionLidarENU(i，:))];结束
运动补偿
为了进行运动补偿，通过使用自我车辆里程计估计点云中每个数据点的校正。该算法假设激光雷达传感器具有恒定的角速度和线速度，因此车辆运动对传感器数据的影响也是线性的。该图显示了激光雷达在一次旋转过程中不同时间戳的测量结果。
将点云数据转换为世界框架。
frameId = 1;currPtCloud = lidarDataAlign(frameId);tformL2W = egoTformLidarENU(frameId);currPtCloudWorld = pctransform(currPtCloud,tformL2W);
本例假设点云数据以有组织的格式存储。在当前点云和下一个点云之间线性插值车辆位置，以估计当前点云中每个点的修正。如果点云数据为无组织格式，请使用pcorganize函数将其转换为有组织的格式。
dX = linspace(0,egoPositionLidarENU(frameId+1,1)-egoPositionLidarENU(frameId,1)，.．.大小(currPtCloud.Location, 2));dY = linspace(0,egoPositionLidarENU(frameId+1,2)-egoPositionLidarENU(frameId,2)，.．.大小(currPtCloud.Location, 2));
对点云数据中的每个点沿X和y方向执行修正。
location = currPtCloudWorld.Location;location(:，:，1) = location(:，:，1)-dX;location(:，:，2) = location(:，:，2)-dY;
创建一个补偿点云，并将其转换为激光雷达传感器框架，以便更好地可视化。
compensatedPtCloudWF = pointCloud(位置，强度=currPtCloudWorld.Intensity);补偿ptcloudlf = pctransform(补偿ptcloudwf,tformL2W.invert);
可视化运动补偿对点云数据的影响。绿色区域为运动补偿对地面的影响，红色区域为运动补偿对标识牌的影响。
helperVisualizeMotionCompensation (currPtCloud compensatedPtCloudLF)
循环数据
在记录的激光雷达数据中循环选择帧，进行运动补偿，并将补偿后的点云数据可视化。
hFigLoop = figure(Position=[0 0 1500 750]);axCurrPt = subplot(1,2,1,Parent=hFigLoop);set(axCurrPt,Position=[0.05 0.05 0.42 0.9]) axCompPt = subplot(1,2,2,Parent=hFigLoop);set(axCompPt,Position=[0.53 0.05 0.42 0.9])为frameId = 1:数字(lidarDataAlign)将当前点云转换为世界帧currPtCloud = lidarDataAlign(frameId);tformL2W = egoTformLidarENU(frameId);currPtCloudWorld = pctransform(currPtCloud,tformL2W);线性插值自我车辆的位置，以估计修正dX = linspace(0,egoPositionLidarENU(frameId+1,1)-egoPositionLidarENU(frameId,1)，.．.大小(currPtCloud.Location, 2));dY = linspace(0,egoPositionLidarENU(frameId+1,2)-egoPositionLidarENU(frameId,2)，.．.大小(currPtCloud.Location, 2));沿X和y方向应用修正location = currPtCloudWorld.Location;location(:，:，1) = location(:，:，1)-dX;location(:，:，2) = location(:，:，2)-dY;创建补偿点云，并将其转换为激光雷达传感器框架compensatedPtCloudWF = pointCloud(位置，强度=currPtCloudWorld.Intensity);补偿ptcloudlf = pctransform(补偿ptcloudwf,tformL2W.invert);可视化原始点云和补偿点云pcshow(currPtCloud,Parent=axCurrPt) set(axCurrPt,XLim=[-5 45]，YLim=[-15 15]) title(axCurrPt, Parent=axCurrPt)“原始点云”，FontSize=15) colormap(axCurrPt,hsv) view(axCurrPt,2) pcshow(compensatedPtCloudLF,Parent=axCompPt) set(axCompPt,XLim=[-5 45]，YLim=[-15 15]) title(axCompPt，“补偿点云”，FontSize=15) colormap(axCompPt,hsv) view(axCompPt,2) drawnow(“limitrate”）结束
您也可以从以下列表中选择一个网站: