The invention was made under contract with an agency of the U.S. Government. The name of the U.S. Government agency and the U.S. Government contract number are NSF and EEC 94 11522.

SPECIFICATION

This application claims priority under 35 U.S.C. § 119(e) from provisional application No. 60/032,840 filed on Dec. 13, 1996.

A Microfiche Appendix containing a computer code listing utilized in practicing this invention is included hereinbelow. The Appendix includes one microfiche with 27 frames.

This invention relates to robotic platforms adaptable to various applications and capable of autonomous navigation. Specifically, the invention relates to the platform's acoustic navigation system.

BACKGROUND OF THE INVENTION

Many implementations of robotic platforms have been developed over the years. Most require human operation and are tethered or wired to a controller. This limits the platform's work area and applicability to particular tasks. A navigation system that allows a mobile robotic platform to operate autonomously in a relatively large work area for varied applications is preferable.

In the prior art, radio frequency (RF) based navigation systems have been utilized, such as the Global Positioning System (GPS). GPS is a system that transmits pseudo-noise (PN) signals from a constellation of orbiting satellites down to earth. Detection of these signals allows an earth-based receiver to determine ranges (distances) to the satellites. In fact, PN ranging is a standard technique in RF based navigation systems.

A problem with using RF based navigation systems involves their level of accuracy. Because the speed of radio propagation is roughly 10⁹ ft/sec, it is difficult to acquire precise range values. For instance, one inch ranging precision in RF based navigation systems requires sub nanosecond timing precision. That type of timing would be very difficult and expensive to incorporate into a navigation system. A GPS system is actually able to place an object within about 10 or 15 yards anywhere on earth. For a robotic application within a bounded area, this degree of accuracy is insufficient.

Alternatively, ultrasound has been used for navigation in various industrial applications. Ultrasonic frequencies are those above 20 kHz. Because high frequency sounds are attenuated rapidly in the air, ultrasound has an effective range of only about fifty feet. Thus, for applications requiring a platform to autonomously travel within an area of 200-300 yards, ultrasound is ineffective.

An object of the present invention is to provide a local area acoustic ranging/positioning system for an independent autonomous robotic platform without any wiring or tethering which results in accuracy greater than systems of the prior art.

An additional object of the invention is to provide a robotic platform that can accommodate a variety of work elements (tools) to adapt to a range of indoor and outdoor applications such as painting parking lots, marking and decorating athletic fields and autonomous surveillance.

SUMMARY OF THE INVENTION

The present invention overcomes the prior art limitations by providing an acoustic ranging system to position and navigate an autonomous mobile robotic platform throughout a work area. The robotic navigation system includes a robotic platform and at least three beacons. The platform includes a drive system, a device for transmitting an acoustic PN signal, a device for receiving an RF signal and a device for processing information from the acoustic PN signal and the RF signal to navigate the platform. Each beacon has a device for receiving the acoustic PN signal transmitted by the platform, a device for processing the acoustic PN signal into an RF signal and a device for transmitting the RF signal to the platform.

This acoustic ranging system uses low frequency audible signals which have a greater range than high frequency sounds and work surprisingly well in the presence of local noise such as that emanating from highway traffic.

BRIEF DESCRIPTION OF THE DRAWING

Further objects, features and advantages of the invention will become apparent from the following detailed description taken in conjunction with the accompanying figures in which:

FIG. 1 is an illustration of the robotic navigation system of the present invention;

FIGS. 2A and 2B are diagrams of one embodiment of the robotic platform viewed from the side and the rear, respectively;

FIGS. 3-5 are charts indicating the sound absorption coefficient plotted against frequency varying humidity and temperature of 20° C. (68° F.), 30° C. (86° F.) and 45° C. (113° F.), respectively;

FIG. 6 is an illustration of a PN signal and associated sync pulse according to the invention;

FIG. 7 is a block diagram of a linear feedback register used to construct a PN code sequence according to one embodiment of the invention;

FIG. 8 is a block diagram of a digital signal processor used in one embodiment of the invention;

FIG. 9 is a data arithmetic logic unit of the digital signal processor of FIG. 8;

FIG. 10 is a block diagram of an address generation unit of the digital signal processor of FIG. 8;

FIG. 11 is a block diagram of an evaluation module of the digital signal processor of FIG. 8;

FIG. 12 is an illustration of the structure of the beacon according to an embodiment of the invention;

FIG. 13 is a block diagram of the receiver program according to one embodiment of the invention;

FIG. 14 illustrates the interactions between the primary and secondary navigation systems of the invention; and

FIG. 15 is a timeline for the secondary navigation system of the invention.

Throughout the figures, the same reference numerals and characters, unless otherwise stated, are used to denote like features, elements, components or portions of the illustrated embodiment. Moreover, while the subject invention will now be described in detail with reference to the figures, it is done so in connection with preferred embodiments. It is intended that changes and modifications can be made to the described embodiments without departing from the true scope and spirit of the subject invention as defined by the appended claims.

