激光雷达和雷达融合在城市空气流动情况

这个示例使用:

进一步研究多目标跟踪器跟踪这个例子展示了如何使用各种无人机(uav)在城市环境。您创建一个使用场景uavScenario基于网上构建和地形数据对象。然后使用激光雷达和雷达传感器模型来生成合成传感器数据。最后,您使用各种跟踪算法来估计场景中所有无人机的状态。

无人机为范围广泛的操作设计。许多应用程序设置在城市环境中,如无人机的包裹递送,空气出租车,电力线路检查。这些操作的安全变得至关重要的应用程序数量的增加,使城市空域控制一个挑战。

创建城市空气流动情况

在本例中,您使用的地形和建筑数据博尔德有限公司数字地形高程数据(DTED)文件下载的SRTM数据集可以从美国地质调查局(U.S. Geological Survey)来填补。southboulder构建数据.osm是下载https://www.openstreetmap.org/,它提供了访问世界各地的众包地图数据。数据开放数据共享开放数据库许可下的(ODbL),https://opendatacommons.org/licenses/odbl/。

dtedfile =“n39_w106_3arc_v2.dt1”;buildingfile =“southboulder.osm”;现场= createScenario (dtedfile buildingfile);

接下来,添加一些无人机的场景。

包裹递送操作模型,定义一个轨迹离开建筑物的屋顶,飞到另一个建筑。轨迹是由三条腿。quadrotor垂直起飞,飞向未来交付目的地,最后土地垂直在屋顶上。

waypointsA = [1895 90 20;1915 108 35;1900 115 20);赈灾发言人timeA = 25 50 [0];trajA = waypointTrajectory (waypointsA,“TimeOfArrival”赈灾发言人timeA,“ReferenceFrame”,“ENU表示”,“AutoBank”,真正的);uavA = uavPlatform (“无人机”场景,“轨迹”trajA,“ReferenceFrame”,“ENU表示”);updateMesh (uavA“quadrotor”{5},(0.6350 0.0780 0.1840),眼睛(4));

添加另一个无人机模型的出租飞机飞行。其轨迹是线性的和略降。使用fixedwing几何模型的一个更大的无人机,适合运输的人。

waypointsB = [1940 120 50;1800 50 20];timeB = 41 [0];trajB = waypointTrajectory (waypointsB,“TimeOfArrival”timeB,“ReferenceFrame”,“ENU表示”,“AutoBank”,真正的);uavB = uavPlatform (“UAV2”场景,“轨迹”trajB,“ReferenceFrame”,“ENU表示”);updateMesh (uavB“fixedwing”{10},(0.6350 0.0780 0.1840),眼睛(4));

然后添加一个quadrotor轨迹后街上的道路。这代表了一个无人机检测电网线路维护的目的。

waypointsC = [1950 60 35;1900 60 35;1890 80 35);timeC = linspace(0, 41岁的大小(waypointsC, 1));trajC = waypointTrajectory (waypointsC,“TimeOfArrival”timeC,“ReferenceFrame”,“ENU表示”,“AutoBank”,真正的);uavC = uavPlatform (“UAV3”场景,“轨迹”trajC,“ReferenceFrame”,“ENU表示”);updateMesh (uavC“quadrotor”{5},(0.6350 0.0780 0.1840),眼睛(4));

最后,增加自我无人机无人机侦查现场,负责跟踪不同的移动平台。

waypointsD = (1900 140 65;1910 100 65];时间= 60 [0];trajD = waypointTrajectory (waypointsD,“TimeOfArrival”时间,…“ReferenceFrame”,“ENU表示”,“AutoBank”,真的,“AutoPitch”,真正的);egoUAV = uavPlatform (“EgoVehicle”场景,“轨迹”trajD,“ReferenceFrame”,“ENU表示”);updateMesh (egoUAV“quadrotor”{5},(0 0 1),眼睛(4));

定义无人机传感器套件

自我车辆上的传感器。使用激光雷达冰球中常用的汽车应用[1]。冰球是一个小的传感器,可以附加quadrotor。激光雷达冰球使用以下规范:

距离分辨率:3厘米
最大射程:100米
360度方位跨度为0.2°决议
30度海拔跨度2°决议
更新速度:10赫兹
山90°倾斜向下看

