arXiv reader

Optimal model-based trajectory planning with static polygonal constraints

Andreas B. Martinsen, Anastasios M. Lekkas and Sebastien Gros

arXiv
2010.14428v1 · PDF · 27 October 2020
Categories
eess.SY, cs.SY, math.OC
Journal
IEEE Transactions on Control Systems Technology, 2021
DOI
10.1109/TCST.2021.3094617
Licence
CC BY 4.0 The site shows the text.

Abstract

The main contribution of this paper is a novel method for planning globally optimal trajectories for dynamical systems subject to polygonal constraints. The proposed method is a hybrid trajectory planning approach, which combines graph search, i.e. a discrete roadmap method, with convex optimization, i.e. a complete path method. Contrary to past approaches, which have focused on using simple obstacle approximations, or sub-optimal spatial discretizations, our approach is able to use the exact geometry of polygonal constraints in order to plan optimal trajectories. The performance and flexibility of the proposed method is evaluated via simulations by planning distance-optimal trajectories for a Dubins car model, as well as time-, distance- and energy-optimal trajectories for a marine vehicle.

Items 17

Lemma

Lemma 1

K6GT

The fixed endpoint value function Q(\cdot) will always be lower bounded by the free endpoint value function V(\cdot):

Q(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_N], \boldsymbol{x}_f) \geq V(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_N])
Proof.

The free endpoint value function V(\cdot) where \boldsymbol{x}_f is free can be expressed in terms of minimizing the fixed endpoint value function Q(\cdot) as follows:

\begin{aligned} V(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_N]) &= \min_{\boldsymbol{x}_f \in \mathcal{T}_N} Q(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_N], \boldsymbol{x}_f) \\ &\leq Q(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_N], \boldsymbol{x}_f) \: \forall \boldsymbol{x}_f \in \mathcal{T}_N \end{aligned}
Proof

Proof

EMSN

The free endpoint value function V(\cdot) where \boldsymbol{x}_f is free can be expressed in terms of minimizing the fixed endpoint value function Q(\cdot) as follows:

\begin{aligned} V(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_N]) &= \min_{\boldsymbol{x}_f \in \mathcal{T}_N} Q(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_N], \boldsymbol{x}_f) \\ &\leq Q(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_N], \boldsymbol{x}_f) \: \forall \boldsymbol{x}_f \in \mathcal{T}_N \end{aligned}
Lemma

Lemma 2

L7WY

Given Assumption 1, the value function V(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_N]) is monotonically increasing with respect to the length of the sequence of triangles.

Proof.
\begin{aligned} V(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_{N}]) &= Q(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_{N - 1}], \boldsymbol{x}_N) + V(\boldsymbol{x}_N, [\mathcal{T}_N])\\ &\geq V(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_{N - 1}]) + V(\boldsymbol{x}_N, [\mathcal{T}_N]) \\ &\geq V(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_{N - 1}]) \end{aligned}
Uses Assumption 1
Proof

Proof

MBQL
\begin{aligned} V(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_{N}]) &= Q(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_{N - 1}], \boldsymbol{x}_N) + V(\boldsymbol{x}_N, [\mathcal{T}_N])\\ &\geq V(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_{N - 1}]) + V(\boldsymbol{x}_N, [\mathcal{T}_N]) \\ &\geq V(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_{N - 1}]) \end{aligned}
Definition

Definition 1 (Triangle sequence completeness)

Z4CA

We say that a sequence of triangles [\mathcal{T}_0, \dots \mathcal{T}_N] is complete if the initial state is within the initial triangle \boldsymbol{x}_0 \in \mathcal{T}_0, and the final goal state is in the final triangle \boldsymbol{x}_f \in \mathcal{T}_N. Similarly, a sequence is incomplete if the initial state is within initial triangle \boldsymbol{x}_0 \in \mathcal{T}_0, and the final goal state is not within the last triangle \boldsymbol{x}_f \notin \mathcal{T}_N.

Uses Assumption 1
Assumption

Assumption 2

LLRN

The heuristic function h(\boldsymbol{x}_M, \boldsymbol{x}_f) is admissible. Hence the the heuristic will always underestimate the true cost or value function for any feasible sequence of triangles [\mathcal{T}_M , \dots \mathcal{T}_N].

h(\boldsymbol{x}_M, \boldsymbol{x}_f ) \leq Q(\boldsymbol{x}_M, [\mathcal{T}_M , \dots \mathcal{T}_N], \boldsymbol{x}_f),
Used by Lemma 3, Theorem 1, Lemma 4, Theorem 2
Lemma

Lemma 3

FN1K

Given Assumption 2 and a triangle sequence [\mathcal{T}_0, \dots \mathcal{T}_M , \dots \mathcal{T}_N], we have the following lower bound on the trajectory cost:

\begin{aligned} Q(\boldsymbol{x}_0, [\mathcal{T}_0,\dots \mathcal{T}_N], \boldsymbol{x}_f) \geq \underbrace{V(\boldsymbol{x}_0, [\mathcal{T}_0,\dots \mathcal{T}_{M-1}]) + h(\boldsymbol{x}_M, \boldsymbol{x}_f)}_{:= \: \underline{Q}(\boldsymbol{x}_0, [\mathcal{T}_0,\dots \mathcal{T}_{M-1}], \boldsymbol{x}_f)} \end{aligned}

where \boldsymbol{x}_M = \boldsymbol{x}(t_M) is the end of the optimal free endpoint trajectory given by V(\boldsymbol{x}_0, [\mathcal{T}_0,\dots \mathcal{T}_{M-1}].

Proof.
\begin{aligned} Q(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_N], \boldsymbol{x}_f) &= Q(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_{M-1}], \boldsymbol{x}_M) \\ &+ Q(\boldsymbol{x}_M, [\mathcal{T}_M , \dots \mathcal{T}_N], \boldsymbol{x}_f) \\ &\geq V(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_{M-1}]) \\ &+ Q(\boldsymbol{x}_M, [\mathcal{T}_M , \dots \mathcal{T}_N], \boldsymbol{x}_f) \\ &\geq V(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_{M-1}]) \\ &+ h(\boldsymbol{x}_M, \boldsymbol{x}_f) \\ &= \underline{Q}(\boldsymbol{x}_0, [\mathcal{T}_0,\dots \mathcal{T}_{M-1}], \boldsymbol{x}_f) \end{aligned}
Uses Assumption 1, Assumption 2 · Used by Proof, Proof
Proof

Proof

4MYV
\begin{aligned} Q(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_N], \boldsymbol{x}_f) &= Q(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_{M-1}], \boldsymbol{x}_M) \\ &+ Q(\boldsymbol{x}_M, [\mathcal{T}_M , \dots \mathcal{T}_N], \boldsymbol{x}_f) \\ &\geq V(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_{M-1}]) \\ &+ Q(\boldsymbol{x}_M, [\mathcal{T}_M , \dots \mathcal{T}_N], \boldsymbol{x}_f) \\ &\geq V(\boldsymbol{x}_0, [\mathcal{T}_0, \dots \mathcal{T}_{M-1}]) \\ &+ h(\boldsymbol{x}_M, \boldsymbol{x}_f) \\ &= \underline{Q}(\boldsymbol{x}_0, [\mathcal{T}_0,\dots \mathcal{T}_{M-1}], \boldsymbol{x}_f) \end{aligned}
Theorem

Theorem 1

Q9QV

Given a complete sequence of triangles \mathcal{S}^* = [\mathcal{T}_0, \dots \mathcal{T}_N], and an incomplete sequence \mathcal{S}' satisfying:

