Incremental Sampling-based Algorithm for Minimum-violation Motion Planning

This paper studies the problem of control strategy synthesis for dynamical systems with differential constraints to fulfill a given reachability goal while satisfying a set of safety rules. Particular attention is devoted to goals that become feasible only if a subset of the safety rules are violated. The proposed algorithm computes a control law, that minimizes the level of unsafety while the desired goal is guaranteed to be reached. This problem is motivated by an autonomous car navigating an urban environment while following rules of the road such as “always travel in right lane’’ and “do not change lanes frequently’’. Ideas behind sampling based motion-planning algorithms, such as Probabilistic Road Maps (PRMs) and Rapidly-exploring Random Trees (RRTs), are employed to incrementally construct a finite concretization of the dynamics as a durational Kripke structure. In conjunction with this, a weighted finite automaton that captures the safety rules is used in order to find an optimal trajectory that minimizes the violation of safety rules. We prove that the proposed algorithm guarantees asymptotic optimality, i.e., almost-sure convergence to optimal solutions. We present results of simulation experiments and an implementation on an autonomous urban mobility-on-demand system.

💡 Research Summary

**
The paper addresses the synthesis of control strategies for dynamical systems with differential constraints that must reach a specified goal while respecting a set of safety rules, some of which may need to be temporarily violated to make the goal feasible. The authors propose an incremental sampling‑based algorithm, called Minimum‑Violation RRT* (MVRRT*), that integrates ideas from sampling‑based motion planning (RRT*, PRM) with formal methods (finite automata, temporal logic) to simultaneously minimize a “level of unsafety” and a motion‑cost (e.g., travel time).

Key technical contributions

1. Trace‑inclusive durational Kripke structure – The continuous dynamics (\dot{x}=f(x,u)) are discretized into a finite transition system where each state is a sampled point in the state space and each transition corresponds to a feasible trajectory segment. The transition is annotated with a duration (\Delta) and a labeling function (L) that maps states to atomic propositions. The construction guarantees that the labeling changes at most once along a transition, preserving the correspondence between continuous trajectories and discrete timed words.

2. Weighted finite automata for safety rules – Each safety rule is expressed as a nondeterministic finite automaton (A_{i,j}) equipped with a priority function ($(A_{i,j})). The automaton is extended with “violation” transitions for every possible label pair ((\sigma,\sigma’)) and assigned a weight equal to the priority. Consequently, the weight of the shortest accepting run on a word equals the defined level of unsafety (\lambda(w,A_{i,j})).

3. Unified weighted automaton (A_{\Psi}) – All rule automata are combined via a Cartesian product into a single weighted automaton that captures the cumulative unsafety across the entire rule set (\Psi). The weight of a transition in (A_{\Psi}) is the vector of individual rule weights, enabling a lexicographic ordering of unsafety levels.

4. Product of Kripke structure and weighted automaton – The product system (P = K \times A_{\Psi}) has states ((s,q)) and transition cost equal to the sum of the trajectory duration and the weighted unsafety incurred by the label change. A goal state in the product is reached when the underlying Kripke state lies in the goal region and the automaton component is in an accepting state. Finding the shortest path in this product yields a trajectory that (i) reaches the goal, (ii) minimizes the unsafety level, and (iii) among those, minimizes the motion cost.

5. MVRRT algorithm* – The classic RRT* sampling, nearest‑neighbor, steer, and rewiring steps are retained, but each new edge is evaluated in the product space. When a new sample is generated, the algorithm computes the corresponding continuous trajectory segment, extracts its label pair and duration, and updates the product edge weight accordingly. Rewiring proceeds only if the new product‑path cost is lower.

6. Asymptotic optimality proof – By showing that as the number of samples (N\to\infty) the Kripke structure becomes dense in the reachable set of the continuous system, the product graph approximates the continuous optimal problem arbitrarily closely. Consequently, MVRRT* inherits the almost‑sure convergence property of RRT* while simultaneously handling the weighted unsafety metric.

Experimental validation
The authors evaluate the method on a simulated urban road network where two safety rules are considered: “always stay in the right lane” (high priority) and “minimize lane changes” (lower priority). When the right lane is blocked, the algorithm deliberately performs a lane change, but only for the minimal necessary duration, thereby achieving a lower unsafety level than a naïve RRT* that either ignores the rules or fails to reach the goal. The results demonstrate that MVRRT* produces trajectories that dominate baseline methods in both unsafety and travel time.

A real‑world implementation on an autonomous mobility‑on‑demand vehicle further confirms practicality. The system runs online, continuously updating the product graph as new sensor data arrive, and successfully navigates city streets while respecting the prioritized safety constraints.

Conclusion and outlook
The paper introduces a novel framework that unifies sampling‑based motion planning with formal verification to solve minimum‑violation motion planning problems. It provides a rigorous discretization of continuous dynamics, a systematic quantification of rule violations via weighted automata, and an asymptotically optimal algorithm that can be deployed on real autonomous platforms. Future work may explore more efficient sampling strategies for high‑dimensional systems, online adaptation to dynamically changing rule sets, and extensions to multi‑robot coordination under shared safety constraints.