% quadrotor挂载激光雷达lidarOrient = (90 90 0);lidarSensor = uavLidarPointCloudGenerator (“MaxRange”,100,…“RangeAccuracy”,0.03,…“ElevationLimits”15 [-15],…“ElevationResolution”2,…“AzimuthLimits”(-180 180),…“AzimuthResolution”,0.2,…“UpdateRate”10…“HasOrganizedOutput”、假);激光雷达= uavSensor (“激光雷达”egoUAV lidarSensor,“MountingLocation”(0 0 3),“MountingAngles”,lidarOrient);

接下来,添加一个雷达使用radarDataGenerator系统对象的雷达工具箱。这个传感器添加到无人机平台,您需要定义一个自定义的适配器类。细节,请参阅模拟雷达传感器安装在无人机(无人机工具箱)的例子。在本例中,您使用helperRadarAdaptor类。这个类使用目标定义长方体的网格几何尺寸对雷达模型。网还用于推导出一个简单的为每个目标RCS的签名。基于Echodyne EchoFlight无人机雷达[2],设置雷达配置为:

频率:24.45- - - - - -24.65 GHz
视野:120°方位80°高程
解析:在海拔2度方位,6°
全扫描率:1 Hz
灵敏度:0 dBsm,海拔200米

此外,配置雷达输出多个检测每个对象。尽管代表点的雷达可以输出跟踪目标,你想估计目标的程度,这不是可以用默认跟踪输出。因此,设置TargetReportFormat财产检测因此雷达原油直接检测报告。

% quadrotor挂载雷达。radarSensor = radarDataGenerator (“没有扫描”,“SensorIndex”,1…“FieldOfView”(120 80),…“UpdateRate”,1…“MountingAngles”,30 0][0,…“HasElevation”,真的,…“ElevationResolution”6…“AzimuthResolution”2,…“RangeResolution”4…“RangeLimits”200年[0],…“ReferenceRange”,200,…“CenterFrequency”24.55 e9,…“带宽”200 e6,…“TargetReportFormat”,“检测”,…“DetectionCoordinates”,“传感器矩形”,…“HasFalseAlarms”假的,…“FalseAlarmRate”1 e);雷达= uavSensor (“雷达”、egoUAV helperRadarAdaptor (radarSensor));

定义跟踪系统

激光雷达点云处理

激光雷达传感器返回点云。融合激光雷达输出,必须集中提取点云检测的对象。段地形使用segmentGroundSMRF从激光雷达工具箱函数。剩下的点云是集群,一个简单的阈值应用到每个集群平均海拔过滤检测。适合每个集群的长方体来提取边界框检测。辅助类helperLidarDetector在这个例子中实现细节。

激光雷达长方体检测使用的格式objectDetection对象。这些检测的测量状态 $(x, y, z, l, W, H, 问_{0}, 问_{1}, 问_{2}, 问_{3}]$ ,地点:

$x, y, z$ 沿着东是长方体的中心坐标,北,和(ENU表示)轴的情况下,分别。
$l, W, H$ 的长度、宽度和高度分别为长方体,。
${问 = 问}_{0} + 问_{1} 。我 + 问_{2} 。 j + 问_{3} 。 k$ 是四元数定义的方向长方体ENU表示轴。

lidarDetector = helperLidarDetector(场景)

lidarDetector = helperLidarDetector属性:MaxWindowRadius: 3 GridResolution: 1.5000 SegmentationMinDistance: 5 MinDetectionsPerCluster: 2 MinZDistanceCluster: 20 EgoVehicleRadius: 10

激光雷达跟踪

使用一个点目标跟踪,trackerJPDA,跟踪激光雷达探测边界框。点跟踪器假定每个无人机最多只能生成一个每个传感器扫描检测。这种假设是有效的,因为你有集群点云成长方体。建立一个跟踪器,您需要定义运动模型和测量模型。在本例中,您使用的无人机的动力学建模一个增广的恒定速度模型。恒定的速度模型是足够的跟踪轨迹连续飞行腿或缓变部分组成。此外,假设无人机的方向是恒定的,并假定无人机的尺寸不变。因此,跟踪状态和状态转移方程 $X = (x, v_{x}, y, v_{y}, z, v_{z}, l, W, H, 问_{0}, 问_{1}, 问_{2}, 问_{3}]$ 和

$X_{k + 1} = (\begin{array}{c} (\begin{array}{c} 1 & t_{年代} & 0 & 。 & 。 & 0 \\ 0 & 1 & 0 & 。 & 。 \\ 。 & 。 & 1 & t_{年代} & 。 & 。 \\ 。 & 。 & 1 & 0 & 0 \\ 。 & 。 & 1 & t_{年代} \\ 0 & 。 & 。 & 。 & 0 & 1 \end{array}] & 0_{3} & 0_{3} \\ 0_{3} & 我_{3} & 0_{3 x4} \\ 0_{3} & 0_{4 x3} & 我_{4} \end{array}] X_{k} + 问_{k}$

