【MatLab学习笔记】大型实例Highway Trajectory Planning Using Frenet Reference Path

最新推荐文章于 2024-05-22 23:30:42 发布

Pinoxchio

最新推荐文章于 2024-05-22 23:30:42 发布

阅读量2.9k

点赞数 6

分类专栏： MatLab学习笔记文章标签： matlab 自动驾驶

本文链接：https://blog.csdn.net/weixin_42325129/article/details/117652582

版权

本文介绍了使用MATLAB进行高速公路轨迹规划的方法，具体包括加载驾驶环境、创建参考路径、轨迹生成器、动态碰撞检测器的创建以及规划自适应路线的详细步骤。通过Frenet参考路径，结合动态障碍物和驾驶场景，实现自车的自适应巡航控制和无碰撞行驶。案例展示了如何进行轨迹评估、成本计算、约束检查以及选择最优轨迹，以确保行驶的平滑和安全性。

摘要由CSDN通过智能技术生成

以下实例为MathWorks官方实例，本人翻译整理了相关知识点，记录一下学习笔记。

原文链接：https://ww2.mathworks.cn/help/driving/ug/highway-trajectory-planning-using-frenet-reference-path.html

Highway Trajectory Planning Using Frenet Reference Path

本例演示了如何在高速公路驾驶场景中规划局部轨迹。此示例使用参考路径和动态障碍物列表来生成自车的替代轨迹。自车通过从drivingScenario对象提供的驾驶场景中定义的交通情况中进行巡航。车辆在基于行驶代价函数、可行性分析和无碰撞运动检测三项条件来实现自适应巡航控制、换道和车辆跟随操作之间的交替行驶。

加载驾驶环境

从加载提供的drivingScenario对象开始，该对象定义了当前工作区中的车辆和道路属性。此场景是使用Driving scenario Designer应用程序生成的，并导出到MATLAB®函数DrivingScenarioTrafficExample中。

scenario = drivingScenarioTrafficExample;
%加载内置好的驾驶环境

carLen   = 4.7; %车长，米为单位
carWidth = 1.8; %车宽，米为单位
rearAxleRatio = .25; %后轴比
%初始化车辆信息

laneWidth   = carWidth*2; % 车道宽为车宽的两倍
%初始化道路信息

plot(scenario);

Figure contains an axes. The axes contains 9 objects of type patch, line.

创建参考路径

本例中的所有局部路径规划都是针对参考路径执行的，由referencePathFrenet对象表示。该对象可以返回给定路径长度的曲线状态，找到路径上与全局xy坐标最近的点，并便于实现全局坐标系与Frenet坐标系之间的坐标转换。

对于本例，参考路径被视为四车道公路的中心，并且waypoints与提供的drivingScenario对象中定义的道路匹配。

waypoints = [0 50; 150 50; 300 75; 310 75; 400 0; 300 -50; 290 -50; 0 -50]; 
% 路径点，单位米

refPath = referencePathFrenet(waypoints);
%定义一个通过Frenet转换的基于路径点的参考路径

ax = show(refPath);
%显示参考路径

axis(ax,'equal');

Figure contains an axes. The axes contains 2 objects of type line.

创建轨迹生成器

对于局部规划器来说，其目标通常是在满足当前运动学和动力学条件的同时，对朝着最终目标移动的各种可能的运动进行采样。TraightoryGeneratorFrenet对象通过使用四阶或五阶多项式轨迹将初始状态与一系列的终端状态连接起来来实现这一点。在Frenet坐标系中定义初始状态和终止状态，每个多项式解均要满足横纵向位置限制、速度和加速度边界条件，同时最小化跃度。

终端状态通常使用自定义行为来计算。这些行为利用了在本地环境中发现的信息，例如车道信息、速度限制以及在自车附近的交通参与者的当前或未来状态来进行预测。

使用参考路径构造一个轨迹生成器Frenet对象：

connector = trajectoryGeneratorFrenet(refPath);

创建动态碰撞检测器

dynamicCapsuleList对象是一种数据结构，它表示动态环境在一组在离散的时间步长上的状态。然后，该环境可用于有效地验证自车的多个潜在轨迹。场景中的每个对象由以下表示：

唯一整数值标识符
用于有效碰撞检查的胶囊几何的性质
SE2状态的序列，其中每个状态表示时间上的离散快照。

在本例中，由TrajectoryGeneratorFrenet对象生成的轨迹发生在一段时间内，称为时间范围。为了确保冲突检查覆盖所有可能的轨迹，dynamicCapsuleList对象应该包含跨越最大预期时间范围的所有参与者的预测轨迹。

capList = dynamicCapsuleList;
%创建动态胶囊列表

基于给定的参数为自车创建一个几何形状结构体。

egoID = 1; %自车ID
[egoID, egoGeom] = egoGeometry(capList,egoID);
%创建自车几何形状结构体的语法，返回胶囊列表中指定ID的几何形状信息

