FDSPC: Fast and Direct Smooth Path Planning via Continuous Curvature Integration

In recent decades, mobile robot motion planning has seen significant advancements, but mainstream path planning algorithms often ignore vertical obstacle traversability and require extensive post-processing for smoothness. This work proposes a fast, direct motion planning method based on continuous curvature integration that generates smooth paths directly with pseudo-constant velocity and limited curvature in complex 2.5-D terrain environments, eliminating the subsequent path smoothing process and enabling the robot to track the path generated directly.

How it works

The core of the FDSPC algorithm is a fast motion planning method based on continuous curvature integration that iteratively explores collision-free path segments satisfying G2 smoothness (curvature continuity) without requiring re-collision checking or explicit smoothing. The method operates by mapping curvature to path points using double integration on the 2-D plane and then adjusting the curvature to ensure collision avoidance, which guarantees G2 continuity for the generated path.

The algorithm's planning process is structured around two main parts:

1. Direct Planning (Alg. 1): This part checks if the current orientation can reach the target position directly; if not, it iteratively adjusts the current orientation using a curvature integral until the direction aligns with a calculated angle, updating the current position based on Eq. (3). This constructs a basic path structure by combining segments of indefinite radius arcs with straight lines.

2. Exploration Planning (Alg. 2): This part establishes connections between the current position and the endpoint along orientation using a straight line, calculates collision indices, and iteratively increases extension angles to explore around obstacles until the collision index becomes empty. It utilizes an ordered dictionary to store node information, allowing for a fast heuristic search of paths.

Curvature Planning in 2-D Plane

The representation of a path in 2-D space is linked to curvature through the relationship where the tangent angle θ can be obtained by integrating the curvature function κ and the pseudo velocity ds/dt. The model for curvature planning on the 2-D plane involves equations that relate the rate of change of curvature (κ˙) to its derivative with respect to time, and define how coordinates (x˙, y˙) are determined by the tangent angle θ. Collision avoidance is achieved by adjusting the curvature κ in Eq. (3), ensuring that every generated path segment possesses G2-continuity.

Curvature Planning in 2.5-D Terrain Space

For complex 3-D environments, FDSPC simplifies the problem to 2.5-D path planning by combining a continuous transformation scaling function τ in the z-direction with a 2-D plane planning, resulting in a path that satisfies G2 continuity. The curvature planning model in this space incorporates the scaling function τz and its rate of change ρz, alongside the standard 2-D curvature dynamics. When an obstacle is detected on the plane, FDSPC retreats a distance backobs and introduces a tilt along the z-axis. It then compares the (x, y) coordinates of the new collision point with the original to determine if it is crossable; if below a maximum tilt angle (θmax), it continues z-axis expansion until a collision-free node is found, or backtracks otherwise.

Velocity Planning

Since the path obtained by FDSPC possesses G2 continuity, corresponding velocity and acceleration functions can be derived based on the curvature κ or ρz to yield a smooth trajectory. The velocity planning function is defined piecewise: if the curvature κ is zero or ρz is zero, velocity changes according to vi-1 + a · dt; if not, it changes according to vi-1 - a · dt (if vi-1 < vmax); otherwise, the velocity remains vi-1. This curvature-based planning anticipates path changes in advance, adjusting the velocity timely to enhance comfort and stability without requiring complex numerical optimization.

Performance and Analysis

The performance of FDSPC is evaluated against state-of-the-art methods across five typical scenarios: Long Obstacle, Long Corridor, Semi-Enclosed, Random Complex, and Simple Maze. The comparison metrics include solution time (s), memory usage (MB), path length (m), S1 smoothness (deg/m), and S2 smoothness (deg). FDSPC demonstrated superior performance in terms of solution time on grid maps and excelled significantly in S2 smoothness, outperforming other algorithms by two orders of magnitude. Specifically, it ranks among the best for S1 smoothness, slightly outperforming θ∗ and lazy θ∗. The algorithm is most sensitive to parameter settings: the integration step size ∆t (recommended 0.01-0.015 s), the curvature variation rate ρ (recommended 0.3-0.5 m−1s−1), the angular increment θa1 (recommended 0.1-0.2 rad), and the linear extension length ladd (recommended 0.4-0.8 m).

Here are the specific improvements to AI systems that can be derived from this research, along with what those improved systems could achieve:

1. The proposed FDSPC (Fast and Direct Smooth Motion Planning) algorithm based on continuous curvature integration provides a method for generating globally smooth paths directly in 2.5-D terrain environments without needing post-processing smoothing steps.

2. The core improvement lies in replacing traditional sampling-based or search-based methods (like RRT, AHP) that yield non-continuous, zigzag paths with a curvature planning approach that inherently guarantees G2 continuity (curvature continuity).

3. The system can perform curvature-based speed planning directly within complex 2.5-D terrain, accounting for the up and down of the terrain (via the scaling function τ in Eq. 4) and obstacle crossing ability by dynamically adjusting vertical movement (z-axis expansion/tilt).

These improvements enable the following capabilities for AI systems:

1. The improved system can execute high-speed, kinematically feasible path planning for mobile robots operating on complex, non-flat surfaces (e.g., uneven terrain, ramps, and obstacles in 2.5-D space).

2. It allows robots to generate trajectories that are inherently smooth (G2 continuous), significantly reducing mechanical stress on the robot's actuators and improving ride comfort compared to paths requiring external smoothing post-generation.

3. The system can efficiently navigate complex environments (mazes, corridors, semi-enclosed areas) by intelligently exploring branching possibilities using a binary tree structure combined with an ordered dictionary heuristic search, ensuring rapid solution times while maintaining path feasibility.

4. The AI can perform direct trajectory tracking by generating velocity and acceleration functions derived directly from the curvature profiles or vertical scaling function (ρz), allowing for anticipatory control adjustments based on terrain changes, leading to enhanced stability during traversal of inclines and declines.

5. The system is optimized for efficiency in practical applications; while it requires many integrations, its structure allows for potential optimization using real-time sensor data (LiDAR/depth cameras) to reduce computational load and memory usage compared to purely abstract planning methods.