# Closed-loop proportional motion control algorithm.

Been trying to get this algorithm right. I read it in Siegwart & Nourbakhsh's Intro to Autonomous robotics.
It involves describing the position of a robot in polar coordinated (p,
a, b) where p = straight line distance from robot's position (x,y in euclidean space), a = angle from Xr (robots frame of ref) to p and b angle of p to interial frame (assume interial frame and goal frame are same).
So it works great when the goal is infront of the robot (a = -pi/2 to pi/2) but it fails when the goal is behind. The alogorithm should have the robot go in reverse in that case (not turn around and go forward).
Heres what I got (written for octave, matlab compatable).
clear;
x = 0; y = 20; theta = pi/2; dT = 0.01; kp = 3; ka = 8; kb = -1.5;
t = 1; rho(t) = sqrt(x^2 + y^2); a = atan2(-y,-x); alpha(t) = -theta+a beta(t) = -theta-alpha(t)
if (alpha(t) > -pi/2 && alpha(t) <= pi/2) direction = 1 else direction = -1 endif
while((abs(rho(t)) > 0.1 || abs(alpha(t)) > 0.1 || abs(beta(t)) > 0.1) && t < 300 )
#if (alpha(t) < -pi) # alpha(t) = alpha(t) + 2*pi; #elseif (alpha(t) > pi) # alpha(t) = alpha(t) - 2*pi; #endif
v = kp*rho(t) w = ka*alpha(t) + kb*beta(t)
rho(t+1) = rho(t) + rhod*dT alpha(t+1) = alpha(t) + alphad*dT beta(t+1) = beta(t) + betad*dT
t = t + 1;
endwhile
polar(-beta+pi,rho)
Some things it states, alpha and beta should always be between -pi and pi. Alpha above blasts out, so what do you do? I implement a 'wrap around' which is commented out but that doesnt work.
Anyone familiar with this control scheme care to lend a hand!
Thanks, Chris
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
Should proof read before posting. p (rho) = straight line distance from robots position (x,y) to the goal (0,0).
Chris Fairles wrote:

<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
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).
#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
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
Randy M. Dumse wrote:

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.
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
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( /^image/.test(type) ){ %>
<% } %>
<%-name%>
... 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.
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
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.