INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 12, No. 5, pp. 783-790

OCTOBER 2011 / 783

DOI: 10.1007/s12541-011-0104-x

Ultrasonic Satellite System Moving Positioning by Extended Kalman Filter

Object

Kang Sup Yoon1, Su Yong Kim2 and Ju Yong Choi3,# 1 Division of Mechanical and Automotive Engineering, Daegu University, Jillyang, Gyeongsan, Gyeongbuk, South Korea, 712-714 2 Hyundai Mobis, 80-9, Mabook-dong, Giheung-gu, Yongin, South Korea, 449-910 3 Department of Mechatronics Engineering, Kyungsung University, 314-79, Daeyeon-dong, Nam-gu, Busan, South Korea, 608-736 # Corresponding Author / E-mail: [email protected], TEL: +82-51-663-4695, FAX: +82-51-624-5980 KEYWORDS: Ultrasonic satellite system(USAT), Geometric positioning, Sensing noise, Extended Kalman filter(EKF), Mobile robot

This paper presents positioning algorithms for an ultrasonic satellite system (USAT) consisting of multiple ultrasonic transmitters and receivers in buildings. The previously used inverse matrix method of calculating USAT positions suffers from problems related to transmitter layout, and the method is sensitive to sensor noise. To solve these problems, a geometric approach with verification by a comparison of simulations with the inverse matrix method, is suggested. However, when an object is moved quickly, the positioning error is increased. The moving object positioning algorithm with an extended Kalman filter (EKF),which takes account of the dynamics of the moving object, is therefore proposed for estimating USAT positioning during movement. The accuracy of the proposed algorithm is evaluated by simulations and experiments. The experimental results show that the proposed algorithm gives a better performance for dynamic states. Manuscript received: August 20, 2010 / Accepted: April 17, 2011

1. Introduction Recently, the demand for intelligent and efficient service robots has been increasing because of changes in society, such as ageing populations, the need for personalized services, and information needs. In robot path planning and obstacle avoidance, accurate positioning is important. There are many different positioning algorithms which depend on the characteristics of the sensor system. Dead reckoning (DR) and inertial navigation systems (INSs) are based on acceleration sensors, gyro sensors, and odometers; they are self-contained and not dependent on external signals. However, for inertial sensors, these methods generate an unlimited number of errors. CCD camera vision systems require complicated signal processing to analyze images. In addition, they are expensive and highly dependent on camera calibration and image sensitivity.1,2 Positioning using infrared light is easy and inexpensive but suffers from problems such as poor performance and limited application in outdoor environments.3 A global positioning system (GPS) can give accurate information for identifying locations; moreover, a differential GPS provides improved positioning accuracy. However, this type of sensor usually provides measurements at 1–10 Hz. This sampling rate may not be sufficient if the dynamics of the positioning object changes relatively quickly. Besides, it does not operate well in places where there is no GPS satellite signal, or less © KSPE and Springer 2011

than four visible satellites.4 To cope with these problems, the integration of GPS/INS has been used to deal with the long-term errors in INS and the short-term errors in GPS.5 Pseudo-satellites for indoor or outdoor use have also been researched. The ultrasonic positioning system, the basic concept of which is similar to that of the GPS, has the advantages of low cost and a rapid update rate. Lee6,7 introduced an ultrasonic satellite system (USAT) based on a period-detecting algorithm,8 which uses the inverse matrix method to solve the six equations obtained for the distances between four fixed transmitters and a moving receiver. However, the transmitters should be fixed at different heights, and the positioning error increases in proportion to the sensor noise and receiver moving speed. Yi,9 Hwang,10 and Tsai11 used a Kalman filter to solve the problem of sensor noise. An extended Kalman filter (EKF) algorithm for noisy ultrasonic signals has limitations in its positioning performance because only one receiver is used and the angular velocity is ignored in the position state equation.9 Hwang10 used information from two receivers and applied the information from each receiver to the EKF algorithm separately. A combined gyro-compass and ultrasonic positioning system can be used with a Kalman filter, but angular motion is not considered in the linearized measurement equation.11 Generally, EKFs dealing with nonlinear systems tend to diverge when the initial values and covariances are larger than the actual values.12,13

784 / OCTOBER 2011

INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 12, No. 5

