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

Description

'I

1- P/ot=o11 Regulafion 3.2

AUSTRALIA

Patents Act 1990 COMPLETE SPECIFICATION FOR A STANDARD PATENT *0 0 o .000 100000 O 0 0 0*0* 0* 0* 0 0 0 00*0.10 0

ORIGINAL

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.VWEINBECK, Jay H. WEST, William L. WHIIAKER, 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 -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 There is presently under development a terrestrial position determining system, referred to as the global positioning system (GPS), designated NAVSTAR by the U.S. Government. In this system, a multitude of orbiting satellites will be Sused to determine the terrestrial position of receivers on the Earth. In the planned Ssystem, there will be eight orbiting satellites in each of three sets of olrbits, 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 i navigation is also known in the field.

Systems exist which rely on vision based positioning. For instance, vision DOTJH.M1:#717517.SPC 10 Mauh 1995 -3based positioning is used in the Martin Marietta Autonomous Land Vehicle, as descibed 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

I

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 present invention, there is disclosed a system for generating a series of paths to enable a vehicle to traverse a predetermined route by tracking said paths, the system including: DCCTHI1:TG#:7S17SFC 10 Math 1995 -4means for storing route data representing beginning and ending points on a predetermined route; means for storing sets of compressed path data for each predetermined route; means for successively retrieving sets of compressed path data associated with a predetermined route; and means for successively generating a continuous path from each of said sets of compressed path data.

In accordance with a second aspect of the present invention, there is disclosed a method for generating a series of paths to enable a vehicle to traverse a predetermined route by tracking said paths, the method comprising the steps of: storing route data representing beginning and ending points on a :predetermined route; storing sets of compressed path data for each predetermined route; ii o o successively retrieving sets of compressed path data associated with a 15 predetermined route; and successively generating a continuous path from each of said sets of compressed path data.

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

The present invention includes novel systems for determining paths or roads for 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. And it includes systems for accurately tracking these paths. The present invention also possesses the capability to travel autonomously without preset paths from beginning to ending points.

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 DMCJL TG:#17SI7.SPC O 199 m ble for hazardous or remote work sites, such as at the site of a nuclear accident, -xample.

The present invention provides the capability for monitoring of vehicle and equipment control and mechanical systems, such as oil, air, and/or hydraulic pressures and temperatures. It provides for the indicating of a problem in any one of its systems, and for the scheduling of regular and emergency maintenance.

The present invention also provides for efficient scheduling of work and dispatching of vehicles and equipment at an autonomous work site, for instance, by providing a host processing system at a base station.

The host processing system coordinates various positioning and navigation tasks. It also acts as the supervisory interface between the autonomous system and 0 its human managers.

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

15 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 different types of vehicles, such as farm tractors or automobiles.

There is also disclosed novel systems for ensuring safe operation of a manned or unmanned autonomous vehicle. These may be embodied in manual override features, failsafe features, error checking features, supervisory features, obstacle detection and ivoidance, and redundancy features.

The vehicle positioning and navigation system disclosed includes systems which provide for a hierarchy of control such that manual control when asserted takes precedence as a safety feature.

The vehicle positioning and navigation system includes systems which provide for a safe failure mode. This insures that shouid a system or subsystem malfunction, or drift unacceptably out of calibration, the system will detect this condition and bring the vehicle to a stop. The brake system, as an example, is designed with the parking brakes normally set. Any failure or loss of power will result in the brakes DCC.TJII:TG:#17 .$17.SPC 10 March 1995 Ii ;1 i -6- 0 eO 0000* being returned to their normal set mode, stopping the vehicle in the shortest distance possible.

The vehicle positioning and navigation system includes systems which provide for error checking to detect, for instance, garbled messages or out-of-limit commands, which may indicate system malfunction.

The vehicle positioning and navigation system includes systems which monitor and supervise the operation of other systems and sub-systems to assure proper and safe operation. This may include checking actual performance against a multi-state model of ideal performance of the vehicle and its systems.

