Hi everyone,
The OpenMV Cam was able to compete at the DYI Robocars Meetup on 7/15/2017 using a new algorithm to follow lines using linear regression versus blob tracking. While using a linear regression is slower it allows for much better tracking control since it can find the line anywhere in the field of view. Anyway, here's the video:
Code
Now for how it works. First, we setup the camera to Grayscale QQQVGA (80x60) mode along with vertically flipping and horizontally mirroring the camera since we've mounted the camera upside-down on the robot. We're using grayscale and not RGB565 to go faster along with shrinking the resolution to 80x60 which is the minimum amount we need to track the line correctly. Using a larger resolution with robust linear regression is possible but dangerous. Note that since we're not tracking color we don't need to turn auto-gain or auto-white-balance off.
sensor.reset() sensor.set_pixformat(sensor.GRAYSCALE) sensor.set_framesize(sensor.QQQVGA) sensor.set_vflip(True) sensor.set_hmirror(True) sensor.skip_frames(time = 2000) # let auto-gain/auto-white-balance run before using images
Next, for the main loop we're going to take a snapshot() and then histogram equalize it using histeq() which pumps up the contrast to overcome the low contrast difference between the white line and gray concrete ground. After histeq() has been run all of the bright lines in the image will be pushed up to have values near 255 in Grayscale. We then exploit this fact using binary with a threshold of [240:255] to segment the image into just the bright line parts of the image. Finally, an erode operation cleans up any noise.
while True:
img = sensor.snapshot().histeq()
img.binary([GRAYSCALE_THRESHOLD])
img.erode(1)
Now we're ready to hit the image with robust linear regression algorithm. This algorithm computes the slope between all pairs of set pixels after binary() has run and then finds the median slope. Using this technique it can handle about 25% of the pixels in the image being outliers before returning the wrong result. However, the robust linear regression is an NxN algorithm... so, if too many pixels are set it will run for a looooong time. This is why we use an 80x60 resolution. To avoid taking seconds to process the image if too many pixels are set.
line = img.get_regression([(255, 255)], robust=True)
Note that get_regression() does not need binary() to binary the image. It will do that itself. But, we won't be able to see the binary image output if we do so we move that operation outside and just look for white pixels with get_regression([(255, 255)])...
Anyway, get_regression() returns to use a line object which has the theta(), rho(), and magnitude() values. Before we can use these values we first need to understand that they mean. Magnitude() is straight forwards. It's the strength of the line detection. A simple threshold on it is enough to accept or reject the line. Figuring out what to do with theta() and rho() is a little bit harder. The image below shows what theta() and rho() are:
So, first, why are we using polar math here? Well, we're doing it because regular my/mx math you learned in school doesn't work when mx gets small. With polar math we can handle any slope. Anyway, theta() is defined from the origin. It spins around along with rho() which describes the length of the line from the origin. Both of them are used to project a perpendicular line on the image area. So, the theta() and rho() values get_regression() is returning are the values that were used to find the perpendicular line through the points you want to track in the image. Note that the Cartesian Coordinate Space is upside down since images are indexed with Y increasing downward. The Image Area above is where the actual image is in the Cartesian Coordinate Space.
Now, how do we use theta() and rho() to follow a line? This is a real mind bender. It took me about 6 hours thinking about it to come up with this:
def line_to_theta_and_rho(line):
if line.rho() < 0: # quadrant 3/4
if line.theta() < 90: # quadrant 3 (unused)
return (math.sin(math.radians(line.theta())),
math.cos(math.radians(line.theta() + 180)) * -line.rho())
else: # quadrant 4
return (math.sin(math.radians(line.theta() - 180)),
math.cos(math.radians(line.theta() + 180)) * -line.rho())
else: # quadrant 1/2
if line.theta() < 90: # quadrant 1
if line.theta() < 45:
return (math.sin(math.radians(180 - line.theta())),
math.cos(math.radians(line.theta())) * line.rho())
else:
return (math.sin(math.radians(line.theta() - 180)),
math.cos(math.radians(line.theta())) * line.rho())
else: # quadrant 2
return (math.sin(math.radians(180 - line.theta())),
math.cos(math.radians(line.theta())) * line.rho())
def line_to_theta_and_rho_error(line, img):
t, r = line_to_theta_and_rho(line)
return (t, r - (img.width() // 2))
So, what's going on here? Well, we're basically trying to map different behaviors for the robot to follow given where the line is in the image. First, as a background note... to mimic OpenCV output (which may not have been a good idea now that I think about it), theta() and rho() values are defined like OpenCV outputs from their Hough line detector. That is, theta() goes from 0 to 179 and rho() from negative to positive. However, negative rho() doesn't make any sense. What negative rho() actually means is that you should add 180 to theta() and then make rho() positive. I think it might make a lot of sense to change all OpenMV code to just output a theta() from 0 to 359 and rho() that is only positive instead of doing things the OpenCV way... and change this soon... anyways...
Moving on, there are basically four areas above where we want different robot behavior given where the line is. Also, note that get_regression() never returns negative rho() and theta() less than 90. So, the first case above is actually never executed (if you look at the image above you'll see that there's no line you could draw through the image from quadrant 3). This leaves quadrants 1, 2, and 4.
Now, for example, when the line is being drawn from quadrant 4 in the above image this means that the robot is generally on the line and you thus want it to drive straight and subtlety correct itself while driving to get back on the line.
return (math.sin(math.radians(line.theta() - 180)),
math.cos(math.radians(line.theta() + 180)) * -line.rho())
The code above achieves helps to achieve this by splitting the problem up into two parts. First, we want to map drive theta() towards zero. So, the first part of the tuple returned above maps theta() using sin() to a value of [-1:1]. When theta() is zero sin() is zero. But, as theta() increases sin() increases too. Since each quadrant is only 90 degrees we don't have to worry about sin() starting to decrease again. Other than that... the minus 180 part above was just empirically guessed and put in to invert the operation to get the robot to drive in the right direction.
Anyway, if just put the above value through a PID loop it won't help the robot drive and follow the line. This is because the robot will only try to make the line straight with just this part. But, it won't try to center the line under the robot. The second equation achieves this.
math.cos(math.radians(line.theta() + 180)) * -line.rho())
So, the above equation is just "cos(theta()) * rho()". Which is the X length of the vector created by theta() and rho(). The negative sign above and plus 180 are just to undo the negative rho() stuff mentioned earlier. Later on, the X length is then subtracted from the X center of the image, (80 / 2) = 40, to create another error vector which we'll try to minimize using the PID loop to get the robot on the line.
The rest of the quadrants above follow similar logic for the case where you are on the left of the line, the right of the line, heading perpendicularly towards the line from the left, and heading perpendicularly towards the line from the right. If you want to understand what's going on more I recommend drawing out possible cases on paper using theta() and rho() like in the above image and see how the math works out. Finally, note that quadrant 2/4 are like half strength quadrants compared to quadrant 1. This is why we split quadrant 1 into two parts.
Next, to finally drive the robot we just take the two error function outputs and feed them into a PID loop:
t, r = line_to_theta_and_rho_error(line, img)
new_result = (t * THETA_GAIN) + (r * RHO_GAIN)
delta_result = new_result - old_result
old_result = new_result
new_time = pyb.millis()
delta_time = new_time - old_time
old_time = new_time
p_output = new_result
i_output = max(min(i_output + new_result, I_MAX), I_MIN)
d_output = (delta_result * 1000) / delta_time
pid_output = (P_GAIN * p_output) + (I_GAIN * i_output) + (D_GAIN * d_output)
output = 90 + max(min(int(pid_output), 90), -90)
So, in the above we linearly combine each of the error function outputs which can be weighted differently, do the standard PID magic, and then drive the servo between 0 to 180 degrees. The complete code for my robot is below. Only 92 lines!
BINARY_VIEW = True
GRAYSCALE_THRESHOLD = (240, 255)
MAG_THRESHOLD = 4
THETA_GAIN = 40.0
RHO_GAIN = -1.0
P_GAIN = 0.7
I_GAIN = 0.0
I_MIN = -0.0
I_MAX = 0.0
D_GAIN = 0.1
STEERING_SERVO_INDEX = 0
import sensor, image, time, math, pyb
from machine import I2C, Pin
from servo import Servos
i2c = I2C(sda=Pin('P5'), scl=Pin('P4'), freq=10000)
servos = Servos(i2c, address=0x40, freq=50, min_us=1000, max_us=2200, degrees=180)
servos.position(STEERING_SERVO_INDEX, 90)
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.QQQVGA)
sensor.set_vflip(True)
sensor.set_hmirror(True)
sensor.skip_frames(time = 2000)
clock = time.clock()
def line_to_theta_and_rho(line):
if line.rho() < 0:
if line.theta() < 90:
return (math.sin(math.radians(line.theta())),
math.cos(math.radians(line.theta() + 180)) * -line.rho())
else:
return (math.sin(math.radians(line.theta() - 180)),
math.cos(math.radians(line.theta() + 180)) * -line.rho())
else:
if line.theta() < 90:
if line.theta() < 45:
return (math.sin(math.radians(180 - line.theta())),
math.cos(math.radians(line.theta())) * line.rho())
else:
return (math.sin(math.radians(line.theta() - 180)),
math.cos(math.radians(line.theta())) * line.rho())
else:
return (math.sin(math.radians(180 - line.theta())),
math.cos(math.radians(line.theta())) * line.rho())
def line_to_theta_and_rho_error(line, img):
t, r = line_to_theta_and_rho(line)
return (t, r - (img.width() // 2))
old_result = 0
old_time = pyb.millis()
i_output = 0
output = 90
while True:
clock.tick()
img = sensor.snapshot().histeq()
if BINARY_VIEW:
img.binary([GRAYSCALE_THRESHOLD])
img.erode(1)
line = img.get_regression([(255, 255)], robust=True)
print_string = ""
if line and (line.magnitude() >= MAG_THRESHOLD):
img.draw_line(line.line(), color=127)
t, r = line_to_theta_and_rho_error(line, img)
new_result = (t * THETA_GAIN) + (r * RHO_GAIN)
delta_result = new_result - old_result
old_result = new_result
new_time = pyb.millis()
delta_time = new_time - old_time
old_time = new_time
p_output = new_result
i_output = max(min(i_output + new_result, I_MAX), I_MIN)
d_output = (delta_result * 1000) / delta_time
pid_output = (P_GAIN * p_output) + (I_GAIN * i_output) + (D_GAIN * d_output)
output = 90 + max(min(int(pid_output), 90), -90)
print_string = "Line Ok - turn %d - line t: %d, r: %d" % (output, line.theta(), line.rho())
else:
print_string = "Line Lost - turn %d" % output
servos.position(STEERING_SERVO_INDEX, output)
print("FPS %f, %s" % (clock.fps(), print_string))
Now, what's important about all the above is the line_to_theta_and_rho() function. While the code could use some optimizing to make it simpler... it correctly maps how to drive the robot to follow a line for you! This means we can use it with find_lines() which is much faster than get_regression() and can work on 160x120 images or above to follow multiple lines and linear combine multiple lines detected to drive the robot!
Anyway, if you made it this far thanks for reading! Me and Chris Anderson will continue to work on the OpenMV Donkey Robocar platform. Once we've got all the kinks out and have the system working perfectly we'll post instructions on how to make one yourself.
