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

arXiv:2504.05727 · cs.RO · Submitted 2025-04-08 · Read on arXiv

Listen

Radio episode about this paper

Transcript

Introduction to the show: ident: Robotics Radio. Generated commentary on the latest robotics and control papers.

Rosa: I'm Rosa, and with me are Dev and Taro, guest researcher.

Dev: Today's paper: "SAP-CoPE: Social-Aware Planning using Cooperative Pose Estimation with Infrastructure Sensor Nodes".

Rosa: The gist 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,

Dev: First, who's behind it and why it matters.

Title and authors: Rosa: This paper introduces the SAP-CoPE framework, which basically builds this indoor navigation system by combining cooperative infrastructure with a novel three dee human pose estimation method and an MPC-based motion planner <ref:2504.05727#pg1,cooperative infrastructure with a novel 3D human pose estimation method and>. It’s all about making sure the robot can navigate crowded spaces without just relying on its own sensors. Dev The title highlights that it’s cooperative perception combined with social-aware planning, which means they aren't just looking at where the robot is; they are trying to understand what people want to do and how those movements affect their comfort.

Taro: From my side, I see the main implication here being how this changes human intention recognition in crowded settings. If a robot can actually figure out a person's pose accurately using infrastructure nodes, that’s a big step toward safer interaction because you aren't just reacting to where they are physically present.

Rosa: Exactly. The system uses these ceiling-mounted sensor nodes to give the robot expanded and occlusion-robust perception, which is crucial when people block the direct line of sight from onboard sensors. Dev And that robust perception feeds into the motion planner, which is what handles all that socially acceptable motion planning we talked about earlier.

The paper's summary: Rosa: So, to summarize SAP-CoPE, they have this real-time cooperative perception framework using sensor node networks to get a comprehensive environmental awareness. This involves multi-modal ISNs with LiDAR and cameras for local detection and three dee human pose estimation <ref:2504.05727#pg1>. Dev And then they have this synchronized clock system that aligns the LiDAR scans with the camera operations so the data is temporally consistent before it gets fused in a Central Layer for tracking and delay compensation.

Taro: The core technical achievement here, according to the summary, is developing a novel approach for three dee human pose detection that combines geometry-based techniques and sensor fusion between image data and sparse point clouds <ref:2504.05727#pg1>. That’s how they aim to enhance detection accuracy in those challenging indoor scenarios.

Rosa: Right. They also address the issue of delays by using a delay-aware cooperative perception pipeline, which includes global fusion across nodes and multi-object tracking with an extended Kalman filter for each object's state over time. Dev That tracking system then gets refined by fusing multi-view pose constraints at the Central Layer to make the pose estimation more robust against occlusions and partial views.

Taro: I think that delay compensation is key because in a real indoor setting, communication lag is inevitable, and having a mechanism to extrapolate stale data using motion prediction models sounds like it’s addressing one of the biggest practical hurdles for deploying this indoors.

The paper's improvements: Rosa: The authors suggest several key improvements to SAP-CoPE. One major point is moving away from planning methods that assume a symmetric influence, which is what they see in older Social Force Model approaches. Dev They show that those models can lead to unexpected deceleration or stopping when things get crowded, which reduces efficiency and increases collision risk.

Taro: So the improvement here is integrating the personal space field into an MPC controller that uses an asymmetric shaped field, intentionally nonuniform in the y-direction and concentrated along the human’s facing orientation. That reflects natural psychological comfort zones better than older methods.

Rosa: That asymmetry is designed to reflect how people are more sensitive to being disturbed from the front, which is a specific observation from human interactions. Dev They formulate this personal space field by defining regions like public distance, social distance, personal distance, and intimate distance using 2D Gaussian functions based on covariance and relative distance <ref:2504.05727#pg2>.

Taro: That’s interesting because it’s not just a simple circle around the person; they are modeling different levels of social interaction zones. The MPC objective function then minimizes a cost function that penalizes entering those personal spaces, J1, while also keeping tracking error low and respecting kinematic limits.

Conclusion: Rosa: So, to wrap up SAP-CoPE, the system uses a cooperative infrastructure sensor network and a novel three dee human pose estimation method to handle perception limitations and occlusion challenges <ref:2504.05727#pg1,a novel 3D human pose estimation method>. Dev And they integrate this with an MPC controller using a human-pose based personal space field to generate trajectories that prioritize human comfort while respecting robot constraints.