The ,ehicle positioning and navigation system includes systems for detecting obstacles in the vehicle path and enabling the vehicle to successfully avoid colliding with those obstacles, thereafter returning to the path. Such obstacles may be fallen rocks, people, other vehicles, and so on.

There is also disclosed novel scanning systems for use in detecting obstacles.

These may include scanner systems designed around laser, sonar, radar, or other electromagnetic or acoustical wave apparatus.

The vehicle positioning and navigation system includes redundancy of functional units, as backup and/or as checks on the system operation.

The navigation portion may control flexibility by providing for at least three 20 modes of operation. These modes include a manual mode, a remote control or "tele" mode, and a fully autonomous mode. The "tele" mode may be over a remote radio, or cable, with the controlled vehicle in the line of sight of the "tele" operator. There is also a transitional or intermediate "ready" mode, used when going between certain other modes.

This facilitates on-board, tele, and host displays for status and control information and manipulation.

There is also disclosed novel vehicle control sub-systems, including sp.ed control, steering control, and shutdown systems.

The system disclosed is able to achieve some of its advantages by providing 303 for vehicle control which serves to decouple speed and steering control so that they

I

E

r ii c a S. Of) 0c 8 0 000* DCcr:T:#117S7SPC0 10 M=b 190 a -7may be controlled separately. This simplifies the problems that may be associated with accurate path tracking, for instance.

There is also disclosed a novel global memory structure for communicating data between plural processing units, or tasks, and/or processors. This communication structure enables real-time, multi-tasking control of vehicle navigation.

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 0 The present invention is better understood by reference to the following o V 4 drawings in conjunction with the accompanying text.

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

