基于动态占用网格地图的城市环境运动规划

此示例使用：

打开生活的脚本

这个示例向您展示了如何使用Frenet参考路径在城市驾驶场景中执行动态重规划。在本例中，您使用局部环境的动态占用网格地图估计来找到最优的局部轨迹。

介绍

自动驾驶汽车的动态重新规划通常是通过一个局部运动规划器完成的。局部运动规划器负责根据全局规划和周围环境信息生成最优轨迹。关于周围环境的信息主要有两种描述方式:

在周围环境中的离散对象集，具有定义的几何形状。
对周围环境中的自由区域和占用区域进行估计的离散网格。

在环境中存在动态障碍时，局部运动规划需要对周围环境的信息进行短期预测，以评估规划轨迹的有效性。环境表示的选择通常由上游感知算法控制。对于规划算法，基于对象的表示提供了对环境的高效内存描述。它还允许以一种更简单的方式定义行为预测的对象间关系。另一方面，基于网格的方法允许无对象模型的表示，这有助于在具有大量对象的复杂场景中进行有效的碰撞检查。基于网格的表示对目标提取的缺陷，如假目标和漏检目标的敏感性较低。通过从基于网格的表示中提取目标假设，可以实现两种方法的混合。

在本例中，您将周围环境表示为一个动态占用网格地图。有关使用离散对象集的示例，请参阅基于Frenet参考路径的公路轨迹规划(导航工具箱)示例。动态占用网格图是基于网格的对ego车辆周围局部环境的估计。除了估计占用概率外，动态占用网格还估计每个单元的运动学属性，如速度、转弯率和加速度。此外，动态网格的估计值可为在不久的将来进行短期预测，以评估当地环境在不久的将来的占用情况。在本例中，通过融合安装在ego车辆上的六个激光雷达的点云，获得基于网格的环境估计。

设置场景和基于网格的跟踪器

本例中使用的场景表示城市交叉口场景，包含各种对象，包括行人、自行车手、汽车和卡车。ego车辆配备六个同质激光雷达传感器，每个传感器的视野为90度，可在ego车辆周围提供360度的覆盖范围。有关场景和传感器型号的更多详细信息，请参阅基于栅格的多激光雷达城市环境跟踪实例场景和传感器的定义包装在helper函数中helperGridBasedPlanningScenario。

%为了获得可重复的结果rng (2020);%创建场景、ego车辆和模拟激光雷达传感器[场景、电子车辆、激光雷达]=基于帮助的规划场景；

现在，使用trackerGridRFS系统对象™。跟踪器输出环境的目标级和网格级估计。网格级估计描述了局部环境的占用和状态，可作为跟踪器的第四个输出。有关如何设置基于网格的跟踪器的更多细节，请参阅基于栅格的多激光雷达城市环境跟踪实例

%为每个激光雷达设置传感器配置sensorConfigs=单元（numel（激光雷达），1）；%填写传感器配置为i=1:numel（传感器配置）传感器配置{i}=helperGetLidarConfig（激光雷达{i}，电子车辆）；结束%设置跟踪器跟踪器=跟踪器(“传感器配置”，传感器配置，...“HassensorConfiguration输入”符合事实的...“GridLength”,120,...“GridWidth”,120,...“GridResolution”2....“GridOriginInLocal”(-60 -60),...“NumParticles”，1e5，...“NumBirthParticles”2 e4,...“VelocityLimits”,[-15 15;-15 15],...“出生概率”,0.025,...“ProcessNoise”，5*眼（2），...“死亡率”，1e-3，...“FreeSpaceDiscountFactor”，1e-2，...“AssignmentThreshold”8....“MinNumCellsPerCluster”4....“ClusteringThreshold”4....“确认阈值”(3 - 4),...“删除阈值”,[4 4]);

设置运动规划器

建立局部运动规划算法，在Frenet坐标系下沿全局参考路径规划最优轨迹。

属性定义全局引用路径参考网(导航工具箱)对象的方法是在驾驶场景的笛卡尔坐标系中提供路径点。本例中使用的参考路径定义了一个在交叉口右转的路径。

航路点=[-110.6-4.50；49-4.50；55.5-17.7-pi/2；55.5-130.6-pi/2]；% [x y]%使用航路点创建参考路径refPath = referencePathFrenet(锚点);%可视化参考路径无花果=图(“单位”,“正常化”,“位置”,[0.1 0.1 0.8 0.8]); ax=轴（图）；握紧（斧头），“上”)；情节（情景），“父”，ax）；显示（参考路径，“父”、ax);xlim (ax, (-120 80));ylim (ax, 40 [-160]);