In this paper, USAT positioning algorithms are presented. Calculation of USAT positions using the inverse matrix method7 suffers from singular problems caused by the USAT transmitter layout. A geometric method, which is insensitive to the USAT transmitter layout, is suggested for solving this problem; the calculations are verified by comparing the simulations with the inverse matrix method. The heading angle is also calculated using the information from the four receivers on the moving object. In order to improve the positioning performance in situations where the positioning errors increase during rapid motion, an EKF moving object positioning algorithm that takes the dynamics of the moving object into consideration is proposed. Simulations and experiments are performed to verify the performance of the proposed algorithm. The remainder of the paper is organized as follows. Section 2 presents USAT positioning. In section 3, the proposed EKF positioning algorithm for USAT is derived. Section 4 describes the experimental results. Finally, the paper is summarized in section 5.

2. Positioning of Ultrasonic Satellite System 2.1 Period-detecting method In the USAT, the measurement distance is proportional to the time for the wave to travel from the transmitter to the receiver and is estimated as follows: d = c × TOF + d 0 ,

(1)

where c is the speed of sound, TOF is defined as the difference between transmitting time tT, and receiving time tR, and d0 is the distance offset due to mismatching between the acoustic center and the geometric center of real transmitters. The measurement accuracy is determined by the accuracy of detection of tR. Generally, tR is obtained by the threshold and envelope methods. In the threshold method, the attenuation of the signal increases the distance error. If the threshold level is decreased to overcome this attenuation problem, the influence of noise is increased. In contrast, the envelope method is independent of attenuation; however, the amplification gain at the transmitters must be continually modified to prevent the saturation of the signal at the receiver. There are also other methods to detect tR, such as methods using the maximizing time of the cross correlation between the transmitted signal and the received signal,14 or using phase information for the modified transmitted signal as binary frequency shift.15 This study uses the period-detecting method,8 which is based on the fact that the period of the transmitted signal wT(k), does not change although the amplitude of wT(k) is attenuated with an increase in the sensing range. This method is fulfilled as follows: the first step is the filtering of high frequency noises in the received signal wT(t) using an analog low-pass filter in the circuit. The filtered signal wˆ R (t ) is then converted into a digital signal wˆ R (k ) by an A/D converter. To find the period of the received signal wˆ R (k ) is processed to Cw(k) by a convolution operation as follows:

Cw (k ) = ∑ wT ( j ) ⋅ wˆ R (k + 1 − j ). j

Analog filtering and digital signal processing play a role in band-pass filtering. Although this filtering method can be applied to the threshold method tR cannot be determined when the amplitude of wR(t) is smaller than that of noise. To take into account the Doppler effect in this period-detecting method, tR is determined when the differences between the periods TR and TT of wR(t) and wT(t) respectively, are within ε:

TR − TT ≤ ε .

(3)

To distinguish between noises having the same period TT, the period should be repeated n-times consecutively as TT because the frequency of noise varies greatly.

2.2 Positioning of Ultrasonic Satellite System 2.2.1 Inverse matrix method Fig. 1 shows an infrastructure transmitting configuration of the USAT. Four ultrasonic transmitters are installed in fixed positions such as a ceiling, and the receiver is installed on the moving object. A moving receiver is synchronized with each fixed transmitter by an RF signal. When many transmitters transmit ultrasonic waves at the same time, the receiver cannot distinguish them because of the interference of the waves. The ultrasonic waves should therefore be sent successively from each transmitter. The synchronizing interval of the transmitter should be determined by considering the maximum sensing range and the speed of sound. In ideal cases, the receiver position R ( xR , yR , z R ) is the point of contact with spheres of radiuses di from the transmitters Ti(ai, bi, ci). So, the vector representation of an ultrasonic receiver position is written as ( xR − ai ) 2 + ( yR − bi ) 2 + ( z R − ci ) 2 = d i2 .

(4)

As shown in Fig. 1, a position can be determined by four distances di from different transmitters Ti (i = 1, 2, 3, and 4): (ai − ai +1 ) xR + (bi − bi +1 ) yR + (ci − ci +1 ) z R = Bi ,

where B = {(a 2 − a 2+1 ) + (b 2 − b 2+1 ) + (c 2 − c 2+1 ) − ( d 2 − d 2+1 )}/ 2. i

i

i

i

i

i

i

i

i

(5) The

T

receiver position R = [ xR yR z R ] is then obtained from R = A−1B,

(6)

 a1 − a2 b1 − b2 c1 − c2   B1    where A =  a2 − a3 b2 − b3 c2 − c3  and B =  B2  .  B3   a3 − a4 b3 − b4 c3 − c4 