DETAILED DESCRIPTION

The robotic navigation system 1, according to one embodiment of the present invention, is illustrated in FIG. 1. The system includes a mobile robotic platform 5 and three or more remote receivers 20, called "beacons," situated at the borders of a work area 18 within which the platform travels. Preferably, four beacons 20 are used in the system, as illustrated in FIG. 1.

Platform and Beacons

The robotic platform 5 includes, on board, a power source, a drive system, a navigation (hardware/software) subsystem, controllers and a structure with connections for carrying required tools for a given application. One embodiment of the platform is illustrated in FIGS. 2A and 2B. Preferably, the power source includes one or more batteries 6, e.g., two 12 volt Concord gel 34 AL. The drive system preferably incorporates two Bodine 1/4 horsepower gear motors 7 with a 10:1 ratio, each mounted above one of two rear wheels 8. Each motor preferably has a coupled speed sensor disk 16. The speed sensor is, preferably, a magnetic Hall effect sensor. Varying the speeds of each motor provides the steering capability of the platform. The rear wheels 8 preferably are 16 inch casters. The two front wheels 9 are not driven and preferably are eight inch casters. Each of the rear wheels 8 has its own drive shaft 10. The drive shafts 10 are preferably made of steel 3/4 inch in diameter and 18 inches long. Preferably, two sprocket/chain drives 11 with a 45:1 ratio are used.

The navigation subsystem includes a speaker tower assembly 12 mounted between the two motors with a four inch mid-range speaker and an omni-directional cone reflector. A laptop computer (not shown) is, preferably, mounted on the speaker tower assembly 12 to provide navigation and drive processing capability. An electronics box 13 having an antenna 14 is mounted to the platform to receive and process RF signals sent from the beacons as part of the navigation subsystem and to provide other drive controls.

The platform preferably includes pressure sensors, proximity sensors and a magnetic compass (none shown) to further aid navigation.

Because the platform is general purpose, it can be fitted with various tools to create robotic devices for performing many applications. A payload/tool region 15 of the platform is reserved for mounting the desired tool(s) such as a pressurized liquid spray unit for painting, powder dispensing systems for marking and decorating playing fields and video equipment and robotic manipulators.

Operation of the beacon 20 is controlled by the use of a circuit board with a low cost digital signal processor (DSP). A DSP, such as the Motorola DSP56002 processor, is a special purpose microprocessor that is tailored for numerical signal processing computations, such as those that arise in correlation receivers. Each beacon also includes a microphone, a preamplifier and a radio transmitter. Further discussion of the beacon design is included below.

Operation of Platform

The robotic platform operates in two modes: survey mode and navigation mode. In the survey mode, the platform determines the location of each of the beacons using the on board acoustic ranging system. In the navigation mode, the platform uses the acoustic ranging system to accurately go through the maneuvers required to perform the sequence of tasks stored in the program selected by the user.

In the survey mode, the platform's navigation subsystem transmits an acoustic PN signal (described hereinbelow) that is detected by the beacons. The robot-to-beacon link is called the uplink. Each beacon derives a synchronization ("sync") pulse from the PN signal that "tags" the time-of-arrival of the received signal. This sync pulse is transmitted to the platform's navigation subsystem via an RF link, called the downlink. The RF signal is created in a manner known to those skilled in the art (e.g., pulse modulation). Since the speed of radio propagation is substantial relative to the speed of sound, the total uplink/downlink propagation delay is essentially just the acoustic robot-to-beacon propagation delay. In other words, the radio propagation time is negligible in comparison to the acoustic uplink propagation time. The robotic platform has, at its disposal, data indicative of the time of transmission, as determined by a transmitter sync pulse and the beacon's generated sync pulse that was returned to the platform via the RF downlink. Comparison of these two pulses determines the propagation delay of the acoustic signal, and multiplication by the speed of sound determines the measurement of the range (distance) to the beacon.

Although the navigation system employs several beacons, there is only one acoustic uplink that is transmitted from the platform using the four inch mid-range speaker and the omni-directional cone reflector, as illustrated in FIGS. 1 and 2. Different beacons are identified by the platform by using different RF carrier frequencies for the downlinks. Navigation is performed by measuring the range to three or more known beacon locations. The platform's position coordinates are derived from range measurements by using standard navigation practice techniques known to those skilled in the art.

During the navigation mode, these derived coordinates are compared with the coordinates predicted from a predetermined program in the on-board computer. Any deviation of the platform's coordinates from its programmed position is processed by the computer and the drive system is operated using standard motor control technology known to those skilled in the art to move the platform to its programmed position.

Acoustic PN Signal