Fig. 2 is a diagrammatic representation of the interrelationships between the navigator, the vehicle positioning system (VP and the vehicle controls.

Fig. 2A is a context diagram, illustrating the various elements and their interrelationship in an autonomous control system according to the present invention.

Fig. 3 is a diagram of route definitions using nodes and segments according to the present invention.

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

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

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

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

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

DC nTII'T:#17517.SPC 10M 199S -8- Fig. 9 is a diagram showing the replanning of a path.

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

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

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

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

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

Fig. 15 is a diagram showing how an error vector including curvature is computed w'th the vehicle path included.

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

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

Figs. are navigator data flow summaries.

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

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

Fig, 20 is a diagram of selected scan lines in an embodiment of a laser 20 scanner system of the present invention.

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

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

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

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

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

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

DCXMI'TTO:#1717.SPC 10 Muh 1995 jp r- i -L -9- Fig. 27 is a representational diagram of an embodiment of the speed control system of the present invention.

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

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

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

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

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

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

15 Fig. 34 is a diagram showing navigator tasks.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENT .0 TABLE OF CONTENTS SI. General Overview II. Navigation A. Overview 0.4. 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 DC.'CnHTLO:#171.SPC 10 MarIh 1995 '0 o o+P o o,p o o iloo o r~ a 09 b r r o 0 9 o a a ooau~ 9 9 Summary B-splines 2. Route Creation and Storage a. Introduction b. Route Definition c. Navigator Route Usage 3. Posture Generation 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 20 position is knov-n, 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) DIocIol:17S17.spc 10 Mad 199 i nui--i-(k=ir -w~iiw~~li 11 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 vehicle 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 uimanned vehicles.

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

0 The present invention makes use 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. Bsplines or clothoid curves may be used to model the actual path the vehicle is to navigate. Using modelling or representational techniques provides for enhanced data communication, storage and 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 t':!reby 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.

DC.THIi:T':if17517.SPC 10 Mach 1995

'I

12- 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 required to move, stop, and steer the vhicle. 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 S0 0 of the system and vehicle components. If detected, the system provides for fail-safe 2 shutdown, bringing the vehicle to a stop.

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

15 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 the vehicle; and a manual mode, where a human operator sitting in the cab can 0: take control of the vehicle and drive it manually.

In the autonomous mode, obstacle detection is critical, and is provided for in the navigation system. 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 approximately 30 mph. safely.

The vehicle positioning system (VPS) is a positioning system based on the incorporation of Inertial Reference Unit (IRU) Data, Odometer Data and Global Positioning System (GPS) Data. j DCCTnilO:1717SIPC 10 MWilAt 1995 '4 13 The VPS incorporates these three subsystems with extensive innovative methodology to create a high accurate position determination system for moving or j stationary vehicles, or any point on the Earth.

The GPS comprises a space segment and a land or atmospheric based processing system. The space segment includes 24 special purpose satellites (not yet fully implemented) which are being launched and operated by the U.S. Government.

These satellites continually transmit data to the Earth that can be read by a GPS receiver. The GPS receiver is part of a processing system which produces an estimate of the vehicle position based on the transmitted data.

When multiple satellites are in view, the GPS receiver can read each satellite's navigation messages and determine position based on known triangulation methods.

The accuracy of the przess is in part dependent on how many satellites are in view.

The IRU comprises laser gyroscopes and accelerometers which produce position, il velocity, roll, pitch and yaw data. The IRU combines this information into an 15 estimate of vehicle position. The odometer produces data on distance travel that is incorporated with the IRU data.

A base station provides a geographic proximate reference point and reads the data transmitted by each satellite. Using the transmitted data, the base station makes improvements in accuracy on the estimate of the vehicle position.

The position of any satellite can be predicted at the current time or any future time. Using that information, the GPS receiver can determine the optimum satellite constellation.

Vehicle "wandering" may also be reduced by using the path history of the vehicle to statistically determine the accuracy of future estimates of position.

transmitted from the satellite is. Included in these methods are techniques to compensate for data with error.

II. Navigation I A. Overview i In considering implementation of an autonomous navigation system, there are DCff-:TiO:#1"517.SPC 10 March 199 Dc.nfansIp lOui f 14 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.

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) in part, and discussed fully in the Australian patent referred to above.

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 06.'0 0(steering, speed, braking, and so on), to get there?". This is in the domain of the *0 "vehicle controls subsystem of the navigation system, also discussed in the above 0.4 Australian patent.

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

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

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

There are, for instance, some mining sites where visibility is so poor that j work is not possible 200 days of the year. There are other areas which may be 1 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. 1, with DCC.T:T::I17S17.SPC1 10 M 1995 r I ~u Ir e 15 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 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. 2 and 2A, Navigation of the AMT (Autonomous Mining Truck) encompasses several systems, apparatus and/or functions. The VPS o (Vehicle Positioning System) 1000 subsystem of the overall AMT system as 15 described above, outputs position data that indicates where the vehicle is located, including, for example, a North and an East position.

S. Referring now to Figs. 2 and 2A, 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) 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.

DCC.HO:#I17S17.SPC 10 Mch 99S .n-lr.~.rr I_ _ii i 11 16- 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. 16, titled the context diagram, and Figs. 18A-18D, definitions of the communications, which are shown as circles with numbers in them, are provided below: 502. Host commands queries: Commands given by the host to the vehicle manager. These commands could be of several types: initiate/terminate; supply parameters; emergency actions; and 8 directives.

15 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: s This is streamed information provided by the VPS system.

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

0 o 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.

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

DOTI:TO:17517.SPC 10 March 1995 17 Referring to Fig. 2A, 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 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 S. 15 interface for a human manager.

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

Another factor in determining the particular location of elements of the system of the present invention, is the time-criticality of autonomous navigation. The navigation system must continually check its absolute and relative locations to avoid unacceptable inaccuracies in following a route. Ihe 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.

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 DOMCn UlTi:«17517.SPC 10 Muchi 199S ,Er -r i -L ~TL' DCCTJH:TG:#17517.SPC March 1995S L- 18 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.