Taro: What I think is the real impact is how effectively it maintains proper clearance even when the number of participants increases, unlike conventional systems where clearance tends to decrease.

Rosa: That’s a key experimental finding they highlight. It maintains appropriate clearance to participants, reasonable total travelled distance, and shortest travelled time in their tests. Dev And from an engineering standpoint, they confirm that the system runs well within the required loop periods of one hundred ms for real-time feasibility.

Taro: If you’re listening, this paper on SAP-CoPE shows how to build a system that goes beyond just avoiding collisions; it aims to generate motion that is actually comfortable for people in shared spaces.

Rosa: That’s the big picture. So, we've seen how they tackle perception, planning, and social comfort with this framework called SAP-CoPE. We’re going to take a quick break and then talk about some other work in the area.

University of Waterloo Department of Mechanical and Mechatronics Engineering Department of Data and Systems Engineering University of Hong Kong Mechanical Engineering Department University of Alberta

cs.RO

Submitted: 2025-04-08

Updated: 2026-10-08

Comments: This paper has been accepted for publication in IEEE Transactions on Automation Science and Engineering (T-ASE)

License: http://creativecommons.org/licenses/by/4.0/

Importance score: 84/100

The gist: The gist Autonomous driving systems must operate smoothly in human-populated indoor environments, where challenges arise including limited perception and occlusions when relying only on onboard

Key concepts

Cooperative Infrastructure Sensing
This involves using a network of sensors mounted on the environment, like ceiling nodes, instead of relying solely on the robot's own limited sensors. These nodes work together to provide a more complete and reliable view of the indoor environment, helping to overcome limitations caused by occlusions or poor onboard sensing.
3D Human Pose Estimation
This technique uses camera observations to estimate the three-dimensional position and orientation of a human's joints. It first finds 2D joint coordinates on the camera image and then uses prior knowledge about human height distribution to create a more accurate, coherent 3D pose representation in world coordinates.
Personal Space (PS) Field
This is a psychological concept modeled as an asymmetric field around each person. The planner uses this field, which defines different zones like public, social, and personal distance, to calculate a social cost. This ensures the robot plans paths that maintain appropriate psychological comfort distances from humans.

Terminology

Summary

The gist 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<ref:2504.05727#pg2>.

How it works

The proposed SAP-CoPE framework integrates cooperative infrastructure with a novel 3D human pose estimation method and a socially-aware model predictive control (MPC)–based motion planner<ref:2504.05727#pg3>. This system addresses the challenges of limited perception and occlusions in indoor environments by leveraging ceiling-mounted sensor nodes to provide expanded and occlusion-robust perception through uncertainty-aware 3D human pose estimation from cameras, optionally supplemented by sparse LiDAR data<ref:2504.05727#pg3>.

The architecture is modular and compatible with existing indoor robotic platforms using standard hardware<ref:2504.05727#pg3>. Ceiling-mounted sensor nodes provide expanded and occlusion-robust perception through uncertainty-aware 3D human pose estimation from cameras, optionally supplemented by sparse LiDAR data<ref:2504.05727#pg3>. Leveraging these pose estimates, an MPC-based planner incorporating a human personal space field generates trajectories that balance social comfort, efficiency, and safety while respecting the robot’s kinematic constraints<ref:2504.05727#pg3>.

Perception System

The perception system consists of a real-time cooperative perception framework that leverages sensor node networks to provide comprehensive environmental awareness<ref:2504.05727#pg2>. This framework employs a group of multi-modal ISNs equipped with LiDAR, cameras, and edge computing for local object detection and 3D human pose estimation<ref:2504.05727#pg2>. A synchronized clock system aligns LiDAR scans with camera operations, ensuring temporally consistent data, which is transmitted to a Central Layer (CL) for global fusion, multi-object tracking, and delay compensation<ref:2504.05727#pg2>.

Local perception involves camera-based 2D detection using fine-tuned YOLOv11 models combined with LiDAR point cloud processing through region-of-interest filtering and hierarchical clustering<ref:2504.05727#pg4>. The novel proposed 3D human pose estimation method first obtains the 2D pixel coordinates of each joint and leverages prior height distribution information to estimate the rough 3D distribution of each joint as P i ∼ N(µP i, ΣP i) in the absolute world coordinate frame<ref:2504.05727#pg5>. This estimation is performed using the camera projection matrix H, ensuring a more robust and coherent 3D pose representation<ref:2504.05727#pg6>.

