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

这个例子使用:

打开实时脚本

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

介绍

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

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

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

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

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

本例中使用的场景表示一个城市十字路口场景，并包含各种对象，包括行人、骑自行车的人、汽车和卡车。ego车辆配备了6个均匀激光雷达传感器，每个传感器的视野为90度，提供360度覆盖ego车辆周围。有关场景和传感器模型的更多详细信息，请参阅城市环境中使用多激光雷达的网格跟踪的例子。场景和传感器的定义封装在helper函数中helperGridBasedPlanningScenario。

%用于重复结果rng (2020);创建场景、自我车辆和模拟激光雷达传感器[scenario, egoVehicle, lidar] = helperGridBasedPlanningScenario;

控件定义一个基于网格的跟踪器trackerGridRFS系统对象™。跟踪器输出对象级和网格级对环境的估计。网格水平估计描述了局部环境的占用和状态，可以作为跟踪器的第四个输出。有关如何设置基于网格的跟踪器的详细信息，请参阅城市环境中使用多激光雷达的网格跟踪的例子。

%设置每个激光雷达的传感器配置sensorConfigs = cell(numel(lidar)，1);%填写传感器配置为i = 1: nummel (sensorConfigs) sensorConfigs{i} = helperGetLidarConfig(lidar {i}，egoVehicle);结束%设置跟踪器tracker = trackerGridRFS(“SensorConfigurations”sensorConfigs,…“HasSensorConfigurationsInput”,真的,…“GridLength”, 120,…“GridWidth”, 120,…“GridResolution”2,…“GridOriginInLocal”(-60 -60),…“NumParticles”1 e5,…“NumBirthParticles”2 e4,…“VelocityLimits”，[-15 15;-15 15]，…“BirthProbability”, 0.025,…“ProcessNoise”5 *眼(2),…“死亡率”1 e - 3,…“FreeSpaceDiscountFactor”1飞行,…“AssignmentThreshold”8…“MinNumCellsPerCluster”4…“ClusteringThreshold”4…“ConfirmationThreshold”(3 - 4),…“DeletionThreshold”4 [4]);

设置运动规划器

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

方法定义全局引用路径referencePathFrenet(导航工具箱)对象通过在驾驶场景的笛卡尔坐标框架中提供路径点。本例中使用的参考路径定义了一条在十字路口右转的路径。

Waypoints = [-110.6 -4.5 0;49 -4.5 0;55.5 -17.7 -pi/2;55.5 -130.6 -pi/2];% [x y θ]使用路径点创建一个参考路径refPath = referencePathFrenet(路径点);可视化参考路径FIG =图(“单位”，“归一化”，“位置”，[0.1 0.1 0.8 0.8]);Ax =坐标轴(图);(ax,“上”);情节(场景中,“父”、ax);显示(refPath“父”、ax);xlim (ax, (-120 80));ylim (ax, 40 [-160]);

snapnow;

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

局部轨迹样本
找到可行且无碰撞的轨迹
选择最优准则，选择最优轨迹

下面几节将讨论本地规划算法的每个步骤以及用于执行每个步骤的辅助函数。

样本局部轨迹

在仿真的每一步，规划算法生成一个样本轨迹列表，供自我车辆选择。通过将自我飞行器的当前状态与期望的终端状态连接起来，对局部轨迹进行采样。使用trajectoryGeneratorFrenet(导航工具箱)对象连接当前和终端状态以生成局部轨迹。通过提供参考路径和轨迹所需的及时分辨率来定义对象。该对象使用五阶多项式在Frenet坐标中连接初始状态和最终状态。

connector = trajectorygenerator (refPath，“TimeResolution”, 0.1);

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

%可视化路径区域采样策略可视化pathPoints = closestPoint(refPath, refPath. waypoints (:，1:2));roadS = pathPoints(:，end);十字路口=道路(2、尽头);intersectionBuffer = 20;pathGreen = [interpolate(refPath,linspace(0,intersectionS-intersectionBuffer,20));…南(1,6);…插入(refPath linspace(交叉口、道路(结束),100)));pathBlue = interpolate(refPath,linspace(intersectionS-intersectionBuffer,roadS(2,end)，20));(ax,“上”);情节(ax, pathGreen (: 1), pathGreen (:, 2),“颜色”，[0 1 0]，“线宽”5);情节(ax, pathBlue (: 1), pathBlue (:, 2),“颜色”，[0 0 1]，“线宽”5);

