The laser sensor program is run on a laptop, not in the microcontroller. The API is Matlab. In Matlab, the Image Processing Toolbox and the Image Acquisition Toolbox are the main tools for this algorithm.
Using the Image Acquisition Toolbox, MatLab is acquiring image frames from the on-board webcam at 24 fps. Each frame is in a format of a 700×350 matrix. Each index stores the color intensity of that pixel.
Since Matlab is a scripting language, it is very slow at compiling iterations such as while loops and for loops. To increase processing speed, C functions are called within Matlab to increase the processing time. In Matlab, C functions are known as MEX functions which have their own syntax a little different from C.
From the raw image frame matrix, image processing operations are performed:
1. The raw matrix is acquired in a color space of RGB. Since the laser is green, we extract the green space from the raw matrix.
2. Since the matrix stores the green color intensity of the image at each pixel, we minus 200 from all the index value. The result gives a matrix that has index value of 0 when laser the pixel is not laser. Otherwise the index value is none zero.
3. To simplify the matrix, the bwareaopen() function from the Image Processing Toolbox is used to convert the image matrix into a binary matrix. That is, all zero value will remain as 0 and all non-zero values will be converted to 1.
4. orglaser.c MEX function
- Input: raw image matrix 700×350 at Step 3
- Output: a 1×700 array that stores the first
This function compresses the raw matrix into a 1×700 array. For each row position, the first column index position that stores a non-zero value is returned.
5. realDistance.c MEX function
- Input: 1×700 array return from the orglaser MEX function
- Output: a 1×25 array that stores the approximate real distance at each segment of 28 pixels
To calculate real distance, linearization is used because the lens of the webcam is a fish eye lens which returns a distorted image. We plot the real distance VS the vertical index position at each column. A exponential relationship is found. The formula is generated using MS Excel.
6. obstacle.c MEX function
- Input: 1×25 array return from the realDistance MEX function
- Outputs: 5 parallel arrays and 1 flag
The purpose of this function is to analyze the distance array returned by realDistance.c. “Gaps” (discontinuous laser lines) in the image are what the function trys to look for.
In the 5 parallel arrays, each index position are read in parallel.
For instance, in each parallel index position, these arrays store:
- GapTypeArray: 1 or 0 (1 means the discontinuous lines are from close to far in the order of left to right, and the opposite for 0)
- GapLeftDistance: the distance on the left of gap
- GapRightDistance: the distance on the right of gap
- GapLeftPosition: the horizontal position (0 to 24) of the left of the gap
- GapLeftPosition: the horizontal position (0 to 24) of the right of the gap
- Finally the flag indicates if the image displays a continuous or discontinuous laser line.
7a. If the flag from obstacle.c indicates that the laser line is continuous, then we use the curve.c MEX function:
- Input:1×25 array return from the realDistance.c MEX function
- Outputs: 6 variables
By analyzing the array of real distance, more useful variables are derived for the robot’s navigation.
- Difference of the left and right end
- Direction: if distance of left end > distance of right end, then direction is left, otherwise direction is right
- Distance of lower end
- Midpoint distance: that is the distance at index 13 as the array has 25 indexes
- Number of contiguous indexes on the left that store an out of range distance
- Number of contiguous indexes on the right that store an out of range distance
Using these data, we can process the image further in the next MEX function drive.c
8a. drive.c MEX function
- Input: 6 variables return from analyzing the continuous laser curve in the curve.c MEX function
- Output: steering signal from 0 to 7. 4 is the center angle. 0 is the largest left angle and 7 is the largest right angle.
Using the 6 variables, 3 major cases of the laser is considered in order to deliver an appropriate steering output.
Case 1. the continuous laser line is from the left end to the right end:
This usually mean inputs 5 and 6 from the curve.c function are 0. Then depending on the distance of the lower end and the midpoint, the turning angle is stored for the output. The closer the laser line is, the sharper turn the output will be.
Case 2. the continuous laser line is from left or right end to the top or bottom. This usually means that on the other end is wide open so the laser line is out of range.
In this case, one of the input 5 and 6 from the curve.c function will be 0 and the other will be non-zero. Again, depending on the distance of the lower end and the midpoint, the turning angle is stored for the output. The closer the laser line is, the sharper turn the output will be.
Case 3. the continuous laser line is from top to bottom. This usually mean that the robot is closely beside a wall.
In this case, both input 5 and 6 from the curve.c function will be non-zero. Depending the number of non-zeros on the left and right, the turning angle is determined and stored as the output.
The output are then sent to the XBee module via USB COM port. The XBee module that is connected to the laptop will then send the output data wirelessly to the XBee receiver on the robot.
7b. If the flag from obstacle.c indicates that the laser line is discontinuous, then we use the farthest.c MEX function to analyze the turning angle of the robot:
- Input: 5 parallel arrays return from the obstacle.c MEX function
- Outputs: 2 variables
Using the 5 parallel arrays as outputs of the obstacle.c, more simplified data is generated to make the navigation decision simpler.
- midpoint distance of the furthest segment: the goal of this function is to determine where the furthest empty route is ahead
- angle of the midpoint: the horizontal position of the midpoint variable
Using these data, we can process the image further in the next MEX function race.c
8b.
Using the 2 variables from the farthest.c function, the steering output can be determined.
The output steering will be sharper if the input 2 from farthest.c is on the very left or right or if the input 1 is really close. Otherwise the output steering will be in between the center and the sharpest turn.
If the input 1 from farthest.c is smaller than 3 inches, the output is 8 which tells the robot to perform a reverse operation.
If the input 1 from farthest.c is larger than 68 inches, then the output is 4 which tells the robot to go straight.
The output are then sent to the XBee module via USB COM port. The XBee module that is connected to the laptop will then send the output data wirelessly to the XBee receiver on the robot.