Position Calculating and Path Tracking of Three Dimensional Location System based on Different Wave Velocities

Transcription

1 Position Calculating and Path Tracing of Three Dimensional Location System based on Different Wave Velocities Chih-Chun Lin She-Shang ue Leehter Yao Intelligent Control Laboratory, Department of Electrical Engineering, National Taipei University Technology Taipei, Taiwan Abstract This paper presents the design and implementation of a three-dimensional (3D) location system to provide accurate location information. The location information is computed by applying Trilateration technique on three sets of collected distance measurements. Another technique, the Time Difference of Arrival (TDOA), is used to obtain accurate distance measurements between the Transmission Processing Unit (TPU) and the Distance Sensor (DSR) module. TPU wors as the beacon transmitter of infrared (IR) and ultrasonic signals, while DSRs act as fixed reference points of the locating system and are generally attached to the ceiling and wall of the room. Each DSR is a receiver of IR and ultrasonic signals. Up to 250 DSRs are arranged and connected together on a single RS-485 bus called Sensor Grid (SG). Located at the end of the RS-485 bus, the Sensor Processing Unit (SPU) collects and sends data produced by DSR to TPU using Zigbee modules in the construction of a Wireless Sensor Networ (WSN). Both Trilateration technique and Kalman filter are implemented to improve the accuracy of the location system. The 3D system is able to provide accurate locations for path tracing in robot navigation experiments conducted at a large indoor area of our lab. Keywords location system, TDOA, wireless sensor networ, trilateration, path tracing. I. INTRODUCTION In the early stages of development, most robots that were built were intended for industrial, military, and research applications. Various robots were there to replace humans in tedious jobs performed in critical conditions or risy environments. Compare to humans, robots have several advantages such as: (1) high level of precision, (2) suitable for highly repetitive tass, (3) consistent performance for long term use, and (4) always available for deployment. This paper designs and builds an indoor location system to provide location information for mobile robots. The robots can combine location information and information of other sensors such as an ultrasonic range finder and a wheel encoder for the completion of a specific tas. For example, the combination with other sensors can speed up the completion of a complex path planning and exploration tas carried out in an unnown territory. This location system also can be used by robot designers to observe path patterns and motion behaviors of their robots. Robots need additional instruments to improve their navigation capabilities. In outdoor area, some mobile robots can get location information from GPS [1]. But the poor coverage of satellite signal in indoor area is maing GPS unsuitable for indoor applications. Since enough indoor resolution and accuracy are basic requirements, indoor location system is needed; especially in large scale indoor applications, so that the navigation of mobile robots can achieve better navigation performance. All indoor location systems currently available are using one of the three basic techniques to determine a given location. The three basic techniques for automatic location sensing are triangulation, proximity, and scene analysis [2]. Location of an object can be determined by measuring either the radial distances (Lateration), or directions (Angulation). For measuring the distances, several approaches are associated with the Lateration technique. They are: (1) the Time-of-Flight (TOF) method [3], the Time Difference of Arrival (TDOA) method [4], and the Received Strength Signal Index (RSSI) method [5]. Angulation technique is usually used by location system that requires the implementation of phase antenna arrays [6] or ultrasonic receiver array [7]. The ubiquitous Wireless LAN [8] and Zigbee [9] infrastructures are other communication techniques widely use by practitioners who design indoor location systems. Several advantages are associated with using Zigbee in sensor networ applications; low power consumption, the ease of creating a mesh networ, the self-healing capability, and the support for large number of nodes [10]. Below are several most common approaches for developing indoor location systems: (1) Active Badge (system using Infrared signals) [11], (2) Active Bats (system using ultrasonic signals) [12], (3) RADAR (system based on RSSI for Wireless LAN) [13], (4) Cricet (system based on TDOA which is using RF signal and ultrasonic signal) [14], and (5) LANDMARC (location system is based on RFID technology) [15]. Under the Cricet architecture, the Cricet beacons are installed on the ceiling and two signals are sent out at the same time. On each transmission, a beacon concurrently sends information about the coverage space over RF, together with an ultrasonic pulse [14]. The listener wors as the receiver that is attached to the object and the object s location is determined by estimating the receiver s distance from the beacon. The listener determines its distance to the beacon by using the time difference between the receipt of the first bit of