snapnow;

当自我车辆处于绿色区域时，采用以下策略对局部轨迹进行采样。自我载具后的终端状态 $Δ T$ 时间定义为:

$x_{自我} （ Δ T ） = ［南 \dot{年代} 0 d 0 0 ］；$

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

${Δ T \in {linspace （ 2 ， 4 ， 6 ）} ， \dot{年代} \in {linspace （ 0 ， {\dot{年代}}_{马克斯，} 10 ）} ， d \in {0 w_{车道}}}$

的使用南在终端状态下启用trajectoryGeneratorFrenet对象，以自动计算在最小扰动轨迹上行进的纵向距离。这种策略产生了一组轨迹，使自我车辆能够加速到最高速度限制( ${\dot{年代}}_{马克斯}$ )速率或以不同速率减速至完全停止。此外，侧向偏移量( $d_{des}$ )允许自我车辆在这些机动过程中改变车道。

定义smax和planespeedLimit = 15;laneWidth = 2.975;

当自我车辆处于轨迹的蓝色区域时，采用以下策略对局部轨迹进行采样:

$x_{自我} （ Δ T ） = ［ {年代}_{停止} 0 0 0 0 0 ］$ ；

在哪里 $Δ T$ 的选择是为了使轨迹中的震动最小。此策略可使车辆停在所需距离( ${年代}_{停止}$ )在右车道上以最小的颠簸轨迹行驶。轨迹采样算法封装在辅助函数中，helperGenerateTrajectory，附在此示例中。

寻找可行和无碰撞的轨迹

上一节中描述的采样过程可以产生运动学上不可行的轨迹，并且超过运动学属性(如加速度和曲率)的阈值。因此，您可以使用辅助函数限制ego车辆的最大加速度和速度helperKinematicFeasibility，它根据这些运动学约束检查每个轨迹的可行性。

定义运动学约束accMax = 15;

此外，您还设置了一个碰撞验证器，以评估自我车辆是否可以在运动学可行的轨迹上进行机动，而不会与环境中的任何其他障碍物发生碰撞。要定义验证器，请使用helper类HelperDynamicMapValidator。这个类使用predictMapToTime的作用trackerGridRFS目标是获得周围环境占用率的短期预测。由于评估中的不确定性随着时间的推移而增加，所以将验证器配置为最大2秒的时间范围。

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

vehDims = vehicleDimensions(egoVehicle.Length,egoVehicle.Width);collisionValidator = HelperDynamicMapValidator“MaxTimeHorizon”2,…%验证的最大范围“TimeResolution”连接器。TimeResolution,…%轨迹样本之间的时间步长“追踪”跟踪器,…%提供跟踪预测“ValidPredictionSpan”5,…%预测有效期为5步“VehicleDimensions”, vehDims);提供自我的维度

选择最优准则

在针对障碍物或环境占用区域验证可行轨迹后，通过定义轨迹的成本函数为每个有效轨迹选择最优性准则。期望不同的成本函数从自我载体中产生不同的行为。在本例中，您将每个轨迹的成本定义为

$C = J_{年代} + J_{d} + 1000 P_{c} + One hundred. {（ {\dot{年代}}_{（ Δ T ）} - {\dot{年代}}_{限制} ）}^{2}$

地点:

$J_{年代}$ 加速度是否在参考路径的纵向上

$J_{d}$ 加速度是否在参考路径的横向

$P_{c}$ 碰撞概率是否由验证器获得

每个轨迹的成本计算是使用辅助函数定义的helperCalculateTrajectoryCosts。从有效轨迹列表中，选取代价最小的轨迹作为最优轨迹。

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

运行场景，从所有激光雷达传感器生成点云，并估计动态占用网格地图。使用动态地图估计和它的预测来规划自我飞行器的局部轨迹。

