State estimation for a team of soccer robots

RoboTeam Twente is a student team from the University of Twente that develops a team of autonomous soccer robots. Each year we compete in the RoboCup in the small size league, where teams from all over the world put there robots to the test in a tournament. Small size league teams consist of 8 small robots that try to collaborate in order to get the orange golf ball in the opponents goal. Speed, precision and strategy are especially important in this league, as the games are typically fast-paced and chaotic. An accurate and low-latency tracking of all the robots and the ball is therefore of vital importance. For all of our tests and demos, we make use of the Basler acA1300-75gc camera, which is very suitable for reliably tracking our robots at low latency. The vision software that we use is called SSL- Vision, which is specifically designed for this competition. It recognizes the colored marker patterns on top of the robots (see figure 1), and links these to information about the robots ID and team. The position of the markers in the captured images are geometrically transformed to field coordinates, so for each robot (and the ball) we receive information about its position and yaw.

The software is already quite efficient, but still computer vision always introduces significant latency. From measurements on our setup we found roughly 80ms of delay between sending a command to a robot, and measuring the resulting movement in our software. For tactical decision making this is not a significant problem. However, for the stability of our positional control loops and obstacle avoidance, this delay turned out to be detrimental. To ensure stability, we had to make concessions on performance and responsiveness of our robots in e.g. obstacle avoidance situations.To improve our situation, we decided to equip each robot with a high-end IMU that would give us information about its orientation and accelerations. Using the yaw and x, y acceleration measurements, we could use sensor fusion to combine the speed of these measurements with the accuracy of the vision measurements. For this end, we designed a discrete Kalman filter (DKF), running at 100 Hz, to obtain fast and accurate estimations of the pose and velocity of each robot. In this design it was especially important to correct for the delay between the IMU and vision measurements, which further improved our estimations.

The most important gain of fusing the sensors is summarized in the plots in figure 2-4. The errors in our pure vision measurements, caused by the 80ms delay, are compared against the errors in the sensor fusion estimations. We saw that on average, when applying the DKF, the positional error caused by the delay is reduced by roughly a factor of 5. For the velocity estimation the improvement was not as huge, but still significant. In addition it led to filtering out the sharpest peaks in the velocity estimation.