SAP-CoPE: Social-Aware Planning using Cooperative Pose Estimation with Infrastructure Sensor Nodes

Autonomous driving systems must operate smoothly in human-populated indoor environments, where challenges arise including limited perception and occlusions when relying only on onboard sensors, as well as the need for socially compliant motion planning that accounts for human psychological comfort zones. These factors complicate accurate recognition of human intentions and the generation of comfortable, socially aware trajectories. To address these challenges, we propose SAP-CoPE, an indoor navigation system that integrates cooperative infrastructure with a novel 3D human pose estimation method and a socially-aware model predictive control (MPC)-based motion planner. In the perception module, an optimization problem is formulated to account for uncertainty propagation in the camera projection matrix while enforcing human joint coherence. The proposed method is adaptable to both single- and multi-camera configurations and can incorporate sparse LiDAR point-cloud data. For motion planning, we integrate a psychology inspired personal-space field using the information from estimated human poses into an MPC framework to enhance socially comfort in human-populated environments. Extensive real-world evaluations demonstrate the effectiveness of the proposed approach in generating socially aware trajectories for autonomous systems.