Deploying SICNav in the Field: Safe and Interactive Crowd Navigation using MPC and Bilevel Optimization

The autonomy stack integrates four core components on a Clearpath Jackal robot equipped with an Ouster OS1-128 LiDAR and an onboard computer running Ubuntu 20.04 and ROS 1 Noetic.

SLAM: 2D Google Cartographer processes LiDAR point clouds, IMU data, and wheel odometry to build and maintain a map of the environment. A novel 2D projection technique using the LiDAR's built-in IMU eliminates the need for a separate LiDAR-to-IMU calibration. Pose estimates are further filtered through an Extended Kalman Filter.

Human Detection & Tracking: YOLOv9 runs on the Ouster's intensity and reflectivity images — eliminating the need for a camera entirely. Detected contours are projected into 3D using the point cloud, and the aUToTrack method maintains consistent human identities across frames using Kalman Filters with a constant-acceleration motion model.

Global Planning: The ROS move_base framework generates a global occupancy map from LiDAR data, filtering out tracked humans so they are handled by SICNav rather than treated as static obstacles. A hybrid A* algorithm finds the global path.

SICNav Local Planner: SICNav replaces the default local planner in move_base. It solves a bilevel MPC problem that jointly optimizes the robot trajectory and predicts human motions via ORCA (Optimal Reciprocal Collision Avoidance), ensuring collision-free trajectories by construction. The MPC runs at 10 Hz with a 2-second prediction horizon.

The robot was deployed at the Technical University of Munich campus in Maxvorstadt, Germany, across three distinct environments:

Main Campus Area: An outdoor pedestrian zone with cobblestones, asphalt, and pavement. Contains light poles, benches, bicycle parking, and parked cars.

Sidewalk & Accessible Areas: Sidewalks adjacent to university buildings and city roads, including moving cars nearby.

Indoor Areas: Narrow hallways, staircases, and open spaces within two university buildings with stone flooring.

The robot navigated autonomously for 1 hour and 51 minutes, traversing 6.73 km across all three environments. The sidewalk area had the highest manual takeover frequency (0.0121 s⁻¹), primarily due to the system's inability to detect downward steps — a limitation identified for future work. Indoor environments had the fewest takeovers (0.0019 s⁻¹).

SICNav solve times remained well within the 0.25-second discretization period across all environments (indoor μ=0.102s, sidewalk μ=0.100s, campus μ=0.114s), confirming real-time feasibility even in crowded outdoor settings.

A simulation study evaluated robustness to localization and perception noise. SICNav was compared against SICNav-Diffusion (data-driven human intention prediction) and a non-interactive MPC baseline using Constant Velocity Motion Model (CVMM). Results showed that noise levels beyond σ=0.03 begin to narrow the performance gap between methods, motivating further study with per-component noise analysis in future work.