Highway Trajectory Planning Using Frenet Reference Path
4 次查看(过去 30 天)
显示 更早的评论
Good day. I have a question regarding the following example: https://de.mathworks.com/help/nav/ug/highway-trajectory-planning-using-frenet.html . The are plenty helper functions, which I am trying to work through and understand systematically. At the moment I have some problems understanding the concept of exampleHelperPredictLane. The function predicts the vehicle lane in the future based on the vehicles lateral motion characteristics. The code is written as follows:
function laneNum = exampleHelperPredictLane(frenetState, laneWidth, dt)
%exampleHelperPredictLane Predicts a vehicles lane for a given set of times
%
% This function is for internal use only. It may be removed in the future
% LANENUM = exampleHelperPredictLane(FRENETSTATE, LANEWIDTH, DT) predicts
% a vehicle's lane over one or more times, DT, in the future based on its
% current FRENETSTATE, a 1-by-6 vector in Frenet coordinates [S dS ddS L dL ddL],
% and the following assumptions:
%
% 1) Bang bang acceleration control
% 2) Constant change in lateral acceleration for two control sections
% 3) Terminal lateral velocity = 0
% 4) Terminal lateral acceleration = 0
% 5) Scenario occurs on a 4-lane highway with fixed lane width, LANEWIDTH
%
% NOTE: When DT is zero, no motion model assumptions are applied.
% Copyright 2020 The MathWorks, Inc.
narginchk(3,3);
laneBounds = [inf (2:-1:-2)*laneWidth -inf];
laneNum = zeros(numel(dt),1);
for i = 1:numel(dt)
if dt(i) == 0
dLaneEgo = laneBounds-frenetState(4); % [S dS ddS L dL ddL]
laneNum(i) = min(find(dLaneEgo(2:(end-1)) >= 0 & dLaneEgo(3:(end)) < 0,1),4);
else
% Retrieve current lateral velocity/acceleration/time
t = dt(i);
a0 = frenetState(6);
v0 = frenetState(5);
% Solve for the constant change in acceleration and time of
% application that arrest the ego vehicle's lateral
% velocity and acceleration over a given number of seconds.
if a0 == 0
avgAcc = -v0/t;
Ldiff = v0*t + avgAcc/2*t^2;
else
a = a0;
b = (-2*v0-2*a0*t);
c = (v0*t+a0/2*t^2);
% Possible time switches
r = (-b+(sqrt(b^2-4*a*c).*[-1 1]))/(2*a);
% Select the option that occurs in the future
rS = r(r>0 & r <= t);
% Calculate the constant change in acceleration
da0 = a0/(t-2*rS);
% Predict total distance traveled over t seconds
Ldiff = v0*t + a0/2*t^2 + da0/6*t^3 - da0/6*(t-rS)^3;
end
% Find distance between predicted offset and each lane
dLaneEgo = laneBounds-(frenetState(4)+Ldiff);
% Determine future lane
laneNum(i) = min(find(dLaneEgo(2:(end-1)) >= 0 & dLaneEgo(3:(end)) < 0,1),4);
end
end
end
What I do not understand is the idea behind the time switches, starting with the definition of the "a", "b" and "c" - values.
First of all, why is the initial speed v0 in variable "b" multiplied by 2 ? Why is the speed "b" defined in contrast to "a" and "c" as negative?
Second of all, what is the overall idea behind modelling the time switches and solving the problem with a quadratic a,b,c-formula?
Thank you in advance!
Best Regards
2 个评论
Anurag R
2022-6-21
编辑:Anurag R
2022-6-21
Just went throug the write up, my observation is as follows
(1) big bang control: for the time interval jerk changes at a particular point which is the time switch point.
(2) Jerk is constant as per assumption 2, and the time interval is split into two control sections
(3) time is from 0 to t, rS is the time switch point
(4) Lateral velocity increases from vo reaches max at rS, due to change in jerk it reduces to zero at t
(5) Lateral acceleration increases linearly from v0 reaches maximum at rS, and then linerarly reduces to 0 at t.
from 5
a@rS = a0 + da0 * rS (da0 is the constant jerk) and
0 = a@rs - da0*(t-rS)
0 = a0 + da0*rS - da0*t + da0*rS
0 = a0 + da0(2rS-t)
da0 = -a0/(2rS-t) or a0/(t-2rS) (which is the formula used in the code for change in acceleration
Switching time is calculated by taking the 4th condition and solving it
But the solution I obtained is different from one that is in the code
回答(0 个)
另请参阅
类别
在 Help Center 和 File Exchange 中查找有关 Statics and Dynamics 的更多信息
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!