基于合成雷达和视觉数据的传感器融合

打开脚本

这个例子展示了如何生成一个场景，模拟传感器检测，并使用传感器融合来跟踪模拟车辆。与传感器记录相比，使用场景生成和传感器模拟的主要好处是能够创建罕见的潜在危险事件，并使用它们测试车辆算法。

这个示例涵盖了生成合成数据的整个编程工作流程。要以交互方式生成合成数据，请使用驾驶场景设计示例请参见交互式创建驾驶场景并生成合成传感器数据．

生成场景

场景生成包括生成道路网络，定义在道路上移动的车辆，以及移动车辆。

在本例中，您将测试传感器融合跟踪从自我车辆左侧通过的车辆的能力。该场景模拟了高速公路的设置，另外的车辆在自我车辆的前面和后面。

定义一个空场景。scenario = drivingScenario;场景。SampleTime = 0.01;

加上一段500米长的典型的两车道高速公路。道路是用一组点来定义的，每个点都定义了道路在3-D空间中的中心。在道路右边缘加一个泽西路障。

roadCenters = [0 0;50 0;100 0;250 20;500年40];mainRoad =道路(scenario, roadCenters，“道”lanespec (2));障碍(场景,mainRoad);

创造自我载具和它周围的三辆车:一辆超越自我载具并从左边超过它，一辆开在自我载具正前方，一辆开在自我载具正后方。所有车辆都遵循由道路路径点定义的轨迹轨迹推动政策。超车将从右车道开始，移动到左车道超车，然后返回右车道。

创造一个以25米/秒的速度沿着道路行驶的自我车辆。把减去半个车道宽度(1.8米)右车道车辆的百分比从路的中心线开始。egoCar =车辆(场景，“ClassID”1);轨迹(egoCar, roadCenters(2:end，:) - [0 1.8]， 25);%在右车道在自我车前面加一辆车leadCar =车辆(场景，“ClassID”1);轨迹(leadCar， [70 0;roadCenters(3:end，:)] - [0 1.8]， 25);%在右车道添加一辆以35米/秒的速度行驶的汽车，并超过自我车辆passingCar =车辆(场景，“ClassID”1);路点= [0 -1.8;50 1.8;100年1.8;250年21.8;400年32.2;500年38.2);轨迹(passingCar, waypoints, 35);在自我车后面加一辆车chaseCar =车辆(场景，“ClassID”1);轨迹(chaseCar， [25 0;roadCenters(2:end，:)] - [0 1.8]， 25);%在右车道

定义雷达和视觉传感器

在这个例子中，你模拟一辆自我车辆，它有6个雷达传感器和2个覆盖360度视野的视觉传感器。传感器有一些重叠和覆盖差距。ego车辆在车辆的前部和后部都配备了一个远程雷达传感器和一个视觉传感器。车辆的每一侧有两个近程雷达传感器，每个覆盖90度。两侧各有一个传感器，从车辆中间覆盖到后部。两侧的另一个传感器从车辆中间向前覆盖。下一节中的图显示了覆盖率。

