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

这个例子使用了:

打开实时脚本

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

简介

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

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

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

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

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

本例中使用的场景表示一个城市十字路口场景，包含各种对象，包括行人、骑自行车的人、汽车和卡车。ego车辆配备了六个同质激光雷达传感器，每个传感器的视野为90度，提供360度的覆盖。更多场景和传感器型号请参见基于网格的多激光雷达城市环境跟踪的例子。场景和传感器的定义包含在helper函数中helperGridBasedPlanningScenario．

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

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

设置每个激光雷达的传感器配置sensorConfigs = cell(数字(激光雷达)，1);填写传感器配置为i = 1:numel(sensorConfigs) sensorConfigs{i} = helperGetLidarConfig(lidar {i}，egoVehicle);结束%设置跟踪器跟踪器= trackerdrfs (“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(导航工具箱)对象，在驾驶场景的笛卡尔坐标系中提供路径点。本例中使用的参考路径定义了在十字路口右转的路径。

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

snapnow;

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

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

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

局部轨迹样本

在模拟的每一步，规划算法生成自我飞行器可以选择的样本轨迹列表。通过将自我载具的当前状态连接到所需的终端状态，对局部轨迹进行采样。使用trajectoryGeneratorFrenet(导航工具箱)对象连接当前状态和终端状态，以生成局部轨迹。通过提供参考路径和所需的弹道时间分辨率来定义对象。该对象使用五阶多项式在弗莱内坐标中连接初始状态和最终状态。

连接器=轨迹生成器frenet (refPath，“TimeResolution”, 0.1);

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

为抽样策略可视化可视化路径区域pathPoints = closepoint (refPath, refPath. waypoints (:，1:2));roadS = pathPoints(:，end);十字路口=道路(2，结束);intersectionBuffer = 20;pathGreen = [interpolate(refPath,linspace(0,intersection - intersectionbuffer,20));.．.南(1,6);.．.插入(refPath linspace(交叉口、道路(结束),100)));pathBlue = interpolate(refPath,linspace(intersectionbuffer,roadS(2,end)，20));(ax,“上”）;情节(ax, pathGreen (: 1), pathGreen (:, 2),“颜色”，[0 10 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和wlanespeedLimit = 15;laneWidth = 2.975;

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

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

在哪里 $Δ T$ 选择的目的是为了最小化轨迹期间的颠簸。该策略使车辆能够在所需的距离( ${年代}_{停止}$ )在右车道以最小颠簸轨迹行驶。轨迹采样算法被包裹在辅助函数中，helperGenerateTrajectory，附上这个例子。

寻找可行且无碰撞的轨迹

前一节中描述的采样过程可以产生运动学上不可用的轨迹，并且超过了加速度和曲率等运动学属性的阈值。因此，您可以使用助手函数限制自我车辆的最大加速度和速度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(数字(激光雷达)，1);sensorConfigs = cell(数字(激光雷达)，1);%模拟回路而推进(场景)%当前模拟时间时间= scenario.SimulationTime;%物体相对于自我载具的姿态tgtpose = targetpose (egoVehicle);模拟来自每个传感器的点云为i = 1: nummel (lidar) [ptClouds{i}， isValidTime] = step(lidar {i}， tgtpositions,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(连接器，refPath, currentEgoState, speedLimit, laneWidth, intersectionBuffer);计算生成轨迹的运动学可行性iskinematics可行= helpkinematic可行性(frenetTrajectories,speedLimit,accMax);计算可行轨迹的碰撞有效性globalTrajectories = globalTrajectories(iskinematicsviable);frenettrajectory = frenettrajectory (iskinematicsviable);[isCollisionFree, collisionProb] = isTrajectoryValid(collisionValidator, feasleglobaltrajectories);计算成本和最终的最佳轨迹noncollidingglobaltrajectory = feasleglobaltrajectories (isCollisionFree);nonCollidingFrenetTrajectories = feaslefrenettrajectories (isCollisionFree);nonCollodingCollisionProb = collisionProb(isCollisionFree);成本= helperCalculateTrajectoryCosts(非collidingfrenettrajectory，非collodingcollisionprob, speedLimit);找到最优轨迹[~，idx] = min(成本);optimalTrajectory = noncollidingglobaltrajectory (idx);组装用于绘图轨迹= helperassembletrajectory forplots (globalTrajectories，.．.iskinematicsviable, isCollisionFree, idx);%更新显示显示(场景，egoVehicle，激光雷达，ptClouds，跟踪器，轨道，轨迹，collisionValidator);以最佳轨迹移动自我如果~isempty(optimalTrajectory) currentEgoState = optimalTrajectory. trajectory (2，:);helperMoveEgoVehicleToState (egoVehicle currentEgoState);其他的所有轨迹都违反了运动学可行性。%约束或导致碰撞。更多的行为%轨迹采样可能需要。错误(“无法计算最佳轨迹”）;结束结束

结果

分析本地路径规划算法的结果，以及来自地图的预测如何帮助规划者。这个动画显示了整个场景中规划算法的结果。请注意，自我载具成功地到达了它所期望的目的地，并在必要时围绕不同的动态对象进行操作。由于抽样政策中增加的区域变化，自我车辆也在十字路口停了下来。

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

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

showSnaps(显示，3,1);

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

f = showSnaps(display, 2,1);如果~isempty(f) ax = findall(f，“类型”，“轴”）;斧子。XLim = [0 40];斧子。YLim = [-20 20];S = findall(ax，“类型”，“冲浪”）;s. xdata = 36 + 1/3*(s. xdata)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: number (ax) ax(I)。XLim = [0 40];ax (i)。YLim = [-20 20];结束结束

总结

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

金宝app支持功能

函数sensorData = packAsSensorData(ptCloud, configs, time)将传感器数据打包为跟踪器要求的格式％pointCloud对象的单元格数组% configs -传感器配置的单元阵列% time -当前模拟时间激光雷达模拟以pointCloud对象的形式返回输出。的位置点云的%属性用于提取的x,y, z位置%返回，并将它们打包为带有跟踪器所需信息的结构。sensorData = struct(“SensorIndex”{},.．.“时间”{},.．.“测量”{},.．.“MeasurementParameters”, {});为i = 1:数字(ptCloud)%这个传感器的点云thisPtCloud = ptCloud{i};允许数据和配置之间的映射，而不强制%有序输入和静态传感器要求配置输入。sensorData(我)。SensorIndex = config {i}.SensorIndex;%当前时间sensorData(我)。时间=时间;提取测量为3 × n定义点的位置sensorData(我)。测量=重塑(thisPtCloud.Location，[]，3)';数据在传感器坐标系中报告，因此测量%参数与传感器变换参数相同。sensorData(我)。测量参数=配置{i}.SensorTransformParameters;结束结束函数配置= helperGetLidarConfig(激光雷达，ego)获取跟踪器激光雷达传感器的配置％% config -世界帧中激光雷达传感器的配置% lidar - lidarPointCloudGeneration对象自我驱动。场景。场景中的演员定义从传感器到自我的转换senToEgo =结构(“帧”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 speed 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);结束函数is可行= helperkinematic可行性(frenetTrajectories, speedLimit, aMax)检查轨迹的运动可行性％frenetTrajectories - Frenet坐标中的轨迹数组% speedLimit -速度限制(m/s)% aMax -最大加速度(m/s^2)isviable = false(数值(frenetTrajectories)，1);为i = 1:数值(frenetTrajectories)%弹道速度速度= frenettrajectory (i).Trajectory(:，2);%轨迹加速度acc = frenettrajectory (i).Trajectory(:，3);速度有效吗?isSpeedValid = ~any(speed < -0.1 | speed > speedLimit + 1);加速度是否有效?isAccelerationValid = ~any(abs(acc) > aMax);%如果速度和acc都有效，弹道可行isviable (i) = isSpeedValid & isAccelerationValid;结束结束函数成本= helperCalculateTrajectoryCosts(frenettrajectory, Pc, smax)计算每个轨迹的成本。％frenetTrajectories - Frenet坐标中的轨迹数组% Pc -由验证器计算的每个轨迹的碰撞概率n = numel(frenettrajectory);Jd = 0 (n,1);Js = 0 (n,1);S = 0 (n,1);为I = 1:n%的时间时间= frenettrajectory (i).Times;%的决议dT = time(2) - time(1);沿着小路颠簸dds = frenettrajectory (i).Trajectory(:，3);Js(i) = sum(梯度(dds，时间).^2)*dT;%垂直于路径% d2L/dt2 = d/dt(dL/ds*ds/dt)ds = frenettrajectory (i).Trajectory(:，2);ddL = frenettrajectory (i). trajectory (:，6).*(ds.^2) + frenettrajectory (i). trajectory (:，5).*dds;Jd(i) = sum(梯度(ddL，时间).^2)*dT;s(i) = frenettrajectory (i).Trajectory(end,2);结束成本= Js + Jd + 1000*Pc(:) + 100*(s - smax).^2;结束