Motion Interpolation / Trajectory generation

Please can someone point me in the right direction? I'm hoping that this is
a solved problem for somebody out there.

I need a function that will limit Acceleration and Velocity, generating at
each sample time a new position that meets the V A constraints.  The input
position is kept within bounds.

A vital design requirement is low signal latency, ideally I want a current
answer for each sample.

I started by looking at motion interpolators of various kinds, e.g.

http://people.mech.kuleuven.be/~bruyninc/blender/doc/interpolation-api.html#one-dof

but interpolators assume that you begin with a fixed start and stop
position, and from those generate a series of interpolation points (within
the sample time) that will take you from start to stop, such that you reach
the end position with velocity zero.

I started with code that calculated the velocity and acceleration and
limited them:

deltaT = time step between points
start with cur_pos = cur_vel = cur_acc = 0
max_acc and max_vel set to +ve values, don't change during runtime

PSEUDOCODE:

function LimitVA ( new_pos )
		// calculate actual distance from current position
	dist = cur_pos - new_pos
		// velocity needed to get there from here
	vel = dist / deltaT
		// acceleration needed to reach that velocity
	acc = (cur_vel - vel ) / deltaT 
		// Limit the acceleration to allowed values
	acc = limit ( acc, -max_acc,  max_acc )
		// calculate new velocity (using "v = u + a*t")
	vel = cur_vel + acc * timestep		
		// Limit the velocity to allowed values
	vel = limit ( vel, -max_vel,  max_vel )
		// calculate new position 
	cur_pos = cur_pos + vel * deltaT 
		// remember velocity
	cur_vel = vel
		// result
	return cur_pos


Now, this version has terrible overshoot, followed by ringing.  For
example, using a step function as input, the output position accelerates to
the end point, and is going far too fast to stop there.

So I modified the code to take into account the 'time to stop' as shown:

PSEUDOCODE:

function LimitVA ( new_pos )
		// how long to stop at current velocity?
	time_to_stop = abs ( cur_vel / max_acc )
		// where will we be then? ( s = u*t + 0.5*a*t^2 )
	projected_move = cur_vel * time_to_stop + 0.5 * cur_acc * time_to_stop^2
	dist = cur_pos - new_pos
		// change velocity to reflect possible stop position
	vel =  ( dist - projected_move ) / deltaT
	acc = (cur_vel - vel ) / deltaT 
	acc = limit ( acc, -max_acc,  max_acc )
	vel = cur_vel + acc * timestep
	vel = limit ( vel, -max_vel,  max_vel )
	cur_pos = cur_pos + vel * deltaT 
	cur_vel = vel
	return cur_pos


Now this is better, but it is 'unstable'.  For some values of 'max_vel'
and 'max_acc', and for different position sequences, the output can 'hunt'
around a reasonable value.  Also, this code assumes that we will stop. -
which may not be true.  With slowly changing signals, sometimes the result
is a series of steps - which I want to avoid.

Variations on the above code have not given significantly better results.


Now, I'm looking at other possibilities, which have taken me in the
direction of curve fitting.  But I'm not sure if I've missed something in
the above.

Also, choosing a curve fitting algorithm, given the desired constraints on
velocity and acceleration, looks like a non-trivial task.  

So, if anyone out there has a suitable alternative, advice where I may be
going wrong, or can point me at a web resource, I would be very grateful.

Thanks,
TrissT

On Tue, 03 Mar 2009 11:34:59 -0600, TrissT wrote:

