how to implement trapezoidal velocity profile in code

I want to use a trapezoidal velocity profile(cartesian) but I struggle with implementing the equations to code(c++/python). Does anyone have an example for this? UPDATE more details about my problem. I want to create a Cartesian trajectory with a trapezoidal velocity profile from point A to point B. The start and end velocities must be zero

• Take a look at Trajectory Planning for Automatic Machines and Robots by luigi biagiotti et al for this. It will give you clear mathematical explanation which is easy to implement. Aug 2 '18 at 13:15
• – 50k4
Aug 2 '18 at 14:34
• notice that the velocity in the target point is not zero. I want to plan from point to point Aug 5 '18 at 9:25
• @mrrobot - "Notice that the velocity in the target point is not zero." We can't notice that. You' didn't say that. You have to state the entire problem if you want meaningful help. Aug 6 '18 at 12:58
• @Chuck -I updated my description. Thanks! Aug 6 '18 at 14:24

Not sure how you're choosing to structure your code, but typically I'll have a master reference, in percent, and a rate limit, in percent per second. You can have different rates for acceleration or deceleration, but I'll post a generic snippet that uses the same rate limit for both.

In the code, the master reference is the unramped (typically square) input, and then the ramped reference is the trapezoidal output. I'm a heavy Matlab user and only dabble in C++, so I'll write in Matlab, but you should get the gist of it:

persistent rampedRef; % 'static' in most other languages
dT = ElapsedTime(); % You need to know the time between function calls. How you get this information varies by language and platform.

desiredAcceleration = (masterRef - rampedRef)/dT;
if abs(desiredAcceleration)>rateLimit
desiredAcceleration = sign(desiredAcceleration)*rateLimit;
end

rampedRef = rampedRef + desiredAcceleration*dT;


There you go! Pretty straightforward. First, calculate a "test acceleration" between your ramped reference and the desired master reference. If your test acceleration exceeds your acceleration limit, then use the acceleration limit.

:EDIT:

Position is the integral of speed. Integral just means "area," so if you can find the area of the speed curve then you can find the total distance traversed.

For a trapezoidal speed profile, this is pretty straightforward. Acceleration and deceleration both form triangular regions, then constant speed forms a rectangular region.

The area for a triangle is:

$$A_{\mbox{triangle}} = \frac{1}{2}bh \\$$

Where, in this case, $b$ is the base, or the time spent accelerating, and $h$ is the height of the triangle, or the top/final speed for the vehicle. Acceleration and deceleration will both have the same $h$ term, because you accelerate to a speed and then decelerate from that speed. I'll substitute $v_{\mbox{top}}$ for $h$ and $t_{\mbox{accel/decel}}$ for $b$, and since the area under the curve is the change in position, I'll use $dS$ instead of $A$. Now you can see that the total distance traversed during the acceleration and deceleration transients are:

$$dS_{\mbox{accel and decel}} = \left(\frac{1}{2}t_{\mbox{accel}}v_{\mbox{top}}\right) + \left(\frac{1}{2}t_{\mbox{decel}}v_{\mbox{top}}\right) \\$$

If you have the same rate limit for acceleration and deceleration then $t_{\mbox{accel}} = t_{\mbox{decel}}$, and the equation above reduces to:

$$dS_{\mbox{accel and decel}} = \left(\frac{1}{2}t_{\mbox{ramp}}v_{\mbox{top}}\right) + \left(\frac{1}{2}t_{\mbox{ramp}}v_{\mbox{top}}\right) \\ \boxed{dS_{\mbox{accel and decel}} = t_{\mbox{ramp}}v_{\mbox{top}}}\\$$

The other part of the formula is now just the "rectangle" of constant motion;

$$dS_{\mbox{constant speed}} = v_{\mbox{top}}t_{\mbox{constant speed}} \\$$

Again, simply put, position is (speed) x (time). Now, the total distance traversed is:

$$dS = dS_{\mbox{accel and decel}} + dS_{\mbox{constant speed}} \\$$

So, finally, to solve the problem of generating the speed profile, you would:

1. Get the distance to travel, $dS = s_{\mbox{end}} - s_{\mbox{start}}$.
2. Find out how far you travel by only accelerating to top speed then immediately decelerating back to zero speed - $dS_{\mbox{accel and decel}} = t_{\mbox{ramp}}v_{\mbox{top}}$
3. If the accelerate/decelerate profile causes you to travel further than you want, then solve for a lower top speed by solving $dS = t_{\mbox{ramp}}v_{\mbox{top}}$ for $v_{\mbox{top}}$.
4. If the accelerate/decelerate profile does not get you far enough, then solve for the amount of time you need to be at top speed by solving $dS - dS_{\mbox{accel and decel}} = v_{\mbox{top}}t_{\mbox{constant speed}}$ for $t_{\mbox{constant speed}}$.
• Thanks! I use it for planning trajectory, I need to know when to deaccelerate to reach the exact point Aug 5 '18 at 9:24
• @mrrobot - I added content to give an algorithm for solving the trapezoidal speed profile for a particular distance target. Aug 6 '18 at 14:58