Thus, the transmitters should be located so that the inverse matrix of A in eq. (6) may exist, that is, det A≠ 0. If the condition number

T4

Transmitters d4

T1

T3 d3

T2

d1 d2

Sensing Area

R Receiver

(2) Fig. 1 Configuration of an ultrasonic satellite system (USAT)

INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 12, No. 5

of A is large, the influence of B on R is increased; therefore the positioning error is sensitive to the noise in B.

2.2.2 Geometric method A geometric method is introduced to deal with restrictions on the location of transmitters and the sensitivity to sensor noise. Πij is an intersecting plane between eq. (4) from the transmitter Ti and the transmitter Tj. If the z-axis positions of the transmitters are the same height, the receiver position Ri, as shown in Fig. 2, is obtained as x =C −D ⋅z ,

(7)

y = E − F ⋅ z , and

(8)

ijk

ijk

ijk

ijk

e − ab E ij

ij

a

E = ijk

jk

a b −a b

jk

ij

jk

ij

jk

ij

ij

jk

a b −a b

jk

ij

ij

jk

ij

jk

,

a = a −a , b =b −b , c = c −c , ij

i

j

2

ij

i

j

H

i

ijk

2

ij

i

j

i

2

2

ijk

ijk

2

2

2

j

i

j

j

e = (a − a ) + (b − b ) + (c − c ) − (d 2 − d 2 ), ij

2

In the USAT system, the heading angle ψR of an object is detected by at least two receivers. If four receivers are located on the edge of an object of length 2L and width 2W, the angle ψij between R R and the x-axis is written as i

j

 yR i − yR j ψ ij = tan −1   xR i − xR j 

 .  

(11)

Finally, the heading angle of an object is obtained from the average of angles ψij: ψR =

ψ 12 + (ψ 23 − π / 2) + ψ 34 + (ψ 41 − π / 2) . 4

i

j

= D + F + 1,

Because the error pattern of the positioning at each receiver is similar, the heading angle is not much affected by the positioning error.

In the simulations for the evaluation of the geometric method, an object is initially located at (1, 1, 0) m and 45°. Noises, which are normally random distributed signals with zero mean value and standard deviation (STD) 6 mm are added to radiuses di. Fig. 3 shows the positioning results for a stationary object. The z-axis

I = − 2[ F ( E − a ) + H (G − b ) + c ], and ijk

ijk

ijk

j

ijk

2

ijk

j

2

j

2

2

j

j

J = ( E − a ) + (G − b ) + c − d . ijk

ijk

j

ijk

j

(12)

2.4 Simulation results

ij

a c −a c ij

ijk

(9)

2.3 Heading angle calculation

,

ijk

ab

, F =

,

ijk

ijk

ij

a e −a e ij

ijk

c −b F

ijk

ij

ij

ijk

ijk

2H , D =

ijk

ijk

ijk

ijk

ijk

ijk

− I ± I 2 − 4H J

z = where C =

ijk

OCTOBER 2011 / 785

x-axis error y-axis error z-axis error

60

ijk

information from the four transmitters successively, the receiver position is defined in vector form as




 R = R123 + R 234 + R 341 + R 412 / 4, (10)
 where R

(

ijk

)

Positioning error [mm]

40

Generally, the receivers mounted on the moving object are below transmitters in fixed locations such as the ceiling in a building. So, z is selected to be negative in equation (9). By averaging the

ijk

0

-20

-40

= ( x , y , z ). If the transmitters are located in a ijk

20

-60

ijk

rectangle and at the same height (det A = 0), the previous inverse matrix method cannot solve equation (6), but equations (7)-(9) can be simplified by the geometric method.

0

1

2

3

4

5

Time [sec]

(a) Inverse matrix method x-axis error y-axis error z-axis error

60

ij

R ' ijk

d

Π

i

T

i

d

ik

Positioning error [mm]

40

Π

20

0

-20

-40

i

R

ijk

-60

0

1

2

3

4

5

Time [sec]

(b) Geometric method Fig. 2 Geometric method intersection

Fig. 3 Simulation results for staionary object positions (vP = 0 m/s)

786 / OCTOBER 2011

INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 12, No. 5

