Trajectory Planning for Safe Dual Control with Active Exploration

1. Problem Statement

The paper addresses the fundamental challenge of safe trajectory planning under model uncertainty in robotics and control systems.

• The Conflict: Traditional robust planning ensures safety by considering worst-case uncertainty realizations but ignores uncertainty reduction, leading to overly conservative behavior and degraded performance. Conversely, dual control aims to balance task execution (exploitation) with active uncertainty reduction (exploration), but existing methods often lack formal guarantees on safety or fail to determine when exploration is beneficial relative to mission costs.
• The Specific Challenge: The authors formulate a budget-constrained dual control problem. The system must:
  1. Satisfy state and input constraints under bounded model uncertainty.
  2. Actively reduce parametric uncertainty through exploration.
  3. Ensure the total mission cost (performance degradation due to exploration) remains within a prescribed budget ($B_{exp}$).
• Goal: To maximize cumulative uncertainty reduction over the mission horizon while strictly adhering to safety constraints and a user-defined cost budget, without relying on heuristic weighted trade-offs in the objective function.

2. Methodology: The Dual-gatekeeper Framework

The authors propose a framework called Dual-gatekeeper, which operates in a receding-horizon manner. It treats exploration not as an implicit optimization trade-off, but as a verifiable decision.

Core Architecture

At each replanning time $t_k$, the framework executes the following pipeline:

1. Robust Backup Construction: A conservative, robust feedback policy ($\pi^{rob,B}_k$) is computed. This policy guarantees safety under the current worst-case uncertainty set ($\Theta_k$) and serves as a "fallback" baseline.
2. Candidate Generation:
   • Conservative Candidates: Segments of the robust backup policy.
   • Informative Candidates: Trajectories explicitly designed to reduce parametric uncertainty (e.g., by exciting specific dynamics). These are generated to satisfy a terminal recoverability condition (ensuring the system can return to the safe backup trajectory).
3. Feasibility Verification (The "Gatekeeper"):
   • Safety Check: Each candidate is verified against the uncertainty set $\Theta_k$ and disturbance bounds. If a candidate cannot be certified as safe (e.g., via tube invariance or probabilistic rollouts), it is rejected.
   • Budget Check: The predicted mission cost of the candidate is compared against the robust backup cost. The excess cost is checked against the remaining exploration budget ($B_{exp}$).
4. Selection & Commitment:
   • Among all valid (safe) and budget-feasible candidates, the one with the highest score is selected.
   • Score Function: $s_{k,i} = \exp(-\lambda T) \cdot \Delta \xi_i$, where $\Delta \xi_i$ is the predicted reduction in the width of the uncertainty set, and $\lambda$ discounts future gains to prioritize immediate information.
   • If no informative candidate is feasible, the system defaults to the conservative backup policy.
5. Execution & Update: The committed trajectory is executed. New data is collected, and the uncertainty set $\Theta_k$ is updated using Set-Membership Identification (SMID), shrinking the feasible parameter set for the next cycle.

Uncertainty Reduction Prediction

To evaluate candidates before execution, the framework predicts uncertainty shrinkage using two approaches:

1. Simulation-Based: Parallel forward rollouts with sampled parameters and disturbances to estimate the post-execution uncertainty set.
2. Data-Consistency-Based: A mathematical analysis of the regressor matrix along the planned trajectory to bound the "error set" of indistinguishable parameters, providing a computationally cheaper estimate.

Safety Instantiations

The framework is modular and demonstrated with two safety mechanisms:

• Tube MPC Instantiation: Uses a nominal trajectory with an invariant "tube" to guarantee constraint satisfaction for all uncertainties.
• Gatekeeper (Safety Filter) Instantiation: Separates nominal planning from safety verification. A gatekeeper module performs probabilistic safety checks (via Monte Carlo rollouts) on candidate trajectories before allowing execution.

3. Key Contributions

1. Formal Budget-Constrained Dual Control: Introduces a formulation where exploration is a distinct, budget-limited decision rather than a weighted term in the cost function.
2. The Dual-gatekeeper Framework: A novel architecture that decouples safety certification, budget feasibility, and information gain. It guarantees that informative trajectories are only executed if they are provably safe and cost-effective.
3. Modularity: Demonstrates the framework's flexibility by instantiating it with two distinct safety mechanisms (Tube MPC and Gatekeeper filtering).
4. Theoretical Guarantees: Provides formal proofs that the framework ensures:
   • Safety: State and input constraints are satisfied for all admissible uncertainties.
   • Budget Feasibility: The cumulative exploration cost never exceeds the prescribed budget.
5. Empirical Validation: Validates the approach on two complex case studies: quadrotor navigation and autonomous car racing.

4. Results

The framework was tested on two case studies under parametric uncertainty:

• Case Study 1: Quadrotor Navigation (Tube MPC)

  • Scenario: A quadrotor with unknown aerodynamic drag coefficients navigating to a goal.
  • Outcome: The proposed method reduced the parameter uncertainty bounds significantly (e.g., from [0.0, 0.50] to [0.0, 0.33]) while maintaining safety.
  • Performance: Achieved 82.5% of the baseline mission cost (where baseline is 100%) while staying within an 110% budget, demonstrating improved efficiency over purely conservative robust control.

• Case Study 2: Autonomous Car Racing (Gatekeeper)

  • Scenario: A race car with unknown tire-road friction ($\mu$) attempting to minimize lap time.
  • Comparison: Compared against baselines including pure nominal control, weighted exploration (no safety), fallback-only, and gatekeeper with weighted objectives.
  • Outcome:
    • Safety: The proposed method achieved 100% safe runs (10/10 trials), whereas weighted exploration baselines failed frequently (20–30% success).
    • Performance: Achieved an average lap time of 8.07s, significantly faster than the fallback-only (17.0s) and comparable to the unsafe nominal controller (5.01s) but with guaranteed safety.
    • Uncertainty Reduction: Achieved 95.5% uncertainty reduction.
    • Budget Efficiency: Only 23.8% of the exploration budget was consumed, indicating that a few strategic informative maneuvers were sufficient to drastically reduce uncertainty and enable high-performance racing.

5. Significance and Future Work

• Significance: This work bridges the gap between robust control (safety) and active learning (exploration). It moves beyond heuristic trade-offs by providing a principled, verifiable mechanism to decide when to explore. It proves that active exploration can improve long-term performance without compromising safety or exceeding cost limits.
• Limitations & Future Directions:
  • Time-Varying Parameters: The current SMID approach assumes time-invariant parameters. Future work aims to handle drifting parameters (e.g., changing friction).
  • Budget Quantification: The budget is currently prescribed by the user. Future work seeks to derive a principled relationship between uncertainty size and future mission cost to automate budget allocation.
  • Execution-Aware Prediction: Improving uncertainty prediction to account for discrepancies between planned and executed trajectories (tracking errors).

In summary, Dual-gatekeeper offers a rigorous solution for safe dual control, enabling autonomous systems to learn about their environment dynamically while strictly adhering to safety and resource constraints.