使用动态占用网格地图在城市环境中的运动规划

此示例使用：

打开直播脚本

此示例演示如何使用Frenet参考路径在城市驾驶场景中执行动态重新规划。在本例中，您将使用本地环境的动态占用栅格地图估计来查找最佳的本地轨迹。

介绍

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

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

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

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

设置方案和基于网格的跟踪器

本示例中使用的场景表示城市十字路口场景，并包含各种对象，包括行人、自行车手、汽车和卡车。ego车辆配备了6个同质激光雷达传感器，每个传感器都有90度的视野，为ego车辆提供360度的覆盖。更多场景和传感器型号请参见城市环境中基于网格的多激光雷达跟踪的例子。方案和传感器的定义在辅助功能中包装HelpergridBasedPlanningscenario.．

%用于可重复性结果rng（2020年）；％创建场景，自我车辆和模拟激光雷达传感器[情景，Egovehicle，Lidars] = HelpergridBasedPlanningscenario;

现在，定义一个基于网格的跟踪器追踪器System Object™。跟踪器输出对象级和环境的网格级估计。网格级估计描述了本地环境的占用和状态，并且可以作为跟踪器的第四个输出获得。有关如何设置基于网格的跟踪器的更多详细信息，请参阅城市环境中基于网格的多激光雷达跟踪的例子。

为每个激光雷达设置传感器配置sensorConfigs =细胞(元素个数(激光雷达),1);%填写传感器配置对于i = 1：numel（sensorconfigs）sensorconfigs {i} = helpergetlidarconfig（lidars {i}，Egovehicle）;终止％设置跟踪器追踪= trackerGridRFS (“SensorConfigurations”，sensorconfigs，．..'hassensorconfigurationsinput',真的,．..'gridlength', 120,．..“网格宽度”, 120,．..'gridresolution'2，．..“GridOriginInLocal”，[ -  60-60]，．..“NumParticles”1 e5,．..'numbithparticles'，2e4，．..'velocitylimits'，[ -  15 15; -15 15]，．..'出生性', 0.025,．..“加工噪音”5 *眼(2),．..“脱笼”1 e - 3,．..'freespacediscountfactor'1飞行,．..“AssignmentThreshold”8．..“MinNumCellsPerCluster”，4，．..“ClusteringReshold”，4，．..“ConfirmationThreshold”,[3 4],．..'deletionthreshold'，[4 4]）;

设置运动计划器

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

使用该定义全局参考路径referencePathFrenet（导航工具箱）通过在驾驶场景的笛卡尔坐标框架中提供航点来实现对象。此示例中使用的参考路径定义了在交叉路口右转的路径。

路径点= [-110.6 -4.5 0;49 -4.5 0;55.5 - -17.7 -π/ 2;55.5 - -130.6 -π/ 2];％[x y theta]使用路径点创建一个参考路径Refpath =参考水平血统（航点）;%可视化引用路径图('单位'，“归一化”，'位置'，[0.1 0.1 0.8 0.8]);ax =轴(图);(ax,“开”）;情节(场景中,'父母'、ax);显示(refPath'父母'、ax);XLIM（AX，[ -  120 80]）;ylim（斧头，[ -  160 40]）;

snapnow;

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

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

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

样本当地轨迹

在仿真的每个步骤中，规划算法生成了自我车辆可以选择的样本轨迹列表。通过将EGO车辆的当前状态连接到期望的终端状态来采样局部轨迹。使用trajectoryGeneratorFrenet（导航工具箱）对象连接当前和终端状态以生成局部轨迹。通过提供参考路径和所需的轨迹时间分辨率来定义对象。该对象在弗涅特坐标系中使用五阶多项式连接初态和末态。

连接器= trajectoryGeneratorFrenet (refPath,'timeresolution', 0.1);

在近距离坐标中的终端状态的采样策略通常取决于道路网络以及在全球路径的不同阶段期间自我车辆的所需行为。有关使用不同的自我行为的更详细示例，例如Cruise-Control和Car-inft，请参阅“通过流量”部分的“规划自适应路线”部分基于Frenet参考路径的公路轨迹规划（导航工具箱）的例子。在本例中，根据参考路径上车辆的位置，使用两种不同的策略对终端状态进行抽样，如下图中的蓝色和绿色区域所示。

用于采样策略可视化的％可视化路径区域pathPoints = closepoint (refPath, refPath. waypoints (:，1:2));道路= pathPoints(:,结束);道路交叉口= (2);intersectionBuffer = 20;pathGreen =[插入(refPath linspace (0 intersectionS-intersectionBuffer 20));．..南（1,6）;．..插值（Refpath，Linspace（交叉路口，道路（终端），100））]];pathblue =插值（Refpath，Linspace（交叉点 - 交叉路口缓冲，道路（2，端），20））;(ax,“开”）;绘图（AX，Pathgreen（：，1），pathgreen（:,2），'颜色'，[0 1 0]，'行宽',5); 绘图（ax，路径蓝（：，1），路径蓝（：，2），'颜色'，[0 0 1]，'行宽'5);