As stated above, the robotic platform surveys the work area and its position therein by using an acoustic PN ranging system. Unlike an RF-based navigation system such as GPS that, as described hereinabove, requires sub nanosecond timing precision for one inch ranging precision and actually has a ranging precision of 10 to 15 yards, the speed of sound being roughly 10³ ft/sec allows acoustic ranging with one inch precision by determining the time of arrival with timing precision of 10^-4 sec (=1/10 msec) Therefore, within an operating range of up to approximately 200-300 yards, navigation precision better than one inch is attainable with use of the acoustic system.

The absorption of acoustic frequencies in air is a complicated function of temperature, pressure, humidity, and frequency. The equation for the absorption coefficient is as follows: ##EQU1## where F=f/p_s, F_r.0 =f_r.0 /P_s and F_r.N =f_r.N /p_s, are frequencies scaled by atmospheric pressure (p_s) p_s0 is the reference pressure (1 atm), T₀ =293.15 K, T is the atmospheric temperature in K, and f is the frequency in Hz. Formulas for the scaled relaxation frequencies for oxygen and nitrogen are: ##EQU2## respectively, where h is the absolute humidity in %. For completeness the relationship between h and the relative humidity h_r is: ##EQU3##

The equation for p_sat, the saturation vapor pressure, is ##EQU4## where T₀₁ =273.16 K. is the triple-point isotherm temperature, and all other variables are as stated hereinabove. Although any consistent pressure units may be used, expressing the pressure in atm makes the factor p_s0 unity, and the preceding equations simplify somewhat.

FIG. 3 shows the dependency of the absorption coefficient, α/p_s on frequency and relative humidity at a temperature of 20° C. (68° F.). Because the system is being operated at ranges in the neighborhood of 200 m, an absorption coefficient of 0.05 dB/m results and a 10 dB drop in signal power occurs due to absorption alone. Therefore, the absorption coefficient for the acoustic carrier frequency chosen needs to be below 0.05 dB/m, a value for α/p_s that seems reasonable for success in the present invention.

In FIGS. 4 and 5, the absorption coefficient is plotted against frequency for varying humidity and temperatures of 30° C. (86° F.) and 45° C. (113° F.). It can be seen from these figures that the absorption increases with temperature as well as with humidity. Even where the humidity is approximately 10%, and the temperature is 45° C. (112° F.), absorption coefficients of approximately 0.05 dB/m for the carrier frequencies used in the present invention result. This value is close to the maximum tolerable value for the absorption coefficient.

In addition to the absorption power losses, there is a dispersion power loss that increases as a function of the square of the distance from the source (r²). An estimate of the power loss by dispersion comes about by assuming that the constant power transmitted by the speaker is distributed over the surface of a sphere (4πr²). A hemispherical model would be more applicable in this case since the radiation pattern is approximately that of a point source over a plane. In this case, the power is distributed over 2πr², or one half of the spherical surface. It can be seen that only the constant term is affected by choosing better approximations of the dispersion pattern, the dispersion power loss still increases as a function of r². Thus, absorption and dispersion should be taken into account when choosing an acoustic carrier frequency.

There are two main reasons for using a PN signal for acoustic ranging in the present invention. First, it is a continuous constant amplitude signal, thus minimizing the peak-to-average power ratio. Minimizing peak acoustic power allows use of cheaper low power acoustic transducers. This also eliminates the potential for hearing damage due to brief bursts of intense acoustic energy that might be associated with a pulse ranging system. The second advantage of the PN signal is its noise and interference rejection. A PN signal is jam resistant. The system is very robust to in band interference such as human speech and road noise.

A PN signal is derived from a periodic binary sequence that has many properties of a totally random binary sequence. The transmitted signal has the form

x(t)=Ap(t)cos(2πfc t)

where A is the signal amplitude, p(t) is the binary ±1-valued PN signal, and f_c is the carrier frequency. The PN signal itself is of the form ##EQU5## where u₀, . . . , u_n-1 is the binary ±1-valued PN code of length n and r(t) is a rectangular pulse of length T_c (r(t)=1 for 0≦t<T_c, and r(t)=0 otherwise). The successive pulses in the PN signal are called "chips," and 1/T_c is the "chip rate." The PN signal p(t) is periodic of period T=nT_c.

FIG. 6 illustrates a PN signal (p(t)) 2 without the carrier frequency present. This figure also shows how the associated sync pulse (s(t)) 3 marks the first chip of each fundamental period of the PN signal.

There are standard techniques for constructing the PN code sequence u₀, . . . , u_n-1 having the desired statistical properties. The type of sequence used in one embodiment of the present invention is a maximal length linear feedback shift register sequence, often called an m-sequence in the literature. FIG. 7 illustrates the linear feedback register 4 used to construct a binary ±1-valued 255 bit long PN code sequence. This conventional register employs eight shift registers 65 each holding one bit and three XOR gates 60 used to compute the value in the first shift register. Employing an m-sequence with n=255, chip rate 1/T_c =800 Hz, and acoustic carrier frequency f_c =4 kHz results in a PN signal period of T≅318 msec. The particular design parameters are only nominal design values. For example, decreasing f_c may increase the operating range of the system, and increasing the chip rate may increase the accuracy of the system.

