Collision-Prone B-Spline Trajectories May Be Published Due to Soft Obstacle Constraints and Missing Pre-Publication Validation

[dawang3111]

Summary

RACER’s local trajectory generation process generally consists of two stages:

1. A search algorithm, such as A*, generates a discrete collision-free path.
2. The discrete path is optimized into a smooth and dynamically feasible B-spline trajectory.

During the search stage, collision avoidance is treated as a hard requirement. If the searched path intersects an obstacle, the search fails and another path must be generated.

However, during B-spline optimization, obstacle clearance is primarily represented as a cost term rather than a hard feasibility constraint. When a control point is too close to an obstacle, the optimizer increases the distance cost, but it does not necessarily reject the candidate trajectory.

As a result, even if the original A* path is collision-free, the optimized B-spline trajectory may deviate from the searched path and intersect an obstacle. If the total objective value of this trajectory remains lower than that of other candidates, the optimizer may still return it as the final result.

More importantly, the final optimized B-spline trajectory does not appear to undergo a mandatory collision validation before it is published. Collision checking is instead mainly performed by a periodic safety timer after the trajectory has entered the execution state.

This creates a safety-critical time window:

A trajectory containing a collision point may be published and begin execution before the next periodic safety check is triggered.

If the collision point is located near the beginning of the new trajectory, the UAV may reach the obstacle before the safety timer can detect the problem and trigger replanning.

The core issue is therefore:

Obstacle avoidance is treated as a soft optimization objective, while the final trajectory is not subject to a mandatory collision-free validation before publication. Consequently, system safety depends on a periodic runtime check that may occur too late.

---

Relevant Code

The obstacle-distance cost is calculated in BsplineOptimizer::calcDistanceCost():

void BsplineOptimizer::calcDistanceCost(
    const vector<Eigen::Vector3d>& q,
    double& cost,
    vector<Eigen::Vector3d>& gradient_q) {
  cost = 0.0;
  Eigen::Vector3d zero(0, 0, 0);
  std::fill(gradient_q.begin(), gradient_q.end(), zero);
double dist;

Eigen::Vector3d dist_grad, g_zero(0, 0, 0);
for (int i = 0; i < q.size(); i++) {

if (!dynamic_) {

edt_environment_->evaluateEDTWithGrad(

q[i], -1.0, dist, dist_grad);
  if (dist_grad.norm() &gt; 1e-4)
    dist_grad.normalize();
} else {
  double time =
      double(i + 2 - order_) * knot_span_ + start_time_;

  edt_environment_-&gt;evaluateEDTWithGrad(
      q<span data-placeholder-token="true" class="text-token-text-primary cursor-text rounded-sm">[i]</span>, time, dist, dist_grad);
}

if (dist &lt; dist0_) {
  cost += pow(dist - dist0_, 2);
  gradient_q<span data-placeholder-token="true" class="text-token-text-primary cursor-text rounded-sm">[i]</span> +=
      2.0 * (dist - dist0_) * dist_grad;
}

}

}

Obstacle clearance is handled as a soft cost

The optimizer queries the distance between each control point and nearby obstacles:

edt_environment_->evaluateEDTWithGrad(

q[i], -1.0, dist, dist_grad);

When the distance is below the configured clearance threshold dist0_, a quadratic penalty is added:

cost += pow(dist - dist0_, 2);

The corresponding gradient is then calculated:

gradient_q[i] +=

2.0 * (dist - dist0_) * dist_grad;

This mechanism encourages the optimizer to move control points away from obstacles, but it does not enforce collision avoidance as a hard constraint.

When:

dist < dist0_

the candidate trajectory is not immediately rejected. Its objective value is only increased.

Therefore, if the benefits from smoothness, feasibility, trajectory length, or other optimization terms outweigh the distance penalty, a trajectory with insufficient clearance—or potentially a collision—may still be selected.

No hard collision rejection in the cost function

The current function does not contain logic equivalent to:

if (dist <= collision_threshold) {

return OPTIMIZATION_FAILED;

}

Consequently, entering an occupied region and merely reducing the desired safety clearance are both represented numerically through the same type of cost function.

The difference is only the size of the penalty.

This means that the optimizer itself does not guarantee that the final result remains entirely within free space.

Checking control points alone is insufficient

The values in q[i] are B-spline control points. Even if all control points are located in free space, this does not automatically guarantee that the continuous curve between them is collision-free.

The final B-spline should therefore be checked as a continuous trajectory, either through sufficiently dense temporal or spatial sampling or through a conservative continuous-curve collision-checking method.