传感器= cell(8,1);%汽车前保险杠中央的前置远程雷达传感器。传感器{1}= drivingradardatgenerator (“SensorIndex”, 1“RangeLimits”， [0 174]，.．.“MountingLocation”, [egoCar。轴距+ egoCar。FrontOverhang, 0,0.2]，“FieldOfView”， [20,5]);%汽车后保险杠中心的后置远程雷达传感器。传感器{2}= drivingradardatgenerator (“SensorIndex”2,“MountingAngles”， [180 0 0]，.．.“MountingLocation”, [-egoCar。RearOverhang, 0,0.2]，“RangeLimits”， [0 30]，“FieldOfView”， [20,5]);%汽车左后轮井处面向左侧的近程雷达传感器。传感器{3}= drivingradardatgenerator (“SensorIndex”3,“MountingAngles”， [120 0 0]，.．.“MountingLocation”， [0, egoCar.]宽/ 2 0.2),“RangeLimits”， [0 30]，“ReferenceRange”,50岁,.．.“FieldOfView”， [90,5]，“AzimuthResolution”10“RangeResolution”, 1.25);%汽车右后轮井的后向右近程雷达传感器。传感器{4}= drivingradardatgenerator (“SensorIndex”4“MountingAngles”， [-120 0 0]，.．.“MountingLocation”， [0， -egoCar.]宽/ 2 0.2),“RangeLimits”， [0 30]，“ReferenceRange”,50岁,.．.“FieldOfView”， [90,5]，“AzimuthResolution”10“RangeResolution”, 1.25);%车辆左前轮井处的左前面向近程雷达传感器。传感器{5}= drivingradardatgenerator (“SensorIndex”5,“MountingAngles”， [60 0 0]，.．.“MountingLocation”, [egoCar。轴距,egoCar。宽/ 2 0.2),“RangeLimits”， [0 30]，.．.“ReferenceRange”,50岁,“FieldOfView”， [90,5]，“AzimuthResolution”10.．.“RangeResolution”, 1.25);%汽车右前轮井前向右近程雷达传感器。传感器{6}= drivingradardatgenerator (“SensorIndex”6“MountingAngles”， [-60 0 0]，.．.“MountingLocation”, [egoCar。轴距,-egoCar。宽/ 2 0.2),“RangeLimits”， [0 30]，.．.“ReferenceRange”,50岁,“FieldOfView”， [90,5]，“AzimuthResolution”10.．.“RangeResolution”, 1.25);前置摄像头位于前挡风玻璃。%传感器{7}=视觉检测生成器(“SensorIndex”7“FalsePositivesPerImage”, 0.1,.．.“SensorLocation”(0.75 * egoCar。轴距0),“高度”, 1.1);%后置摄像头位于后挡风玻璃上。传感器{8}=视觉检测生成器(“SensorIndex”8“FalsePositivesPerImage”, 0.1,.．.“SensorLocation”(0.2 * egoCar。轴距0),“高度”, 1.1,“偏航”, 180);向传感器注册参与者配置文件。profiles = actorProfiles(场景);为M = 1:数字(传感器)如果isa(传感器{m},“drivingRadarDataGenerator”{m})传感器。Profiles = Profiles;其他的传感器{m}。ActorProfiles =配置文件;结束结束

创建跟踪器

创建一个multiObjectTracker追踪接近自我车辆的车辆。跟踪器使用initSimDemoFilter金宝app支持函数初始化一个与位置和速度一起工作的等速线性卡尔曼滤波器。

跟踪是二维的。虽然传感器返回的测量结果是三维的，但运动本身仅限于水平平面，因此不需要跟踪高度。

跟踪器= multiObjectTracker(“FilterInitializationFcn”@initSimDemoFilter,.．.“AssignmentThreshold”30岁的“ConfirmationThreshold”， [4 5]);positionSelector = [1 0 0 0;0 0 10 0];%位置选择器velocitySelector = [0 1 0 0;0 0 0 1];%速度选择器创建显示并返回鸟瞰图的句柄。BEP = createDemoDisplay(egoCar，传感器);

模拟场景

下面的循环移动车辆，调用传感器模拟，并执行跟踪。

请注意，场景生成和传感器模拟可以有不同的时间步骤。为场景和传感器指定不同的时间步骤使您可以将场景模拟与传感器模拟解耦。这对于独立于传感器的测量速率而高精度地建模角色运动是有用的。

另一个例子是传感器具有不同的更新速率。假设一个传感器每20毫秒更新一次，另一个传感器每50毫秒更新一次。您可以指定更新速率为10毫秒的场景，传感器将在正确的时间提供更新。

在本例中，场景生成的时间步长为0.01秒，而传感器每0.1秒检测一次。传感器返回一个逻辑标志，isValidTime在美国，如果传感器产生检测，这是正确的。此标志仅用于在检测到时调用跟踪器。

另一个重要的注意点是传感器可以模拟每个目标的多次探测，特别是当目标非常接近雷达传感器时。因为跟踪器假设每个传感器对每个目标都有一个检测，所以在跟踪器处理这些检测之前，必须对这些检测进行聚类。这是通过设置来完成的TargetReportFormat到'Clustered detections'，这是默认值。传感器模型还可以输出原始检测数据，或使用内部跟踪器跟踪更新。

