Method of evolving junction on optimal path planning in flows fields

Abstract

We propose an algorithm using method of evolving junctions to solve the optimal path planning problems with piece-wise constant flow fields. In such flow fields, we prove that the optimal trajectories, with respect to a convex Lagrangian in the objective function, must be formed by piece-wise constant velocity motions. Taking advantage of this property, we transform the infinite dimensional optimal control problem into a finite dimensional optimization and use intermittent diffusion to solve the problems. The algorithm is proven to be complete. At last, we demonstrate the performance of the algorithm with various simulation examples.

Appendix

In this appendix, we give proofs for the properties related to Algorithm 1.

First we present proof of Theorem 4.1. The proof leverages the following theorem in Chow et al. (2013).

Theorem 6.1

Let Q be the set of global minimizers, U be a small neighborhood of Q and $\lambda _{opt}$ the optimal solution obtained by the ID process. Then for any given $\epsilon >0$, there exists $\tau >0$, $\sigma _0>0$ and $N_0>0$ such that if $T_i-S_i>\tau$, $\sigma _i<\sigma _0$ (for $i=1,\cdots ,N$) and $N>N_0$,

$$\begin{aligned} \mathbb {P}(\lambda _{opt}\in U)\ge 1-\epsilon . \end{aligned}$$

Then we provide completeness proof of Algorithm 1.

Proof of Theorem 4.1

The proof includes two steps. First, we show that the decision tree returns all cell sequences with total cost less than or equal to the lowest-cost path found so far. Then we prove that given a fixed cell sequence, the global minimizer can be found by Algorithm 2.

The DFS algorithm, which avoids repeated states in the graph, is complete in finite state spaces (Russell and Norvig, 2002). In a static flow field divided into convex regions, the optimal path will not visit a cell boundary curve more than one time. Hence, the optimal path connecting the root and the target node in the decision tree does not contain loops. Therefore, the BnBDFS returns all cell sequences with total cost less than or equal to the lowest-cost found so far.

Next we show that the ID algorithm is complete. We combine Theorem 4.4, together with Bellman principle, to show that the global optimal path must be in the structure of constant motion within each flow region. To prove that the proposed algorithm is convergent, we only need to show that there exists a global minimizer $\lambda ^*=(\lambda _1^*,\cdots ,\lambda _K^*)$, around which there is a closed neighborhood $U\subset \prod _{i=1}^KD_i$ such that $vol(U)>0$ (vol is the product Lebesgue measure in $\prod _{i=1}^KD_i$) and for all $\lambda \in U$, the gradient flow $\dot{\lambda }=-\nabla J(\lambda )$ converges to $\lambda ^*$. If this condition holds, we can follow the proof of intermittent diffusion and get the desired results.

To this end, if there exists such U that $vol(U)>0$ and for all $\lambda \in U$, we have $J(\lambda )\le J(\mu )$ for arbitrary $\mu \in S$ for some $S\subset U$, then the proof is done. Now if the global minimizers are isolated, then given any global minimizer $\lambda ^*=(\lambda _1^*,\cdots ,\lambda _K^*)$, since J is continuous differentiable, we can have a closed neighborhood $U\subset \prod _{i=1}^KD_i$ with $vol(U)>0$ (within the neighborhood, the dimension of the domain does not change) such that $J(\lambda )>J(\lambda ^*)$ and $\nabla J(\lambda )\ne 0$ for all $\lambda \in U\backslash \{\lambda ^*\}$, then the gradient flow starting at $\lambda \in U$ converges to $\lambda ^*$.

Therefore, we prove the algorithm is complete. $\square$

Proof of Lemma 4.2

We write the value function as

$$\begin{aligned} \psi (x,t)=\frac{\Vert x-x_0\Vert }{\Vert v+u\Vert }. \end{aligned}$$

To make the problem complete, we define $\psi (x,t)=+\infty$ if the vehicle cannot reach x in time t, which gives the final value function to be

