Fault Detection Algorithm for Automatic Guided Vehicle ... - IEEE Xplore

Fault Detection Algorithm for Automatic Guided Vehicle Based on Multiple Positioning Modules Pandu Sandi Pratama, Yuhanes Dedy Setiawan, Dae Hwan Kim, Young Seok Jung, Hak Kyeong Kim, Sang Bong Kim*

Han Sung Well Tech Co, Ltd. Busan, South Korea

Department of Mechanical and Automotive Engineering Pukyong National University Busan, South Korea [email protected] *Corresponding author: [email protected]

YAHOTEC CO., LTD Gimhae-si, Gyeongsangnam-do, South Korea [email protected]

Abstract—This paper presents implementation and experimental validation of fault detection algorithm for sensors and motors of Automatic Guided Vehicle (AGV) system based on multiple positioning modules. In this paper, firstly the system description and mathematical model of differential drive AGV system are described. Then, characteristics of each positioning modules are explained. On the next step, the fault detection based on multiple positioning modules is proposed. The fault detection method uses two or more positioning systems and compares them to detect unexpected deviation effected by drift or different characteristics of each positioning systems. For fault detection algorithm, an Extended Kalman Filter (EKF) is used. EKF calculates the measurement probability distribution of the AGV position for nonlinear models driven by Gaussian noise. Using the probability distribution of innovation obtained from EKF, it is possible to test if the measured data are fit with the models. When the faults such as sensors malfunction, wheel slip or motor broken, the models will not be valid and the innovation will not be Gaussian and white. The pairwise differences between the estimated positions obtained from sensors are called as residue. Fault isolation is obtained by examining the biggest residue. Finally, to demonstrate the capability of the proposed algorithm, the algorithm is implemented on a differential drive AGV system, which uses encoder, laser scanner, and laser navigation system to obtain position information. The experimental result shows that the proposed algorithm successfully detects faults when the faults occur. Keywords—fault detection; automatic guided vehicle; extended Kalman filter; multiple positioning

I. INTRODUCTION Automatic Guided Vehicles (AGVs) have been utilized for many applications such as material handling system, transport, and storage. The AGV which are working autonomously have to works nearby human and interacts with human. In addition, the AGV often have to deal with dangerous situation such as transferring volatile material. Therefore, reliability and safety are the most important parameters in AGV operation. In industrial application, fault detection algorithm reduces productivity loss and helps operator to detect faults and prevent serious damage on the system. The objective of fault

c 978-1-4799-3080-7/14/$31.00 2014 IEEE

Sang Kwun Jeong

Jin Il Jeong

detection algorithm is to develop a system which detects faults as quick as possible with minimum false alarms [1]. Various approaches of fault detection algorithm for different applications have been extensively reported in several literatures. In [2], fault detection algorithm based on local model neural networks was proposed. This algorithm used local model neural networks (LMNs) to deal with the absence of a mathematical model. However, its computation burden was high and the model that has been produced by this method depended on the training. For known mathematical model, fault detection algorithm based on unknown input PI observer was proposed by [3-4]. In this algorithm, states of system were estimated with unknown input and faults of system were detected by fault detection algorithm. This algorithm is easy to apply but only works for linear descriptor system. Real-time model based sensor fault detection for unmanned ground vehicles was proposed by [5] which used multiple sensors such as GPS, IMU and encoder. However, this method only operated on a sensor level. Another approach applied temporal causal graphs (TCGs) proposed by [6]. The TCG constructed from bond-graph model of physical process provided basic for isolation methodology. In [7-8], a particle filter was used for fault detection and isolation. Several discrete states were used for each fault mode. Particle filters were very powerful for tracking systems, and produced a probability distribution over the states, given as samples (particles). It can be used for nonlinear, non-Gaussian and multi modal distributions. However, the computational demands for this algorithm were increased according to the state dimension. To solve those problems, this paper proposes a fault detection algorithm based on multiple positioning modules. In this algorithm, to deal with a nonlinear system, Extended Kalman Filter (EKF) is applied. Differently from particle filter that needs model for each fault condition, an EKF is possible to test deviation from the normal model, and requires a model only for normal behavior. Moreover, instead of using data obtained from sensors level, this algorithm uses high level positioning modules data. Therefore, implementation of this

