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-3310111 discloses a navigation system with drift compensation. US-A-3630079 discloses a navigation system using multiple sensors and error correction.

The Scripture "GPS / PLRS Aided Inertial Land Navigation System Performance" by S. Bose from IEEE Plans 86 Position Location and Navigation Symposium, 4-7. November 1986, Las Vegas, USA, pages 496-504 discloses a computer-based system for autonomously navigating a vehicle along a predetermined path.

DE-A-39 12 353 discloses an autonomous vehicle traveling on a travel route and the associated method.

In accordance with the present invention, a system is provided for enabling a vehicle to follow a predetermined route by generating a series of paths to be followed, the system comprising a system for generating the paths comprising: :

Means for storing route data representing start and end points on a predetermined route;

Means for storing sets of compressed path data for each predetermined route;

Means for successively extracting sets of compressed path data associated with a predetermined route (3312); and further characterized by

Means for successively generating a continuous path from each of the sets of compressed path data.

The present invention also provides a method performed in a system to allow a vehicle to travel a predetermined route by creating a series of paths to follow, using the method of creating the paths the method comprising the following steps:

Storing route data representing start and end points on a predetermined route;

Storing sets of compressed path data for each predetermined route;

successively extracting sets of the compressed path data associated with a predetermined route; and further characterized by successively generating a continuous path from each of the sets of compressed path data.

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;

Fig. 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 representation 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 deposit;

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 to calculate an error vector including a curvature;

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 selected scanning lines in a laser scanning system;

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

Fig. 28 is a block diagram of a laser scanner system used for obstacle detection;

Fig. 29 is a block diagram 4300 of a control system for an autonomous mining vehicle;

Fig. 30 is a state diagram showing the transitions between the operating states of the control system of Fig. 29;

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 * 10 & sup8; 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 108 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 (1)" 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 in the present invention. 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 gravity) as measured by the accelerometer is integrated into a mathematical navigation equation to generate the vehicle position and speed. In the preferred embodiment, the inertial reference unit is a part of the moving position determining 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 passing them at the speed of light or with 2.9979245898 * 10 & sup8; Meters per second multiplied.

