Re: Motion Interpolation / Trajectory generation

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


api.html#one-dof

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