$$\begin{aligned} \psi (x,t)=\left\{ \begin{array}{ll}\frac{\Vert x-x_0\Vert }{\Vert v+u\Vert }&{}\frac{\Vert x-x_0\Vert }{\Vert v+u\Vert }\le t\\ +\infty &{}\text {otherwise}\end{array}\right. . \end{aligned}$$

If only the reachable part is considered, from the above equation, we can calculate $\psi _t=0$ and

$$\begin{aligned} \begin{aligned} \nabla \psi =\frac{1}{\Vert v+u\Vert ^2}\Big (&\Vert v+u\Vert \frac{x-x_0}{\Vert x-x_0\Vert }\\ {}&-\Vert x-x_0\Vert \nabla \Vert v+u\Vert \Big ). \end{aligned} \end{aligned}$$

(21)

We can rewrite $v=v^0+v^\perp$ and $V^2=\Vert v^0\Vert ^2+\Vert v^\perp \Vert ^2$ if we denote

$$\begin{aligned} v^0= & {} \frac{(x-x_0)}{\Vert x-x_0\Vert }\frac{(x-x_0)^Tv}{\Vert x-x_0\Vert },\\ v^\perp= & {} \left( I-\frac{(x-x_0)}{\Vert x-x_0\Vert }\frac{(x-x_0)^T}{\Vert x-x_0\Vert }\right) v, \end{aligned}$$

where I is the identity matrix. And u can be decomposed in the same manner $u=u_0+u^\perp$. It is easy to see that $v^\perp =-u^\perp$ since $(v+u)/\Vert v+u\Vert =(x-x_0)/\Vert x-x_0\Vert$. Then, we see that $\Vert v+u\Vert =\Vert v_0\Vert +\Vert u_0\Vert$ and

$$\begin{aligned} \begin{aligned}&\sqrt{\left( \frac{(x-x_0)^Tu}{\Vert x-x_0\Vert }\right) ^2+V^2-\Vert u\Vert ^2}\\ =&\Big (\Vert u^0\Vert ^2+\Vert v^0\Vert ^2+\Vert v^\perp \Vert ^2-\Vert u^0\Vert ^2-\Vert u^\perp \Vert ^2\Big )^{1/2}\\ =&\Vert v^0\Vert . \end{aligned} \end{aligned}$$

Hence, we have

$$\begin{aligned}&\nabla \Vert v+u\Vert \\ =&\nabla \left( \frac{(x-x_0)^Tu}{\Vert x-x_0\Vert }+\sqrt{\left( \frac{(x-x_0)^Tu}{\Vert x-x_0\Vert }\right) ^2+V^2-\Vert u\Vert ^2}\right) \\ =&\frac{\Vert v+u\Vert }{\Vert v^0\Vert }\nabla \frac{(x-x_0)^Tu}{\Vert x-x_0\Vert }\\ =&\frac{\Vert v+u\Vert }{\Vert x-x_0\Vert \Vert v^0\Vert }\left( I-\frac{(x-x_0)}{\Vert x-x_0\Vert }\frac{(x-x_0)^T}{\Vert x-x_0\Vert }\right) u\\ =&\frac{\Vert v+u\Vert }{\Vert x-x_0\Vert \Vert v^0\Vert }u^\perp . \end{aligned}$$

Taking $\nabla \Vert v+u\Vert$ back to (21) and noticing that $v^\perp =-u^\perp$, we reduce the gradient to be

$$\begin{aligned} \nabla \psi= & {} \left( \frac{x-x_0}{\Vert x-x_0\Vert }-\frac{u^\perp }{\Vert v^0\Vert }\right) \frac{1}{\Vert v+u\Vert }=\frac{v}{\Vert v^0\Vert \Vert v+u\Vert }. \end{aligned}$$

With the above equation, the Hamiltonian is

$$\begin{aligned} H&=\sup _{\hat{v}:\Vert \hat{v}\Vert \le V}\left( \nabla \psi ^T(\hat{v}+u)-1\right) \\&=\sup _{\hat{v}:\Vert \hat{v}\Vert \le V}\left\{ \frac{v^T}{\Vert v^0\Vert }\frac{\hat{v}+u}{\Vert v+u\Vert }\right\} -1\\&=\frac{1}{\Vert v+u\Vert }\left( \sup _{\hat{v}:\Vert \hat{v}\Vert \le V}\left\{ \frac{v^T\hat{v}}{\Vert v^0\Vert }\right\} +\Vert u_0\Vert -\frac{\Vert u^\perp \Vert ^2}{\Vert v^0\Vert }\right) -1\\&=\frac{1}{\Vert v+u\Vert }\left( \frac{V^2}{\Vert v^0\Vert }+\Vert u^0\Vert -\frac{\Vert u^\perp \Vert ^2}{\Vert v^0\Vert }\right) -1\\&=\frac{1}{\Vert v+u\Vert }(\Vert v^0\Vert +\Vert u^0\Vert )-1=0, \end{aligned}$$

which leads to the conclusion that the value function induced by the maximum speed constant velocity motion solves the Hamilton-Jacobi equation, thus is the optimal moving pattern in a constant flow speed region since $\min _t\psi =\psi$. $\square$

Meanwhile, using the same notation and logic, we can give the proof of Proposition 3.1:

Proof of Proposition 3.1

First we show that the objective function is well-defined if there exists a feasible trajectory, and $\Vert u_{c_i}\Vert \le V$. If $(x_i-x_{i-1})^T u_{c_i}\le 0$, unless $V>\Vert u_{c_i}\Vert$, there does not exists a feasible path. Therefore,

$$\begin{aligned} \begin{aligned}&(x_i-x_{i-1})^Tu_{c_i}<\\&\sqrt{\left( (x_i-x_{i-1})^Tu_{c_i}\right) ^2+\Vert x_i-x_{i-1}\Vert ^2(V^2-\Vert u_{c_i}\Vert ^2)}. \end{aligned} \end{aligned}$$

Since $\Vert u_{c_i}\Vert ^2-V^2<0$, we have $t_i^*>0$. On the other hand, if $(x_i-x_{i-1})^Tu_{c_i}>0$, we can have two cases: $V>\Vert u_{c_i}\Vert$, which shares the same conclusion as the first case, and $V<\Vert u_{c_i}\Vert$. In the latter circumstance, since $\Vert u_{c_i}\Vert ^2-V^2>0$ and

$$\begin{aligned} \begin{aligned}&(x_i-x_{i-1})^Tu_{c_i}>\\ {}&\sqrt{\left( (x_i-x_{i-1})^Tu_{c_i}\right) ^2+\Vert x_i-x_{i-1}\Vert ^2(V^2-\Vert u_{c_i}\Vert ^2)}, \end{aligned} \end{aligned}$$

it is still true that $t_i^*>0$.

Meanwhile, When $(x_i-x_{i-1})^Tu_{c_i}>0$, $t_i^*>0$ still holds if $V=\Vert u_{c_i}\Vert$ and actually

$$\begin{aligned}&t_i^*=\lim _{V^2-\Vert u_{c_i}\Vert ^2\rightarrow 0}\frac{1}{\Vert u_{c_i}\Vert ^2-V^2}\Big ((x_i-x_{i-1})^Tu_{c_i}\\&-\sqrt{\left( (x_i-x_{i-1})^Tu_{c_i}\right) ^2+\Vert x_i-x_{i-1}\Vert ^2(V^2-\Vert u_{c_i}\Vert ^2)}\Big )\\&=\frac{\Vert x_i-x_{i-1}\Vert ^2}{2(x_i-x_{i-1})^Tu_{c_i}}>0. \end{aligned}$$

However, if $(x_i-x_{i-1})^Tu_{c_i}\le 0$, $V=\Vert u_{c_i}\Vert$ becomes a singular point since there is no feasible path. Thus, in this case, we cannot formally solve the problem.

Since $g^t_i(x_i,x_{i-1})=g^t_i(x_i-x_{i-1})$ and $g^e_i(x_i,x_{i-1})=g^e_i(x_i-x_{i-1})$, we only need to consider the differentibility of

$$\begin{aligned} g(a)=\left\{ \begin{array}{ll} g_1(a) &{} \text {if 10 holds} \\ g_2(a) &{} \text {otherwise} \end{array}\right. \end{aligned}$$

where

$$\begin{aligned} g_1(a)&=2\sqrt{\Vert u_{c_i}\Vert ^2+C}\Vert a\Vert -2a^Tu_{c_i}\\ {}&=2\Vert a\Vert \left( \sqrt{\Vert u_{c_i}\Vert ^2+C}-\Vert u^0_{c_i}\Vert \right) ,\\ g_2(a)&=\frac{V^2+C}{\Vert u_{c_i}\Vert ^2-V^2}\Big (a^Tu_{c_i}\\ -&\sqrt{(a^Tu_{c_i})^2+\Vert a\Vert ^2(V^2-\Vert u_{c_i}\Vert ^2)}\Big )\\ {}&=\frac{(V^2+C)\Vert a\Vert }{\Vert u_{c_i}^0\Vert +\Vert v^0\Vert }. \end{aligned}$$

First of all, when equality in (10) holds, we have

$$\begin{aligned} \begin{aligned}&\sqrt{\Vert u_{c_i}\Vert ^2+C}=\Vert u_{c_i}^0\Vert \pm \Vert v^0\Vert \\ \Longrightarrow&\sqrt{\Vert u_{c_i}\Vert ^2+C}=\Vert u_{c_i}^0\Vert +\Vert v^0\Vert . \end{aligned} \end{aligned}$$

(22)

We take the plus sign since $\sqrt{\Vert u_{c_i}\Vert ^2+C}\ge \Vert u_{c_i}\Vert$. Meanwhile from (10) and (22), we can derive the following equation

$$\begin{aligned} V^2+C=2\Vert v^0\Vert (\Vert u_{c_i}^0\Vert +\Vert v^0\Vert ). \end{aligned}$$

Therefore, g(a) is continuous. Similar calculations shows that

$$\begin{aligned} \nabla g_1&=2\left( (\sqrt{\Vert u_{c_i}\Vert ^2+C})\frac{a}{\Vert a\Vert }-u_{c_i}\right) =v^0-u_{c_i}^\perp =2v,\\ \nabla g_2&=\frac{V^2+C}{\Vert v^0\Vert \Vert v+u_{c_i}\Vert }v=2v, \end{aligned}$$

which gives us the desired result. $\square$

Proof of Lemma 4.3

In the case of minimum energy planning, $L(x,v)=\Vert v\Vert ^2+C$ where $C\ge 0$ is a constant running cost. To calculate the optimal solution for the vehicle running from $x_0$ to the target x in a constant flow velocity field, we again study the constant speed straight line motion. However in this circumstance, the vehicle may no longer travel with maximum speed, hence we take the travel time in the region into consideration. Suppose that the the vehicle moves from $x_0$ to x in time t, we set the vehicle velocity to be

$$\begin{aligned} v=\frac{x-x_0}{t}-u, \end{aligned}$$

assuming that

$$\begin{aligned} \Vert v\Vert ^2=\frac{\Vert x-x_0\Vert ^2}{t^2}+\Vert u\Vert ^2-\frac{2(x-x_0)^Tu}{t}\le V^2. \end{aligned}$$

(23)

Then the value function is

$$\begin{aligned} \begin{aligned} \psi (x,t)&=(\Vert v\Vert ^2+C)t\\ {}&=\frac{\Vert x-x_0\Vert ^2}{t}-2(x-x_0)^Tu+(C+\Vert u\Vert ^2)t. \end{aligned} \end{aligned}$$

Further we take

$$\begin{aligned} \psi (x,t)=\left\{ \begin{array}{ll} \begin{aligned} &{}\frac{\Vert x-x_0\Vert ^2}{t}\\ {} &{}-2(x-x_0)^Tu+(C+\Vert u\Vert ^2)t \end{aligned} &{} \Vert v\Vert \le V \\ +\infty &{} \text {otherwise} \end{array}\right. . \end{aligned}$$

Then by direct calculation with the finite part of $\psi$, we have

$$\begin{aligned} \psi _t&=C+\Vert u\Vert ^2-\frac{\Vert x-x_0\Vert ^2}{t^2}, \end{aligned}$$

(24)

$$\begin{aligned} \nabla \psi&=\frac{2(x-x_0)^Tu}{t}-2u, \end{aligned}$$

(25)

$$\begin{aligned} \Vert \nabla \psi \Vert ^2&=\frac{4\Vert x-x_0\Vert ^2}{t^2}+4\Vert u\Vert ^2-\frac{8(x-x_0)^Tu}{t} \end{aligned}$$

(26)

The Hamilton-Jacobi equation is in the form of

$$\begin{aligned} \psi _t+\sup _{v:\Vert v\Vert \le V}\left\{ \nabla \psi ^T(v+u)-\Vert v\Vert ^2-C\right\} =0. \end{aligned}$$

(27)

To solve the optimization part of (27), we denote

$$\begin{aligned} F(v)=\nabla \psi ^T(v+u)-\Vert v\Vert ^2-C \end{aligned}$$

and calculate its critical point as

$$\begin{aligned} v^*=\frac{1}{2}\nabla \psi , \end{aligned}$$

which means that the optimal is

$$\begin{aligned} H=\sup _{v:\Vert v\Vert \le V}F(v)=\frac{1}{4}\Vert \nabla \psi \Vert ^2+\nabla \psi ^Tu-C \end{aligned}$$

(28)

and by (25), we have

$$\begin{aligned} \begin{aligned} \Vert v^*\Vert ^2&=\frac{1}{4}\Vert \nabla \psi \Vert ^2=\frac{\Vert x-x_0\Vert ^2}{t^2}+\Vert u\Vert ^2-\frac{2(x-x_0)^Tu}{t} \\&\le V^2, \end{aligned} \end{aligned}$$

(29)

which leads to the fact that $F(v^*)=\sup _{v:\Vert v\Vert \le V}F(v)$. Let us take (25),(26) into (28) and the result is

$$\begin{aligned} H=\frac{\Vert x-x_0\Vert ^2}{t^2}-\Vert u\Vert ^2-C. \end{aligned}$$

(30)

Combining (24) and (30) finally results in the constructed $\psi$ being the solution of (27).

Based on the solution $\psi$, we further find the minimizer over time t and solve the minimization problem as follow:

$$\begin{aligned} \begin{aligned}&\min _{t\ge 0}\psi =\min _{t\ge 0}(\Vert v\Vert ^2+C)t\\=&\frac{\Vert x-x_0\Vert ^2}{t}-2(x-x_0)^Tu+(C+\Vert u\Vert ^2)t. \end{aligned} \end{aligned}$$

It is easy to see that the global minimizer of $\psi$ over t is

$$\begin{aligned} t^*=\frac{\Vert x-x_0\Vert }{\sqrt{C+\Vert u\Vert ^2}} \end{aligned}$$

and the corresponding minimum is

$$\begin{aligned} \psi ^*=2\Vert x-x_0\Vert \sqrt{C+\Vert u\Vert ^2}-2(x-x_0)^Tu. \end{aligned}$$

(31)

Thus, if $t^*$ is reachable, that is, using (23), we have

$$\begin{aligned} \Vert v(t^*)\Vert ^2=C+2\Vert u\Vert ^2-\sqrt{C+\Vert u\Vert ^2}\frac{2(x-x_0)^Tu}{\Vert x-x_0\Vert }\le V^2 \end{aligned}$$

the optimal is given as (31).

On the other hand, if $\Vert v(t^*)\Vert >V$, the global minimizer $t^*$ is on longer in the domain of our problem. In this case, we notice that $\Vert v\Vert ^2$ is decreasing on the interval

$$\begin{aligned} \left[ t^*,\frac{(x-x_0)^Tu}{\Vert x-x_0\Vert ^2}\right] , \end{aligned}$$

and is increasing on

$$\begin{aligned} \left[ \frac{(x-x_0)^Tu}{\Vert x-x_0\Vert ^2},+\infty \right) . \end{aligned}$$

Also by noticing that $\lim _{t\rightarrow +\infty }\Vert v\Vert ^2=\Vert u\Vert ^2\le V^2$, we conclude that there exists $t_0>t^*$ when $t\ge t_0>t^*$, $\Vert v\Vert \le V$. Meanwhile, when $t>t^*$, $\psi$ is monotone increasing with respect to t. Hence, to get the minimum, we should take the time $t=t_0$, where $\Vert v(t_0)\Vert =V$. By taking the equality in (23), we have then

$$\begin{aligned} (\Vert u\Vert ^2-V^2)t^2-2(x-x_0)^Tut+\Vert x-x_0\Vert ^2=0, \end{aligned}$$

from which we have

$$\begin{aligned} \begin{aligned} t_0=&\frac{1}{\Vert u\Vert ^2-V^2}\Big ((x-x_0)^Tu\\ {}&-\sqrt{\left( (x-x_0)^Tu\right) ^2+\Vert x-x_0\Vert ^2(V^2-\Vert u\Vert ^2)}\Big ), \end{aligned} \end{aligned}$$

and $\min _{t}\psi =(V^2+C)t_0$. $\square$

Proof of Theorem 3.1

Denoting g(w) to be the inverse of f such that if $w=f(u+v)$ then $u+v=g(w)$, we will show that

$$\begin{aligned} \psi (x,t)=\left\{ \begin{array}{ll} tL\left( g\left( \frac{x-x_0}{t}\right) -u\right) &{}\Vert g\left( \frac{x-x_0}{t}\right) -u\Vert \le V\\ +\infty &{}\text {otherwise} \end{array}\right. \end{aligned}$$

satisfies the HJB equation (20). First of all, the Hessian matrix $\mathcal {H}(v)$ is positive definite for all $\Vert v\Vert \le V$ since L is convex. Therefore, for any $v_1,v_2$ in the domain, there exists $\xi$ such that

$$\begin{aligned} \nabla _vL(v_1)=\nabla _vL(v_2)+\mathcal {H}(\xi )(v_2-v_1). \end{aligned}$$

Further if $\nabla _vL(v_1)=\nabla _vL(v_2)$, then $\mathcal {H}(\xi )(v_2-v_1)=0$. Because of the positive definite property for $\mathcal {H}$, we have $v_2=v_1$, which implies that $\nabla _vL(v)$ is one-to-one.

Then we do the following calculation on the non-infinity part of $\psi$

$$\begin{aligned}&\begin{aligned}&\psi _t=L\left( g\left( \frac{x-x_0}{t}\right) -u\right) \\ {}&-\left[ \nabla _vL\left( g\left( \frac{x-x_0}{t}\right) -u\right) \right] ^T\nabla _wg\left( \frac{x-x_0}{t}\right) \frac{x-x_0}{t}, \end{aligned} \end{aligned}$$

(32)

$$\begin{aligned}&\nabla \psi =\left[ \nabla _wg\left( \frac{x-x_0}{t}\right) \right] ^T\nabla _vL\left( g\left( \frac{x-x_0}{t}\right) -u\right) . \end{aligned}$$

(33)

Since L is convex, we further have the relaxed optimization

$$\begin{aligned} \max _v\{\nabla \psi ^T(v+u)-L(v)\} \end{aligned}$$

is a convex problem and get the condition for the optimal $v^*$ to be

$$\begin{aligned} \begin{aligned}&\left[ \nabla _vf(u+v^*)\right] ^T\nabla \psi =\nabla _vL(v^*)\\ \Longrightarrow&\nabla \psi =\left[ \nabla _wg(f(u+v^*))\right] ^T\nabla _vL(v^*). \end{aligned} \end{aligned}$$

Combining this with (33), we have

$$\begin{aligned} v^*=g\left( \frac{x-x_0}{t}\right) -u, \end{aligned}$$

(34)

and $\Vert v^*\Vert \le V$ holds. Thus, $v^*$ is the maximizer of $H(x,\nabla \psi )$. Taking (32), (33) and (34), we have

$$\begin{aligned} \psi _t+\nabla \psi ^T(v^*+u)-L(v^*)=0, \end{aligned}$$

implying that (20) holds. At last notice that

$$\begin{aligned} \lim _{t\rightarrow \infty }L\left( g\left( \frac{x-x_0}{t}\right) -u\right) =L(g(0)-u)<\infty , \end{aligned}$$

since 0 is in the range of f. We have that

$$\begin{aligned} \lim _{t\rightarrow \infty }tL\left( g\left( \frac{x-x_0}{t}\right) -u\right) =\infty . \end{aligned}$$

Thus, we have $t^*>0$ such that given x,

$$\begin{aligned} t^*={{\,\mathrm{arg\,min}\,}}_{t\ge 0}\psi (x,t). \end{aligned}$$

Thus, $v^*=g((x-x_0)/t^*)-u$ gives us a constant velocity motion. $\square$