# What is the best path for a given initial and final state?

I am trying to calculate an efficient acceleration curve given starting and final positions and velocities. I'm assuming no friction, and that the acceleration can be applied in any direction at any time.

Given:

• $p_0$ = starting position
• $v_0$ = starting velocity
• $p_f$ = final position
• $v_f$ = final velocity
• $T$= total time

I want to find a nice $a(t)$ function that will produce final conditions.

So far I have the following solution that works, but produces an incredibly inefficient $a(t)$ curve:

First I calculate the constant acceleration required to get from $v_{0}$ to $v_{f}$ :

$$a_v = \cfrac{v_f - v_{0}}{T}$$

Then I calculate the change in position this acceleration will create over $T$ :

$$p_v = \cfrac{1}{2} a_v T^2$$

Next I calculate the average velocity required to get from $p_{0}$ to $p_f$ and to counteract $p_v$ :

$$v_p = \cfrac{p_f - (v_{0} + p_f ) }{ T }$$

Next I calculate the acceleration needed to produce this average velocity over the total time:

$$a_p= \cfrac{2 v_p}{ T}$$

Finally, I add twice that acceleration to the first half of my acceleration function, and subtract twice that acceleration from the second half. This produces the net position change that I want, but has a net $0$ velocity/acceleration change so $v_f$ stays correct:

$$a(t) = \begin{cases} a_v + 2 a_p & t \leq \frac T 2 \\ a_v - 2 a_p & t > \frac T 2 \end{cases}$$

While this solution provides a result, it can cause the simulated objects I'm working with to move backward before moving toward their final goal, along with other weird behavior. I think the ideal solution would minimize total acceleration applied over time (and thus force, since the mass of the object will stay constant over this time).

I know that the constraints on this problem are that the integral of $a(t)$ must equal $v_f - v_{0}$, and that the integral of that integral must equal $p_f - p_0$ . I just don't know how to setup the problem to solve for those constraints. I don't even really know what I should Google for to try and solve this problem. Any help would be greatly appreciated.

-
It sounds like you want the path linking your initial and final conditions that minimizes $\int |a(t)|^2 dt$. This is a calculus of variations problem that I believe you can solve with the Euler-Lagrange equations, but I am too rusty. Does this make sense as what you are looking for? – Ross Millikan Jun 28 '14 at 22:36

Here's my messy approach. It's not ideal, but it may work reasonably well:

Approximate the position function $x(t)$ as a fourth degree polynomial:

$$x(t) = at^4 + bt^3 + ct^2 + dt + e$$

This way, you have one extra degree of freedom to manipulate, which you can use to minimize unnatural motion. Let's assume the motion starts at $t = 0$ and ends at $t = T$.

Then $x(0) = p_0 = e$ and $x'(0) = v_0 = d$, so we can write:

$$x(t) = at^4 + bt^3 + ct^2 + v_0 t + p_0$$

For the remaining three variables, we write a matrix system. Here's the augmented matrix:

$$\begin{bmatrix} T^4 & T^3 & T^2 & p_f - p_0 - v_0 T \\ 4 T^3 & 3 T^2 & 2 T & v_f - v_0 \end{bmatrix}$$

With Mathematica, I row-reduced the above to

$$\begin{bmatrix} 1 & 0 & -1/T^2 & \frac{3p_0 - 3p_f + 2 v_0 T + v_f T}{T^4} \\ 0 & 1 & 2/T & \frac{-4p_0 + 4p_f - 3 v_0 T - v_f T}{T^3} \end{bmatrix}$$

For simplicity, I'll define $\alpha = \frac{3p_0 - 3p_f + 2 v_0 T + v_f T}{T^4}$ and $\beta = \frac{-4p_0 + 4p_f - 3 v_0 T - v_f T}{T^3}$.

This system tells us that as long as we ensure $a = \alpha + c/T^2$ and $b = \beta - 2c/T$, any value of $c$ will result in a polynomial $x(t)$ that matches your conditions.

You can then choose $c$ based on any number of criteria. For example, minimizing acceleration yields $c = -\alpha T^2$ and Floris' result, although this may sometimes result in retrograde motion.

-
Thanks so much for the quick answers! I'll try these out in my simulation and play around with values of c. It's been a long while since I've solved this kind of system, but your work above is ringing some bells and should be able to take it from here. – generesque Jun 29 '14 at 0:29
I like this approach. Clever idea to add just one more degree of freedom. Not sure it is enough to prevent retrograde motion (think of case with rapid initial and final velocity but a long time in between) but better than just a spline for sure. – Floris Jun 29 '14 at 11:19

Let us try Ross Millikan's suggestion, cf. his comment above: Let us minimize the (higher-order) functional

$$\tag{1} S~:=~ \frac{1}{2}\int_{t_i}^{t_f} \!dt~ a^2, \qquad a~\equiv~\dot{v},\qquad v~\equiv~\dot{x},$$

with fixed endpoints

$$\tag{2} x(t_i)~=~x_i , \qquad v(t_i)~=~v_i , \qquad x(t_f)~=~x_f , \qquad v(t_f)~=~v_f ,$$

and with fixed initial and final times, $t_i$ and $t_f$. Let $j\equiv\dot{a}$ denote the jerk. An infinitesimal variation of the functional $S$ yields

$$\delta S~=~\int_{t_i}^{t_f} \! dt~ a \delta a~\stackrel{\text{int. by parts}}{=}~ \int_{t_i}^{t_f} \!dt~\frac{dj}{dt} \delta x +\left[ a \underbrace{\delta v}_{=0} - j \underbrace{\delta x}_{=0}\right]^{t_f}_{t_i}$$ $$\tag{3} ~\stackrel{(2)}{=}~\int_{t_i}^{t_f} \!dt~\frac{dj}{dt} \delta x.$$ The higher-order Euler-Lagrange equation becomes

$$\tag{4} \frac{dj}{dt}~=~0,$$

cf. the fundamental lemma of calculus of variations. In other words, the jerk

$$\tag{5} j~=~A$$

should be a constant of motion; the acceleration

$$\tag{6} a(t)~=~At+B$$

should be an affine function in $t$; the velocity

$$\tag{7} v(t)~=~\frac{A}{2} t^2+ Bt +C$$

is a quadratic polynomial in $t$; while the position

$$\tag{8} x(t)~=~\frac{A}{6} t^3+ \frac{B}{2}t^2+Ct +D$$

is a cubic polynomial in $t$. Thus we arrive at the (cubic) spline fitting problem mentioned in Floris' answer: Four linear equations (2) with four unknowns $(A,B,C,D)$. To solve them, notice the similarity between the two formulas

$$\tag{9} \frac{v_i+v_f}{2}~\stackrel{(2)+(7)}{=}~\frac{A}{4}(t_i^2+t_f^2)+B\frac{t_i+t_f}{2}+C$$

and

$$\tag{10} \frac{\Delta x}{\Delta t}~\stackrel{(2)+(8)}{=}~\frac{A}{6}(t_i^2+t_it_f+t_f^2)+B\frac{t_i+t_f}{2}+C.$$

The difference of (9) and (10) depends only on one unknown $A$! Here we have defined

$$\tag{11} \Delta t ~:=~ t_f-t_i, \qquad \Delta x ~:=~ x_f-x_i\quad\text{and} \quad\Delta v ~:=~ v_f-v_i.$$

The jerk and average acceleration become

$$\tag{12} j~\stackrel{(5)}{=}~A~\stackrel{(9)+(10)}{=}~6\frac{v_i+v_f}{(\Delta t)^2}-12\frac{\Delta x}{(\Delta t)^3}$$

and

$$\tag{13} \langle a\rangle ~:=~\frac{\int_{t_i}^{t_f} \! dt~ a}{\Delta t} ~\stackrel{(6)}{=}~A\frac{t_i+t_f}{2}+B~\stackrel{(2)+(7)}{=}~\frac{\Delta v}{\Delta t},$$

respectively. Similarly, the remaining unknowns $B$, $C$, and $D$ can now easily be determined uniquely in terms of the boundary conditions (2).

-
I like this approach. Could you describe more in detail how to get from $(3)$ to $(4)$? I assume one has to integrate $\delta S$ first to obtain the Lagrangian subsequently to apply the E-L- equation. Am I right? – Statics Sep 16 '15 at 16:11
The step $(3)\Rightarrow (4)$ uses the fundamental lemma of calculus of variations. I updated the answer. – Qmechanic Sep 16 '15 at 16:20

This is a very heavily constrained problem - and it sounds a lot like a spline fitting problem. As you may know, a spline is a curve that has continuous third derivative - and as a side effect, it minimizes the curvature as it connects points. In this case, the problem would then be re-stated as a spline that goes through the two points given, with the slope given. The smoothest acceleration / deceleration would follow from that spline fit.

So if we draw a diagram:

We can see that a spline may well be what you are after. So - how do we solve the set of equations? We know the spline should be a cubic - and it so happens we have four constraints:

position at t1
position at t2
velocity at t1
velocity at t2


I am sure you can solve four equations with four unknowns… but if you can't, let me know and I will try to write the solution later tonight. Or Google "spline fit".

-

I was thinking you could do a Bezier curve inspired approach (but with quadratic interpolation instead of linear interpolation). You know that at the beginning of time you want the path to look like $$p_0 + v_0 t$$ and at the end of the trajectory you want it to look like $$p_f + v_f (t-T)$$ So, let's try to smoothly interpolate between these two trajectories. In order to not screw up the derivatives, we will interpolate between them with a quadratic obtaining a function of the form $$f(t) = \left( 1 - \left(\frac t T\right)^2 \right) ( p_0 + v_0 t ) + \left( \frac t T \right)^2 \left( p_f + \alpha ( t - T ) \right)$$ Where I have left $\alpha$ because that will be determined by setting the derivative at $t =T$ equal to $v_f$, giving us $$\alpha = v_f + 2 v_0 + \frac 2 T ( p_0 - p_f )$$

Using this interpolation, we get paths that look pretty good as far as I can tell, for example in two dimensions we get things that look like:

or:

This should be equivalent to the other cubic approaches mentioned.

As another alternative, I was thinking about this problem some more, and if you want to try to prevent some of the retrograde motion, it would help to add a cost term proportional to the kinetic energy of the path as well. This ought to lead to paths that are more physical, turning the problem into one more like traditional trajectory optimization. So in particular, we try to minimize the objective: $$S = \int dt\, \frac{\alpha^2}{2} \dot x^2 + \frac 12 \ddot x^2$$ subject to the constraints $$x(-1) = x_0 \quad x(1) = x_f \quad \dot x(-1) = v_0 \quad \dot x(1) = v_f$$ it will be convenient for me to imagine time as running from $t=-1$ to $t=1$. Since this objective is quadratic we can solve it exactly with variational calculus, just like Qmechanic did for the case that minimizes just the acceleration. Taking a variation, we obtain $$\delta S = \int dt\, \alpha^2 \dot x \delta \dot x + \ddot x \delta \ddot x$$ and we can integrate by parts and obtain $$\delta S = \int dt\, \left( \ddddot x - \alpha^2 \ddot x \right) \delta x = 0$$ which gives us an ordinary differential equation for $x$, which we can solve. Notice that for the acceleration the diff eq is the equation for an exponential. To make it slightly easier to specify the solution I'll choose $\sinh$ and $\tanh$ for the solution basis. $$\ddot x = A \sinh(\alpha t) + B \cosh(\alpha t )$$ which we can integrate twice to get the position itself. $$x = \frac{A}{\alpha^2} \sinh(\alpha t) + \frac{ B}{ \alpha^2 } \cosh(\alpha t) + C t + D$$ and our boundary conditions give us the solutions for $A,B,C,D$, we obtain $$B = \frac{ \alpha \Delta v}{2 \sinh \alpha}$$ $$D = \bar x - \Delta v \frac{ \coth \alpha }{ 2 \alpha }$$ $$A = \alpha^2 \frac{ \bar v - \frac 12 \Delta x }{ \alpha \cosh \alpha - \sinh \alpha }$$ $$C = \frac{ \frac \alpha 2 \Delta x \cosh \alpha - \bar v \sinh \alpha }{ \alpha \cosh \alpha - \sinh \alpha }$$ where $$\Delta x = x_f - x_0 \quad \bar x = \frac 12 ( x_0 + x_f )$$ $$\Delta v = v_f - v_0 \quad \bar v = \frac 12 ( v_0 + v_f )$$

As an example, below I show the trajectories that result for $\alpha \in ( 0.1, 1, 3, 10, 100 )$ for $x_0 = 0, x_f = 1, v_0 = 5, v_f = 5$. This is an example for which minimizing the acceleration alone results in retrograde motion.

In the limit that $\alpha \to 0$ we recover the other solutions, and in the limit that $\alpha \to \infty$ our particles just move at constant velocity from one point to the other, with large kicks in the velocity at the start and end.

I thought I'd offer this as another alternative.

-

This question has a nice answer. It was found by John Hobby in his paper Smooth, Easy to Compute Interpolating Splines and it is the system used by Metapost to create pleasing trajectories.

To summarize the results of the paper, the goal is to find an aesthetically pleasing path $z(t)$ (in 2d) subject to the constraints $z(0) = z_0$, $z(1) = z_1$, $z'(0) = v_0$, $z'(1) = v_1$. To do so, he uses a parameter $\tau$, the tension (which in the paper is general between points but we'll treat it as the same for both end points since you presumably want multiple points connected together).

The solution is given by the following: $$z(t) = z_0 + \begin{pmatrix} x_1 - x_0 & y_0 - y_1 \\ y_1 - y_0 & x_1 - x_0 \end{pmatrix} \cdot \hat z(t)$$ $$\hat z(t) = \frac{\rho}{\tau} t (1-t)^2 \left\{ \cos \theta, \sin \theta \right\} + t^2 ( 1- t) \left\{ 3 - \frac{\sigma \cos \phi}{\tau} , \frac{\sigma \sin \phi}{\tau} \right\} + t^3 \left\{ 1 , 0 \right\}$$ where $$\theta = \arg v_0 - \arg ( z_1 - z_0 ) \qquad \phi = \arg ( z_1 - z_0 ) - \arg v_1$$

$$\rho = \frac{ 2 + \alpha }{ 1 + (1-c) \cos \theta + c \cos \phi }$$ $$\sigma = \frac{ 2 - \alpha }{ 1 + (1-c) \cos \phi + c \cos \theta }$$ $$\alpha = a ( \sin \theta - b \sin \phi )(\sin \phi - b \sin \theta)(\cos \theta - \cos \phi)$$ and $$a = \sqrt 2 \quad b = \frac 1{16} \quad c = (3 - \sqrt 5 )/2$$

An an example, here:

I show some examples. On the left I show the effect of the tension parameters for fixed initial and final state and on the right I show some curves generated for a tension of 2 as I vary the final velocity.

Based on what you've said I'd recommend a tension of 2 or so.

-