在这里, $v_{x}$ , $v_{y}$ , $v_{z}$ 是长方体的速度矢量坐标沿着场景ENU表示轴。跟踪定位使用四元数的不连续欧拉角当使用跟踪滤波器。 $t_{年代}$ ,更新之间的时间间隔k和k + 1,等于0.1秒。最后, $问_{k}$ 是捕获的加法过程噪声建模不准确。

内部转移矩阵对应于恒定的速度模型。定义一个增强版的状态constvel和cvmeas考虑到额外的恒定状态。支持功能的实现细节金宝appinitLidarFilter,augmentedConstvel,augmentedConstvelJac,augmentedCVmeas,augmentedCVMeasJac在最后的例子。

lidarJPDA = trackerJPDA (“TrackerIndex”2,…“AssignmentThreshold”(70 150),…“ClutterDensity”1 e-16…“DetectionProbability”,0.99,…“DeletionThreshold”,10 [10],…删除激光雷达跟踪如果错过了1秒“ConfirmationThreshold”[4 - 5],…“FilterInitializationFcn”@initLidarFilter)

lidarJPDA = trackerJPDA属性:TrackerIndex: 2 FilterInitializationFcn: @initLidarFilter MaxNumEvents:正EventGenerationFcn:“jpdaEvents”MaxNumTracks: 100 MaxNumDetections:正MaxNumSensors: 20 TimeTolerance: 1.0000 e-05 AssignmentThreshold: 150年[70]InitializationThreshold: 0 DetectionProbability: 0.9900 ClutterDensity: 1.0000 e-16 OOSMHandling:“终止”TrackLogic:“历史”ConfirmationThreshold: [4 - 5] DeletionThreshold: 10 [10] HitMissThreshold: 0.2000 HasCostMatrixInput:假HasDetectableTrackIDsInput:假StateParameters: [1×1 struct] NumTracks: 0 NumConfirmedTracks: 0 EnableMemoryManagement:假的

雷达跟踪

在这个例子中,假设雷达返回等预处理,只返回从移动对象是保存的,也就是说,没有返回从地面或建筑物。雷达测量状态 $(x, v_{x}, y, v_{y}, z, v_{z}]$ 。雷达分辨率很好足以生成多个返回/无人机目标及其检测不应美联储直接点目标跟踪。有两种可能的方法来跟踪与高分辨率雷达探测。集群的一个方法是,你可以检测和增强国家尺寸和取向与激光雷达常数如前面做的长方体。在另一种方法,您可以为检测到一个扩展目标跟踪在这个例子通过使用GGIW-PHD跟踪。这个跟踪估计每个目标的程度使用逆Wishart分布,其期望是一个3×3正定矩阵,表示目标的程度作为一个3 d椭圆。第二种方法是可取的,因为你没有太多的每个对象检测和集群比扩展目标跟踪不准确

创建一个GGIW-PHD跟踪器,首先定义跟踪传感器配置为每个传感器追踪报道。在这种情况下,您需要定义的配置只有一个雷达。移动雷达安装平台时,您需要更新该配置与当前雷达构成每个跟踪步骤之前。接下来,定义一个过滤器的初始化函数基于传感器配置。最后,构造一个trackerPHD对象和增加分区阈值捕获跟踪对象的维度。实现细节的例子所示的支持功能金宝appcreateRadarTracker。

radarPHD = createRadarTracker (radarSensor egoUAV)

radarPHD = trackerPHD属性:TrackerIndex: 1 SensorConfigurations: {[1×1 trackingSensorConfiguration]} PartitioningFcn: @(检波器)partitionDetections(精细、阈值(1)阈值(2),“距离”,“欧几里得”)MaxNumSensors: 20 MaxNumTracks: 1000 MaxNumComponents: 1000 AssignmentThreshold: 50出生率:1.0000 e 03死亡率:1.0000 e-06 ExtractionThreshold: 0.8000 ConfirmationThreshold: 0.9900 DeletionThreshold: 0.1000 MergingThreshold: 50 LabelingThresholds: [1.0100 - 0.0100 0] StateParameters: [1×1 struct] HasSensorConfigurationsInput:真正的NumTracks: 0 NumConfirmedTracks: 0

跟踪融合

创建跟踪系统的最后一步是定义一个跟踪熔化炉对象融合激光雷达跟踪和雷达跟踪。使用激光雷达跟踪的13-dimensional状态作为熔融状态的定义。

