klst 2013 trajectory tracking of automatic guided vehicle based on simultaneous localization and...
DESCRIPTION
This paper proposes trajectory tracking algorithm for differential drive type of Automatic Guided Vehicle (AGV) using backstepping control and simultaneous localization and mapping (SLAM). To guarantee the tracking errors go to zero, backstepping control method is proposed. By choosing appropriate Lyapunov function based on its kinematic modeling, system stability is guaranteed and a control law can be obtained. For its positioning, simultaneous localization and mapping (SLAM) algorithm is employed. The landmarks are detected using spike algorithm. The AGV position can be estimated using Kalman filter by combining the encoder result and landmark positions. The simulation and experimental results show that the proposed controller successfully tracks the given trajectory.TRANSCRIPT
-
- -
| | 2013. 10. 17()~18() | |
: , ()
: ()
- / : , ,
- / : , ,
- / : , ,
-
01
02
02
02
03
A 05
B 06
C 07
D 08
_A 09
_B 10
_C 11
_D 12
Contents
-
. , .
4 , .
2013 .
.
( ) ( ) ( )
2013 10
-
2013
- -
1.
2.
: 2013. 10. 17() ~ 18() : : ,
: , :
: + + 5
2
( )
/
/
/
3. : 100,000( 50,000)
-
4.
3
1017()
09:30 ~ 10:00
10:00 ~ 12:30
1) (20)
: ( ) :
: ( )
:
:
2) (30) : R&D
3) (90)
: :
:
:
( )
(() )
( )
( )
()
12:30 ~13:30 ( )
-
2013 4
2
5 314
2 203
1 209
13:45 ~ 14:45Session A-1 :
Session B-1 :
Session C-1 :
Session D-1 :
14:45 ~ 15:00 Coffee Break
15:00 ~ 16:00Session A-2 :
Session B-2 :
Session C-2 :
Session D-2 :
16:00 ~ 16:15 Coffee Break
16:15 ~ 17:15Session A-3 :
Session B-3 :
Session C-3 :
Session D-3 :
17:30 ~ 18:00
18:15 ~
()
1018()
09:30 ~ 10:30
10:30 ~ 11:00
11:00 ~ 12:00 ICD
5.
-
5
Session A-1 ()
13:45~14:00
14:00~14:15 Ex-PAS CJ
14:15~14:30 ()
14:30~14:45
14:45~15:00 , ()
15:00~15:15 Coffee Break
Session A-2 (CJ)
15:15~15:30
14:00~14:15 DC
15:45~16:00
16:00~16:15
16:15~16:30 Coffee Break
Session A-3 ()
16:30~16:45
16:45~17:00
17:00~17:15 RAM
17:15~17:30 PRT
Session-A (2 )
-
2013 6
Session B-1 / 1 ()
13:45~14:00
14:00~14:15Path Following Control of Automated Guide Vehicle Using Camera Sensor
14:15~14:30Tracking Controller of Automatic Guided Vehicles Based on Laser Sensor
14:30~14:45
14:45~15:00 Coffee Break
Session B-2 / 2 ()
15:00~15:15Obstacle Avoidance Control of Omni-directional Robot Using Potential Function Method
15:15~15:30
15:30~15:45Trajectory Tracking of Automatic Guided Vehicle Based on Simultaneous Localization and Mapping Method
15:45~16:00
16:00~16:15Modeling and Control System Design of Four Wheel Independent Steering Automatic Guided Vehicle Based on Optimal Control Method
16:15~16:30 Coffee Break
Session B-3 / ()
16:30~16:45
16:45~17:00 ( ) ()
17:00~17:15 ~
17:15~17:30 (Piggy Back)
Session-B (5 314)
-
7
Session C-1 1 ()
13:45~14:00 (HEMU-430X)
14:00~14:15 UIC 513R (HEMU-430X)
14:15~14:30
14:30~14:45 /
14:45~15:00 Coffee Break
Session C-2 2 ()
15:00~15:15
15:15~15:30
15:30~15:45 -
15:45~16:00 :
16:00~16:15 Coffee Break
Session C-3 3 ()
16:15~16:30 (HEMU-430X)
16:30~16:45
16:45~17:00
17:00~17:15
Session-C (2 203)
-
2013 8
Session D-1 1 ()
13:45~14:00
14:00~14:15
14:15~14:30
14:30~14:45 RTI
14:45~15:00 Coffee Break
Session D-2 2 ()
15:00~15:15
15:15~15:30 R&D
15:30~15:45
15:45~16:00
16:00~16:15 Coffee Break
Session D-3 3 ()
16:15~16:30The Effects of Physical Distribution Infrastructure Construction on Modal Shift Cost Minimization : The Moderating Role of R&D Policies
16:30~16:45 /
16:45~17:00 -
17:00~17:15
Session-D (1 209)
-
9
Session-A
A1-1KLST-2013-
AC-042
A1-2KLST-2013-
AC-010Ex-PAS
CJ [email protected] IT
A1-3KLST-2013-
AC-004 () [email protected]
A1-4KLST-2013-
AC-022
A1-5KLST-2013-
AC-045
()
A2-1KLST-2013-
AC-016
A2-2KLST-2013-
AC-031DC
A2-3KLST-2013-
AC-032
A2-4KLST-2013-
AC-047
A3-1KLST-2013-
AC-059
A3-2KLST-2013-
AC-054
A3-3KLST-2013-
AC-055 RAM
A3-4KLST-2013-
AC-049PRT
-
2013 10
Session-B
B1-1KLST-2013-
AC-017
B1-2KLST-2013-
AC-005
Path Following Control of Automated Guide Vehicle Using
Camera Sensor [email protected]
/
B1-3KLST-2013-
AC-007
Tracking Controller of Automatic Guided Vehicles Based on Laser
Sensor [email protected]
/
B1-4KLST-2013-
AC-041
/
B2-1KLST-2013-
AC-006
Obstacle Avoidance Control of Omni-directional Robot Using
Potential Function Method [email protected]
/
B2-2 KLST-2013-AC-043
/
B2-3 KLST-2013-AC-057
Trajectory Tracking of Automatic Guided Vehicle Based on
Simultaneous Localization and Mapping Method
/
B2-4 KLST-2013-AC-029
B2-5 KLST-2013-AC-058
Modeling and Control System Design of Four Wheel
Independent Steering Automatic Guided Vehicle Based on Optimal
Control Method
/
B3-1 KLST-2013-AC-039
B3-2 KLST-2013-AC-044
( )
B3-3 KLST-2013-AC-014
~
B3-4 KLST-2013-AC-013
(Piggy Back)
-
11
Session-C
C1-1KLST-2013-
AC-018(HEMU-430X)
C1-2KLST-2013-
AC-021
UIC 513R (HEMU-430X)
C1-3KLST-2013-
AC-036
C1-4KLST-2013-
AC-028/
[email protected] Cold Chain
C2-1KLST-2013-
AC-053
C2-2KLST-2013-
AC-026
C2-3KLST-2013-
AC-033- [email protected]
C2-4KLST-2013-
AC-034
:
C3-1KLST-2013-
AC-023
(HEMU-430X)
C3-2KLST-2013-
AC-019
C3-3KLST-2013-
AC-025
C3-4KLST-2013-
AC-027 [email protected]
-
2013 12
Session-D
D1-1KLST-2013-
AC-046
D1-2KLST-2013-
AC-002
D1-3KLST-2013-
AC-030
D1-4KLST-2013-
AC-051 RTI
D2-1KLST-2013-
AC-035
D2-2KLST-2013-
AC-056 R&D
D2-3KLST-2013-
AC-037
D2-4KLST-2013-
AC-040
D3-1KLST-2013-
AC-011
The Effects of Physical Distribution Infrastructure
Construction on Modal Shift Cost Minimization : The Moderating
Role of R&D Policies
D3-2KLST-2013-
AC-024/
D3-3KLST-2013-
AC-038 -
D3-4KLST-2013-
AC-001
-
2013 KLST-2013-AC-057
SLAM Trajectory Tracking of Automatic Guided Vehicle Based on
Simultaneous Localization and Mapping Method
Pandu Sandi Pratama*, Thanh Luan Bui*, *, *, **
Pandu Sandi Pratama*, Thanh Luan Bui
*, Jeong-Geun Kim
*, Hak-Kyeong Kim
*, Sang-Bong Kim
*
Abstract
This paper proposes trajectory tracking algorithm for differential drive type of Automatic Guided
Vehicle (AGV) using backstepping control and simultaneous localization and mapping (SLAM). To
guarantee the tracking errors go to zero, backstepping control method is proposed. By choosing
appropriate Lyapunov function based on its kinematic modeling, system stability is guaranteed and a
control law can be obtained. For its positioning, simultaneous localization and mapping (SLAM)
algorithm is employed. The landmarks are detected using spike algorithm. The AGV position can be
estimated using Kalman filter by combining the encoder result and landmark positions. The simulation
and experimental results show that the proposed controller successfully tracks the given trajectory.
Keyword: Trajectory Tracking, Differential Drive, Automatic Guided Vehicle, Backstepping, SLAM
1. Introduction
Trajectory tracking is the most important issue to the AGV when it operates in industrial environment.
There are several control algorithms that have been proposed to accomplish this task. Particle swarm
optimization (PSO) method to determining the optimal PID controller [1] and fuzzy-PID [2] is proposed to
control the navigation of AGV. Those controllers are easy to be applied to AGV system but not robust.
Parameter-based controller design method has been proposed using sliding mode control theory [3] and
linear control based on Lyapunov function [4]. In those controllers, the stability of the system is guaranteed,
but it is not an easy task to find appropriate controller law.
In the past, there were several kind navigation methods for tracking control algorithm of AGV. P. T. Doan
et al. [2] proposed AGV path tracking control algorithm to follow a visual line painted on the floor using
camera sensor. This algorithm is easy and cheap, but the path should be keep clean. Inductive guidance using
electrical wire buried under the floor was proposed [5]. This navigation system isnt flexible since the path cant be changed easily. Semi-guided navigation method by using magnetic tapes was proposed [6], but not suitable for navigation on steel floors. Wall following algorithm [7] was proposed, but its difficult to apply
the algorithm in open space. The most advanced positioning system for indoor application was laser
navigation system [4]. It is flexible, but very expensive.
To solve the tracking control problem, this paper proposes trajectory tracking algorithm for Automatic
Guided Vehicle (AGV) using backstepping control and simultaneous localization and mapping (SLAM). This
: ([email protected]) * **
-
paper focuses on differential drive type AGV. To guarantee the tracking errors go to zero, backstepping
control method is proposed. By choosing appropriate Lyapunov function based on its kinematic modeling,
system stability is guaranteed and a control law can be obtained. Furthermore, to solve navigation problem,
simultaneous localization and mapping (SLAM) algorithm is proposed. The landmarks are detected using
spike algorithm. The AGV position can be estimated using Kalman filter by combining the encoder result
and landmark positions. The simulation and experimental results show that the proposed controller
successfully tracks the given trajectory.
2. System Description and Modeling
The AGV shown in Fig. 1(a) has dimension 100 cm x 60 cm x 80 cm. This system uses differential drive
wheeled 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.
The electrical design shown in Fig. 1(b) consists of sensors such as encoder and laser measurement
system, controller such as Industrial PC, touch screen monitor as display, keyboard and mouse as the input,
and actuators.
(a) Mechanical Design (b) Electrical Design (c) Configuration for system modeling
Fig. 1 Configuration of AGV system
Fig. 1(c) shows configuration for the system modeling of the differential drive system. Kinematic
equation of nonholonomic differential drive type of AGV system as shown in Fig. 1(c) can be expressed as
follows:
v vdt x x ;
cos 0
sin 0
0 1
A A
A
v A A
A
A
XV
Y
x ;
1 1
1 12
A R
A L
V r
b b
(1)
or in discrete type
1vk vk v x x x ;
cos( ) 0
sin( ) 0
0 1
A A
v A A
A
Xs
Y
x ;
1 1
1 12
r
l
s r
b b
(2)
where vx is posture vector of AGV, ( , )A AX Y is AGV position in global coordinates, A is AGV
orientation that is taken counterclockwise from the X axis, AV is linear velocity and A is angular
-
velocity, r is wheel radius, b is distance between the wheels and center of AGV, R and L are right
and left wheel angular velocities, s is linear displacement of AGV , is the change of rotational angle
of AGV, R and L are the change of right and left wheel rotation angle.
3. Controller Design
The purpose of this section is to design an trajectory tracking controller for AGV to track the reference
position ( ( ), ( ))r rx t y t and reference orientation ( )r t with reference linear velocity ( )rV t and angular
velocity ( )r t . As shown in Fig. 1(c), the tracking error vector and its time derivative are defined as:
1
2
3
cos sin 0
( ) sin cos 0
0 0 1
A A r A
A A r A
r A
e x X
t e y Y
e
e and
1 3 2
2 3 1
3
cos 0 1
( ) sin 0 0
0 1 0 1
r A
r A
e e eV V
t e e e
e
e (3)
To guarantee the stability of the system, Lyapunov function is chosen as:
2 20 1 2 3 22
1 1 1(1 cos ) for 0
2 2V e e e k
k (4)
and its derivatives becomes
0 1 1 2 2 3 3 1 3 3 2 22 2
1 1(sin ) ( cos ) (sin )( )A r r A rV e e e e e e e V V e e k e V
k k (5)
To achieve 0 0V , the control law U is chosen as follows:
3 1 1
2 2 3 3
cos
sin
rA
r rA
V e k eV
k V e k e
U (6)
4. Simultaneous Localization and Mapping (SLAM)
The AGV position can be obtained using SLAM method using Extended Kalman filter (EKF). The EKF
algorithm consists of prediction and update. Firstly, the landmarks are detected as shown in Fig. 2(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. 2(b). Secondly, Landmarks are then extracted from the environment
from the AGV new position as in Fig. 2(c). The AGV then attempts to associate these new landmarks to
observations of landmarks that it previously has seen. Re-observed landmarks are then used to update the
AGV position using EKF update step as shown in Fig. 2(d). Landmarks which have not previously been seen
are added to the EKF as new observations. The real position, encoder position and estimated position are
shown in Fig. 2(e).
(a) (b) (c) (d) (e)
Fig. 2 SLAM algorithm
-
4.1. Prediction
The estimation position obtained from prediction step | 1 k kx and covariance matrix obtained from
prediction step | 1k kP at current sampling time k based on encoder data is:
Prediction:
(3) (3) (2 )| 1 1| 1 , 1 1
(3 2 )| 1 1| 1 1 ,
( , ) for [ ]
for
T nk k k k x v k k x
T T n Tk k k k k k k k k k x x k x
f
x x F x u F O
P A P A W Q W A I F A F (7)
where 1T
v n x x y y is state vector that consists of AGV posture vector T
v A A AX Y x and
landmark positions vector T
i i ix y y . | 1k k is probability of data at sampling time k given data at
sampling time 1k . In prediction step, xF is used to make sure that only AGV position is updated.
This prediction is based on the input u that consists of changes of right encoder r and left encoder
l respectively. AGV mathematic modeling in discrete type in Eq. (2) is reduced as follows:
1
cos( ) 0
( , ) sin( ) 0
0 1
A
v vk vk v A
sf
x u x x x ;
1 1
1 12
r
l
s r
b b
; r l u (8)
where b is distance between left and right wheels, s is linear displacement of AGV , and is
rotational angle of AGV.
The covariance matrix | 1k kP and Jacobian of prediction model kA are defined as:
1
1 1 1 1
1 1
| 1
n
n
n n n
xx xy xy
y x y y y y
k k
y x y y y y
P P P
P P P
P P P
P ; (3 2 )
,n T
k x x k x A I F A F for ,
0 0 sin( / 2)
0 0 cos( / 2)
0 0 0
A
x k Av
sf
s
Ax
(9)
Process noise ,u kW and process noise covariance 1kQ with error constants rk and lk are defined
as:
,
1 1cos( ) sin( ) cos( ) sin( )
2 2 2 2 2 2 2 2
1 1sin( ) cos( ) sin( ) cos( )
2 2 2 2 2 2 2 2
1 1
A A A A
u k A A A A
s s
b b
f s s
b b
b b
Wu
; 10
0
r rk
l l
k
k
Q (10)
4.2. Update
The estimation position obtained from update step | k kx and covariance matrix obtained from update step
|k kP at sampling time k based on landmarks data are:
Update:
1| | 1 | 1
| | 1
for
( )
Tk k k k i i i k k i i
k k i i k k
x x K v K P H S
P I K H P (11)
where iK is Kalman gain, iv is innovation as difference between landmark positions obtained from
-
measurement and prediction. |k k is probability of data at sampling time k given data at sampling time
k . i is the number of landmark. The innovation iv is calculated as follows:
, | 1 , | 1( , )i v k k i k kh v z x y for
2 2
, | 1 , | 1 1
( ) ( )
( , )tan ( )
i v i v
v k k i k k i vv
i v
x x y y
h y y
x x
x y (12)
where range is distance between AGV and landmarks current positions, and bearing is angle
between AGV and landmark current position. z is vector of measurement value of landmarks.
Innovation covariance iS is defined as:
| 1T
i i k k i S H P H R (13)
The covariance noise of sensor is 0
0
R with and that is measurement accuracy
of the sensor.
The landmarks are detected using spike algorithm as shown in Fig. 3. They are identified by finding
values of a laser scan where two values difference by more than a certain amount. This will find big changes
in the laser scan from. The detected landmarks are then associated with the previous known landmarks based
on Mahalanobis distance ( i ) that 2 1Ti i i i
v S v . The detected landmark belongs to the previous known
landmark if 2i that is threshold value [8].
Fig. 3 Landmark detection
Jacobian of measurement model kH is defined as:
x
k x yy
FH H H
F for
2 2
0
1
i v i v
xi v i vv
x x y y
h
y y x x
Hx
; and
2 2
i v i v
yi v i vi
x x y y
h
y y x x
Hy
(14)
2 2( ) ( )i v i vx x y y and (2) (3) (2) (2( 1)) (2) (2) (2) (2( 1))i n i
y
F I O I O
where xH is Jacobian related with AGV position and yH is Jacobian related with landmark position.
If new landmarks are detected, the new landmarks are included in the state vector | k kx as follows:
| 1
|, | 1
( , )
k k
k kv k ky
xx
x z for
cos( )( , )
sin( )
v Av
v A
xy
y
x z (15)
-
Then the covariance matrix |k kP can be obtained as follows:
| 1 ( ~ )
|
( ~ )
n
n
T Tk k x x y x
k k T Tx x x y x xx x z z
P P YP
Y P Y P Y Y RY for
1( ~ )n nx x y xx xy xy
P P P P (16)
where
1 0 sin( )
0 1 cos( )
Ax
Av
y
Y
x;
cos( ) sin( )
sin( ) cos( )
A Az
A A
y
Y
z
5. Simulation and Experimental Result
To verify the effectiveness of the proposed controller, simulation and experiment have been done. The
numerical values and initial values for simulation and experiment are shown in Table 1.
Table 1 Parameter and initial values
Parameter Values Parameter Values Parameter Values Parameter Values
r 0.09 m b 0.3 m (0)AV 0 m/s rk
0.01
1k 0.7 (0)AX 2 m
(0)A 0 rad/s lk 0.01
2k 6 (0)AY 0 m 0
x [2 0 0]
T
0.002
3k 0.5 (0)A 0 rad 0P
(3 3)O
0.001
The simulation and experimental results are shown in Fig. 4-7. Fig. 4 shows the simulation process of
trajectory tracking and SLAM. The LMS measurement result is shown as 180 area in front of AGV. The
positions of landmarks are estimated using EKF based on LMS measurement result. The circular area around
the landmark is the probability position of the landmark. Based on the positioning obtained from SLAM, the
AGV successfully tracks the given trajectory.
Fig. 4 Simulation process
-
Fig. 5 shows the experimental process using real AGV system. The trajectory and landmark position for
experiment is similar with that is used in simulation. The landmarks are metal rods with diameter 5 cm
placed within certain distance. The AGV then follows the given trajectory. This experimental result shows
that the proposed algorithm is successfully applied to real system.
Fig. 5 Experimental process
Fig. 6 shows the trajectory tracking result of AGV obtained from simulation and experiment. The
simulation result is similar with the experimental result. It is shown that the proposed controller successfully
makes AGV track the reference trajectory in simulation and experiment.
Fig. 6 Trajectory tracking of AGV in simulation and experiment
Fig. 7 shows the tracking errors obtained from simulation and experiment. It is shows that the position
errors are converged to zero. The position error 1e from simulation converges to zero after 12s and bounded
around 2 mm. On the other hand, position error 1e from experiment converges to zero after 15s and
bounded around 10 mm. Position error 2e from simulation converges to zero after 12s and bounded
around 2 mm. From experiment, position error 2e converges to zero after 20s and bounded around 20
mm. Orientation error 3e from simulation converges to zero after 15s and bounded around 0.08 rad.
Orientation error 3e from experiment converges to zero after 20s and bounded around 0.15 rad. The
simulation and experimental results show that the proposed controller successfully tracks reference trajectory
with acceptable small error.
-
(a) (b) (c)
Fig. 7 Position error from simulation and experiment
6. Conclusion
This paper proposed backstepping control method for AGV to tracks the reference trajectory based on
kinematic modeling of differential drive system, and simultaneous localization and mapping (SLAM) for
positioning. The controlled system is stable in sense of Lyapunov stability. The simulation and experimental
results show that the proposed controller successfully tracks reference trajectory with acceptable small error.
Acknowledgments
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.
Reference
[1]. T.Y. Abdalla, A.A. Abdulkarem (2012) PSO-based optimum design of PID controller for mobile robot
trajectory tracking, International Journal of Computer Applications, 47 (23), pp. 30-35.
[2]. P.T. Doan, T.T. Nguyen, V.T. Dinh, H.K. Kim, et al (2011) Path tracking control of automatic guided vehicle
using camera sensor, Proceeding of The 1st International Symposium on Automotive and Convergence
Engineering, Ho Chi Minh, Vietnam, pp. 20-26.
[3]. A. Filipescu (2011) Trajectory-tracking and discrete-time sliding-mode control of wheeled mobile robots, IEEE
International Conference on Information and Automation, Shenzhen, China, pp. 27-32.
[4]. T.L. Bui, P.T. Doan, S.S. Park, H.K. Kim, S.B. Kim (2013) AGV trajectory control based on laser sensor
navigation, International Journal of science and Engineering, 4(1), pp. 16-20.
[5]. C. Chen, et al (2004) Application of automated guided vehicle (AGV) based on inductive guidance for
newsprint rolls transportation system, Journal of Donghua University, 21 (2), pp. 88-92.
[6]. S.Y. Lee, H.W. Yang (2012) Navigation of automated guided vehicles using magnet spot guidance method,
Journal of Robotics and Computer-Integrated Manufacturing, 28 (2), pp. 425-436.
[7]. S.C. Yuan (2009) Robust type-2 fuzzy control of an automatic guided vehicle for wall-following, Proceedings
of International Conference of Soft Computing and Pattern Recognition, Malacca, Malaysia, pp. 172-177.
[8]. N. M. Kwok, Q. P. Ha, G. Fang (2007) Data association in bearing-only SLAM using a cost function-based
approach, IEEE International Conference on Robotics and Automation, Roma, Italy, pp. 4108-4113.
-
2013
: Contract Logistics, , , , , SCM Consulting : 02-3782-0114 : www.cjkoreaexpress.co.kr
: 3, , , , , , , : 02-728-5114 : www.hanjin.co.kr
: , , : 02-3669-7300 : kcp.logisall.com
: 3, , , , : 032-886-3003 : www.sytpl.com
: , , : 031-776-0500 : www.bowoosystem.com
: , : 031-511-0855 : www.chunmalogistics.com
: , , , , : www.mhconstruction.co.kr
: , , , : 031-631-9400 : www.dpic.co.kr
: , ITS, LBS, : 02-553-2525 : www.wavem.net
: : 02-3281-9001 : www.citus.co.kr
: : 051-313-4555 : www.hansungwt.com
MH
-
: 437-757 176 Tel : 031-460-5397, 5873 E-mail : [email protected] : http://www.klst.or.k
()