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 & Outputs

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 referenced as $(p_x, p_y, v, \theta, \dot{\theta})$. 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.

Kinematics + Dynamics

Kinematics

To model the buggy, we are using the kinematic bicycle model assuming knife-edge contacts, defined below:

L is the wheelbase, defined to be the distance between the back wheels and the front wheel.

Using this, we can define the derivative of the cross track error (defined later in this writeup):

$$\dot{e} = ||v|| \cdot \sin(\theta - \delta) \hspace{2in} \text{eq. 1}$$

We can also define the yaw rate to be:

$$\dot{\theta} = \dfrac{||v||\tan(\delta)}{L} \hspace{2in} \text{eq. 2}$$

This is different then what the stanley paper, writes, which has the equation $\dfrac{||v||\sin(\delta)}{L}$, although it's unclear to Mehul where the paper got that equation from, or which one is right.

This may be a less noisy to calculate yaw rate in our code, rather than reading the value from the EKF

Dynamics

As of this moment, our controller does not model the nonlinear dynamic motion of the buggy. This includes things like tire slip (why we don't roll on wet roads), side forces that may happen from the curvature of the road, etc. The original paper does if someone wants to check that out.

Primary Errors

There are two different errors, cross track error $e$ and the heading error $\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_t} = \begin{pmatrix} \hat{p_t}_x \\ \hat{p_t}_y \end{pmatrix}$$

Where $p$ was defined originally, $\hat{p}$ is the closest point on the actual path, and $\hat{p_t}$ 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_t} - \hat{p})}{\mid \hat{p_t} - \hat{p} \mid}$.

Heading Error

$$\psi = \theta - \theta'$$

where $\theta'$ 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{v}_x, \hat{v}_y)$

The Controller

Theorem 1

Based on the kinematic equations of motion provided by equations 1 and 2, with the steering control law:

$$\delta(t) = \begin{cases} \psi + \tan^{-1}\dfrac{ke}{v} & \hspace{0.5in} \text{if}\hspace{0.1in} |\psi + \tan^{-1}\dfrac{ke}{v}| < \delta_{max} \\\ \delta_{max} & \hspace{0.5in} \text{if}\hspace{0.1in} \psi + \tan^{-1}\dfrac{ke}{v} \geq \delta_{max} \\\ -\delta_{max} & \hspace{0.5in} \text{if}\hspace{0.1in} \psi + \tan^{-1}\dfrac{ke}{v} \leq -\delta_{max} \end{cases}$$

$k$ is stanley's cross track gain, which is a constant set at the top of the controller

resuls in a closed loop system with a globablly asympotitically stable equilibrium at $e = 0$ for $v &gt; 0, 0 &lt; \delta_{max} &lt; \frac{\pi}{2}$.

This is proven to be true within the paper, and the rate of convergence is bounded exponentially with respect to $k$, or linearily with respect to $v$.

Active Damping

At lower speeds, the tires act as dampers, minimizing the effect of sideways velocities in turns. However, as speed increases, the damping effect will begin to vanish, creating a need for active damping.

We do this through negative feedback on yaw rate, as that was experimentally found to work best according to the paper, using the equation

$$k_{d, yaw}(r_{meas} - r_{traj})$$

where $k_{d, yaw}$ is a constant, and $r$ symbolizes yaw rate.

Low Speed Driving

At low speeds, $v$ will be near 0, making the equation $\dfrac{ke}{v}$ incredibly large, leading to problems where it will act on noise on the cross track error. Thus a tuned gain called $k_soft = 1$ is added to the term, to allow the controller to work better at lower sppeds. Thus, it is now: $\dfrac{ke}{k_{soft} + v}$

Final Steering Equation

Thus, this gives us the final steering equation of:

$$\delta = \psi + \tan^{-1}\frac{k e}{k_{soft} + v} + k_{d, yaw}(r_{meas} - r_{traj})$$

The active damping is not applied to SC's controller of NAND, due to us not measuring yaw rate within SC's estimation of NAND