ME461: Ping Pong Ball Chasing Robot

Overview

The ping pong ball chasing robot is our final project for Dan Block's "Computer Control of Mechanical Systems" course at the University of Illinois. In this project, a base station with a camera locates ping pong balls and a robot car and instructs the robot car to "kick" the balls until all are outside of the boundary. The base station uses OpenCV's Python wrappers for ball detection and ArUco tag detection. The ArUco tags are used to define the origin and virtual boundary. An ArUco tag on the robot car allows the base station to calculate the position of the robot relative to the origin. The differential-steering robot car, built over the semester in a series of labs, features a Texas Instruments TMS320F28379D processor that controls two drive motors and an additional DC brush motor that drives the ball-kicking paddle. The TMS320F28379D runs a PI control loop to track a commanded forward and angular speed using motor encoder data for feedback. A Raspberry Pi 4 provides these desired speeds over UART, along with a command to turn the paddle on or off.

MS320F28379D Launchpad

The Texas Instruments TMS320F28379D Launchpad Interfaces with the Raspberry Pi over a Serial UART Interface. The communication protocol consists of four states triggered by the Raspberry Pi: An echo state for debugging, a "send data" state, a "receive data" state, and a paddle motor control state. In the receive state, the Launchpad receives two floating point values, desired forward and angular speed, from the Raspberry Pi. In the send state, the Launchpad sends estimated forward speed and angular velocity. In the paddle motor control state, a GPIO output driving the gate of a transistor is set high or low depending on the received data.

Motor encoders are used to calculate the angular position of the two wheels using the enhanced quadrature encoder pulse (eQEP) peripheral. After converting from encoder pulses to linear distance, the left and right wheel velocity is calculated using a discrete derivative. The forward velocity is taken as the average of the left and right speeds, and the angular velocity is the difference between the left and right wheel velocities divided by the width of the robot. PI (proportional integral) control is used to track the desired forward and angular speed. Errors for the left and right velocity are calculated as a function of the desired speed, the turn rate error, and the estimated left and right speeds. The controller calculations are triggered by a timer interrupt every 4ms.

The code running on the Launchpad can be found in RED_BOARD/labstarter/final_project_main.c

Raspberry Pi

The bulk of the robot's control takes place on the Raspberry Pi Model 4. A control loop and a base station loop run simultaneously on the Pi using Python's threading module. The state of the robot, the pose estimation, and the desired pose are accessed and/or modified by both threads. Data locks are therefore used to prevent race conditions, or bugs caused when two processes try to operate on the same data at the same time.

BaseStationThread

The base station thread initializes by connecting to a TCP server running on the base station PC. The initial state of the robot is calculated by averaging the robot's position data over several frames, and the desired position accessed by the control thread is initialized to the position of the closest ball. The base station thread then continuously polls for the robot's position, the position of the closest ball, and the distance from the robot and the ball to the nearest boundary. If the nearest ball is outside the virtual boundary or the robot is more than a foot outside the virtual boundary, then the robot's state is set to WAIT. In this state, the robot is stationary and the paddle motor is off. In the other case, the state is set to KICK. In the KICK state, the control thread drives the robot to the nearest ball and triggers the paddle motor. Robot pose information from the base station is fused with estimated forward and angular speed from the Launchpad with a Kalman filter. The state of the robot is corrected every iteration of the base station loop. Finally, the desired position is updated to the location of the closest ball. The base station loop runs with a period of 75ms, limited by the base station's frame rate. Deterministic timing is difficult without a real-time OS or timer interrupts, but software timers allowed us to account for the time taken to run the loop and get timing that was plenty accurate.

Control Thread

