Organizations: Mechanical and Mechatronics Eng. Department, University of Waterloo, 200 University Ave W, Waterloo, ON N2L3G1, Canada · Department of Data and Systems Engineering, University of Hong Kong, Pok Fu Lam, Hong Kong, China · Mechanical Engineering Department, University of Alberta, Alberta, T6G1H9, Canada
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.
Figures & tables
Fig. 1 : Key challenges: (1) How to reliably detect humans in crowded, occluded scenarios? (2) How to guide the robot for human-comfortable motion?
Fig. 2 : SAP-CoPE Structure Overview
Fig. 3 : Kinematic Motion Model of Robot
Types of Distance
Values
Relationship
Intimate distance
0 - 0.45 m
Very intimate relationship
Personal distance
0.45 - 1.2 m
Friends
Social distance
1.2 - 3.5 m
Strangers
Public distance
> 3.5 m
Public speaking
TABLE I : Personal Space Distance Specifications [ 32 ]
Fig. 4 : Graphical representation of the PS field: a) definition of Personal Field in psychology [ 32 ] . b) constructed a personal field using a mathematical expression. In both figures, X and Y represent the body-fixed coordinates of the human.
Fig. 5 : Spatial distribution of position errors.
Fig. 6 : Spatial distribution of yaw errors.
Method
Position Error (m)
Yaw Error (deg)
MAE / RMSE
MAE / RMSE
3DNet [ 37 ] (2017)
0.1795 / 0.2163
8.5629 / 12.1001
Metrabs [ 38 ] (2023)
0.2294 / 0.2926
8.1819 / 11.2037
NLF [ 39 ] (2024)
0.1659 / 0.2103
8.6010 / 11.3837
LiCamPose [ 40 ] (2025)
0.1188 / 0.1653
15.0739 / 19.3666
Cam-only
0.1098 / 0.1308
8.5199 / 11.7416
TABLE II : Pose Estimation Performance Evaluation
Delay
Mode
F1
Recall
Avg. DE
Avg. AE
0ms
Baseline
0.9456
0.9512
0.0829
7.2475
Delay-Aware
0.9485
0.9739
0.0818
5.9557
50ms
Baseline
0.9456
0.9510
0.0864
7.2314
Delay-Aware
0.9480
0.9739
0.0840
6.1045
100ms
Baseline
0.9455
0.9510
0.0975
7.2110
Delay-Aware
0.9480
0.9738
0.0876
6.2546
TABLE III : Delay mitigation evaluation results
Number of Human
Method
Collision / Stuck Rate ↓ [%]
Clearance ↑ (Avg. / Std.) [m]
Total Traveled Dist. ↓ (Avg. / Std.) [m]
Total Traveled Time ↓ (Avg. / Std.) [sec]
Computational Time ↓ (Avg. / Std.) [ms]
15
SFM
50 / 4
0.10 / 0.025 a
18.02 / 0.76 a
25.05 / 7.19 a
8.56 / 1.55
SARL
50 / 0
0.12 / 0.011 a
18.79 / 2.47 a
22.91 / 3.62 b
29.66 / 2.38
MPC + PF
10 / 3
0.18 / 0.022 b
16.61 / 0.30 b
23.31 / 4.82 ab
37.69 / 3.31
Proposed
7 / 0
0.17 / 0.090 b
16.50 / 0.24 c
20.36 / 3.67 c
41.69 / 4.72
TABLE IV : Simulation Statistics Results Comparison in the Dense Crowd Scenario
Fig. 7 : Experiment setup with all participants. Images captured from a) ISN1’s left camera. b) ISN1’s right camera. c) ISN2’s left camera. d) ISN2’s right camera.
Symbol
Value
Symbol
Value
lf,lr
1.2 m
ucmax
[0.7,0.7,2π,2π]
Ts
0.1 sec
ucmin
- [0.7,0.7,2π,2π]
Np
20
Δucmax
[1,1,24π,24π]
Nc
10
Δucmin
- [1,1,24π,24π]
TABLE V : Robot and MPC Parameters. Units for velocity (or acceleration) are in [m/s] (or [m/s 2 ]), and steering angles (or steering rate) are in [rad] (or [rad/s]).
Clearance (Avg. / Std.) [m] ↑
Traveled Distance (Avg. / Std.) [m] ↓
Traveled Time (Avg. / Std.) [sec.] ↓
# Human
SFM
MPC + PF
Proposed
SFM
MPC + PF
Proposed
SFM
MPC + PF
Proposed
2
0.22 / 0.056 a
0.39 / 0.052 b
0.39 / 0.068 b
12.05 / 0.15 a
12.23 / 0.078 b
12.14 / 0.22 ab
18.11 / 0.18 a
17.78 / 0.15 b
17.77 / 0.16 b
4
0.071 / 0.048 a
0.17 / 0.11 b
0.29 / 0.048 c
12.06 / 0.11 a
12.21 / 0.23 ab
12.33 / 0.19 b
18.58 / 0.43 a
19.74 / 2.31 a
18.16 / 0.14 b
6
0.022 / 0.031 a
0.11 / 0.087 b
0.26 / 0.042 c
21.69 / 1.06 ab
21.86 / 0.59 a
20.76 / 0.18 b
36.12 / 3.96 a
33.43 / 1.58 a
30.73 / 0.39 b
TABLE VI : Experimental Statistics Results Comparison between the Proposed Method and Other Methods
Fig. 8 : Representative trajectories in the 2-participant experiment: (a) SFM; (b) proposed method combining MPC and the PS model.
Fig. 9 : Proposed Motion Planning Method Computation Time Distribution
Department of Electronics and Telecommunications, Politecnico di Torino, 10129, Torino, Italy. · School of Engineering, Pablo de Olavide University, Crta. Utr-era km 1, Seville, Spain