751

algorithm to existing systems is easy since changes in the hardware configuration are not needed.

III. MATHEMATICAL MODEL The system modeling for AGV system is shown in Fig. 3.

II. SYSTEM DESCRIPTION The system description of the AGV system is shown in Fig. 1. The AGV dimension is 60 x 100 x 190 cm. This system uses differential wheel drive system. Two driving wheels are mounted on the left and right sides of AGV, and are driven by two BLDC motors. Two passive castor wheels are installed in front and back sides of AGV to support the AGV.

Fig. 3. System modeling.

Kinematic equation of nonholonomic differential drive type of AGV system as shown in Fig. 3 can be expressed as follows:

Fig. 1. Mechanical system description.

The electrical system description is shown in Fig. 2. The laser navigation system NAV-200 for positioning sensor with accuracy ±25 mm is mounted on the top of AGV. The laser measurement system LMS-151 is attached in front of AGV. Two incremental encoders are attached on the left and right wheels to count the rotation of the wheels. Industrial PC as the main controller is placed inside the AGV platform. Touch screen monitor as the input and display is placed on back side of AGV. The batteries for power supply are placed in the middle of AGV.

ª X A º ª cos θ A − sin θ A «  » « xv = ³ x v dt ; x v = « YA » = « sin θ A cos θ A « θ » « 0 0 ¬ A¼ ¬

0 º ªVAx º 0 »» ««VAy »» (1) 1 »¼ «¬ ω A »¼

or in discrete type ªXAº ªXA º ª cos(θ A ) − sin(θ A ) 0 º ª Δx º « Y » = « Y » + « sin(θ ) cos(θ ) 0 » « Δy » A A « A» « A» « »« » «¬ θ A »¼ «¬ θ A »¼ «¬ 0 0 1 »¼ «¬ Δθ »¼ k k k −1

(2)

where xv is posture vector of AGV, ( X A , YA ) is AGV position in global coordinates, θ A is AGV orientation, VAx and VAy are linear velocity, ω A is angular velocity, r is wheel radius, Δx and Δy are linear displacements of AGV, Δθ is the change of rotational angle of AGV. Normally the value of VAy and Δy are zero because the AGV cannot move in perpendicular direction with orientation of AGV under no slipping and pure rolling condition. As a result, the second column of rotation matrix in (1) and (2) can be omitted. IV. POSITIONING MODULES

The term positioning module is used for sensors or algorithms that provide position estimation. In this paper, positioning modules consist of encoders, laser scanner LMS151, and laser navigation system NAV-200. In extended Kalman filter, process model, process noise covariance and measurement noises covariance are important parameters. Therefore, in this section, modeling of each positioning modules is presented, and its noises covariance is defined. Fig. 2. Electrical system description.

752

2014 International Conference on Advances in Computing,Communications and Informatics (ICACCI)

A. Encoder positioning module The encoder positioning for differential drive AGV is based on the accumulation of wheel rotation. Therefore, small measurement errors will cause drift after passing through the integration. The mathematical model of encoder positioning is ªXAº ªX Aº ªcos(θ A ) − sin(θ A ) 0 º ª Δx º «Y » « Y » + « sin(θ ) cos(θ ) 0 » « 0 » (3) = A A « A» « A» « »« » 0 1 ¼» ¬« Δθ ¼» k ¬« θ A ¼» k |k −1 «¬ θ A ¼» k −1 ¬« 0

ª1 ª Δx º r « = 1 « Δθ » ¬ ¼ 2 «« ¬b

1 º ª Δφr º 1»« » − » ¬ Δφl ¼ b ¼»

where the input for this model is u = ¬ªΔφr The process noise covariance is

Q k −1

ª k Δφr =« r «¬ 0

0 kl Δφl

(4)



(d)



Fig. 4. Laser scanner positioning modeling.



(e)

T

The process noise covariance noise is

Q k −1 º » »¼

(5)

