For this project the first milestone was to move the Sphero to desired points on the floor. There are two aspects associated with this: first we need to know the location of the Sphero ball and second we need to determine the speed and direction commands in order to send the ball to a desired point.
Sphero SDK Commands
The SDK for the Sphero accepts two parameters when commanding it to move. It can be commanded to move with a speed between 0 and 1, with 0 being stopped and 1 being the fastest speed. It can also be commanded to go in a particular direction based on its frame of reference with values between 0 and 360 degrees.
Tracking the Location
In order to track the Sphero we tested a couple of methods. The first method was using Vicon cameras. A group of these cameras can track special markers in 3D space. I created covers that could be placed on the Spheros that would hold the markers, it was not possible to stick the markers directly onto the balls. The below video demonstrates tracking with the Vicon system. In the video when I pinch my fingers the Sphero comes under my hand.
The cover designed to hold Vicon markers hid the shape and colors of the Spheros which was not ideal.Therefore we changed the tracking method to a ceiling mounted camera with a fisheye lens so that it could track a large surface area on the floor. OpenCV was used to unfish the lens and with background subtraction we could track Sphero coordinates based on their spherical shape. Below is the video stream from the camera as the ball follows a path drawn by a laser pointer.
When implementing the control system the first thing that was done was to define global coordinates so that we could give commands to the Sphero based on its local coordinate system. As the Sphero is a sphere, we don’t know where it’s front or back is pointing towards. When the Sphero is rolled into the point of view of the Camera we need to calibrate to determine it’s 0 degrees. We do this by making it move with reference to it’s own 0 degrees. We store the difference between the Sphero’s 0 degrees and the global 0 degrees. This value allows as to give commands that a Sphero can follow. For example, the user points to location that they want to move the Sphero to. This point is relative to the global coordinate system, motion commands for the Sphero would be converted relative to its point of reference so that it can move towards the location. As well, as the Sphero moves around for long periods of time Sphero’s 0 degrees drifts and you may notice that it is not moving “correctly”. This is the point where you would need to recalibrate.
Handling Noisy Data
We also encountered noise in the Sphero coordinates that we received through the overhead camera. In order to accurately predict the ball’s current location we placed a filter on the data. We averaged the previous 5 to 10 coordinates to predict the current location. The frame rate of the camera was about 50 frames per second.
Implementing a P Controller
Finally some kind of proportional-integral-derivative (PID) control was needed to be in place so that the Sphero could reach its destination without overshooting and re-adjust its path as it moves towards the target if it is slightly diverted. A proportional controller was enough to accurately move the Sphero ball. Below are the basic equations I used to calculate the heading and velocity commands with a P controller.
headingCMD = desiredHeading + (headingKp*currErrorHeading) velocityCMD = velocityKp*distance
The Kp constants for both heading and velocity were determined based on trial and error. For further insight on PID controllers see this resource. The below video shows Sphero motion without noise filtering and P controller. The Sphero keeps on overshooting the desired location.
On the other hand, the below video shows Sphero motion with noise filtering and P controller. The Sphero is successfully able to reach all of the desired points.