关闭原始图形并初始化一个新显示关闭(图);display = helperGridBasedPlanningDisplay;%初始自我状态currentEgoState = [-110.6 -1.5 0 0 15 0];helperMoveEgoVehicleToState (egoVehicle currentEgoState);初始化每个传感器的pointCloud输出ptClouds = cell(numel(lidar)，1);sensorConfigs = cell(numel(lidar)，1);%模拟回路而推进(场景)%电流模拟时间time = scenario.SimulationTime;物体相对于自我载体的姿态tgtpose = targetpose (egoVehicle);模拟每个传感器的点云为i = 1:num (lidars) [ptClouds{i}， isValidTime] = step(lidars{i}， tgtpose,time);sensorConfigs{i} = helperGetLidarConfig(lidar {i}，egoVehicle);结束将点云包为跟踪器所需的传感器数据格式sensorData = packAsSensorData(ptClouds,sensorConfigs,time);%呼叫跟踪器[tracks， ~， ~， map] = tracker(sensorData,sensorConfigs,time);使用当前估计更新验证器的未来预测step(collisionValidator, currentEgoState, map, time);%样本轨迹使用当前自我状态和一些运动学%的参数[frenetTrajectories, globalTrajectories] = helperGenerateTrajectory(connector, refPath, currentEgoState, speedLimit, laneWidth, intersectionBuffer);%计算生成轨迹的运动学可行性iskinematicsviable = helperkinematic可行性(frenetTrajectories,speedLimit,accMax);%计算可行轨迹的碰撞有效性可行的全球轨迹=全球轨迹(iskinematicsviable);可行的frenetTrajectories = frenetTrajectories(iskinematicsviable);[isCollisionFree, collisionProb] = isstrajectoryvalid (collisionValidator, practicleglobaltrajectories);计算成本和最终的最优轨迹nonCollidingGlobalTrajectories = feasleglobaltrajectories (isCollisionFree);nonCollidingFrenetTrajectories = feaslefrenettrajectories (isCollisionFree);nonCollodingCollisionProb = collisionProb(isCollisionFree);cost = helperCalculateTrajectoryCosts(nonCollidingFrenetTrajectories, nonCollodingCollisionProb, speedLimit);找到最优轨迹[~，idx] = min(成本);optimalTrajectory = nonCollidingGlobalTrajectories(idx);%绘图装配轨迹= helperassembletrajectories;…iskinematicsviable, isCollisionFree, idx);%更新显示显示(场景、egoVehicle、激光雷达、ptcloud、跟踪器、轨迹、轨迹、碰撞验证器);以最佳轨迹移动自我如果~isempty(optimalTrajectory) currentEgoState = optimalTrajectory. trajectory (2，:);helperMoveEgoVehicleToState (egoVehicle currentEgoState);其他的所有轨迹都违反了运动学可行性%约束或导致碰撞。更多的行为可能需要%的轨迹采样。错误(“无法计算出最优轨迹”);结束结束

结果

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

其次，分析第一次变道时的局部规划算法。在模拟过程中，本节中的快照在时间= 4.3秒时捕获。

在这张快照中，ego车辆刚刚开始执行变道机动进入右侧车道。

showSnaps(显示，3,1);

下面的快照显示了在同一时间步动态网格的估计。网格单元的颜色表示占用该网格单元的对象的运动方向。注意，表示自我车辆前面的汽车的单元格是红色的，表示这些单元格被动态对象占用。此外，汽车在场景的正X方向上移动，因此根据色轮，相应的网格单元的颜色是红色。

f = showSnaps(display, 2,1);如果~isempty(f) ax = findall(f)“类型”，“轴”);斧子。XLim = [0 40];斧子。YLim = [-20 20];S = findall(ax，“类型”，“冲浪”);. xdata = 36 + 1/3*(s。XData - mean(s.XData(:)));s.YData = 16 + 1/3*(s。YData - mean(s.YData(:)));结束

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

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

f = showSnaps(display, 1,1);如果~isempty(f) ax = findall(f)“类型”，“轴”);为I = 1:numel(ax) ax(I)。XLim = [0 40];ax (i)。YLim = [-20 20];结束结束

总结

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

支持功能

