The Old Trajectory Is Not Continuously Collision-Checked During Replanning, Potentially Causing the UAV to Follow an Unsafe Trajectory

[wong-772]

1. Issue Overview

While a UAV is executing a trajectory, RACER periodically checks the future trajectory for potential collisions using a safety timer. When a collision risk is detected, the system enters the replanning state and shortens the remaining execution time of the current trajectory.

However, the UAV does not stop immediately after a collision risk is detected. Instead, it continues following the old trajectory while waiting for a new trajectory to be generated. The current implementation does not continuously verify whether this remaining segment of the old trajectory is safe, nor does it verify whether the trajectory truncation point is suitable as a hovering position.

As a result, the following situations may occur:

1. The UAV continues following an old trajectory that is already known to be unsafe during replanning.
2. A collision occurs before the new trajectory is generated.
3. If a new trajectory is not generated in time, the UAV attempts to hover at an unverified truncation point.
4. Newly generated trajectories may remain unsafe, causing repeated replanning.

---

2. Current Collision-Checking and Replanning Mechanism

While the UAV is executing a trajectory, the safety timer checks the future trajectory approximately every 0.05 s. The checked range is approximately the next 6 m of the trajectory.

When a future collision is detected, the finite-state machine enters the replanning state. The state machine executes the corresponding state callback approximately every 0.01 s.

During replanning:

• The start state of the new trajectory is usually selected from a point approximately 0.2 s ahead on the old trajectory.
• The planner is given a certain amount of time to generate a new trajectory.
• The UAV continues executing the old trajectory during this period.
• The old trajectory may continue to be executed for up to approximately 0.4 s.
• If the UAV has not switched to a new trajectory when the old trajectory reaches its truncated duration, the trajectory server continuously publishes the truncation point while setting the commanded velocity and acceleration to zero.

The problem is that neither the remaining segment of the old trajectory nor its truncation point is continuously collision-checked during this process.

---

3. Code Analysis

3.1 Shortening the Lifetime of the Old Trajectory During Replanning

After receiving a replanning request, the trajectory server shortens the valid duration of the current trajectory using the following code:

void replanCallback(std_msgs::Empty msg) {
  // Informed of new replan, end the current traj after some time
  const double time_out = 0.2;
  ros::Time time_now = ros::Time::now();
  double t_stop =
      (time_now - start_time_).toSec() + time_out + replan_time_;
  traj_duration_ = min(t_stop, traj_duration_);
}

The expression:

time_out + replan_time_

defines the time window during which the old trajectory may continue to be executed while replanning is in progress. Under the current configuration, this window is approximately 0.4 s.

However, this code only modifies traj_duration_. It does not check:

• Whether a collision may occur between the current time and t_stop;
• Whether the predicted collision time is earlier than the truncation time;
• Whether the UAV can brake before reaching the collision point;
• Whether the truncation point maintains sufficient clearance from obstacles.

Therefore, shortening the trajectory lifetime only limits how long the old trajectory may be executed. It does not guarantee that the remaining segment is safe.

3.2 Publishing the Trajectory Endpoint as a Hovering Command

When the current time exceeds the valid trajectory duration, the trajectory server continuously publishes the trajectory endpoint and sets the commanded velocity and acceleration to zero:

if (t_cur < traj_duration_ && t_cur >= 0.0) {
  // Current time within range of planned traj
  pos = traj_[0].evaluateDeBoorT(t_cur);
  vel = traj_[1].evaluateDeBoorT(t_cur);
  acc = traj_[2].evaluateDeBoorT(t_cur);
  yaw = traj_[3].evaluateDeBoorT(t_cur)[0];
  yawdot = traj_[4].evaluateDeBoorT(t_cur)[0];
  jer = traj_[5].evaluateDeBoorT(t_cur);
} else if (t_cur >= traj_duration_) {
  // Current time exceed range of planned traj
  // keep publishing the final position and yaw
  pos = traj_[0].evaluateDeBoorT(traj_duration_);
  vel.setZero();
  acc.setZero();
  yaw = traj_[3].evaluateDeBoorT(traj_duration_)[0];
  yawdot = 0.0;
} else {
  cout << "[Traj server]: invalid time." << endl;
}