error in the inverse matrix method is larger than that for the other axes; however, in the geometric method, the errors on all of the axes are similar. Since the influence of the z-axis positioning error on the heading angle ψR in the x-y plane is small, the heading angle errors are similar in both methods, as shown in Fig. 4. Simulated movements of an object from (1, 1, 0) m to (4, 4, 0) m at constant longitudinal velocities (vP = 0.2, 0.6, 0.8, and 1.0 m/s) are summarized in Table 1. Fig. 5 and 6 show the simulation results

when vP = 1 m/s. As shown in these Figures, the z-axis positioning error is larger than the x-axis and y-axis positioning error due to the geometric arrangement of the ultrasonic transmitter and receiver. As the velocity of the object increases, the positioning errors increase in proportion. Therefore, another approach should be considered for moving object positioning.

3. EKF Algorithm for Moving Object Positioning inverse matrix method geometric method

1

0

-1

-2 0

1

2

3

4

5

Time [sec]

Fig. 4 Simulation results for stationary object heading angles (vP = 0 m/s) x-axis error y-axis error z-axis error

1500

Positioning error [mm]

1000

Since USAT positioning errors are increased by factors such as the movement of the receiver, variations in sound speed, and external noise, it is difficult to estimate positions accurately. Among these problems, the fast movement of the object is the major cause of positioning error. When the receiver is fixed (see Fig. 7(a)), position is constant. However, since the USAT successively sends ultrasonic waves with the same interval, the spheres with radiuses di intersect at different positions during the movement of the receiver, as shown in Fig. 7(b). Therefore, in order to estimate the position of the moving object accurately, a moving object positioning algorithm with an EKF to take into account the dynamics of the moving object is proposed.

3.1 Moving object positioning algorithm In the previous section, the position was determined as the average of the intersecting positions. If the kinematics of the receiver is considered, the error caused by movement can be reduced. The positions of receivers Ri(xR, yR, zR) on object P(xP, yP, zP) are given by

500

2

inverse matrix method geometric method

0

-500

-1000

-1500 0

1

2

3

4

5

Time [sec]

(a) Inverse matrix method x-axis error y-axis error z-axis error

200

heading angle error [deg]

heading angle error [deg]

2

1

0

-1

-2

Positioning error [mm]

0

1

2

3

4

5

Time [sec]

100

Fig. 6 Simulation results of for moving object heading angles (vP = 1 m/s)

0

Table 1 Positioning errors for each axis [mm] -100

-200 0

1

2

3

4

5

Time [sec]

(b) Geometric method Fig. 5 Simulation results for moving object positions (vP = 1 m/s)

Inverse matrix method Velocity xerror yerror zerror [m/s] Max. R.M.S Max. R.M.S Max. R.M.S 0.0 10.4 3.2 19.4 6.2 94.9 31.3 0.2 50.2 24.6 102.8 34.2 389.3 168.4 0.4 87.5 49.6 175.0 67.6 556.7 326.3 0.6 129.1 74.3 253.7 100.2 830.6 485.0 0.8 173.3 96.9 338.7 130.7 1,128 646.0 1.0 213.6 122.6 398.5 163.1 1,306 792.2

Geometric method xerror yerror zerror Max. R.M.S Max. R.M.S Max. R.M.S 8.6 3.3 19.4 5.2 22.7 5.2 24.5 8.8 29.3 10.6 42.5 19.7 36.1 16.4 43.0 19.0 74.0 38.4 56.9 24.2 54.6 28.5 107.3 57.6 81.6 33.3 77.1 36.9 143.0 75.8 97.3 39.2 93.7 45.7 172.9 96.0

INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 12, No. 5

OCTOBER 2011 / 787

R1 ( xP − W ⋅ Cψ + L ⋅ Sψ , yP − W ⋅ Sψ − L ⋅ Cψ , z P )

∆xP (k ) = vP (k ) ⋅ Cψ ⋅ ∆t ,

R2 ( xP + W ⋅ Cψ + L ⋅ Sψ , yP + W ⋅ Sψ − L ⋅ Cψ , z P )

∆yP ( k ) = vP (k ) ⋅ Sψ ⋅ ∆t ,

(13)

R3 ( xP + W ⋅ Cψ − L ⋅ Sψ , yP + W ⋅ Sψ + L ⋅ Cψ , z P ) R4 ( xP − W ⋅ Cψ − L ⋅ Sψ , yP − W ⋅ Sψ − L ⋅ Cψ , z P ),

where Cψ = cosψP and Sψ = sinψP. Taking the variations in sound speed and external noise into account, the virtually sensed distance d
from the i-th transmitter to the j-th receiver is expressed as

