Speed Profile Generation: The Constraint-Driven Approach

In this guide, we delve into the creation of speed profiles that scrupulously adhere to predefined physical limits. The conventional approach often involves formulating this as an optimization problem. Given a set of waypoints and a constant sampling distance $\Delta s$, the problem can be stated as:

Where:

• $v(i)$ is the speed at point $i$
• $N$ is the total number of points
• $v_{\text{max}}$ is the maximum speed
• $a_{y,\text{max}}$ is the maximum lateral acceleration
• $\kappa[i]$ is the curvature at point $i$
• $a_{x,\text{max}}$ is the maximum forward acceleration
• $a_{x,\text{brake}}$ is the maximum braking deceleration
• $\Delta s$ is the constant sampling distance

Instead of using iterative optimization algorithms, like gradient descent or nonlinear programming, this article introduces a streamlined procedural method to solve this problem.

Why Speed Profiles Matter
In autonomous systems ranging from self-driving cars to warehouse robots, generating velocity plans that respect physical limits is crucial for safety and efficiency. This guide reveals a battle-tested method that enforces three critical constraints in just 3 computational passes.

1. Lateral Constraint Pass

The first step ensures that the vehicle's speed is limited by the maximum lateral acceleration it can handle while following a curved path. This is based on the relationship between speed, curvature, and lateral acceleration.

Mathematical Explanation:

The lateral acceleration $a_y$ of a vehicle moving along a curved path is given by:

Where:

• $v$ = speed of the vehicle (m/s)
• $R$ = radius of curvature (m)

Since curvature $\kappa$ is the inverse of the radius ($\kappa = \frac{1}{R}$), we can rewrite the equation as:

To ensure the lateral acceleration does not exceed the maximum allowable lateral acceleration $a_{y,\text{max}}$, the speed $v$ must satisfy:

If the curvature $\kappa = 0$ (straight path), the speed is limited only by the maximum speed $v_{\text{max}}$.

Implementation:

• For each point along the path, calculate the speed limit based on the curvature:
  $$v_{\text{lat}}[i] = \min\left(\sqrt{\frac{a_{y,\text{max}}}{\kappa[i]}}, v_{\text{max}}\right)$$

2. Forward Pass (Acceleration Constraint)

The second step ensures that the vehicle does not exceed its maximum forward acceleration while speeding up. This is done by propagating the speed forward along the path, ensuring that the acceleration constraint is satisfied.

Mathematical Explanation:

The relationship between speed, acceleration, and distance is given by the kinematic equation:

Where:

• $v$ = final speed (m/s)
• $v_0$ = initial speed (m/s)
• $a$ = acceleration (m/s²)
• $\Delta s$ = distance traveled (m)

To ensure the vehicle does not exceed the maximum forward acceleration $a_{x,\text{max}}$, the speed at each point must satisfy:

Implementation:

• Start with the initial speed $v[0] = v_{\text{lat}}[0]$.
• For each subsequent point $i$, calculate the speed:
  $$v_{\text{forward}}[i] = \min\left(v_{\text{lat}}[i], \sqrt{v_{\text{forward}}[i-1]^2 + 2 \cdot a_{x,\text{max}}} \cdot \Delta s\right)$$

3. Backward Pass (Deceleration Constraint)

The third step ensures that the vehicle can safely decelerate to a stop or lower speed without exceeding its maximum braking deceleration. This is done by propagating the speed backward along the path.

Mathematical Explanation:

Using the same kinematic equation as above, but solving for the initial speed $v_0$:

To ensure the vehicle does not exceed the maximum braking deceleration $a_{x,\text{brake}}$, the speed at each point must satisfy:

Implementation:

• Start with the final speed $v_{\text{backward}}[N-1] = v_{\text{forward}}[N-1]$, where $N$ is the total number of points.
• For each preceding point $i$, calculate the speed:
  $$v_{\text{backward}}[i] = \min\left(v_{\text{forward}}[i], \sqrt{v_{\text{backward}}[i+1]^2 + 2 \cdot |a_{x,\text{brake}}}| \cdot \Delta s\right)$$

Why Avoid Traditional Optimization? The Procedural Advantage

Traditional optimization methods often rely on iterative solvers that can be computationally expensive and time-consuming. Usually there are not guarantees of convergence. By contrast, the constraint-driven approach outlined here offers a fast and efficient alternative that guarantees adherence to physical constraints.

Conventional Approach: Most speed profile methods formulate this as an optimization problem:

• Objective Function: Typically a cost function that balances speed, acceleration, and jerk
• Constraints: Physical constraints such as maximum speed, acceleration, and jerk
• Solver: Iterative optimization algorithms like gradient descent or nonlinear programming

Constraint-Driven Approach: The proposed method enforces constraints directly, avoiding the need for iterative optimization:

• O(N): The algorithm complexity is linear with the number of points along the path
• Guaranteed Convergence: The algorithm guarantees adherence to physical constraints

Additional Considerations

While this guide covers the fundamental constraints of lateral acceleration, forward acceleration, and braking deceleration, additional factors can be incorporated to refine the speed profile further:

• Maximum Jerk: Jerk, the rate of change of acceleration, can be limited to improve ride comfort and reduce wear on mechanical components. This can be implemented by constraining the difference in acceleration between consecutive points.

• Power Constraints: The power capabilities of the vehicle can impose additional constraints on the speed profile. These constraints can be translated into acceleration limits (as function of actual accelration and speed), ensuring that the vehicle operates within its power limits.

• Dynamic Acceleration Limits: In real-world scenarios, acceleration limits can vary based on factors such as road conditions, payload, speed, and battery charge. By incorporating these dynamic constraints, the speed profile can be optimized for specific operating conditions.