2 RF information and the ultrasonic signal. This location system also relies on implementing the TDOA architecture to estimate the distance between the listener and the beacon. In contrast to the Cricet architecture, this system uses IR signal and ultrasonic signal and tries to estimate the distance between a set of sensors as fixed reference points and the beacon transmitter. The sensor, also nown as Distance Sensor (DSR), is a small hardware platform comprised of an interval counter, a microcontroller, RS-485 transceiver, and associated hardware for receiving both IR signal and ultrasonic signal. Since location technique such as Trilateration uses geometric properties of triangles to compute object locations, we now we need at least the distances of three sensors from the beacon transmitter to determine a single object location. In order to enable our system to cover a wide area, up to 250 DSRs are arranged and connected in a single RS-485 bus to form a Sensor Grid (SG). Each SG has a specific coverage area, a Sensor Processing Unit (SPU) located within the SG is used to send data received from each DSR bac to the Transmission Processing Unit (TPU) attached to the host device (the AGV). Zigbee modules are used to maintain a wireless communication lin between the SPU and the TPU. Figure 1 is an illustration of the location system with two sensor grids (SG) and three units of Automatic Guided Vehicles (AGV). Figure 2 Locating System architecture This system is based on artificial landmars which are installed on ceiling called Distance Sensor (DSR). Transmission Processing Unit (TPU) wors as the beacon transmitter attached to a host device (the AGV). The system provides local position information for the host device navigating in each space or Sensor Grid (SG). All data from every DSR in the same SG are maintained by Sensor Processing Unit (SPU). Indoor location system for large area can simply be achieved by adding more SG to cover required area. Since all wireless communication networs in the system use Zigbee modules, a mesh networ topology that performs the complex data routing between all nodes is constructed. Figure 1 Illustration of the 3D location system A. Transmission Processing Unit (TPU) The Transmission Processing Unit (TPU) is equipped with a beacon transmitter that sends out two signals of different velocities. The two signals are sent concurrently, and each DSR can detect both signals at different time intervals to compute the Time Difference of Arrival (TDOA) of the signals. Information at the DSRs are sent bac to TPU by the SPU. Communication lin between TPU and SPU is wireless and uses Zigbee communication protocol. TPU is also equipped with a temperature sensor to compensate for environmental factors that can affect the calculation of ultrasonic speed. II. SYSTEM STRUCTURE In Figure 2, we show the architecture of the Three Dimensional Locating System Based on Different Wave Velocities. The location system consists of three main parts: Transmission Processing Unit (TPU). Distance Sensor (DSR). Sensor Processing Unit (SPU). B. Distance Sensor (DSR) The Distance Sensor (DSR) functions as a timer module that counts the TDOA of two different signal waves. All the DSRs are connected together on a RS-485 bus, allowing ID of each DSR and its timing data to be sent to the SPU for further processing. Figure 3 illustrates the flight time (Δt) of two signals propagating with different speeds over a certain distance.

