Policy Optimization to Learn Adaptive Motion Primitives in Path Planning with Dynamic Obstacles

Brian Angulo, Aleksandr Panov, Konstantin S. Yakovlev

Abstract

This paper addresses the kinodynamic motion planning for non-holonomic robots in dynamic environments with both static and dynamic obstacles—a challenging problem that lacks a universal solution. One promising approach is decomposing the problem into smaller sub-problems and combining local solutions into a global one. The crux of any planning method for non-holonomic robots is the generation of motion primitives that solve local planning sub-problems. In this work, we introduce a novel learnable steering function (policy) that accounts for the kinodynamic constraints of the robot and both static and dynamic obstacles. This policy is efficiently trained via policy optimization. Empirically, we show that our steering function generalizes well to unseen problems. We then integrate the trained policy into sampling-based and lattice-based planners, evaluating the resultant POLAMP algorithm (Policy Optimization that Learns Adaptive Motion Primitives) in challenging setups involving a car-like robot in obstacle-rich parking-lot environments. We demonstrate that POLAMP plans collision-free kinodynamic trajectories with success rates higher than 92% in environments with 50 simultaneously moving obstacles, outperforming state-of-the-art competitors.

Introduction

Kinodynamic motion planning for non-holonomic robots, such as autonomous vehicles, is a fundamental challenge in robotics. Planners must simultaneously respect physical constraints—like minimum turning radii and acceleration limits—while navigating environments where obstacles are not only static but also moving dynamically. Traditional lattice-based planners often rely on pre-computed motion primitives that are blind to dynamic threats, while sampling-based planners like RRT can be computationally inefficient when attempting to find feasible local connections in dense, moving traffic.

The POLAMP Framework

The core contribution of this work is the POLAMP (Policy Optimization that Learns Adaptive Motion Primitives) algorithm. POLAMP bridges the gap between classical global search and modern reinforcement learning by replacing fixed mathematical steering functions with a learned local policy.

Local Planner Architecture

The local planner acts as the "brain" of the system, trained using Proximal Policy Optimization (PPO). The network architecture processes a 39-beam 2D LiDAR scan providing 360° coverage alongside a 10-dimensional feature vector containing the robot's state (velocity, steering angle) and relative goal position. This information is passed through fully connected layers and an LSTM to produce continuous control actions: linear acceleration ($a$) and rotation rate ($\omega$).

The POLAMP neural network architecture, showing the fusion of LiDAR data and state features into an Actor-Critic model.

Training via Curriculum Learning

To ensure stable convergence, the policy is trained through a three-stage curriculum:

  1. Basic Physics: Learning to drive in an empty environment.
  2. Static Navigation: Learning to avoid fixed obstacles.
  3. Dynamic Avoidance: Learning to dodge adversarial moving agents.

Global Integration and Generalization

The learned local policy is integrated into classical global planners such as RRT or A*. In this hybrid setup, the global planner explores the workspace while the learned policy serves as the steering function to connect nodes. This ensures that the generated local paths are both kinodynamically feasible and reactive to the immediate surroundings.

Evaluation of POLAMP across three distinct, unseen parking lot maps, demonstrating the policy's ability to generalize to new geometries and high obstacle densities.

One of the most significant findings is the superior generalization of on-policy PPO compared to off-policy methods like DDPG. While previous RL-based planners often overfit to specific training geometries, POLAMP maintains high performance on entirely unseen maps. In environments populated with up to 50 dynamic obstacles, POLAMP achieved a success rate exceeding 92%. Furthermore, the "intelligence" of the local planner allows the global search to be far more efficient, requiring 12x to 14x fewer samples than non-learned baselines like RRT-ES.

Conclusion

POLAMP demonstrates that learned adaptive motion primitives can significantly enhance the robustness and efficiency of kinodynamic motion planning in complex, dynamic environments. By combining the strengths of global search with the reactive capabilities of deep reinforcement learning, the framework achieves high success rates where traditional methods struggle. Future work will focus on addressing sensor noise and the uncertainty of obstacle trajectory prediction to move closer to real-world autonomous driving deployments.

Summary updated

Abstract

This paper addresses the kinodynamic motion planning for non-holonomic robots in dynamic environments with both static and dynamic obstacles -- a challenging problem that lacks a universal solution yet. One of the promising approaches to solve it is decomposing the problem into the smaller sub problems and combining the local solutions into the global one. The crux of any planning method for non-holonomic robots is the generation of motion primitives that generates solutions to local planning sub-problems. In this work we introduce a novel learnable steering function (policy), which takes into account kinodynamic constraints of the robot and both static and dynamic obstacles. This policy is efficiently trained via the policy optimization. Empirically, we show that our steering function generalizes well to unseen problems. We then plug in the trained policy into the sampling-based and lattice-based planners, and evaluate the resultant POLAMP algorithm (Policy Optimization that Learns Adaptive Motion Primitives) in a range of challenging setups that involve a car-like robot operating in the obstacle-rich parking-lot environments. We show that POLAMP is able to plan collision-free kinodynamic trajectories with success rates higher than 92%, when 50 simultaneously moving obstacles populate the environment showing better performance than the state-of-the-art competitors.
BibTeX
@article{angulo2023policy,
  author = {Brian Angulo and Aleksandr Panov and Konstantin S. Yakovlev},
  title = {{Policy Optimization to Learn Adaptive Motion Primitives in Path Planning with Dynamic Obstacles}},
  journal = {IEEE Robotics Autom. Lett.},
  year = {2023},
  doi = {10.1109/lra.2022.3233261},
}
Figure