AUVSI11 Selfridge

Autonomous Vehicle Path Planning and Tracking: A Vision Based Approach

Justin M. Selfridge, Loc D. Tran, Gene Hou
Data acquisition, navigational path planning, and path tracking are important features for any autonomous system, because the fundamental requirement for these vehicles is to successfully navigate through their environments. This research addresses the problem of integrating autonomous path planning with path tracking. For an autonomous vehicle to effectively navigate and interact with the world, three key components must be addressed. The vehicle must (1) create an accurate assessment of its surroundings, (2) develop a feasible navigational path based on the current surroundings and the vehicles destination, and (3) monitor the progress along the current path updating the navigational plan as the vehicle moves through the environment. A deficiency in any of these requirements leads to results that are less desirable, and often dangerous. Much research has been conducted on these individual tasks; such as dynamic vision processing, multisensor data fusion, Kalman filter applications, and MIMO feedback control systems. This research documents the navigational path planning and tracking capabilities of ODUs Autonomous Surface Vehicle (ASV). The initial goal for the team was to implement a vision processing and navigation planning algorithm that successfully guides the boat through a navigation channel at AUVSIs RoboBoat competition. This research describes the design process and the computer algorithms that were implemented in the 2011 competition. With a solid foundation in vision processing and navigational planning, the team is well poised for future work, such as adding sensors like GPS, a compass, and an IMU, and adding more robust state feedback control algorithms to aid the navigation path planning and tracking for ODUs Autonomous Surface Vehicle.

Master of Science in Mechanical Engineering, Old Dominion University Master of Science in Electrical and Computer Engineering, Old Dominion University Professor in Mechanical Engineering, Old Dominion University

OVERVIEW OF PATH PLANNING AND TRACKING

For autonomous vehicles to successfully interact with their environment, they must be able to gather an accurate assessment of their surroundings. Various sensors give an autonomous vehicle the distance to objects, how fast the vehicle is moving, its position and orientation in space, and various forms of object detection. Many of these sensors lay the foundation for obstacle avoidance. However, for an autonomous vehicle to truly interact with its environment, it must be able to discriminate between objects of interest and those that present obstacles. Drawing this distinction requires information about an objects shape, color and/or movement which is only possible through a robust vision system. The application for this research is directly tied to the Autonomous Surface Vehicle (ASV) project at Old Dominion University (ODU). The main task for the student project is to design and build an autonomous boat to compete in AUVSIs RoboBoat competition. Each year, competitors from around the world meet in Virginia Beach, Virginia to demonstrate their vehicles capabilities through the aquatic obstacle course. The tasks for the competition include successfully navigating through a set of speed gates and a navigation channel, avoiding obstacles, and performing other autonomous activities such as object detection and water canon firing.

Figure 1 - ODU's Autonomous Surface Vehicle Design.

ODUs boat consists of a modular framework design that supports two pontoons, and a mounted electrical box that houses all of the computer hardware to run the boat. Two fixed trolling motors are located in the rear of the boat where turning is accomplished through varying the thrust of each motor. Two small web cameras are mounted near the front of the boat. They are positioned side-by-side to capture the widest possible field of view with only a small overlap in the images down the vertical center line. Each video captures a 640x480 color image, which is processed through a multi-channel video stream library called videoImage. Once the images are captured, they are processed through an onboard laptop computer that runs the main logic for the entire boat. Microsoft Visual Studio C++ 2010 was used to build the code, and an open source computer vision library, called OpenCV, was used for the many vision processing tasks. The underlying design philosophy with this student project was to start with the basics, master the fundamentals, and then build in more complex tasks and processes over time. The most versatile sensor available to an autonomous vehicle is the camera and its vision system, so the team concentrated its efforts to create a robust vision system to handle the boats navigation. With the vision system in place, future ASV developments will include GPS, a compass, and IMU sensors. The ASV computer algorithm runs through a continuous loop that is used to change user defined parameters and the underlying logic code for the vehicle. The Graphical User Interface