In order to simplify the carrier demodulation at the beacon by having sequential samples 90° out of phase with one another, the sampling frequency, f_s, must be an integer multiple of four times f_c in order to use a technique known as quarter wave sampling. Because the absorption coefficient must be below 0.05 dB/m, possible carrier frequencies include 1.6 kHz, 2.4 kHz, 3.2 kHz as well as the above-referenced 4 kHz. However, it can be shown that choosing carrier frequencies of 2.4 kHz or 4 kHz greatly simplifies the carrier demodulation scheme at the beacon.

Beacon Processing

Each beacon processes the received signal y(t)∝x(t-τ) where τ is the uplink propagation delay. The function of the beacon is to reconstruct the delayed sync pulse signal S(t-τ) and transmit it back to the platform on the RF downlink. To do this, the receiver compares y(t) against a locally generated PN signal p(t-τ) through a signal processing technique known as cross-correlation. In essence, the receiver adjusts its delay estimate τ to maximize the correlation between the received signal and the locally generated PN signal.

The beacon design is complicated by the fact that robot dynamics introduce a Doppler frequency shift in both the acoustic carrier and the PN signal. This is a marked contrast to RF systems where Doppler shift must be corrected only for carrier tracking. A significant point of contrast is that the signal bandwidth ∝1/T_c is a large fraction of the carrier frequency f_c, which is not the case in RF systems. The effect of robot motion is that the propagation delay is itself a function of time: τ=τ(t). During one period of the PN sequence (approximately 1/3 sec) constant velocity motion may be assumed, which yields a delay model of the form τ(t)=τ₀ +τ(t-t₀). The received signal can then be expressed as

y(t)∝x(t-τ(t))=p((1-τ)t-τ0)cos(2π(fc +λ)t+φ)

where λ=-τf_c is the carrier Doppler frequency shift and φ=-2πf_c τ₀. Thus, vehicle motion results in both a Doppler shift on the carrier frequency and a time compression (or expansion) on the PN modulation. In order to perform accurate PN correlation, the beacon must address both effects.

Beacon Design

As indicated in the above discussion, accurate detection of the acoustic PN signal requires an advanced beacon design. There are several features of the beacons' DSP56002 processor that distinguish it from other general purpose processors and make it particularly suitable for the navigation system of the present invention. Three of the features are: (1) program execution time of 20 million instructions per second (MIPS) at 40 MHz, (2) single-cycle 24×24 bit parallel multiply-accumulator, and (3) highly parallel instruction set with unique DSP addressing modes.

The parallel data moving capabilities of the DSP56002 are shown in the block diagram of FIG. 8. In addition to the program address bus (PAB) 26, the DSP56002 25 includes the x-memory (XAB) 27 and y-memory (YAB) 28 address buses, all of which are 16-bits wide. Associated with each of these address buses are the data buses for the program (PDB) 29, x-memory (XDB) 30 and y-memory (YDB) 31. The data moving capability is such that data can be moved simultaneously over each data bus in one execution cycle. There also is a Global Data Bus (GDB) 32 which handles all of the external data moving tasks. The parallelism of the data and address buses combine to supply a substantially fast program execution time, as stated above.

A notable architectural feature of the DSP56002 is the data arithmetic logic unit (ALU) 33 shown, with detail, in FIG. 9. The data ALU features the multiply and accumulate (MAC) unit. A major part of the PN ranging system is the cross-correlation process which consists of the operation Σ_i α_i ·β_i. The MAC unit is optimized to perform the Σ_i α_i ·β_i operation on 24-bit operands, producing a 56-bit result. This calculation is performed in one instruction cycle. Parallel pipelining allows the sum over n terms to be performed in n+2 instructions. The first two instructions prefetch the α_o and β_o values over the X and Y data buses. Then the remaining n instructions multiply α_i and β_i while simultaneously fetching α_i+1 and β_i+1 over the X and Y data buses, respectively, for the next MAC cycle. This strategy is used extensively in the correlation portion of the beacon code.

The address generation unit (AGU) 34 of the DSP56002 shown in FIG. 8 uses integer arithmetic to perform the effective address calculations necessary to address data operands in memory, and contains the registers (Rn) used to generate the addresses. FIG. 10 shows the block diagram of the AGU 34. It implements linear, modulo, and reverse-carry arithmetic by means of the modifier registers (Mn) 35. The AGU is divided into two identical halves comprising the low address ALU 36 and the high address ALU 37, each of which have four sets of three registers. These registers are the address registers (R0-R3 and R4-R7) 38, the offset registers (N0-N3 and N4-N7) 39, and the modifier registers (M0-M3 and M4-M7) 35. The eight Rn, Nn and Mn registers are treated as register triplets. The modifier registers encompass the unique features of the addressing capabilities of this AGU. By setting Mn equal to negative one, linear addressing results, which is normally how the addressing works in general purpose processors. By setting Mn equal to some positive number (p-1), a circular buffer of length p (some power of two) results. When the bottom of the circular buffer is reached, the modulo addressing capability resets the buffer pointer to the beginning of the buffer. This is very useful when coding digital filtering applications. When Mn is set to zero, bit-reversed, reverse carry addressing is in progress. This is very useful in Fast Fourier Transform applications. For the PN Ranging application, the circular buffer capabilities of the AGU are used extensively.

