Towards Using Fast Embedded Model Predictive Control for Human-Aware Predictive Robot Navigation

Till Hielscher, Lukas Heuer, Frederik Wulle, Luigi PalmieriarXiv 2024 · DOI · arXiv

Abstract

Navigating mobile robots through crowded human environments requires real-time prediction and high-speed optimization. This paper presents HuMAN-MPC, a high-performance navigation system that achieves sub-5ms computation times for tracking up to 30 humans simultaneously. By utilizing a mathematically optimized $C^2$ continuous cost function and the acados embedded optimization framework, the system provides smooth, legible, and safe robot motion in dynamic settings.

Introduction: The Challenge of Crowded Navigation

Navigating a robot through a crowd involves more than simple obstacle avoidance; it requires anticipating human movement over a future time horizon. While Model Predictive Control (MPC) is ideal for this task, it often suffers from high computational costs as the number of agents increases—a phenomenon known as the "curse of dimensionality." Traditional solvers frequently struggle to find solutions quickly enough for real-time control, leading to jerky or unsafe robot behavior.

Methodology: HuMAN-MPC

The HuMAN-MPC system addresses these challenges through three primary innovations:

1. Fast Embedded Optimization

The system leverages acados, a software package tailored for fast embedded optimal control. It employs a Sequential Quadratic Programming (SQP) scheme with Real-Time Iterations (RTI). This allows the robot to "warm-start" calculations using previous solutions, significantly reducing the time required for each control loop.

2. A Solver-Friendly Cost Function

To ensure compatibility with fast solvers like acados, the cost function must be twice-continuously differentiable ($C^2$). The authors developed a novel cost function based on a logistic "potential field" around humans. This smoothness ensures the optimizer can efficiently find feasible, collision-free paths while maintaining a positive semi-definite Hessian.

Conceptual illustration of the HuMAN-MPC navigation strategy, showing distance thresholds and predicted human paths.

3. Safety Monitoring

The system integrates monitors compliant with ISO safety norms (e.g., DIN EN ISO 3691-4). If the optimization fails to converge or exceeds time limits, a "protective stop" is triggered to ensure human safety.

Evaluation and Results

The system was evaluated in both Gazebo simulations with up to 30 humans and real-world tests using the DARKO robot platform, a large-scale mobile manipulator.

The DARKO robot platform (left) and its real-time perception and planning visualization (right) during navigation tasks.

Key Performance Metrics

  • Computational Efficiency: Mean iteration times were approximately 2.47 ms for 5 humans and remained under 5 ms for 30 humans. This performance allows for control loops exceeding 100 Hz.
  • Solver Comparison: The acados-based approach proved to be roughly 60 times faster than the standard IPOPT interior-point solver while maintaining equivalent path quality.
  • Path Quality: Compared to the non-predictive Dynamic Window Approach (DWB), HuMAN-MPC produced smoother trajectories and avoided "freezing" in complex dynamic scenarios by looking up to 5 seconds into the future.

Conclusion and Future Work

HuMAN-MPC demonstrates that high-speed, predictive navigation in dense crowds is achievable by prioritizing mathematical compatibility between cost functions and embedded solvers. Future work will focus on improving human motion prediction by incorporating contextual environmental data and activity recognition, moving beyond the current constant-velocity assumption to better handle complex multi-agent interactions.

Summary updated

Abstract

Predictive planning is a key capability for robots to efficiently and safely navigate populated environments. Particularly in densely crowded scenes, with uncertain human motion predictions, predictive path planning, and control can become expensive to compute in real time due to the curse of dimensionality. With the goal of achieving pro-active and legible robot motion in shared environments, in this paper we present HuMAN-MPC, a computationally efficient algorithm for Human Motion Aware Navigation using fast embedded Model Predictive Control. The approach consists of a novel model predictive control (MPC) formulation that leverages a fast state-of-the-art optimization backend based on a sequential quadratic programming real-time iteration scheme while also providing feasibility monitoring. Our experiments, in simulation and on a fully integrated ROS-based platform, show that the approach achieves great scalability with fast computation times without penalizing path quality and efficiency of the resulting avoidance behavior.
BibTeX
@misc{hielscher2024towards,
  author = {Till Hielscher and Lukas Heuer and Frederik Wulle and Luigi Palmieri},
  title = {{Towards Using Fast Embedded Model Predictive Control for Human-Aware Predictive Robot Navigation}},
  year = {2024},
  eprint = {2405.12616},
  archivePrefix = {arXiv},
  url = {https://arxiv.org/abs/2405.12616},
}
Figure