B. Laser scanner positioning module For laser scanner positioning module, Simultaneous Localization and Mapping (SLAM) algorithm from [9] is applied based on location of the landmarks as shown in Fig. 4. Firstly, landmarks are detected as shown in Fig. 4(a). When the encoder data change because the AGV moves, AGV new position is predicted using EKF prediction step based on encoder data as shown in Fig. 4(b). Secondly, landmarks are detected from the AGV new position as in Fig. 4(c). The AGV then associates these landmarks to observations of landmarks that are previously observed. Re-observed landmarks are used to update the AGV position using EKF update step as shown in Fig. 4(d). The real position, encoder position and estimated position are shown in Fig. 4(e).

(b)

the input for this model is u = [ Δx Δy Δθ ]

T

The fault in encoder is caused by slip, calibration error, communication error, and damage on the sensor.



ªXAº ªX Aº ª cos(θ A ) − sin(θ A ) 0 º ª Δx º «Y » « » = « YA » + «« sin(θ A ) cos(θ A ) 0 »» «« Δy »» (6) « A» 0 1 »¼ «¬ Δθ »¼ k ¬« θ A ¼» k |k −1 ¬« θ A ¼» k −1 ¬« 0

Δφl ¼º

where r is wheel radius, b is a distance between the wheels and center of AGV, ΔφR and ΔφL are the change of right and left wheel rotation angle, kr and kl is error constants.

(a)

The position estimation will drift because of the integration, matching error, noise and numerical errors being accumulated overtime. The mathematical model of laser scanner positioning module is

(c)



ªkx « =«0 «0 ¬

0 ky 0

0º » 0» kθ »¼

(7)

where k x , k y and kθ are error constants. Equation (6) is reasonable model because SLAM error is independent from the robot direction. The error depends on the difference between current scanning result and previous scanning result. Communication error, sensor damage and singularity when the AGV moves at the long corridor or open space are several example of fault related with laser scanner.

C. NAV-200 positioning module NAV-200 positioning sensor using triangulation method to obtained the AGV position. Reflectors are positioned around the environment and their locations are given to the sensor as shown in Fig. 5. The sensor then calculates its position in the environment from the observed positions of the reflectors [10].

 Fig. 5. NAV-200 positioning modeling

Unlike another positioning module, NAV-200 is a position sensor. Therefore, the position data can be obtained directly through serial data communication without any further calculation. However, the position obtained from this sensor also has error caused by measurement noise. Therefore, the mathematical model of NAV-200 positioning model including measurement noise is as follows:

2014 International Conference on Advances in Computing,Communications and Informatics (ICACCI)

753

ªXAº ª X A º ª ex º «Y » « Y » + «e » = « A» « A » « y» «¬ θ A »¼ k |k −1 «¬ θ A »¼ k «¬eθ »¼ k

(8)

The fault related with NAV-200 is caused by communication error, insufficient number of reflectors, and singularity.

h(⋅) is observation nonlinear vector function, and z k is output vector. In this paper, the state equation f (⋅) can be obtained from (2) for reference speed, (3) for encoder positioning module, and (6) for laser scanner positioning module. For the measurement model, z k is use following equation:

z k = xk + v k

V. FAULT DETECTION ALGORITHM The schematic diagram of fault detection algorithm is shown in Fig. 6. Extended Kalman Filter (EKF) calculates the measurement probability distribution of the AGV position for nonlinear models driven by Gaussian noise. Using the probability distribution of innovation obtained from EKF, it is possible to test if the measured data are fit with the models. When the faults occur, the models will not be valid and the innovation will not be Gaussian and white. The residue calculation estimates the residue from pairwise differences between the estimated positions obtained from positioning modules. Fault isolation is obtained by examining the biggest residue. Finally, from the Table I as shown in the next section, the fault position is known.

(10)

EKF consist of two steps as follows: • Prediction step: xˆ k |k −1 = f (xˆ k −1|k −1 , u k −1 ) Pk |k −1 = Fk −1Pk −1|k −1FkT−1 + Wk Q k −1WkT

(11)

where F = ∂f and W = ∂f k −1 k ∂u xˆ k −1|k −1 ,uk −1 ∂x xˆ k −1|k −1 ,uk −1 where xˆ k |k −1 is predicted state estimation, Pk |k −1 is predicted covariance matrix estimation. •