(GUI) is the main console the user has to interact with and to monitor ASVs performance. At the start of each loop the GUI is refreshed to provide the user the most current information at the ground control station. The GUI is divided into four main windows: Vision Window, Motor Window, Direction Window, and Command Window.

Figure 2 - Display of the ASV's Ground Control Station GUI.

Vision Window: This window encompasses the majority of the screen which displays the ASVs vision as it is captured from the two cameras. This window also projects which objects are currently detected, the location of the target direction, and the centerline for the boat. Motor Window: The lower left block displays the current motor thrust for each of the two motors. The magnitude of each motor is displayed as a bar graph, and the numerical values used for processing are printed beside the graphical display. Direction Window: The lower right block graphically shows the topographical view of the target point the boat is tracking. The motor commands are based on the horizontal error and distance to the target point. Command Window: The lower center console provides the most information about the logic the boat is currently running. The left side includes the mode (Autonomous or Remote Control), the logic (exiting the speed gate, entering the navigation channel, etc), the frames per second (FPS) of the vision system, and the type of targeting the boat using (multiple buoys, no buoys, etc). The right side of the window provides information about the closest targets and the target point between them. The rows include the horizontal pixel position, the vertical pixel position, and the distance (in centimeters) between the boat and the buoy.

The GUI has several functions to interface with the user. The boat has an Idle function to halt the looping algorithm while the boat is not active, a command to toggle between autonomous and remote control modes, a button to start and stop video recording, an option to adjust camera

settings and color filter calibration. The algorithms used to generate this display will be reviewed through the remained of this paper.

VISION CODE
The goal of the vision processing code is to focus on the colors of interest, test whether they are likely an object of interest or are random noise, and then extract information about the objects of interest. To increase the processing speed, the image is scaled down by an adjustable scaling factor. The image is then converted from RGB (Red/Green/Blue) color space into an HSV (Hue/Saturation/Value) color space. When processing colors, the RGB space is very difficult because a color of interest is a mix of all three color channels. Working with HSV simplifies things because all the colors are described on the Hue channel. The remaining two channels (Saturation and Value) are used to describe the brightness and intensity of a color. Color Filtering Each color of interest has six variables used to define the minimum and maximum boundaries for the hue, saturation and value channels. The algorithm loops through the pixels in the image and tests whether the pixel is within the desired range. One difficulty with image processing is that the pixel values of an object can vary significantly under different lighting conditions. The time of day and the amount of cloud cover can change the pixel values of an object drastically over a short period of time. To ensure the best color detection, a calibration feature was built into the program to allow to user to adjust the filter setting while navigating the vessel. The calibration window displays the six variables that define the filter boundaries. As the user moves the sliders, the program changes the variable settings so the user can see which pixels are passed through the filter.

Figure 3 - Video Capture with Poor Filter Settings.

Figure 4 Video Capture with Adjusted Thresholds.

Once the filters are properly calibrated, the colors of interest are stored in separate grayscale images. The images now contain pixels that are either black or white; however, the images usually contain a great deal of noise. Ideally, an object of a solid color should pass through the filter and then be recorded as a solid white region; however, black specs appear on the image because those individual pixels did not meet the filter threshold criteria. To clean up the grayscale images, they are each passed through a Dilate function which expands the light regions within an image. The result is an image that is almost completely black and white, but the specs of noise are now represented as shades of gray blended into the highlighted regions. Shape Detection The grayscale images are then passed through a contour finding algorithm. The algorithm tests each pixel against a threshold value and determines if the pixels value is above or below the prescribed threshold. The algorithm forms boundaries, or contours, where there is a change between the threshold regions. Each contour is stored in a dynamic array that includes basic info about each contour, like the length of the contours edge, the regions area in pixels, and the height and width of the contours bounding box. Not all contours will be objects of interest. To further refine the image, the shape of the object is also taken into consideration. Originally, the computer code used a Hough Transform to detect circles within the image. The main obstacle with this process was that most regions within the grayscale images did not pass as circles. Because the filters exclude some regions from an object of interest, the shape of the object is not always completely intact. For instance, it is common for the filter to pass through circular images that resemble crescent moons because the filter eliminates areas of high and low intensity. To circumvent this issue, a crude approximation of the objects shape is determined by the ratio of the contours height to its width. The speed gate buoys are known to be cylindrical with a width of 18, and a height of 48. Even if the contour does not produce a completely rectangular shape, the algorithm can still look for contours that have a height to width ratio around 2.666 (48/18 = 2.666), so a conditional test keeps contours with ratios between 2.5 and 2.8. Similarly, when the boat is in the navigation channel, the buoys

