Hello, everyone.
Welcome to MATLAB and Simulink Racing Lounge.
Today, we are going to talk about vehicle path tracking
using Stanley Controller.
And to talk about this, I have my colleague David Barnes, so
David, why don't you introduce yourself?
Hey everybody.
My name is David Barnes.
I'm an engineer here in the engineering development group.
I had the opportunity to work with Veer on this new project,
where we're going to be looking at vehicle path tracking
using Stanley controller.
Awesome, so David, why don't you show us
some pictures, some videos, what we're going to see today.
Awesome, yeah.
We'll go ahead and get started here.
As you can see, we're going to have
this car that's going to be exiting this highway off
towards the right.
As it's going to be sweeping, you
see the vehicle being able to be controlled,
both in its longitudinal and its lateral direction,
as it's going through making some velocity sweeps.
And we'll be following it as it goes back off towards the left
as well.
So let's move to the agenda, and see what all we
are going to cover today.
So we're going to look at the Stanley Controller itself.
We'll take a look at its implementation,
starting with generation of waypoints,
and then moving on to actually implementing this, and building
the models in Simulink, and of course
doing all of these visualizations of the vehicle
motion.
And of course, we'll wrap up with some key takeaways
for you all.
OK, so basically, we are going to focus mostly
on the implementation part.
Yes.
For those of you that don't know,
the Stanley Controller has been around for a little while,
and it's actually a path tracking algorithm.
It was actually first used in real life
in the Stanford racing team in the DARPA GRAND Challenge,
and it's using and computing the steering wheel angle to follow
a reference trajectory.
We don't want to get into all the details of the derivation
of this, but you can feel free to look at the reference here.
So basically, we have taken this formula from the reference
that we have provided.
And if you want to know more, you can refer to the paper.
So let's take a look at how this is actually
implemented with the products.
We're first going to start by generating waypoints in Driving
Scenario Designer, then we'll move over
into actually calculating some of the reference input
parameters for our Simulink model,
using a MATLAB live script.
Then we'll actually build the vehicle and the controller
model in Simulink.
And finally, wrap up with visualizing
all of this motion in bird's eye view, two dimensional plots,
as well as the 3D environment using Unreal.
Let's go to MATLAB and get started.
So let's first open up the Driving Scenario Designer,
coming over here to the apps.
I happen to have it In the start,
but you can also search for it up here at the top.
Driving Scenario Designer.
We can get started by actually adding the road to the scenario
canvas.
Once we click on the Add Route button,
we're able to just click waypoints.
And as we're clicking through, we're
going to be adding these as our road centers,
and then heading enter button.
Driving Scenario will generate our road for us.
We can go ahead and add an actor,
and there's several options for you
to choose from, including cars, pedestrians, or bicycles.
We'll get started today with just adding
a car to our starting area.
We'll start by clicking on there to add
our actor as its initial point.
And then we can right click on it, and add forward waypoints.
Once we're here, we can continue to click through on these road
centers, and show the path that we want this vehicle to follow.
Again hitting Enter, we'll save these waypoints,
and we can visualize this within Driving Scenario Designer
by hitting the run button.
As you can see, this vehicle oscillates back and forth
following with these waypoints, and if you want to,
you can change the vehicle's constant velocity here.
We can save this as a scenario file.
And this is going to save a ton of information
from the scenario, including the road information,
information about the actors, as well as any sensors you
might have added.
Once we save this, we can then call this from a live script.
Now that we've saved our scenario file,
and exported it from Driving Scenario Designer,
we've saved it here as simple track.
We can now use this in our live scripts.
The script that we have here is going
to set up everything for our Simulink model.
The first thing we'll do is add in some of the images
that we use in the model, but then we're actually
going to load in scene files.
We're going to grab the waypoints from the references
that are in there from the actor's specifications,
and then we'll define those waypoints, based
on the x and the y values as the first and the second column.
Then, we need to give some of the blocks
the initial position of the vehicle.
So we can describe the first element
in those reference vectors.
Some of the other information that we'll need
is going to include the distance vector
to see how far along the vehicle is moving along its path.
So after calculating that, we will also linearize and smooth
this data, so that it's nice and clean for our Simulink model.
We'll also need to calculate the angle for it.
This is using the a tan 2d, which is the four quadrant
tangent calculation, and it's going
to output all the values in degrees.
And then again, smoothing this, and grabbing some initial
values for the yaw angle.
Next, we'll also need to create the direction vector.
This is just simple vector of 1's, because it's always
moving forward.
In the chance that you have a scenario where
you are moving forwards and backwards,
you can use negative 1 for those directions.
Then, we're going to calculate the curvature,
using just a simple curvature calculation
function down the bottom.
For our x in our y values, and we're all
ready to get started with our model.
OK, David.
Thanks for covering that.
So it means that it is not necessary
that the user has to grab the waypoints from the Driving
Scenario Designer, even if they are
having some kind of test data.
So they can have the values over here,
and then they can go through the same procedure.
Exactly.
Awesome.
With all of these values calculated,
we can go ahead and go to the model.
Now that we've got the model open,
we can go ahead, and take a look at the different components
for it.
The first, starting over on the left, is the reference data.
We went through and calculated all of these different values
as a function of the vehicle's distance.
So as we input the vehicle's velocity,
we're just going to simply integrate
that to see how far along its path it is.
And then we'll be outputting from these simple lookup
tables, the x values, y values, angle, as well
as its curvature and direction.
Now that we have all of those states,
we can actually pass this in to the Stanley Controller.
So using the automated driving toolbox implementation
of the Stanley controller, it's relatively simple for you
to get started with the kinematic bicycle model.
All that you need to do is change
the gains for your forward motion as well as your reverse,
and then give it some specifications
about your vehicle, such as its total wheel base, as well
as the maximum steering angle that you
want for your controller.
So yeah, and I think the advantage
in using Stanley Controller is that we do have an upper bound.
For here, we are using a maximum steering
angle, which we have kept 35.
So, yeah.
Simple as that.
Now that we have everything defined
and our Controller ready, all we have to do
is pull these signals into the three degree of freedom vehicle
body to do all of these calculations.
For this simulation, we're using just a constant velocity
that will change.
Let's go ahead and see how the simulation runs.
As you can see the vehicle is following the path fairly well.
As it's moving through, you'll have a little bit
of cross-track error as it's going,
but it will slowly start to match over
the course of some time.
So now that our controller is working fairly well,
let's see what happens if we increase
the velocity a little bit.
So we can go ahead and increase this velocity
from 8 meters per second to, let's say,
12 meters per second.
Just a little bit faster.
Now when we run the simulation, let's
see how our controller responds.
After the model compiles, we'll see that the vehicle starts off
doing fairly well, in terms of its track but--
OK, I can see some deviation.
Yes.
Quite a bit from this controller.
And as you can see, as the curvature starts
to increase for the waypoints, the vehicle definitely
has a little bit of spin off as it goes
in its different directions.
But we can actually make this a whole lot better
by switching the type of Stanley implementation
that we're using.
And that's going to be moving and switching
the actual type of model that's used in the Stanley Controller.
This is going to be taking and moving
from this vehicle model of a kinematic bicycle model,
which is a little bit unstable in terms of its system,
to the dynamic model.
So with these new values, we actually
have new ports that become available.
Starting from the top, one of the first ones
is curvature, which we fortunately already
have calculated using the reference data.
So I'll go ahead and connect that here.
The next is going to be the current yaw rate, which
we already get out of our three degree of freedom dual track
model, which is this value r.
I can just right-click the signal, and drag it in.
And then the next is the current steering rate,
and so we'll be able to use that by passing it into a unit delay
block, and get this connected in.
So this is basically to avoid the algebraic loop?
Exactly.
Now that we have our model ready to go,
I'm going to turn off stimulation pacing now
that we've seen it a few times, and we'll go ahead
and let this model run.
As you see, the model runs quite quickly
without simulation pacing--
Ah, OK.
And we have fantastic values in terms
of how our vehicle's responding to this high velocity,
as well as the curves.
So yeah David, this looks great, and just a question.
So the vehicle parameters that you
defined when you selected the dynamics model
in the lateral controller.
So are those also defined in the vehicle body 3DOF dual track?
Yes, they certainly are.
You want to make sure that all those values match
as you're making sure that your vehicle is there.
And you can find these hidden into these different options
here.
So for your vehicle parameters, you'll
have those matching here, such as your vehicle mass in those
distances to the to the axle.
Is there a way where we can compare all these vehicles
and have a proper Visualization.
Yes, we certainly do, and that's going
to be using the Simulink data inspector.
So we've already saved this from some previous runs,
and we can go ahead and grab that.
And what we can see from here is based off
of the different parameters that we had,
such as the type of model that's used in the Stanley
Controller, as well as those gains,
we can actually press play down here at the bottom,
and watch and see these values as a function of time,
as well as their positions x and y.
So that's a great tool, because it
is showing the x, y comparison, and also the state and command
comparison.
Awesome.
Now that we've seen a fairly simple model,
let's see what happens when we increase some
of the fidelity for our models.
Well, we're adding in higher fidelity
by modeling the powertrain and the driveline in this block
right here.
By going in, we have created a simple powertrain model,
as well as drivelines, braking systems, as well as
adding in the brakes and wheels subsystem.
And so David, if you can click on the brakes and wheels.
So this tire has been taken from the vehicle dynamics blocks.
Correct, yes.
Now that we've had this powertrain model,
we actually have a little bit of a change to our vehicle
dynamics block.
Instead of having a constant input, or maybe
a changing input for your velocity,
we're now modeling the velocity changes,
based on the forces on the front and the rear axles.
This means that we'll take our calculated vehicle velocity,
and change this into a distance.
This will allow us to then be able to control
the velocity using a Stanley-based controller.
Based on the position of the vehicle, similar to how
we have our other states, we're now
going to have a velocity to control for,
in our Stanley-based controller.
It's going to be a combined longitudinal and lateral
controller.
So let's take a look.
In here we have our same Stanley Controller
for our lateral dynamics.
That's going to be taking into account our states,
and it's going to be using the dynamic bicycle
model, so that we have this nice, clean control.
But we're also going to have the longitudinal control, here,
based off of the current velocity and the direction
of the vehicle.
This is also very similar, where we're
using this simple controller with a proportional
and an interval gain, and with a specified sample time.
We're also going to use vehicle parameters,
like the maximum and minimum longitudinal acceleration
and deceleration rates.
So David, this is basically to track the velocity reference.
Exactly.
So now we'll be able to control the vehicle's position, as well
as how fast it's going.
OK, in the last model, we were just having constant velocity.
Now we have a varying velocity, a reference velocity for this.
Correct.
So that's awesome, that's a good addition to the model.
Yes.
So now let's see where this velocity profile is actually
coming from.
It's in the script here that's hyperlinked in this link,
and feel free to take a look at this script
to see how we're using the trapezoidal rule to calculate
and smooth out a velocity based on of its position.
And then, going back to the model,
we've added some additional ways that we
can visualize this vehicle.
We still have our 2D visualization
with that x y plotter that's available in some
of the other vehicle dynamic block sets examples.
As well as adding in the 3D visualization
in Unreal using just these two blocks.
There's also the opportunity to use the bird's eye scope
using the block that's here, and is readily available
using the bird's eye scope, up here at the top ribbon.
Now that we have the bird's eye scope set up,
we're ready to run the simulation.
By pressing run either here or in the simulation tab,
we can watch all of our different visualization
options.
And here we go.
Now we have the vehicle moving along its path,
similar to the video that we saw earlier.
Now that the simulation has finished,
we can actually see that the circles that
were previously uniform, now have sections
that are scrunched up and more spread apart,
and this is based on the vehicle's velocity changing
as a function of path.
So now we're able to see that across our simulation,
the Stanley controllers were able to fairly well keep
both the longitudinal and the lateral states of the vehicle
intact.
OK, that's great.
These were using the automated driving toolboxes
implementation, but there's also some available in the vehicle
dynamics block set.
So David, can you show the block in the library browser
where we can find it in the vehicle dynamics blocks?
We certainly can.
Opening up the library browser, we
can actually see where the original blocks came from,
here out of the Automated Driving Toolbox.
They're coming here out of your vehicle control.
And then you have your lateral Stanley Controller, as well as
your longitudinal one.
Or, going all the way down to the vehicle dynamics block set,
we can see that these are available here in your vehicle
scenarios, and then moving into the different driver
models that are available.
You'll have these available as a lateral driver,
a longitudinal one, or available as combined in the Predictive
driver as well.
So David, do we have any example of where
we can see the implementation of this predictive driver, where
we can select the option for the Stanley Controller?
Yes, we do.
Actually, shipping in 21a, there's
a new example reference application
that I think would be perfect.
As you can see this is a very similar layout to the model
we have before, where we have some reference values coming
from here, whereas instead of our calculations,
this oval track reference, where we
can see the implementation of the predictive driver,
here in this model.
Opening up this combined longitudinal and lateral
driver, we see that actually the lateral control type can be
either Stanley or predictive.
So we are also defining the same parameters
that we defined in the previous block.
So David, can you show where we are defining those parameters?
Certainly.
So over here under the reference control,
we can see that the Stanley value still
has these gains associated with it,
and we're still also passing in vehicle parameters, here shown
as a part of the structure of the vehicle.
So David, do we have any visualization in this one,
because I can see a subsystem in the visualization,
so can you go inside that and show
the viewers what extra we got?
Exactly.
So similar to what we had, there's
the 3D visualization using Unreal.
There's also a brand new GG plotter that's
available for the simulation.
Let's go ahead and run the model,
and see what this looks like.
As you can see, we're going to have a little bit
more information about the vehicle forces that
are involved in the velocity on the tires,
but we also now have this new GG diagram,
which shows as the vehicle goes around the track, how
these accelerations are actually occurring.
OK, that's awesome, David.
Because I think that the GG plotter addition in this model
is very much relevant to the racing teams.
So that's great, David.
I think we have seen all the models.
So can we go to the key takeaways of this video?
Yeah, so today, we've gotten to see
quite a few different things.
But first off, was showing how we
can smooth some of these reference input parameters,
and that's going to be a very important step for you,
as you're going to continue developing your own models,
so that you have really good tracking.
And a lot of that has to come in to the second point, which
is based on higher velocities and varying curvatures,
combining these lateral and longitudinal dynamics are
really going to give you better path tracking results,
based on that nice, smooth data.
And MATLAB and Simulink do provide you
a whole lot of algorithms and tools
for doing these different parts of the process,
from generating some of these waypoints,
to building the vehicle model and the controllers,
and also visualizing these in different levels of fidelity
in different environments.
Yeah, thanks David, and for all the viewers,
if you want to contact us, you can contact us
at racinglounge@mathworks.com, and feel
free to join our Facebook group, where
we keep on sharing the latest technical resources,
and even updates regarding our upcoming webinars.
And for more videos and tutorials,
you can visit the student tutorials and videos website.
So with this, we've come to the end of the video.
Thanks David, that was a great presentation,
and we covered a lot of things like implementation
of Stanley Controllers using MATLAB and Simulink.
Thank you, so much.