Optimal Control using Dynamic Programming - Optimizing for Furthest Distance

So I have been investigating a problem to get a glider with control of its elevator to fly as far as possible from any given initial state. To keep this simple, we will view this in 2D space with the following differential equation:

\begin{align} \dot{\boldsymbol{q}} = \dot{\begin{bmatrix} x \\ y \\ \theta \\ \phi \\ \dot{x}\\ \dot{y}\\ \dot{\theta} \end{bmatrix}} &= \begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot{\theta} \\ u \\ -\left(f_w \sin\theta + f_e \sin\left(\theta + \phi\right)\right)m^{-1} \\ \left(f_w \cos\theta + f_e \cos\left(\theta + \phi\right)\right)m^{-1} - g\\ \left(f_e (l \cos\phi + l_e ) - f_w l_w\right) I^{-1} \end{bmatrix} \\ \end{align}

and the following relationships:

\begin{align} f_w &= \rho S_w |\dot{\boldsymbol{x}}_w|^2 \sin\alpha_w \\ f_e &= \rho S_e |\dot{\boldsymbol{x}}_e|^2 \sin\alpha_e \\ \alpha_w &= \theta - \tan^{-1}\frac{\dot{y}_w}{\dot{x}_w} \\ \alpha_e &= \theta + \phi - \tan^{-1}\frac{\dot{y}_e}{\dot{x}_e} \\ \dot{x}_w &= \dot{x} + l_w \dot{\theta} \sin\theta \\ \dot{y}_w &= \dot{y} - l_w \dot{\theta} \cos\theta \\ \dot{x}_e &= \dot{x} + l\dot{\theta}\sin\theta + l_e\left(\dot{\theta} + \dot{\phi}\right)\sin\left(\theta + \phi\right)\\ \dot{y}_e &= \dot{y} - l\dot{\theta}\cos\theta - l_e\left(\dot{\theta} + \dot{\phi}\right)\cos\left(\theta + \phi\right) \\ \dot{\boldsymbol{x}}_w &= \dot{x}_w \hat{e}_x + \dot{y}_w \hat{e}_y \\ \dot{\boldsymbol{x}}_e &= \dot{x}_e \hat{e}_x + \dot{y}_e \hat{e}_y \\ \end{align}

where $u$ is the control, essentially a choice for $\dot{\phi}$, $\phi$ is the relative elevator angle with respect to the pitch angle $\theta$, $x$ and $y$ are the horizontal and vertical positions, $\dot{x}$ and $\dot{y}$ are the horizontal and vertical speeds, $\dot{\theta}$ is the angular velocity of the glider, $f_w$ is the net aerodynamic force magnitude from the main wing, $f_e$ is the net aerodynamic force magnitude from the elevator wing, $g$ is gravitational acceleration constant, and the other constants are tied to glider physical traits.

The values being used for the various constants are the following:

\begin{align} m &= 0.05 \\ g &= 9.81 \\ \rho &= 1.292 \\ S_w &= 0.1 \\ S_e &= 0.025 \\ I &= 6 \cdot 10^{-3} \\ l &= 0.35 \\ l_w &= -0.03 \\ l_e &= 0.05 \end{align}

and the initial condition I primarily use to test is the following:

\begin{align} \boldsymbol{q}_0 = \begin{bmatrix} x_0 \\ y_0 \\ \theta_0 \\ \phi_0 \\ \dot{x}_0\\ \dot{y}_0\\ \dot{\theta}_0 \end{bmatrix} &= \begin{bmatrix} 0 \\ 2 \\ 0 \\ 0 \\ 6 \\ 0\\ 0 \end{bmatrix} \\ \end{align}

I am experimenting with using Dynamic Programming to tackle this problem when $u$ is constrained such that $-1 \leq u \leq 1$. Since Dynamic Programming is memory intensive for large state spaces, I recognized that for an optimal distance controller, I don't actually need the first two states, $x$ and $y$. With this change, I defined $\boldsymbol{q} = \lbrack \theta, \phi, \dot{x}, \dot{y}, \dot{\theta} \rbrack^T$ along with the associated differential equations truncation. I also define the discrete dynamical system using the following:

\begin{align} \boldsymbol{q}_{k+1} &= \boldsymbol{q}_k + \Delta t \dot{\boldsymbol{q}}\left(\boldsymbol{q}_k, u_k\right)\\ &= f\left(\boldsymbol{q}_k,u_k\right) \end{align}

To go along with this change, I made the overall optimization problem to maximize the following:

\begin{align} V &= \sum_{i=1}^N \Delta t \dot{x}_i - \gamma \dot{y}^2_i\\ \text{subject to}& \begin{matrix} -1 \leq u_k \leq 1 \\ \boldsymbol{q}_{k+1} = f\left(\boldsymbol{q}_k,u_k\right) \end{matrix} \end{align}

because the cost function should approximate the value for $x$ at the end of a flight, which is obviously what I would have optimized if using the full system of equations.

After doing some experiments, it seems the cost function chosen doesn't really work well for reasons I am unsure of. However, if I change the cost function to the following, it performs much better:

\begin{align} V &= \sum_{i=1}^N \theta_i^2 + \gamma \dot{\theta}_i^2 \end{align}

for some $0 \leq \gamma \lt 1$. I chose this second cost function thinking one thing that might help a long flight is the glider remaining level instead of diving too soon and losing a lot of energy. It works decent, but I am still wondering why the first isn't doing too well.

With all this said, is there any problems with the first optimization formulation that stand out?

• Shouldn't there be some constraint like $y(t)\geq0$ or $y(T)=0$? In other words, could it plunge arbitrarily far down, get lots of speed, and then level out? – Kirill Jun 20 '17 at 19:38
• @Kirill You're correct that is possible if that was a superior trajectory. Technically I did add a $-\gamma\dot{y}^2$ term to the cost function in my code (which I did not write here at the time of posting) to hopefully keep it from making a massive vertical maneuver. Note that since I removed the $x$,$y$ terms in the dynamics, I don't have an explicit value for $y$ that I can use. When I did first try it with the full system, I also did constrain $y$ to be between some height and $0$. Still was not great. – spektr Jun 20 '17 at 19:52
• Have you tried solving the ODE-constrained optimization problem directly, then comparing with the good/bad solutions you get from dynamic programming? – Kirill Jun 20 '17 at 20:04
• @Kirill No I have not. Any recommended methods/software for doing that? The methods I think of for this sort of problem, other than DP, are typical optimal control numerical methods (shooting method, forward-backward-sweep, collocation). – spektr Jun 20 '17 at 20:21
• Since the problem looks small enough, does the Ponrtyagin minimum principle give you a closed form here, or help simplify the problem? – Kirill Jun 20 '17 at 20:55