FIG. 11 shows a block diagram of the DSP56002 Evaluation Module (EVM). The EVM is built around the combination of the Motorola DSP56002 25 and the Crystal Semiconductor CS4215 Codec 40. The module also contains support components for memory access 41, an RS-232 link 42, microphone input conditioning 43 and power supply 44. The DSP56002 is coupled to the Codec via a synchronous serial interface (SSI) 45, which can transfer data at sample rates of up to 50 kHz. The RS-232 interface 42 allows the programmer to use the on chip emulator (OnCE) features of the EVM module to test the program on the actual hardware using the Debug-EVM software supplied with the module. In addition to the SSI, there are also fifteen general purpose input/output (GPIO) pins 46 on the module that can be programmed at the developer's discretion.

Receiver Software

The beacon's PN "Receiver" software can be programmed in Motorola DSP56002 assembly language. The software code is attached as Appendix A. FIG. 12 illustrates the software 53 and the relationships between the different subroutine functions, which were developed simultaneously and then linked together in the main program (MAIN.ASM) using include statements. The main memory map (MEM_-- MAP.ASM) is where the starting addresses of all the various circular buffers and constants are defined for use in the other subroutines. This is so the locations of all the large buffers used in the processing stage are all in one place so they can be readily modified should any other features be added to the software design.

The first executed subroutine is the initialization code 54 (INIT.ASM) which configures the digital signal processor and the Codec 40 on the EVM module. This subroutine is executed first because it initializes all of the various features of the DSP56002 that will be used in the Receiver program.

The second subroutine to be executed is the PN code generator 55 (PNGEN.ASM), a software implementation of the linear feedback shift register sequence illustrated in FIG. 7. This subroutine fills up the internal X-memory with the PN code used to correlate the received signal. This subroutine generates what is referred to as the locally generated PN code, and will be compared to the received PN code to test for the maximum correlation.

The Receiver code main loop 56 (CONTROL.ASM) controls the flow of the foreground processing. It is responsible for calling the correlation routines and handing the sync pulse output period correction to a timer control subroutine (TIMER.ASM) (not illustrated). This timer subroutine handles the sync pulse output and controls the period of the main execution loop. The execution of the main loop 56 occurs approximately every 318 msec with a maximum wait time at the top of the loop of approximately 20 msec.

The correlation portion of the main loop 56 takes place in two distinct steps. First, the received signal is modified by the rotation and time compression/expansion routines (TAU_-- DOT.ASM & TAU_-- SRT.ASM) to correct for any compression or expansion on the received PN modulation due to the motion of the robotic platform. Then, the modified received signal is correlated with the locally generated PN code in internal X-memory created by a correlation subroutine (PN_-- CORR_-- X.ASM). The correlation routine locates the maximum correlation out of the cross-correlation process and returns a value used to adjust the sync pulse output. The system performs a two dimensional delay/Doppler carrier frequency search that involves compensating for both the time compression/expansion of the PN modulation and the Doppler carrier frequency shifts due to the motion of the platform.

The sync pulse output Interrupt Service Routine (ISR) 57 handles the output of the sync pulse, and sets pointers used in the main loop to their corrected values. The index in the In-phase and Quadrature (I/Q) data buffer, IQ_-- BUFF, corresponding to the maximal correlation is used by this ISR (TIME_-- ISR.ASM) 57 to determine the sync pulse period correction.

The ISR 58 (SSI_-- ISR.ASM) that samples the microphone input calls the front end processing subroutine 59 (FR_-- END.ASM) every time the SSI receive data interrupt occurs. The front end processing subroutine consists of a four pole Butterworth band pass filter centered on the carrier frequency and a decimation portion to reduce the amount of data to four samples per chip. The front end section then loads the filtered samples into the I/Q data buffer for the correlation routines to process.

The timer control subroutine is divided into the foreground processing subroutines and the Timer ISR (TISR). The foreground processing consists of the main control loop algorithms used to process the received signal and perform the correlation calculations. The main loop control program begins when a TISR flag is set.

The TSR functions in one of three interrupt states. The first state completes several tasks, the first of which is to transmit the reconstructed sync pulse over the RF link. The next task is to define values for the cyclic pointers that are used in the buffer that stores I/Q samples during foreground processing. The last task completed is to set the time until its next interrupt. In the first state, the timer is set for 2 ms which is the width of the sync pulse.

