# How can I implement S-curve motion profile on mobile robotic platform / wheeled robot?

I want to plan a robot's motion according to an S curve profile. I am following the article mention below, which contains basics related to S curve motion profiles in robotic platforms.

https://www.pmdcorp.com/resources/type/articles/get/mathematics-of-motion-control-profiles-article

For this purpose I am using one sigmoidal function,

https://en.wikipedia.org/wiki/Sigmoid_function

Is there any other equation or any other method available to achieve this type of S curve profile.

I use python programming language, are there methods available in python other than the Boltzmann sigmoid function?

Also, what are the basic steps which I have to follow in order to implement S-curve motion profile on a robot?

There are many ways to create a trajectory between two points using a sigmoid function. It's not a simple question, and the math is a little involved. Here's how I would do it.

You can plan an "S curve"-style path for your robot if you know the following information: your start position, your end position, your tolerance on those positions, and either the desired time to get from one position to the other, or the maximum speed of the robot.

You can use the following equation to calculate each point of your trajectory based on the time, $$t$$:

$$x(t) = x_0 + (x_f - x_0)\frac{1}{1+e^{-b(t - c)}},$$ where $$x_0$$ is the desired starting position, $$x_f$$ is the desired final position, and $$b$$ and $$c$$ are parameters defined below.

## Time-based trajectory

If you know the time, $$t_{max}$$, you want for your robot to take to go from a point $$x_0$$ to a second point $$x_f$$, then $$b$$ and $$c$$ are given by:

$$c = t_{max}/2$$

$$b = \frac{1}{c}\text{ln}\left(\frac{1-\lambda}{\lambda}\right)$$ where $$\text{ln()}$$ is the natural logarithm, and $$\lambda$$ is defined as $$\lambda = \frac{E_{max}}{x_f - x_0}.$$

$$\lambda$$ is the tolerance on the starting and ending positions, based on the maximum allowable position error $$E_{max}$$. See the detailed explanation for more info on the tolerance. If you choose a really small tolerance ($$\lambda < 0.001$$) it would probably be good enough. Note that by using time, there's no guarantee your robot can actually follow this trajectory if you use a really small time value.

## Speed-based trajectory

If you want to get to the next point as quickly as possible, you can use the maximum speed of your robot, $$|v_{max}|$$, to calculate the trajectory. In that case $$b$$ and $$c$$ are given by:

$$b = 4\frac{|v_{max}|}{|x_f - x_0|}$$

$$c = \frac{1}{b}\text{ln}\left(\frac{1-\lambda}{\lambda}\right)$$ where $$\lambda$$ is the same as in the time-based case.

# Detailed Explanation

The sigmoid function is described by the following equation:

$$f(t) = \frac{1}{1 + e^{-t}}.$$

This function varies from 0 when $$t \rightarrow -\infty$$ to 1 when $$t \rightarrow \infty$$. To get the desired behaviour for path planning, we can use linear transformations of this function to build our path. We can stretch and translate the function in the horizontal and vertical axes by adding these additional parameters:

$$x(t) = a f(b(t-c)) + d.$$

Let's start with the initial position. For simplicity, let's choose $$x(t=-\infty)=x_0$$ so we can solve for $$d$$:

\begin{align} a f(-\infty) + d &= x_0\\ a\cdot 0 + d &= x_0\\ d &=x_0 \end{align}

Next, we choose our final position, $$x(t = \infty) = x_f$$, to solve for $$a$$:

\begin{align} a f(\infty) + x_0 &= x_f\\ a \cdot 1 + x_0 &= x_f\\ a &= x_f - x_0 \end{align}

Now, choosing our initial and final positions at positive and negative infinite time made the math easier, but it does mean that our path will never actually reach those positions. Therefore, we need to decide how close we need the path to get to those positions for the error to be negligible. This "acceptable error" is determined by the tolerance, $$\lambda$$.

Let's say the acceptable error on the start and end position is $$E_{max}$$. Then, the position at $$t=0$$ would be $$x(0) = x_0 + E_{max}$$. Plugging that into our equation:

\begin{align} x_0 + E_{max} &= (x_f - x_0) f(b(0 - c)) + x_0\\ E_{max} &= (x_f - x_0) f(-bc)\\ f(-bc) &= \frac{E_{max}}{x_f - x_0} \end{align}

To make the following math easier, we let $$\lambda = \frac{E_{max}}{x_f - x_0}$$. Replacing the function with the expression for the sigmoid curve, we can rearrange to find an equation in terms of $$b$$ and $$c$$: \begin{align} \frac{1}{1 + e^{-(-bc)}} &= \lambda,\\ 1 + e^{bc} &= \frac{1}{\lambda},\\ e^{bc} &= \frac{1}{\lambda} - 1,\\ e^{bc} &= \frac{1 - \lambda}{\lambda},\\ bc &= \text{ln}\left(\frac{1 - \lambda}{\lambda} \right). \end{align}

Since $$b$$ and $$c$$ are not independent in this equation, we need another constraint. This is where we choose between using a maximum time duration $$t_{max}$$ or a maximum speed $$|v_{max}|$$.

## Time-based trajectory

If you know the maximum time, the solution is fairly simple using some geometry. Assuming we want the path to be symmetrical between the starting time $$t=0$$ and the ending time $$t=t_{max}$$, and knowing that the sigmoid function is symmetric about zero, we can say: \begin{align} b(t_{max}/2 - c) &= 0,\\ t_{max}/2 - c &= 0,\\ c = t_{max}/2. \end{align}

Now that we have $$c$$, we can rearrange the previous equation to get $$b$$: $$b = \frac{1}{c}\text{ln}\left(\frac{1-\lambda}{\lambda}\right).$$

## Speed-based trajectory

To figure out the trajectory based on speed, we need to take the derivative of our path to get the velocity (note that I'm skipping the algebra): \begin{align} v(t) &= \frac{d}{dt}\left(x_0 + (x_f - x_0)\frac{1}{1+e^{-b(t - c)}} \right)\\ v(t) &= (x_f - x_0)\frac{be^{-b(t - c)}}{\left(1+e^{-b(t - c)}\right)^2} \end{align}

Now using some geometry, or mathematical intuition, we know that the rate of change of the sigmoid function peaks at zero. Therefore we can set $$-b(t-c) = 0$$, and solve for $$b$$: \begin{align} |v_{max}| &= (x_f - x_0)\frac{be^{0}}{\left(1+e^{0}\right)^2}\\ |v_{max}| &= (x_f - x_0)\frac{b}{2^2}\\ b &= 4\frac{|v_{max}|}{x_f - x_0} \end{align}

Now that we have $$b$$, we can rearrange the previous equation to get $$c$$: $$c = \frac{1}{b}\text{ln}\left(\frac{1-\lambda}{\lambda}\right).$$

# Examples

I use MATLAB for quick prototyping since I have a license through my work, but you can also implement this in Python using the numpy and matplotlib packages. I made two examples, each with a starting position of $$x_0 = 0.1 \text{ m}$$, and a final position of $$x_f = 1.5 \text{ m}$$. I used a tolerance of $$\lambda = 0.001$$, a maximum time of $$t_{max} = 4 \text{ s}$$ for the first example and a maximum speed of $$|v_{max}| = 2 \text{ m/s}$$ for the second example.  