snapnow;

本例中的局部运动规划算法包括三个主要步骤：

尝尝当地的轨迹
找到可行的和无碰撞的轨迹
选择最优准则，选择最优轨迹

以下各节讨论局部规划算法的每个步骤以及用于执行每个步骤的辅助函数。

局部轨迹样本

在模拟的每一步，规划算法都会生成一个样本轨迹列表，供ego车辆选择。通过将ego车辆的当前状态连接到所需的终端状态，对局部轨迹进行采样。使用轨迹发生器(导航工具箱)对象连接当前和终端状态以生成局部轨迹。通过提供参考路径和轨迹的所需时间分辨率来定义对象。对象使用五阶多项式在Frenet坐标系中连接初始和最终状态。

连接器=轨迹生成器FRENET（参考路径，“时间分辨率”,0.1);

在Frenet坐标下的终端状态采样策略通常取决于道路网络和自我车辆在全局路径不同阶段的期望行为。关于使用不同自我行为的更详细的例子，如巡航控制和车辆跟随，请参阅“通过交通规划自适应路线”一节基于Frenet参考路径的公路轨迹规划(导航工具箱)示例。在此示例中，根据车辆在参考路径上的位置，使用两种不同的策略对终端状态进行采样，如下图中的蓝色和绿色区域所示。

