主要内容

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

打开实时脚本

这个示例向您展示了如何通过保持平滑的曲率和与停车场中的障碍物的安全距离来优化类车机器人的路径。

在本例中，可以使用optimizePath函数与optimizePathOptions对象来优化计划的路径。你可以使用任何二维路径规划器，比如plannerRRT，plannerRRTStar，plannerAstar，plannerHybridAStar等，以规划从停车场入口到所需停车位的道路。在类似停车场的环境中，汽车经常需要急转弯，并避开其他车辆、柱子、招牌等障碍物。路径规划者生成的路径可能并不总是安全的、易于导航的、平滑的或运动学上可行的。在这种情况下，路径优化变得至关重要。的optimizePathOptions对象有大量的参数和权重，你可以调整环境和车辆约束，权重允许你在优化路径时设置它们的相对重要性。

设置P发出火花的很多环境

创建一个binaryOccupancyMap对象，并将地图分辨率设置为3单元格/米。

负载(“parkingMap.mat”）;分辨率= 3;map = binaryoccuancymap (map,resolution);

想象地图。该地图包含一个停车场的平面图，其中一些停车位已经被占用。

显示(map)标题(“停车场地图”）

图中包含一个轴对象。标题为Parking Lot Map的axes对象包含一个image类型的对象。

选择停车位

将车辆在停车场入口处的当前位置作为起始姿态，并选择一个所需的未占用车位作为车辆的目标姿态。

定义车辆的起始和目标姿态为[xy)向量。x而且y位置单位为米，和θ指定以弧度为单位的方向角度。

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

视觉化姿势。

显示(map)在箭袋(startPose (1) startPose(2),因为(startPose(3)),罪(startPose (3)), 2,.．.color=[0 0.75 0.23]，LineWidth=2，.．.标志=“o”，MarkerFaceColor=[0 0.75 0.23]，MarkerSize=5，.．.DisplayName =“开始姿势”ShowArrowHead =“关闭”）;箭袋(goalPose (1) goalPose(2),因为(goalPose(3)),罪(goalPose (3)), 2,.．.color=[1 0 0]，LineWidth=2，.．.标志=“o”，MarkerFaceColor=[1 0 0]，MarkerSize=5，.．.DisplayName =“目标姿势”ShowArrowHead =“关闭”）;传奇(位置=“东南”）;标题(“停车场地图中的起始和目标姿势”)举行从

图中包含一个轴对象。标题为Start and Goal pose的坐标轴对象在停车场地图中包含了3个类型为image, quiver的对象。这些对象代表开始姿势、目标姿势。

路径规划

创建一个validatorOccupancyMap状态验证器stateSpaceSE2定义。指定插值和验证路径段的映射和距离。

validator = validatorOccupancyMap(stateSpaceSE2,map=map);验证器。ValidationDistance = 0.1;

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

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

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

rng (“默认”）;

计划从开始姿势到目标姿势的路径。

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

想象计划好的路径。

显示(规划师、树=“关闭”）;传奇(位置=“东南”）;持有从

图中包含一个轴对象。标题为Hybrid A* Path Planner的axis对象包含7个类型为image、line、scatter的对象。这些对象表示前进路径、反向路径、路径点、方向、起点、目标。

想象一下车辆的方向。

情节(rad2deg (refPath.States (:, 3)));标题(“车辆沿路径的方向(以度为单位)”）

图中包含一个轴对象。标题为Orientation of Vehicle Along The Path in degrees的axis对象包含一个类型为line的对象。

配置路径优化参数

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

创建优化选项

创建optimizePathOptions对象的行为来配置optimizePath函数及其产生的路径。

options = optimizePathOptions
options = optimizePathOptions轨迹参数MaxPathStates: 200 ReferenceDeltaTime: 0.3000 MinTurningRadius: 1 MaxVelocity: 0.4000 MaxAngularVelocity: 0.3000 MaxAcceleration: 0.5000 MaxAngularAcceleration: 0.5000障碍物参数ObstacleSafetyMargin: 0.5000 ObstacleCutOffDistance: 2.5000 ObstacleInclusionDistance: 0.7500求解器参数NumIteration: 4 MaxSolverIteration: 15 Weights WeightTime: 10 WeightMinTurningRadius: 10 WeightVelocity: 10 WeightVelocity:100 WeightAngularVelocity: 10 WeightAngularAcceleration: 10 WeightAngularAcceleration: 10 WeightObstacles: 50

优化选项分为四类:

  1. 轨道参数

  2. 障碍参数

  3. 解算器参数

  4. 权重

轨道参数

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

  1. MinTurningRadius-车辆的转弯半径。增加了MinTurningRadius会导致更大的路径曲率。

  2. MaxVelocity-车辆能达到的最大速度。改变这一点将改变飞行器的速度和轨迹时间。

  3. MaxAcceleration-车辆可能的最大加速。

  4. ReferenceDeltaTime-两个连续姿势之间的旅行时间。增加这将增加车辆的速度。

  5. MaxPathStates-路径中允许的最大姿势数。增加这个值可以获得更流畅的轨迹，但也可能会增加优化时间。

选项。MinTurningRadius = 3;%米选项。MaxVelocity = 5;% m / s选项。MaxAcceleration = 1;% m / s / s选项。ReferenceDeltaTime = 0.1;%的第二个separationBetweenStates = 0.2;%米numStates = refPath.pathLength/separationBetweenStates;选项。MaxPathStates = round(numStates);

障碍参数

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

  1. ObstacleSafetyMargin-路径与障碍物之间应保持的安全裕度。增加安全裕度将使路径更安全，并增加与障碍物的距离。

  2. ObstacleInclusionDistance-这段距离内的障碍物将被纳入优化范围。

  3. ObstacleCutOffDistance-在优化过程中忽略距离路径超过这个距离的障碍物。此值必须始终大于障碍物包含距离。

对于之间的障碍ObstacleInclusionDistance而且ObstacleCutOffDistance考虑了夹杂物与截止距离之间左右两侧最接近的障碍物。

选项。ObstacleSafetyMargin = 2;%米选项。绊脚石inclusiondistance = 0.75;%米选项。ObstacleCutOffDistance = 2.5;%我米

解算器参数

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

  1. NumIteration-在优化过程中调用求解器的次数，在每次调用之前，路径中的姿势数量根据引用增量时间进行调整。

  2. MaxSolverIteration-每次求解器调用的最大迭代次数。

如果路径已经是密集的，则NumIteration可以减少MaxSolverIteration是增加了。如果路径稀疏，则NumIteration可以更高MaxSolverIteration可以保持低。

选项。NumIteration = 4;选项。MaxSolverIteration = 15;

权重

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

  1. WeightTime-时间分量的权重，增加时间分量会缩短行程时间和路径。

  2. WeightSmoothness增加这个重量会使路径更平滑。

  3. WeightMinTurningRadius增加这个值将会保持转弯半径大于大部分路径的最小值。

  4. WeightVelocity-增加这个可以确保速度约束得到更严格的遵守。

  5. WeightObstacles增加此值将确保车辆不会越过障碍物。

选项。WeightTime = 10;选项。重量平滑度= 1000;选项。WeightMinTurningRadius = 10;选项。WeightVelocity = 10;选项。体重= 50;

路径优化

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

[optimizedPath,kineticInfo] = optimizePath(路径，地图，选项);drivingDir = sign(kineticInfo.Velocity);

可视化优化的路径。

显示(规划师、树=“关闭”）;持有在forwardMotion = optimizedPath(driverdir ==1，:);reverseMotion = optimizedPath(drivingDir==-1，:);箭袋(forwardMotion (: 1) forwardMotion(:, 2),因为(forwardMotion(:, 3)),罪(forwardMotion (:, 3)),.．.0.1,Color=[0 0.45 0.74]，LineWidth=1,DisplayName=“优化的前进路径”）;箭袋(reverseMotion (: 1) reverseMotion(:, 2),因为(reverseMotion(:, 3)),罪(reverseMotion (:, 3)),.．.0.1,Color=[0.47 0.68 0.19]，LineWidth=1,DisplayName=“优化逆向路径”）;传奇(位置=“东南”）;标题(“规划路径与优化路径”)举行从

图中包含一个轴对象。标题为“规划路径”和“优化路径”的轴对象包含图像、线、散点、抖动等9个对象。这些对象表示前进路径、反向路径、路径点、方向、起点、目标、优化的前进路径、优化的反向路径。

绘制车辆沿优化路径的方向。

情节(rad2deg (optimizedPath(:, 3)))标题(“车辆沿优化路径的方向(以度为单位)”）

图中包含一个轴对象。标题为Orientation of Vehicle Along The Optimized Path(以度为单位)的axis对象包含一个类型为line的对象。

使用实时控件调优参数

在上一节中，基于一些预设的参数值对路径进行了优化。然而，设置一组良好的参数会对路径产生重大影响。

在本节中，您可以使用滑块更新参数，并可视化它们对优化路径的影响。这将帮助您更好地理解参数如何影响最终的平滑路径。

轨道参数

选项。MinTurningRadius =1．5;% m选项。MaxVelocity =5;% m / s选项。MaxAcceleration =1;% m / s / s

ReferenceDeltaTime是一个重要的参数，可以对结果产生巨大的影响。

选项。ReferenceDeltaTime =0.2;% sseparationBetweenStates =0.2;% mnumStates = refPath.pathLength/separationBetweenStates;选项。MaxPathStates = round(numStates);

障碍参数

选项。ObstacleSafetyMargin =1．5;% m选项。ObstacleCutOffDistance =4;% m选项。ObstacleInclusionDistance =2;% m

解算器参数

选项。NumIteration =2;选项。MaxSolverIteration =8;

权重

选项。WeightTime =200;选项。WeightSmoothness =1000;选项。WeightMinTurningRadius =140;选项。WeightVelocity =230;选项。WeightObstacles =320;

优化路径和可视化

新的优化路径与先前生成的优化路径一起绘制。这将帮助您比较优化结果。

首先绘制规划器规划的原始路径和之前优化的结果。

显示(规划师、树=“关闭”)举行在箭袋(forwardMotion (: 1) forwardMotion(:, 2),因为(forwardMotion(:, 3)),罪(forwardMotion (:, 3)),.．.0.1,Color=[0 0.45 0.74]，LineWidth=1,DisplayName=“先前优化的前进路径”）;箭袋(reverseMotion (: 1) reverseMotion(:, 2),因为(reverseMotion(:, 3)),罪(reverseMotion (:, 3)),.．.0.1,Color=[0.47 0.68 0.19]，LineWidth=1,DisplayName=“先前优化的反向路径”）;

现在根据新的优化选项集优化路径。

[optimizedPath,kineticInfo] = optimizePath(路径，地图，选项);

最后绘制新的优化路径。

drivingDir = sign(kineticInfo.Velocity);forwardMotion = optimizedPath(driverdir ==1，:);reverseMotion = optimizedPath(drivingDir==-1，:);箭袋(forwardMotion (: 1) forwardMotion(:, 2),因为(forwardMotion(:, 3)),罪(forwardMotion (:, 3)),.．.0.1,Color=[0.3 0.75 0.93]，LineWidth=1,DisplayName=“优化的前进路径”）;箭袋(reverseMotion (: 1) reverseMotion(:, 2),因为(reverseMotion(:, 3)),罪(reverseMotion (:, 3)),.．.0.1,Color=[0.85 0.33 0.1]，LineWidth=1,DisplayName=“优化逆向路径”）;传奇(位置=“东南”）;标题(“以前和更新的优化路径”)举行从

图中包含一个轴对象。标题为“Previous”和“Updated Optimized Path”的坐标轴对象包含图像、直线、散点、抖动等11种类型的对象。这些对象表示正向路径、反向路径、路径点、方向、起点、目标、先前优化的正向路径、先前优化的反向路径、优化的正向路径、优化的反向路径。

参考

罗斯曼，克里斯托夫，弗兰克·霍夫曼和托斯滕·伯特伦。类车机器人运动动力学轨迹优化与控制在2017 IEEE/RSJ智能机器人与系统国际会议(IROS), 5681 - 86。温哥华，BC省:IEEE, 2017。https://doi.org/10.1109/IROS.2017.8206458．