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

v = kp

rhod = -cos(alpha(t))

rho(t+1) = rho(t) + rhod

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

It involves describing the position of a robot in polar coordinated (p,

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; #endifv = 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***directionrho(t+1) = rho(t) + rhod

***dT alpha(t+1) = alpha(t) + alphad***dT beta(t+1) = beta(t) + betad*dTt = 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