For the second interrupt state, a flag for the start of the foreground processing is set. The value given to the timer is 200 ms, which is the time to complete the foreground processing plus a small margin to ensure the processing is completed.

The third interrupt state has the sole task of setting the timer with the value returned by the foreground processing. This value represents the time delay that is being compensated for in the time scaling portion. This makes it possible to accurately mark the completion of the data collection for the next set of data, and thereby start the cycle over again. In all states, the front end ISR is cycling in order to acquire the needed data samples from the microphone input.

The main elements of the DSP software are schematically illustrated in FIG. 13. The front end section of the ranging system is required for signal acquisition. In this embodiment, this section has been implemented as an ISR and contains one band pass filter, two low pass filters and complex hybrid signal multiplication and decimation functions. After the acoustic signal is detected by a microphone (not shown), it is then sampled by an A/D converter 47, located in the Codec chip module, for digital processing. The sampled signal is first processed by a band pass filter (BPF) 48 to remove out-of-band interference. This is followed by a standard I/Q demodulator 49 in which the signal is multiplied by cos(2πf_c t) and sin(2πf_c t) and low pass filtered (LPF) to produce the lowpass I & Q signals. The high rate necessary for sampling the bandpass signal is not necessary for representation of the lowpass I & Q signals. Thus, decimation is performed at the output of the low pass filters. The PN signal, after being run through the BPF, is sampled with a sampling frequency equal to an integer multiple (m=1) of four times the carrier frequency (f_c) which is in turn n times faster than the chip rate of 800 Hz (n=f_c /800). Using every nth sample the following four state repeating sequence of In-phase and Quadrature components result: (I,Q)₁, (I,-Q)₂, (-I,-Q)₃, (-I,Q)₄. This data is accumulated in an I/Q data buffer 50. There is no need for carrier demodulation because the sampling rate of four times the carrier frequency effectively addresses this problem because sequential samples turn out to be ninety degrees out of phase from one another.

Once an entire period of the PN sequence has been sampled and preprocessed as described above, the beacon performs the PN cross-correlations. One block of data from the I/Q buffer is first processed to remove Doppler and time compression/expansion in block 51. This is done for several preselected values of delay rate τ. Hence, after the Doppler and time compression/expansion, there is still a residual frequency error. For this reason, the beacon employs an incoherent correlator 52.

For instance, the number of preset values of τ may be determined by the requirement that the maximal post-correction residual time expansion/compression error be no more than a quarter chip (T_c /4). This requirement results in seven preset τ values. In this instance, the data sample rate is decimated to four samples per PN chip. This results in 4×255=1020 I & Q data samples per PN signal period, and hence, 1020 possible settings for τ₀. Seven preset values of τ result in 7×1020=7140 correlation computations, each requiring ≈2×255 multiply/addition operations. All correlation may be performed on the DSP56002 processor within the 318 msec cycle time.

The index in the I/Q data buffer corresponding to the maximal correlation determines the sync pulse period correction. The sync pulse period and the code period should be identical, but the sync pulse output occurs ≈318 msec±error. The error occurs because the robotic platform may be making a change in direction, accelerating or decelerating causing a Doppler effect on the code. It is this Doppler effect on the code, either shortening or lengthening the code period, that necessitates the sync pulse period correction.

System Operation

In the navigation mode, the platform uses a two-tiered motion control strategy to take it through a predefined set of tasks within its working perimeter. The two levels are termed primary navigation system (open loop) and secondary navigation system (closed loop).

The primary navigation system operates in an "open loop" fashion called dead reckoning, i.e. there is no outside reference to correct position. The controller of the dead reckoning system receives motion commands (direction, velocity, acceleration, duration) and tool commands (on/off, tool parameters) from the on board computer. The commands are interpreted and converted step-by-step to signals for controlling the drive system and tool(s). The primary navigation system uses several feedback elements, such as speed sensors, pressure sensors, proximity sensors (to determine hazardous obstacles) and a magnetic compass. The end result of dead reckoning is that the platform moves to a new location while the tool(s) are off or performing the desired function.

In dead reckoning, two quantities are measured: the robotic platform's forward speed s(t) and heading rate ψ(t)=dψ(t)/dt. Given perfect knowledge of s(t) and the heading angle ψ(t), the platform's velocity is ##EQU6##

The quantities s(t), ψ(t), p(t) and v(t) denote the true values of speed, heading, position and velocity, respectively. The primary navigation system measurements are denoted s^m (t) and ψ^m (t). Of course, there are errors associated with these measurements and computations. The navigation system of the present invention never knows these "true values." Rather, the purpose of a navigation system is to process the measurements to compute estimates of p(t), ψ(t) and v(t). Estimates are denoted with a hat; for example p(t;k) will denote the primary navigation position estimate. (The index k is explained below). Since the estimates are not the true values, they have error. Estimate errors are denoted with a prefix δ. For example, δp(t;k)=p(t;k)-p(t) is the primary navigation system position error.