radarConfig = fuserSourceConfiguration (“SourceIndex”,1…“IsInitializingCentralTracks”,真正的);lidarConfig = fuserSourceConfiguration (“SourceIndex”2,…“IsInitializingCentralTracks”,真正的);熔化炉= trackFuser (“SourceConfigurations”{radarConfig, lidarConfig},…“ProcessNoise”blkdiag(2 *眼(6),1 *(3),0.2 *眼(4)),…“HasAdditiveProcessNoise”,真的,…“AssignmentThreshold”,200,…“ConfirmationThreshold”[4 - 5],…“DeletionThreshold”,5 [5],…“StateFusion”,“十字”,…“StateTransitionFcn”@augmentedConstvel,…“StateTransitionJacobianFcn”,@augmentedConstvelJac);

可视化

使用一个辅助类来可视化场景。本例中使用的助手类uavScenario可视化功能和theaterPlot绘图仪代表检测和跟踪信息。

显示分为五块,分别显示,整个3 d场景,三大通相机无人机,传奇。

观众= helperUAVDisplay(现场);

图UAV-borne激光雷达和雷达融合包含4轴uipanel类型的对象和其他对象。轴1包含33块类型的对象,对象。轴2包含33块类型的对象,对象。轴3包含33块类型的对象,对象。坐标轴对象4包含33块类型的对象,线。

%显示雷达和激光雷达保险[radarcov, lidarcov] = sensorCoverage (radarSensor,激光雷达);

模拟的场景

运行场景和可视化的结果跟踪系统。每个目标的真正构成以及雷达、激光雷达和融合跟踪保存离线度量分析。

设置(现场);s =提高;rng (2021);numSteps = scene.StopTime * scene.UpdateRate;truthlog =细胞(1、numSteps);radarlog =细胞(1、numSteps);lidarlog =细胞(1、numSteps);fusedlog =细胞(1、numSteps);logCount = 0;而(场景)= scene.CurrentTime;时间提前%更新传感器读数和读取数据。updateSensors(现场);egoPose =阅读(egoUAV);%与雷达跟踪[radardets, radarTracks inforadar] = updateRadarTracker(雷达、radarPHD、egoPose、时间);%与激光雷达跟踪[lidardets, lidarTracks nonGroundCloud groundCloud] = updateLidarTracker(激光雷达、lidarDetector lidarJPDA, egoPose);%融合激光雷达和雷达跟踪rectRadarTracks = formatPHDTracks (radarTracks);如果isLocked(熔化炉)| | ~ isempty (radarTracks) | | ~ isempty (lidarTracks) [fusedTracks ~, allfused信息]=熔化炉([lidarTracks; rectRadarTracks]、时间);其他的fusedTracks = objectTrack.empty;结束%保存日志logCount = logCount + 1;lidarlog {logCount} = lidarTracks;radarlog {logCount} = rectRadarTracks;fusedlog {logCount} = fusedTracks;truthlog {logCount} = logTargetTruth (scene.Platforms (1:3));%更新图查看器(radarcov、lidarcov nonGroundCloud、groundCloud lidardets, radardets, lidarTracks, radarTracks, fusedTracks);结束

图UAV-borne激光雷达和雷达融合包含4轴uipanel类型的对象和其他对象。坐标轴对象1包含37个对象类型的补丁,行文本。轴2包含37块类型的对象,对象,文本。轴3包含37块类型的对象,对象,文本。轴4包含37块类型的对象,对象,文本。

基于可视化的结果,执行一个初始跟踪性能的定性评估。显示的最后场景显示了所有三个无人机追踪的自我。与电流传感器套件配置,激光雷达跟踪只有建立部分由于有限的报道激光雷达传感器。更广泛的视野雷达允许建立雷达跟踪更加一致的这一场景。

上面的三个动态gif追逐的显示部分的观点。你可以看到质量的激光雷达跟踪(橙色盒子)是受场景的几何形状的影响。无人机(左)被激光雷达(黄色所示)几乎直接从上面。这使得跟踪捕获的无人机。然而,无人机C(右)部分被雷达导致低估了无人机的大小。此外,估计在真正的无人驾驶飞机重心周期性震荡中心。大型固定翼无人机(中间)生成许多激光雷达点。因此,跟踪器可以探测和跟踪目标的全部一旦完全进入激光雷达的视野。在所有三个案例中,雷达,蓝色所示,提供更准确的信息目标的程度。结果,融合跟踪框(紫色)更紧密地捕捉每个无人机的程度。 However, the radar returns are less accurate in position. Radar tracks show more position bias and poorer orientation estimate.

