Cube Swarm

I was in charge of the navigation system. The swarm uses the Extended Kalman Filter (EKF) as the sensor fusion algorithm to localize itself in the environment. Encoder readings are used to get a baseline prediction of the robot's positions. AprilTags are placed on every side of each robot and on landmarks around the environment. The robots use the onboard Arducam for AprilTags detection and position correction to prevent error accumulation. 

To avoid collisions, the swarm uses Conflict-based Search (CBS) on a centralized computer as the multi-agent path planning algorithm. CBS uses a set of constrained, single-agent pathfinding problems to find paths that are consistent with these constraints. At the low level, the algorithm uses A* with an additional timestep component for individual robot path planning. To demonstrate the navigation system, I made a Gazebo simulation with simplified URDF models of the robots as seen below. The simulation initializes the cubes at random locations and has them line up without collisions, attach, and cross a gap.