3 TPU Send Synchronous Signal (IR) TPU Send Distance Measure Signal (Ultrasonic) DSR Receive Synchronous Signal (IR) DSR Receive Distance Measure Signal (Ultrasonic) Flight Time (t) Figure 3 TDOA distance measurement scheme Time D. Sensor Grid (SG) DSRs are arranged and connected together on a single RS-485 bus called Sensor Grid (SG). As shown in Figure 5, each SG is associated with one SPU module connected to numerous DSRs on the RS-485 bus. A wireless technique using Zigbee modules is used to send processed information from SPU to TPU. TPU is attached to the host device. Since each SG is associated with a specific coverage area, SPU s ID contains a string of human-readable text indicating the name of the space such as living room that is related to where the SPU is deployed. The DSR measures the time interval Δt between the arrival of IR signal and the arrival of the US signal. This information is used to calculate d, which is the distance between DSR and TPU using the following equation: Δ d d t = v v (1) us Speed of IR signal refers to the speed of light, v IR 3 x 10 8 m/s. At normal room temperature and humidity, the speed of sound v US 344 m/s. Since v IR >> v US,, we can rewrite equation (1) as in: d Δt v us (2) IR C. Sensor Processing Unit (SPU) Sensor Processing Unit (SPU) located in every Sensor Grid (SG) functions as an information processing center. It is used to process collected data from every DSR modules connected in the same SG. SPU also sends the collected data, normally distance information, to TPU via Zigbee. Data are collected from DSR via a polling technique implemented for transferring of information on the RS-485 bus. The technique wors by sending a query command to each DSR (Figure 4). When multiple AGVs are using the networ, the SPU holds important rules regarding the sharing of time slots for processing numerous beacons in a first-come-first-serve basis. This approach is implemented to avoid collision of the beacons. MA485 DSR1 MA485 RS-485 bus SPU MA485 DSRn Figure 4 Communication between DSR and SPU on RS-485 E. Host Firmware Figure 5 Configuration of Sensor Grid Figure 6 shows the bloc diagram of the host firmware which consists of six modules. Since USB interface on the host creates virtual communication port, the host is connected to the TPU using serial communication port. The transmission speed of the port can be set up to 115,200 bps, depending on configurations on the TPU. Data sets received from the SPU may contain data from more than three sets of DSRs, the data set selection module is activated to perform minimum selection. Only three sets of data are needed for Trilateration computation. Error Compensation module compensates for errors that are liely to occur during distance measurement. Next, the identities of those three data sets must be confirmed by checing corresponding DSR s ID before computation is performed by Trilateration algorithm. The Data Set Pattern Selection module is activated to determine and display the triangular pattern of the data sets. The Trilateration Algorithm module then converts the three data sets into distances and computes the TPU s position with respect to the DSR locations stored earlier in a database. Since Trilateration computation can not tolerate any small bit of error, errors that are committed during the sensor operations can produce computational errors. To reduce such errors, the Kalman Filter module is applied to estimate the current object position using previous nown results and time as references. The output from this module can be used by path tracing or other context-aware projects.

4 Figure 6 Bloc diagram of host firmware III. POSITION LOCATING AND PATH TRACKING This section uses Trilateration technique to describe the located position and Kalman filter to improve estimation error. The overall result can enhance tracing performance. A. Trilateration technique Figure 7 shows the three DSRs (DSR1, DSR2 and DSR3) that act as reference points in a nown coordinate system. The host device is an AGV that carries a TPU on board. Once TPU transmits IR and ultrasonic signal as a beacon, all the DSRs can determine their own distances relative to TPU position by measuring the time difference of arrival of both signals. The distance measurement is performed by estimating the distance between the beacon and the DSRs within the coverage area under the ceiling. Only three sets of distance measurements are needed to find the 3D position of the object. Figure 7 Layout of 3D location system Figure 8 shows the 2D layout of Trilateration technique. The purpose is to simplify the concept and Trilateration computation in order to show how to locate 3D position of the AGV. In Figure 8, assume we now the values of r1, r2, and r3 are the relative distances between DSRs and AGV and the YZ coordinates of every DSR. Assume we do not now the radius r of a sphere (denoted by the circle) that is used to represent the distance of the AGV relative to the DSR and as the position of the AGV in the YZ coordinate system. We denote the unnown coordinates of the AGV which are to be located as T x, T y, T z. From available information corresponding to DSRs positions and their relative distances to AGV, we can define a 3D YZ coordinate system with the following equation: ( T - DSR ) + ( T - DSR ) + ( T - DSR ) = r, = 1~3 Y Y Z Z (3) Since there are only three reference points which are needed for computation, we can always assume that all DSRs are located on the flat DSR plane (Z=0) shown in Figure 7. Then we can assume that DSR1 is located at the origin of the coordinate system (0,0,0). Referring to Figure 8, a, b, c are variables used to formulate equations of each sphere: ( DSR, DSR, DSR ) ( DSR, DSR, DSR ) 3 3Y 3Z a Y Z b (T,T,T ) Y Z r 2 1 DSR 1 DSR 3 r 3 c r DSR 2 ( DSR, DSR, DSR ) 2 2Y 2 Z Figure 8 2D layout of 3D location system using Trilateration technique Where a, b, c are: a = DSR3 DSR 1 b = DSR3 DSR Y 1Y c = DSR DSR + Y + Z = r 1 (4) ( c) + Y + Z = r (5) 2 3 ( a) + ( Y b) + Z = r (6) 2 1 Using equation (4) and (5), is defined as in equation (8): r1 r2 + c = (8) 2c Substituting from equation (8) into equation (4), the intersection of the first two spheres is defined as in equation (9): r r c Y + Z = r1 (9) 2c By putting equation (9) into equation (6), Y is defined as in equation (10): ( ) ( r ) 2 1 r2 + c r r + a b Y = + (10) 2 2b 2 8bc Since and Y coordinates of the point in the middle is the AGV position, equations (8) and (10) can be substituted into equation (4) to obtain the Z-coordinate as in equation (11): (7)

