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

---

前言

• 来源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