are a spherical shape, so the conditional statement is set to keep contours that have ratios between 0.85 and 1.15. Once the code has determined that a contour meets the proper ratio requirement, the code does one final evaluation to determine if it is a buoy. A major hurdle with vision processing is the amount of uncertainty within the image. Early tests showed that small surface reflections and large background objects were detected as buoys. Errors of this nature are inherent in most vision processing algorithms. Fortunately, one advantage of tracking buoys is that their size is known in advance, and the buoys all sit on a flat horizontal surface the water. These two facts can be used together as a final test to eliminate unwanted errors.

Figure 5 - Size and Position are Related.

Because the cameras are mounted at fixed height and angle, the horizon is relatively constant in the image. Therefore, if an object is a buoy, the size of the buoy should correspond to a certain distance beneath the horizon line. Figure 5 shows the perspective view of how navigation buoys should appear in the camera image. The size of the buoy and the vertical distance from the horizon line are very closely linked. Comparing the objects size and position relative to the horizon quickly eliminates small surface reflections and large background objects. Of course, the boat pitches slightly (horizon line wobbles), and the bounding boxes do not always fit the buoys exactly (larger or smaller than the actual image), so there is a threshold built into the algorithm to allow for variance within the evaluation. Distance Estimation The vision code has systematically worked through an algorithm that reliably detects objects that are likely navigation channel buoys. The vision system can now store the information about the buoys which will be used by the navigation code. Information about the buoys color and position are obtained directly from the contour. The final piece of information needed for the navigation system is an estimate of how far away the buoy is from the ASV. Once again, because the size of the buoy is know in advance, a formula is used to calculate an estimate of the distance to the buoy. For every image, the angle of the field of view and the width

of the image in pixels are both constant values, so each column of pixels represents a discrete angle. Therefore, an objects screen width can be used to determine the angle of the projection back to the camera. Once the pixel width of an object is converted into an angle, standard trigonometry can be used to find the distance. Because all the angles are known, and the width of the buoy is known, the only unknown variable is the physical distance to the buoy. This process for estimating a buoys distance works very well under most operating conditions. For well defined buoys within 10 meters of the boat, the distance estimate was typically accurate to within 10 cm of the actual distance. Two main factors are known to degrade the accuracy of the estimate. The first factor is the objects distance. The distance estimate becomes less accurate as the buoys are moved farther away. This is because smaller objects represent smaller angles which provide less resolution in the calculation. The other factor is based on how well defined the buoys are. When the color calibration detects smaller or larger bounding boxes than the actual image, the error propagates through the calculation. Despite these limitations, the algorithm provides an acceptable distance estimate which the navigation system uses to plot the ASVs course.

NAVIGATION CODE
The purpose of the navigation system is to take the information obtained through the vision system, and make decisions about where the boat should navigate to and how it should get there. The vision system provides plenty of information about what objects are seen, where they are located and how far away they are. Now the navigation system needs to move the boat based on that information. Target Point Despite having a large amount of information to evaluate, it is important to remember that the navigation plan reduces down to a single decision regarding the best speed and an appropriate amount of turn. All of the information provided by the vision system is used to find these two simple parameters. Starting with the end in mind, the navigation system picks a target point and then applies a formula to decide what motor thrusts are needed to get to that target point. The Direction Window provides a glimpse into what target point the boat is tracking, and the diagram shows how the speed and turn will depend entirely on where this target point is located. The distance from the target point dictates the speed the boat is willing to travel. Points that are farther away warrant a faster speed; but as the target point moves closer, the boat adjusts to a slower speed. The horizontal offset determines how aggressive the boat turns. More significant error warrants a greater correction, whereas target points close to center will not need significant turning adjustments. It is also important to address target points where the boat should not move forward, but simply rotate in place. In other cases, the boat will be too close and too far offcenter that the only choice is to back up and start the approach again.

