Enhancing Social Robot Navigation with Integrated Motion Prediction and Trajectory Planning in Dynamic Human Environments

In plain words

A service robot moving through a room full of people has to decide where to go next while people walk around it. A planner that only looks at the static map and the current obstacles tends to react late, and it may cut across someone's path or push too close to them.

This paper combines two things. A trained model guesses where each nearby person will walk next. A local planner, which already picks a short, smooth, collision-free path, is then given extra costs based on those guesses. The planner is also told to keep to the side of a corridor that a person would normally use. The result is meant to look more like how people move around each other.

The authors test this in a simulator against two standard planners and report that their version gets to the goal faster in most of the test scenes.

TL;DR

• Adds Social GAN human-path forecasts to the Timed Elastic Band (TEB) local planner §2.1 §2.2.
• Extends the TEB cost with three social terms: following a predicted human path, tracking a predicted obstacle position, and a priority-side term eq. 5–8.
• Author-reported in Gazebo simulation on four scenarios against DWA and TEB: best total time in 3 of 4 scenarios tab. 1.
• The planner code that would implement the cost is not in the official repository, so the method has to be built from the paper .gitmodules.

Problem

The robot must reach a goal pose in an environment where people and other dynamic obstacles move. Obstacles that move cannot be placed on a static map, so the robot needs a local planner that predicts their motion §1. The paper states that prior planners such as DWA and TEB avoid obstacles but lack human-specific constraints for friendly and safe navigation §1.

The problem is set up with a robot pose \bm p_r = [x_r, y_r, \theta_r]^T, a goal pose \bm p_g, and for each person i a trajectory of positions sampled at intervals \Delta T_k §2. The paper states that the robot, like humans, aims to reach a predefined goal while staying safe §2.

Method

The pipeline has two units fig. 1 §2:

1. Social awareness. RGB-D images are used to detect and track people. Their positions are placed in world coordinates, and the robot pose comes from localization §2. A person's image-plane box is turned into a planar position with depth and camera intrinsics eq. 1. The predictor then uses the last 8 positions of each tracked person, sampled every 0.5 s, and forecasts the next 12 positions, which covers 6 s §2.1. The forecaster is a Social GAN generator with a pooling module that models interactions between people §2.1 [gupta2018social].
2. Social trajectory planning (MP-TEB). The TEB planner [rosmann2012trajectory] chooses a band of poses \mathbf B between the robot and the goal. Its cost is the sum of a time term and penalty terms for violating kinematic, clearance, velocity and acceleration limits eq. 2. The paper solves for m locally optimal bands and keeps the one with the lowest cost eq. 3 eq. 4. The social extension adds three terms to that cost eq. 5–8:
   • a term that penalises distance from a predicted human position, meant to make the robot follow the human-like path eq. 5;
   • a term that penalises distance from a predicted dynamic-obstacle position, which moves with the obstacle velocity over time eq. 6;
   • a priority-side term that tells whether an obstacle is on the robot's non-priority side (positive means non-priority) and is subtracted from the cost eq. 7 §2.2.

The final cost \tilde{\mathcal V} adds the social terms to the TEB cost with a normalisation factor \delta_{mp} eq. 8. The control command (v_r, \omega_r) is taken from the optimal band, and the kinematic model used for the wheels is given in eq. 9 §2.2. The paper does not state how the command is computed from the band.

Equations

Person position from a detection box and depth eq. 1:

s^i = \begin{bmatrix} x^i \\ y^i \end{bmatrix} = \begin{bmatrix} \left(x^c_{min} + \frac{x^c_{max} - x^c_{min}}{2} - m_{cx}\right) \frac{d}{m_{fy}} \\ d \end{bmatrix} eq. 1

Here x^c_{min} and x^c_{max} are the box limits along the image x-axis, d is depth, and m_{cx}, m_{fy} are camera intrinsics §2.1.

TEB cost of a band \mathbf B with N poses eq. 2:

\mathcal{V}(\mathbf B) = \sum_{i=1}^{N-1} \left[ \Delta T_i^2 + \delta_h \lVert \mathbf h_i \rVert_2^2 + \delta_v \lVert \min\{\mathbf 0, \mathbf v_i\} \rVert_2^2 + \delta_o \lVert \min\{\mathbf 0, \mathbf o_i\} \rVert_2^2 + \delta_\alpha \lVert \min\{\mathbf 0, \bm\alpha_i\} \rVert_2^2 \right] = \mathbf w^T \mathbf f(\mathbf B) eq. 2

Subject to the constraints listed with eq. 2: 0 \le \Delta T_i \le \Delta T_{max}; \mathbf h_i = 0 (non-holonomic kinematics); \mathbf o_i \ge 0 (clearance); \mathbf v_i \ge 0 (velocity limits); \bm\alpha_i \ge 0 (acceleration limits) §2.2.

Local optimum for each of m obstacle configurations eq. 3:

\bm B^*_1, \bm B^*_2, \dots, \bm B^*_m = \operatorname*{argmin}_{\bm B \setminus \{\bm s_1, \bm s_N\}} \mathcal{V}(\mathbf B) eq. 3

Global choice among the m candidates eq. 4:

\hat{\bm B}^* = \operatorname*{argmin}_{\bm B^*_j \in \{\bm B^*_1, \dots, \bm B^*_m\}} \mathcal{V}(\mathbf B_j) eq. 4

Human-path term eq. 5:

e^{hum\text{-}like}_t = w_1 \lVert \bm p_{r_t} - \bm p_{s_t} \rVert eq. 5

Dynamic-obstacle term eq. 6:

e^{dyn\text{-}obs}_t = w_2 \lVert \bm p_{r_t} - (\bm p_{obs_t} + \bm v_{obs_t}\, t) \rVert eq. 6

Priority-side term eq. 7:

e^{pri}_t = \cos(\theta_r)\,(y_{obs_t} - y_{r_t}) - \sin(\theta_{r_t})\,(x_{obs_t} - x_{r_t}) eq. 7

Social objective eq. 8:

\tilde{\mathcal V}(\mathbf B) = \mathcal V(\mathbf B) + \delta_{mp}\,(e^{hum\text{-}like}_t + e^{dyn\text{-}obs}_t - e^{pri}_t) eq. 8

Differential-drive kinematics eq. 9:

\begin{bmatrix} x_r^{i+1} \\ y_r^{i+1} \\ \theta_r^{i+1} \end{bmatrix} = \begin{bmatrix} x_r^{i} \\ y_r^{i} \\ \theta_r^{i} \end{bmatrix} + \begin{bmatrix} \frac{v_r^r + v_r^l}{2} \cos\theta_i\, d_t \\ \frac{v_r^r + v_r^l}{2} \sin\theta_i\, d_t \\ \frac{v_r^r - v_r^l}{L}\, d_t \end{bmatrix} eq. 9

with v_r^r = v_r + \frac{L\omega_r}{2} d_t and v_r^l = v_r - \frac{L\omega_r}{2} d_t, where L is the wheelbase eq. 9 §2.2.

Pseudocode

The control step as the paper describes it. Steps 1 and 5 are where the paper is silent, and the notes say so.

# MP-TEB control step  [§2] [§2.1] [§2.2]
# Input:  robot_pose (x, y, theta); goal_pose (x, y, theta)
#         humans: list of (8, 2) past positions, one array per person
#         obstacles: list of (position (2,), velocity (2,))
# Output: velocity command (v, omega)

def mp_teb_step(robot_pose, goal_pose, humans, obstacles):
    # 1. Forecast each person's next 12 positions  [§2.1]
    #    Sampling and number of samples are not stated.
    human_paths = []
    for past in humans:
        human_paths.append(social_gan(past))     # (12, 2)

    # 2. Create an initial band from robot to goal  [§2.2]
    #    Initialisation is not stated.
    band = initial_band(robot_pose, goal_pose)

    # 3. Minimise the cost with the endpoints fixed  [eq. 3]
    #    Eq. 3 writes V(B); eq. 8 defines the social cost V~(B).
    local_bands = minimise_local(band, human_paths, obstacles)

    # 4. Keep the candidate with the lowest cost  [eq. 4]
    best_band = lowest_cost(local_bands)

    # 5. Turn the optimal band into a command  [§2.2]
    #    The derivation is not stated.
    return command_from_band(best_band)


