Closed-loop proportional motion control algorithm.

Should proof read before posting. p (rho) = straight line distance from
robots position (x,y) to the goal (0,0).

Chris Fairles wrote:

I don't have the book. Please feel free to buy me a copy. ;)


Until you added this above line I was confused why it had -x and -y in

a = atan2(-y,-x);

But 0,0 is your origin, the point you're going back to, so the robots
position is from the origin.


You know, in my navigation programming I had the robot turn around, so
it was always traveling forward. Made for better application of any
sensors also pointed forward.

You did notice, didn't you the direction detection:

 if (alpha(t) > -pi/2 && alpha(t) <= pi/2)
   direction = 1
 else
   direction = -1
 endif

is not inside the while loop.

 while((abs(rho(t)) > 0.1 ||
        abs(alpha(t)) > 0.1 ||
        abs(beta(t)) > 0.1)
       && t < 300 )
      ...etc.

so the direction never changes, should an overshoot occur or what not
(more likely a real world problem than a simulation one).

Now about your "commented out" angle limiting code...

   #if (alpha(t) < -pi)
   #  alpha(t) = alpha(t) + 2*pi;
   #elseif (alpha(t) > pi)
   #  alpha(t) = alpha(t) - 2*pi;
   #endif

you do not choose a unique value of +pi or -pi. Either can pass, adn
they both represent the same angle. In fact. the code has a strong
posibility when at the angle, of it being -pi one time, and +pi the next
pass. You need to chose what the angle directly behind the robot is,
whether -pi or +pi.

In the direction finding code, you used   alpha(t) > -pi/2   which
excluded -pi/2 from being a positive direction and   alpha(t) <= pi/2)
to include pi/2 as a positive direction. So you probably need to change
the conditional from   (alpha(t) < -pi)   to   (alpha(t) <= -pi)   for
the 2pi correction to be added, so the -pi angle will be eliminated from
your allowed set of angles.

Of course this might depend on the abilities of your language/compiler,
which I have never used before. It will probably handle the situation,
but it is somewhat an item for concern.

   v = kp*rho(t)

I take it this is a computed velocity, where some proportional gain, kp,
is applied to the distance, to command a velocity for the robot body.

   w = ka*alpha(t) + kb*beta(t)

and this I don't get yet. What is w supposed to be? Why is there an
alpha and beta anyway, what are they? Does the text describe these
variables? Are these the loop variants of a and b, like rho is the loop
variant of p?

Further, I have no solid idea what theta is for. Perhaps an initial
heading for the robot?

In the initial code,   a = atan2(-y,-x);   would indicate a is the true
bearing (in the goal coordinate system) of the robot to the goal
(opposite of the bearing of the robot from the goal). However the text
says, a = angle from Xr (robots frame of ref) to p. The later is the
relative bearing of the goal reference to the robots heading. The two
descriptions are mutually exclusive.

However,  alpha(t) = -theta+a  makes alpha a - pi/2, which leaves me
wondering.

For b the initial code,

alpha(t) = -theta+a
beta(t) = -theta-alpha(t)

reduces to

beta(t) = -theta-(-theta+a )

which reduces to

beta(t) = -a

which would be the true bearing of the robot from the origin. Yet the
text description of b [is the] angle of p to interial frame, which would
instead be a - 2pi.

The setup constants show x=0, y , and theta of pi/2. Now there's the
whole issue of what grid you are using, mathematicians, navigators, etc.
Too much here to sort through.

Can you shed any light on these issues? Without the book, I'm having
trouble following.

--
Randy M. Dumse
www.newmicros.com
Caution: Objects in mirror are more confused than they appear.

This is correct, the robot should not have to 'turn around' from start
to finish. If the goal is behind the robot, it should back up to the
goal. This is just a simulation, point a to point b, in real world yes,
path will change robot may turn around etc.


Sorry this is unclear, w = angular velocity of robot (theta-dot) in the
inertial refernce frame. But the rotation is same in robot frame and
intertial frame.

rho (p looks like rho, same thing :D) = sqrt((-x)^2 + (-y)^2) = straight
line distance from robot to orgin
alpha = -theta + arctan(-y / -x) = angle of robots frame to rho, the
straight line to goal (want this to be zero so robot is heading straight
for goal)

beta = -theta - alpha = angle of rho to inertial frame. want this to be
whatever orientation the robot should end up in at the goal, in my case
facing to the right (0 degrees).


Theta is the inital angle of the robots reference frame to the inertial
reference frame.

Havent taken into account your suggestions yet, but thanks. I wish i
could draw a diagram for you! would make it much easier to visualize. I
haven't worked with polar coordinates in a long time which is what beta
and rho are.

Chris Fairles wrote:

Your basic problem is with the line, w = ka*alpha(t) + kb*beta(t) .
alpha and beta must go to 0 for the proportionality concept to work.
If you want to go to alpha = -pi, you have to have a term, ka*(alpha(t)+pi),
which will go to 0 as alpha goes to -pi, but this still can run away.

In fact, I didn't find stability when I set theta=0 to get direction=1
using your code translated into C.

What kind of behavior is being sought?

Lew Mammel, Jr.

... if you make the appropriate change in the law of motion,
having the robot go backwards is the same thing as starting
it out pointed in the opposite direction and moving forwards,
so the whole thing seems to be a formal exercise.

Lewis Mammel wrote:

I was setting up theta incorrectly.


Of course, it's trying to approach the origin with beta = 0.

I found that with these changes : ( Note C language )

< if (alpha(t) > -pi/2 && alpha(t) <= pi/2)
<    direction = 1
< else
<    direction = -1


/* and */

<    w = ka*alpha(t) + kb*beta(t)
<
<    rhod = -cos(alpha(t))*v*direction
<    alphad = sin(alpha(t))/rho(t)*v*direction - w*direction
<    betad = -sin(alpha(t))/rho(t)*v*direction


... I get identical results for initial theta = pi/2 and theta = -pi/2,
giving initial alpha = pi and 0, respectively :

( I converted angles to degrees for output )

t = 268, rho = 0.00711315, alpha = -177.738, beta = 5.77617
t = 268, rho = 0.00711315, alpha =   2.2619, beta = 5.77617

This is line with my comments in my "besides" post.

I think the proportional control of the steering is a little
funky. It would make more sense just to set theta after each
iteration, since this would just correspond to setting an
initial heading and then adjusting it incrementally thereafter.

Setting an absolute rate of turn proportional to the angle magnitude
factor doesn't correspond to any sort of engineering concept of
steering that I can see.

Lew Mammel, Jr.