Modular Robotic Platform With Acoustic Navigation System

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.

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.sup.9 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.

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.

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.degree. C. (68.degree. F.), 30.degree. C. (86.degree. F.) and 45.degree. C. (113.degree. 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.

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.

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.

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.

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.sup.3 ft/sec allows acoustic ranging with one inch precision by determining the time of arrival with timing precision of 10.sup.-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.sub.s, F.sub.r.0 =f.sub.r.0 /P.sub.s and F.sub.r.N =f.sub.r.N /p.sub.s, are frequencies scaled by atmospheric pressure (p.sub.s) p.sub.s0 is the reference pressure (1 atm), T.sub.0 =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.sub.r is: ##EQU3##

The equation for p.sub.sat, the saturation vapor pressure, is ##EQU4## where T.sub.01 =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.sub.s0 unity, and the preceding equations simplify somewhat.

FIG. 3 shows the dependency of the absorption coefficient, .alpha./p.sub.s on frequency and relative humidity at a temperature of 20.degree. C. (68.degree. 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 .alpha./p.sub.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.degree. C. (86.degree. F.) and 45.degree. C. (113.degree. 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.degree. C. (112.degree. 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.sup.2). 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.pi.r.sup.2). 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.pi.r.sup.2, 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.sup.2. 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

where A is the signal amplitude, p(t) is the binary .+-.1-valued PN signal, and f.sub.c is the carrier frequency. The PN signal itself is of the form ##EQU5## where u.sub.0, . . . , u.sub.n-1 is the binary .+-.1-valued PN code of length n and r(t) is a rectangular pulse of length T.sub.c (r(t)=1 for 0.ltoreq.t<T.sub.c, and r(t)=0 otherwise). The successive pulses in the PN signal are called "chips," and 1/T.sub.c is the "chip rate." The PN signal p(t) is periodic of period T=nT.sub.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.sub.0, . . . , u.sub.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.sub.c =800 Hz, and acoustic carrier frequency f.sub.c =4 kHz results in a PN signal period of T.congruent.318 msec. The particular design parameters are only nominal design values. For example, decreasing f.sub.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.degree. out of phase with one another, the sampling frequency, f.sub.s, must be an integer multiple of four times f.sub.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.

Each beacon processes the received signal y(t).varies.x(t-.tau.) where .tau. is the uplink propagation delay. The function of the beacon is to reconstruct the delayed sync pulse signal S(t-.tau.) 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-.tau.) through a signal processing technique known as cross-correlation. In essence, the receiver adjusts its delay estimate .tau. 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 .varies.1/T.sub.c is a large fraction of the carrier frequency f.sub.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: .tau.=.tau.(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 .tau.(t)=.tau..sub.0 +.tau.(t-t.sub.0). The received signal can then be expressed as

where .lambda.=-.tau.f.sub.c is the carrier Doppler frequency shift and .phi.=-2.pi.f.sub.c .tau..sub.0. 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.

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.times.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 .SIGMA..sub.i .alpha..sub.i .multidot..beta..sub.i. The MAC unit is optimized to perform the .SIGMA..sub.i .alpha..sub.i .multidot..beta..sub.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 .alpha..sub.o and .beta..sub.o values over the X and Y data buses. Then the remaining n instructions multiply .alpha..sub.i and .beta..sub.i while simultaneously fetching .alpha..sub.i+1 and .beta..sub.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.

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.sub.-- 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.sub.-- DOT.ASM & TAU.sub.-- 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.sub.-- CORR.sub.-- 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.sub.-- BUFF, corresponding to the maximal correlation is used by this ISR (TIME.sub.-- ISR.ASM) 57 to determine the sync pulse period correction.

The ISR 58 (SSI.sub.-- ISR.ASM) that samples the microphone input calls the front end processing subroutine 59 (FR.sub.-- 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.pi.f.sub.c t) and sin(2.pi.f.sub.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.sub.c) which is in turn n times faster than the chip rate of 800 Hz (n=f.sub.c /800). Using every nth sample the following four state repeating sequence of In-phase and Quadrature components result: (I,Q).sub.1, (I,-Q).sub.2, (-I,-Q).sub.3, (-I,Q).sub.4. 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 .tau.. 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 .tau. may be determined by the requirement that the maximal post-correction residual time expansion/compression error be no more than a quarter chip (T.sub.c /4). This requirement results in seven preset .tau. values. In this instance, the data sample rate is decimated to four samples per PN chip. This results in 4.times.255=1020 I & Q data samples per PN signal period, and hence, 1020 possible settings for .tau..sub.0. Seven preset values of .tau. result in 7.times.1020=7140 correlation computations, each requiring .apprxeq.2.times.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 .apprxeq.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.

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 .psi.(t)=d.psi.(t)/dt. Given perfect knowledge of s(t) and the heading angle .psi.(t), the platform's velocity is ##EQU6##

The quantities s(t), .psi.(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.sup.m (t) and .psi..sup.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), .psi.(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 .delta.. For example, .delta.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.sup.m (t) and .psi..sup.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.sub.1 <t.sub.2 <. . . , the secondary navigation system processes its range measurements to produce estimates x.sub.k.sup.+. This state vector estimate is the best available estimate of x(t.sub.k) given all of the primary and secondary navigation data collected up to time t.sub.k. Computation of x.sub.k.sup.+ is discussed in detail later; for now, the following analysis assumes that it is available at time t.sub.k.

Let x(t;k) denote the primary system state vector estimate for time t.gtoreq.t.sub.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.sup.m (t) and .psi..sup.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.sup.m (t) and .psi.(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.sub.k.sup.+ that initializes the dead reckoning integration at time t.sub.k for dead reckoning in the time interval ›t.sub.k,t.sub.k+ !. The secondary navigation system utilizes two types of data to compute x.sub.k.sup.+ : 1) an a priori estimate x.sub.k.sup.-, and 2) the range-to-beacon measurements for time t.sub.k. The a priori estimate x.sub.k.sup.- is provided by the primary navigation system:

x.sub.k.sup.- =x(t.sub.k ;k-1) is just the result at time t.sub.k of the dead reckoning integrations that were initialized at t.sub.k-1. Thus, the a priori estimate x.sub.k.sup.- is the best estimate of x(t) given the range-to-beacon data up to time t.sub.k-1, and the primary speed and heading up to time t.sub.k. The basic secondary navigation computation incorporates the new range-to-beacon measurements for time t.sub.k to compute x.sub.k.sup.+. Thus, the "a priori" terminology means "prior to utilization of the new range-to-beacon data." Accordingly, x.sub.k.sup.+ 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 .tau..sub.i.k.sup.m denote this propagation delay measurement for the with beacon as measured for time t.sub.k. As a vector, .tau..sub.k.sup.m =›.tau..sub.l.k.sup.m . . . .tau..sub.4.k.sup.m !.sup.t is written for a four beacon system. Then the basic estimate update equation is

where .tau..sub.k.sup.- is the a priori estimate of the propagation delay measurement. K.sub.k is called the update gain matrix. Observe that the update equation adjusts the a priori estimate x.sub.k.sup.- to account for the difference between the actual measurement, .tau..sub.k.sup.m, and the a priori estimate of what the measurement will be, .tau..sub.k.sup.-.

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.sub.k, perhaps a fixed gain K.sub.k .ident.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 .tau..sub.k.sup.- are to be computed is shown to complete the presentation of the basic filter structure.

Let p.sub.i =›p.sub.i.x, p.sub.i.y !.sup.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.sub.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.sub.k +.tau..sub.i.k, the propagation delay satisfies the equation ##EQU12## where c is the speed of propagation.

Equation (7) is nonlinear in .tau..sub.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,

Applying this to (7) and solving for .tau..sub.i.k yields ##EQU16##

The best a priori estimate .tau..sub.i.k.sup.- would be evaluated by replacing r(t) and .tau..sub.i in (7) with r(t;k) (computed by applying p(t;k) and .tau..sub.i.k.sup.-). The problem is that this is a nonlinear equation in .tau..sub.i.k.sup.- 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.sub.k.sup.+ cannot be computed before the propagation delay time measurement .tau..sub.i.k.sup.m is available, and this does not happen until time t.sub.k +.tau..sub.i.k. This suggests that two dead reckoning integrations will be performed for the time interval ›t.sub.k,t.sub.k +max.sub.i .tau..sub.i.k !: the old integration x(t;k-1) must be processed past time t.sub.k to time t.sub.k +max.sub.i .tau..sub.i.k. At that time, the updated estimate x.sub.k.sup.+ may be computed. Only then can the dead reckoning integrator be initialized with x.sub.k.sup.+ and, hence, dead reckoning integration for the time interval ›t.sub.k, t.sub.k +max.sub.i .tau..sub.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.sub.k,t.sub.k +max.sub.i .tau..sub.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.sup.0 (t;k) depends only on the measured speed and heading rate s.sup.m (u;k) and .psi..sup.m (u;k) for t.sub.k .ltoreq.u.ltoreq.t.ltoreq.t.sub.k+1, and not on any preset estimate (in particular, x.sub.k.sup.+). x.sup.0 (t;k) is just the dead reckoning equation preset with a zero position and heading estimate: x.sup.0 (t.sub.k ;k)=0.

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

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

Applying trigonometry, the following results:

and

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.sub.k,t.sub.k !. Once the updated estimate x.sub.k.sup.+ has been computed, at time t.sub.k +max.sub.i .tau..sub.i.k (+processing time), the following may be evaluated: ##EQU23##

The matrix R(.psi..sub.k.sup.+) is called the rotation matrix.

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

Notice that this last expression utilized x.sup.0 (t;k) , not x.sup.o (t;k-1) Thus, x.sup.o (t;k-1) does not need to be integrated past time t.sub.k, and there is no need to store the primary measurement data s.sup.m (t) and .psi..sup.m (t) and re-integrate the dead reckoning equation over the time interval ›t.sub.k,t.sub.k +max.sub.i .tau..sub.i.k !.

In summary, the processing of data for a single cycle ›t.sub.k,t.sub.k ! proceeds as follows:

At time t.sub.k

Evaluate the a priori estimates x.sub.k.sup.- =x(t.sub.k ;k-1), .tau..sub.k.sup.-, etc.

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

Continue computing x(t;k-1)=x.sub.k.sup.- +R(.psi..sub.k.sup.-)x.sup.o (t;k).

At time t.sub.k +max.sub.i .tau..sub.i.k, when the measurement vector .tau..sub.k.sup.m is available,

Evaluate the updated estimates x.sub.k.sup.+ =x.sub.k.sup.- +K(.tau..sub.k.sup.m -.tau..sub.k.sup.-).

Begin computing x(t;k)=x.sub.k.sup.+ +R(.psi..sub.k.sup.+)x.sup.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.