If you were Registered and logged in, you could reply and use other advanced thread options
|
Posted by Chris Fairles on October 10, 2005, 11:03 pm
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)
rhod = -cos(alpha(t))*v*direction
alphad = sin(alpha(t))/rho(t)*v*direction - w*direction
betad = -sin(alpha(t))/rho(t)*v*direction
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
|
|
Posted by Chris Fairles on October 10, 2005, 11:04 pm
Should proof read before posting. p (rho) = straight line distance from
robots position (x,y) to the goal (0,0).
Chris Fairles wrote:
> 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)
>
> rhod = -cos(alpha(t))*v*direction
> alphad = sin(alpha(t))/rho(t)*v*direction - w*direction
> betad = -sin(alpha(t))/rho(t)*v*direction
>
> 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
|
|
Posted by Randy M. Dumse on October 12, 2005, 12:08 am
I don't have the book. Please feel free to buy me a copy. ;)
> Should proof read before posting. p (rho) = straight line distance
> from robots position (x,y) to the goal (0,0).
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.
> 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).
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.
|
|
Posted by Chris Fairles on October 12, 2005, 6:51 pm
Randy M. Dumse wrote:
> 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).
>
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.
> 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?
>
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).
> Further, I have no solid idea what theta is for. Perhaps an initial
> heading for the robot?
>
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.
|
|
Posted by Lewis Mammel on October 18, 2005, 8:05 pm
Chris Fairles wrote:
>
> 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)
>
> rhod = -cos(alpha(t))*v*direction
> alphad = sin(alpha(t))/rho(t)*v*direction - w*direction
> betad = -sin(alpha(t))/rho(t)*v*direction
>
> 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)
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.
|
Page 1 of 2 1 2 > last >>
Related Posts
Latest Posts
|
|
Yahoo! 
Google 
Windows Live 
del.icio.us 
digg 
Netscape 
XML site map