Collision checking occurs after publication

According to the observed state-machine behavior, an optimized trajectory enters PUB_TRAJ and is subsequently activated in EXEC_TRAJ.

The periodic safety timer runs approximately once every 0.05 s and checks the future portion of the trajectory during execution.

The effective safety process is therefore:

B-spline optimization completes

|

v

Trajectory is published

|

v

Trajectory begins execution

|

v

Wait for the periodic safety timer

|

v

Collision risk is detected

|

v

Replanning is triggered

This runtime safety check is useful for detecting newly introduced risks, such as map updates or dynamic changes. However, it cannot replace validation of a newly generated trajectory before publication.

---

Experimental Observation

Test scenario

During Round 11, UAV 1 collided with a static obstacle while executing a newly generated B-spline trajectory.

Timeline

Time	Event
294.5276	Trajectory 57 begins execution.
294.5488	The safety timer detects that trajectory 57 may collide in the future and changes the state to PLAN_TRAJ.
294.5723	Generation of trajectory 58 begins.
294.5821	Trajectory 58 is successfully generated, and the state enters PUB_TRAJ.
294.7776	Trajectory 58 begins execution, and the state changes to EXEC_TRAJ.
294.7865	A physical collision with a static obstacle is detected.
294.8046	The safety timer detects a collision risk on trajectory 58 and changes the state back to PLAN_TRAJ.

Trajectory 58 was published and executed

The state-transition log contains:

[294.777567] PUB_TRAJ -> EXEC_TRAJ

The corresponding published trajectory information is:

drone_id=1
source=/planning/bspline_1
traj_id=58
stamp=1780904294.7722998

This indicates that UAV 1 was executing B-spline trajectory 58, published through /planning/bspline_1, when the collision occurred.

Collision occurred shortly after execution began

The recorded collision position was:

collision at: -4.83169 11.1611 1.21522

Trajectory 58 entered EXEC_TRAJ at:

294.777567

The physical collision was detected at approximately:

294.7865

The elapsed time was therefore:

294.7865 - 294.777567 ≈ 0.0089 s

The UAV collided approximately 8.9 ms after trajectory 58 began execution.

This interval is substantially shorter than the approximately 0.05 s safety-timer period. Therefore, even if the timer operated normally, it could not reasonably be expected to detect the collision, trigger replanning, publish a replacement trajectory, and cause the controller to respond before impact.

This observation suggests that the unsafe point was located near the beginning of trajectory 58 and that the trajectory was already unsafe when it was published.

The safety timer detected the problem after the collision

The following transition was recorded later:

[294.804570] EXEC_TRAJ -> PLAN_TRAJ

This indicates that the safety timer eventually detected a risk on the currently executing trajectory and triggered replanning.

However, the detection occurred after the physical collision:

Physical collision: 294.7865
Safety detection:   294.804570

The difference was approximately:

294.804570 - 294.7865 ≈ 0.0181 s

Therefore, although the runtime safety checker eventually identified the unsafe trajectory, its response occurred too late to prevent the collision.

---

Findings Supported by the Logs

The logs support the following observations:

1. The safety checker correctly detected a future risk on trajectory 57.
2. The system subsequently generated and published trajectory 58.
3. Trajectory 58 was accepted and entered the execution state.
4. UAV 1 collided with a static obstacle approximately 8.9 ms after trajectory 58 began execution.
5. The periodic safety checker triggered replanning only after the collision had already occurred.
6. Runtime periodic checking was therefore insufficient to protect the initial segment of the newly published trajectory.

The logs demonstrate that an unsafe trajectory reached the execution state. Combined with the soft obstacle-distance cost in the optimizer, this indicates that collision avoidance is not enforced as a mandatory acceptance condition for the final published trajectory.

---

Root Cause

The issue appears to result from the combination of the following design limitations.

1. Collision avoidance is represented as a soft optimization objective

Obstacle distance contributes to the objective function, but a collision does not necessarily cause optimization failure.

2. A collision-free search path does not guarantee a collision-free optimized trajectory

A* validates the discrete searched path. The UAV, however, executes the subsequently optimized B-spline.

The optimization process changes the geometry of the path and may move the continuous trajectory outside the free-space corridor of the original search result.

3. The final B-spline is not rejected before publication when it contains a collision

After optimization, the continuous trajectory should be validated before entering PUB_TRAJ. Without this validation, an unsafe result may be published.

4. Safety relies excessively on a periodic execution-time check

