AU685568B2 - Integrated vehicle positioning and navigation system, apparatus and method - Google Patents

Description

-1pM~x011 Regulation 32

AUSTRALIA

Patents Act 1990 COMPLETE SPECIFICATION FOR A STANDARD PATENT

ORIGINAL

B B B. Name of Applicant: Actual Inventors: Address for service in Australia: Invention Title: CATERPILLAR INC.

Adam J. GUDAT, Walter J. BRADBURY, Dana A. CHRISTENSEN, Richard G. CLOW, Lonnie J. DEVIER, Carl A. KEMNER, Karl W. KLEIMENHAGEN, Craig L. KOEHRSEN, Christos N. KYRTSOS, Norman K. LAY, Joel L. PETERSON, Prithvi N. RAO, Larry E. SCHMIDT, James W. SENNOTT, Gary K. SHAFFER, WenFan SHI, Dong Hun SHIN, Sanjiv J. SINGH, Darrell E. STAFFORD, Louis J. WEINBECK, Jay H. WEST, William L. WHITTAKER, BaoXin WU CARTER SMITH BEADLE 2 Railway Parade Camberwell Victoria 3124 Australia Integrated Vehicle Positioning and Navigation System, Apparatus and Method The following statement is a full description of this invention, including the best method of performing it known to us 111_ -2- INTEGRATED VEHICLE POSITIONING AND NAVIGATION SYSTEM, APPARATUS AND METHOD BACKGROUND OF THE INVENTION 1. Field of the Invention The present application is a divisional application of Australian Patent Application No. 46045/93 which was divided from Australian Patent No. 642638.

The present application relates to systems for navigating an autonomous vehicle.

Other inventions relating to apparatus/methods for determining terrestrial position information are discussed and claimed in other co-pending applications e.g.

Australian Patent Application No. 46043/93. For an overview of the interrelated navigation and positioning systems/apparatus/methods, the reader should refer to the specification accompanying Australian Patent No. 642638.

2. Related Art .eoo There is presently under development a terrestrial position determining 15 system, referred to as the global po...Juning system (GPS), designated NAVSTAR by the U.S. Government. In this system, a multitude of orbiting satellites will be used to determine the terrestrial position of receivers on the Earth. In the planned system, there will be eight orbiting satellites in each of three sets of orbits, 21 satellites on line and three spares, for a total of 24 satellites. The three sets of orbits will have mutually orthogonal planes relative to the Earth. The orbits are neither polar orbits nor equatorial orbits, however. The satellites will be in 12-hour orbits.

The position of each satellite at all times will be precisely known. The longitude, latitude, and altitude with respect to the center of the Earth, of a receiver at any point close to Earth at the time of transmission, will be calculated by determining the propagation time of transmissions from at least four of the satellites to the receiver. The more satellites used the better. A current constraint on the number of satellites is that the currently available receiver has only five channels.

Rudimentary autonomous, meaning unmanned or machine controlled, vehicle navigation is also known in the field.