where ∆t is the sampling time. It is assumed that when (ωP(k) = 0) the process noise increases; however, the modeling errors ξx, ξy, ξz, and ξxv are included in equation (18). The state equation with process noise Ξ(k) can be simplified to X (k + 1) = f ( X (k )) + Ξ(k ),

ij

d
= α ⋅ d + n , ij

ij

(14)

ij

where α is the compensation coefficient of the sound speed, dij is the real distance from the i-th transmitter to the j-th receiver, and nij is the noise in the virtual sensor. In order to reflect the error caused by object movement, the noises are added to position P(xP, yP, zP), heading angle ψP, and longitudinal velocity vP of the object. Then the following states are virtually sensed. x
P = xP + nx y
P = yP + n y z
P = z P + nz ψ
P = ψ P + nψ

(15)

v
P = vP + nv ,

where the noises nx, ny, nz, nv, nψ, and nv are white Gaussian signals of zero mean value. From equations (14) and (15), the measurement equation for the virtual sensor with noise V is obtained as Z = h( X ) + V ,

(16) T

where h( X ) = [ xP yP z P ψ P α ⋅ di1 α ⋅ d i 2 α ⋅ d i 3 α ⋅ d i 4 vP ] ,

V = [nx ny nz nψ ni1 ni 2 ni 3 ni 4 nv ] T . To derive an EKF for the positioning of a moving object, the discrete state variables are defined as follows: X (k ) = [ xP (k ) yP (k ) z P ( k ) ψ P ( k ) α ( k ) vP ( k )] T .

(17)

 xP ( k + 1)   xP (k ) + ∆xP (k )   ξ x ( k )   y (k + 1)   y (k ) + ∆y ( k )  ξ (k )  P  P   P   y   z P ( k + 1)     ξ z (k )  zP (k ) .  = + ψ ( + 1) ψ ( ) + ∆ ψ ( ) k k k P  P   P  ξψ (k )  α ( k + 1)    ξ (k )  α P (k )  P     α   vP (k + 1)   vP (k ) + ∆vP (k )   ξ v (k ) 

T3

T4

d4

d3

(18)

T1

Κ(k ) = P(k | k − 1)H(Xˆ ( k | k − 1))T (21) ⋅ [H(Xˆ (k | k − 1))P(k | k − 1)H(Xˆ ( k | k − 1))T + R( k )] −1 ,

where Xˆ (k ) is the estimated state vector of X(k), R(k) is the process noise covariance matrix, and (k|k-1) represents the previous value and the filtered value. The next step is the compensation of the estimated value by Z(k):

T3

∂h( X ) H(Xˆ ( k | k − 1)) = ∂X 1   0   0  0   α (x − a ) R i  di  =  α ( x R − ai )  di   ( x α − ai ) R   di   α ( x R − ai )  di  0 

T2

0 0 1 0

0 0 0 1

0 0 0 0

α ( y R − bi ) di

α ( z R − ci ) di

h

1

di

4

1

α ( y R − bi ) di

α ( z R − ci ) di

h

2

di

4

2

α ( y R − bi ) di

α ( z R − ci ) di

h

3

di

4

3

α ( y R − bi ) di

α ( z R − ci ) di

h

4

di

4

4

0

0

0

0

2

4

(a) Stationary object

(b) Moving object

Fig. 7 Moving object positioning problem

X = X ( k | k − 1)

0 1 0 0

1

d3

d2

T1

(23)

where H(Xˆ (k | k − 1)) is the Jacobian matrix of the measurement

3

d1

(22)

equation in equation (16) as follows:

R d2 T 2

Based on the white Gaussian measurement noise V(k) and process noise Ξ(k) with zero mean values and variances R and Q respectively, the discrete time extended Kalman filter is derived as follows.12 The first step is the selection of initial values. When an object is stopped for several seconds, the initial position and heading angle are determined by equations (10) and (11). The compensation coefficients of the sound speed αP and the longitudinal velocity vP are 1 and 0, respectively. The error covariances are selected by the analysis of the initial signals. The next step is the calculation of Kalman gain K(k) by

P( k | k ) = [I − K(k )H(Xˆ (k | k − 1))]P( k | k − 1),

R

