The gap follow algorithm was the first lab in a series meant to get us familiar with the hardware platform and simulation environment. This control scheme entailed taking an array of LiDAR points and finding the best gap through which the car should steer through. The error in heading would then be used to inform the steering correction term based on PID control theory.

In order to enable more complex planning and control algorithms, mapping and localization were used to determine the pose of the car at any given moment. A ROS2 particle filter package was used to complete the initial course mapping. The 2D pixel map from this course map was then used to localize using the same particle filter package providing real-time pose data for the car. This mapping allowed for the creation of waypoint maps that could be used for more complex control schemes such as pure pursuit and Model Predictive Control for future labs.

For the pure pursuit lab, a simple point trajectory was followed with the error between car heading and desired point along a pre-planned path being used as a steering correction term. This error was then used in a PID controller to steer the car in the right direction along the path. The desired waypoint was then updated as the car went along the track allowing the car to progress forward through the whole planned trajectory. Utilizing the ROS2 SLAM package mentioned previously, this control method was successfully implemented on hardware.

Model predictive control (MPC) was used to help improve the pure pursuit method. As a cost function for this optimization problem, a simple quadratic regulator was used placing weight on both deviation from the desired trajectory and the control inputs used to keep the car on this desired trajectory. This cost function was coupled with physical constraints to ensure a dynamically feasible solution on each compute cycle. For these dynamics conditions, both the vehicle dynamics, approximated as a linearized and discretized kinematic bicycle model, and the hardware control input limits were considered.

This lab focused on implementing planning methods to allow for real time obstacle avoidance. In order to accomplish this, a Rapidly-exploring Random Tree algorithm was utilized. This algorithm generated random points in a pre-determined area in front of the car that were then used to generate a tree of points. Once the expansion had reached the desired waypoint, the expansion would end and an optimal path to the next way point was calculated. These new waypoints were then used in the pure pursuit algorithm to guide the car in real-time.

As a measure to speed up the compute time of this planning, the search space was discretized. The discretization and update rate of this algorithm were all tuned in simulation and then tested on hardware for optimal performance.