基于优化的自动驾驶车辆路径平滑

柯藤

已于 2022-07-26 15:29:45 修改

阅读量1.5k

点赞数 1

分类专栏：路径规划文章标签：自动驾驶人工智能机器学习

于 2022-07-26 15:26:30 首次发布

原文链接：https://ww2.mathworks.cn/help/nav/ug/optimization-based-path-smoothing-for-autonomous-vehicles.html

版权

路径规划专栏收录该内容

3 篇文章 2 订阅

订阅专栏

文章目录

前言
一、设置停车场环境
二、选择停车位
三、路径规划
四、配置路径优化参数
五、路径优化

前言

来源matlab工具箱Optimization Based Path Smoothing for Autonomous
Vehicles；此示例向您展示如何通过保持平滑的曲率和与停车场障碍物的安全距离来优化汽车类机器人的路径。
在此示例中，您可以使用 optimizePath 函数和 optimizePathOptions对象来优化规划路径。您可以使用任何二维路径规划器，plannerRRT、plannerRRTStar、plannerAstar、plannerHybridAStar等来规划从停车场入口到所需停车位的路径。在类似停车场的环境中，汽车经常需要急转弯并避开障碍物，如其他汽车、柱子、招牌等。路径规划器生成的路径可能并不总是安全、易于导航、平滑或运动学可行.在这种情况下，路径优化变得至关重要。optimizePathOptions对象具有大量参数和权重，您可以调整这些参数和权重以考虑环境和车辆约束，权重允许您在优化路径时设置它们的相对重要性。

一、设置停车场环境

从停车场地图创建 binaryOccupancyMap 对象，并将地图分辨率设置为 3 米。

load("parkingMap.mat");
resolution = 3;
map = binaryOccupancyMap(map,resolution);

可视化地图。该地图包含一个停车场的平面图，其中一些停车位已被占用。

show(map)
title("Parking Lot Map")

在这里插入图片描述

二、选择停车位

以车辆当前在停车场入口处的位置为起始姿态，选择一个想要的无人停车位作为车辆的目标姿态。定义车辆的起始姿势和目标姿势为 [x y theta]向量。X和y用米表示位置，theta用弧度表示方向角。

startPose = [2 9 0];
goalPose = [27 18 pi/2];

可视化姿势

show(map)
hold on
quiver(startPose(1),startPose(2),cos(startPose(3)),sin(startPose(3)),2,...
       color=[0 0.75 0.23],LineWidth=2,...
       Marker="o",MarkerFaceColor=[0 0.75 0.23],MarkerSize=5,...
       DisplayName="Start Pose",ShowArrowHead="off");
quiver(goalPose(1),goalPose(2),cos(goalPose(3)),sin(goalPose(3)),2, ...
       color=[1 0 0],LineWidth=2,...
       Marker='o',MarkerFaceColor=[1 0 0],MarkerSize=5,...
       DisplayName="Goal Pose",ShowArrowHead="off");
legend(Location="southeast");
title("Start and Goal Poses in Parking Lot Map")
hold off

在这里插入图片描述

三、路径规划

使用 stateSpaceSE2 定义创建一个 validatorOccupancyMap 状态验证器。指定用于插值和验证路径段的地图和距离。

validator = validatorOccupancyMap(stateSpaceSE2,map=map);
validator.ValidationDistance = 0.1;

使用状态验证器对象初始化 plannerHybridAStar 对象。指定规划器的 MinTurningRadius 和 MotionPrimitiveLength 属性。

planner = plannerHybridAStar(validator,MinTurningRadius=3,MotionPrimitiveLength=4);

为可重复性设置默认随机数

rng("default");

规划从起始姿势到目标姿势的路径

refPath = plan(planner,startPose,goalPose);
path = refPath.States;

可视化规划的路径

show(planner,Tree="off");
legend(Location="southeast");
hold off

在这里插入图片描述
可视化车辆的方向

plot(rad2deg(refPath.States(:,3)));
title("Orientation of Vehicle Along the Path in degrees")

在这里插入图片描述

四、配置路径优化参数

规划器生成的路径由连续的路径段组成，但交叉点可能是不连续的。这些交叉点会导致转向角的突然变化。路径还可能包含增加额外驾驶时间的路段。为了避免这种运动，需要优化和平滑路径。路径有时非常靠近障碍物，这可能是有风险的，尤其是对于卡车等重型车辆。

创建优化选项
创建 optimizePathOptions 对象以配置 optimizePath 函数的行为以及由此产生的路径。

options = optimizePathOptions

优化选项分为四类：

Trajectory Parameters 轨迹参数
Obstacle Parameters 障碍物参数
Solver Parameters 求解器参数
Weights 权重

轨迹参数
轨迹参数用于指定车辆在沿路径移动时的约束，如速度、加速度、转弯半径等。它们是软限制，这意味着求解器在优化路径时可能会稍微改变它们。调整以下参数，

