对象跟踪和运动规划使用Frenet参考路径

这个示例使用:

这个例子向您展示了如何动态地重新计划自主车辆的运动估计的基础上,周围的环境。你使用Frenet参考路径和联合概率数据关联(JPDA)跟踪估计和预测其他车辆在高速公路上的运动。相比高速公路使用Frenet参考路径轨迹规划(导航工具箱)示例中,您使用这些估计多目标跟踪的轨迹在这个例子代替地面真理运动规划。

介绍

动态重新规划自主车辆通常是用当地的运动计划。当地的运动计划负责生成最优轨迹根据全球计划和实时信息周围的环境。全球计划公路轨迹规划可以被描述为一个协调的公路中心线预先生成。周围的环境可以描述主要在两个方面:

离散的对象周围环境定义的几何图形。
离散网格估计对自由和被占领的地区周围的环境。

存在动态障碍物,当地的一个运动计划还需要预测环境评估计划的有效性的轨迹。在本例中,您代表周围的环境使用离散的对象的方法。使用离散网格为例,参考运动规划在城市环境中使用动态占用网格地图(传感器融合和跟踪工具箱)的例子。

测量对象状态转换和建模

对象列表和他们的未来预测运动规划一般估计多目标跟踪器。多目标跟踪器接收数据从传感器和估计对象的列表。在跟踪社区,这个列表的对象通常被称为跟踪列表。

在本例中,您使用雷达和摄像头传感器和估计使用JPDA多目标跟踪器跟踪列表。使用任何多目标跟踪器的第一步是定义对象状态,状态如何随时间演化(状态转换模型)以及传感器感知(测量模型)。常见的状态转换模型包括常速模型、恒定加速度模型等。然而,在地图信息的存在,道路网可以集成到运动模型。在本例中,您使用Frenet坐标系来描述对象状态在任何给定的时间步长, $k$ 。

$x_{k} = ({年代}_{k} \dot{{年代}_{k}} d_{k} \dot{d_{k}}]$

在哪里 ${年代}_{k}$ 和 $d_{k}$ 表示物体的距离和垂直于公路中心线,分别。你使用恒速状态转换模型来描述物体运动沿着公路和decaying-speed模型来描述运动垂直于公路中心线。这种衰减速度模型允许你代表车道改变动作由其他车辆在高速公路上。

$(\begin{array}{c} {年代}_{k + 1} \\ \dot{{年代}_{k + 1}} \\ d_{k + 1} \\ \dot{d_{k + 1}} \end{array}] = (\begin{array}{c} 1 & Δ T & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & τ (1 - - - - - - e^{(- - - - - - \frac{Δ T}{τ})}) \\ 0 & 0 & 0 & e^{(- - - - - - \frac{Δ T}{τ})} \end{array}] (\begin{array}{c} {年代}_{k} \\ \dot{{年代}_{k}} \\ d_{k} \\ \dot{d_{k}} \end{array}] + (\begin{array}{c} \frac{Δ T^{2}}{2} & 0 \\ Δ T & 0 \\ 0 & \frac{Δ T^{2}}{2} \\ 0 & Δ T \end{array}] (\begin{array}{c} w_{年代} \\ w_{d} \end{array}]$

在哪里 $Δ T$ 步骤的时差吗 $k$ 和 $k + 1$ , $w_{年代}$ 和 $w_{d}$ 是零均值高斯噪声代表未知的加速Frenet坐标,然后呢 $τ$ 是一个衰减常数。

这个选择的坐标建模对象运动允许您将高速公路参考路径的多目标跟踪框架。参考路径的集成作为附加信息的追踪,并允许追踪改善当前状态估计以及估计对象的预测轨迹。您可以获得度量模型首先将对象状态转换为笛卡儿的位置和速度,然后将它们转换为各自的数量测量方位角和范围等。

设置

场景和传感器