Systems exist which rely on vision based positioning. For instance, vision DCCnI:17482.SPC 3 Match 199S -3based positioning is used in the Martin Marietta Autonomous Land Vehicle, as described in "Obstacle Avoidance Perception Processing for the Autonomous Land Vehicle," by R. Terry Dunlay, IEEE, CH2555-1/88/0000/0912$01.00, 1988. (See also "A Curvature-based Scheme for Improving Road Vehicle Guidance by computer Vision," by E. D. Dickmanns and A. Zapp, as reported at SPIE's Cambridge Symposium on Optical and Optoelectronic Engineering, October 26-31, 1986.) Some of these vision based positioning systems may use fixed lines or markings on, for instance, a factory floor, to navigate from point to point. Others may involve complex pattern recognition hardware and software. Other systems may navigate by keeping track of their position relative to a known starting point by measuring the distance they have travelled and the direction from the starting point.

These measuring type systems are known as "dead-reckoning" systems.

.Such navigation systems suffer from numerous drawbacks and limitations.

15 For instance, if the system cannot recognize where it is, looses track of where it has been, or miscalculates its starting point, it will become unable to accurately reach its goal. Because errors in position accumulate in such systems, they may require °frequent, time consuming initializations. Such systems may require patterns and markers to be placed along their route, which is also time consuming and costly, and limits their usefulness to small, controlled areas.

SUMMARY OF THE INVENTION The present invention is an integrated vehicle positioning and navigation system which, as used throughout, means apparatus, method or a combination of both apparatus and method. The present invention overcomes many of the limitations present in conventional technology in the fields of positioning and navigation, and thereby provides for highly accurate and autonomous positioning and navigation of a vehicle.

In accordance with a first aspect of the invention, there is disclosed a system for a vehicle for enabling said vehicle to track a preset path comprising: DCC:CJZ:#17482.RS1 13 May 1997 -4means for executing vehicle commands that cause said vehicle to attempt to follow said preset path; means for periodically calculating errors in following said preset path; means for adjusting said vehicle commands based on said calculated errors to reduce said calculated errors; wherein said means for adjusting includes means for constructing a smooth path back to the preset path using a quintic polynomial.

In accordance with a second aspect of the invention, there is disclosed a method for a vehicle for enabling a vehicle to track a preset path comprising the steps of: executing vehicle commands that cause said vehicle to attempt to follow said preset path; periodically calculating errors in following said preset path; and adjusting said vehicle commands based on said calculated errors to reduce said calculated errors; ~wherein said step of adjusting includes the step of constructing a smooth path back to said preset path using a quintic polynomial.

In a preferred embodiment of the invention, the system includes navigating a vehicle along a predetermined route comprising: means for storing route data representing said predetermined route, said route data including a plurality of contiguous path segments, each of said path segments connecting two nodes; means for storing path data for said predetermined route, said path data including postures of said vehicle along each of said path segments; 25 means for retrieving from said route data storing means route data representing said predetermined route; means for retrieving from said path data storing means path data for said retrieved route data; means for generating a path from said retrieved path data, said path representing at least a portion of said predetermined route; DCC;TG:#17482.RS2 22 October 1997 -0A4 1,.'rO C_ means for associating vehicle commands with said path; means for executing said associated vehicle commands to cause said vehicle to follow said path; means for periodically calculating errors in following said path; and means, responsive to calculated errors, for adjusting the execution of said associated vehicle commands to reduce said calculated errors so that said vehicle tracks said path with a high degree of accuracy.

One preferred method of the invention involves a method of navigating a vehicle along a predetermined route comprising the steps of: storing route data representing said predetermined route, said route data including a plurality of path segments, each of said path segments connecting two nodes; S. 5 S. S storing path data for said predetermnined route, said path data including postures of said vehicle along each of said path segments; retrieving route data representing said predetermined route; retrieving path data for said retrieved route data; generating a path from said retrieved path data, said path representing at least a portion of said predetermined route; associating vehicle commands with said generated path; executing associated vehicle commands to cause said vehicle to attempt to follow said generated path; periodically calculating errors in following said generated path; and adjusting in accordance with said calculated errors, execution of said associated vehicle commands to reduce said calculated errors so that said vehicle tracks said generated path with a high degree of accuracy.

The superior positioning data capability of the system disclosed herein is used to aid the autonomous vehicle navigation system of the present invention. The navigation system of the present invention, provides for highly accurate vehicle pathtracking on predetermined, or dynamically determined, vehicle paths.

The present invention includes novel systems for determining paths or roads CC:TG:#17482,RS2 22 October 1997 QK1AT 0t

VT~Z

-6for an autonomous vehicle to follow at a work site, both dynamically during operation and/or in advance. It includes systems for efficiently storing and accessing these paths. The present invention includes systems for accurately tracking these paths.

The present invention contemplates a totally autonomous work site, where numerous autonomous vehicles and equipment may operate with minimal human supervision. This reduces labor costs. This also makes the present invention suitable for hazardous or remote work sites, such as at the site of a nuclear accident, for example.

A preferred embodiment or "best mode" of the present invention, would be S.in an autonomous mining vehicle, such as is used for hauling ore, at a mining site.

.i However, it is noted that it is contemplated that the present invention may be implemented in other vehicles performing other tasks. Examples are, for instance, in other types of heavy equipment for mining or construction, or in other totally 15 different types of vehicles, such as farm tractors or automobiles.

A better appreciation of these and other advantages and features of the present invention, as well as how the present invention realizes them, will be gained from the following detailed description and drawings of the various embodiments, and "from the claims.

BRIEF DESCRIPTION OF THE DRAWINGS The present invention is better understood by reference to the following drawings in conjunction with the accompanying text.

S. Fig. 1 is a diagrammatic view of the GPS satellites in 6 orbital planes.

~Fig. 2 is a diagrammatic view illustrating the GPS navigation equations.

Fig. 3 is a diagrammatic view of a typical autonomous work site in which the present invention is practiced.

Fig. 4 is a diagrammatic representation of the interrelationships between the navigator, the vehicle positioning system (VPS), and the vehicle controls.

Fig. 5 is a context diagram, illustrating the various elements and their DCC;CJZ#17482.RS 13 May 1997 interrelationship in an autonomous control system according to the present invention.

Fig. 6 is a diagrammatic representation of the operation of a Global Positioning System (GPS), including a satellite constellation, A pseudolite, the base station and a vehicle.

Fig. 7 is a block diagram representing the GPS Processing System.

Fig. 8 is a flow diagram illustrating communications in one embodiment of the GPS Processing System of Fig. 7.

Fig. 9 is a block diagram illustrating the Motion Positioning System (MPS) including the Inertial Reference Unit and the Odometer.

Fig. 10 is a block diagram illustrating one embodiment of the VPS System Architecture.

Fig. 11 is a detailed block diagram of the embodiment of the VPS System Architecture of Fig. Fig. 12 is a block diagram of one embodiment of the VPS Main Processor of Fig. 10 showing the VPS Kalman Filter and the Weighted Combiner.

Fig. 13 is a flowchart of the Constellation Effects Method for improving position accuracy.

Fig. 14 is a polar plot illustrating the computed pseudo ranges from a four satellite constellation.

Fig. 15 is a flowchart of the original bias technique for Differential Corrections.

Fig. 16 is a flowchart of the Parabolic Bias Method for Differential Corrections.

Fig. 17 is a flowchart of the BASE residuals as BIAS Method for Differential Corrections.

Fig. 18 is a flowchart of the method for Satellite Position Prediction.

Fig. 19 is a flowchart of the Weighted Path History Method.

Fig. 20 is a diagrammatic representation of the Weighted Path History Method.

Fig. 21 is a flowchart of the Anti-Spoofing Method.

DO JMM:#17482.SPC 3 Mal 1995 Fig. 22 is a diagram of route definitions using nodes and segments according to the present invention.

Fig. 23 is a diagrammatical representation of how postures and associated circles are obtained from objective points.

Fig. 24 is a diagrammatical representation of how the sign of the first clothoid segment is determined.

Fig. 25 is a diagrammatical representation of how the sign of the last clothoid segment is determined.

Fig. 26 is a graphical illustration of a clothoid curve.

Fig. 27 is a flowchart of a numerical method for calculating approximate Fresnel integrals.

Fig. 28 is a diagram showing the replanning of a path.

Fig. 29 is a graph of B-spline curves of second, third and fourth order.

Fig. 30 is a diagram of an embodiment of the posture ring buffer of the 15 present invention.

Fig. 31 is a diagram of a path tracking control structure of an embodiment of the present invention.

Fig. 32 is a diagram showing relevant postures in steering planning cycle.

Fig. 33 is a diagram showing how an error vector including curvature is computed.

Fig. 34 is a diagram showing how an error vector including curvature is computed with the vehicle path included.

Fig. 35 is a context diagram of an embodiment of the navigator of the present invention.

Fig. 36 is a context diagram of a path tracking structure of the present invention.

Figs. are navigator data flow summaries.

Fig. 38A is an illustration of a vehicle mounted scanner.

Fig. 38B is an illustration of an autonomous vehicle scanning for obstacles.

Fig. 39 is a diagram of selected scan lines in an embodiment of a laser DCC."M:#17482sPC 3 Mah 1995 9 scanner system of the present invention.

Fig. 40 is a diagram of an autonomous vehicle avoiding obstacles.

Fig. 41 is a diagram of obstacle handling according to an embodiment of the present invention.

Fig. 42 is a schematic of a laser scanner system used for obstacle detection in an embodiment of the present invention.

Fig. 43 is a block diagram of an autonomous mining truck vehicle controls system of the present invention.

Fig. 44 is a state diagram showing the transitions between modes of operation.

Fig. 45 is a diagram of an embodiment of a tele-line of sight remote control system of the present invention.

Fig. 46 is a representational diagram of an embodiment of the speed control system of the present invention.

Fig. 47 is a diagram of an embodiment of the service brakes control circuit of the speed control system of the present invention.

Fig. 48 is a diagram of an embodiment of the governor control circuit of the speed control system of the present invention.

Fig. 49 is a diagram of an embodiment of a steering control circuit of the steering control system of the present invention.

Fig. 50 is a diagram of an embodiment of a park brake control circuit in the speed control system of the present invention.

Fig. 51 is a diagram of a tricycle steering model used to develop navigation system of the present invention.

Fig. 52 is a diagram showing an embodiment of a shutdown circuit of the present invention.

Fig. 53 is a diagram showing navigator tasks.

Fig. 54 is a diagram showing an embodiment of navigator shared memory.

Fig. 55 is an executive flowchart.

Fig. 56 is an illustration of how Figs. relate.

Figs. together form a flowchart of the executive decisions block DCCTJII:#1748s2.SPC 3 Mach 1995 (0 13in detail.

Figs. are flowcharts each showing an "act on" block of the executive decisions flowchart in detail.

Fig. 58 is an illustration of how Figs. relate.

Figs. are parts of a flowchart showing the "act on state" block of the executive flowchart in more detail.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENT TABLE OF CONTENTS I. General Overview II. Navigation A. Overview B. Route Planning/Path Generation 1. Introduction a. Clothoid Path Segments b. Modeling A Vehicle Path c. Clothoid Curves d. Generation of a Posture Continuous Path Existing Methods Path generation from a sequence of points Clothoid Replanning Paths Summary B-splines 2. Route Creation and Storage a. Introduction b. Route Definition c. Navigator Route Usage 3. Posture Generation C. Path Tracking 1. Introduction DCc:TJI:#17482.SPC 3 March 1995 -4- 2. Considerations a. Global position feed back b. Separate steering and driving control 3. Embodiments a. Tracking Control Structure b. Quintic Method c. Latency and slow systems d. Vehicle-Ground Interaction (VGI) e. Sensing and Actuation Timing f. Look-ahead g. Optimal Control Method h. Conclusion D. Obstacle Handling 1. Introduction 15 2. Detection of Obstacles a. Clearance checking b. Filtering and edge detection c. Obstacle extraction Finding the road Modeling road height Thresholding Blob extraction Applications 3. Avoidance of Obstacles 4. Return to Path Scanner System a. Introduction b. Laser scanner c. Scanner system interface d. Scanner system buffer circuit DCCT.'Iw17482SPC 3 March 1995 15 1 E. Vehicle Controlling Systems 1. Introduction 2. Vehicle Manager (modes) a. Ready mode b. Tele mode c. Manual mode d. Autonomous mode 3. Speed Control 4. Steering Control a. Steering Model b. Path Representation c. Posture Definition d. Position Information e. VPS Short Definition f. Steering Method 5. Monitor/Auxiliary 6. Safety System a. Introduction b. Shutdown Control 7. Bus Architecture F. Functional Descriptions/Methods 1. The NAVIGATOR a. MAIN b. MONITOR_VEH STATUS c. SCANNER d. CONSOLE and CONSOLE_PARSER e. GETDIRECTIVES f. MSG TOHOST g. VPSPOSITION h. VPS POSTURE DCCTI1:117482.SPC 3 Mmch 199S 13 6i. TRACKER j. NAVIGATOR Shared A iobal) Memory k. Flow Charts I. General Overview In the following description of the present invention, and throughout the specification, the term "system" is used for shorthand purposes to mean apparatus, method, or a combination of both apparatus and method.

Autonomous is used herein in its conventional sense. It indicates operation which is either completely automatic or substantially automatic, that is, without significant human involvement in the operation. An autonomous vehicle will generally be unmanned, that is without a human pilot or co-pilot. However, an autonomous vehicle may be driven or otherwise operated automatically, and have one or more human passengers.

The task of guiding an autonomous vehicle along a prescribed path requires that one accurately knows the current position of the vehicle. Once the current position is known, one can command the vehicle to proceed to its next destination.

For a truly autonomous vehicle to be practical, very accurate position information is required. In the past, it was not believed possible to obtain this nearly absolute position information without using a prohibitively large number of reference points. All positioning was done relative to the last reference point using inertial navigation or dead reckoning.

Using the systems discussed in Australian Patent No. 642638, we are able to obtain the nearly absolute position information required for a truly autonomously navigated vehicle. This led us to the development of simpler, more reliable vehicle controllers and related systems as well.

The development and implementation of the GPS (global positioning system) was a necessary and vital link which allowed us to develop our inventive systems to obtain more accurate position information. The GPS satellite position information, significantly enhanced through our inventive techniques, is combined and filtered with IRU (Inertial Reference Unit) inertial navigation information, and 3 M=sh 199$

'I-

-4-7vehicle odometer information, resulting in a highly accurate system for determining position and effecting navigation. See Australian Patent No. 642638 for a discussion of positioning systems.

Integration of both positioning and navigation systems, apparatus, methods and techniques which, provide for the highly accurate transportation of unmanned vehicles.

The navigation system using the highly accurate position information obtained from the positioning system, is able to provide for accurate navigation between points along pre-established or dynamically generated paths.

Use is made of various models or conceptual representations to generate and follows these paths. For example, lines and arcs may be used to establish paths "'"between objective points the vehicle is to obtain. B-splines or clothoid curves may S *be used to model the actual path the vehicle is to navigate. Using modelling or representational techniques provides for enhanced data communication, storage and 15 handling efficiencies. It allows for simplification of supervisory tasks by providing a hierarchy of control and communication such that the higher the level of control, the simpler the task, and the more compact the commands.

At the base station, where GPS positioning data is also received from the satellites, a host processing system provides for the highest level of control of the navigation system. The host handles scheduling and dispatching of vehicles with much the same results as a human dispatcher would achieve. The host thereby determines the work cycle of each vehicle.

The host commands each of the vehicles to proceed from a current position to another position taking a specified route in order that they effect their work goals.

The host specifies the routes by name, and not by listing each point along the route.

Each vehicle has an on-board system which looks up the named route and translates the named route into a set of nodes and segments along the route, using, for example, the models or representations discussed above to connect the nodes.

The on-board system also provides for controlling the vehicle systems, such as brakes, steering, and engine and transmission, to effect the necessary physical acts 3Mach 1995 required to move, stop, and steer the vehicle. These may be designed to be retrofitted onto existing vehicles, such as the Caterpillar, Inc. 785 off-highway truck, for instance.

The on-board system also checks actual position against desired position and corrects vehicle control to stay on route. The system may run multi-state models to enhance this checking capability. The system also checks for errors or failures of the system and vehicle components. If detected, the system provides for fail-safe shutdown, bringing the vehicle to a stop.

The on-board navigation system provides for different modes of control.

These include a fully autonomous mode, where navigation of the vehicle is automatically handled by the on-board system; a tele or remote control mode, where a remote human operator may control the direction and motion, and so on, of 44 4 the vehicle; and a manual mode, where a human operator sitting in the cab can .ooo take control of the vehicle and drive it manually.

15 In the autonomous mode, obstacle detection is critical, and is provided for in the navigation system of the present invention. Boulders, animals, people, trees, or other obstructions may enter the path of a vehicle unexpectedly. The on-board system is capable of detecting these, and either stopping, or plotting a path around :""the obstruction, to return to its original route when it is safe.

Accurately tracking the desired route is another function of the on-board navigational system. System function and architecture has been designed for real time tracking of the path at speeds up to approxiniately 30 mph. safely.

II. Navigation A. Overview In considering implementation of an autonomous navigation system, there are some basic questions which any autonomous system must be able to answer in order to successfully navigate from point A to point B. The first question is "where are we (the vehicle) now?". This first question is answered by the positioning system portion of the present invention, as fully discussed in Australian Patent No. 642638 and other co-pending applications.

Y\ DCEIH:11-7482SPC 3 March 1995 The next or second question is "where do we go and how do we get there?".

This second question falls within the domain of the navigation system portion of the present invention, discussed in this section (II).

A further (third) question, really a refinement of the second one, is "how do we actually physically move the vehicle, for example, what actuators are involved (steering, speed, braking, and so on), to get there?" This is in the domain of the vehicle controls subsystem of the navigation system, also discussed below.

In the preceding and following discussions of the present invention, recall that, "system(s)" may include apparatus and/or methods.

As has been discussed implicitly above, autonomous navigation, of a mining vehicle as an example, may provide certain significant advantages over conventional navigation. Among them is an increased productivity from round the clock, 24 hr.

operation of the vehicles. The problems presented by dangerous work environments, or work environments where visibility is low, are particularly well suited to solution 15 by an autonomous system.

There are, for instance, some mining sites where visibility is so poor that work is not possible 200 days of the year. There are other areas which may be hazardous to human life because of being contaminated by industrial or nuclear pollution. An area may be so remote or desolate that requiring humans to work there may pose severe hardships or be impractical. The application of the present invention could foreseeably include extraterrestrial operations, for example, mining on the Moon, provided that the necessary satellites were put in Moon orbit.

In a typical application of the present invention, as shown in Fig. 3, with regard to the navigation of a mining vehicle at a mining site, there are three basic work areas: the load site, the haul segment, and the dump site. At the load site, a hauling vehicle may be loaded with ore in any number of ways, by human operated shovels for instance, controlled either directly or by remote control, or by autonomous shovels. The hauling vehicle then must traverse an area called the haul segment which may be only a few hundred meters or may be several km's. At the end of the haul segment is the dump site, where the ore is dumped out of the hauling 3 Mamb 1995 em .,ee.

me em

S.

me .me.

C

vehicle to be crushed, or otherwise refined, for instance. In the present invention, autonomous positioning and navigation may be used to control the hauling vehicle along the haul segment. Autonomously navigated refueling and maintenance vehicles are also envisioned.

Referring now to Figs. 4 and 5, Navigation of the AMT (Autonomous Mining Truck) encompasses several systems, apparatus and/or functions. The VPS (Vehicle Positioning System) 1000 subsystem of the overall AMT system as described above, outputs position data that indicates where the vehicle is located, including, for example, a North and an East position.

Referring now to Figs. 4 and 5, position data output from the VPS is received by a navigator 406. The navigator determines where the vehicle wants to go (from route data) and how to get there, and in turn outputs data composed of steer and speed commands to a vehicle controls functional block 408 to move the vehicle.

The vehicle controls block then outputs low level commands to the various vehicle systems, 310, such as the governor, brakes and transmission. As the vehicle is moving towards its destination, the vehicle controls block and the VPS receive feed-back information from the vehicle indicative of, for example, any fault conditions in the vehicle's systems, current speed, and so on.

Navigation also must include an obstacle handling (detection and avoidance) 20 capability to deal with the unexpected. A scanning system 409 detects obstacles in the vehicle's projected trajectory, as well as obstacles which may be approaching from the sides and informs the navigator of these.

The navigator may be required to then decide if action is required to avoid the obstacle. If action is required, the navigator decides how to avoid the obstacle.

And after avoiding the obstacle, the navigator decides how to get the vehicle back onto a path towards its destination.

Referring now to Fig. 35, titled the context diagram, and Fig. 37A-37D, definitions of the communications, which are shown as circles with numbers in them, are provided below: 502. Host commands queries: C em mm e m C D IN:#17482.SPC 3 Mamh 1995

S.

S

S

Commands given by the host to the vehicle manager. These commands could be of several types: initiate/terminate; supply parameters; emergency actions; and directives.

Queries inquire about the status of various parts of the navigator.

504. replies to host: These are responses to the queries made by the host.

432. position data: This is streamed information provided by the VPS system.

416. Range data: This is range data from the line laser scanner.

432. VPS control: These are commands given to the VPS system to bring it up, shut it down and switch between modes.

416. scanner control: These are commands sent to the laser scanner to initiate motion and set follow velocity profile.

20 420. steering speed commands These are commands given to the vehicle to control steering and speed. These commands are issued at the rate of 2-5 Hz.

Referring to Fig. 5, in a preferred embodiment of the present invention, as described above, both the VPS and the navigator are located on the vehicle and communicate with the base station 314 to receive high level GPS position information and directives from a host processing system, discussed below. The system gathers GPS position information from the satellites 200-206 at the base station and on-board the vehicle so that common-mode error can be removed and positioning accuracy enhanced.

In an alternate embodiment of the present invention, portions of the VPS and 04

S

*.SS

S S S *5 5

*S

S

S S DC'nT :1 17482SPC3 3Mach 1995 navigator may be located at the base station.

The host at the base station may tell the navigator to go from point A to point B, for instance, and may indicate one of a set of fixed routes to use. The host also handles other typical dispatching and scheduling activities, such as coordinating vehicles and equipment to maximize efficiency, avoid collisions, schedule maintenance, detect error conditions, and the like. The host also has an operations interface for a human manager.

It was found to be desirable to locate the host at the base station and the navigator on the vehicle to avoid a communications bottleneck, and a resultant degradation in performance and responsiveness. Since the host sends relatively high-level commands and simplified data to the navigator, it requires relatively little communication bandwidth. However, in situations where broad-band communication is available to the present invention, this may not be a factor.

SAnother factor in determining the particular location of elements of the system 15 of the present invention, is the time-criticality of autonomous navigation. The "i navigation system must continually check its absolute and relative locations to avoid unacceptable inaccuracies in following a route. The required frequency of checking ::location increases with the speed of the vehicle, and communication speed may become a limiting factor even at a relatively moderate vehicle speed.

20 However, in applications where maximum vehicle speed is not a primary consideration and/or a high degree of route following accuracy is not critical, this communication factor may not be important. For example, in rapidly crossing large expanses of open, flat land, in a relatively straight path, it may not be necessary to check position as often in the journey as it would be in navigating a journey along a curvaceous mountain road.

Conceptually, the navigation aspects of the present invention can be arbitrarily divided into the following major functions: route planning/path generation; path tracking; and obstacle handling.

DI V1lI:#17482SPC 3 Mam 1995 The function of the present invention are discussed below.

B. Route Planning/Path Generation 1. Introduction Autonomous vehicle navigation in accordance with the present invention, conceptually consists of two sub problems, path generation and path tracking, which are solved separately.

Path generation uses intermediate goals from a high level planner to generate a detailed path for the vehicle 310 to follow. There is a distinct trade-off between simplicity of representation of such plans and the ease with which they can be executed. For example, a simple scheme is to decompose a path into straight lines and circular curves. However, such paths cannot be tracked precisely simply because of discontinuities in curvature at transition points of segments that require instantaneous accelerations.

Following path generation, path tracking takes, as input, the detailed path 15 generated and controls the vehicle 310 to follow the path as precisely as possible.

It is not enough to simply follow a pre-made list of steering commands because failure to achieve the required steering motions exactly, results in steady state offset o errors. The errors accumulate in the long run. Global position feedback 432 may be used to compensate for less than ideal actuators. Methods have been developed for the present invention which deviate from traditional vehicle control schemes in which a time history of position (a trajectory) is implicit in the plan specified to the vehicle 310.

These methods are appropriately labeled "path" tracking in that the steering motion is time decoupled; that is, steering motions are directly related to the geometric nature of the specified path, making speed of the vehicle 310 an independent parameter.

Referring now to Fig. 3, an autonomous vehicle 310 may be required to leave a load site 318, traverse a haul segment 320 to a dump site 322, and after dumping its load, traverse another haul segment to a service shop 324, under the direction of the host 402. The host 402 determines the vehicles' destinations, which is called 3 Malh 1995 "cycle planning." Once the work goals have been determined by "cycle planning", determination of which routes to take to get to a desired destination must be accomplished by "route planning." "Route planning" is the determination of which path segments to take to get to a desired destination. In general, a route can be thought of as a high-level abstraction or representation of a set of points between two defined locations. Just as one can say to a human driver "take route 95 south from Lobster, Maine to Miami, Florida," and the driver will translate the instruction into a series of operations (which may include starting the vehicle 310, releasing the brake 4406, engaging the transmission 4610, accelerating to the posted speed limit, turning the steering wheel 4910, avoiding obstacles 4002, and so on), the autonomous navigation system of the present invention performs similarly. As used in the system of the present invention, a "route" is a sequence of contiguous "segments" between the start and end of a trip.

An autonomous vehicle 310 may begin at any position in the sequence and traverse the route in either direction. A "segment" is the "path" between "nodes." A "node" is a "posture" on a path which requires a decision. Examples of nodes are load sites 3318, dump sites 322,, and intersections 326.

There are various types of segments. For instance, there are linear and 20 circular segments. Linear segments (lines) are defined by two rnodes. Circular segments (arcs) are defined by three nodes.

"Postures" are used to model parts of a route, paths and nodes for instance.

Postures may consist of position, heading, curvature, maximum velocity, and other information for a given point on the path.

A "path" is a sequence of contiguous postures.

A segment is, therefore, a sequence of contiguous postures between nodes.

All segments have a speed associated with them, which specifies the maxinnim speed with which the vehicle 310 is to traverse that segment. The navigator 406 can command slower speeds, if necessary, to meet other requirements.

Determining which postures are required to define a path segment by 3 hbach 1995 e, -125analytical, experimental or a combination of both, is called "path planning" in accordance with the present invention. To bring the discussion full circle, a sequence of contiguous routes, as mentioned above, is referred to as a "cycle," and a vehicle's 310 work goals determine its "cycle." Therefore, to define a route one must first define the nodes and segments.

Next, the nodes and segments must be ordered. Finally the routes must be defined by specifying where in the ordered set a route is to begin, and in which direction the ordered set is to be traversed (See Fig. 22 which illustrates these concepts of the present invention).

The aforementioned method of defining routes was developed for memory efficiency in the present invention. It is also a convenient way to define many routes on a specific set of nodes and segments.

In a real world example of the present invention, picture a site where there S.are many intersecting roads 326. A route programmer would define nodes at the .ooo 15 intersections, and segments to define the roads between the intersections. Routes would therefore be determined by the roads and intersections. There will however, be many ways to get from point A to point B (many routes) with a fixed set of intersections and roads.

The path-tracking method of the present invention (discussed below) uses 20 route curvature to steer the vehicle. Methods of route definition using lines and arcs do not provide for continuous curvature. Clothoid curves are another way to define routes.

Another method of defining routes developed by the inventors, fits B-splines to the driven data. B-splines provide continuous curvature and therefore enhance tracking performances. In addition, since B-splines are free form curves, a route may be defined by a single B-spline curve. By using free form curves, a more robust method (semi-automatic) for fitting routes to data collected by driving the vehicle over the routes is produced by the present invention.

Referring to Figs. 4 and 22, in operation, the host 402 from the base station 314 commands an identified vehicle 310 to take route N from its present location.

3 Mach 1995 The navigator 406 functions to generate a path by translating "route 1" into a series of segments, each of which may have a "posted" or associated maximum speed limit, which together form a generated path for the vehicle to attempt to follow. By specifying routes and commanding the autonomous vehicle 310 with high-level commands this way, enormous data requirements and inefficiencies are in the present invention avoided in giving directions.

The navigator 406 stores the routes as a linked-list of path segments, rather than the set or series of sets of individual points. These segments are also abstractions of the set of points between defined locations or nodes.

A LINKER then takes given path segments and generates a linked-list of control points, allowing for flexibility and efficiency. Path segments are shared by different routes, as is shown in Fig. 22.

The path segments are stored in a memory called the TARGA 5302 as a set of arcs, lines, and postures. For instance, in one embodiment of the present invention, an analytical generator function generates paths using these arcs, lines and postures. In another embodiment of the present invention, B-splines are used as a mathematical representation of a route, as mentioned above.

In another embodiment or the present invention, "clothoid" curves are used in generating path segments. These are discussed below.

S: 20 a. CLOTHOID PATH SEGMENTS As discussed above, part of the navigation problem addressed and solved by the present invention is really two sub-problems: path planning and path generation.

These are solved separately by the present invention.

Path planning proceeds from a set of sub-goals using some path optimization function and generates an ordered sequence of "objective" points that the vehicle 310 must attain.

The challenge of path generation is to produce from the objective points (of path planning), a continuous, collision-free path 3312, smooth enough to be followed easily by the autonomous vehicle 310. For example, a simple scheme is to decompose a path 3312 into straight lines and circular curves. The path 3312 is Dc:11:#17482SP3C 3 Maia 1995

O

D

o .2 then converted into a sequence of explicit directives provided to the vehicle 310 actuators to keep the vehicle on the desired path 3312. It should be noted that there is a distinct trade-off between simplicity of representation of such plans and the ease with which they can be executed.

The ability of an autonomous vehicle 310 to track a specified path 3312 is dependant on the characteristics of the path. Continuity of curvature and the rate of change of curvature (sharpness) of the generated path 3312 are of particular importance since these parameters dictate steering motions required of a vehicle 310 for it to stay on the desired path 3312. Discontinuities in curvature are impossible to follow since they require an infinite acceleration. For some autonomous vehicle configurations, the extent to which the sharpness of a path is linear is the extent to which steering motions are likely to keep the vehicle on the desired path 3312, since linear sharpness of a path equates to approximately constant velocity of steering.

One method used by the present invention, is to compose paths as a ,.quence of straight lines and circular arcs. This method suffers from discontinuities in curvature where arcs meet. Another method of the present invention, is to use polynomial splines to fit paths between objective points. Splines assure continuity in curvature, but do not make any guarantees of line.~"ty in sharpness.

Inability to track the requisite curvature results in steady state offset errors from the desired path 3312. These errors can be compensated for by closing a feedback loop on position 3314. This is sufficient in those scenarios where the response of the actuators is fast enough to guarantee negligible tracking errors and position sensing is accurate, such as on a factory floor. However, path tracking is simpler if the path is intrinsically easier to track.

The method of the present invention generates explicit paths that pass through a sequence of objective points. A derivative method of the present invention replans paris of the path dynamically in case the tracking error becomes large or the desired path is changed.

b. Modeling A Vehicle Path Any path can be parameterized as a function of path length by position '&l/1 D1"QMThl#174.SPC 3 Marc h 199 coordinates 3304. That is, position coordinates x and y can be written as explicit functions of the path length s. Heading 3318 and curvature 3316 can be derived: 0(s) d s) (EQ.1) dx(s) c(s) dO(s) (EQ.2) ds The quadruple of these parameters, p is a posture 3314 that describes the state of an autonomous vehicle 310 at any point in time.

c. Clothoid Curves Clothoid curves are used in an embodiment of the present invention. They are a family of curves that are posture-continuous, and are distinct in that their curvature varies linearly with the length of the curve: c(s) ks+C i (EQ.3) 15 where k is the rate of change of curvature (sharpness) of the curve and subscript i denotes the initial state. A clothoid curve segment 2002 is shown in Fig. 26.

Given an initial posture, sharpness of the clothoid segment and the distance along that segment, position, orientation and curvature at any point are calclated as tllows: s c(s)d S (EQ. 4) s s x(s) i cos( )d 2 S0 Cos {2(E cE.

Qi}d x i (EQ. DCC:Tl:# 17482.SPC 3 March 1995 s s y(s) Yj sin%(-)d 2 o s ;in {(2 2 c }d yi (EQ. 6) d. Generation of a Posture-Continuous Path Practical navigation problems require composite paths whose range and complexity cannot be satisfied by a single clothoid segment. Most paths require multiple segments that pass through a sequence of objective points.

Existing Methods Hongo et al., "An Automatic Guidance System of a Self-Controlled Vehicle The Command System and Control Algorithm", Proceedings IECON. 1985, MIT Press, 1985. proposed a method to generate continuous paths composed of connected 'straight lines and circular arcs from a sequence of objective points. While paths comprised solely of arcs and straight lines are easy to compute, such a scheme 15 leaves discontinuities at the transitions of the segments as discussed above.

Kanayama et al., "Trajectory Generation for Mobile Robots", Robotics Research: The Third International Symposium. ISIR, Gouvieux, France, 1986 makes use of paired clothoid curves with straight line transitions between postures. The constraint of straight line transitions is due to the integrals in Eqs. and which do not have closed form solutions. Kanayama simplified this problem by requiring ci=O. Also, by rotating the reference frame by the amount of the initial orientation, 0 i=O; only a straight forward approximation of o sin(k 2)d is left.

Kanayama's method leads to paths that are sharper at some points and less compact than necessary, with adverse consequences to control. In addition, the requirement for straight-line transitions precludes the local replanning of paths because there are no guarantees that a segment to be replanned will include an R A DCCnmi:1742sSPC 3 Mawh 1995 It-, I S S uncurved section.

2. Path generation from a sequence of points A two-step method of the present invention, to generate a unique posturecontinuous path from a sequence of points is now described.

Referring now to Figs. 23, 24 and 25), the first step is to derive a sequence of unique postures 2302, 2304, 2306, 2308, 2310 from the objective points. The second step is to interpolate between those postures with clothoid segments.

Heading and curvature at the starting and ending positions 2402, 2404 are presumed.

Let Pi, Pf be the starting and ending postures 2402, 2404, respectively.

It is not always possible to connect two postures with one clothoid curve segment because four equations EQ.2, EQ.4, EQ.5, and EQ.6 cannot be satisfied simultaneously with only two parameters (sharpness k and length s) of a clothoid curve.

In order to satisfy the four equations EQ.2, EQ.4, EQ.5, and EQ.6, one needs 15 at least two clothoid curve segments. However, the general problem cannot be solved with two clothoid segments because if ki and kf have the same sign, in most cases a third segment is required in between. One adequate set of the clothoids connecting a pair of neighboring associated postures is the set of three clothoid segments (-k,s 2 (k,S 3 The subscripts denote the order of the clothoid segments from Pi. This combination is plausible for the following reasons: 1. The signs of k for the first and the last clothoid segments are the same.

2. k for the second clothoid segment is equal in magnitude and opposite in sign to that of the first and last segments. This enables the curve of three clothoid segments to satisfy the curvature variation between the starting and the ending curvatures by varying sl, s 2 s 3 even though the sign of the first and the last clothoid segments satisfies the curve location requirement.

3. There are four variables in the combination: k, sl, s 2 s3. It is possible to find a unique solution satisfying the following four equations which describe the mathematical relationship between the IX. TI11:i7482SPC 3 Maich 1995

RA

a 7 O Luo starting and the ending postures.

C f C k(s 1 -s 2

+S

3 e.=I'e Cs +S-2.+S3 +k(s 1s2-s 2s3+S3) f~ +c( 1 2 3 1 (EQ. 7) (EQ. 8) X f-=x f cosO 1 )d+ 0 f cosE® 2 )dg 0 10 cos 3 )d (EQ. 9) 0 0 b 0 0 0 Yf Yj f sine 1 IsinEO 2 )d 0 0 f sinO 3 )dt- 0 (EQ. 3 March 199S -32where O2 cis 2+ (c ksi) k 2 2 9 f o 25 *e f k 2 2 ©3 °i ci(sl+s 2 k(s 1 s 2 (s1s22

(C

i k(s 1 -s 2 k2 Referring now to the method shown in Fig. 27. Since equations 9 and above contain Fresnel integrals, for which there is no closed form solution, the values of k,sl,s2,and s3 are computed.

Paths resulting from the method have the following advantages over other methods: o The method proceeds from an arbitrary sequence of points. Generation of postures is essential to exploratory planning where goals are commonly posed as an evolving string of points. Paths generated by the method pass through all the objective points whereas paths from Kanayama's method and the arc method are only proximate to many of the points because these methods start from a sequence of postures.

o The method guarantees continuity of position, heading and curvature along the path. Further, sharpness is DOCTflI:f17492.SPC

.A)

3 Mach 1995 -33piecewise constant.

o Paths generated by the method always sweep outside the acute angles formed by straight line connection of the way points. The resulting paths are especially useful for interpolating around obstacles that are commonly on the inside of angles. In contrast, Kanayama's paths are always inside the angles.

3. Clothoid Replanning Paths Clothoid replanning is done either to acquire the path initially, or to guide the vehicle 310 back to the desired path 3312 through normal navigation according to the present invention.

To avoid abrupt accelerations in an attempt to make gross corrections in tracking a pre-specified path, a path replanner is used by the present invention to generate a new path which converges smoothly to the desired path 3312 from the S 15 current position. Replanning decomposes to two sub problems: S. 1. Determining the point of convergence to the intended path 3308.

2. Planning a path from the current position 3302 to the convergent point 3308.

Reference is made to Fig. 28, which graphically illustrates replanning a path 20 in accordance with the present invention. A pre-specified path consists of interpolations 2804 between postures (k,s)m 2804-2810 and the postures Pm (located at the end of segment Assuming that the vehicle 310 deviates from the path between Pm and Pm+1, then Pm+ 2 is chosen as the posture 334 to which the replanned path 2816 converges. The distance to Pm+2 is variable.

A curve composed of two curve segments is fitted to the postures (the current posture and the one chosen as a convergence posture) to obtain a replanned path 2816, satisfying four governing posture equations EQ.7, EQ.8, EQ.9, EQ.10. If we assume that the threshold that determines whether a path is to be replanned or not is much smaller than the length of each clothoid curve segment we can find a new posture-continuous path k+1, sk+ 1 (K k+ 2 s k+ 2 using a small 3 Mah 1995 31 34perturbation from known sk+l), (kk+ 2 sk+ 2 Since the replanned path 2816 is not likely to be very far from the original path 3312, two clothoid segments can be used.

4. Summary In accordance with the present invention, generation of continuous paths for autonomous vehicles 310 can use clothoid segments to generate paths not only because the resulting path is posture continuous but also because linear curvature along the curve leads to steering angles that vary approximately linearly along the path, facilitating path tracking.

The approach of the present invention is as follows: first, a sequence of the postures is obtained using the objective points. Then, each of the adjacent postures is connected with three clothoid curve segments.

The present method accrues additional advantages in that preprocessing of the

S

B objective points is not necessary as with arcs and zero curvature clothoids. Further, the geometry of the paths generated always sweeps outside the acute angles formed by straight line connection of the way points. These are especially useful for interpolating around obstacles that are commonly on the inside of angles.

From the set of stored arcs, lines and postures, clothoid curves, B-splines, and so on, points along a path are generated with the VPS posture block.

20 Advantages of the present invention's handling routes in this way, besides reducing the bandwidth requirements between the host and the vehicle, effects data compression reducing data storage requirements, and functions to smooth-out paths.

B-Splines B-splines are wei! known by mathematicians and those familiar with computer graphics (see "Mathematical Elements for Computer Graphics," by David F. Rogers and J. Alan Adams, McGraw-Hill Book Company, New York, N.Y., pages 144 to 155) as a means of describing the shape of a series of points by a specifying the coefficients of a polynomial equation. This curve fit function is an Nth order polynomial, where N is user specified and depends on the desired shape of the curve. The B-spline curve can be of any order and are continuous to the IQ 1 11:07482-SPC 3 Much 1995 -Uo7 0T Ft4 PB a V^

V"

D C order of the curve fit function minus one.

B-splines are used in an embodiment of the present invention. B-splines lend themselves well to path generation in the present invention because an arbitrarily long path can be described by a low number of coefficients, thus reducing the amount of data storage. Provided that the order of the curve fit function is high enough (three or larger), then the generated path will be smooth in curvature, resulting in a path which is inherently easy to track with the aforementioned embodiments of the preseat invention.

Figs. 29 shows an example of B-spline curves.

2. Route Creation and Storage a. Introduction: In one embodiment of the present invention, in order to create routes for a site 300, data is first collected from the VPS system 1000 and stored while a human drives the vehicle 310 over the road system of the work site 300. Nodes and segments are then fitted to the stored driven data, and organized into routes per the aforementioned procedure.

An application on an APOLLO Computer (now HEWLETT PACIARD CO.

of Palo Alto, California) work station (a graphics display system, not shown) was developed to graphically fit route data to the stored driven data and to further define routes (that is, speeds, sequences, starting point, traversal direction). Any graphics work stations equivalent to the APOLLO could be used.

Once the routes for a site are defined, the route data is written to a permanent storage device. In one embodiment of the present invention, the storage device used is a bubble memory cartridge 5302 with an associated reader/writer. The bubble memory device 5302 is durable and retains the data when power is disconnected.

The APOLLO application is capable of writing data to a cartridge 5302 and reading data from a cartridge 5302.

As implied above, routes in the present invention may be predefined, or they may be generated dynamically.

In mining applications, generally a site 300 is surveyed and roads are pre- Rq C 3 MaM 1995 V (3 ST 7) planned, carefully laid out and built. The routes used by the navigation system may then either be obtained from a manually created computer data base (created specifically to be used by the navigation system), or alternately, a vehicle may be physically driven over the actual routes on site to learn the routes as described above. In the learning method, several trips over a given route may be made. Then the variations in the data (due for instance to driver weaving) are averaged, and a smoothed-out best fit developed.

b. ROUTE DEFINITION: In one embodiment of the present invention, the following method is used for route definition.

1. Define the nodes and segments upon which the routes will be built. Place the oo 0 node and segment data into an array called the "routeData" array. Each iii ~record in the array contains the following information: 1. Type of item (that is, node, linear segment, S 15 circular segment, end of route marker).

2. If node item, define the north and east S.coordinates of the node; else if linear segment item, define the speed along the segment; 20 else if circular segment item, define the north and east coordinates of the center, the radius, the direction the circle is traversed (that is, clockwise, or counterclockwise), and the speed along the segment; else if end of route marker, there is no other information.

2. Link the node and segment data together into sequences. The sequences are simply an array of indexes into the routeData array. Each sequence must begin with an end of route marker, followed by a node, then the remainder 3 Match 1995 a

C

of the sequence alternate between segments and nodes until the sequence is terminated by another end of route marker. An example sequence would be, 1, 6, 3, 4, 7, 9, 10, 23, 78, 1 where the integers are indexes into the routeData array.

3. Finally define a route by specifying an index into the sequence array and whether to index through the sequence in the positive or negative direction.

Place the index and index direction into an array called the "routeSpec" array.

An item in the route spec array may look like the following: 6, 1 This specification defines a route which begins at node 6 and is indexed in the positive direction.

78, -1 This specification defines a route which begins at node 78 and is indexed in the negative direction.

A user simply tells the vehicle which item in the routeSpec array to use as a route.

4. The aforementioned data is stored onto the storage device in the order which it was defined in steps 1 3.

c. NAVIGATOR ROUTE USAGE: The following describes how the navigator 406 uses the defined routes from the above method of the present invention.

When the navigator 406 is powered on, it reads the route information from the storage device 5302 and stores it in RAM in the syntax already presented.

Next the operator specifies a route for the vehicle 310 to follow. Again, the route is simply an index into the routeSpec array.

When the navigator 406 decides that all systems are ready for auto-operation, it sends a message to the vps_posture task 5324 telling it to engage.

The vpsposture task 5324 then determines the position, along the route which is closest to the vehicle's 310 present position 2812. The search for the closest position 284 on the route proceeds as follows: 1. A pointer is set to the first segment in the route.

RI/

1 4.

DOCC'TM#17482 SPC 3 March 1995 -8- 2. The perpendicular distance from the vehicle position to the segment is determined.

3. The pointer is moved to the next segment in the route.

4. The perpendicular distance from the vehicle position to the next segment is determined.

Repeat steps 3 and 4 until the end of route marker 2218 is reached.

6. Determine the distance from the vehicle position to the end points 2218 of the route.

7. Set a pointer to the route segment which had the closest distance and store the coordinates of the closest distance.

The vps_posture task 53.-4 then uses the description of the route (lines, arcs and speeds) to generate posture at one meter intervals. The task 5324 generates a predefined distance of postures plus a safety margin and puts the postures into a S• buffer 3000. To generate a posture which is one meter from a given posture the vps_posture task 5324 uses the following procedure: 1. Determine the type of segment from which the given posture was generated.

2. Use the proper formula for the type of segment to determine the change in north and east per meter of segment length.

20 3. Add the change in north and east per meter to the last given posture.

4. If the generated posture is beyond the end of the current segment, set a pointer to the next segment and repeat steps 2 and 3.

else, return the generated posture.

The vps_posture task 5324 then informs the executive 5316 that it is ready for tracking.

As the autonomous vehicle 310 moves along the posture in the buffer 3000, the safety margin 3006 is depleted. When the safety margin is below a specified amount, the vps_posture task 5324 generates another safety margin 3006 of postures and appends them to the current buffer 3000. The vps_posture task 5324 depletes the posture buffer 3000 by monitoring the current position 2812 of the vehicle 310 DC:'il:#17482SPC 3 Mlh I9 3' and moving a pointer 3002 in the buffer 3000 to the nearest posture. The posture buffer 3000 is constructed as a ring which is traversed in the clockwise direction (see Figure 30, Posture Ring Buffer). That is, postures are placed in the ring such that the direction of vehicle travel corresponds to a clockwise traversal of the posture ring buffer 3000. Therefore, as the vehicle 310 moves the pointer 3002 to the nearest posture in the buffer 3000 will be moved in the cloclkvise direction. When the pointer 3002 will be moved in the clockwise direction, memory in the ring behind posture (counterclockwise of the pointer) is free to be over written.

Step 7 (in the search routine above) is registered until the end of route marker 2218 is reset at which time the vpsposture task 5324 ceases to generate posture and informs the executive 5316 that it has reached the end of the route.

9.9. :As mentioned above, a path is as a series or sequence of contiguous "postures." A posture includes the speed and steering angle required to be on track.

A posture may include latitude, longitude, heading, curvature (1/turning radius), S 15 maximum velocity and distance to next posture information.

3. Posture Generation The tracking method of the present invention, requires certain information about the route it is tracking. The information is contained in a packet called a "posture" 3314. A single posture 3314 may contain position (that is, north and east coordinates), heading, and curvature data, for a specified location on the route.

Therefore, a way of producing posture data from the route specification is required in accordance with the present invention.

Among the navigator tasks, (discussed below) is a task which reads the route information and produces postures at intervals (one meter for instance) along the route which are used by the tracking method. In one embodiment of the present invention, each posture requires 36 bytes of memory which translates to about 36k of memory for each kilometer of route. To reduce the memory requirements, the navigator buffers posture data.

The task which produces the postures reads the current position of the vehicle 310, finds the nearest point on the route to the current position, then generates a DCcMIH:#17482SPC 3 Mh 1995 37 *D 4. 5 specified number of postures ahead of the vehicle 310. The number of postures generated is dependent on the maximum stopping distance of the vehicle 310. That is, there should always be enough postures in the buffer 3000 to guide the vehicle 310 to a stopping point.

In the B-spline approach to route definition according to the present invention however, the need for a posture buffer is eliminated, since the tracking method is able to directly produce posture information from the B-spline curve.

C. Path Tracking 1. Introduction Path tracking or following is a critical aspect of vehicle navigation according to the present invention. The technique of the present invention uses position based navigation (rather than vision based navigation used in conventional navigation systems) to ensure that the correct autonomous vehicle path 3312 is followed. The present invention is also innovative in that it provides for separate control of steering angle 3116 and vehicle speed 3118. Figure 36 graphically illustrates the path tracking system 3100 of the present invention.

For an autonomous vehicle 310 according to the present invention to track specified paths, it is necessary to generate referenced inputs for the vehicle servocontrollers. Thus, path tracking can be considered as a problem of obtaining a referenced steering angle and a reference speed for the next time interval in order to get back to the referenced path ahead from the current deviated position.

In general terms, path tracking is determining the autonomous vehicle commands (speed, steer angle) required to follow a given path. Given Frespecified steering angle, driven wheel velocity values and error components, the command steering and driving inputs are computed in the present invention.

2. Considerations a. Global position feedback: The path to be tracked is specified in cartesian coordinates. If the control scheme consists of only a servo-control to reference steering commands, vehicle position and heading errors accumulate. Position and heading result from integrating I :JH:17482SPC 4/ \CorVR 0 3 Mach 1995 I- 41 r e the whole history of steering and driving. Thus, it is necessary to feedback vehicle position 3304 and heading 3318 in cartesian space.

Consequently, referenced inputs to the servo-controllers are generated in real time, based on positioned feedback 3114 (as shown in Figure 36).

b. Separate steering and driving control: Steering and driving reference inputs are computed in the present invention, from the given path and vehicle speed, respectively. This enables easy integration of path tracking with other modules of the present invention, such as collision avoidance.

3. Embodiments a. TRACKING CONTROL STRUCTURE One of the challenges of vehicle autonomy is to determine the steering inputs required to track a specified path. For conventionally steered vehicles, in the present invention the desired path and the desired speed along the p.i can be tracked separately, reducing the problem to one of controlling the steering. (A path, for this discussion, being a geometric curve independent of time in :oatrast to a trajectory, which is a time history of positions.) Steering angles are planned from the desired path 3312 and sensed vehicle positions. These angles are commanded to the vehicle via a steering controller 3104.

The functional block diagram in Figure 31, shows a tracking control structure according to the present invention.

In kineaatic steering schemes, errors in position, heading and curvature are reduced based on the geometry of the errors without consideration of actuator saturation, compliance, any friction or mass terms. Tuning values, such as lookahead distance and selection of curvature on the path, are selected through empirical trials and simulations in order to get good performance.

In a manually driven vehicle, the look-ahead distance is the distance 3310 in front of a vehicle that a driver looks during driving. The look-ahead distance in the present invention, is the distance by which the errors in position, heading and curvature are planned to be reduced to zero. It varies with the speed of the

I

r o 3 March 1995 -42- 0 conventional or autonomous vehicle.

Varying the look-ahead distance varies the degree to which steering adjustments must be made to effect a change of course. Look-ahead distance is discussed in more detail in a following section.

However, real vehicles depart from kinematic idealization, and their control response departs accordingly. As vehicle speed, mass and path conditions change, actual vehicle response departs even further from kinematic idealization. Hence, kinematic idealization is generally valid only at low speeds with constant conditions.

An embodiment of the present invention uses a model which includes considerations of cornering stiffness, mass and slip angle. The control problem is formulated as a linear quadratic optimal tracking problem where the errors in position, heading and curvature are minimized based on the vehicle control model.

The optimal path and controls are computed from the desired path 3312 and the currently sensed vehicle position using the current errors as initial conditions to the optimal control problem. A few computed steering angles along the initial part of the optimal path are used as references to the low level steering controller for the next sensing time interval.

This preview optimal steering planning has the advantage of guaranteeing stability and optimality with respect to the given performance index. The optimal 20 preview control method of the present invention is central to the steering planning of an autonomous vehicle.

Turning again to Figu: 31, the inner loop 3116 of steering control 3104 is executed on the order of 10 milliseconds, while the outer loop 3114 is closed at the rate of 0.25-0.5 second.

The following procedure is used to close the loop on position. After sensing the current position (Pak) 3210, the posture at the end of the current time interval 3216 is expected.

Then, the desired posture at the end of the next time interval (Pd,k+2) 3218 is computed in a referenced steering angle between 3216 and (Pdk+2) 3218 are determined.

U- o~ 4 :TIIff742.P \§i7 3 Mat 199S A43-- Significantly, as mentioned above, these vehicle and path techniques of the present invention, decouple steering control from velocity control at the vehicle.

b. Quintic method: Shown in the navigator task diagram, Figure 53, which is discussed in more detail below, is a functional block called the tracker 5306. The tracker 5306 operates to cos4.,nt a smooth path back to the desired or correct path. In one embodiment of tfh present invention, as mentioned above, a quintic method is used.

This involves a fifth order curve in error space for steering commands.

The quintic polynomial method of the present invention replans a simple, continuous path that converges to a desired path in some look-ahead distance 3310 and computes a steering angle corresponding to the part of the replanned path 2816 to be followed for the next time interval.

If the desired path is considered as a continuous function of position and the O vehicle is currently at Pa 3320, an error vector can be calculated (Figure 33) that 15 represents error in the distance transverse to the path (eO) 3322 relative to Po 3304, in heading (Bo) 3322, and in curvature (yo) 3404. If the vehicle is to be brought back onto the specified path within distance L 3310 (measured along the reference path), six boundary conditions can be stated corresponding to the initial errors and to zero errors at PL.

E(P) E; E(PL) P(Po) PO; P(PL) 0 Y(Po) Yo; Y(PL) 0 (EQ. 11) A quintic polynomial can be constructed to describe the replanned path (in error space) as follows:

T

PIC-MI:,17482SPC 3 Mat 299S q4 44 E(s) a 0 als a 2 s2 a3s3 a4s4 a 55 2 aas a s (EQ. 12) where s is in the set of [0,L] The expression for e(s) gives the error along the replanned path 2816 from Po 3304 to PL 3308. The second derivative describes path curvature, which can in turn, be used to calculate a steering command to guide the vehicle back to the desired path 3312. Variation in the steering angle 3116 from the replanned path 2816 (or in error space) is computed from the second derivative of error function e(s).

Then, curvature along the new path can be computed as: 10

I

ft Cnew(s) new Cold(s) d2E(S) d2 s (EQ. 13) 0 r The reference steering angle 3112 along the new path can be converted from curvature. Since this procedure is executed at every planning interval, the entire new path back to the reference path 3312 is not required. Only the steering angle 3112 for the next time interval is computed from the curvature at the point on the new path that can be achieved in the next time interval.

The look-ahead distance, L 3310, is a parameter that can be used to adjust how rapidly the vehicle steers to converge to the desired path. Additionally, better performance is obtained if L 3310 is chosen proportional to the vehicle speed because for small values of L 3310, the vehicle oscillates around the path 3312, while for large values of L 3310 the variation introduced by the quintic polynomial is small enough that the tracking performance is poor.

Since there are six boundary conditions used: ep0 error in position at the current position (distance) and epl (look-ahead), eh0 error in heading and ehl (look-ahead), and ecO error in curvature and ecl (look-ahead), a fifth order curve is required. This is used to generate a steering angle 3112.

11:01748 PC '1 0 t, D OV~2 3 Mach 199S ee Recall that path tracking schemes in general, perform better when the path specified is intrinsically easier to track. This is especially the case when the steering actuators are slow compared to the speed of the vehicle.

Other vehicle characteristics, like steering response, steering backlash, vehicle speed, sampling, and planning time intervals significantly affect vehicle performance.

As expected, at higher vehicle speeds, faster and more accurate actuators are necessary, if sensing and planning time intervals are kept constant.

An advantage of the quintic polynomial method in general, is that it is simple, and reference steering angles can be computed very easily. However, since there is no consideration of vehicle characteristics (mass, inertia, time delays, vehicle ground interaction, and so on) in the control scheme, stability and convergence are not guaranteed.

The parameter L 3310 (look-ahead distance) can be adjusted to modify response to the vehicle, and the value of L 3310 can be chosen based on trial and 15 error. This scheme has provided good results at speeds up to approximately 28 Km per hour at the time this application disclosure was prepared.

The method used by the tracker of the present invention is: estimate the next position by either averaging or evaluating the states of position; compensate for delays using either of the estimating methods; dynamic look-ahead charnges at different speeds the coefficients of the quintic: look-ahead distance.

c. Latency and slow system response An additional path tracking embodiment of the present invention uses various compensation techniques to improve vehicle response characteristics. This is used in conjunction with the quintic polynomial method to realize improved tracking performance.

Some vehicle response characteristics include latency of vehicle control commands, slow system response, and vehicle dynamic characteristics including 4 4 bTF 3 Mat 1995 q 1 I3 -46vehicle-ground interaction (VGI), (slip angle and under/over steer).

The latency of vehicle commands was compensated in one embodiment of the present invention, by modifying the vehicle control hardware to reduce time delays, and by utilizing a method which sets control commands far enough in advance to compensate for the existing delays.

Decreasing the time lag between when the vehicle position is sensed and when the command is issued reduces prediction errors, which reduction is required to plan steering angles, and results in better tracking performance.

A varying look-ahead distance with speed also improves the tracking performance in comparison to the constant look-ahead distance.

A tracking method outputs steering and speed commands over a serial link to a vehicle control system. The vehicle control system is a multi-processor, multitasking system, relying on a mailbox queue for communication between taskF.

This mailbox queue is composed of two types of queues, a high performanu.

queue and an overflow queue. During high data flow rates from the tracking task, the high performance queues spill into the overflow queue, degrading the performance of inter-task communication. This can result in total latency times between the tracking task and actual steering actuator commands which are on the order of seconds.

The steering dynamics may be modeled as a first order lag system. It takes a period equivalent to one time constant for a first order lag response to reach approximately 63% of the desired final value. As can be appreciated, for slow systems with large time constants, the response time can be significant.

To resolve the latency and response problems, hardware may be adjusted to be used in close conjunction with the tracking method to control vehicle steering, and a new control scheme devised to compensate for pure time delay and poor response.

The hardware may be adjusted, for example, to reside on the same back plane as the processor which executes the tracking method and controls the vehicle steering system directly. This serves to eliminate delays due to the serial link and 3 Mad, 19i9 q-q- -47queuing.

To compensate fr the remaining delays (delays due to processing time of the tracking method and inter-task communication within the tracking system), a method which sends speed and steering commands in advance to counteract any delays is used in accordance with the present invention. The method may be executed as follows: sense the current position Pactual (Initialization: Pactual P[O] P[l] P[d index+l]) compute error between predicted and sensed position: P Pactual P[O] for i d_index P[i] P[i+l] P e compute the position on the path corresponding to the position of the beginning of the time interval: get 15 P on(P[d index], P on get initial condition:errors(0) P[d_index] S* on compute a quintic polynomial curve in error space[l] 20 predict a posi tiun at the end of the planning time interval; get despos(Pon,ds,P+d_index+l) P[d+index+l] errors(ds) For example, to compensate a system which has time delays on the order of 25 two planning intervals (on the order of 250 mSec), the variable dindex is set to Tracking performance improves as the compensation index (d_index) is increased to match the delays inherent to the system.

d. Vehicle-ground interaction (VGI): Reference commands for steering angle and vehicle speed result in varying angular velocities and accelerations of the vehicle wheels.

VGI describes how the vehicle moves, given steered wheel angles and wheel angular velocities. The principal VGI phenomena are slip angle and under/oversteer characteristics which are based on the tire/road contact region geometry, and are affected by tire elastic deformation. These phenomena require a larger steering angle DCC'mHm#17s2SPC 3 Mh 1995 as compared to a kinematically computed one.

e. Sensing and actuation timing Since actual path tracking is controlled by digital processors in the present invention, a discrete time interval is used. It is governed by the position sensing time interval (which may be on the order of 0.25 Sec) which is much longer than the computing time required for steering planning (which may be on the order of 16 mSec).

At times, especially when the discrete time inter al is large, poor predictions of the vehicle position may be made which degrade performance of the tracking method.

k k+l k+2 executed old sensing and actuation timing 15 sense P expect Pk+l plan #k+l A compensation method of the present invention, serves to reduce the error in predicting the next vehicle position by decreasing the discrete time interval. In 20 this method, the vehicle position is predicted for the end of the computing interval (16 mSec) rather than at the end of the planning interval (250 mSec). The method is executed as follows: *SC 1995 D(ETOt:1I7482SPC 3 Muvh 1995

I-

c i- r sense the current position Pactual (Initialization: Pactual P[O] P[ 1 P[d_index+l] Pactual' k+1 compute error between predicted and sensed position: Pe Pactual actual' k+l for i 0, dindex P[i] Pe e compute the position on the path corresponding to the position of the beginning of the time interval: get Pon(P[dindex], P get initial condition: errors(0) P[d_index] Pon compute a quintic polynomial curve in error space compute a quintic polynomial curve in error space[l] predict a position at the end of the planning time interval: get despos(Po d s P+dindex+l) don s P[d+index+l] errors(ds) predict a position at the next sensing time: 'P ac k+l P[O] P[O])*(dtplan dt actual S 20 comput)/dt_plan f. Look-ahead: Human operators use different look-ahead distances 3310 when driving. At slow speeds, a driver generally looks at a point on the road relatively close to the vehicle, while at higher speeds, this point is generally farther ahead of the vehicle.

The higher the speed, the farther ahead the reference point is, resulting in smaller steering corrections.

Thus, in an autonomous application, a look-ahead distance which varies with speed, logically helps to improve tracking performance.

A desired steering angle may consist of a steering angle from the reference path 3312 and a steering angle 3112 which is computed with a quintic method to correct for tracking errors. These steering angles are summed to give the vehicle steering comma.id as shown in equation below: =ref error Note that look-ahead in the autonomous scheme affects only Derror, even DCC:TIi:1742.SPC 3 M w ah 1995 -v **-r~11 q-sthough look-ahead in manual driving affects both the reference and error compensating steering angles. Shorter look-ahead values result in large steering corrections; the look-ahead distance can therefore be interpreted as a gain in an error feedback system.

An arbitrary model for varying look-ahead distance with speed is expressed with three parameters, Vref Lref and slope, as shown in equation (2) below: L slope (V-Vref)+Lre f where V is the speed of a vehicle and L should be between Lmin=lO and Lmax= 3 0.

Tracking performance is improved with the varying look-ahead distance 3310 of the present invention.

g. Optimal Control Method As mentioned above, an embodiment of the present invention uses a model which includes considerations of cornering stiffness, mass and slip angle.

15 The control problem is formulated as a linear quadratic optimal tracking problem where the errors in position, heading and curvature are minimized based on the vehicle control model. The optimal path and controls are computed from the desired path 3312 and the currently sensed vehicle position 3304 using the current eS. S errors as initial conditions to the optimal control problem.

20 A few computed steering angles along the initial part of the optimal path are used as references to the low level steering controller for the next sensing time S"interval. This preview optimal steering control has the advantage of guaranteeing stability and optimality with respect to the given performance index. The optimal preview control method according to the present invention, is applicable to the steering planning of an autonomous vehicle 310.

The model is derived from a standard telescoped, or bicycle model (not shown) or approximation of the vehicle. The equations describing the vehicle motion include terms which represent the VGI described earlier. These equations use the state variables: R DCCTIII:#I742.SPC 3 Mtch 199S I ~I 4-S x I x y, 0, 0 1 where x and y represent the global position of the vehicle; 0 is the heading 3318 of the vehicle, and 0 is the rate of change of heading.

Using these variables, the equations are: X1 =-X 3 sin(x 4 Vncos(x 4

X

2

X

3 cos(Y 4 Vnsin(x 4 aF aR my X 3 o en0.

a.

*ec 15 f*ee (bC aC aR aF

V

n -V x n x (bC aC aR aF

IV

n X (a 2C +b 2C aF aR

IV

x 5 aC aF u

I

U-

1 where: mL C= [-I t DMM111~f17487-SI'C 3Mi19 3 Mamh 1995

I

V lateral velocity Vn: constant forward velocity 6 steering angle aF: slip angle VF: speed of the front wheel FF: lateral force between front wheel and ground FR: lateral force between rear wheel and ground m: vehicle mass I: vehicle moment of inertia 0: vehicle heading CaF CaR: Front and rear tire cornering stiffness y n

V

Vehicle s c s a o o x,y: inertial system It is well known in the art of optimal control theory, that a cost function must be selected which is used to minimize selected parameters in the system. The cost function used in this problem was selected as: 3 MAb 1995 DCnlTJH:#17482SPC 53- J(t) -2 xl(tf) xd(tf) }2 x 2 (tf) Yd(tf) }2 1 tf[ R u(t) 2 x xd(t) 2 2 t0 x 2 yd(t) }2 dt (EQ. There are several problems to solve the optimal control problem with the state equations (14) and the cost function 1. The system is nonlinear. Usually, a two point boundary value problem 20 which results from a nonlinear system does not have an analytic solution. Numerical solutions, on the other hand, take a long time to compute.

2. The resulting optimal control problem is a free final time problem.

:Generally fixed final time problems are simpler to solve than those with free final time.

25 3. The first term inside the integration (within the above cost function) is the time derivative of the control input, which is not usual in a quadratic cost function of an optimal control problem. However, the time rate change of steering is very important for smooth path following, because it is directly related to the time rate of change of centrifugal force (due to lateral accelerations of the vehicle).

Note that the steering angle is dependent on the curvature of the path as in Figure 51.

The following approaches are applied in order to overcome the above three problems and to make the resulting optimal control problem tractable: 1. Since the sinusoidal functions in the first and the second equations of (14) make the system nonlinear, a new coordinate system, an axis of which is parallel to the tangent direction of the corresponding point of the path to the current vehicle position, is used. The deviations only in the lateral direction are considered in the cost function. These two approximations not only eliminate the nonlinearity DCT I R!,;#17482.SPC 3 MArh 1995 I I I -54in the system equation but also reduce the number of equations to deal with; the first equation of (14) is not required now. (Refer to "Coordinate Systems.") 2. This problem with free final time, tf, can be converted to the one with a fixed final value of the independent variable by writing the differentials in the equation of motion with respect to the forward distance. To this end, a nondimensional independent variable, s, is defined as: s ds s- 6 n 0 s d s accel s dt s f f 3. To solve the third problem pointed out above, a new state vector and a control input are defined as: so

T

new old' Uold] new old where Xnew satisfies:

A

new I1I I A I B old I old I I I I I 0 0 II I I I! 1 0 new 1 y=Ax+B where Aold and Bold denote the old system matrix and the old input matrix.

Then, the state variables and the control input are defined as: Z vT, u=6 (EQ. 16) which satisfies the system equations as the following: oFC>\ 0 DOCCTJII:#17482.SPC 3 Manh 1995 d Z VZ a d9 2 n 3 dv, di C aF +CaR) 2 my (bCR aCaF n -vn z 4 d) z 3 z a dG di z4 (bC aR aC aF)

IV

n (a 2C aF b 2C R) IVz 4 z 2 Z ]y aC aF d8 d9

U

(EQ. 17) The new cost function becomes: L Z qf 2 L E: z(1) d 1 0 (EQ. 18) The steering planning with resulting system equation (17) and the cost function (18) above, can be solved as a linear quadratic tracking problem as the following. Suppose the system equation and the cost function are described as 9" DCcrl1.17482.SPC 0 7~~ '4' 3 March 1995 C1131~~ 53 56- X AX BU t t (EQ. 19) L X(tf) Xd(tf) Q f X(tf) Xd(tf) f t X(t) Xd(t) T Q(t) X(t) Xd(t) o U dt (EQ. and Qf>, o, Q o, R> o are chosen all symmetric. Then the resulting equations are:

S

20 oooo e P A P PA PBR B P Q -1 T k(t) R B P(t) 0 P(tf) Qf (EQ. 21) (EQ. 22) (EQ. 23) ~r r v (A-BK) v QXd V(tf X(tf) U -KX R -B v (EQ. 24) Thus, the riccati equation (21) must be solved first and the gains are 30 computed from the result of the riccati equation and then the forcing function driven by the desired path are computed by solving equation Then, the control and states are obtained by solving equation (19) and (24).

The MacFarlane-Potter integration method was tried to solve the riccati equation. This method is known to be very effective for the steady-state solution of the time-invariant problem. Since the previewed distance is quite long and the initial parts of the solution are used, this method seems good to reduce the computation time.

Hence, equation (23) is changed as the following equation (25) and solved, because the previewed distance is long and only the inertial part of the solution is used.

DXCII:#17482.SPC 3 March 1995 1- (A-BK)Tv QXd 0 (EQ. h. Conclusion: Tracking performance has been improved according to the present invention, by investigating and understanding vehicle and control system dynamics and by designing compensation methods given this understanding.

A degraded performance of a tracking method is attributable to latency of vehicle control commands, slow system response, and vehicle dynamic characteristics. It is possible to counteract each of these effects.

Latency of Vehicle commands, a dominant effect, can be successfully compensated by modifying the vehicle control hardware and by utilizing a method which set control commands far enough in advance to compensate the delays.

5 Decreasing the time lag between when the vehicle position is sensed and when the 15 command is issued reduces prediction errors. This is required to plan steering angles, and results in better tracking performance.

l Varying look-ahead distance with speed also improves tracking performance in comparison to using a constant look-ahead distance.

In general terms then, path tracking is the function of staying on course. In path tracking in the present invention, as discussed, some of the considerations are errors in distance, heading and curvature, delays in the system including processing delays and delays in vehicle response to actuators, and so on, dynamic look ahead distance, weighted path history, and extrapolation.

D. Obstacle Handling 1. Introduction Obstacle handling involves at least three major functions: detection of obstacles 4002, avoidance of obstacles 4002, and returning to path 3312. The returning to path function is similar to path generation and tracking, as described above.

In addition to path tracking (following), successful navigation of vehicle 310 requires that vehicle 310 be able to detect obstacles 4002 in its path, thus allowing the vehicle to stop or otherwise avoid such an obstacle before a collision occurs.

IXC:fIU:#17482,PC3m 1 3 Mucb 199S

D

In one embodiment of the present invention, a single line infra-red laser scanner 404 (See Fig. 38) is used in a configuration where the scan is horizontal (not shown). The scan line 3810 does not contact the ground, so any discontinuities in the range data can be attributed to objects 4002 in the environment.

Since a reference path 3312 is available and the vehikie position is known relative to the reference path, only the range data and a region bounding the reference path 3312 is processed for threatening objects 4002. Objects outside of this region, or boundary zone, are ignored. The width of the boundary zone (not shown) is equal to the vehicle width plus some selected safety buffer to allow for tracking and positioning errors. This method is limited in its usefulness and is referred to as "clearance checking".

2. Detection of Obstacles a. Clearance checking: In the simplest case of the present invention, the laser 404 may be used in a single line scan mode with successive range measurements being made at regular angular intervals as the laser scans over the field of view. Again for simplicity, these scans can commence at regular time intervals. The term "clearance checking" has been used to describe this method. In this version of the present invention, the method has been limited to processing only two dimensional data.

This type of obstacle method is limited to checking to see if the path 3312 is clear using a single line scan mode with successive range measurements being made at regular angular intervals as the scanner 404 scans over the field of view.

It does not include any methods to establish the existence of any obstacle 4002 or to create a path around it if the path is not clear. This type of method is not deemed to be a particularly useful obstacle detection method, except in very rigidly controlled environments, such as on a factory floor.

b. Filtering and edge detection scheme: A second obstacle detection embodiment of the present invention uses a multiple-line scanner 3804 (See Fig. 38), whose scan 3810 contacts the ground at some distance in front of the vehicle 310. Since the scan line contacts the ground, DU 1I11:17482.SPC 3 MJAh 199S discontinuities in range data can no longer be attributed to threatening objects 4002.

For example, profiles from natural objects such as hills and banked or crowned roads can cause discontinuities in range data. This technique of the present invention can discern discontinuities in range data between threatening objects 4002 and natural objects (not shown).

In this embodiment of the present invention, a filtering scheme is used to decrease the amount of data processed and is independent of the scanner configuration used. The edges of the boundary zone are found by transferring the range data to an image plane representation 3900 (See Fig. 39), where each range value is located by a row number 3908 and a column number 3910 (a matrix representation).

i'"'"Processing load is minimized by selecting a relatively small number of the scan lines available in a range image 3900. The scan lines are selected by vehicle speed, and are concentrated at, and beyond, the vehicle stopping distance. The 15 selected scan lines from successive frames of data can overlap.

In this method, if the vehicle 310 is moving fast, the selected scan lines 3906 are far in front of the vehicle (near the top of the range image 3900). In contrast, when the vehicle is traveling slowly, the selective scan lines 3906 are closer to the vehicle (near the bottom of the range image 3900).

Each scan line is made up of many pixels of data. Each pixel has two parameters associated with it. First, the actual value of the pixel is the range value returned by the scanner 3804. Second, the location of the pixel on the scan line gives an indication of the angle, relative to the vehicle centerline, at which the range value was recorded. This corresponds to a cylindrical coordinate frame (R,THETA,Z) description.

Given the cylindrical description and the known scanner location with respect to the vehicle 310, the range values can be converted to a cartesian coordinate system. The result is a road profile description which can be used by a novel filtering scheme to determine if threatening objects 4002 are present in the vehicle path 3812, while ignoring effects due to natural hills and valleys in a typical .T R DCCIXTI:#17487.SPC 3 Match 199 1' -Q n r S7 66roadway.

After the scanner data is converted to cartesian coordinates, the data is processed to determine which part of the scan is actually on the road 3312 and which part of the scan line is outside of the vehicle path and therefore safely ignored. Given the vehicle position and the width of a boundary (which is equal to the vehicle width plus some safety margin), the coordinates of the boundary on either side of the vehicle path can be determined. The coordinates of the boundary can be compared to the coordinates of each pixel on the current scan line. The pixels which have coordinates outside of the boundary are ignored.

The filtering scheme builds an expectation of the road profile from previously sensed road profiles. This expectation is based on three parameters which were found to adequately describe typical paved roads. These three parameters are: o road crown: the curvature of the road cross section (perpendicular to the road centerline).

15 o road bank: the 'tilt' of the road profile (perpendicular to the centerline).

o road height: the height of the road centerline above a reference plane described by the location of the four tires of the vehicle 310.

Expected values of the road crown and the road bank are determined by performing a standard, least-squares Kalman filtering technique on previously sensed scanner data. The Kalman filter basically keeps a type of running average of the two parameters based on the values determined from the previous data.

In accordance with the present invention, the expected road height for a particular scan can be determined through one of two similar methods.

One is to average the road height at each pixel within the current scan line to determine a characteristic height of the scan line in question.

The second method is to filter the road height using the standard Kalman filter similar to that used when determining crown and bank expectations.

These three parameters can be used to determine a second order equation D IMl:#17482SPC 3Mabla19 6-1which describes the expected road profile. This expected profile is compared to the actual road profile. Any deviations between the two which exceed a preset threshold value are assumed to be threatening objects.

This scheme of the present invention is viable given the assumption that any detected objects 4002 are small in comparison to the width of the road. Then, when these averaging or least squares methods are used, the effects due to objects are negligible in comparison to natural road data.

This filtering scheme also includes a very simple edge detection method which convolves the selected range data with a simple seven point weighing function.

c. Obstacle extraction: additional technique of the present invention processes an entire range *n timage 3900 from a multi-line scanner 3804 for objects. This method of the present invention accomplishes three goals: 15 1. Do not detect obstacles 4002 when none exists, 2. Detect obstacles 4002 when obstacles do exist, and Q 3. Detect the correct obstacles 4002 when obstacles exist.

Obstacle extraction is obstacle detection through the use of blob extraction.

Blob extraction is well known in the art of computer graphics. Obstacles are found by clustering similar pixels into groups, called blobs. The goal of obstacle extraction is to store and process obstacles as units rather than as individual pixels.

The obstacle extraction of the present invention may be done by preforming the following steps in the image plane 3900: 1. Project the vehicle path into the image plane 3900, 2. Transform range data into height data, 3. Fit a curve to the height at the center of the road (this represents the expected road height at each row), 4. Threshold the actual road height against the height expectation, and Extract the obstacles (indicated by differences in actual and expected road heights which exceed the threshold).

Dc =TH.:#1742SPC 3 Mamb 1995

D

s

C

Finding the road: In order to process all the available data, the images 3900 must be processed at the frame rate of the scanner 3804. For this reason, most of the computations in the obstacle extraction method are done in the image plane 3900. By projecting the path into the image, a large portion of the image can be ignored, and many needless computations avoided.

Assuming that the vehicle path 3812 is specified at regular intervals, the current vehicle position can be used to locate the path segment line 3902 in front of the scanner. This path 3812 is transformed from world coordinates into image coordinates by projecting the points corresponding to the road or boundary edges 3902 into the image plane 3900 (see Figure 39).

A cubic spline is used to interpolate between the gaps. Thus, the center and edges of the row 3902 are found for each row 3908 in the image 3900. The pixels isolated between the road edges 3902 are converted (cylindrical to cartesian coordinates) from range to height data. The outlying pixels are discarded and not processed any further.

Modeling road height: Once the center of the road is known for every row 3908 in the image 3900, the height for each of these points can be determined. A third order least squares curve is fit to these data.

This has the effect of modeling the general trend of the road (up and down hills) as well as filtering out the effects of noise and small objects lying in the center of the road.

Thresholding: Obstacles may be located by using a height threshold. A straight height threshold would be meaningless since the surrounding terrain is not necessarily flat.

Hence, the threshold is referenced against the expected height, as predicted by the third order fit, at the row number 3908 of the given pixel.

In this manner, a hill is not considered an obstacle since the height expectation and the actual height should match very closely. On the other hand, a Pc

CRC,

c Ni7-4 'pC~7 O( DE2:TJII:#17482SPC 3 Mb 1995 6O real obstacle 4002 would barely reflect the expected road height (due to the least squares fit), and therefore is readily found by thresholding. The result of this thresholding is a binary image (not shown) suitable for a "blob extraction." The binary image only indicates where an object is or is not present in the image.

Blob extraction: Blob extraction works by clustering adjacent set pixels 'indicating an obstacle 4002 is present) together and treating them as a unit. Two pixels are adjacent if they are either: 1. In the same column 3910 and have consecutive row numbers 3908, or 2. In the same row 3908 and have consecutive column numbers 3910.

By grouping pixels together into blobs, the obstacles 4002 can be treated as a whole unit and are suitable for further pro-essing.

0. Applications: One way to use extracted blobs is to pipe them as input into another program.

15 For example, the objects 4002 can be parsed into coordinates and used to accumulate a global object map 4004 (See Fig. 40). This map 4002 is then passed into another program, and used to do collision avoidance or path planning.

3. Avoidance of Obstacles ~Once the present invention detects an obstacle 4002 in the path of the vehicle 310 (See Fig. 40), it must then avoid a collision with the object. Certain assumptions are made concerning the obstacle avoidance problem: 1. The obstacle environment is populated with obstacles 4002 that can be represented by convex-polygons or convex lines; 2. The navigation methods only have access to the local environment information in the form of a local map representing all of the visible faces of the obstacle from the position of the vehicle 310, which can be obtained from unprocessed laser range data or from data processed through blob-extraction; 3. The vehicle 310 is a conventionally steered type which has constraints on its speed and acceleration and constraints on its steering angle and DCCTl'l:#N17482HSPC3 3 Ma 1995 64 the rate of change in the steering angle.

To deal with the obstacle avoidance problem, the present invention divides it into two sub-problems.

First, to decide if any obstacles are in the way, and if so, which side should the vehicle pass on. Then select a sub-goal 4006, which will lead the vehicle 310 around the obstacle 4002, leading towards a higher level goal 4008, which is to get back on the desired path.

Second, once a sub-goal 4006 is selected, make a steering decision which drives the vehicle 310 towards the sub-goal 4006, while steering clear of the obstacle 4002. A sub-goal selection method and a steering decision method of the present invention solve these two sub-problems.

The above enumerated assumptions are managed in the following process: ooeo Te The obstacle locations are obtained from the laser range scanner 3804 or 404.

The range data generated by the scanner 3804 or 404 are processed to produce a list 15 of polygona! faces, modeling the visible portions of the obstacle 4002 from the *vehicle position. Each time new range data become available, a sub-goal selection method is executed to generate a sub-goal 4006 and determine regions of safe navigation (free-space 4010) for the steering decision method. The frequency at which the sub-goal selection method can be executed depends on the rate at which o 20 the scanner 3804 or 404 can collect data. The achievable vehicle speed, in turn, depends on this frequency of execution.

For the steering decision method, a higher sampling rate is desirable in order to produce a smooth path. Therefore, the steering decision method is executed more frequently than the sub-goal method.

The basic flow of the sub-goal method is the following: 1 save last initial-subgoal, subgoal, and free-space, set goalblocked flag to true; 2 if final goal is visible generate direct goal if direct goal is visible DCLil:lf1742SPC 3 hM(mh 199 set goal_blocked flag to false; 3 otherwise generate an initial subgoal set subgoal to initial subgoal recursively generate subgoals until the latest one is visible, if subgoal not feasible, abort; 4 if goal_blocked flag is true restore old initial-subgoal, subgoal, and free_space; otherwise generate free-space if free-space is not safe restore old initial-subgoal, subgoal, and freespace.

SSub-goal Method: First (step 1 above), the initial-subgoal, subgoal, and freespace generated from the previous iteration is saved. This assures that when the 15 newly generated subgoal is not safe, the old subgoal can continue to be pursued.

Next (step 2 above), when the final goal is visible, attempt to generate a direct goal which is not associated with any obstacles 4002. Although the final goal is visible in the local map, it does not necessarily mean that no obstacle is blocking the final goal because obstacles outside the scanner range (both distance and angular 20 wise) will not be represented in the local map. Therefore, when generating a direct goal, ensure that the goal is located in the cone area which is covered by the scanner 3804 or 404 to avoid placing a subgoal on or behind an obstacle 4002 that is not in the local map.

The next step (step 3 above) handles the situation where the final goal is blocked by an obstacle 4002 in the local map. In this case, the obstacle 4002 that blocks the line of sight to the final goal is first determined.

Given a blocking obstacle, there are two possible ways of going around it.

If both edges of the obstacle are in the range of the scanner 3804 or 404, we may choose to go around the edge which gives the minimum sum of the distances from the vehicle 310 to edge and from the edge to the final distance. If only one edge DCCTJH:#17482SPC 3 Mrch 1995 66

S.

S..

S

So of the obstacle 4002 is in the range, choose that edge to go around. If none of the edges is visible, always arbitrarily choose the left edged to go around. Once the edge to go around is determined, place tlie initial subgoal away from the edge at a distance that is proportional to the vehicle size.

Because of this displacement, the resulting subgoal may be blocked by other obstacles 4002. This calls for the recursive generation of ,ubgoal on the obstacle, which blocks the line of sight to the subgoals just generated. This recursive process continues until a subgoal visible to he vehicle 310 is generated. Each subgoal so generated is checked for viability. By viability it is meant that the subgoal does not lead the vehicle 310 towards a gap between two obstacles 4002 which is too small for the vehicle to pass through. When such a condition is detected, the vehicle 310 will stop.

The direct subgoal generated in the second step (step 2 above) could possibly be obscured from the vehicle 310. If such is indeed the case, the old subgoals from the previous iteration is restored and used next (step 4 above).

In the final step (step 5 above), generate the free-space 4010 for the visible subgoal, which is a triangular region that contains no obstacles. Once the free-space 4010 is generated, the safeness of the subgoal and free-space 4010 can be determined. When the new subgoal and free-space 4010 is not safe, the old subgoal and free-space is again retained. Otherwise, the new subgoal and free-space is used.

The steering decision method of the present invention is composed of two major components: transferring state constraints to control constraints; and determination of the desired control vector.

Once the control constraints and the desired control vector are computed, the control vectors can be determined using optimization techniques well known in the art.

4. Return to Path The present invention includes a method, as shown diagrammatically in Figure 40, whereby a safe path around a detected object 4002 will be plotted and navigated DCM'.Tlf17482.SPC9 3 Much 1995 so that the vehicle 310 will reacquire the reference path after avoiding the object 4002.

Scanner System a. Introduction: Referring to Figures 38 and 42, the present invention also includes a laser scanner system 404. The scanner 404 is used to find obstructions 4002 (See Figure that randomly crop up in the vehicles 310 path, as previously discussed.

Sources of such obstructions 4002 may be varied and numerous depending on the particular work site. They may include fallen trees and branches, boulders, moving and parked vehicles, and people.

The scanner 404 gives the autonomous vehicle 310 the ability to detect and deal with the external world as conditions require.

b. LASER Scanner: The major components of the laser scanner system 404 are depicted in Fig.

42.

S

S roeooo

S

°e e e A laser range finder 3804 uses an infra-red beam 3810 to measure distances between the range finder unit 3804 and the nearest object 4002. A brief pulse is transmitted by the unit 3804 and the time for the beam 3810 to reflect off an object 4002 and return gives the distance.

The beam 3810 from the range finder 404 is reflected by a rotating mirror 4222 giving the range finder 404 a 360* view of the world. Mirror rotation is accomplished through a motor 4206. The motor speed is controlled via a terminal 4210, which communicates with a motor amplifier/controller 4220 through a standard RS232C serial link 4224. Synchronization between laser firings and mirror angular position is done with an encoder.

Distance data on a line 4226 from the laser range finder 404 is taken by an interface circuit 4228, which transmits the data differentially to a buffer circuit 4214.

Individual pieces of data are collected by the buffer circuit 4214 until the mirror 4222 makes one full revolution. This set of data comprises one scan. When a scan is complete, the buffe; circuit 4214 signals a processor 4212, whereupon data for the DOCrI#:#17482.SPC 3 Marrh 1995 s

D

entire scan is transferred to the processor 4212 for processing.

c. Scanner System Interface: The interface circuit 4228 has three functions.

First, it acts as a safety monitor. A situation could occur where the mirror 4222 would stop rotating, as in the case of the drive belt 4230 between the motor 4206 and mirror 4222 breaking. Under this condition, the laser 4204 would continue to fire, and since the mirror 4222 is stationary, it would fire at a single point (dangerous for anyone looking directly into the laser beam). The interface circuit 4228, however, senses when the angular velocity of the mirror 4222 falls below half a revolution per second, and disables the laser 4204 if such a condition occurs.

The second function is to disable the laser 4204 from firing for part of the 360 degrees scan area. Typically, the laser scanner unit 404 will be mounted in front of a vehicle 310, and thIe field of interest is in the 180 degree area in front of the vehicle. The vehicle itself will block the back portion of the 360 degree scan area. In this case, the circuitry 4228 will prevent the laser 4204 from firing into the vehicle, extending the life of the laser diode while receiving range data for the area in front of the vehicle. The enabling and disabling of the laser range-finder 4204 is done through two sensors (not shown) mounted near the mirror housing 4222. For testing purposes, or for applications where a 360 degree scan is desirable, the disable feature can be turned off through a DIP switch.

The third function of the circuit 4228 is to convert signals between single ended and differential form. TIL signals from the laser unit 4204 are differentially transmitted to the buffer circuit 4214, and differentially transmitted signals from the buffer circuit 4214 are converted to TIL levels. This prevents noise contamination along the cable 4226 connecting the two circuits.

d. Scanner System Buffer Circuit: The function of the buffer circuit 4214 is to synchronize laser 404 firings with the angular position of the mirror 4222, to collect data for one complete scan, and to transmit the scan to computer 4214 for processing.

The angular position of the mirror 4222 can be determined through signals DCC:TI:17482SPC 3 Match 1995 sent by the encoder 4208. The buffer circuit 4214 uses two signals from the encoder 4208: the Z and A channels.

The Z channel is the encoder index; it gets asserted once per revolution of the encoder 4208, and is used to signal the beginning of the scan area.

The A channel is one line of the two line quadrature output of the encoder 4208, and pulses 1000 times per revolution of the encoder. This channel is used to trigger laser firings.

One additional signal is needed to fully synchronize the scan field with the encoder signals, There is a gearing ratio of 2:1 between the encoder/motor 4206 and the mirror 4222. Two revolutions of the encoder 4208 rotates the mirror 4222 once.

This translates to 2Z channel pluses and 2000 A channel pulses per revolution of the mirror 4222, and the inability to differentiate the beginning of the first half of the oeoo scan with the beginning of the second half.

To fully synchronize the scan field, the DB (dead band) signal generated by 15 the interface circuit 4222 is used. The DB signal, used to disable the laser 4204 from firing in the back half of the scan, allows the differentiation of the front and back halves of the scan. The Z and DB signal together signal the beginning of the scan area.

The second task of the buffer circuit 4214, to collect data tur one complete scan, is accomplished through the A channel of the encoder 4208. The 2000 pulses of the channel are divided by eithez 2, 4, 8, or 16, selected through DIP switches (not shown) on the circuit board 4228. This allows the number of data points per scan to be varied between 1000, 500, 250, and 125. The divided signal is used to trigger the laser range-finder 4204 at appropriate angular intervals, and to store the resulting range data in memory 4214.

The sequence of events is as follows. W (write) is asserted one clock cycle upon a rising edge on the divided A signal. At this point, data from a previous T (laser trigger) is available and is stored in memory 4214. T is asserted the following clock cycle, triggering the laser and putting the resulting range data onto the memory input bus 4226. This data is written on the next W pulse, repeating the cycle.

D I:#17482.SPC 3 Marh 1995 Lfl3 o r r o~

L

The final task of the buffer circuit 4214 is to transmit the scan data to a computer 4212 for processing. Completed scans are signaled by the Z and the DB signals (the beginning of a scan is also the end of a previous one). Upon a completed scan, an interrupt request lin, asserted, and remains asserted until either the mirror 4222 has made half a revolution, :ne processor 4212 acknowledges the interrupt. In the first case, the half revolution of the mirror 4222 is signaled by a subsequent Z pulse and indicates a timeout condition; the processor 4212 has failed to respond and the data is lost.

In the normal case, the interrupt is acknowledged. Upon receipt of the acknowledgement, STR (data strobe) is asserted and held until IBF (input buffer full) is received. During this time, data is put on the data bus 4230 and may be ready by the computer 4212. Data is valid on the bus 4230 until IBF is asserted, at which time STR is de-asserted and the data removed from the bus 4230. Once the processor 4212 detects the de-assertion of STR, it de-asserts IBF. This causes STR to be asserted for the next piece of data, repeating the cycle.

Scan data is collected and stored in two memory banks 4214. This avoids shared memory and synchronization problems between scan storage and scan transmission. Data for a new scan is stored in one bank, while the previous scan is being transmitted from the other bank.

The buffer circuit 4214 removes from the processor 4212 the responsibility of synchronizing laser findings with mirror position and collecting individual pieces of data. It allows more efficient use of CPU time, as data is received in scan sized chunks. The processor 4212 spends its time processing the data, not in collecting it.

E. Vehicle Controlling Systems 1. Introduction Referring now to Figure 43, the vehicle controls are comprised of four, lowlevel functional blocks.

One is called a "vehicle manager" (4302). A second is callied a "speed control" (4304). The third is called a "steering control" (4306). The fourth is called DCC:Tn :#174SSPC 3 March 1995

C

a "monitor/auxiliary control" (depicted as two separate blocks 4310 and 4308. These are described in turn below.

They are all tied together with a high-speed serial data bus 4314. The bus 4314 is a data collision detection, packet passing system.

Each of these functional blocks have separate microprocessors, for instance of the Motorola 68000 16 bit series. Each of these microprocessors talks to and listens to the others over the bus 4314.

While each functional block has a more or less specific function, the vehicle manager 4302 functions as a communications hub. It sends to and receives messages from the navigator 406 via an RS-422, 9600 Baud serial link 4316. It is also listening to and sending to the remote control or "tele" panel 410 via an FM radio communications link 4318.

2. Vehicle Manager (modes) As mentioned above, the vehicle manager 4302 receives commands from a remote control panel 410 and the navigator 406. It then decides which modi M, T, or R" (for Autonomous, Manual, Tele, or Ready) the vehicle 310 should be in.

a. Ready Mode Reference is now made to Figure 44, which shows the states (modes) and how the vehicle 310 changes between states. The navigator 406 cannot set the mode itself. Notice that the vehicle 310 cannot change from tele to auto, for instance, directly. It must pass through the ready mode 4404 first in that case.

The ready mode 4404 brings the vehicle 310 to a stop in a known state. This is because it would be difficult to make a smooth transition, from, for instance, auto mode 4408 to tele mode 4406 while the vehicle 310 was moving. The tele control panel joy-stick 4502, 4504 would have to be in just the right position when control was switched.

Going from tele 4406 to auto 4408 mode, there is the consideration that the navigator 406 must initialize. For example, it must determine where it is with respect to a route before taking control, which takes some finite time, during which the vehicle 310 might otherwise drive off uncontrolled.

C

C

1 0 0- DCC.,T11I:17482.SPC 3 Mamh 199S b. Tele Mode Tele control mode 4406, also referred to as tele-operation, remote control or radio control mode, provides a way of controlling the vehicle 310 from a remote location while the vehicle 310 is kept in view.

Shop personnel would use the tele-operation mode 4406 to move the vehicle 310 in the yard, for example. Advantageously, this mode would also be used by a shovel or loader operator to maneuver the vehicle into position for loading or unloading, and moving the vehicle into a location where autonomous mode 4408 would resume control.

In tele-operation mode 4406, each vehicle 310 at an autonomous work site 300 would have its own unique identification code that would be selected on a radio control panel 410 to ensure communication with and control of the correct vehicle only. The vehicle 310 would only respond to tele-operation commands 4318 when its unique identification code is transmitted. Any conflict between modes, such as 15 between manual 4402 and tele 4406, would be resolved in favor of manual mode 4402, for obvious safety reasons.

The navigator 406 keeps track of where the vehicle 310 is while being operated in the tele mode 4406, even though, in tele mode, the vehicle can be maneuvered far off of a known route.

c. Manual Mode Manual control mode 4402 may be required when the vehicle 310 is being maneuvered in very close quarters, for example, at a repair shop, equipment yard, and so on, or when a control subsystem needs to be removed for repair or maintenance.

This control mode may be implemented to be invoked whenever a human operator activates any of the manual controls. The simple action of stepping on the brakes 4708, moving the shift lever from some predetermined, autonomous mode position, or grasping the steering wheel 4910, for example, would immediately signal the control system that manual control mode 4402 is desired and the system would immediately go to the manual mode.

DC.- Tni:17482ISPC 3 Murh 1995 73- While in manual mode, the autonomous system would continuously monitor vehicle motion and maintain an updated record of the vehicle position so that when and if autonomous mode 4408 was desired, a quicker and more efficient transition could be made.

When autonomous mode 4408 is again desired, the human operator would then affirmatively act to engage autonomous mode 4408, by physically moving a switch or lever, for instance, to the autonomous control mode. A time delay would preferably be built in so that the human operator would have the opportunity to leave the vehicle 310 if desired. At the end of the time delay, the system would then give 10 several levels of warning, such as lights, horn, or the like, indicating autonomous eooo :takeover of the vehicle 310 was imminent.

o: d. Autonomous Mode The autonomous mode 4408 is entered into from ready mode 4404. In the autonomous mode 4408, the vehicle 310 is under the control of the autonomous navigation system.

S:In this mode, the vehicle control system receives messages from the navigator 406 as discussed above, through the vehicle manager 4302. The vehicle manager 4302 is, as discussed, basically the communications and command hub for the rest of the controllers.

The vehirdF manager 4302, and the other functional control blocks, all communicate with the shutdown circuits 4312 as well. The shutdown circuits 4312 are discussed in more detail below.

3. Speed Control The speed control subsystem 4302 may be organized to contain a speed command analyzer, closed loop controls 4800 for the engine 4614, transmission and brakes 4700, 5000, a real time simulation model of the speed control system, and a monitor 4310 that is tied to an independent vehicle shutdown system 4312. It is designed to be placed in parallel to the production system on the vehicle 310.

The speed control functional block 4304 takes care of three basic functions.

It controls the governor on the engine 4614. It controls the brake system 4606. And DCC.7T1:117482S PC 3 Mama 199S it controls the transmission 4610 via the production transmission control block 4616.

The production transmission control block 4616 is interfaced with the speed control block 4304 in a parallel retro-fit of the autonomous system onto the production system as shown in Figure 48. The production transmission control block 4616 is a microprocessor based system which primarily monitors speed and shifts gears accordingly.

The autonomous system speed control block 4304 feeds the transmission control block 4616 the maximum gear desired. For instance, if the vehicle 310 is to go 15 mph, the maximum gear might be third gear. The production transmission 10 control block 4616 will control all the shifting necessary to get to that gear uoo o appropriately.

The governor 4626 (Fig. 46) controls the amount of fuel delivered to the engine 4616. Thus, it controls engine speed. The autonomous system is capable of being retro-fitted in parallel with the production governor control system, in a 15 similar fashion as described with respect to the transmission system.

The brake system is shown in Figures 47 and 50. The autonomous system here is also capable of being retro-fitted to the production brake system.

The following discusses vehicle systems shown in Figures 46, 48, 47, 50 and 49. These systems relate to the vehicle drive train 4600 and steering 4900 systems.

Referring to Figure 46 a governor 4626 controls engine speed 4222, which in turn controls vehicle speed 4624. The engine power is transferred to the drive wheels through the drive train 4600 which is comprised of: torque converter 4612 transmission 4610 final drive 4608 brake system 4606 wheels 4604 The function of these systems is well known in the art.

Several key systems were modified in accordance with the present invention to effect autonomous control. The primary systems were the speed control (engine DCCTJIi:117482SPC 3 Mawk 199 speed, transmission, vehicle speed, and brakes) and steering systems. Each key system is design with manual override capability as a safety measure. In all cases, manual control has priority so that if the vehicle is operating autonomously, and an operator takes control of any one of the vehicle functions, control automatically is returned to the operator.

The system also provides an emergency override button (not shown; also referred to as a 'panic' button) which, when activated, disables all electronically controlled systems and returns the vehicle 310 to manual control 4402.

The system also provides for sensing the pneumatic pressure which is a key 10 part for actuating some of the key systems. If this pressure falls below some preset oooo :threshold, it is assumed that there is a problem and the vehicle control system reverts to manual control 4402 and the vehicle 310 is stopped.

Figure 48 depicts the system used to control engine speed. This system uses electronically controlled valves 4808 and 4812 to regulate pneumatic pressure in 15 parallel to a pedal 4806 which can be manually operated to override electronic control of the engine speed 4622. The pressure sensor 4802 and the engine speed sensor 4622 provide the necessary feedback for the electronic speed control system S4304.

Also required to control the vehicle speed is a transmission control 4616. The basic control system is readily available on the particular vehicle used for this purpose.

In addition to controlling the engine speed 4622 as a means of regulating vehicle speed, it is also necessary to control the vehicle service brakes 4606. This system is shown in Figure 47 and is necessary to effect normal stoppage or slowing of the vehicle 310. This system uses electronically controlled pneumatic valves 4712 and 4716 in parallel with a manually operated brake pedal 4708 and/or retarder lever 4710 to regulate the braking force. These two manual inputs can override the electronic control system when actuated. The pressure sensor 4702 and the vehicle speed sensor 4624 provide the necessary feedback to regulate the braking force.

Control of vehicle steering is also required for the vehicle to operate DCC*.Th1 17482SPC 3 Maw 1995 -73 76 ar onomously. The system which performs this function is shown in Figure 49.

The system consists of a Rexroth proportional hydraulic valve 4912 which can be actuated electronically to provide flow to hydraulic cylinders 4914 and 4916 attached to the vehicle steering linkage. The system also comprises a manually operable hand-metering unit, or HMU, 4918, which is in parallel to the electronically controlled system. The manual system can override the electronic system, if required, as a safety measure. Also, the system provides a switch 4920 on the IMU to detect when the manual steering wheel 4910 is different from the centered position. When not centered, the autonomous system assumes that the system is 10 being operated manually 4402 and disables autonomous control of the vehicle 310.

oooo :Electronic control of the vehicle parking brake is a' o included as an added safety feature. This system is shown in Figure 50. For proper operation under autonomous control, the parking brake is manually placed in the 'ON' position.

When the vehicle proceeds through the status modes (MANUAL 4402, READY 15 4404, and AUTO 4408), the parking brake is automatically released by electronically controlling the pneumatic valve 5008. This system is in parallel to the manual systems comprised of the brake lever release valve 5016 and the Emergency brake lever 5014.

When a problem is encountered, the vehicle 310 is automatically placed under manual control. Since the manual setting of the park brake is normally this activates the parking brake, stopping the vehicle 310 as quickly as possible.

4. Steering Control Referring again to Figure 43, the steering control functional block 4306 is responsible for controlling the steer angle of the vehicle's wheels. It sends out commands to a valve 4912 to control the steer angle and receives information from a resolver (not shown) mounted on the tie rod system, so that it knows what the actual wheel angle is.

The steering angle can be controlled with an accuracy on the order of a half a degree, and the resolver is accurate to something less than that, on the order of an eighth of a degree.

DCC T II:1748ZSPC 3 Mach 1995 At some point in the useful life of the vehicle 310 the resolver may go out of adjustment. If this happens, the vehicle will not be able to track the path 3312 properly.

However, the navigator 406 constantly monitors the vehicle 310 to determine how far the vehicle 310 is from the desired path 3312. (The vehicle 310 is always off the desired path 3812 to some extent, and the system is constantly correcting.) If the vehicle 310 is more than a certain distance, for example several meters, from the desired path 3312, the navigator 406 stops the vehicle as a safety precaution.

The steering control system 4306 itself is also always checking to make sure 10 the resolver is accurate, and that steering commands 420 received have not been eeoe corrupted (not shown) by noise or other error sources. A steering simulation model may also be implemented as an additional check of the system.

The autonomous steering system 4900 may be designed to be implemented in parallel with a manual steering system, and can be retro-fitted on to the vehicle 15 310 in a similar manner as the speed control system.

As shown in Figure 49, the existing or production manual steering system has a manual steering wheel 4910 which turns a hand metering unit, or HMU 4918. The HMU 4918 controls a valve 4912 which controls flow of hydraulic fluid to steering i' cylinders 4914, 4916, which turn the wheels (not shown).

A switch 4920 on the HMU 4918 detects off-center position of the steering wheel 4910 as an indication to change to manual control of steering. An operator riding in the cab can merely turn the steering wheel 4910 to disable autonomous steering control 4408.

Under autonomous steering control 4408, the manual steering wheel 4910 in the cab remains centered no matter what position the autonomous steering control has turned the wheels to. There is no mechanical linkage between the steering wheel 4910 and the wheels themselves.

Of course a vehicle 310 may be manufactured without any manual steering system at all on the vehicle if desired. To drive the vehicle manually, the tele-panel _3 410 could be used, or some sort of tele-panel might be plugged into the side of the 3 Mwh 199 i e e r r -'78vehicle 310 to control it without a radio link 4506 in close quarters, for instance.

A jump seat might be provided for an operator in such situations.

Some discussion of the steering model developed may facilitate a better understanding of the present invention.

a. Steering Model The basis for the steering planner is a tricycle steering model shown in Fig.

5.1. This model permits the calculation of the required steer angle independent of the velocity of the vehicle.

tan -1 LC path To use this model, the desired path 3312 must contain the curvature of the path to be followed. The curvature is the inverse of the instantaneous radius of curvature at the point of the curve.

f(s) P C /r 15 Tr o I f(s) p:position curve f'(s)lp:tangent to curve or heads p:curvature at the point This is also equal to the second path derivative at the point.

b. Path Representation Referring to Figures 22-34, the response of autonomous vehicle 310 in tracking a path 3312 depends partly on the characteristics of the path 3312. In particular, continuity of the curvature and the rate of change of curvature (sharpness) of the path 3312 are of particular importance, since these parameters govern the idealized steering motions to keep the vehicle 310 on the desired path 3312. In the case where a path 3312 is specified as a sequence of arcs and lines, there are discontinuities of curvature at the point where two arcs of differing radii meet.

Discontinuities in curvature are troublesome, since they require an infinite Vr

-SI

I U O 4~ DCCf TJn 17482.SPC 3 Ma 1995 acceleration of the steering wheel. A vehicle travelling through such transition points with non-zero velocity will experience an offset error along the desired path 3312.

In general, and as shown in Figure 33, if a posture 3314 is desired as the quadruple of parameters---position 3320, heading 3318, and curvature 3316 y, 0, then it is required that the path 3812 be posture-continuous. In addition, the extent to which steering motions are likely to keep the vehicle 310 on the desired path 3312 correlates with the linearity of sharpness of the path, since linear curvature along a path means linear steering velocity while moving along the path.

Certain spline curves guarantee posture continuity. However, these spline curves do not guarantee linear gradients of curvature along curves. Clothoid curves 2602 have the "good" property that their curvature varies linearly with distance along the curve. Paths composed of arcs and straight lines or clothoid segments have been developed.

15 A path that has discontinuities in curvature results in larger steady state tracking errors. This is particularly the case when the actuators are slow.

The path representation must contain sufficient information to calculate the steer angle 3112 (See Fig. 31) needed to drive the desired path 3312, that is, it must consist of at least the position, heading, curvature and speed. A position on the desired path 3312 has been defined as a posture 3314, and the structure of a posture in the present invention is given by: c. Posture Definition North: desired north coordinate East: desired east coordinate Heading: desired heading Curvature: desired curvature Speed: desired ground speed Distance: distance between current posture and the previous posture.

d. Position Information The position information 3322 is obtained from the vehicle positioning system S DOC :TJM11:#17482SPC 3 h(ai 1995 -1 *77 (VPS) 1000 and is, for example, 71 bytes of data. The structure of the information used to track the desired path 3312 is a subset of the 71 byte VPS output and is given by the VPS short definition shown below.

e. VPS Short Definition Time: gps time North: wgs 84_northing East: wgs 84_easting Heading: compass direction vehicle is moving Curvature: calculated from other variable N_velocity: north velocity E_velocity: east velocity Yaw rate: rate of change of the heading Gspeed: ground speed distance travelled f. Steering Method 15 The steering planner calculates the steer angle needed to follow the desired path. If the vehicle 310 was on the desired path 3312, the steer angle is: ON PATH Fsteer f(Cdesired) tan 1 LC S' If the vehicle 310 is off the desired path 3312, then the steer angle is: OFF PATH lsteer f(Cdesired Cerror).

The method of the present invention used to calculate Cerror is a quintic method. The quintic is a fifth degree polynomial in an error space that defines a smooth path back to the desired path 3312. The degree of the polynomial is defined by the needed data, that is, Cerror and the known end constraints.

DC:T11:17482SPC 3 MAh 1995 4 11 4 kti 841- Polynomial in error space: error 02 3 4 L error(s)= a,+a s +a sa2 +a s '0aS 3 5 1 2 3

L

IIerror''(s)= 2a 2 +6a 3 s+12a 4 s +20a 5 s 110 o s=speed*dt-plan L s at s=O: error position current desired position current actual position error' heading current desired heading current actual heading error t curvature current desired curvature current actual curvature 20 at s=L (L lookahead distance): oerror position 0 error heading 0 error curvature 0 The coefficients of the polynomial error(s) are functions of L, the distance at which the errors go to zero: error =a 0 error'(0) =a, error"(0) 2a error ao aj L+a 2 L 2 aL 3 aL 4 a 5

L

error'(L) =al 2a 2 L 3a 3

L

2 4a 4

L

3 5a 5

L

4 error"(L) =2a 2 6a 3 L 12a 4 L2 20a 5 L3 These five equations are solved symbolically for the coefficients a 0 a, a 5 DM:TIA:17492SPC

LA

7 3 March 1995 82- Then, each coefficient can be easily determined for any reasonable set of boundary conditions.

Once the coefficients of the polynomial are obtained, the error"(s) can be evaluated for some picked_s, which corresponds to a distance along desired path from s=O and is presently defined as: spicked=ground speed planning interval to obtain the correction term: Cerror error"(spicked)curvature to calculate the new steer angle: lsteer tan -1 [(Cdesired Cerror+spicked) L] This calculation is done at each planning interval which is presently .25 sec.

(dt_ plan).

5. Monitor/Auxiliary Referring now to Fig. 43, the monitor/auxiliary functional block(s) 4308 and 15 4310 take care of some miscellaneous functions not performed by the other blocks of the vehicle control system. For instance, start or kill the engine 4616, honk the horn, raise or lower the bed, setting the parking brake on or off, turning the lights on or off, are some of its functions.

The monitor block 4310 also checks the commands that are being sent by or to the other functional blocks on the bus 4314 to see if they are valid. If error is detected, it will signal the shutdown circuits block 4312 and the system will shutdown as discussed below.

6. Safety System (Shutdown) a. Introduction The safety system, including shutdown circuits 4312, (see Figs. 43 and 52) operates to stop the vehicle 310 on detection of a variety of error conditions by setting the parking brake on. This results in the vehicle 310 coming to a safe stop in the shortest distance possible.

Since the parking brake is designed to be normally "set" or and the electronic circuits operate to release it, upon a failure of the electronic controlling DCWCnII:7482.SPC 3 L 1995 X) 0 U o system(s) the power 5216 is turned off to the actuators 5006, so that there is no power to actuate valves, and the parking brake returns to its normal position, called "set." Whenever several erroneous commands are received, or whenever the speed and/or steering simulation models disagree beyond an acceptable tolerance with vehicle sensor outputs 4622 and 4624, are examples of conditions which could result in shutdown of the system. The shutdown system 4312 is an independent and separate subsystem from the other autonomous control subsystems (see Figs. 43 and 52).

b. Shutdown Control :The safety system shutdown circuits 4312 shown in Fig. 43 connected to receive the outputs of the other vehicle control system functional blocks is shown in more detail in Fig. 52.

It is a fail-safe type design. It contains no microprocessor at all. It is all 15 hard-wired, discrete logic.

A feature of the vehicle control system 4312 design is that all functional blocks are capable of detecting errors in the output of the others on the serial bus 4314. So if one of them senses that another is not functioning correctly, it can send a signal to the shutdown circuits 4312 to shut the system down.

For example, the speed and steering blocks each look at their received commands (received via the vehicle manager 4302) to make sure they are valid.

They also make sure that what they are told to execute, that is, what they are requested to command, is within predetermincd bounds. If not, they will act to shut the system down.

The safety system may also be monitoring oil, hydraulic and pneumatic pressures, and temperatures, for instance, making sure they are sufficient to safely operate and control the vehicle.

The safety system includes switches for manual override, including a panic stop 5208, switches on the brake pedal 5202 and steering wheel 5206.

7. Bus Architecture 3 Mareb 1995 The bus 4314 that inter-connccts the vehicle control system functional units 4302, 4304, 4306, 4308, and 4310 is a serial data type common bus implemented in a ring structure using a data packet collision detection scheme.

F. Functional Descriptions/Methods 1. The NAVIGATOR The following is a description of the navigator 406, shown in Fig. 53, titled TASK DIAGRAM. Each of the tasks diagrammed is discussed below.

a. MAIN (executive) In the center of Fig. 53 is a task labelled "main (exec)" 5316. This task 5316 coordinates inter-task communications and performs high level decision making for the navigator 406. One of the primary decisions the task 5316 makes is when to (dis)engage the tracker 5306, based on messages received from the other tasks in the system.

b. MONITOR_VEH STATUS 15 This task 5308 is shown above and to the right of the "main" task 5316. It functions to read the vehicle port 5326, and report vehicle mode changes and navigator-to-vehicle communication state to the "main" 5316 via the EXEC QUEUE 5328. Additionally, the status of the vehicle 310 is written to a global memory structure 5400 (see Fig. 54).

c. SCANNER Shown in the lower right-hand corner of the task diagram Fig. 53 is the scanner task 5310, which provides for communication to the "main" 5316 of data from the obstacle detection system 404.

d. CONSOLE AND CONSOLEPARSER The console 5312 and the console-parser 5314 are shown just below the "main" task 5316 in the task diagram Fig. 53. These tasks were developed as a debugging tool during the development of the system. They display and manipulate navigator 406 states according to user input from a terminal 5302. The console_parser task 5314 also is used to set tracker parameters.

e. GETDIRECTIVES S'D DCC.TMH:17482SPC 3 Mah 1995 K^(0 This task 5320 is shown in the upper left-hand corner of the task diagram Fig. 53. It is part of the host-navigator interface 5330. Messages from the host 402 are received and decoded by this task 5320. Then, depending on the message, the message is either communicated to the "main" task 5316, or to another task. This other task would then formulate an appropriate response from the navigator 406 to the host 402.

f. MSG_TO_HOST This task 5318, shown just above and to the left of the "main" task 5316, formulates messages from the navigator 406 to the host 402 and communicates them to the host 402.

g. VPS_POSITION This task 5322 is shown at the left side of the task diagram Fig 53. The vps..position task 5322 reads the (20 Hz) output from the VPS system 1000. The data is checked for correctness (for example, "checksum") and if correct, it is put 15 into a global memory structure 5400, the position buffer (VPS_POSITION_QUEUE) 5332. The task sends a message to the "main" 5316 whenever a position fault occurs.

h. VPS_POSTURE This task 5324 is shown at the lower left-hand corner of the task diagram.

When the vehicle is tracking, this task maintains the posture buffer (VPS_POSTURE_QUEUE) 5334. The task (5324) monitors the vehicle's position and maintains approximately 50 postures, from the current vehicle position in the direction of travel, in the posture buffer (3000).

i. TRACKER Shown in the upper right-hand comer of the task diagram Fig. 53, the task 5306 reads the current position 5332 and posture buffers 5334. Based on the information read, task 5306 calculates steer and speed correctionls 420. It sends them to the vehicle 310, thereby controlling the vehicle's course.

j. NAVIGATOR SHARED (GLOBAL) MEMORY As mentioned above with regard to the navigator tasks 5300, the navigator -v DC'ml174s2.SPC 3 Match 1995

I

406 has a global memory structure 5400 which the various tasks read/write. This memory structure 5400 is illustrated in Fig. 54.

Referring now to Fig. 54, the tasks are depicted as ellipsoids, with the particular task written inside. The memory 5400 is depicted in the center section of Fig. 54 as a stack of boxes. Unprotected memory is depicted as a single box in the stack of boxes. Semaphore protected memory is depicted as a box within a box in the stack.

An arrow points in the direction of data transfer between tasks and memory.

Therefore, a write to memory from a task is shown as a line with an arrow pointing towards the memory in question from the task. Likewise, a read from memory by a task is depicted by a line with an arrow pointing towards the task in question from S Sthe memory. Where two-way data transfer between task and memory exists, a line with an arrow at both ends is shown.

k. MAIN (EXEC) FLOW CHARTS 15 Figs. 55 and 56A-56D are flow charts of the navigator main or executive task be. 5316.

Referring first to Fig. 55, it is a diagram of the general structure of the main or executive task flow. The following describes several flowcharts associated with the navigator executive task 5316.

Referring to Fig. 55, which is the executive flowchart, it shows of five blocks: block 5502 which is the Start block; block 5504 which is the initialize navigator; block 5506, which is the Pend on Exec Queue; block 5506, which is the executive decisions; and block 5510, which is the act on state.

Flowchart Fig. 55 describes how the executive task 5316 executes its functions beginning at power up (switching on electrical power) of the navigator 406. Upon power up, the executive task 5316 (or Executive) begins at the start block 5502 and proceeds immediately to initialize navigator 5504, where the executive 5316 puts the navigator 406 in a known initial state. The executive then proceeds to the Pend on exec queue 5506 and waits for a message from a number of sources to arrive in its message queue 5328. For example, a typical message D CJII1742.SPC 3 Maih 1995 4 87could be a query for information from the host computer 402.

Upon receipt of a message in the Exec Queue 5328, the executive 5316 proceeds to the executive decisions block 5508. In this block, the executive 5316 sets a series of status flags in a known manner. These flags put the navigator 406 in a known state, particular to the message received.

Once the status flags have been properly set, the executive 5316 then proceeds to the Act on State 5510, where the necessary action is carried out according to the type of instruction received.

Referring now to Figs. 56A-56D, they show the flow of the "executive decisions" block 5508 of the general structure diagram Fig. The various responses which the executive task 5316 can initiate are now described in more detail. There are a known set of messages which are expected within the Exec Que 5328. These messages are shown in detail in Figs. 56A-56D.

Fig. 56A diagrams the organization of Figs. 56A-56D. Figs. 56A-56D describe in deta;l the procedure which the executive 5316 uses to respond to various messages.

o" I Referring to Fig. 56A, the action of the executive 5316 to particular messages is described. Upon receipt of a message to the Exec Queue 5328, the program flow leaves block 5506 and proceeds to block 5602, where the executive 5316 determines 20 if the message is 'NEW_ROUTE_DIRECTIVE'. If the message is 'NEW_ROUTE_DIRECTIVE', then the executive 5316 proceeds to the Act On "NEW_ROUTE_DIRECTIVE' block 5604. Once the action particular to the ',EW_ROUTE_DIRECTIVE' message has been completed successfully, the executive 5316 then proceeds to the Act on State block 5510. Once the action has been completed, the executive 5316 returns to the Pend on Exec Que block 5506 to await another message. If the initial message in block 5602 is not 'NEWROUTE_DIRECTIVE', then the executive 5316 proceeds to block 5606 to determine if the message is 'CHANGE_SPEED_DIRECTIVE'.

The response to messages such as 'CHANGE_SPEED_ DIRECTIVE', R DaI:#17482SC 3 March 1995 'VEH_RESPONDING', NO_VEH_RESPONSE', and VEH_CHECKSUM_ERR', follow a procedure similar to that described for the message 'NEW_ROUTE_DIRECTIVE'. However, the actions performed in the Act On blocks 5604 through 5620 are different for the different possible messages. The various types of valid messages and a brief description of each are: NEWROUTEDIRECTIVE: set the route number for the vehicle to follow.

CHANGE_SPEED DIRECTIVE: command a maximum possible speed for which the vehicle can traverse a particular part of the route.

VEH_RESPONDING: the vehicle is responding to commands properly, set 10 Navigator status flags to Healthy.

NOVEH RESPON' the vehicle is not responding to commands, stop the vehicle.

VEH_CHECKSUM_ERR: the vehicle is not receiving/sensing data correctly, stop the vehicle.

TELE, MANUAL, READY, or AUTO: set the mode of the vehicle IN THE PROPER ORDER.

VPS_TIMOUT: VPS is not sending data, stop the vehicle.

VPS_CHECKSUM_ERROR: the VPS is sending garbled data, stop the vehicle.

VPS_POSTURE_READY: ready to generate path postures.

VPSPOSITIONREADY: VPS data is available.

RA, DCX.'rnl:117482SPC 3 Mad 1995 VPS_POSITION_ALIGN: the VPS system is initializing, do not move the vehicle.

END_OF_ROUTE: the vehicle is approaching the end of the current route, has been reached inform the host computer.

SCAN_READY: the scanning system is ready look for objects in the path.

SCAN_ALL_CLEAR: no objects have been detected in the vehicle path, continue normally.

SCAN_OBSTACLE: an object has been detected on the vehicle path, stop the vehicle.

TRACKER_OFF_COURSE: the vehicle is not following the desired path within tolerance, stop the vehicle.

TRACKER_END_OFROUTE: tracker has reached the end of the path, stop the vehicle.

TRACKER_STOPPED: notify the Navigator that the tracking task has stopped the vehicle.

The responses to the messages TELE', 'MANUAL', 'AUTO', and 'READY' are somewhat different because these messages are related and must be acted upon in a specific order. This has been described above. The program flow for these messages is shown in Figs. 56A and 56B with respect to block 5622-5630.

The response to subsequent message possibilities are depicted by blocks 5632 through 5678 in Figs. 56B through 56D. These responses are similar to those described for the message 'NEW_ROUTE_DIRECTIVE'.

R

7 P 1* o -o oi~c $v 7 DCCnH:#17482.SPC 3 Mach 1995 90 o

I

If the received message is not one of the expected messages, or if the message is garbled, then the executive 5316 is directed to block 5680, where the host computer 402 is informed of the problem. The executive 5316 then returns to the Exec Queue 5506 to respond to the next message in the queue.

Figs. 57A through 57R show the specific procedures which the executive 5316 uses to respond to a particular message. For example, Fig. 57A details how the executive 5316 responds to a 'NEW_ROUTE_DIRECTIVE' message. Once this message arrives in the Exec Queue 5328, the executive 5316 then proceeds to a block 5702 to determine what the message is: in this case, 'NEW_ROUTE_DIRECTIVE'. If the message is 'NEW_ROUTE_DIRECTIVE', the executive 5316 then proceeds to a block 5705 to respond to the message. Otherwise, it proceeds on to a block 5704 to determine if it is valid (one of the other possible messages) or invalid.

Given that the message is a 'NEW_ROUTE_DIRECTIVE', then the executive 15 5316 follows the process described in Fig. 57A to respond to the message. This process is depicted by blocks 5706 through 5714. In this procedure (and in responses to other directives), the executive 5316 checks the states of different tasks within the navigator 406 and reacts to these states in a known, predetermined manner.

The effect of this response is to set a series of status flags, which effect subsequent responses by other tasks in the navigator 406 when the executive 5316 reaches the Act On State 5510. The actual procedures implemented in block 5510 is shown in Fig. 58.

The responses of the executive 5316 to other valid messages are similar to that described for the 'NEW_ROUTE_DIRECTIVE'. The effect of each response to directives first changes a set of flags, which in turn affect the state of the navigator 406. The particular flags set depend on the particular directive. The navigator 406 responds to the changes in these flags when the executive 5316 moves to the Act On State block 5510.

Figs. 58A-58C illustrate the flow of the "act on state" block 5510.

ft o° 3 Murh 1995 b--9 91 The act on state block 5510 is shown in Figures 58A-58C. Figure 58 shows the interrelationship of Figs. 58A-58C, where each of these three figures depict a portion of the Act On State block 5510.

Once the executive task 5316 has set the appropriate flags in response to a particular Exec message, the executive 5316 then proceeds to send messages to the appropriate tasks or entities which must be informed of changes to the navigator 406 system as a result of the Exec message.

For example, when the executive task 5316 leaves the executive decisions 5508, and first enters the act on state block 5510, it checks to see that the status is set such that the vehicle is ready for autonomous mode (for example VPS is ready, vehicle is communicating properly, a proper route has been commanded, and the vehicle is ready for auto mode). See block 5802. If one or more of these conditions is not met, then the Exec returns to wait for another valid message. If all of these conditions are met, then the executive 5316 checks to see that the path generator 5804 is operating. If so, then the executive 5316 proceeds to start the other systems required for autonomous operation.

If the path generation system is not operating, then the executive 5316 task sends the message 'VPSPOSTURE_ENGAGE' to the Vps Posture Queue 5334, in order to start the path generator. The executive task will then return to the Pend on Exec Queue 5506, to wait for another directive so that proper operation of the vehicle 310 is ensured.

3 Marhi 1995

Claims (24)

1. A system for a vehicle for enabling said vehicle to track a preset path including: means for executing vehicle commands that cause said vehicle to attempt to follow said preset path; means for periodically calculating errors in following said preset path; means for adjusting said vehicle commands based on said calculated errors to reduce said calculated errors; wherein said means for adjusting includes means for constructing a smooth path back to the preset path using a quintic polynomial.

2. The system of claim 1 wherein said means for adjusting further comprises means for compensating for vehicle response characteristics, said vehicle response characteristics including latency of vehicle control commands, system response and vehicle dynamics. S 15 3. The system of claim 2 wherein said vehicle dynamics include vehicle-ground interaction, slip angle and under/over steering.

4. The system of claim 1 wherein said means for adjusting further comprises means for compensating for sensing and actuation timing characteristics of said system.

5. The system of claim 1 wherein said means for adjusting further comprises means for varying a look-ahead distance based on the speed of travel of said vehicle.

6. The system of claim 1 wherein said means for adjusting said vehicle commands to reduce said calculated errors further comprises means for adjusting control of the vehicle using an optimal control system. 25 7. The system of claim 1 wherein said vehicle commands include steering control aspects and speed control aspects, and wherein said means for executing vehicle commands causing said vehicle to attempt to follow said preset path comprises: means for executing said steering control aspects; and means for executing said speed control aspects; DCC:TGI:#17482.RSZ 22 October 1997 I 90 wherein steering and speed control of said vehicle are decoupled,

8. A method for navigating a vehicle for enabling said vehicle to track a preset including comprising the steps of: executing vehicle commands that cause said vehicle to attempt to follow said preset path; periodically calculating errors in following said preset path; and adjusting said vehicle commands based on said calculated errors to reduce said calculated errors; wherein said step of adjusting includes the step of constructing a smooth path back to said preset path using a quintic polynomial.

9. The method of claim 8 wherein said step of adjusting further comprises the steps of compensating for vehicle response characteristics, said vehicle response characteristics including latency of vehicle control commands, system response delay and vehicle dynamics.

10. The method of claim 9 wherein said vehicle dynamics includes vehicle-ground I :interaction, slip angle and under/over steering,

11. The method of claim 8 wherein said step of adjusting further comprises the step of compensating for sensing and actuation timing characteristics of said system.

12. The method of claim 8 wherein said step of adjusting further comprises the step of varying a look-ahead distance based on a speed of travel of said vehicle.

13. The method of claim 8 wherein said step of adjusting said vehicle commands based on said calculated errors to reduce said calculated errors further comprises the step of adjusting control of the vehicle using an optimal control system.

14. The method of claim 8 wherein said vehicle commands include steering 25 control aspects and speed control aspects, and wherein said step of executing vehicle commands causing said vehicle to attempt to follow said preset path comprises the steps of: executing said steering control aspects; and executing said speed control aspects; wherein steering and speed control of said vehicle are decoupled. ff C DCC:TU:#17482.RS2 22 October 1997 I -91 A system according to claim 1 for navigating a vehicle along a predetermined route comprising: means for storing route data representing said predetermined route, said route data including a plurality of contiguous path segments, each of said path segments connecting two nodes; means for storihg path data for said predetermined route, said path data including postures of said vehicle along each of said path segments; means for retrieving from said route data storing means route data representing said predetermined route; means for retrieving from said path data storing means path data for said retrieved route data; means for generating a path from said retrieved path data, said path representing at least a portion of said predetermined route; means for associating vehicle commands with said path; means for executing said associated vehicle commands to cause said vehicle S" to follow said path; means for periodically calculating errors in following said path; and means, responsive to calculated errors, for adjusting the execution of said associated vehicle commands to reduce said calculated errors so that said vehicle tracks said path with a high degree of accuracy.

16. The system of claim 15 wherein said means for periodically calculating errors in following said path comprises: means for determining an actual position of said vehicle; means for determining a desired position of said vehicle; and 25 means for comparing said actual position with said desired position thereby calculating said errors in following said path.

17. The system of claim 15 wherein said means, responsive to said calculated errors, for adjusting execution of said associated vehicle commands to reduce said calculated errors so that said vehicle tracks said path with a high degree of accuracy comprises: DCC:TG:#17482.RS2 22 October 1997 92 means for dynamically determining a correcting steering angle so that said correcting steering angle causes said vehicle to steer from an actual position to a desired position.

18. A method according to claim 8 for navigating a vehicle along a predetermined route comprising the steps of: storing route data representing said predetermined route, said route data including a plurality of path segments, each of said path segments connecting two nodes; storing path data for said predetermined route, said path data including postures of said vehicle along each of said path segments; retrieving route data representing said predetermined route; retrieving path data for said retrieved route data; generating a path from said retrieved path data, said path representing at least a portion of said predetermined route; associating vehicle commands with said generated path; executing associated vehicle commands to cause said vehicle to attempt to follow said generated path; periodically calculating errors in following said generated path; •adjusting in accordance with said calculated errors, execution of said associated vehicle commands to reduce said calculated errors so that said vehicle tracks said generated path with a high degree of accuracy; C

19. The method of claim 18 wherein said step of periodically calculating errors in following a generated path comprises: :determining an actual position of said vehicle; 25 determining a desired position of said vehicle; and comparing said actual position with said desired position; thereby calculating said errors in following said generated path. The method of claim 18 wherein said step of adjusting in accordance with said calculated errors, execution of said associated vehicle commands to reduce said U DCC;TG:#17482.RS2 22 October 1997 AIT 0, 93 calculated errors so that said vehicle tracks said generated path with a high degree of accuracy comprises: dynamically determining a correcting steering angle so that said correcting steering angle causes said vehicle to steer from an ,ctual position to a desired position.

21. The method of claim 18 further comprising the steps of: detecting obstacles in said vehicle's direction of travel; and avoiding collision with said obstacles.

22. The method of claim 21, wherein said step of avoiding collision with said obstacles comprises: stopping said vehicle to prevent a collision with said obstacles.

23. The method of claim 21, wherein said step of avoiding collision with said obstacles comprises: generating a second path around said obstacles.

24. The method of claim 21, wherein said step of avoiding collision with said obstacles comprises: stopping said vehicle to prevent a collision with said obstacles; and °generating a second path around said obstacles. o 25. The system of claim 15, further comprising: o means for detecting obstacles in said vehicle's direction of travel; and means for avoiding collision with said obstacles.

26. The system of claim 25 wherein said means for avoiding collision comprises S°means for stopping said vehicle to prevent a collision with said obstacles.

27. The system of claim 25 wherein said means for avoiding collision comprises means for generating a second path around said obstacles.

28. The system of claim 25 wherein said means for avoiding collision comprises: means for stopping said vehicle to prevent a collision with said obstacles; and means for generating a second path around said obstacles.

29. The system as claimed in claim 1, 15 or 17 substantially as hereinbefore described with reference to the accompanying figures. DCC:C 17482.RSI 13 May 1997 94 The method as claimed in claim 8 or 18 substantially as hereinbefore described with reference to the accompanying figures. DATED: 13 May 1997 CARTER SMITH BEADLE Patent Attorneys iuf the Applicant: CATERPILLAR INC. 4 0* DCC;CJZ.#17482.RS1 3My19 13 May 1997 ABSTRACT A system (3100) is disclosed which enables a vehicle (310) to track a preset path (3312). Included in the system are means for executing vehicle commands (3116, 3118) causing said vehicle (310) to attempt to follow the preset path (3312) and means for periodically calculating errors in the path (3312). Means is therefore provided to adjust the vehicle commands (3116, 3118) to reduce the calculated errors so that the vehicle (310) tracks the path (3312) with a high degree of accuracy. A method for a vehicle to track a preset path is also disclosed. ee DCCTnlI:N17482SPC 3 Mnh 1995 rr9