d1

 ξ x (k )   xP (k ) + vP (k ) ⋅ Cψ ⋅ ∆t  ξ ( k )   y (k ) + v (k ) ⋅ S ⋅ ∆t  P ψ  P   y     ξ z (k )  z P (k ) where f ( X (k ) ) =   and Ξ(k ) =  . ( k ) ψ P   ξψ (k )  ξ (k )    α P (k )  α    vP ( k )    ξ v (k ) 

The error covariance is calculated by

Because this paper only deals with USAT systems without angular velocity sensors, the angular velocity ωP(k) is assumed to be zero. Then, the kinematics of the moving object is simply described as11 d4

(20)

Xˆ ( k | k ) = Xˆ ( k | k − 1) + K( k )[ Z ( k ) − h( Xˆ ( k | k − 1))].

The discrete state equation with modeling errors ξ is written as

T4

(19)

1

1

2

2

3

3

4

4

1

1

2

2

3

3

4

4

0 0  0  0  (24) 0  , 0   0   0   1 

788 / OCTOBER 2011

INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 12, No. 5

∂f ( X (k )) A(Xˆ (k | k )) = ∂X (k )

where h1 =

α  ( xR 1 − ai )(W ⋅ Sψ + L ⋅ Cψ ) − ( y R 1 − bi )(W ⋅ Cψ − L ⋅ Sψ )  d i1 

h2 =

α  − ( x R 2 − ai )(W ⋅ Sψ − L ⋅ Cψ ) + ( y R 2 − bi )(W ⋅ Cψ + L ⋅ Sψ )  di 2 

h3 =

α  − ( x R 3 − ai )(W ⋅ Sψ + L ⋅ Cψ ) + ( y R 3 − bi )(W ⋅ Cψ − L ⋅ Sψ )  di3 

h4 =

α  ( x R 3 − ai )(W ⋅ Sψ − L ⋅ Cψ ) − ( y R 4 − bi )(W ⋅ Cψ + L ⋅ Sψ )  . di 4 

Then, the next time estimated state and covariance are updated by Xˆ (k + 1| k ) = f ( Xˆ (k | k )), P(k + 1| k ) = A(Xˆ ( k | k ))P(k | k )A(Xˆ (k | k )) −1 + Q (k ),

(25)

where Q(k) is the measurement noise covariance matrix and A(Xˆ (k | k )) is the Jacobian matrix in equation (20): Table 2 Simulation results for increasing velocities Velocity [m/s] 0.0 0.2 0.4 0.6 0.8 1.0

Geometric method Perror [mm] ψerror [deg] Max. R.M.S Max. R.M.S 12.20 8.53 1.3685 0.4135 30.04 24.00 1.4118 0.434 55.62 46.22 1.3991 0.4841 84.17 68.13 1.3116 0.5128 109.25 90.09 1.3066 0.579 129.73 113.20 1.7048 0.7102

Proposed algorithm Perror [mm] ψerror [deg] Max. R.M.S Max. R.M.S 20.24 4.02 1.5975 0.4144 21.14 6.58 1.6506 0.3958 29.85 13.81 1.1538 0.4168 24.78 13.97 1.0414 0.4112 41.21 24.82 1.1995 0.4459 35.63 22.82 1.6093 0.5348

25

10

(26)

3.2 Simulation results This section verifies the moving object positioning algorithm by comparison with the geometric method. In the simulation, noises are added to the position P(xP, yP, zP), the heading angle ψP, and the longitudinal velocity vP of an object. Based on the characteristics of the USAT, the covariance matrices of the process noise and the measurement noise in equations (21) and (25) are selected by 2 2 2 2 R (k ) = diag(σ nx2 , σ ny2 , σ nz2 , σ n2ψ , σ nd , σ nd , σ nd , σ nd , σ nv2 ) 2 2 Q(k ) = diag(σ ξ2x , σ ξ2y , σ ξ2z , σ ξψ , σ ξα , σ ξ2v ),

where noises mm/s, mm/s.

the standard deviations of the normally distributed random are σnx = σny = σnz = 50 mm, σnψ = 8.6°, σnd = 6 mm, σnv = 5 σξx = σξy = σξz = 1 mm, σξψ = 1.7°, σξα = 10-4, and σξv = 0.7 As the velocity increases, the positioning error in both the 1.2

Heading angle error [deg]

15

X ( k ) = Xˆ ( k | k )