toSnap = true;而advance(scenario) && ishhandle (BEP.Parent)获取场景时间时间= scenario.SimulationTime;在本车辆坐标中获取其他车辆的位置。ta = targetpose (egoCar);%模拟传感器detectionClusters = {};isValidTime = false(1,8);为i = 1:8 [sensorDets,numValidDets,isValidTime(i)] = sensors{i}(ta, time);如果numValidDets为j = 1:numValidDets%视觉检测不报告信噪比。跟踪器要求%，它们具有与雷达相同的对象属性%检测。这将SNR对象属性添加到视觉%检测，并将其设置为NaN。如果~ isfield (sensorDets {j}。ObjectAttributes {1},“信噪比”) sensorDets {j} .ObjectAttributes{1}。信噪比= NaN;结束移除被测位置和速度的z分量%来自测量和测量噪声场sensorDets {j}。测量= sensorDets{j}。测量([1 2 4 5]);sensorDets {j}。测量噪声= sensorDets{j}。测量噪声([1 2 4 5]，[1 2 4 5]);结束detectionClusters = [detectionClusters;sensorDets];% #好< AGROW >结束结束如果有新的检测，更新跟踪器。%如果任何(isValidTime)如果isa(传感器{1},“drivingRadarDataGenerator”) vehicllength =传感器{1}.Profiles.Length;其他的vehicllength = sensors{1}.ActorProfiles.Length;结束confirmedTracks = updateTracks(跟踪器，detectionClusters, time);更新鸟瞰图updateBEP(BEP, egoCar, detectionClusters, confirmedTracks, positionSelector, velocitySelector);结束当汽车经过自我车辆时，为文件抓拍一个数字如果ta(1).Position(1) > 0 && toSnap toSnap = false;snapnow结束结束

总结

这个例子展示了如何生成一个场景，模拟传感器检测，并使用这些检测来跟踪自我车辆周围的移动车辆。

您可以尝试修改场景道路，或添加或删除车辆。您还可以尝试添加、删除或修改自我车辆上的传感器，或修改跟踪器参数。

金宝app支持功能

initSimDemoFilter

这个函数基于检测初始化一个恒速滤波器。

函数filter = initSimDemoFilter(检测)%使用2-D恒定速度模型初始化跟踪kf滤波器。%状态向量为[x;vx;y;vy]%检测测量向量为[x;y;vx;vy]因此，测量模型为H = [1 0 0 0;0 0 10 0;0 1 0 0;0 0 0 1]H = [1 0 0 0;0 0 10 0;0 1 0 0;0 0 0 1];filter = trackingKF(“MotionModel”，“2D匀速”，.．.“状态”， H' *检测。测量,.．.“MeasurementModel”H,.．.“StateCovariance”， H' *检测。噪声* H，.．.“MeasurementNoise”, detection.MeasurementNoise);结束

createDemoDisplay

这个函数创建了一个三面板显示:

显示的左上角:跟随自我车辆的俯视图。
显示的左下角:跟踪自我车辆的跟踪摄像头视图。
右半部分显示:AbirdsEyePlot显示。

