Chapter 8.2 Jo-Car2 Autonomous Mode Path Planning (Cost Matrix Algorithm) Introduction: In order to achieve its mission and reach the GPS goal safely; without crashing into obstacles or leaving the lane, Jo-Car2 uses three main sources of information; GPS with magnetometer, Camera and Laser Range Finder. Using the combination between the GPS and magnetometer readings, we can determine the desired direction of the robot. The Camera is used to detect the path lines and also the obstacles, by applying the image processing algorithm described in chapter 9. While the Laser Range Finder is used also to detect obstacles. Camera Image Processing LRF Mapping Cost Matrix Path Selection Motors Driving GPS Direction Detection Magnetometer Path Planning Algorithm Jo-Car2 Autonomous Mode - Cost Matrix Algorithm - Mahmood Shubbak 54
The three sensors are then fused into one source of information; The Cost Matrix, that Jo-Car2 depends basically on for its artificial intelligence and path planning. Look at the following diagram. The Cost Matrix: The Cost Matrix is a 7x7 matrix, whose values represent the risk value for each position on the camera image plane; i.e. it has large values where obstacles or path lines exist, and low values in safe positions. Each sequence of matrix elements represents a path; which has its own cost (the sum of elements values), the path of lowest cost is the best path. For thorough understanding of this method, we provide in the following lines a step by step explanation. Step by Step Explanation: 1. Starting from the original image captured by the camera, the first step is to generate a binary image that only includes the obstacles and path lines; this can be achieved by detecting and removing the green grass from the image. Using an image processing algorithm depends basically on the ratio between red, green and blue channels for each pixel; we can get the desired binary image. (For more details about the image processing algorithm refer to Chapter 9). Look at the following example: 2. The second step is to detect and remove noise; this is also achieved by some image processing depends on the size of the white regions; the small ones are considered as noise and removed consequently. (more details can be found in the image processing chapter 9) Look at the following example: Jo-Car2 Autonomous Mode - Cost Matrix Algorithm - Mahmood Shubbak 55
Up to this point, we obtain the final binary image that contains the path lines and the obstacles only; without any noise. 3. Now we can generate the cost matrix; which is 7x7 matrix contains the average of pixels intensities as described previously, look at the figure below: Notice that on the obstacles positions (totally white) the matrix value is 255 which is the highest intensity; while on safe positions (totally black) the value is zero which is the lowest intensity. This matrix is the primary framework, to which all other sensors will be fused, and on which the robot will depend in planning its path. 4. The next step is to fuse other sensors into the cost matrix; sensor fusion here means adding the laser range finder LRF, the GPS and the magnetometer information into the path matrix. Firstly, fusing the LRF; by mapping the laser data (which is points on the horizontal ground plane) to the camera image plane. This has been done using some experimental methods for calibration. More details can be found in the LRF Mapping chapter 10. Jo-Car2 Autonomous Mode - Cost Matrix Algorithm - Mahmood Shubbak 56
After determining the positions of LRF points on the image plane, and their positions on the matrix consequently, an extra cost of 100 will be added to each position. Look at the following figure. The GPS direction can be then fused into the matrix by detecting wrong way motion; comparing the difference between current position and next GPS with the history data. 5. Now we reach paths evaluation step; on which pre-defined paths will be evaluated in order to determine the best path. Here we will show a sample of five paths; look at the following figure. By adding the cost values of each cell in the forward path we got a sum of 468; which represents the total cost of following the straight forward path. Applying the same procedure, we got results for another four paths as shown in the following figures. Jo-Car2 Autonomous Mode - Cost Matrix Algorithm - Mahmood Shubbak 57
Notice that the straight-right path costs 498, while the straight-left path costs 159. The right-straight path costs 713, and the left-straight path costs 45. 6. The last step in our path planning algorithm is choosing the best path; which is the lowest cost path. In our sample image here, it is obvious that the left straight path is the best path with the lowest cost of 45. Jo-Car2 Autonomous Mode - Cost Matrix Algorithm - Mahmood Shubbak 58
Summary: After choosing the best path, the controller must convert it into its relevant desired speeds for the both motors according to the differential drive model, explained in the actuators Chapter 6. The PID controller will guarantee that the motor will operate at the desired speed. Jo-Car2 uses a path planning algorithm called cost matrix algorithm; it depends on fusing all sensors into one 7x7 matrix, then evaluating alternative paths on it by their costs. The matrix is generated through six steps; 1 converting the original image from camera to binary image by image processing algorithm, 2 detecting and removing the noise, then 3 generating the matrix by averaging pixels, 4 fusing all sensors, after that 5 evaluating pre-defined paths, and finally 6 selecting the best path with the lowest cost. Jo-Car2 Autonomous Mode - Cost Matrix Algorithm - Mahmood Shubbak 59