Figure 6 - Various Target Points Displayed in the Direction Window

All of these decisions are encompassed in a straight-forward algorithm that calculates the motor thrusts based on the geometric layout of the target point. The target point is visually displayed in the Direction Window (Figure 6) which represents a topographical vantage point with the ASV located at the bottom center of the display. A white circle represents the point in space that the boat is targeting, where the vertical axis represents the distance between the boat and the target, and the horizontal axis represents the horizontal error between the target and the boats center line. The angled lines on the diagram represent points in space where the boat will halt forward motion and enter into pure rotation. The two regions below the lines are targets that are too close and too far off-center to continue moving forward. In these cases, the boat will reverse its direction and turn towards the target until it has enough distance to begin a new approach. The area above the lines describes the region where the boat has enough clearance to navigate forward. The speed and magnitude of the turn are dictated by how far off-center and how far away the target point is located. Figure 6 shows several target points displayed in the Direction Window. The left display shows the target point is nearly centered, so the boat will primarily navigate forward. The next display shows the target point is very close to the angled line, so the error and distance dictate the boat will not move forward, but rotate in place. The final display shows the target point is extremely off-centered, so the boat will reverse direction and turn toward the target to obtain a better view. Picking the Target Point Now that the boat can effectively target a point in space, it is time to process the video image to locate the point to target. When no targets are located, the boat is instructed to scan in a circle until a new object is detected. When a single buoy is detected the vessel evaluates whether to track straight at the buoy or to pick a point on either side. In some cases it is desirable to track straight toward a buoy. A blue buoy marks the end of the navigation channel, so the boat should move toward it. Yellow buoys are obstacles located within the navigation channel, so when no other buoys are seen, tracking straight toward the yellow buoy will ensure that the boat remains within the navigation channel. Other times it makes sense to pick one side of a buoy and then navigate around it. When the yellow buoy comes too close, the boat picks a target point to the side of the buoy to avoid the obstacle. The red and green buoys denote the boundaries of the navigation channel, so the boat determines which navigation plan it is following (into or out of the harbor), and then tracks the side of the buoy that will keep the boat inside the navigation channel.

Figure 7 - A Buoy is Detected and A Target Point is Selected.

Working with pixels slightly complicates the process because absolute sizes are not known. As objects move farther away from the camera, their size (in pixels) decreases. Therefore, many of the navigation decisions are based on multiples and ratios of the objects detected. For instance, the navigation channel buoys are known to be 9 in diameter. If it is desired to track a point 27 off-center from the buoy, the target location should be offset by 3x the buoy diameter. This way, no matter what size the buoy appears on the screen, the target point will always maintain a consistent offset distance from the buoy. The real test for the navigation code comes when detecting multiple buoys at a time. Earlier versions of the code instructed the boat to track a middle point between two buoys. However, the width of the navigation channel is very narrow compared to the width of the boat. The boat is able to pass through a pair of buoys when the vessel is properly centered with the buoys; however, when the boat is off-center the buoys are too close together to pass straight through them. To address this issue, the code needed to account for the orientation of the boat with respect to the navigation channel buoys so that the ASV could center itself as it passed through the pair. When the buoys are adequately spaced apart from each other, the ASV targets this center point. When the buoys are too close together the ASV recognizes that it is off-center and then adjusts its heading so that it may swing into the pair of buoys. The speed and amount of turn are based entirely on where the target point is located. Evaluating the position and distance to each buoy helps the boat decide where the ideal target point should be located.