Update step:

y k = z k − h(xˆ k |k −1 ) S k = H k Pk |k −1HTk + R k K k = Pk |k −1HTk ( S k )

−1

(12)

xˆ k |k = xˆ k |k −1 + K k y k Pk |k = (I − K k H k )Pk |k −1 where H = ∂h k ∂x xk|k −1

Fig. 6. Schematic diagram of fault detection algorithm

A. Extended Kalman Filter Firstly, pairwise of positioning modules, as shown in Table I, are tested using Kalman filter. Kalman filter theory provides a method to calculate measurement probability for linear model driven by Gaussian noise. Since the proposed system is nonlinear, an Extended Kalman Filter (EKF) is used. In the EKF, the state transition and observation models is not a linear functions of the state but may be differentiable functions as follows:

x k = f (x k −1 , u k −1 ) + w k −1 z k = h( x k ) + v k

B. Residue Calculation From EKF, measurement innovation y k and innovation covariance S k can be obtained. To check the residue measurement, Mahalanobis distance is used. The residue is calculated as follows:

(9)

where x k is state vector, w k −1 is process noise and v k is observation noises which are assumed to be zero mean multivariate Gaussian noises with covariance Q k and R k respectively, f (⋅) is process nonlinear vector function,

754

where y k is measurement innovation, z k is output vector from estimation, h(xˆ k |k −1 ) measurement result, S k is innovation covariance, R k is measurement noise covariance, K k is Kalman gain, xˆ k |k is updated state estimation, and Pk |k is updated covariance matrix estimation.

st = y kT S k−1 y k where st is residue.

2014 International Conference on Advances in Computing,Communications and Informatics (ICACCI)

(13)

If the value of st is larger than threshold value Th , one of the pairwise sensors is fault as follows: ­ normal 0 if st < Th Fault condition ® X if st > Th ¯ fault

(14)

The threshold value is related with the sensitivity of the system to the fault. C. Fault Isolation Finally, to isolate the fault, 4 test conditions has been setup as shown in Table I. The residue that is more than threshold value is examined which is then matched at the table relation between residues and faults. Several fault condition are considered as follows:

VI. EXPERIMENTAL RESULT To verify the effectiveness of the proposed fault detection algorithm, several experiments has been conducted. During the experiments, the robot follows the given trajectory as shown at Fig. 7. The experimental environment is corridor surrounded by wall and windows. The algorithm was run in realtime on industrial computer inside the AGV. In the experiment, the sensor outputs are observed at 10Hz. The fault data and any other information are saved inside industrial computer during experiment and can be copied after experiment finished.

• In normal condition, all positioning modules provide correct position. Therefore, all tests are normal since all residues are less than threshold value. • When the encoder is broken, all tests related with encoder such as test T2 and test T3 have the fault results since the positioning from encoder is false and the resulting residue from both tests T2 and T3 are greater than threshold value. • When the BLDC motor is broken, all tests related with reference speed such as test T1 and test T3 have the fault results since the positioning from reference speed is false and the resulting residue from both tests T1 and T3 are greater than threshold value. • When laser scanner is broken, all tests related with laser scanner such as test T1 and test T2 have the fault result since positioning from laser scanner is false. The resulting residue from both tests T1 and T3 are greater than threshold value. • When NAV-200 is broken, the NAV would not provide the correct position. Therefore, the residue from test T4 is greater than the threshold value. TABLE I.

Fig. 7. Trajectory for experiment

The proposed algorithm is tested during several conditions T1, T2, T3, and T4. During the experiment, the trajectories from each test such as reference speed + laser scanner (LMS), Encoder + laser scanner (LMS), Encoder + reference velocity, and reference velocity + NAV 200 can be observed. The residue of each test also can be otained and it is compared with the threshold value Th . On this experiment the threshold value Th is chosen as 500. A. Normal condition The experimental results for normal condition are shown in Fig. 8 and Fig. 9. Fig. 8 shows trajectory tracking in several conditions T1, T2, T3, and T4 obtained from each positioning module fused by EKF are almost similar since fault condition doesn’t exist. Therefore, the residues values shown in Fig. 9 are less than threshold value Th .

