用于汽车安全应用的航迹间融合
此示例显示了如何融合两辆车的轨迹，以提供比每辆车所能看到的更全面的环境估计。此示例演示了轨迹级融合器和对象轨迹数据格式的使用。在此示例中，您使用自动驾驶工具箱中的驾驶场景和视觉检测生成器™, 雷达工具箱中的雷达数据生成器™, 以及传感器融合与跟踪工具箱中的跟踪与跟踪融合模型™.
动机
汽车安全应用依赖于安装在车辆上的不同传感器系统的数据融合。单个车辆通过使用集中式跟踪器或采用更分散的方法融合传感器检测，并融合单个传感器产生的轨迹。除了车内数据融合外，还融合来自以下方面的数据：m多车辆提供了额外的好处，包括更好的覆盖范围、态势感知和安全性［1］．这种介质传感器融合方法利用了各种传感器，并为每个车辆提供更好的覆盖范围，因为它使用该地区其他车辆上的传感器更新的数据。政府和车辆制造商长期以来一直认为需要在车辆之间分享信息，以提高汽车安全。例如，正在开发V2X协议和蜂窝通信链路。
虽然多辆汽车的传感器融合是有益的，但大多数汽车都需要满足一定的安全要求，即使只有内部传感器可用。因此，车辆可能配备一个跟踪器，一个跟踪器，或两者兼有。这些跟踪算法提供了单车水平的态势感知。因此，在这个例子中，假设车辆通过广播轨道和执行轨道对轨道融合来共享态势感知。
该示例展示了来自两个车辆的熔断轨道的好处，以提高情境意识和安全性。此示例不会模拟通信系统。相反，该示例假设通信系统提供在两个车辆之间传输轨道所需的带宽。
进行航迹架构
以下框图描绘了两个车辆中的主要功能，其中：
车辆1有两个传感器，每个传感器向本地车辆跟踪器提供检测。跟踪器使用来自本地传感器的检测来跟踪对象，并将这些本地跟踪输出到车辆跟踪定影器。
车辆2只有一个传感器，是一个跟踪雷达。跟踪雷达输出轨迹，作为车辆2的本地跟踪器。来自跟踪雷达的轨迹输入到车辆2上的车辆轨迹fuser。
每辆车上的航迹融合器将本地车辆航迹与从另一辆车的航迹融合器接收到的航迹进行融合。每次更新后，每辆车上的航迹融合器都会广播其融合的航迹，这些航迹会反馈到另一辆车上的航迹融合器的下一次更新中。
在这个例子中，使用了atrackerJPDA对象定义车辆1跟踪器。
%为车辆1创建跟踪器v1Tracker = trackerJPDA (“TrackerIndex”，1，“删除阈值”(4 - 4),“AssignmentThreshold”, 100年正);%车辆1跟踪器posSelector = [1 0 0 0 0 0;0 0 1 0 0 0];
在此架构中，来自一辆车的融合轨迹会更新另一辆车上的融合轨迹。这些融合轨迹随后会广播回第一辆车。为避免谣言传播，请注意来自另一辆车的轨迹如何更新轨迹融合器。
考虑下面的谣言传播示例:在某个更新步骤，车辆1使用其内部传感器跟踪对象。然后车辆1融合目标轨迹并将其传送给车辆2，车辆2将目标轨迹与自己的轨迹融合并感知目标。到目前为止，这正是轨道对轨道融合的目标:用车辆1的信息增强车辆2的态势感知。因为车辆2现在知道了这个物体，它也开始广播轨道，也许是为了让另一辆车(在示例中没有显示)受益。
然而，车辆1现在从车辆2接收关于仅车辆1实际轨道的对象的轨道信息。因此，车辆1上的轨道定影必须意识到它来自车辆2的该对象的曲目实际上并不包含由独立源更新的任何新信息。为了使包含只重复信息的新信息和跟踪的曲目之间的区别，必须将车辆2定义为一个外部源到1号车的履带熔断器。类似地，车辆1必须定义为车辆2上的履带补板的外部源。此外，您需要将仅由跟踪fuser根据来自内部源的信息更新的跟踪定义为自我报告的．通过这样做，每辆车的轨道熔断器忽略来自轨道的更新，在轨道熔断器之间来回反弹，没有任何新的信息。
每个车辆的局部跟踪器相对于车辆的参考框架(称为自我框架)来跟踪目标。航迹到航迹的融合是在场景帧完成的，场景帧是全局级别的帧。助手自我中心函数将轨迹从自我框架转换到场景框架。同样地,函数scenarioToEgo将轨迹从情景框架转换到任何自我框架。两种转换都依赖于StateParameters财产objectTrack对象。当trackFuser对象计算场景帧中的中心轨迹到任何帧中的局部轨迹的距离，它使用StateParameters要执行坐标变换的局部轨迹的。
以达到上述目的trackFuser定义，将下列来源定义为fuserSourceConfiguration对象。
％定义每个车辆的来源v1TrackerConfiguration=fuserSourceConfiguration(“SourceIndex”，1，“IsInternalSource”,真的,…% v1Tracker是v1Fuser的内部“CentralToLocalTransformFcn”，@scenariotoego，“LocalToCentralTransformFcn”, @egoToScenario);%坐标变换v2fuserconfiguration = fusersourceconfiguration（“SourceIndex”4.“IsInternalSource”、假);% v2Fuser在v2Fuser之外v1Sources = {v1TrackerConfiguration;v2FuserConfiguration};v2TrackerConfiguration = fuserSourceConfiguration (“SourceIndex”2.“IsInternalSource”,真的,…% v2Tracker是v2Fuser的内部“CentralToLocalTransformFcn”，@scenariotoego，“LocalToCentralTransformFcn”, @egoToScenario);%坐标变换v1FuserConfiguration = fuserSourceConfiguration (“SourceIndex”3.“IsInternalSource”、假);% v1Fuser在v2Fuser之外v2Sources = {v2TrackerConfiguration;v1FuserConfiguration};
您现在可以定义每个车辆履带fuser为trackFuser对象。
extendparams = struct（“框架”，“矩形”，“位置”(0 0 0),“速度”, 0 0 0);v1Fuser = trackFuser (“FuserIndex”3.…“AssignmentThreshold”，[100款]，…“MaxNumSources”2.“SourceConfigurations”v1Sources,…“状态融合”，“交叉口”，“删除阈值”3 [3],…“StateParameters”, stateParams);v2Fuser = trackFuser (“FuserIndex”4.…“AssignmentThreshold”，[100款]，…“MaxNumSources”2.“SourceConfigurations”v2Sources,“状态融合”，…“交叉口”，“删除阈值”3 [3],…“StateParameters”, stateParams);%初始化下列变量fusedTracks1 = objectTrack.empty (0,1);fusedTracks2 = objectTrack.empty (0,1);wasFuser1Updated = false;wasFuser2Updated = false;
定义场景
下面的场景显示了两辆车在街道上行驶。车辆1是先导车辆，配备了两个前视传感器:一个近程雷达传感器和一个视觉传感器。2号车与1号车相距10米，装有远程雷达。街道的右侧停放着车辆。一个行人站在车辆之间。这条行人以一个点的形式显示在X = 60米处。
由于车辆2与车辆1距离较近，车辆2雷达传感器覆盖范围大部分被车辆1遮挡。因此，车辆2上的轨道fuser维护的大部分轨道首先由车辆1广播的轨道初始化。
％创建驱动器通道对象和两个车辆[scenario, vehicle1, vehicle2] = createDrivingScenario;%创建所有传感器[sensors, numSensors, attachdvehicle] = createSensors(scenario);%创建显示[f，绘图仪] = createv2vdisplay（场景，传感器，连接仪）;
下面的追逐情节是从第二辆车的角度来看的。箭头表示行人几乎完全被停放的车辆和第一辆车辆遮挡的位置。
将每辆车定义为一个参与者、传感器、跟踪器和绘图仪的组合v1 =结构(“演员”{vehicle1},“传感器”,{传感器(attachedVehicle = = 1)},'跟踪器'{v1Tracker},“绘图仪”{plotters.veh1DetPlotter},“TrkPlotter”, {plotters.veh1TrkPlotter});v2 =结构(“演员”{vehicle2},“传感器”,{传感器(attachedVehicle = = 2)},'跟踪器'，{{}}，“绘图仪”，{{}}，“TrkPlotter”，{plotters.veh2TrkPlotter}）；%车辆2上没有侦测到或追踪器
运行仿真
下面的代码运行这个模拟。
运行= true;可重复结果的％，设置随机数种子s =提高;Rng (2019) snaptimes = [0.5, 2.8, 4.4, 6.3, inf];照片=细胞(元素个数(snaptimes 1));i = 1;f.Visible =“上”；而运行&& ishhandle (f) time = scenario.SimulationTime;%检测和跟踪车辆水平[tracks1, wasTracker1Updated] = detectAndTrack (v1、时间posSelector);[tracks2, wasTracker2Updated] = detectAndTrack (v2、时间posSelector);%保留上一个fuser更新的轨迹oldFusedTracks1 = fusedTracks1;oldFusedTracks2 = fusedTracks2;%更新熔断器如果wasstracker1updated || wasFuser2Updated tracksToFuse1 = [tracks1;oldFusedTracks2];如果isLocked(v1Fuser) || ~isempty(tracksToFuse1) [fusedTracks1，~，~，info1] = v1Fuser(tracksToFuse1,time);wasFuser1Updated = true;pos = getTrackPositions (fusedTracks1 posSelector);字符串id = ((fusedTracks1.TrackID) ');plotTrack (plotters.veh1FusePlotter、pos、ids);其他的wasFuser1Updated = false;fusedTracks1 = objectTrack.empty (0,1);结束其他的wasFuser1Updated = false;fusedTracks1 = objectTrack.empty (0,1);结束如果wasstracker2updated || wasFuser1Updated tracksToFuse2 = [tracks2;oldFusedTracks1];如果isLocked(v2Fuser) || ~isempty(tracksToFuse2) [fusedTracks2，~，~，info2] = v2Fuser(tracksToFuse2,time);wasFuser2Updated = true;pos = getTrackPositions (fusedTracks2 posSelector);字符串id = ((fusedTracks2.TrackID) ');plotTrack (plotters.veh2FusePlotter、pos、ids);其他的wasFuser2Updated = false;fusedTracks2 = objectTrack.empty (0,1);结束其他的wasFuser2Updated = false;fusedTracks2 = objectTrack.empty (0,1);结束％更新显示updateV2VDisplay(绘图仪，场景，传感器，附属车辆)%将场景向前推进一步，如果场景完成，则退出循环运行=提前（方案）;%在指定的时间捕获帧的图像如果Time >= snaptimes(i) snaps{i} = takesnap(f);I = I + 1;结束结束
图中显示了场景和场景结束时的跟踪结果。本示例的后续部分将在关键时刻分析跟踪结果。
在仿真开始时分析跟踪
当模拟开始时，车辆1检测到停在街道右侧的车辆。然后，车辆追踪器确认与停放车辆相关的轨迹。此时，车辆2跟踪器检测和跟踪的唯一对象是车辆1，它就在它的正前方。一旦车辆轨道熔断器确认轨道，它广播它们，车辆轨道熔断器熔断它们。因此，车辆2在自己检测到停车车辆之前就会意识到它们。
showsnap(四合扣,1)
分析街道侧面行人的跟踪
随着仿真的继续，车辆2能够检测和跟踪停放在侧面的车辆，并用来自车辆的轨道熔化它们1.车辆2能够检测和跟踪行人约4秒钟进入模拟，以及车辆2将与行人相关联的轨道约为4.4秒，进入模拟（参见Snapshot 2）。但是，它需要车辆2大约两秒钟，然后才能通过自己的传感器检测和跟踪行人（参见快照3）。在街上检测到两秒前的行人可以显着提高安全性。
showsnap(四合扣,2)
showsnap(四合扣,3)
避免谣言传播
当两辆车相互传递轨道时，存在一种风险，即他们将继续传递关于他们不再检测到的物体的信息，只需重复另一辆车传递的信息。这种情况称为谣言传播。
车辆通过的对象,这些对象出去他们的视野,相关的融合跟踪这些对象都下降了追踪器(见4)快照。下降轨道表明两者之间的融合跟踪广播来回车辆不习惯传播谣言。
showsnap(断了,4)
%重新启动驱动场景，使角色返回到初始位置重启(场景);释放所有的传感器对象，这样它们可以再次使用为sensorIndex = 1:numSensors release(sensors{sensorIndex});结束%返回随机种子到它以前的值rng (s)
总结
在此示例中，您看到了跟踪融合如何提高情境感知并提高汽车应用中的安全性。你看到了如何设置一个trackFuser来执行轨迹对轨迹的融合，以及如何定义源是内部的还是外部的fuserSourceConfiguration通过这样做，你可以避免谣言传播，只保留每辆车真正观察到的融合轨迹。
参考文献
[1] Duraisamy, B.， T. Schwarz和C. Wohler。“用于汽车安全应用的轨道水平融合算法”。在2013国际信号处理、图像处理与模式识别学术会议, 179 - 84年,2013年。https://doi.org/10.1109/ICSIPR.2013.6497983．
金宝app支持功能
createdrivingscenario.
创建一个在中定义的驾驶场景驾驶场景设计师应用程序。
函数[scenario, egoVehicle, secondVehicle] = createDrivingScenario%构造一个drivingScenario对象场景= drivingScenario ('采样时间', 0.1);％添加所有道路段道路中心=[50.80.50；253.41.50]；道路宽度=12；道路（场景、道路中心、道路宽度）；道路中心=[100.7-100.6 0；100.7 103.7 0]；道路（场景、道路中心）；道路中心=[201.1-99.20；199.799.50]；道路（场景、道路中心）；添加自我载体egoVehicle =车辆(场景中,“ClassID”，1，“位置”， [65.1 -0.9 0]，'plotcolor'，[0 0.7410 0.4470]）;航点= [71-0.5 0;148.7 -0.5 0];速度= 12;轨迹（Egovehicle，航点，速度）;增加第二辆车secondVehicle =车辆(场景中,“ClassID”，1，“位置”，[55.1 -0.9 0]）;航点= [61-0.5 0;138.7 -0.5 0];速度= 12;轨迹（第二次，航点，速度）;把停着的车加起来车辆(场景中,“ClassID”，1，“位置”， [111.0 -3.6 0]);车辆(场景中,“ClassID”，1，“位置”, [140.6 -3.6 0]); 车辆（场景，“ClassID”，1，“位置”， [182.6 -3.6 0]);车辆(场景中,“ClassID”，1，“位置”，[211.3 -4.1 0]）;%增加行人演员(场景中,“ClassID”，4，“长度”, 0.5,“宽度”, 0.5,…“高度”, 1.7,“位置”， [130.3 -2.7 0]，'rcspattern'， [-8 -8]);％添加停放的卡车车辆(场景中,“ClassID”2，“长度”, 8.2,“宽度”, 2.5,…“高度”, 3.5,“位置”， [117.5 -3.5 0]);结束
createSensors
创建场景中使用的传感器，并列出它们与车辆的附件。
函数[sensors, numSensors, attachdvehicle] = createSensors(scenario)% createSensors返回所有产生检测的传感器对象%createSensors和createDrivingScenario中使用的单位％距离/位置 - 仪表%速度-米/秒%角度-度% RCS模式- dBsm%为每个传感器分配物理和雷达配置文件的所有参与者概要文件= ActorProfiles（方案）;车辆1雷达报告发现群集传感器{1}=雷达数据发生器(“没有扫描”，'sensorindex'，1，“UpdateRate”10…“安装位置”， [3.7 0 0.2]，“RangeLimits”, 50 [0],“视野”60, [5],…'rangeresolution', 2.5,“方位角解”，4，…“个人资料”配置文件,'哈塞普州'符合事实的“HasFalseAlarms”， 错误的，…“TargetReportFormat”，“集群检测”）；车辆2雷达跟踪传感器{2}= radarDataGenerator (“没有扫描”，'sensorindex'2，“UpdateRate”10…“安装位置”， [3.7 0 0.2]，“RangeLimits”120年[0],“视野”30 [5],…'rangeresolution', 2.5,“方位角解”，4，…“个人资料”配置文件,'哈塞普州'符合事实的“HasFalseAlarms”， 错误的，…“TargetReportFormat”，“跟踪”，“删除阈值”, [3 3]);%车辆1视觉传感器报告检测到传感器{3}= visionDetectionGenerator ('sensorindex'，3，…“MaxRange”，100，“SensorLocation”1.9 [0],“DetectorOutput”，'只对象'，…“ActorProfiles”,概要文件);attachedVehicle = [1, 2, 1];numSensors =元素个数(传感器);结束
scenarioToEgo
执行从场景到自我坐标的坐标转换。
trackinscenario.已StateParameters定义以将其从场景坐标转换为自我坐标。
该状态采用恒定速度模型[x;vx;y;vy;z;vz]。
函数trackInEgo = scenario (trackInScenario) egoPosInScenario = trackInScenario. stateparameters . originposition;egoVelInScenario = trackInScenario.StateParameters.OriginVelocity;stateInScenario = trackInScenario.State;stateShift = [egoPosInScenario (1); egoVelInScenario (1); egoPosInScenario (2); egoVelInScenario (2); egoPosInScenario (3); egoVelInScenario (3)];stateInEgo = stateInScenario - stateShift;trackInEgo = objectTrack (“UpdateTime”，trackInScenario.UpdateTime，“状态”stateInEgo,“StateCovariance”, trackInScenario。StateCovariance,“StateParameters”，trackInScenario.StateParameters）；结束
自我中心
执行从自我坐标到场景坐标的坐标转换。
trackInEgo已StateParameters定义以将其从自我坐标转换为场景坐标。
该状态采用恒定速度模型[x;vx;y;vy;z;vz]。
函数trackInScenario = egoToScenario(trackInEgo) egoPosInScenario = trackInEgo. stateparameters . originposition;egoVelInScenario = trackInEgo.StateParameters.OriginVelocity;stateInScenario = trackInEgo.State;stateShift = [egoPosInScenario (1); egoVelInScenario (1); egoPosInScenario (2); egoVelInScenario (2); egoPosInScenario (3); egoVelInScenario (3)];statinego = statinscenario + statshift;trackInScenario = objectTrack (“UpdateTime”, trackInEgo。UpdateTime,“状态”stateInEgo,“StateCovariance”，trackinego.statecovariance，“StateParameters”，trackinego.stateParameters）;结束
detectAndTrack
该功能用于收集来自一辆车的传感器的所有检测，并更新跟踪器。
代理是一个包含参与者信息和传感器、跟踪器和绘图仪的结构，用于绘制检测和车辆轨迹。
函数[tracks，wasTrackerUpdated]=detectAndTrack（代理、时间、posSelector）%从车辆创建检测提出了= targetPoses (agent.Actor);[检测,isValid] = vehicleDetections (agent.Actor.Position agent.Sensors,姿势、时间agent.DetPlotter);％更新跟踪器以获取报告检测的传感器的曲目如果iSvalid代理.tracker.stateParameters = struct（…“框架”，“矩形”，…'originposition'agent.Actor.Position,…“OriginVelocity”, agent.Actor.Velocity);跟踪= agent.Tracker(检测、时间);tracksInScenario =跟踪;为i=1:numel（轨道）轨道情景（i）=电子竞技（轨道（i））；结束pos=getTrackPositions（tracksInScenario，posSelector）；plotTrack（agent.TrkPlotter，pos）wasTrackerUpdate=true；其他的曲目= ObjectTrack.empty（0,1）;wastrackerupdated = false;结束从跟踪传感器获得额外的轨迹[sensorTracks, wasSensorTrackerUpdated] = vehicleTracks (agent.Actor agent.Sensors,姿势、时间agent.TrkPlotter);跟踪= vertcat(跟踪、sensorTracks);wasstrackerupdated = wasstrackerupdated || wasSensorTrackerUpdated;结束
vehicleDetections
收集所有连接在车辆上的传感器的检测结果。
函数[objectDetections,isValid] = vehicleDetections(位置，传感器，姿势，时间，绘图仪)numSensors = nummel(传感器);objectDetections = {};isValidTime = false(1, numSensors);%为每个传感器生成检测为SENSORINDEX = 1：NUMSENSORS传感器=传感器{SENSORINDEX};如果isa(传感器,“visionDetectionGenerator”）||〜Strcmpi（Sensor.targetReportFormat，“跟踪”) [objectDets， ~， sensorConfig] =传感器(姿势，时间);如果islogical(sensorConfig) isValidTime(sensorIndex) = sensorConfig;其他的isValidTime（sensorIndex）=sensorConfig.isValidTime；结束objectDets=cellfun（@（d）setAtt（d），objectDets，“UniformOutput”、假);numObjects =元素个数(objectDets);objectDetections = [objectDetections;objectDets (1: numObjects)];% #好< AGROW >结束结束IsValid =任何（IsValidTime）;%绘制检测如果numel（ObjectDetections）> 0 detpos = cellfun（@（d）d.measurement（1：2），ObjectDetection，“UniformOutput”、假);detPos = cell2mat(detPos')' + position(1:2);plotDetection(绘图仪,detPos);结束结束函数d = setAtt (d)%设置属性为一个结构d.ObjectAttributes =结构;％只保留位置测量并去除速度如果nummel (d.Measurement)==6;d.MeasurementNoise = d.MeasurementNoise (1:3, 1:3);d.MeasurementParameters{1}。HasVelocity = false;结束结束
vehicleTracks
从报告车辆轨迹的传感器收集所有轨迹。
函数[轨道，wasstrackerupdated] =车辆轨道(演员，传感器，姿势，时间，绘图仪)%从车辆创建检测numSensors =元素个数(传感器);曲目= ObjectTrack.empty;isValidTime = false(1, numSensors);%为每个传感器生成检测为SENSORINDEX = 1：NUMSENSORS传感器=传感器{SENSORINDEX};如果isa(传感器,“radarDataGenerator”)&&strcmpi（sensor.TargetReportFormat，“跟踪”) [sensorTracks， ~， sensorConfig] =传感器(姿势，时间);如果islogical(sensorConfig) isValidTime(sensorIndex) = sensorConfig;其他的isValidTime（sensorIndex）=sensorConfig.isValidTime；结束numobjects = numel（sensortracks）;曲目= [曲目;sensortracks（1：numobjects）];% #好< AGROW >结束结束wasTrackerUpdated = (isValidTime);如果~wasTrackerUpdated百分比无车辆跟踪传感器UDPated返回结束在跟踪状态参数中添加车辆位置和速度为i = 1:numel(tracks) tracks(i). stateparameters . ..StateParameters OriginPosition =歌曲(我)。OriginPosition + actor.Position ';跟踪.StateParameters(我)。.StateParameters OriginVelocity =歌曲(我)。OriginVelocity + actor.Velocity ';结束%情节轨迹如果numel（曲目）> 0 trpos = arrayfun（@（t）t.state（[1,3]），曲目，“UniformOutput”，假）；trPos=单元2Mat（trPos’’）+演员位置（1:2）；绘图仪（绘图仪、trPos）；结束结束
你也可以从以下列表中选择一个网站:
