0
$\begingroup$

I'm trying to create a high-level API for controlling a drone, where I tell it how far I want it to go (eg. moveForward(10)). The SDK allows me to give the drone an instantaneous velocity at 10 Hz (every 100ms) to control it. Currently, I have a naive solution of fixing a velocity (say $v = 1 ms^{-1}$) and then computing time as $t=\text{distance}/\text{velocity}$, then sending it the command of v velocity for t seconds every 100ms and after t seconds, I send it a command of 0 velocity. As expected, this causes a highly jerky motion, since the drone accelerates to the given speed instantly and then decelerates immediately to 0 at the end of t seconds.

I want to create a more smooth motion, with a lesser jerk. Reading around, I found out about S-curve profiles. I have gone through some math, and currently have the following equations to build an S-curve profile:

sigmoid = lambda x,a,c: 1/(1+exp(-a*(x-c))
for t in timesteps:
    v1 = sigmoid(t,a,c1)
    v2 = sigmoid(t,d,c2)
    v[i] = abs(v1 - v2) * v_max

Here a is the acceleration, d is the deceleration, c1 is the time at which the curve first becomes flat, c2 is the time at which the curve is flat for the last time and v_max is the maximum allowed velocity.

In mathematical notation:

$$ v(t) = V\left|\dfrac{1}{\mathrm{e}^{-D\cdot\left(t-C_2\right)}+1}-\dfrac{1}{\mathrm{e}^{-A\cdot\left(t-C_1\right)}+1}\right| $$

One of the curves generated this way is for example:

enter image description here

I can also get the position curve verses time using scipy's numeric integration on the array of velocities:

enter image description here

I wanted an analytical equation for position versus time, so I used an online anti-derivative calculator and got the analytical equation for the position. This equation is correct, since I compared it against the numerical solution and they are nearly identical. It is pretty dense, so not sure if I'm digressing here:

$$ P(t) = -\dfrac{V\cdot\left(\mathrm{e}^{D\cdot\left(t-C_2\right)}-\mathrm{e}^{A\cdot\left(t-C_1\right)}\right)\left(-A\ln\left(\mathrm{e}^{Dt}+\mathrm{e}^{C_2D}\right)+D\ln\left(\mathrm{e}^{-At}\cdot\left(\mathrm{e}^{At}+\mathrm{e}^{AC_1}\right)\right)+ADt\right)}{AD\left|\mathrm{e}^{D\cdot\left(t-C_2\right)}-\mathrm{e}^{A\cdot\left(t-C_1\right)}\right|} + C $$

Here C would be the integration constant, which I think is -P(0), since I want to consider the initial position to be $0$.

Even after all of this, the problem is not addressed, since I actually need to find t for which the final position is equal to the distance I tell the drone to travel. I thought of taking an inverse of $P(t) = k$ as $t = P^{-1}(k)$, but this approach comes with its own problems:

  1. The inverse of this function is going to be quite complex, and something I would like to avoid as much as possible.

  2. I am currently computing c1 and c2 as functions of t, namely c1 = 0.1*t and c2=0.9*t. I can probably replace c1 and c2 in the $v(t)$ equation as a function of time to get a purely time-based function, but again, that further makes the equation more complex.

While this is my current approach to solving the problem, I am not too happy with it. Is there another, simpler way to get the s-curve velocity profile for a given distance, where the time taken to complete the trajectory is flexible, or is there a way to simplify my current approach?

Edit 1: Trying to replace c1 and c2 as functions of t doesn't work because c1 and c2 are functions of the total time T and not the current timestep t, and T is what I need to solve for.

$\endgroup$

3 Answers 3

1
$\begingroup$

You were probably trying to avoid this and generate some kind of closed form solution, but as you show this is really burdensome.

They way I would solve this (which might not be the best way) would be to work in the discrete time domain:

$$ v[n] = v(nT;V,A,D,C_1,C_2) $$

$$ x[n] = \sum_{m=0}^{n}v[m]\,T $$

where $T$ is the discrete time (e.g. sampling) interval.

Then you want

$$ x[N] = \sum_{m=0}^{N}v[m]\,T $$

So just do an exhaustive search for the $N$ that yields $x[N]$ closest to some desired $x_{final}$.

Sorry in advance if I am missing something.

$\endgroup$
2
  • 2
    $\begingroup$ Hey. Thanks for your solution! I did manage to come up with a working solution. So I used the 3rd order equations of motion (with Jerk) and derived a piecewise function that satisfies my requirements. The continuous segments of the function depend on which time step I'm currently in. I'll upload that as an answer as soon as I'm sure it works. $\endgroup$
    – dkapur17
    Aug 7, 2022 at 15:19
  • $\begingroup$ Great! My only input would be to make sure that you are working with a time interval that is short enough. If you under-sample you can get into aliasing problems - but perhaps you are already on to this. Good luck! $\endgroup$
    – guero64
    Aug 7, 2022 at 15:25
0
$\begingroup$

I was able to derive a piecewise continuous velocity function that satisfied my needs, by going back to the basics (Newton's equations of motion). First, we need to analyse the curve that needs to be designed. The S-curve velocity can be broken down into 7 phases (and the corresponding accelerations in those phases):

enter image description here

As mentioned, we use Newton's equations of motion, but with the third derivative of displacement (constant jerk $j$):

$$ a = a_0 + jt\\ v = v_0 + a_0t + \frac{1}{2}jt^2\\ x = x_0 + v_0t + \frac{1}{2}a_0t^2 + \frac{1}{6}jt^3 $$

If interested, you can find the full derivation here.

Basically, the idea is to compute the time taken and distance covered in each phase. Phases 1, 3, 5 and 7 are constrained by the value of jerk and the maximum acceleration. Phases 2 and 6 are constrained by the value of acceleration, and Phase 4 has no constraints. As such, we can say whatever distance is not covered in phases 1,2,3,5,6 and 7, must be covered with constant velocity $v_{max}$ in phase 4. Using this approach, we can ensure the total distance covered is the same as the desired distance. Further, computing the time for each phase allows us to add up the times for all the phases and get the total time of flight. A detailed derivation can be found in the link above.

Skipping the derivation here, the final velocity curve obtained is a piecewise function depending on which time phase we are currently in:

$$ v(t)=\begin{cases} \frac{1}{2} jt^2 \qquad \text{in Phase 1}\\ v_{f_1} + a_{max}(t-t_1)\qquad \text{in Phase 2}\\ v_{f_2} + a_{max}(t - \sum_{i=1}^2 t_i) - \frac{1}{2}j (t-\sum_{i=1}^2t_i)^2\qquad \text{in Phase 3}\\ v_{f_3}\qquad \text{in Phase 4}\\ v_{f_4} - \frac{1}{2}j(t-\sum_{i=1}^4t_i)^2\qquad \text{in Phase 5}\\ v_{f_5} -a_{max}(t-\sum_{i=1}^5t_i)\qquad \text{in Phase 6}\\ v_{f_6} - a_{max}(t-\sum_{i=1}^6t_i) + \frac{1}{2}j(t-\sum_{i=1}^6t_i)^2\qquad \text{in Phase 7} \end{cases} $$

Here, $v_{f_i}$ is the velocity at the end of phase $i$ and $t_i$ is the time taken in phase $i$. The respective values are:

Velocities

  • $v_{f_1} = \frac{a_{max}^2}{2j}$
  • $v_{f_2} = v_{max} - \frac{a_{max}^2}{2j}$
  • $v_{f_3} = v_{max}$
  • $v_{f_4} = v_{max}$
  • $v_{f_5} = v_{max} - \frac{a_{max}^2}{2j}$
  • $v_{f_6} = \frac{a_{max}^2}{2j}$

Times

  • $t_1 = \frac{a_{max}}{j}$
  • $t_2 = \frac{(v_{f_2} - v_{f_1})}{a_{max}}$
  • $t_3 = \frac{a_{max}}{j}$
  • $t_4 = \frac{(a_{max}jD) - (v_{max}a_{max}^2) - (jv_{max}^2)}{ja_{max}v_{max}}$
  • $t_5 = \frac{a_{max}}{j}$
  • $t_6 = \frac{(v_{f_5} - v_{f_6})}{a_{max}}$
  • $t_7 = \frac{a_{max}}{j}$

How these values came to be can be found in the derivation.

Finally, based on the constraints of maximum velocity, acceleration and constant jerk, the created piecewise curve has a definite integral from $0$ to $T$ equal to the desired distance to cover $D$, where $T$ is the total time of flight, computed as $T = \sum_{i=1}^7 t_i$.

The idea for this approach came from this article.

$\endgroup$
0
$\begingroup$

As one of the options, you could use a version of the formula from Cye Waldman that I have changed to constrain maximum velocity and acceleration:

$$f\left( t, p \right)=\frac{1}{2}\left[ 1+\text{sgn} (x)\,\frac{B\left( {1}/{2}\;,p+1,{t^2} \right)}{B\left( {1}/{2}\;,p+1 \right)} \right],$$

$$motionTime(robotVmax, robotAmax,motionRange)=\\ max\left[\frac{2\cdot3^{3/4}}{\sqrt{\pi}}\cdot\sqrt\frac{motionRange}{robotAmax},\frac{32}{5\pi}\cdot\frac{motionRange}{robotVmax}\right],$$

$$position(t,robotVmax,robotAmax,motionRange)=\\ motionRange\cdot f\left(\frac{2 t}{motionTime}-1,2.5\right),$$

where sgn(x) is Sign function, numerator and denominator 𝐵s are the incomplete and complete beta functions, respectively, robotVmax is the maximum velocity, robotAmax is the maximum acceleration and motionRange is the absolute value of the motion (from start to end).

Here are 2 examples of motion planning with $motionRange=2$:

motion plot

The advantage of this method is continuity in every derivative (including jerk) and very low values of jerk.

The disadvantage is the large motions will take longer time.

You can see more examples on the page of my python package s-curve-beta that implements this method:

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.