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:
I can also get the position curve verses time using scipy's numeric integration on the array of velocities:
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:
The inverse of this function is going to be quite complex, and something I would like to avoid as much as possible.
I am currently computing
c1
andc2
as functions oft
, namelyc1 = 0.1*t
andc2=0.9*t
. I can probably replacec1
andc2
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.