> Please can someone point me in the right direction? I'm hoping that this
> is a solved problem for somebody out there.
> 
> I need a function that will limit Acceleration and Velocity, generating
> at each sample time a new position that meets the V A constraints.  The
> input position is kept within bounds.
> 
> A vital design requirement is low signal latency, ideally I want a
> current answer for each sample.
> 
> I started by looking at motion interpolators of various kinds, e.g.
> 
> http://people.mech.kuleuven.be/~bruyninc/blender/doc/interpolation-
api.html#one-dof
> 
> but interpolators assume that you begin with a fixed start and stop
> position, and from those generate a series of interpolation points
> (within the sample time) that will take you from start to stop, such
> that you reach the end position with velocity zero.
> 
> I started with code that calculated the velocity and acceleration and
> limited them:
> 
> deltaT = time step between points
> start with cur_pos = cur_vel = cur_acc = 0 max_acc and max_vel set to
> +ve values, don't change during runtime
> 
> PSEUDOCODE:
> 
> function LimitVA ( new_pos )
> 		// calculate actual distance from current position
> 	dist = cur_pos - new_pos
> 		// velocity needed to get there from here
> 	vel = dist / deltaT
> 		// acceleration needed to reach that velocity
> 	acc = (cur_vel - vel ) / deltaT
> 		// Limit the acceleration to allowed values
> 	acc = limit ( acc, -max_acc,  max_acc )
> 		// calculate new velocity (using "v = u + a*t")
> 	vel = cur_vel + acc * timestep
> 		// Limit the velocity to allowed values
> 	vel = limit ( vel, -max_vel,  max_vel )
> 		// calculate new position
> 	cur_pos = cur_pos + vel * deltaT
> 		// remember velocity
> 	cur_vel = vel
> 		// result
> 	return cur_pos
> 
> 
> Now, this version has terrible overshoot, followed by ringing.  For
> example, using a step function as input, the output position accelerates
> to the end point, and is going far too fast to stop there.
> 
> So I modified the code to take into account the 'time to stop' as shown:
> 
> PSEUDOCODE:
> 
> function LimitVA ( new_pos )
> 		// how long to stop at current velocity?
> 	time_to_stop = abs ( cur_vel / max_acc )
> 		// where will we be then? ( s = u*t + 0.5*a*t^2 )
> 	projected_move = cur_vel * time_to_stop + 0.5 * cur_acc *
> 	time_to_stop^2 dist = cur_pos - new_pos
> 		// change velocity to reflect possible stop position
> 	vel =  ( dist - projected_move ) / deltaT acc = (cur_vel - vel ) /
> 	deltaT
> 	acc = limit ( acc, -max_acc,  max_acc ) vel = cur_vel + acc * 
timestep
> 	vel = limit ( vel, -max_vel,  max_vel ) cur_pos = cur_pos + vel *
> 	deltaT
> 	cur_vel = vel
> 	return cur_pos
> 
> 
> Now this is better, but it is 'unstable'.  For some values of 'max_vel'
> and 'max_acc', and for different position sequences, the output can
> 'hunt' around a reasonable value.  Also, this code assumes that we will
> stop. - which may not be true.  With slowly changing signals, sometimes
> the result is a series of steps - which I want to avoid.
> 
> Variations on the above code have not given significantly better
> results.
> 
> 
> Now, I'm looking at other possibilities, which have taken me in the
> direction of curve fitting.  But I'm not sure if I've missed something
> in the above.
> 
> Also, choosing a curve fitting algorithm, given the desired constraints
> on velocity and acceleration, looks like a non-trivial task.
> 
> So, if anyone out there has a suitable alternative, advice where I may
> be going wrong, or can point me at a web resource, I would be very
> grateful.
> 
> Thanks,
> TrissT

You're trying to control some physical thing, right?  I'm taking the 
liberty of cross-posting this answer to sci.engr.control, so the experts 
there can hack it apart, along with the experts here on comp.dsp.

This can be done, but the mathematics -- at least the way that I know how 
to do it -- get a bit odd.  So forgive me if it's a bit mind-bending.

If your plant were a perfect double integrator and if you could measure 
it's position and velocity perfectly, this would be a moderately easy 
problem in optimal control.  I'll start my development with that 
assumption, then later on I'll break it and show you what to do about it.