RELATION BETWEEN RESIDUES AND FAULTS

2000

Fault F2 F3 F4 BLDC Laser Normal Encoder NAV motor scanner

T1

Reference speed + laser scanner (LMS)

0

0

X

X

0

T2

Encoder + scanner (LMS)

0

X

0

X

0

0

X

X

0

0

0

0

0

0

X

T3 T4

Reference encoder Reference NAV

1500

F1

laser

speed

+

speed

+

1000 500 Y axis (mm)

N

Test

Ref Ref+LMS Encoder+LMS Ref+Encoder Ref+NAV

0 -500 -1000

where 0 means normal, and X means fault.

-1500

-2000 -2000

-1500

-1000

-500

0 500 X axis (mm)

1000

1500

2000

Fig. 8. Trajectory tracking in normal condition

2014 International Conference on Advances in Computing,Communications and Informatics (ICACCI)

755

1000

2000

Th Ref+LMS Encoder+LMS Ref+Encoder Ref+NAV

800

600

1500

St

1000 400

Y axis (mm)

500 200

0

0

5

10

15

20 Time (s)

25

30

35

40

Ref Ref+LMS Encoder+LMS Ref+Encoder Ref+NAV

0 -500

Fig. 9. Residue in normal condition

-1000

B. Encoder fault The experimental results for encoder fault are shown in Fig. 10 and Fig. 11. To resemble the encoder fault condition, at t = 20 s, the controller stops receiving the data from encoders. Therefore, the encoder positioning modules don’t provide the positioning data anymore. Fig. 10 shows the trajectory tracking when the encoder fault happens. In Fig. 11, the residue values of encoder + laser scanner (LMS) and reference speed + encoder are increased over the threshold value at t = 21.6 s. From the Table I, it is easily can be observe that the encoder fault happens.

-1500

-2000 -2000

-1500

-1000

-500

0 500 X axis (mm)

1000

1500

2000

Fig. 12. Trajectory tracking when BLDC motor fault

Fig. 13 shows that since the motor stops at t = 25 s, the residue values of Reference speed + laser scanner (LMS) and Reference speed + encoder are increased over the threshold value at t = 26.2 s. From the Table I, it is easily can be observed that the BLDC motor fault happens. 1000

800

1500

600

Th Ref+LMS Encoder+LMS Ref+Encoder Ref+NAV

St

2000

1000

400

Y axis (mm)

500

200

Ref Ref+LMS Encoder+LMS Ref+Encoder Ref+NAV

0 -500

0

0

5

10

15 Time (s)

20

25

30

Fig. 13. Residue when BLDC motor fault

-1000

D. Laser scanner fault The experimental results for laser scanner (LMS) fault are shown in Fig. 14 and Fig. 15. The fault condition can be obtained by pulling of the power connection of the sensor when the AGV moves at t = 30 s.

-1500

-2000 -2000

-1500

-1000

-500

0 500 X axis (mm)

1000

1500

2000

Fig. 10. Trajectory tracking when encoder fault 1000

2000 Th Ref+LMS Encoder+LMS Ref+Encoder Ref+NAV

800

1500 1000

St

600

400

Y axis (mm)

500

200

0

0

5

10

15

20

25

Ref Ref+LMS Encoder+LMS Ref+Encoder Ref+NAV

0 -500

Time (s)

Fig. 11. Residue when encoder fault

-1000 -1500

C. BLDC motor fault The experimental results for BLDC motor fault are shown in Fig. 12 and Fig. 13. The fault condition can be obtained by turning off the motors when the AGV moves.

756

-2000 -2000

-1500

-1000

-500

0 500 X axis (mm)

1000

1500

Fig. 14. Trajectory tracking when laser scanner fault

2014 International Conference on Advances in Computing,Communications and Informatics (ICACCI)

2000

VII. CONCLUSION

1000 Th Ref+LMS Encoder+LMS Ref+Encoder Ref+NAV

800

St

600

400

200

0

0

5

10

15

20

25

30

35

Time (s)