跟踪指标

在本节中,您使用OSPA分析跟踪系统的性能(2)跟踪指标。首先定义距离函数量化跟踪和真理之间的错误使用标量值。OSPA值意味着一个更好的整体性能较低。

ospaR = trackOSPAMetric (“指标”,“OSPA (2)”,“距离”,“自定义”,“DistanceFcn”,@metricDistance);ospaL =克隆(ospaR);ospaF =克隆(ospaR);numSteps ospaRadar = 0 (1);numSteps ospaLidar = 0 (1);numSteps ospaFused = 0 (1);为i = 1: numSteps真理= truthlog {};ospaRadar (i) = ospaR (radarlog{},真理);ospaLidar (i) = ospaL (lidarlog{},真理);ospaFused (i) = ospaF (fusedlog{},真理);结束图绘制(ospaRadar,“颜色”viewer.RadarColor,“线宽”2);持有在网格在情节(ospaLidar“颜色”viewer.LidarColor,“线宽”2);情节(ospaFused“颜色”viewer.FusedColor,“线宽”2);传奇(“雷达”,激光雷达的,“激光雷达+雷达”);包含(“步骤”)ylabel (“OSPA (2)”)

分析了系统的整体性能。每个追踪处罚没有跟踪的无人机即使目标无人机传感器覆盖范围之外的。这表明融合时的性能提升激光雷达和雷达由于添加监视区。尤其明显的仿真跟踪两个目标,一个由雷达和其他激光雷达,但都fuser追踪的。此外,您可以看到融合OSPA激光雷达和雷达OSPA低于最低,显示融合跟踪比每个人更好的质量跟踪。

%清理removeCustomTerrain (“southboulder”);rng(年代);

总结

这个例子向您展示了如何建模UAV-borne激光雷达和雷达跟踪系统和测试它在一个城市空气流动情况。你使用了uavScenario对象创建一个实际的城市环境与地形和建筑物。然后生成合成传感器数据来测试一个完整的跟踪系统链,包括点云处理,点目标和扩展目标跟踪,跟踪融合。

金宝app支持功能

createScenario创建uavScenario使用OpenStreetMap地形和建筑网数据。

函数现场= createScenario (dtedfile buildingfile)试一试addCustomTerrain (“southboulder”,dtedfile);抓%定义地形已经补充道。结束minHeight = 1.6925 e + 03;latlonCenter = (39.9786 - -105.2882 minHeight);现场= uavScenario (“UpdateRate”10“StopTime”现年40岁的…“ReferenceLocation”,latlonCenter);%增加地形网格sceneXLim = (1800 - 2000);sceneYLim = [0 200];scene.addMesh (“地形”,{“southboulder”,sceneXLim sceneYLim} [0 0 0]);%增加建筑scene.addMesh (“建筑”,{buildingfile, sceneXLim sceneYLim,“汽车”}[0 0 0]);结束

createRadarTracker创建trackerPHD跟踪融合雷达探测。

函数egoUAV追踪= createRadarTracker(雷达)%为trackerPHD创建传感器配置fov = radar.FieldOfView;sensorLimits =[视场(1)/ 2 fov (1) / 2;视场(2)/ 2视场(2)/ 2;0正);sensorResolution = [radar.AzimuthResolution; radar.ElevationResolution;radar.RangeResolution];Kc = radar.FalseAlarmRate / (radar.AzimuthResolution * radar.RangeResolution * radar.ElevationResolution);Pd = radar.DetectionProbability;sensorPos = radar.MountingLocation (:);sensorOrient = rotmat(四元数(radar.MountingAngles“eulerd”,“ZYX股票”,“帧”),“帧”);%指定帧信息对无人机的雷达sensorTransformParameters(1) =结构(“帧”,“球”,…“OriginPosition”sensorPos,…“OriginVelocity”0 (1),…%传感器不移动相对于自我“定位”sensorOrient,…“IsParentToChild”,真的,…%坐标系旋转提供方向“HasElevation”,真的,…“HasVelocity”、假);%指定无人机对场景的帧信息egoPose =阅读(egoUAV);sensorTransformParameters(2) =结构(“帧”,“矩形”,…“OriginPosition”egoPose (1:3)…“OriginVelocity”egoPose(6节),…“定位”,rotmat(四元数(egoPose (13)),“帧”),…“IsParentToChild”,真的,…“HasElevation”,真的,…“HasVelocity”、假);radarPHDconfig = trackingSensorConfiguration (radar.SensorIndex,…“IsValidTime”,真的,…“SensorLimits”sensorLimits,…“SensorResolution”sensorResolution,…“DetectionProbability”帕金森病,…“ClutterDensity”Kc,…“SensorTransformFcn”@cvmeas,…“SensorTransformParameters”,sensorTransformParameters);radarPHDconfig。FilterInitializationFcn = @initRadarFilter;radarPHDconfig。MinDetectionProbability = 0.4;%阈值分割阈值= 16 [3];追踪= trackerPHD (“TrackerIndex”,1…“HasSensorConfigurationsInput”,真的,…“SensorConfigurations”{radarPHDconfig},…“出生率”1 e - 3,…“AssignmentThreshold”,50岁,…%的最小负对数似检测细胞出生添加组件“ExtractionThreshold”,0.80,…%重量阈值过滤组件的宣布道“ConfirmationThreshold”,0.99,…%重量阈值的过滤器组件宣布确认跟踪“MergingThreshold”,50岁,…%阈值合并组件“DeletionThreshold”,0.1,…%阈值删除组件“LabelingThresholds”(1.01 - 0.01 0),…%这就意味着没有track-splitting。读LabelingThresholds帮助“PartitioningFcn”@(检波器)partitionDetections(精细、阈值(1)阈值(2),“距离”,“欧几里得”));结束