在这个例子中使用的场景使用的创建驾驶场景设计师然后出口到一个MATLAB®函数。自我车辆安装1前瞻性的雷达和5相机提供360度覆盖。模拟雷达和摄像机使用drivingRadarDataGenerator和visionDetectionGenerator分别系统对象。

整个场景和传感器设置中定义的helper函数,helperTrackingAndPlanningScenario,附加的例子。您定义描述公路中心线使用的全球计划referencePathFrenet(导航工具箱)对象。在这个例子中需要访问多个算法参考路径,你定义helperGetReferencePath函数,它使用一个持续的对象,可以由任何函数访问。

rng (2022);%的可重复的结果%设置场景和传感器[场景、egoVehicle传感器]= helperTrackingAndPlanningScenario ();

联合概率数据关联跟踪

你建立了一个联合概率数据关联跟踪使用trackerJPDA(传感器融合和跟踪工具箱)系统对象。你设置FilterInitializationFcn财产的追踪helperInitRefPathFilter函数。这个helper函数定义了一个扩展的卡尔曼滤波器,trackerJPDA(传感器融合和跟踪工具箱),用来估计一个对象的状态。本地函数内helperInitRefPathFilter文件定义了状态转换以及测量模型过滤器。此外,预测轨道运动计划未来的时候,你使用predictTracksToTime(传感器融合和跟踪工具箱)跟踪器的功能。

追踪= trackerJPDA (“FilterInitializationFcn”@helperInitRefPathFilter,…“AssignmentThreshold”,200年正无穷,…“ConfirmationThreshold”,10 [8],…“DeletionThreshold”5 [5]);

运动规划

你使用类似的公路轨迹运动计划的概述高速公路使用Frenet参考路径轨迹规划(导航工具箱)的例子。运动计划使用5秒的规划周期,认为三种模式对采样轨迹自我汽车巡航控制系统,导致车辆,和基本车道改变。整个过程生成最优轨迹裹在helper函数,helperPlanHighwayTrajectory。

helper函数接受一个dynamicCapsuleList(导航工具箱)对象作为输入来找到non-colliding轨迹。碰撞检查执行在整个规划周期的间隔0.5秒。跟踪状态随时间,你更新dynamicCapsuleList对象的仿真循环使用helperUpdateCapsuleList附加功能,与这个例子。

%碰撞检查时间戳tHorizon = 5;%秒deltaT = 0.5;%秒tSteps = deltaT: deltaT: tHorizon;%创建dynamicCapsuleList对象capList = dynamicCapsuleList;capList。MaxNumSteps =元素个数(tSteps) + 1;%指定自我车辆几何carLen = 4.7;carWidth = 1.8;rearAxleRatio = 0.25;egoID = 1;[egoID, egoGeom] = egoGeometry (capList egoID);%增加允许的不确定性和安全缺口egoGeom.Geometry。长度= 2 * carLen;%在米egoGeom.Geometry。半径= carWidth / 2;%在米egoGeom.Geometry.FixedTransform (1) = 2 * carLen * rearAxleRatio;%在米updateEgoGeometry (capList egoID egoGeom);

运行仿真

在本节中,您推进仿真,生成传感器数据和执行动态重新规划使用估计的环境。整个过程分为五个主要步骤:

你收集传感器数据模拟从雷达和摄像头传感器。
你喂JPDA跟踪器的传感器数据来估计当前状态的对象。
你预测对象使用的状态predictTracksToTime函数。
你更新的对象列表计划和计划高速公路轨迹。
你计划上的自我模拟车辆轨迹移动。

