计划
计划最优轨迹
描述
(计算一个可行的轨迹,traj,指数,成本,国旗]=计划(规划师,开始)traj,从一个候选轨迹生成的列表trajectoryOptimalFrenet对象,规划师。开始被指定为six-element向量(年代,ds / dt,d2s / dt2,l,dl / ds,d2l / ds2],在那里年代是第一点的弧长在参考路径,然后呢l是正常的距离最近的点吗年代的参考路径。
输出轨迹,traj也有关联成本和指数为TrajectoryList财产的计划。国旗是一个数字退出标志指示状态的解决方案。
改善计划的结果输出,修改参数规划师对象。
例子
在Frenet空间最优轨迹规划
这个例子展示了如何使用最优轨迹规划trajectoryOptimalFrenet对象。
创建和分配映射到状态验证器
创建一个状态验证器对象的碰撞检查。
stateValidator = validatorOccupancyMap;
创建一个障碍网格地图。
网格= 0 (50100);网格(24:26 48:53)= 1;
创建一个binaryOccupancyMap网格地图。
地图= binaryOccupancyMap(网格);
分配和国家边界映射到状态验证器。
stateValidator。地图=地图;:stateValidator.StateSpace.StateBounds (1:2) = [map.XWorldLimits;map.YWorldLimits];
计划和设想的轨迹
创建一个参考路径规划。
refPath =[0, 25岁,100年,25);
初始化规划对象的引用路径,验证器。
规划师= trajectoryOptimalFrenet (refPath stateValidator);
分配纵向终端状态、横向偏差和最大加速度值。
planner.TerminalStates。纵向= 100;planner.TerminalStates。横向= 10:5:10;planner.FeasibilityParameters。MaxAcceleration = 10;
指定偏移补偿值接近左边侧终端状态优先左车道上的变化。
计划。DeviationOffset = 5;
轨迹规划
最初的笛卡尔的车辆状态。
initCartState =(0 25π/ 9 0 0 0);
笛卡儿的车辆状态转换为Frenet状态。
initFrenetState = cart2frenet(规划师,initCartState);
计划一个轨迹从最初Frenet状态。
计划(计划、initFrenetState);
轨迹可视化
可视化的地图和轨迹。
显示(map)在显示(规划师,“轨迹”,“所有”)
分区纵向终端状态轨迹生成
这个例子展示了如何分区的纵向终端状态最优轨迹规划使用trajectoryOptimalFrenet对象。
创建和分配映射到状态验证器
创建一个状态验证器对象的碰撞检查。
stateValidator = validatorOccupancyMap;
创建一个障碍网格地图。
网格= 0 (50100);网格(25:27 28:33)= 1;网格(16:18,37:42)= 1;网格(29:31 72:77)= 1;
创建一个binaryOccupancyMap网格地图。
地图= binaryOccupancyMap(网格);
分配和国家边界映射到状态验证器。
stateValidator。地图=地图;:stateValidator.StateSpace.StateBounds (1:2) = [map.XWorldLimits;map.YWorldLimits];
计划和设想的轨迹
创建一个参考路径规划。
refPath =[0, 25岁,30岁,30岁,75年,20,100,25);
初始化规划对象的引用路径,验证器。
规划师= trajectoryOptimalFrenet (refPath stateValidator);
分配纵向终端状态、横向偏差和最大加速度值。
planner.TerminalStates。纵向= 100;planner.TerminalStates。横向= 5:5:5;planner.FeasibilityParameters。MaxAcceleration = 10;
指定分区的数量的纵向终端状态。
计划。NumSegments = 3;
轨迹规划
初始Frenet车辆的状态。
initFrenetState = 0 (1,6);
计划一个轨迹从最初Frenet状态。
计划(计划、initFrenetState);
轨迹可视化
可视化的地图和轨迹。
显示(map)在显示(规划师,“轨迹”,“所有”)举行在
生成车道边界
计算参考路径Frenet状态的结束。
refPathEnd = cart2frenet(规划师,[planner.Waypoints(最终,:)0 0 0 0]);
计算车道偏移两边的侧终端州一半车道宽度值。
laneOffsets =独特([planner.TerminalStates.Lateral planner.TerminalStates.Lateral + 2.5 - 2.5]);
计算车道在笛卡尔的位置状态。
numLaneOffsets =元素个数(laneOffsets);xRefPathEnd =装天花板(refPathEnd (1));laneXY = 0 ((numLaneOffsets * xRefPathEnd) + numLaneOffsets, 2);xIndex = 0;为laneID = 1: numLaneOffsets为x = 1: xRefPathEnd laneCart = frenet2cart(规划师,[x 0 0 laneOffsets (laneID) 0 0]);xIndex = xIndex + 1;laneXY (xIndex:) = laneCart (1:2);结束xIndex = xIndex + 1;laneXY (xIndex:) =南(1、2);结束
情节车道边界。
情节(laneXY (: 1) laneXY (:, 2),“线宽”,0.5,“颜色”(0.5 0.5 0.5),“DisplayName的”,“车道边界”,“线型”,“——”)
