Integrated vehicle positioning and navigation system, apparatus and method

Background of the invention

1. Field of the invention

The present invention relates to positioning systems, and more particularly to a positioning system and method for determining the terrestrial position of an autonomous vehicle at or near the surface of the earth.

Second Related Technology

Various national governments, including the United States of America (USA), are currently developing a terrestrial positioning system, commonly referred to as the Global Positioning System (GPS). In a GPS, a number of satellites are placed in orbit around the planet Earth. The GPS satellites are designed to transmit electromagnetic signals. From these electromagnetic signals, the absolute terrestrial position (position with respect to the center of the earth) can be absolutely determined by any receiver at or near the surface of the earth.

The US government has called their GPS "NAVSTAR". The NAVSTAR GPS is declared operational by the US Government in 1993. In addition, the Government of the Union of Soviet Socialist Republics (USSR) is developing a GPS known as "GLONASS", which is essentially similar to the NAVSTAR GPS.

The NAVSTAR GPS is designed to have four orbiting GPS satellites in each of six separate orbits. A total of 24 GPS satellites will be in orbit at any given time with 21 GPS satellites in operation and three GPS satellites serving as replacements. The three GPS satellite orbits will have mutually perpendicular planes relative to the earth. The GPS satellite orbits will be neither polar or equatorial. In addition, the GPS satellites will run around the Earth once every 12 hours.

Using the NAVSTAR GPS, the relative position of orbiting GPS satellites with respect to any earth receiver can be determined from the electromagnetic signals. The relative position is generally referred to as a "pseudorange". In addition, the relative position of two methods can be calculated.

One method is to measure the propagation time delays between the transmission and reception of the propagating electromagnetic signals. In the NAVSTAR GPS, the electromagnetic signals are continuously encoded with the time at which the signals are sent from the GPS satellites. It is clear that one can record the recording time and subtract the coded transmission time to derive time delays. From the calculated time delays and from knowing the speed at which electromagnetic waves travel through the atmosphere, pseudoranges can be accurately deduced. Pseudoranges calculated using the foregoing method are referred to as "true" pseudoranges in the context of this document.

Another method takes into account satellite position data encoded in the electromagnetic signals emitted by the orbiting satellites. Almanac data relating to the satellite position data of the NAVSTAR GPS is publicly available. Reference to this almanac data with respect to the signals encoded in the electromagnetic signals permits accurate derivation of the pseudoranges. Pseudoranges calculated using the foregoing method are referred to as "estimated" pseudoranges in the context of this document.

However, with reference to the previous method of deriving estimated pseudoranges, it should be noted that the satellite position data on the GPS satellite is updated to that hour only once per hour. As a result, the accuracy of the estimated pseudorange decreases with time after every hour until the next hour when a newly estimated pseudorange is calculated using the updated satellite position data.

Further, while knowing the relative position of at least three of the orbiting GPS satellites, the absolute terrestrial position (ie, length, width, and altitude with respect to the center of the earth) can be calculated by any earthcatcher via simple geometric theories involving triangulation techniques. The accuracy of terrestrial position estimation depends in part on the number of orbiting GPS satellites that are being received or received. The use of more GPS satellites in the computation can increase the accuracy of terrestrial position estimation.

Conventionally, four GPS satellites are sampled to determine each terrestrial position estimate because of errors contributed by circuit clock differences between the earth receiver and the various GPS satellites. Clock differences can be several milliseconds. If the earthmovie's clock were synchronized with that of the GPS satellites, then only three GPS satellites would need to be recorded to accurately detect the earthmoving location.

In NAVSTAR GPS, electromagnetic signals are transmitted continuously from all GPS satellites on a single carrier frequency. However, each of the GPS satellites has a different modulation scheme, allowing discrimination of the signals. In the NAVSTAR GPS, the carrier frequency is modulated using a pseudo random signal which is unique to each GPS satellite. Consequently, the orbiting GPS satellites can be identified on the NAVSTAR GPS as the carrier frequencies are demodulated.

Furthermore, the NAVSTAR GPS provides two modes of operation for modulating the carrier wave using pseudorandom number (PRN) signals. In an operating state referred to as coarse / capture mode (C / A), the PRN signal is a gold code sequence with a clock rate of 1.023 MHz. The gold code sequence is a well-known conventional pseudorandom sequence in the art. A chip is an individual pulse of the pseudo-random code. The chip rate of a pseudorandom code sequence is the rate at which the chips are generated in the sequence. Thus, the chip rate is equal to the code repetition rate divided by the number of terms in the code. Accordingly, with respect to the coarse / seek operation state of the NAVSTAR GPS, there are 1023 chips in each gold code sequence, and the sequence is repeated every millisecond. The use of the 1,023 MHz gold code sequence from four orbiting GPS satellites allows the terrestrial position of a terrestrial receiver to be determined with an approximate accuracy of within 60 to 300 meters.

The second modulation mode in the NAVSTAR GPS is generally referred to as the "precise" or "protected" (P) mode of operation. In the protected operating state, the pseudo-random code has a chip rate of 10.23 MHz. In addition, the protected mode sequences are extremely long so that the sequences do not repeat more than once every 267 days. As a result, the terrestrial position of any terrestrial receiver can be determined within an approximate accuracy of 16 to 30 meters.

However, the sequences are classified under protected operating conditions and are not made publicly available by the United States Government. In other words, the protected operating condition is only for the use of earthcounters authorized by the United States Government.

In order for the earthmoving receivers to distinguish the various C / A signals from the other circulating GPS signals, Earthcopper's usually have a variety of different gold code sources to produce localized gold code sequences. Each locally derived gold code sequence corresponds to each unique gold code sequence of each of the GPS satellites.

The locally derived gold code sequences and the transmitted gold code sequences are cross-correlated with each other over gold code sequence intervals of one millisecond. The phase of the locally derived gold code sequences varies on a chip-by-chip basis and then within a chip until the maximum cross-correlation function is obtained. Since the cross relationship or Cross correlation for two gold code sequences having a length of 1023 bits, about 16 times as large as the cross correlation function of any of the other combinations of gold code sequences, it is relatively easy to lock the locally derived gold code sequence to the same gold code sequence as that of one the GPS satellite has been transmitted.

The gold code sequences of at least four of the GPS satellites in the field of view of an earth receiver are thus separated using a single channel which responds sequentially to each of the locally derived gold code sequences, or alternatively by using parallel channels which simultaneously respond to the different gold code sequences. After four locally derived gold code sequences have been locked in phase with the gold code sequences received from four GPS satellites in the field of view of a terrestrial receiver, the relative position of the terrestrial receiver can be determined to an accuracy of approximately 60 to 300 meters.

The foregoing approximate accuracy of the NAVSTAR GPS is affected by (1) the number of GPS satellites transmitting signals to which the earth receiver effectively responds, (2) the variable amplitudes of the received signals, and (3) the magnitude of the cross correlation peaks between the received signals from the different GPS satellites.

Since multiple PRN or pseudo-random number signals are received simultaneously at the earth receiver, there is a common time interval in which some of the codes may conflict. In other words, the codes cause a deterioration of the arrival time measurements of each received pseudo-random number because of the cross-correlations between received conflicting signals.

The time of arrival measurement for each PRN signal is made by determining the time of a peak amplitude of a cross-correlation between the gold code sequence of the received PRN signal and the locally derived PRN signal. When a locally derived PRN signal is superimposed over a received PRN signal, thereby increasing the average time of its cross-correlation, the average noise contribution decreases. However, because cross-correlation errors between the received PRN signals are periodic, increasing the average time also results in enlargements of both the error signal and the cross-correlation value between the received pseudo-random numbers. Consequently, errors in the arrival time of the PRN signals are not reduced by the cross-correlation.