Figure 8 - The ASV Needs to Swing Into A Pair of Angled Buoys

A similar approach is used as the case for navigating around a single buoy. The number of pixels of an object is dependent on how close the object is to the camera. Because the size of the buoys is known in advance, the computer algorithm can use relative sizes and ratios to adjust the target point. A numerical example is the best way to illustrate the point. The buoys have a 9 diameter and the navigation channel has a minimum width of 60. Therefore, the ratio of channel width to buoy diameter is 60/9 = 6.667. Any time the width of the channel is less than about 6.5 times the buoy width, the boat determines that it is not centered with the buoy pair. Additionally,

this ratio will vary anywhere between ~6.5 (perfectly centered) to 0 (off-center by 90 degrees). The ratio is then used to move the target point proportionally away from the buoys to allow the boat to swing into the pair.

Figure 9 - Narrow Channels Require An Off-Center Target Point.

This target point is not set in stone. The perceived width of the channel widens as the boat navigates around the pair of buoys. As the ASV continues this path, the width between buoys begins to increase, and the ASV adjusts its target point so that the target point gradually becomes more centered with the buoy pair. This dynamic process continues until the boat is centered with the buoys, or until the target distance becomes close enough that the boat starts to reverse course and attempt the passage again.

Figure 10 - ASV Targets the Middle When the Navigation Channel is Wide Enough.

Figure 11 - The Target Point is Offset to Adjust For an Off-Center Approach.

10

The other factor that determines the speed of the boat is the distance to the target location. If the boat is centered enough to pass through the buoys, then it makes sense to target a location that is slightly beyond the center point of the buoy pair. As the buoys become more offset, the target distance should decrease so the boat proceeds more slowly. The same ratio that was used to determine the horizontal offset was used to calculate the distance offset. When the boat is properly centered it targets a distance that is slightly past the distance of the buoys, and when the buoys are highly off-center, the boat targets a point that is much closer than the distance of the buoys. This allows the boat to proceed more cautiously until the right angle of approach is achieved.

CONCLUSION
Autonomous navigation is no trivial task. To create a robust system requires plenty of advanced planning, and thorough testing during the development process. Most environment detecting algorithms cannot be designed entirely from advanced planning. Improvements often require knowledge of how the current system behaves in the environment before advancing to the next level. ODUs current ASV might not contain many of the systems and sensors that are readily found on other autonomous vehicles, but that is not necessarily a disadvantage. The underlying design philosophy was to start with the basics, master the fundamentals, and design the best possible algorithms to achieve the current goal. More advanced features are only incorporated once the basics are mastered. This proved to be an efficient design strategy, because there were many occurrences where the code did not perform as well as expected when the system was tested in the physical environment. This type of iterative testing lead the team to discover that pixel width could be used to reliably detect buoy distance. Testing also showed that the navigation channel was too narrow to allow the boat to pass through at wide angles, which was the impetus for the design revision that directs the boat to center itself with a buoy pair. Furthermore, by not adding every type of sensor all at once, the team had to be more resourceful with the assets available. This creativity pushed the envelope and maximized the vision systems capabilities. In conclusion, this project has demonstrated that developing an autonomous vehicles navigation and control system is an iterative and dynamic process; one that requires an open mind, and a willingness to attempt new ideas.

References
Bradski, Gary; Kaehler, Adrian. Learning OpenCV: Computer Vision with the OpenCV Library. O'Reilly Media; 1st edition. 2008. Jain, Ramesh; Kasturi, Rangachar; Schunck, Brian G. Machine Vision. McGraw-Hill, 1995. VanZwieten, Tannen. Dynamic Simulation and Control of an ASV. Masters Thesis, Florida Atlantic University. 2003. Caccia, Massimo; Bibuli, Marco; Bono, Riccardo; Bruzzone, Gabriele. Basic Navigation, Guidance and Control of an Unmanned Surface Vehicle. Springer Science and Business Media, LLC. 2008.

11