Q(\boldsymbol{x}_0, \mathcal{S}^*, \boldsymbol{x}_f) \leq \underline{Q}(\boldsymbol{x}_0, \mathcal{S}', \boldsymbol{x}_f),

Then completing the incomplete sequence \mathcal{S}' can not result in a trajectory with a lower value Q(\cdot) then the sequence \mathcal{S}^*.

Proof.

From Lemma 3, we have that extending any incomplete sequence \mathcal{S}' to a complete sequence \mathcal{S} will result in a higher cost, i.e:

\underline{Q}(\boldsymbol{x}_0, \mathcal{S}', \boldsymbol{x}_f) \leq Q(\boldsymbol{x}_0, \mathcal{S}, \boldsymbol{x}_f).

Given the condition in (1), we get the following result:

\begin{aligned} Q(\boldsymbol{x}_0, \mathcal{S}^*, \boldsymbol{x}_f) \leq \underline{Q}(\boldsymbol{x}_0, \mathcal{S}', \boldsymbol{x}_f) \quad \Rightarrow \quad \\ Q(\boldsymbol{x}_0, \mathcal{S}^*, \boldsymbol{x}_f) \leq Q(\boldsymbol{x}_0, \mathcal{S}, \boldsymbol{x}_f). \end{aligned}

This means that all sequences \mathcal{S} that can result from the incomplete sequences \mathcal{S}' will have higher cost then the optimal sequence \mathcal{S}^* if (1) holds.

Uses Assumption 1, Assumption 2 · Used by Proof
Proof

Proof

HS2Y

From Lemma 3, we have that extending any incomplete sequence \mathcal{S}' to a complete sequence \mathcal{S} will result in a higher cost, i.e:

\underline{Q}(\boldsymbol{x}_0, \mathcal{S}', \boldsymbol{x}_f) \leq Q(\boldsymbol{x}_0, \mathcal{S}, \boldsymbol{x}_f).

Given the condition in (1), we get the following result:

\begin{aligned} Q(\boldsymbol{x}_0, \mathcal{S}^*, \boldsymbol{x}_f) \leq \underline{Q}(\boldsymbol{x}_0, \mathcal{S}', \boldsymbol{x}_f) \quad \Rightarrow \quad \\ Q(\boldsymbol{x}_0, \mathcal{S}^*, \boldsymbol{x}_f) \leq Q(\boldsymbol{x}_0, \mathcal{S}, \boldsymbol{x}_f). \end{aligned}

This means that all sequences \mathcal{S} that can result from the incomplete sequences \mathcal{S}' will have higher cost then the optimal sequence \mathcal{S}^* if (1) holds.

Uses Lemma 3
Lemma

Lemma 4

VEGY

In Algorithm ?, the list of sequences to be searched (open\_list) will always contain a sub-sequence \mathcal{S}' of any possible complete path \mathcal{S}

Proof.

Algorithm ?, changes the open\_list by iterative removing incomplete sequences, and adding all feasible sequences that can be extended by one triangle from the sequence that is removed. Since any possible complete path must be extended from the sequence only containing the initial triangle \mathcal{T}_0. Then the list of sequences to be searched (open\_list) will always contain a sub-sequence of any possible complete path.

Uses Assumption 1, Assumption 2 · Used by Proof
Proof

Proof

X5MF

Algorithm ?, changes the open\_list by iterative removing incomplete sequences, and adding all feasible sequences that can be extended by one triangle from the sequence that is removed. Since any possible complete path must be extended from the sequence only containing the initial triangle \mathcal{T}_0. Then the list of sequences to be searched (open\_list) will always contain a sub-sequence of any possible complete path.

Theorem

Theorem 2

38KN

Algorithm ? will find the optimal sequence of triangles \mathcal{S}^*, and hence the globally optimal trajectory.

Proof.

Given that Algorithm ? terminates with the optimal sequence \mathcal{S}^*. If we assume there exists a better sequence \tilde{\mathcal{S}}^*. such that:

Q(\boldsymbol{x}_0, \tilde{\mathcal{S}}^*, \boldsymbol{x}_f) < Q(\boldsymbol{x}_0, \mathcal{S}^*, \boldsymbol{x}_f)

Then from Lemma 4, a sub-sequence \tilde{\mathcal{S}}' of \tilde{\mathcal{S}}^* must exist in the list of possible sequences to be extended (open\_list). Given the result in Lemma 3 we get that:

\underline{Q}(\boldsymbol{x}_0, \tilde{\mathcal{S}}', \boldsymbol{x}_f) \leq Q(\boldsymbol{x}_0, \tilde{\mathcal{S}}^*, \boldsymbol{x}_f) < Q(\boldsymbol{x}_0, \mathcal{S}^*, \boldsymbol{x}_f).

This contradicts the termination condition in (1), and hence no sequence \tilde{\mathcal{S}}^* that is better then \mathcal{S}^* can exist.

Uses Assumption 1, Assumption 2
Proof

Proof

MA2G

Given that Algorithm ? terminates with the optimal sequence \mathcal{S}^*. If we assume there exists a better sequence \tilde{\mathcal{S}}^*. such that:

Q(\boldsymbol{x}_0, \tilde{\mathcal{S}}^*, \boldsymbol{x}_f) < Q(\boldsymbol{x}_0, \mathcal{S}^*, \boldsymbol{x}_f)

Then from Lemma 4, a sub-sequence \tilde{\mathcal{S}}' of \tilde{\mathcal{S}}^* must exist in the list of possible sequences to be extended (open\_list). Given the result in Lemma 3 we get that:

\underline{Q}(\boldsymbol{x}_0, \tilde{\mathcal{S}}', \boldsymbol{x}_f) \leq Q(\boldsymbol{x}_0, \tilde{\mathcal{S}}^*, \boldsymbol{x}_f) < Q(\boldsymbol{x}_0, \mathcal{S}^*, \boldsymbol{x}_f).

This contradicts the termination condition in (1), and hence no sequence \tilde{\mathcal{S}}^* that is better then \mathcal{S}^* can exist.

Uses Lemma 4, Lemma 3, Theorem 1
Assumption

Assumption 3

C8BK

The optimal path will only pass through any given triangle \mathcal{T} once.

Assumption

Assumption 4

3DDB

If two initial starting points \boldsymbol{x}_1, \boldsymbol{x}_2 \in \mathcal{T} are sufficiently close:

||\boldsymbol{x}_1 - \boldsymbol{x}_2||_2 \leq \epsilon.

Then the optimal sequences of triangles \mathcal{S}^* to the goal will be the same for both trajectories, and the difference between values of the trajectories is bounded.

||Q(\boldsymbol{x}_1, \mathcal{S}^*, \boldsymbol{x}_f) - Q(\boldsymbol{x}_2, \mathcal{S}^*, \boldsymbol{x}_f)|| \leq \delta

Source and licence

The text on this page is from arXiv:2010.14428v1 by Andreas B. Martinsen, Anastasios M. Lekkas and Sebastien Gros, under the licence CC BY 4.0. The corpus made these changes: it read the items out of the LaTeX source, it expanded the macros of the paper, it put each proof after its statement, and it converted the text to HTML. The formulas are the TeX of the paper, and KaTeX renders them. A formula or a reference can look different from the PDF. The page is published under CC BY 4.0.

The record of each item is in analysis/objects/2010/2010.14428.jsonl.