(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 can be calculated by a number of different methods. 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. 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. In the preferred embodiment, position estimates from the vehicle positioning system are referred to 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 an exemplary 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

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 can be used. Fig. 1A illustrates the NAVSTAR GPS. GPS satellites 130-168 travel around the 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 provide the GPS Improve 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. To increase the accuracy of the first position estimate, the exemplary system uses a number of methods. 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.

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 21. 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 present invention 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. 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 IFD 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 IFD 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 positioning system part of the exemplary system.

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 has been implicitly discussed above, autonomous navigation of, for example, a mining vehicle may 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 present invention could foreseeably include off-earth operations, for example, mining on the moon, provided that the necessary GPS satellites are placed in 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. In the present invention, autonomous position determination and navigation may 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 career 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:

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, in the exemplary system as described above, 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, as below discussed. The system obtains GPS position information from the GPS satellites 200-206 at the base station and aboard the vehicle so that a common operating state error can be removed and the accuracy improved.

In an alternative example, 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 of the present invention may 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 automatic vehicle according to the present invention conceptually consists of two sub-problems, the path generation and the 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 5302 as a set of arcs, lines and layers. For example, in one embodiment of the present invention, an analytical generator function generates paths using these arcs, lines, and layers. In another example, B-splines (curves) are used as a mathematical representation of a route, as mentioned above.

In another example 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 position, 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.

Second 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, sk + 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.

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 generated 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 blister storage cartridge 5302 having an associated read / write device. The bubble memory device 5302 is durable and holds the data when the power supply is disconnected. The Apollo application can write data to a cartridge 5302 and read data from a 5302 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_Location task 5324 to cause it to operate.

The VPS_Lage task 5324 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 5324 then uses the description of the route (lines, arcs, and velocities) to create a location at one meter intervals. Task 5324 creates a predefined distance of plies plus a safety margin and places the plies in a buffer 3000. To create a ply one meter from a given ply, VPS_lay task 5324 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_Layer task 5324 then informs the executive 5316 that it is ready to be tracked.

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 5324 creates another security frame 3006 of layers and attaches it to the current buffer 3000. The VPS_Lage task 5324 clears the location buffer 3000 by monitoring the current position 2812 of the vehicle 102 and by moving a pointer or Pointers 3002 in the buffer 3000 to the next layer. The position buffer 3000 is constructed as a ring, which is passed through in a clockwise direction (see Fig. 20, Lageringbuffer). 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_Layer task 5324 stops generating a location, and the executive 5316 is informed that the end of the Route was reached.

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 the steering angle 3316 and the vehicle speed 3318. Fig. 23 graphically illustrates the path tracking system 3102 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

a. Clearance verification

In the simplest case of the present invention, the laser 404 may be used in a single line scan 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 version of the present invention, the method has been limited to the processing of 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.

b. Filtering and edge detection scheme

A second embodiment of obstacle detection of the present invention uses a multi-line scanner 3804 (see FIG. 25) whose scan 3810 contacts the ground a certain distance ahead of the vehicle 102. Since the scan line touches the ground, unevenness of the area data may no longer be associated with threatening objects 4002. For example, profiles of natural objects, such as hills and sloping or arched roads, may cause unevenness in the area data. This technique of the present invention can distinguish unevenness in the area data between threatening objects 4002 and natural objects (not shown).

In this embodiment of the present invention, a filtering scheme is used to reduce the amount of data processed, and it is independent of the scanner configuration used. The edges of the boundary zone are found by transmitting the area data in an image plane representation 3900 (see Fig. 26) where each area value is located by a line number 3908 and a column number 3910 (a matrix representation).

The processing load is minimized by selecting a relatively small number of scan lines available in a range image representation 3900. The scan lines are selected by the vehicle speed and are concentrated at and above the vehicle stop distance. The selected scan lines of successive data frames may overlap.

In this method, when the vehicle 102 is moving rapidly, the selected scan lines 3906 are far ahead of the vehicle (near the top of the area image representation 3900). In contrast, when the vehicle is traveling slowly, the selective scan lines 3906 are closer to the vehicle (near the bottom of the area image representation 3900).

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

Given the cylindrical description and the known scanner attitude with respect to the vehicle 102, the range values ​​may be converted into a Cartesian coordinate system (X, Y, Z). The result is a road profile description that may be used by a novel filtering scheme to determine if threatening objects 4002 are present in the vehicle path 3812, while ignoring effects due to natural hills and valleys on a typical road.

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

The filter scheme builds an expectation of the road profile from previously sensed road profiles. This expectation is based on three parameters which are found to adequately describe typical paved roads. These three parameters are the following:

- Road curvature: The curvature of the road cross section (perpendicular to the road center line).

- Road slope: The "slope" of the road profile (perpendicular to the center line).

Road height: The height of the road centerline above a reference plane, which is described by the location of the four wheels of the vehicle 102.

Expected values ​​of road curvature and road grade are determined by performing a standard Kalman least squares filter technique on previously sensed scanner data. The Kalman filter basically holds some kind of running average of the two parameters based on the values ​​determined from the previous data.

The expected road height for a particular scan may be determined according to the present invention by one of two similar methods.

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

The second method is to filter the road height using the standard Kalman filter, similar to the one used when estimating the curvature and slope.

These three parameters can be used to determine a second order equation describing the expected road profile. This expected profile is compared to the actual road profile. Any discrepancies between the two that exceed a pre-set threshold are likely to be threatening objects.

This scheme of the present invention is feasible on the assumption that any detected objects 4002 are small compared to the width of the road. When using these averaging methods or least squares methods, the effects due to objects are negligible compared to natural road data.

This filter scheme also has a very simple edge detection method which associates the selected area data with a simple seven point weighting function.

c. obstacle extraction

An additional technique of the present invention processes an entire area image representation 3900 from a multi-object scanner 3804. This procedure achieves three goals:

1. Do not recognize any obstacles 4002, if none exist,

Second Recognize obstacles 4002 when obstacles exist, and

Third Recognize the correct obstacles 4002 when obstacles exist.

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

The obstacle extraction may be performed by preforming the following steps in the image plane 3901:

1. Project the vehicle path onto the image plane 3901,

Second Convert the area data into elevation data,

Third Adjust a curve to the height in the middle of the road (this represents the expected road height for each line),

4th Make the threshold of the actual road height over the altitude expectation, and

5th Extract the obstacles (indicated by differences in actual and expected road heights that exceed the threshold).

(1) Finding the road

To process all available data, the images must be processed at the frame rate of the 3804 scanner. For this reason, most of the calculations in the obstacle extraction process in the image plane 3901 are performed. By projecting the path into the image, much of the image can be ignored and many useless calculations avoided.

Assuming that the vehicle path 3812 is set at regular intervals, the current vehicle position may be used to locate the path segment line 3902 in front of the scanner. This path 3812 is converted from world coordinates to image coordinates by projecting the points corresponding to the road or road edges 3902 into the image plane 3901 (see FIG. 26).

A cubic spline (curve) is used to interpolate between the columns. Thus, the center and edges of the road 3902 are found for each line 3908 in the image plane 3901. The pixels isolated between the road edges 3902 are converted (cylindrical into Cartesian coordinates) from range to height data. The outer pixels are stored and not further processed in any way.

(2) modeling the road height

Once the center of the street for each row 3908 in the image plane 3901 is known, the height for each of these points can be determined. A least squares third order curve fits into this data.

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

(3) thresholding

Obstacles can be located by using a height threshold. A threshold of even height would be meaningless because the surrounding terrain is not necessarily flat. Therefore, the threshold is seen with reference to the expected height, as predicted by the third-order fit, at the row number 3908 of the given pixel.

In this way, a hill is not considered an obstacle, since the altitude expectation and the actual altitude should be very close together. On the other hand, a real obstacle 4002 would just render the expected road height (due to the least squares fit) and is therefore easily found by thresholding. The result of this thresholding is a binary image (not shown) suitable for "spot extraction". The binary image only indicates where an object exists in the image or not.

(4) spot extraction

The spot extraction works by clustering adjacently set pixels (indicating that an obstacle 4002 is present) with each other and treating them as a unit. Two pixels are adjacent if they either:

1. In the same column 3910, and consecutive line numbers have 3908, or

Second in the same line are 3908 and have consecutive column numbers 3910.

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

(5) Applications

One way to apply extracted patches is to pass them (pipe) as input to another program. For example, the objects 4002 may be decomposed into coordinates and used to collect a global feature map 4004 (see FIG. 27). This card 4002 is then routed to another program and used to perform collision avoidance or path planning.

Third Avoiding obstacles

Once the present invention detects an obstacle 4002 in the path of the vehicle 102 (see FIG. 27), 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:

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 this is indeed the case, 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. 22 whereby a safe 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

a. introduction

Referring to FIGS. 25 and 28, the present invention also includes a laser scanner system 404. The scanner 404 is used to find obstacles 4002 (see FIG. 27) 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.

b. laser scanner

The main components of the laser scanner system 404 are shown in FIG.

A laser range finder 3804 uses an infrared ray 3810 to measure the distances between the range finder unit 3804 and the next object 4002. A short pulse is transmitted by the unit 3804, and the time taken for the beam 3810 to reflect and return from an object 4002 gives the distance.

The beam 3810 from the range finder 404 is reflected by a rotating mirror 4222, giving the range finder 404 a 360 ° view of the world. The mirror rotation is achieved by an engine 4206. The engine speed is controlled via port 4210, which communicates with a motor amplifier and controller 4220 through a conventional serial RS232C link 4224. The synchronization between the laser fire and the mirror angle position is performed with an encoder.

Distance data on a line 4226 from the laser range finder 404 is received by an interface circuit 4228 which differentially transfers the data to a buffer circuit 4214. Individual pieces of data are collected by the buffer circuit 4214 until the mirror 4222 makes a full turn. This record has a scan. When a scan is completed, the buffer circuit 4214 signals a processor 4212, whereupon data is transferred from the full scan to the processor 4212 for processing.

c. Scanner system interface

The interface circuit 4228 has three functions.

First, it acts as a security monitor. A situation could occur where the mirror 4222 would stop rotating, as in the case where the drive belt 4230 between the motor 4206 and the mirror 4222 breaks down. In this condition, the laser 4204 would continue to fire, and since the mirror 4222 is stationary, it would shine to a single point (dangerous to someone looking directly into the laser beam). However, the interface circuit 4228 senses when the angular velocity of the mirror 4222 falls below half a revolution per second and shuts off the laser 4204 when such a condition occurs.

The second function is to turn off the laser 4204 from firing for a portion of the scan area of ​​360 degrees. Typically, the laser scanner unit 404 will be mounted on the front of a vehicle 102, and the interesting field is in the 180 degree front of the vehicle. The vehicle itself will block the back of the scan area of ​​360 degrees. In this case, the circuit 4228 will prevent the laser 4204 from illuminating the vehicle, extending the life of the laser diode while taking data for the area in front of the vehicle. The turning on and off of the laser range finder 4204 is performed by two sensors (not shown) mounted near the mirror housing 4222. For test purposes or for applications where a scan of 360 degrees is desirable, the turn-off feature can be turned off by a DIP switch.

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

d. Scanner system buffer circuit

The function of the buffer circuit 4214 is to synchronize the firing operations of the laser 404 with the angular position of the mirror 4222 to collect data for a complete scan and transmit the scan to the computer 4214 for processing.

The angular position of the mirror 4222 may be determined by signals sent from the encoder 4208. The buffer circuit 4214 uses two signals from the encoder 4208: the Z and A channels.

The Z channel is the encoder index (pointer); is set once per revolution of the encoder 4208 and is used to signal the beginning of the scan area.

The A channel is one line of the two-wire quadrature output of the 4208 encoder, giving pulses a thousand times per revolution of the encoder. This channel is used to trigger the laser illuminations.

An additional signal is needed to completely synchronize the scan field with the encoder signals. There is a 2: 1 gear ratio between the 4206 encoder / motor and the 4222 mirror. Two turns of the 4208 encoder rotate the 4222 mirror once. This is converted into 2 Z-channel pulses and 2000 A-channel pulses per revolution of the mirror 4222 and the inability to distinguish the beginning of the first half of the sample from the beginning of the second half.

To completely synchronize the scan field, the DB signal (DB = Deadband) generated by interface circuit 4228 is used. The DB signal, which is used to turn off the laser 4204 to fire in the back of the scan, allows the discrimination of the front and back halves of the scan. The Z signal and the DB signal together signal the beginning of the scan area.

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

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

The ultimate task of the buffer circuit 4214 is to transmit the sample data to a computer 4212 for processing. Completed samples are signaled by the Z and DB signals (the beginning of a sample is also the end of a previous one). In a complete scan is an interrupt or Associated with the interrupt request line, and remains allocated until either the mirror 4222 has made half a turn, or the processor 4212 acknowledges the interrupt. In the first case, half the revolution of the mirror 4222 is signaled by a subsequent Z-pulse and indicates a timeout condition; the processor 4212 has failed to respond and the data is lost.

In the normal case the break is recognized. Upon receipt of the acknowledgment, STR (data strobe) is allocated and held until IBF (input buffer full) is received. During this time, data is placed on the data bus 4230 and can be read by the computer 4212. Data is valid on bus 4230 until IBF is allocated, at which time STR is no longer allocated and the data is removed from bus 4230. Once the processor 4212 detects the removal of STR, it removes IBF. This causes STR to be allocated for the next data parts, which repeats the cycle.

Sampling data is collected and stored in two memory banks 4214. This avoids problems with shared memory and synchronization between the sample memory and the sample transfer. Data for a new scan is stored in one bank while the previous scan is transferred from the other bank.

The buffer circuit 4214 takes the responsibility of the processor 4212 to synchronize the laser results with the mirror position and to collect individual pieces of data. It allows more efficient use of CPU time when capturing data in one-sample increments. The processor 4212 spends its time processing the data rather than collecting it.

E. Vehicle control systems

1. introduction

Referring to Fig. 29, the vehicle controls consist of four functional blocks at a low level.

One is called "Vehicle Manager" (4302). A second is called "speed control" (4304). The third is called "steering control" (4306). The fourth is called "Monitor / Auxiliary Control" (shown as two separate blocks 4310 and 4308). These are again described below.

They are all connected by a high-speed serial data bus 4314. Bus 4314 is a data collision detecting packet forwarding system.

Each of these functional blocks has separate microprocessors, such as the Motorola 68000 16-bit series. Each of these microprocessors talks to and hears about the bus 4314 with the others.

While each functional block has a more or less specific function, the vehicle manager 4302 acts as a communications hub. It sends messages to the Navigator 406 via a 9600 baud serial R5-422 connection 4316 and also receives messages from them. It also listens to the remote control or "teletafel" 401 via an FM radio communication link 4318 and transmits to it.

Second Vehicle manager (operating states)

As mentioned above, the vehicle manager 4302 receives commands from a remote control panel 410 and the navigator 406. It then decides in which operating state "A, M, T or R" (for autonomous, manual, tele or ready) the vehicle 102 should be.

a. Ready mode (ready)

Reference is now made to Fig. 30, which shows the states (operating conditions) and how the vehicle 102 changes between states. The Navigator 406 can not set the operating state by itself. It should be noted that the vehicle 102, for example, can not switch directly from telephoto to automatic. In this case, it must first pass through ready state 4404.

The ready mode 4404 causes the vehicle 102 to stop in a known state. This is because it would be difficult to make a smooth transition from, for example, the automatic operating state 4408 to the tele-operating state 4406 while the vehicle 102 is moving. The telephoto panel operating lever 4502, 4504 should be in exactly the right position when the control is switched.

When going from the teleprocess state 4406 to the automatic operating state 4408, there is the consideration that the navigator 406 must initialize. For example, he must determine where he is with respect to a route before taking control, which takes some limited time during which the vehicle 102 could otherwise break out in an uncontrolled manner.

b. Tele mode

The telecontrol mode 4406, also referred to as telephoto, remote, or radio control mode, provides a way to control the vehicle 102 from a remote location while maintaining the vehicle 102 in view.

Workshop personnel, for example, would use the telephoto state 4406 to move the vehicle 102 in the yard, for example. Advantageously, this operating condition would also have to be used by a shovel or loader operator to maneuver the vehicle into a loading or unloading position and move the vehicle to a location where autonomous operating condition 4408 would resume control.

In telephoto state 4406, each vehicle 102 at an autonomous workstation 300 would have its own unique identification code selected on a radio control panel 410 to ensure communications only with the correct vehicle and control thereof. The vehicle 102 would only respond to the teleprocess commands 4318 when its unique identification code is transmitted. Any conflict between operating conditions, such as between manual operating state 4402 and teleprocess state 4406, would be resolved for obvious safety considerations in case of doubt for manual operating state 4402.

The navigator 406 keeps track of where the vehicle 102 is while operating in the telephoto state 4406, even though the vehicle may be maneuvered in the teleprocess state far from a known route.

c. manual operating state

The manual mode 4402 may be required when the vehicle 102 is maneuvered in very tight environments, such as a repair shop, depot, etc., or when a control subsystem needs to be removed for repair or maintenance.

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

While in the manual mode, the autonomous system would continuously monitor vehicle motion and maintain an updated vehicle position record so that if and when an autonomous mode 4408 is desired, a faster and more efficient transition could be made.

Again, if autonomous operating state 4408 is desired, the human operator would then act in an affirmative manner to turn on autonomous operating state 4408 by physically moving a switch or lever to the autonomous control mode, for example. A time delay would preferably be set up such that the human operator would have the option of leaving the vehicle 102, if desired. At the end of the time delay, the system would then go through various warning levels, such as lights, horn or the like, indicating that the autonomous takeover of the vehicle 102 is imminent.

d. autonomous operating state

In the autonomous operating state 4408, the ready operating state 4404 (ready) is entered. In the autonomous operating state 4408, the vehicle 102 is under the control of the autonomous navigation system.

In this mode, the vehicle control system receives messages from the navigator 406, as discussed above, by the vehicle manager 4302. The vehicle manager 4302 is basically, as discussed, the communication and command hub for the remainder of the controllers.

The vehicle manager 4302 and the other functional control blocks are all also in communication with the shutdown circuits 4312. The shutdown circuits 4312 will be discussed in greater detail below.