%为可视化创建显示结果显示= HelperTrackingAndPlanningDisplay;%初始状态的自我refPath = helperGetReferencePath;egoState = frenet2global (refPath [0 0 0 0.5 * 3.6 0 0]);helperMoveEgoToState (egoVehicle egoState);而推进(场景)%当前时间时间= scenario.SimulationTime;%步骤1。收集数据检测= helperGenerateDetections(传感器、egoVehicle、时间);%步骤2。饲料检测跟踪跟踪=追踪(检测、时间);%步骤3。预测规划周期的跟踪步伐=时间+ tSteps;predictedTracks = repmat(跟踪[1元素个数(步伐)+ 1]);为i = 1:元素个数(步伐)predictedTracks (:, i + 1) = predictTracksToTime(追踪,“确认”步伐(我));结束%步骤4。高速公路轨迹更新胶囊列表和计划currActorState = helperUpdateCapsuleList (capList predictedTracks);[optimalTrajectory, trajectoryList] = helperPlanHighwayTrajectory (capList、currActorState egoState);%可视化结果显示器(场景、egoVehicle传感器、检测、跟踪,capList, trajectoryList);%步骤5。自我规划轨迹移动egoState = optimalTrajectory (2);helperMoveEgoToState (egoVehicle egoState);结束

结果

下面的动画,你可以观察计划自我车辆轨迹中突出显示绿色。动画也展示了自我车辆其他采样轨迹。对于这些其他轨迹,碰撞轨迹被显示为红色,未鉴定的轨迹显示在灰色,kinematically-infeasible轨迹显示在青色的颜色。每个跟踪注释ID代表其独特的身份。注意自我车辆成功演习场景中的障碍。

在接下来的小节,你分析的估计跟踪在特定的时间步骤,了解其如何影响所做的选择运动计划。

Road-integrated运动预测

在本节中,您将了解如何road-integrated运动模型允许追踪获得更准确的长期预测对象在高速公路上。如下所示是一个快照从仿真时间= 30秒。注意绿色车辆的轨迹预测右边的蓝色自我。预测轨迹跟随车辆的车道因为道路网络信息集成与追踪。如果相反,你使用一个常速模型假设对象,瞬时速度的预测轨迹将遵循方向并将错误地当作一个碰撞的运动计划。在这种情况下,运动计划可能产生不安全的操作。

showSnaps(显示器、2、4);%显示快照同时出版

图包含一个uipanel类型的对象。

车道变化预测

在第一部分中,您了解了车道改变动作捕获通过使用一个腐烂的横向速度的模型对象。现在,注意到快照时间= 17.5秒。这时,自我的黄色车右边车辆启动一个车道变化和打算进入自我的车道车辆。注意,其预测轨迹捕获这个策略,跟踪预测它在同一车道自我规划地平线的尽头。这种预测通知该运动计划可能碰撞车辆,因此,计划第一个收益测试可行性自我车辆向左变更车道。然而,紫色的车辆在左边的存在及其预测轨迹使自我车辆右车道改变。你也可以观察这些碰撞轨迹颜色如下红色快照。

showSnaps(显示2 1);%显示快照同时出版

图包含一个uipanel类型的对象。

跟踪缺陷

多目标跟踪器可能有一定的缺陷,会影响运动规划决策。具体来说,多目标跟踪器可以错过对象,报告错误的轨道,或有时冗余的跟踪报告。在下面的快照拍摄时间= 20秒,前面两辆车的跟踪滴跟踪自我车辆由于闭塞。在这种特殊的情况下,这些错过的目标不太可能影响运动的决定计划由于其自我车辆的距离。

showSnaps(显示、2、2);%显示快照同时出版

图包含一个uipanel类型的对象。

然而,当自我车辆接近这些车辆,对自我车辆增加的决定的影响。注意跟踪能够建立一个跟踪这些车辆的时间= 20.4秒,见下面的快照,从而使系统稍微健壮的这些缺陷。配置跟踪运动规划算法时,重要的是要考虑这些缺陷跟踪和调整跟踪确认和跟踪删除逻辑。

showSnaps(显示器、2、3);%显示快照同时出版

图包含一个uipanel类型的对象。

总结

您了解了如何使用一个联合概率数据关联跟踪跟踪车辆使用Frenet引用路径与雷达和摄像头传感器。你配置了跟踪器使用公路地图数据提供关于对象的长期预测。你也使用这些长期预测驱动运动规划师规划轨迹在高速公路上。

