UBC Theses and Dissertations

UBC Theses Logo

UBC Theses and Dissertations

Navigation system for semi-autonomous tracked machines Kusalovic, Dejan 1998

Your browser doesn't seem to have a PDF viewer, please download the PDF to view this item.

Item Metadata


831-ubc_1998-0257.pdf [ 4.99MB ]
JSON: 831-1.0065084.json
JSON-LD: 831-1.0065084-ld.json
RDF/XML (Pretty): 831-1.0065084-rdf.xml
RDF/JSON: 831-1.0065084-rdf.json
Turtle: 831-1.0065084-turtle.txt
N-Triples: 831-1.0065084-rdf-ntriples.txt
Original Record: 831-1.0065084-source.json
Full Text

Full Text

NAVIGATION SYSTEM FOR SEMI-AUTONOMOUS T R A C K E D MACHINES By DEJAN K U S A L O V I C  B.A.Sc. (Electrical Engineering) University of Belgrade, 1994 A THESIS SUBMITTED IN PARTIAL F U L F I L L M E N T OF T H E REQUIREMENTS FOR T H E D E G R E E OF M A S T E R OF APPLIED SCIENCE in T H E F A C U L T Y OF G R A D U A T E STUDIES DEPARTMENT OF E L E C T R I C A L A N D COMPUTER ENGINEERING  We accept this thesis as conforming to the required standard  T H E UNIVERSITY OF BRITISH C O L U M B I A April 1998 © Dejan Kusalovic, 1998  In presenting degree  this  thesis  in partial  fulfilment  of the requirements  at the University of British Columbia, I agree that the Library shall make it  freely available for reference and study. I further copying  for an advanced  of this  department  or  agree that permission for extensive  thesis for scholarly purposes may be granted by his or  her  representatives.  It  is  by the head  understood  that  publication of this thesis for financial gain shall not be allowed without permission.  Department  of  gtecfrical aid Ccwjodfr &yfrifi/?r/rij  The University of British Columbia Vancouver, Canada  Date  DE-6 (2/88)  fy / n  /9%f  .  of my  copying or my written  Abstract The issues researched in this thesis work relate to the theoretical and practical problems in analysis, design and implementation of mobile, tracked, semi-autonomous machines in outof-door environments (particularly the work sites of small to medium size, 100x100m). A complete navigation system is analyzed and proposed while the focus has been placed on developing an efficient path planning subsystem. Mechanisms of ensuring that the paths are safe are also proposed. The path planner is based on searching an elevation map of the work space. The search algorithm is based upon the A * search with a novel cost function. The cost function optimizes the path with regard to the cost map that reflects length of the path and relative altitude change along the path. These objectives contribute toward improving path efficiency and satisfying environmental standards which are general objectives for heavy mobile machines in off-road environments. Multiple versions of the cost function are used to provide multiple paths which allow an operator to have final control over the choice. Having multiple paths increases the chances that the operator (or supervisor) will be able to find a path that fits a variety of other objectives that have to be considered in real world conditions (for example moving obstacles, soil type etc.). One special case of terrain is studied in great detail. When travelling slopes it is necessary to enforce switchbacking (climbing in a zig-zag fashion) to account for mechanical limitations of the vehicle and to reduce the soil disturbance on these specially sensitive areas. The additional features for the search algorithm are developed to produce such paths when travelling slopes. This has been achieved by choosing suitable cost functions and placing additional constraints on the number and nature of turns along the path generated by the search algorithm. This is the area that no other existing grid search path planner covers. ii  The path planner was implemented and tested on grids representing different terrain types that range from flat to severely hilly. The 30x30 grid required an average of 1.55 sec C P U time to perform which is suitable for the intended usage. The off-line implementation of the path planner is accompanied by a custom graphical interface for path presentation. This GUI is also a prototype of the operator's interface planned for the final version of the navigation system. A partial integration of the navigation system was achieved and evaluated on a miniexcavator. An already existing control system was adjusted for use in leading the mini-excavator along the path determined by the path planner. The control strategy was very simple, yet we demonstrated good precision of path following (position error was less then 3% on an 8x8m field), based on the fact that a path is made up of links between neighbouring nodes in a regular grid. Ways of reducing potentially accrued errors on bigger fields are proposed.  in  Contents Abstract  ii  List of Tables  viii  List of Figures  ix  Acknowledgments  xii  Chapter 1  Introduction and Motivation  1  Section 1  Operating Scenario  3  Section 2  The Path Planner Requirements  3  Section 3  Overview of the Thesis Work  6  Section 4  Thesis Organization  6  Background on Mobile Robot Motion Planning Systems  8  Section 1  Mobile Robot Motion Problem  8  Section 2  Extensions of the Mobile Robot Motion Problem to Real World  Chapter 2  Applications Topic 1  24  Inaccuracy of the world map and low level control in motion planning  25  Topic 2  The Moving Obstacles Problem  26  Topic 3  The Kinematic Constraints Problem  28  Section 3  Existing Mobile Robot Navigation in Outdoor Environments . . 29  Topic 1  Underwater AMVs  30  Topic 2  Air based AMVs  31  Topic 3  Ground based AMVs  31 iv  Section 4  Grid-map Based Path Planners  36  The Path Planning Algorithm  38  Requirements for the Path Planner  39  Topic 1  Obstacle Avoidance  40  Topic 2  Path Efficiency  41  Topic 3  Soil Disturbance  42  Graph Search Algorithms  44  Topic 1  An Alternative Search Approach  47  Topic 2  Summary of the Characteristics of MOA*  49  Section 3  Structure of the Gridmap and The Derived Graph  52  Section 4  Design of the Path Planner  53  Topic 1  The Algorithm Set-Up and the Operator's Input  54  Topic 2  The Cost Map Generation  57  Topic 3  The Search Algorithm  59  Topic 4  Handling Critical Situations  67  Chapter 3 Section 1  Section 2  Subtopic 1 Critical Proximity Situations Subtopic 2 Traversing Areas with Existing Roads Section 5  67 69  Off-Line Implementation and Evaluation of the Path Planner . 70  Topic 1  Test Cases  70  Topic 2  Presentation of Results and Discussion  71  Subtopic 1 Speed Analysis of the Algorithm in the Case of Single Cost per Arc Subtopic 2 Algorithm With Up to Six Costs per Arc  71 72  Subtopic 3 Path Planning on Slopes Chapter 4  77  The Navigation System Requirements and Analysis  81  Section 1  Requirements Analysis for the Experimental System  81  Section 2  The Hardware Requirements and Analysis  82  Topic 1  Sensor System Analysis  84  Topic 2  Track Control Actuator System Analysis  85  Topic 3  Computing Resources and Connections Analysis  85  Software Requirements and Analysis  88  Topic 1  Functional Requirements  88  Topic 2  Software Design  94  Topic 3  Non-Functional Requirements and Analysis  94  The Navigation System Integration and Experiments  96  The Integration and Experimental Methods  96  Topic 1  Hardware Configuration  96  Topic 2  Software Methods and Implementation  97  Topic 3  The Experiment Setup and Procedure  101  Section 2  The Experimental Results and Observations  103  Section 3  Discussion  105  Summary and Conclusions  106  Section 1  Summary  106  Section 2  Conclusions  107  Section 3  Directions for Future Work  108  Section 3  Chapter 5 Section 1  Chapter 6  vi  Bibliography  111  vn  List of Tables Table 3.1  Possible combinations of ( A , A )  Table 3.2  Average execution times for different sizes of the grid in the  1  2  59  case of one cost per arc  72  Table 3.3  The weight combinations  75  Table 5.1  Position errors measured while running the demo system . . 104  Table 5.2  Percent errors  104  viii  List of Figures Figure 2.1  Motion Control Layers  10  Figure 2.2  Example of visibility graph  11  Figure 2.3  A Voronoi diagram  12  Figure 2.4  Illustration of the cell decomposition method  14  Figure 2.5  Vertical cell decomposition  15  Figure 2.6  Connectivity graph for examples from Figure 2.4 and 2.5 . . . 15  Figure 2.7  Illustration of the approximate cell decomposition method . . . 17  Figure 2.8  Approximate cell decomposition  18  Figure 2.9  A negative potential field  20  Figure 2.10  Gradient vector of the potential function  21  Figure 3.1  An excavator on a slope  43  Figure 3.2  The A* search illustration  46  Figure 3.3  Structure of the grid map and the graph  52  Figure 3.4  The complete path planning algorithm  54  Figure 3.5  The search set-up and operators input  56  Figure 3.6  Minimizing the number of turns  61  Figure 3.7  The search algorithm block diagram . ,  62  Figure 3.8  The angle checking  65  Figure 3.9  Relation between the grid map, AMV and obstacles and their sizes  68  Figure 3.10  Path planning when a road is present  69  Figure 3.11  Examples of solution paths for 30x30 and 60x60 sized grid maps  72 ix  Figure 3.12  The solution paths for a configuration space with a hill  Figure 3.13  The solution paths for a severely hilly configuration space . . . 74  Figure 3.14  The contour view for a red (A-|=1) solution path  76  Figure 3.15  Unacceptable path planning on slopes, example 1  77  Figure 3.16  Unacceptable path planning on slopes, example 2  78  Figure 3.17  Path planning on wide slopes  79  Figure 3.18  Path planning on narrow slopes  79  Figure 3.19  Path planning on slopes with obstacles  80  Figure 3.20  A way of slowing down the climbing rate  80  Figure 4.1  System's hardware overview  83  Figure 4.2  Actuators' hardware diagram  85  Figure 4.3  System's computers and their connections  86  Figure 4.4  The RTSA notation  89  Figure 4.5  System's software overview  89  Figure 4.6  High level data/control flow diagram  90  Figure 4.7  The sensor data processing bubble  91  Figure 4.8  The control calculating and execution bubble  92  Figure 4.9  The path_planner bubble  93  Figure 4.10  The operators'interface bubble  94  Figure 5.1  High Level Software Design  97  Figure 5.2  Control system diagram (Modified from Poul Jacobsen's thesis)  Figure 5.3  73  98  Interface between the path planner and low level c o n t r o l . . . . 99  Figure 5.4  Deriving the output for the low level control (inverse kinematics)  100  Figure 5.5  Illustration of the experiment and measurements  103  Figure 5.6  Solution paths for different cases of the configuration space and goal positions  103  XI  Acknowledgments I would like to express my sincere gratitude to my supervisor, Dr. P. D. Lawrence, for his guidance, advice, support and patience throughout this thesis work. I would also like to acknowledge Dr. S. E . Salcudean and Dr. G. W. Bond for reading my thesis and their valuable comments. I wish to thank the research engineers of the robotics group, in particular, Poul Jacobsen, Shahram Tafazoli-Bilandi and Henry Wong for the technical help in the laboratory. The project funding provided by FERIC (Forestry Engineering Research Institute of Canada) is very appreciated.  xii  Chapter 1 Introduction and Motivation Developing mobile robots with human-like capabilities of moving efficiently in different environments is a very challenging task due to the complexity of its work environment. Research on mobile robot navigation systems still has not provided us with satisfactory algorithms and technology to design a mobile robot that would be able to operate autonomously in dynamically changing real-world environments. The techniques used, based on different heuristic search algorithms and neural networks that are intended to provide a robot with learning capabilities, can lead to very complex, computationally heavy systems. It is also a fact that numerous different sensors have to be used and the amount of data to be processed and fed to the navigating algorithms is increased. Despite the above facts, we are witness to a growing number of autonomous machines moving and operating in the world today. Some of them not only are research projects but also are fully operating machines that are performing their tasks and helping humans increase their productivity and welfare in general. A very recent and exciting example is NASA's mobile robotic platform that roamed the surface of Mars collecting valuable data about the planet. The approach that allowed implementation of such machines is the narrowed field of operation. In other words, the solution is to consider a specific area of usage for a robot and then to develop an adequate navigation system for it. By doing that computational capacity is left for further refining and optimizing the performance of robot movement.  Most of these very practical  solutions are induced by a need for having a job done in dangerous and barely accessible areas. Certain branches of research and implementation in the field of navigating mobile robots have had considerable attention in the literature and practice. A few for example are: submarine l  autonomous systems, aerospace autonomous systems, unmaned military vehicles and intelligent cars.  In this thesis work a path planner for an semi-autonomous tracked vehicle has been  designed, implemented and tested. The requirements and design for the complete navigatio system of an semi-autonomous tracked forestry machine is proposed and the partial integration and testing of the system has been performed..The research work done for this thesis is placed into a specific framework of using tracked machines on work sites of a medium size (900 to 10000 square meters) though its results can be easily transferred into similar applications on large areas wherever heavy tracked hydraulic machines such as bulldozers and excavators are used. Special attention has been given to the problems in forestry. The major goal of this thesis was developing a path planner that would provide an optimized path for a tracked machine in a worksite. The foremost problem, solved here, was to optimize the path by variety of diverse objectives.  We wanted to have a path planner that produces the most efficient path with  regard to challenges set by the nature of the worksites for tracked hydraulic machines. These challenges were possible slopes in the work field, different terrain types (often prone to soil disturbance) and a desire to find a short path among possibly numerous obstacles. Given the complexity of the task, we have projected our system to have two modes of operation: a semi-autonomous and a manual mode. As already mentioned, considerable attention has been given to the problem of navigating autonomous machines in off road environments in the literature[l], [2], [3], [4]. For example at the Carnegie Mellon University several off road vehicles have been developed to be guided by a roughly specified path with a lot of reactive behaviour [1], [2]. The path is generated upon a previously known coarse world map and the obstacles are also observed on-line. A 2  robot named R A N G E R , based on a US military jeep, is capable of moving with a significant speed (up to 15 km/h) but it is allowed to make big diversions from the original path. An industrial example are robotic trucks. Both Caterpillar and Komatsu are developing semi-autonomous trucks.  The trucks are travelling predefined routes (given by a remote  operator) and are equipped with proximity sensors (lasers and sonars) so they can stop or, in some cases, go around a newly discovered obstacle. They are slower machines (up to lOkm/h with precision of 2m over 100m travelled distance.  1.1 Operating Scenario The intended operating scenario for our system assumes that an operator/supervisor is present. He/she would have an opportunity to choose between multiple paths offered by the path planner. He/she could switch to the manual mode of operation (using a joystick). It is also assumed that the elevation map of the worksite is provided. The elevation map of the work site can be obtained from satellite images. It is expected that adequate images (with spatial resolution of l m or better) will be commercially available before the year 2000 [5], [6]. The position of the obstacles is to be determined by analyzing the images using an on-board vision system. Trees, buildings, roads etc. are very distinctive features on an image. Abrupt changes in elevation would also be considered to be boundaries of obstacles. A stereo vision or sonar system mounted on the machine would help in detecting and localizing obstacles [7]. We also need a GPS (Global Positioning System) for determining the position of the machine.  1.2 The Path Planner Requirements The elevation map is transformed into a regular 2D graph. The path planner is based on the A * search with a custom-developed cost function and additional constraints employed when defining a suitable path. Among numerous possible choices for searching 2D graphs, 3  the choice was made because of the algorithm's robustness, relative efficiency and popularity in literature [8], [9], [10]. The objectives that we tried to achieve with our navigation system could be classified into two categories, where the second one has been given little or no attention previously.  1.  Standard objectives, namely, to guide the machine toward its goal position in a safe and efficient way (i.e. obstacle avoidance and the shortest path). . The goal was to replace some of the reactive behaviour with repetitive path planning and enforce optimization and safety of a trajectory. The above mentioned examples sacrificed some of the precision in trajectory following in exchange for the continuous movement. Our approach assumes that it is better to stop and recalculate a new optimal and safe path. The grid search offers great flexibility because we can control the precision and safety of the path by controlling the coarseness of the grid. We will now map some of the above requirements into standard artificial intelligence terms. Many grid map search algorithms have been presented in the past [11] and the general characteristics that are defined and that we want to enforce here are  a.  An ability to optimize the path.  b.  An ability to guarantee finding a solution if one exists.  c.  A superior speed and minimized memory requirements while satisfying the above requirements.  2.  Doing it in a way that would minimize negative environmental effects, namely, soil disturbance. In the light of recent studies of the impact of forestry operations on the environment, new regulations for forestry operations and public opinion on these issues, the goal of providing environmentally friendly procedures for heavy machine operations has become very  4  important (for example see The Alaskan Administrative Act in its section on Forest Resources and Practices). Soil disturbance is induced when tracked machines operate on steep slopes resulting in track slip. A tracked machine climbing straight up the hill would induce severe soil disturbance if it would be able to climb straight up the hill. The solution is to include switchbacking in the path when it goes over a slope. Sharp turns performed by the heavy tracked machines are another cause of soil disturbance. It has been recognized, from personal communication with Marv Clark of Forestry Engineering Research Institute Canada (FERIC), that the case when one track is stalled while the other one is turning is the most harmful and should be avoided. The turns where the tracks are moving in opposite directions are acceptable. We opted for this version. The long smooth turns, where one track is moving slower than the other one are also acceptable, though by enforcing the large turning circle some of the indigenous mobility of the tracked machines in potentially cluttered worksites would be sacrificed.  Some of the above objectives are obviously conflicting. For example switchbacking conflicts with the requirement for the shortest path. This problem is approached in two different ways in this work. A trade-off approach is used in the generation of the cost function for the search algorithm. Another approach is taken for dealing with few recognized, important and extreme work place settings, for example a steep uniform slope where a special optimized variation of the algorithm is used. Moreover, a design of the path planner is suggested that offers several different paths to an operator to choose from. This approach has another hidden benefit: there are, still, many situations and optimization issues where human judgement is irreplaceable. For example, an operator might foresee that a seemingly less optimal path will be the only one available during a session so he/she might decide to give it a preference and save some time.  5  1.3 Overview of the Thesis Work This thesis has contributed to knowledge in the following ways: 1.  The path planner is developed. It is based on the A * search algorithm with a novel cost function. The algorithm is based upon the existence of the elevation map of the work field that can be produced from satellite images. The paths are chosen to reflect the need to be as short as possible while avoiding to travel slopes.  2.  Multiple paths, with different weight distribution among the objectives, are provided by the path planner to offer an operator a choice. This offers a great flexibility for dealing with other factors in the work field (for example, moving obstacles) while preserving the ease of supervising the machine.  3. The modifications are devised for the search algorithm to enable the path planner to produce a suitable path when the machine has to travel a slope. The slope will be travelled in a zig-zag fashion, which reduces the power requirements for the engine and, also, reduces soil disturbance. 4. A simple control strategy is devised upon the fact that the path planner produces paths that consist of the links between neighbouring nodes in a regular grid. The path planner is integrated with a track control system on a mini-excavator and the ability of the system to derive an optimal path and to follow it precisely in a semi-autonomous mode is demonstrated.  1.4 Thesis Organization The thesis is divided into six chapters. The following Chapter 2 contains a brief overview of the background research on mobile robot navigation system.  Due to a huge amount of  work done in this field this chapter is organized into sections covering the robot motion  6  planning problem in general and practical algorithms with regard to specific areas of application, respectively.  Chapter 3 describes the path planning algorithm.  It goes through possible  alternative ways of achieving our goals and evaluates them. This chapter also includes overview of background material on the heuristic search algorithms which are the back bone of the path planner. Off line implementation of the path planning algorithm is described and the results of the test on a variety of synthetic terrain models are discussed. Chapter 4 offers the requirements analysis for our test system based on a mini excavator available in the Robotics and Control Laboratory at U B C . While being mostly focused on the actual test bed system, this chapter also proposes modifications of the requirements and design for transferring to an 'ideal' system that would eventually operate in real, industrial conditions. The word 'system', here, includes both hardware (sensors, actuators and computer system) and software (track control, path and trajectory planner and operators interface, all integrated into a real-time program). Chapter 5 provides us with the details of the implementation of the experiment on the mini excavator and evaluates the results. Finally, Chapter 6 offers concluding remarks on the results of the thesis work and suggestions for the future work.  7  Chapter 2 Background on Mobile Robot Motion Planning Systems This chapter provides an overview of both academic and industry related work, done previously in the area of mobile robot motion planning. The chapter is organized by sections. The first section covers the background material on the problem of motion planning in its most general setting. The most basic aspects of the motion planning problem are brought out and the classical literature on the issue is surveyed The second section in this chapter offers a very brief overview of the extensions of the motion problem from the first section. It considers a probabilistic approach to motion planning and the case of multiple robots. The third section in this chapter focuses on navigation in real-world, previously unknown, outdoors environment. This section is broken down into three topics dealing with submarine, aerospace and ground mobile robot navigation, respectively. The most prominent implementations are mentioned and briefly described. Final section of this chapter narrows our focus. It deals solely with grid map based path planning systems.  2.1 Mobile Robot Motion Problem Having a machine that operates autonomously, i.e. to understand high level task descriptions and to perform them correctly and efficiently, is the ultimate goal of robotics. The goal of mobile robot navigation goes along with the goal of the robotics in general. We would like  to have a robot that can change its position in space, with a purpose defined on a high leve without specifying the details of the motion. It is quite clear that this definition of our goal is  8  too general. To be able to study the problem we need to specify the terms from our mission statement more precisely, if possible, in mathematical terms. First, we usually describe space in the terms of our natural 3D space (and consequently we use three coordinates to refer to the position of the robot with regard to a reference point). Furthermore, orientation of the robot is often important so we might need three more variables to describe the robot's position in space. In some cases the robot's field of movement is presented in 2D, whether the actual field of movement is a flat area or the actual area is projected into 2D to simplify the analysis of the problem. Development of the theory of robot motion planning can be traced to the 1970's even to the late 1960s [12], [13], [14], [15]. It is interesting to point out that many of the early contributions were intended for use in the motion planning of robotic arms (which is actually the inverse kinematics problem) and then it became clear that the same tools can be used in motion planning of mobile robots. For example, some of the first considerations for the motion planning in general were brought out in Lozano-Perez's report from 1976[16]. The mathematical foundations for the theory were developed well back in the 1960's [17], [18]. The terms used in the area are summarized below by offering a variant of the set, suitable for describing motion problem solved in this thesis. They will be used in the further text. 1.  Autonomous mobile robot: AMR. It is usually considered to be a rigid body. In some instances it is represented as a material point. An additional case is an articulated robot which consists of more than one moving rigid body (link) that are connected by joins. An example of such a device is a robotic arm. A mobile robot can be a free flying object (it can move freely in space) or its movements can be limited by mechanical or any logical constraints. Mechanical constraints are commonly divided into holonomic and nonholonomic constraints. 9  Task Level y  Path Planner f  Trajectory Planner f  Actuator Control Figure 2.1 Motion Control Layers  2.  Configuration space: S . N  This represent the work field of a robot.  The number N  represents the dimension of the space so its value is usually 2 or 3. The positions and orientation of a robot in the space are defined with a properly chosen set of coordinates. Areas of the configuration space that are not traversable by a robot (i.e. that are forbidden) are called obstacles O . t  3.  Solution path: P. A solution path is a set of arcs that connect the start point to the goal point in a configuration space. An arc consists of two points from the configuration space and the line connecting them that is completely contained in the configuration space. The important thing to emphasize is that every arc in a path must not have any common points with any of the obstacles.  4.  Cost function: C. A cost function C  : (pi,Pj)j. —>  R  M  defines an M-dimensional vector  cost function for travelling a trajectory between the points p\ and P2 in the configuration space. The restriction that the arc cannot have any common points with any of the obstacles holds here, too. The diagram in Figure 2.1 shows the levels of a mobile robot motion control system and the position of path-planner in it.  10  The task level is responsible for defining goals, the path planner finds solution paths, the trajectory planner refines the path (at least it is accepted as the role of trajectory planner in this thesis) while the actuator control performs the lowest level feedback control that enforces the robot to follow the trajectory. Latombe [10] in his book Robot Motion Planning offers a comprehensive overview and survey of articles published before 1991 on the basics of the theory of robot motion planning. He offered following classification of planning approaches: 1.  The roadmap family of methods: These methods are based on the idea of creating a network of lines within the free parts of a configuration space and then searching the network for possible solution path. The most popular variants of roadmap methods are the visibility graph and Voronoi diagram methods. The difference between these methods lies in the type of the network created. a.  A 2D visibility graph is illustrated in Figure 2.2 The black areas represent cluttered  Figure 2.2 Example of visibility graph  obstacles and the network is created by connecting vertices of the obstacles to each other without crossing any of the obstacles. The sides of the configuration space can be considered as edges of the outside obstacles. It is important to notify that edges of the obstacles (or their vertices) can be part of a solution path. We can deal with that 11  by refining the path to move away from obstacles or by enlarging the obstacles. This leads to the conclusion that a special care has to be taken of safety while designing systems based on this approach. Given the start and goal position a standard graph search algorithm can be employed to find the shortest path. The obstacles don't have to be polygons. The extension of the method that included obstacles with circular edges was introduced in [19]. Though very simple the method is still used. Suggestions for improving the method are still being published, for example a method to smooth sharp angles by using B-splines is discussed in [20] In a Voronoi diagram the roadmap network is created out of curved lines that are placed in the mid space between the obstacles. This approach emphasizes the distance from the obstacles. A disadvantage here is that creating these lines is as not straightforward as with visibility graphs. The theoretical introduction of the method can be found in [21].  An illustration of Voronoi diagrams is in Figure 2.3. A related method to  Figure 2.3 A Voronoi diagram  the Voronoi diagrams is the freeway method introduced by Brooks in 1983 [22]. The method is based on traversing network of lines that are desired to be as far away of obstacles as possible as is case with Voronoi diagrams. The difference is in the technique for extracting these lines. The free space is divided into geometrical figures  12  and their mid lines are the lines in the network. Brooks considered the problem of orientation of a robot. He defined that a line is traversable if the robot can translate it with fixed orientation. The orientation is only changed at the intersections between the figures that consist the freeway. The method doesn't guarantee finding the solution even if one exists and its incompleteness is the major drawback of the method, c.  A generalized roadmap method that involves multiple robots and articulated robots is proposed by Canny [23]. Number of dimensions of the configuration space may be more than 3 in this generalization.  The cell decomposition family of methods: These methods could be defined in short as dividing a robot's free space into appropriate cells and then searching a path from the start to the final destination through a connected array of the cells. Searching cells for the solution path is performed on a list of cells with the information on connectivity of the cell with all the other cells. This list is often called connectivity graph (referring to this as a list is more related to software implementation of the algorithm while it can be considered a non directed graph for analytical purposes: one such graph is shown in Figure 2.6. The array of cells connecting the start and the end point is often called the channel and the exact path and sequence of robot's movements is defined within the channel as the next step in the algorithm. As should be obvious, this definition of method is very wide and lots of consideration should be given to every step in the algorithm which makes actual implementation of the algorithm a complex process. The first doubt encountered here concerns the guidelines for choosing cells. It is clear that the decomposition of the free space can be performed in infinite number of ways. The most common reasoning is to make the cells be convex polygons. That assumption allows most of the flexibility for choosing final path out of a channel: for example, in the case of a robot that could be 13  considered a material point a path among every two points from a cell is feasible. Final path can be found by connecting midpoints from boundaries of the adjacent cells from a channel. This method is illustrated in Figure 2.4. The obstacles are represented with dark  3  V  www\  \ 11  1  Goal / 1  6  '  Start \  > /  T^^/  1  / 8 /  ^ ^ - - ^ 9  W  1 ' / jl ; / \ ' I  J  Figure 2.4 Illustration of the cell decomposition method  polygons while the cell borders are straight lines. The solution path is made of the arrowed lines while the cells containing these lines form the channel. The above figure illustrates one possible way to form the cells: to connect the vertices of the obstacles (the edges of the configuration space are considered obstacles, too) with each other without having any intersection. It is clear that this process is not completely determinated and that it is hard to automatically implement it in an efficient way. One drawback of the method is clearly visible: if we want to implement division of the space in a high number of cells any employed algorithm would be very complex and inefficient (number of edges between cells would be big and their positions would need to be memorized, one at the time, which leads to extremely inefficient computation. On the other hand, too few cells leads to a solution that doesn't leave any room for optimizing the solution path. One attempt to deal with this problem of dividing the free space into cells in a well defined and automated way is the  14  vertical division method, introduced by Chazelle, illustrated in Figure 2.5. Following figure  Figure 2.5 Vertical cell decomposition  presents connectivity graphs for examples from above two figures. The node numbers here relate to those of the cells in Figure 2.4 and Figure 2.5.  Connectivity graph for the cell decomposition in Figure 2.4  Connectivity graph for the cell decomposition in Figure 2.5 Figure 2.6 Connectivity graph for examples from Figure 2.4 and 2.5  A good characteristic of the cell decomposition method is that the step of the algorithm dealing with choosing exact path within a channel can be postponed and performed in real  15  time so that additional sensors and reactive strategies can be employed. This leads to the idea that the method can be easily generalized to deal with certain kinds of uncertainty and optimization in terms of safety. The above statement is based on the assumption that a robot has freedom of movement within a cell (they are considered safe regions) and that it can freely modify its intended movement within one. For example it is possible to employ the proximity sensors to keep the robot away of obstacles as much as possible or to avoid a certain previously undetected and possibly moving obstacle by additionally translating and rotating within a cell. It is important to say that work published on this method covers both the 2D and 3D cases while the case where the configuration space is 3D is generally very complex and inefficient. An interesting concept of reducing the complexity by creating a 2D representation of 3D space for cell decomposition is introduced by Avnaim at al in 1988 [24]. Schwartz at al [25] offers a cell decomposition algorithm where configuration space is even more generalized to the algebraic manifolds level. Some of the material in this book was previously publisher in authors' series of articles 'On Piano Movers Problem'. The robot considered there is a rigid rectangle, and both translation and rotation are considered and the obstacles are polygonal (these are very common assumptions in all algorithms based on the cell decomposition concept). All the above mentioned variants have one thing in common: the configuration space is divided into strictly divided cells.  These algorithms are therefore called exact cell  decomposition based methods. Given the accurate information on position of the obstacles (or more precisely: on the areas occupied by them), a proper search algorithm (for defining the channel) and an intra-cell moving strategy the exact cell decomposition method will find a solution path whenever one exists. A similar approach to the exact cell decomposition is the approximate cell decomposition  16  method.  This family of methods emerged from the need to reduce complexity of the  cell decomposition of the free space of the configuration space. The cells are formed by iteratively spliting the configuration space into regular rectangles. An example of this is given in Figure 2.7. In this figure the first division is already done. The configuration  occupied cell j  i  semi-free cell free cell  Figure 2.7 Illustration of the approximate cell decomposition method  space is divided into four equal rectangles (the configuration space is a rectangle itself). Among the four cells formed this way only one is completely obstacle free. We'll call it a free cell. If any of the newly formed cells is a free cell or occupied cell (a cell completely covered by an obstacle) it is no longer divided. Cells that contain both the obstacles and free space are called semi-free cells. They are further separated into subcells in the same way as their parent cell until all cells belong either in the free or in the occupied class. Of course, it is easy to comprehend example in which such an division would go indefinitely so an error margin is set up so that the process of cell division is stopped after they have become too small. So, in practice, in the final stage we would usually have free space defined by free cells. The free space is usually separated from obstacles by a layer of semi-free cells. A trade-off is involved here: the bigger the margin cell the less time we will spend on division but at the same time we will increase the chances of missing to find  17  a solution path even though one theoretically exists. A possible final stage of the division process is shown in Figure 2.8.  o c c u p i e d cell  —  |  s e m i - f r e e  [  cell  free cell  Figure 2.8 Approximate cell decomposition  Even though the approximate cell decomposition doesn't guarantee finding the solution though theoretically we can almost always decrease the final cell size to find one. On the other hand the method is definitely more practical than exact cell decomposition because the complex and time consuming task of predefining the free space out of the configuration space is skipped. The process of defining cells, here, is also easier, too because they are regular polygons (usually rectangles) of defined sizes and countable positions. The potentialfieldfamily of methods: Arguably, the most used and researched methods for path planning emerged from need to have a path planning algorithm fast enough to be implemented in real time. The concept of using potential fields for robot guidance is introduced by Khatib in 1986 [26]. This method is closely related to searching a regular and dense grid of the configuration space. The characteristic that distinguishes it is that a specific heuristic is used to guide and speed up the search. That heuristic is that the robot is attracted by goal's attractive force and pushed away by repulsive force with an obstacle as the source.  18  The idea is simple: the goal point is assigned attractive potential that is usually implemented as Ktt(p) = Kd? {p) where K is a scalar and d al  goa  i is the distance from the current robot's  position p to the goal. The attractive force is then defined as Fait = — V K « ( p ) Obstacles are otherwise sources of the repulsive force. Their potential is in the simplest implementation represented as  Vrep = [ {d(p)  I0 K  ~i)  > (p)<do , d(p) > d  (2.1)  d  0  The repulsive force is defined in the same way as the attractive force. It is often case that the goal can be assumed to be a point. It is not the case with obstacles. That is way d(p) had to be further discussed: here, it is defined as the distance of the robot from the closest point of the obstacle (robot is assumed to be a material point, otherwise similar discussion for the robot's side would be necessary). This brings up a problem when an obstacle is of a nonconvex shape. In that case we can have more then one points on the obstacle with the same distance from p. This can lead to oscillatory paths. This can be overcome by representing the nonconvex obstacle by more that one convex obstacles. Potential field based methods produce safe solutions because the repulsive force tends toward infinity as robot gets closer to an obstacle and that is what keeps it away from them. A major drawback of the method is that it can converge toward local minima of the potential field. Implementation of the potential field approach to path planning is equally suitable for 2D and 3D though the visual representation is much easier to create and understand in the 2D case. An example is shown in figures Figure 2.9 and Figure 2.10. The start point in this example is at (0,0) (lower left corner of the figure) while the goal is at (30,30). Figure 2.9 presents an illustration of a field of negative potential. It is noticable that there  19  are three obstacles in the configuration space.  Figure 2.9 A negative potential field  Next figure Figure 2.10 represent orientation and amplitude of the gradient vector of the potential function from Figure 2.9.  20  1  i  •  i  •  25  20  15  '  ' I  • '  "  ,  , *  \ —•  i  •  is  •  * > .  - - -  - .  - • • i i • • - \ i  . . . . 1 1 I 1 • • 1 1 is— •  .  .  ~- s *  \  \  .  .  .  \  \  x  *  • I  - \  .  ,  , -  ,  - <  ,  .  ,  ,  .  .  11 I • • I is — \  \  \  \  \  ,  ,  < < . -  5  0)l 0  1 1  1  iI  I  I  I  5  10  15  20  25  30  Figure 2.10 Gradient vector of the potential function  The major problem related to using potential functions in the mobile robot path planning, the existence of local minima, was dealt with in, basically, two ways: a.  By creating the potential functions without local minima. potential functions introduced in [27] are local minima free.  For example harmonic The method is, also,  capable of dealing with obstacles of arbitrary shapes which is the second foremost problem associated with the method.  b.  By providing ways of escaping from a local minimum if trapped within one. The solutions here are found in different ways. Some of them are genuinely local methods i.e. if we encountered the local minimum and our robot stopped there after confirming 21  that the point is not the goal point, the robot is moved out of the local minimum in a, more or less, random fashion. This way of coping with the problem is simple but in the local minima cluttered fields the delay due to wandering can add up significantly. The other approach is to search for local minima and declare them a forbidden regions for the search phase. This was done by Poon in [28]. Similarly search for the solution can be done iteratively: if the solution path was not found in an iteration the local minimum encountered is marked and the search is repeated (for an example look at [29]).  There is another division of path planning strategies commonly found in the literature, for example look at [30].  1.  Reactive methods: these methods are based around the concept where information from various sensors obtained in real-time directly influence actuator actions on robot. The characteristic of these methods are summarized in [31] as suitable for dynamically changing environments but very unsuitable for any sort of path optimization. The behaviour of a robot controlled by these methods is typically decomposed into simple actions induced by a single sensor.  2.  World representation methods: Methods covered by this definition have similar structure that can be expressed in the following three steps:  a.  Collecting the data of the environment (world).  b.  Processing the data and building appropriate world model out of it.  c.  Path planing.  The third popular classification of navigation methods is the division on global and local methods, for example, it is found in [27].  22  1. Global methods relay on having a map of complete configuration space, with positions of obstacles and possibly with some other information used in finding the most suitable path. These methods guarantee finding the solution path if it exist but their computational complexity grows exponentially with the dimension of the configuration space.  Their  character allows path optimization to be employed in many different ways. It is obvious that this category is very close to the world representation methods. 2.  Local methods are evaluating possibility of traversing a point in the space locally and are driven toward the goal by the artificial intelligence potential function or some other heuristic strategy. These techniques, when employed, generally cannot guarantee finding the solution path. Their major advantage is limited amount of information processing and an innate ability to deal with uncertainty and moving obstacles. In each of these three categories there have been attempts to devise combined methods in  order to overcome problems related to a single approach. Most of those are done on practical, industry based projects and some recent and moderately successful ones are discussed in following sections of this chapter. In addition, it is important to notice that the above mentioned three categorizations are not hierarchically organized i.e. none of them is more general then another one. They exist in parallel though the first one is slightly less comprehensive but more detailed than the other two. One global method that has been considered from the very beginnings of research on robot motion planning but not often used as a basic way of finding the path in real time applications is the grid search. It was combined with some other techniques that would reduce the computation, like retraction methods where a mobile robot is supposed to choose among limited number of possible routes or was combined with heuristics that were leading it toward the goal but without guaranties that the path will be found and if found that it will be the desired one. Actually,  23  all of the above families of methods sorted by Latombe have been combined with a heuristic search algorithm in some versions. The problems related to using heuristic search algorithms were based on two things. First, search algorithms are generally time consuming and, second, their usage as a primary tool for determining the path assumes having a reliable and detailed map of the configuration space. The second reason can be analyzed on two levels: 1.  Lack of sensors which would be able to produce a map of configuration space with precise enough information to enable creating globally optimized paths based on it.  2.  Computation needed to post-process the map, even if we had it, into information required by the search algorithms would be huge, on top of the computational burden induced by an extensive search algorithm. It wasn't before the early 90's that computational resources capable of performing the  computations at sufficient speed were generally available and affordable. The next breakthrough was development in theory and practice of real-time image processing. The computer vision proved to be a suitable tool for building maps of the configuration space with enough information to build a path planner upon it.  2.2 Extensions of the Mobile Robot Motion Problem to Real World Applications The issues dealt with in the extensions of the mobile robot problem could be separated into three general classes: 1.  Inaccuracy of the sensors for obtaining the information about the configuration space information or of the low level robot's motion control system.  2.  Moving obstacles.  3.  Kinematic constraints.  24  The above three classes will be introduced in more details and the background research on solving them will be surveyed in the following three topics.  2.2.1 Inaccuracy of the world map and low level control in motion planning In the review presented in the previous section it was implicitly assumed that the sensor information, upon which the path planning strategy is built, is accurate and reliable. The reality is that the sensor information is not absolutely reliable and accurate and that fact has to be accounted for in any real world application of autonomous navigating theory. Another problem encountered is that we cannot assume that robot can be absolutely precise in following the desired route due to the limitations of the actuators and their control systems. Beside trying to solve this problem by improving above mentioned factors it is sometimes necessary to account for these limitations by placing additional requirements on the path planning system. In general, these are the areas where the uncertainty theory is being involved in solving the problems. The simplest way of solving such situations is to determine margins of errors both in control of movements and in the sensor obtained world model. The next step would be enlarging the obstacles in the model to account for that. This solution doesn't spoil the performance of the navigation system a lot but can result in not finding the solution path even if one exists theoretically. In that case we can possibly try to employ another movement strategy in the critical areas. This new strategy often involves more sensors and significantly reduced speed of movement. The above strategy is implicitly involved when path planning is followed by path refinement. In this technique we exploit our knowledge of the robot movement limitations (for example some robots cannot handle sharp turns well) and in the first place we try to generate a solution path free of such situations so that the uncertainty error, that is the basic for deciding how much  25  to enlarge the obstacles is reduced. But again, we can be in a situation where the solution path is not found even though it might exist. Generally, to achieve the highest possible rate of finding the path we need to combine more than one of these strategies in a trial-and-error fashion. To achieve most efficient results in terms of speed, an expert system with extensive knowledge of the configuration space and the limitations of robot's movement control could be employed. It would be responsible for deciding which strategy is to be chosen first to reduce computational. In recent years sensing technology has improved a lot, allowing the construction of world models of bigger areas with more information on the nature of the areas and positions of the obstacles. processing.  Most of the progress has been sparked by development in the area of image Satellite generated maps of vast areas have become available for use in the  autonomous mobile vehicle motion planning. A new technique for image processing emerged, based on wavelets, a family of functions intended to replace classical Fourier analysis in some cases [32].  Wavelets are particularly useful in the cases where it is important to have an  explicit estimate of the approximation error. They often offer faster and more explicit analysis of signals. But their usage is limited to the world model preprocessing phase and uncertainty treatment is still necessary in the path planning phase. An example of using the multiresolution character of the wavelet representation is found in [33]. The authors proposed a hierarchical path planning algorithm in vast areas. It starts the path planning based on the coarsest level of terrain representation avoiding 'rough' areas (areas with high uncertainty) and then refines the solution path on the smaller area with higher resolution of the representation.  2.2.2 The Moving Obstacles Problem A very common situation is when obstacles in the configuration space are moving. In that case it is obvious that the solution path, as defined in the previous section, is impossible to find. In other words, it is not possible to create a world model and find a stationary path  26  among the obstacles. The key concept to resolving these situations is based on combining the reactive behaviour into the path planning strategy and taking time into account as a new dimension of the configuration space. If the movement of the obstacles in the robot's workfield is predictable then taking the time into account can offer theoretical solution to the path planing. The trade off here is that the computational time and complexity of the algorithm increases with the number of moving objects and complexity of their movement pattern. Knowing that the computational complexity of the stationary case is already exponential in most cases it is clear that the problem can easily become practically unsolvable. In the case of obstacles with partially predictable movements (this often involves uncertainty theory), it is possible to devise a strategy that can guide robot through the configuration space. Adding additional sensors to create reactive behaviour is necessary. For example if we know that an obstacle is moving along a line with a limited speed we can plan the robots movement across the line as is the obstacle is not existent. By using, for example, ultrasound sensors, we can detect how far the obstacle is and the direction of its movement when we approach the line. Than we can either stop and wait until the obstacle is far enough away for the robot to cross the line or we can proceed immediately if the conditions are satisfied. It is not possible to guarantee finding a solution path in this case. From the simple example above it is obvious that this family of problems can easily become too complicated to deal with. With more then few of the obstacles moving in an unpredictable way the motion of the robot would ultimately become completely reactive. We would also need to consider how to swiftly move away from obstacles approaching a robot. It would imply having a robot with very high upper bound for speed and responsiveness, which is not the case in most applications. It should be clear that it would be almost impossible to even  27  consider a notion of path and its optimization in the case of a configuration space with many obstacles moving in unpredictable ways. The case where a configuration space is populated by multiple robots is related to the case with multiple obstacles. In the case where robots can communicate among themselves it is possible to devise a path planning algorithm that guarantee finding a solution if it is theoretically possible, given the positions of obstacles and initial position of the robots. The major drawback here is that as number of robots grows complexity of algorithms and computational time grow, too. The situation with independent robots must involve robots with partially reactive behavior and finding a solution path is, again, not guarantied.  2.2.3 The Kinematic Constraints Problem With regard to the path planning, a very desirable characteristic of a robot would be its ability to rotate and translate freely in a configuration space. Regarding rotation, the maximum freedom of movement that a robot can achieve is the ability of rotating on the spot. The absolute freedom of translating means that a robot can translate in any direction from its present position no matter how it is oriented in space. Many mobile robots don't even come close to achieving such a degree of mobility. It is said that those robots' movement is bounded by kinematic constraints. In the literature two examples are widely used and studied: autonomous cars and robotic manipulators with an arm consisting of links connected by joints [10]. An example of an autonomous mobile machine whose kinematic constraints are almost negligible would be autonomous helicopter. Tracked machines (such as excavators) have fewer kinematic constraints than most other mobile ground systems (such as cars). The tracked machines have the ability to turn on the spot. They can easily master minor obstacles. Their ability to travel over rugged terrain is surpassed only by that of legged robots, which are much harder to control and keep stable. On  28  the other hand, these advantages of the tracked machines are offset by the increased negative impact on the environment.  2.3 Existing Mobile Robot Navigation in Outdoor Environments Yet another classification of the mobile robot navigation systems exists. This classification emerged from the efforts to achieve practical implementations of the navigation systems:  1.  Indoor implementations include mobile robots intended for use in hospitals, warehouses etc. Many indoors mobile robot implementations were also developed for purely research reasons in laboratories. Due to the limited range and high predictability of the configuration space some of these robots achieved remarkable level of precision in mobility.  2. Outdoors implementations are the major topic of interest in this section. The development in this area was slower due to potentially higher complexity of the environment and problems related to finding suitable sensors. Indoor mobile robots can often make use of navigating techniques relying on marking the work space. For example, it is often possible to draw marks on the floor that robot is supposed to follow. Furthermore, solving the problem of robot positioning in an indoors environment is much easier to solve due to limited and controlled work spaces and possibility to install necessary infrastructure. In contrast to that, sensors for outdoors mobile robot implementation have to be able to operate in hard conditions (temperature, dirt etc.) and in uncontrolled and sometimes hardly accessible work spaces. That is why the sensing systems on outdoor mobile robots are more complex, more expensive then those found in indoor mobile autonomous machines, and most of all, their performance is usually not as good as that of the indoor sensing systems.  This  issue will be elaborated in more details in the fourth chapter (The Navigation System  29  Requirements and Analysis) of this thesis. The research efforts in the area of navigating outdoors autonomous mobile vehicles (AMV) is traditionally divided into three branches: a.  Underwater AMVs.  b.  Air based AMVs.  c.  Ground based AMVs.  Results achieved in these three areas are surveyed in the following three topics.  2.3.1 Underwater A M V s From the very beginnings of the development in the area of autonomous mobile machines underwater vehicles have had a very significant amount of work dedicated to them. It was, basically, due to two reasons. First, there has been a constant need for such vehicles. Some of the industry branches and government organizations, that can make full use of underwater A M V s are oil and gas companies and military. The scientific community, namely ocean scientists would also benefit from having "intelligent", autonomous, data gathering machines, able to explore very deep or very cold water, where having a human operator would be a very expensive or even impossible task. The second reason why the development of underwater A M V s was so rapid compared to the other two branches is in the fact that water offers easier and more predictable ways of reaching the goal position. A contemporary survey of previous work done on underwater A M V s can be found in [34]. The authors recognized several distinct areas of research: control (that includes low-level control and high level control whose part can be path planning), sensors, communication, energy providing systems and vehicle design. The problem of providing energy for long operations is pointed out as the most critical one at the time. Regarding control, the trend is to use more reactive based motion planning algorithms.  30  This is mostly because of inability to obtain reliable models of previously unknown wide, underwater, areas in real-time. Image processing techniques, used in aerial and ground A M V s , are not suitable for implementations in deep waters. The other, often used, way of guiding these machines is through teleoperation. The research subarea of sonar sensing and its applications has been studied and developed extensively and the results could be utilized for ground based A M V s which can use sonars as, usually, an additional sensing system. It has been known that the ultrasound based sensors can successfully sense obstacles, close to a mobile robot, that are missed by the image processing system or have appeared between two updates of the world map.  2.3.2 Air based A M V s The research in this area was mostly tied to military applications. In recent years other fields of potential use of these machines has been recognized: low altitude satellite systems for radio and T V transmission, resource inspection (crop mapping, forestry), law enforcement (suspect surveillance, tear gas deployment), transportation (traffic congestion monitoring)[34]. Among air based AMVs, developed so far, one common trend can be seen: they don't employ very complex or sophisticated path planning algorithms. This is due to the fact that their environment (air) usually doesn't require them. It is often enough to have some reactive, fast response, system for obstacle avoidance and positioning and navigation system able to guide them toward the goal in a straight forward way with detours due to the obstacle avoidance. An example of the aerial A M V s is the Tomahawk missile. One feature that has been used and studied extensively in aerial A M V s is the Global Positioning Satellite system (GPS). It can, also, be very useful in ground AMVs, too.  2.3.3 Ground based A M V s Developing an ground A M V is probably the most challenging undertaking. The configu-  31  ration spaces, here, are severely limited compared to the cases of underwater and aerial A M V s and frequency of obstacles is, usually, much bigger. Even in very controlled environments like a highway for "intelligent" cars it is clear that the interaction with the surroundings is more frequent than in the air or under the sea. Therefore hardware (sensors, communications, mechanical motion system etc.)  and software (path planning, trajectory planning, low-level  control etc.) requirements are more comprehensive and demanding. The set of research issues is similar to those of underwater A M V s and aerial AMVs: path planning, robotic control, sensing and mechanical design. The notion of mechanical design primarily concerns the locomotion system.  Due to the great variety of terrains that might have to be covered by a  mobile robot, different locomotion systems have to be considered. They range from wheeled or tracked robots, that are usually commercial vehicles equipped with sensing, control and reasoning systems, to the legged robots, built in labs, for extremely rough terrains. In this thesis our main concern are tracked machines. Possible applications of the ground based A M V s seem to be countless. They range from military vehicles, mining and forestry equipment to the vehicles intended for use on other planets or space objects. We will focus our attention to A M V s intended to operate in off-road, outdoor terrains while those intended for use in somewhat artificial conditions like marked highways will be mentioned when there might be a possible transfer of technology or just for a comparison. A very advanced work on ground based A M V s has been done in the Robotics Institute at C M U . Significant, often pioneering, research has been done in all aspects of mobile robotics: planning, control, sensors and their combinations, and finally in integrating complete navigation systems. During the NavLab II project an off road A M V is built upon a US military vehicle (Hummer). The vehicle is equipped with a standard reactive behavior based upon proximity  32  sensors but the most interesting feature is an algorithm implemented to build, in real-time, an elevation map of the surrounding. The map is built upon data from variety of sensors such as video cameras, lasers and sonars. The maps are sufficiently precise and detailed, and combined with additional sensors for supporting quick response behaviour for fast changing dynamic environments, they provide a basis for real time path planning with the possibility of path optimization. A way of incrementally creating an elevation map for rugged terrains is represented by Kweon and Kanade in [7]. The accumulative error problem is still significant and using a prior digital elevation map (DEM) obtained by some means, for example from satellite produced image processing, can reduce these errors significantly. Then, a map produced in real time would uncover recent changes and obstacles. The elevation information of a point is derived from the information on its distance. Therefore some other range sensors can be used to produce the elevation map. Two obvious choices are the laser scanner and sonar. Their use even results in some benefits in terms of computing time over stereo vision systems. The benefit is that the range is immediately read while in the case of the vision system we need to calculate it by the triangulating corresponding points from both images. Even though the path planner exists in this implementation, it is not a particularly elaborate one. The path optimization was not a primary goal. The path planner was based on the potential field method. The objective was to optimize the speed of the vehicle and therefore a lot of reactive behaviour is incorporated so the path is loosely followed.  Another interesting implementation is found in the MDARS exterior project. Its development began in 1994 [4]. The robot is a joint venture by the US Army and Navy. It is designed to autonomously operate (move and perform different tasks) in an unmarked outdoor environment. Beside reactive behavior it has path planning capabilities. To support its intelligent bahaviour multiple sensors were used for extracting particular features of the environment.  33  This partial redundancy in sensory equipment is found on most of the competent implementations of autonomous mobile robots. It is necessary, because of the need to balance different requirements for the sensory information (for example a need to have global path planning and very precise obstacle avoidance in critical areas at the same). The list of sensors sorted by usage areas is showed below:  1.  Positioning sensors: Differential GPS, Azimuth Rate Gyroscope, Vehicle Compass, Vehicle Pitch and Roll, Four Wheel Encoders, Steering Position Sensors, Accelerometer  2.  Obstacle Avoidance Sensors: Stereo Vision, Radar, Laser 4 Line Scanner, Ultrasonics  3.  Intrusion Detection Sensors: FLIR, Doppler Radar  It is quite obvious that this vehicle would be fairly expensive and that it would be hard to mount all these sensors an board if the intended usage area would be forestry, for example. In parallel to the research performed in North America, research on mobile robotics have been going on in Europe. The biggest research project up to date in Europe was ESPRIT II Panorama (1989-1993). The outdoors part of this E U project was based around the Finish national project called Outdoor Robotics [3]. The project was more industry more industry oriented which resulted in more selective use of sensory equipment. The design was intended to cover both visual global navigation and inertial sensor based dead-reckoning navigation. Another important characteristic of the Panorama perception and navigation system (PNS) is its emphasized modularity in both hardware and software domain [35]. Major modules are:  1.  Man Machine Interface (MMI). This module allows mission preparation and monitoring by remote operator and in critical situations even teleoperation. This is the only off-board based part of the system.  34  2.  Intelligent System Controller. Global path planning. It has certain capabilities for handling unexpected events and path planning.  3.  Localization and Navigation System is based on an inertial navigation module which was determining the vehicle's position and orientation by the beacons or landmark detection. The navigation included trajectory planning (it takes into account limits for speed, turning radius, main dimensions etc.) based on the global path and choosing proper piloting mode. The term piloting is explained below in more detail.  4. Sensing System included a color camera for visual recognition of roads, landmarks and beacons, a laser range finder for obstacle detection and an ultrasonic array for proximity sensing. 5. Piloting system could be calling low-level control system with several modes of operation.  a.  Piloting on a path  b.  Road following  c.  Object contour following  d.  Fine positioning and docking  e.  Tele-operated piloting  The safety monitoring with possible emergency stop is placed into this layer, too. The safety parameters, taken care of, are:  a.  Collision danger monitoring (proximity sensors)  b.  Stability  c.  Vehicle and control system condition monitoring  35  It is important to mention that the piloting on a path was intended to be the main mode of operation. It is following a reference trajectory inside a free space corridor which is basically a variant of the freeway method discussed earlier. With regard to this thesis work the most important feature found on the above implementations is ability to perform global path planning and the necessary prerequisite for doing so is having a world model (elevation map in our case). The alternative way of getting precise terrain elevation map is from completely aerial images. Recent advances in the geo-observation technology made it possible to obtain high resolution (lm) and precision elevation maps of rough terrains. To conclude, even though the path planning algorithm presented in this thesis is based on the assumption that we have the elevation map of the configuration space, building one with sufficiently high precision and resolution is still an open research topic.  2.4 Grid-map Based Path Planners The path planner in this thesis is a grid-map based one. That is why this section is solely dedicated to presenting the benefits of this approach (some of them were mentioned earlier) and the important background literature related to it. Having a grid map representation of the configuration space and searching it to find a path (it would consist of an array of adjacent nodes (or arcs) from the start to the goal node) was not considered as a sole method at the beginning of the practical mobile robot motion planning research because of the undeveloped sensing technology to create the map and to extreme computational demands. Nonetheless, these techniques have some obvious advantages, for example:  36  1.  Dense enough searching of the grid not only offers a solution path in most cases but also it can be optimized in variety of ways.  2.  The grid map represents the terrain in great detail while being in fairly structured and computer friendly format. For comparison, we just need to imagine how complex the data format would be for an equally detailed exact cell decomposition case.  3.  Many mathematical and software techniques associated with searching a grid map are widely researched in the Artificial Intelligence and Operational Research literature.  With the advancement in sensing technology (stereo vision, laser scanners and sonars) and computer performance these methods emerged as the methods of choice in some real world implementations. Even today the size of configuration space and required resolution make it impossible to apply certain search algorithms directly. As a solution to this problem a hierarchical approach is a possible choice. It is the usual approach to start the procedure at a low resolution grid, find a preliminary path, and then search areas along the path for afinertrajectory. Such an algorithm is presented in [36] by Benton at al. The intended application field of the algorithm is military. Besides the elevation map, use of other information per node was pondered such is the soil type, vegetation and weather. Some researchers have chosen to decompose the grid map into regions based on constant traversability over a region [37]. Then a search is performed over the regions trying to travel those with better traversability. The benefit here is that searching a region is less time consuming then searching the whole grid but on the other hand preprocessing time to form those regions out of the nodes has to be considered. These methods bear great resemblance to the cell decomposition methods. The common feature among most of the path planning algorithms based on a grid map search is the actual search algorithm. The following chapter introduces the problems related to the search and offers justification for our choice.  37  Chapter 3 The Path Planning Algorithm As mentioned earlier, research in mobile robotics is slowly widening its focus. Capable mobile robot implementations were previously reserved for military use and space exploration purposes.  Now, it is possible to foresee using mobile autonomous vehicles in many other  circumstances. It is obvious that A M V s would be very usable in dangerous environments ( disposing nuclear waste, firefighting, underwater exploration, exploring extremely cold areas etc.)  Some of these environments have, already been populated by teleoperated vehicles.  Teleoperated vehicles require constant guidance by an operator and the amount of information, that is being transferred constantly, is significant. The ultimate goal is to have a completely autonomous vehicle which would eliminate need for constant, low level, human guidance. It is also likely that use of A M V s in industry would reduce the number of errors and increase productivity as was the case with implementing robotic manipulators in many industry areas (as an example take car industry). One of the industrial areas seen as direct beneficiaries of implementing A M V s is forestry. In this chapter we will examine the path planning algorithm intended to be used in tracked machines in real world environments. The first section exposes and explains the requirements for the path planner. The second section in this chapter explains "the heart" of the path planner which is the A * graph search algorithm. The pros and cons are given for the chosen method. An alternative choice based on the multiobjective and multi-choice search algorithm M O A * is examined in detail and compared to A * . The third section presents the structure of grid-map and the graph produced upon it.  38  The fourth section presents the path planning algorithm, as a whole, including the preprocessing step. The final, fifth, section brings up some implementation issues for the algorithm, presents the test results and comments for the off line implementation.  3.1 Requirements for the Path Planner The requirements that our system is supposed to fulfill, with regard to its mobility, can be categorized into two broad classes. First, It has to be able to find a path from the start position to the goal position. The objective is to simple find a way among the obstacles is further refined, or it is better to say restricted, by the requirements to achieve superior efficiency and safety of operations. Second , the chosen solution path should minimize soil disturbance that is unavoidable when operating heavy machinery in outdoor environments. The requirement is also further divided into well defined objectives, which allow us to devise suitable parameters and metrics for each objective so that we can develop mathematical algorithm that optimizes these parameters. From the above introductory statements it is clear that the algorithm is optimizing the solution path by multiple objectives.  It is a very natural thing, knowing that the vehicle  is supposed to operate in a natural, real-world environment. New generations of intelligent systems developed by humans to serve humans have to be intelligent enough to consider the environment as a partner and not as an enemy to conquer. Among the objectives there are obvious priorities. For example, the highest priority is held by the objective to actually find a safe path among the obstacles. The rest of them just help distinguish between the possible solution paths. This property is reflected in the algorithm structure. 39  Another fact worth pointing out at this point is that some objectives are mutually contradictory while some others are partially redundant. This fact influenced the solution significantly, too. An important assumption made here is that we have the world model in the form of the elevation grid map. This form is chosen because it offers a fairly detailed, yet simple, world model which provides a basis for the path optimization with regard to chosen objectives. It offers information on changes in altitude within a configuration space which is, as will be clarified in this chapter, important factor regarding the environmental concerns. It also offers an easy way to calculate the length of a path. The grid map is then transferred into an undirected graph (this means that travel is possible in both directions along an arc if it is possible at all) with a cost assigned to every traversable arc in the graph. The following topics contain detailed descriptions of the objectives, their position in "the big picture" and references that led to their acceptance as objectives.  3.1.1 Obstacle Avoidance The primary task of a path planner is to provide a path from a start node to a goal node. Obstacles that populate the configuration space have to be avoided. Things to be considered here are:  1.  Even though searching a grid map for an array of nodes (or arcs) from the start to the end point implies the representation of the A M V as a material point the dimensions of the machine have to be accounted for. Our vehicle has significant dimensions and they have to be considered in combination with the grid density to declare whether a path is a solution path.  40  2. Obstacle declaration has to be defined in accordance with the size and nature of the vehicle and terrain.  In our case we are dealing with big tracked machines so smaller objects  and roughness in terrain might be neglected. On the other hand it is known that tracked machines impose harsher soil disturbance on rougher terrains than on smoother ones. This is an example of the objectives overlapping their areas of influence. 3. Other special type of obstacles that have to be pointed out and avoided are areas where there are no men-made or natural obstacles but the terrain is not traversable due to some mechanical constraints of the machine. In our case those are steep slopes where a machine becomes instable (tends to flip over). The other constraints, such as turning radius are rare due to the nature of tracked machines. They have the ability to turn on the spot. Although this is a desirable feature of an AMVs, it is a drawback from the environmental point of view. While turning on the spot, soil is being disturbed more than during other motions.  3.1.2 P a t h Efficiency There are several different, commonly used criteria for path efficiency. Namely, they are the path length, the time required to travel a path, and fuel consumption. These criteria are sometimes used in parallel but in most cases there is a correlation between these three. Intuitively, it is likely that the shorter a path, the less time and fuel will be required to traverse it.  Of course, it is possible to think of a situation where this is not the case. One simple  example would be a path with lots of area where a machine moves slowly or even needs to stop (especially if equipped with some reactive behavior). Here, the path length criterion is chosen. The reason behind this decision is that the metric is simple and it is possible and easy to precisely calculate the path length for a given path. It is not always possible for the other two criteria because machine's velocity, possible stops and other factors have to be accounted for. 41  The time required for traveling a distance and fuel consumption are also very dependant on the specific make and model of the vehicle that an A M V would be built upon so it would be necessary to calibrate the metrics for every implementation.  3.1.3 Soil Disturbance It has been known that using heavy mechanization can induce great damage to the soil. Abeels [38] attempts to categorize the types of the damage. There have also been attempts to implement techniques for reducing the damage. Scandinavia [39].  This is an especially noticable trend in  Traces of recognizing the problem can be found in many forest practice  acts and guides in North America, too. The recommended solutions range from recommending reduced usage of tracked and wheeled forestry machinery in favor of helicopters and skylines to recommending good engineering practices and encouraging road building so that off road operation is limited to a minimum. Some types of soil are recognized as being more prone to damage so that it is suggested that such areas have heavy machinery traffic reduced to an absolute minimum. The soil analysis goes beyond the scope of this thesis. Nevertheless, excavators, feller-bunchers, tractors and similar machines are still widely used off-road and the problems associated with it have to be considered. When operating a tracked vehicle in a forest (for skidding operation as an example) two situations are recognized as the most harmful. 1.  Going over slopes and specially going straight up or down a slope. The Alaskan Administrative Act in its section on Forest Resources and Practices requires an operator to outslope skid trails where feasible. It also bans operations on a slope if it is likely to cause damage due to the soil saturation or some other soil condition. The slopes are also more vulnerable to soil damage because waters can wash the soil down if it is damaged. The influence of waters is less harmful on a plain area.  42  2. Making turns. When a tracked machine is turning on the spot it is clear that the ground under the tracks is exposed to prolonged shear forces acting from different directions which leads to more damage then would be induced by soil disturbance from translational traversing. The sharper the turn is, the more damage is imposed so the paths with less sharp turns are preferred. The case where one track is motionless while the other one is turning is a specially harmful one. An explanation of why an excavator induces more soil disturbance when going straight up the hill can be intuitively seen from Figure 3.1. The center of the mass is projected at the small  5  1 — tracks 2 — boom 3 — stick 4 — bucket 5 — operators cabin Figure 3.1 An excavator on a slope  area at the back of the tracks which causes damage while the tracks are rotating. A partial remedy to this problem is having the tracked machine climb gradually in a spiral or zig-zag way to the top of a hill.  Including this reasoning into the path planner would  also account for mechanical limitation (power) of a semi-autonomous vehicle to climb steep slopes. Besides path planning, this problem can be attacked by shifting the center of mass.  43  Alaskan Forest Resources and Practices suggest using puncheons filled with water for these purposes. A similar effect could be achieved by properly positioning boom, stick and bucket on an excavator, for example. These techniques also enhance stability of vehicles. Forestry codes across the globe suggest applying these rules when designing roads. Moreover, if a road exists and is traversable, an A M V s navigation system should accept the road as the most desired path between any two points on the road.  3.2 Graph Search Algorithms Graph searching has been a widely used tool for solving complex problems in many different areas for many years. The research fields that developed and utilized most of the search algorithms are Artificial Intelligence (Al) and Operational Research (OR). Researchers in these two areas developed many of the results in parallel and only recently has there been attempts to coordinate their efforts [11]. Search methods are also found in many applications commonly used by almost everyone who is connected to the modern technology and computing. An example would be a routing system in modern computer internets. In the Artificial Intelligence literature two criteria have been recognized as important when evaluating a search algorithm. Those criteria are convergence (ability to find a solution if one exists) and computational complexity (with the closely related execution time). These two criteria are very important for us because our path planner is supposed to be embedded into a real-time system. Another desired feature of a search algorithm is the ability to optimize solution paths. In the previous chapters we defined our requirements for the path planner so it is clear that this ability to balance between conflicting objectives is a crucial one. Among numerous search algorithms for searching 2-D problems A * is chosen to be used as a basis for the path planner. The A * approach is chosen because of its popularity in the off-  44  road route planning literature ([36], [37]), its well analyzed structure and characteristics ([18], [10], [8], [9]) so that the specific constraints could be easily incorporated. The reason for its popularity lies in its simple yet robust structure. It belongs to the heuristic search class (this means that different heuristic strategies can be combined into the basic structure to improve the speed of the algorithm). The algorithm starts at the start node and then iteratively checks surrounding nodes for their accessibility. When a traversable (accessible) node is visited for the first time a backpointer to the previous node and the cost of the path, so far travelled, is associated with it. Each time a traversable node is accessed from one of its neighbours the new path cost is compared with the old favourite and if it is less than the old path's cost the backpointer is redirected. The cost function associated with the path to node N is expressed as:  f(N)=g(N)  + h(N)  (3.1)  where g(N) is sum of the costs associated with the arcs leading from start to the node N and h(N) is a heuristic estimate of the cost from the node N to the goal node. A * search algorithm guaranties finding the cheapest solution path (i.e. it converges) if at least one exists or otherwise reports the failure, if following condition is satisfied [8]: 1.  The graph G modeling the configuration space C is finite. Note: It is also possible to prove that the solution would be found for an infinite graph after certain conditions are fulfilled but that discussion goes beyond the scope of this work.  2.  The arc costs are non-negative:  Vpair of neigbouring nodes(m,n) £ graphG : (3.2)  c(m, n) > 0 3.  The heuristic function has to be admissible 3:  \JnodeN £ graphG : h(N) < h*{N) 45  (3.3)  , where h*(N) is the real value of the h(N) The second part of the inequality 3 The A * search is illustrated here on a trivial example from Figure 3.2.  C (goal) 1 *  F \ (start)  Figure 3.2 The A * search illustration  For this example h(N)=0. At the start point A is open ('open' node is a node that is visited, it becomes closed when all of its neighbours are open) and f(A)=0. In the next cycle of the algorithm B and C as A's neighbours are open. Their backpointers point to A and f(B)=l  and f(C)=1.41.  A is closed.  The next step is to further expand open nodes. C is the goal point so it needs no further searching. B can be expanded toward C and the cost of that path would be 1+1. This is more than f(C)=l.41.  so the C s backpointer is not switched to B. B and C are closed.  The set of open nodes is empty now, and we backtrace the solution path from the goal node to the start node which is the line CB in this trivial case. The time complexity of A * is 0(n ) 2  if the candidate nodes for the search expansion are  not sorted by values of the/(A ) function. If they are sorted the complexity is 0(rlog(n)) where 7  r is the number of arcs of the graph). This is, generally, equal or better then the rest of the search algorithms solving similar problems. The memory storage requirements of the A* search algorithm are relatively modest. We need to store information on the graph (an array of nodes and with identifications of their neighbours, costs of the traversable arcs connecting each pair of adjacent nodes, a backpointer  46  and the cost of the path associated with that backpointer. The storage needed for this is a linear function of the number of nodes. We also need information on the nodes that are candidates for the search expansion in every iteration which is also a linear function of n. So, the total memory requirement is a linear function of the number of nodes n. As is explained in the following sections of this chapter we have customized and applied the A * algorithm to successfully meet all the requirements set up in the previous section. A C-like pseudo code of the algorithm is presented at the end of Topic 3.4.3 after the solutions to the requirements are presented.  3.2.1 A n Alternative Search Approach As mentioned earlier, we are trying to optimize the solution path by more than one criteria. The great majority of all path planners designed in the past optimized the path by only one criteria (it was usually the shortest path length or the time required to travel it). There are, basically, two approaches to take into account for multiple objectives:  1.  The approach based on devising a single cost function that reflects influence of all the objectives. The most usual form of this cost function is a weighted sum of costs, each associated with a single objective [40].  2.  The multiobjective A* (MOA*) search. This is relatively a new research area [41]. This algorithm optimizes the solution path search by multiple criteria and generally offers more then one nondominated solution. There have been very few attempts to use this approach in robotics [42]. Even though some of its features look promising the problems related to the implementation are still overwhelming.  The MOA* is performed upon a graph where the cost assigned to every traversable arc of the  47  graph is a vector in the form given in equation 3.4.  C(i,j) = [c  hj  c  2iJ  ... c ] kiJ  (3.4)  where k is the dimension of the vector i.e. number of objectives. Therefore the cost of a path from the start node to a node N is:  (3.5)  i.e. the cost vector is sum of all the vectors along a path /. A path / is nondominated when its associated cost vector from 3.5satisfies following relations 3.6.  {path £ {nondominated}) q  (3.6)  In English this would be translated like: if a path to N is not worse by all the criteria than any other path to N then that path is nondominated. The MOA* works in similar way as A*. It starts at the start node and spreads to the neighbouring nodes. The major difference from A* is that we need to keep track of all the nondominated paths already created to the node N. Moreover in the case where a newly found nondominated path has made some of the previously nondominated paths to N dominated the backpointers have to be removed selectively all the way down to the start node. The final solution usually contains a number (possibly a large number) of nondiminated paths and further selection strategy and computation is necessary. For the detailed outline and theoretical analysis of the algorithm look at [43] and [41].  48  3.2.2 Summary of the Characteristics of M O A * The benefits and drawbacks of using these two approaches are explained below. The A * search was, again, used as an reference point in the discussion. It was taken as an representative of the variety scalar-cost algorithms available.  MOA* Search 1. The MOA* search algorithm offers a mechanisms for dealing with multiple and conflicting objectives.  All the objectives are considered in an explicit form and none of them is  sacrificed in any way while producing an initial set of nondominated solutions.  The  objectives are used in their original form so there is no need for a preprocessing step in which they would be combined. On the other hand, MOA* provides us with the potentially large number of nondominated solution paths which implies the necessity of having further nontrivial processing steps in order to pick the reduejd set of solutions (normally only one path is chosen at the end). This wouldn't be necessary if a global solution exists (one that is best by all the criteria) but in that case a single objective search optimized by any of linear combination of the criteria would suffice. To solve this two approaches are possible. Either one objective is declared dominant or the initial solution set is reduced by defining a range of desired values for some objectives. 2.  A very important argument in favor of MOA* is that it can explicitly answer the question of type: Show all the paths that have objective x < a and objective y < b. The solutions satisfying similar inequalities can be selected from the set of nondominated solutions if they exist. This is one way of limiting the original set of solutions.  3.  Major disadvantages of MOA* are its excessive demands for memory storage and computational power. Due to the need to store information in every node on every discovered nondominated path to that node the storage required is not the linear function of the number  49  of nodes. Determining the number of possible nondominated paths in a node of a graph is a complicated process if we approach the graph with all its limitations and features. Regarding the nature of our system it is reasonable to expect it to work well for the worst case situation or any other specific one. That is why the analysis of the storage requirements is performed on a special case.  From the combinatorial theory it is known that the number of arcs r in a graph with n nodes is of order 0(n ) 2  in the worst case. The graph theory has proven that number of  possible paths in this graph is exponential function of n: n where p>2. In the special case p  where all the cost vectors are the 0 vector, we would end up with the situation where we need to store information on n possible nondominated paths in every node. p  It is obvious that the demand for storage becomes big for a relatively modest grids of 30x30 points.  It is possible to attack this problem by artificially limiting the maximum number of candidate paths in every node because in many situations maximum number is truly less than the theoretical maximum. This in exchange implies performing a complex preanalysis of the graph and introducing new heuristics that would select the information to be saved in the case when the maximum storage is filled and new information is available. This approach will possibly to produce failure even when a solution path exists and an expert system is needed to deal with it. All of this would make the analysis and implementation even more complex.  Analysis and software implementation of the algorithm is a significant task. It requires much more effort to analyze and properly implement the multiobjective algorithm than its  50  single objective counterpart. For example, in A* it is enough to keep track of a single predecessor and at the end the solution is backtraced from the goal node to the start node. In M O A * we have multiple possible backpointers from a node but it is not enough to update only them. In the case where a new nondominated candidate path has emerged all the paths in the node's list that became dominated by the newcomer path have to be removed. It is obvious that none of two above presented options satisfies all the requirements for our path planner. MOA* seems to have some indigenous features that make it a natural choice for a multiobjective problem but its notorious computational demand and complexity make it unsuitable for implementation in real-time embedded applications even for the grids with relativelly modest size (up to 10000 nodes). During our experiments for utilizing the M O A * algorithm as an alternative path planner, its negative characteristics were fully experienced and they completely overshadowed the benefits. The memory occupied by the program was more then ten time bigger than for the A * based search. It was about 6 Mbytes comparing to 35 Kbytes. This number was observed when artificially limiting the number of paths in a node to 900 for a 30x30 grid. This kind of limitation is a heuristic, but in most cases it didn't cause the search to fail. The optimallity could be guarantied only if there were no candidate paths discarded in a node. The other heuristic that was tried on both M O A * and on our A * based algorithm was to limit the number of nodes that can be open in one step. The results in this case were even worse. In about 15% cases the M O A * algorithm failed when a solution path existed. Another point worth mentioning when talking about our experiments with M O A * is the dynamic memory allocation. As already mentioned, the number of nondominated paths in a node could be huge. If we reserve memory for it, most of the time it will be unused. The dynamic memory allocation seemed like a natural solution but it wasn't like that. The system calls associated with the huge number of 'malloc' 51  calls introduced too large of an overhead and additionally slowed down the search (already more then 10 times slower then A * for a 30x30 grid). An additional problem was that there were cases when the operating system couldn't allocate the required memory space. On the other hand, a scalar-cost search algorithm is much more suitable for real time embedded implementations, where speed and memory resources matter, and, enhanced with the added features, produced desired multiobjective results.  3.3 Structure of the Gridmap and The Derived Graph The A M V s configuration space is chosen to be of a square form. It consists of 30x30 points where every point is equally distant from its neighbours. This structure is simple and easily described in terms of computer programming. This grid map is assumed to be provided by the sensor system. Every point in the map can be identified by 2 coordinates (x,y) which represent rows and columns, respectively. The grid map is accompanied by the graph in the preprocessing step. Each arc in the graph is assigned a scalar cost c(m,n) where m and n are the nodes bounding the arc. The arc is directed from m to n. The grid map and the associated graph are represented in Figure 3.3 with the size reduced to 4x4 points.  (1,1)  (1,2)  (1,3)  (1,4)  Figure 3.3 Structure of the grid map and the graph  52  As is obvious from the above figure, a node can have the maximum of eight (8) neighbours and corresponding arcs and costs. Due to the nature of requirements arcs are symmetric in terms of the associated costs. The requirements reflected in an arc cost are the arc length and the elevation difference along the arc. It is expressed in Equation 3.7.  c(m,n) — c(n,m)  (3.7)  3.4 Design of the Path Planner The path planner is divided into several subsystems or layers. Figure 3.4 shows block diagram of the complete path planner's functionality. Four layers of the path planner (numbered 1 to 4 in Figure 3.4) are explained briefly below. The remaining two squares (presentation of the results and informing on failure are very simple and can be considered parts of the search and the alternate path planning layers, respectively). 1.  This layer is in charge of setting up the resolution of the grid, type(s) of the cost function and possible other operators inputs. Some of these control variables might be, theoretically, obtained by analyzing sensory input but not at this stage of technology development.  2.  The second layer transfers sensory input into the data form used by the third layer. The input to this layer is the elevation map and the parameters set up in the first layer. The output is the costmap of the grid of the configuration space..  3.  Search algorithm uses parameters set up in the lower two levels (start, goal, value(s) of cost functions for arcs in the graph. It optimizes the solution path(s) in terms of the path length and soil disturbance in the way described in the topic titled The Search Algorithm Design, below.  53  Start  Elevation map from the task that processes the vision signal data or a syntheticaly generated elevation  am  >  Algorithm S e t - U p a n d O p e r a t o r ' s Input  Cost M a p Generation  2  S e a r c h Algorithm  Yes  Present solution in a s u i t a b l e f o r m  H a n d l i n g Critical Situations  The End  Yes  Inform a b o u t t h e failure  Figure 3 . 4 The complete path planning algorithm  4.  The last layer is a subsystem is designed to deal with emergency responses and the situations where a solution is likely to exist but due to the resolution of the grid and the safety concerns cannot be guarantied. Functionality of every layer is presented in detail in the separate topics that follow.  3.4.1 The Algorithm Set-Up and the Operator's Input At this level the several key parameters are decided.  54  First, the coordinates of the start node  (x  t,  star  ystart)  and the goal node (x [, goa  y  g o a  i)  are  defined. As previously pointed out, the A* algorithm itself optimizes the path by a single objective. In the requirements analysis it is concluded that the objectives to travel the shortest path and to avoid slopes are mutually conflicting. Besides that, the desired means of dealing with slope-traversing differ from situation to situation. The factors influencing the choice of the strategy for dealing with slopes are the degree of a slope, terrain type, weather conditions (wet ground is more prone to soil disturbance) and many more factors related to the specific conditions and operator's goals. The problem is attacked here by calculating more than one cost for every arc in the graph. The weighted sum method of dealing with the multiple objective is employed besides internal modifications of the search algorithm (these issues are described in detail in two topics following this one). The different effects are achieved by balancing the weights in the cost function. The maximum number of weight combinations is chosen to be six. This number allows us a wide range of choices that are likely to be suitable for every situation. On the other hand, time required to perform a search is still less than one required for M O A * . The storage requirements are much better here than for M O A * . The storage requirements grow somewhat less than 6 times from a single cost A * while M O A * would require n times more memory. For example in the case of a 30x30 points grid map the memory required is more than 100 times less than for 2 objective M O A * search. The actual experimental results and the numbers that support the above statements are provided in Section 3.5 (on the off-line implementation). Postprocessing step to chose the most suitable path is much simpler. The best path is chosen from the set of six paths with predictable characteristics. This can be done by the software itself. It would be done at this level (the highest one) and the expert system would  55  be guided by information on the soil type, roughness of terrain and weather conditions. The alternative solution is to have an operator deciding in the final stage among the six choices. This is a desirable solution because of the complexity of the objectives and possible dynamic changes in objectives in the middle of the path planning process. It is necessary to distinguish this mode of operations from an teleoperated mode because an operator would only pick final solution from a small and optimized set of potential solutions and would eventually supervise the path following process. This mode requires significantly less efforts from an operator than it is the case with the classical teleoperated mode where an operator devises a path and guide a robot along it. The step is represented in the diagram in Figure3.5.  Figure 3.5 The search set-up and operators input  To summarize: The goal and start positions are defined as well as to whether there will be a default solution path among the possible six candidates. Here, it is also determined how 56  many different sets of costs for a graph will be. The maximum number is 6 and the minimum is, of course 1. This number determines how many searches will be performed and, therefore, from how many options an operator will be able to chose. In the automated mode the operator wouldn't have to pick a path at the end because a default choice would be established if operator's input hadn't arrived in a specific time.  3.4.2 The Cost Map Generation The cost function associated with every traversable arc in the graph is a scalar calculated by the following simple Equation 3.8:  . c{m,n) = \\arc-length  + A2  elevation_change : : arc-length  (3.8)  The arcjtength and elevation_change variables are defined in equations 3.9 and 3.10.  arc-length = yj(x  m  - xf  + (y  n  m  elevation-change  where (x , y ) and (x , y ) m  m  n  n  - yf  + (z  = \z  n  m  m  - zf  (3.9)  n  — z\  (3.10)  n  are coordinates of nodes m and n, respectively. z  m  and z are n  their elevations. A clarification is needed for the notion of arc_length defined in Equation 3.9: The equation calculates this distance between the two neighbouring nodes as if they were point on a ideally smooth plain in space. This of course is not true but given the targeted maximum distance of 1-2 meters between the points and the size of the excavator tracks (>2 meters) and the fact that any thin but tall object would be characterized as an obstacle this can be accepted as a good approximation. 57  Now,  let's look at the core character of the cost function. It is basically a weighted sum  of two elements: 1.  \iarc_length  directly reflects our objective of travelling the shortest possible path and  doesn't require further explanation. 2.  A2 arlThngth eleV  9t  r e  A  e c t s  the requirement to avoid going up and down slopes if it possible  to reach the goal just by going through valleys. In the case when it is necessary to clime the slope (if goal is on the top of one, for example), due to the fact that the A * is optimizing the ratio (elevation_change/arc_length),  this cost element will favoure path that reaches  the goal climbing a hill in a spiral fashion (the ratio is going to be less that way then if the A M V  climes up the hill in the shortest way). If the slope is not a rounded hill but a  width-limited inclined slope the path would turn out to be a zig-zag path. The angle under which the path is inclined with regard to going straight to the top or staying at the same altitude (which is not possible due to the fact that A * used without heuristics guarantees reaching the goal if traversable path exists) is 45° due to the fact that the local movement of the A M V  is limited to the neighbouring points in the graph. Additional requirements  like reduced number of sharp turns and other refinements of the solution path are done by modifying the A * search as explained in the next topic.  The  influence of the above presented elements of the cost function are balanced by two  parameters A7and A2. They satisfies relations in 3.11:  0 < Ai,A < 1 2  (3.11) Ai + A = 1 2  As the influence of A; increases, one of A2 decreases and vice versa. In the case where A; = 1 we have a case of the classic shortest path optimization.  58  The program accepts information on the number of combinations of  (A/,A ). 2  All the cases  are represented in 3.1. N  (A.,A )  1  (1,0)  2  (1,0) (0, 1)  3  (1, 0) (0.5, 0.5) (0, 1)  4  (1, 0) (2/3, 1/3) (1/3, 2/3) (0, 1)  5  (1,0) (3/4, 1/4) (0.5, 0.5) (1/4, 3/4) (0, 1)  6  (1, 0) (4/5, 1/5) (3/5, 2/5) (2/5, 3/5) (1/5, 4/5) (0, 1)  2  Table 3.1 Possible combinations of (XiM)  These particular combinations for any N are chosen because they are easy to automatically generate only based on N and they offer intuitively good coverage of the all possible combinations for N > 4.  It would be possible and probably useful to have other combinations of  (A/,A ) 2 /  for the  cases where N<3. A reasonable choice for N=l would be, for example, (0.75, 0.25) because it works well in most cases as will be presented in the next section. The purpose of having different choices for N is being able to reduce the computational time in the cases where the suitable weights combination can be predicted given the conditions. In general, it is more likely that a truly superior path (this may be a subjective decision by an operator) will be found for a bigger N. In our implementation N is limited to 6 which represent an engineering trade off: we wanted more choice but at the time we wanted to keep the execution time under control. Another concern is that in a case with N growing significantly more then 6 the screen would become too cluttered.  3.4.3 The Search Algorithm The search algorithm performed here is based on the modified and customized A * search  59  algorithm performed N times. Of course, care has been taken of efficiency so some parts of the calculations that are common are performed only once. These issues will be brought up in the next section, when the actual implementation is discussed. The modifications of the classical algorithm were targeted toward minimizing number of sharp turns in the path. Due to the nature of the graph, the A M V s next travelled arc can form an angle of 45°, 90° or 135° with the previous arc or can continue without turning. The case of going back (after turning on the spot for 180°) is automatically eliminated because of the fact that arc costs are greater or equal to 0. The idea is to avoid specific path refinement i.e. smoothing the path. In that case a sharp angle like 135° would be achieved by the A M V with a significant amount of turning on the spot which is undesirable. On the other hand the angle of 45° is easily mastered by the low level track control with less soil impact. This requirement is divided into two parts: 1.  Banning sharp turns. The idea here is to give this requirement the highest priority. The constraint of avoiding sharp turns is achieved by dividing the search process into three consecutive steps (that share some costly initialization operations): First, the angles of 135° and 90° are banned. By doing this we are basically reducing the connectivity of the graph. This is done by detecting new 'run-time' obstacles. Whenever a new optimal path is discovered to a node it is checked whether it contains the sharp angles with its two predecessors.  If it does  the 'obstacle' is encountered and the backpointer is not switched. This custom designed heuristic spoils the A * indigenous convergence i.e. it is possible to declare a failure even if a path exists. This will only happen in a small number of cases where we have narrow (one node wide) channels containing sharp angles on every possible solution path. In those cases the first search attempt will fail and new one with the constraints reduced only to  60  banning 90° angles is performed. The possible next step is a search with no constraints which would produce a solution path, if it exists at all, so the algorithm convergence is not affected. Due to the fact that the percentage of success at the first search is high the efficiency of the algorithm is not much deteriorated. The second problem to be solved is minimizing the number of turns in general. The purpose of this requirement is very clearly visible when we are supposed to climb a bounded slope (on the sides). In terms of the path length the paths in the Figure 3.6 are the same (given the fact that the turn angles are equal). According to our requirements the left path is  Figure 3.6 Minimizing the number of turns  clearly superior. In the algorithm the right choice has been enforced by employing one more test at the step in the algorithm where a new path to a node is discovered. In the original A * algorithm if a new path's cost is the same as the previous best, the backpointer is not switched. In this case, if the costs are equal, the number of turns is compared and the backpointer is switched to the neighbouring node that is on the path with fewer turns. This feature does not affect convergence of the algorithm because, the change does not deal with the main cost function and when the paths (backpointer in the node) are switched the procedure performs in the usual way: a potential solution path can be formed from the current node to the goal node and it is not affected by the part of the path leading from the start to the current node. The search algorithm is described in the following block diagram (Figure 3.7). 61  The area  Initialize set 'open' to 'empty'. Define proper turn constraint  I  Place the start node in 'open' No Yes  Extract next node for expansion from| 'open'  Solution available Ml  Declare failure Ml  Figure 3.7 The search algorithm block diagram  enclosed in the dashed box is performed for every node nprime, added to the set open, when a node n is expanded to its neighbours. It means it is performed for up to 8 times in every cycle of the main loop (when new n is chosen).  62  The outline of the algorithm with the extensions that cover multiple solution paths and enforcing suitable behaviour on slopes is given bellow: 1.  input_parameters(); % Input positions of the start and goal and number N of desired solution choices to be produced %  2.  initialize(); % Read required costmaps (up to six of them) and initialize the variables associated with the cost functions for every arc in the graph %  3. for (number_of_ choices=0;number_of_ choices<N;number_of_ choices++) a.  small_initialize(); % Sets local variables in nodes: backpointer to empty, minimum cost from the start to the node to 0, number of turns to 0. Sets the 'visited' flags to 0 and to 1 for the start node which is put into 'dejo'.. Sets 'condition' to FALSE (signals end of the search). Sets 'count_dejo' to 1.  b.  set_constraintes(); % First we try to find the solution without sharp turns (135°, 90°), in the possible second try only 135° is banned while third times all the constraints are released. •  while( count_dejo) •  n = first(); % Takes the current node from 'dejo' %.  •  if (n==goal) found = TRUE; else •  expend_n(); % Expend to all the neighbours of 'n' that are not within an obstacle or out of borders and place then into 'dejo' % •  for(all new nodes 'prime') •  if(check_turn()); % Check if the angle is within the constraints and if new angle is formed at all.% 63  •  if(turn) nprime.number_turns++  •  if(! nprime.visited) nprime.backpointer=&n;  •  else •  if(check_cost()) % The cost of the arc is added to the cost of the 'n' node for comparison with the old cost to 'nprime' and a flag is set to T R U E if new path is better. •  switch(); % if newly found path through 'n' to 'nprime' is cheaper or equal but with fewer turns switch the backpointer to 'n'. %  •  update_graph(); % Update the node in the graph that was copied into 'nprime'.  c.  •  remove_n(); % Removes 'n' from 'dejo .%  •  count_dejo = count_dejo-l  if(!found && underconstraints) go to 3.a elseif(!found && constraints_released) declareJailure else declare_success();  produce_paths() % Backtrace paths produced, if any. % More details are provided on the check_turn() and switch() functions:  check_turn( ) a.  int auxl, aux2, aux3;  b.  auxl=(nprime.x-n.x) + (nprime. y-n.y) ;  c.  aux2=(n.x-n.prex) + (n.y-n.prey) ;  2  2  2  2  64  d.  aux3=(nprime.x-n.prex) + (nprime.y-n.prey) ;  e.  if(!( nprime.x= -n.x= =n.prex)\\(nprime.y==n.y==n.prey))  2  • f.  2  is_turns=l; /* otherwise it is 0 */  if(135 and 90 banned) •  if(aux3<=auxl+aux2) either an turn of 135 or 90° is created so discard the new path  g.  if(135 banned) •  if(aux3<auxl +aux2) an turn of 135 is created so discard the new path  The following figure should be helpful for understanding the above functions: It can be  (nprime.x, nprime.y)  * 45°  *  (n.x, n.y)  •  •  (n.prex, n.prey)  Figure 3.8 The angle checking  seen that because of the regularity of the grid the absolute value of the angle formed in the path can have only four (including the case of no turning) values and the angle can be detected from the relation between the sides of the triangle formed by three most recent nodes in the candidate path.  65  The switch() function is fairly simple:  1. switch()  a.  if (nprime.cost>n.cost + cost_nprime-n_arc) switch the back pointer of nprime to n  b.  else if ((nprime.cost==n.cost + cost_nprime-n_arc)&& (nprime. number_turns>n.number_turns+isjturn)) switch the back pointer of nprime to n I* The A * doesn't have this part */  At the end let me point out at the most prominent differences between this algorithm and MOA*.  The switch() function would be significantly different. We would need to compare  the vector cost functions there to prove that new path to nprime is not dominated by an old one. Because of the fact that more then one nondominated candidate path can be discovered at a node we would need to perform this for every single one of them. Additional problems would be raised if we discovered that one (or more) of the previously nondominated paths to nprime is dominated by the path coming through n. It is there where we can realize that we needed to memorize the whole tree of possible paths from nprime to the start node and to remove the dominated nodes. This can be done by having a table with all this paths at every node which is an extreme memory requirement or with having only one backpointer and employing a complex technique to for removing them at every step. Only those backpointers that have not a single candidate path going through them would be removed. This procedure is very complex and time consuming and eventually requires additional tables. The number of solutions can be very high with M O A * , so we need to reserve a significant memory space in any case, when employing that search technique.  66  3.4.4 Handling Critical Situations Two different advanced features are discussed here: 1.  Situations in which a solution path is not found but there is a chance that it would exist if the search was performed at a higher precision (lower coarseness of the grid). This part completely fits into the handling of critical situations square in Figure 3.4.  2.  Satisfying some advanced requirements that don't fit into the off-road navigation system presented into Figure 3.4. Namely it is dealing with situations where roads exist in the area to be traversed by an A M V . These issues are discussed in the following two subsections. Critical Proximity Situations  As an introduction to this issue, some critical facts  about the relation between the size of the A M V , the real-world distance between the point in the grid map and the definition of the obstacle areas is explained here in detail. The dimensions of the A M V s base L and W, and the distance between points in the grid map d are given in general numbers (Figure 3.9). It is assumed that L>W. To be able to declare a point in the gridmap free for travelling it has to be at least L/2 away from the closest obstacle edge. The other factor that has to be considered here is the error in the world model. The actual position of an obstacle can differ slightly from the position in the gridmap. One possible solution to this is enlarging an obstacle by the points whose distance from the original obstacle's points is d i.e. the closest points. This way the clearance of the A M V from obstacle would be ^p-.  This way of dealing with the issues is pretty simple and  easy to implement yet offers an improvement in safety. The Figure 3.9 illustrates this concept. The real world obstacle is the black hexagon. The inner dashed polygon (with bold lines) represent the obstacle after considering the dimensions  67  I  1  V  \  •• • • ••  • > v-^--» *  • • •  •• •  Center of the AMV  i  Figure 3.9 Relation between the grid map, AMV and obstacles and their sizes  of A M V . The outer dashed polygon shows further enlarging the obstacle to improve safety of travelling around it. The dashed borders are inclusive i.e. both the points on them and those inside are considered to be within the obstacle. The trade-off here is that safety is paid for by the possibility of not finding a solution path even if one exists. So, in the case of failure, obstacles should be reduced to their original size (bold dashed polygon in Figure 3.9) and search should be repeated. Nonetheless, if the path is found it couldn't be treated as an ordinary solution path. The areas close to the obstacles should be traversed at reduced speed with increased focus on reactive behaviour. Another possibility is combining the search with a road map method where critical parts (between obstacles) of the path would be travelled through a constructed corridor  68  with a constant orientation and a reduced speed. Traversing Areas with Existing Roads  A very important requirement for a path  planner for A M V s operating in forestry is to follow a path if it connects the start position to the goal position. A significant amount of work has been presented on road recognition and road following. Road are very distinctive features in an image and can be successfully detected.  The road  following research is also on an upswing due to the development of the intelligent highways. The problem that we are discussing here is related to the situations in which certain amount of off road travelling is required despite the presence of roads. Those situations are created when the start or/and goal position are not on the road.  Figure 3.10 Path planning when a road is present  In cases where the start and goal are very close to the road the simplest solution is to get on the road in the shortest way and exit it at the closest place to the goal. In other cases separate searches might be performed to determine the connecting and disconnecting paths. For the connecting path the search algorithm presented here would work perfectly since it supports multiple goal nodes (all the sampled points on the road would be declared goal  69  points) while the disconnecting point would be chosen as the closest one to the goal. Of course, it should be checked if the total cost of these two paths is less then the optimal cost if we ignored existence of the road (this reasoning is reserved for the cases where the start and goal are on the same side of the path like is the in Figure 3.10). The final remark here would be that this discussion is limited to the cases where using the road wouldn't cause excessive circumventing i.e. road is within the configuration space of the same size as for completely off-road path planning.  3.5 Off-Line Implementation and Evaluation of the Path Planner The preprocessing and search are implemented in C, tested and evaluated. The results and discussion are presented bellow. The remaining modules of the path planner (Figure 3.4), operator's input and handling critical situations, were not fully implemented off line because they are closely related to the implementation on a full scale developed A M V . Instead, a presentation tool with a GUI was implemented in M A T L A B . It served as a development and evaluation tool while testing different variations of the search algorithms. It can also serve as a rapid prototype for the operator's interface. The hardware platform used here was a networked SUN SPARC 5 workstation running under the SUN OS 4.1 operating system.  3.5.1 Test Cases The input is the elevation grid map of the configuration space. Several different terrain types are used to prove functionality and evaluate efficiency of the suggested search algorithm. Those terrain types are sorted into four categories;  70  1.  Flat terrains. These were used to prove basic functionality (finding the goal point and enforcing the shortest path) of the search algorithm and to determine the behaviour of the algorithm as the dimensions of the configuration space grow. The initial size of the grid maps was 30x30. Further tests were performed on bigger grid maps (60x60 and 100x100)  2.  Inclined planes were used to prove additional functionality of the algorithm when going up-hill.  3.  The terrains with a single significant hill within a single gridmap. These cases are an extension of the cases from 2. Such cases might be very common in practice.  4.  Severely hilly terrains. These terrains are suitable to evaluate overall performance of the path planner with focus to the choice of the parameters Ay and A2 for the cost function  3.5.2 Presentation of Results and Discussion Speed Analysis of the Algorithm in the Case of Single Cost per Arc  Table  3.1  presents average execution time for both the preprocessing and the search with single cost function for three different grid map sizes. The numbers out of the bracket represent the total time spent while the numbers in the brackets represent an estimate of the CPU time spent. It can be concluded that the algorithm, in the case of 30x30 points, is fast enough for the target applications because the target A M V s (tracked machines) are slow and the dynamics of changes in their environment is even slower. The both operations include costly initialization and input operations with internal checking of the data transfer which implies possibility of further improvement in the speed performance.  It is important to note that the search is  performed in shorter time as percentage of the area covered by obstacles grows. This is due to the nature of the search algorithm: the nodes within obstacles are not used for enlarging the  71  set of open nodes and thus the expansion process is reduced. Grid size  Cost map building  Search execution time  execution time 30x30 points  0.75 sec (0.40 sec)  3.16 sec (1.55 sec)  60x60 points  5.5 sec (3.6 sec)  33 sec (18 sec)  100x100 points  36 sec (20 sec)  5.5 min (2.35 min)  Table 3.2 Average execution times for different sizes of the grid in the case of one cost per arc  Figure 3.11 shows two of the solution paths obtained while performing the above mentioned speed tests.  Figure 3.11 Examples of solution paths for 30x30 and 60x60 sized grid maps Algorithm With Up to Six Costs per Arc Figures 3.12 and 3.13 show results obtained by using six cost functions as explained in Table 3.1 with two different terrain types. The presentation tool is featured in these pictures. Beside the terrain types it is possible to chose which one of the six solution paths is to be presented. Another possibility is to show all the paths on the same screen. In this version it also allowed to changing the start and goal positions through an edit field. 72  Figure 3.12 The solution paths for a configuration space with a hill  73  Figure 3.13 The solution paths for a severely hilly configuration space  In the abovefigurethe start and goal nodes are marked by the stars. The paths are shown in different colours. The traversable nodes are represented as the green line intersections while those within obstacles are covered by red diamonds. The shortest path is in red and the path with (A;,A2)=(0,1) is black. The complete list is presented in Table 3.2: 74  (A/,A ) = (1,0) — red(l). 2  (A ,A J = (0.8, 0.2) —cyan (2) ;  2  (Xl,X ) = (0.6, 0.4) — magenta (3) 2  (A,,A ) = (0.4, 0.6) — yellow (4) 2  (A ,A ) = (0.2, 0.8) — blue (5) ;  2  (X \ ) h  2  = (0, 1)  — b l a c k (6) Table 3 . 3 The weight combinations  It is observed that the paths created with (0.8, 0.2) and (0.4, 0.4) work very well with the real-world-like hilly terrains. Paths that are formed with these combinations have fewer turns (which is reasonable knowing that more weigh is placed on enforcing shorter length) while still avoiding slopes when possible. In opposite, the black path can create unnecessary long paths because within a flat area all the arcs have the same cost: 0.. The (0, 1) combination showed to be useful strictly for climbing slopes and then it has to combined with the mechanism that enforces the zig-zag paths while banning sharp turns and reducing number of turns in general. The related results are presented in Figures 15-19 in the next subtopic.  Figure 3.14 presents another feature of the presentation tool which is the contour view of a terrain and a solution path. The contours are the lines of the same elevation in a configuration space. Their elevations are given in the adjacent numbers.  Figure 3.14 represents the contour view of the red (shortest) path on the same configuration space as is in Figure 3.12 with the slightly moved goal node. From the intersections of the path with the terrain contours it is possible to estimate how sharp the climbing is. 75  Figure 3.14 The contour view for a red (Ai=l) solution path  Regarding performance, the six choice path planner is a very acceptable performance. Due to the software implementation that performs the costly initialization and transfer operations only once, before the six searches are performed, the time spent is between 3 and 4 times larger then the time required by the single cost search (Table3.1) so absolute time spent is well less then 3 seconds for the preprocessing and 10 seconds for the search.  76  The preprocessing and search could be easily parallelized. On a six processor hardware platform each processor would perform the same procedure which would cut the execution time to just above that required for the single cost search. The parallelization is not this obvious for the M O A * algorithm which is complicated to implement even on a single processor computer. Path Planning on Slopes  The implementation results of the algorithm for path plan-  ning on slopes is represented in Figures 3.15-3.19.  Figure 3.15 Unacceptable path planning on slopes, example 1  Figures 3.15 and 3.16 represent paths produced with (Ay,A2J=(0.8, 0.2) and (Xj,X2)=(0,l), respectively.  It is obvious that these paths don't satisfy the requirements for path planning  on slopes set up in Section 3.1.  The shortest path would not be acceptable, too (reasons  are, again, the limitations of a machine to master rapid elevations and the soil disturbance). It would, anyway, have very similar characteristics to the path from Figure 3.15 in such an uniform configuration space.  77  Figure 3.16 Unacceptable path planning on slopes, example 2  Figure 3.17 shows the resulting solution path obtaining by employing the algorithm presented in Section 3.4, Topic 3. The traversable area here is wide so the number of turns is reduced to only two (it would be one if we didn't ban the sharp turns at all) which is the absolute minimum. Following Figure 3.18 presents the result on a narrower slope. The final example shows the behaviour of the algorithm when obstacles populate the free area on a slope.  The path planner, again, obeys the same principles not to climb directly  upward and to minimize number of turns while circumventing. At the beginning it made more turns (the path arcs were narrower due to fact that the obstacle narrowed the free space) and it gradually increased the path arcs and reduced number of turns when the obstacle is passed and the free area became wider. It is noticable here that when moving (climbing) to the next point an A M V have three choices:  78  0  0  Figure 3.17 Path planning on wide slopes  Figure 3.18 Path planning on narrow slopes  1.  To climb directly upward  2. To go toward the diagonal neighbour (45° with regard to 1.)  79  Figure 3.19 Path planning on slopes with obstacles  3. To stay at the same altitude This is due to the nature of the search algorithm that allows moving only toward the closest neighbour. The more gradual climbing could be achieved by modifying the search algorithm to allow direct moving toward more distant neighbours. This would require thorough rewriting of the software. This concept is presented in Figure 3.20. If we rewrote the search to allow  • • • •• •  • •  • •  Figure 3.20 A way of slowing down the climbing rate  it to explore more then 8 neighbours (for example 24, like those presented in Figure 3.20) we would be able to derive even slower climbing paths. 80  Chapter 4 The Navigation System Requirements and Analysis This chapter provides the requirements analysis of the semi-autonomous, mini-excavator based, experimental system. The analysis applies to the implemented demo system as well as to the target system that is supposed -to operate in real world industrial conditions. The chapter is divided into three sections: high level general requirements, hardware requirements and analysis and, finally, software requirements and analysis. It is possible to identify a certain amount of inconsistency in using the term navigation system in the mobile robotics literature. Some authors tend to use it to refer to the global positioning system. In this thesis the term "navigation system" is used to refer to a comprehensive mobile robot motion system, including path planning, low-level track control, sensing systems and hardware and software used to implement concepts from the above mentioned parts of the system.  4.1 Requirements Analysis for the Experimental System The ultimate goal of the project that my thesis is part of is developing a forestry machine that can operate in a semi-autonomous mode in a real-world environment. We are aiming to eliminate the need for an on-board operator, to simplify the operator's task and to reduce the soil disturbance. To accomplish this a computer processes all the sensor information and outputs control signals to the valves. This computer should be mounted onboard. It is reasonable to develop the system the assuming existence of an teleoperator who would supervise the operation. He/she would sit somewhere remote from the machine and would have a camera-generated view of the machine's environment in a window of the application interface  81  on a remote computer screen. The operator would be able to choose one among several offered paths after the paths are calculated. The interface would allow him/her to have the highest level of control over the machine. An override button is to be present, which would allow an operator to completely take over the control over the system i.e. to transfer it into the fully teleoperated mode from a semi-autonomous mode. The system is clearly a real-time system with both hardware and software issues to be considered. The requirements analysis and design overview will cover both aspects of the system [44]. Since the system is a real-time one, both functional and non-functional requirements analysis for it should be considered. Another point to be made here is that this is a prototype system. The system will evolve significantly before it will be possible to say it is a machine that can actually do the job in a forest. The requirements analysis and design presented here are targeted toward the design of the experimental excavator based system but the industrial implementation is not forgotten. Whenever possible, a real life solution that is likely to differ from the solution provided for the experimental system will be suggested.  4.2 The Hardware Requirements and Analysis The system requirements for the hardware part are shown in a hierarchical manner which provides better understanding to readers and implies modularity and therefore flexibility of the system . Let me set up a few conventions about usage of the line styles on the diagrams while discussing hardware requirements and design. At the same time we will analyze high level hardware issues for the actual system by providing examples from Figure 4.1: 1.  A line means that there is a direct connection between the hardware elements in the sense that there exists a flow of information and commands between them. For example, directed  82  Excavator Motion Control Hardware  Pedals"  Joystick  S E N Onboard Computer  S O  V A L V E S  T  R S  Network connection Departmental SUN SPARC Network  Networked Computer #2  Networked Computer #1  Figure 4.1 System's hardware overview  line between the sensors and onboard computer blocks means that the data from sensors is received by the computer for further use and processing. Dotted lines, like those connecting networked computers to the hardware onboard, mean that the connection exists in the design and will be implemented due to the available resources and the need for flexibility in the system development.  On the other hand it would be  desirable to get rid of these in the later improvements since it means having wires from the cabin to a network computer. The other drawback of having these networked computers is a big overhead (interrupt handlers, for example) and unpredictable and sometimes slow  83  response times which are very important issues in designing hard real-time systems. The advantage of having these is the availability of variety of software tools during development. 3.  Dashed lines like those related to the pedals block mean that this element of hardware exists but it is meant to be replaced by the elements developed for this project at its earlier stage.  4.2.1 Sensor System Analysis Sensors needed on this system are: 1.  Position sensors: They are supposed to provide us with the position and orientation of the machine with respect to a chosen coordinate system. Since our track control algorithm is a position control algorithm (i.e. sensors is absolutely crucial.  it follows a given trajectory), existence of these  Specific requirements are that they are very precise and  fast enough to have an updated state ready for the next control cycle. This usually cannot be achieved, for example if the control software cycles at 20Hz on the mini-excavator, so the requirement is loosened but not to the point that the accrued position error becomes unmanageable. Differential GPS and beacon based positioning systems, though attractive and improved [45], [46], still cannot satisfy all the above mentioned requirements. For example, their signals can be hidden by natural obstacles and the high precision versions are very expensive. Some problems are accounted by existence of relative position sensors, too. These sensors need to provide us with the precise and very often updated information on the path travelled by the vehicle so that we can correct the information obtained by the DGPS or beacons. The relative position sensors register small, local movements of the machine or the tracks (which are, again, translated into movements of the machine). Their readings are integrated over time and combined with the data obtained by DGPS and/or beacons to eliminate the accumulated error. These sensors (see below) are supplementary ones so they need to be very inexpensive.  84  2. Track velocity sensors: In our case the track control algorithm deals with velocity, too, so a feedback information on the track velocity is necessary. Track velocity is sensed by measuring the flowrate of hydraulic oil into the track hydraulic motors. 3.  Joystick: The basic requirements for joystick are the same as those from case 4. A specially important requirement for joystick is the ruggedness.  4.  Operators interface: Ideally, it would be a touch screen interface with overlay graphics capabilities (the solution paths as well as positions of goals and obstacles would be presented over the real world images). It would be equipped with a simple to use GUI, like one presented in the previous section). Besides the above we would like to have a sensing system that would provide a world  map in real-time. It could be based on stereo vision, lasers or sonars. During the work on this thesis we were using world maps obtained off-line.  4.2.2 Track Control Actuator System Analysis Signals are sent to actuator valves through the drivers from the D / A board in the V M E cage mounted onboard. The drivers control the electro-hydraulic valves that control the flow to pedal cylinders. Manual pedal operation is allowed so that an operator can take over the control over the machine in a case of failure of the electronic system.  from D/A  •  P02 current drivers  Pedal valves  push pedals Cylinders  Figure 4.2 Actuators' hardware diagram  4.2.3 Computing Resources and Connections Analysis The general requirement is to have a commercially available embedded system with large on-board memory so the time consuming, disk-related, I/O operations would be reduced. It  85  would be also desirable to have multiprocessor system (not network connected computers, but multiple processors on the same system bus) to split the heavy work-load for building a world map and navigating the system. Computation is naturally separable into two tasks that can be executed in parallel and synchronized to maximize throughput of the system. The first task is to process the data obtained from the sensing system and build a world map. The second task is to decide on a path and to execute the control to lead the machine along the path. It is possible to split the computation into more tasks but we need to be careful about introducing overhead. This software can be implemented on a single processor architecture by employing multitasking (multiple threads in a process or multiple processes though the first one is a better solution due to less overhead). The processor should have embedded image processing features (an Intel Pentium M M X looks like a suitable solution since it is relatively cheap and is widely spread and accepted in industry). The communication with the tele-operators's computer could be done through a wireless link. The ideal implementation language might be Ada or C on a small real-time kernel (VxWorks for example) or with no operating system at all. The system architecture is shown in the Figure 4.3  SUN SPARC 1 VME cage  Ethernet SUN SPARC 5  SUN SPARC 20  Figure 4.3 System's computers and their connections  86  The computers are connected to each other by Ethernet. That is the main weakness of the whole system. A SPARC 20 is suitable for image processing because of its fast computing capabilities and big on-chip memory space. A SPARC 5 is a host computer for the whole system. An operator or researcher is supposed to interact with the system through this machine. Executable files to run on the onboard machine are compiled and downloaded from the SPARC 5. A program running on the SPARC 1 machine is precompiled and completely downloaded into its onboard memory. Since the variables related to the grid map for the search algorithm are pretty big we must ensure that there is enough memory to store the program. Those variables should be global ones so that preprocessor reserves memory space for them at the beginning. This way we make sure that program won't crash during the execution due to lack of memory space. Since this is a hard real-time system (failures could be very costly) we must make sure that it works for the worst case conditions, in this case for the theoretically highest memory space usage. Some of this reserved space won't be used all the time but it is not important in our case since our software won't share the SPARC 1 processor with other processes. Having other (unrelated) processes sharing a processor with a hard real-time task is not good since it would be hard to analyze the timing of the system and make sure it meets its deadlines (it can be interrupted in variety of unpredictable ways by foreign processes). Besides the possibility of running out of memory space at some point of the program execution, dynamic memory allocation is not recommended in hard-real time systems since its use is based on having system calls and sometimes overhead associated with it can be high so deadlines can be missed and the program can fail.  The other hardware issue that needs attention is the Ethernet connection. It is generally a bad solution for systems like this one due to its high overhead, variable data packaging (a file is being split into packages whose size varies) and we have a wire connection from the  87  excavator to the SPARC 5 workstation At the end of this topic we can say that it seems that the SPARC 5 workstation is a completely redundant here. All the work that it does can be done on SPARC 20 and the time it spent would be mostly reimbursed by having less inter-computer communication.  4.3 Software Requirements and Analysis  As an introduction into this section I will go over some basic facts about the software engineering analysis. It is very common (for example look at: [47]) to divide the requirements document into the functional requirements and nonfunctional requirements. The functional requirements are: desired input/output behavior as observed at the system interfaces and the different modes of system operation. I would also say at the subsystem level too. The nonfunctional requirements are: timing requirements, task scheduling requirements, store occupancy etc. In general they define system properties and constraints. These requirements are often called the real-time requirements.  4.3.1 Functional Requirements For the purpose of the analysis of the software requirements The Real-Time Structured Analysis (RTSA) [48], [49] notation is used (en extension of data flow diagrams). The RealTime Structured Analysis (RTSA) notation is shown in Figure 4.4.  88  Transformation  /  \  / Control t 'Transformation I \ /  Data Flows  Discrete Data »».  Continuous Data  Event Flows  Data Store  Shared Data  Figure 4.4: The RTSA notation  Lets go over the data/control diagrams for this system. They are organized in an hierarchical fashion. Figure 4.5 shows the highest level overview of the system. Only the system's inputs and outputs are represented here.  Figure 4.5 System's software overview  89  Figure 4.6 shows the high level functional parts of the system. It is necessary to notice that the terms used here are compatible with those from the hardware requirement e.g. image data is included in the sensor data term The control events are limited to those that change the mode of operation (for example from the manual control to the semi-autonomous mode). There are some other events (timer interrupts, for example) here that are not explicitly showed. They are enclosed in the data flow from one bubble to another. This should make the diagrams more readable and it leaves those details to be discussed later separately (in the nonfunctional requirements). Also, when the data storage is shown in the diagrams, it means that some sort of synchronization is necessary when dealing with those data. Those are time-consuming and critical parts of the code that have to be taken special care of. This little change (or addition) in meaning of the symbols from Figure 4.4 should improve usability of the diagrams in the requirement phase while keeping them equally readable. These remarks are related to all of the data/control diagrams in this document.  Sensor data  Interfacing t o \ operators  L  an operator  P  a t n s  Operator choses the path  Figure 4.6 High level data/control flow diagram  Figure 4.7 shows details of the sensor data processing bubble.  90  /  input  Display *  World map  Input from absolute position sensors  To Control calculating and execution  Absolute position position error, slippage  Figure 4.7 The sensor data processing bubble  All four inputs on Figure 4.7 are represented by a single input (Sensor data) on Figure 4.6. Another comment is that even though it is shown that both outputs require synchronization, the world map one is much more critical shared data. The following figure (Figure 4.8) represents control calculating and execution bubble.  91  Flow/control diagram for the control calculating and execution bubble:  Joystick input  Related state transition diagram: Manual or teleoperated mode CD  ° "S o E  o <u ° "S o E  Semiautonomous mode Figure 4.8 The control calculating and execution bubble  As is shown in Figure 4.8 the Choose mode event toggles the mode of the system. If  92  was in the semi-autonomous mode it changes the mode to the manually operated mode (then an operator drives the machine with the joystick) and vice versa. The operator is in charge of changing modes through a touch (or mouse click) on a GUI. Figure 4.9 shows internal structure of the path planner bubble.  To control  Gridmap with nodes marked so that we can backtrace the solution paths •  From operator's nterface Operator choses a path  Path selection criteria from operator  Figure 4.9 The path_planner bubble  Figure 4.9 requires some additional comments.  The operator can choose a path among  the multiple paths supplied. The path-planner reads last saved version of those criteria. If there was no 'operator chooses a path' event, the path-planner would pick a default single path among those available. But the choice is left to the operator to pick one by clicking it on the screen. We might do this in few ways: for example, to have a watch dog timer that would wait for operators event and if it doesn't occur to latch the path-planner's suggested path. The last flow/control diagram is for the operators' interface bubble:  93  I  from Control  to Path_planning  *  /  Solution path criteria  Figure 4.10 The operators' interface bubble  4.3.2 Software Design The high level software design could be traced to the above requirements in the easiest way if we take a look at Figure 4.6. It is an obvious solution to consider a software task for each of the bubbles in the figure. Moreover, considering the speed of the path planner and need to produce a new path when the goal is reached or in a case of an unpredictable real world event, it is reasonable to implement it as a function and reduce the number of tasks and therefore the overhead and complexity of the software. The other three tasks: control, world map building and operators interface task should be operating in parallel. Of course, this design relates to the completely embedded (target) system. At this phase only the path planner and tracks control are implemented onboard.  4.3.3 Non-Functional Requirements and Analysis In the above analysis of our functional software requirements, they were noted with the RTSA notation. Though that notation offers some insight into the real-time (non functional)  94  behavior of the system, event flow for example, non functional analysis should be performed. Here, only the most important requirements are mentioned. The control task should be given the highest priority and the operators interface the lowest except for the Change Mode event which should have a hardware interrupt priority. The most important timing constraint is the control loop period which is 0.05 sec. Memory is a definite concern because it is necessary to store and process the big structures for the world model.  95  Chapter 5 The Navigation System Integration and Experiments This chapter describes and discusses implementation issues and experimental results obtained with a real world semi-autonomous tracked machine. The implementation is based on the theory set forth in the previous chapters. The demo system presented here is a limited implementation of the semi-autonomous tracked machine system that is to be achieved in the final stage of the project. The requirements of that target system are analyzed in Chapter 4. The first section of this chapter describes the hardware aspects of the system while the second one deals with the software implementation. Finally in the last section the results and observation of the experiments performed are presented.  5.1 The Integration and Experimental Methods 5.1.1 Hardware Configuration The experiments are performed on a Takeuchi miniexcavator.  It was equipped with  an onboard processor SPARC le mounted inside a V M E cage in the miniexcavator's cabin. Communication with the onboard computer and supervision of the program execution is performed on a remote workstation. The communication is performed through Ethernet. An overview of the computing subsystem used is given in previous chapter in Figure 4.1. Resolvers placed on the tracks of the mini-excavator were used for speed and position feedback. The joystick is also present offering an operator a chance to make on line corrections to the trajectory if they are needed. As mentioned earlier the path planner offers a node-to-node path from a start point to a goal point. When translated into the real world circumstances it means that, ideally, the center of the excavator's projection on the ground will travel along the lines in the solution path all  96  the time. In our case the center of the mini-excavator is defined as being in the center of the tracks (between two tracks).  5.1.2 Software Methods and Implementation The software component of the system is significantly different from the target software architecture presented in Chapter 4. The world modelling package based on image processing was missing and the experiments were performed on a set of synthetically created world maps. The operator's interface was also placed outside the cabin and has a slightly different set of features. In this demo version of the system an operator inputs the desired goal position and can check out the solution path provided from the path planner. The joystick control is not implemented as a separate mode of operation. The operator has a chance to modify the trajectory of the miniexcavator at any time by superposing over the path planner generated input. These changes reduced complexity of the software and improved safety while performing experiments within the laboratory. The software design is presented in the following figures. The path planner is described  The Path Planner  ^-  Path Planner Control Interface (Inverse Kinematics)  Low Level Control  •  Figure 5.1 High Level Software Design  in previous chapters. Already existing low level track control system [50] was utilized. The  97  control system's block diagram is presented in Figure 5.2. As can be seen above the input Feedforward  rxi Joystick input  Tde l  Kinematics J .JStoSPEED and Kinematics J , 'sPEEDIoPOS  !el '  /Error' generation.  LvJ Pes -  Toil  Trajectory  ryl le'l  lYl  /xx\ F L C  Error.  Tdcol  LvJ  Right Hydraulic motor closed loop control  n  LdvJctrl  Inverse kinematics —  •  J  PWS  r«oi '  H  LdeJ  Lv J.„,  Des  '  r  Left Hydraulic motor closed loop control  Caterpilar tracks  rxi lYl lei  Machine dynamics  Excavator Motion relations  2  re i Le J R  L  Odometry wheels  A  Figure 5.2 Control system diagram (Modified from Poul Jacobsen's thesis[50])  for the control system is the desired velocity (translational and rotational) which is integrated into desired position. Due to the feed forward branch in the above figure the desired velocity still directly influences the control parameters. This imposes a need for additional interface between the path planner and the low level control. A very simple approach has been taken in designing the interface. Due to the nature of the path planner any solution path can be split into a sequence of actions. Lets assume that the distance between two closest points in the grid is /. The possible actions are: 1.  Move forward / meters.  2.  Move forward 1.41/ meters.  3.  Make 45° left turn.  4.  Make 45° right turn.  5.  Make 90° left turn.  6.  Make 90° right turn.  7.  Make 135° left turn.  98  8.  Make 135° right turn. These eight cases were performed manually by using joystick control and the joystick input  needed to perform them was recorded. These input sequences were stored in adequate variables. The above described operations were performed off line and the interface code was using the stored data. The interface program has the solution path as input. The path is calculated by the path planner. In every new node the mini-excavator reaches, the interface program provides new output sequence to the control code. The calculation of the new sequence is based the the previous, current and next desired position of the mini-excavator. Then the adequate sequence is copied from the list of possible choices and sent to the control. This concept is illustrated in Figure 5.3. Input the solution path from path planner (list of consecutive node coordinates)  The output sequence is set to 0 (the control is completely returnee to joystick)  From previous, current and next node coordinates ((xp, yp), (xc, yc) and (xn, yn)) look up the next input sequence from the inverse kinematics table.  • Add the output sequence to the joystick input.  Figure 5.3 Interface between the path planner and low level control  99  Figure 5.4 offers more details of the mechanism of deriving the next output sequence from the coordinates in the solution path.  • r /• (xn, yn)  Ia  /  (xp, yp) Figure 5.4 Deriving the output for the low level control (inverse kinematics)  In the above example the coordinates of the current position are (xc, yc). The previous position was (xp, yp). In order to get to the point (xn, yn) the machine has to make a left turn (ccw) of 45 degrees and to travel forward for / meters after that. Four numbers: xc-xp, yc-yp, xc-xn and yc-yn (which take values from the set {-1, 0, 1}) define uniquely the required sequence to travel from (xc, yc) to (xn, yn). The number of possible sequences is 14. When values of the above four numbers are determinated the matching output sequence is looked up from the table. This approach has several benefits: 1.  It is simple.  2.  It connects well with the existing control code because the performance of the joystick driven machine control code was at its worst during turns. The machine slows down, even brings itself to a full stop during a turn so the fact that we stop for a short time after a travelled segment doesn't deteriorate the performance in terms of speed. In terms of precision, the performance is improved because after every segment the global position  100  for the control system is reset to (0, 0) so the position error that accumulates during the integration from the resolvers is reduced.  The main disadvantage of this approach is that for more advanced applications (for example with variable grid coarseness, different levels of throttle) more extensive mapping would be required. The other problem is that errors (even though very small on a single segment) might accumulate on a longer path. This might be fixed by an operator through the joystick input. An alternative way is developing a global positioning control system (the present one is basically a velocity control system and its transfer into position control system would result in poor performance). In that case it would be possible to feed the solution path directly to the track control program. A big problem for achieving such systems of high precision is lack of a reliable and accurate global positioning systems (the differential GPS, still, lacks in reliability and is very expensive). At this point beacons are probably the best answer to that problem [45].  5.1.3 The Experiment Setup and Procedure The start point for the machine was fixed at node (0, 0) to make the most out of the range of motion allowed by the wire connections. The positions of the obstacles were preloaded to be read by the grid map generating program. Due to the uniform configuration space (flat, concrete floor of the lab) and its small size, a stripped down version of the search algorithm is used. The second weight A 2 in the cost function (Equation 3.7) was set to zero. The path planner, running on the onboard computer, prompts the operator/supervisor (working on the SUN SPARC5 workstation) to input the coordinates of the goal position. After inputting a valid value the costmap is calculated and the solution path is provided. At that point, the program waits for the confirmation to proceed. This gives an operator a chance  101  to take a look at the path, in the configuration space context, presented in a M A T L A B window. The examples...are presented in Figure 5.5. All the important variables in the track control system are monitored in real time through a separate window of the real time monitoring and debugging software tool called Stethoscope. A separate, low-priority, task was spawned in the program running under the VwWorks operating system, onboard, to collect these data in real time. For example we can monitor the resolvers' readings, joystick input from both sources: from the path planner interface and from the actual joystick and all the other important signals. A very nice feature is monitoring the movement of the mini-excavator in real-time. A vision based position detection system, developed in our lab, is mounted on the ceiling of the lab. The processing of the images is performed on the SPARC20 workstation. The connection between the onboard program and the SPARC20 is achieved through a socket. The navigation system reads the socket every 0.05 sec. and check if there is an update in the position because the vision system's update period is much longer then the control period (about 1 sec. comparing to 0.05 sec.  for the control period).  The precision of the vision system is better then 8 cm in the central parts of the lab while the error grows as we approach peripheral parts of the lab due to the distortion in the images. As is presented bellow the precision of the navigation system came very close to the precision of the vision system so the position error was measured manually to obtain more precise measurements. Figure 5.6 illustrates the way the measurements were performed. While we can measure the position error of the center of the mini-excavator directly, for obtaining the orientation error, the positions of two more points: PI and P2 were measured and the orientation error was calculated. The logical (0,0) is the start position of the mini-excavator 102  Real world (0,.0)  Logical (0,01  Figure 5.5 Illustration of the experiment and measurements  before an experiment and it represents the (0,0) point in the grid map while the real world (0,0) was just the reference point for the measurements.  5.2 The Experimental Results and Observations The experiments were performed indoors. The configuration space was a square concrete floor area sized about 8x8 meters. A square of the size 5x5 meters, exactly, in the middle of the configuration space was mapped into a regular grid map with 36 nodes. The limited size of the configuration space was dictated by the size of the lab and with problems induced by the need to communicate with the V M E platform through a wire connection. The measurements were performed on the experimental settings shown in Figure 5.5.  complex path  simple path  Figure 5.6 Solution paths for different cases of the configuration space and goal positions  103  Table 5.1 shows the measured position errors.  x-coordinate  y-coordinate  error [cm]  error [cm]  #1 simple path  4  10  -3  #2 simple path  3  7  -1.5  #3 simple path  5  10  -3  #1 complex path  12  14  -1  #2 complex path  12  7  3  #3 complex path  10  15  -4.5  0-angle error [°]  Table 5.1 Position errors measured while running the demo system  The amount of the displacement of the center of the excavator was less then 20cm during all of the experiments which is very good when the dimensions of the work space and the mini-excavator are taken into consideration. Table 5.2 shows the percent error for all the cases from Table 5.1. The numbers are obtained by dividing the absolute values of the errors by the length/angle travelled. x-coordinate  y-coordinate  0-angle error  error [%]  error [%]  [%]  #1 simple path  0.9  2.3  3.3  #2 simple path  0.7  1.5  1.6  #3 simple path  1.1  2.3  3.3  #1 complex path  1.9  2.2  0.4  #2 complex path  1.9  1.1  1.3  #3 complex path  1.6  2.4  2  Table 5.2 Percent errors  104  5.3 Discussion While performing the experiments the existence of the wire connection between the V M E based onboard computer and the operator's workstation was a serious drawback. It limited the area of possible motions. An additional person was required to make sure that the wires don't end up under the tracks. Despite the fact that the wire connection is technically simpler and cheaper then the wireless connection, switching to this method would increase the reliability of the system because most of the problems encountered while preparing and performing the demo was caused by bad connections in different wires. It was specially observed with the joystick and Ethernet connection that were moved the most (the joystick is not fixed and it was frequently taken back and forth between the operator's cabin and the supervisor's workstation. Another observation, related to the hardware reliability, is that it is absolutely necessary to check the values of every input signal in the system so that the hardware troubleshooting would be a little bit easier task.  105  Chapter 6 Summary and Conclusions 6.1 Summary We have described a navigation system for semi-autonomous tracked machines. The main focus was placed on developing the path planner. Dijkstra's algorithm based path planner assumes the existence of an elevation map of the work space. The elevation map represents a uniformly sampled squared area. This form is suitable because it simplifies the software for the path planner and allows a convenient way to control the precision and the size of the area covered by the path planner by changing only two parameters: the number of nodes in a row (and a column) of the grid and the distance between two sampled, neighboring, points in the real world. The requirements that the path planner was supposed to satisfy were minimizing the path length and the reduction of soil disturbance. A weighted sum cost function for the arcs in the grid map was devised to reflect these objectives. The first element of the sum reflects the length of an arc while the second part reflects a relative change of elevation along the arc. The cost function reflects two main objectives: to shorten the path and to avoid travelling slopes which induces increased environmental damage. A path planner design is proposed that has multiple solution paths generated at the same, time, with different weight combinations for the cost function. We implemented and tested a version with 6 combinations. This number represents a trade off between the definition of the solution space and the execution time of the search. The wide range of additional factors in a industrial work space (soil type, saturation of a terrain, etc.) make it a desirable feature to have different variations of the solution path offered for an operator to choose from. This way we achieved similar effects to those that would be generated by employing the M O A * algorithm [41] but with a controllable computational complexity of the algorithm. It is important to  106  emphasize that some parts of the algorithm need to be performed only once for the all 6 paths and that the software design is easy to implement on a parallel C P U which would cut the execution time to very close to that of the single choice algorithm. Moreover, additional modifications to the algorithm are made to account for the special case where a vehicle is required to climb slopes. The cost function is reduced to its second part. The sharp turns are initially banned, then, if the solution is not found the constraint is gradually released. The number of turns, in general, along a solution path is used in selecting the preferable path at every step of the search as a secondary objective to favor the paths with the same cost and less turns. This way the computational complexity of the A * search algorithm is not affected while the execution time is only slightly increased. The search algorithm, still, guaranties finding the solution path if one exists, which is a desirable characteristic compared to some other heuristic based search algorithm [11].  6.2 Conclusions  The off line tests on a variety of synthetic test terrains showed that algorithm performs sufficiently fast to be implemented in real-time. The single choice algorithm for a 30x30 grid map needs an average of 1.55 seconds CPU time on a SUN SPARC 5 workstation. The CPU time required for the six choice version is 6.2 seconds. The results showed that the search performed with the cost functions where the weights are chosen to be (0.8 0.2) or (0.6 0.4) (or in general where the shortest path objective is emphasized but not absolutely pursued) will product satisfactory results for most of the real world terrains containing both plains and hills. Handling dynamic changes in the environment and precision critical situations are discussed and solutions to the problems are suggested.  107  The integration of the path planner with the existing track control system resulted in a very precise navigation system able to reach every point in the a 6x6 grid map it was tested on. The position error accrued along a complex path within a squared field of work, sized 8x8, meters was less then 15cm in x and y coordinates and 4° in orientation. The navigation system took advantage of the fact that the path planner produces the solution path that is a combination of the limited number of possible movements so that control system can be tuned to perform well for this limited number of possible tasks. The fact that the mini-excavator performs its turns on the spot is not a step back from the results achieved by the control system [50] alone. The precision of the path-following for the control system was at its worst during the sharp turns and machine was very often brought to a full stop. In integrating the control with the path planner the precision of path-following was improved (from more then 30cm on a straight line, 6 meters long, to less then 20cm on a same or more complex path of the same length) with little or no speed sacrificed.  6.3 Directions for Future Work  Several opportunities for further improvements and problems that require closer attention were recognized during the development and implementation of the navigation system. First, implementation of the multiple choice path planner on a multiple processor hardware platform would cut the processing time significantly. Another important area for future improvement is handling the described critical proximity situations (Topic 3.4.4). Some elements of the reactive behaviour should be further analyzed because their integration with the core of the navigation system would make a step further toward having a completely autonomous machine. The parallelization of the algorithm would  108  leave more CPU time for incorporating these additional 'intelligent' features into the navigation system. Even though the experiments on the mini-excavator returned very satisfactory results in terms of the path following, a few limitations of the method are obvious. The mapping of the inputs into the control system for the allowed elementary movements was performed for a fixed throttle level. The input vectors will also vary for the same movement if the work space of the machine wasn't so uniform (the concrete floor in the lab is fiat). For dealing with the more diverse terrain types and different throttle levels more comprehensive mapping should be performed. For example the input sequence should be mapped separately for the same movement on slopes of different degrees. The other, more significant, problem is the position error. Even though the error was very small in our experiments it can still accrue significantly over substantially bigger paths. This problem could be treated in several ways. In our implementation, we left the possibility for an operator to manually correct the path in real-time by the joystick input. Another possibility is to map the required inputs for several, well selected, micro movements (for example 10cm forward, 10 cm backward, 1° left and 1° right). In that case it would be easy to develop a discrete ('digital') control algorithm that would create a sequence of these micro moves that would annul the accumulated error on designated check points. The third and the most conservative approach would be to design a position control track control system (the present one takes desired velocity through the joystick deflections and then integrates the desired position). Finally, as none of these approaches is absolutely dominant over any other, these approaches could be combined as is done in Chapter 4 where the complete navigation system is proposed. The above mentioned methods for eliminating the accrued position error have one thing in 109  common. A global positioning system is required and it remains an open issue for the future outdoor tests of the navigation system. We are hoping that in the near future the sensor system for building the world map (the elevation map plus coordinates of the points in the grid that are within obstacles). It could be obtained from the satellite generated images. Even more challenging would be to obtain it in real-time from a sensor system mounted on the mini-excavator. Finally, wireless communications between different, remote, elements of the system (onboard computer, supervisor's workstation and the sensor system for building the world map) would increase usability and reliability of the navigation system.  110  Bibliography  [I] B. Brumitt, R. Coulter, A . Kelly, and A . Stentz. A system for autonomous cross country  navigation. IFAC Symposium on Intelligent Components and Instruments for Control Applications, Malaga, Spain, 1992. [2] A. Kelly and A. Stenz. An approach to rough terrain autonomous mobility. International  Conference on Mobile Planetary Robots, Santa Monica, 1997'. [3] K. Koskinen. Autonomous mobile machines, a case study of technology development and  transfer. Espoo 1994, Technical Research Centre of Finland, VTT Publications 191, 1994. [4] S.D. Myers. The mdars exterior platform. 22nd Annual Symposium and Exhibition , Association of Unmaned Vehicle Systems, AUVS '95, Washington, DC, 1995. [5] F. J. Doyle. Thirty years of mapping from space. In In Proceedings of the XVIII Congress  of the International Society for Photogrammetry and Remote Sensing ISPRS 1996, Vienna, volume XXXI part B4 of International Archives of Photo grammetry and Remote Sensisng, 227-230, 1996. [6] H. Spitzer B. Prinz, R. Wiemker. Simulation of high resolution satellite imagery from multispectral airborne scanner imagery for accuracy assesment of fusion algorithms. In  Contributions to the Joint Workshop of ISPRS 1/1, 1/3 and IV/4 Sensors and Mapping from Space, Hannover, 1997. [7] IS. Kweon and T. Kanade. High-resolution terrain map from multiple sensor data. IEEE  Transactions on Pattern Analysis and Machine Intelligence, 14, 2, 278-292, 1992. [8] L . Bole and J. Cytowski. Search Methods for Artificial Intelligence. Academic Press, 1992. [9] C.J. Thornton and B. du Boulay. Artificial Intelligence Through Search. Kluwer Academic Publishers, 1992. [10]J.C. Latombe. Robot Motion Planning. Kluwer Academic Publishers, Boston, 1993. [II] B.S. Stewart, C. Liaw, and C.C. White. A bibliography of heuristic search research through  1992. IEEE Transactions on Systems, Man and Cybernatics, 24, 2, 268-293, 1994. Ill  [12]N.J. Nilsson. A mobile automation: An application of artificial intelligence techniques. Proceedings of the 1st International Joint Conference on Artificial Intelligence, 1969. [13]A.P. Ambler and R.J. Popplestone. Inferring the positions of bodies from specified spatial relationships. Artificial Intelligence, 6, 157-174, 1975. [14]R.H. Taylor. Synthesis of Manipulator Control Programs from Task-Level Specifications. PhD thesis, Dept. of Computer Science, Stanford University, 1976. [15]T. Lozano-Perez and M.A. Wesley. An algorithm for planning collision-free paths among polyhedral obstacles. Journal of ACM, 22(10), 1979. [16]T. Lozano-Perez. The design of a mechanical assembly system. Technical report, Artificial Intelligence Laboratory, MIT, 1976. [17]W.S. Massey. Algebraic Topology: An Introduction. Springer-Verlag, New York, 1969. [18JP.E. Hart, N.J.Nilsson, and B. Raphael. A formal basis for the heuristic determination of minimum cost path. IEEE Transactions on Systems, Science and Cybernetics, 1968. [19]J.P. Laumond. Obstacle growing in a non-polygonal world. Information Processing Letters, 25(1), 41-50, 1987. [20]J.M. Ibarra-Zannatha, J.H. Sossa-Azuela, and H . Gonsales-Hernandez. A new roadmap approach to automatic path planning for mobile robot navigation. Systems, Man and Cybernetics, 3, 2803-2808, 1994. [21]C. O'Dunlaing and C.K. Yap. A retraction method for planning the motion of a disc. Journal of Algorithms, 6, 104-111, 1982. [22]R.A. Brooks. Solving the find-path problem by good representation of free space. IEE Transactions on Systems, Man and Cybernetics, 13(3), 190-197, 1983. [23]J.F. Canny and B.R. Donald. Simplified voronoi diagrams. Discrete and Computational Geometry, 3, 219-236, 1988. [24]F. Avnaim, J.D. Boissonnat, and B. Faverjon. A practical exact motion planning algorithm for polygonal objects among polygonal obstacles, tec. rep. 890. Technical report, INRIA, Sophia-Antipolis, France, 1988.  112  [25]J.T Schwartz, M . Sharir, and J.Hopcroft. Plannning, Geometry and Complexity of Robot Motion. Ablex, Norwood, NJ, 1987. [26]0. Khatib. Real-time obstacle avoidance for manipulators and mobile robots. International Journal of Robotic Research, 5(1), 90-98, 1986. [27]J. K i m and P.K. Khosla. Real-time obstacle avoidance using harmonic potential functions. IEEE Transactions on Robotics and Automation, 8, 338-349, 1992. [28]J.K.S. Poon. Multiprocessor-compatible inverse kinematics and path planning for robots. PhD thesis, University of British Columbia. Dept. of Electrical Engineering, 1988. [29]Y. K. Hwang and N . Ahuja. A potential field approach to path planning. IEEE Transactions on Robotics and Automation, 8, 1, 23-32, 1992. [30JH.R. Everet. Sensors for Mobile Robots. A K Peters, Ltd., Wellesley, Massachusetts, 1995. [31]R.C. Arkin. Behavior-based robot navigation for extended domains. Adaptive Behavior, MIT, 1, 2, 197-214, 1992. [32]S. Mallat. A theory for multiresolution signal decomposition: the wavelet representation. IEEE Transactions on Pattern Analysis and Machine Intelligence, 11, 674-693, 1989. [33]D. K . Pai and L . M . Reissell. Multiresolution rough terrain motion planning. Technical report, Dept. of Computer Science, U B C , 1994. [34]A.L. Meyrowitz, D.R. Blidberg, and R.C. Michelson. Autonomous vehicles. Proceedings of IEEE, 84, 8, 1147-1164, 1996. [35]K. Koskinen, A . Halme, and A . Tunaanen. Perception and navigation for system for autonomous mobile applications (the panorama project)-experiance and results. Proceedings of the Tampere International Conference on Machine Automation, 111-126, Finland, 1994. [36]J. Benton, S.S. Iyengar, W. Deng, N . Brener, and V.S. Subrahmanian. Tactical route planning: New algorithms for decomposing the map. IEEE, Proceedings of the IX Conference on Mobile Robotics, 1995. [37]J. Michel!. An algorithmic approach to some problems in terrain navigation. Artificial Intelligence, 37, 171-197, 1988.  113  [38]P. Abeels. Reducing impact of forest operations on ecosystem. FAO/IUFRO  Satellite  Meeting, Tampere, Finland, August, 1995. [39]T. Korhonen. Mechanized harvesting operations in finland. FAO/IUFRO Satellite Meeting,  Tampere, Finland, August, 1995. [40]R.L. Keeney and H . Raiffa. Decisions with multiple objectives: Preferences and value tradeoffs. Wiley, New York, 1976. [41]B.S. Stewart. Heuristic Search With a General Order Relation. PhD thesis, Dept. of Computer Science, University of Virginia, 1988. [42]K. Fujimura. Path planning with multiple objectives. IEEE Robotics and Automation  Magazine, 1996. [43]B.S. Stewart and C.C. White III. Multiobjective a*. Journal of ACM, 38, 4, 775-814, 1991. [44]I. Sommersville. Software Engineering. Addison-Westley, 1995.  [45]J. Borenstein, H . R. Everett, and L . Feng. Navigating Mobile Robots: Sensors and Techniques. A K Peters, Ltd., Wellesley, Massachusetts, 1996. [46]D. Singh and H . K. Grewal. Autonomous vehicles using wadgps. In Intelligent Vehicles  '95 Symposium, IEEE, Detroit, Michigen, 1995. [A1]IEEE Software, 11 (2), A special issue on requirements engineering., March 1994. [48]H. Gomaa. Software Design Methods for Concurrent and Real-Time Sustems. AddisonWesley, 1993. [49]P. Ward and S. Mellor. Structured Development for Real-Time Systems. Prentice Hall, Englewood Cliffs, N.J., 1985. [50]P. Jacobsen. Control of a tracked vehicle. Master's thesis, Dept. of Electrical Engineering, U B C , 1995.  114  


Citation Scheme:


Citations by CSL (citeproc-js)

Usage Statistics



Customize your widget with the following options, then copy and paste the code below into the HTML of your page to embed this item in your website.
                            <div id="ubcOpenCollectionsWidgetDisplay">
                            <script id="ubcOpenCollectionsWidget"
                            async >
IIIF logo Our image viewer uses the IIIF 2.0 standard. To load this item in other compatible viewers, use this url:


Related Items