0 0 vψ (k ) ⋅ Sψ ⋅ ∆t 0 Cψ ⋅ ∆t  1 0 vψ (k ) ⋅ Cψ ⋅ ∆t 0 Sψ ⋅ ∆t  0 1 0 0 0   0 0 1 0 0  0 0 0 1 0   0 0 0 0 1 

The steps from equation (21) to equation (26) are then iterated.

Geometric method Extended Kalman filter

20

Positioning error [mm]

1 0  0 = 0 0  0

5

0.6

0.0

-0.6

-1.2

Geometric method Extended Kalman filter 0

-1.8

0

1

2

3

4

5

0

1

2

5

1.2

Heading angle error [deg]

150

Positioning error [mm]

4

(a) Stationary object (vP = 0 m/s)

(a) Stationary object (vP = 0 m/s) 180

120 90 60

Geometric method Extended Kalman filter

30 0

3

Time [sec]

Time [sec]

0.6

0.0

-0.6

-1.2

Geometric method Extended Kalman filter -1.8

0

1

2

3

4

5

Time [sec]

0

1

2

3

4

5

Time [sec]

(b) Moving object with vP = 1 m/s

(b) Moving object with vP = 1 m/s

Fig. 8 Positioning errors in simulations of geometric and moving object positioning algorithm

Fig. 9 Heading angle errors in simulations of geometric and moving object positioning algorithm

INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 12, No. 5 25

Geometric method Extended Kalman filter

20

Positioning error [mm]

geometric method and the proposed algorithm increases in proportion (see Table 2). The maximum positioning error and rootmean-square error in the geometric method increase faster than they do in the proposed algorithm (see Fig. 8). However, the accuracy of the heading angle is similar in both positioning methods and is less influenced by the velocity as shown in Fig. 9.

OCTOBER 2011 / 789

15

10

5

4. Experiment Results

0

0

1

2

3

4

5

4

5

Time [sec]

(a) Stationary object (vP = 0 m/s) 240

Geometric method Extended Kalman filter Positioning error [mm]

On the basis of the simulation results, positioning experiments were carried out. Fig. 10 shows a mobile robot attached to four ultrasonic receivers; the receivers are 815 mm in length and 885 mm in width. The four USAT transmitters were installed at the edge of the ceiling. The proposed algorithm was derived using the process and the measurement covariance matrices described in section 3.2. The experimental results are summarized in Table 3. The EKF converging time is about 2 s, as shown in Fig. 11(a). Fig. 11(b) and 12(b) were drawn after initializing at fixed positions and accelerating. Although the positioning errors increase with increasing velocity, the proposed algorithm is able to reduce the positioning error of the moving object. Unlike the case for the simulation results, the positioning errors in the experiments were increased by distance errors, external noises, or slipping of the mobile robot.

180

120

60

0

0

1

2

3

Time [sec]

(b) Moving object with vP = 1 m/s Fig. 11 Positioning errors in experiments of geometric method and moving object positioning algorithm

5. CONCLUSION This paper dealt with positioning algorithms for a USAT consisting of multiple ultrasonic transmitters and receivers. Because

Heading angle error [deg]

2

1

0

-1

Geometric method Extended Kalman filter -2 0

1

2

3

4

5

Time [sec]

(a) Stationary object (vP = 0 m/s) 2

Fig. 10 Configuration of a USAT system Table 3 Experimental results for increasing velocities Velocity [m/s] 0.0 0.2 0.4 0.6 0.8 1.0

Geometric method Perror [mm] ψerror [deg] Max. R.M.S Max. R.M.S 15.87 7.35 0.621 0.2071 56.75 45.02 1.5242 0.546 93.60 68.62 1.6217 0.7096 143.12 101.25 1.6947 0.7821 176.44 123.47 1.6252 0.7056 218.49 127.06 1.6844 0.8023

Proposed algorithm Perror [mm] ψerror [deg] Max. R.M.S Max. R.M.S 20.45 7.18 0.7356 0.2285 31.34 22.46 1.4669 0.5587 49.17 32.03 1.5644 0.6755 61.16 40.77 1.4655 0.728 73.03 40.73 1.48107 0.637 93.52 42.37 1.4114 0.7377

Heading angle error [deg]

Geometric method Extended Kalman filter 1

0

-1

-2 0

1

2

3

4

5

Time [sec]

(b) Moving object with vP = 1 m/s Fig. 12 Heading angle errors in experiments of geometric method and moving object positioning algorithm

790 / OCTOBER 2011

