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.

Primary Errors

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. When the errors are both $0$, that means that the buggy is on the path, and pointed the right direction. If the cross track error is non-zero, that means we are off the path, which is obviously incorrect. If the heading error is non-zero, that means that we are not pointed in the correct direction, which will it mean the buggy won't be able to follow the path.

Here is the definition of both errors:

Cross Track Error $$ p = \begin{pmatrix} p_x \ p_y \end{pmatrix}, \hat{p} = \begin{pmatrix} \hat{p}_x \ \hat{p}_y \end{pmatrix}, \hat{p}' = \begin{pmatrix} \hat{p}'_x \ \hat{p}'_y \end{pmatrix}$$ Where $p$ was defined originally, $\hat{p}$ is the closest point on the actual path, and $\hat{p}'$ is a point $0.0001m$ after $\hat{p}$ on the path.

This means that the cross track error is defined as $e = \dfrac{(p - \hat{p}) \times (\hat{p}' - \hat{p})}{\mid \hat{p}' - \hat{p} \mid}$.

Heading Error $$e_\psi = \psi - \psi'$$ where $\psi'$ is calculated as follows:

1. Find the closest point on the path to where the buggy currently is.
2. Step forward $x$ meters on the path, where $x$ is a pre-defined constant called the lookahead time
3. Calculate the heading of the path at that moment, which is the first derivative of the path equivalent to $\texttt{arctan2}(\hat{\dot{p}}_x, \hat{\dot{p}}'_y)$