MinTurningRadius - 车辆的转弯半径。增加 MinTurningRadius 将导致更大的路径曲率。
MaxVelocity - 车辆可以达到的最大速度。改变它会改变车辆速度和轨迹时间。
MaxAcceleration - 车辆可能的最大加速度。
ReferenceDeltaTime - 两个连续姿势之间的旅行时间。增加这将增加车辆速度。
MaxPathStates - 路径中允许的最大姿势数。增加它可以提供更平滑的轨迹，但也可能会增加优化时间。

options.MinTurningRadius = 3; % meters
options.MaxVelocity = 5; % m/s
options.MaxAcceleration = 1; % m/s/s
options.ReferenceDeltaTime = 0.1; % second

separationBetweenStates = 0.2; % meters
numStates = refPath.pathLength/separationBetweenStates;
options.MaxPathStates = round(numStates);

障碍物参数
障碍物参数指定了路径中障碍物的影响。如果障碍物有移动的可能，或者它们的尺寸不准确，那么你应该保持更高的安全边际。在这个例子中，由于大多数障碍是车道标记和静止的车辆，安全边际可以更小。调优以下参数:

ObstacleSafetyMargin - 路径应与障碍物保持的安全边距。增加安全裕度将使路径更安全并增加与障碍物的距离。
ObstacleInclusionDistance - 距离路径此距离内的障碍物将被包括在优化中。
ObstacleCutOffDistance - 距离路径超出此距离的障碍物将在优化期间被忽略。该值必须始终大于障碍物包含距离。
对于 ObstacleInclusionDistance 和 ObstacleCutOffDistance 之间的障碍物，考虑在包含距离和截止距离之间的左侧和右侧最近的障碍物。

options.ObstacleSafetyMargin = 2; % meters
options.ObstacleInclusionDistance = 0.75; % meters
options.ObstacleCutOffDistance = 2.5; %i meters

求解器参数
求解器参数指定优化路径时使用的求解器选项。这些参数的较高值会改善优化结果，但也会影响优化时间，因此应根据需要对其进行调整。调整以下参数，

NumIteration - 在优化期间调用求解器的次数，在每次调用之前，路径中的位姿数会根据参考增量时间进行调整。
MaxSolverIteration - 每次求解器调用的最大迭代次数。
如果路径已经很密集，则可以减少 NumIteration 并增加 MaxSolverIteration。如果路径稀疏，则 NumIteration 可以更高，MaxSolverIteration 可以保持更低。

options.NumIteration = 4; 
options.MaxSolverIteration = 15;

权重
权重定义了上述各种约束的相对重要性。由于大多数约束都是软限制，因此权重决定了约束在优化路径时的重要性。调整以下权重，

WeightTime - 时间分量的权重，增加它会导致更短的旅行时间和路径。
WeightSmoothness - 增加这个权重将使路径更平滑。
WeightMinTurningRadius - 增加这将尝试保持转弯半径大于大部分路径的最小值。
WeightVelocity - 增加这将确保更紧密地遵循速度约束。
WeightObstacles - 增加这将确保车辆不会越过障碍物。

options.WeightTime = 10;
options.WeightSmoothness = 1000;
options.WeightMinTurningRadius = 10;
options.WeightVelocity = 10;
options.WeightObstacles = 50;

五、路径优化

使用 optimizePath 函数根据上面定义的优化选项优化规划器生成的路径。

[optimizedPath,kineticInfo] = optimizePath(path,map,options);
drivingDir = sign(kineticInfo.Velocity);

可视化优化的路径

show(planner,Tree="off");
hold on
forwardMotion = optimizedPath(drivingDir==1,:);
reverseMotion = optimizedPath(drivingDir==-1,:);
quiver(forwardMotion(:,1),forwardMotion(:,2),cos(forwardMotion(:,3)),sin(forwardMotion(:,3)),...
    0.1,Color=[0 0.45 0.74],LineWidth=1,DisplayName="Optimized Forward Path");
quiver(reverseMotion(:,1),reverseMotion(:,2),cos(reverseMotion(:,3)),sin(reverseMotion(:,3)),...
    0.1,Color=[0.47 0.68 0.19],LineWidth=1,DisplayName="Optimized Reverse Path");
legend(Location="southeast");
title("Planned Path and Optimized Path")
hold off

在这里插入图片描述
沿着优化路径绘制车辆的方向

plot(rad2deg(optimizedPath(:,3)))
title("Orientation of Vehicle Along the Optimized Path in degrees")

在这里插入图片描述

柯藤

关注

1
点赞
踩
27

收藏

觉得还不错? 一键收藏
0
评论
基于优化的自动驾驶车辆路径平滑

来源matlab工具箱OptimizationBasedPathSmoothingforAutonomousVehicles；此示例向您展示如何通过保持平滑的曲率和与停车场障碍物的安全距离来优化汽车类机器人的路径。在此示例中，您可以使用optimizePath函数和optimizePathOptions对象来优化规划路径。......
复制链接

扫一扫