5 Z = r Y (11) Since the solution space of the AGV position always resides in the DSRs plane (the ceiling) and it is a one-sided plane, T Z is defined as in equation (12) with the assumption that all DSRs must lie in the single flat plane with their Z-coordinates equal to: TZ = DSRn Z Z, n = 1~3 (12) Therefore, an AGV position in a three-dimensional coordinate system can be determined by the following equations: T = DSR + (13) Y 1 T = DSR + Y (14) B. Kalman Filter The Kalman filter is an efficient recursive filter that estimates the state of a dynamic system by taing sequential measurement of related noise. This means that only the estimated state of the previous time step and current state measurement are needed to compute the estimate of the current state. Kalman filter time update equations ( Predict ) Project the state ahead 1 Y xˆ = Axˆ + Bu (15) 1 Project the error covariance ahead T P = AP 1A + Q (16) Initial estimate for xˆ 1 and P 1. Kalman filter measurement update equations ( correct ) Compute the Kalman gain T T 1 K = PH ( HPH + R) (17) Update estimate with measurement z xˆ = xˆ + K ( Z Hxˆ ) (18) Update the error covariance P = ( I K H) P (19) We define xˆ as the a priori state estimate at step, and x ˆ the a posteriori state estimate at step. The a priori estimate error covariance is P, and the a posteriori estimate error covariance is P. The matrix A relates the state at the previous time step 1 to the state at the current step. The matrix B relates the optional control input u to the state x. The matrix H relates the state to the measurement z. Q is the process noise covariance and R is the measurement noise covariance. Figure 9 shows the test result of position measurements obtained from Trilateration computation and the application of Kalman filter. The result shows that Kalman filter reduces the variance in -coordinate. Each of the Trilateration measurements is indicated by a cross and each of the values estimated by Kalman filter is shown as a dot. Fig. 9 position measurements with Kalman filter IV. EPERIMENT RESULT Since location system that uses Trilateration technique requires three distance measurements as input, the accuracy of these distance measurements will directly affect the accuracy of the located position. Figure 10 shows the set-up of an experiment to observe errors that can be generated from distance measurement. Figure 10 Diagram of Experiment setup to observe the error in distance measurement For this experiment, distance measurements are conducted at different distances ranging from 250 cm to 500 cm with steps of 25 cm at ambient temperature of 23.7 C and the measurement at each position is conducted for 300 times. Figure 11 shows the result of distance measurements conducted at the various stipulated distances. The gap between the two lines is the difference between experiment results and the actual distances acting as reference for this experiment. As shown, the difference gets wider as the distance gets bigger. Figure 11 Distance measurement experiment result with distance from cm Figure 12 shows the errors resulted from distance measurement and also a linear aggregation function that is implemented to approximate the best error compensation function. This function is used to compensate for errors resulting from distance measurements at the various located positions designated for a path tracing experiment.