snapnow;

当EGO车辆处于绿色区域时，以下策略用于采样局部轨迹。自我车辆之后的终端状态 $δ. 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}$ )在这些操纵过程中，允许ego车辆改变车道。

定义smax和wlane限速=15；laneWidth=2.975；

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

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

在哪里 $δ. T$ 是为了减小弹道时的冲击。这种策略使车辆能够在期望的距离( ${年代}_{停止}$ )在右车道以最小挺举轨迹行驶。轨迹采样算法包含在辅助函数中，helperGenerateTrajectory，随附此示例。

寻找可行和无碰撞轨迹

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

％定义运动约束accMax=15；

此外，您将碰撞验证器设置为评估EGO车辆是否可以在运动学上的轨迹上操纵，而不会与环境中的任何其他障碍碰撞。要定义验证器，请使用辅助类HelperdynamicMapValidator..这个类使用预测的函数追踪器目的是获得对周围环境占用率的短期预测。由于估计中的不确定性随着时间的增加而增加，因此配置验证器的最大时间范围为2秒。

在每个步骤中，环境的预测占用率被转换为膨胀的肋骨图，以考虑自我车辆的大小。路径规划师使用0.1秒的时间步，预测时间范围为2秒。为了降低计算复杂性，假设周围环境的占用率为5时间步长，或0.5秒。结果，在2秒的计划地平线中只需要4个预测。除了对碰撞或没有碰撞的二进制决定之外，验证器还提供了自我车辆的碰撞概率的衡量标准。这种概率可以被纳入成本函数中，以获得最佳标准，以解释系统中的不确定性，并在不增加规划师的时间范围内的情况下做出更好的决定。

vehdims =车辆levedimensions（Egovehicle.Length，Egovehicle.Width）;CollionValidator = HelperdynamicMapValidator（“MaxTimeHorizon”2，．..验证的最高％地平线'timeresolution'连接器。TimeResolution,．..%轨迹样本之间的时间步长'跟踪器'，跟踪器，．..％提供跟踪器以进行预测'validpredictionspan'，5，．..%预测有效期为5步“VehicleDimensions”, vehDims);提供自我的维度

选择最优标准

在对障碍物或被占领区域的可行轨迹进行验证后，通过定义轨迹的代价函数为每个有效轨迹选择最优准则。不同的成本函数期望产生不同的自我车辆行为。在本例中，您将每个轨迹的成本定义为

$C ＝ J_{年代} + J_{d} + 1000 P_{c} + 100. {（ {\dot{年代}}_{（ δ. T ）} - {\dot{年代}}_{限制} ）}^{2}$

哪里：

$J_{年代}$ 是在参考路径的纵向方向上的混蛋

$J_{d}$ 震动是否在参考路径的横向方向

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

每个轨迹的成本计算使用辅助功能定义helperCalculateTrajectoryCosts．在有效轨迹列表中，将成本最小的轨迹视为最优轨迹。

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

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

关闭原来的数字并初始化一个新的显示关闭（图）;display = helpergridbasedplanningdisplay;最初自我状态currentEgoState = [-110.6 -1.5 0 0 15 0];helperMoveEgoVehicleToState (egoVehicle currentEgoState);%初始化每个传感器的点云输出ptClouds =细胞(元素个数(激光雷达),1);sensorConfigs =细胞(元素个数(激光雷达),1);％模拟环路尽管预付款（情景）%当前模拟时间时间= scenario.SimulationTime;物体相对于自我载体的姿态tgtposs =针织品（例如，Egovehicle）;％模拟来自每个传感器的点云对于i = 1:numel(lidar) [ptClouds{i}， isValidTime] = step(lidar {i}， tgtpose,time);sensorConfigs{我}= helperGetLidarConfig(激光雷达{},egoVehicle);终止%将点云打包为跟踪器所需的传感器数据格式sensoldata = packassensordata（ptclouds，sensorconfigs，时间）;%打电话给追踪者[track， ~， ~， map] = tracker(sensorData,sensorConfigs,time);%使用当前估计更新验证器的未来预测step(collisionValidator, currenttegostate, map, time);%样本轨迹使用当前自我状态和一些运动学%的参数[FreneTtrajections，GlobalTrajections] =辅助生根（Connector，Refpath，Currentegostate，Speedlimit，LaneWidth，交叉点，交叉口）;％计算产生的轨迹的运动学可行性isKinematicsFeasible = helperKinematicFeasibility (frenetTrajectories speedLimit accMax);计算可行轨迹的碰撞有效性feasibleGlobalTrajectories = globalTrajectories (isKinematicsFeasible);feasibleFrenetTrajectories = frenetTrajectories (isKinematicsFeasible);[isCollisionFree, collisionProb] = isstrajectoryvalid (collisionValidator, feasibleGlobalTrajectories);计算成本和最终最优轨迹非共聚体=可行的全球共聚体（无共聚体）；非共聚体=可行的法国共聚体（无共聚体）；非共聚体冲突概率=冲突概率（无共聚体）；成本=帮助计算四项目成本（无共聚体，无共聚体冲突概率，速度限制）；％找到最佳轨迹[〜，IDX] = min（成本）;OptimalTrajectory = NONCOLLIVINGGLOBALTRAINGERS（IDX）;％组装用于绘图轨迹= helperAssembleTrajectoryForPlotting (globalTrajectories,．..isKinematicsFeasible、isCollisionFree idx);%更新显示显示（场景，Egovehicle，Lidars，Ptclouds，跟踪器，轨道，轨迹，CollisionValidator）;％将自我搬到最佳轨迹如果〜isempty（最佳轨迹）currentegostate = optimalTrajectory.Traptory（2，:);helperMoveEgoVehicleToState (egoVehicle currentEgoState);其他的所有的轨迹都违反了运动学的可行性%约束或导致碰撞。更多关于%可能需要轨迹采样。错误（“无法计算最佳轨迹”）;终止终止