Route planning/path generation is discussed below. For a discussion of path tracking and obstacle handling, refer to Australian Patent No. 642638.

0: B. Route Planning/Path Generation 00 0 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 00 00 o o S 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 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 i failure to achieve the required steering motions exactly, results in steady state offset 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 DCOC'TJiTM:#175S17.SPC 10 March 1995 m I '8 I I F I I •Ir s. i 19vehicle 310.

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

Referring now to Fig. 1, 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 j the host 402. The host 402 determines the vehicles' destinations, which is called "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 j to a desired destination. In general, a route can be thought of as a high-level i 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 i operations (which may include starting the vehicle 310, releasing the brake 4406, engaging the transmission 4610, accelerating to the posted speed limit, turning the 20 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 t0; "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 i circular segments. Linear segments (lines) art defined by two nodes. Circular segments (arcs) are defined by three nodes.

DCxn.TOU:#17S17.SPC 10 itod 1995 1 I 20 "ostures" are used to model parts of a route, paths and nodes for instance.

Postures ma:, 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 maximum 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 analytical, 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 I 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. 3 which illustrates these concepts of the present invention).

The aforementioned method of defining routes was developed for memory 20 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 are many intersecting roads 326. A route programmer would define nodes at the intersections, and segments to define the roads between the intersections. Routes 25 would therefore be determined by the roads and intersections. There will however, be many ways to get from point A to point B3 (many routes) with a fixed set of intersections and roads.

The path-tracking method of the present invention (discussed below) uses 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 DCnTJI.T#1517.SPC 10 Much 199 21 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 7y the present invention.

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

The navigator 406 functions to generate a path by translating "route 1" into a series

A

s" o 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. f 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. I A LINKER then takes given path segments and generates a linked-list of 20 control points, allowing for flexibility and efficiency. Path segments are shared by different routes, as is shown in Fig. 3.

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.

a. CLOTHOID PATH SEGMENTS As discussed above, part of the navigation problem addressed and solved by DCTIII.Ta:1717sPC 10 Much 199S C, 1 22 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 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 15 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 20 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 Y-.

1 city of steering.

One method used by the present invention, is to cornmyK paths as a sequence 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 linearity 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 DC=.H.:TOG: 1717.SPC 10 M h 199S -23 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 parts 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 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: S0(s) (EQ.1) d x (s) d(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. J c. Clothosd Curves S 20 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) where k is the rate of change of curvature (sharpness) of the curve and subscript i denotes the initial state. A clothoid curve segment 2602 is shown in Fig. 7.

Given an initial posture, sharpness of the clothoid segment and the distance along that segment, position, orientation and curvature at any point are calculated as follows: s O(s) i c()d s2 c s (EQ. 4) DOCN1n:T017517.SPC 10 Much 1995 24 s s k 2 x(s) z f cos0( cos 2 c i)d x i (EQ. s k y(s) Yi sine()dj f 2 Oi}dp Yi (EQ. 6) d. Generation of a Posture-Continuous Path Practical navigation problems require composite paths whose range and 10 complexity cannot be satisfied by a single clothoid segment. Most paths require multiple segments that pass through a sequence of objective points.

ii Existing Methods Hongo et al., "An Automatic Guidance System of a Self-Controlled Vehicle The Command System and Control Algorithm", Proceedings IECON. 1985, MIT 15 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 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=0. Also, by rotating the reference frame by the amount of the initial orientation, Oi=O; only a straight forward approximation of fs sin(k 2 )d is left.

DOCTnHIt:#1171.SPC 10 MuNc 1995 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 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. 4, 5 and 6, the first step is to derive a sequence of unique postures 2302, 2304, 2306, 2308, 2310 from the objective points. The a 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 1 0 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 at least two clothoid curve segments. However, the general problem cannot be 20 solved with two clothoid segments because if k i 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, s2, s3, even though the sign of the first and the last clothoid segments satisfies the curve DCCnHI.TO#T17S17,SPC 10 Marh 1995 iA rj~ ii 26 location requirement.