This paper proposed implementation and experimental validation of fault detection algorithm for Automatic Guided Vehicle (AGV) system based on multiple positioning modules. Experimental results in normal condition and fault condition showed that the proposed algorithm successfully detected the fault condition. The algorithm could calculate the residue of each pairs positioning modules based on extended Kalman filter. Based on those residue values, the fault condition was detected.

Fig. 15. Residue when laser scanner fault

Since the laser scanner doesn’t provide any data, the SLAM positioning cannot provide new position data. Therefore, as shown in Fig. 15, the residue values of Reference speed + laser scanner (LMS) and Encoder + laser scanner (LMS) are over the threshold value at t = 31.2 s. E. NAV fault The experimental results for NAV fault are shown in Fig. 16 and Fig. 17. The fault condition can be obtained by pulling of the power connection of the sensor when the AGV moves at t = 35 s. Since the NAV sensor doesn’t provide any data, as shown in Fig. 17, the residue value of reference + NAV is increased and over the threshold value at t = 36.1 s. 2000 1500 1000

Y axis (mm)

500

Ref Ref+LMS Encoder+LMS Ref+Encoder Ref+NAV

0 -500 -1000 -1500

-2000 -2000

-1500

-1000

-500

0 500 X axis (mm)

1000

1500

2000

Fig. 16. Trajectory tracking when NAV-200 fault 1000 Th Ref+LMS Encoder+LMS Ref+Encoder Ref+NAV

800

St

600

400

ACKNOWLEDGMENT This work (Grants No.C0149697) was supported by Business for Cooperative R&D between Industry, Academy, and Research Institute funded Korea Small and Medium Business Administration in 2014, and this research was supported by a grant (11 Transportation System- Logistics 02) from Transportation System Efficiency Program funded by Ministry of Land, Infrastructure and Transport (MOLIT) of Korean government. REFERENCES [1]

F. Baghernezhad and K. Khorasani,”A robust fault detection scheme with an application to mobile robots by using adaptive thresholds generated with locally linear models,” IEEE CICA, 2013, pp. 9-16. [2] E.N. Skundrianos and S.G. Tzafestas, “Finding fault - Fault diagnosis on the wheels of a mobile robot using local model neural networks,” IEEE robotics and automation magazine, September 2004, pp. 83-90. [3] H. Sadaka, B. Shafai, and R. Sipahi, “Design of PI observer for disturbance attenuation and fault detection in linear time-delay systems,” Proceeding of Asian Control Conference, August 2009, pp. 63-68. [4] M.M.S. Pasand and H.D. Tahirad, “Descriptor approach to unknown input PI observer design: Application to fault detection,” International conference on control automation and systems, 2010, pp.600-604. [5] A. Monteriu, P. Asthana, K. Valavanis, and S. Longhi, “Experimental validation of a real-time model-based sensor fault detection and isolation system for unmanned ground vehicles,” IEEE Mediterranean Conference on Control and Automation, June 2006, pp. 1-8. [6] M. Ji, Z. Zhang, and G. Biswas, “Hybrid fault adaptive control of a wheeled mobile robot,” IEEE Trans. on Mechatronics, vol.8, 2003, pp. 226-233. [7] Z.X. Cai, Z.H. Duan, J.F. Cai, X.B. Zou, and J.X. Yu, “A multiple particle filters method for fault diagnosis of mobile robot deadreckoning system,” IEEE IROS 2005, pp. 481-486. [8] Z.H. Duan, Z.X. Cai, J.X. Yu,” Adaptive particle filter for unknown fault detection of wheeled mobile robots,” IEEE International conference on Intelligent Robot and System 2006, pp. 1312-1315. [9] P.S. Pratama, T.L. Bui, J.G. Kim, S.B. Kim, “Trajectory tracking of automatic guided vehicle based on simultaneous localization and mapping method,” KLST, 2013. [10] H.H. Lin, and C.C. Tsai, “Laser pose estimation and tracking using fuzzy extended information filtering for an autonomous mobile robot”, Journal of Intelligent Robotic system, 2008, vol. 53 (2), pp. 119-143.

200

0

0

5

10

15

20 Time (s)

25

30

35

40

Fig. 17. Residue when NAV-200 fault

2014 International Conference on Advances in Computing,Communications and Informatics (ICACCI)

757