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

这个示例使用:

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

介绍

动态重新规划自主车辆通常是用当地的运动计划。当地的运动计划负责生成最优轨迹根据全球计划和周围环境的信息。周围环境的信息可以被描述主要在两个方面:

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

存在动态障碍物环境中,一个当地的运动计划需要短期的预测信息环境评估计划的有效性的轨迹。环境的选择表示通常是由上游感知算法。节省内存规划算法,基于对象的表示提供了一个环境的描述。它还允许一个更简单的方法来定义inter-object关系行为的预测。另一方面,基于网格的方法允许一个object-model-free表示,这有助于高效collision-checking在复杂场景中与大量的对象。基于网格的表示也不敏感等缺陷的对象提取错误,错过了目标。混合这两种方法也可以通过提取对象的假设基于网格表示。

在本例中,您代表了周围环境作为一个动态占用网格地图。例如使用离散的对象集,请参考高速公路使用Frenet参考路径轨迹规划(导航工具箱)的例子。动态占用网格地图是一个基于网格的估计当地环境的自我。除了占用的概率估计,动态占用网格还估计每个单元的运动属性,如速度、转弯速度和加速度。此外,估计动态网格可以预测在未来短期评估当地环境的入住率在不久的将来。在本例中,您通过熔点获得环境的基于网格的估计云从六个激光雷达安装在自我。

设置场景和基于网格的追踪

本例中使用的场景代表了一个城市的十字路口场景,包含各种各样的对象,包括行人、自行车、汽车和卡车。自我车辆配备了六个同质激光雷达传感器,每一个90度的视野,提供360度覆盖在自我。更多细节的方案和传感器模型,参考基于网格跟踪在城市环境中使用多个激光雷达的例子。场景和传感器的定义是包裹在helper函数helperGridBasedPlanningScenario。

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

现在,定义一个基于网格的跟踪器使用trackerGridRFS系统对象™。跟踪输出对象级和网格级环境的估计。网级估计描述了当地环境的入住率和状态,可以获得第四的输出跟踪。更多细节关于如何建立一个基于网格的追踪,请参考基于网格跟踪在城市环境中使用多个激光雷达的例子。

%设置为每个激光雷达传感器配置sensorConfigs =细胞(元素个数(激光雷达),1);%填写传感器配置为i = 1:元素个数(sensorConfigs) sensorConfigs{我}= helperGetLidarConfig(激光雷达{我},egoVehicle);结束%建立跟踪器追踪= 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(导航工具箱)对象通过提供路径点笛卡尔坐标系的驾驶场景。这个示例中使用的参考路径定义了一个路径,在十字路口右转。

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

snapnow;

当地的运动规划算法在本例中包含三个主要步骤:

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

下面的章节将讨论每一个步骤的地方规划算法和辅助函数用于执行每一步。

尝尝当地的轨迹

每一步的模拟样本轨迹规划算法生成一个列表的自我车辆可以选择。当地轨迹采样通过连接的当前状态自我车辆所需的终端状态。使用trajectoryGeneratorFrenet(导航工具箱)当前对象连接和终端状态生成本地轨迹。定义对象通过提供参考路径和所需的分辨率的轨迹。对象连接初始和最终州Frenet坐标使用5次多项式。

连接器= trajectoryGeneratorFrenet (refPath,“TimeResolution”,0.1);

抽样终端战略州Frenet坐标往往取决于自我的道路网络和期望的行为的车辆在不同阶段全球路径。为更详细的例子使用不同的自我行为,如巡航、车辆,是指“通过交通规划适应路线”的部分高速公路使用Frenet参考路径轨迹规划(导航工具箱)的例子。在本例中,您样品终端状态使用两种不同的策略,根据车辆的位置参考路径,显示为蓝色和绿色区域如下图所示。