Delay-aware Global Perception

To address inconsistencies introduced by computational and communication delays, a delay-aware cooperative perception pipeline is employed<ref:2504.05727#pg7>. This pipeline contains three key components: global fusion across sensor nodes, multi-object tracking and delay compensation<ref:2504.05727#pg7>. The CL refines the 3D human pose estimation via Eqn. 6 at the CL by fusing multi-view pose constraints from different ISNs to enhance robustness against occlusions and partial views<ref:2504.05727#pg7>. Furthermore, it performs multi-object tracking using an extended Kalman filter (EKF) for each detected object to keep track of their states over time<ref:2504.05727#pg7>. The system incorporates timestamp-based delay compensation, allowing the CL module to compute the delay relative to its current time and extrapolate stale data forward using motion prediction models<ref:2504.05727#pg7>.

Socially-aware Motion Planning

The motion planning system is an MPC–based planner designed to generate optimal poses and implemented in the CL<ref:2504.05727#pg2>. The MPC’s objective function incorporates a personal space (PS) field inspired from psychology studies, serving as a penalty as robot enters human’s personal space, where Nhuman denotes the number of humans in the environment and P S(q) represents the social cost associated with the qth human<ref:2504.05727#pg2>. The formulation of the PS field is adapted to model human behaviour in interaction with the robot, dividing it into four regions: public distance, social distance, personal distance, and intimate distance<ref:2504.05727#pg7>.

The proposed PS field adopts an asymmetric shaped field proposed in [34], where the field is intentionally shaped to be nonuniform in the y-direction and concentrated along the human’s facing orientation<ref:2504.05727#pg7>. This design reflects natural psychological comfort zones observed in human interactions, where individuals are more sensitive to being disturbed from the front<ref:2504.05727#pg7>. The formulation of the PS field involves defining omegapq = γomegapq,f + (1 − γ)omegapq,r where γ is a function of xp<ref:2504.05727#pg7>. Each front and rear personal space omegapq,i, i ∈ f, r is defined as a 2D Gaussian function using the covariance matrix Σ and relative distance d<ref:2504.05727#pg7>.

The MPC optimization problem minimizes the cost function J = J1 + J2 + J3<ref:2504.05727#pg8>. The term J1 serves as a penalty for entering human’s personal space, while J2 penalizes tracking error between the predicted output η and ηRef throughout the prediction horizon Np<ref:2504.05727#pg8>. Constraints are imposed on both the control increment and the absolute control input to ensure feasibility and respect kinematic limits<ref:2504.05727#pg8>.

Experimental Validation

Extensive real-world experiments demonstrate the effectiveness of the proposed approach in generating socially aware trajectories for autonomous systems<ref:2504.05727#pg3>. Perception results show that integrating LiDAR measurements consistently improves localization accuracy over camera-only methods, with Cam+LiDAR (coop) achieving the lowest error of 0.0771 m MAE and 0.0862 m RMSE<ref:2504.05727#pg5>. Cooperative multi-node perception reduces occlusion and improves both position and heading estimation through multi-view redundancy, reducing yaw MAE to 6.86◦ for Cam+LiDAR (coop)<ref:2504.05727#pg6>.

The delay-aware pipeline preserves stable perception quality and consistently outperforms the baseline, yielding higher recall as well as lower displacement error (Avg. DE) and angular error (Avg. AE) across all simulated communication delays<ref:2504.05727#pg7>. The proposed planner method achieved best overall performance than other methods in general, maintaining an appropriate clearance to participants, a reasonable total travelled distance and the shortest travelled time through all experiments<ref:2504.05727#pg10>. The proposed method can maintain a proper clearance even as the number of participants increases, unlike conventional SFM which shows decreasing clearance<ref:2504.05727#pg10>.

The system maintains computation times well below the MPC loop periods (100 ms in our case), confirming real-time feasibility for autonomous navigation systems<ref:2504.05727#pg10>. The SAP-CoPE framework has strong potential for deployment in practical applications such as indoor service robots, healthcare logistics, and autonomous navigation in shared public spaces<ref:2504.05727#pg8>.

Conclusion