%可视化采样策略可视化的路径区域路径点=闭合点（参考路径，参考路径。路径点（：，1:2））；道路=路径点（：，结束）；交叉点=道路（2，结束）；相交缓冲区=20；路径绿色=[插值（参考路径，林空间（0，交叉点相交缓冲区，20））；...南(1,6);...插值（参照路径、邻域空间（交叉点、道路（终点）、100））；pathBlue=插值（参考路径，邻域空间（交叉点相交缓冲区，道路（2，终点），20））；握紧（斧头），“上”）;情节(ax, pathGreen (: 1), pathGreen (:, 2),“颜色”(0 1 0),“线宽”5);情节(ax, pathBlue (: 1), pathBlue (:, 2),“颜色”,[0 0 1],“线宽”5);

snapnow;

当ego车辆处于绿色区域时，使用以下策略对局部轨迹进行采样。完成后ego车辆的终端状态 $Δ T$ 时间定义为：

$x_{自我} (Δ T) = [南 \dot{s} 0 D 0 0];$

其中变量的离散样本是使用以下预定义集合获得的:

${Δ T \in {邻域 (2., 4., 6.)}, \dot{s} \in {邻域 (0, {\dot{s}}_{马克斯,} 10)}, D \in {0 W_{车道}}}$

使用南在终端状态下，启用轨迹发生器目标自动计算在最小急动轨迹上行驶的纵向距离。此策略生成一组轨迹，使ego车辆加速至最大速度限制( ${\dot{s}}_{马克斯}$ )以不同的速率减速至完全停止。此外，还讨论了横向偏移的采样选择( $D_{des}$ )允许自我车辆在这些机动中改变车道。

%定义smax和wlanespeedLimit = 15;巷宽= 2.975;

当ego车辆位于轨迹的蓝色区域时，使用以下策略对局部轨迹进行采样：

$x_{自我} (Δ T) = [s_{停止} 0 0 0 0 0]$ ;

哪里 $Δ T$ 选择此策略可最大限度地减少轨迹中的颠簸。此策略可使车辆停在所需距离( $s_{停止}$ )在右车道上，以最小的急转弯轨迹行驶。轨迹采样算法被包装在辅助函数中，帮助生成代理的例子。

寻找可行且无碰撞的轨迹

上一节中描述的采样过程可能会产生运动上不可行的轨迹，并超过运动属性（如加速度和曲率）的阈值。因此，您可以使用辅助功能限制ego车辆的最大加速度和速度helperKinematicFeasibility，在这些运动学约束条件下，检验每个轨迹的可行性。

%定义运动约束accMax = 15;

此外，您建立了一个碰撞验证器，以评估ego车辆是否可以在不与环境中任何其他障碍发生碰撞的情况下，在运动学上可行的轨迹上机动。要定义验证器，请使用helper类HelperDynamicMapValidator。这个类使用预测映射时间委员会的职能trackerGridRFS目的获得周围环境占用率的短期预测。由于估计中的不确定性随着时间的推移而增加，请将验证器的最大时间范围配置为2秒。

预测的环境占用率在每一步转换为膨胀的成本图，以说明ego车辆的大小。路径规划器使用0.1秒的时间步长，预测时间范围为2秒。为降低计算复杂性，假设周围环境的占用率在5个时间步长内有效，或0.5秒。因此，在2秒的计划范围内只需要4个预测。除了做出关于碰撞或无碰撞的二元决策外，验证器还提供了ego车辆碰撞概率的度量。该概率可纳入最优标准的成本函数中，以考虑r系统中的不确定性，并在不增加规划者时间范围的情况下做出更好的决策。

vehDims=车辆尺寸（egoVehicle.Length，egoVehicle.Width）；碰撞验证器=HelperDynamicMapValidator(“MaxTimeHorizon”2....验证的最大范围“时间分辨率”，connector.TimeResolution，...%轨迹样本之间的时间步长“追踪器”，追踪器，...%提供预测跟踪器“有效预测范围”5....%预测有效的5步“车辆尺寸”，vehDims）；%提供自我的维度

选择最优性准则

在验证针对障碍物或环境占用区域的可行轨迹后，通过定义轨迹的成本函数，为每个有效轨迹选择最优标准。不同的成本函数预计会产生不同于自我载体的行为。在本例中，您定义了每个轨迹的成本科托里as

$C = J_{s} + J_{D} + 1000 P_{C} + One hundred. {({\dot{s}}_{(Δ T)} - {\dot{s}}_{限制})}^{2.}$

地点:

$J_{s}$ 加速度是在参考路径的纵向上吗

$J_{D}$ 是参考路径横向上的加速度

$P_{C}$ 是验证器获得的碰撞概率

使用辅助函数定义每个轨迹的成本计算助手计算四项目成本. 从有效轨迹列表中，将代价最小的轨迹视为最优轨迹。

运行场景、估计动态地图和规划局部轨迹

运行场景，从所有激光雷达传感器生成点云，并估计动态占用栅格地图。使用动态地图估计及其预测来规划ego车辆的局部轨迹。

%关闭原始图形并初始化新显示关闭（图）；显示=helperGridBasedPlanningDisplay；%初始自我状态currentEgoState=[-110.6-1.50 0 15 0]；helperMoveEgoVehicleToState（egoVehicle，currentEgoState）；%初始化每个传感器的pointCloud输出ptClouds=单元（numel（激光雷达），1）；sensorConfigs=单元（numel（激光雷达），1）；%模拟循环而推进(场景)电流模拟时间时间=scenario.SimulationTime；%物体相对于载体的姿态tgtPoses=目标姿态（电子车辆）；%模拟每个传感器的点云为i=1:numel（激光雷达）[ptClouds{i}，isValidTime]=step（激光雷达{i}，tgtPoses，时间）；sensorConfigs{i}=helperGetLidarConfig（激光雷达{i}，电子车辆）；结束%将点云打包为跟踪器所需的传感器数据格式sensorData = packAsSensorData (ptClouds、sensorConfigs、时间);呼叫追踪者[tracks，~，~，map]=跟踪器（传感器数据、传感器配置、时间）；%使用当前估计更新验证器的未来预测步骤（collisionValidator、currentEgoState、map、time）；%使用当前自我状态和一些运动学参数对轨迹进行采样%参数[frenetTrajectories，globalTrajectories]=helperGenerateTrajectory（连接器、参考路径、currentEgoState、限速、车道宽度、交叉口、交叉缓冲区）；%计算生成轨迹的运动学可行性IsKinematicsPersibility=帮助者Kinematic可行性（法国航线、速度限制、accMax）；%计算可行轨迹的碰撞有效性可行的全球轨道=全球轨道（IsKineticsAvailable）；可行的全球轨道=法国轨道（IsKineticsAvailable）；[isCollisionFree，collisionProb]=isTrajectoryValid（碰撞验证器，可行的全球轨道）；%计算成本和最终最优轨迹nonCollidingGlobalTrajectories = feasibleGlobalTrajectories (isCollisionFree);nonCollidingFrenetTrajectories = feasibleFrenetTrajectories (isCollisionFree);nonCollodingCollisionProb = collisionProb (isCollisionFree);成本= helpercalculatetrajectory (noncollidingfrenettrajectory, noncollodingcollisionprobb, speedLimit);%寻找最优轨迹[~, idx] = min(成本);optimalTrajectory = nonCollidingGlobalTrajectories (idx);%装配用于绘图轨迹=用于绘图的帮助程序装配轨迹（全局轨迹，...是否运动可行，是否无碰撞，idx）；%更新显示显示（场景、电子车辆、激光雷达、云彩、跟踪器、轨道、轨迹、碰撞验证器）；用最优轨迹移动自我如果~isempty（OptimalTraction）currentEgoState=OptimalTraction.Traction（2，：）；helperMoveEgoVehicleToState（egoVehicle，currentEgoState）；其他的%所有轨迹都违反了运动学可行性%限制或导致冲突。更多的行为可能需要轨迹采样。错误(“无法计算最佳轨迹”）;结束结束

后果

分析局部路径规划算法的结果，以及来自地图的预测如何辅助规划者。这个动画显示了整个场景中规划算法的结果。请注意，自我飞行器成功地到达了它想要的目的地，并在必要时绕过了不同的动态对象。由于采样政策的区域变化，ego车辆也在十字路口停了下来。

接下来，分析第一次换道期间的局部规划算法。在模拟过程中，在时间=4.3秒时捕获此部分中的快照。

在此快照中，ego车辆刚刚开始执行右车道的换道操纵。

显示快照（显示，3，1）；

下面的快照显示了在同一时间步长的动态网格的估计。网格单元格的颜色表示占据该网格单元格的物体的运动方向。注意，表示ego车辆前面的汽车的单元格被涂成红色，表示单元格被动态对象占据。此外，汽车在场景中正向X方向移动，因此根据色轮，相应的网格单元格的颜色为红色。

f=显示快照（显示，2，1）；如果~isempty（f）ax=findall（f，“类型”,“轴”）;斧子。XLim = [0 40];斧子。YLim = [-20 20];s = findall (ax,“类型”,“冲浪”）;s. xdata = 36 + 1/3*(s。XData意味着(s.XData (:)));s. ydata = 16 + 1/3*(s。YData意味着(s.YData (:)));结束

根据先前的图像，自我飞行器的计划轨迹通过空间中被占用的区域，如果执行传统静态占用验证，则表示碰撞。然而，动态占用图和验证器通过验证每个时间步骤的轨迹状态与预测占用情况，来说明网格的动态特性。下一个快照显示了不同预测步骤的预测成本图( $Δ T$ )，以及自我飞行器在轨道上的计划位置。预测的成本图被夸大，以说明自负车辆的大小。因此，如果一个代表自我车辆起源的点对象可以被放置在占用地图上而不发生任何碰撞，就可以解释为自我车辆没有与任何障碍发生碰撞。成本图上的黄色区域表示保证与障碍物发生碰撞的区域。碰撞概率在黄色区域外呈指数衰减，直到膨胀区域结束。蓝色区域表示当前预测的碰撞概率为零的区域。

注意，当预测未来地图时，代表ego车辆前面的黄色区域会向前移动。这反映出占用率的预测考虑了周围环境中物体的速度。另外，请注意，在预测期间，分类为静态对象的单元格在网格上保持相对静态。最后，注意自我车辆起源的计划位置不会与成本地图中任何被占用的区域相冲突。这表明自我飞行器可以在这个轨道上成功机动。

f=显示快照（显示，1，1）；如果~isempty（f）ax=findall（f，“类型”,“轴”）;为i=1:numel（ax）ax（i）.XLim=[040]；ax（i）.YLim=[-20]；结束结束

总结

在本例中，您学习了如何使用基于栅格的跟踪器的动态地图预测，trackerGridRFS，以及如何将动态地图与局部路径规划算法相结合，为自我车辆在动态复杂环境中生成轨迹。你们还学习了如何利用占位的动态特性在环境中更有效地规划轨迹。

金宝app辅助功能

函数sensorData = packAsSensorData(ptCloud, configs, time)%按照跟踪器要求的格式打包传感器数据%%ptCloud-点云对象的单元数组%configs-传感器配置的单元阵列% time -电流模拟时间%激光雷达模拟以点云对象的形式返回输出%点云的属性用于提取点云的x、y和z位置%返回并将其打包为包含跟踪器所需信息的结构。sensorData =结构(“传感器索引”{},...“时间”{},...“测量”{},...“MeasurementParameters”, {});为i=1:numel（ptCloud）这个传感器的点云thisPtCloud=ptCloud{i}；%允许数据和配置之间的映射，而不强制%静态传感器的有序输入和需要配置输入。sensorData(我)。SensorIndex =款{我}.SensorIndex;%当前时间传感器数据（i）。时间=时间；%作为3×N定义点位置的超距测量sensorData(我)。测量=重塑(thisPtCloud.Location [], 3) ';%数据在传感器坐标系中报告，从而进行测量%参数与传感器变换参数相同。sensorData（i）.MeasurementParameters=configs{i}.SensorTransformParameters；结束结束函数config=helperGetLidarConfig（激光雷达、ego）获取跟踪器的激光雷达传感器配置%%配置-世界帧中激光雷达传感器的配置% lidar - lidarPointCloudGeneration对象%场景中的自我驱动.scenario.Actor定义从传感器到自我的转变senToEgo =结构(“框架”fusionCoordinateFrameType (1)...“原始位置”, (lidar.SensorLocation (:); lidar.Height],...“定位”rotmat(四元数([激光雷达。偏航激光雷达。Pitch lidar.Roll],“欧勒德”,“ZYX股票”,“帧”)，“帧”)，...“IsParentToChild”，对）；%定义从自我坐标到跟踪坐标的转换egoToScenario=struct(“框架”fusionCoordinateFrameType (1)...“原始位置”，自我定位（：），...“定位”，rotmat（四元数（[ego.Yaw ego.Pitch ego.Roll]，“欧勒德”,“ZYX股票”,“帧”)，“帧”)，...“IsParentToChild”，对）；%使用TrackingSensor配置进行组装。配置=跟踪传感器配置(...“传感器索引”激光雷达。SensorIndex,...“IsValidTime”符合事实的...“传感器限制”，[lidar.AzimuthLimits；0 lidar.MaxRange]，...“SensorTransformParameters”(senToEgo; egoToScenario),...“DetectionProbability”,0.95);结束函数helperMoveEgoVehicleToState (egoVehicle currentEgoState)%将场景中的车辆移动到计划员计算的状态%%egoVehicle-场景中的driving.scenario.Actor%电流状态-[x yθ-卡帕速度acc]设置2-D位置电子车辆位置（1:2）=当前电子车辆状态（1:2）；%设置二维速度（s*cos（偏航）s*sin（偏航））速度（1:2）=[cos（currentEgoState（3））sin（currentEgoState（3））]*currentEgoState（5）；%以度为单位设置偏航egoVehicle.偏航=当前EgoState（3）*180/pi；%将Z轴角速度（横摆率）设置为v/regoVehicle.AngularVelocity (3) = currentEgoState (4) * currentEgoState (5);结束函数IsAbility=Helper运动学可行性（法国航线、速度限制、aMax）检查运动轨迹的可行性%% frenettrajectory -在Frenet坐标下的轨迹数组% speedLimit -速度限制(m/s)%aMax-最大加速度（m/s^2）IsAbility=false（numel（frenetTrajectories），1）；为i=1:numel（法语区）%弹道速度速度=法向轨迹（i）。轨迹（：，2）；%弹道加速度acc = frenetTrajectories (i) .Trajectory (:, 3);%速度有效吗？isSpeedValid = ~任意(速度< -0.1 |速度>速度限制+ 1);加速有效吗?isAccelerationValid=~any（abs（acc）>aMax）；%如果速度和acc都有效，轨迹可行isFeasible(i) = isSpeedValid & isAccelerationValid;结束结束函数cost = helpercalculatetrajectory (frenettrajectory, Pc, smax)%计算每条轨道的成本。%% frenettrajectory -在Frenet坐标下的轨迹数组%Pc-验证器计算的每条轨迹的碰撞概率n=numel（frenetTrajectories）；Jd=zeros（n，1）；Js=zeros（n，1）；s=zeros（n，1）；为i=1:n%时间时间=法语区（i）.次；%决议dT=时间（2）-时间（1）；沿着小路颠簸dds = frenetTrajectories (i) .Trajectory (:, 3);Js (i) =总和(梯度(dds、时间)。^ 2)* dT;挺拔垂直于路径%d2L/dt2=d/dt（dL/ds*ds/dt）ds=frenetTrajectories（i）.轨迹（：，2）；ddL=frenetTrajectories（i）.轨迹（：，6）。*（ds.^2）+frenetTrajectories（i）.轨迹（：，5）。*dds；Jd（i）=和（梯度（ddL，时间）。^2）*dT；s（i）=frenetTrajectories（i）.轨迹（结束，2）；结束cost = Js + Jd + 1000*Pc(:) + 100*(s - smax).^2;结束