函数BEP = createDemoDisplay(egoCar，传感器)%做一个数字hFigure = figure(“位置”， [0,0,1200,640]，“名字”，“传感器融合与合成数据示例”）;movegui(hFigure， [0 -1]);将图形向左移动，并从顶部向下移动一点添加一个汽车情节，从后面跟随自我车辆。hCarViewPanel = uipanel(hFigure，“位置”， [0 0 0.5 0.5]，“标题”，“追逐相机视图”）;hCarPlot = axes(hCarViewPanel);chasePlot (egoCar“父”, hCarPlot);添加一个从顶视图跟随自我车辆的汽车图。hTopViewPanel = uipanel(hFigure，“位置”， [0 0.5 0.5 0.5]，“标题”，顶视图的）;hCarPlot = axes(hTopViewPanel);chasePlot (egoCar“父”hCarPlot,“ViewHeight”, 130,“ViewLocation”， [0 0]，“ViewPitch”, 90);为鸟瞰图添加一个面板。hBEVPanel = uipanel(hFigure，“位置”， [0.5 0 0.5 1]，“标题”，“鸟”眼中的阴谋”）;为自我车辆和传感器覆盖创建鸟瞰图。hBEVPlot =坐标轴(hBEVPanel);frontBackLim = 60;BEP = birdsEyePlot(“父”hBEVPlot,“Xlimits”， [-frontBackLim]，“Ylimits”， [-35 35]);绘制雷达覆盖区域为i = 1:6 cap = coverageareaplot (BEP，“FaceColor”，“红色”，“EdgeColor”，“红色”）;如果isa(传感器{我},“drivingRadarDataGenerator”) plotCoverageArea(cap，传感器{i}.MountingLocation(1:2)，.．.sensors{i}.RangeLimits(2)， sensors{i}.MountingAngles(1)， sensors{i}.FieldOfView(1));其他的plotCoverageArea(帽,传感器{}。SensorLocation,.．.传感器{我}。{我}MaxRange,传感器。{我}.FieldOfView偏航,传感器(1));结束结束绘制视觉传感器的覆盖区域为i = 7:8 cap = coverageareaplot (BEP，“FaceColor”，“蓝”，“EdgeColor”，“蓝”）;如果isa(传感器{我},“drivingRadarDataGenerator”) plotCoverageArea(cap，传感器{i}.MountingLocation(1:2)，.．.传感器{i}.RangeLimits(2)，传感器{i}.MountingAngles(1)， 45);其他的plotCoverageArea(帽,传感器{}。SensorLocation,.．.传感器{我}。{我}MaxRange,传感器。偏航、45);结束结束创建一个视觉检测绘图仪，把它放在一个结构中以备将来使用。detectionPlotter (cep“DisplayName的”，“愿景”，“MarkerEdgeColor”，“蓝”，“标记”，“^”）;将所有雷达探测合并到一个条目中，并存储它以供以后更新。detectionPlotter (cep“DisplayName的”，“雷达”，“MarkerEdgeColor”，“红色”）;添加道路边界的情节laneMarkingPlotter (cep“DisplayName的”，“车道标记”）;将轨道添加到鸟瞰图中。。显示最近10个轨道更新。trackPlotter (cep“DisplayName的”，“跟踪”，“HistoryDepth”10);轴(cep)。父母,“平等”）;xlim (cep)。父母,[-frontBackLim frontBackLim]); ylim(BEP.Parent, [-40 40]);为基本事实添加一个轮廓绘图仪。outlinePlotter (cep“标签”，“地面实况”）;结束

updateBEP

这个函数用道路边界、探测和轨道更新鸟瞰图。

函数updateBEP(BEP, egoCar，检测，confirmedTracks, psel, vsel)更新道路边界及其显示[lmv, lmf] = laneMarkingVertices(egoCar);plotLaneMarking (findPlotter (cep),“DisplayName的”，“车道标记”)、lmv lmf);%更新地面真实数据[位置，偏航，长度，宽度，originOffset, color] = targetOutlines(egoCar);plotOutline (findPlotter (cep),“标签”，“地面实况”)、位置、偏航、长度、宽度、“OriginOffset”originOffset,“颜色”、颜色);%更新屏障数据[bPosition,bYaw, blth,bWidth,bOriginOffset,bColor,numBarrierSegments] = targetOutlines(egoCar，“障碍”）;plotBarrierOutline (findPlotter (cep),“标签”，“地面实况”)、numBarrierSegments bPosition、bYaw bLength, bWidth,.．.“OriginOffset”bOriginOffset,“颜色”, bColor);准备和更新检测显示N = numel(检测);detPos = 0 (N,2);isRadar = true(N,1);为i = 1:N detPos(i，:) = detection {i}.Measurement(1:2)';如果检测我{}。6 . SensorIndex >视力检测百分比isRadar(i) = false;结束结束plotDetection (findPlotter (cep),“DisplayName的”，“愿景”), detPos (~ isRadar,:));plotDetection (findPlotter (cep),“DisplayName的”，“雷达”), detPos (isRadar:));移除所有被视觉检测识别的物体轨迹。%%生成器，然后更新轨道显示。这些有ObjectClassID%参数值为0，包括障碍物等对象。isNotBarrier = arrayfun(@(t)t. objectclassid,confirmedTracks)>0;confirmedTracks = confirmedTracks(isNotBarrier);准备和更新轨道显示trackid = {confirmedTracks.TrackID};标签= cellfun(@num2str, trackIDs，“UniformOutput”、假);[tracksPos, tracksCov] = getTrackPositions(confirmedTracks, psel);tracksVel = gettrackvelocity (confirmedTracks, vsel);plotTrack (findPlotter (cep),“DisplayName的”，“跟踪”)， tracksPos, tracksVel, tracksCov，标签);结束

另请参阅