Swerve Robot Localization

A Dual EKF Particle Filter localization stack was used to localize a swerve robot in real-time. It was deployed on the robot in the top right-most quadrant of the field shown here. The robot is equipped with wheel encoders, an IMU, and a LIDAR that is visible in the right photo.

The localization stack needed to be robust enough to run in a dynamic environment with unmodeled disturbances, like the field elements and other moving robots. Despite running into numerous obstacles, including obstacles that could "beach" the robot causing wheel slip and inaccurate encoder data, the localization stack proved to be reliable for the entirety of a multi-day robotics competition. Here is a visualization of the performance of the localization stack running with pre-recorded sensor data via ROS2 bags.

This project was a challenge in sim-to-real adaptation. The LIDAR data quality degraded as the angular velocity of the robot increased so the data was ignored while the robot turned. Another interesting quirk of tuning the EKFs is that the configured starting position and the incoming bagged sensor data must agree or else the convergence struggles in the beginning of a run. The estimated state would either oscillate between two positions or the EKF would diverge. A fix for the problem was to ensure that all future recorded data coincided with the starting position given to the localization stack. 

The estimated robot state was used to implement Driver-Oriented Control, a teleop mode where the robot would always move in the direction of the joystick regardless of the orientation of the robot. This mode made driving a swerve drive robot more intuitive and, in my opinion, fun!