Technical Summary: SAP-CoPE

Problem Statement

Autonomous mobile robots operating in crowded indoor environments (e.g., hospitals, shopping malls) face two primary challenges that hinder safe and efficient navigation:

1. Perception Limitations: Relying solely on onboard sensors leads to limited fields of view (FOV) and frequent occlusions, making it difficult to accurately detect human positions and predict intentions in real-time.
2. Social Compliance: Traditional motion planning algorithms often prioritize physical collision avoidance but neglect human psychological comfort zones. This can result in robots invading personal space, causing discomfort, or behaving inefficiently (e.g., unnecessary stopping) in dense crowds.

Existing infrastructure-based systems often lack the ability to estimate accurate 3D human poses and headings using cost-effective sensors, while social planning methods frequently assume symmetric human influence or ignore the specific kinematic constraints of omnidirectional robots.

Methodology

The paper proposes SAP-CoPE (Social-Aware Planning using Cooperative Pose Estimation with Infrastructure Sensor Nodes), a framework integrating cooperative infrastructure sensing with socially aware motion planning. The system architecture consists of three layers:

• Intelligent Sensor Nodes (ISNs): Ceiling-mounted nodes equipped with cameras, LiDAR, and edge computing. They perform local 2D detection and 3D pose estimation.
• Central Layer (CL): Aggregates data from ISNs for global fusion, multi-object tracking, delay compensation, and motion planning.
• Application Layer (AL): Executes control commands on the robot (specifically an autonomous medical bed in this study).

1. Cooperative Perception Module

The perception system addresses occlusion and depth ambiguity through a novel 3D human pose estimation approach:

• Sensor Fusion: It combines 2D camera detections (using a fine-tuned YOLOv11) with sparse LiDAR point clouds.
• Uncertainty-Aware Estimation: The method formulates an optimization problem that propagates uncertainty from the camera projection matrix to the world coordinates. It enforces joint coherence by incorporating prior knowledge of bone lengths and their uncertainties.
• Probabilistic Modeling: Human joints are modeled as multivariate Gaussian distributions. A Maximum Likelihood Estimation (MLE) problem refines the 3D pose by integrating joint coherence constraints and optional LiDAR measurements.
• Delay Compensation: To handle communication and computation latencies, the system employs timestamp-based delay compensation and motion prediction (using a Constant Velocity and Turning Rate model) to align stale data with the current system state.

2. Socially-Aware Motion Planning

The motion planner utilizes Model Predictive Control (MPC) to generate trajectories that respect human personal space while adhering to the robot's kinematic constraints.

• Personal Space (PS) Field: Inspired by psychological studies (Hall, Sommer), the system models human personal space as an asymmetric field. The field is larger in front of a human than behind them, reflecting that humans are more sensitive to disturbances from the front. This contrasts with symmetric Artificial Potential Fields (APF) or Social Force Models (SFM).
• MPC Formulation: The objective function minimizes three terms:
  1. Social Cost ($J_1$): A penalty for entering the asymmetric PS field of humans.
  2. Tracking Error ($J_2$): Deviation from a reference trajectory.
  3. Control Effort ($J_3$): Deviation of wheel velocities and steering angles from references.
• Kinematic Constraints: The planner explicitly models the omnidirectional capabilities of the robot (independent front/rear wheel steering and speed) and includes linearized constraints to prevent wheel slippage.

Key Contributions

1. Real-time Cooperative Perception: A framework leveraging ceiling-mounted ISNs to provide occlusion-robust, expanded FOV perception.
2. Novel 3D Human Pose Detection: A method combining geometry-based techniques, sensor fusion (image + sparse LiDAR), and joint coherence constraints to achieve accurate 3D pose estimation with uncertainty propagation.
3. Socially-Aware MPC Planner: An algorithm that integrates an asymmetric Personal Space field into an MPC framework. It accounts for the omnidirectional kinematics of the robot and the psychological comfort zones of humans, enabling smooth navigation in dense crowds.
4. Real-World Validation: Extensive experiments demonstrating the system's effectiveness in pose detection and human-robot interaction compared to baseline methods.

Experimental Results

The system was evaluated in a hallway environment with varying numbers of human participants (2, 4, and 6).

• Perception Performance:
  • The cooperative Cam+LiDAR approach achieved the lowest position error (MAE: 0.0771 m) and yaw error (MAE: 6.86°), outperforming learning-based baselines (3DNet, Metrabs) and single-node methods.
  • The delay-aware pipeline significantly mitigated the impact of communication delays (up to 200 ms), maintaining higher recall and lower displacement/angular errors compared to a baseline without delay compensation.
• Motion Planning Performance:
  • Clearance: The proposed method maintained a larger and more consistent clearance from humans compared to the Social Force Model (SFM) and MPC with Potential Field (MPC+PF), especially as crowd density increased.
  • Efficiency: The proposed method achieved the shortest travel times across all scenarios. In contrast, the MPC+PF method exhibited longer travel times in crowded scenarios due to its conservative, symmetric field causing unnecessary deceleration.
  • Real-time Feasibility: Computation times for the planner (including perception) remained well below the 100 ms control loop period (median ~11–19 ms depending on obstacle count), confirming real-time capability.

Significance and Claims

The authors claim that SAP-CoPE offers a deployable framework for indoor autonomous systems that balances social comfort, efficiency, and safety.

• By integrating infrastructure sensing, the system overcomes the limitations of onboard sensors regarding occlusion and FOV.
• The asymmetric Personal Space model allows the robot to navigate more naturally and efficiently than symmetric models, avoiding the "trapped" behavior often seen in dense crowds.
• The explicit consideration of omnidirectional kinematic constraints ensures that the generated trajectories are physically feasible for the specific robot platform.
• The modular architecture is compatible with existing indoor robotic platforms using standard hardware, suggesting strong potential for deployment in healthcare logistics and public service robots.

The paper concludes that while the current system is effective, future work will focus on enhancing robustness in highly dynamic environments and improving human intention prediction through learning-based algorithms.