# How to drive a vehicle (limited by acceleration) on a flat ground to a given point as fast as possible?

So I have a function $\mathbf{x}(t): \mathbb{R} \rightarrow \mathbb{R}^2$, which is supposed to mean the path of the vehicle (time mapped to position). The initial conditions $\mathbf{x}(0)$ and $\dot{\mathbf{x}}(0)$ (the derivative, the velocity) are known. The acceleration is limited by vehicle's traction so it cannot exceed a certain value $a$ so $\left|\ddot{\mathbf{x}}(\cdot)\right| \le a$. The vehicle is capable to omnidirectional movement, so it can accelerate in any direction without having to turn first. The goal is getting to the given point $P$ as fast as possible and stop there, so $\mathbf{x}(t) = P$ and $\dot{\mathbf{x}}(t) = \mathbf{0}$, with minimal $t$.

How to control the vehicle to achieve this goal?

What I found on the internet so far deals with the one dimensional case with a simple bang-bang control.

The current solution first attempts to eliminate the tangential velocity component, turning the problem into an one dimensional one then using the solution of that. But I don't think that's optimal, I guess there is a better way to do this.

I faced this problem while designing an RTS game so I'm pretty new to optimal control and looking for pointers on how to solve this problem.

• This is a duplicate of math.stackexchange.com/questions/1463943/… I have answered this question therein. The only difference is the bound on $u$.
– RTJ
Commented Dec 5, 2015 at 15:49
• @CTNT It looks like your solution is for 1 dimensions only. In my case there are at least 2 controls: one for the length of the acceleration and another for direction. Commented Dec 5, 2015 at 17:08
• Sorry, I did not notice that $x\in\mathbb{R}^2$. However, the solution can be easily extended to higher-dimensional since there is no coupling in the dynamics.
– RTJ
Commented Dec 5, 2015 at 17:48
• On a second thought your constraint makes the problem harder. If you only wanted $|\ddot{x_i}|\leq a$ for both $i=1,2$ then the solution is a simple generalization as I described above. But now you have $\ddot{x}_1^2+\ddot{x}_2^2\leq a^2$ which is a more difficult problem.
– RTJ
Commented Dec 5, 2015 at 18:31
• How about solving it numerically? For instance, by MPC or even some sorts of dynamic programming suitable for "online" applications? I think you'd be having a hard time trying to figure out convexity of your optimization problem. Regarding obstacles, you should google in the direction of path finding and reachability analysis. I am not sure that the latter is used in any game though. Commented Dec 7, 2015 at 10:51

A simple bang-bang controller is indeed optimal for minimum-time problems, and it's a well known result of Pontryagin's Minimum Principle. If you want the derivations, that's the place to start.

I guess the caveats you might run into is that you're in 2-D and perhaps have obstacles that you need to avoid (or maybe even a maze). This will still result in bang-bang, you'll just get a path that avoids obstacles with bang-bang.

• So, the vehicle should do full throttle/full brake all the time? Is that what you suggest? Commented Dec 7, 2015 at 13:06
• If minimum time is what you care about, yes. Note that if you can do "full throttle in the opposite direction", then that is the lower bang. Commented Dec 8, 2015 at 0:26

$\newcommand{\v}[1]{\mathbf{#1}}$

Using the Pontryagin's Maximum Principle I was able to characterize the optimal control. But I believe it's not possible to get a formula for the control parameters algebraically, only numerically.

Let $\v x, \v v, \v a \in \mathbb{R}^n$, denote the position, velocity and acceleration respectively. Let $t \in \mathbb{R}$ the time. $t=0$ is the start. $t = \tau$ is the moment when system system reach the desired target state.

Let $\v X \in \mathbb{R}^{2n}$ denote the state of the system. The state contains the position and velocity of the vehicle:

$$\v X = (\v x, \v v)$$

The state equation will be:

$$\frac{d\v X}{dt} = (\v v, \v a)$$

Let $r \in \mathbb{R}$ be the running payoff (rate we gain or lose score). In this time optimal problem $r = -1$, as we are losing score in a constant rate as time elapses.

Let $\v P \in \mathbb{R}^{2n}$ the costate of the system. $\v P = (\v p_1, \v p_2)$.

Then we can write up the control Hamiltonian of the system:

$$H = \v V \cdot \v P + r$$

Expanding:

$$H = \v v \cdot \v p_1 + \v a \cdot \v p_2 - 1$$

The optimal control maximizes the $H$. We control the $\v a$. To maximize $H$, we must maximize the $\v a \cdot \v p_2$ term. When the vector lengths are fixed the dot product is maximal if the two vector points to the same direction. We also control the length. So it should be as long as possible. So it should be $a_{max}$ all the time.

This way $\v a = a_{max}\frac{\v p_2}{|\v p_2|}$

From the Hamiltonian mechanics, the rate of change of the co-state is given by the formula:

$$\frac{d\v P}{dt} = - \nabla_{\v X} H$$

So it's the vector formed by the partial derivatives of $H$ with respect to the elements of the $\v X$.

In our particular case:

$$\frac{d\v P}{dt} = (0, -\v p_1)$$

This yields a fairly simple general solution for $\v P$:

$$\v P(t) = (\v q, \v p - \v qt)$$

Where $\v p, \v q \in \mathbb{R}^n$ are the initial control parameters.

This way the acceleration we control given by:

$$\v a = a_{max}\frac{\v p - \v qt}{|\v p - \v qt|}$$

This won't be a bang-bang control in general.

While $\v a$ can be integrated twice with respect to time in closed form. The result is quite messy. $\v p$ and $\v q$ are inside square roots and $\mathrm{arsinh}$ functions. Using the initial and terminal conditions we can write up 4 vector equations, plus one where we lock the length of $\v q$ to 1 (as both $\v p$ and $\v q$ can be scaled up with and arbitrary constant without changing anything). Although and there are enough equations to determine all the variables I don't believe a closed form formula exists for it. So the only solution is a numeric iterative solution. Not something I would do in real time 25 times a second for thousands of vehicles in the simulation...