How to write a script for a go-to-goal differential robot using just a proportional controller?

I'm trying to write a script where a differential drive robot travels from an origin to a goal location. I've used just a proportional controller, but upon simulation the trajectory of the robot to the goal location doesn't look very much like a proportional controller. Can anyone tell me if there is anything wrong with my code?

%UAV final destination
r=5;
s=2;
plot(r,s,'*')
hold on
%UAV initial position
a=0;
b=0;
%UAV current co-ordinates
x_old=a;
y_old=b;
x_new=0;
y_new=0;
%desired angle to goal
phi_desired= atan((s-a)/(r-a))
%gain
k=1;
%time step
t=.1;
%velocity
vel_x=1;
vel_y=1;
v=1;

%initial heading

phi_old=0; phi_new=0;

while(1)

    if(x_new==r && y_new==s)
        break;
    end

        error=phi_desired-phi_old
        % w: angular velocity

        w=k*error

        phi_new=t*w+phi_old
        vel_x=v*cos(phi_new)
        vel_y=v*sin(phi_new)

        pause(t);

        x_new=x_old+vel_x * t
        y_new=y_old+vel_y * t
        phi_old=atan((s-y_new)/(r-x_new))    

        plot(x_new,y_new,'*');
        hold on
        x_old=x_new;
        y_old=y_new;

end

Answer 1

[Geoff Hayes]

Based on what I glanced at Proportional Control your code seems reasonable. Though the line

 phi_new=t*w+phi_old;

could be replaced by

 phi_new=k*error+phi_old;

with

 k=0.1;

just to avoid the confusion as to why the time step t variable is being used to update the new heading (when it should be the gain k doing this instead).

And I do wonder about the use of phi_old twice in the following equation

 error=phi_desired-phi_old;
 phi_new=k*error+phi_old;

especially when compared with the notes at the wikipedia article. Or, maybe error should be replaced by

 error=phi_desired-phi_new;

NOTE use of error as a variable name should be discouraged since error is a built-in MATLAB function.

But is calculating the heading, and adjusting it at each iteration, something that a Proportional Controller is intended for? If the difference between the actual heading and the desired is large, do we do anything different than if the difference were small? The speed (for example) of the robot doesn't increase because of this great difference...

When I ran your code, the robot/uav moved towards the goal (as expected) and then overshot the goal, continuing on its way. While it is unlikely that the condition to exit will ever be met (due to the new (x,y) coordinate never being equal to the goal (r,s) coordinate), I had thought that the robot would correct itself and return towards the goal.

I noticed that your code uses atan which will only ever return values in the range [-pi/2,pi/2]. Instead, atan2 should be used since it will return values in the range [-pi,pi].

Because of this, I changed your code as follows. The desired phi (heading) from

 phi_desired= atan((s-a)/(r-a))

to

 phi_desired = atan2((r-a),(s-a));

The velocity calculations from

 vel_x=v*cos(phi_new)
 vel_y=v*sin(phi_new)

to

 vel_x=v*sin(phi_new);
 vel_y=v*cos(phi_new);

And the new/old phi from

 phi_old=atan((s-y_new)/(r-x_new))

to

 phi_old=atan2((r-x_new),(s-y_new));

So the code is almost the same as yours, just using atan2 AND I reversed the order of the inputs. This is just a personal preference of mine when dealing with headings (phi). Now, the headings are relative from north (y-axis) as opposed to east (x-axis) so it is a little more intuitive when you see the position of the robot versus the position of the goal, and the heading from the former to the latter.

I re-ran your code (with the above changes) and observed that when the robot overshot the goal, it did "turn around".

You may also want to add some additional logic to handle the case when the angles straddle 0 radians

 diff = phi_desired-phi_old;
 if diff > pi
     diff = diff - 2*pi;
 elseif diff < -pi
     diff = diff + 2*pi;
 end

For example, this would handle the case where phi_desired=355*pi/180 and phi_old=5*pi/180 (for example).

Hope that some of the above helps!

[HJ Jay]

Hello Geoff,

Thanks for your answer.

I figured out what my problem was. Like you said one was the velocity doesn't die down to zero as it approaches the destination and secondly my phi_desired had to be calculated online. I fixed the code also made changes to the part where the desired angle heading is found out, by using switch cases depending upon the quadrant(trigonometry).

My code is avaiable at https://github.com/hjjayakrishnan/go-to-goal, go-to-goal.m

[Geoff Hayes]

Great that you figured out the problem!