STREAM FUNCTION PATH PLANNING AND CONTROL FOR UNMANNED GROUND VEHICLES Except where reference is made to the work of others, the work described in this dissertation is my own or was done in collaboration with my advisory committee. This dissertation does not include proprietary or classified information. _______________________________ Robert L. Daily Certificate of Approval: ______________________________ Nels Madsen Professor Mechanical Engineering ______________________________ David M. Bevly, Chair Associate Professor Mechanical Engineering ______________________________ John Y. Hung Professor Electrical and Computer Engineering ______________________________ George T. Flowers Interim Dean Graduate School STREAM FUNCTION PATH PLANNING AND CONTROL FOR UNMANNED GROUND VEHICLES Robert L. Daily A Dissertation Submitted to the Graduate Faculty of Auburn University in Partial Fulfillment of the Requirements for the Degree of Doctor of Philosophy Auburn, Alabama August 9, 2008 iii STREAM FUNCTION PATH PLANNING AND CONTROL FOR UNMANNED GROUND VEHICLES Robert L. Daily Permission is granted to Auburn University to make copies of this dissertation at its discretion, upon request of individuals or institutions and at their expense. The author reserves all publication rights. __________________________________ Signature of Author __________________________________ Date of Graduation iv DISSERTATION ABSTRACT STREAM FUNCTION PATH PLANNING AND CONTROL FOR UNMANNED GROUND VEHICLES Robert L. Daily Doctor of Philosophy, August 9, 2008 (M.S., Auburn University, 2005) (B.S., Auburn University, 1999) 188 Typed Pages Directed by David M. Bevly This research develops a harmonic potential field based path planner and controller. The first component of the dissertation compares several analytical and numerical potential field generation methods. To overcome limitations present in the existing methods, two new methods are developed to meet the specific requirements of this research (generic shape obstacles and an explicit stream function). In particular, an analytic method to combine circles and create generic shaped obstacles is presented. Additionally, a numeric technique to directly calculate a potential field stream function is developed. v The second part of this research extends the traditional potential field controller to track a desired streamline as well as the potential field gradient. In addition to tracking a streamline, the reference path is also modified to maximize safety as the vehicle is driving. In particular, a separate harmonic potential field is created for the desired speed. This speed is low near obstacles and high in the open field. The lateral acceleration of the vehicle is also limited by reducing the steer angle and desired speed whenever an acceleration threshold is crossed. Finally, to create a buffer between the vehicle and obstacles, whenever the vehicle is close to an obstacle the desired streamline is shifted away from the obstacles. Together these three real-time modifications to the controller keep the vehicle safely away from obstacles and within its handling limitations. vi ACKNOWLEDGEMENTS The author would like to thank his entire family for their support throughout his school career. Without their encouragement, this dream would have died long ago. He also wishes to express his gratitude to each of the members of his advisory committee, both the members present at the start of the process and the ones present for its conclusion. Their support leading up to and during the creation of this dissertation has been priceless. In particular, the committee chair, Dr. David Bevly, has provided advice, encouragement, and perhaps most importantly the opportunity to work on incredible research projects. A great research lab has been built during the author?s time working for Dr. Bevly. Although their signatures are not on this dissertation for unforeseeable reasons, Dr. A. S. Hodel, and Dr. Subhash Sinha provided valuable insight into the subjects presented in this dissertation both in and out of class. In addition, the author would like to thank the many other professors he has worked for or studied under during his career at Auburn University. This dissertation is the culmination not only of the presented research, but also of many years of education. He would also like to thank Ms. Susan Wooden for her encouragement during this process and for introducing him to this field. Finally, the author would like to thank the past and present members of the GPS and Vehicle Dynamics Lab at Auburn University. Their help has been invaluable in tackling the wide range of challenges that developed throughout his career in the lab. vii Style manual or journal used: Journal of Dynamic Systems, Measurement, and Control. Computer software used: Microsoft Word 2007. viii TABLE OF CONTENTS LIST OF TABLES .................................................................................................................. xi LIST OF FIGURES ................................................................................................................ xii NOMENCLATURE ............................................................................................................. xviii 1 INTRODUCTION .............................................................................................................. 1 1.1 Prior Work ............................................................................................................2 1.2 Contributions.........................................................................................................6 2 BACKGROUND ................................................................................................................ 8 2.1 Potential Flow .......................................................................................................8 2.1.1 Analytical Solutions ...................................................................................11 2.1.2 Numerical Solutions...................................................................................20 2.2 Vehicle Model .....................................................................................................26 3 OBSTACLE REPRESENTATION ....................................................................................... 32 3.1 Analytical Solutions ............................................................................................33 3.1.1 Small Sources in Flow ...............................................................................35 3.1.2 Circle Theorem Method .............................................................................39 3.1.3 Flow Velocity Average Method.................................................................45 ix 3.1.4 Flow Potential Average..............................................................................52 3.2 Numerical Solutions............................................................................................68 3.2.1 Dirichlet Boundary Condition....................................................................68 3.2.2 Neumann Boundary Condition ..................................................................77 3.2.3 Stream Function Boundary Condition .......................................................83 4 CONTROLLER DEVELOPMENT ...................................................................................... 92 4.1 Bicycle Model Vehicle Controller ......................................................................92 4.1.1 Uncontrollable Speed .................................................................................94 4.1.2 Constant Closed-Loop Pole Controller ....................................................100 4.1.3 LQR Design .............................................................................................106 4.2 Lateral Control to Reference Circle ..................................................................110 4.2.1 Reference Circle Calculation ...................................................................117 4.2.2 Controller Response .................................................................................121 5 PATH MODIFICATION ................................................................................................. 131 5.1 Reference Speed................................................................................................131 5.2 Reference Streamline Shifting ..........................................................................142 6 CONCLUSIONS ............................................................................................................ 154 6.1 Overall Results ..................................................................................................154 6.2 Future Work ......................................................................................................158 x 6.2.1 Potential Field ..........................................................................................158 6.2.2 Vehicle Controller ....................................................................................161 BIBLIOGRAPHY ................................................................................................................ 165 xi LIST OF TABLES Table 2.1 Lateral vehicle model parameters ..................................................................... 28 Table 2.2 Closed-loop speed model parameters ............................................................... 30 Table 3.1 Potential field method calculation times ........................................................... 90 xii LIST OF FIGURES Figure 2.1 Uniform flow complex potential ..................................................................... 12 Figure 2.2 Source flow complex potential ........................................................................ 13 Figure 2.3 Vortex flow complex potential ........................................................................ 15 Figure 2.4 Circular obstacle in uniform flow.................................................................... 16 Figure 2.5 Circle Theorem coordinate transformation ..................................................... 18 Figure 2.6 Circular obstacle in source/sink flow .............................................................. 19 Figure 2.7 Four wheel vehicle model schematic .............................................................. 27 Figure 3.1 Source and sink potential................................................................................. 34 Figure 3.2 Source in uniform flow .................................................................................... 36 Figure 3.3 Source in source/sink flow .............................................................................. 38 Figure 3.4 Two vertical, separated circles in uniform flow calculated using the Circle Theorem method ............................................................................................................... 40 Figure 3.5 Two vertical, touching circles in uniform flow calculated using the Circle Theorem method ............................................................................................................... 41 Figure 3.6 Five vertical, touching circles in uniform flow calculated using the Circle Theorem method ............................................................................................................... 42 Figure 3.7 Two horizontal, separated circles in uniform flow calculated using the Circle Theorem method ............................................................................................................... 43 xiii Figure 3.8 Two horizontal, touching circles in uniform flow calculated using the Circle Theorem method ............................................................................................................... 44 Figure 3.9 Five horizontal, touching circles in uniform flow calculated using the Circle Theorem method ............................................................................................................... 45 Figure 3.10 Two vertical, separated circles in uniform flow calculated using the Flow Velocity Average method ................................................................................................. 48 Figure 3.11 Two vertical, touching circles in uniform flow calculated using the Flow Velocity Average method ................................................................................................. 49 Figure 3.12 Five vertical, touching circles in uniform flow calculated using the Flow Velocity Average method ................................................................................................. 50 Figure 3.13 Two horizontal, separated circles in uniform flow calculated using the Flow Velocity Average method ................................................................................................. 51 Figure 3.14 Two horizontal, touching circles in uniform flow calculated using the Flow Velocity Average method ................................................................................................. 51 Figure 3.15 Five horizontal, touching circles in uniform flow calculated using the Flow Velocity Average method ................................................................................................. 52 Figure 3.16 Two vertical, separated circles in uniform flow calculated using the Flow Potential Average method ................................................................................................. 54 Figure 3.17 Two vertical, touching circles in uniform flow calculated using the Flow Potential Average method ................................................................................................. 56 Figure 3.18 Five vertical, touching circles in uniform flow calculated using the Flow Potential Average method ................................................................................................. 57 xiv Figure 3.19 Two horizontal, separated circles in uniform flow calculated using the Flow Potential Average method ................................................................................................. 58 Figure 3.20 Two horizontal, touching circles in uniform flow calculated using the Flow Potential Average method ................................................................................................. 58 Figure 3.21 Five horizontal, touching circles in uniform flow calculated using the Flow Potential Average method ................................................................................................. 59 Figure 3.22 Flow Potential Average method, vertical/horizontal obstacle transition....... 61 Figure 3.23 Square obstacle formed of four touching circles calculated using the Flow Potential Average method ................................................................................................. 63 Figure 3.24 Square obstacle formed of eight touching circles calculated using the Flow Potential Average method ................................................................................................. 64 Figure 3.25 Square obstacle formed of four separated circles calculated using the Flow Potential Average method ................................................................................................. 65 Figure 3.26 Averaging stream function, sample map ....................................................... 67 Figure 3.27 Start and goal potential obstacle potential field, using Dirichlet boundary conditions .......................................................................................................................... 70 Figure 3.28 Start, goal, and zero potential obstacle potential obstacle potential field, using Dirichlet boundary conditions ........................................................................................... 72 Figure 3.29 Start, goal, and positive potential obstacle potential field, using Dirichlet boundary conditions .......................................................................................................... 73 Figure 3.30 Start, goal, and three obstacles potential field, using Dirichlet boundary conditions .......................................................................................................................... 74 xv Figure 3.31 Goal and three obstacles potential field, using Dirichlet boundary conditions ........................................................................................................................................... 76 Figure 3.32 Start and goal potential field, using Neumann boundary conditions............. 78 Figure 3.33 Start, goal, and obstacle potential field, using Neumann boundary conditions ........................................................................................................................................... 79 Figure 3.34 Start, goal, and three obstacles potential field, using Neumann boundary conditions .......................................................................................................................... 80 Figure 3.35 Start and goal boundary values for the stream function boundary condition 85 Figure 3.36 Start and goal potential field, using the stream function boundary condition 87 Figure 3.37 Start, goal, and obstacle potential field, using the stream function boundary condition ........................................................................................................................... 88 Figure 3.38 Start, goal, and three obstacles potential field, using the stream function boundary condition ........................................................................................................... 89 Figure 4.1 Bicycle model pole characteristics .................................................................. 94 Figure 4.2 Bicycle model real poles and zeros ................................................................. 95 Figure 4.3 Bicycle model real/complex transition speed .................................................. 98 Figure 4.4 Norm of inverse of bicycle model controllability gramian (Vcrit = 5.8 m/s) ... 99 Figure 4.5 Bicycle model control gains: constant closed-loop poles (Vcrit = 5.8 m/s) .... 101 Figure 4.6 Bicycle model response: constant closed-loop poles, varying speed ............ 103 Figure 4.7 Bicycle model response: constant closed-loop poles, constant speed ........... 104 Figure 4.8 Bicycle model response: constant closed-loop poles, with noise .................. 105 Figure 4.9 Bicycle model control gains using LQR design (Vcrit = 5.8 m/s) .................. 107 Figure 4.10 Bicycle model and LQR closed-loop real poles and zeros .......................... 108 xvi Figure 4.11 Bicycle model response: LQR, varying speed ............................................ 109 Figure 4.12 Potential field gradient control .................................................................... 112 Figure 4.13 Reference circle additional control states .................................................... 114 Figure 4.14 Radius calculation validation ...................................................................... 120 Figure 4.15 Trajectory of the circle controller validation, with a linear vehicle model . 121 Figure 4.16 States of the circle controller validation, with a linear vehicle model ........ 123 Figure 4.17 Trajectory of the circle controller validation, with a nonlinear vehicle model ......................................................................................................................................... 124 Figure 4.18 States of the circle controller validation, with a nonlinear vehicle model .. 125 Figure 4.19 Trajectory of the streamline controller response ......................................... 126 Figure 4.20 States of the streamline controller response ................................................ 128 Figure 4.21 Tire curves from the streamline controller response ................................... 129 Figure 5.1 Reference speed potential using Dirichlet boundary conditions ................... 133 Figure 5.2 Trajectory of the streamline controller response, adjusting speed ................ 134 Figure 5.3 States of the streamline controller response, adjusting speed ....................... 136 Figure 5.4 Tire curves from the streamline controller response, adjusting speed .......... 138 Figure 5.5 Trajectory of the streamline controller response, limiting lateral acceleration ......................................................................................................................................... 140 Figure 5.6 Tire curves from the streamline controller response, limiting lateral acceleration ..................................................................................................................... 140 Figure 5.7 States of the streamline controller response, limiting lateral acceleration .... 141 Figure 5.8 Reference speed gradient ............................................................................... 143 Figure 5.9 Reference streamline shift near an obstacle .................................................. 145 xvii Figure 5.10 Trajectory of the streamline controller response, shifting the reference streamline ........................................................................................................................ 147 Figure 5.11 States of the streamline controller response, shifting the reference streamline ......................................................................................................................................... 149 Figure 5.12 Streamline controller response with a large streamline shift gain ............... 151 Figure 5.13 Streamline controller response for a dense obstacle field ........................... 152 xviii NOMENCLATURE ? ......................................................................................................... vehicle side slip angle ? ................................................................................................................ vehicle steer angle ? ..................................................................................................................... vehicle course ? .................................................................................................................... stream function ? ................................................................................................................ velocity potential ?V ................................................................................................... reference speed potential ? ................................................................................................................... vehicle heading C??T .................................................................................................... tire cornering stiffness C?? .................................................................................................... axle cornering stiffness E ........................................................................................................................ East position Fymax? ........................................................................................... maximum lateral tire force Iz ........................................................................................... vehicle yaw moment of inertia N ..................................................................................................................... North position R .......................................................................................................................... Turn radius T .............................................................................................................. vehicle track width xix V ....................................................................................................................... vehicle speed VE ............................................................................................... Potential field east velocity VN ............................................................................................ Potential field north velocity a................................................................................ distance from front axle to vehicle CG ay ................................................................................................ lateral vehicle acceleration b................................................................................. distance from rear axle to vehicle CG m ........................................................................................................................ vehicle mass r ................................................................................................................... vehicle yaw rate yerr .......................................................................................................... vehicle lateral error 1 1 INTRODUCTION Potential fields are a method of path planning and control for directing a robot through an obstacle field to a goal location. To accomplish this, an artificial potential field is created over the range of the world map. Based on this potential field, a controller is developed for the robot to reach the goal, typically by following the gradient of the potential field. Potential fields offer an alternative to the traditionally independent operations of path planning and vehicle control. Typical path planners calculate a collision free route from a vehicle?s current location to a goal. This route is represented (either directly or after a translation step) as a reference trajectory, e.g. a series of waypoints, lines and arcs, or clothoids. The reference trajectory is passed to the vehicle controller, which drives the vehicle onto the path. Usually, information other than the reference trajectory is not passed to the controller. An example of this method of path planning and control was represented in Auburn University?s entries in the DARPA Grand Challenge series [Daily 2006]. Potential field techniques differ from traditional path planning and control methods in that a potential field over the entire world map is created and available to the controller. The potential field is directly used to compute the control effort. Because the potential field is global, the controller has more information about the vehicle?s environment than simply a reference trajectory. 2 Harmonic potential fields improve on the traditional potential field concept. In particular, early potential fields suffered from local minima into which the controlled robot could become stuck. Harmonic potential fields are also used by fluid dynamicists to model ideal fluid flow. Several methods have been developed to study flow around obstacle fields. These same methods can be adapted into paths for a vehicle to follow. This research utilizes the stream function component of the potential field to control the vehicle. Contours of the stream function are streamlines, which are the paths the fluid or vehicle should follow. Tracking the streamlines is the basis for the vehicle navigation controller developed in this work. This dissertation is divided into six chapters. The Introduction presents prior and ongoing research in the potential field control area as well as the specific contributions of this research. Chapter 2 discusses background information about potential field theory, specifically with respect to the harmonic potential fields used in this research as well as the vehicle model used in the simulation studies. Chapter 3 examines different methods of computing the potential fields and develops new potential field generation techniques. Chapter 4 develops the streamline controller and discusses controllability issues related to the vehicle model. Chapter 5 modifies the streamline controller to improve safety by incorporating vehicle dynamic limitations. Finally, the Conclusion (Chapter 6) summarizes the work and provides avenues for continuing this research. 1.1 Prior Work Khatib first proposed the concept of potential field control [Khatib 1985]. His research was directed toward robotic manipulators as well as mobile robots. The idea 3 behind the potential field was, ?The manipulator moves in a field of forces. The position to be reached is an attractive pole for the end effector and obstacles are repulsive surfaces for the manipulator parts.? [Khatib 1985] The attractive and repulsive potentials were simple functions based on the distance to either the goal or an obstacle. The total potential was the sum of the individual goal and obstacle potentials. The control effort applied to the robot was the gradient of the potential field (magnitude and direction), along with dissipative terms to ensure asymptotic stability. This potential field controller provided a simple method to reach a goal while avoiding obstacles. However, it suffered from the fact that local minima were present in the potential field, which could cause the robot to be attracted to the local minima instead of the goal location. Several attempts were made to correct the local minima problem. For example, Kholsa and Volpe modified the repulsive potential such that the contours tended to circles as distance from the obstacles increased [Kholsa 1988]. Another approach to eliminate the local minima was to generate the potential field by solving the Laplace equation, i.e. harmonic functions. One method to generate harmonic potential fields is to calculate analytic solutions to the Laplace equation. Analytic harmonic functions are typically modeled to represent fluid flow, i.e. hydrodynamic potentials. Akishita, Kawamura, and Hayashi were the first to propose analytic solutions to the Laplace equation for potential field control [Akishita 1990]. Obstacles were modeled as doublets (defined in Section 2.1.1) inserted into the flow. Kim and Khosla studied using distributed sources as opposed to the traditional point source to approximate polygon shaped obstacles [Kim 1992]. More recently Waydo and Murray inserted circular obstacles into analytic potential fields using the Circle Theorem [Waydo 4 2003]. Their research also examined the effects of multiple obstacles in the potential field. Sato and Connolly, Burns, and Weiss independently pioneered the use of numerical solutions to the Laplace equation, specifically using Dirichlet boundary conditions [Connolly 1990, Sato 1993]. Dirichlet boundary conditions approximate Khatib?s potential, i.e. the robot is repelled from an obstacle. However, as Section 3.2.1 describes, they may not be the most effective boundary condition. Keymeulen and Decuyper studied different types of boundary conditions, in particular Dirichlet and Neumann boundary conditions [Keymeulen 1994]. Specifically, they studied how the number of singular points in the potential field varies with the boundary conditions. Connolly and Grupen extended the boundary conditions to combining both Dirichlet and Neumann boundary conditions in a single potential field [Connolly 1993]. In addition to the fluid dynamics analogy, other physical phenomena have also been studied as foundations for the potential field. For example Masoud, Masoud, and Bayoumi employed a stress analogy utilizing biharmonic functions for the potential fields [Masoud 1994]. Biharmonic potential fields do not have the large streamline curvature that may occur in harmonic potential fields. A heat flow analogy has also been proposed [Wang 2000]. In the heat flow potential, the robot follows the heat flux. Finally, Guldner and Utkin utilized an electrostatic potential field [Guldner 1995]. Electrostatic potentials are essentially hydrodynamic potentials consisting of only sources and sinks (described in Sections 2.1.1 and 3.1.1). The research into potential field control has not only concentrated on the potential field development. Efforts have also been placed on incorporating potential field 5 techniques into a wider range of control schemes. One area of interest is incorporating moving obstacles into the potential fields [Ge 2002, Newman 1987]. Analytic harmonic functions can also be modified to allow moving obstacles, [Akishita 1990, Waydo 2003]. Another area of interest is adapting potential field methods for cooperative and multiple robot control [Song 2002, Sullivan 2003]. The potential field method has also been modified to deflect a virtual beam as opposed to controlling the robot directly [Hesse 2007, Quinlan 1993]. The robot then follows the beam as a path. Potential field methods have also been applied to underwater vehicles [Eichhorn 2004]. Potential field techniques have also been adapted to drive full size vehicles as opposed to small mobile robots. In particular, Kyriakopoulos, Kakambouras, and, Krikelis considered control of nonholonomic vehicles [Kyriakopoulos 1995]. This is significant because they began to appreciate the fact that most vehicles cannot react instantaneously to follow the gradient of the potential field. Accounting for vehicle dynamics is a critical aspect of the research presented in this dissertation, although the approach uses a streamline tracking controller instead of the cascaded holonomic/nonholonomic controllers employed by Kyriakopoulos. Rossetter and Gerdes applied potential field control to automobile lane keeping [Rossetter 2003]. Their research did not use traditional goal/obstacle potential fields. Instead, a parabolic potential centered on the lane was created. Finally, off road navigation of full size vehicles has also been studied [Shimoda 2005]. This research also did not use traditional potential fields. The potential field was defined over a trajectory space consisting of steer angle and speed axes. It was created by summing individual potential fields corresponding to obstacles, vehicle constraints, and speed. The vehicle control 6 calculation was also non-traditional for potential fields. The steer angle and speed applied to the vehicle were the location in the trajectory space where the potential field was minimized. This results in the potential field effectively serving as a cost function for the steer angle and speed commands. 1.2 Contributions This research advances the body of knowledge in two broad areas: potential field generation and controlling a vehicle to a streamline. First, different analytical and numerical methods of generating potential fields are compared. The analytic comparisons concentrate on various methods to represent generic shaped obstacles. Specifically the flow resulting from using a combination of circles to approximate an obstacle is studied. The numeric comparisons investigate how different boundary conditions along the obstacle and world edge affect the flow. While these potential field generation techniques are not novel, an in depth study comparing analytic and numeric solutions has not been previously presented. Additionally, to overcome the shortcomings of the existing techniques, two new potential field generation techniques, one analytical and one numerical, are proposed and demonstrated. Second, the traditional potential field controller is modified to account for the dynamics of high speed front wheel steered (as opposed to skid steered) vehicles. Developing this controller involves several new techniques. Specifically, for the vehicle controller, an issue with the model becoming uncontrollable at a critical speed is exposed and a method to counter the problem is proposed. The potential field tracking system is also augmented to follow a reference streamline in addition to the typical potential field 7 gradient. This prohibits tracking errors (due to system dynamics) from compounding when the traditional potential field gradient controller is used. The streamline tracking controller is then augmented to account for the specific dynamics of ground vehicles. In particular, the reference speed and lateral acceleration are limited and the reference streamline is shifted as the vehicle travels. To verify the potential field and control techniques developed in this dissertation, simulation tests are performed using a widely accepted vehicle model. 8 2 BACKGROUND Potential flow theory forms the building block for the path planning algorithms in this research. Before the path planning techniques are described, some foundational knowledge is presented. The underlying theory behind potential flow, specifically in relation to fluid dynamics, is explained along with the analytical and numerical techniques used to calculate the flow. Additionally, the vehicle model used in the simulation study is presented. 2.1 Potential Flow Potential function techniques have long been used to model certain types of fluid flow [Currie 1993, Milne-Thomson 1996]. In particular, the fluid flow must be in steady state (no change with time), incompressible (no change in density), inviscid (viscous terms are negligible), and irrotational (zero vorticity) to be modeled with potential functions. For a fluid meeting these assumptions with position and velocity vectors defined as ??EN??x I J (2.1) and ??ENVV??V I J (2.2) respectively, the continuity equation (conservation of mass) becomes 0NE VVEN??? ? ? ???V? (2.3) 9 i.e. the divergence of the velocity vector field is identically zero. The irrotational assumption results in 0N EV VEN? ?? ? ? ? ???V (2.4) This is equivalent to stating that the velocity vector field is conservative, i.e. a vector field is conservative if and only if its curl is identically zero (assuming the vector field is defined over a simply connected region) [Barr 1997]. For the velocity vector field to be conservative, a scalar velocity potential, ?, must exist [Barr 1997] such that ??EN??? ??? ? ? ?V I J (2.5) i.e. , ENVVEN?????? (2.6) Contours of the velocity potential are known as equipotential lines. Inserting Equation (2.6) into (2.3) yields the Laplace equation, 222 0EN??? ??? ? ? ? (2.7) 2? is known as the Laplace operator. This is the equation that must be satisfied to generate potential flow. The stream function, ?, is defined as ??NE??????V I J (2.8) i.e. , ENVVNE????? ? ? (2.9) 10 Note that by inserting Equation (2.9) into (2.4) the stream function also satisfies the Laplace equation, 222 0 xy??? ??? ? ? ? (2.10) Contours of the stream function are known as streamlines. Streamlines are tangent to the fluid flow; the fluid flow follows the streamlines. If a velocity vector field is derived from a potential field, its corresponding streamlines and equipotential lines will always be perpendicular, i.e. Figure 2.1 below. The velocity potential and stream function are typically combined into a complex potential, Fi???? (2.11) along with a complex position, z E iN?? (2.12) where E and N are the east and north positions respectively. To determine both the velocity potential and stream function, the two partial differential equations, Equations (2.7) and (2.10) must be solved. Both of these equations are the Laplace equation. Solutions to the Laplace equation are known as harmonic functions. A critical property of harmonic functions is that they have no local minima or maxima interior to the flow [Needham 1997]. This property makes harmonic functions quite popular in the area of potential field control as was mentioned earlier. Two general methods to solve the Laplace equation exist: analytical and numerical. 11 2.1.1 Analytical Solutions Analytic solutions to the Laplace equation have been developed by fluid dynamicists to model common fluid flows. In particular, for this research uniform, source/sink, vortex, and doublet flows are of interest. Uniform flow is flow in a straight line. Given a velocity magnitude, U, and a direction, ? (from the E-axis), the complex potential for uniform flow is iF Ue z??? (2.13) and the velocity is ??c o s s inUU????V I J (2.14) The velocity potential, stream function, and contour lines for uniform flow are shown in Figure 2.1. Recall that the fluid flow follows the streamlines and thus models straight line flow. This flow is not directly useful in vehicle path planning, but it is a simple flow for testing techniques and is used as the basis for more complex flows. 12 Figure 2.1 Uniform flow complex potential Sources or sinks model flow from a faucet or to a drain, except the flow is two- dimensional. This flow allows fluid to be added to or removed from the flow, violating the conservation of mass. In particular, a singularity exists at the location of the source or sink. Given a strength C, the potential for source/sink flow is ? ?ln iF C ze ?????? ?? (2.15) The sign of C determines whether the potential is a source or sink; a positive C is a source (fluid is introduced into the flow). Note that at the location of the source/sink the potential and velocity are both infinite. Also, note that this form is slightly modified from most references by the introduction of a rotation angle, ?. The rotation angle does not change the flow created; it simply rotates the stream function discontinuity. This is ?? 13 important when the gradient is calculated numerically as the discontinuity produces an incorrect gradient. The rotation allows the discontinuity to be out of the region of interest. The potential for source flow is shown in Figure 2.2. The only difference for sink flow is that the images are mirrored about the E-N plane. Note that the flow is from low potential to high. This is slightly counterintuitive physically, but mathematically correct. For path planning, the vehicle will drive up the slope of the potential instead of following it down. Figure 2.2 Source flow complex potential Sources and sinks are used to model start and goal locations in path planning. Sources can also be used as a simple obstacle model. The flow is directly away from the source thus inhibiting vehicle paths from approaching the source. This direct repulsion represents electrostatic potentials more than fluid flow. Source and sink flow also forms Discontinuity ?? 14 the basis of doublet flow that is used for adding circular obstacles to a potential field as is discussed below. Vortex flow simply swaps the real and imaginary parts, i.e. the stream function and velocity potential, of source/sink flow. This flow represents rotation in the fluid as happens near drains. The equation for vortex flow is lnF iC z? (2.16) Note that a rotation angle could be added as with the source/sink flow, but because vortex flow is not directly used in path planning, it is not included. The potential for vortex flow is shown in Figure 2.3. The flow circles the vortex with streamlines of constant radius. This creates a singular point at the vortex that has infinite vorticity, just as the source/sink has infinite flow rate. Vortex flow is not used in path planning, but it is the potential field version of tracking a circle, which is the basis of the vehicle controller, described in Chapter 4 and so is used in controller testing. 15 Figure 2.3 Vortex flow complex potential The final basis flow that this research relies upon is doublet flow. A doublet is created when a source and sink lie infinitely close. In particular, a doublet can be inserted into uniform flow to produce a circular obstacle of radius a: 2aF U z z???????? (2.17) A circular obstacle in uniform flow is shown in Figure 2.4. Note that the streamlines form a circle. Inside this circle, the doublet can be seen. However, since fluid cannot pass through a streamline the flow inside the circle is unimportant. In practice, only flow outside of the circle is calculated. Doublets form the basis for analytically representing obstacles in potential fields. However, the potential must be generalized to insert a circle into any existing flow. 16 Figure 2.4 Circular obstacle in uniform flow Milne-Thomson introduced the Circle Theorem which allows a circular obstacle to be inserted into a generic flow [Milne-Thomson 1996]. The resulting potential is ? ? 2 uuaF F z F z???? ???? (2.18) where Fu is the undisturbed potential. F is the conjugate analytic function defined as ? ? ? ?F z F z? (2.19) Note that inserting a circular obstacle into uniform flow satisfies Equation (2.18). Also note that on the circle edge, |z| = a, the stream function is identically zero; thus the circle is a streamline. 17 ? ? ? ? ? ? ? ?2 20 u u u u u uza z zzF F z F F z F F z F z zz ? ?? ?? ??? ? ? ? ? ? ? ? ? ? ??? ?? ???? (2.20) The procedure for programmatically computing the complex potential is described below and represented in Figure 2.5. 1. The undisturbed potential is calculated over a grid of points (?). 2. The original grid points are transformed using the conjugate analytic function argument, 2 trans az z? . For this transformation, only points that satisfy 220.9E N a?? are considered. As is previously stated, the potential inside the circle is not important. The slight overlap (0.9a) aids interpolation in the next step. The transformed points (?) are now inside the circle. 3. The conjugate of the undisturbed potential is interpolated over the transformed coordinates and becomes the conjugate analytic potential. This interpolation is made more difficult due to the transformed coordinates being irregularly spaced. In Matlab, the command ?griddata? must be used in place of ?interp2?. 4. The undisturbed potential is added to the conjugate analytic potential to produce the total potential. 18 Figure 2.5 Circle Theorem coordinate transformation As an example of the Circle Theorem, consider a circle inserted between a source and sink. To keep the method general, the potential is not analytically calculated but determined using the algorithm above. This potential is shown in Figure 2.6. Notice that the circle is a streamline. 19 Figure 2.6 Circular obstacle in source/sink flow The different flows presented above provide the basis for the analytic methods to calculate potential field based vehicle paths. Because the Laplace equation is linear, a linear combination of harmonic functions is also harmonic. This concept has already been used when the circular obstacle in source/sink flow is generated by superimposing a source and sink (explained in more detail is Section 3.1) and then applying the Circle Theorem. The simple basis potentials described above can be combined into quite complicated world maps and corresponding potential fields. A downside of this approach is that a streamline in a basis potential is not guaranteed to be a streamline in the overall potential. This causes problems when an obstacle edge is intended to be a streamline, e.g. the Circle 20 Theorem. Section 3.1 of this dissertation investigates many methods of combining the basis potentials to generate streamlines that closely match the true obstacle shape. 2.1.2 Numerical Solutions The first step in developing numerical solutions to the Laplace equation is to discretize the equation. To do this, a grid of solution points is created over the solution area. There are many methods to generate the solution grid [Ferziger 2002]. However, this research utilizes a simple square grid. A center difference is used to approximate the second derivatives. The Laplace equation can be generalized by including a forcing function, G, to become the Poisson equation, 222 GEN??? ??? ? ? ? (2.21) Note that if the forcing function is zero, the Poisson equation simplifies to the Laplace equation. The forcing function is used in the numeric solution algorithm. The discrete Poisson equation is 1 , , 1 , , 1 , , 12 ,,2222j k j k j k j k j k j kj k j kGEN? ? ? ? ? ?? ? ? ? ?? ? ? ?? ? ? ??? (2.22) where j and k are the indices of the east and north coordinates respectively and ?E and ?N are the east and north grid point spacing. Note that the discrete solution is in general real. The velocity potential is calculated, but not the stream function. The discrete Poisson equation can be solved directly by rearranging the M equations (where M is the total number of solution points) into matrix form and inverting an M?M matrix to solve for the potential at each point in the solution grid. However, the solution grid is typically large enough that the matrix inversion becomes impractical to perform 21 quickly. Instead, an approximate solution is computed recursively. This research uses a multi-grid Gauss-Seidel method with successive over-relaxation to solve the Poisson equation. The Gauss-Seidel method is an iterative approach to solving discrete partial differential equations [Ferziger 1998]. For the discrete Poisson equation, the algorithm involves rearranging Equation (2.22) to solve for the potential in terms of its surrounding potential and the forcing function: ? ? ? ?? ?2 2 2 2, 1 , 1 1 , 1 , , , 222j k j k j k j k j kjk E N G E N EN ? ? ? ?? ? ? ? ?? ? ? ? ? ? ? ?? ? ? ? (2.23) This solution is then repeated for each of the points in the solution grid. Once the entire grid is solved, the residual error, R, is calculated, 1 , , 1 , , 1 , , 1 ,, 2222j k j k j k j k j k j kj k j kRG EN? ? ? ? ? ?? ? ? ?? ? ? ?? ? ??? (2.24) If the norm of the residual is small enough, the algorithm is complete; otherwise, Equation (2.23) is used over the solution grid again. The residual norm used to test for convergence is scaled by the mean of ?E2 and ?N2 to remove the dependence on the grid spacing. Successive over-relaxation modifies Equation (2.23) to speed up convergence [Ferziger 2002]: ? ? 1, , ,1mmj k j k j k? ? ? ???? ? ? (2.25) where m is the iteration index and ,jk? is the output of Equation (2.23). The value of the relaxation factor, ?, can vary between 0 and 2. The optimal relaxation factor (for fastest 22 convergence) depends on the particular problem; for this work, ? ? 1.4 is used. Note that when ? ? 1 the method simplifies to the Gauss-Seidel method. Multi-grid methods also speed up convergence. However, they work on the entire solution grid instead of at the individual solution point level. Even with successive over- relaxation, a solution point in the Gauss-Seidel method is only affected by its immediate neighbors. Therefore, it can take an excessively long time for a solution to propagate from known points to the entire solution grid. Multi-grid methods attempt to address this problem by changing the number of points in the solution grid [Briggs 1987]. When there are fewer points in the solution grid, information propagates through the grid faster. Ultimately, the coarse grid information must be passed back to the original, fine grid. In particular, once a candidate solution, ?*, for the Poisson equation is calculated, its residual can be determined using Equation (2.24). Although the exact solution is not known, a solution error can be defined, e*? ? ??? (2.26) The Poisson operator is linear, allowing the solution error to be related to the residual, i.e. the solution error satisfies the Poisson equation with the residual as a forcing function: ? ?2 e 2 2 * G G R R? ? ?? ? ? ? ? ? ? ? ? (2.27) Note that for compactness Equation (2.27) is written in the continuous operator notation, however the concept applies to the discrete Poisson equation as well. Equation (2.27) allows the residual to be used as a forcing function to calculate the solution error. The residual forcing function is the reason the multi grid algorithm must solve the Poisson equation instead of the Laplace equation. 23 To speed convergence, the residual and solution grid are down-sampled before solving the solution error/residual problem. The calculated solution error is then up- sampled back to the original grid and added to the original candidate solution to produce a better solution. Down-sampling is simply taking every other point in the grid; while up-sampling is a linear interpolation. Additionally, before the down-sampling and after the up-sampling a set number of Gauss-Seidel iterations are run to smooth the solution. The technique to solve the down-sampled Poisson?s equation is to repeat the smoothing and down-sampling until the grid size is small enough to be solved without further down- sampling. Overall, the steps in the multi-grid algorithm are Initialize the Poisson equation solution (typically to zero), and set the forcing function to zero. Compute the residual of the Poisson equation solution, Equation (2.24). While the residual is not below a tolerance (0.0001): While the grid size is not at the minimum size (5?5): Run a set number of smoothing iterations (10) of the Gauss-Seidel with successive over-relaxation algorithm to update the Poisson equation solution, Equations (2.25) and (2.23). Save the Poisson equation solution with the corresponding grid size. Note: the solution at a lower grid size is a correction to this solution. Compute the residual of the Poisson equation solution, Equation (2.24). Down-sample the residual and solution grid to reduce the grid size. Set the forcing function to the down-sampled residual and initialize the down-sampled solution (to zero). When the grid size is at the minimum, run the Gauss-Seidel with successive over-relaxation algorithm until the residual is below a tolerance (0.0001) to 24 produce a solution to the Poisson equation, Equations (2.25) and (2.23). While the grid size is not at the original size: Up-sample the solution to obtain an error correction, which increases the grid size. Add the up-sampled error correction to the Poisson equation solution at the corresponding grid size (saved above) to generate an updated Poisson equation solution. Run a set number of smoothing iterations (10) of the Gauss-Seidel with successive over-relaxation algorithm to update the Poisson equation solution, Equations (2.25) and (2.23). When the grid size is back to the original, compute the residual of the Poisson equation solution, Equation (2.24). When the residual is below the tolerance threshold, the multi-grid algorithm is complete. Boundary conditions are what allow numerical solutions of the Laplace equation to model different flows. The area immediately surrounding the solution grid as well as any features inside the solution grid (starts, goals, and obstacles) must be represented by boundary conditions. The need for boundary conditions is also a major difference between analytical and numeric solutions. Analytic solutions are global in scope, while numeric solutions only exist on a finite space. In particular, two boundary conditions are common in fluid dynamics: Dirichlet and Neumann [Ferziger 2002]. Dirichlet boundary conditions set the edge of the potential function to a specified value. This boundary condition is used to model a start or goal, although unlike an analytical source or sink, the potential of the numerical equivalent is finite. Dirichlet boundary conditions are also often used on the boundary of the solution grid due to their 25 simplicity and fast solution time. Dirichlet boundary conditions can also be used to model simple obstacles similar to using analytical sources as obstacles. Neumann boundary conditions set the derivative of the potential field, i.e. the fluid velocity, at the edge of the solution grid. This is the more common boundary condition fluid dynamicists use to model obstacles and the solution boundary because it has more physical meaning to fluids. Specifically, the fluid velocity vector along an obstacle must be parallel with the obstacle boundary such that no fluid can flow into or out of the obstacle. Numerically, this is accomplished by setting the potential equal to a point along a line perpendicular to the obstacle edge a certain distance into the flow. Therefore, the gradient of the potential must be in the direction of the obstacle edge. Along with modeling an obstacle, this boundary condition can be used to model the solution edge so that fluid does not leave the solution area. The down side of this boundary condition is that it significantly increases the solution time for two reasons. First, the boundary potential changes each iteration, so it effectively must be calculated along with the solution potential. Second, because the boundary potential is equal to its interior neighbor, the boundary does not provide a starting point for the solution to propagate to the remainder of the solution grid. The solution must propagate from only the start and goal location instead of from the entire solution boundary. As with analytical solutions being built from many basis potentials, numerical solutions typically use a combination of boundary conditions. For instance, the type of boundary condition used on obstacle edges greatly influences the character of the resulting potential [Keymeulen 1994]. Section 3.2 of this dissertation describes the 26 different boundary conditions and grades the calculated potentials using the specific requirements of the vehicle control algorithm given in Chapter 4. 2.2 Vehicle Model To test the path planning algorithms, a nonlinear vehicle model is utilized [Daily 2004]. This model has been extensively used and verified to model lateral vehicle dynamics [Rossetter 2003, Smith 1995, Wenzel 2005]. A schematic of the vehicle model is shown in Figure 2.7. The lateral dynamics of this vehicle model are described by ? ? ? ? ? ? ? ? ? ? c o s c o s c o s s in 2 y I F y O F y I R y O R y I F y O F y I R y O R y I F y O F z F F F F r mV Ta F F b F F F F r I ? ? ? ?? ? ? ? ?? ? ? ? ? ? ? ? ? (2.28) where r is the vehicle?s yaw rate (rotation about a vertical axis), ? is the side slip (difference in heading and velocity vector direction), V is the magnitude of the velocity vector, and ? is the steer angle input. The lateral forces, Fy?, are described below and the vehicle parameters, m, a, b, etc., are given in Table 2.1 along with the tire parameters. 27 Figure 2.7 Four wheel vehicle model schematic The lateral force at each tire depends on the slip angle at the tire, ?, and the vertical load on the tire as well as tire and surface properties. Many models of the tire forces exist. For this research the Dugoff model is used [Dugoff 1970]: N E V x V y V r ? ? ? a b T ? L F F y L F ? V L F ? R F ? F y R F V R F ? R R V R R F y R R ? L R V L R F y L R 28 ? ? m a x 2 t a n( ) 2 i f 1 () 1 i f 1 ( ) t a n( ) y T yT F C f F f C ? ? ? ? ? ? ? ? ? ?? ? ? ? ? ? ? ? ? ?? ?? (2.29) Note that Fymax?, the peak lateral force, is a property of the tire/surface interaction and vertical load on the tire, and C??T is the tire cornering stiffness, a property of the tire. Although each tire has its individual peak force and cornering stiffness, typically the two tires on the same axle are identical. Due to vehicle geometry, the front and rear tires often have different properties. Table 2.1 lists the parameters used to simulate the lateral vehicle dynamics. These parameters represent a 1997 Corvette [Rossetter 2003]. Further research into estimating vehicle properties (specifically Dugoff tire properties) is presented in [Daily 2007]. Table 2.1 Lateral vehicle model parameters Variable Parameter Value Units m Mass 1860 kg Iz Vertical moment of inertia 3100 kg?m2 a Distance from center of gravity to front axle 1.37 m b Distance from center of gravity to rear axle 1.43 m T Track width 1.5 m C?FT Front tire cornering stiffness 72500 N/rad C?RT Rear tire cornering stiffness 72500 N/rad FymaxF Front tire peak lateral force 3960 N FymaxR Rear tire peak lateral force 3794 N 29 Note that this vehicle model makes several assumptions; the speed is assumed constant and thus longitudinal forces are negligible and all non-planar motion such as roll, pitch, and vertical acceleration is ignored. While models for the neglected dynamics exist, as long as the vehicle maneuver is not extreme, the neglected dynamics do not significantly alter the vehicle response. The path planning algorithm described below has safe guards to prevent drastic maneuvers. Although Equation (2.28) assumes a constant vehicle speed, an important aspect of the path planning algorithm presented below is the ability to adjust the vehicle?s speed when near obstacles. For this reason, a model for the vehicle?s speed is required. Most vehicles now have a cruise control module and algorithms for controlling vehicle speed are widely used. The goal of this research is lateral control, so instead of redeveloping a vehicle speed model and controller, an existing closed-loop model with a desired speed input and actual speed output developed by Rajamani is used [Rajamani 2006]. This model assumes a first order lag between the desired longitudinal acceleration, throttle input, and actual longitudinal acceleration, and an integrator from longitudinal acceleration to speed. The throttle input comes from a PI-controller. Overall, the closed- loop dynamics from reference speed, Vref, to actual speed, V, are described by the transfer function , s p e e d , s p e e d spe 32e d , s p e e d , s p e e d PI r e f P Iss k s kVV k s k? ?? ?? ? (2.30) When simulated, this transfer function is discretized at the sample rate of 100 Hz. Table 2.2 lists the parameters used when simulating the vehicle, which are taken directly from [Rajamani 2006]. 30 Table 2.2 Closed-loop speed model parameters Variable Parameter Value Units kP,speed Throttle proportional control gain 0.75 1/s kI,speed Throttle integral control gain 0.1875 1/s2 ?speed Longitudinal acceleration time constant 0.5 s One critical note about the combined lateral/longitudinal vehicle model is that it is either nonlinear (if speed is counted as a state in the combined model) or more typically time varying (speed is first calculated independent of lateral dynamics then becomes a parameter in the lateral model). In the second case, typical linear stability criteria are not guaranteed to yield correct results. This aspect of the method?s stability is not considered in this research. In general, the speed varies slowly enough that fixed parameter stability analysis should be valid. However, several methods exist to further examine the time- varying stability [Packard 1993, Tan 2000], and this is a topic that should be studied further in all vehicle control applications. In particular, instability has been observed in unrelated autonomous ground vehicle testing when the vehicle was both turning and accelerating heavily. The exact cause was not determined, but both the lateral and longitudinal systems were independently stable, and for most operation, the combined system was stable. The most likely explanation is the coupling when both modes were simultaneously excited. In addition to the vehicle model, kinematic relationships are needed to determine the vehicle?s global east and north location, E and N respectively, and its heading, ?: sin cos EV NV r ? ? ? ? ? ?? ? ? (2.31) 31 Note that ?, the vehicle?s course, is defined as ? ? ??? (2.32) Therefore, its derivative is r?????? (2.33) with ?? given in Equation (2.28). This vehicle model, from steer angle and desired speed to global position and heading, is used in all of the simulation tests presented in this research. The model will also be the starting point for developing the lateral controller presented in Chapter 4. 32 3 OBSTACLE REPRESENTATION The potential flow techniques described in Section 2.1 can be used to develop flow fields that have a start and goal position and which avoid any obstacles in the area to be traversed. This research imposes specific requirements on the potential field being created. Typical potential field control relies only on the gradient of the potential field for the control input [Ge 2002, Khatib 1985, Krogh 1986, Kyriakopoulos 1995]. The algorithm developed in Chapter 4 requires an explicit stream function in addition to the gradient field. Therefore, either a technique must be developed which explicitly calculates the stream function, or a method to calculate the stream function from the gradient field must be employed. This chapter examines two broad techniques for generating potential functions, analytical and numerical. In Section 3.1 analytical methods are considered. In particular, the viable methods under consideration are analyzed based on a standard set of tests involving multiple obstacles in uniform flow. Section 3.1.1 looks at existing methods to represent obstacles as low strength sources. Section 3.1.2 describes the technique Milne- Thomson developed to add circular obstacles to a flow. Section 3.1.3 analyzes Waydo and Murray?s extension to the Milne-Thomson Theorem for averaging the velocity vector fields from individual circles and extends their approach to the case with touching circles. In Section 3.1.4 a new method is developed for averaging the stream functions of 33 individual obstacles. Section 3.2 investigates numerical solutions. As before, the methods are compared using a common set of tests. Sections 3.2.1 and 3.2.2 analyze Dirichlet and Neumann boundary conditions respectively. Finally, Section 3.2.3 presents a new method of calculating the stream function directly and the boundary conditions that are used in the method. 3.1 Analytical Solutions Analytical solutions for the potential field offer many benefits. They explicitly calculate a stream function and the calculation is often much quicker than numeric solutions. The first step with an analytic solution is to generate a field with a start and goal, but no obstacles. Two such fields generated using a source and sink are shown in Figure 3.1. The potential for these fields is ? ? ? ? s ta r t s ta r t go a l go a ll n l nF C z z C z z? ? ? ? (3.1) where zstart and zgoal are the start and goal locations respectively. Notice that due to the linear nature of the Laplace equation, the start and goal locations can be shifted arbitrarily and any number of sources and sinks (or other basis potentials) can be summed and still yield a harmonic function. The difference in the two plots of Figure 3.1 is the relative strength of the source and sink, Cstart and Cgoal respectively. 34 Figure 3.1 Source and sink potential One important feature of this field is that all streamlines should intersect the start, and more importantly, the goal. Therefore, no matter what streamline a vehicle is following, it will eventually reach the goal. The left field exhibits this feature while the right does not. The difference is that in the left field the source and sink are equal strength; however, the source is stronger than the sink in the right field. Notice that in the right field some streamlines go to infinity instead of approaching the sink. Therefore, a vehicle may not reach the goal location. Alternatively, potentials where the sink is stronger than the source look like the right plot except with the start and goal reversed. This condition is not as bad since all streamlines do lead to the goal. However, if the vehicle starts near 35 the source, then a portion of the usable space will be disregarded since some streamlines do not intersect the source. Note that the overall strength of the start and goal does not matter for this research. The strength determines the amount of fluid being added to or removed from the flow and thus the magnitude of the flow velocity. For traditional potential field controllers this also determines the desired vehicle speed. However, for this research the desired speed comes from a separate process so only the flow direction matters. Thus, as long as the source strength matches the sink strength, the overall strength is not critical. 3.1.1 Small Sources in Flow The first obstacle model to investigate is a small source. As Section 2.1.1 described, this obstacle model more closely matches an electrostatic potential instead of a hydrodynamics potential. The potential generated by placing a source in uniform flow is shown in Figure 3.2. This potential is given by lnF C z Uz?? (3.2) A stagnation point is defined as a point in the flow where the velocity is zero. The stagnation point defines the leading edge of the obstacle. It is also a point where the streamline splits. Outside of fluid dynamics, this point would be known as a saddle point and a separatrix in the flow. On one side of the streamline passing through the stagnation point, fluid comes from the uniform flow; on the other side, the fluid comes from the source. For this potential, the stagnation point is located at stag Cz U?? (3.3) 36 Notice that the stagnation point (and thus the obstacle boundary) depends on flow parameters, namely the uniform flow speed. This causes problems when dealing with more complex flows because the flow parameters are not generally known, or easily quantifiable. The circle labeled as ?obstacle? is simply there to aid comparisons to other methods. The true obstacle in this case is the entire region defined by the separatrix, which extends infinitely downstream of the source. This is typically not a desirable feature since an obstacle would often remove an area much larger than itself from the flow. Figure 3.2 Source in uniform flow With an analytic circular obstacle, Figures 2.4 and 2.6, stagnation points are also present at the leading and trailing edge of the circle. However, because there are two stagnation points, the area defined by the separatrix is finite. In particular, the obstacle 37 boundary is exactly equal to the circle itself. In terms of the flow, the finite obstacle is due to the doublet that creates the circle. The doublet is formed from a source and a sink, so no net fluid is added to the flow. Also note that the obstacle does not require flow parameters to define as the source stagnation point does. For an obstacle (defined by strength and location Cobs and zobs respectively) inserted into start/goal flow, the potential, shown in Figure 3.3, is given by ? ? ? ? ? ?? ? ? ?s ta r t s ta r t o b s o b s s ta r t o b s g o a l l n l n ln F z C z z C z z C C z z ? ? ? ? ? ? ? (3.4) In general, the obstacle strength is much less than the start; the goal strength must be equal to the sum of the sources as described above. The stagnation point defining the obstacle is given by ? ?? ?s t a r t o b s o b s s t a r t g o a l s t a r t s t a r t o b s o b s s t a r t o b s s t a g s t a r t o b s g o a l s t a r t s t a r t o b s o b s C z C z z C z z C z zz C C z C z C z? ? ?? ? ? ? (3.5) Calculating the leading edge of the obstacle requires intimate knowledge of the underlying basis flow. The design problem would involve determining an obstacle source location and strength to match an identified obstacle knowing the underlying flow, which is a much more difficult problem. Notice that the area contained by the separatrix is finite in this flow, but it extends from the stagnation point to the goal location. This creates a large wake downstream of the obstacle that has been rendered unusable. Effectively, the disturbance to the potential created by the obstacle does not vanish with distance from the obstacle. 38 Figure 3.3 Source in source/sink flow Modifications to the source obstacle method have been used for obstacle avoidance, [Guldner 1995, Kim 1992]. However, the deficiencies in the flow created by source obstacles, e.g. the disturbance not vanishing with distance and the difficulty in determining the obstacle shape and location, lead it to be discarded as a practical, analytical solution in this dissertation. This obstacle type is revisited in the numeric solution when the obstacle shape can, to a degree, be more strictly controlled. The most common method of representing obstacles in potential fields is using the Circle Theorem described in Section 2.1.1. However, several hurdles need to be overcome in order to use this method. First, if an additional obstacle is added to flow already containing obstacles, then the streamlines defining the previous obstacles will be distorted to some extent. Second, not all obstacles are circular. The solution to both 39 these problems may lie together. If a method to place circular obstacles next to each other without distorting the defining streamline can be developed, then a generic shape obstacle can be generated using many circles. 3.1.2 Circle Theorem Method To study the interaction between multiple obstacles in a potential field, a test scenario of multiple circular obstacles inserted into uniform flow is used. The obstacles are tested with vertical and horizontal orientations, both spaced and touching. Additionally, a test of five circles creating a rough wall is tested. The objective of these tests is to determine a method of generating generic obstacle shapes using known methods for circular obstacles. The first method tested is the Circle Theorem (CT) method. This method is a direct application of the Circle Theorem [Milne-Thomson 1996] using Equation (2.18). It is important to note that using this method, the order of adding circles affects the resulting flow. The theorem requires, and modifies, the existing flow. Therefore, only the last circle added is guaranteed to exactly match a streamline. The streamline distortion can be observed in the test scenario of two vertically separated circular obstacles inserted into horizontal uniform flow from left to right, shown in Figure 3.4. Only the streamlines are shown since these are what the vehicle would follow. The lower circle is added to the flow first. Because of this, the addition of the upper circle causes the streamlines that define the lower circle, the separatrices, to become distorted from the desired circle. However, at a spacing of only one radius between circle edges, the distortion is not large. The missing information in the center of the circles is due to the interpolation required to perform this technique. However, recall that the stream function inside the separatrices is 40 not used in the path planning and so is only shown (and calculated) to fully illustrate the effects of multiple circles. Figure 3.4 Two vertical, separated circles in uniform flow calculated using the Circle Theorem method The distortion caused by the second circle becomes more apparent when the circles touch, as shown in Figure 3.5. More importantly, fluid still flows between the two circles as is represented by the streamlines that pass between them. The fact that streamlines pass between the circles means this method cannot be used directly to represent non- circular obstacles. From a purely mathematical standpoint, notice the second, smaller undefined region in the upper circle. Recall from Equation (2.18) that the potentials of 41 points exterior to the circle are transformed to be interior to the circle for the conjugate analytic function. Thus, the undefined area around the doublet in the first obstacle is reflected in the second obstacle. Figure 3.5 Two vertical, touching circles in uniform flow calculated using the Circle Theorem method To further explore the effects of many circular obstacles together, five circles are placed vertically to form a wall in Figure 3.6. As expected, the last, uppermost, circle placed exactly matches the streamlines. All the other circles are distorted from the desired circle allowing streamlines to pass through the wall. The only streamlines that completely avoid the wall are outside the extreme upper and lower separatrices. It is 42 interesting that the distortion does not vary much with distance from the last obstacle added. Because the disturbance created from adding a circular obstacle decays with distance from the circle, effectively adding a circle only affects its immediate neighbors. Therefore, in the scenario of adding circles from the bottom up, a circle distorts its lower neighbor and is distorted by its upper neighbor. Figure 3.6 Five vertical, touching circles in uniform flow calculated using the Circle Theorem method When inserting a series of circles into a generic existing flow, there is no guarantee that the series will be perpendicular to the flow direction. Therefore, the same tests are performed with both the flow and the circle locations lying horizontal. As before, the 43 first scenario is with the circles spaced apart (Figure 3.7). The right circle is added last and so exactly matches a streamline as expected. However, unlike the perpendicular flow, both circles are completely contained within the separatrices; thus, both circles would be avoided. Figure 3.7 Two horizontal, separated circles in uniform flow calculated using the Circle Theorem method As before, the distortion becomes more apparent when the obstacles touch (Figure 3.8). However, unlike the vertical orientation (Figure 3.5), the distorted streamlines do not penetrate the first circle; instead, the separatrices merge. Note that the closest two stagnation points that originally lay on the centerline of the flow have shifted onto the second circle. Overall, the streamlines produce a path such that the first circle is still completely avoided. 44 Figure 3.8 Two horizontal, touching circles in uniform flow calculated using the Circle Theorem method When a series of five circles are joined to form a wall (Figure 3.9), the same merging phenomenon is observed. The stagnation points shift to the last added circle or form on the interior joints of the circles. This in effect creates a super obstacle that is defined by the streamlines intersecting the leading, leftmost, stagnation point and continues until they intersect the trailing circle where they then follow that circle. All of the inserted circles are contained within this super obstacle. The CT method therefore does form an effective wall obstacle in line with the flow. 45 Figure 3.9 Five horizontal, touching circles in uniform flow calculated using the Circle Theorem method The fact that the behavior is drastically different when the circle formation is parallel or perpendicular to the existing flow presents a problem. When used for path planning, the overall obstacle shape must represent a physical object, and the potential field flow will not necessarily be in any particular orientation with respect to that physical object due to the goal location and the starting position of the vehicle. Therefore, a method is needed that works regardless of the orientation of the existing flow. 3.1.3 Flow Velocity Average Method Waydo and Murray proposed a method to add multiple circular obstacles to a potential field without distorting the separatrix streamlines of any of the obstacles [Waydo 2003], the Flow Velocity Average (FVA) method. This method calculates the potential field created by inserting the individual circles into the unmodified flow using ? ? 2 obs,obs,jj u u jj aF F z F zzz??? ? ???? ?? (3.6) 46 This is effectively Equation (2.18) with two additions. The modified potential, Fj, is saved for each individual circle, j. Additionally, the circle is centered at a generic location in the field, zobs,j. From this potential, the velocity vector field for each obstacle potential, (VE,j, VN,j), is calculated using Equation (2.6) Once a disturbed potential is calculated for each of the circles, a weighting factor, ?, is generated based on the distance to each circle edge, d: k j kj jkddd? ?? ?? (3.7) For a given circle, this weighting is one at the circle edge and zero at all other circle edges. The total velocity vector field is the sum of the weighted individual fields ,,,E j E j N j N jjjV V V V?????? (3.8) Note that the individual potential fields, weighting factors, and velocity vector fields are calculated over the entire solution area. Additionally, since each individual circle is added to the unmodified flow, the order of addition does not matter. The intent of this method is to accurately represent multiple separate circular obstacles, rather than to allow circles to touch and form larger obstacles. However, the weighting factor can be modified to allow for touching obstacles by setting the weight to the inverse of the number of touching circles, ? ? 1 ; 0 & 0 c ount 0 ; othe r w is e jk k j k kj jk d k j dd d dd ? ? ? ? ? ? ? ? ?? ? ? ? ? ?? ? (3.9) 47 To evaluate this method the same series of tests from Section 3.1.2 are performed. The first test, shown in Figure 3.10, is two vertically separated circles. The most obvious difference with the CT method is that no streamlines are visible inside the circles. Because the output of this method is a velocity vector field, an explicit stream function does not exist. Instead, individual streamlines are calculated by simulating the fluid flow from distinct starting locations. For traditional potential field control, an explicit stream function is not required as the control effort is calculated exclusively from the potential field gradient, i.e. the velocity vector field. However, for the vehicle navigation controller presented in Chapter 4, a stream function is necessary. To use this controller a technique of generating a stream function from a gradient field would need to be employed. Section 3.2.2 points to some possible techniques to calculate the stream function. The main reason for the FVA method is to generate multiple circular obstacles with exact streamline edges. If the edges are streamlines both circles are completely avoided. Analytically, this is due to the weighting function, which prevents the disturbance potentials from other obstacles from affecting the potential at a circle edge. 48 Figure 3.10 Two vertical, separated circles in uniform flow calculated using the Flow Velocity Average method As seen in Figure 3.11, a problem still exists when the circles touch. In particular, all the streamlines between lines intersecting the separatrix stagnation points pass between the circles. 49 Figure 3.11 Two vertical, touching circles in uniform flow calculated using the Flow Velocity Average method As would be expected, when five obstacles are lined together (Figure 3.12) streamlines pass between each of the circles. Therefore, the FVA method does not produce an effective wall obstacle. 50 Figure 3.12 Five vertical, touching circles in uniform flow calculated using the Flow Velocity Average method For the series of tests with flow parallel to the obstacle orientation (Figures 3.13- 3.15), the results are similar to the CT method. The main difference is that now all circle edges are streamlines. As with the CT method, the FVA method generates streamlines that allow a vehicle to avoid all the obstacles. However, when multiple circles are touching the separatrices defining the super obstacle exactly match the intended circular shape. 51 Figure 3.13 Two horizontal, separated circles in uniform flow calculated using the Flow Velocity Average method Figure 3.14 Two horizontal, touching circles in uniform flow calculated using the Flow Velocity Average method 52 Figure 3.15 Five horizontal, touching circles in uniform flow calculated using the Flow Velocity Average method The FVA method does perform exactly as it is designed, i.e. the edges of each of the circular obstacles are streamlines. However, it is not intended for touching circles, in which case the FVA method breaks down. In particular, streamlines still pass between the circles, although unlike the CT method, the circle boundaries are not distorted. Additionally, the output of the method is a velocity vector field, not a complex potential function. To utilize this method a stream function would need to be computed from the velocity field. Overall, the FVA method is not effective for creating generic shaped obstacles for vehicle path control. 3.1.4 Flow Potential Average To address the issue of an explicit stream function not being calculated, a new Flow Potential Average (FPA) method is developed that directly averages the complex potential instead of the velocity vector [Daily 2008]. Using the same individual obstacles potentials given in Equation (3.6), the overall complex potential is a weighted average of the individual potentials, 53 jjjFF??? (3.10) The same weighting function given in Equation (3.9), is used. The test case of two vertically separated circles is shown in Figure 3.16. Several features stand out in the FPA method that are not present in the previous methods. First, like the FVA method, the circle edge is a streamline. However, unlike the FVA method, even if the original circular obstacles are separated, a larger super obstacle is formed. This is exhibited by the streamlines intersecting the two stagnation points on the circular obstacles touching at two additional stagnation points. Both circles would therefore be avoided by the path planner and even more, no streamlines pass between the obstacles. As Equation (2.20) states, the stream function on a circle edge is zero. Therefore, the super obstacle separatrices are formed where the weighted average of the two (or more) stream functions cancel out. This is promising for creating generic shaped obstacles. 54 Figure 3.16 Two vertical, separated circles in uniform flow calculated using the Flow Potential Average method Another feature of the FPA method is that both the streamlines and equipotential lines exist without being numerically integrated. As is shown in Figure 3.16, these contours are not always perpendicular. This implies the averaged complex potential is not a harmonic function. Other than demonstrating that the flow does not satisfy the Laplace equation, the equipotential lines are not used in this method. As with the other methods, the vehicle follows the streamlines. However, the velocity vector field must be calculated from the stream function using Equation (2.9), instead of from the velocity potential. 55 Because the weighting function varies over the solution grid, the averaged complex potential is not a superpositioning of individual potentials and thus is not guaranteed to satisfy the Laplace equation. An important extension of this fact is that the potential is not necessarily minima free. In particular, as seen in Figure 3.16, two vortices exist inside the super obstacle, the closed contour streamlines. These represent a local minima and maxima in the stream function. For this scenario, the extrema are contained within the super obstacle and therefore would not affect the vehicle. Additionally, the equilibrium points at the extrema are conservative as they are centers rather than focuses. Therefore, all the streamlines around the extrema form closed contours until a separatrix is encountered, i.e. a vehicle following a streamline far from the extrema should not encounter the extrema without shifting streamlines. However, neither of these features of the extrema (contained in the super obstacle and conservative) is necessarily true for all scenarios. Therefore, in certain situations problems may arise where the vehicle is attracted to a local extremum. The next test is the vertically oriented, touching circles shown in Figure 3.17. As expected, both circles are entirely avoided and a super obstacle is formed that is slightly larger than the original circles. 56 Figure 3.17 Two vertical, touching circles in uniform flow calculated using the Flow Potential Average method The result becomes more interesting when five circles are added (Figure 3.18). While all circles are avoided, the streamlines near the circles are not as smooth as before. The super obstacle formed where the stream function is zero is not a smooth joining of the individual circles. In particular, protrusions are formed off the second and fourth circles. Additionally, stagnation points (and the corresponding separatrices and extrema) appear outside of the super obstacle that is formed. Therefore, streamlines tightly following the super obstacle edge have a very convoluted path. However, only streamlines that approach near the center of the super obstacle wall follow its contour that closely. Most 57 streamlines do smoothly avoid the obstacle. The phenomenon of the streamlines becoming convoluted is present in most scenarios using this method when the obstacle is created from a large number of circles. Figure 3.18 Five vertical, touching circles in uniform flow calculated using the Flow Potential Average method For the two obstacle horizontal tests shown in Figures 3.19 and 3.20, the potential is much like the average velocity method. The potential field has streamlines that avoid the individual circular obstacles, but no larger super obstacle is formed. 58 Figure 3.19 Two horizontal, separated circles in uniform flow calculated using the Flow Potential Average method Figure 3.20 Two horizontal, touching circles in uniform flow calculated using the Flow Potential Average method 59 However, the scenario consisting of five touching circles in line with the flow (Figure 3.21) exhibits slightly different behavior. A super obstacle consisting only of the five circles is formed, but away from the super obstacle, the potential field does not smoothly revert to the underlying uniform flow. In particular, the streamlines initially approach the obstacle, and then a hump exists near the center of the super obstacle. The flow being disturbed far from the obstacle is another undesirable feature of the average potential method when large numbers of circles are used to form the obstacle. In this test scenario, the disturbance would cause a vehicle following the streamlines to initially approach the obstacle before needlessly veering away from it. Figure 3.21 Five horizontal, touching circles in uniform flow calculated using the Flow Potential Average method As with the previous methods, the flow characteristics are different if the circles are oriented perpendicular to or in line with the underlying flow. Specifically for the FPA method, when the circles are perpendicular to the flow, shown previously in Figure 3.16, 60 two streamlines join the two circles and local extrema exist in the region formed by the two streamlines. However, if the circles are oriented in line with the flow only one streamline joins them and no local extrema exist (other than the doublets which are guaranteed to be inside the circular obstacle and so are acceptable). The transition between the inline and perpendicular behaviors is critical because the local extrema are dangerous to a vehicle path planner unless they are contained within a super obstacle. The bifurcation the streamlines experience as the obstacle orientation shifts from perpendicular (0 deg) to in line (90 deg) with the underlying flow is shown in Figure 3.22. The most precise method of analyzing this flow is as a phase portrait in north, east space. For each of the orientations, two saddle points exist on each of the circular obstacles. At 0 deg two additional saddle points exist between the circles. A single separatrix joins all the saddle points and two centers exist inside the region defined by the separatrix. As soon as the orientation leaves perpendicular to the flow, shown at 10 deg, the single separatrix breaks into three distinct streamlines: one joining the two circles and two others intersecting the off-circle saddle points which still exist. The centers also still exist in regions defined by the two off-circle separatrices. As the angle continues to increase, each center/off-circle saddle point pair begins to merge until the singular points cease to exist at 15 deg. Increasing the angle further at 45 deg, smoothes the flow until the obstacles are in line with the underlying flow at 90 deg. 61 Figure 3.22 Flow Potential Average method, vertical/horizontal obstacle transition 62 The inline/perpendicular transitional behavior can be problematical when using this method for path planning. Because the underlying flow is not uniform, it is not easy to define a flow direction much less guarantee its orientation relative to a real world obstacle. Therefore, it is possible that scenarios arise such as the 10 deg example where the path taken by the streamlines (and thus the vehicle) are very convoluted. This is quite undesirable behavior for vehicle path planning. On the positive side, the flow in this test is conservative. The singular points are either saddles or centers as opposed to nodes or focuses. Therefore, no local extrema are encountered outside of closed regions formed by separatrices. Although the FPA method can produce some undesirable behavior particularly when a large number of circles are used to form an obstacle, it does meet the basic criteria for obstacle avoidance. An obstacle can be approximated by a set of circles enclosing it and that set of circles will be avoided. To expand on this concept, a series of circles are used to determine the best representation of a square obstacle. First, four touching circles form the square (Figure 3.23). In this case, the potential field is well behaved. The only area in the super obstacle but outside the original circles is at the leading and trailing edges. This actually helps the path planner since it would prevent abrupt course changes in the vehicle if the streamlines were to approach the leading edge intersection of the two circles. A downside of this representation is that at the top and bottom edges of the obstacle, the streamlines do not form a straight line; they bend toward the center of the obstacle. This means that to avoid a square, the circles need to extend well beyond the edge of the square. 63 Figure 3.23 Square obstacle formed of four touching circles calculated using the Flow Potential Average method A possible solution to the circles extending beyond the desired square is to use more circles to approximate the square. However, as is demonstrated in Figure 3.24, this representation has significant problems. There are many extraneous features outside the designed circles such as the two additional saddle points on both the leading and trailing edge. However, the major problem is the shape of the streamlines above and below the obstacle. A large portion of the flow approaching the obstacle initially is drawn towards it unnecessarily. Although this representation does generate paths that avoid the obstacle, they are not elegant paths. 64 Figure 3.24 Square obstacle formed of eight touching circles calculated using the Flow Potential Average method Recall that with the FPA method the circles are always joined by a streamline. Because of this, the circles do not need to be touching to form the obstacle. A square obstacle created using four separated circles is shown in Figure 3.25. Because only four circles are used the flow is smooth, but the shape of the connecting streamlines cannot be easily predicted so there is no guarantee that a path is generated such that a vehicle would avoid the desired obstacle. 65 Figure 3.25 Square obstacle formed of four separated circles calculated using the Flow Potential Average method Overall, with the new FPA method it is better to use as few circles as possible to represent obstacles. Additionally, to guarantee the shape of the obstacle, the circles usually should touch. One useful obstacle that is created with separated circles is a wall. A small circle is placed on each edge of the wall and the connecting streamline forms the wall. Within these limitations, it is possible to generate vehicle paths using the FPA method. The scheme is as follows. 66 Create a potential field containing only start and goal locations as sources and sinks respectively using Equation (3.1). The discontinuities should be rotated to point away from the line segment between the start and goal. This potential forms the new basis potential. For each obstacle present in the world map: If the obstacle is approximated as a circle: Use the Circle Theorem to add the circular obstacle to the basis potential using Equation (2.18). If the obstacle is approximated as a wall: Use the FPA method to add a small circle to the basis potential at each end of the wall with Equations (3.6) and (3.10). If the obstacle is approximated by a combination of circles: Use the FPA method to add each of the circles to the basis potential, Equations (3.6) and (3.10). The potential resulting after the obstacle is added becomes the new basis potential. Once all obstacles are added to the potential field, the overall potential field is complete. The new FPA method has the same limitation of the original Circle Theorem, i.e. adding an additional obstacle modifies the edges of any previously added obstacles. However, as long as the obstacles are significantly separated relative to their size the distortion is minimal. A sample map generated using this method is shown in Figure 3.26. It contains a wall, a circular obstacle, and a square obstacle approximated by four circles. The streamlines from start to goal smoothly avoid each of the obstacles. 67 Figure 3.26 Averaging stream function, sample map While this map is successful, it is difficult to predict when some of the less attractive aspects of this method will appear in the generated potential field. The method meets the basic requirements, but has significant secondary drawbacks that may be worth investigating in future work. Many other techniques for representing obstacles also exist. For instance, Needham expands the concept of the Circle Theorem with the ?method of images? for inserting a generic shape obstacle into a flow [Needham 1997]. The idea centers around making the obstacle edge a streamline. By reflecting the undisturbed flow about the obstacle edge, that edge becomes a streamline. For a circular obstacle, this reflection is the conjugate analytic function with the argument given in Equation (2.18). Additionally, conformal mapping techniques allow some obstacle shapes to be mapped to a circle along with the 68 resulting fluid flow [Currie 1993, Milne-Thomson 1996, Rimon 1992]. This is a common way to study basic flow around wings. These techniques should be studied in the future to determine their applicability to path planning. 3.2 Numerical Solutions As Section 2.1.2 described, numerical solutions are an alternative method for calculating a potential field. The main down side of the numerical method is that typically only the velocity potential is calculated. The stream function must therefore be determined from the velocity potential. For fluid dynamicists this is acceptable since the objective is the velocity vector field and corresponding streamlines, thus the stream function is not necessary. For this work, however, an explicit stream function is required to control the vehicle to the goal. The benefit of numeric solutions is that any shape obstacle can be represented. This feature makes numerical solutions attractive since generic shaped obstacle representation is the main drawback of analytical solutions. The way any feature (such as a start, goal, or obstacle) is modeled in numerical potential fields is through boundary conditions. The outer edge of the solution area is treated as an obstacle creating a finite area the potential is defined within, or that the vehicle can drive in. Section 2.1.2 mentioned two common types of numerical boundary conditions for potential functions, Dirichlet and Neumann, which are examined here. 3.2.1 Dirichlet Boundary Condition Dirichlet boundary conditions involve setting the velocity potential to a specific value. This is the technique to model start and goal locations. The start is set to a low potential and the goal high. This boundary condition can also be used to model obstacles 69 by making the obstacle potential higher than its surroundings. This concept is appealing because it closely models Khatib?s original potential field idea [Khatib 1985]. However, Dirichlet obstacles have several drawbacks described below. The first test of the Dirichlet boundary condition (DBC) method is a potential field created by a low potential start in the southeast corner and a high potential goal in the northwest as shown in Figure 3.27. The start and goal potentials are negative and positive one respectively (although as with the analytical solutions the magnitude is not critical since the gradient direction is the important aspect). The world border (not shown on the plot) uses Dirichlet boundary conditions with zero potential and no obstacles are present. In order to avoid boundary condition conflicts when a start or goal lies on the border an extra row or column of points are added to each edge of the solution area. This extra set of points becomes the world border for the numerical solver. This technique of increasing the grid size by one in each direction is used on each of the numerical boundary conditions methods described in this dissertation. 70 Figure 3.27 Start and goal potential obstacle potential field, using Dirichlet boundary conditions As is shown in Figure 3.27, the velocity potential is very flat meaning that the effects of the start and goal do not noticeably extend very far into the field. The equipotential contour lines are shown logarithmically spaced to show the slight curvature near the center of the field. Recall that the stream function is not calculated. Instead, the streamlines are generated by numerically integrating the velocity vector field. Therefore, 71 the flat potential may cause numerical problems when calculating the velocity vector. Because the gradient is so small, slight errors in the gradient calculation can cause large changes in the direction of the velocity vector. Beyond the numerical problems, several structural problems exist with the DBC method. As with the analytical techniques, the flow is characterized by its saddle points and corresponding separatrices. Although they are not obvious, there are two saddle points at the northeast and southwest corners of the solution area. These streamlines do not converge to a single point on the plot because of the extra row of grid points. Streamlines contained within the separatrices intersecting these saddle points reach both the start and goal, resulting in a vehicle that would drive where desired. However, streamlines outside this area go outside the solution grid. Also, note that the streamlines shown all converge to virtually a single line approaching the start and goal. Many streamlines converging to a nearly single line is a common feature when dealing with Dirichlet boundary conditions. In this case, it means that even when a vehicle is following one of the streamlines in the area that intersects the goal, near the start it can only drive in one direction. For this scenario, the problem could likely be reduced by linearly varying the border potential near the start and goal to reduce the gradient between the start/goal and the border. It is also possible to add more than one extra row of solution points before the border boundary condition is applied. Either solution should widen the possible departure angles near the start. However, solutions to keep streamlines from merging are not as obvious for other scenarios. When adding an obstacle, the potential value of the obstacle is an important design parameter. Figure 3.28 represents an obstacle with zero potential, i.e. equal to the border. 72 The overall flow around the obstacle is similar to the no obstacle case shown previously in Figure 3.27. This is not surprising since the obstacle potential is so close to the surrounding potential. Streamlines outside the area defined by the separatrices intersecting the border saddle points ( ) still do not intersect both the start and goal. The critical difference between the flows is defined by the separatrices intersecting the saddle points in the northeast and southwest quadrants of the obstacle. All streamlines inside the area defined by these streamlines ( ) intersect the obstacle. This leaves a limited area in which the vehicle can avoid the obstacle but still reach the goal. Effectively a super obstacle is created reaching from the start to the goal and containing the desired obstacle. However, a much larger section of the solution area is also contained completely dividing the solution area. Figure 3.28 Start, goal, and zero potential obstacle potential obstacle potential field, using Dirichlet boundary conditions 73 Increasing the value of the potential function at the obstacle does not help the situation. A potential field with an obstacle potential of 0.0002 is shown in Figure 3.29. Notice that the obstacle saddle points move along the obstacle edge toward the start while the border saddle points move toward the goal. However, the same three regions exist: intersecting the border and either the start or goal ( ), intersecting the obstacle and either the start or goal ( ), and intersecting both the start and goal ( ). If the obstacle potential is increased much further, at least one streamline will intersect both the obstacle and the border. This means that no streamlines intersect both the start and goal. Therefore, the allowable range of obstacle potentials is quite small. Figure 3.29 Start, goal, and positive potential obstacle potential field, using Dirichlet boundary conditions 74 Expanding the concept to multiple obstacles (Figure 3.30) the problems become even more apparent. As with each of the other flows, the separatrices intersecting these singular points along with singular points on the border define the regions that will intersect an obstacle, areas that intersect the border, and areas that intersect the start and goal. However, more sub-regions exist for the flow: avoiding one obstacle but intersecting another, etc. Note that where these sub-regions meet one often collapses to virtually a single line, leading to very small allowable approach and departure angles to/from the goal and start. Because these are caused by the interaction of the obstacles and border, there is no easy solution to this problem. Figure 3.30 Start, goal, and three obstacles potential field, using Dirichlet boundary conditions 75 In the above example, each obstacle has its potential set to zero. Note how the saddle points are in different locations on each of the obstacles: on the northern obstacle they are drifting toward the leading edge of the obstacle, on the western obstacle they have effectively combined into a single singular point, and on the eastern obstacle the saddle points have combined and separated from the obstacle. Ideally, these saddle points would be placed to more closely match the obstacle shape by varying the obstacle potential. However, there is no simple method to determine the ideal potential for each individual obstacle. One possibility for simultaneously removing the need to choose individual obstacle potentials and having some streamlines avoid one obstacle only to intersect another is to remove the start location from the potential field as shown in Figure 3.31. In this case, the obstacles and border are the lowest potential at zero, while the goal is still one. All streamlines begin at either an obstacle or the border and all intersect the goal. Effectively the border and obstacles are the source. Because the obstacles are at the lowest potential, no flow approaches them. The downside of this method as compared to the other Dirichlet methods is that only one streamline connects a given point in the field and the goal. For example, if the southeast corner is the start location of the vehicle, only one streamline passes through that point. With the previous methods presented in this dissertation all streamlines pass through the start point, so many possible paths exist. 76 Figure 3.31 Goal and three obstacles potential field, using Dirichlet boundary conditions Using Dirichlet boundary conditions to represent obstacles can satisfy the basic goal of approaching a goal location and avoiding all obstacles. However, as seen, the resulting potential fields often have undesirable characteristics. The super obstacles generated are often much larger than the true obstacle and like using sources in analytical flows (Section 3.1.1), they often extend to the start, goal, or both. The allowable driving area also often collapses to a single line through the obstacles. While these problems may be reduced (by modifying the border potential, individually optimizing the obstacle potentials, or removing the start location) a larger structural problem still exists. The potential created is virtually flat. Because the border is a uniform potential, most of the potential field is also near that value. This makes numerically calculating the gradient to produce streamline paths problematical. 77 3.2.2 Neumann Boundary Condition The other boundary condition useful for path planning is the Neumann boundary condition. With this boundary condition, the derivative of the potential function is constrained instead of the potential value itself. In particular, to model fluid flow along surfaces the normal velocity is set to zero. The main downside of this method is the time to compute a solution due to the boundary and obstacle potential depending on the surrounding potential. An example of the Neumann boundary condition (NBC) method for a map with a start and goal location but no obstacles is shown in Figure 3.32. The start and goal locations are represented with Dirichlet conditions at negative and positive one respectively, while the border uses a Neumann condition. Compared with the DBC method (Figure 3.27), several features are immediately apparent. The potential is much smoother in varying from the start to the goal, i.e. it does not flatten away from the start and goal. This makes calculating the gradient more robust to numerical error. Like the DBC method, the streamlines are numerically calculated by integrating the velocity vector field. However, in this case all the streamlines stay in the solution area, intersecting both the start and goal. This potential is much more like the corresponding analytical solution shown in Figure 3.1, except for the fact that the solution is over a limited area instead of a global space. 78 Figure 3.32 Start and goal potential field, using Neumann boundary conditions Other benefits of the NBC method become apparent when an obstacle is added to the flow as shown in Figure 3.33. The area of the solution that the streamlines avoid exactly matches the obstacle shape. In addition, all streamlines still intersect both the start and goal. Unlike the DBC method, no streamlines may leave the solution area. 79 Figure 3.33 Start, goal, and obstacle potential field, using Neumann boundary conditions The main advantages compared to analytical methods become apparent when multiple obstacles are in the solution area (Figure 3.34). Each of the obstacle edges is a streamline. Because the entire field is computed at once, obstacles are not distorted by the addition of later obstacles. Note that the obstacles can be any shape, although they are circles in this example for simplicity. As with each of the other methods, the flow is characterized by separatrices intersecting saddle points in the potential field. However, unlike the DBC method the saddle points are always on the obstacle edge. Also unlike the DBC method, where the separatrices determine what portion of the solution area reaches the goal, with the NBC method the separatrices simply determine which side of an obstacle the streamlines pass on. 80 Figure 3.34 Start, goal, and three obstacles potential field, using Neumann boundary conditions The NBC method produces the best potential field for vehicle path planning since any obstacle shape can be represented exactly (limited only by the discretization of the solution grid) without needing any other parameters from the flow and all streamlines intersect both the start and the goal allowing a design parameter of the best streamline to follow. The limitations of the NBC method are that it is computationally expensive and the fact that streamlines are numerically calculated instead of contours of a stream function. There are several possible methods for calculating a stream function given a velocity potential. The first method relies on the fact that the stream function is an exact 81 differential. Recall that streamlines are parallel to the velocity vector field. This implies that cnst. N E V dNV dE ??? (3.11) Along with the conservation of mass requirement given previously in Equation (2.3), this means that the stream function is an exact differential [Rainville 1969] and can be calculated by , ENVVNE????? ? ? (3.12) Notice that this is in fact the definition of the stream function, Equation (2.9). Examining the north velocity equation, ? ? NV dE f N? ? ? ?? (3.13) where ? ?fN is the unknown integration constant function. For each index point, ? ?,jk , in the solution grid, the north velocity integral can be numerically approximated, m in , j k E j k NE NN g V dE ? ?? ? (3.14) To calculate the integration constant function, the east velocity is used, E g d fV N N d N???? ? ? (3.15) The partial derivative of the north velocity integral, g, can be numerically approximated leaving the integration constant function to be approximated with numeric integration, m in , k j N j k E N EE gf V d NN ? ???????? ??? (3.16) 82 Finally, the stream function is given by gf??? (3.17) This method works well in theory; however, there are two main issues that must be resolved. The first is that extensive numeric integration and differentiation is being employed on a potential field that already has some numerical errors due to the iterative solver. The second is how to handle obstacles. The potential field interior to an obstacle is not defined. Technically the obstacle interior is outside the solution grid. The numeric integration must therefore reset across the obstacle gap. What value the integration should reset to is not known. If the obstacles are represented by Neumann boundary conditions then the underlying principle is that the stream function is constant over the obstacle edge. For Dirichlet boundary conditions, the solution is not known. When these issues can be disregarded, i.e. when no obstacles are present in the solution area, this method does produce a workable stream function. However, this would rarely be the case in path planning. Several other methods exist for determining the stream function using properties of analytic harmonic functions [Sarang 2007]. For instance, in early versions of Theoretical Hydrodynamics Milne-Thomson describes one such method [Laitone 1977]. However, this technique is absent in later editions of the book [Milne-Thomson 1996] and thus was not able to be implemented. Many of these methods, however, require analytic solutions for the velocity potential. In the potential field generation techniques used in this research, if an analytic solution exists then the stream function is already known. Still more techniques for calculating the stream function include stream surface methods 83 [Kenwright 1992] and combining distinct streamlines into a complete stream function [van Wijk 1993]. For this research, however, the velocity potential is not necessary to produce a path planning solution. Therefore, instead of using any of the methods mentioned above to calculate the velocity potential and determine the stream function from the velocity potential, it should be possible to develop a new method to calculate the stream function directly. 3.2.3 Stream Function Boundary Condition As Chapter 2 mentioned, the partial differential (or difference in the discrete case) equation that underlies both the velocity potential and the stream function is the Laplace equation. The difference between the two functions therefore lies in the boundary conditions. In particular, start and goal locations, obstacles, and the world border need to be represented for the stream function. To this end, a new stream function boundary condition (SFBC) method is developed. To model a start or goal location, characteristics of the analytical source or sink are matched. In particular, for an analytic source streamlines all lead radially away from a source as in Figure 2.2. Mathematically, the stream function at a point around a source is effectively the angle of that point in polar coordinates centered at the source or sink. The analogous concept for the numerical boundary condition is to linearly vary the stream function of the points surrounding the start or goal location based on their polar angle. This implies that the stream function will have a discontinuity. As with the analytical source, the discontinuity is designed to be oriented facing directly away from the line segment connecting the start and goal. The angle of each point surrounding the start or goal is measured from the line segment connecting the start and goal, i.e. the line segment 84 is defined as 0 deg. Note that because of this definition, points have a different angle when referenced from the start location as opposed to the goal. However, the different reference frames are accounted for in the boundary condition definition. The discontinuity is always at ?180 deg. It is critical that the orientation of increasing stream function match the analytical model so that the velocity vector field (and thus desired heading) will not be rotated by 180 deg. In particular, the stream function should increase counterclockwise at the start and clockwise at the goal. Since the overall magnitude of the stream function is not critical, ?180 deg is chosen to correspond with a stream function of ?1 and 180 deg corresponds to a stream function of ?1. This implies that the polar angle increases counterclockwise at the start and clockwise at the goal. Note that the streamline leaving the start or approaching the goal along the line connecting the two points will have a stream function of zero. Because of the superior flow characteristics of the Neumann velocity potential boundary conditions, the stream function boundary conditions at the world border and obstacles are chosen so that the resulting flow matches the Neumann boundary condition flow. Recall that for the Neumann boundary condition the normal velocity is zero. This means that the stream function will be constant, i.e. the border is a streamline. As described above, the stream function for points with negative polar angles measured from either the start or the goal have a negative stream function value. The minimum stream function is ?1 and should occur at the world border. Therefore, border points with a negative polar angle are assigned a stream function of negative one. Similarly, border 85 points with a positive polar angle are assigned a stream function of ?1. This is effectively a Dirichlet boundary condition on the stream function at the world border. The concept of the stream function boundary condition for start and goal locations as well as the world border is illustrated in Figure 3.35. The upper plot shows the boundary points with assigned stream function values vs. the interior points, which are solved to satisfy the discrete Laplace equation from Equation (2.22) (with the forcing function, G, equal to zero). The lower two plots show the assigned stream function values for the points immediately surrounding the start and goal as well as along the world border. Figure 3.35 Start and goal boundary values for the stream function boundary condition 86 There are two important considerations when using this method to calculate the stream function. The first is that for this research the start and goal location always lie on the world edge (before the extra set of solution points are added as discussed in Section 3.2.1). This condition is not strictly necessary. Just as the stream function discontinuity exists along an infinite line segment for the analytic source, the numeric boundary condition could be applied to the sequence of points immediately next to the discontinuity (positive or negative one depending on the side of the discontinuity). However, for path planning it is rare that the desired behavior is to drive away from the goal before turning towards it. Therefore, in most situations only including points between the start and goal is a valid constraint for the solution grid. The second restriction is that only one start and one goal exist. There is no obvious method for placing the discontinuities for multiple starts or goals. Analytically the discontinuity location does not affect the flow, but when determining correct border boundary conditions it becomes more important. Having multiple starts or goals is not common in vehicle path planning. However, one possible scenario would involve multiple robots starting at the same location and going to different goals; this scenario is beyond the scope of this research. The complete potential field from the newly developed SFBC method is shown in Figure 3.36 for the test scenario used above: start at the southeast corner, goal at the northwest, and no obstacles. This figure is very similar to the Neumann boundary condition (Figure 3.32). The difference is that in this case the stream function is calculated instead of the velocity potential. This means that the equipotential lines are 87 numerically integrated from the velocity vector filed instead of the streamlines. However, for path planning the velocity potential is not needed. Figure 3.36 Start and goal potential field, using the stream function boundary condition When an obstacle is added to the field (Figure 3.37), the obstacle edge is a streamline just as with the world border. The difference between the border and an obstacle edge is that the value of the stream function at the obstacle is not known a priori. More specifically, one streamline will intersect the obstacle and the obstacle edge will take the value of that stream function contour. To accomplish this, at each iteration of the solver the stream function at the obstacle edge is set to the average of the stream function values at the points immediately surrounding the obstacle. When the solution converges the behavior of the stream function is as desired. 88 Figure 3.37 Start, goal, and obstacle potential field, using the stream function boundary condition When multiple obstacles are added to the field (Figure 3.38), the procedure is the same as the single obstacle. During each iteration of the solver, the stream function at each individual obstacle edge is set to the average of the points immediately surrounding that obstacle. It is critical that the obstacles are handled individually. Individual obstacles will very rarely have the same streamline intersect them; therefore setting the stream function at the obstacle edge to the average of the points surrounding all of the obstacles is incorrect. Note that as expected the behavior of the streamlines matches that of the same scenario with Neumann boundary conditions shown previously in Figure 3.34. 89 Figure 3.38 Start, goal, and three obstacles potential field, using the stream function boundary condition The SFBC method satisfies both of the requirements for vehicle path planning: any shape obstacles can be modeled and an explicit stream function is calculated. An additional benefit of this new method is that its calculation time is much shorter than the Neumann boundary condition that it mimics in behavior. Table 3.1 shows the average calculation time for four solution methods: analytical Circle theorem, numerical Dirichlet boundary condition, numerical Neumann boundary condition, and the new numerical stream function boundary condition. For each test, Matlab is used to calculate the potential field 30 times over a 101?101 grid and the calculation time is averaged. The test is performed for both the no obstacle and three obstacle scenarios presented above. 90 Table 3.1 Potential field method calculation times Method No obstacle computation time (s) Three obstacle computation time (s) Circle theorem 0.0161 1.09 Dirichlet BC 0.667 0.642 Neumann BC 13.5 33.7 Stream Function BC 2.02 4.57 With no obstacles present, the CT method is obviously the shortest calculation time; the calculation is a simple analytic function. However, when obstacles are added, the CT method time increases dramatically, becoming longer than the DBC method time. This is most likely due to the interpolations that must be performed to add obstacles to an existing field. As expected the DBC method is the fastest of the numerical techniques due to the boundary condition being fixed across iterations. The calculation time is slightly shorter with obstacles simply because there are fewer interior points to solve in the solution grid. The NBC method takes the longest to calculate because the world border and obstacle edge potential vary with each iteration of the solver. The calculation time is so long that real time implementation may be problematical with reasonable computing power. The SFBC method developed in this dissertation takes longer to compute than the DBC method due to the stream function on obstacle edges varying with solver iteration. However, the computation time is still significantly shorter than the NBC method and should be feasible to implement for real time path planning applications. Overall, several analytical and numerical techniques have been examined. A new analytical method has been developed which meets the requirements of avoiding all obstacles by combining circles to approximate generic shaped obstacles. However, this 91 method suffers when many circles are required to match the obstacle shape. A new numerical solution developed using the stream function boundary condition is the best technique to generate potential fields for vehicle path planning. Unlike analytical methods, exact obstacle shapes can be represented. In addition, unlike the numerical techniques that calculate the velocity potential, an explicit stream function is calculated. This stream function is integral to the vehicle steering controller presented in Chapter 4. Finally, the computation time is shown to be reasonable enough to allow real time implementation on a vehicle. 92 4 CONTROLLER DEVELOPMENT After the potential field method has been developed, the next critical algorithm is the vehicle controller, which utilizes the potential field to move from the start to the goal. To design the controller a linear vehicle model which represents the lateral error to a desired streamline for given steer angles is utilized. The controller relies on both vehicle response measurements as well as reference states determined from the potential field. Section 4.1 presents the simplified model used in the steering controller. This section also exposes a new issue with a vehicle becoming uncontrollable at a critical speed along with methods to mitigate the problem. Section 4.2 expands the vehicle model to include global position states and tracking a reference circle and demonstrates the method using a test potential field. 4.1 Bicycle Model Vehicle Controller Section 2.2 presented the vehicle model used for simulation purposes. This model is simplified to the well known bicycle model [Dixon 1991] to use in controller design. In addition to the assumptions made in the full vehicle model (constant speed and planar motion), the bicycle model has two additional assumptions. First, the inner and outer tires and slip angles are assumed to be identical and second, the equations of motion shown in Equations (2.28) and (2.29) are linearized for small angles and a linear tire model. Under these assumptions the system is described by the following model 93 2 22 1F R F R F FF R F R zzz C C a C b C C m V m V mV aCa C b C a C b C rr II I V ? ? ? ? ? ?? ? ? ? ?? ? ???? ??? ? ? ?? ???? ?? ?????? ???? ?????? ?? ?????? ? ? (4.1) This model fits the common linear state space form, ??x Ax Bu? (4.2) with the state vector r????????x (4.3) and the input (scalar for this system) u ?? (4.4) The parameters for this model are given in Table 2.1. Note that for equation simplicity the cornering stiffnesses in the bicycle model are per axle whereas Table 2.1 lists them per tire. Neglecting weight transfer, the axle cornering stiffness is twice the tire cornering stiffness (assuming two tires per axle). The bicycle model is second order and, as with the nonlinear model, the characteristics change with vehicle speed. The magnitude and damping ratio of the bicycle model poles are shown in Figure 4.1. A transition speed exists where the models shifts from over-damped to under-damped. This speed is defined as ? ? ? ? ? ? ? ? ? ?? ? 22 2 2 2 22 2 12 12 2 2 tr a n z F R F R F R z F R z F R V m I a b C C a C b C m a C b C I C C m I a C b C ? ? ? ? ? ? ? ? ? ? ? ??? ? ? ? ?? ??? ??? ????? ? ? (4.5) Note that the transition speed may be complex implying the model is over-damped for all speeds. For the parameters used in this research, the transition speed is 8.5 m/s (19 mph). 94 In Figure 4.1, this is the speed at which the magnitudes of the two poles converge and the damping ratio becomes less than one. Figure 4.1 Bicycle model pole characteristics 4.1.1 Uncontrollable Speed The magnitudes of the poles and zeros for the range of speed where the poles are real are shown in Figure 4.2. Because the magnitude of the poles and zeros are all close together, the lower plot shows the magnitude relative to the slower pole. The significant aspect of this plot is that a critical speed (Vcrit) occurs at 5.83 m/s (13 mph) where one of the poles cancels the zeros leaving a first order model. This is similar to the pole/zero cancellation that occurs in neutral steer vehicles [Dixon 1991]. However, in this case the cancellation is a function of speed as well as the vehicle parameters. Note that this 95 critical speed is not the same as the speed at which the bicycle model may become unstable (also known as the critical speed) [Gillespie 1992]. Figure 4.2 Bicycle model real poles and zeros The reason this pole/zero cancellation is important is that at the critical speed the model becomes uncontrollable causing problems in designing the control gains. If a system is controllable then control efforts exist to transition the system between any two states in finite time [Bay 1999, Friedland 2005]. In terms of system performance, if a system is controllable it is possible to design a state feedback controller such that the 96 closed-loop system eigenvalues can be chosen freely. A system is controllable if the controllability matrix is full rank. The controllability matrix for the bicycle model is ? ? ? ? ? ? ? ? ? ? 2 2 2 2 22 2 F R FF R FF z F R FF R FF z z z aC bC m V aCC C CC m V m m I V a C b C aCaC bC CaC I m I V V V I ? ? ?? ? ?? ? ? ?? ? ?? ???? ? ?? ?? ?? ?? ? ? ? ? CB B AB (4.6) The critical speed is calculated by determining the speed at which the determinate of the controllability matrix is zero: ? ?? ? 0 c r itVV c r it RzC a b m a b I maV ? ? ? ? ? ? ? CB (4.7) There are two interesting features about this equation. First, the critical speed will not exist if the vertical moment of inertia is large enough compared to the mass and center of gravity location. Specifically, if zI mab? (4.8) then the critical speed is imaginary, i.e. does not exist. This relationship is interesting because for passenger vehicles the yaw moment of inertia is often approximated as [Garrott 1988] zI mab? (4.9) This means that the critical speed is often zero or at least very slow. However, if this relationship does not hold, as with the Corvette, then the critical speed should be taken into consideration when designing a vehicle controller. 97 Another interesting point is that the critical speed depends on all vehicle parameters except the front axle cornering stiffness. This by itself is not significant, but recall that the bicycle model also has a transition speed from over-damped to under-damped which is given in Equation (4.5). For the pole /zero cancellation to occur, the model must be over-damped because it is not possible to cancel half of a complex pair. Therefore, the critical speed must be below the transition speed. For the same critical speed, it is possible to have multiple transition speeds by varying the front axle cornering stiffness. The relationship between the transition and critical speeds is shown in Figure 4.3. Notice that the minimum transition speed is the critical speed. Initially there does not appear to be much in common between the critical and transition speeds, Equations (4.9) and (4.5) respectively. However, when the derivative of the transition speed with respect to the front cornering stiffness is set to zero, the analytical minimum transition speed is the critical speed: ? ? ? ? m in 0tran F Rz tran V C C a b m ab I V ma ? ? ? ?? ? ? ? ? (4.10) As described above, it is necessary for the minimum transition speed to be at least the critical speed. However, it is interesting that the minimum transition speed is the same as the critical speed, particularly because the transition speed depends only on the state matrix, A, while the critical speed depends on the input matrix, B, as well as the state matrix. Although the relationship between the transition speed and critical speed is not necessary to account for the uncontrollability, it is remarkable that mathematically the 98 two seemingly unrelated speeds are linked such that the pole/zero cancellation is possible. Figure 4.3 Bicycle model real/complex transition speed An alternative method to study the controllability of the bicycle model is to study the controllability gramian [Antoulas 2004, Bay 1999, Friedland 2005]. The infinite controllability gramian is defined as TT 0 e e d????? ? AAG BB (4.11) It also satisfies the Lyapunov equation: TT 0? ? ?AG GA BB (4.12) In order for the system to be controllable, the controllability gramian must be non- singular, which corresponds to the controllability matrix not being full rank. However, the controllability gramian provides more insight than the controllability matrix because along with a binary controllable/uncontrollable result, the controllability gramian can be 99 used to calculate the minimum energy necessary to transition between two states, i.e. for the vehicle to move from one yaw rate and side slip to another. The minimum energy required to transition from the null state to a given state, xd, is defined as T1min ddE ?? x G x (4.13) Notice that the control input to transition between the states is not needed to compute this energy. The norm of the inverse of the controllability gramian for the bicycle model is shown in Figure 4.4. As the speed nears the critical speed, the norm increases dramatically, implying the energy necessary to transition to a desired state also increases correspondingly. The uncontrollable problem is not isolated to simply the critical speed. Even in the region around the critical speed, the energy required to control the vehicle is not physically feasible. Figure 4.4 Norm of inverse of bicycle model controllability gramian (Vcrit = 5.8 m/s) 100 4.1.2 Constant Closed-Loop Pole Controller State feedback is used to demonstrate the effects of the controllability on the control design. State feedback controllers have the form ??u Kx (4.14) For this system the control gain matrix, K, is made up of the sideslip and yaw rate gains: rKK???? ??K (4.15) For this example, these gains are chosen so that the closed-loop system ? ???x A BK x? (4.16) has constant closed-loop poles at a frequency, ?n,des, and damping ratio, ?des, of 5 Hz and 0.707 respectively, i.e. 222 2 , ,2 d e s n d e s n d e ss s s? ? ?? ? ? ? ? ?I A B K (4.17) The resulting control gains are shown in Figure 4.5. Note that at the critical velocity the gains are undefined. More importantly, the gains grow very large in the region around the critical velocity. The large gains correspond to the increased energy necessary to transition between states. 101 Figure 4.5 Bicycle model control gains: constant closed-loop poles (Vcrit = 5.8 m/s) To demonstrate the response when these control gains are used, the system is simulated with a linearly varying speed. In this simulation a reference yaw rate, rref, of 10 deg/s is used. From the reference yaw rate, a reference sideslip, ?ref, and steer angle, ?ref, are calculated using the DC gains of the system: 1 re fre f r r re f re f rDC DCDC DC ? ? ? ?? ? ??? ? ? ??? ?? ?AB (4.18) These correspond to the steady state values necessary to maintain the desired yaw rate. The controlled steer angle then becomes r e f r e f r r e f rrKK ??? ?? ??????? ???? ? ?? (4.19) 102 The motivation behind the feed forward formulation for the controller is explained in more detail in Section 4.2. The vehicle plant for this simulation is the bicycle model instead of the nonlinear model in order to eliminate any errors not due to the control gains. The sample rate of the simulation is 100 Hz. As the response shows in Figure 4.6, when the vehicle nears the critical speed (5.83 m/s) it begins to become unstable. The only reason the response returns to the desired behavior is that the speed increases past the critical velocity. One interesting feature of this test is that because the simulation is calculated at discrete time steps, the speed (and thus gain calculation) does not occur at exactly the critical velocity. Because of this, the closed-loop response should still match the desired response. Specifically, the difference in the desired and actual closed-loop poles is never larger than 10?5. 103 Figure 4.6 Bicycle model response: constant closed-loop poles, varying speed The reason for the discrepancy between the actual and desired response is that the system parameters vary with time. Recall that time varying systems are not guaranteed to be stable even if the poles have negative real parts at every time step [Slotine 1991]. This fact is true for every vehicle controller designed from the bicycle model. However, it has never been adequately studied for vehicle systems. For this system when the control gains are reasonable, the varying speed does not have a major effect. However, when the gains become large (and the variation between one time step and the next is correspondingly large) the varying speed causes the system to behave erratically. To 104 justify this conclusion, a similar simulation is shown in Figure 4.7. The difference is that in this simulation the speed is held constant at 5.82 m/s, very close to the critical velocity. In this simulation, the response is as expected even though the control gains are very large. However, the constant speed assumption is not invalidated as it is in the prior simulation. Figure 4.7 Bicycle model response: constant closed-loop poles, constant speed Even when holding the speed constant there are other problems with the large control gains near the critical velocity. For example, the same simulation with the addition of zero mean Gaussian noise to the feedback states is shown in Figure 4.8. The standard deviation of the yaw rate noise is 0.05 deg/s, typical for automotive grade gyroscopes. 105 The standard deviation of the sideslip noise is 0.36 deg. Although there is no typical sideslip sensor, this is the noise when using GPS and a gyroscope to estimate sideslip [Daily 2004]. When the speed is not near the critical velocity (left plots), the response is acceptable. However, near the critical velocity the response is unacceptable (right plots). The large control gains aggravate the uncorrelated noise on the two states and corrupt the system response. The clean (no noise) response shown previously does not have this undesirable behavior because the states are correlated causing the large gains to effectively cancel out and produce a reasonable steer angle. Figure 4.8 Bicycle model response: constant closed-loop poles, with noise 106 In general, a fixed closed-loop pole control is not acceptable even if the speed is not precisely at the critical velocity because the large control gains near the critical velocity drive the system unstable both by varying the model parameters and by exacerbating measurement noise. 4.1.3 LQR Design In order to reduce the effects of uncontrollable speed, a better method to calculate the control gains is using LQR [Stengel 1994]. LQR designs the control gains to minimize the cost function ? ?TT 0 J d t???? x Q x u R u (4.20) where Q and R are the state and input weight matrices respectively. The control gain matrix is calculated as 1T??K R B P (4.21) Where P is the solution to the algebraic Riccati equation: T 1 T?? ? ? ?A P P A P B R B P Q 0 (4.22) For this example, the weights are chosen to be of one on sideslip, ten on yaw rate, and one on steer angle, i.e. 10 1 0 10 R????????Q (4.23) Typically, it is more important to match the reference yaw rate than the sideslip; therefore, the yaw rate weight is higher. Section 4.2 describes the actual weights used in the streamline controller. The control gains resulting from these weights are shown in 107 Figure 4.9. Note that the control gains are acceptable for all speeds, even in the vicinity of the critical velocity. Figure 4.9 Bicycle model control gains using LQR design (Vcrit = 5.8 m/s) Typically for an LQR design to be valid the system must be controllable. However, in this case the design is able to compensate for the uncontrollability. As is shown in Figure 4.10, one of the closed-loop eigenvalues intersects the pole/zero cancellation at the critical velocity. From a systems point of view this makes sense. Recall that for a root- locus the closed-loop poles always move from the open-loop poles to the open-loop zeros (or infinity) as the loop gain increases. If there is a pole /zero cancellation, the closed- loop pole must therefore be at that pole/zero location. In this scenario, LQR designs the closed-loop poles to meet this requirement as is shown in Figure 4.10. Because an attempt is not made to place the closed-loop poles in locations that are not feasible, the 108 control gains remain reasonable over the entire speed range. Note that in the lower plot of Figure 4.10 the large closed-loop pole is not shown to concentrate on the poles and zeros of interest in the cancellation. Figure 4.10 Bicycle model and LQR closed-loop real poles and zeros As before, these control gains are demonstrated in a simulation with increasing speed (Figure 4.11). The same yaw rate reference (10 deg/s) is used and the reference sideslip and steer angle are calculated using Equation (4.18), resulting in the controlled steer angle including a feed forward term given in Equation (4.19). The only difference in this simulation is that the LQR control gains are now used. Using the LQR gains the system 109 responds to the changing speed much better and the critical velocity does not cause the erratic behavior observed with the fixed closed-loop pole controller. Figure 4.11 Bicycle model response: LQR, varying speed Alternative methods to LQR may also avoid the controllability problem. In particular classical transfer function control does not need to consider controllability; however, care would need to be taken. As can be seen in Figures 4.1 and 4.2, as the vehicle speed increases the open-loop poles transition from real to complex and the relative magnitude of the poles and zeros shifts, i.e. the zero becomes larger than one of the poles. The movement in the open-loop pole and zero locations makes classical transfer function 110 control design techniques more difficult. Equivalent to transfer function control is output feedback as opposed to full state feedback. Yaw rate may be considered as the only measurement (similar to a transfer function from steer angle to yaw rate). However, because only one closed-loop pole is being placed the other is floating, i.e. choosing the control gain also chooses the second closed-loop pole. At some speeds, this second pole may have a positive real part making the system unstable. For these reasons, LQR is preferred over the complexities of classical control and output feedback. 4.2 Lateral Control to Reference Circle The bicycle model controller alone is not enough to drive the vehicle. Higher level states must be incorporated into the system to direct the vehicle to the goal position. For typical potential field controllers, the direction of the gradient of the potential field, i.e. the velocity vector field, is used as a desired vehicle course, ?ref. Note that the stream function can be used in place of the velocity potential to calculate the velocity vector field, i.e. Equation (2.9) instead of (2.6). Using only the potential field gradient as the reference state has two main drawbacks. First, the path the vehicle will follow is completely determined by its initial course, since the gradient is calculated at the vehicle?s current location. At any point (other than the start or goal) a unique desired course exists. Therefore, only one path will be taken to the goal. At the start or goal, the desired course does not exist and thus a controller bypass must be employed. The simplest workaround is to drive straight until a desired course exists. Note that although the precise path cannot be chosen by the end user, all available paths do reach the goal. A second and more troubling problem comes from the dynamic response of the vehicle. 111 If the velocity vector field is tracked exactly, then a streamline is followed. However, this requires the ability to instantly follow the desired course. Typical steered ground vehicles are not able to do this. As the bicycle (or nonlinear) model implies, there is a lag between the steer angle input and the yaw rate response. This lag means that the vehicle will not exactly track a streamline. Instead, there is a course error, which causes the vehicle to drift from its initial streamline. The desired course would then be recomputed at the new position as opposed to along the initial streamline, effectively meaning a new streamline is being followed. Additionally, the change in desired course may be larger than the vehicle can achieve, i.e. the vehicle cannot turn tightly enough. For many scenarios, these issues will not cause trouble with the vehicle reaching the goal. However, as is shown in Figure 4.12, these problems can combine to cause the vehicle to intersect an obstacle. The ultimate cause of the collision is the vehicle not being able to turn fast enough to follow the velocity vector field at the obstacle edge. The velocity vector field is parallel to the obstacle along the obstacle edge as is dictated by the boundary conditions. The reason the abrupt change is needed is that the vehicle trajectory drifts from its initial streamline due to the lag in the vehicle response. This streamline is initially followed because it matches the vehicle?s initial heading (pointing directly at the goal location). While this scenario might seem like an unrealistic alignment of factors that leads to the collision, as the number of obstacles increases, the likelihood of a collision also increases. 112 Figure 4.12 Potential field gradient control The solution to pure gradient following is to also track a desired streamline, ?des. In terms of the control model, this requires an additional state, lateral position error, yerr. As shown in Figure 4.13, the lateral error is defined as perpendicular to the vehicle?s velocity. For calculating the change in lateral error, the desired path is assumed to be a circle. This path allows a reference yaw rate to be used along with the corresponding steer angle and sideslip as described in Section 4.1. Under this assumption, the equation for the lateral error can be calculated by finding the intersection of the desired circle (centered at Ec, Nc and with radius R) and the line perpendicular to the vehicle velocity vector: 113 ? ? ? ? ? ? ? ? 22 c os sin sin c os e r r c c cc y E E N N R E E N N ?? ?? ? ? ? ? ? ? ? ? ????? (4.24) After much algebraic and trigonometric simplification the derivative of the lateral error becomes ? ? ta ne rr e rr e rry V y ???? ?? (4.25) Note that unlike the lateral error equation, the lateral error derivative does not depend on the vehicle?s position or global course, only the lateral position and course errors. This is important for having the system model vary with as few parameters as possible. As with the bicycle model, vehicle speed is the only time varying parameter included in the model. The lateral error derivative can also be linearized for small lateral position and course errors for use in the linear control model: err erryV??? (4.26) 114 Figure 4.13 Reference circle additional control states As with the bicycle model controller, the circle tracking controller must be formulated to drive a state to zero (as opposed to a reference state). For this reason, an error state is defined as the difference in the reference and actual state: re f re f e rr re f re f e rr rr y ?? ?? ??? ??? ? ? ? ??? ?? x x x (4.27) The reference states also match those used in the bicycle model controller, Equation (4.18) with the addition of a reference course, ?ref. However, when tracking a reference circle, the reference yaw rate is determined by the circle radius and vehicle speed: ref Vr R? (4.28) V y e r r ? e r r R E c , N c E , N ? E r e f , N r e f 115 As with traditional potential field controllers, the reference course comes from the direction of the velocity vector field of the potential field at the vehicle?s location: 1tan E ref NVV? ? ??? ???? (4.29) Note that the east and north velocities in this equation are for the potential field, not the vehicle. Unlike the other states in Equation (4.27), there is no reference lateral position; the state is an error state directly. The derivative of the error state is similar to the original state given (in linear form) in Equations (2.33) and (4.1). Because the reference sideslip and yaw rate are computed as the steady state values for a reference steer angle, their derivatives are zero. Note that this assumption is exact when tracking a reference circle at a constant speed, i.e. along a circle at a constant speed the yaw rate and thus steer angle and sideslip are constant. The reference course, on the other hand, obviously varies when moving along the reference circle. In particular, ref refr? ?? (4.30) Recall that the reference sideslip derivative is zero, which is why this Equation (4.30) is different than the actual course derivative given previously in Equation (2.33). Also, note that when the reference course is taken to be tangent to the reference circle, the change in reference course is the reference yaw rate. In practice, the reference course comes from the gradient of the potential field at the vehicle?s location, not the intersection point on the reference circle. However, when the vehicle is on the reference circle this distinction is meaningless as the potential field gradient is tangent to the circle. 116 Just as the states must be transformed to error states, the input must also be transformed: err ref? ? ??? (4.31) As with the reference sideslip and yaw rate, the reference steer angle given in Equation (4.18) is the steer angle required to keep the vehicle on the reference circle. The linearized equations of motion for tracking a reference circle then become 2 22 2 1 0 0 00 00 00 0 0 F R F R F FF R F R ze r r e r r e r rzz FF R F R C C aC bC C m V m V mV aCaC bC a C b C II I V CC C aC bC mVm V m V V ? ? ? ? ? ?? ? ? ? ?? ? ? ? ? ???? ?? ? ? ??? ?? ?? ?? ?? ?? ?? ?? ?? ?? ?? ?? ?? ?? ?? ?? ???? xx? (4.32) As with the bicycle model controller shown in Equation (4.19), full state feedback with feed forward is used to control the reference circle tracking system. However, in this case the gain matrix is expanded to account for the additional states: re f re f re f r y re f e rr rr K K K K y ?? ???? ?? ??? ??? ???? ?? ??? ?? (4.33) The uncontrollable speed of the bicycle model presented in the previous section is also present in this system since it contains the bicycle model. For this reason, the gains for the controller are designed with LQR principles, i.e. Equations (4.21) and (4.22). The state and input weights were tuned by hand to yield acceptable system performance without demanding inputs that are not physically realizable, resulting in 117 0 . 0 1 0 0 0 0 0 . 2 0 0 2 0 0 0 . 0 5 0 0 0 0 0 . 5 R ?? ?? ?? ?? Q (4.34) The other information the controller needs, of course, is the error state to be fed back. The yaw rate, sideslip, and course can be measured directly from the vehicle. Lateral error is calculated by numerically searching the stream function along the line perpendicular to the vehicle velocity vector until the desired streamline (stream function contour) is found. This is the reason an explicit stream function is required with this control method. If only the desired streamline were known as opposed to the entire stream function then this search becomes more difficult since it is harder to determine on which side of the streamline the search point lies. Note that the stream function is only defined at distinct points on a solution grid. The search points along the perpendicular line do not fall on these grid points in general. Therefore, a 2D interpolation must be performed for each search point. This can result in many interpolations for each steer angle calculation. This interpolation is one of the computational bottlenecks when implementing this controller in real time. The reference states and input are calculated from the potential field using Equations (4.18), (4.28), and (4.29). 4.2.1 Reference Circle Calculation For the reference yaw rate a turn radius is required. The circle that most closely matches a curve at a given point is known as the osculating circle. The radius of the osculating circle of the stream function contour is defined as [Weisstein 2008] 118 3222 222 2 2 222 EN R E N E N E N N E ?? ? ? ? ? ? ? ? ????? ? ? ?? ??? ? ? ???? ? ? ? ??? ? ? ? ? ? ? ?? ? ? ??? ? ? ? ?? ? ? ? ? ? ?? ? ? ? (4.35) The center of the circle lies along the line perpendicular to the curve: 22 22 c r e f c r e f R EEE EN R NNN EN ? ?? ? ?? ? ??? ??? ? ? ? ?? ? ? ? ??? ? ? ? ? ??? ??? ? ? ? ?? ? ? ? ??? ? ? ? (4.36) where Eref and Nref are the coordinates of the intersection of the reference streamline and the line perpendicular to the vehicle velocity vector given by ? ?? ?c os sinre f e rrre f e rr E E y N N y ? ? ?? ?? (4.37) Note that each of the partial derivatives in Equation (4.35) must be computed over the entire solution grid producing a radius and center for each point. At each time step, the osculating circle parameters are interpolated at the point the desired stream and lateral error line segment intersect. This interpolation is not as troubling as the lateral error search since it only occurs once per steer angle calculation. Note that the radius could be calculated at the vehicle location in the same manner as the reference course. As with the desired course, the points converge when the vehicle is on the desired streamline. Also, note that the radius is not necessarily positive. Equation (4.35) is actually the inverse of the curvature of the osculating circle. The sign of the curvature determines the direction 119 of the center of the osculating circle, which in turn determines the sign of the reference yaw rate: ? ? 1sign sign ta n c re f cEEr NN? ?????? ????? ???? (4.38) Calculating the streamline radius involves many numerical approximations. To verify the accuracy of the approximation, the vortex potential given in Equation (2.16), is used. The streamlines of vortex flow are circles centered at the origin. Therefore, the radius is known exactly. The results of the true and calculated radius from the vortex potential are shown in Figure 4.14. Over the majority of the solution grid, the difference in the true and calculated radius is very small. The error grows in two locations: along the solution edge and near the origin. At the solution edge, the error is due the numerical nature of the partial derivatives. Ideally, the numerical first and second derivative approximations used in Equation (4.35) rely on points on both sides of the calculation grid point. On the solution edge however, points only exist interior of the calculation point. This reduces the accuracy of the numerical derivative. However, the vehicle should not be following a streamline near the edge of the solution grid. If the desired path nears the solution boundary, then the grid should be expanded to allow room around the desired path. The effects of the edge inaccuracy do not extend beyond the grid points immediately on the edge of the solution grid. This error is the reason the radius error is not shown on the solution edge. The remaining large radius error is centered near the origin. This error is due to the grid spacing. As the radius approaches the magnitude of the grid spacing, the numerical approximations begin to break down. Notice that the largest errors (both positive and 120 negative) occur near the origin. The errors in the missing region around the origin grow quite large, and so are not shown. This error only manifests itself when the radius becomes small, e.g. at the singularity in the center of the vortex. While these errors may be significant near streamline singularities such as the stagnation points on obstacles, over most of the potential field the errors in radius are small enough to be ignored. The stagnation points should not present a problem because the desired streamline should not go that close to an obstacle regardless of this radius calculation. Figure 4.14 Radius calculation validation 121 4.2.2 Controller Response The vortex potential is also used to demonstrate the response of the complete streamline tracking controller. For this simulation, the vehicle is initialized with ?0.01 stream function error, which relates to nearly 1 m of lateral error. The vehicle is due north of the reference streamline facing east. The speed is held constant at 10 m/s and the sample rate is 100 Hz. For this test, the bicycle model is used in place of the nonlinear model to remove errors created by the vehicle model. However, the global position is still computed using Equation (2.31), which is then used to calculate the reference states and lateral error. The trajectory resulting from this simulation is shown in Figure 4.15. Note that after ten meters (one second) the vehicle is essentially on the reference streamline with zero tracking error. Figure 4.15 Trajectory of the circle controller validation, with a linear vehicle model The states and steer angle from the simulation are shown in Figure 4.16. Because vortex flow matches the controller assumptions (i.e. tracking a reference circle) it is 122 possible to compare the actual response to that of the expected closed-loop system. Note that the two lie on top of each other even though the actual system does not directly simulate the lateral error state as the closed-loop system does. 123 Figure 4.16 States of the circle controller validation, with a linear vehicle model 124 The previous simulation is repeated using the nonlinear vehicle model with the results shown in Figures 4.17 and 4.18. Note that although the response does not match the closed-loop model, the settle time is approximately the same. The difference in the responses (which is explored in more detail below) is due to the vehicle nonlinearities, in particular tire saturation. However, the controller is still able to compensate for these unmodeled dynamics. Figure 4.17 Trajectory of the circle controller validation, with a nonlinear vehicle model 125 Figure 4.18 States of the circle controller validation, with a nonlinear vehicle model 126 To test the controller in a more realistic scenario, the map from the gradient controller, Figure 4.12, is used. In particular, the map consists of a rectangular and two circular obstacles between a start location to the southeast and goal location to the northwest. The stream function is calculated using the method developed in Section 3.2.3. The vehicle starts directly facing the goal. The desired streamline is chosen to smoothly avoid all obstacles. The simulation is run at a constant speed of 17.9 m/s (40 mph). As with the previous simulations, the sample rate is 100 Hz. Finally for the simulation, the nonlinear vehicle model is used given in Equations (2.28), (2.29), (2.31), and (2.32) is used. The trajectory of the vehicle is shown in Figure 4.19. Overall, the trajectory looks reasonable. The vehicle deviates from the desired streamline in two areas, near the start location and after passing the circular obstacle. Figure 4.19 Trajectory of the streamline controller response 127 The system response is shown in Figure 4.20. This view of the controller is more troubling than the trajectory. As expected, the reference state is not matched for two periods. The first deviation is due to the initial vehicle state not matching the desired streamline. Recall that this was one of the major down sides of the gradient tracking: the trajectory depends entirely on the initial condition. However, with the streamline controller it is possible to choose the desired trajectory. Also, note that during this period the steer angle saturates at 30 deg. If the initial heading is not close to the tangent of the desired streamline problems may occur. The second deviation is more troubling. This deviation occurs after the vehicle passes the circular obstacle. Some tracking error is expected due to the changing reference circle, i.e. the instantaneous streamline radius is not constant. This changing reference can be seen in the reference yaw rate. Because the speed is constant, the reference yaw rate is inversely proportional to the reference radius as seen in Equation (4.28). In this case, however, the deviation from the reference states is extreme. In particular, the vehicle sideslip reaches 30 deg and yaw rate peaks at ?65 deg/s. The vehicle is out of control even if it does eventually return to the desired streamline. Notice that even the reference yaw rate reaches 30 deg/s and this is when the vehicle behavior becomes erratic. 128 Figure 4.20 States of the streamline controller response 129 Ultimately, tracking the desired streamline in this scenario requires the vehicle to perform outside its capabilities. The tire responses for the simulation are shown in Figure 4.21. Note that the tires become saturated during the simulation. For normal driving and to not invalidate the controller assumptions, vehicle maneuvers should be restricted to ensure that the tire forces remain in the linear region of the tire. An initial concept to correct this problem might be to raise the LQR weights on the states so that the states more closely match the reference and thus are in the linear region. However because the reference state is outside the linear region, this would not correct the problem. The solution must lie in modifying the reference states so that unreasonable behavior is not expected and the vehicle limitations are respected. Figure 4.21 Tire curves from the streamline controller response In summary, a vehicle controller that tracks a desired streamline given a potential field stream function has been developed. This controller accounts for a critical speed at which the vehicle becomes uncontrollable by using LQR techniques to design the control 130 gains. The controller is designed around a system based on tracking a constant reference circle. Methods for calculating the reference circle and other reference states have been presented. This controller, and more precisely the reference trajectory, has the problem of exceeding the physical limitations of the vehicle. Chapter 5 describes several methods to modify the reference trajectory to accommodate the constraints imposed by the vehicle. 131 5 PATH MODIFICATION To account for the constraints imposed by the vehicle, three modifications are made to the controller presented in Chapter 4: the vehicle speed is adjusted, lateral acceleration is limited, and the reference streamline is shifted. Specifically, Section 5.1 presents a method to approximate the distance to an obstacle using potential fields. This potential field is then used to determine a reference speed for the vehicle. Additionally, this section discusses a method to limit steer angle and speed when the vehicle exceeds a lateral acceleration limit. Section 5.2 further improves the control algorithm by shifting the reference streamline away from obstacles if the vehicle becomes too close. This serves two purposes: it allows the vehicle to travel faster since the reference speed is slow near an obstacle, and it increases safety since there is more space between the vehicle and obstacles. 5.1 Reference Speed Typically, potential field controllers use the magnitude of the potential field gradient as the vehicle speed reference. In terms of the fluid dynamics scheme for the potential field, this means the fluid speed is also the vehicle speed. This poses problems for vehicle control. When two obstacles are close together, e.g. Figure 3.4, the streamlines become compressed between the obstacles; this indicates an increased fluid speed. In this scenario a vehicle should slow down. Far from the obstacles, the streamlines are spaced 132 out indicating a slow fluid speed, but the vehicle can safely speed up in these regions. Based on these two examples, a solution may be to use the inverse of the gradient magnitude as the vehicle speed. However, recall the stagnation points that exist on the obstacle edges. At these points the fluid velocity is zero implying its inverse, the proposed vehicle speed, is infinite. The obstacle edge is not the location for an infinite speed singularity. Therefore, a new scheme is developed to determine the vehicle?s reference speed. Intuitively, a vehicle?s speed should decrease near an obstacle and increase with distance from obstacles. Distance to a generic shape, however, is not always an easy number to calculate. Alternatively, potential field techniques can be used to implement the same intuitive concept. In particular, using Dirichlet boundary conditions the reference speed can be calculated as a potential field, ?V. The potential is low (or zero) on an obstacle edge and a max allowable speed on the world boundary. Presumably the world boundary is far enough away from any obstacles that a high speed is safe. If this assumption is not true, the solution area should be increased in size. An example of the reference speed potential field is shown in Figure 5.1. Notice that the reference speed is zero on the obstacle edges and increases with distance from the obstacles to a maximum of 17.9 m/s (40 mph) at the world boundary. Note that this potential is not proportional to the distance to the obstacle. Recall that Dirichlet boundary conditions correspond to sources or sinks in analytical flow. From Figure 2.2 it is obvious that the velocity potential of a source does not vary linearly with distance from the source location. The same is true with Dirichlet boundary conditions. Even though 133 this potential is not proportional to distance to an obstacle, it serves the same purpose of creating a low reference speed near obstacles and a high speed far from obstacles. Figure 5.1 Reference speed potential using Dirichlet boundary conditions Incorporating the reference speed into the vehicle controller means that two potential fields must be calculated, the reference streamline and the reference speed. However as is demonstrated in Table 3.1, calculating a potential field with Dirichlet boundary conditions does not add appreciably to the overall computation time. Additionally, the vehicle?s response to the reference speed must be included in the simulation. To this end, Equation (2.30) is used to calculate the actual vehicle speed given a reference speed input, Vref. The reference speed input is interpolated from the reference speed potential, ?V, to the vehicle?s location. 134 To test the reference speed addition to the controller, a vehicle is simulated in the same test map used previously, but with the reference speed potential shown in Figure 5.1. The trajectory resulting from this simulation is shown in Figure 5.2. Notice that unlike the previous simulation (Figure 4.19), the vehicle is able to follow the desired streamline after passing the circular obstacle. However, the initial error is still present. Because the vehicle is not initially near any obstacles, its reference speed is not drastically modified from the initial 17.9 m/s (40 mph) and the response is effectively the same. In other words, modifying the reference speed is designed to mitigate errors near obstacles, not the transient errors due to initial conditions. Figure 5.2 Trajectory of the streamline controller response, adjusting speed The vehicle response to this simulation is shown in Figure 5.3. Note that the vehicle speed significantly drops as the obstacles are passed. Also, note that unlike the constant 135 speed simulation, the reference states are tracked (after the initial transients die out). Reducing the speed near obstacles serves two purposes. First, it increases safety by slowing down near obstacles as a human driver would. Second, it produces lower speeds in the areas most likely to require tighter maneuvering. The most drastic changes in the direction of the potential field gradient occur near singular points or sharp obstacle corners. Recall that the stream function boundary condition only produces singular (stagnation) points on the obstacles edges. Therefore, the majority of sharp turns in the reference trajectory occur near obstacles. The vehicle is better able to negotiate the sharp turns if it is moving slowly. The world boundary corners also produce sharp turns because the boundary is treated like an obstacle. However, as has been mentioned, the vehicle should not be following a streamline near the world boundary. The increased maneuverability at low speeds is the reason the vehicle is able to track the desired streamline when the reference speed is modified (which it was not able to accomplish when driving at a high constant speed). 136 Figure 5.3 States of the streamline controller response, adjusting speed 137 Another interesting feature of this system response is the slight oscillation in the steer angle between 30 and 45 seconds. This is due to the low vehicle speed during this time. Recall from Figure 4.1 that the natural frequency of the vehicle model increases as speed decreases. Because of this, the controller is too aggressive at low speeds, i.e. the gains are too high. The solution to the problem would be to lower the LQR weights on the states. However, in order to concentrate specifically on the interaction of the path planner and controller, in this research a single set of LQR weights is used. This results in the oscillation observed in the response. The last feature of this response is the transient dynamics. During the transient phase of the response, the steering angle still saturates at 30 deg and the lateral acceleration is in excess of 1 g at times. The maximum lateral acceleration possible for a Chevrolet Corvette (such as the vehicle simulated in this work) is 0.93 g, so the simulation results reinforce the nonlinear vehicle model (although the model parameters are slightly incorrect resulting in the higher lateral acceleration). However, this much lateral acceleration is not safe vehicle behavior. As the tire curve from the simulation in Figure 5.4 shows, to create the 1 g of lateral acceleration the tires saturate, just as with the constant speed simulation. Even though the trajectory is followed, the extreme vehicle response is not acceptable. 138 Figure 5.4 Tire curves from the streamline controller response, adjusting speed To keep the tire behavior in the acceptable range as well as to prevent the vehicle from rolling, a limit is imposed on the lateral acceleration. To guarantee the vehicle does not exceed the lateral acceleration limit, both the steer angle and the reference speed are limited. In particular, lateral acceleration can be approximated in steady state as yra Vr V D C ??? ? (5.1) where the DC gain comes from Equation (4.18). If the lateral acceleration from Equation (5.1) exceeds a safe limit, the steer angle is reduced to maintain the lateral acceleration at the safe limit: ? ?m a x m a x lim if si gn ry y r V DC a a V DC ? ?? ? ? ? ? (5.2) Additionally, when the lateral acceleration limit is exceeded, the reference speed is limited: 139 m a x , li m i f &r y re f re f V DC a V VVV? ???? (5.3) The combination of these two limitations together allows the vehicle to turn as much as possible without becoming unsafe. Beyond the potential field path planner and controller, this limit or one similar to it should be included in any lateral vehicle controller to prevent extreme behavior. The simulated response including the lateral acceleration limit is shown in Figures 5.5-5.7. For this simulation, the lateral acceleration limit is set to 0.5 g. For the most part the response is identical to the previous case without the lateral acceleration limit. The only difference is in the initial transients. The vehicle converges to the desired streamline in four seconds as opposed to three seconds when the lateral acceleration is not limited and the maximum lateral error increases from 2.5 m to 3 m. Notice that during the transients the lateral acceleration is held at the limit as the algorithm is designed to do. Because the lateral acceleration is limited, the lateral tire forces do not saturate. In general, the lateral acceleration limit can be adjusted based on the desired aggressiveness of the vehicle controller and the terrain conditions. In particular, methods exist to estimate the peak lateral tire force as the vehicle drives [Daily 2007]. The lateral acceleration limit can then be adjusted in real time based on the peak force so that the tire forces do not saturate regardless of the terrain. 140 Figure 5.5 Trajectory of the streamline controller response, limiting lateral acceleration Figure 5.6 Tire curves from the streamline controller response, limiting lateral acceleration 141 Figure 5.7 States of the streamline controller response, limiting lateral acceleration 142 5.2 Reference Streamline Shifting With the reference speed potential and the lateral acceleration limit, the vehicle controller safely navigates the map of obstacles while keeping the vehicle within acceptable behavior limits. The last modification to the controller involves shifting the desired streamline away from obstacles as the vehicle is driving. There are two main reasons for this modification. The first is to increase the vehicle speed through the obstacle field. Recall from the previous section that reference speed increases with distance from obstacles. Therefore, when the reference streamline is shifted away from the obstacle the vehicle speed increases. The second reason to shift the reference streamline is to keep the vehicle a safe distance from obstacles. Although all streamlines reach the goal without intersecting any obstacles, they may pass quite close to an obstacle. By shifting the desired streamline away from the obstacle, a factor of safety is added to the controller to prevent collisions with obstacles due to tracking error. Ideally the initial reference streamline is chosen with this factor of safety. However, occasionally a single streamline may not be able to avoid all obstacles with a safety buffer. Additionally, if new obstacles are detected and the potential field is modified, then there is no guarantee on how close the reference streamline comes to obstacles. Ultimately, allowing the reference streamline to shift in real time reduces the dependence on the initial reference streamline choice. To accomplish the reference streamline shifting, the reference speed potential developed in Section 5.1 is used. This potential varies with distance from the obstacles and its minimum is at the obstacle edges. The gradient of the reference speed potential points away from the obstacles as shown in Figure 5.8. The reference streamline is 143 shifted in the direction of this gradient and thus away from the obstacles. The purpose of shifting the reference streamline is to keep a buffer between the vehicle and any obstacles. Therefore, when the vehicle is already far from obstacles, the reference streamline is held constant. The reference streamline only shifts when the vehicle is near an obstacle. This aids controller performance since it is easier to track a constant reference rather than one that is continuously being updated. This threshold is implemented by only shifting the reference streamline when the reference speed, which is correlated with distance to an obstacle, is below a limit. The threshold in Figure 5.8 (and the remaining simulations) is 4.47 m/s (10 mph). Figure 5.8 Reference speed gradient 144 An example of the reference streamline shifting is shown in Figure 5.9. The control point on the reference streamline (Eref, Nref) is calculated using Equation (4.37). The direction of the reference speed gradient is calculated at the control point: 1tan V V EN?? ?? ????? ?????? (5.4) The distance the streamline is shifted is 22VV pos K EN? ??? ??? ? ? ?? ? ?? ? ? ?? ? ? ? (5.5) where K? is a tuning parameter to determine how much the reference streamline shifts each time step. Note that as the magnitude of the gradient increases so does the amount the streamline shifts. From this information, the point on the new reference streamline is calculated: ? ? ? ? , , c os sin V ne w re f pos re f V ne w re f pos re f E E E K E N N N K N ?? ??? ??? ?? ? ? ? ? ? ?? ? ? ? ? ? (5.6) The new reference streamline, ?ref,new, is the contour of the stream function that passes through this point, i.e. the value of the stream function at this point. 145 Figure 5.9 Reference streamline shift near an obstacle Note that the streamline shifting bears some resemblance to Khatib?s or other Dirichlet type potentials, [Khatib 1985, Kholsa 1988, Sato 1993]. In particular, the reference streamline is moved directly away from obstacles. However, the fact that the reference streamline is being repelled from the obstacle instead of the vehicle itself is an important distinction. The vehicle control still comes from the stream function potential. In particular, the reference course is calculated at the vehicle?s position and therefore is not affected by the reference streamline shift. The reference yaw rate, steer angle, and sideslip all come from the osculating circle at the control point on the reference streamline. These values change when the reference streamline shifts, but if the shift is small then so are the corresponding changes in reference states. The term that is most directly affected is the lateral error. Near an obstacle, the streamlines are tangent to the ? ? p o s ? E , N E r e f , N r e f ? r e f ? r e f , n e w E ? , n e w , N ? , n e w V y e r r O b s t a c l e 146 obstacle edge. If the controller is working well, the vehicle course will also be tangent to the obstacle edge. This means that the lateral error is nearly normal to the obstacle edge. Due to the nature of the Dirichlet boundary condition used in the reference speed potential, its gradient is also nearly normal to the obstacle edge. Therefore, these two terms effectively add together to produce the new lateral error. Because of the other reference states remaining nearly constant however, the vehicle continues to drive along the reference streamline while also being pulled away from the obstacle. In pure Dirichlet potential field control, the control effort drives the vehicle directly away from the obstacle. The simulated response including the reference streamline shifting is shown in Figure 5.10. For this simulation the streamline shift gain, K?, is 0.08 m?s. Recall that the reference speed decreases near obstacles. Therefore, when the reference speed is over 4.47 m/s (10 mph), i.e. the vehicle is away from obstacles, the reference streamline is constant. Inside the speed threshold the reference streamline changes. Around the rectangular obstacle, the shift is minor since the vehicle is far enough away from the obstacle that the gradient is small. However, as the vehicle begins to approach the circular obstacle the streamline shift is much greater. 147 Figure 5.10 Trajectory of the streamline controller response, shifting the reference streamline The vehicle response for this simulation is shown in Figure 5.11. This response is similar to the constant reference streamline response (Figure 5.7) with the obvious exception of when the reference speed is below 4.47 m/s (10 mph). In this region, the reference streamline, i.e. the stream function value, shifts. In particular, the value initially decreases then increases. Recall from Section 3.2.3 that a stream function of ?1 corresponds to the world border clockwise from the start, e.g. northeast corner, whereas ?1 corresponds to counterclockwise from the start, e.g. southwest corner. Therefore, the reference streamline initially moves to the right (from the vehicle?s perspective) then to the left. This corresponds to the trajectory shown in Figure 5.10. Another interesting feature of this response is that when the reference streamline is shifting there is tracking 148 error. This is not unexpected since changing references often result in tracking error (depending on system type, etc.). For this controller however, the tracking error does not cause any problems. The controller is innately knowledgeable of the fact that no obstacles are nearby due to the potential fields. Therefore, the tracking error is acceptable. Once the reference speed increases past the threshold, the reference streamline remains constant and the vehicle quickly converges to it. 149 Figure 5.11 States of the streamline controller response, shifting the reference streamline 150 Changing the streamline shift gain can alter the behavior of the controller. For instance, the same test scenario with a shift gain of 1 m?s is shown in Figure 5.12. In this simulation, the shift gain is large enough to overcome the controller reference states and force the vehicle to go to the outside of the obstacles. In particular, the shift gain is high enough to force the reference streamline to follow the reference speed shift threshold. The streamline shift gain effectively acts like an additional control gain. This gain determines the importance of keeping a high reference speed vs. the other control states. For instance, with the high shift gain the vehicle took 45.18 seconds to reach the goal with an average speed of 6.82 m/s (15.3 mph) whereas the low shift gain required 50.25 seconds with an average speed of 5.87 m/s (13.1 mph). Notice that both are faster than when the reference streamline is constant; this simulation had a time of 54.06 seconds and an average speed of 5.34 m/s (12 mph). If minimizing distance travelled is critical then a lower shift gain is better so that passing between obstacles is allowed. On the other hand, if minimizing travel time is important, a higher shift gain may be better to keep the vehicle speed higher. Note that a higher shift gain does not guarantee a shorter travel time. In particular, the increased travel distance to go outside the obstacles may outweigh the increased speed resulting in a longer travel time. 151 Figure 5.12 Streamline controller response with a large streamline shift gain A dense obstacle grid simulation is shown in Figure 5.13. In this scenario, twelve 20?40 meter rectangles are spaced 20 meters apart. These could represent buildings separated by roads, although as the trajectory shows the vehicle does not obey traffic laws. This simulation demonstrates the controller?s ability to handle dense obstacle fields. There are two interesting points about this simulation. First, the boundary condition on the reference speed potential at the obstacle edges is set to 2.24 m/s (5 mph) as opposed to zero in the previous simulations. This is so that the vehicle does not stop when passing between obstacles. Recall that potential fields with a Dirichlet boundary condition become flat when a single boundary value dominates the potential field (as shown in Figure 3.27). This is the case for the reference speed potential between the obstacles. If the reference speed is zero at the obstacle edges, then between obstacles the 152 reference speed potential is effectively zero and the vehicle stops. The other feature of this simulation is how close the vehicle comes to the obstacles. The cause of this is also related to the flat reference speed potential. The streamline shift gain is set to 0.08 m?s as in Figure 5.10. However, because the potential is essentially flat in this scenario, the magnitude of the gradient is small and the reference streamline is not appreciably shifted. Figure 5.13 Streamline controller response for a dense obstacle field In this chapter, the streamline controller has been modified, making it more effective and safer. In particular, a lateral acceleration limit is imposed on the vehicle by limiting steer angle and longitudinal acceleration. The reference speed sent to the vehicle is also modified to vary as the vehicle progresses through the course. The reference speed increases with distance from the obstacles. This allows the vehicle to maneuver tighter near obstacles and increases safety by slowing down in the proximity of obstacles. 153 Finally, a mechanism to shift the reference streamline that the vehicle is following away from obstacles has been developed. This modification also increases safety by creating a buffer between the vehicle and obstacles. It may also decrease the time to reach the goal by keeping the reference speed higher. At this point, a successful path planner and controller have been developed. Additionally, several test scenarios have been shown where the vehicle negotiates an obstacle field to reach a goal. 154 6 CONCLUSIONS This work has developed a harmonic potential field based path planner and controller for navigating a vehicle through an obstacle field to a goal location. The potential fields provide a method to combine the path planning and control routines into a single algorithm. The control effort to the vehicle comes directly from the potential field instead of through a reference path. This allows the vehicle controller to be more aware of the vehicle?s surroundings and therefore make better decisions. To this end, a new method of calculating a potential field was developed. Based on this potential field, a controller was designed to drive the vehicle to the goal location. This base controller was then modified to account for vehicle dynamic limitations and increase safety. 6.1 Overall Results The potential field used in this research was derived from fluid dynamics concepts. In particular, it satisfies the Laplace partial differential equation. Solutions of the Laplace equation (harmonic functions) are free of extrema thus the vehicle is guaranteed to reach the goal. Varieties of design methods to generate a potential field that satisfies the Laplace equation and represents the physical world the vehicle is maneuvering in were presented. These methods fall into two categories, analytical and numerical. Each of the potential field design methods was judged based on three requirements of the vehicle controller: the ability to accurately represent generic shaped obstacles, the 155 presence of an explicit stream function for use in the vehicle controller, and the calculation time necessary to compute the potential field. Two new methods of generating the potential field were developed. The first method is an analytic solution. This method uses a weighted average of individual obstacle potential fields to combine circular obstacles and roughly approximate the shape of a physical obstacle. This method satisfies the three judging criteria. However, its behavior outside these criteria often becomes unacceptable. In particular, the reference paths the vehicle follows are convoluted near obstacles and often needlessly approach obstacles before veering away. The second method involves a numeric solution of the stream function (as opposed to the velocity potential most numeric solutions calculate). This method exactly (to the resolution of the solution grid) represents any shape obstacles and satisfies the other judging criteria. Because of the ability to exactly represent any shape obstacle and the smoother streamlines, the numeric stream function potential field was chosen as a more effective path planning method. Typical potential field controllers rely solely on the gradient of the potential field to direct the vehicle to the goal location. This works well for robots that can instantaneously track the changing course defined by the gradient. However, for steered vehicles the dynamic response and vehicle limitations, e.g. turning radius, make exact gradient tracking problematical. To correct this problem, a vehicle controller that tracks a reference streamline in addition to the potential field gradient was designed. This controller is implemented as a full state feedback controller designed around the bicycle vehicle model with two additional states of course and lateral error based on tracking a reference circle. It was shown that the bicycle model becomes uncontrollable at a critical 156 speed. The cause of the uncontrollability is a pole/zero cancellation in the system model. To counter the uncontrollability, LQR techniques are used to calculate the control gains. LQR inherently places one of the closed-loop poles at the pole/zero cancellation. Therefore, the control gains remain reasonable even at the critical speed. The vehicle controller was developed with a feed forward term so that there was no tracking error to the reference circle (assuming no model uncertainty or disturbances). The reference states used to calculate the feed forward steer angle and state error are calculated from the potential field. In particular, the reference course is the direction of the gradient of the potential field, the reference yaw rate is calculated from the osculating circle of the reference streamline, and the reference sideslip and steer angle are the steady state values required to hold the reference yaw rate. Although the numerical stream function potential field is used in this research, the controller will work with any potential field that contains an explicit stream function. In simulation, this controller was shown to work for test scenarios that matched the model assumptions, i.e. the streamlines were circles, as well as on a test obstacle field. While this controller drove the vehicle to the goal, it did so in a method that was not safe for the vehicle. This extreme behavior motivated changes to the base controller. Therefore, three additions were made to the base vehicle controller to remove the vehicle?s severe response: the reference speed was modified, the lateral acceleration was limited, and the reference streamline was shifted. The purpose of modifying the reference speed is to force the vehicle to slow down as it approached obstacles. This obviously increases safety by reducing speed when the chance of a collision is increased, but it also increases the vehicle?s maneuverability near obstacles where tighter turns are 157 often required. To implement the reference speed, a second potential field was created. This potential is low near obstacles and high at the world boundary. Using this method, the solution grid must be chosen such that the world boundary is far enough away from the obstacles to make higher speed driving safe. The reference speed sent to the vehicle speed model (derived from a simplified cruise controller) is the value of the reference speed potential at the vehicle?s location. This modification to the controller reduces unwanted vehicle behavior near obstacles. However, it does not affect the behavior far from obstacles (usually due to initial conditions). To guarantee that the vehicle remained within its safety limits (i.e.to prevent roll over or sliding) a lateral acceleration limit was also imposed. This limit is imposed by restricting the steer angle the controller can command. Additionally, when the vehicle is at this limit the reference speed is not allowed to increase. This effectively keeps the vehicle in the linear tire region and removes extreme handling behaviors. The last modification made to the controller was a method to shift the reference streamline when the vehicle is near an obstacle. This shifting is based on the reference speed potential since that potential is correlated to the distance to an obstacle. If the reference speed potential is below a threshold, i.e. the vehicle is near an obstacle, then the reference streamline is shifted in the direction of the gradient of the reference speed potential, i.e. away from the obstacle. This modification both increases safety by creating a buffer between the vehicle and obstacles and decreases time to the goal location by keeping the reference speed high since the reference speed is reduced near obstacles. Throughout the dissertation, the various controllers were shown navigating a standard test course. In addition, the complete controller was demonstrated on a test course densely 158 populated with obstacles. Once the modifications were made to reduce the aggressiveness of the controller, it performed well in both tests. 6.2 Future Work Although a successful path planner and controller are presented in this dissertation, there remain several areas where both the potential field and the controller can be researched further. For the potential field, as with the work presented in this dissertation, areas of investigation are present in both the analytic and numeric solutions. There are also areas that could be researched that apply to both methods. Possible improvements to the controller exist for both the structure of the controller and for its implementation. 6.2.1 Potential Field Several broad areas could be further researched with regard to the potential field. The first is investigating other differential equations to generate the potential field. In particular, other research has initially investigated biharmonic functions (fourth derivatives instead of second as with the Laplace equation) [Masoud 1994]. However, these techniques have not been studied in the more advanced controller presented in this research. Biharmonic potential fields may produce smoother streamlines that are easier for a vehicle to follow. Another area of major interest is moving obstacles. These have been studied for analytic circular obstacles, but not for the broader case of general shaped obstacles, or numeric solutions. It has been shown that simply updating the obstacle location every time step and assuming it is constant is not sufficient [Waydo 2003]. A technique that is becoming popular for obstacle avoidance is probabilistic obstacles. These techniques acknowledge that the obstacle and vehicle position are not known 159 exactly. Incorporating these techniques into the potential field methods would be a major improvement. Finally, a method to represent not only obstacles but also terrain types in the potential fields would be beneficial. For example, a steep hill may be possible to navigate, but only slowly. Incorporating a scale of drivability into the potential field would allow for more advanced navigation than simply binary (i.e. yes/no) obstacles. For the analytic potential fields, the improvements center on alternative methods to define obstacles since none of the methods studied in this work were entirely successful. In particular, this research concentrated mainly on combining circles to create generic shaped obstacles. However, fluid dynamicists often use conformal mapping techniques to transform the flow around circles into flow around other shapes. This is a common method used to analyze airfoils. These same techniques may allow circles to be transformed into common obstacle shapes. However, problems would still exist with obstacles corrupting the edge streamline of previously added obstacles. Another possibility to form an analytic potential field is to place a series of doublets directly in the flow. Recall that doublets are formed inside the circular obstacles. For that reason, it should be possible to insert a series of doublets into the flow instead of a series of circles. When creating a doublet, two parameters must be defined: strength and direction. If the doublet method is to be pursued, a technique must be developed to calculate these parameters to match a physical obstacle. Another interesting method to represent an obstacle is using a distributed source to model flow around a polygon. However, as described in Section 3.1.1, sources create unwanted wakes downstream of the obstacle. A possible fix for the wake may be to add an equivalent sink inside the polygon obstacle. 160 For the numeric techniques, the stream function calculation method appears to successfully navigate around obstacles. Improvements to the method are either to relax the assumptions this work makes or to improve the solution technique. The stream function potential makes two assumptions about the start and goal location: they are along the world boundary and there is only one of each. Moving the start and goal away from the world boundary is theoretically not difficult. A discontinuity is created along a line segment, preferably facing away from the other start or goal location. Although not theoretically difficult, implementing this may be more complex. Determining which side of the line segment a point is on is not difficult. However, the value on the boundary is more problematical. Either a single line of points must create the boundary condition in which case the boundary condition is different on opposite sides of the point (i.e. the point actually has two boundary conditions), or a region of points around the line segment must be used to set the boundary condition. Additionally, the discontinuity would need to be accounted for when computing the gradient (i.e. the velocity vector field). Adding multiple starts or goals is also problematical. It is not immediately obvious what the boundary condition should be, in particular, the location of the discontinuity. For an analytic source, the discontinuity location does not matter. The same should hold true of the numeric approximation. This implies that the world edge boundary condition switches signs when each of the discontinuities is crossed along the edge, which does not seem intuitive or trivial to account for. One major area that deserves study in all potential field methods is the grid generation technique. With fluid dynamics PDE solvers, creating the solution grid is often a large 161 portion of the computation time. Using a non-uniform grid both speeds computation time and increases accuracy. The computation time is improved by reducing the grid size in areas that are open and do not need many grid points to define. However, around obstacles, in particular, many grid points would be used both to precisely define the obstacle shape and to capture the larger changes in the potential field that occur around obstacles. The last major area that should be studied is adding obstacles to the potential field in real time. In practical applications, the complete world map will not be known until the vehicle begins to navigate through the course and sensors detect the obstacles. Intuitively, numeric potential fields are well suited to handle additional obstacles. The potential field before an obstacle is detected would serve as the initial guess for the updated field. If the changes are minor, the solution should converge quickly. However, for the convergence to occur in real time, better PDE solvers may need to be employed. This research used custom developed software to calculate the potential field. PDE solvers are another aspect of computational fluid dynamics that have seen a large amount of improvement. Applying more complicated (and faster) solvers to the potential fields used in path planning may extend their use into dynamic real world environments. 6.2.2 Vehicle Controller There are several possible improvements that could be made to the controller presented in this dissertation. The main improvement would be to replace the lateral state with a stream function error. These two errors serve the same purpose: to determine the error relative to the reference streamline. The difference is that lateral error is obviously a true distance and so its dynamics are simple kinematic relationships with the other 162 states. The change in stream function as the vehicle moves is not as easy to predict. In particular, the change depends on the gradient of the stream function. Therefore, the state matrix would depend on the potential function gradient as well as the vehicle speed. However, the benefit of this approach is that the computationally expensive lateral error search would not be necessary. The stream function error is simply the difference in the desired streamline and the stream function at the vehicle?s location. This would also be a more elegant solution since it further reduces the similarity to tracking a reference path and fits directly into the potential field control framework. Another improvement would be developing a method to optimally determine the initial reference streamline. This operation however, could be computationally expensive. A cost function for the optimal streamline could be either distance travelled or time to goal. In either case, computing the cost function for a candidate streamline would involve integrating a particle travelling along the streamline. This would need to be done for every streamline under consideration. This integration should include the reference speed potential as the particle?s speed. For complete accuracy, it could also include the streamline shifting. However, including these terms increases the computational burden on the algorithm. Additionally the cost function will typically have local minima in addition to the global minimum. For instance, the streamlines on either side of an obstacle will have faster travel times than those near the obstacle due to the slower reference speed near obstacles. However, only one side of the obstacle can be the true minimum. These local minima would make searching for the true optimum streamline difficult. 163 The reference speed potential could also be improved. The dense grid simulation demonstrated a problem with this potential becoming very flat in an area surrounded by obstacles. Therefore, a potential field proportional to the true distance to the obstacle may make a better reference speed. This potential would have a better defined maximum through the obstacles. This implies a larger gradient, which would improve the streamline shifting algorithm. The downside of using the actual distance to an obstacle is the cost of computing that distance. One other improvement that could be developed for the reference speed is to make it directional. Once the vehicle is past an obstacle, its speed can increase even if it is still close to the obstacle. This improvement would most likely not occur in the reference speed potential creation algorithm since this potential is independent of the vehicle orientation (although direction to the goal may substitute, albeit less safely). Instead, the conversion from reference speed potential to the reference speed for the vehicle could take into account the vehicle orientation, e.g. through a dot product. The controller may also need some modification to allow for a wider application. In particular, sideslip is typically an expensive measurement to acquire. Removing the need for this measurement would make the controller easier to implement. The best way to remove the sideslip measurement is to use an estimator. Typically, model based sideslip estimators have not been successful due to the requirement for an accurate vehicle model and the relatively small magnitude of the signal. However, the controller in this research already assumes an accurate vehicle model exists. Additionally, the state could be deweighted in the control gains so that errors in the measurement/estimate do not have a large impact on the response. An alternative method would be to use output feedback 164 instead of state feedback. However, in this case the location of one of the closed-loop poles could not be arbitrarily chosen. For certain speeds this floating pole may be in the right half plane, causing the vehicle to become unstable. Related to the difficult sideslip measurement, a study into the effects of sensor uncertainties would be beneficial. In particular, how the measurement errors propagate through the potential field interpolation to the reference states is not obvious. Techniques exist for minimizing the effects of sensor error. However, these techniques assume the error is directly applied to an input or measurement, not propagated into the reference state and input. The last obvious improvement to this research would be to implement the controller on an actual vehicle. However, this requires a working autonomous vehicle with known model parameters and the sensor suite necessary to measure the required information. At the time of this writing, such a system was not available for use with this research. 165 BIBLIOGRAPHY [1] Akishita, S., Kawamura, S., and Hayashi, K., 1990, "New Navigation Function Utilizing Hydrodynamic Potential for Mobile Robot," IEEE International Workshop on Intelligent Motion Control, 2, pp. 413-417. [2] Antoulas, T. and Slavinsky, J., 2004, "Controllability and Observability Grammians," , Connexions, March 20, 2008. [3] Barr, T. H., 1997, Vector Calculus, Prentice Hall, Upper Saddle River, NJ. [4] Bay, J. S., 1999, Fundamentals of Linear State Space Systems, WCB/McGraw- Hill, Boston. [5] Briggs, W. L., 1987, A Multigrid Tutorial, Society for Industrial and Applied Mathematics, Philadelphia PA. [6] Connolly, C. I., Burns, J. B., and Weiss, R., 1990, "Path Planning Using Laplace's Equation," IEEE International Conference on Robotics and Automation, 3, pp. 2102-2106. [7] Connolly, C. I. and Grupen, R. A., 1993, "On the Applications of Harmonic Functions to Robotics," Journal of Robotic Systems, 10(7), pp. 931-946. [8] Currie, I. G., 1993, Fundamental Mechanics of Fluids 2nd ed., McGraw-Hill, New York NY. [9] Daily, R. and Bevly, D. M., 2004, "The Use of Gps for Vehicle Stability Control Systems," IEEE Transactions on Industrial Electronics, 51(2), pp. 270-277. [10] Daily, R., Travis, W., Bevly, D. M., et al., 2006, "Sciautonics-Auburn Engineering?s Low-Cost, High-Speed Atv for the 2005 Darpa Grand Challenge," Journal of Field Robotics, 23(8), pp. 579-597. [11] Daily, R., Travis, W., and Bevly, D. M., 2007, "Cascaded Observers to Improve Lateral Vehicle State and Tyre Parameter Estimates," International Journal of Vehicle Autonomous Systems, 5(3), pp. 230-255. [12] Daily, R. and Bevly, D. M., 2008, "Harmonic Potential Field Path Planning for High Speed Vehicles," American Control Conference. 166 [13] Dixon, J. C., 1991, Tyres, Suspension, and Handling, Cambridge University Press, Cambridge England. [14] Dugoff, H., Fancher, P. S., and Segel, L., 1970, "An Analysis of Tire Traction Properties and Their Influence on Vehicle Dynamic Performance," SAE Paper, No. 700377. [15] Eichhorn, M., 2004, "An Obstacle Avoidance System for an Autonomous Underwater Vehicle," International Symposium on Underwater Technology, pp. 75-82. [16] Ferziger, J. H., 1998, Numerical Methods for Engineering Application 2nd ed., Wiley, New York NY. [17] Ferziger, J. H. and Peric, M., 2002, Computational Methods for Fluid Dynamics 3rd ed., Springer, New York NY. [18] Friedland, B., 2005, Control System Design : An Introduction to State-Space Methods Dover ed., Dover Publications, Mineola, NY. [19] Garrott, W. R., Monk, M. W., and Chrstos, J. P., 1988, "Vehicle Inertial Parameters--Measured Values and Approximations," SAE Paper No. 881767. [20] Ge, S. S. and Cui, Y. J., 2002, "Dynamic Motion Planning for Mobile Robots Using Potential Field Method," Autonomous Robots, 13(3), pp. 207-222. [21] Gillespie, T. D., 1992, Fundamentals of Vehicle Dynamics, Society of Automotive Engineers, Warrendale PA. [22] Guldner, J. and Utkin, V. I., 1995, "Sliding Mode Control for Gradient Tracking and Robot Navigation Using Artificial Potential Fields," IEEE Transactions on Robotics and Automation, 11(2), pp. 247-254. [23] Hesse, T. and Sattel, T., 2007, "Path-Planning with Virtual Beams," American Control Conference, pp. 3904-3905. [24] Kenwright, D. N. and Mallinson, G. D., 1992, "A 3-D Streamline Tracking Algorithm Using Dual Stream Functions," IEEE Conference on Visualization, pp. 62-68. [25] Keymeulen, D. and Decuyper, J., 1994, "The Fluid Dynamics Applied to Mobile Robot Motion: The Stream Field Method," IEEE International Conference on Robotics and Automation, 1, pp. 378-385. 167 [26] Khatib, O., 1985, "Real-Time Obstacle Avoidance for Manipulators and Mobile Robots," IEEE International Conference on Robotics and Automation, 2, pp. 500- 505. [27] Kholsa, P. and Volpe, R., 1988, " Superquadric Artificial Potentials for Obstacle Avoidance and Approach " IEEE International Conference on Robotics and Automation, pp. 1778-1784. [28] Kim, J. O. and Khosla, P. K., 1992, "Real-Time Obstacle Avoidance Using Harmonic Potential Functions," IEEE Transactions on Robotics and Automation, 8(3), pp. 338-349. [29] Krogh, B. and Thorpe, C., 1986, "Integrated Path Planning and Dynamic Steering Control for Autonomous Vehicles," IEEE International Conference on Robotics and Automation, 3, pp. 1664-1669. [30] Kyriakopoulos, K. J., Kakambouras, P., and Krikelis, N. J., 1995, "Potential Fields for Nonholonomic Vehicles," IEEE International Symposium on Intelligent Control, pp. 461-465. [31] Laitone, E. V., 1977, "Relation of the Conjugate Harmonic Functions to F(Z)," The American Mathematical Monthly, 84(4), pp. 281-283. [32] Masoud, A. A., Masoud, S. A., and Bayoumi, M. M., 1994, "Robot Navigation Using a Pressure Generated Mechanical Stress Field: "The Biharmonic Potential Approach"," IEEE International Conference on Robotics and Automation, 1, pp. 124-129. [33] Milne-Thomson, L. M., 1996, Theoretical Hydrodynamics 5th ed., Dover Publications, New York NY. [34] Needham, T., 1997, Visual Complex Analysis, Oxford University Press, New York NY. [35] Newman, W. and Hogan, N., 1987, "High Speed Robot Control and Obstacle Avoidance Using Dynamic Potential Functions," IEEE International Conference on Robotics and Automation, 4, pp. 14-24. [36] Packard, A. and Wu, F., 1993, "Control of Linear Fractional Transformations," IEEE Conference on Decision and Control, pp. 1036-1041. 168 [37] Quinlan, S. and Khatib, O., 1993, "Elastic Bands: Connecting Path Planning and Control," IEEE International Conference on Robotics and Automation, 2, pp. 802-807. [38] Rainville, E. D. and Bedient, P. E., 1969, Elementary Differential Equations 4th ed., Macmillan, New York NY. [39] Rajamani, R., 2006, Vehicle Dynamics and Control, Springer Science, New York NY. [40] Rimon, E. and Koditschek, D. E., 1992, "Exact Robot Navigation Using Artificial Potential Functions," IEEE Transactions on Robotics and Automation, 8(5), pp. 501-518. [41] Rossetter, E. J., 2003, "A Potential Field Framework for Active Vehicle Lanekeeping Assistance," Ph.D. dissertation, Stanford University, Stanford, CA. [42] Sarang, S., 2007, "Complex Variables," , Chaitime Education, Feburary 23, 2008. [43] Sato, K., 1993, " Deadlock-Free Motion Planning Using the Laplace Potential Field " Advanced Robotics, 7(5), pp. 449-461. [44] Shimoda, S., Kuroda, Y., and Iagnemma, K., 2005, "Potential Field Navigation of High Speed Unmanned Ground Vehicles on Uneven Terrain," IEEE International Conference on Robotics and Automation, pp. 2828-2833. [45] Slotine, J. J. E. and Li, W., 1991, Applied Nonlinear Control, Prentice Hall, Englewood Cliffs NJ. [46] Smith, D. E. and Starkey, J. M., 1995, "Effects of Model Complexity on the Performance of Automated Vehicle Steering Controllers: Model Development, Validation and Comparison," Journal of Vehicle System Dynamics, 24(2), pp. 163-181. [47] Song, P. and Kumar, V., 2002, "A Potential Field Based Approach to Multi-Robot Manipulation," IEEE International Conference on Robotics and Automation, 2, pp. 1217-1222. [48] Stengel, R. F., 1994, Optimal Control and Estimation, Dover Publications, New York NY. [49] Sullivan, J., Waydo, S., and Campbell, M., 2003, "Using Stream Functions for Complex Behavior and Path Generation," AIAA Guidance, Navigation, and Control Conference and Exhibit. 169 [50] Tan, W., Packard, A. K., and Balas, G. J., 2000, "Quasi-Lpv Modeling and Lpv Control of a Generic Missile," American Control Conference, 5, pp. 3692-3696. [51] Van Wijk, J. J., 1993, "Implicit Stream Surfaces," IEEE Conference on Visualization, pp. 245-252. [52] Wang, Y. and Chirikjian, G. S., 2000, "A New Potential Field Method for Robot Path Planning," IEEE International Conference on Robotics and Automation, 2, pp. 977-982. [53] Waydo, S. and Murray, R. M., 2003, "Vehicle Motion Planning Using Stream Functions," IEEE International Conference on Robotics and Automation, 2, pp. 2484-2491. [54] Weisstein, E. W., 2008, "Curvature," , MathWorld--A Wolfram Web Resource, March 30, 2008. [55] Wenzel, T. A., Burnham, K. J., Williams, R. A., and Blundell, M. V., 2005, "Closed-Loop Driver/Vehicle Model for Automotive Control," International Conference on Systems Engineering, pp. 46-51.