def augmented_cost(band, human_paths, obstacles):
    # Base cost V(B) with its constraints  [eq. 2]
    cost = teb_cost(band)
    # Social terms added with factor delta_mp  [eq. 8]
    cost += delta_mp * social_terms(band, human_paths, obstacles)
    return cost


def social_terms(band, human_paths, obstacles):
    # Eq. 8 has no sum over t; summing over the band is assumed.
    total = 0.0
    for t, robot_pose in enumerate(band):
        # Human-path term, eq. 5 (one human per term is not stated)
        total += w1 * distance(robot_pose, human_paths, t)
        # Dynamic-obstacle term, eq. 6
        total += w2 * obstacle_distance(robot_pose, obstacles, t)
        # Priority-side term, subtracted, eq. 7 and eq. 8
        total -= priority_side(robot_pose, obstacles, t)
    return total

Hyperparameters

Results

Author-reported. Setup: Gazebo simulation with ROS, four scenarios (reverse direction, multi-person, corridor door, turn right), baselines DWA and TEB, each run 10 times and averaged §3.1 §3.2. Table 1 is reproduced as the paper prints it. Bold marks the paper's emphasis of the best value per scenario and metric tab. 1. The corridor row is the scenario called "narrow corridor" in the text §3.1 §3.2.

Source: tab. 1.

Qualitative results are in figures only: a single-person reverse scenario fig. 3, a multi-person scenario with the three planners side by side fig. 4, the corridor door fig. 5, and the turn-right case with TEB and MP-TEB fig. 6. The paper's text describes these figures §3.2.

Paper vs code

The official repository is thanhnguyencanh/SGan-TEB, linked from the abstract and the README README.md. Its owner is one of the authors. The commit checked is 41dcbb3. The checkout is shallow, so the submodules are empty.

• The planner is not in the repository. teb_local_planner is a git submodule pointer at b68c3288a0bca1d4b7c17c5972c2185b08d4cf09, with no files in the checkout. The submodule points to the upstream rst-tu-dortmund/teb_local_planner, not to a fork .gitmodules. The social cost terms of eq. 5–8 therefore cannot be compared with code. Only the upstream TEB is known to exist, and it is not the authors' code.
• Human positions (eq. 1) are not computed here. The node hdetect subscribes to /darknet_ros/detections and treats the input poses as positions in the robot frame hdetect_node.cpp hdetect_node.cpp. The darknet_ros submodule is also empty .gitmodules. The box-to-position step of eq. 1 is therefore not in the repository. The paper does not name a detector; the README runs darknet_ros README.md.
• Localization. The paper says IMU and LiDAR data estimate the robot pose §2. The code subscribes to /amcl_pose hdetect_node.cpp.
• Orientation offset. The AMCL callback adds 4.71 to orientation.z and uses that value as the robot's heading hdetect_node.cpp. The paper has no such offset and gives no frame transform; eq. 7 uses \theta_r directly eq. 7.
• Prediction length. The paper uses 8 past and 12 future positions §2.1. The runtime node reads seq_len = 8 predict_tracks.py and keeps 12 outputs predict_tracks.py. The training script defaults and the shipped training launch use pred_len 8 train.py run_traj.sh. The deployed model is named hotel_12 predict_tracks.py, so its training length is 12 by name only. Its stored arguments were not read.
• Probabilistic forecast. The paper stresses probabilistic forecasting §2.1. The node draws 20 samples predict_tracks.py predict_tracks.py, but the fill loop stops after the first sample supplies all 12 positions predict_tracks.py. The effective output is one sample per person.
• Short histories. The paper uses 8 past positions §2.1. When a track has fewer, the node repeats its first stored position to fill the window hdetect_node.cpp.
• Social GAN weights. predict_tracks.py loads the .pt checkpoint from an absolute path on the author's machine predict_tracks.py. The paper does not say which dataset the forecaster was trained on.

No discrepancies were checked for the planner, because it is absent.

Pitfalls

