Curtis Olson
ME 5271 - Robotics
Winter Quarter, 1994
The purpose of this project is to adapt an existing obstacle detection and avoidance algorithm (developed by Don Krantz) to a roadway environment. The existing algorithm is implemented in a simulation (also developed by Don Krantz.) The existing algorithm/simulation is capable of detecting and avoiding stationary obstacles while guiding a vehicle through a set of predefined goals. Given Don Krantz's work as a starting point, the goal of this project is to adapt and extend the simulation to a roadway environment. The simulated vehicle must drive itself along a previously unknown road while detecting and avoiding obstacles in its path. This simulation task involves creating a pseudo-vision system. The vision system provides information to the simulation about the position and orientation of visible road edges. The existing obstacle detection and avoidance algorithms must be modified to operate within the additional constraints provided by the vision system. With the vision system providing information to the simulation about road edges, ``drive-to'' goals can no longer be predefined. Instead, these goals must be generated dynamically from road edge information provided by the vision system.
The original work on this project was done by Don Krantz in the Spring of
1993.
His intent was to simulate the University of Minnesota Autonomous
Land Experimental Robotic Testbed (ALX). The ALX is an Ackerman-steered four
wheeled vehicle built on a golf cart chassis. As far as the simulation is
concerned, the ALX has three main sensory systems.
The simulation constructs and maintains a map of the local area around the vehicle. As the vehicle moves, this local map scrolls and follows the vehicle. As the map scrolls, the traversed areas ``falls off'' the trailing edge of the map, while new unknown area appears at the leading edge.
At each time step the simulation takes the information received from each of
its sonar sensors and updates its local map. The map is partitioned into a
two-dimensional array of cells. Each cell can be unknown, empty, or occupied.
As the map scrolls, new area is marked as unknown. If a sonar sensor does
not detect an obstacle in its ``cone'', the entire area covered by this cone is
marked as unoccupied. If the sensor detects an obstacle in its cone, all
cells at that distance within the cone are marked as occupied. Note: the
actual algorithm which determines if a given cell is unknown, empty, or
occupied is somewhat more complex than I have just described. The algorithm
must be able to handle conflicting data from overlapping sensor ranges, and
updated information as the vehicle moves.
The simulation relies on the idea of a stack of goals. The simulation attempts to move the vehicle in the direction of the top goal on the stack. When the vehicle has ``passed'' this top goal, the goal is popped off the stack, and the vehicle moves towards the next goal. This process continues until the stack is emptied.
The actual planning and moving algorithm is relatively simple. The vehicle begins by simple moving towards the first goal on the stack. Once this goal has been passed, the vehicle moves towards the next goal. This process continues until the stack of goals is exhausted, or until a potential collision is detected.
When a potential collision is detected, the vehicle stops. It then searches for an unobstructed path through the environment as ``viewed'' through the sonar sensors. If no path can be found the vehicle drops a ``repeller'' and backs up a small amount in the direction of the most recently passed goal. From its new vantage point it retries the search. This process is repeated until a search is successful. A successful search returns a new set of goals. When followed, these goals will lead the vehicle through the obstacles. After these goals are exhausted, the previous forward goals are resumed.
A repeller is simply a mark on the local area map. When searching for a path through the environment, a repeller acts as a virtual obstacle. In other words, a search will never find a path through a repeller since the search will be blocked as soon as it intersects with the repeller area. Note: repellers have no affect on the collision detection system and cannot be detected by the sonar sensors since they are not real obstacles. The semantics of using repellers is that if a search fails to find a path to the goal from the current location, we know that the chances of ever finding a solution from that point is low. Therefore, if in the process of searching for a path through the environment, the search algorithm encounters a repeller, then we can know that the chances of finding a solution past that point are low. In this case the depth first search can backup and try a different path.
Repellers have the effect of causing the search algorithm to build up an aversion to problematic areas such as dead ends so that the vehicle can eventually find a way around.
The existence of a repeller provides the search algorithm with a heuristic. Using repellers the algorithm can decide (in a very limited sense) if the current search path is ``bad'' or if it is still ``promising.'' There is an inherent problem with repellers, however. Due to the limited steering angle of the vehicle, a search from a certain position and orientation may be blocked. However, given a different orientation, a search from the same position may be successful. Therefore, a repeller can block a potentially successful search. However, despite their inherent problems, in simulation, repellers seem to work relatively well.
The goal of this project was to take Don Krantz's algorithms and simulation as a starting point and extend them to a roadway environment. This process involves three major components.
Through out the work on this project, a considerable effort was put into maintaining a modular ``plug-and-play'' structure. The goal was to add additional features without replacing or removing existing features. Therefore it is still possible to run the original simulation in its original form with the same results. In other words it is possible to enable and disable the different features of the simulation. It is possible to choose between different kinematic models, different search algorithms, different pseudo-vision systems, different sensor configurations and different environments. It is also possible to define additional kinematic models, search algorithms, and vision systems, and include them into the simulation.
Figure 1: Simple track composed of line segments.
In this simulation, a roadway is defined by its left and right edges.
The edges can be approximated by a series of line segments.
Figure 1 shows a simple track layout consisting of a left
edge and a right edge. The left edge is defined by the points
The right edge is defined by the points
These series of points can be saved in
a file. In this way multiple courses can be defined and the desired one
can be selected at run time.
A real vision system is able to determine the edges of the road from the image received from a video camera. It can then map the coordinates of the road edges into the coordinate frame used by the vehicle. This information can then be passed to the vehicle so that it can make decisions on how to move. An alternative approach is for the vision system to make the driving decisions and simply pass the movement commands to the vehicle. However, we felt that it was important to let the vehicle decide its own path. This way, it can merge sensory information from multiple sources, and make its own policy decisions on how to move.
Once the course is suitably defined in the simulation, the pseudo-vision system can begin to return information about it. The road information is then used by the simulation to dynamically derive forward goals. The approach taken is that the vehicle will move towards the current goal points. If there are no forward goals, or if the forward goals are exhausted, the simulation will query the vision system for the visible road data. From this information, the simulation will derive several new goal points and proceed towards them.
The question now is, how many goals, and how far ahead should be generated. This question has not been studied closely. However, by running the simulation, reasonable values can be found. In general, the vehicle is moving relatively slowly (less than 5 mph.) Therefore, the vehicle can react to goals that are generated immediately in front of it and to goals that are closely spaced together. Also, the vehicle has plenty of time to generate new goals as it moves forward. We can also make the assumption, that the road edge information returned by the vision system becomes less accurate as the distance from the camera increases. Therefore, it makes sense to accurately generate a relatively small number of goals near to the front of the vehicle. Additional goals can be easily generated as needed, and the vehicle can react quickly in order to follow goals that are immediately in front of it. The number of generated forward goals that seems to work well for this simulation is six. The goal spacing that seems to work well is 1.5 meters. These values will no doubt have to be adjusted for the actual vehicle.
Another issue to consider is when should new forward goals be generated. Currently the simulation generates new goals only when all forward goals are exhausted. Another option would be to generate new goals at each time step. This would have a computational effect, but may result in smoother, more predictable, more reliable motion. With this technique, the vehicle would never actually reach any goals because they would be continually regenerated before the vehicle could reach any of them.
Under all but the most degenerate situations, the vehicle will find itself in one of two vision situations. The first is the most ideal. In this case, the vision system will be able to see both road edges. When both road edges are visible (see Figure 2) the vehicle can generate goals that follow the center line of the road. Figure 8 shows an actual view of the simulation generating goals from both road edges.
Figure 2: Both road edges are visible.
The method for calculating goal points from vision data is as follows.
The camera has a limited field of view represented by 
. The point
where the right edge of the camera's field of view intersects with the
right road edge is point 
. Similarly the point where the left edge of
the camera's field of view intersects with the left road edge is point 
.
Point 
is the midpoint of the line segment connecting 
and 
.
Point becomes the first generated goal.
The next goals are generated in a similar manner. Point 
is located
at a distance 
from point along the right road edge in the positive
direction. Likewise, point 
is located 
units from point along
the left road edge. The next goal is simply the midpoint of the line
segment connecting points and 
. This process is continued for all
the goal points the simulation wishes to generate. These goals then become
the primary forward goals towards which the simulation moves.
Often when avoiding an obstacle or negotiating a turn, the vehicle enters a state where one of the road edges is not visible (see Figure 3). When this occurs, the previous method of locating forward goals can no longer work. Figure 9 shows an actual view of the simulation generating goals from both road edges.
Figure 3: Single road edge is visible.
Instead of locating points on each side of the road, the simulation is
limited to locating points on only one side of the road. The method for
calculating goal points from the limited vision data is as follows. The
camera's field of view is represented by 
. The point where the
edge of the camera's field of view intersects with the visible road edge
is point called point . Instead of taking a midpoint, the simulation
calculates a point at a distance along the road edge in the
positive direction. The algorithm then calculates a normal to the visible
road edge at . The goal is placed at a predefined distance towards the
center of the road on this normal. Thus, the goal is located ``near''
the center of the road.
The next goals are generated in a similar manner. Point is located
at a distance from point 
along the visible road edge in the
positive direction. A normal to the road edge is calculated at point
. The goal is placed at the predefined distance from the road edge
on this normal. This process is continued for all the goal points the
simulation wishes to generate. These goals then become the primary
forward goals towards which the simulation moves.
Notice that a goal is not generated from a normal at point . Often
this `single-edge-visible' situation arises during a cornering maneuver.
Usually in this case, the outer road edge is the visible edge. The goal
generated from point is usually close to the edge and at a sharp turn
angle from the vehicle. This forces the vehicle to turn sharply outwards
to try to reach the goal. This can have the effect of placing the vehicle
nearly perpendicular to the road edge. Once this goal has been passed
the turn to reach the next goal is nearly impossible to make without
leaving the road. For this reason, the first goal is skipped, this causes
the vehicle motion to be much smoother and keeps the vehicle on the road.
Notice that the goal generating algorithm only follows the road edge, it
makes no attempt to find an optimal trajectory.
The search algorithm as written by Don Krantz attempts to find any path through a set of obstacles. However, with the addition of road edges, finding any path is no longer sufficient. The search algorithm needs to confine itself within the boundaries of the road edges. The path returned by the algorithm should never leaves the roadway. This is accomplished by checking to see if any search segment crosses a road boundary (see Figure 4.)
Figure 4: The search tree cannot cross road boundaries.
At each iteration of the search algorithm, a line segment from the front of the vehicle to the current search point is intersected with the road edge. If a point can be found that lies on both the line segment and an edge of the road, then the search in that direction is stopped. This does not mean that the entire search fails, only that the search down that particular branch fails. The algorithm will then back track and attempt to find a different path.
Figures 8, 9, 10, 11, and 12 show actual snap shots of the the simulation. The key for the various symbols used in the simulation is shown in figure 5.
Figure 5: Key of symbols used in the simulation output.
Figure 8 shows the simulation calculating some forward goals when both road edges are visible. Figure 9 shows the simulation calculating some forward goals when only a single road edge is visible.
Figures 10, 11, and 12 show the vehicle approaching an obstacle, detecting it, and successfully avoiding it.
In figure 10 the vehicle is travelling down the center of the road approaching an obstacle. The shaded area represents area covered by the sonar sensors that is free of any obstacles. (As the vehicle moves this shaded area will grow.) The small thick ``plus'' symbols represent sonar pings. Due to the nature of the sonar sensors, when a ping is returned, the simulation does not know where within the sensor's cone an obstacle lies, just the distance. So the simulation marks each cell within the sonar cone at the specified distance as occupied.
Figure 11 shows the vehicle at the next time step from the previous figure. Here, the vehicle is detecting a potential collision. The line drawn from the front of the vehicle to the sonar ping represents which sensor detected the potential collision. Often multiple sensors can detect collisions simultaneously.
Once the vehicle has detected a collision, it must search for a clear path around the obstacle. If it cannot find a path, it will backup a small amount and search again. Figure 12 shows how the vehicle has found a path around the obstacles. The small circles represent the path that was found. Each circle represents a goal on the secondary stack. Once every goal on this secondary stack has been passed, the simulation resumes following goals generated from the road edge information. In this case, there are still several objects ahead that must also be avoided. Due to the limited sonar sensor range, the vehicle is not yet aware of them. However, as it moves closer to the obstacles it can detect and successfully avoid them as well.
The user interface of the original simulation was written in Xview. Although, Xview works, the binaries that are linked with it are excessively large. The striped Sun executable when linked with the Xview shared library had a size of 168kb. This is reasonable, however I did most of my development on a PC 486 running Linux. Since shared versions of the Xview libraries are not available for Linux, the stripped executable was well over 1Mb. This large size tremendously increases link times and execution load times. Also, the appearance of Xview, although nice at one time, does not measure up to todays standards.
For these reasons I made the decision to rewrite the user interface. The tools with which I chose to do this are Tcl and Tk. Tcl stands for Tool Command Language. Tcl is a general purpose text based scripting language. Tk stands for Tool Kit. Tk is a set of graphical extensions to the Tcl scripting language.
The Tcl/Tk combination provides several major benefits when creating user interfaces.
Tcl provides a shell which can accept Tcl commands directly, or can execute
Tcl scripts. Tk provides a windowing shell called wish. Since
it is an extension of Tcl, it can accept both Tcl and Tk commands directly,
or execute them in a script form. By executing Tk script commands,
various widgets such as buttons, menus and scroll bars can be added to the
wish window. Tcl/Tk procedures and commands can be bound to the
widgets in order provide an entire working application, or just a front end
to another application.
For this project, Tcl/Tk was used to create a front end interface to the
actual simulation. The Tcl/Tk script provides all the button, menu,
check box, and scale widgets. It also organizes, sizes, and places them in
the wish window. Once the interface has been configured, the script
opens a Unix pipe to the actual simulation program. Every item of the
interface is bound to a Tcl command which sends a text string through the
pipe to the actual simulation. In this way, the Tk interface can
control the simulation. The simulation simply reads and executes text string
commands from its stdin. When the Tk interface opens a pipe to
the simulation, it automatically connects the output of the interface to the
input of the simulation.
Figure 6: User interface (command palette) written with Tcl/Tk.
Along with rewriting the user interface in Tk, a significant amount of functionality was added. Data files are selected from a menu rather than being specified on the command line. A reset button was added so the simulation can be reset and rerun. A check box was added to turn the camera field of view and goal generation representation on and off. A second check box was added to toggle the sonar sensor configuration display. Finally, menus were added to select from the defined kinematic models, search algorithms, and pseudo-vision system models.
The user interface commands for this simulation are organized into five main groups. These groups are Commands, Display Options, Configuration Parameters, Data Files, and Miscellaneous.
The Commands group includes five push buttons for controlling the operation of the simulation, Run, Step, Pause, Reset, and Quit. The Run button causes the simulation to run. The Step button advances the simulation one frame at a time. The Pause button pauses the simulation. The Reset button aborts the current simulation and resets everything to the starting point. Finally the Quit button provides a clean way to exit.
The Display Options group consists of a set of check boxes for specifying what items to display in the course of the simulation. These functions have no affect on the simulation, only the display. The Show Camera option specifies whether or not to display the camera field of view, as well as the road edge connections and normals used for generating forward goals. The Show Goals option specifies whether or not goals (represented by a Maltese cross) are displayed. The Show Repels option specifies whether or not repellers are displayed. The Show Search options specifies whether or not the search progress is displayed as the simulation searches for a paths around obstructions. The Show Sensors options specifies whether or not the location and orientation of the sonar sensors are displayed on the vehicle. The Show Targets option specifies whether or not the forward targets are displayed. A target is a subgoal that must be passed in order to reach the immediate main goal. The Show Trails option specifies whether or not the vehicle is undrawn before drawing its next position. When Show Trails is enabled, all previous vehicle positions are left drawn on the screen. (This option can produce a very messy display but can be useful for viewing the vehicle's trajectory.)
The Configuration Parameters group includes three menus, Kinematics, Search, and Vision. The Kinematics menu allows the user to specify the desired vehicle kinematics model. The Search menu allows the user to specify the desired search algorithm. The Vision menu allows the user to specify the desired vision model. The current vision choices are: no vision, a simple fake vision system (used for testing), and the ``real'' pseudo-vision system.
The Data Files group includes four menus, Courses,
Obstacles, Goals, and Sensors. The Courses menu
allows the user to specify the desired course (or road edges.) The
Obstacles menu allows the user to choose from a variety of predefined
sets of obstacles. The Goals menu allows the user to choose from
a variety of predefined sets of goals. This option is a carry over from
Don Krantz simulation. Normally, goals are generated dynamically from
vision data. However, in the case where the simulation is run with no
road edge data, a set of predefined goals must be selected. Finally, the
Sensors menu allows the user to choose from a variety of predefined
sonar sensor configurations. The sensor configuration labelled
golf-cart is a close approximation to the sensor configuration of
the actual vehicle.
The Miscellaneous group includes two items. The first is a slider for specifying simulation speed. The speeds available range from as fast as the hardware can run to a one second delay between each time step. The second item is an ``About'' button. This pops up a window giving appropriate credit to the authors.
Tcl/Tk was written by John K. Ousterhout of the University of California,
Berkeley. His electronic mail address is ouster@cs.berkeley.edu.
Tcl/Tk can be retrieved by anonymous ftp from ftp.cs.berkeley.edu.
From a Unix workstation, the procedure for obtaining these packages is as
follows.
ftp ftp.cs.berkeley.edu''.
ftp'' as your user name.
bin'' to specify that files should be transferred in
binary mode.
dir'' to get a listing of all the
available files in this directory.
get tcl7.3.tar.Z''. Note: over time, the
version numbers will increase, but the base name will remain
the same.
tcl7.3.tar.Z. The
current version of Tk is called tk3.6.tar.Z.
Documentation can be retrieved from the same directory as Tcl and Tk. It
is in postscript form and comes in four parts. The file names to look for
are book.p1.ps.Z, book.p2.ps.Z, book.p3.ps.Z, and
book.p4.ps.Z. Note, this documentation is Copyright ©1993
by Addision-Wesley Publishing Company, Inc. It is a partial draft copy of
a book to be published ``soon'' and is for personal use only.
One nice feature that was added to the simulation is an overall map view. This view shows a small scale representation of the entire track. This view also includes the obstacles that the vehicle must traverse and a pointer to represent the current position and orientation of the vehicle. The simulation window only shows the local area around the golf cart. Since this view is very limited, the map view can be very useful for monitoring the vehicles overall progress around the track.
Figure 7: View of entire track.
The simulation was enhanced to accept some real parameters. Specifically, a maximum left steering angle and a maximum right steering angle were defined. The maximum left steering angle for the vehicle is 26 degrees. The maximum right steering angle is 30 degrees. Also, minimum and maximum sensor ranges can be defined on a per sensor basis. Real vehicle dimensions already could be defined in the original simulation, however actual measurements of the vehicle were taken and these numbers plugged into the new simulation.
In its original form, the simulation required the Sun Xview graphics libraries and could only be run on the Sun workstations. One small goal of mine is to make the simulation as portable as possible. This was another reason for abandoning Xview in favor of something better. Currently the simulation is designed to depend on the availability of Xlib only. Xlib is a low level X-windows graphic library that can be found on any workstation supporting X. (Note, the user interface does depends on Tcl/Tk as well. However, Tcl/Tk's only graphical dependency is on Xlib, so the entire package is dependent only on Xlib.) This way, the simulation should be easily portable to just about any version of Unix.
The original simulation was enhanced in several other ways. In working on the code throughout the quarter, several major bugs were uncovered. These problems didn't affect the original simulation too severely. However, enhancing and extending the simulation put a certain ``stress'' on the code which caused some bugs to be exposed.
One such bug involved the repeller array. A repeller is dropped each time a search fails and the vehicle must back up. The list of repeller was kept in a fixed size array. Over time, after much backing up, the repeller array would fill up. It turns out that the full array condition was never checked, so as repellers continued to be dropped, they were added past the end of the array. This had the unfortunate effect of overwriting other valuable data. I fixed this problem by dropping off old repellers once the array filled up. Dropping old repellers can be justified since inaccuracies in the dead reckoning system will make old position data nearly useless over time.
Another problem that was exposed occurred when the array of map cells was scrolled as the vehicle moves. The code that shuffled the cells occasionally wrote outside the array space. This also had an unfortunate affect. I fixed this problem by simply preventing writes to invalid array positions.
One last enhancement I made was the addition of revision control information.
The revision control system I used is called Concurrent Versioning System
(CVS.) CVS allows a master copy of the source code to be kept in a
repository. The user then ``checks out'' a working copy. As changes are
made, the user can check these back into the main repository. The
CVS system keeps track of what changes were made when and by whom. It allows
multiple users to work on the same code and arbitrates between any conflicting
modifications. It allows code to be ``backed out'' to previous versions and
has many other nice features for code development. CVS is maintained by the
free software foundation. It is available through anonymous ftp from
prep.ai.mit.edu in the directory /pub/gnu. Note: CVS is not
necessary for future code development of this project, it only provides a
structure for software development if such structure is desired.
When I first put the three major components together (pseudo-vision system, roadways, and modified search algorithm) and ran the simulation, I was discouraged to see the simulation became very unstable. Often when forced to find a path around an obstacle, the vehicle would wander off the road and begin a cross country trek. I traced this problem to the way the main forward goals were handled when they were temporarily replaced by a series of subgoals representing a path around an obstacle. When switching between primary and secondary goals, the simulation would often delete some of the primary forward goals so that after the secondary goals were passed, the simulation would go back to following nonexistent primary goals and get lost.
I fixed this problem by having the simulation regenerate the primary forward goals each time the vehicle is blocked and must backup. This has the effect of greatly stabilizing the simulation. It is now very robust when faced with normal situations. Unfortunately, it still can become unstable when faced with degenerate situations such as excessively sharp turns, or dead ends where there is no possible path around the obstacles.
There is still some inherent problems with repellers. Repellers provide the search algorithm with a heuristic. Using repellers the algorithm can decide if the current search path appears ``bad'' or if it is still ``promising.'' Unfortunately, repellers are not always good heuristics. Due to the limited steering angle of the vehicle, a search from a certain position and orientation may be blocked. However, given a different orientation, a search from the same position may be successful. Therefore, a repeller can block a potentially successful search. If repellers are removed from the simulation, then searches will no longer be needlessly blocked. Unfortunately repellers are the only mechanism the simulation has for building up an aversion to a local minimum. Without repellers, the simulation could find itself stuck in an endless cycle of being blocked, backing up, finding a path forward, being blocked, and so on. It may never being able to find a way around. So, for lack of a better technique, repellers are a necessary evil for this simulation.
There is another problem that is open for improvement. Currently the search algorithm, when looking for an unobstructed path, only makes sure the path of goals does not leave the road. At times, the path can come very close to the edge. When the vehicle follows such a path perfectly, almost half of the vehicle can be off the road at any given instance. This behavior will not cause problems if there is a sufficient shoulder. However, if the edge of the road represents a cliff, then vehicle and any potential occupants will be placed in jeopardy. This problem could be solved by adding a parameter to the simulation. This parameter would specify how close to, or how far over the road edge, the search algorithm is allowed to look.
The possibilities for future extensions of this project are endless. Since it is a simulation of a real world vehicle, any enhancement that makes the simulation more accurate is desirable. There are also a number of other types of extensions that would make this simulation more useful.
Figure 8: Forward goal generation with both road edges visible.
Figure 9: Forward goal generation with single road edge visible.
Figure 10: Vehicle is approaching an obstacle.
Figure 11: Vehicle detects a potential collision with an obstacle.
Figure 12: Vehicle has found a path around the obstacles.
Constrained Obstacle Detection
and Avoidance
AV1
This document was generated using the LaTeX2HTML translator Version 0.6.4 (Tues Aug 30 1994) Copyright © 1993, 1994, Nikos Drakos, Computer Based Learning Unit, University of Leeds.
The command line arguments were:
latex2html -split 0 navsim.tex.
The translation was initiated by Curt L. Olson (Admin) on Fri Feb 17 15:11:09 CST 1995