The relevant dynamic limits are configured as:

<arg name="max_vel" value="1.5" />
<arg name="max_acc" value="1.0" />

Directly setting the commanded velocity and acceleration to zero does not mean that the UAV can stop instantaneously. The UAV still requires a finite braking time and braking distance.

In addition, the position corresponding to:

traj_[0].evaluateDeBoorT(traj_duration_)

is directly used as the hovering position without verifying that it lies in safe free space.

---

4. Root Cause

Collision detection is mainly used to trigger replanning, but it does not continue constraining the UAV's actual motion while the planner is generating a replacement trajectory.

The current implementation assumes that the old trajectory remains safe until it is truncated. However, this assumption may not hold when replanning is triggered precisely because the old trajectory has already been predicted to collide.

In addition, the truncation time is calculated from fixed waiting and replanning durations. It is not dynamically adjusted according to the predicted collision time, braking distance, or obstacle clearance.

---

5. Log Analysis

In our experiment, UAV 4 detected a collision risk while executing trajectory 37. The following sequence was then observed:

Execute trajectory 37
→ Detect a collision and trigger replanning
→ Generate trajectory 38
→ Trajectory 38 is still predicted to collide
→ Perform normal planning
→ Generate trajectory 39
→ Replan again and generate trajectory 40

The continuously increasing trajectory IDs indicate that the system successfully detects collision risks and repeatedly invokes the planner.

However, before each new trajectory is generated and takes control, the UAV may continue executing the previous trajectory. If the predicted collision occurs before the old trajectory reaches its truncation time, the UAV may collide before the new trajectory is activated.

Furthermore, continuing to move toward the obstacle during replanning causes the starting point of the next planning attempt to become closer to the obstacle. This may reduce the likelihood of finding a feasible trajectory and lead to repeated replanning.

---

6. Safety Impact

This issue may cause:

• The UAV to continue following a known unsafe trajectory during replanning;
• A collision with a static obstacle or another UAV before the new trajectory is generated;
• Hovering at a position that has not been verified as safe;
• Failure to brake in time because the remaining distance is insufficient;
• Replanning oscillation caused by repeatedly generating trajectories that cannot be safely executed.

---

7. Suggested Fixes

7.1 Continue Checking the Old Trajectory During Replanning

After entering the replanning state, the safety timer should continue checking the old trajectory that is actually being executed. The checked segment should at least cover the expected takeover time of the new trajectory and the truncation point of the old trajectory.

If the old trajectory is predicted to collide before the new trajectory takes control, the system should stop executing it immediately.

7.2 Consider Braking Distance and Time to Collision

When a collision is detected, the system should calculate:

• The distance and time to the collision point;
• The current velocity and acceleration;
• The minimum braking time and braking distance;
• Control latency and an additional safety margin.

If the remaining distance is shorter than the minimum safe braking distance, or the remaining time is insufficient for braking, emergency braking should be triggered immediately instead of waiting for normal replanning.

if (distance_to_collision <= braking_distance + safety_margin ||
    time_to_collision <= braking_time + latency_margin) {
  triggerEmergencyBrake();
} else {
  triggerReplan();
}

7.3 Validate the Trajectory Truncation Point

The truncation point of the old trajectory should not be directly used as a hovering position. Its occupancy state and obstacle clearance should first be checked against the map.

If the truncation point is unsafe, the system should identify a safe stopping position or generate a dynamically feasible braking trajectory from the current motion state.

7.4 Generate a Complete Emergency-Braking Trajectory

The system should not rely only on setting the commanded velocity and acceleration to zero. Instead, it should generate a continuous braking trajectory based on the current position, velocity, acceleration, and maximum allowable deceleration.

The entire braking trajectory should also be collision-checked before execution.

7.5 Limit Repeated Replanning

If several consecutively generated trajectories are still predicted to collide, the system should stop repeating normal replanning and switch to a more conservative safety response, such as emergency braking, replanning at a reduced speed, or retreating to a previously verified safe area.