Highway Trajectory Planning Using Frenet Reference Path

Question

Alexander Maier 2021-1-4

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/708413-highway-trajectory-planning-using-frenet-reference-path

编辑： Anurag R 2022-6-21

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-16

Hello Alexander,

Are you able to decode the idea of time switching.

Anurag R 2022-6-21

编辑：Anurag R 2022-6-21

在 MATLAB Online 中打开

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

请先登录，再进行评论。

请先登录，再回答此问题。

Highway Trajectory Planning Using Frenet Reference Path

2 个评论
显示无隐藏无

回答（0 个）

另请参阅

类别

标签

Community Treasure Hunt

Highway Trajectory Planning Using Frenet Reference Path

2 个评论 显示 无隐藏 无

回答（0 个）

另请参阅

类别

标签

Community Treasure Hunt

2 个评论
显示无隐藏无