Controller

Controller

Our current controller is based off of the Stanley Controller. An in-depth explanation of the stanley algorithm can also be found later in this file.

Inputs

At a high level, the controller takes in two different inputs:

• The state of the buggy at hand, which should include the position in easting, northing, the speed of the buggy, the heading of the buggy, and how the heading rate. These will henceforth be references as $(p_x, p_y, v, \psi, \dot{\psi})$. This should be inputted at 100Hz
• The trajectory that should be followed. This comes from the path planner, and is a new path generated at 10Hz

Parameters

• dist - A parameter which is the initial guess to where we are on the path, is not that important as after the first iteration of the loop, we find a more accurate index
• controllerName - tells the ROS node what the name of the node is, i.e. SC_controller, SC_NAND_controller, NAND_controller
• traj_name - the name of the initial path we will be following
• stateTopic - the ROS topic name of the buggy state input.
• trajectoryTopic - the ROS topic name of the trajectory input
• controller - which controller we are using, as of right now the only option is Stanley.
• useHeadingRate - a boolean flag that if set to false, means that we don't need the input $\dot{\psi}$, and will be further explained in the stanley controller section.

Outputs

There is one non-debug output topic from the system, which is the steering command, capped between $-20^\circ, 20^\circ$. It will be later called $\delta$ within documents

Initialization Check

Before we allow the controller to publish steering outputs, we wait for the init checks. Below is a list of all the possible init checks:

• Has the controller ever received an odom input
• Is the covariance of the system relatively small ($<1m^2$), in relation to the easting and northing position
• Compared to the closest position on the path, are we faced the right direction. This is if the offset between both is in between $[-\pi, \pi]$

Stanley Controller

This is a deep dive into the stanley controller, a controller published from Stanford in 2005, in this paper. The main idea is to focus on converging two different errors, heading and cross track at the same time.

Error

There are two different errors, cross track error $e$ and the heading error $e_\psi$. Here is an image describing these errors: In this image, pink is the current buggy, green is the target position that the buggy is following. Yellow is the cross track error $e$, and the heading error is in orange. The goal is that with both converged, we would be able to follow the track, but the convergence of one of these errors is not enough.

Here is the definition of both errors. Cross Track $$p = \begin{vector}p_x & p_y \end{vector}$$