3. There are four variables in the combination: k, s2' s3. It is possible to find a unique solution satisfying the following four equations which describe the mathematical relationship between the starting aiid the ending postures.

C f C +ks -s 2+S (EQ. 7) Of=0e t 4~ C 4

I

I CC I 4

CC

girt It *41 C 60 C i(S 1 S2+ k(s 1

S

2 -S 2

S

3 +S 3

S

1 k( 2_S2 +S2) rT Cbsei 1 )d r cosO 2 ()d 0 (EQ. 8) x f= x

S

fo cos0 3 )d (EQ. 9) 1 i41 Yf =Yj sinej( )d sin 2 )d 0 0 f sin0) 3 (g)d 0 (EQ. DCC11H,:TU:#17517.SPC 10 MsAvh 199S .1e. m

B

a -~i

C

27 where o+ ci kg2 00 *0 0 SP 99*0 0 0 6 4 E I CS +s 2 (c ksi k 2 9+ cis I s i )c i ksi) 2 03( i ci(s+s 2 1 s 2 1 2 -s 2 ci k(s-s2)} 2 Referring now to the method shown in Fig. 8. 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

I

~1 E

*I

I

i i; DO HIT:#17517.SPC 10 Marh 1995 'Le I Ci ju* 4 28 oi cc *4 *9 C...r 04444 piecewise 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 15 current position. Replanning decomposes to two sub problems: 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. 9, which graphically illustrates replanning a path 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 Pil+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 (k,s)m we can find a new posture-continuous path k+ 1 sk+ 1 (K k+ 2 s using a small DOCTJl:TO:#1717SPC 10 MatI 1995 -i 29 perturbation from known sk+1), (kk+ 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 objective points is not necessary as with arcs and zero curvature clothoids. Further, S, 15 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 S 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 well known by mathematicians and those familiar with j |25 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 DCC:TJII:T:#17S17.SPC 10 Mach 1995 30 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, reF-lting in a path which is inherently easy to track with the aforementioned embodiments of the present invention. Fig. 10 shows an example of B-spline curves.

2. Route Creation and Storage a. Introduction: o o s.3o00 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 .l 15 segments are the, t,'ed to the stored driven data, and organized into routes per the i aforementioned procedure.

An application on an APOLLO Computer (now HEWLETT PACKARD 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 20 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 i may be generated dynamically. i In mining applications, generally a site 300 is surveyed and roads are pre- DC'ITlTa 17517.SPCMc 10 Muc 1995

F-

31 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 node and segment data into an array called the "routeData" array. Each record in the array contains the following information: 1. Type of item (that is, node, linear segment, 15 circular segment, end of route marker). t 2. If node item, define the north and east 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 of the sequence alternate between segments and nodes until the sequence Is DCr.TJ 'U:N17S17.SPC 10 Marh 199S I I U I; ~l Llll- -1 (:.ll.ili.i i liii. -~a~-Rhi' 32-

I

o 4Q ooo a 0 00 0o 0 0 0 coo o 0 0 i 0 040 0 o 0 00 0000 0 00 00 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. he 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 10 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.

15 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 routeSpet 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 vps_posture 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.

2. The perpendicular distance from the vehicle position to the segment is DICC'I1JHlT:177.SP 10 Mrch 199S -33 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.

5. 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 5324 then uses the description of the route (lines, arcs and speeds) to generate posture at one meter intervals. The task 5324 generates a lpredefined distance of postures plus a safety margin and puts the postures into a :E buffer 3000. To generate a posture which is one meter from a given posture the Svps_posture task 5324 uses the following procedure: 15 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.

i 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 i a pointer to the next segment and repeat steps 2 and 3.

else, return the generated posture.

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

25 As the autonomous vehicle 310 moves along the posture in the buffer 3000, T 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 and moving a pointer 3002 in the buffer 3000 to the nearest posture. The posture DCMIV"I."lf :#17517.SC 10 Mawh 1995 j I 1. 10 -34buffer 3000 is constructed as a ring which is traversed in the clockwise direction (see Fig. 11, 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 clockwise 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 vps_posture task 5324 ceases to generate posture and 10 informs the executive 5316 that it has reached the end of the route.

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

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

S. 15 3. Posture Generation 4 too 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.

20 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 specified number of postures ahead of the vehicle 310. The number of postures DMc I.:hTO:#IS171.SC 10 M&Mh 199S j *i Vr~ -9R

A-_

35 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.

o 099 @9 9 94 9 0r4*9 D~C.Tn:T:#17s17.SPC1 10 Mch 1995 L 1 1~ L

Claims (10)

1. A system for generating a series of paths to enable a vehicle to traverse a predetermined route by tracking said paths, the system including: means for storing route data representing beginning and ending points on a predetermined route; means for storing sets of compressed path data for each predetermined route; means for successively retrieving sets of compressed path data associated with a predetermined route; and j means for successively generating a continuous path from each of said sets 10 of compressed path data. a o 4

2. The system as claimed in claim 1, wherein said compressed path data for each predetermined route is comprised of nodes, lines, and arcs, and said means for successively generating a path from each of said sets of compressed path data includes means to select a series of nodes, means to connect said series of nodes 15 using lines, arcs, or both, and means to compile a set of points along said connected nodes.

3. The system as claimed in claim 1, wherein said compressed path data for each j predetermined route is comprised of B-splines.

4. The system as claimed in claim 1, wherein said compressed path data for each 20 predetermined route is comprised of clothoid curves. A method for generating a series of paths to enable a vehicle to traverse a predetermined route by tracking said paths, the method comprising the steps of: storing route data representing beginning and ending points on a predetermined route; storing sets of compressed path data for each predetermined route; successively retrieving sets of compressed path data associated with a predetermined route; and successively generating a continuous path from each of said sets of compressed path data.

I

6. The method as claimed in claim 5, wherein said compressed path data for DKC ITG:#17S17.SPC 10 M h 1995 L- -37- each predetermined route stored in step is comprised of nodes, lines, and arcs, and said step of successively generating a continuous path from each of said sets of compressed path data includes the steps of: selecting a series of nodes; connecting said series of nodes using lines, arcs, or both, and; compiling a set of points along said connected nodes.

7. The method as claimed in claim 5, wherein said compressed path data for each predetermined route stored in step is comprised of B-splines.

8. The method as claimed in claim 5, wherein said compressed path data for 10 each predetermined route stored in step is comprised of clothoid curves. 0 '09

9. The system as claimed in claim 1 substantially as hereinbefore described with reference to the accompanying figures.

10. The method as claimed in claim 5 substantially as hereinbefore described with 0 °reference to the accompanying figures. a" DATED: 10 March 1995 00 00 0 0 CARTER SMITH BEADLE 00 Patent Attorneys for the Applicant: CATERPILLAR INC. DOCCT1:TO:S:#177.SPC 10 Math 199S ABSTRACT A vehicle (310) may traverse a predetermined route by tracking a path. A system (406) is disclosed herein which generates a series of paths which may be tracked by the vehicle (310) to follow the predetermined route. In the system (406), data is stored representing the beginning and ending points of the predetermined route. Also stored are sets of the compressed path data for the route. This compressed path data may be retrieved by the system (406) and used to generate a continuous path. The compressed path data may be comprised of B-splines or clothoid curves or as nodes, lines and arcs. r1 4 oa a o o 0 4 4o o A Sor or 04 4 4a i. a e •0 0 0 o a 0 oo o ft o r or a r DCC 7nII'Ti1717.SPC 10 MAch 199S I'y