In addition to the GPS, it is known in the conventional art to use inertial systems in navigation systems to obtain a position estimate of the vehicles. Such an inertial reference unit (IRU) receives specific force measurements from accelerometers in a reference coordinate frame stabilized by gyroscopes or gyroscopes. An inertial reference unit may be of several types, such as lasers, mechanics or fiber optics. In an unsupported navigation system using an inertial reference unit, the specific force (corrected for the effects of Earth's gravity) as measured by an accelerometer is integrated into a mathematical navigation equation to produce the speed and position of the vehicle.

The instrument measurements of the inertial reference unit may be set in a rectangular coordinate frame other than the reference navigation frame, depending on the platform device. The most commonly used reference navigation frame for navigating near the surface of the earth is the local level frame (latitude-longitude-vertical position). Several gimbal platform devices exist in the previous reference navigation frame.

In a gimbal-mounted north-facing local level reference unit, the gyroscopes and accelerometers are mounted on a platform that is rotated to keep the platform level and azimuth north-facing. The platform is the reference level. In contrast, in a gimballed azimuth-traveling local level inertial reference unit, the platform is maintained at the level, but not twisted about the vertical axis.

Furthermore, in a slimmed down or strap-down inertial reference unit, the gyroscopes and accelerometers are mounted directly on the vehicle body. They measure the linear and angular motion of the vehicle relative to the inertial space. The movement is expressed in vehicle coordinates. Therefore, in a strap-down inertial reference unit, it is necessary to first calculate the height of the vehicle to the reference navigation frame. Then the calculated height is used to transform the acceleration measurements into the reference frame. After the acceleration measurement data of a strap-down inertial reference unit in the reference frame has been extrapolated, the solution of the aforementioned navigation equations is identical to both the gimbal-mounted inertial reference unit and the strap-down inertial reference unit.

In the strap-down inertial reference unit, the height calculations required to resolve the acceleration measurements are performed at a high rate. The calculations suffer from numerical errors because of the limited computer bit size and throughput availability. These calculation errors depend on the frequency response of the sensor loop, the data rate and the resolution and size of the sensor output at the time of recording.

However, considerable benefits accrue from the use of the strap-down inertial reference unit instead of the gimbal-mounted inertial reference unit. The strap-down inertial reference units are cheaper. In addition, the strap-down inertial reference units are generally smaller in physical size. Thus, the potential for achieving size and cost savings on the inertial reference units may make the strap-down inertial reference units attractive for both military and commercial applications.

The performance of navigation systems using inertial reference units is limited primarily by errors contributed by the various proportionate sensors within the inertial reference units. Gyroscopes show a drift or a deviation. Accelerometers have inherent biases. Other errors are contributed by inaccurate scaling factors and inaccurate inertial reference unit alignment angles. Typically, the previous errors cause inaccuracies in the estimates of vehicle positions, speed, and altitude that accumulate over time as a vehicle mission progresses. To a certain extent, the errors depend on the dynamic actions of the user.

When a very accurate navigation system is required for a vehicle, high-precision gyroscopes and accelerometers can be used to meet this need. However, such high-precision equipment adds to the complexity and cost of the vehicle.

EP-A-0181012 discloses a vehicle position estimation system combining GPS and inertial navigation systems. IEEE Position Location and Navigation Symposium, 4.-7. November 1986 discloses an inertial navigation system in comparison with the GPS.

DE-A-33 10 111 discloses a navigation system with drift or deviation compensation. US-A-3630079 discloses a navigation system using multiple sensors and error correction.

WO-A-8707056 discloses a system for avoiding obstacles in the path of a vehicle.

According to the present invention, there is provided a system for navigating a vehicle along a route, comprising:

Means for storing route data representing a predetermined route;

Means for storing path data for each predetermined route;

Means for selecting a predetermined route following the vehicle;

Means for extracting route data representing a selected predetermined route;

Means for extracting path data for the extracted route data;

Means for generating a path from the extracted path data;

Means for associating vehicle commands with a generated path;

Means for executing the associated vehicle commands that cause the vehicle to attempt to follow a generated path;

Means for periodically calculating errors in following a generated path;

Means, responsive to the calculated errors, for adjusting the execution of the associated vehicle commands to reduce the calculated errors so that the vehicle follows a generated path with a high degree of accuracy;

Means for detecting obstacles in the direction of travel of the vehicle;

Means for avoiding collision with the obstacles, which in turn comprise means for stopping the vehicle to prevent collision with the obstacles and means for creating a path around the obstacles;

whereby an autonomous system for the safe and efficient navigation of a vehicle along a route is realized; characterized in that the means for creating a path produces a path of contiguous arrays with rates of change of curvature dependent on the steering constraints of the vehicle.

The present invention also provides a method for navigating a vehicle along a route, comprising the steps of:

Storing route data representing a predetermined route;

Selecting a predetermined route following the vehicle;

Extracting the route data representing a selected predetermined route;

Extracting path data for the extracted route data;

Generating a path from the extracted path data; Associating vehicle commands with a generated path;

Executing the associated vehicle commands that cause the vehicle to attempt to follow a generated path;

Periodically calculating errors in following a generated path;

Adjusting, in response to the calculated errors, the execution of the associated vehicle commands to reduce the calculated errors so that the vehicle follows a generated path with a high degree of accuracy;

Detecting a collision with the obstacles by stopping the vehicle to prevent collision with the obstacles, creating a path around the obstacles, or performing both; whereby an autonomous method for a safe and efficient navigation of a vehicle along a route is realized; characterized in that the generated path is a path of attached arrangements or Positions with a rate of change of curvature is dependent on the steering restrictions of the vehicle.

The present invention can be used to help any navigation system for autonomous vehicles. The autonomous vehicles may be stationary or mobile. In addition, the autonomous vehicles may be at or near the surface of the earth. In other words, the present invention provides a very accurate and fast tracking of any terrestrial vehicle. It provides both devices and methods which provide a superior positioning or Positioning ability and thus allow a flexible autonomous navigation capability.

Other features and advantages of the present invention will become apparent to those skilled in the art upon examination of the following drawings and detailed description. It is intended that any additional features and advantages be included herein.

The present invention as defined in the claims may be better understood with reference to the text and the following drawings.

Fig. 1 illustrates a block diagram of the preferred embodiment of the present invention;

Fig. 1A is a block diagram 100A of the operating GPS satellites in the NAVSTAR GPS;

Fig. 2 illustrates four simultaneous navigation equations that consider four GPS satellites of the NAVSTAR GPS;

Fig. 3 is a block diagram of a typical stand-alone work site;

4 is a block diagram of relationships between a navigator, VPS and vehicle controls of the present invention;

Fig. 5 is a block diagram illustrating the elements in an autonomous control system;

Fig. 6 is a block diagram of the operation of a GPS;

Fig. 7 is a block diagram of a GPS processing system of the preferred embodiment;

Fig. 8 is a flowchart of the GPS processing system of Fig. 7;

9 is a block diagram of an MPS (Motion Position System) including an odometer 902 and an inertial reference unit (IRU) 904;

10 is a block diagram of the VPS (Vehicle Positioning System).

Fig. 11 is a block diagram of the VPS architecture of Fig. 10;

Fig. 12 is a diagram of vehicle route definitions;

Fig. 13 is a diagrammatic illustration of how layers and associated circles are obtained from lens points;

Fig. 14 is a diagram of how the sign of a first clothoid segment is determined;

Fig. 15 is a diagram of how the sign of a last clothoid segment is determined;

Fig. 16 is a graphical representation of a clothoid curve;

Fig. 17 is a flowchart of a numerical method for calculating approximated Fresnel integrals;

Fig. 18 is a diagram showing the re-planning of a path;

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

Fig. 20 is an illustration of an embodiment of the storage buffer;

Fig. 21 is a diagram showing how an error vector including a curvature is calculated;

Fig. 22 is a context diagram of the navigator of the present invention;

Fig. 23 is a context diagram of a path trace structure;

Figs. 24A-24D are data flow summaries of the navigator 406;

Fig. 25A is an illustration of a vehicle-mounted scanner 404;

Fig. 25B is an illustration of an autonomous vehicle which relates to. an obstacle scans or feels;

Fig. 26 is a diagram of an autonomous vehicle which avoids obstacles;

I. Definitions

(1) "Absolute position" in relation to this document refers to a position with respect to the center of the earth. In general, it will be an absolute position relative to a vehicle or base station, both on and near the surface of the earth. First, second and third position estimates are all absolute positions.

(2) "Actual Pseudorange" means an approximation to the distance between (1) a reference point and (2) a source of a terrestrial positioning system. In this document, the actual pseudoranges usually refer to an approximation of the distance between (1) an Earth receiver and (2) GPS satellites and / or pseudolites. Actual pseudoranges are approximated by first measuring the propagation time delays between the transmission and reception of the electromagnetic signals coming out of the GPS satellites and / or pseudolites. Actual pseudoranges can be easily calculated by multiplying the calculated time delays by the speed of light or by 2.9979245898 x 108 m / s.

(3) "Antiselective availability" refers to a method / technique / process for detecting and compensating corrupted GPS data in the coarse / seek (C / A) modulation mode.

(4) "Autonomous" is used in this document in the traditional sense. It indicates the operation, which is either completely automatic or essentially automatic or without significant involvement of a human being in operation. In general, an autonomous vehicle means an unmanned vehicle in service or a vehicle in service without a human driver or passenger. However, an autonomous vehicle may be automatically driven or otherwise operated and may also have one (more) human (human) passenger (s).

(5) "Basic correlation derivation" means a spatial derivative.

(6) "Basic correlation derivation technique" means a method / process for calculating the base correlation derivatives.

(7) "Estimated Base Position" or "BEP" refers to the relative position of the base station with respect to a vehicle. The estimated base position is used in the base correlation derivation technique.

(8) "Known base position" or "BKP" is the absolute position of the base station (used as a reference point) which is known. The known base position may itself be an estimate derived from any precise positioning system. It is assumed that the known base position is a more accurate estimate of the base station's absolute position than any other position estimate.

(9) "Base Position Estimate" means the estimation of the absolute position of the base station as derived from the GPS processing system within the host processing system. The base position estimate is substantially similar to the first position estimate derived from the GPS processing system in the vehicle. The base position estimate is calculated in the base residual derivative technique.

(10) "Base residual derivative" means a spatial derivative which is the effective difference of the base station's known base position (BKP) and the base station position estimate calculated by the host processing system.

(11) "Basic residual derivative technique" refers to a method for deriving base residual derivatives.

(12) "bias" refers to a difference between two measurements, usually position estimates (spatial derivatives) or clock rates (clock bias). Since it is known that one measurement is usually more accurate than the other, the derivative is often referred to as an "error".

(13) "clock derivative" means the difference of the clock times between (1) the transmission circuit of the GPS satellites and / or GPS pseudolites and (2) the reception circuit of a terrestrial receiver. Using a clock derivative in the computation of a spatial derivative, the clock derivative is multiplied by the speed of light or 2.988 x 10 $ meters per second. Consequently, the clock derivation is converted to units of length.

(14) "Constellation" refers to a group consisting of GPS satellites and / or pseudolites whose signals are used to derive an absolute position estimate of the point at or near the surface of the earth. See below "optimal constellation".

(15) "Constellation effect method" means a technique or process whereby an optimal constellation of GPS satellites is selected from a larger group of GPS satellites in the field of view of a vehicle.

(16) "Data radio" refers to a transmitter, receiver, transceiver or any combination thereof for the transmission of data at radio frequencies (RF = radio frequencies).

(17) "Earth recipient" refers to any device or device or any part thereof that receives and processes signals from a GPS and / or pseudolite. Earth recipients may be located at or near the surface of the earth. In addition, earthquake recipients may take the form of a vehicle or a base station, for example.

(18) "Estimated Pseudorange" refers to the approximation of the distance between (1) a reference point and (2) a source of a terrestrial positioning system. In this document, the actual pseudoranges usually refer to an approximation of the distance between (1) an Earth receiver and (2) GPS satellites and / or pseudolites. Estimated pseudoranges are calculated from GPS data encoded in the electromagnetic signals transmitted by the GPS satellites and / or the pseudolites. Almanac equations for calculating estimated pseudoranges from the GPS data of the NAVSTAR GPS are publicly available.

(19) "First Position Estimate" or "FPE" or "FPE (i)" refers to an estimated absolute position of any vehicle output in some form from the GPS. The first position estimate and the second position estimate are independently derived. Subsequently, these estimates are combined and filtered to derive a third position estimate. Consequently, the accuracy of the first position estimate affects the accuracy of the third position estimate.

(20) "GLONASS-GPS" refers to the GPS which has been constructed by the USSR and is currently used by it.

(21) "Global Positioning System" or "GPS" is a type of terrestrial positioning system. In a GPS, a number of satellites are placed in orbit around the planet Earth. The GPS satellites are designed to transmit electromagnetic signals. From these electromagnetic signals, the absolute terrestrial position (position with respect to the center of the earth) can be ultimately determined by any receiver at or near the earth's surface. The US government has called their GPS "NAVSTAR". The USSR government has called their GPS "GLONASS".

(22) "GPS data" means all data encoded on signals transmitted by GPS satellites of a GPS. For example, GPS data includes ephemeris data and time data.

(23) "GPS processing system" refers to a system for receiving signals from a terrestrial positioning system and for deriving first position estimates of vehicles from the recorded signals. The GPS processing system receives electromagnetic signals from the GPS satellites of a GPS and / or pseudolites.

(24) "host processing system" refers to a computer system operating at the base station for carrying out methods and techniques that improve the accuracy of the vehicle position estimates. Data derived from these methods and techniques are transmitted to vehicles so that the vehicles can use the data when calculating first, second and third position estimates. The architecture / hardware (components) of the host processing system are substantially similar to the architecture / hardware of the vehicle positioning system.

(25) "Inertial Reference Unit" or "IRU" refers to a system, usually onboard a vehicle, to assist in the derivation of a second position estimate of the vehicle. An inertial reference unit receives specific force measurements from accelerometers in a reference coordinate frame stabilized by gyroscopes or gyroscope means. An inertial reference unit may be of a laser type or of a mechanical type. In an unsupported navigation system using an inertial reference unit, the specific force (corrected for the effects of earth's gravity) as measured by the accelerometer is integrated into a mathematical navigation equation to produce the vehicle position and velocity. The inertial reference unit is a part of the movement position determination system.

(26) "Kalman filter" is used in the conventional sense. This refers to a software program for filtering out noise or errors in the data. A GPS Kalman filter is used to filter out noise or errors in the GPS processing system to improve the accuracy of the first position estimates. It is also a VPS or Vehicle Positioning Kalman filters are used to filter out the noise in the vehicle position determination system to improve the accuracy of the second position estimates.

(27) "Motion Positioning System" or "MPS" (MPS) means a system having at least one inertia reference unit and a vehicle odometer. The travel position determination system derives the second position estimate from any vehicle at or near the earth's surface. Moreover, a moving position determination system need not be present at the base station because of its stationary nature.

(28) "Optimal constellation" means a satellite constellation in which the relative positions of the GPS satellites in space require superior triangulation capabilities to derive the most accurate estimate of a point on or near the surface of the earth.

(29) "Initial derivative" means a spatial derivative calculated by subtracting both estimated pseudoranges and clock derivatives (in units of length) from the actual pseudoranges. Clock derivatives are converted to units of length by multiplying them by the speed of light or by 2.9979245898 10 meters per second.

(30) "Original derivation technique" is a method of calculating the original derivatives.

(31) "NAVSTAR-GPS" means the GPS that has been designed by the US Government and is currently used by the US Government.

(32) "Navigation system" refers to any systems and / or methods for guiding a vehicle on or near the surface of the earth. The navigation systems can be on board a vehicle. The vehicle positioning system of the present invention can provide the vehicle's navigation system with a very accurate third position estimate of the vehicle so that the navigation system can thereby precisely guide the vehicle.

(33) "Parabolic derivative" is a spatial derivative calculated by constructing parabolic models for the actual pseudoranges of each observed GPS satellite and extrapolating values ​​from the parabolic models. The parabolic derivatives are the actual pseudoranges minus the value extrapolated from the constructed parabolic models and minus the clock derivatives (in units of length by multiplying by the speed of light).

(34) "Parabolic Derivative Technique" is a method of calculating parabolic derivatives of each of the GPS satellites that are used.

(35) "Preferred Embodiment" refers to the best way to set up the present invention. The preferred embodiment is only exemplary. The present invention should not be interpreted as being limited to the preferred embodiment.

(36) "Pseudolite" refers to a radiation system or transmission system at or near the surface of the Earth for the emulation of a GPS satellite. Electromagnetic signals, similar to GPS satellites, are transmitted by land-based pseudolites. One or more pseudolites may be used to emulate GPS satellites to improve the calculation of the first position estimates.

(37) "Pseudolite data" means all data encoded in signals picked up by the pseudolites. The pseudolite data is similar in many ways to the GPS data and has similar information.

(38) "Pseudorange" means the distance between a source of a terrestrial positioning system and a point on or near the surface of the earth. The sources may be GPS satellites and / or pseudolites. The terrestrial positioning system may be a GPS used with pseudolites, if any. Furthermore, the point on or near the surface of the earth may be the base station and / or vehicles.

(39) "Satellite position prediction" is a method of determining the future positions of the GPS satellites. The method allows the premature selection of optimal constellations.

(40) "Second Position Estimation" or "SPE" (SPE) refers to an estimated absolute position of any vehicle output in some form by the MPS. The second position estimates have at least one position information from an inertial reference unit. The second position estimate may include position information from a vehicle odometer located on a vehicle.

(41) "Spatial derivative" refers to a derivative that refers to approximations of positions in a two-dimensional or three-dimensional space. Spatial derivatives are used to offset a position estimate to improve the accuracy of the position estimate. Spatial derivatives may be calculated by a number of different methods of the present invention. Included in these methods are, for example, an original derivation technique 1500, a parabolic derivation technique 1600, a base residue derivation technique 1700, and a base correlation derivation technique 1700A.

(42) "System" is used for the purpose of signifying a device, a method or a combination of both a device and a method. In addition, it could include programs, hardware or components or a combination of hardware and programs.

(43) "Positioning system" means any system with sources that emit signals that can be used by a receiver of the signals to estimate the relative distance between the sources and the receiver. The signals may, for example, be in the form of electromagnetic waves, percussion waves and / or sound waves.

(44) "Terrestrial positioning system" means any positioning system that can be used to ultimately estimate the terrestrial position of an Earth receiver. The signals may, for example, be in the form of electromagnetic waves, percussion waves and / or sound waves. The terrestrial positioning system is the NAVSTAR GPS.

(45) "Third position estimate" or "TPE" (TPE) refers to an estimated absolute position of any vehicle issued in any form by the vehicle positioning system. The third position estimates are more accurate position estimates of the vehicle positions compared to the first and second position estimates. The third position is derived from the first and second position estimates by the VPS processing system.

(46) "Vehicle" means any carrier for transporting physical things. Vehicles may take the form of mining trucks, construction trucks, farm tractors, automobiles, ships, boats, railways, balloons, rockets or airplanes. In the preferred embodiment, a 785 PFF off-highway truck from Caterpillar Inc. is used.

(47) "Vehicle Positioning System" or "VPS" (VPS) refers to the system that derives position estimates from any vehicle. The position estimates from the vehicle positioning system are extremely accurate and can be used by a navigation system on any vehicle to accurately guide the vehicle. Position estimates are referred to by the vehicle positioning system as third position estimates.

(48) "VPS processing system" means the processing system of the vehicle positioning system or the VPS. The VPS processing system derives third position estimates from the first and second position estimates. The architecture is shown in FIGS. 10 and 11.

(49) "Weighted combination" refers to a special software program that processes data. The input data is assigned a predetermined weighting factor based on the estimated accuracy of the data and the technique used to collect the data. For example, the first position estimate of the GPS signal 716 is weighted more heavily than the second position estimate of the inertial reference unit signal 910 because the former is inherently more accurate. Furthermore, the speed measured by the inertial reference unit may be weighted more heavily than the speed measured by the GPS processing system because the former is more accurate. The speed measured by the GPS processing system is not used at all, but could be used in other constructions.

(50) "Weighted Path History Technique" is a method or process for increasing the accuracy of the first position estimates output from the GPS processing system. The technique uses earlier first position estimates to derive a vehicle path model to test the validity of the future first position estimates. The application of the weighted path history technique results in a reduction in wandering of the first position estimates and improved immunity to incorrect position calculations.

II. General Overview

FIG. 1 illustrates a high level block diagram 100 of the example system. To provide for the precise autonomous operation of a vehicle 102 at or near the earth's surface, the exemplary system includes both a vehicle positioning system (VPS) 1000 and a navigation system 1022. Both of these systems include apparatus, methods, and techniques that, when integrated together, provide for highly accurate control of unmanned vehicles.

A. Vehicle Positioning System (VPS)

The task of guiding the autonomous vehicle 102 along a prescribed path requires inter alia an accurate estimate of the current vehicle position relative to a reference point. Once the current position is known, the vehicle 102 may be instructed to proceed to its next destination.

Using the VPS 1000, position estimates of the vehicle 102 can be determined with extreme accuracy. The VPS 1000 receives GPS data from GPS satellites 104 from a GPS, such as the NAV-STAR GPS or the GLONASS GPS.

The NAVSTAR GPS is used. Fig. 1A illustrates the NAVSTAR GPS. GPS satellites 130-168 travel around earth 172 into six orbitals 174-184.

Referring to FIG. 1, the VPS 1000 may also receive pseudolite data from pseudolite (s) 105. The term "pseudolite" in the context of this document means a radiation emitting device on or near the surface of the earth to emulate a GPS satellite.

From the GPS data and / or the pseudolite data, the VPS 1000 derives accurate estimates of the position of the vehicle 102. The GPS data and / or the pseudolite data is significantly enhanced over numerous inventive techniques and methods of the exemplary system to improve the accuracy of vehicle position estimation.

In particular, the VPS 1000 is a positioning system based on the use of GPS data from the NAVSTAR GPS 104 and a motion positioning system 900. The motion positioning system 900 includes an inertial reference unit (IRU) 904 and or a vehicle odometer 902. The IRU 904 includes one or more laser gyroscopes 106 and one or more accelerometers 108 that may be used to generate position, velocity, roll, altitude, and yaw data. The vehicle odometer 902 generates data about the distance traveled by the vehicle 102.

A first position estimate of the vehicle 102 is derived by the GPS processing system 700 from the GPS data received from the GPS satellites 104 and from the pseudolite data received from the dummy list (s) 105. In order to improve the accuracy of the first position estimate, the exemplary system uses a number of methods discussed below. In addition, a second position estimate is derived by the MPS communication processor 906 of the travel position system 900 having the IRU 904 and / or the vehicle odometer 902. As shown by the respective arrows 112 and 114, the first position estimate and the second position estimate are then combined and filtered by a VPS processing system 116. The result, as shown by an output arrow 118, is a more accurate third position estimate.

B. Navigation system

The navigation system 1022 receives the third position estimate from the VPS 1000. The navigation system 1022 uses the precise third position estimate to accurately navigate the vehicle 102. A primary purpose of the navigation system 1022 is to guide the vehicle 102 between points along a predetermined or dynamically generated path.

The navigation system 1022 is located in the vehicle 102 itself. In other words, it is essentially an "on-board" system. In addition, the navigation system 1022 may be designed to be retrofitted in the vehicle 102.

So that the navigation system 1022 can route the vehicle 102 to follow preset or dynamically generated paths, various models or conceptual representations are created and used. For example, lines and curves may be used to establish vehicle paths between object points. Mathematical B-splines or clothoid curves may be used to model the actual path where the vehicle 102 must navigate. These mathematical curves will be discussed later in this paper.

The use of the above modeling techniques provides for improved data communications, storage and handling of the vehicle 102. The techniques further allow for simplification of monitoring tasks by providing a hierarchy of control and communication. The higher a control level in the hierarchical control scheme, the simpler the task and the more compact the commands.

The navigation system 1022 further provides control of the vehicle's mechanical systems, such as brakes, steering, and engine and transmission, to perform the necessary physical actions required to move, stop, and steer the vehicle 102 ,

The navigation system 1022 also checks the actual position of the vehicle 102 against the desired position to correct the vehicle control according to the desired position. The navigation system 1022 may run multi-state models to improve that verification capability. The navigation system 1022 also checks for errors or failures in the system itself and in vehicle components. If errors or failures are detected or can be determined, the navigation system 1022 can ensure a fail-safe shutdown, or by bringing the vehicle 102 to a complete stop.

The navigation system 1022 further provides various operating conditions for controlling the vehicle 102. These include: (1) a fully autonomous mode where the navigation of the vehicle 102 is automatically handled by the navigation system 1022; (2) a telephoto or remote control mode where a remote human operator (not shown) controls the direction and movement, etc. the vehicle 102 can control; and (3) a manual mode in which a human operator sitting in the vehicle 102 can take control of the vehicle 102 and manually drive it.

In autonomous mode, obstacle detection is critical because if the vehicle 102 is not under control, it could cause large property damage and severe injury to living beings. The navigation system 1022 can efficiently detect obstacles. Stones, animals, humans, trees or other obstacles may unexpectedly enter the path of the vehicle 102. The navigation system 1022 is capable of detecting these obstacles, either by stopping or by drawing a path around the obstacle and returning the vehicle 102 to its original path if the route is considered safe.

Accurate tracking of the desired route is another function of the navigation system 1022. The operation and architecture of the navigation system 1022 has been designed for real-time tracking of the vehicle paths at speed up to about 30 miles per hour (mph).

C. Base Station

The example system may include a host processing system 186 in a base station 188. The host processing system 186 performs functions for both the VPS 1000 and the navigation system 1022.

With respect to the VPS 1000, the host processing system 186 receives GPS data and / or pseudolite data as shown by respective arrows 190 and 192. In effect, the host processing system 186, as well as the base station 188, may function as a known reference point to improve the accuracy of the vehicle position estimates, as discussed in detail below.

The host processing system 186 implements a number of methods for improving the accuracy of the vehicle position estimates. The satellite position prediction method 1800 discussed above is also implemented by the host processing system 186. The host processing system 186 will recognize the same satellite constellation observed by the vehicle 102.

Calculations are performed on the GPS data and / or the pseudolite data to derive deflections (biases). The term "bias" in the context of this document refers to a difference between two measurements, usually position estimates (spatial derivative) or clock rates (clock derivative). Because one measurement is usually known to be more accurate than the other, the derivation or often referred to the bias as an "error".

To calculate spatial derivatives, the host processing system 186 implements a number of methods. Among these methods, for example, there are provided an original derivation technique 1500, a parabolic derivation technique 1600, a base residual derivation technique 1700, and a base correlation derivation technique 1700A.

The foregoing differential correction techniques compensate for data errors. In other words, the biases computed in the host processing system 186 indicate data errors. As shown by an arrow 194, the leads are transferred to the GPS processing system 700 of the vehicle 102. The GPS processing system 700 uses these biases to eliminate errors in the vehicle position estimates.

The host processing system 186 further provides functions related to the navigation system 1022 of the example system. The host processing system 186 serves as the highest control level of the navigation system 1022, as indicated by an arrow 196. It handles the scheduling and scheduling of the vehicle 102 with exactly the same results as a human dispatcher would achieve. As a result, the host processing system 186 may thereby determine the duty cycle of the vehicle 102.

The host processing system 186 commands the vehicle 102 to proceed from a current position to a future position via a predetermined route so that the vehicle 102 can perform its work objectives. The host processing system 186 may specify the vehicle routes by name, rather than by listing each point along the route, as is conventionally the case. Accordingly, the on-board navigation system 1022 of the vehicle looks up the designated vehicle route and transmits the named vehicle route into sets of nodes and segments along said vehicle route.

III. Vehicle positioning system

The following discussion with respect to the VPS 1000 will refer in particular to FIGS. 7 to 11. Figures 10 and 11 show the architecture / hardware of the VPS 1000. The VPS 1000 is a high accuracy position determination system for a moving or stationary vehicle 102 at or near the surface of the earth.

It is recalled that the VPS 1000 has the GPS processing system 700 and the MPS 900 shown in the respective FIGS. 7 and 9. Further, it should be remembered that the MPS 900 includes the IRU 904 and the vehicle odometer 902, both of which are shown in FIG. In effect, these systems have been improved and integrated by the exemplary system to produce a highly effective position determination system.

Referring to FIG. 7, the GPS processing system 700 includes an antenna 702 connected to a GPS receiver 706. When the GPS satellites 104 in the field of view of the antenna 702 have multiple GPS satellites 200-206, as shown in FIGS. 2 and 3, the GPS receiver 706 reads all of its GPS data along with any pseudolite data from any (any) Pseudolit (s) 105 in the field of view of the antenna 702. In the preferred embodiment, the GPS receiver 706 is responsible for calculating the first position estimate of the vehicle 102 from the GPS data and / or the pseudolite data.

In order to increase the accuracy of the first position determination method, the satellite position prediction method 1800 is implemented by a GPS processor 710 of the GPS processing system 700. The satellite position prediction method 1800 predicts the position of any GPS satellite at the current time or for any future time.

Using the satellite position information, the GPS processing system 700 may determine the optimal GPS satellite constellation for detection using a constellation effect method 1300. The constellation effect method 1300 is also implemented by the GPS processor 710. According to the constellation effect method 1300, a best constellation is selected from the data sources comprising the GPS satellites 200-206 and pseudolit (s) 105.

The GPS processor 710 computes a first position estimate of the vehicle 102 based on the best constellation and on geometry / triangulation methods. The accuracy of the first position estimate depends in part on the number of GPS satellites used in the calculation. Any additional GPS satellite used can increase the accuracy of the first position estimate. After the calculation, the first position estimate of the vehicle 102 is transmitted to a VPS main processor 1002 of FIG.

Referring to FIG. 9, the IRU 904 includes laser gyroscopes and accelerometers that generate position, velocity, roll, pitch, and grade data. The IRU 904 combines this information into a second position estimate of the vehicle 102. The odometer 902 may be implemented to measure the distance traveled by the vehicle 102. The data from the IRU 904 and the odometer 902 are also transmitted via the MPS communications processor 906 to the VPS main processor 1002, as shown in FIG.

The VPS main processor 1002 combines the second position estimate from the MPS 900 (the IRU 904 and perhaps the odometer 902) with the first position estimate from the GPS processing system 700 to produce a more accurate third position estimate.

The VPS 1000 further implements a method for eliminating erroneous or incorrect third position estimates that may cause "vehicle drifting". This method is called weighted path history methods. In essence, the path history of the vehicle 102 is used to statistically determine the accuracy of future estimates of the position of the vehicle 102.

With reference to FIGS. 1 and 3, a base station 188 provides a near geographic reference point for the VPS 1000. The base station 188 includes a host processing system 186. The host processing system 186 has a similar architecture and performs the same functions as the GPS processing system 700. However, the host processing system 186 performs additional functions to increase the accuracy of the first position estimates.

The satellite position prediction method 1800 is implemented by the host processing system 186, in addition to the GPS processing system 700 discussed above. Accordingly, the host processing system 186 will detect the same GPS satellite constellation being observed by the vehicle 102 have the same GPS satellite in a larger constellation.

Calculations are then performed on the GPS data and / or the pseudolite data to derive leads (biases), including spatial derivatives and clock derivatives. To calculate spatial derivatives, the host processing system 186 implements a number of methods.

As shown by an arrow 194, the spatial derivatives and timing derivatives are transmitted to the GPS processing system 700 of the vehicle 102. The GPS processing system 700 uses these derivatives to eliminate errors in the vehicle position estimates.

IV. Navigation system

A. Overview

When considering the establishment of an autonomous navigation system, there are certain basic questions that 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?". The first question is answered by the position determination system part of the present invention, as discussed above in Section III.

The next or second question is "Where are we going and how are we going to get there?". This second question falls within the scope of the navigation system portion of the exemplary system discussed in this section (IV).

Another (third) question, actually a refinement of the second, is "How do we actually physically move the vehicle, for example, what actuators (steering, speed, brake, etc.) are involved to get there?". This is the area of ​​the vehicle control subsystem of the navigation system, which is also discussed below.

As implicitly discussed above, the autonomous navigation of, for example, a mining vehicle can provide certain significant advantages over conventional navigation. Among them is an increased productivity of a 24-hour operation of the vehicle around the clock. The problems presented by hazardous work environments or work environments where visibility is poor are particularly well suited for solution by an autonomous system.

For example, there are some mining areas where visibility is so poor that working for 200 days of the year is not possible. There are other areas that are dangerous to human life because they are contaminated by industrial or nuclear pollution. An area may be so remote or devastated that if it were required that people work there, it would impose severe restrictions or be impractical. The application of the exemplary system could foreseeably include off-ground operations, such as mining on the moon, provided that the necessary GPS satellites are placed in the orbit of the moon.

In a typical application, as shown in Fig. 3, there are three basic areas of operation relating to the navigation of a mining vehicle on a mining site: the loading site, the delivery segment and the unloading site. At the point of loading, a delivery vehicle can be loaded with ore in a variety of ways, such as man-operated shovels controlled either directly or remotely, or by autonomous shovels. The delivery vehicle must then travel over an area called delivery segment, which may be only a few hundred meters or several kilometers. At the end of the delivery segment is the unloading point, where the ore is unloaded from the delivery vehicle, for example, to be crushed or otherwise processed. Autonomous position determination and navigation can be used to control the delivery vehicle along the delivery segment. Autonomously navigated tank vehicles and maintenance vehicles are also considered.

Referring to FIGS. 4 and 5, the autonomous mining truck (AMT) navigation system includes various systems, devices, and / or functions. The subsystem of the vehicle positioning system 1000 of the entire autonomous mining truck system, as discussed above, outputs position data indicating where the vehicle is located having, for example, north and longitude and east and latitude positions, respectively.

With reference to FIGS. 4 and 5, position data output from the vehicle position determination system is taken by a navigator 406. The navigator determines where the vehicle wants to go (from the route data) and how it gets there, and in turn outputs data composed of steering and speed commands to a vehicle control function block 408 to move the vehicle.

The vehicle control block then issues commands at a low level to the various systems of the vehicle 102, such as the governor, the brakes, and the transmission. When the vehicle is moving to its destination, the vehicle controls block, and the vehicle positioning system receives feedback information from the vehicle indicating, for example, any fault conditions in the vehicle systems, the current speed, and so forth.

The navigation must also have an obstacle handling capability (detection and avoidance) to cope with the unexpected. A scanner system 404 detects obstacles in the projected track of the vehicle, as well as obstacles that may approach from the sides, and informs the navigator about it.

It may then be necessary for the navigator to decide if action is required to pass the obstacle. If an action is required, the navigator decides how to avoid the obstacle. And after the obstacle has been avoided or bypassed, the navigator decides how to get the vehicle back on a path to its destination.

With reference to Fig. 22, which will be referred to as a context diagram, and with reference to Figs. 24A-24D, definitions of the communications shown as circles with numbers therein are described below:

502nd Host commands and requests:

Commands given by the host to the vehicle manager. These commands could be of various types:

Introducing / end

Supply parameters;

emergency action; and

instructions

The requirements ask about the condition of different parts of the navigator.

504th Responses to the host:

These are answers to the questions asked by the host.

432nd Position data:

This is a flow of information provided by the vehicle positioning system (VPS).

416th Range data:

These are area data from the line laser scanner.

432nd VPS control:

These are commands given to the vehicle positioning system to turn it on, shut down, and switch between operating states.

416th Scanner Control:

These are commands that are sent to the laser scanner to initiate movement and adjust the following speed profile.

420th Steering and speed commands:

These are commands issued to the vehicle to control steering and speed. These commands are issued at a rate of 2 to 5 Hertz.

Referring to FIG. 5, both the vehicle positioning system and the navigator are located on the vehicle and communicate with the base station 188 to receive high-level GPS position information and instructions from a host processing system 186. The system obtains GPS position information from the GPS satellites 200-206 at the base station and on-board the vehicle so that a common operating state error can be removed and the accuracy improved. Alternatively, parts of the vehicle positioning system and the navigator may be located at the base station.

The host at the base station may instruct the navigator, for example, to travel from point A to point B, and may direct one of a set of fixed routes to use. The host also handles other typical scheduling and scheduling activities, such as the coordination of vehicles and equipment, to maximize efficiency, avoid collisions, schedule maintenance, detect fault conditions, and so on. The host also has an operational interface for a human manager.

It has been found that it is desirable to place the host at the base station and navigate the vehicle to avoid a bottleneck in communication and consequent degradation in performance and responsiveness. Because the host sends commands at a relatively high level and simplified data to the navigator, it requires relatively little communication bandwidth. However, in situations where broadband communication is available, this can not be a factor.

Another factor in determining the specific location of elements of the system is the sensitivity of autonomous navigation over time. The navigation system must continuously check its absolute and relative position to avoid unacceptable inaccuracies in tracking a route. The required frequency of checking the attitude increases with the speed of the vehicle, and the communication speed can 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 where a high degree of tracking accuracy of the road is not critical, this communication factor may be unimportant. For example, with a relatively fast ride over large expanses of open flat land on a relatively straight path, it may not be necessary to check the position on the trip as often as it would take to navigate while traveling along a winding mountain road.

Conceptually, the navigation aspects can be arbitrarily divided into the following main functions:

Route planning / path generation;

Path tracking; and

Obstacle treatment.

The function of the present invention will be discussed below.

B. route planning / path generation

1. introduction

The navigation of an autonomous vehicle according to the present invention conceptually consists of two sub-problems, path generation and path tracking, which are solved separately.

Path generation utilizes intermediate goals from a high level scheduler to generate a detailed path that the vehicle 102 is to follow. There is a certain trade-off between simplifying the presentation of such levels and the ease with which they can be performed. For example, it is a simple scheme to decompose a path into straight lines and circular curves. However, such paths can not be accurately tracked in a straightforward manner because of curvature irregularities at transition points of segments requiring sudden accelerations.

Following path generation, the path trace inputs the detailed, generated path and controls the vehicle 102 to follow the path as precisely as possible. It is not enough simply to follow a list of guidance commands previously established, as failing to accurately achieve the required steering movements will result in steady state steady state transients. The errors accumulate over time. 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 the traditional vehicle control scheme in which a time history of the position (a trajectory) is provided in the schedule set for the vehicle 102.

These methods are suitably referred to as "path tracking" in that the steering movement is decoupled from time; that is, steering motions are directly related to the geometric nature of the fixed path, which makes the speed of the vehicle 102 an independent parameter.

With reference to FIG. 3, an autonomous vehicle 102 may be required to traverse a delivery segment 320 to an unloading point 322 and, after having unloaded its load, to drive over another delivery segment to a shop floor 324 under the instruction of Host processing system 186. Host processing system 186 determines the destinations of vehicle 102, which is called "cycle planning." The determination of which routes should be taken to get to a desired destination must be achieved by "route planning".

"Route planning" is the determination of which path segments to take to get to a desired destination. In general, a route may be considered as an abstraction or representation of a set of points between two defined high level layers. 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 sequence of operations (starting vehicle 102, releasing the brake 4406, the insertion of the gear 4610, the acceleration to the commanded speed limit, the steering of the steering wheel 4910, the avoidance of obstacles 4002, etc. may include), the autonomous navigation system of the present invention operates in a similar manner. As used in the system of the present invention, a "route" is a sequence of contiguous "segments" between the beginning and end of a journey.

An autonomous vehicle 102 may begin at any position in the sequence and travel the route in either direction. A "segment" is the "path" between "nodes". A "node" is an "alignment" on a path that requires a decision. Examples of nodes are load locations 3318, unload locations 322, and interfaces 326.

There are different types of segments. For example, there are linear and circular segments. The linear segments (lines) are defined by two nodes. Circular segments (arcs) are defined by three nodes.

"Postures" are used, for example, to model parts of a route, paths and nodes. Layers may consist of a position, a direction, a curvature, a maximum speed, and other information for a given point on the path.

A "path" is a succession of successive layers.

A segment is therefore a sequence of successive layers between nodes. All segments have an associated speed which determines the maximum speed at which the vehicle 102 can travel over that segment. The Navigator 406 may command slower speeds to meet other requirements, if necessary.

The determination of which layers are required to define a path segment by analytical methods, experimental methods or a combination of both is called "path planning" according to the present invention. To complete the discussion, a sequence of contiguous routes, as mentioned above, is referred to as a "cycle" and the work objectives of a vehicle 102 determine its "cycle".

Therefore, to define a route, one must first define the nodes and segments. Next, the nodes and segments must be instructed. Finally, the routes must be defined by specifying where in the commanded set a route should begin and in which direction the commanded set is to be overrun (see Figure 12, which illustrates these concepts of the present invention).

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

In an actual world example of the present invention, an image of a terrain may be provided 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. The routes would therefore be determined by roads and intersections. However, there will 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 the route curvature to steer the vehicle. The route definition methods that use lines and arcs do not provide continuous curvature. Clothoid curves are another way to define routes.

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

Referring to FIGS. 4 and 12, in operation, host processing system 186 from base station 188 instructs an identified vehicle 102 to take route N from the current location. The navigator 406 acts to generate a path by translating "Route 1" into a series of segments, each having a "commanded" or associated maximum speed limit, which together form a generated path to which the vehicle is assigned should try to follow. By setting routes and instructing the autonomous vehicle 102 with high-level commands in this manner, enormous demands of data and inefficiencies in giving instructions are avoided in the present invention.

The navigator 406 stores the routes as a linked list of path segments instead of the set of sequences of sentences of the individual points. These segments are also abstractions of the set of points between defined locations or nodes.

A LINKER then takes the given path segments and creates a linked list of control points, allowing flexibility and efficient arrangement. Path segments are shared by different routes, as shown in FIG. 12.

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

In another embodiment of the present invention, clothoid curves are used in the generation of path segments. These are discussed below.

a. Clothoid path segments

As discussed above, the portion of the navigation problem addressed and solved by the present invention actually has two sub-problems: path planning and path generation. These are solved separately from the present invention.

Path planning proceeds from a set of sub-targets that use some path-optimization function and generates an ordered sequence of "lens points" that the vehicle 102 must reach.

The challenge of path generation is to generate from the objective points (path planning) a continuous collision-free path 3312 that is smooth enough that the autonomous vehicle 102 can easily follow it. For example, a simple scheme is to decompose a path 3312 into straight lines and circular curves. The path 3312 is then divided into a sequence of separate destinations or Instructions are provided which are provided to the actuators of the vehicle 102 to keep the vehicle on the desired path 3312. It should be noted that in simplifying the illustration of these plans, there are disadvantages in the simple way in which they can be performed (Figure 21).

The ability of an autonomous vehicle 102 to track a set path 3312 depends on the characteristics of the path. Continuity of curvature and the rate of change of curvature (sharpness) of the generated path is of particular importance as these parameters dictate steering motions required for a vehicle 102 to remain on the desired path 3312. Unevenness of curvature can be impossibly followed as it requires infinite acceleration. For some autonomous vehicle configurations, the extent to which the sharpness of a path is linear is the extent to which steering movements are likely to keep the vehicle on the desired path 3312 because the linear sharpness of a path is equal to an approximate constant steering speed.

One method used by the present invention is to assemble paths as a series of straight lines and circular arcs. This procedure suffers from discontinuities in the curve where the arcs meet. Another method of the present invention is to use polynomial splines to fit paths between lens points. Splines ensure continuity of curvature, but do not guarantee any linearity of sharpness.

The fact that one can not track the required curvature results in a steady state offset from the desired path 3312. These errors can be compensated by closing a feedback loop at position 3314. This is sufficient in those scenarios where the actuation of the actuators is fast enough to cause negligible tracking. Guarantee tracking errors, and where position sensing is accurate, such as on a factory floor. However, path tracking is easier if you can follow the path more easily.

The method of the present invention generates explicit paths that pass through a sequence of lens points. A derivative method of the present invention re-schedules portions of the path in the event that the tracking error becomes large or the desired path is changed.

b. Modeling a vehicle path

Any path may be parameterized as a function of path length (s) by position coordinates (x (s), y (s)) 3304. That is, position coordinates x and y can be represented as explicit functions of the path length. The orientation (o (s)) 3318 and the curvature (c (s)) 3316 can be deduced:

o (s) = dy (s) / dx (s) (Eq. 1)

c (s) = do (s) / ds (equation 2)

The quadruple arrangement of these parameters P = (x, y, 0, c) is a location 3314 describing the state of an autonomous vehicle 102 at any one time.

c. Clothoid curves

Clothoid curves are used in one embodiment of the present invention. They are a family of curves that are continuous, and that are different in that their curvature varies linearly with the length of the curve:

c (s) = ks + ci (equation 3)

where k is the rate of change of the curvature (sharpness) of the curve and where the index i denotes the initial state. A clothoid curve segment 2002 is shown in FIG.

For a given initial location, the sharpness of the clothoid segment and the distance along that segment, the position and orientation, and the curvature at any point are calculated as follows:

d. Generation of a continuous position path

Practical navigation problems require compound paths whose scope and complexity can not be met by a single clothoid segment. Most paths require multiple segments that pass through a series of lens points.

(1) Existing procedures

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

An article by Kanayama et al., Titled "Trajectory Generation for Mobile Robots," Robotics Research: The Third International Symposium, ISIR, Gouvieux, France, 1986, uses paired clothoid curves with transitions from a straight line between the layers. The restriction of the straight-line transitions is due to the integrals in equations (7) and (8) which have no closed-form solutions. Kanayama simplifies this problem by asking that the following holds: Ci = 0. By rotating the reference frame or reference coordinate system by the amount of initial orientation, oi = 0, there will be only a straight line approximation of the following shape:

sin (kξ²) dξ

Kanayama's method results in paths that are sharper at some points and less compact than necessary, with detrimental consequences for the controller. In addition, the requirement of straight-line transitions implies the local re-planning of paths, since there is no guarantee that a segment to be re-scheduled will include a non-curved section.

Second Path generation from a sequence of points

A two-step method of the present invention for creating a unique continuous-position path from a sequence of points will now be described.

Referring to Figures (13, 14 and 15), the first step is to derive a sequence of unique layers 2302, 2304, 2306, 2308, 2310 from the lens points. The second step is to interpolate between these layers with the clothoid segments. The orientation and curvature at the beginning and end of the positions 2402, 2404 are presumed. Let Pi, Pf respectively be the start and end position 2402 or 2404.

It is not always possible to join two layers to a clothoid curve segment since four equations, Eq. 2, Eq. 4, Eq. 5 and Eq. 6 can not be solved simultaneously with only two parameters of a clothoid curve (sharpness k and length s).

To get the four equations, Eq. 2, Eq. 4, Eq. 5 and Eq. 6, one needs at least two clothoid curve segments. However, the general problem can not be solved by two clothoid segments, because if ki and kf have the same sign, in most cases a third segment in between is required. An adequate set of clothoid curves connecting a pair of adjacent associated layers is the set of three clothoid segments (k, s 1), (-k, s 2), (k, s 3). These indices denote the order of Pi's clothoid segments. This combination is plausible for the following reasons:

1. The signs of k for the first and last clothoid segments are the same.

2, k for the second clothoid segment is equal in magnitude and has the opposite sign as the first and last segments. This allows the curve of three clothoid segments to determine the curvature variation between the beginning and the end of the curves by varying s 1, s 2, s 3. fulfilled, even if the sign of the first and last Clothoid segments meets the requirement of cornering.

Third There are four variables in the combination: k, s 1, s 2, s 3. It is possible to find a unique solution that satisfies the following four equations describing the mathematical relationship between the start and end positions.

cf = ci + k (s 1 -s 2 + s 3) (Eq. 7)

where is true

Referring now to the method shown in FIG. Since Equations 9 and 10 above contain Fresnel integrals for which there is no solution in closed form, the values ​​k, s & sub1 ;, s & sub2; and s & sub3; calculated.

Paths resulting from the process have the following advantages over other methods:

- The procedure is based on an arbitrary sequence of points. Creating plies is important to progressive planning, where goals are collectively presented as an evolving sequence of points. Paths generated by the method pass through all the lens points, while paths from the Kanayama method and the arc method are only near many of the points since these methods start from a succession of layers.

- The process guarantees the continuity of the position, orientation and curvature along the path. Furthermore, the sharpness is piecewise constant.

- Paths generated by the method are always outside the acute angles formed by a straight line connection of the waypoints. The resulting paths are particularly useful for interpolating around obstacles that lie together on the inside of the angles. In contrast, Kanayama's trails are always within the angle.

Third Rescheduling Clothoid Paths

The re-planning of clothoids is performed either to initially pick up the path or to route the vehicle 102 back to the desired path 3312 through normal navigation in accordance with the present invention.

To avoid abrupt accelerations in an attempt to make strong corrections in tracking a predetermined path, re-scheduling the path is used by the present invention to create a new path that smoothly leads to the desired path 3312 from the current one Position converges. The re-planning decomposes into two sub-problems:

1. Determination of the convergence point to the intended path 3308.

Second Plan a path from the current position 3302 to the convergence point 3308.

Reference is now made to FIG. 18 which graphically depicts a re-scheduling of a path in accordance with the present invention. A predefined path consists of interpolations 2804 between locations (k, s) m (m = 1, ..., n) 2804-2810 and the locations Pm (located at the end of the segment (k, s) m). Assuming that the vehicle 102 deviates from the path between Pm and Pm + i, then Pm + 2 is selected as the location 334 to which the re-planned path 2816 converges. The distance to Pm + 2 is variable.

A curve composed of two curve segments is fitted into the layers (the current layer and the one selected as the convergence point) to obtain a re-planned path 2816 that satisfies four rule-layer equations: Eq. 7, Eq. 8, Eq. 9, g. 10. If we assume that the threshold that determines whether or not a path is to be rescheduled is much smaller than the length of each clothoid curve segment (k, s) m, we can find a new continuous position path ((k * k + 1, sk + 1), (K * k + 2, s * k + 2)) using a small perturbation from the known expression ((kk + 1, 5k + 1, (kk + 2, sk + 2)) , Since the re-planned path 2816 is not likely to be very far from the original path 3312, two clothoid segments may be used.

4th Summary

According to the present invention, the creation of continuous paths for autonomous vehicle 102 may use clothoid segments to create paths, not only because the resulting path is continuous in position, but also because the linear curvature along the curve results in steering angles. which vary approximately linearly along the path, facilitating path tracking.

The approach of the present invention is as follows: First, a sequence of plies using the lens points is obtained. Then, each of the adjacent layers is connected to three clothoid curve segments.

The present method has additional advantages in that preprocessing of the lens points is not necessary, as in arcs and clothoids without curvature. Furthermore, the geometry of the generated paths is always outside the acute angles formed by a straight-line connection of the waypoints. This is particularly useful for interpolating around obstacles that are usually on the inside of the angles.

From the set of stored arcs, lines and plies, clothoid curves, B-splines, etc., points are created along a path with the VPS location block.

Advantages of handling routes in this manner by the present invention are, in addition to reducing the bandwidth requirements between the host and the vehicle, effects of data compression, which reduces data storage requirements and works to smooth paths.

5th B-splines

B-splines are well known to the mathematician and computer graphics expert (see "Mathematical Elements for Computer Graphics" by David F. Rogers and J. Alan Adams, McGraw-Hill Book Company, New York, NY., Pages 144-155 ) as a means of describing the shape of a series of points by setting the coefficients of a polynomial equation. This curve fitting function is an Nth order polynomial where N is set by the user and depends on the desired shape of the curve. The B-spline curve may be of any order and is continuous with the order of the curve fitting function minus one.

B-splines are used in one embodiment of the present invention. B-splines are very well suited for path generation in the present invention, since an arbitrary long path can be described by a small number of coefficients, thus reducing the amount of data storage. Provided that the order of the curve fitting function is high enough (three or greater) then the path created will be smooth with respect to the curvature, resulting in a path that is inherently easy to follow with the aforementioned embodiments of the present invention.

Fig. 19 shows an example of B-spline curves

Second Route generation and storage

a. introduction

In one embodiment of the present invention, data is first collected and stored by the vehicle positioning system 1000 to generate routes for a terrain 300 while a human travels the vehicle 102 over the road system of the work site 300. Nodes and segments are then fitted to the stored drive data and organized into routes for the aforementioned method.

An application on an Apollo computer workstation (now Hewlett-Packard, Palo Alto, California) (not shown graphical display system) has been developed to graphically fit route data to the stored wandered data and further define routes (ie, speeds, sequences, a Starting point, a transverse direction). Any graphics workstation or graphic computer equivalent to the Apollo could be used.

Once the routes for a terrain 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 bladder storage cartridge having an associated read / write device. The bladder memory device is durable and holds the data when the power supply is disconnected. The Apollo application can write data to a cartridge and read data from a cartridge.

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

In mining applications, terrain 300 is generally monitored and roads are pre-planned, carefully designed and built. The routes used by the navigation system may then be obtained either from a manually generated computer database (specially created to be used by the navigation system), or alternatively, a vehicle may be physically driven over the actual routes on the site to learn the routes as described above. In the learning process, various journeys can be made over a given route. Then, the changes in the data (for example, due to the driver shaking) are grouped together and a smoothed best fit is developed.

b. route definition

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

1. Define nodes and segments that make up the routes. Put the node and segment data in an array called "route data arrangement". Each record in the array contains the following information:

1. Type of part (ie node, linear segment, circular segment and end-of-route marker)

Second If it is a node part, define the longitude and latitude coordinates of the node.

Otherwise, if it is a linear segment part, define the velocity along the segment.

Otherwise, if it is a circular segment, define the latitude and longitude coordinates of the center, the radius, the direction in which the circle passes (ie, clockwise or counterclockwise), and the velocity along the segment)

Otherwise, if it is a mark of the end of the route, there is no further information.

Second Connect the node and segment data to each other in sequences. The sequences are simply an array of indices in the route data arrangement. Each sequence must begin with a mark of the end of the route followed by a node, then the rest of the sequence will alternate between segments and nodes until the sequence is terminated by another mark of the end of the route. An example sequence would be 1, 6, 3, 4, 7, 9, 10, 23, 78, 1 where the integers are indices in the route data array.

Third Finally, define a route by setting an index in the order of succession, and defining whether to run indexically through the sequence in the positive or negative direction. Place the index and index direction in an array called the "route description arrangement" (the routepec arrangement). A part of the route description arrangement may look like this:

6.1 this description defines a route that starts at node 6 and is indexed in the positive direction.

78, -1 This description defines a route that starts at node 78 and is indexed in the negative direction.

A user simply tells the vehicle which part in the route description arrangement to use as a route.

4th The aforementioned data is stored in the storage device in the order in which they were defined in steps 1 to 3.

C. Navigator route usage

The following describes how navigator 406 uses the defined routes from the above method of the present invention.

When the navigator 406 is turned on, it reads the route information from the storage device 5302 and stores it in the random access memory (RAM) in the syntax already shown.

Next, the operator sets a route to follow the vehicle 102. Again, the route is simply an index to the route description arrangement.

When the navigator 406 decides that all systems are ready for automatic operation, it sends a message to the VPS_Lage task to cause it to operate.

The VPS_Lage task then determines the position along the route that is closest to the current position 2812 of the vehicle 102. The search for the next position 284 on the route proceeds as follows:

1. A pointer is set to the first segment in the route.

Second The vertical distance from the vehicle position to the segment is determined.

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

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

5th Repeat steps 3 and 4 until the end-of-route marker 2218 has been reached.

6th Determining the distance from the vehicle position to the endpoints 2218 of the route.

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

The VPS_Lage task then uses the route description (lines, arcs, and velocities) to create a location at one-meter intervals. The task creates a predefined distance of layers plus a safety frame and places the layers in a buffer 3000. To create a layer one meter from a given location, the VPS_lay task uses the following procedure:

1. Determining the type of segment from which the given position was generated.

Second Apply the proper formula for the type of segment to determine the change in length and width per meter of segment length.

Third Adding the change in length and width per meter to the last given location.

4th If the created location is beyond the end of the current segment, set a pointer to the next segment and repeat steps 2 and 3, otherwise return to the created location.

The VPS_Lage task then informs the executive that it is ready to persecute.

When the autonomous vehicle 102 moves along the position in the buffer 3000, the security frame 3006 is cleared.

If the security frame is below a specified size, the VPS_lay task creates another security frame 3006 of layers and appends it to the current buffer 3000. The VPS_Lage task flushes the location buffer 3000 by monitoring the current position 2812 of the vehicle 102 and moving a pointer 3002 into the buffer 3000 to the next location. The position buffer 3000 is constructed as a ring, which is passed through in a clockwise direction (see FIG. 20, storage buffer). That is, layers are arranged in the ring so that the direction of vehicle movement corresponds to passing the bearing-in buffer 3000 clockwise. Therefore, as the vehicle 102 moves, the pointer 3002 will be moved to the next location in the buffer 3000 in a clockwise direction. When the pointer 3002 is moved clockwise, the memory in the ring is vacant (counterclockwise from the pointer) to be overwritten.

Step 7 (in the search routine above) is in action until the end of route marker 2218 is reset, at which time the VPS Location task ceases to generate a location and the executive is informed that the end of the route was achieved.

As mentioned above, a path is like a series or sequence of contiguous "layers". The location includes the speed and longitudinal angle that need to be tracked. A ply may have the width, length, orientation, curvature (1 / radius of curvature), maximum speed, and distance to the next location information.

Third location production

The tracking method of the present invention requires some information about the route being tracked. The information is contained in a package called "Location" 3314. A single layer 3314 may include a location (latitude and longitude coordinates), orientation, and curvature data for a particular location on the route. Therefore, a way to generate location data from the route description according to the present invention is required.

Among the navigator tasks (discussed below) is a task that reads out the route information and generates locations at intervals along the route (for example, at one meter intervals) used by the tracking method. In one embodiment of the present invention, each layer requires 36 bytes of memory, yielding approximately 36K of memory for every kilometer of the route. To reduce memory requirements, the navigator buffers the location data.

The task generating the plies reads out the current position of the vehicle 102, finds the next point on the route to the current position, then generates a fixed number of plies in front of the vehicle 102. The number of plies generated depends on the maximum Stopping distance (braking distance) of the vehicle 102. That is, there should always be enough layers in the buffer 3000 to guide the vehicle 102 to a stopping point.

However, in the route definition B-spline approach of the present invention, the need for a location buffer is eliminated because the tracking method can directly generate location information from the B-spline curve.

C. path tracking

Path tracking is a critical aspect of vehicle navigation in accordance with the present invention. The technique of the present invention utilizes position-based navigation (rather than sight-based navigation used in conventional navigation systems) to ensure that the correct path 3312 of the autonomous vehicle is tracked. The present invention is also novel in that it provides separate control of steering angle and vehicle speed. Fig. 23 graphically illustrates the path tracking system of the present invention.

For an autonomous vehicle 102 in accordance with the present invention, it is necessary to trace reference paths for the vehicle servo-control devices to track fixed paths. Thus, path tracking may be considered a problem in obtaining a reference steering angle and a reference speed for the next time interval to return to the reference path before the presently-deviated position.

Generally speaking, the path trace determines the commands of the autonomous vehicle (speed, steering angle) required to follow a given path. For a given predetermined steering angle, driven wheel speed values, and error components, the steering command and drive inputs are calculated in the present invention.

D. Obstacle treatment

1. introduction

The obstacle treatment provides at least three major functions: detecting obstacles 4002, avoiding obstacles 4002 and returning to path 3312. The function of returning to the path is similar to the path creation and tracking as described above.

In addition to path tracking (tracking), successful navigation of vehicle 102 requires that vehicle 102 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.

In one embodiment of the present invention, an infrared laser scanner 404 is used with a single line (see FIG. 25) in a configuration where the scan is horizontal (not shown). The scan line 3810 does not touch the ground, so any inequalities in the area data may be associated with the objects 4002 in the environment.

Since a reference path 3312 is available and the vehicle position relative to the reference path is known, only the area data and a region boundary of the threat path reference path 3312 4002 are processed. The objects outside this region or boundary zone are ignored. The width of the boundary zone (not shown) is equal to the vehicle width plus a certain selected safety buffer to compensate for tracking and positioning errors. This method is limited in its usefulness and is referred to as "free space check".

Second Detection of obstacles

In a simple case, the laser 404 may be used in a single line scanning mode with subsequent range measurements made at regular angular intervals as the laser sweeps across the field of view. Again, for simplicity, these samples may begin at regular time intervals. The term "free space check" has been used to describe this method. In this release, the process has been limited to processing only two-dimensional data.

This type of obstacle detection method is limited to checking to see if the path 3312 is free using a single line scan mode with subsequent range measurements performed at regular angular intervals when the scanner 404 over the field of view. It does not exclude any procedures for setting up or Check the existence of any obstacle 4002, nor does it create a path around it if the path is not clear. This type of method is not considered to be a particularly useful obstacle detection method except in highly controlled environments such as on a factory floor.

Third Avoiding obstacles

Once the present invention detects an obstacle 4002 in the path of the vehicle 102 (see FIG. 26), it must then avoid collision with the object. Certain assumptions are made regarding the obstacle avoidance problem:

1. The obstacle environment is populated with obstacles 4002, which may be represented by convex polygons or convex lines;

Second The navigation methods have access only to the local environment information in the form of a local map representing all visible end-faces of the obstacle from the position of the vehicle 102, which may be obtained from unprocessed laser range data or data processed by spot extraction ;

Third The vehicle 102 is a conventionally steered type having limitations on its speed and acceleration, and restrictions on the steering angle and the rate of change of the steering angle.

To cope with the problem of avoiding obstacles, the present invention divides this into two sub-problems.

First, the distinction as to whether any obstacles are in the way, and if so, to which side the vehicle should proceed. Then, the selection of a sub-goal 4006, which will lead the vehicle 102 around the obstacle 4002, resulting in a higher-level goal 4008, which is to return to the desired path.

Second, once a sub-goal 4006 is selected to make a steering decision that drives the vehicle 102 toward the sub-goal 4006 while steering around the obstacle 4002. A sub-target selection method and a steering decision method of the present invention solve these two sub-problems.

The assumptions listed above are treated in the following process:

The obstacle locations are obtained from the laser range scanner 3804 or 404. The area data generated by the scanner 3804 or 404 is processed to produce a list of polygon faces, with the visible portions of the obstacle 4002 being modeled from the vehicle position. Each time new area data becomes available, a subdestination selection process is executed to generate a subdestination 4006 and determine regions of safe navigation (clearance 4010) for the steering decision process. The frequency at which the sub-target selection process can be performed depends on the rate at which the scanner 3804 or 404 can collect data. The achievable vehicle speed in turn depends on this execution frequency.

For the steering decision process, a higher sampling rate is desirable to create a smooth path. Therefore, the steering decision process is executed more often than the sub target method.

The basic flow of the sub-target process is as follows:

1 save last initial subdestination, subdestination and clearance, set target blocked flag to true

2 when the final destination is visible,

create direct target if the direct target is visible

set target_blocked flag to false

3 otherwise

create an initial sub-goal set sub-target on initial sub-goal recursive generation of sub-targets until the last one is visible, if the sub-target is unavailable, abort

4 if target blocked flag is true

restore old starting subdestination, subdestination and clearance

5 otherwise

Creation of the free space when the clearance is not secure

restore old starting subdestination, subdestination and clearance.

Subtotal Method: First (Step 1 above) Initial subdestination, subdestination, and free space created in the previous iteration are stored. This ensures that if the newly created sub-destination is not secure, the old sub-destination can be tracked further.

Next (step 2 above), if the final destination is visible, try to create a direct destination that is not associated with any of the obstacles 4002. Although the final destination is visible in the local map, this does not necessarily mean that no obstacle will block the final destination because obstacles outside the scanner area (both in terms of distance and angle) are not displayed in the local map. Therefore, if a direct target is generated, ensure that the target is located in the cone area covered by the scanner 3804 or 404 to avoid placing a sub-target on or behind an obstacle 4002 that is not in the local area Card is.

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

Given a flowchart for a blocking obstacle, there are two possible ways to get around this. If both edges of the obstacle in the area of ​​the scanner are 3804 or 404, we can choose to walk around the edge that gives the minimum sum of the distances from the vehicle 102 to the edge and from the edge to the final distance. If only one edge of the obstacle 4002 is in the range, the edge to be traveled around is selected. If none of the edges are visible, the left corner is always arbitrarily selected to go around there. Once the edge to go around has been determined, the initial sub-target is placed away from the edge by a distance that is proportional to vehicle size.

Because of this shift, the resulting sub-goal may be blocked by other obstacles 4002. This invokes the recursive generation of the sub-goal on the obstacle which blocks the line of sight to the currently generated sub-targets. This recursive process continues until a sub-destination that is visible from the vehicle 102 is generated. Each subgoal thus generated is checked for reachability. By reachability is meant that the sub-target does not lead the vehicle 102 to a gap between two obstacles 4002 that is too small for the vehicle to pass through. If such a condition is detected, the vehicle 102 will stop.

The direct subdestination created in the second step (step 2 above) could possibly be obscured from the vehicle 102. If so, the old subdestinations are restored from the previous iteration and used next (step 4 above).

In the last step (step S above), the visible sub-destination clearance 4010 is to be generated, which is a triangular region containing no obstacles. Once the clearance 4010 is created, the security of the sub-goal and clearance 4010 may be determined. If the new subdestination and the clearance 4010 are not safe, the old subdestination and the clearance are restored. Otherwise, the new subdestination and the new space will be used.

The steering decision method of the present invention consists of two main components: transmission of state restrictions to control restrictions; and determining the desired control vector.

Once the control constraints and the desired control vector have been calculated, the control vectors may be determined using optimization techniques that are well known in the art.

4th Return to the path

The present invention includes a method as diagrammed in FIG. 26 whereby a secure path around a detected object 4002 is recorded and navigated so that the vehicle 102 will again reach the reference path after having received the object 4002 avoided.

5th scanner system

Referring to FIG. 25, the exemplary system also includes a laser scanner system 404. The scanner 404 is used to find obstacles 4002 (see FIG. 26) that incidentally accumulate in the path of the vehicle 102, as previously discussed.

Sources of such obstacles 4002 can be varied and numerous, depending on the particular work site. You may have fallen trees and branches, rocks, moving and parked vehicles, and people.

The scanner 404 gives the autonomous vehicle 102 the ability to detect and cope with the surrounding world, as circumstances require.