The laser, along with sonar and infrared sensors, give a pretty good map of where objects are in the environment, and the laser can even describe the shapes. A robot needs a camera, though, to detect color or fine features such as faces. In this project, we learn how to connect to a vision module and use features it detects to direct the robot's behavior. In particular, we have the robot follow pink blobs.
Interfacing with the Camera
As with the laser, the Magellan robots do not have a built-in camera, so we must interface with cameras through another program. The laser uses player, which we run in a separate terminal, and our professor wrote a C++ object through which we can query the laser readings. To use the camera (a Cannon VC-C4), we run SVM (standard vision module) as a separate program. To communicate with SVM, we send and receive messages through central, a third program.
We can send messages instructing SVM which attributes to calculate and then receive messages describing what it detected.
We wrote a program to have the robot attempt to maintain a fixed distance from a pink object. The robot turns left or right to keep the pink object as centered in its sights as possible. If it does not sense a pink object, the robot goes into a wander.
Centering on Pink
The SVM module has a pink detector, so we simply subscribe to receive messages (checked at the end of every cycle). Each message provides, among other things, the centroid of the primary pink blob, in image coordinates. We save this as our
pinkPoint in the state structure and update so that
timeSincePink is zero.
The x-coordinate provides sufficient information to calculate the angle between the center of the pink and the robot's current orientation. We experimentally found our camera's range to be about .88rad and the image width seems to be 680 pixels. Therefore, for every pixel away from the center, the blob is .88/680=.0013 radians off from straight ahead. This means that, for this camera, every pixel corresponds to approximately 1 milliradian. We want it to rotate at 1rad/s for each radian it needs to cover, so the general equation for computing angular velocity is
We have the camera and image attributes hard-coded, but they are isolated so that it is easy to change.
WANDER state, we use our algorithm from project 2
In project 2, we used the reactive control law from our book, section 3.6.2. Since we implemented velocity space navigation in project 3 (and improved it in project 4), we decided to use that algorithm for the "move toward" step. This will cause it to smoothly avoid obstacles.
A problem with the algorithm given above is that the random location may not be reachable. We handled this in project 2 by making the inner loop's condition be
This time, we want the robot to be able to move around obstacles, even if it briefly gets so close that
canMove returns false, which causes the robot to decelerate as quickly as possible. We added a field to the robot structure to record when, on the previous time step, the forward velocity was forced to be 0. We store this as a float so that if the angular velocity was nonzero, we add only .1. This will let the robot turn for one second before giving up on the destination.
As mentioned in the previous projects, the velocity space navigation algorithm has several parameters that we never took the time to tune. We learned in class about linear optimization, which is a way to select parameters based on repeated trials, which are rated according to some fitness function. The parameters to tune are
- alpha - importance of being headed in the correct direction, projecting heading_dt into the future
- beta - importance of staying far away from obstacles, projecting obstacle_dt into the future
- gamma - importance of moving quickly
- heading_dt - length of time (in s) to project along the path defined by the proposed velocities to evaluate the heading term
- obstacle_dt - length of time (in s) to project along the path defined by the proposed velocities to evaluate the obstacle term
Since they are so related, we decided to use the variation we learned in class that was used by Aibo researchers. This variation perturbs all parameters together and adjusts the values based on the results of several perturbations.
We wrote a program
optimizeNav that performs the perturbation and recalculation. To get the fitness value, it runs the navigation algorithm with the new parameters and records the (approximate) absolute amount turned + penalty for trying to run into obstacles. Between trials, the program waits for keyboard input, at which point we turn on the brake, roll the robot back to its starting position, remove the break, and finally hit
return. This allows us to perform the same experiment (move forward two meters with an obstacle in the way) for each parameter set.
Unfortunately, the program would have a player error every so often, so we chose a small number (7) of parameter sets to evaluate between each recalculation. Since we perturbed the values randomly between half and double the original value, this does not provide enough time to thoroughly investigate the area. Another shortcoming is that our fitness function does not do a great job of detecting small differences in the "goodness" of various parameter sets.
Nevertheless, after 5 recalculations we observed a couple of good parameter sets. We manually recorded these values and let the algorithm keep running. The next few sets were noisy, though, and the next recalculation did not lead to more good parameter sets. We took the liberty to restart the program, setting the seed to one of the successful sets.
It still produced many terrible sets, which is likely due to our large variation. We could use a simulated annealing technique by tweaking the given parameter set less now that we think we are close to the answer, but instead we decided that we are satisfied with this set.
We saw the biggest change in the value of
heading_dt. We had thought that a small amount, such as .1s would work best, but it turns out that looking a full second ahead prevents over-turning.
In this video, the robot initially detects the pink ribbon, so it follows it until losing sight for 10 cycles. At that point, it goes into the wander state. We try to regain its attention to the pink, which works only some of the time. Part of the problem is that the dim lighting makes it difficult to identify the bright pink. When using larger pink items (such as a laptop or coat), it has a hard time determining the center of the object, which can result in constant turning back and forth as the "center" shifts.