万博1manbetx支持功能

函数检测= helperGenerateDetections(传感器、egoVehicle、时间)检测=细胞(0,1);为i = 1:元素个数传感器(传感器)thisDetections ={我}(targetPoses (egoVehicle)、时间);检测=[检测;thisDetections];% #好< AGROW >结束检测= helperAddEgoVehicleLocalization(检测、egoVehicle);检测= helperPreprocessDetections(检测);结束函数detectionsOut = helperAddEgoVehicleLocalization (detectionsIn egoPose) defaultParams =结构(“帧”,“矩形”,…“OriginPosition”0 (1),…“OriginVelocity”0 (1),…“定位”眼睛(3),…“HasAzimuth”假的,…“HasElevation”假的,…“HasRange”假的,…“HasVelocity”、假);帧=字段名(defaultParams);detectionsOut =细胞(元素个数(detectionsIn), 1);为i = 1:元素个数(detectionsIn) thisDet = detectionsIn {};如果iscell (thisDet.MeasurementParameters) measParams = thisDet.MeasurementParameters {1};其他的measParams = thisDet.MeasurementParameters (1);结束newParams =结构;为k = 1:元素个数(帧)如果isfield (measParams帧{k}) newParams。(帧{k}) = measParams。(帧{k});其他的newParams。(帧{k}) = defaultParams。(帧{k});结束结束%为自我车辆添加参数thisDet。MeasurementParameters = [newParams; newParams];thisDet.MeasurementParameters (2)。帧=“矩形”;thisDet.MeasurementParameters (2)。OriginPosition = egoPose.Position (:);thisDet.MeasurementParameters (2)。OriginVelocity = egoPose.Velocity (:);thisDet.MeasurementParameters (2)。取向= rotmat(四元数([egoPose。偏航egoPose。距egoPose.Roll),“eulerd”,“ZYX股票”,“帧”),“帧”)”;%没有信息对象类和属性thisDet。ObjectClassID = 0;thisDet。ObjectAttributes =结构;detectionsOut{我}= thisDet;结束结束函数检测= helperPreprocessDetections(检测)%这个函数预处理雷达和摄像头的检测%配合使用的建模假设跟踪% 1。它删除从相机检测速度信息。这是%,因为这些都是过滤后的估计和假设相机%不可能结合之前定义的信息跟踪。%% 2。它修复相机传感器出现的偏差,因为相机%预测汽车向左或向右自我车辆。%% 3。它膨胀的测量噪声数据与报告的%雷达匹配的数据分辨率传感器为i = 1:元素个数(检测)如果检测我{}。SensorIndex > 1%的相机%去除速度检测我{}。测量=检测{我}.Measurement (1:3);检测我{}。MeasurementNoise = blkdiag(检测{我}.MeasurementNoise (1:2, 1:2), 25);检测{我}.MeasurementParameters (1)。HasVelocity = false;%修正偏差pos =检测{我}.Measurement (1:2);如果abs (pos (1)) < 5 & & abs (pos (2) < 5 (az, ~, r) = cart2sph (pos pos (1), (2), 0);pos (pos (1), (2)] = sph2cart (az 0 r + 0.7);%增加范围检测{我}.Measurement (1:2) = pos;检测{我}.MeasurementNoise (2, 2) = 0.25;结束其他的%雷达检测{我}.MeasurementNoise (3,3) = 0.5 ^ 2/4;结束结束结束

函数helperMoveEgoToState (egoVehicle egoState) egoVehicle.Position (1:2) = egoState (1:2);egoVehicle.Velocity (1:2) = (cos (egoState(3)罪(egoState (3))) * egoState (5);egoVehicle。偏航= egoState(3) * 180 /π;egoVehicle.AngularVelocity(3) = 180 /π* egoState (4) * egoState (5);结束