the inverse matrix method is not acceptable for the singular matrix obtained with the USAT transmitter layout, a geometric method for positioning was proposed. To verify the performance of the proposed method, simulations were performed. An algorithm for detecting the heading angle was also introduced. However, the positioning error in the geometric method increased for rapidly moving objects. A moving object positioning algorithm with an EKF, which takes into account the dynamics of the moving object, is proposed for accurate positioning of the moving object. Simulations and experimental results show that the proposed algorithm, which included virtual sensors for position, heading angle, and velocity, was able to reduce positioning errors. The positioning accuracy of moving objects can be improved by USAT with the proposed algorithm.

ACKNOWLEDGEMENT This research was supported by Kyungsung University Research Grants in 2011.

INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 12, No. 5

Using Period Detecting Method,” J. of KSPE, Vol. 24, No. 8, pp. 41-49, 2007. 9. Yi, S. Y. and Jin, J. H., “Self-localization of a Mobile Robot Using Global Ultrasonic Sensor System,” J. of ICASE, Vol. 9, No. 2, pp. 145-151, 2003. 10. Hwang, B. H. and Yi, S. Y., “Autonomous Navigation of Mobile Robot using Global Ultrasonic System,” J. of ICASE, Vol. 10, No. 6, pp. 529-536, 2004. 11. Tsai, C. C., “A Localization System of a Mobile Robot by Fusing Dead-reckoning and Ultrasonic Measurements,” IEEE Trans. on Instrumentation and Measurement, Vol. 47, No. 5, pp. 1399-1404, 1998. 12. Brown, R. G. and Hwang, P. Y. C., “Introduction to Random Signals and Applied Kalman Filtering,” John Wiley & Sons, 1997. 13. Choi, K. S. and Lee, S. G., “Enhanced SLAM for a mobile robot using extended Kalman Filter and neural networks,” Int. J. Precis. Eng. Manuf., Vol. 11, No. 2, pp. 255-264, 2010.

REFERENCES

14. Parrila, M., Anaya, J. J. and Fritsch, C., “Digital Signal Processing Techniques for High Accuracy Ultrasonic Range Measurement,” IEEE Transactions on Instrumentation and Measurement, Vol. 40, No. 4, pp. 759-763, 1991.

1. Lee, J. M., Son, K., Lee, M. C., Choi, J. W., Han, S. H. and Lee, M. H., “Localization of a Mobile Robot Using the Image of a Moving Object,” IEEE Trans. on Industrial Electronics, Vol. 50, No. 3, pp. 612-619, 2003.

15. When, H. W. and Belanger, P. R., “Ultrasound-based Robot Position Estimation,” IEEE Transactions on Robotics and Automation, Vol. 13, No. 5, pp. 682-692, 1997.

2. Bae, J., Lee, S. and Song, J., “Localization for Mobile Robots using IRID,” J. of ICASE, Vol. 13, No. 9, pp. 903-909, 2007. 3. Bang, S. and Kim, J., “Initialization Problem of Indoor Mobile Robots with Artificial Stars,” J. of ICASE, Vol. 13, No. 8, pp. 804-809, 2007. 4. Evans, A. G., Swift, E. R., Cunningham, J. P., Hill, R. W., Blewitt, G., Yunck, T. P., Lichten, S. M., Hatch, R., Malys, S. and Bossler, J., “The Global Positioning System Geodesy Odyssey,” Navigation : J. of The Institute of Navigation, Vol. 49, No. 1, pp. 7-33, 2002. 5. Hong, S., Lee, M. H., Kwon, S. H. and Chun, H. H., “A Car Test for the Estimation of GPS/INS Alignment Errors,” IEEE Trans. on Intelligent Transportation Systems, Vol. 5, No. 3, pp. 208-218, 2004. 6. Lee, D. H., Park, J. J., Kim, S. Y., Mun, Y. S. and Lee, M. H., “A Study on the Application of U-SAT system for the Indoor Positioning Technology of Ubiquitous Computing,” J. of ICASE, Vol. 12, No. 9, pp. 876-882, 2006. 7. Lee, D. H., Kim, S. Y., Yoon, K. S. and Lee, M. H., “USAT (Ultrasonic Satellite System) for the Autonomous Mobile Robots Localization,” J. of ICASE, Vol. 13, No. 10, pp. 956961, 2007. 8. Lee, D. H., Kim, S. Y., Yoon, K. S. and Lee, M. H., “A Long Range Accurate Ultrasonic Distance Measurement System by