%可视化路径可视化区域抽样策略pathPoints = closestPoint (refPath refPath.Waypoints (: 1:2));道路= pathPoints(:,结束);道路交叉口= (2);intersectionBuffer = 20;pathGreen =[插入(refPath linspace (0 intersectionS-intersectionBuffer 20));…南(1,6);…插入(refPath linspace(交叉口、道路(结束),100)));pathBlue =插入(refPath linspace (intersectionS-intersectionBuffer、道路(2),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对象自动计算纵向距离minimum-jerk轨迹。这个策略产生一组轨迹,使自我车辆加速到最大速度限制( ${\dot{年代}}_{马克斯}$ )利率或减速完全停止以不同的速率。此外,横向偏移量的抽样选择( $d_{des}$ )允许自我车辆变换车道在这些动作。

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

当自我车辆在蓝色区域的轨迹,以下策略用于当地样本轨迹:

$x_{自我} (Δ T) = ({年代}_{停止} 0 0 0 0 0]$ ;

在哪里 $Δ T$ 在轨迹选择最小化混蛋。这种策略使车辆停止时所需的距离( ${年代}_{停止}$ )在右车道minimum-jerk轨迹。轨迹采样算法是包裹在helper函数,helperGenerateTrajectory,附加的例子。

找到可行的和无碰撞轨迹

在前一节中描述的采样过程可以产生轨迹运动学上地不可行和超过阈值的运动加速度和曲率等属性。因此,你自我的最大加速度和速度限制车辆使用helper函数helperKinematicFeasibility检查每个轨迹的可行性,针对这些运动学约束。

%定义运动学约束accMax = 15;

进一步,你建立了一个collision-validator如果自我评估车辆可以机动动可行轨迹不与任何其他障碍环境中发生碰撞。定义验证器,使用助手类HelperDynamicMapValidator。这个类使用predictMapToTime的函数trackerGridRFS对象来获得短期的预测入住率周围的环境。从估计的不确定性增加随着时间的推移,配置验证器的最大时间2秒。

环境的预测入住率转化为一个充气的每一步costmap占自我车辆的大小。路径规划使用0.1秒的步伐的预测时间范围2秒。为了减少计算复杂度,入住率的周边环境被认为是有效的步骤5次,或0.5秒。因此,只有4两秒钟规划周期的预测是必需的。除了二元决策对碰撞或没有碰撞,碰撞概率的验证器还提供了一个衡量自我的工具。这个概率可以被纳入成本函数最优标准占系统中的不确定性,并做出更好的决策不增加计划的时间范围。

vehDims = vehicleDimensions (egoVehicle.Length egoVehicle.Width);collisionValidator = HelperDynamicMapValidator (“MaxTimeHorizon”2,…%最大地平线进行验证“TimeResolution”connector.TimeResolution,…%的时间轨迹样本之间的步骤“追踪”跟踪器,…%为预测提供了跟踪器“ValidPredictionSpan”5,…%预测有效期为5个步骤“VehicleDimensions”,vehDims);%提供自我的维度

选择最优性准则

后验证可行轨迹对障碍或占领地区的环境中,选择一个最优性准则为每个有效轨迹通过定义一个成本函数的轨迹。不同的成本函数将从自我产生不同的行为。在本例中,您定义每个轨迹的成本

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

地点:

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

$J_{d}$ 是混蛋的横向参考路径

$P_{c}$ 通过验证器碰撞概率是多少

成本计算为每个使用helper函数定义轨迹helperCalculateTrajectoryCosts。从列表中有效的轨迹,以最低的成本获取轨迹被认为是最优轨迹。

运行场景,估计动态地图,和当地的轨迹规划

运行场景,产生点云的激光雷达传感器,和估计动态占用网格地图。使用动态地图估计和预测计划当地自我车辆的轨迹。

%接近原始图和初始化一个新的显示关闭(图);显示= helperGridBasedPlanningDisplay;%初始自我状态currentEgoState = (-110.6 - -1.5 0 0 15 0);helperMoveEgoVehicleToState (egoVehicle currentEgoState);%初始化pointCloud从每个传感器输出ptClouds =细胞(元素个数(激光雷达),1);sensorConfigs =细胞(元素个数(激光雷达),1);%模拟循环而推进(场景)%当前仿真时间时间= scenario.SimulationTime;%的对象构成对自我tgtPoses = targetPoses (egoVehicle);%从每个传感器模拟点云为i = 1:元素个数(激光雷达)[ptClouds {}, isValidTime] =步骤(激光雷达{我}、tgtPoses、时间);sensorConfigs{我}= helperGetLidarConfig(激光雷达{},egoVehicle);结束%包点云传感器追踪所需的数据格式sensorData = packAsSensorData (ptClouds、sensorConfigs、时间);%的追踪[痕迹,~,~,地图]=追踪(sensorData、sensorConfigs、时间);%更新验证器使用当前估计的未来预测步骤(collisionValidator, currentEgoState、地图、时间);%样本使用当前的自我状态和一些运动轨迹%的参数[frenetTrajectories, globalTrajectories] = helperGenerateTrajectory(连接器,refPath、currentEgoState speedLimit,巷宽,十字路口,intersectionBuffer);%计算运动轨迹生成的可行性isKinematicsFeasible = helperKinematicFeasibility (frenetTrajectories speedLimit accMax);%计算碰撞可行轨迹的有效性feasibleGlobalTrajectories = globalTrajectories (isKinematicsFeasible);feasibleFrenetTrajectories = frenetTrajectories (isKinematicsFeasible);[isCollisionFree, collisionProb] = isTrajectoryValid (collisionValidator feasibleGlobalTrajectories);%计算成本和最终的最优轨迹nonCollidingGlobalTrajectories = feasibleGlobalTrajectories (isCollisionFree);nonCollidingFrenetTrajectories = feasibleFrenetTrajectories (isCollisionFree);nonCollodingCollisionProb = collisionProb (isCollisionFree);成本= helperCalculateTrajectoryCosts (nonCollidingFrenetTrajectories nonCollodingCollisionProb speedLimit);%找到最优轨迹[~,idx] = min(成本);optimalTrajectory = nonCollidingGlobalTrajectories (idx);%组装的策划轨迹= helperAssembleTrajectoryForPlotting (globalTrajectories,…isKinematicsFeasible、isCollisionFree idx);%更新显示显示器(场景、egoVehicle激光雷达,ptClouds,追踪,追踪,轨迹,collisionValidator);%将自我与最优轨迹如果~ isempty (optimalTrajectory) currentEgoState = optimalTrajectory.Trajectory (2:);helperMoveEgoVehicleToState (egoVehicle currentEgoState);其他的%所有轨迹要么违反运动可行性%约束或导致碰撞。更多的行为%轨迹采样可能需要。错误(无法计算最优轨迹的);结束结束

结果

从本地路径规划算法和分析结果的预测如何映射协助计划。这个动画显示的结果在整个场景规划算法。注意自我车辆成功达到目的地,在不同的动态对象上时必要的。自我车也停在十字路口由于区域变化添加到抽样策略。

接下来,分析了地方规划算法在第一车道改变。本节中的快照捕获在仿真时间= 4.3秒。

在这个快照,自我车辆刚刚开始执行一个车道改变机动到右车道。

showSnaps(显示,3,1);

接下来的快照显示了估计的动态网格同时一步。网格单元的颜色表示物体的运动方向网格单元的占领。注意,表示汽车的细胞自我面前的车辆的颜色为红色,表示的细胞占据一个动态对象。此外,汽车正朝着积极的X方向的场景,所以基于色轮,相应的网格细胞的颜色是红色的。

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

基于先前的形象,自我的计划轨道车辆通过占领地区的空间,代表一个碰撞如果您执行传统的静态占用验证。动态占用地图和验证器,然而,占电网的动态特性验证的状态轨迹预测入住率在每个时间步。下一个快照显示了预测costmap在不同预测步骤( $Δ T$ ),以及计划的自我在轨道车辆的位置。预测costmap膨胀占自我车辆的大小。因此,如果一个点对象代表自我的起源可以放置在车辆占用地图没有任何碰撞,可以解释,自我车辆没有撞上任何障碍。costmap上的黄色区域表示保证与障碍物碰撞的地区。黄色区域外的碰撞概率衰减指数到通货膨胀。蓝色区域显示的地区根据当前预测零碰撞的概率。

注意,黄色区域表示前面的汽车上的自我车辆前进costmap地图是预测未来。这反映出的预测入住率考虑周边环境中的对象的速度。也注意到细胞分为静态物体保持相对静态网格在预测。最后,注意自我的计划位置车辆来源没有撞上任何成本占领地区的地图。这表明自我车辆能够成功机动轨迹。

f = showSnaps(显示1 1);如果~ isempty (f) ax = findall (f,“类型”,“轴”);为i = 1:元素个数(ax) ax (i)。XLim = 40 [0];ax (i)。YLim = 20 [-20];结束结束

总结

在本例中,您学习了如何使用动态映射网格跟踪预测,trackerGridRFS,以及如何整合动态地图进行局部路径规划算法为自我生成轨迹车辆在动态复杂的环境。您还了解了如何使用占用的动态特性更有效地计划轨迹的环境。

万博1manbetx支持功能

函数sensorData = packAsSensorData (ptCloud、配置、时间)%包传感器数据追踪所需的格式%% ptCloud——细胞pointCloud对象的数组%配置——单元阵列传感器的配置%的时间-电流仿真时间%激光雷达仿真作为pointCloud对象返回的输出。的位置%属性用于提取点云的x, y, z的位置%的回报和包结构所需的信息跟踪。sensorData =结构(“SensorIndex”{},…“时间”{},…“测量”{},…“MeasurementParameters”,{});为i = 1:元素个数(ptCloud)%这个传感器的点云thisPtCloud = ptCloud {};%没有迫使一个允许之间的映射数据和配置%命令输入和要求配置静态传感器。sensorData(我)。SensorIndex =款{我}.SensorIndex;%当前时间sensorData(我)。时间=时间;% Exctract 3-by-N定义点的位置测量sensorData(我)。测量=重塑(thisPtCloud.Location [], 3) ';%数据报告在传感器坐标系,因此测量%参数相同传感器变换参数。sensorData(我)。MeasurementParameters =款{我}.SensorTransformParameters;结束结束函数自我配置= helperGetLidarConfig(激光雷达)%得到激光雷达传感器跟踪器的配置%%配置——世界上激光雷达传感器的配置框架%激光雷达——lidarPointCloudGeneration对象%的自我——driving.scenario。演员的场景%定义转换从传感器到自我senToEgo =结构(“帧”fusionCoordinateFrameType (1)…“OriginPosition”,(lidar.SensorLocation (:); lidar.Height],…“定位”rotmat(四元数([激光雷达。偏航激光雷达。Pitch lidar.Roll],“eulerd”,“ZYX股票”,“帧”),“帧”),…“IsParentToChild”,真正的);%定义转换从自我到跟踪坐标egoToScenario =结构(“帧”fusionCoordinateFrameType (1)…“OriginPosition”ego.Position (:)…“定位”rotmat(四元数([自我。偏航自我。Pitch ego.Roll],“eulerd”,“ZYX股票”,“帧”),“帧”),…“IsParentToChild”,真正的);%使用trackingSensorConfiguration组装。配置= trackingSensorConfiguration (…“SensorIndex”lidar.SensorIndex,…“IsValidTime”,真的,…“SensorLimits”(lidar.AzimuthLimits; 0 lidar.MaxRange),…“SensorTransformParameters”(senToEgo; egoToScenario),…“DetectionProbability”,0.95);结束函数helperMoveEgoVehicleToState (egoVehicle currentEgoState)%自我移动车辆在场景状态计算的计划%% egoVehicle——driving.scenario。演员的场景% currentEgoState - [x yθkappa速度acc)%设置二维位置egoVehicle.Position (1:2) = currentEgoState (1:2);%设置二维速度(s * cos(偏航)s * sin(偏航))egoVehicle.Velocity (1:2) = (cos (currentEgoState(3)罪(currentEgoState (3))) * currentEgoState (5);%设置偏航度egoVehicle。偏航= currentEgoState(3) * 180 /π;%设置角速度在Z(偏航率)v / regoVehicle.AngularVelocity (3) = currentEgoState (4) * currentEgoState (5);结束函数isFeasible = helperKinematicFeasibility (frenetTrajectories speedLimit aMax)%检查运动轨迹的可行性%% frenetTrajectories——Frenet轨迹的坐标数组% speedLimit——速度(米/秒)% aMax最大加速度(m / s ^ 2)isFeasible = false(元素个数(frenetTrajectories), 1);为i = 1:元素个数(frenetTrajectories)%的速度轨迹速度= frenetTrajectories (i) .Trajectory (:, 2);%的加速度轨迹acc = frenetTrajectories (i) .Trajectory (:, 3);%的速度有效吗?isSpeedValid = ~任何(速度< -0.1 |速度> speedLimit + 1);%加速有效吗?isAccelerationValid = ~ (abs (acc) > aMax);%可行轨迹如果速度和acc有效isFeasible (i) = isSpeedValid & isAccelerationValid;结束结束函数成本= helperCalculateTrajectoryCosts (frenetTrajectories、Pc、smax)%计算成本为每一个轨迹。%% frenetTrajectories——Frenet轨迹的坐标数组%的Pc -为每个轨迹计算验证器碰撞的概率n =元素个数(frenetTrajectories);Jd = 0 (n, 1);Js = 0 (n, 1);s = 0 (n, 1);为i = 1: n%的时间时间= frenetTrajectories(我)同学们;%的决议dT =时间(2)- (1);%混蛋沿着路径dds = frenetTrajectories (i) .Trajectory (:, 3);Js (i) =总和(梯度(dds、时间)。^ 2)* dT;%混蛋垂直于道路%网站/ 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 + Pc (:) + 1000 * 100 * (s - smax)。^ 2;结束