In reality, the primary system measurements may be discrete in nature, not the continuous time functions s^m (t) and ψ^m (t) indicated here. For example, these measurements may be derived by counting "clicks" from rotation sensors on the left and right wheels. In addition, the dead reckoning integration (presented below) is numerically evaluated as a discrete sum. Treatment, herein, of these quantities as continuous time functions is merely a mathematical convenience for the purpose of exposition of the fundamental principles at play. The data collection process of the primary navigation system is much faster than the secondary system. Thus, a fast primary data stream may be viewed as a continuous time stream relative to the secondary navigation system.

The goal of the primary navigation system is to estimate position and heading on demand. It is appropriate to group these quantities into a single 3-dimensional state vector ##EQU7##

At times t₁ <t₂ <. . . , the secondary navigation system processes its range measurements to produce estimates x_k ⁺. This state vector estimate is the best available estimate of x(t_k) given all of the primary and secondary navigation data collected up to time t_k. Computation of x_k ⁺ is discussed in detail later; for now, the following analysis assumes that it is available at time t_k.

Let x(t;k) denote the primary system state vector estimate for time t≧t_k. This estimate is initialized by the secondary navigation system: ##EQU8##

The primary navigation system extrapolates this estimate forward in time utilizing the speed and heading rate measurements s^m (t) and ψ^m (t). This computation is ##EQU9##

Equation (2) is the dead reckoning equation. Dead reckoning is an open loop integration. Errors associated with the measurements s^m (t) and ψ(t) are accumulated by this integration, and as a result, the position and heading errors grow without bound. In other words, the unaided dead reckoning is unstable.

Thus, because non-uniform surface conditions or internal errors may cause the platform to take a somewhat different path from what the computer instructs it to take in the primary navigation system, the secondary navigation system provides an independent coordinate measurement and error correction as it constantly monitors the position of the platform using the acoustic PN ranging system described hereinabove. In other words, the secondary navigation system "closes the loop" that stabilizes the system. FIG. 14 illustrates this interaction between the primary and secondary navigation systems.

The secondary navigation system computes the estimate x_k ⁺ that initializes the dead reckoning integration at time t_k for dead reckoning in the time interval t_k,t_k+ !. The secondary navigation system utilizes two types of data to compute x_k ⁺ : 1) an a priori estimate x_k ^-, and 2) the range-to-beacon measurements for time t_k. The a priori estimate x_k ^- is provided by the primary navigation system:

xk - =x(tk ;k-1)                             (3)

x_k ^- =x(t_k ;k-1) is just the result at time t_k of the dead reckoning integrations that were initialized at t_k-1. Thus, the a priori estimate x_k ^- is the best estimate of x(t) given the range-to-beacon data up to time t_k-1, and the primary speed and heading up to time t_k. The basic secondary navigation computation incorporates the new range-to-beacon measurements for time t_k to compute x_k ⁺. Thus, the "a priori" terminology means "prior to utilization of the new range-to-beacon data." Accordingly, x_k ⁺ is called the updated estimate, meaning "updated by the new range-to-beacon data." The time line for the secondary navigation system processing is illustrated in FIG. 15.

The range-to-beacon measurements are really measurements of the propagation delay of a signal transmitted from the platform to the beacon and then retransmitted back to the platform, as described above. Let τ_i.k^m denote this propagation delay measurement for the with beacon as measured for time t_k. As a vector, τ_k ^m = τ_l.k^m . . . τ₄.k^m !^t is written for a four beacon system. Then the basic estimate update equation is

xk + =xk - +Kk (τk m -τk -)(4)

where τ_k ^- is the a priori estimate of the propagation delay measurement. K_k is called the update gain matrix. Observe that the update equation adjusts the a priori estimate x_k ^- to account for the difference between the actual measurement, τ_k ^m, and the a priori estimate of what the measurement will be, τ_k ^-.

The optimal gain matrix is called the Kalman gain matrix. The basic estimate update structure of equation (4) would also be used in a suboptimal system. The only difference is that a suboptimal gain K_k, perhaps a fixed gain K_k ≡K, would be used. Computation of the optimal Kalman gain and suboptimal gain design is deferred to a later section. How the a priori measurement estimates τ_k ^- are to be computed is shown to complete the presentation of the basic filter structure.

Let p_i = p_i.x, p_i.y !^t denote the position of the ith beacon. Then ##EQU10## is the true range to the ith beacon, and ##EQU11## is the line-of-sight unit vector that points from the platform towards the ith beacon. During the time that the signal propagates from the platform to the beacon and back to the platform, the platform moves. This motion must be accounted for in the measurement equation. Assume a signal (perhaps a simple pulse) that is transmitted from the platform at time instant t_k, propagates to the beacon where it is retransmitted and propagates back to the platform. Since the platform receives the retransmitted signal at time t_k +τ_i.k, the propagation delay satisfies the equation ##EQU12## where c is the speed of propagation.