• The core method has no code here. The MP-TEB cost is not in the repository (see Paper vs code). An implementation has to be built from eq. 2–9 and the upstream TEB, and the result cannot be checked against the authors' planner.
• Eq. 3–4 optimise V, not Ṽ. Eq. 3 and eq. 4 minimise \mathcal V(\mathbf B), the base TEB cost eq. 2, while the text and eq. 8 define the social cost \tilde{\mathcal V}. Which cost is optimised in the social version is not stated.
• Sign and intent of eq. 5 and 6 as written. Both terms grow with the distance from a human or an obstacle, and eq. 8 adds them with a positive factor. Read literally, the planner is pulled toward those points, which is the opposite of the avoidance the text describes §2.2. The paper also does not say whether w_1 and w_2 are positive or how the human-path term is meant to balance the obstacle term. Settle this from the planner code before relying on the sign.
• Time index in eq. 6 is undefined. t in \bm v_{obs_t}\, t is not tied to the band time step \Delta T_i or to the 0.5 s forecast step, and its unit is not stated eq. 6.
• Social sum unclear. Eq. 5–8 index the social terms by t, and eq. 8 has no sum. Whether they are summed over all band poses is not stated.
• Several humans. Eq. 5 has one human position \bm p_{s_t}. How several people combine is not stated.
• Eq. 2 typesetting. The source places \Delta T_i^2 + \delta_h as one group before \lVert \mathbf h_i\rVert^2. The card reads this as \Delta T_i^2 + \delta_h\lVert \mathbf h_i\rVert^2, which matches the other terms. This is a reading of the source, not a checked fact.
• Eq. 3 subscripts. The source prints \bm B^*_1 twice. The card assumes \bm B^*_2 for the second candidate.
• Eq. 9 units. v^r_r = v_r + \frac{L\omega_r}{2} d_t multiplies a velocity term by d_t, so the units do not match. The standard differential-drive form has no d_t in the wheel speeds. The paper also reuses the superscript i for the time step and the person index.
• Command derivation missing. The paper says (v_r, \omega_r) is "generated" from the optimal band §2.2 but gives no formula, and the time step d_t used in eq. 9 is not stated.
• Text overstates the table. The text claims MP-TEB is better than DWA and TEB on both efficiency and safety in every scenario §3.2. Table 1 does not support this. In the corridor row, DWA has the shorter total time (23.12 s against 24.46 s), and TEB has the shorter path (6.95 m against 7.11 m) tab. 1. In the multi-person row, TEB has the larger minimum distance (0.46 m against 0.36 m).
• "Much shorter" is not general. The text says MP-TEB's total time is "much shorter" than the other two §3.2. The corridor row contradicts this.
• Captions say "real trajectory". The experiments run in Gazebo §3.1, but figure captions say "real trajectory" fig. 3 fig. 4. No real robot test is reported.
• No variance. Results are averages over 10 runs with no standard deviation or range §3.2 tab. 1. The differences in some rows are small, for example 0.36 m against 0.29 m in the multi-person row.
• Baseline setup. The DWA and TEB baseline parameters and the simulation map are not stated §3.1. The code launches a turtlebot3_house world README.md, which is not named in the paper.
• Naming. The text uses both MP-TEB and MT-TEB §3.1, and the conclusion calls it Motion-Predictive Timed Elastic Band §4.
• Compute cost. The node draws 20 samples per call and sets the generator to training mode predict_tracks.py predict_tracks.py. Training mode keeps dropout and batch-norm active, and the paper does not discuss inference cost or real-time rates.
• Known bugs in the detection node. These were read from the source, not run.
  • Inside the per-detection loop, humanCount is reset to zero for all tracks after each detection hdetect_node.cpp. The next association then reads index humanCount[m]-1 hdetect_node.cpp, which is index −1 when the count is zero. This is undefined behaviour and it also drops track history.
  • The paper's 8-position history is therefore not kept across detections in one callback.

Lineage

• Builds on: Social GAN [gupta2018social] for forecasting; the Timed Elastic Band planner [rosmann2012trajectory] for the base cost. Neither has a card in this wiki.
• Compared against: DWA [fox1997dynamic] and TEB [rosmann2012trajectory] in Table 1 tab. 1.
• Superseded by: not stated.