The acceleration part of the control rule is easy:  if your error is 
negative, accelerate positive; if your error is positive, accelerate 
negative; if your error is zero, don't accelerate.  So:

    { -A      target - pos < 0
a = {  0      target - pos = 0
    {  A      target - pos > 0

Of course, this results in your velocity getting too high.  So, modify 
the rule to take that into account (note that I'm ignoring the problem of 
getting back to the correct velocity if you go over -- if we can assume a 
perfect plant we can assume that'll never happen):

    { -A      target - pos < 0 && velocity < Vmax
a = {  0      target == pos || velocity == -Vmax || velocity == +Vmax
    {  A      target - pos > 0 && velocity > -Vmax

This still leaves you with significant (ehem) overshoot when you reach 
your target.  To solve this one you need to look at how your plant 
decelerates:

dv/dt = -A,

dx/dt = v.

This is just a simple state-space system with two states.  If you're 
twisted, you can eliminate the time variable in the above differential 
equation, reducing it from a two-state linear differential equation to a 
one-state nonlinear one:

dv/dx = -A / v.

Since it's a single-order diff. eq, it's pretty easy to guess at a 
solution:

v = sqrt(b(x + c)), therefore

dv/dx = (b / 2) / sqrt(b(x + c)).

The two dv/dx terms can be equated:

(b/2) / sqrt(b(x + c)) = -A / v,

which implies that b = +/- 2A (note the ambiguity of the sqrt function).

We want the thing to come to a stop at pos == target, so it's quite 
sensible to set c = -target.  Keeping in mind that x = pos:

v = +/- sqrt(+/- 2A(pos - target)).

We still have that +/- term, but that can be resolved with logic: if pos 
< target then we want a positive velocity, and if pos > target then we 
want a negative velocity.  We'd also kind of like to have real terms 
inside of the square root.  All that's left is to accept the reality that 
the above expression is for a _target_ velocity, and we have:

           {   Vmax                    sqrt(2A(pos - target)) > Vmax
           {  sqrt(2A(pos - target))   pos < target
v_target = {   0                       pos == target
           { -sqrt(2A(target - pos))   pos > target
           {  -Vmax                    -sqrt(2A(target - pos)) < -Vmax

Now we just remember that we're dealing with an absolutely perfect plant, 
so we get

    {  +A       v < v_target
a = {   0       v == v_target  .
    {  -A       v > v_target

What could be easier?

(A system that actually works could be easier).

Remember that we started with the assumption that we have an absolutely 
perfect plant.  If we have a plant that's in anything _at all_ other than 
absolutely perfect, the above rigamarole is just mathematical 
fantasizing.  Why?  Because it has infinite gain in not one, but two 
places.  

The first place where the infinite gain is apparent is in the calculation 
of the acceleration command from the velocity -- the acceleration command 
_jumps_ from -A to +A (forget about 0) at the slightest discrepancy -- so 
you'd have a system that oscillated madly right there.

The second place where the infinite gain creeps in is where you take the 
square root of the position error.  This term has an infinite gain when 
the position error gets close to zero, so once again you'll have a system 
that oscillates madly.

You could incorporate the above rules into a sliding mode controller, and 
if you didn't mind the bad noises and hot motors you could call it a day 
and go home.

Or you could stabilize the beast.

Stabilizing in velocity is easy.  Just change the acceleration command 
equation from infinite gain to some finite gain:

    {  +A                    (v_target - v)*kv > A
a = {  (v_target - v)*kv      otherwise               .
    {  -A                    (v_target - v)*kv < -A

This will give you a controller that will -- assuming you set kv right -- 
rapidly accelerate to your desired maximum velocity, then start smoothly 
decelerating to your desired position -- then start oscillating like mad.

I am _not_ going to do all the remaining math here, because it gives my 
brain cramps.  But I'll outline it, and leave the exact solution as an 
exercise for the reader.  The problem you're dealing with is the two-
sided square root function that you're trying to use (pardon the poor 
resolution of ASCII art):

                          
    -------------------------
                             \
                              -
                               \
                                \
                                 |
---------------------------------|-------------------------------
                                 |
                                  \
                                   \
                                    -
                                     \
                                      -------------------------

What you need to do is to flatten the thing out in the middle.  You do 
this by separating the two pieces, and laying a line of your desired 
slope in the middle:

                          
  -------------------------
                           \
                            -
                             \      add this section in
                              \    /
                               |\
-------------------------------|-\-|-----------------------------
                              /   \|
                  move this over    \
                                     \ -- this, too
                                      -
                                       \
                                        -------------------------

So you end up with a target velocity curve that has 

           {   Vmax              sqrt(2A(pos - x1 - target)) > Vmax
           {  sqrt(2A(pos - x1 - target))   pos + x2 < target
v_target = {   (target - pos)*kp            otherwise
           { -sqrt(2A(target - pos + x1))   pos - x2 > target
           {  -Vmax              -sqrt(2A(target - pos + x1)) < -Vmax

Your task is to find the x1 and x2 so the lines all sit nicely when 
you've chosen a kp.

Notice that the nice thing about this controller is that when your 
v_target calculation is in the center region, it is just a plain old PD 
controller, so you can analyze it for stability as such (and you can even 
add an integral term to the velocity servo, if you need to).  All of the 
nonlinear stuff just makes sure that the controller puts the brakes on 
far enough ahead of the target position that if it can stop on a dime, it 
will.

(someone will come in and whine about this controller requiring lots of 
math -- well, yes it does.  But it's not that much more than what's 
required to do a trapezoidal profile from point A to point B: the biggest 
difference is that you're constantly updating the thing on line instead 
of pre-calculating a profile.  If you're concerned about having to do the 
square root online, you can pre-compute the v_target as a function of 
position offset in a variety of ways depending on your desired tradeoff 
of code complexity, portability, memory usage, speed, etc.).

-- 
http://www.wescottdesign.com

The obvious thing is that your calculation is bad for calculating the
dist and vel.  You should subtract the current value from the new or
reference value that you are chasing.

Peter Nachtwey

Tim:

Thank you very much for your very detailed and insightful response. You
cast the problem in a different light for me.  

I'm taking the time to work through it carefully.

Thanks again,
TrissT

On Tue, 03 Mar 2009 16:35 , Tim Wescott wrote:
>You're trying to control some physical thing, right?  I'm taking the 
>liberty of cross-posting this answer to sci.engr.control, so the experts

>there can hack it apart, along with the experts here on comp.dsp.

Thank you Tim.

>
>This can be done, but the mathematics -- at least the way that I know how

>to do it -- get a bit odd.  So forgive me if it's a bit mind-bending.
>

I certainly found it stretching...

..
>
>What you need to do is to flatten the thing out in the middle.  You do 
>this by separating the two pieces, and laying a line of your desired 
>slope in the middle:
>

>So you end up with a target velocity curve that has 
>
>           {   Vmax              sqrt(2A(pos - x1 - target)) > Vmax
>           {  sqrt(2A(pos - x1 - target))   pos + x2 < target
>v_target = {   (target - pos)*kp            otherwise
>           { -sqrt(2A(target - pos + x1))   pos - x2 > target
>           {  -Vmax              -sqrt(2A(target - pos + x1)) < -Vmax
>
>Your task is to find the x1 and x2 so the lines all sit nicely when 
>you've chosen a kp.
>

>Notice that the nice thing about this controller is that when your 
>v_target calculation is in the center region, it is just a plain old PD 
>controller, so you can analyze it for stability as such (and you can even

>add an integral term to the velocity servo, if you need to).  All of the

>nonlinear stuff just makes sure that the controller puts the brakes on 
>far enough ahead of the target position that if it can stop on a dime, it

>will.

My attempts at an implementation so far are problematical, but I'm still
working on it.

>
>(someone will come in and whine about this controller requiring lots of 
>math -- well, yes it does.  But it's not that much more than what's 
>required to do a trapezoidal profile from point A to point B: the biggest

>difference is that you're constantly updating the thing on line instead 
>of pre-calculating a profile.  If you're concerned about having to do the

>square root online, you can pre-compute the v_target as a function of 
>position offset in a variety of ways depending on your desired tradeoff 
>of code complexity, portability, memory usage, speed, etc.).
>

The most important thing is to get it right.  Optimization comes last.
So far the trade off looks good if I can get it working.

I'll report back with more details when it's implemented.

I'd just like to thank you again for the time and trouble taken to answer
me.

TrissT

On Mar 6, 7:46&#4294967295;am, "TrissT" <tr...@ymir.net> wrote:
>
> The most important thing is to get it right. &#4294967295;Optimization comes last.
> So far the trade off looks good if I can get it working.

Do you have the power to execute sqrt() functions each deltaT?  I
think Tim was hinting at a differential equation like equation which I
think is the better way to go.

>
> I'll report back with more details when it's implemented.
>
It hasn't been made clear what you are trying to do.   My
interpretation is that there is a command/reference position from some
input like a joystick or some other input that doesn't have smooth
motion profile.  You are trying to generate a smooth motion profile
that follows the command/reference position within velocity and
acceleration limits.  This target position and an actual/feedback
position is used to compute the error for a control loop.
It will be interesting to see what you come up with.

Peter Nachtwey

On Fri, 06 Mar 2009 10:28:36 -0800, pnachtwey wrote:

> On Mar 6, 7:46&nbsp;am, "TrissT" <tr...@ymir.net> wrote:
>>
>> The most important thing is to get it right. &nbsp;Optimization comes last.
>> So far the trade off looks good if I can get it working.
> 
> Do you have the power to execute sqrt() functions each deltaT?  I think
> Tim was hinting at a differential equation like equation which I think
> is the better way to go.
> 
> 
>> I'll report back with more details when it's implemented.
>>
> It hasn't been made clear what you are trying to do.   My interpretation
> is that there is a command/reference position from some input like a
> joystick or some other input that doesn't have smooth motion profile. 
> You are trying to generate a smooth motion profile that follows the
> command/reference position within velocity and acceleration limits. 
> This target position and an actual/feedback position is used to compute
> the error for a control loop. It will be interesting to see what you
> come up with.
> 
> Peter Nachtwey

I've done just that with the method I outlined.  The purpose isn't so 
much to achieve the smooth profile, but to stop a limited-deceleration 
mechanism as quickly as possible without building a system that has a 
hard limit cycle just waiting for a customer to excite.

-- 
http://www.wescottdesign.com

On Mar 7, 9:20&#4294967295;am, Tim Wescott <t...@seemywebsite.com> wrote:
> On Fri, 06 Mar 2009 10:28:36 -0800, pnachtwey wrote:
> > On Mar 6, 7:46&#4294967295;am, "TrissT" <tr...@ymir.net> wrote:
>
> >> The most important thing is to get it right. &#4294967295;Optimization comes last.
> >> So far the trade off looks good if I can get it working.
>
> > Do you have the power to execute sqrt() functions each deltaT? &#4294967295;I think
> > Tim was hinting at a differential equation like equation which I think
> > is the better way to go.
>
> >> I'll report back with more details when it's implemented.
>
> > It hasn't been made clear what you are trying to do. &#4294967295; My interpretation
> > is that there is a command/reference position from some input like a
> > joystick or some other input that doesn't have smooth motion profile.
> > You are trying to generate a smooth motion profile that follows the
> > command/reference position within velocity and acceleration limits.
> > This target position and an actual/feedback position is used to compute
> > the error for a control loop. It will be interesting to see what you
> > come up with.
>
> > Peter Nachtwey
>
> I've done just that with the method I outlined. &#4294967295;The purpose isn't so
> much to achieve the smooth profile, but to stop a limited-deceleration
> mechanism as quickly as possible without building a system that has a
> hard limit cycle just waiting for a customer to excite.
>
> --http://www.wescottdesign.com
But one can limit the velocity, accel and decel AND get a smooth
motion profile too

Peter Nachtwey