Optimal Trajectory Through Elevation Field $E(x,y)$ (Flattest Path through Hills)

Question

I would like to find an optimal path through an area with hills, i.e. the flattest trajectory. I would like to model this as an optimal control problem, and was wondering if I could base it on fuel consumption problem for lunar landers. In such problems the system is defined like,

$$ h'(t) = v(t) $$

$$ v'(t) = -g -k(\frac{u(t)}{m(t)}) $$

$$ m'(t) = u(t) $$

$h$ is the height of the lander, $v$ velocity, and $u$ outside control, the thrust whose value effects upward velocity and how much fuel is spent (the 2nd, and 3rd equations). The measure to be optimized is the fuel consumption

$$ J = - \int_{0}^{b} m'(t) dt $$

Link

I was wondering if I could use a similar approach for hiking trajectory optimization. The elevation data (hills) will be available, say $E(x,y)$, the movement is obviously through 3D space.

The idea is seperating $v_x$, $v_y$, $v_z$.. and writing motion equations for each, for $z$ axis there would be $g$ to fight against, for $x,y$ there is friction $f$ (while walking). $u$ would also be three dimensioal, $u = (u_x,u_y,u_z)$. All these would subtract from a person's "fuel", that is decrease its $m$. The cost is like above, minimizing fuel which I am guessing would favor trajectories away from hills and try to make a path as short as possible.

Few differences, in my case fuel is consumed in proportion to $g \cdot \partial E / \partial z$ for vertical and $f \cdot x'(t)$, $f \cdot y'(t)$ for horizontal.

My constraint is little different as well, there is a set end-point, $x(b),y(b),z(b) = x_f,y_f,z_f$, $(x_f,y_f,z_f)$, the goal. Time can be free, or constrained, I believe they would both work. Lunar lander constrains for $v(b)=0$ meaning soft landing.

How would I model such a problem, using a seperate variables for each, like above, or using vectors?

It seems like I can put together a functional, use Lagrange multipliers creating a combined results, use Euler-Lagrange on it and solve the resulting ODE numerically. Does this approach make sense? Any advice on the formulation of the problem, or pointers to a similary finished system?

Note: I left $E(x,y)$ undefined, just indicated it is differentiable. I have a model for $E$, "the hills" using RBF, which is

$$ E(\bar{x}) = \sum_{n=1}^N \exp (-\gamma || \bar{x}-x_n ||^2 ) $$

For details see here.

Some other problems that might be useful as a starting point and solved with Optimal Control are:

1) This question models velocity with simple $v(x) = \sqrt{x^2+y^2}$. I would have to still have to model "multiple hills" effecting a certain location, so multiple parameterized $v_i$'s need to be added up.. Or inverting $E$ so higher elevation results in lower velocity? But the basic approach makes sense, defining a functional for time, which is effected by the velocity field, and integrating over it minimizing through Euler-Lagrange. I am not wedded to the fuel minimization angle for this problem.

2) A ship's optimal movements through a current field (can be different at each $x,y$) is shown in Bryon and Ho's book. Control parameter is $\theta$. My elevation field $E$ could be converted into "water currents" that are pushing out therefore discouraging certain locations is one idea. The gradient $\nabla E$ is an obvious choice.

3) Same book, now for wind, but using vector notation throughout.

4) Here is a gentleman HJ Westergard who uses PDEs and fast marching for "orienteering" purposes to solve the flattest path problem.

5) Another paper here talks about how a helicopter control can be modeled to avoid obstacle areas, take into account wind, through control theory.

Answer 1

This kind of optimization problem is pretty well studied, and it more or less comes down to how "nice" your landscape is and how your cost function depends on the landscape/path. To pose it generally, we can define a few things:

The elevation field $E:\mathbb{R}^2\to\mathbb{R}$

The initial and final positions

The path $\vec{x}:[t_1,t_2]\to\mathbb{R}^2$ with fixed endpoints $\vec{x}(t_1)=\vec{x}_1,\ \vec{x}(t_2)=\vec{x}_2$

A cost function $\mathcal{C}[\vec{x}]$ to minimize. We generally wish to write this as an integral over $\vec{x}$ of some function $L$ (the Lagrangian) which depends only on local quantities. For a hiker it seems sensible to stop at one derivative; inertia is generally not an important factor for walking if we're smoothing out person-scaled stuff. It may or may not be useful to have dependence on velocity. We can write down this (more or less) general form $$ \mathcal{C}[\vec{x}]=\int_{t_1}^{t_2}L(\vec{x}(t),\dot{\vec{x}}(t))dt $$ As an example, we may expect the cost to only depend on the horizontal and vertical speed, in that case, we can write the Lagrangian in terms of a function $f$ depending on only those: $$ \mathcal{C}_0[\vec{x}]=\int_{t_1}^{t_2}f(\|\dot{\vec{x}}(t)\|, \vec{\nabla}E\cdot\dot{\vec{x}}(t))dt $$ This is more or less the standard starting point for the Euler-Lagrange relation, yielding the following equations of motion for locally optimal paths. $$ \frac{d}{dt}\left(\frac{\partial L}{\partial\dot{\vec{x}}}\right)+\frac{\partial L}{\partial\vec{x}}=0 $$ Of course, for numerically determining the optimal path given endpoints, the above ODE is not particularly useful. We can approximate the locally optimal path from initial conditions, but we cannot easily find a path which terminates at the desired destination, and even if we do, there is no guarantee of a global optimum. Geodesics on a sphere/torus are good examples of this kind of non-optimality.

One way of getting at the optimal solution from here Hamilton-Jacobi-Bellman Equation, which requires solving a partial differential equation globally (essentially solving EL for all initial conditions), which in turn allows the globally optimal path to be generated (under certain smoothness/solvability conditions). This approach is standard in these types of continuous control problems, but also somewhat involved.

One more numerically motivated method for this is to instead solve the optimization problem directly with e.g. gradient descent, aided by discretization/interpolation of the path.

As an example of this type of method, suppose the path consists of points $\vec{x}_0,...,\vec{x}_N$ corresponding to times $0,T,2T,...,NT$ with $\vec{x}_0,\vec{x}_N$ fixed and $T$ free. We can approximate the cost function in lots of ways: here's a simple expression for the trapezoid rule, though less primitive interpolation methods can be treated in the same way: $$ \mathcal{C}[\vec{x}]=\frac{T}{2}\sum_{i=0}^{N-1}\left[L\left(\vec{x}_i,\frac{\vec{x}_{i+1}-\vec{x}_i}{T}\right)+L\left(\vec{x}_i+1,\frac{\vec{x}_{i+1}-\vec{x}_i}{T}\right)\right] $$ We can directly calculate the gradient of this expression, provided we know the derivatives of the Lagrangian. $$ \frac{\partial\mathcal{C}}{\partial\vec{x}_i}=\frac{T}{2}\frac{\partial L}{\partial\vec{x}}\left(\vec{x}_i,\frac{\vec{x}_{i-1}-\vec{x}_i}{T}\right)+\frac{T}{2}\frac{\partial L}{\partial\vec{x}}\left(\vec{x}_i,\frac{\vec{x}_{i+1}-\vec{x}_i}{T}\right)+\frac{1}{2}\frac{\partial L}{\partial\dot{\vec{x}}}\left(\vec{x}_{i-1},\frac{\vec{x}_{i}-\vec{x}_{i-1}}{T}\right)+\frac{1}{2}\frac{\partial L}{\partial\dot{\vec{x}}}\left(\vec{x}_{i},\frac{\vec{x}_{i}-\vec{x}_{i-1}}{T}\right)-\frac{1}{2}\frac{\partial L}{\partial\dot{\vec{x}}}\left(\vec{x}_i,\frac{\vec{x}_{i+1}-\vec{x}_i}{T}\right)-\frac{1}{2}\frac{\partial L}{\partial\dot{\vec{x}}}\left(\vec{x}_{i+1},\frac{\vec{x}_{i+1}-\vec{x}_i}{T}\right) $$ $$ \frac{\partial\mathcal{C}}{\partial T}=\frac{\mathcal{C}}{T}-\frac{1}{2T}\sum_{i=0}^{N-1}\left[\frac{\partial L}{\partial\dot{\vec{x}}}\left(\vec{x}_i,\frac{\vec{x}_{i+1}-\vec{x}_i}{T}\right)(\vec{x}_{i+1}-\vec{x}_i)+\frac{\partial L}{\partial\dot{\vec{x}}}\left(\vec{x}_i+1,\frac{\vec{x}_{i+1}-\vec{x}_i}{T}\right)(\vec{x}_{i+1}-\vec{x}_i)\right] $$ For sufficiently large $N$, (compared to the roughness of the landscape), descending along this gradient will converge to the "closest" approximately stationary path given the endpoints. The speed with which these paths can be calculated allows the set of stationary paths to be sampled many times by initializing random starting paths and minimizing, eventually finding the best (or at least better than typical) path. Of course, designing a sufficiently labyrinthine landscape will give this method problems, and in any case, the sampling protocol can become very important if there are many paths and the optimal one is "hard" to find.

There's a huge amount of literature on this type of problem ("optimal control" or perhaps "continuous shortest path" are the standard terminology as far as I know), and there are plenty of sources which go into much, much more depth.