egoGeom.Geometry.Length = carLen; % 车长，米为单位
egoGeom.Geometry.Radius = carWidth/2; % 车宽，米为单位
egoGeom.Geometry.FixedTransform(1,end) = -carLen*rearAxleRatio; % 指定自车原点，单位米
%为创建好的自车几何结构体进行赋值操作

将自车添加到动态胶囊列表中。

updateEgoGeometry(capList,egoID,egoGeom);
%更新自车几何形状信息

将drivingScenario中的交通参与者添加到dynamicCapsuleList对象中。这里设置了几何形状，并且在规划循环期间定义了状态。可以看到dynamicCapsuleList现在包含一个自车和五个障碍物。

actorID = (1:5)';
%为障碍物分配ID，从1到5

actorGeom = repelem(egoGeom,5,1);
%将egoGeom矩阵中的每个元素复制到一个新矩阵的5x1块中

updateObstacleGeometry(capList,actorID,actorGeom）
%更新障碍物几何形状信息

结果如下图：

ans = 
  dynamicCapsuleList with properties:

     MaxNumSteps: 31
          EgoIDs: 1
     ObstacleIDs: [5x1 double]
    NumObstacles: 5
         NumEgos: 1

通过交通情况规划自适应路线

规划程序支持局部路径规划策略，该策略在选择最优轨迹之前，基于环境的当前和预见状态对一组局部轨迹进行采样。仿真循环被分成以下几个部分：

预先设定真实路面环境
创建终端状态
计算终端状态的成本
创建轨迹并进行运动学可行性检测
进行轨迹碰撞检测并选择最优轨迹
显示选择的轨迹并绘制场景仿真

预先设定真实路面环境

在动态环境中进行规划时，往往需要估计环境的状态或预测其一段时间内的状态。为了简单起见，本例使用drivingScenario作为规划范围内每个参与者的轨迹的真实路面的数据源。若要测试自定义的预测算法，可以使用自定义代码替换或修改ExampleHelperRetrieveActorGroundTrue函数。

function [curState, futureTrajectory, isRunning] = exampleHelperRetrieveActorGroundTruth(scenario, futureTrajectory, replanRate, maxHorizon)

% ExampleHelperRetrieveActorGroundTrue检索给定时间范围内每个执行元的当前状态和未来状态。
%
%   此功能仅供内部使用。 将来可能会被移除。
%   将drivingScenario对象SCENARIO提前到当前模拟时间步，并将每个参与者的状态存储在
%   FutureTrajectory中。 FutureTraimtory是一个m-y-1结构数组，包含场轨迹，一个n-y-6矩阵，
#   其中M是非自我行动者的数量，N是轨迹存储的未来步骤的数量。
%   N是使用规划器的重新规划率和规划轨迹可以发生的最大时间跨度，即maxhorizon来确定的。
%   该功能可由自定义预测模块或外部地面真值数据代替。
%
% Copyright 2020 The MathWorks, Inc.
    
    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;
            xy = poses(actIdx).Position(1:2);
            v  = norm(poses(actIdx).Velocity,2);
            th = atan2(poses(actIdx).Velocity(2),poses(actIdx).Velocity(1));
            k = poses(actIdx).AngularVelocity(3)/v/180*pi;
            % Assume acceleration = 0
            futureTrajectory(j).Trajectory(i,:) = [xy th k v 0];
        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
end

创建终端状态

自动驾驶的一个共同目标是确保规划的轨迹不仅是可行的，而且是自然的。典型的高速公路驾驶包括车道保持、限速保持、变换车道、使速度适应交通情况等等。每个自定义行为可能需要不同的环境信息。此示例演示了如何生成实现三种行为的终端状态：巡航控制、车道变换和跟随前车。

巡航控制

exampleHelperBasicCruiseControl函数生成执行巡航控制行为的终端状态。该函数使用自车的横向速度和一个时间范围来预测自车未来N秒的预期车道。计算出车道中心，使其成为终端状态的横向偏差，并将横向速度和加速度设为零。

对于纵向边界条件，终端速度设为道路限速，终端加速度设为零。纵向位置不受限制，指定为NaN。通过去掉经度约束，TrajectoryGeneratorFrenet 可以使用较低的4阶多项式来求解纵向边值问题，从而得到在给定时间范围内平滑地匹配道路速度的轨迹：

$cruiseControlState=[NaN\:\: \dot{s}_{des}\:\: 0\:\: l_{expLane} \:\:0\:\: 0]$

function [terminalStates, times] = exampleHelperBasicCruiseControl(refPath, laneWidth, 
egoState, targetVelocity, dt)
% 要生成巡航的终端状态，需要输入参考路径、车道宽度、自车状态、目标速度和dt。

% exampleHelperBasicCruiseControl为巡航控制行为生成终端状态。
%
%   此功能仅供内部使用。 未来可能会被移除
%   [TER