3rd order motion profile programming ("sinusoidal")

[Dominik Tugend]

What I am actually trying to do:

I actually want to program (better) smooth camera tracking of objects for a hobby project of mine: HLAE / AfxHookSource ( http://www.advancedfx.org )

I made some compromises already to make things not even more complicated, so I am down to trying to use a 3rd order motion profile (so not specifically for angular motion), assuming that the error will be a constant linear factor.

I already posted my question on math.stackexhange.com here:
http://math.stackexchange.com/questions/1750143/3rd-oder-motion-profile-programming-characterized-by-limits-for-jerk-accelerat [Broken]
But it looks as if won't get any feedback there and a friend recommended me to try this physics forum instead.


1. Problem

I am trying to program a 3rd order motion profile. It is driven by controlling the Jerk in a somewhat "sinusoidal" way. The acceleration and velocity are limited. There can be initial acceleration and velocity.

The closest graphical visualization I found is this (main difference is it starts with zero velocity and acceleration): https://en.wikipedia.org/wiki/Jerk_(physics)#/media/File:Schematic_diagram_of_Jerk,_Acceleration,_and_Speed.svg

It has these phases:

1. acceleration (decelleration) build-up
2. limit acceleration (decelleration)
3. acceleration (decelleration) ramp-down
4. limit speed
5. deceleration (accelleration) build-up
6. limit deceleration (accelleration)
7. deceleration (accelleration) ramp-down

The complete programming problem I derived from these properties is as follows:

1.1. Input:

• $\text{LimitVelocity} \in \mathbb{R}_{\gt 0}$ - absolute velocity limit
• $\text{LimitAcceleration} \in \mathbb{R}_{\gt 0}$ - absolute acceleration limit
• $\text{LimitJerk} \in \mathbb{R}_{\gt 0}$ - absolute jerk limit
• $\text{targetPos} \in \mathbb{R}$ - target position
• $\text{lastPos} \in \mathbb{R}$ - last (initial) position
• $\text{lastVel} \in \mathbb{R}$ - last (initial) velocity, where $\left| \text{lastVel} \right| \leq \text{LimitVelocity}$
• $\text{lastAccel} \in \mathbb{R}$ - last (initial) acceleration, where $\left| \text{lastAccel} \right| \leq \text{LimitAcceleration}$

1.2. Output:

• $\text{phase1T} \in \mathbb{R}_{\geq 0}$ , $\text{phase2T} \in \mathbb{R}_{\geq 0}$ , $\text{phase3T} \in \mathbb{R}_{\geq 0}$ , $\text{phase4T} \in \mathbb{R}_{\geq 0}$ , $\text{phase5T} \in \mathbb{R}_{\geq 0}$ , $\text{phase6T} \in \mathbb{R}_{\geq 0}$ , $\text{phase7T} \in \mathbb{R}_{\geq 0}$ - Phase times
• $\text{dir} \in \{-1, 1\}$ - base/intital jerk direction

1.3. Further Equations:

The idea of those equations is that the model is controlled by driving the Jerk in a specific "sinusoidal" (?) way over a to be determined period of time (basically modelling the phases mentioned above).

1.3.1. Jerk

$$\text{jerk}(1) = +\text{dir} \cdot \text{LimitJerk} \\ \text{jerk}(2) = 0 \\ \text{jerk}(3) = -\text{dir} \cdot \text{LimitJerk} \\ \text{jerk}(4) = 0 \\ \text{jerk}(5) = -\text{dir} \cdot \text{LimitJerk} \\ \text{jerk}(6) = 0 \\ \text{jerk}(7) = +\text{dir} \cdot \text{LimitJerk} \\$$

1.3.2. Acceleration

Derived by integrating the jerk.

$$\text{accel}(0) = \text{lastAccel} \\ \text{accel}(1) = \text{accel}(0) +\text{jerk}(1) \cdot \text{phase1T} \\ \text{accel}(2) = \text{accel}(1) +\text{jerk}(2) \cdot \text{phase2T} \\ \text{accel}(3) = \text{accel}(2) +\text{jerk}(3) \cdot \text{phase3T} \\ \text{accel}(4) = \text{accel}(3) +\text{jerk}(4) \cdot \text{phase4T} \\ \text{accel}(5) = \text{accel}(4) +\text{jerk}(5) \cdot \text{phase5T} \\ \text{accel}(6) = \text{accel}(5) +\text{jerk}(6) \cdot \text{phase6T} \\ \text{accel}(7) = \text{accel}(6) +\text{jerk}(7) \cdot \text{phase7T} \\ \text{accel}(7) = 0$$

1.3.3. Acceleration limits

$$-\text{LimitAccel} \leq \text{accel}(1) \leq \text{LimitAccel} \\ -\text{LimitAccel} \leq \text{accel}(2) \leq \text{LimitAccel} \\ -\text{LimitAccel} \leq \text{accel}(3) \leq \text{LimitAccel} \\ -\text{LimitAccel} \leq \text{accel}(4) \leq \text{LimitAccel} \\ -\text{LimitAccel} \leq \text{accel}(5) \leq \text{LimitAccel} \\ -\text{LimitAccel} \leq \text{accel}(6) \leq \text{LimitAccel} \\ -\text{LimitAccel} \leq \text{accel}(7) \leq \text{LimitAccel}$$

1.3.4. Velcoity

Derived by integrating the acceleration.

$$\text{vel}(0) = \text{lastVel} \\ \text{vel}(1) = \text{vel}(0) +\text{accel}(0) \cdot \text{phase1T} +\text{jerk}(1)/2 \cdot \text{phase1T}^2 \\ \text{vel}(2) = \text{vel}(1) +\text{accel}(1) \cdot \text{phase2T} +\text{jerk}(2)/2 \cdot \text{phase2T}^2 \\ \text{vel}(3) = \text{vel}(2) +\text{accel}(2) \cdot \text{phase3T} +\text{jerk}(3)/2 \cdot \text{phase3T}^2 \\ \text{vel}(4) = \text{vel}(3) +\text{accel}(3) \cdot \text{phase4T} +\text{jerk}(4)/2 \cdot \text{phase4T}^2 \\ \text{vel}(5) = \text{vel}(4) +\text{accel}(4) \cdot \text{phase5T} +\text{jerk}(5)/2 \cdot \text{phase5T}^2 \\ \text{vel}(6) = \text{vel}(5) +\text{accel}(5) \cdot \text{phase6T} +\text{jerk}(6)/2 \cdot \text{phase6T}^2 \\ \text{vel}(7) = \text{vel}(6) +\text{accel}(6) \cdot \text{phase7T} +\text{jerk}(7)/2 \cdot \text{phase7T}^2 \\ \text{vel}(7) = 0$$

1.3.5. Velocity limits

$$-\text{LimitVelocity } \leq \text{vel}(1) \leq \text{LimitVelocity } \\ -\text{LimitVelocity } \leq \text{vel}(2) \leq \text{LimitVelocity } \\ -\text{LimitVelocity } \leq \text{vel}(3) \leq \text{LimitVelocity } \\ -\text{LimitVelocity } \leq \text{vel}(4) \leq \text{LimitVelocity } \\ -\text{LimitVelocity } \leq \text{vel}(5) \leq \text{LimitVelocity } \\ -\text{LimitVelocity } \leq \text{vel}(6) \leq \text{LimitVelocity } \\ -\text{LimitVelocity } \leq \text{vel}(7) \leq \text{LimitVelocity }$$

1.3.6. Position

$$\text{targetDeltaPos} = \text{targetPos} -\text{lastPos}$$

Derived by integrating the velocity:
$$\text{resultDeltaPos} = 0 +\left( \text{vel}(0) \cdot \text{phase1T} +\text{accel}(0)/2 \cdot \text{phase1T}^2 +\text{jerk}(1)/6 * \text{phase1T}^3 \right) +\left( \text{vel}(1) \cdot \text{phase2T} +\text{accel}(1)/2 \cdot \text{phase2T}^2 +\text{jerk}(2)/6 * \text{phase2T}^3 \right) +\left( \text{vel}(2) \cdot \text{phase3T} +\text{accel}(2)/2 \cdot \text{phase3T}^2 +\text{jerk}(3)/6 * \text{phase3T}^3 \right) +\left( \text{vel}(3) \cdot \text{phase4T} +\text{accel}(3)/2 \cdot \text{phase4T}^2 +\text{jerk}(4)/6 * \text{phase4T}^3 \right) +\left( \text{vel}(4) \cdot \text{phase5T} +\text{accel}(4)/2 \cdot \text{phase5T}^2 +\text{jerk}(5)/6 * \text{phase5T}^3 \right) +\left( \text{vel}(5) \cdot \text{phase6T} +\text{accel}(5)/2 \cdot \text{phase6T}^2 +\text{jerk}(6)/6 * \text{phase6T}^3 \right) +\left( \text{vel}(6) \cdot \text{phase7T} +\text{accel}(6)/2 \cdot \text{phase7T}^2 +\text{jerk}(7)/6 * \text{phase7T}^3 \right)$$

1.4. Minimize:

1. Position error: $\left| \text{targetDeltaPos} -\text{resultDeltaPos} \right|$
2. Position time: $\text{phase1T} +\text{phase2T} +\text{phase3T} +\text{phase4T} +\text{phase5T} +\text{phase6T} +\text{phase7T}$

2. Question(s)

Can this be solved without using non-linear programming?

If so, I'd welcome any hints on how to attempt to solve this problem or any literature (if possible freely accessible online) I should read for educating myself about it. I'd prefer practical / easy literature, because as you probably already figured I am not good at mathematics.

(I think non-linear mixed-integer programming can be avoided by trying to solve it for each possible value of $\text{dir}$, meaning solving two times and taking the "better" one of the feasible solutions (at least one should be feasible).)

I was actually hoping for solving it analytical with different cases or s.th. like that, but I can't even find a way to make it in a linear programming problem (maybe that is not possible).

Please hint me on typos or things I might have forgot, I will try to edit them in (if that is possible).


Other notes

If it helps anything, then I can try to write down everything by making the jerk a left-continuous step function and deriving acceleration, velocity and resultDeltaPos from that. However I am not sure if that would make things better, because the functions would still have many cases then.

I hope I posted in the right sub-forum, because this might be more of an artificial model than modelling some real motion.
I selected Intermediate level thread prefix, because I don't know if this is an advanced problem or not.

 

[Dominik Tugend]

Well currently I am examining if it's at least a convex optimization problem.


The absoulte objective function can be replaced as it is done with LP-problems, by adding two constraints and a new variable and changing
the first objective function:

$$\text{absDiff} \in \mathbb{R}_{\geq 0} \\ \text{targetDeltaPos} -\text{resultDeltaPos} <= \text{absDiff} \\ -\text{targetDeltaPos} +\text{resultDeltaPos} <= \text{absDiff}$$

New 1.4. Minimize:

1. $\text{absDiff}$
2. as in first post


If we fix dir on any of the possible two values and if we have any two feasible solutions
$$u_1 = \left( \text{phase1T}_1, \text{phase2T}_1, \text{phase3T}_1, \text{phase4T}_1, \text{phase5T}_1, \text{phase6T}_1, \text{phase7T}_1 \right) \\ u_2 = \left( \text{phase1T}_2, \text{phase2T}_2, \text{phase3T}_2, \text{phase4T}_2, \text{phase5T}_2, \text{phase6T}_2, \text{phase7T}_2 \right)$$

Then we can check if the linear combination $u_3 = \lambda_1 \cdot u_1 +\lambda_2 \cdot u_2$ where $\lambda_1 + \lambda_2 = 1$ and $\lambda_1 \in \mathbb{R}_{\geq 0}$ and $\lambda_2 \in \mathbb{R}_{\geq 0}$ is a feasible solution too:


Checking the Jerk equations:

Those are not affected.


Checking the Acceleration equation constraints:

$$\text{accel}(0) = \text{lastAccel} \\ \text{accel}(1) = \text{accel}(0) +\text{jerk}(1) \cdot \left(\lambda_1 \cdot \text{phase1T}_1 + \lambda_2 \cdot \text{phase1T}_2 \right) \\ \text{accel}(2) = \text{accel}(1) +\text{jerk}(2) \cdot \left(\lambda_1 \cdot \text{phase2T}_1 + \lambda_2 \cdot \text{phase2T}_2 \right) \\ \text{accel}(3) = \text{accel}(2) +\text{jerk}(3) \cdot \left(\lambda_1 \cdot \text{phase3T}_1 + \lambda_2 \cdot \text{phase3T}_2 \right) \\ \text{accel}(4) = \text{accel}(3) +\text{jerk}(4) \cdot \left(\lambda_1 \cdot \text{phase4T}_1 + \lambda_2 \cdot \text{phase4T}_2 \right) \\ \text{accel}(5) = \text{accel}(4) +\text{jerk}(5) \cdot \left(\lambda_1 \cdot \text{phase5T}_1 + \lambda_2 \cdot \text{phase5T}_2 \right) \\ \text{accel}(6) = \text{accel}(5) +\text{jerk}(6) \cdot \left(\lambda_1 \cdot \text{phase6T}_1 + \lambda_2 \cdot \text{phase6T}_2 \right) \\ \text{accel}(7) = \text{accel}(6) +\text{jerk}(7) \cdot \left(\lambda_1 \cdot \text{phase7T}_1 + \lambda_2 \cdot \text{phase7T}_2 \right) \\ \text{accel}(7) = 0$$

Let us check if thes equations still hold valid (meaning don't get infeasible):

It follows:
$$\text{lastAccel} +\text{jerk}(1) \cdot \left(\lambda_1 \cdot \text{phase1T}_1 + \lambda_2 \cdot \text{phase1T}_2 \right) +\text{jerk}(2) \cdot \left(\lambda_1 \cdot \text{phase2T}_1 + \lambda_2 \cdot \text{phase2T}_2 \right) +\text{jerk}(3) \cdot \left(\lambda_1 \cdot \text{phase3T}_1 + \lambda_2 \cdot \text{phase3T}_2 \right) +\text{jerk}(4) \cdot \left(\lambda_1 \cdot \text{phase4T}_1 + \lambda_2 \cdot \text{phase4T}_2 \right) +\text{jerk}(5) \cdot \left(\lambda_1 \cdot \text{phase5T}_1 + \lambda_2 \cdot \text{phase5T}_2 \right) +\text{jerk}(6) \cdot \left(\lambda_1 \cdot \text{phase6T}_1 + \lambda_2 \cdot \text{phase6T}_2 \right) +\text{jerk}(7) \cdot \left(\lambda_1 \cdot \text{phase7T}_1 + \lambda_2 \cdot \text{phase7T}_2 \right) = 0$$

Now let's assume $\text{dir} = 1$ and $\text{LimitJerk} = 1$

Then it follows that:
$$\text{lastAccel} + \left(\lambda_1 \cdot \text{phase1T}_1 + \lambda_2 \cdot \text{phase1T}_2 \right) - \left(\lambda_1 \cdot \text{phase3T}_1 + \lambda_2 \cdot \text{phase3T}_2 \right) - \left(\lambda_1 \cdot \text{phase5T}_1 + \lambda_2 \cdot \text{phase5T}_2 \right) + \left(\lambda_1 \cdot \text{phase7T}_1 + \lambda_2 \cdot \text{phase7T}_2 \right) = 0$$

We know that both solutions $u_1$, $u_2$ are 0, so it follows:

$$\text{lastAccel} + \left(\lambda_1 \cdot \text{phase1T}_1 + \lambda_2 \cdot \text{phase1T}_2 \right) - \left(\lambda_1 \cdot \text{phase3T}_1 + \lambda_2 \cdot \text{phase3T}_2 \right) - \left(\lambda_1 \cdot \text{phase5T}_1 + \lambda_2 \cdot \text{phase5T}_2 \right) + \left(\lambda_1 \cdot \text{phase7T}_1 + \lambda_2 \cdot \text{phase7T}_2 \right) = \text{lastAccel} + \left(1 \cdot \text{phase1T}_1 \right) - \left(1 \cdot \text{phase3T}_1 \right) - \left(1 \cdot \text{phase5T}_1 \right) + \left(1 \cdot \text{phase7T}_1 \right)$$

Which is equivalent to:

$$+ \lambda_1 \cdot \text{phase1T}_1 + \lambda_2 \cdot \text{phase1T}_2 - \lambda_1 \cdot \text{phase3T}_1 - \lambda_2 \cdot \text{phase3T}_2 - \lambda_1 \cdot \text{phase5T}_1 - \lambda_2 \cdot \text{phase5T}_2 + \lambda_1 \cdot \text{phase7T}_1 + \lambda_2 \cdot \text{phase7T}_2 = + 1 \cdot \text{phase1T}_1 - 1 \cdot \text{phase3T}_1 - 1 \cdot \text{phase5T}_1 + 1 \cdot \text{phase7T}_1$$

Which is equivalent to:

$$+ \lambda_1 \cdot \text{phase1T}_1 + \lambda_2 \cdot \text{phase1T}_2 - \lambda_1 \cdot \text{phase3T}_1 - \lambda_2 \cdot \text{phase3T}_2 - \lambda_1 \cdot \text{phase5T}_1 - \lambda_2 \cdot \text{phase5T}_2 + \lambda_1 \cdot \text{phase7T}_1 + \lambda_2 \cdot \text{phase7T}_2 = + \left( \lambda_1 + \lambda_2 \right) \cdot \text{phase1T}_1 - \left( \lambda_1 + \lambda_2 \right) 1 \cdot \text{phase3T}_1 - \left( \lambda_1 + \lambda_2 \right) 1 \cdot \text{phase5T}_1 + \left( \lambda_1 + \lambda_2 \right) 1 \cdot \text{phase7T}_1$$

Which is equivalent to:

$$+ \lambda_1 \cdot \text{phase1T}_1 + \lambda_2 \cdot \text{phase1T}_2 - \lambda_1 \cdot \text{phase3T}_1 - \lambda_2 \cdot \text{phase3T}_2 - \lambda_1 \cdot \text{phase5T}_1 - \lambda_2 \cdot \text{phase5T}_2 + \lambda_1 \cdot \text{phase7T}_1 + \lambda_2 \cdot \text{phase7T}_2 = + \left( \lambda_1 + \lambda_2 \right) \cdot \text{phase1T}_1 - \left( \lambda_1 + \lambda_2 \right) \cdot \text{phase3T}_1 - \left( \lambda_1 + \lambda_2 \right) \cdot \text{phase5T}_1 + \left( \lambda_1 + \lambda_2 \right) \cdot \text{phase7T}_1$$

Which is equivalent to:

$$+ \lambda_2 \cdot \text{phase1T}_2 - \lambda_2 \cdot \text{phase3T}_2 - \lambda_2 \cdot \text{phase5T}_2 + \lambda_2 \cdot \text{phase7T}_2 = + \lambda_2 \cdot \text{phase1T}_1 - \lambda_2 \cdot \text{phase3T}_1 - \lambda_2 \cdot \text{phase5T}_1 + \lambda_2 \cdot \text{phase7T}_1$$

If $\lambda_2 = 0$ the solution is trivial, otherwise
it follows that

$$+ \text{phase1T}_2 - \text{phase3T}_2 - \text{phase5T}_2 + \text{phase7T}_2 = + \text{phase1T}_1 - \text{phase3T}_1 - \text{phase5T}_1 + \text{phase7T}_1$$

However that would mean that for any feasible solution the sum of these times of phase 1,3,5,7 have to be equal.

In other words if we can show that there are feasible solutions for $\text{dir} = 1$ and $\text{LimitJerk} = 1$, that have different values for the sum of the times of phase 1,3,5,7,
then the optimization problem is not even a convex optimization problem.

I will try to continue from here within the next days.

 

[Dominik Tugend]

There is a stupid mistake by me in the previous post at the end:
Of course I shouldn't have written "sum".
It would need to be the exact equation I have written at the end.

This is always fulfilled, because both sides of the equation would have to be zero by definition of the acceleration equations under the given conditions ($\text{dir}=1$ and $\text{LimitJerk}=1$).

In other words, I have not shown anything at all yet about whether this problem is convex or not :-(

 

[Stephen Tashi]

1.4. Minimize:

1. Position error: $\left| \text{targetDeltaPos} -\text{resultDeltaPos} \right|$
2. Position time: $\text{phase1T} +\text{phase2T} +\text{phase3T} +\text{phase4T} +\text{phase5T} +\text{phase6T} +\text{phase7T}$

How is that a specific objective function? For example, suppose by using a long position time that you can reduce position error and that by allowing a larger position error that you can reduce position time. How do you pick which of those alternatives to use ?

Is this a problem in 1-dimensional position space ? - or 3 ?

From your algorithm, I would assume the variables we are trying to solve for are Jerk's 1,3,5,7 and the variable "dir". Is that correct ?

 

[Dominik Tugend]

How is that a specific objective function? For example, suppose by using a long position time that you can reduce position error and that by allowing a larger position error that you can reduce position time. How do you pick which of those alternatives to use ?

Is this a problem in 1-dimensional position space ? - or 3 ?

From your algorithm, I would assume the variables we are trying to solve for are Jerk's 1,3,5,7 and the variable "dir". Is that correct ?

Thank you very much for your feedback!

By that I meant, that first an optimal solutions that minimize the position error are found (1.) and then from those an optimal solution that minimizes the sum of the phase times is picked (2.) - meaning I want (1.) to be the main objective and (2.) to be the sub-objective. I sadly don't know how to express such a thing properly.

The problem is in one dimensional position space, the position variables are scalars.

No, I am only trying to solve for the variables in Output (phase1T,phase2T,phase3T,phase4T,phase5T,phase6T,phase7T,dir) all variables in Input are given (constants).


To be honest I was able to solve a simpler version (only 2nd order motion profile) for my needs, which I presented here:
https://www.physicsforums.com/threads/2nd-oder-motion-profile-trapezoidal-velocity-code.869551/ [Broken]
It's really good enough for me, so I am not that much interested in trying to solve this much more complicated problem anymore to be honest.

Also I think you could either run into patent conflicts or make some money when selling the solution to an elevator company or s.th. like that, because I heard that elevators actually would use such 3rd order motion profiles.


Again thank you very much for your feedback :-)

 

[Dominik Tugend]

I can't edit the post anymore, just wanted to add, that I have phrased the problem with the main / sub objective as separate question here:
https://www.physicsforums.com/threads/how-to-write-main-and-sub-objective-for-optimisation.869647/

An answer would be really useful to me :-)