函数sensorData = packAsSensorData(ptCloud, configs, time)按照跟踪器要求的格式打包传感器数据％% ptCloud -点云对象的单元数组% configs -传感器配置的单元阵列% time -当前模拟时间激光雷达模拟将输出作为点云对象返回。的位置%属性用于提取点云的x、y和z位置%返回，并将它们打包为包含跟踪器所需信息的结构。sensorData = struct“SensorIndex”{},…“时间”{},…“测量”{},…“MeasurementParameters”, {});为i = 1: nummel (ptCloud)这个传感器的点云thisPtCloud = ptCloud{i};%允许数据和配置之间的映射，而不强制使用%有序输入和静态传感器需要配置输入。sensorData(我)。SensorIndex = config {i}.SensorIndex;%当前时间sensorData(我)。Time =时间;以3 × n定义点的位置来提取测量值sensorData(我)。测量=重塑(thisPtCloud.Location，[]，3)';数据在传感器坐标系中报告，因此进行测量%参数与传感器变换参数相同。sensorData(我)。MeasurementParameters = configs{i}.SensorTransformParameters;结束结束函数config = helperGetLidarConfig(lidar, ego)%获取跟踪器的激光雷达传感器配置％% config -世界框架中激光雷达传感器的配置% lidar - lidarPointCloudGeneration对象自我驱动。场景。场景中的演员定义从传感器到自我的转换senToEgo = struct(“帧”fusionCoordinateFrameType (1)…“OriginPosition”, (lidar.SensorLocation (:); lidar.Height],…“定位”rotmat(四元数([激光雷达。偏航激光雷达。Pitch lidar.Roll],“eulerd”，“ZYX股票”，“帧”)，“帧”)，…“IsParentToChild”,真正的);定义从自我到跟踪坐标的转换egoToScenario = struct(“帧”fusionCoordinateFrameType (1)…“OriginPosition”ego.Position (:)…“定位”rotmat(四元数([自我。偏航自我。Pitch ego.Roll],“eulerd”，“ZYX股票”，“帧”)，“帧”)，…“IsParentToChild”,真正的);使用trackingSensorConfiguration组装。配置= trackingSensorConfiguration(…“SensorIndex”激光雷达。SensorIndex,…“IsValidTime”,真的,…“SensorLimits”, (lidar.AzimuthLimits; 0激光雷达。MaxRange),…“SensorTransformParameters”(senToEgo; egoToScenario),…“DetectionProbability”, 0.95);结束函数helperMoveEgoVehicleToState (egoVehicle currentEgoState)将场景中的自我车辆移动到计划者计算的状态％车辆驾驶。场景。场景中的演员% currentEgoState - [x y theta kappa速度acc]%设置二维位置egoVehicle.Position(1:2) = currentEgoState(1:2);%设置二维速度(s*cos(偏航)s*sin(偏航))egoVehicle.Velocity(1:2) = [cos(currentEgoState(3)) sin(currentEgoState(3))]*currentEgoState(5);%设置偏航度egoVehicle。偏航= currentEgoState(3)*180/pi;设置角速度Z(横摆率)为v/regoVehicle.AngularVelocity(3) = currentEgoState(4)*currentEgoState(5);结束函数isviable = helperkinematic可行性(frenetTrajectories, speedLimit, aMax)检查轨迹的运动可行性％frenetTrajectories - Frenet坐标中的轨迹数组% speedLimit -速度限制(m/s)% aMax -最大加速度(m/s^2)isviable = false(numel(frenetTrajectories)，1);为i = 1:numel(frenetTrajectories)%轨迹的速度speed = frenetTrajectories(i).Trajectory(:，2);%轨迹的加速度acc = frenetTrajectories(i).Trajectory(:，3);速度是否有效?isSpeedValid = ~any(speed < -0.1 | speed > speedLimit + 1);加速有效吗?isAccelerationValid = ~any(abs(acc) > aMax);如果速度和acc都有效，轨迹是可行的isviable (i) = isSpeedValid & isAccelerationValid;结束结束函数cost = helperCalculateTrajectoryCosts(frenetTrajectories, Pc, smax)%计算每条轨迹的成本。％frenetTrajectories - Frenet坐标中的轨迹数组% Pc -验证器计算的每个轨迹的碰撞概率n = numel(frenetTrajectories);Jd = 0 (n,1);Js = 0 (n,1);S = 0 (n,1);为I = 1:n%的时间time = frenetTrajectories(i).Times;%的决议dT = time(2) - time(1);沿着小路颠簸dds = frenetTrajectories(i).Trajectory(:，3);Js(i) = sum(gradient(dds,time).^2)*dT;%垂直于路径的震动% d2L/dt2 = d/dt(dL/ds*ds/dt)ds = frenetTrajectories(i).Trajectory(:，2);ddL = frenetTrajectories(i). trajectory (:，6).*(ds.^2) + frenetTrajectories(i). trajectory (:，5).*dds;Jd(i) = sum(gradient(ddL,time).^2)*dT;s(i) = frenetTrajectories(i).Trajectory(end,2);结束成本= Js + Jd + 1000*Pc(:) + 100*(s - smax).^2;结束