Waypoint guidance with pass angle constraints¶

$$ \newcommand{\eg}{{\it e.g.}} \newcommand{\ie}{{\it i.e.}} \newcommand{\argmin}{\operatornamewithlimits{argmin}} \newcommand{\mc}{\mathcal} \newcommand{\mb}{\mathbb} \newcommand{\mf}{\mathbf} \newcommand{\minimize}{{\text{minimize}}} \newcommand{\diag}{{\text{diag}}} \newcommand{\cond}{{\text{cond}}} \newcommand{\rank}{{\text{rank }}} \newcommand{\range}{{\mathcal{R}}} \newcommand{\null}{{\mathcal{N}}} \newcommand{\tr}{{\text{trace}}} \newcommand{\dom}{{\text{dom}}} \newcommand{\dist}{{\text{dist}}} \newcommand{\R}{\mathbf{R}} \newcommand{\SM}{\mathbf{S}} \newcommand{\ball}{\mathcal{B}} \newcommand{\bmat}[1]{\begin{bmatrix}#1\end{bmatrix}} $$

Jong-Han Kim (jonghank@inha.ac.kr)

A discrete-time linear dynamical system consists of a sequence of state vectors $x_t \in \R^n$, indexed by time $t\in \{0,\dots,N-1\}$ and dynamics equations

$$ \begin{aligned} x_{t+1} &= Ax_t + Bu_t \end{aligned} $$

where $u_t\in\R^m$ is a control input to the dynamical system (say, a drive force or steering force on the vehicle). $A$ is the state transition matrix, $B$ is the input matrix.

Given $A$ and $B$, the goal is to find the optimal $u_0, \dots, u_{N-1}$ that drives the systems state to the desirable state at the final time, $x_N= x_\text{des}$, while minimizing the size of $u_0, \dots, u_N$.

A minimum energy controller finds $u_t$ by solving the following optimization problem

$$ \begin{aligned} \underset{u}{\minimize} \quad & \sum_{t=0}^{N-1} \|u_t\|^2 \\ \text{subject to} \quad & x_N = x_{\text{des}} \\ \quad & x_{t+1} = Ax_t + Bu_t, \qquad & t=0,\dots,N-1 \end{aligned} $$

We'll apply a minimum energy control to a vehicle guidance problem with state $x_t\in\R^4$, where the first two states are the position of the vehicle in two dimensions, and the last two are the vehicle velocity, that is $x_t = ( p_x, p_y, v_x, v_y)_t$. The vehicle's control $u_t\in\R^2$ is acceleration control for the two axes.

Then the following matrices describe the above dynamics.

$$ A = \bmat{ 1 & 0 & \left(1-0.5\gamma\Delta t\right)\Delta t & 0 \\ 0 & 1 & 0 & \left(1-0.5\gamma\Delta t\right)\Delta t \\ 0 & 0 & 1-\gamma\Delta t & 0 \\ 0 & 0 & 0 & 1-\gamma\Delta t } \\ B = \bmat{ 0.5\Delta t^2 & 0 \\ 0 & 0.5\Delta t^2 \\ \Delta t & 0 \\ 0 & \Delta t } $$

We consider the finite horizon of $T=50$, with $\Delta t=0.05$, and $\gamma = 0.05$.

Instead of the terminal constraints, $x_N = x_\text{des}$, we will impose constraints on positions and pass angles at several intermediate times, $t_1 < t_2 < \cdots < t_K = N$. More specifically, we will let $w_1, w_2, \dots, w_K\in\R^2$ be the $K$ waypoints, by which the vehicle needs to pass at $t_1, t_2, \dots t_K$, and let $\alpha_1, \alpha_2, \dots, \alpha_K\in\R$ be the $K$ pass angles, with which the vehicle's velocity vectors need to be aligned at $t_1, t_2, \dots t_K$. In other words, for every $k=1,2,\dots,K$, we have the following constraints.

$$ \begin{aligned} p_{t_k} &= w_k \\ \angle v_{t_k} &= \alpha_k \end{aligned} $$

So the waypoint guidance problem with pass angle constraints can be formulated as follows.

$$ \begin{aligned} \underset{u}{\minimize} \quad & \sum_{t=0}^{N-1} \|u_t\|^2 \\ \text{subject to} \quad & x_{t+1} = Ax_t + Bu_t, \qquad & t=0,\dots,N-1\\ \quad & p_{t_k} = w_k, \qquad & k=1,\dots, K \\ \quad & \angle v_{t_k} = \alpha_k. \qquad & k=1,\dots, K \end{aligned} $$

Now, the problem is here. The vehicle is initially at $(10,-20)$ with initial velocity of $(30,-10)$, and we impose the waypoint and the pass angle constraint at $(t_1, t_2, t_3) = (300, 600, 1000)$ as follows.

$$ \begin{aligned} w_{300} &= (100,0) \\ w_{600} &= (50,50) \\ w_{1000} &= (150,80) \\ \alpha_{300} &= 135 \deg \\ \alpha_{600} &= 60 \deg \\ \alpha_{1000} &= 0 \deg \\ \end{aligned} $$

Find the optimal control and the optimal trajectory. Your solution should look like below.