A periodic safety timer is inherently subject to detection latency. If a collision point occurs near the start of a new trajectory, the UAV may reach it before the next timer callback.

---

Expected Behavior

A newly generated trajectory should not be published unless the final continuous trajectory has been verified to be collision-free with respect to:

• the current occupancy or distance map;
• the UAV body radius;
• the required safety margin;
• expected tracking and localization errors.

A trajectory that intersects an occupied region or violates the minimum allowed clearance should be rejected before entering PUB_TRAJ.

---

Actual Behavior

The optimizer may return a B-spline with an unsafe segment because obstacle distance is handled as a finite cost.

The trajectory may then be published and begin execution. Collision detection is deferred to the next runtime safety-timer callback, which may occur after the UAV has already reached the obstacle.

---

Suggested Fix

1. Add mandatory collision validation before publishing the trajectory

After B-spline optimization and before entering PUB_TRAJ, sample the final continuous trajectory and verify that every checked point satisfies the required clearance.

For example:

bool collision_free = true;
for (double t = 0.0;

t <= trajectory_duration;

t += collision_check_dt) {

Eigen::Vector3d pos = bspline.evaluateDeBoorT(t);
if (environment->getDistance(pos) <= collision_threshold) {

collision_free = false;

break;

}

}
if (!collision_free) {

// Do not publish this trajectory.

// Retry optimization or return to path search.

return false;

}

The publication process should become:

Optimization completes

|

v

Validate the final continuous trajectory

|

+-- Collision-free --> PUB_TRAJ

|

+-- Collision found --> Reject and replan

This is the most important mitigation because it closes the safety gap before execution begins.

2. Treat an actual collision as a hard rejection condition

The existing distance cost may remain useful for encouraging larger clearance. However, actual collision and reduced safety margin should be handled differently.

For example:

if (min_distance <= collision_threshold) {

return TRAJECTORY_INVALID;

}

Recommended behavior:

Below preferred safety clearance, but still collision-free

-> Apply a soft optimization penalty

Inside the minimum allowed clearance or occupied space

-> Reject the trajectory

A very large finite penalty is not equivalent to hard rejection because the optimizer may still accept it when other objective terms produce a lower total cost.

3. Validate the continuous curve instead of only the control points

The collision validator should inspect the actual B-spline, not only q[i].

Possible approaches include:

• dense temporal sampling;
• adaptive spatial sampling;
• subdivision of B-spline segments;
• conservative checking based on B-spline convex hulls.

The sampling resolution should account for:

• map resolution;
• UAV maximum velocity;
• UAV body dimensions;
• obstacle dimensions;
• trajectory curvature;
• controller update frequency.

The spatial distance between adjacent samples should be sufficiently smaller than the map voxel size so that thin obstacles cannot be skipped between samples.

4. Include the UAV body radius and uncertainty margins

The collision condition should not only check whether the UAV center lies inside an obstacle.

The required clearance should include at least:

required_clearance =

drone_radius

+ tracking_error_margin

+ localization_error_margin

+ map_uncertainty_margin;

A trajectory should only be published when:

dist > required_clearance

Alternatively, the occupancy map may be inflated by the equivalent required clearance before collision checking.

5. Retry optimization or fall back to path search

When the final optimized trajectory fails collision validation, the planner may use a staged recovery process:

1. Increase the obstacle-distance weight.
2. Change the initialization or optimization parameters.
3. Add control points or modify the knot span.
4. Retry optimization from a different initial trajectory.
5. If repeated optimization attempts fail, rerun A* or another path-search method.
6. If no safe trajectory can be generated, command a safe hover or braking maneuver instead of publishing the unsafe trajectory.

Example:

Optimized trajectory is unsafe

|

v

Retry optimization with adjusted parameters

|

v

Still unsafe

|

v

Run path search again

|

v

No safe result

|

v

Hover or execute a safe braking trajectory

6. Keep the runtime safety timer as a secondary defense

Reducing the safety-timer interval may reduce detection latency, but it does not resolve the underlying issue.

In this experiment, the UAV collided approximately 8.9 ms after trajectory execution began. Even a 10 ms timer interval would not necessarily provide enough time to complete:

• collision checking;
• state transition;
• replanning;
• trajectory publication;
• controller reception;
• physical braking or trajectory switching.

The runtime safety timer should remain as a secondary safeguard for changes that occur after publication, such as:

• newly observed obstacles;
• map updates;
• dynamic obstacles;
• tracking deviations;
• localization changes.

It should not be the primary mechanism for validating a newly generated trajectory.

---