2.2 Process Algebra
2.2.3 Automata-based motion planning
In order to solve a optimal motion planning problem while satisfying a mis- sion specification provided as a P A term, a modified version of the previously described sampling-based algorithms is proposed in [19]. However, for both simplicity of formulation and implementation, it looks desirable and more elegant to perform the search for a solution with the exact same algorithms presented in Chapter 1.
Algorithm 4:Process Algebra Parser
/* Parser definition */
1 PA parse (string t, vertex I, vertex F , graph G):
2 addends←− splitMain (t, ‘+’) ; 3 if addends ¬ = ∅ then 4 foreacht0 ∈ addends do 5 PA parse(t0, I, F , G) ; 6 else 7 factors←− splitMain (t, ‘·’) ; 8 if factors¬ = ∅ then
9 lastFactor←− popBack (factors) ;
10 prevVert←− I ;
11 foreacht0 ∈ factors do
12 newVert ←− addVertex (G) ;
13 PA parse (t0, prevVert, newVert, G) ;
14 prevVert ←− newVert;
15 PA parse(lastFactor, prevVert, F , G) ;
16 else /* t is an atomic action here */
17 addEdge(G, I, F , t) ;
/* Parser call */
18 PG ←− createEmptyGraph () ;
19 initVert ←− newVert () ; // will be 0
20 endVert ←− newVert () ; // will be 1
21 PA parse(t, initVert, endVert, PG);
To achieve this, the crucial idea is extending the domain where the plan- ning takes place, by embedding in it both dynamical and logical information, so that the new definition of state captures the evolution of both the dynam- ical system and the underlying automaton.
A similar approach is followed in [3], where Linear Temporal Logic (LTL) is used to encode a set of safety road rules and an automata-based approach
provides minimum-violating plans for an autonomous vehicle. However, the problem formulation in [3] is history-less, since the state of the automaton is fully recoverable from the robot’s state variables.
The following formulation overcomes this limitation and is indeed language- independent, as long as a language can be correctly translated into a non deterministic finite state automaton.
For motion planning purposes, each action(↔ input symbol of the au-
tomaton) is in fact a predicate over the robot’s state variables, i.e. formally ai(↔ σi) : X → {true, false}. For simplicity we will also use ai(↔ σi) to refer to the set of states which satisfy the predicate.
In the sequel, we will specifically consider an action ai(↔ σi) as a given region in the state space of the dynamical system that models the robot, i.e. ai ⊂ X (σi ⊂ X). For single integrator and Dubins dynamics, this basically decribes locations in the robot’s environment, i.e. goals/subgoals.
However, it is important to notice that - in the most general case - this definition can include state variables that are not strictly related to the sys- tem’s dynamics (e.g. battery level, sensor data, or - for an autonomous vehicle - number of passengers aboard).
Definition 17 (labelled state space). Let X be the state space of a dynam- ical system and Q the set of states of a non-deterministic finite automaton
M. We will call ˜X = X × Q the M-labelled state space of the system. A
given element ˜z ∈ ˜X is a pair ˜z = (z, q), with z ∈ X and q ∈ Q the logical
label of ˜z.
Definition 18 (trajectory in ˜X). A trajectory in theM-labelled state space is a function ˜x : [0, T ] → ˜X with ˜x(t) = (x(t), q(t)), t ∈ [0, T ] where x is a trajectory of the dynamical system and q(t) is a piecewise-constant function
q : [0, T ] → Q.
A labelled trajectory ˜x can be naturally associated with a run ρ of the underlying automaton if i) the logical transitions along it are transitions of the automaton and ii) they happen when the symbols are verified, formally:
Definition 19 (timing). Given a labelled trajectory ˜x(t) = (x(t), q(t)), the timing of ˜x(t) is the set of time values T˜x :={t ∈ R≥0 : limτ→t−q(τ )6= q(t)}, with T˜x = (t0, t1, . . . , tn) and ti−1 < ti ∀i ∈ [1, n]. By convention t0 = 0 is
always included.
The timing of a labelled trajectory is basically the ordered set of time istants when the logical label q changes along the trajectory.
Definition 20 (run generated by a trajectory). Given a labelled trajec-
tory ˜x(t) = (x(t), q(t)) and its timing Tx˜ = (t0, t1, . . . , tn), the sequence ρ = (q0, q1. . . qn) with qi = q(ti) ∀i ∈ [0, n] is said to be a run of the au-
tomaton M generated by trajectory ˜x iff ∀i ∈ [0, n − 1] ∃ σi such that
(qi, σi, qi+1)∈ δ ∧ x(ti)∈ σi.
According to the definitions, not all the labelled trajectories generate a run of the automaton. In such cases, a trajectory is said to be rejected by the automaton.
Indeed, a labelled trajectory that generates a run ρ = (qinit, q1, . . . , qn) of M with its last state being an accepting state qn ∈ F is said to be accepted by the automaton.
With the given premises, the following problem is now defined.
Problem 5 (NFA-based Optimal Kinodynamic motion planning problem). Given a dynamical system, an obstacle-free space Xf ree ⊆ X, an initial state xinit ∈ X, a set of predicates Σ = {σk: X → {true, false} , k ∈ [1, m]} and
a finite automaton M with alphabet Σ, find a control law u : [0, T ] → U
such that:
• the corresponding unique trajectory x(t) : ˙x(t) = f(x(t), u(t)) ∀t ∈ [0, T ], is obstacle-free, i.e. x(t)∈ Xf ree ∀t ∈ [0, T ],
• there exists a labelled trajectory ˜x(t) = (x(t), q(t)) which is accepted
by M,
• the above mentioned labelled trajectory minimizes a cost functional J (˜x, u) = R0T g(˜x(t), u(t))dt, with g : ˜X× U → R>0.
♦
A final noticeable remark is that the cost functional J is now defined over the labelled trajectory space and therefore it depends not only on the dynamical trajectory of the system, but also on the geneated run of the automaton and its timing. Therefore it is possible to define order relations over trajectories that take into account their logical features, in addition to their geometry and dynamics. This possibility will be extensively used to correctly implement algorithms for the urban-mobility robotic application in Chapter 3.