最优轨迹生成城市驾驶
这个例子展示了如何执行动态重新规划在城市场景中使用trajectoryOptimalFrenet。
在本例中,您将:
探索与预定义的车辆一个城市场景。
使用
trajectoryOptimalFrenet为导航做地方规划城市场景。
内容
介绍
自动驾驶的城市场景需要规划两个层次,全球和地方。的全球计划发现开始点和目标点之间的最可行的路径。的当地的规划师并动态生成最优轨迹的基础上,重新规划全球计划和周围的信息。在这个例子中,一个自我车辆(绿框)遵循全球计划(蓝色的点线)。地方规划完成橙色(实线),而试图避免另一辆车(黑色矩形)。
当地规划者使用障碍信息来生成最优无碰撞轨迹。障碍信息等各种车载传感器的相机,雷达,激光雷达融合保持入住率地图更新。这占用地图以自我为中心,当地框架以自我为中心。地图是用于地方规划从传感器和检测到障碍物时放置在地图上。
探索地方规划的城市场景
这个示例场景有四个其他车辆(蓝色矩形),这是在预定义的路径在不同的速度移动。下图说明了这个场景和全球计划(固体红线)本例中使用。固体下面图中红点代表路径点之间的全球计划启动和目标位置。绿色矩形代表自我。
自我车辆使用trajectoryOptimalFrenet从位置导航一个定位D在三个部分三个不同的配置参数。
第一次(一个来B),车辆展示了自适应巡航控制系统(ACC)的行为。
第二个(B来C),车辆协商将遵循全球计划。
第三(C来D),车辆执行一个车道改变策略。
设置所需的数据和环境变量:
%从urbanScenarioData加载数据。垫文件,初始化所需的变量[otherVehicles, globalPlanPoints stateValidator] = exampleHelperUrbanSetup;
otherVehicles(1 * 4):结构数组包含字段:
位置,偏航,速度,SimulationTime,每辆车的场景。globalPlanPoints:[18 x 2]矩阵包含预先计算的全球计划组成的18路点,分别代表一个在场景中的位置。stateValidator:validatorOccupancyMap对象作为国家验证器基于给定二维控制地图。完全占领了自我中心的入住率地图更新基于障碍信息和道路边界。一个定制的验证器也可以使用基于状态应用程序。有关更多信息,请参见nav.StateValidator。
画出场景。
exampleHelperPlotUrbanScenario;
创建本地规划师
指定验证器和全球计划创建一个本地计划使用trajectoryOptimalFrenet。
localPlanner = trajectoryOptimalFrenet (globalPlanPoints stateValidator);
探索的性质localPlanner
的localPlanner有多种属性,可以通过调优来实现所需的行为。本节探讨了其中的一些属性和它们的默认值。
localPlanner.TerminalStates
纵向:[45 60 75 90]:定义了纵向采样距离米。这个值可以是一个标量或矢量。侧:(2 1 0 1 2):定义了横向偏差的米参考路径(全球计划在我们的例子中)。时间:7:时间在几秒钟内到达轨道的结束。速度:10:在米每秒的速度,实现末端的轨迹加速度:0:加速度的轨迹在m / s ^ 2。
localPlanner.FeasibilityParameters
MaxCurvature:0.1:最大可行的曲率值m ^ 1MaxAcceleration:2.5:最大可行的加速度在m / s ^ 2。
localPlanner.TimeResolution:0.1:轨迹离散化区间在几秒钟内
使用trajectoryOptimalFrenet为了证明自适应巡航控制系统(ACC)的行为
在本节中,分配所需的属性配置localPlanner为了证明自适应巡航控制系统(ACC)的行为。
为了演示ACC,自我车辆需要跟随一个车辆保持安全距离。这段的领头车是拿来使用的otherVehicles (1)。
%得到leadVehicle B段从一个位置到位置leadVehicle = otherVehicles (1);%定义ACC安全距离ACCSafeDistance = 35;%在米%调整规划对象的时间分辨率的自我%的旅行顺利timeResolution = 0.01;localPlanner。TimeResolution = TimeResolution;
建立自我车辆位置并定义其初始速度和方向(偏航)。
%设置位置A, B, C和DpositionA = (5.1, -1.8, 0);positionB = (60, -1.8, 0);positionC = (74.45, -30.0, 0);positionD = (74.45, -135, 0);goalPoint = (145.70, -151.8, 0);%设置自我车辆的初始状态egoInitPose = positionA;egoInitVelocity = (10, -0.3, 0);egoInitYaw = -0.165;egoInitPose currentEgoState = [egoInitPose(1)(2),函数(egoInitYaw),…0规范(egoInitVelocity) 0);vehicleLength = 4.7;%在米%重新计划数量区间仿真步骤% 50仿真步骤(默认)replanInterval = 50;
可视化仿真结果。
%初始化可视化exampleHelperInitializeVisualization;
ACC的行为是通过设置TerminalStates的localPlanner如下:
保持安全距离车辆,集localPlanner.TerminalStates.Longitudinal距离领导车辆,车辆长度;
保持相对速度的车辆,集localPlanner.TerminalStates.Speed导致车辆速度;
继续在全球导航计划,设置localPlanner.TerminalStates.Lateral为0;
下面的代码片段,localPlanner生成轨迹执行和可视化exampleHelperUpdateVisualization为每个模拟步骤。然而,在每个replanInterval仿真步骤进行重新规划。下面是重新规划期间的事件顺序:
更新占用地图使用车辆信息使用
exampleHelperUpdateOccupancyMap。更新
localPlanner.TerminalStates。轨迹生成使用
计划(localPlanner currentStateInFrenet)。
B %模拟到自我车辆到达的位置simStep = 1;%只检查X Y没有变化。而currentEgoState (1) < = positionB (1)%重新计划在每一个“replanInterval”模拟步骤如果快速眼动(simStep replanInterval) = = 0 | | simStep = = 1%计算重新规划时间previousReplanTime = simStep * timeResolution;%更新入住率与车辆信息地图exampleHelperUpdateOccupancyMap (otherVehicles simStep currentEgoState);导致车辆和leadVehicleVelocity %计算距离distanceToLeadVehicle = pdist2 (leadVehicle.Position (simStep 1:2),…currentEgoState (1:2));leadVehicleVelocity = leadVehicle.Velocity (simStep:);%设置localPlanner。TerminalStatesfor ACC behavior如果distanceToLeadVehicle < = ACCSafeDistance localPlanner.TerminalStates。纵向= distanceToLeadVehicle - vehicleLength;localPlanner.TerminalStates.Speed= norm(leadVehicleVelocity); localPlanner.TerminalStates.Lateral = 0; desiredTimeBound = localPlanner.TerminalStates.Longitudinal/…localPlanner.TerminalStates.Speed;localPlanner.TerminalStates。时间= desiredTimeBound;localPlanner.FeasibilityParameters。MaxCurvature = 0.5;结束%为当前设置的参数生成最优轨迹currentStateInFrenet = cart2frenet (localPlanner [currentEgoState (1:5) 0]);轨迹=计划(localPlanner currentStateInFrenet);%入住率地图可视化自我中心显示(egoMap“父”,hAxes1);hAxes1.Title。字符串=“以自我为中心的二进制入住率地图”;%想象自我车辆占用地图上egoCenter = currentEgoState (1:2);egoPolygon = exampleHelperTransformPointtoPolygon (rad2deg (currentEgoState (3)), egoCenter);持有(hAxes1“上”)填充(egoPolygon (1:), egoPolygon (2:)“g”,“父”hAxes1)%可视化轨迹参考路径和轨迹显示(localPlanner“轨迹”,“最佳”,“父”hAxes1)结束%执行和更新可视化[isGoalReached, currentEgoState] =…exampleHelperExecuteAndVisualize (currentEgoState simStep,…轨迹,previousReplanTime);如果(isGoalReached)打破;结束%更新仿真步骤为下一次迭代simStep = simStep + 1;暂停(0.01);结束
![图包含2轴uipanel类型的对象和其他对象。轴与标题以自我为中心的二进制对象1入住率地图,包含X [m], ylabel Y(米)包含5类型的对象形象,补丁,线。一个或多个行显示的值只使用这些对象标记代表锚点,参考路径,最优轨迹。坐标轴对象2的标题轨道执行场景包含16线类型的对象,补丁。](http://www.tatmou.com/es/help/examples/nav/win64/OptimalTrajectoryGenerationForUrbanDrivingExample_04.png)
![图包含2轴uipanel类型的对象和其他对象。轴与标题以自我为中心的二进制对象1入住率地图,包含X [m], ylabel Y(米)包含5类型的对象形象,补丁,线。一个或多个行显示的值只使用这些对象标记代表锚点,参考路径,最优轨迹。坐标轴对象2的标题轨道执行场景包含16线类型的对象,补丁。](http://www.tatmou.com/es/help/examples/nav/win64/OptimalTrajectoryGenerationForUrbanDrivingExample_05.png)
最后执行、自我车辆位置B。
接下来,配置trajectoryOptimalFrenet谈判将在第二段的位置B位置C。
使用trajectoryOptimalFrenet到Negotiate顺利转
当前设置的属性localPlanner是充分的谈判顺利。然而,在第二段,导致车辆从otherVehicles获取(2)。
%设置导致车辆对应车辆在第二段%从位置B位置CleadVehicle = otherVehicles (2);% C模拟到自我车辆到达位置为Y %只检查没有改变在X C而currentEgoState (2) > = positionC (2)%重新计划在每一个“replanInterval”模拟步骤如果快速眼动(simStep replanInterval) = = 0%计算重新规划时间previousReplanTime = simStep * timeResolution;%更新入住率与车辆信息地图exampleHelperUpdateOccupancyMap (otherVehicles simStep currentEgoState);导致车辆和leadVehicleVelocity %计算距离distanceToLeadVehicle = pdist2 (leadVehicle.Position (simStep 1:2),…currentEgoState (1:2));leadVehicleVelocity = leadVehicle.Velocity (simStep:);如果(distanceToLeadVehicle < = ACCSafeDistance) localPlanner.TerminalStates。纵向= distanceToLeadVehicle - vehicleLength;localPlanner.TerminalStates.Speed= norm(leadVehicleVelocity); localPlanner.TerminalStates.Lateral = 0; desiredTimeBound = localPlanner.TerminalStates.Longitudinal/…localPlanner.TerminalStates.Speed;localPlanner.TerminalStates。时间= desiredTimeBound;localPlanner.FeasibilityParameters。MaxCurvature = 0.5;localPlanner.FeasibilityParameters。MaxAcceleration = 5;结束%为当前设置的参数生成最优轨迹currentStateInFrenet = cart2frenet (localPlanner [currentEgoState (1:5) 0]);轨迹=计划(localPlanner currentStateInFrenet);%入住率地图可视化自我中心显示(egoMap“父”,hAxes1);hAxes1.Title。字符串=“以自我为中心的二进制入住率地图”;%想象自我车辆占用地图上egoCenter = currentEgoState (1:2);egoPolygon = exampleHelperTransformPointtoPolygon (rad2deg (currentEgoState (3)), egoCenter);持有(hAxes1“上”)填充(egoPolygon (1:), egoPolygon (2:)“g”,“父”hAxes1)%可视化轨迹参考路径和轨迹显示(localPlanner“轨迹”,“最佳”,“父”hAxes1)结束%执行和更新可视化[isGoalReached, currentEgoState] =…exampleHelperExecuteAndVisualize (currentEgoState simStep,…轨迹,previousReplanTime);如果(isGoalReached)打破;结束%更新仿真步骤为下一次迭代simStep = simStep + 1;暂停(0.01);结束
![图包含2轴uipanel类型的对象和其他对象。轴与标题以自我为中心的二进制对象1入住率地图,包含X [m], ylabel Y(米)包含5类型的对象形象,补丁,线。一个或多个行显示的值只使用这些对象标记代表锚点,参考路径,最优轨迹。坐标轴对象2的标题轨道执行场景包含16线类型的对象,补丁。](http://www.tatmou.com/es/help/examples/nav/win64/OptimalTrajectoryGenerationForUrbanDrivingExample_06.png)
最后执行、自我车辆位置C。
接下来,配置trajectoryOptimalFrenet执行一个车道改变策略从C D定位位置。
使用trajectoryOptimalFrenet执行车道改变策略
车道改变操作可以由适当配置的侧终端状态规划师。这可以通过设置侧终端状态车道宽度(3.6米在这个例子中)和假设参考路径是一致的中心当前自我巷。
% D模拟到自我车辆到达位置%设置车道宽度巷宽= 3.6;为Y %只检查没有改变在X D而currentEgoState (2) > = positionD (2)%重新计划在每一个“replanInterval”模拟步骤如果快速眼动(simStep replanInterval) = = 0%计算重新规划时间previousReplanTime = simStep * timeResolution;%更新入住率与车辆信息地图exampleHelperUpdateOccupancyMap (otherVehicles simStep currentEgoState);%谈判巷TerminalState设置改变localPlanner.TerminalStates.Longitudinal= 20:5:40; localPlanner.TerminalStates.Lateral = laneWidth; localPlanner.TerminalStates.Speed = 10; desiredTimeBound = localPlanner.TerminalStates.Longitudinal/…((currentEgoState (1、5) + localPlanner.TerminalStates.Speed) / 2);localPlanner.TerminalStates。时间= desiredTimeBound;localPlanner.FeasibilityParameters。MaxCurvature = 0.5;localPlanner.FeasibilityParameters。MaxAcceleration = 15;%为当前设置的参数生成最优轨迹currentStateInFrenet = cart2frenet (localPlanner [currentEgoState (1:5) 0]);轨迹=计划(localPlanner currentStateInFrenet);%入住率地图可视化自我中心显示(egoMap“父”,hAxes1);hAxes1.Title。字符串=“以自我为中心的二进制入住率地图”;%想象自我车辆占用地图上egoCenter = currentEgoState (1:2);egoPolygon = exampleHelperTransformPointtoPolygon (rad2deg (currentEgoState (3)), egoCenter);持有(hAxes1“上”)填充(egoPolygon (1:), egoPolygon (2:)“g”,“父”hAxes1)%可视化轨迹参考路径和轨迹显示(localPlanner“轨迹”,“最佳”,“父”hAxes1)结束%执行和更新可视化[isGoalReached, currentEgoState] =…exampleHelperExecuteAndVisualize (currentEgoState simStep,…轨迹,previousReplanTime);如果(isGoalReached)打破;结束%更新仿真步骤为下一次迭代simStep = simStep + 1;暂停(0.01);结束
![图包含2轴uipanel类型的对象和其他对象。轴与标题以自我为中心的二进制对象1入住率地图,包含X [m], ylabel Y(米)包含5类型的对象形象,补丁,线。一个或多个行显示的值只使用这些对象标记代表锚点,参考路径,最优轨迹。坐标轴对象2的标题轨道执行场景包含16线类型的对象,补丁。](http://www.tatmou.com/es/help/examples/nav/win64/OptimalTrajectoryGenerationForUrbanDrivingExample_07.png)
模拟自我执行车辆到达目标点
的localPlanner现在配置导航从D位置到目标位置。
%模拟到自我车辆到达目标位置%只检查X Y在目标没有改变。而currentEgoState (1) < = goalPoint (1)%重新计划在每一个“replanInterval”模拟步骤如果快速眼动(simStep replanInterval) = = 0%计算重新规划时间previousReplanTime = simStep * timeResolution;%更新入住率与车辆信息地图exampleHelperUpdateOccupancyMap (otherVehicles simStep currentEgoState);localPlanner.TerminalStates.Longitudinal= 20; localPlanner.TerminalStates.Lateral = [-1 0 1]; desiredTimeBound = localPlanner.TerminalStates.Longitudinal/…((currentEgoState (1、5) + localPlanner.TerminalStates.Speed) / 2);localPlanner.TerminalStates。时间= desiredTimeBound: 0.2: desiredTimeBound + 1;%为当前设置的参数生成最优轨迹currentStateInFrenet = cart2frenet (localPlanner [currentEgoState (1:5) 0]);轨迹=计划(localPlanner currentStateInFrenet);%入住率地图可视化自我中心显示(egoMap“父”,hAxes1);hAxes1.Title。字符串=“以自我为中心的二进制入住率地图”;%想象自我车辆占用地图上egoCenter = currentEgoState (1:2);egoPolygon = exampleHelperTransformPointtoPolygon (rad2deg (currentEgoState (3)), egoCenter);持有(hAxes1“上”)填充(egoPolygon (1:), egoPolygon (2:)“g”,“父”hAxes1)%可视化轨迹参考路径和轨迹显示(localPlanner“轨迹”,“最佳”,“父”hAxes1)结束%执行和更新可视化[isGoalReached, currentEgoState] =…exampleHelperExecuteAndVisualize (currentEgoState simStep,…轨迹,previousReplanTime);%的目标达到本节只会是真的。如果(isGoalReached)打破;结束%更新仿真步骤为下一次迭代simStep = simStep + 1;暂停(0.01);结束
![图包含2轴uipanel类型的对象和其他对象。轴与标题以自我为中心的二进制对象1入住率地图,包含X [m], ylabel Y(米)包含5类型的对象形象,补丁,线。一个或多个行显示的值只使用这些对象标记代表锚点,参考路径,最优轨迹。坐标轴对象2的标题轨道执行场景包含16线类型的对象,补丁。](http://www.tatmou.com/es/help/examples/nav/win64/OptimalTrajectoryGenerationForUrbanDrivingExample_08.png)
日志自我车辆位置egoPoses变量可以在基本工作空间。你可以回放车辆路径在DrivingScenario使用exampleHelperPlayBackInDS (egoPoses)。
%明显例子运行期间创建工作空间变量。%这排除了egoPoses允许用户播放DS的模拟exampleHelperUrbanCleanUp;
结论
这个例子解释了如何执行动态规划在城市场景中使用trajectoryOptimalFrenet。特别是,我们已经学习了如何使用trajectoryOptimalFrenet实现以下行为。
自适应巡航控制系统
谈判结果
车道改变策略。
