关于lattice planner
使用编程创建驾驶场景。
1.使用Driving scenario Designer 交互方式创建驾驶场景
2.导出matalb function
3.修正这个函数，创建原始场景的变体。
4.调用这个函数，生成drivingScenario object。
5.在simulink中仿真，导入这个objcet ，使用Scenario Reader 模块
在APP中创建场景。
drivingScenarioDesigner('LeftTurnScenarioNoSensors.mat')
点击导出MATLAB Function
生成了下图的函数
function [scenario, egoVehicle] = LeftTurnScenarioNoSensors()
% createDrivingScenario Returns the drivingScenario defined in the Designer
% Generated by MATLAB(R) 9.13 (R2022b) and Automated Driving Toolbox 3.6 (R2022b).
% Generated on: 21-Aug-2023 22:30:51
% Construct a drivingScenario object.构建结构体对象
scenario = drivingScenario;
% Add all road segments
roadCenters = [150 24.2 0;
134.3 24.7 0;
54 24.2 0];
laneSpecification = lanespec([2 2]);
road(scenario, roadCenters, 'Lanes', laneSpecification, 'Name', 'Road');
roadCenters = [105 75 0;
105.2 54.5 0;
105 -20 0];
laneSpecification = lanespec([2 2]);
road(scenario, roadCenters, 'Lanes', laneSpecification, 'Name', 'Road1');
% Add the ego vehicle
egoVehicle = vehicle(scenario, ...
'ClassID', 1, ...
'Position', [56 19 0], ...% 初始位置
'Mesh', driving.scenario.carMesh, ...% 车的形状
'Name', 'egoVehicle');
waypoints = [56 19 0;
135 19 0];
speed = 10;
smoothTrajectory(egoVehicle, waypoints, speed);
% Add the non-ego actors
otherVehicle = vehicle(scenario, ...
'ClassID', 1, ...
'Position', [103 73 0], ...
'Mesh', driving.scenario.carMesh, ...
'Name', 'otherVehicle');
waypoints = [103 73 0;
102.8 34.3 0;
114.3 22.4 0;
148.1 22.4 0];
speed = [11;10;10;11];
yaw = [-90;-90;0;0];
smoothTrajectory(otherVehicle, waypoints, speed, 'Yaw', yaw);
运行这个函数会返回道路场景、车以及车上传感器所采集的信息。
修改函数，创建场景变体。
上边代码相当于给你个代码壳子，你可以往里填充。一个常见的变体是测试主车以不同的速度行驶。在上边代码中，ego车的车速是10 m/s。为了生成不同的速度，你可以在上述代码中添加接口。
（1）添加输入变量。
（2）在平滑轨迹中，替换了speed.
(3)还可以考虑
修改路和车道的参数。修改轨迹或者车辆初始位置；修改车辆参数。
（4）调用你修改的这个函数
可以看到确实是20m/s
没修改之前的场景 （10m/s）VS 修改之后的场景（20m/s）
主车跟左拐车没有相遇。
注意，如果你的主车有传感器，你要使用这个命令去打开app
drivingScenarioDesigner(scenario,sensors).
注意更改classId ,car是1，truck是2。
在simulink中导入场景
使用Scenario Reader 模块。
关于Simulink中如何使用，
可以通过一个简单的例子来学会。
首先输入Scenario Reader,依次选择
最后加个小wifi
炫酷的鸟瞰图。这个场景过于简单，所以不炫酷。
回到lattice
首先运行的是这个函数。
首先看一下车道中心线
主车和五辆车
function [scenario, egoVehicle] = drivingScenarioTrafficExample()
% createDrivingScenario Returns the drivingScenario defined in the Designer
% Generated by MATLAB(R) 9.9 (R2020b) and Automated Driving Toolbox 3.2 (R2020b).
% Generated on: 28-Apr-2020 15:04:44
% Construct a drivingScenario object.
scenario = drivingScenario;% 创建一个drivingScenario对象
% Add all road segments
roadCenters = [0 50 0;
150 50 0;
300 75 0;
310 75 0;
400 0 0;
300 -50 0;
290 -50 0;
0 -50 0;
0 -50 0];
laneSpecification = lanespec(4);% 四个车道
road(scenario, roadCenters, 'Lanes', laneSpecification, 'Name', 'Road');
% Add the ego vehicle
egoVehicle = vehicle(scenario, ...
'ClassID', 1, ...
'Position', [10.7 51.4 0], ...
'Name', 'Car');
% Add the non-ego actors
car1 = vehicle(scenario, ...
'ClassID', 1, ...
'Position', [34.7 49.3 0], ...
'Name', 'Car1');
waypoints = [34.7 49.3 0;
60.1 48.2 0;
84.2 47.9 0;
119 49.3 0;
148.1 51.4 0;
189.6 58.7 0;
230.6 68 0;
272.6 74.7 0;
301.4 77.5 0;
316.7 76.8 0;
332.4 75.2 0;
348.9 72.2 0;
366.2 65.1 0;
379.6 55.6 0];
speed = [10;10;10;10;10;10;10;10;10;10;10;10;10;10];
trajectory(car1, waypoints, speed);
car2 = vehicle(scenario, ...
'ClassID', 1, ...
'Position', [17.6 46.7 0], ...
'Name', 'Car2');
waypoints = [17.6 46.7 0;
43.4 45.5 0;
71.3 43.8 0;
102.3 43.5 0;
123.5 45.5 0;
143.6 47.4 0;
162.4 50 0;
198.5 61 0;
241.1 70.1 0;
272.3 74.1 0;
292 76.6 0;
312.8 77.2 0;
350.3 75.2 0;
362.5 70.4 0;
375.9 63.3 0;
390.7 49.9 0;
401.3 33 0];
speed = [9;9;9;9;9;9;9;9;9;9;9;9;9;9;9;9;9];
trajectory(car2, waypoints, speed);
car3 = vehicle(scenario, ...
'ClassID', 1, ...
'Position', [62.6 51.9 0], ...
'Name', 'Car3');
waypoints = [62.6 51.9 0;
87.4 51.3 0;
117.7 52.2 0;
147.6 55 0;
174.9 59.7 0;
203.3 65.8 0;
265 69.7 0;
288.3 73.1 0;
314.5 73.1 0;
334.9 70.8 0;
360 59.9 0];
speed = [6;6;6;6;6;6;6;6;6;6;6];
trajectory(car3, waypoints, speed);
car4 = vehicle(scenario, ...
'ClassID', 1, ...
'Position', [101.7 41.1 0], ...
'Name', 'Car4');
waypoints = [101.7 41.1 0;
124.6 42 0;
148.5 43.9 0;
171.9 48.2 0;
197.1 52.8 0;
222.3 58.5 0;
252.4 64.4 0;
281.4 68.5 0;
307.7 69.5 0;
329.9 68.2 0;
352.7 62.8 0];
speed = [7;7;7;7;7;7;7;7;7;7;7];
trajectory(car4, waypoints, speed);
car5 = vehicle(scenario, ...
'ClassID', 1, ...
'Position', [251.3 75.6 0], ...
'Name', 'Car5');
waypoints = [251.3 75.6 0;
255.7 76.7 0];
speed = [0.01;0.01];
trajectory(car5, waypoints, speed);
车辆的参数
carLen = 4.7; % in meters
carWidth = 1.8; % in meters
rearAxleRatio = .25;
画出这个场景
构造参考路径
结构体，referencePathFrenet。这个结构题可以给出，沿着路径长度的状态。
找到匹配点，提供坐标系转换。
waypoints = [0 50; 150 50; 300 75; 310 75; 400 0; 300 -50; 290 -50; 0 -50]; % in meters
refPath = referencePathFrenet(waypoints);
这两个代码生成了这个结构体
这六列是什么（x,y,）
使用show函数show出来，这里就不求甚解了。
ax = show(refPath);
axis(ax,'equal'); xlabel('X'); ylabel('Y');
详细阅读 referencePathFrenet
给wayPoints (x,y)或者（x,y,theta）--》平滑修补成连续曲线。
查询路径上最接近全局状态的点；插值；坐标系转换‘
全局状态是[x,y,theta,kappa,speed,accel]
例子
waypoints = [0 0; 100 50; 400 -50; 800 0];
refPath = referencePathFrenet(waypoints);？？
dt=0.5;
sInitial = [0 0 0]; %分别代表？
ddS = [linspace(0,5,10) linspace(5,0,10)]';
dS = cumsum(ddS*dt);
S = cumsum(dS*dt);
latScale = 0.1;
Lpp = ddS*latScale;
Lp = cumsum(Lpp*dt);
L = cumsum(Lp*dt);
frenetTraj = [S dS ddS L Lp Lpp];
% 转化轨迹到全局坐标系下。
globalTraj = frenet2global(refPath, frenetTraj);
function globalState = frenet2global(obj, varargin)
narginchk(2,3);% 检查输入，并提供报错信息。最少两个输入，最多3个
validateattributes(varargin{1},{'numeric'},{'nonnan','nonempty','finite','size',[nan 6]},'frenet2global','frenetState');% 检查状态有效，是实数。
if nargin == 3
% 检查
% Validate lateral time derivatives and heading invert
validateattributes(varargin{2},{'numeric'},{'nonnan','finite','nonempty','size',[size(varargin{1},1) 3]},'frenet2global','latTimeDerivs');
validateattributes(varargin{2}(:,3),{'numeric'},{'binary'},'frenet2global','invertHeadingTF');
end
% 转化，这里是matlab面向对象的写法，我并不熟悉，暂时搁置，这里的FrenetReferencePath找不到
globalState = [email protected](varargin{:});
end
% 找到路径上离期望点最近的状态，期望点是：
xyDesired = waypoints(3,:) + [20 -50];
nearestPathPoint = closestPoint(refPath, xyDesired);% 找到路径上最近的点，应为waypoints(3,:)?
看一下closestPoint这个函数
function [pathPoints, inWindow] = closestPoint(obj, points, searchWindow)
%inwindow是一个N行一列的布尔值boolean值，N个点是否在路径的边界内。
%这个函数，需要一些时间阅读。
pathPoints返回路径上最近的点；InWindow是一个N行一列的向量，暗示是否在路径中。SEARCHWINDOW是一个1*2的向量，确定了路径的间隔。
例子：
% 创建一个自相交的参考路径。
refPath = referencePathFrenet([0 100 -pi/4; 50 50 -pi/4;...
75 50 pi/2; 50 50 -3*pi/4; 0 0 -3*pi/4]);
% 画出这个路径
figure;
show(refPath); axis equal; hold on;
title("Closest Points Around Intersection");
% 寻找相交点的弧长
sIntersection = refPath.SegmentParameters(2,end);
% Generate Frenet states lying just before and after the intersection
% 生成相交点之前和之后的frenet 状态，疑问，这里的ds,dds,...怎么弄出来的
f0 = [sIntersection-20 5 0 10 0 0]; % [S dS ddS L Lp Lpp]
f1 = [sIntersection+20 5 0 -5 0 0]; % [S dS ddS L Lp Lpp]
% Calculate time to travel with constant longitudinal velocity
% 计算时间
T = (f1(1)-f0(1))/f1(2);
% Create trajectory generator 创建轨迹生成器
generator = trajectoryGeneratorFrenet(refPath);
% Generate trajectory between points生成两个点之间的轨迹
[fTraj, gTraj] = connect(generator,f0,f1,T);
% 看下一段代码，关于connect的注释
pts = gTraj.Trajectory;% gTraj是什么，没有猜测出来/
% 写成handle的形式。
mergeFcn = @(v1,v2)reshape([v1 v2 nan(size(v1,1),size(v2,2))]',[],1);
plotFcn = @(L1,L2,linespec)plot(mergeFcn(L1(:,1),L2(:,1:min(1,size(L2,2)))), ...
mergeFcn(L1(:,2),L2(:,2:min(2,size(L2,2)))),linespec{:});
plotInterval = @(bounds,nPt,linespec)plotFcn(interpolate(...
refPath,linspace(bounds(1),bounds(2),nPt)'),[],linespec);
% Calculate closest point on path to each global state 计算最近的点
closestPts = closestPoint(refPath,pts);
% Plot closest points vectors
plotFcn(pts,closestPts,{'b'});
% Define a window in which to search for closest points 确定窗口搜寻最近点
buffWindow = [f0(1)-5 f1(1)+5];
plotInterval(buffWindow,100,{'Color',[.5 .5 .5],'LineWidth',5});
% Find closest points within window 寻找窗口内最近点，自己调用自己。
closestPtsInWindow = closestPoint(refPath,pts,buffWindow);
% 缩短窗口
% Find closest points using a window that is too small
smallWindow = [f0(1)+5 f1(1)-5];
[closestPtsSmall,inWindow] = closestPoint(refPath,pts, ...
smallWindow);
plotFcn(pts(inWindow,:),closestPtsSmall(inWindow,:),{'Color', [.5 1 .5]});
plotFcn(pts(~inWindow,:),closestPtsSmall(~inWindow,:),{'r'});
legend({'Waypoints','ReferencePath','Trajectory','ClosestPoints',...
'BuffWindow','ClosestInsideBuffWindow','SmallWindow',...
'ClosestInsideSmallWindow','ClosestOutsideSmallWindow'});
% 例子结束
% Validate inputs 验证输入，最少两个，最多三个。
narginchk(2,3);
xy = obj.validateXYPoints(points,'closestPoint','points');
if nargin == 2
% Find closest points 寻找最近点
pathPoints = [email protected](obj,xy);
clippedWindow = [0 obj.Length];
else
clippedWindow = obj.validateSearchWindow(searchWindow,'closestPoint','searchWindow');
pathPoints = obj.searchInWindow(xy,clippedWindow,false);
end
if nargout == 2
%%这个obj.verifyInWindow??
inWindow = obj.verifyInWindow(xy,pathPoints,clippedWindow);
end
endfunction [aa,bb,cc,dd]=connect(a,b,c,d,varagin)
% 调用格式 [fTraj, gTraj] = connect(generator,f0,f1,T)
%比较难懂，暂时忽略
narginchk(5,7)
error(abcdchk(a,b,c,d));
[aa,bb,cc,dd] = ssdata(connect(ss(a,b,c,d),q,varargin{:}));
end
计算出最近的轨迹点，path上，离插补出来的轨迹
缩小窗口
函数referencePathFrenet有许多疑问，共900行，暂时掠过
refPath = referencePathFrenet(waypoints);
如上图为生成的参考线。
trajectoryGeneratorFrenet 这个对象通过使用四次或者五次多项式曲线连接初始状态和最终状态来描述可能的运动状态。初始状态和最终状态都位于Frenet坐标系下。终端状态通常是自定义的行为。车道线信息，速度限制、主车和目标车的预测位置定义。
trajectoryGeneratorFrenet对象代码，下一段主要是例子展示
classdef trajectoryGeneratorFrenet < nav.algs.internal.InternalAccess
% 这个对象产生轨迹使用4阶或者5阶多项式。
Frenet状态描述了他们的位置、速度、加速度相对于精止的参考线，定义为[s,ds,dds,l,dl,ddl]。
ds相对于时间。dl相对于s。
默认，生成的轨迹是0.1s采样一次。
trajectoryGeneratorFrenet定义了三个方法：
connect,连接初始状态和终端状态
copy,复制对象
createParallelState，创建状态。
例子：
waypoints = [100 100; 150 200; 200 400; 0 0]；
refPath = referencePathFrenet(waypoints);
connector = trajectoryGeneratorFrenet(refPath,'TimeResolution',0.2)；
arcLen = 0;
pathStart = interpolate(refPath,arcLen);
%%refpath中只有四个状态，【x,y,theta,kappa】;添加两个状态【x,y,theta,kappa,v,a】
vInit = 1;
aInit = 0.1;
initGlobalState = [pathStart(1:4) vInit aInit];
% 把初始的全局状态转换为frenet,[s,ds,dds,l,dl,dd,ddl]
initFrenetState = global2frenet(refPath, initGlobalState);
arcLenOffsets = [10; 20; 30];
lateralDeviation = [-5; 0; 5];
% 构建终端状态，【x,y,theta,】
termFrenetStates = zeros(3,6)
% termFrenetStates(:,[1 4]) = initFrenetState([1 4]) + ...
% [arcLenOffsets lateralDeviation];
timeSpanSingle = 1;
% frenetTrajSet1 = connect(connector,initFrenetState, ...
% termFrenetStates,timeSpanSingle)
% 多种采样时间/
timeSpanMultiple = [1;2;3];
frenetTrajSet2 = connect(connector,initFrenetState, ...
% termFrenetStates,timeSpanMultiple);这一段是代码原理
% 默认属性，采样时间0.1
properties (Hidden,Constant)
%DefaultTimeResolution Default value for TimeResolution
DefaultTimeResolution = 0.1;
end
properties
%ReferencePath Reference path for generated trajectories
%
% A referencePathFrenet object used to map trajectories generated
% between Frenet states into the global coordinate system.
ReferencePath
%TimeResolution Discretization time interval
TimeResolution = trajectoryGeneratorFrenet.DefaultTimeResolution;
end
构建动态碰撞检查
dynamicCapsuleList是一个数据结构对象，代表了动态环境的状态。这个环境可以被使用，验证主车多条可能的轨迹。这个结构体包含：
（1）独特的整型id
(2)用于有效碰撞检查的胶囊几何图形的特性
(3) SE2的状态序列。
capList = dynamicCapsuleList;% 创建一个对象
[egoID, egoGeom] = egoGeometry(capList,egoID); 关于egoGeometry,看下一段注释
egoGeom.Geometry.Length = carLen; % in meters
egoGeom.Geometry.Radius = carWidth/2; % in meters
egoGeom.Geometry.FixedTransform(1,end) = -carLen*rearAxleRatio; % in meters
function [egoIDs, geomStruct, status] = egoGeometry(obj, varargin)
%egoGeometry Geometric properties for set of ego bodies
%
% [EGOIDS, GEOMSTRUCT] = egoGeometry(OBJ) returns a structure
% array, GEOMSTRUCT, containing geometry of all ego bodies in the
% list. The Nth element of GEOMSTRUCT contains the geometric
% properties of the ego body matching EGOIDS(N).
%
% [EGOIDS, GEOMSTRUCT] = egoGeometry(OBJ, EGOIDS) returns a
% structure array for ego bodies matching the EGOIDS vector. If an
% identifier is not found in the list, the corresponding entry of
% GEOMSTRUCT contains default geometry.
%
% [EGOIDS, GEOMSTRUCT, STATUS] = egoGeometry(OBJ, EGOIDS) returns
% an optional output, STATUS, an N-element vector with values of
% 1 (body found), 0 (body not found), -1 (duplicate input).
%
% See also dynamicCapsuleList.GEOMETRYSTRUCT, dynamicCapsuleList3D.GEOMETRYSTRUCT
narginchk(1,2);
% Validate inputs and retrieve indices of matching objects
[status, egoIDs, internalIndices] = obj.validateIntrospectionInputs('egoGeometry','egoIDs',obj.EgoIDs,varargin{:});
% Construct output variable
geomStruct = obj.getGeometry(egoIDs);
% Populate ID's and geometric properties for objects found in
% the list
for i = 1:numel(geomStruct)
if internalIndices(i) ~= 0
idx = internalIndices(i);
geomStruct(i).Geometry = obj.EgoObjectsInternal(idx).Geometry;
end
end
end
updateEgoGeometry(capList,egoID,egoGeom);actorID = (1:5)';
actorGeom = repelem(egoGeom,5,1);
updateObstacleGeometry(capList,actorID,actorGeom)Plannig Adaptive Routes Through Traffic
1.使用的是真实场景的真值来当作预测。
2.产生终端状态：定速巡航、跟车、换道
exampleHelperBasicCruiseControl
function [terminalStates, times] = exampleHelperBasicCruiseControl(refPath, laneWidth, egoState, targetVelocity, dt)
%exampleHelperBasicCruiseControl Generates terminal states for cruise control behavior.
%
% This function is for internal use only. It may be removed in the future
% [TERMINALSTATES, TIMES] = exampleHelperBasicCruiseControl(REFPATH, LANEWIDTH, EGOSTATE, TARGETVELOCITY, DT)
% Generates terminal states that attempt to obey the speed limit,
% TARGETVELOCITY, while following a lane center over a given spans of time, DT.
%
% REFPATH is a referencePathFrenet object used to convert the ego
% vehicle's state, EGOSTATE, from global coordinates, [x y theta kappa v a],
% to Frenet coordinates, [S dS ddS L dL ddL].
%
% Once in Frenet coordinates, exampleHelperPredictLane is used to predict
% the future lane that the vehicle would reside in based on the current
% Frenet state and zero terminal velocity/acceleration over the given
% span of time.
%
% The function returns TERMINALSTATES, an N-by-6 matrix where each row
% is a Frenet state defined as follows: [NaN TARGETVELOCITY 0 L_lane 0 0], where
% L_lane is the lateral deviation of the predicted lane-center. TIMES is
% also returned as an N-by-1 vector of doubles.
%
% Copyright 2020 The MathWorks, Inc.
% Convert ego state to Frenet coordinates
frenetState = global2frenet(refPath, egoState);
% Determine current and future lanes
futureLane = exampleHelperPredictLane(frenetState, laneWidth, dt);
% Convert future lanes to lateral offsets
lateralOffsets = (2-futureLane+.5)*laneWidth;
% Return terminal states
terminalStates = zeros(numel(dt),6);
terminalStates(:,1) = nan;
terminalStates(:,2) = targetVelocity;
terminalStates(:,4) = lateralOffsets;
times = dt(:);
end计算代价的函数exampleHelperEvaluateTSCost
function costs =
exampleHelperEvaluateTSCost
(terminalStates, times, laneWidth, speedLimit, speedWeight, latWeight, timeWeight)
%exampleHelperEvaluateTSCost Evaluate trajectory cost.
% COSTS = exampleHelperEvaluateTSCost(TERMINALSTATES, times, laneWidth, speedLimit, speedWeight, latWeight, timeWeight)
% Evaluates the cost of an N-by-6 matrix of Frenet states, TERMINALSTATES,
% and corresponding N-by-1 vector of timespans, TIMES.
%
% Cost is based on lateral deviation from a lane center, calculated using
% LANEWIDTH, deviation from the desired velocity, SPEEDLIMIT, and the span
% of time required by the trajectory, TIMES.
%
% Each of these metrics is scaled by the accompanying weights,
% LATWEIGHT, SPEEDWEIGHT, and TIMEWEIGHT, respectively.
%
% Copyright 2020 The MathWorks, Inc.
% Find lateral deviation from nearest lane center
laneCenters = (1.5:-1:-1.5)*laneWidth;
latDeviation = abs(laneCenters-terminalStates(:,4));
% [s,ds,dds,l,dl,ddl]
% Calculate lateral deviation cost
latCost = min(latDeviation,[],2)*latWeight;
% 这个函数min(latDeviation,[],2)
% Calculate trajectory time cost
timeCost = times*timeWeight;
% Calculate terminal speed vs desired speed cost
speedCost = abs(terminalStates(:,2)-speedLimit)*speedWeight;
% Return cumulative cost
costs = latCost+timeCost+speedCost;
endGenerate Trajectories and Check for Kinematic Feasibility 检查生成的轨迹是否符合约束。
function isValid =
exampleHelperEvaluateTrajectory(globalTrajectory, maxAcceleration, maxCurvature, minVelocity)
% ISVALID = exampleHelperEvaluateTrajectory(GLOBALTRAJECTORY, MAXACCELERATION, MAXCURVATURE, MINVELOCITY)
% Takes in a N-element struct array of trajectories, GLOBALTRAJECTORY, and
% checks whether each trajectory violates the MAXACCELERATION, MAXCURVATURE,
% or MINVELOCITY constraints. N维的结构体，globalTrajectory 轨迹。
%
% Each element of GLOBALTRAJECTORY contains the fields Trajectory, an
% N-by-[x y theta kappa v a] matrix, and Times, an N-by-1 vector of timesteps.
% 轨迹是0s [x,y,theta,v,a]
% 0.1 s [x,y,theta,v,a]
%
% Returns an N-by-1 vector of logicals, ISVALID, where an entry of false
% means that the corresponding trajectory violated one or more constraints,
% and true means that all constraints were met.
isValid = true(numel(globalTrajectory),1);
for i = 1:numel(globalTrajectory)
% Acceleration constraint
accViolated = any(abs(globalTrajectory(i).Trajectory(:,6)) > abs(maxAcceleration));
% Curvature constraint
curvViolated = any(abs(globalTrajectory(i).Trajectory(:,4)) > abs(maxCurvature));
% Velocity constraint
velViolated = any(globalTrajectory(i).Trajectory(:,5) < minVelocity);
isValid(i) = ~accViolated && ~curvViolated && ~velViolated;
end
end碰撞检测
% Update the collision checker with the predicted trajectories
% of all actors in the scene.
for i = 1:numel(actorPoses)
actorPoses(i).States = futureTrajectory(i).Trajectory(:,1:3);
end
% [x,y,theta,kappa,v,a]
updateObstaclePose(capList,actorID,actorPoses);
% Determine evaluation order.
[cost, idx] = sort(costTS);
optimalTrajectory = [];
trajectoryEvaluation = nan(numel(isValid),1); % 作一个空的向量，记录轨迹
% Check each trajectory for collisions starting with least cost.
for i = 1:numel(idx)
if isValid(idx(i))
% Update capsule list with the ego object's candidate trajectory.
egoPoses.States = globalTraj(idx(i)).Trajectory(:,1:3);
updateEgoPose(capList,egoID,egoPoses);
% Check for collisions.
isColliding = checkCollision(capList);
if all(~isColliding)
% If no collisions are found, this is the optimal.
% trajectory.
trajectoryEvaluation(idx(i)) = 1;
optimalTrajectory = globalTraj(idx(i)).Trajectory;
break;
else
trajectoryEvaluation(idx(i)) = 0;
end
end
end工具函数checkCollision
function [collisionStatus, separationDist, witnessPts] = checkCollision(geom1, geom2)
%CHECKCOLLISION Report collision status between two convex geometries
% COLLISIONSTATUS = checkCollision(GEOM1, GEOM2) check if GEOM1 and GEOM2
% are in collision at their specified poses, respectively. COLLISIONSTATUS
% is set to 1 if collision happens and 0 if no collision is found.
%
% [COLLISIONSTATUS, SEPDIST, WITNESSPTS] = checkCollision(___) returns
% additional information related to the collision check in SEPDIST and
% WITNESSPTS.
% 如果没有碰撞，SEPDIST代表两个物体间的最小距离，withesspts代表了检查点？每个物体都是（3*2）的矩阵，每一列都是检查点？
如果碰撞了，那么就设置为nan。
例子，这个demo唯一不好的就是使用了很多的工具函数，无法直接get到原理。See also collisionBox, collisionCylinder, collisionSphere,collisionMesh
% Example:
%
% % Create a box primitive
% bx = collisionBox(1,2,3);
%
% % Create a cylinder primitive
% cy = collisionCylinder(4,2);
%
% % Translate the cylinder along x axis
% T = trvec2tform([2 0 0]);
% cy.Pose = T;
%
% % Query collision status
% [isIntersecting, dist, witnessPoints] = checkCollision(bx, cy);
%
%
%
%#codegen
narginchk(2,2);
% 碰撞检测的函数直接用它的，不看原理，学习的是流程，后期自己写可以用AABB，或者用沃尔沃的。
validateattributes(geom1, {'robotics.core.internal.CollisionGeometryBase'}, ...
{'scalar', 'nonempty'}, 'checkCollision', 'geom1');
validateattributes(geom2, {'robotics.core.internal.CollisionGeometryBase'}, ...
{'scalar', 'nonempty'}, 'checkCollision', 'geom2');
[collisionStatus,separationDist,witnessPts]=robotics.core.internal.checkCollision(geom1,geom2);
end
定义仿真和planning的参数
% Synchronize the simulator's update rate to match the trajectory generator's
% discretization interval.
scenario.SampleTime = connector.TimeResolution; % in seconds
% Define planning parameters.
replanRate = 10; % Hz
% Define the time intervals between current and planned states.
timeHorizons = 1:3; % in seconds
maxHorizon = max(timeHorizons); % in seconds
% Define cost parameters.
latDevWeight = 1;
timeWeight = -1;
speedWeight = 1;
% Reject trajectories that violate the following constraints.
maxAcceleration = 15; % in meters/second^2
maxCurvature = 1; % 1/meters, or radians/meter
minVelocity = 0; % in meters/second
% Desired velocity setpoint, used by the cruise control behavior and when
% evaluating the cost of trajectories.
speedLimit = 11; % in meters/second
% Minimum distance the planner should target for following behavior.
safetyGap = 10; % in metersInitialize Simulator 初始化仿真器
[scenarioViewer,futureTrajectory,actorID,actorPoses,egoID,egoPoses,stepPerUpdate,egoState,isRunning,lineHandles] = ...
exampleHelperInitializeSimulator(scenario,capList,refPath,laneWidth,replanRate,carLen);
function [viewer, futureTrajectory, actorID, actorPoses, egoID, egoPoses, stepPerUpdate, egoState, isRunning, lineHandles] = ...
exampleHelperInitializeSimulator(scenarioObj, capList, refPath, laneWidth, replanRate, carLen)
%exampleHelperInitializeSimulator Set up scenario and scenario viewer
% 若干天之后，我已经忘记了capList是什么了。后期自己实现
% Reset the scenario
restart(scenarioObj)
close all;
% Create storage container for predicted actor trajectories
numActors = numel(scenarioObj.Actors)-1;
futureTrajectory = repelem(struct('Trajectory',[]),numActors,1);
% Place the ego vehicle at the start of the reference path，初始化自车位置
egoState = frenet2global(refPath,[0 0 0 -0.5*laneWidth 0 0]);
% Retrieve pose struct from dynamicCapsuleList for all actors and ego car
[actorID, actorPoses] = obstaclePose(capList, capList.ObstacleIDs);
[egoID, egoPoses] = egoPose(capList, capList.EgoIDs);
stepPerUpdate = (1/replanRate)/scenarioObj.SampleTime;
% Create a second scenario object. This will be used to visualize the
% scenario in real time.
viewer = drivingScenarioTrafficExample;
viewer.SampleTime = scenarioObj.SampleTime;
% Create a chasePlot using the visualization scenario.
chasePlot(viewer.Actors(1),'ViewLocation',-[carLen*3, 0],'ViewHeight',10,'ViewPitch',20);
% Create a pop-out figure and display the current scene.
f = gcf;
f.Visible = 'on';
advance(viewer);
annotationSpacing = 0.05;
a = [];
x = [.9 .95];
y = [.95 .95];
bdims = [x(1)-.175 ...
y(1)-(annotationSpacing*3.5) ...
(x(2)-x(1))+.2 ...
annotationSpacing*4];
p = uipanel(f,'Position',bdims,'BackgroundColor',[.75 .75 .75]);
commonProps = {'VerticalAlignment','middle','HorizontalAlignment','center','Margin',0,'FontUnits','normalized','Color','k'};
a = annotation(p,'textbox', [.05 .725 .9 .15],'String','Optimal ', 'EdgeColor','g','BackgroundColor','g','LineWidth',5, commonProps{:});
a2 = annotation(p,'textbox',[.05 .525 .9 .15],'String','Colliding ', 'EdgeColor','r','BackgroundColor','r','LineWidth',2, commonProps{:});
a3 = annotation(p,'textbox',[.05 .325 .9 .15],'String','NotEvaluated ', 'EdgeColor','w','BackgroundColor','w','LineWidth',2, commonProps{:});
a4 = annotation(p,'textbox',[.05 .125 .9 .15],'String','ConstraintViolated ','EdgeColor','c','BackgroundColor','c','LineWidth',2, 'LineStyle','--',commonProps{:});
a.FontSize = .1;
a2.FontSize = .1;
a3.FontSize = .1;
a4.FontSize = .1;
lineHandles = [];
isRunning = true;运行放真
%% 1
tic
while isRunning
% Retrieve the current state of actor vehicles and their trajectory over
% the planning horizon.
[curActorState,futureTrajectory,isRunning] = ...
exampleHelperRetrieveActorGroundTruth(scenario,futureTrajectory,replanRate,maxHorizon);
%四个输入：场景、未来轨迹、重规划周期、最大预测时领域
%三个输出。现在的npc状态，未来的轨迹，运行标志位
%% 2.生成终端状态
% Generate cruise control states.
[termStatesCC,timesCC] = exampleHelperBasicCruiseControl(...
refPath,laneWidth,egoState,speedLimit,timeHorizons);
% Generate lane change states.
[termStatesLC,timesLC] = exampleHelperBasicLaneChange(...
refPath,laneWidth,egoState,timeHorizons);
% Generate vehicle following states.
[termStatesF,timesF] = exampleHelperBasicLeadVehicleFollow(...
refPath,laneWidth,safetyGap,egoState,curActorState,timeHorizons);
%% 3. 计算代价
% Combine the terminal states and times.
allTS = [termStatesCC; termStatesLC; termStatesF];
allDT = [timesCC; timesLC; timesF];
numTS = [numel(timesCC); numel(timesLC); numel(timesF)];
% Evaluate cost of all terminal states.
costTS = exampleHelperEvaluateTSCost(allTS,allDT,laneWidth,speedLimit,...
speedWeight, latDevWeight, timeWeight);
%% 4.生成轨迹，并评估是否违反了约束
% Generate trajectories.
egoFrenetState = global2frenet(refPath,egoState);
[frenetTraj,globalTraj] = connect(connector,egoFrenetState,allTS,allDT);
% 这个connect是啥来？？connector ;虽然我知道是连接初始状态和终端状态的意思。
% Eliminate trajectories that violate constraints.
isValid = exampleHelperEvaluateTrajectory(globalTraj,maxAcceleration,maxCurvature,minVelocity);
%% 4. 碰撞检测
% Update the collision checker with the predicted trajectories
% of all actors in the scene.
for i = 1:numel(actorPoses)
actorPoses(i).States = futureTrajectory(i).Trajectory(:,1:3);
end
updateObstaclePose(capList,actorID,actorPoses);% 这个工具函数，封装的
% Determine evaluation order.
[cost, idx] = sort(costTS);
optimalTrajectory = [];
trajectoryEvaluation = nan(numel(isValid),1);
% Check each trajectory for collisions starting with least cost.
for i = 1:numel(idx)
if isValid(idx(i))
% Update capsule list with the ego object's candidate trajectory.
egoPoses.States = globalTraj(idx(i)).Trajectory(:,1:3);
updateEgoPose(capList,egoID,egoPoses);
% Check for collisions.
isColliding = checkCollision(capList); % 碰撞检测的原理不知道。
if all(~isColliding)
% If no collisions are found, this is the optimal.
% trajectory.
trajectoryEvaluation(idx(i)) = 1;
optimalTrajectory = globalTraj(idx(i)).Trajectory;
break;
else
trajectoryEvaluation(idx(i)) = 0;
end
end
end
%% 展现采样的轨迹
% Display the sampled trajectories.
lineHandles = exampleHelperVisualizeScene(lineHandles,globalTraj,isValid,trajectoryEvaluation);
hold on;
show(capList,'TimeStep',1:capList.MaxNumSteps,'FastUpdate',1);
hold off;
if isempty(optimalTrajectory)
% 所有轨迹 违反约束/碰撞
error('No valid trajectory has been found.');
else
% Visualize the scene between replanning.
for i = (2+(0:(stepPerUpdate-1)))
% Approximate realtime visualization.
% 跟运行时间对齐
dt = toc;
if scenario.SampleTime-dt > 0
pause(scenario.SampleTime-dt);
end
egoState = optimalTrajectory(i,:);
scenarioViewer.Actors(1).Position(1:2) = egoState(1:2);【x,y】
scenarioViewer.Actors(1).Velocity(1:2) = [cos(egoState(3)) sin(egoState(3))]*egoState(5);【vx,vy】
scenarioViewer.Actors(1).Yaw = egoState(3)*180/pi;
scenarioViewer.Actors(1).AngularVelocity(3) = egoState(4)*egoState(5);
% Update capsule visualization.
hold on;
show(capList,'TimeStep',i:capList.MaxNumSteps,'FastUpdate',1);%?
hold off;
% Update driving scenario.
advance(scenarioViewer);
tic;
end
end
end
工具函数
function [curState, futureTrajectory, isRunning] = exampleHelperRetrieveActorGroundTruth(scenario, futureTrajectory, replanRate, maxHorizon)
%exampleHelperRetrieveActorGroundTruth Retrieve current and future states of each actor over a given time horizon.检索现在和未来的npc状态。也就是所有轨迹都是已经知道，不做预测，只测试算法逻辑是否正确。
% FUTURETRAJECTORY is an M-by-1 轨迹结构体，M是npc的数目。
% N 是未来轨迹的步数。
%N 由于两个参数决定，重规划周期REPLANRATE 和最大预测时域MAXHORIZON
numActor = numel(futureTrajectory);
curState = zeros(numActor,6);
% 这个代码很关键。如果不到重新规划的时间，那么用最大预测-现在的步长；如果大于重新规划，则用宠幸规划的步长。
% Number of states needed
minUpdateSteps = (1/replanRate)/scenario.SampleTime;
maxNumStates = maxHorizon/scenario.SampleTime;
statesNeeded = max(maxNumStates-size(futureTrajectory(1).Trajectory,1),minUpdateSteps);
for i = 1:statesNeeded
% Advance the scenario
isRunning = advance(scenario);
% Retrieve actor information
poses = scenario.actorPoses;
for j = 1:numActor
actIdx = j+1;% 1是自车。
xy = poses(actIdx).Position(1:2);向量【x,y】
v = norm(poses(actIdx).Velocity,2);
th = atan2(poses(actIdx).Velocity(2),poses(actIdx).Velocity(1));atan2(vy/vx);
k = poses(actIdx).AngularVelocity(3)/v/180*pi;kappa *pi/180(度数转换弧度)
% Assume acceleration = 0
futureTrajectory(j).Trajectory(i,:) = [xy th k v 0];【横纵向位置 航向、kappa、v、加速度】
end
if ~isRunning
break;
end
end
% Reorder the states 记录状态
for i = 1:numActor
futureTrajectory(i).Trajectory = circshift(futureTrajectory(i).Trajectory,-statesNeeded,1);% 这个函数不知道
curState(i,:) = futureTrajectory(i).Trajectory(1,:); % 抽取出现在的状态量
end
endconnector是什么？
工具函数connect？？
capList中有什么？
主车的id,障碍物的id
简介版代码
%加载驾驶场景
scenario = drivingScenarioTrafficExample;
% scenario = drivingScenarioTrafficExample;
% Default car properties
carLen = 4.7; % in meters
carWidth = 1.8; % in meters
rearAxleRatio = .25;
% Define road dimensions
laneWidth = carWidth*2; % in meters
plot(scenario);
%构造参考路路径
waypoints = [0 50; 150 50; 300 75; 310 75; 400 0; 300 -50; 290 -50; 0 -50]; % in meters
refPath = referencePathFrenet(waypoints);
ax = show(refPath);
axis(ax,'equal'); xlabel('X'); ylabel('Y');
%构造轨迹连接器
connector = trajectoryGeneratorFrenet(refPath);
%构造碰撞检测器
capList = dynamicCapsuleList;
egoID = 1;
[egoID, egoGeom] = egoGeometry(capList,egoID);
egoGeom.Geometry.Length = carLen; % in meters
egoGeom.Geometry.Radius = carWidth/2; % in meters
egoGeom.Geometry.FixedTransform(1,end) = -carLen*rearAxleRatio; % in meters
updateEgoGeometry(capList,egoID,egoGeom);
actorID = (1:5)';
actorGeom = repelem(egoGeom,5,1);
updateObstacleGeometry(capList,actorID,actorGeom)
%定义仿真和planning参数
% Synchronize the simulator's update rate to match the trajectory generator's
% discretization interval.
scenario.SampleTime = connector.TimeResolution; % in seconds
% Define planning parameters.
replanRate = 10; % Hz
% Define the time intervals between current and planned states.
timeHorizons = 1:3; % in seconds
maxHorizon = max(timeHorizons); % in seconds
% Define cost parameters.
latDevWeight = 1;
timeWeight = -1;
speedWeight = 1;
% Reject trajectories that violate the following constraints.
maxAcceleration = 15; % in meters/second^2
maxCurvature = 1; % 1/meters, or radians/meter
minVelocity = 0; % in meters/second
% Desired velocity setpoint, used by the cruise control behavior and when
% evaluating the cost of trajectories.
speedLimit = 11; % in meters/second
% Minimum distance the planner should target for following behavior.
safetyGap = 10; % in meters
%初始化仿真器
[scenarioViewer,futureTrajectory,actorID,actorPoses,egoID,egoPoses,stepPerUpdate,egoState,isRunning,lineHandles] = ...
exampleHelperInitializeSimulator(scenario,capList,refPath,laneWidth,replanRate,carLen);
%% 1
tic
while isRunning
% Retrieve the current state of actor vehicles and their trajectory over
% the planning horizon.
[curActorState,futureTrajectory,isRunning] = ...
exampleHelperRetrieveActorGroundTruth(scenario,futureTrajectory,replanRate,maxHorizon);
%四个输入：场景、未来轨迹、重规划周期、最大预测时领域
%三个输出。现在的npc状态，未来的轨迹，运行标志位
%% 2.生成终端状态
% Generate cruise control states.
[termStatesCC,timesCC] = exampleHelperBasicCruiseControl(...
refPath,laneWidth,egoState,speedLimit,timeHorizons);
% Generate lane change states.
[termStatesLC,timesLC] = exampleHelperBasicLaneChange(...
refPath,laneWidth,egoState,timeHorizons);
% Generate vehicle following states.
[termStatesF,timesF] = exampleHelperBasicLeadVehicleFollow(...
refPath,laneWidth,safetyGap,egoState,curActorState,timeHorizons);
%% 3. 计算代价
% Combine the terminal states and times.
allTS = [termStatesCC; termStatesLC; termStatesF];
allDT = [timesCC; timesLC; timesF];
numTS = [numel(timesCC); numel(timesLC); numel(timesF)];
% Evaluate cost of all terminal states.
costTS = exampleHelperEvaluateTSCost(allTS,allDT,laneWidth,speedLimit,...
speedWeight, latDevWeight, timeWeight);
%% 4.生成轨迹，并评估是否违反了约束
% Generate trajectories.
egoFrenetState = global2frenet(refPath,egoState);
[frenetTraj,globalTraj] = connect(connector,egoFrenetState,allTS,allDT);
% 这个connect是啥来？？connector ;虽然我知道是连接初始状态和终端状态的意思。
% Eliminate trajectories that violate constraints.
isValid = exampleHelperEvaluateTrajectory(globalTraj,maxAcceleration,maxCurvature,minVelocity);
%% 4. 碰撞检测
% Update the collision checker with the predicted trajectories
% of all actors in the scene.
for i = 1:numel(actorPoses)
actorPoses(i).States = futureTrajectory(i).Trajectory(:,1:3);
end
updateObstaclePose(capList,actorID,actorPoses);% 这个工具函数，封装的
% Determine evaluation order.
[cost, idx] = sort(costTS);
optimalTrajectory = [];
trajectoryEvaluation = nan(numel(isValid),1);
% Check each trajectory for collisions starting with least cost.
for i = 1:numel(idx)
if isValid(idx(i))
% Update capsule list with the ego object's candidate trajectory.
egoPoses.States = globalTraj(idx(i)).Trajectory(:,1:3);
updateEgoPose(capList,egoID,egoPoses);
% Check for collisions.
isColliding = checkCollision(capList); % 碰撞检测的原理不知道。
if all(~isColliding)
% If no collisions are found, this is the optimal.
% trajectory.
trajectoryEvaluation(idx(i)) = 1;
optimalTrajectory = globalTraj(idx(i)).Trajectory;
break;
else
trajectoryEvaluation(idx(i)) = 0;
end
end
end
%% 展现采样的轨迹
% Display the sampled trajectories.
lineHandles = exampleHelperVisualizeScene(lineHandles,globalTraj,isValid,trajectoryEvaluation);
hold on;
show(capList,'TimeStep',1:capList.MaxNumSteps,'FastUpdate',1);
hold off;
if isempty(optimalTrajectory)
% 所有轨迹 违反约束/碰撞
error('No valid trajectory has been found.');
else
% Visualize the scene between replanning.
for i = (2+(0:(stepPerUpdate-1)))
% Approximate realtime visualization.
% 跟运行时间对齐
dt = toc;
if scenario.SampleTime-dt > 0
pause(scenario.SampleTime-dt);
end
egoState = optimalTrajectory(i,:);
scenarioViewer.Actors(1).Position(1:2) = egoState(1:2);%【x,y】
scenarioViewer.Actors(1).Velocity(1:2) = [cos(egoState(3)) sin(egoState(3))]*egoState(5);%【vx,vy】
scenarioViewer.Actors(1).Yaw = egoState(3)*180/pi;
scenarioViewer.Actors(1).AngularVelocity(3) = egoState(4)*egoState(5);
% Update capsule visualization.
hold on;
show(capList,'TimeStep',i:capList.MaxNumSteps,'FastUpdate',1);%?
hold off;
% Update driving scenario.
advance(scenarioViewer);
tic;
end
end
end