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.
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.
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