initRadarfilter实现了GGIW-PHD过滤器使用的trackerPHD对象。这个过滤器用于追踪更新初始化期间新生组件的密度和初始化新组件检测分区。

函数博士= initRadarFilter (detectionPartition)如果输入参数个数= = 0%过程噪声sigP = 0.2;sigV = 1;Q =诊断接头([sigP、sigV sigP, sigV, sigP, sigV]。^ 2);博士= ggiwphd(零(0),repmat(眼(6),(1 1 0)),…“ScaleMatrices”0 (3 3 0),…“MaxNumComponents”,1000,…“ProcessNoise”问,…“HasAdditiveProcessNoise”,真的,…“MeasurementFcn”@cvmeas,…“MeasurementJacobianFcn”@cvmeasjac,…“PositionIndex”(1 3 5),…“ExtentRotationFcn”@ (x, dT)眼(3类(x)),…“HasAdditiveMeasurementNoise”,真的,…“StateTransitionFcn”@constvel,…“StateTransitionJacobianFcn”,@constveljac);其他的输入参数个数百分比= = 1% - - - - - - - - - - - - - - - - - -% 1)高斯混合配置% 2)配置逆Wishart混合物% 3)配置伽马混合% - - - - - - - - - - - - - - - - - -% % 1)高斯混合配置meanDetection = detectionPartition {1};n =元素个数(detectionPartition);%收集所有测量和测量噪音。allDets = [detectionPartition {}):;zAll = horzcat (allDets.Measurement);拉尔=猫(3、allDets.MeasurementNoise);%指定意味着噪声和测量z =意味着(zAll, 2);R =意味着(拉尔,3);meanDetection。测量= z;meanDetection。MeasurementNoise = R;%解析意味着对协方差的位置和速度的检测。[posMeas, velMeas posCov] = matlabshared.tracking.internal.fusion.parseDetectionForInitFcn (meanDetection,“initRadarFilter”,“双”);%创建一个恒定速度状态和协方差州= 0 (6,1);协方差= 0 (6,6);州(1:2:结束)= posMeas;州(2:2:结束)= velMeas;协方差(1:2:最终,1:2:结束)= posCov;协方差(2:2:最终,2:2:结束)= 10 *眼(3);%过程噪声sigP = 0.2;sigV = 1;Q =诊断接头([sigP、sigV sigP, sigV, sigP, sigV]。^ 2);% % 2)配置逆Wishart混合参数%的程度将在positional-space测量的传播。e = zAll - z;Z = e * e ' / n + R;景深= 150;%测量雅可比矩阵p = detectionPartition {1} .MeasurementParameters;H = cvmeasjac (p);Bk = H(1:2,::结束);Bk2 =眼睛(3)/汉堡王;V = (dof-4) * Bk2 * Z * Bk2 ';%配置伽马混合参数的标准差%的数量检测n / 4α= 16;%的形状β= 16 / n;%的速度博士= ggiwphd (……高斯参数州,协方差,…“HasAdditiveMeasurementNoise”,真的,…“ProcessNoise”问,…“HasAdditiveProcessNoise”,真的,…“MeasurementFcn”@cvmeas,…“MeasurementJacobianFcn”@cvmeasjac,…“StateTransitionFcn”@constvel,…“StateTransitionJacobianFcn”@constveljac,…“PositionIndex”(1 3 5),…“ExtentRotationFcn”@ (x, dT) (3),……逆Wishart参数“DegreesOfFreedom”景深,…“ScaleMatrices”V,…“TemporalDecay”,150,……γ参数“形状”α,“利率”,β,…“GammaForgettingFactors”,1.05);结束结束