结果

分析本地路径规划算法的结果以及地图的预测方式如何辅助计划者。此动画显示整个场景期间规划算法的结果。请注意，自助车辆在必要时成功地达到其所需目的地并在不同的动态物体周围操纵。由于添加到抽样政策的区域变更，自我车辆也在交叉路口停下来。

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

在此快照中，自助式车辆刚刚开始在右车道上执行车道变换机动。

showSnaps(显示,3,1);

下面的快照显示了同时执行动态网格的估计。网格单元的颜色表示占据网格单元的物体的运动方向。请注意，代表自我车辆前面的汽车的电池是有色红色，表示细胞被动态对象占用。而且，车辆正在沿着场景的正x方向移动，因此基于颜色轮，相应的网格单元的颜色是红色的。

f = shownnaps（显示器，2,1）;如果~isempty(f) ax = findall(f，'类型'，'轴'）;ax.xlim = [0 40];ax.ylim = [-20 20];s = findall（ax，'类型'，'冲浪'）;s.xdata = 36 + 1/3 *（s.xdata  - 均值（s.xdata（:)））;s.ydata = 16 + 1/3 *（s.ydata  - 平均值（s.ydata（:)））;终止

根据前一张图像，ego车辆的计划轨迹通过占用的空间区域，如果执行传统的静态占用验证，则表示碰撞。但是，动态占用地图和验证程序通过验证轨迹的状态来说明网格的动态性质每个时间步的预测占用率。下一个快照显示不同预测步的预测成本图( $δ. T$ ），以及自助式车辆在轨迹上的计划位置。预测的Costmap膨胀为占EGO车辆的尺寸。因此，如果可以在没有任何碰撞的占用地图上放置代表自我车辆的起源的点对象，则可以解释为自我车辆不与任何障碍物碰撞。Costmap上的黄色区域表示具有障碍物的保证碰撞的区域。碰撞概率呈指数逐渐衰减直到充气区域的结束。蓝色区域根据当前预测表示诸有碰撞概率的区域。

请注意，在EGO车辆前面代表汽车的黄色区域向前移动，因为将来预测地图。这反映了占用率的预测来考虑周围环境中的物体的速度。另外，请注意，在预测期间，在网格上归类为静态对象的单元格仍然相对静态。最后，请注意，EGO车辆原产地的计划位置与成本图中的任何被占用的区域不碰撞。这表明EGO车辆可以在这个轨迹上成功操纵。

f = showSnaps(display, 1,1); / /显示如果~isempty(f) ax = findall(f，'类型'，'轴'）;对于i = 1：磁轴（ax）ax（i）.xlim = [0 40];斧头（i）.ylim = [-20 20];终止终止

概括

在此示例中，您学习了如何使用来自基于网格的跟踪器的动态映射预测，追踪器以及如何将动态映射与本地路径规划算法集成，以在动态复杂环境中为自我车辆产生轨迹。您还了解了占用的动态性质如何用于在环境中更有效地规划轨迹。

金宝app支持功能

功能sensorData=软件包传感器数据（ptCloud、配置、时间）%将传感器数据按跟踪器要求的格式打包％% ptCloud - pointCloud对象的单元数组% configs -传感器配置单元阵列%时间-电流模拟时间LIDAR模拟％返回输出作为PointCloud对象。那个地点属性用于提取点云的x、y、z位置％返回并将它们作为结构包装，并通过跟踪器所需的信息。sensorData=struct(“SensorIndex”,{},．..“时间”, {},．..'测量', {},．..“测量参数”, {});对于i = 1:元素个数(ptCloud)%这个传感器的点云thisPtCloud = ptCloud {};％允许在没有强制上迫使数据和配置之间的映射%命令输入和静态传感器需要配置输入。sensorData（i）.SensorIndex=configs{i}.SensorIndex；%当前时间sensorData(我)。时间=时间;%提取测量作为一个3 × n定义点的位置sensorData（i）.测量=重塑（thisPtCloud.位置，[]，3'）；在传感器坐标帧中报告％数据并因此进行测量％参数与传感器变换参数相同。sensorData(我)。MeasurementParameters =款{我}.SensorTransformParameters;终止终止功能config = helperGetLidarConfig(lidar, ego)％获取跟踪器的LIDAR传感器的配置％％config  - 世界框架中激光雷萨传感器的配置%激光雷达-激光雷达云生成对象自我驱动。场景。场景中的演员％将传感器的变换定义为自我sentoego = struct（'框架'，FusionCoordinationFrameType（1），．..'originposition'，[激光雷达传感器位置（：）；激光雷达高度]，．..'方向'，rotmat（四元数（[lidar.yaw lidar.pitch lidar.roll]，“eulerd”，“ZYX”，“框架”),“框架”),．..'Isparenttochild',真正的);％将自我的变换定义为跟踪坐标egoToScenario =结构('框架'，FusionCoordinationFrameType（1），．..'originposition'，ego.Position（:)，．..'方向'rotmat(四元数([自我。偏航自我。Pitch ego.Roll],“eulerd”，“ZYX”，“框架”),“框架”),．..'Isparenttochild',真正的);%使用trackingSensorConfiguration进行组装。配置= trackingSensorConfiguration (．..“SensorIndex”，lidar.SensorIndex，．..'isvalidtime'， 真的，．..“SensorLimits”, (lidar.AzimuthLimits; 0激光雷达。MaxRange),．..'sensortransformparameters'，[senToEgo；egoToScenario]，．..'检测可力'，0.95）;终止功能HelperMoveGeoVehicleTostate（egoVehicle，currentEgoState）%将场景中的自我战车移动到计划者计算的状态％％EGOVEHICLE  - 驾驶。驾驶.Cenario.Actor在情景中百分比稳定 -  [x y theta kappa sceeg acc]％设置2-D位置egoVehicle.Position (1:2) = currentEgoState (1:2);设定二维速度(s*cos(偏航)s*sin(偏航))egoVehicle.Velocity(1:2) = [cos(currentEgoState(3)) sin(currentEgoState(3))]*currentEgoState(5);%设置偏航角度Egovehicle.yaw = Currentegostate（3）* 180 / pi;将角速度设为Z(偏航率)为v/regoVehicle.角速度（3）=当前EgoState（4）*当前EgoState（5）；终止功能isFeasible = helperKinematicFeasibility(frenetTrajectories, speedLimit, aMax)％检查轨迹的运动可行性％％frenettrajections  - 围裙坐标的轨迹阵列%速度限制-速度限制（m/s）％AMAX  - 最大加速度（M / S ^ 2）isFeasible = false(元素个数(frenetTrajectories), 1);对于i = 1:元素个数(frenetTrajectories)弹道的速度速度= frenetTrajectories (i) .Trajectory (:, 2);%轨迹加速度ACC = Frenettrajections（i）.traptory（：3）;％是速度有效吗？IsspeedValid =〜任何（速度<-0.1 |速度> SpeedLimit + 1）;%加速度有效吗？isAccelerationValid = ~任意(abs(acc) > aMax);%如果速度和acc有效，轨迹可行Is可行（i）=IsPeedValid&isAccelerationValid；终止终止功能成本= HelpercalculateTrajectorycosts（Frenettrajections，PC，Smax）计算每个轨迹的成本。％％frenettrajections  - 围裙坐标的轨迹阵列% Pc -验证器计算的每个轨迹的碰撞概率n =元素个数(frenetTrajectories);Jd = 0 (n, 1);Js = 0 (n, 1);s = 0 (n, 1);对于我= 1：n％ 时间时间= frenettrajections（i）。%的决议dT = time(2) - time(1);沿着路径的％jerkDDS = Frenettrajections（i）.traptory（:,3）;js（i）= sum（梯度（dds，时间）。^ 2）* dt;％jerk垂直于路径% 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) =总和(梯度(ddL,时间)。^ 2)* dT;(我)= frenetTrajectories(我).Trajectory (, 2);终止成本=Js+Jd+1000*Pc（：）+100*（s-smax）。^2；终止