Highlight text to annotate itX
Hello and welcome back. This is the third Programming and
Simulation lecture. This week I'll be talking about go-to-goal
controllers. These controllers are used to steer mobile
robots to, from point A to point B. In fact, in this week's programming
assignment you'll be implementing a PID based
go-to-goal controller. Therefore, you will need to implement the
proportional, integral, and derivative, derivative terms of the controller as well
as adjust the gains for optimal performance.
The notation that I will use in this week's assignment is as follows.
First, we have the location of the robot denoted by x and y.
The robot also has an orientation denoted by theta with respect to the x axis.
The location of the goal is denoted by x_g and y_g, and the vector between
the goal, and the, and the position of the robot is denoted by u_gtg.
This vector also has an angle, theta_g, with
res, or with the respect to the x-axis. Now, this particular go-to-goal controller
that you'll be implementing works in the following
way. We're going to keep the linear velocity
constant and use PID to compute omega, the angle of
velocity of the robots, that will steer it towards the
goal, and you will implement this in the go-to-goal
class, defined in GoToGoal.m. An important part of this controller is
memory. We need memory
to keep track of the previous and accumulated error for
the integral and the derivative terms of the PID controller.
The way we keep memory is through variables in the class.
We have two variables. The first is this variable right here,
which is the total accumul, accumulated error.
Whereas this, whereas, whereas this variable right here is the previous error.
And both of these will be remembered at each time step so that we
can keep track and use them in the, when you're calculating the
derivative and when you're calculating the integral.
You also should note, take note of these three variables up here, which correspond
to the proportional, integral, and derivative
gains of the controller, which you can adjust for the PID
controller. You will implement all three parts of the
PID controller in the execute function. This execute function is also in the
GoToGoal.m file. And this execute function takes in the
location of the goal, as well as a linear velocity and the
location and orientation of the robot. And computes the appropriate linear and
angular velocity, and outputs this. So let's take a look at the skeleton code
for this function. This is the skeleton code for the execute
function. At the top, we're retrieving the location
of the goal as well as the location of the robot and its
orientation, and the design of linear velocity.
Then you will need to compute the heading to the goal.
And compute the error between the robot's orientation and
the orientation of the goal with respect to the robot.
In the last part, you need to complete the three terms
of PID controller. First, you calculate the proportional
term, then the integral term, and then the derivative
term. And then you combine all three terms along
with their gains to compute the appropriate angular
velocity of the robot. Last, the integral, the accumulated error
and the previous error are saved for the next time
step. Now, if you run this PID controller,
you're going to get something similar to the graph on the left, where
the red dashed line is the desired angle to the goal, and the
blue line, the blue solid line is the actual
orientation of the robot. And what you want to see is that the
difference between those two is minimized by the PID
controller. And the other thing you want to see is
that your gains ensure that there is very low overshoot and almost no osco,
oscillations in the, in the output. You can see there's a little bit,
but not too much. And also the blue line almost perfectly
matches up the red line. Now, to help you test this, we've included
a stop condition so that the robot stops when it gets near
the goal. And you can adjust this goal location in
the constructor of the supervisor. And what you want to edit is this variable
right here, obj.goal. And, and, by default I have selected -1
meter in the x direction and 1 meter up in the y
direction, as indicated right here. Now, let's see this in action.
I'm going to launch the simulator. And this brings it up, us, to the spar
screen. I'm going to hit the play button and what
you should see is the graph of the output come up as well as the robot
should start immediately moving towards the goal location.
So, let's do this. There's the graph.
I'm going to move it out of the way. And, here you can see the robot driving
towards the goal location. And I can click over here and look at the
graph, and I see that the output is very nice.
And, if I close down the simulator, I'll show you that I've used fprintf statement
to print out the current position of the
robot as well as the estimated position of the
robot. And you should be very, very close to
stock condition stops the robot within 5 centimeters of the goal.
So you, you should be fairly close here. My tips for this week are the same as last
week. Just make sure to read the Week 3 section
in the manual for more details. And, use as many fprintf statements as you
can to help you debug your code just like I did in the
demo. And, that's it for this week.