formatPHDTracks格式椭圆GGIW-PHD追踪到矩形增广状态轨迹跟踪融合。convertExtendedTrack返回的状态和状态协方差增强矩形状态。逆Wishart随机矩阵的特征值是用来推导出矩形尺寸。特征向量提供取向四元数。在本例中,您使用一个任意的协方差为雷达跟踪维度和取向,这通常是足够的跟踪。

函数tracksout = formatPHDTracks (tracksin)%从ggiwphd objectTrack与状态转换跟踪结构的定义% (x y z vx v vz L W H q0 q1 q3)N =元素个数(tracksin);tracksout = repmat (objectTrack N 1);为i = 1: N tracksout (i) = objectTrack (tracksin(我));(状态,statecov) = convertExtendedTrack (tracksin(我));tracksout(我)。=状态;tracksout(我)。StateCovariance = statecov;结束结束函数[状态,statecov] = convertExtendedTrack(跟踪)%增加程度的状态信息程度上= track.Extent;[V D] = eig(程度);%选择L > W > h .使用1.5σ作为维度[dim, idx] =排序(1.5 * sqrt(诊断接头(D)),“下”);V = V (:, idx);q =四元数(V,“rotmat”,“帧”);q =问:/规范(q);(q1、q2,第三、第四季度)= (q)部分;状态= [track.State;dim (:);第一季度;q2;第三季度;第四季度);statecov = blkdiag(轨道。StateCovariance, 4*eye(3), 4*eye(4));结束

updateRadarTracker更新雷达跟踪链。函数首先读取当前雷达返回。然后传递给GGIW-PHD跟踪雷达返回更新后的传感器配置与当前姿势自我无人机。

函数[radardets, radarTracks inforadar] = updateRadarTracker(雷达、radarPHD egoPose,时间)[radardets ~, ~, ~, ~] =阅读(雷达);% isUpdated和时间输出不兼容这个工作流inforadar = [];如果国防部(时间,1)~ = 0 radardets = {};结束如果国防部(时间,1)= = 0 & & (isLocked (radarPHD) | | ~ isempty (radardets))%更新跟踪雷达传感器配置款= radarPHD.SensorConfigurations;款{1}.SensorTransformParameters (2)。OriginPosition = egoPose (1:3);款{1}.SensorTransformParameters (2)。OriginVelocity = egoPose (6);款{1}.SensorTransformParameters (2)。取向= rotmat(四元数(egoPose (13)),“帧”);[radarTracks, ~, ~, inforadar] = radarPHD (radardets、配置、时间);elseifisLocked (radarPHD) radarTracks = predictTracksToTime (radarPHD“确认”、时间);radarTracks = arrayfun (@ (x) setfield (x,“UpdateTime”、时间)、radarTracks);其他的radarTracks = objectTrack.empty;结束结束

updateLidarTracker更新激光雷达跟踪链。函数首先读取当前点云的输出激光雷达传感器。然后点云处理提取对象检测。最后,这些检测传递到目标跟踪。

函数[lidardets, lidarTracks nonGroundCloud groundCloud] = updateLidarTracker(激光雷达、lidarDetector lidarJPDA, egoPose)[~,时间,ptCloud] =阅读(激光雷达);%激光雷达总是更新[lidardets, nonGroundCloud groundCloud] = lidarDetector (egoPose、ptCloud、时间);如果isLocked (lidarJPDA) | | ~ isempty (lidardets) lidarTracks = lidarJPDA (lidardets、时间);其他的lidarTracks = objectTrack.empty;结束结束

initLidarFilter激光雷达跟踪初始化滤波器。最初的追踪状态来源于检测位置测量。协方差速度设置为0,允许未来的检测跟踪有关。增广状态运动模型,测量函数,定义和雅克比也在这里。