The control thread first initializes the serial communication between the Pi and the Launchpad. The control loop then runs continuously with a period of 5ms. the robot's position is estimated each iteration of the control loop. Forward and angular velocities are read from the LaunchPad and numerically integrated to calculate the predicted pose. Because these estimates are prone to drift, they are periodically corrected with more accurate but less responsive base station data using a Kalman filter. Then, the control loop checks the state of the robot, which is set in the base station loop. In the WAIT state, a desired forward and angular speed of 0 is sent to the Launchpad along with the command to turn the paddle motor off. In the KICK state, the desired forward and angular speeds are determined by the position controller. The position controller takes the current position and desired position as inputs. The desired heading is then calculated with atan2, and the error between the current heading and desired heading is calculated so that the robot turns clockwise or counterclockwise depending on which angle is smaller. The desired angular speed is then a constant gain multiplied by the error. The desired forward speed is the product of a constant gain, the norm of the distance between current and desired positions, and a windowing function that prevents the robot from moving forward until it's pointed in the right direction. Finally, the desired forward and angular speeds are saturated to some maximum value. After sending desired velocity commands to the Launchpad, the control loop determines the state of the paddle motor. If the robot is within a foot of the ball or the estimated time to reach the ball is less than two seconds, the paddle motor is turned on. The paddle motor is turned off if these conditions are not met.

The code running on the Raspberry Pi is found in RASPI/main.py,RASPI/PiThreads.py, RASPI/RobotControl.py, and RASPI/SerialCommuncation.py

Base Station

The purpose of the base station was to determine the pose of the robot in a global coordinate system, find the position of the balls near the robot, and host a TCP server from which the robot could request this data from. The base station program was run on a PC with one thread hosting the TCP server, while the main thread determined the robot pose and ball position using a Logitech C920X Pro webcam and OpenCV. The pose was calculated for every frame of the video and then updated on the TCP server.

Marker Tracking and Robot Pose Estimation

To track the robot's position and establish a global coordinate system, fiducial markers were tracked using OpenCV with the ArUco module, which provides functions to generate, track, and estimate the pose of ArUco fiducial markers.

To estimate the pose of the markers, first the corners of each marker are found. Using the corners and the side length of the markers, the pose of the camera relative to each marker can be determined in terms of rotation and translation vectors. Further information regarding marker detection and pose estimation can be found here.

To accurately estimate the pose of the markers, the intrinsic parameters and distortion coefficients of the camera need to be accounted for. These parameters were determined by performing a camera calibration. Following this tutorial, the camera was calibrated using a ChArUco board, which combines the typical chessboard pattern used for camera calibration with ArUco markers. This allows for the calibration board to be occluded or partially visible in the images used for calibration, making the process easier. The code used for camera calibration can be found in BASE_STATION/cameraCalibration.py.

To establish a world coordinate system, four markers are placed on the floor, with the bottom left marker being defined as the origin. Next, the rotation vectors of the camera with respect to the origin are converted to a rotation matrix using cv.Rodrigues(). Multiplying the inverted rotation matrix by the translation vectors for all markers yields the position of each marker with respect to the origin. The orientation of the markers in the plane of the global coordinate system is obtained by first converting the rotation vectors for each marker to rotation matrices using cv.Rodrigues(), then extracting the Euler angles from each rotation matrix using cv.RQDecomp3x3(). This results in pose of any marker in the global coordinate system. Further information regarding the coordinate transformations can be found here.

With the pose of the four markers on the floor in the global coordinate system and their location and pixel coordinates, a perspective transformation matrix can be found using cv.findHomography(), which maps pixel coordinates to global coordinates. This assumes that all pixel coordinates are on the plane established by the four markers (i.e. the floor). Hence, it does not account for height above the plane.

Ball Detection

To detect the balls from the video frame, a binary mask was created by finding the parts of the frame that were in a certain range of HSV values. This isolates only the part of the image that falls within that range, thus isolating the balls in the frame. Since the HSV ranges for each color of ball were different, four binary masks were created offline corresponding to the four colors of balls (blue, orange, pink, and green). Next, the closed contours in the binary mask were found and then filtered by area to filter out other objects that may have been in the HSV range but were too large or small to be balls. The pixel coordinates of the ball were determined by finding the center of the minimum enclosing circle of the contour. Since the balls are relatively close to the floor, their coordinates can be converted to global coordinates using the perspective transformation matrix and cv.perspectiveTransform() with minimal error.

The logic for determining which ball to target is as follows: 1) Find the first ball inside the boundary (set by the markers forming the world coordinate system) and set that as the target 2) If the distance to one of the other balls is less than the distance to the current target minus 1 foot, make that the new target 3) Once all balls have been compared, store the position of the target ball on the TCP server for the robot to access. Targeting was done in this manner to mitigate the robot switching between two balls that were roughly the same distance from the robot.

The code running on the base station is found in BASE_STATION/main.py and BASE_STATION/BaseStationControl.py.