Given belowModel Overview, click on the right to run the code online.

Model Overview

1. Model overview:

Lattice Planning is a common path-planning method used to generate diverse feasible paths for autonomous vehicles. It discretizes the state space and turns a continuous path-planning problem into a search between a finite set of lattice nodes. Lattice planners are particularly suitable for high-dimensional continuous state and action spaces and are widely used in autonomous driving and robot navigation.

This project aims to combine a lattice-based path generator with obstacle detection and trajectory optimization to plan vehicle paths that pass safely and smoothly through complex traffic environments.

（1）Lattice planning: Lattice planning is an algorithm for generating paths by discretizing state space and action space. This method places a grid in the vehicle's driving space. The nodes on the grid represent possible path points, and the vehicle's path is constructed by connecting these nodes. Specifically, the Lattice planner discretizes the state space (e.g., position, velocity, acceleration) within which the vehicle can make action decisions.

The core of Lattice Planning is to construct a set of candidate trajectories, optimize them against vehicle dynamic constraints and environmental information such as obstacles, and select the best path to follow.

（2）Trajectory optimization: During the path generation process, the Lattice planner generates multiple candidate trajectories. To ensure that the vehicle can drive safely and smoothly, these trajectories need to be evaluated and optimized. The goal of trajectory optimization is to select a path that maximizes vehicle driving stability while avoiding obstacles.

In this project, trajectory optimization is mainly implemented through the following steps:

• path matching: Match the trajectory points through the reference path to ensure that the vehicle can effectively follow the planned path.
• Obstacle detection: Use sensor data to detect obstacles in the environment and avoid collisions with them during path planning.
• Sampling and Expansion: For scenarios where no feasible solution is found, the algorithm will expand the sampling range to ensure a safe path is found.

2. Implementation:

• Initialization:
  • Load reference-path data from the input path file.
  • Initialize the vehicle state, including position, velocity, acceleration, and other information.
  • Initialize obstacle information, including obstacle positions, sizes, and headings.
• Trajectory-point matching:
  • Match the vehicle’s current position with the reference path to generate initial trajectory points.
  •  Initialize the trajectory sampling basis from the matched path points and obstacle information.
• Path Planning:
  • Use LocalPlanner for local path planning to generate a set of feasible candidate trajectories.
  • Evaluate each trajectory to determine whether it avoids obstacles, then select the best trajectory to follow.
  • If no feasible path is found, expand the sampling range and plan again.
• Real-time updates:
  • At each time step, update the trajectory based on the vehicle’s current state and adjust the path in real time.
  • Also plot the vehicle’s current path, obstacle positions, and optimized trajectory to observe the driving process.

3. Algorithm Optimization:

• Obstacle detection and path avoidance: after detecting an obstacle, the algorithm adjusts trajectory points to avoid potential collisions. If no safe path is found, it expands sampling to enlarge the search space and find a safe trajectory.
• Dynamic sampling: dynamically adjust the trajectory sampling range according to the vehicle’s current speed, heading, and environmental information to ensure smooth driving.

0. Preparation:

• Python 3.x
• numpy: for numerical calculations
• matplotlib: for path visualization
• Custom classes:
  • Obstacle: Define obstacles and their related properties
  • TrajPoint: trajectory point class, used to represent the current trajectory status
  • SampleBasis: sampling base class, used to generate path sampling points
  • LocalPlanner: Local planner, responsible for generating feasible trajectories and selecting optimal paths
  • CalcRefLine`: Calculates the reference line the vehicle should follow

1. Steps:

(1) Place the path data file roadMap_lzjSouth1.txt in the project root directory. The file should contain the path’s X and Y coordinates.

(2) Run the program using the following command:

(3) The program displays planning results in the terminal and shows the vehicle path and obstacles graphically. If no feasible path is found, it automatically expands the sampling range and attempts to find a suitable trajectory.

2. Notes:

• The vehicle speed and heading thresholds can be adjusted through the v_tgt and theta_thr parameters to suit different scenarios.
• The current version handles static obstacles. Dynamic obstacles or more complex scenarios can be addressed by extending LocalPlanner.
• If path planning fails frequently, adjust the sampling range and heading threshold for better planning performance.