函数卡尔曼滤波器= initLidarFilter(检测)%激光雷达测量:[x y z L W H q0 q1 q3)量= detection.Measurement;initState =[量(1),0;量(2);0;量(3);0;量(4:6);量(7:10)];initStateCovariance = blkdiag(100 *眼(6),100 *眼(3)、眼睛(4));%过程噪声标准差sigP = 1;sigV = 2;sigD = 0.5;%的尺寸不变,但部分观察sigQ = 0.5;Q =诊断接头([sigP、sigV sigP, sigV, sigP, sigV, sigD, sigD, sigD, sigQ, sigQ, sigQ, sigQ]。^ 2);卡尔曼滤波器= trackingEKF (“状态”initState,…“StateCovariance”initStateCovariance,…“ProcessNoise”问,…“StateTransitionFcn”@augmentedConstvel,…“StateTransitionJacobianFcn”@augmentedConstvelJac,…“MeasurementFcn”@augmentedCVmeas,…“MeasurementJacobianFcn”,@augmentedCVmeasJac);结束函数dt stateOut = augmentedConstvel(状态)%增广状态恒定速度stateOut = constvel(状态(1:6,:),dt);stateOut = vertcat (stateOut、州(7:最后,:));%规范化四元数在预测阶段idx = 13;qparts = stateOut (idx:);n =√总和(qparts ^ 2));qparts = qparts. / n;:stateOut (idx qparts (1) < 0) = -qparts (:, qparts (: 1) < 0);结束函数雅可比矩阵= augmentedConstvelJac(状态,变长度输入宗量)雅可比矩阵= constveljac(状态(1:6,:),变长度输入宗量{:});雅可比矩阵= blkdiag(雅可比矩阵、眼睛(7));结束函数测量= augmentedCVmeas(州)测量= cvmeas(状态(1:6,:));测量=[测量;:状态(七);:状态(13)];结束函数雅可比矩阵= augmentedCVmeasJac(状态,变长度输入宗量)雅可比矩阵= cvmeasjac(状态(1:6,:),变长度输入宗量{:});雅可比矩阵= blkdiag(雅可比矩阵、眼睛(7));结束

sensorCoverage构造传感器覆盖配置结构可视化。

函数[radarcov, lidarcov] = sensorCoverage (radarSensor,激光雷达)radarcov = coverageConfig (radarSensor);%缩小覆盖范围限制视觉上的混乱radarcov。= 10;lidarSensor = lidar.SensorModel;lidarcov = radarcov;lidarcov。我ndex = 2; lidarcov.FieldOfView = [diff(lidarSensor.AzimuthLimits); diff(lidarSensor.ElevationLimits)]; lidarcov.Range = 5; lidarcov.Orientation = quaternion(lidar.MountingAngles,“eulerd”,“ZYX股票”,“帧”);结束

logTargetTruth日志真正构成整个模拟的性能分析和维度。

函数logEntry = logTargetTruth(目标)n =元素个数(目标);targetPoses = repmat(结构体(“位置”[],“速度”[],“维度”[],“定位”[]),1,n);uavDimensions = [5 5 0.3;9.8 8.8 2.8;5 5 0.3];为i = 1: n构成=阅读(目标(i));targetPoses(我)。位置=姿势(1:3);targetPoses(我)。速度=姿势(6);targetPoses(我)。尺寸= uavDimensions(我:);targetPoses(我)。取向=姿势(13);targetPoses(我)。PlatformID =我;结束logEntry = targetPoses;结束

metricDistance定义一个自定义为GOSPA距离。这个距离包含错误的位置,速度,轨道的维度,和方向。

函数= metricDistance(跟踪、真理)positionIdx = [1 3 5];velIdx = (2 4 6);dimIdx =七章;qIdx = 13;trackpos = track.State (positionIdx);trackvel = track.State (velIdx);trackdim = track.State (dimIdx);trackq =四元数(track.State (qIdx) ');truepos = truth.Position;truevel = truth.Velocity; truedim = truth.Dimension; trueq = quaternion(truth.Orientation); errpos = truepos(:) - trackpos(:); errvel = truevel(:) - trackvel(:); errdim = truedim(:) - trackdim(:);重量百分比表示为逆所需的精度posw = 1/0.2;% m ^ 1velw = 1/2;% (m / s) ^ 1dimw = 1/4;% m ^ 1orw = 1/20;%度^ 1distPos =√errpos‘* errpos);distVel =√errvel‘* errvel);distdim =√errdim‘* errdim);distq = rad2deg (dist (trackq trueq));= (distPos * posw + distVel * velw + distdim * dimw + distq * orw) / (posw + velw + dimw + orw);结束

引用

调速发电机激光雷达冰球:https://velodynelidar.com/下载188bet金宝搏products/puck/
Echodyne无人机雷达:https://www.echodyne.com/defense/uav-radar/