Equation (7) is nonlinear in τ_i.k. As a result, the following linearization proves useful in the analysis.

Start with ##EQU13##

Differentiating (5), using ##EQU14## yields ##EQU15## where denotes the vector dot product. Thus,

ri (tk +τi.k)=ri (tk)+ei (tk)·v(tk)τi.k +linearization error.(8)

Applying this to (7) and solving for τ_i.k yields ##EQU16##

The best a priori estimate τ_i.k^- would be evaluated by replacing r(t) and τ_i in (7) with r(t;k) (computed by applying p(t;k) and τ_i.k^-). The problem is that this is a nonlinear equation in τ_i.k^- that cannot be solved. However, if the linearized version (9) is used, then an a priori estimate is easily evaluated as ##EQU17## where ##EQU18##

Once again, taking into consideration the dead reckoning equation (2) which is rewritten verbatim below: ##EQU19## the updated estimate x_k ⁺ cannot be computed before the propagation delay time measurement τ_i.k^m is available, and this does not happen until time t_k +τ_i.k. This suggests that two dead reckoning integrations will be performed for the time interval t_k,t_k +max_i τ_i.k !: the old integration x(t;k-1) must be processed past time t_k to time t_k +max_i τ_i.k. At that time, the updated estimate x_k ⁺ may be computed. Only then can the dead reckoning integrator be initialized with x_k ⁺ and, hence, dead reckoning integration for the time interval t_k, t_k +max_i τ_i.k ! is performed a second time to compute x(t;k). Moreover, there is a problem that the primary speed and heading rate data for the time interval t_k,t_k +max_i τ_i ! must be stored in order to perform the second dead reckoning integration. Fortunately, however, none of this is necessary.

The solution is in the way that the dead reckoning is performed. Define ##EQU20##

Observe that x⁰ (t;k) depends only on the measured speed and heading rate s^m (u;k) and ψ^m (u;k) for t_k ≦u≦t≦t_k+1, and not on any preset estimate (in particular, x_k ⁺). x⁰ (t;k) is just the dead reckoning equation preset with a zero position and heading estimate: x⁰ (t_k ;k)=0.

The following relates x⁰ (t;k) to x(t;k). From the heading component of the dead reckoning equation (2) it is immediately evident that

ψ(t;k)=ψk + +ψ0 (t;k).

Using this last expression, the position components of the dead reckoning equation (2) can be rewritten as ##EQU21##

Applying trigonometry, the following results:

cos(ψk + +ψ0 (u;k))=cos (ψk +)cos(ψ0 (u;k))-sin(ψk +)sin(ψ0 (u;k))

and

sin(ψk + +ψ0 (u;k))=sin (ψk +)cos(ψ0 (u;k))+sin(ψk +)sin(ψ0 (u;k))

Applying these expansions to the posizion yields ##EQU22##

Combining the position and heading results, it is apparent that the dead reckoning can be performed by actually integrating only (11), which is the dead reckoning equation initialized with zero position and heading, for each time interval t_k,t_k !. Once the updated estimate x_k ⁺ has been computed, at time t_k +max_i τ_i.k (+processing time), the following may be evaluated: ##EQU23##

The matrix R(ψ_k ⁺) is called the rotation matrix.

It may also be desirable to continue computing x(t;k-1) past time t_k in order to have position and heading estimates available in the time interval t_k, t_k +max_i τ_i.k ! before the kth update processing has been completed. Following exactly the same arguments as above, it can be shown that

x(t;k-1)=xk - +R(ψk -)xo (t;k) for t≧tk.

Notice that this last expression utilized x⁰ (t;k) , not x^o (t;k-1) Thus, x^o (t;k-1) does not need to be integrated past time t_k, and there is no need to store the primary measurement data s^m (t) and ψ^m (t) and re-integrate the dead reckoning equation over the time interval t_k,t_k +max_i τ_i.k !.

In summary, the processing of data for a single cycle t_k,t_k ! proceeds as follows:

At time t_k

Evaluate the a priori estimates x_k ^- =x(t_k ;k-1), τ_k ^-, etc.

Initialize the dead reckoning integrator to 0 and begin computing x^o (t;k).

Continue computing x(t;k-1)=x_k ^- +R(ψ_k ^-)x^o (t;k).

At time t_k +max_i τ_i.k, when the measurement vector τ_k ^m is available,

Evaluate the updated estimates x_k ⁺ =x_k ^- +K(τ_k ^m -τ_k ^-).

Begin computing x(t;k)=x_k ⁺ +R(ψ_k ⁺)x^o (t;k).

Thus, the range to beacon measurements derived from the acoustic navigation system of the present invention, when incorporated into the algorithms above, provide for the constant correction of the platform's position and direction as it autonomously travels within the work area.

Other modifications of the invention will occur to those skilled in the art.