6 Fig. 12 Error from distance measurement and linear aggregation function In the second experiment, a testing site of 250cm 250cm wide and a height of 3 meter is created. The coverage area is installed with a total of 9 DSRs attached to the ceiling. The DSRs are located at a distance of 120 cm apart. We did some experiments to test our system s performance. First, we tried to locate some nown positions. Figure 13 shows the result of estimated positions from the testing ground. The triangle represents the location of DSR installed on the ceiling. The cross indicates the real TPU position, and the dot shows the estimated position by the system. And Table 1 shows the exact positions of each marer. Figure 14 2D layout of AGV on two circular movement Figure 15 3D layout of AGV on two circular movement V. CONCLUSION Figure 13 The result of position estimation in our testing environment Table 1. Data of position for each marer. Position Real position (cm) Estimation (cm) Marer Y Z Y Z a b c d e f Next, an AGV is programmed to mae two circular trajectories, each of them has 150 cm diameter, in a path tracing experiment. Figure 14 and Figure 15 show the trajectories from the AGV experiment. We present an inexpensive, wide area indoor location system that determines 3D location for a target object. Our approach uses Trilateration method and Kalman filter to locate positions and implementing wireless networ with Zigbee modules. The sensor grids are used to trac locations of moving object in an environment where information transfer is carried out by wireless networ. We address the time synchronization and processing time problems associated with IR signal to enable the signal to automatically reach the number of sensors that are also covered by the ultrasonic signal. Because each sensor can only cover an area of the grid, we implement RS-485 bus as the means of communication between sensors and Sensor Processing Unit. Wireless lins between sensor grids and moving objects mae this system applicable for wide area and low cost implementations. REFERENCES [1] Bulusu, N., Heidemann, J., Estrin, D., GPS-less low cost outdoor localization for very small devices, IEEE Wireless Communications, vol. 7, issue 5, Oct. 2000, pp

7 [2] M. Maeda, T. Ogawa, K. Kiyoawa, and H. Taemura, Tracing of user position and orientation by stereo measurement of infrared marers and orientation sensing, 8th International Symposium on Wearable Computers, vol. 1, pp , 31 October-3 November [3] S. Y. Par, H. S. Ahn, and W. Yu, Round-trip time-based wireless positioning without time synchronization, in Proc. IEEE Int. Conf. Control Automation and System, pp October [4] A. Ward, A. Jones, and A. Hopper, A New Location Technique for the Active Office, IEEE Personal Communications Magazine, vol. 4, no. 5, pp , October [5] Abedin, Mohammed Jainul; Mohan, Ananda Sanagavarapu; Localization of 3-D Near-field Electromagnetic Sources Using C-SPRIT, Radio and Wireless Symposium, 2007 IEEE 9-11 Jan [6] Y. Shen and M. Z. Win, Performance of Localization and Orientation Using Wideband Antenna Arrays, in Proc. IEEE Int. Conf. on Ultra-Wideband, pp , September [7] N. B. Priyantha, A. K. L. Miu, H. Balarishnan, and S. Teller, The Cricet Compass for Context-Aware Mobile Applications, in Proc. 7th ACM MOBICOM Conf., pp. 1 14, Rome, Italy, July [8] Widyawan, M. Klepal, and D. Pesch, Bayesian Approach for RF-based Indoor Localization, IEEE 4th International Symposium on Wireless Communication Systems, pp , Trondheim, Norway, October [9] S. Schwarzer, M. Vossie, M. Pichler, and A. Stelzer, Precise distance measurement with IEE (Zigbee) devices, IEEE Radio and Wireless Symposium, pp , Orlando, FL, January [10] W. Qiu, P. Hao, and R.J. Evans, An efficient self-healing process for Zigbee sensor networs, International Symposium on Communication and Information Technologies, pp , Sapporo, Japan, October [11] R. Want, A. Hopper, V. Falcao, and J. Gibbons, The Active Badge Location System, ACM Trans. on Office Information Systems (TOIS), vol. 10. no. 1, pp , Jan [12] A. Harter, A. Hopper, P. Steggles, A. Ward, and P. Webster, The Anatomy of a Context-Aware Application, In Proc. 5th ACM MOBICOM Conf., pp , Seattle, WA, August [13] P. Bahl and V. N. Padmanabhan. RADAR: An in-building RF-based user location and tracing system. In INFOCOM, pages , [14] N. B. Priyantha, A. Charaborty, and H. Balarishnan, The Cricet Location-Support System, in Proc. 6th ACM MOBICOM Conf., pp , Boston, MA., August [15] L. M. Ni, Y. Liu, Y. C. Lau, and A. P. Patil. LANDMARC: Indoor location sensing using active RFID. Wireless Networs, 10(6): , 2004.