In this paper, we propose a SAP-CoPE framework for autonomous driving systems operating in human-populated environments<ref:2504.05727#pg3>. By leveraging a cooperative infrastructure sensor network and a novel 3D human pose estimation method, our approach effectively addresses perception limitations and occlusion challenges of accurate human intention recognition and safe navigation<ref:2504.05727#pg8>. Additionally, the integration of human-posebased PS field into an MPC controller enables the generation of trajectories that prioritize human comfort<ref:2504.05727#pg3>. Extensive evaluations in real-world environments validate the effectiveness of our proposed SAP-CoPE framework<ref:2504.05727#pg10>. Due to the modular design and real-time performance, our proposed SAP-CoPE framework has strong potential for deployment in practical applications such as indoor service robots, healthcare logistics, and autonomous navigation in shared public spaces<ref:2504.05727#pg8>. Future work will focus on enhancing system robustness in highly dynamic environments and further improving human intention prediction with the combination of learning-based algorithms<ref:2504.05727#pg3>.

How it works

The proposed SAP-CoPE method consists of two main systems: a real-time cooperative perception and a socially aware motion planning (Fig. 2)<ref:2504.05727#pg2>. For the perception systems, it employs a group of multi-modal ISNs equipped with LiDAR, cameras, and edge computing for local object detection and 3D human pose estimation<ref:2504.05727#pg2>. A synchronized clock system aligns LiDAR scans with camera operations, ensuring temporally consistent data, which is transmitted to a Central Layer (CL) for global fusion, multi-object tracking, and delay compensation<ref:2504.05727#pg2>.

The motion-planning system is designed to generate optimal poses and implemented in the CL<ref:2504.05727#pg2>. The MPC consists of three objective terms and a set of constraints<ref:2504.05727#pg8>. J1 is the PS field, which serves as a penalty as robot enters human’s personal space, where Nhuman denotes the number of humans in the environment and P S(q) represents the social cost associated with the qth human<ref:2504.05727#pg8>. J2 penalizes tracking error between the predicted output η and ηRef throughout the prediction horizon Np, weighted by the positive definite matrix Q<ref:2504.05727#pg8>. J3 regulates the front and rear wheels’ steering action δ = δf δr T and the deviation of the robot front and rear wheels’ velocity V = vf vr T from its reference Vref over the control horizon Nc, with weighting matrices R and T, respectively<ref:2504.05727#pg8>.

The kinematic model describes the robot’s motion as it operates at speeds below 5 km/h<ref:2504.05727#pg2>. The kinematic model of the proposed robot is formulated as follows<ref:2504.05727#pg2>.

Improvements for AI systems

  1. The system can perform real-time cooperative perception by leveraging ceiling-mounted ISNs to provide expanded and occlusion-robust perception through uncertainty-aware 3D human pose estimation from cameras. This allows autonomous systems to accurately detect humans in crowded, occluded indoor spaces.

  2. The motion planning system can generate trajectories that prioritize human comfort by incorporating a psychology inspired personal-space field into the MPC objective function. This enables the robot to generate socially acceptable motion while accounting for the omnidirectional type robot’s kinematic constraints.

  3. The perception module can achieve high accuracy in 3D pose estimation by employing a novel framework that combines geometry-based techniques and sensor fusion, specifically by calculating world coordinates using the camera projection formula and propagating uncertainties through the Jacobian matrix J.

  4. The system can maintain temporal consistency under network latency because it utilizes a delay-aware cooperative perception pipeline that incorporates timestamp-based delay compensation and motion prediction models to align stale data with the current system state.

  5. The robot can navigate crowded environments more naturally than conventional methods by using an asymmetric shaped field proposed in [34], which is intentionally shaped to be nonuniform in the y-direction and concentrated along the human’s facing orientation.

Abstract

Autonomous mobile robots must operate smoothly in human-populated indoor environments, where onboard-only sensing suffers from limited perception range and occlusions, and socially compliant motion planning must account for human psychological comfort zones. These factors complicate accurate recognition of human intentions and the generation of comfortable, social-aware trajectories. To address these challenges, we propose SAP-CoPE, an indoor navigation framework that integrates cooperative infrastructure sensing, human pose estimation, and a social-aware MPC planner. In the perception module, an uncertainty-aware optimization problem is formulated to estimate 3D human poses from camera observations, with optional incorporation of sparse LiDAR point-cloud measurements, while considering human joint coherence. In the planning module, the estimated human pose and orientation are used to construct a personal-space (PS) field within an MPC framework, enabling the robot to account for both physical safety and psychological comfort. Extensive real-world evaluations demonstrate the effectiveness of the proposed approach in generating social-aware trajectories for autonomous systems.

Sources

Related papers