模型介绍

1. 模型概述：

格点规划（Lattice Planning）是一种常见的路径规划方法，主要用于生成多样化的可行路径供自动驾驶车辆选择。它通过在状态空间中进行离散化，将连续的路径问题转化为有限个格点之间的搜索问题。Lattice 规划器特别适合高维连续状态和动作空间的情况，广泛用于自动驾驶和机器人导航领域。

本项目的目标是通过基于格点的路径生成器，结合障碍物检测和轨迹优化，实现车辆的路径规划，使其能够安全、顺畅地通过复杂的交通环境。

（1）Lattice 规划：Lattice 规划是一种通过离散化状态空间和动作空间来生成路径的算法。该方法在车辆的行驶空间中放置一个网格，网格上的节点代表可能的路径点，通过连接这些节点来构建车辆的路径。具体来说，Lattice 规划器将状态空间（例如位置、速度、加速度）进行离散化，车辆可以在这些离散状态下做出动作决策。

格点规划的核心是构建一系列候选轨迹，并基于车辆的动态约束和环境信息（如障碍物）进行优化，最终选择最优的路径进行跟随。

（2）轨迹优化：在路径生成过程中，Lattice 规划器会产生多个候选轨迹。为确保车辆能够安全、平稳地行驶，需要对这些轨迹进行评估和优化。轨迹优化的目标是选择一条能够最大化车辆行驶稳定性，同时避开障碍物的路径。

在本项目中，轨迹优化主要通过以下步骤实现：

• 路径匹配：通过参考路径进行轨迹点的匹配，确保车辆能够有效跟随规划路径。
• 障碍物检测：利用传感器数据检测环境中的障碍物，并在路径规划过程中避免与其碰撞。
• 采样与扩展：对于未找到可行解的场景，算法会扩展采样范围，以确保找到安全的路径。

2. 具体实现：

• 初始化：
  • 从输入的路径文件中加载参考路径数据。
  • 初始化车辆的状态，包括位置、速度、加速度等信息。
  • 初始化障碍物信息，包含障碍物的位置、大小和朝向等。
• 轨迹点匹配：
  • 对车辆的当前位置与参考路径进行匹配，生成初始的轨迹点。
  •  根据匹配的路径点和障碍物信息，初始化轨迹采样基础。
• 路径规划：
  • 使用 LocalPlanner 进行局部路径规划，生成一系列可行的候选轨迹。
  • 对每个轨迹进行评估，判断其是否避开障碍物，并选择最优的轨迹进行跟随。
  • 若未找到可行路径，扩大采样范围并重新进行规划。
• 实时更新：
  • 在每个时间步，车辆根据当前状态更新轨迹，并实时进行路径调整。
  • 同时绘制车辆的当前路径、障碍物位置及优化后的轨迹，以便观察车辆的行驶过程。

3. 算法优化：

• 障碍物检测与路径避让：在检测到障碍物后，算法会通过调整轨迹点来规避潜在的碰撞风险。若无法找到安全路径，系统会进行采样扩展，增加搜索空间以找到安全轨迹。
• 动态采样：根据车辆当前的速度、角度和环境信息，动态调整轨迹的采样范围，确保车辆能够平稳、顺畅地行驶。

0. 准备工作：

• Python 3.x
• numpy：用于数值计算
• matplotlib：用于路径可视化
• 自定义类：
  • Obstacle：定义障碍物及其相关属性
  • TrajPoint：轨迹点类，用于表示当前的轨迹状态
  • SampleBasis：采样基础类，用于生成路径采样点
  • LocalPlanner：局部规划器，负责生成可行轨迹并选择最优路径
  • CalcRefLine`：计算车辆应跟随的参考线

1. 使用步骤：

（1）将路径数据文件 roadMap_lzjSouth1.txt放置于项目根目录中。该文件应包含路径的 X、Y 坐标。

（2）使用以下命令运行程序：

（3）程序将在终端中显示规划结果，并通过图形界面展示车辆的路径及障碍物情况。若未能找到可行路径，系统会自动扩展采样范围，尝试找到合适的轨迹。

2. 注意事项：

• 车辆的速度和角度阈值可以通过  v_tgt 和  theta_thr 参数进行调整，以适应不同的场景需求。
• 当前版本处理静态障碍物。对于动态障碍物或更复杂场景的处理，可通过扩展  LocalPlanner 的功能实现。
• 若路径规划频繁失败，可以调整采样范围及角度阈值，以获得更好的规划效果。