Odometer Module for Mobile Robot with Position Error Estimation

14th IFAC Conference on Programmable Devices and Embedded 14th Systems 14th IFAC IFAC Conference Conference on on Programmable Programmable Devices Devices and and Embedded Embedded 14th IFAC Conference on Programmable Devices and Embedded Systems October 5-7, 2016. Brno, Czech Republic Available online at www.sciencedirect.com Systems Systems October 5-7, 2016. Brno, Czech Republic October October 5-7, 5-7, 2016. 2016. Brno, Brno, Czech Czech Republic Republic

ScienceDirect

IFAC-PapersOnLine 49-25 (2016) 346–351

Odometer Module for Odometer Module for Odometer Module for Odometer Module for Position Error Position Error Position Error Position∗ Error Daniel Daniel Daniel Daniel

Brno Brno Brno Brno Brno Brno Brno Brno Brno Brno Brno Brno Brno Brno Brno Brno

Mobile Robot with Mobile Robot Mobile Robot with Mobile Robot with with Estimation Estimation Estimation Estimation ∗∗ ∗∗∗

Davidek ∗ Jan Klecka ∗∗ Karel Horak ∗∗∗ ∗∗∗∗ ∗ Jan ∗∗ Karel Horak ∗∗∗ Davidek Klecka Novacek ∗ Jan ∗∗ Karel Horak ∗∗∗ Davidek Klecka DavidekPetr Jan Klecka ∗∗∗∗ Karel Horak ∗∗∗∗ Petr Novacek ∗∗∗∗ Petr Novacek Petr Novacek ∗ Department of Control and Instrumentation, ∗ ∗ Department of Control and Instrumentation, University of Technology, Technicka 3058/10, 616 00 Brno, ∗ Department of and Department of Control Control and Instrumentation, Instrumentation, University of Technology, Technicka Czech Republic University of of Technology, Technology, Technicka Technicka 3058/10, 3058/10, 616 616 00 00 Brno, Brno, University 3058/10, 616 00 Brno, Republic (e-mail: Czech [email protected]) Czech Republic Czech Republic ∗∗ (e-mail: [email protected]) Department Control and Instrumentation, (e-mail: [email protected]) (e-mail:of [email protected]) ∗∗ ∗∗ Department of Control and Instrumentation, University of Technology, Technicka 3058/10, 616 00 Brno, ∗∗ Department of and Department of Control Control and Instrumentation, Instrumentation, University of Technology, Technicka Czech Republic University of of Technology, Technology, Technicka Technicka 3058/10, 3058/10, 616 616 00 00 Brno, Brno, University 3058/10, 616 00 Brno, Republic (e-mail:Czech [email protected]) Czech Republic Czech Republic ∗∗∗ (e-mail: [email protected]) Department Control and Instrumentation, (e-mail: [email protected]) (e-mail:of [email protected]) ∗∗∗ ∗∗∗ Department of Control and Instrumentation, University of Technology, Technicka 3058/10, 616 00 Brno, ∗∗∗ Department of and Department of Control Control and Instrumentation, Instrumentation, University of Technology, Technology, Technicka 3058/10, 616 616 00 Brno, Brno, Czech Republic University of Technicka 3058/10, University of Technology, Technicka 3058/10, 616 00 00 Brno, Republic (e-mail:Czech [email protected]) Czech Republic Czech Republic ∗∗∗∗ (e-mail: Department [email protected]) Control and Instrumentation, (e-mail: [email protected]) (e-mail: [email protected]) ∗∗∗∗ ∗∗∗∗ Department of Control and Instrumentation, University of Technology, Technicka 3058/10, 616 00 Brno, ∗∗∗∗ Department of and Department of Control Control and Instrumentation, Instrumentation, University of Technology, Technology, Technicka 3058/10, 616 616 00 Brno, Brno, Czech Republic University of Technicka 3058/10, University of Technology, Technicka 3058/10, 616 00 00 Brno, Czech Republic (e-mail: [email protected]) Czech Republic Czech Republic (e-mail: [email protected]) (e-mail: (e-mail: [email protected]) [email protected])

Abstract: Abstract: In this article, an odometry module for a mobile robot with probabilistic position estimation is Abstract: Abstract: In odometry module mobile with position is proposed. The an localization Extended Kalman Filter (EKF)estimation from which In this this article, article, an odometryprocessing module for forisaaabased mobileonrobot robot with probabilistic probabilistic position estimation is In this article, an odometry module for mobile robot with probabilistic position estimation is proposed. The localization processing is based on Extended Kalman Filter (EKF) from which only prediction phase is used. This configuration is prepared for a possible later extension with proposed. The localization processing is based on Extended Kalman Filter (EKF) from which proposed. The localization processing is based on Extended Kalman Filter (EKF) from which only prediction phase is used. This configuration prepared for a possible later extension with Global navigation satellite system Scaleis correction and University of Michigan only prediction phase is This configuration is prepared for later with only prediction phase is used. used. This (GNSS). configuration isfactor prepared for aa possible possible later extension extension with Global navigation satellite system (GNSS). Scale factor correction and University of Michigan Benchmark test (UMBmark) measurements were concluded to correct model parameters of Global navigation satellite system (GNSS). Scale factor correction and University of Michigan Global navigation satellite system (GNSS). Scale factor correction and University of Michigan Benchmark test (UMBmark) measurements were to parameters of the robot. The outcome was tested onconcluded an experiment with model simultaneous Global Benchmark test correction (UMBmark) measurements were concluded to correct correct model parameters of Benchmark test (UMBmark) measurements were concluded to correct model parameters of the robot. The correction outcome was tested on an experiment with simultaneous Global Positioning System (GPS) and odometry localization. The realized differential drive mobile the robot. The correction outcome was tested on an experiment with simultaneous Global the robot. The correction outcome was tested on an experiment with simultaneous Global Positioning and odometry localization. realized differential mobile robot has anSystem optical(GPS) encoder each wheel. They are The connected STM32F4 drive development Positioning System (GPS) and odometry localization. The realized differential drive mobile Positioning System (GPS) andfor odometry localization. The realizedto differential drive mobile robot has an optical encoder for each wheel. They are connected to STM32F4 development board which calculates the odometric data and sends them with a time-stamp from GPS robot has an optical encoder for each wheel. They are connected to STM32F4 development robot has an optical encoder for each wheel. They are connected to STM32F4 development board calculates the data sends them with time-stamp module over the Bluetooth PC. Visualization control created from to logGPS and board which which calculates the toodometric odometric data and and and sends themprogram with aaa was time-stamp from GPS board which calculates the odometric data and sends them with time-stamp from GPS module over the Bluetooth to PC. Visualization and control program was created to log and store the odometric data and also to set up the odometric parameters in the firmware of the module over the Bluetooth to PC. Visualization and control program was created to log and module over the Bluetooth to also PC. to Visualization and controlparameters program was created to log and store the odometric data and set up the odometric in the firmware of the microcontroller. store the odometric data and also to set up the odometric parameters in the firmware of store the odometric data and also to set up the odometric parameters in the firmware of the the microcontroller. microcontroller. microcontroller. © 2016, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Mobile robots, Extended Kalman filters, Odometry, Localisation error, Keywords: Mobile Mobile robots, drive, Extended Kalman encoders, filters, Odometry, Odometry, Localisation error, Visualisation, Differential Incremental GPS, UMBmark, Scale factor, Keywords: robots, Extended Kalman filters, Localisation error, Keywords: Mobile robots, drive, Extended Kalman encoders, filters, Odometry, Localisation error, Visualisation, Differential Incremental GPS, UMBmark, UMBmark, Scale factor, Visualisation, Differential drive, Incremental encoders, GPS, Scale Visualisation, Differential drive, Incremental encoders, GPS, UMBmark, Scale factor, factor, 1. PROLOGUE detectable initial point which is always visible viz Moreira 1. PROLOGUE PROLOGUE detectable initial point point which is is always visible visible viz Moreira Moreira et al. (2007). 1. detectable initial 1. PROLOGUE detectable initial point which which is always always visible viz viz Moreira et al. al. (2007). (2007). et et al. (2007).as these techniques may be, there are still As precise In the field of robotics, there are many possible ways of As precise as these techniques may be, there are still applications simple wheel As precise these techniques may be, are still In the the field field of ofThe robotics, there techniques are many many possible possible ways of As precise as aswhere thesethe techniques may odometry be, there there can are be stillaa localization. localization can be ways divided In robotics, there are of applications where the simpleindoor wheel odometry can be In the field of robotics, there are many possible ways of proposed choice for example wheeled robot localapplications where the simple wheel odometry can be localization. The localization localization techniquestechniques can be be divided divided where simpleindoor wheelwheeled odometry can localbe aa into two groups. Absolute localization allow applications localization. The techniques can proposed choice for the example robot localization. The localization techniques can be divided ization in darkness. proposed choice for example indoor wheeled robot localinto two two measurement groups. Absolute Absolute localization techniques allow for example indoor wheeled robot localposition in absolute coordinates whichallow are proposed into groups. localization techniques ization in in choice darkness. into two measurement groups. Absolute localization techniques allow position in absolute absolute coordinates which are ization ization in darkness. darkness. measuring start position independent. Relative localizaThe localization can be also combined, alposition measurement in coordinates which are position measurement in absolute coordinates are The localization techniques measuring start position independent. Relativewhich localizatechniques can with be also also combined, altion methods can only measure relative movement to first though in this paper the fusion other localization measuring start position independent. Relative localizaThe localization techniques can be combined, almeasuring start position independent. Relative localizaThe localization techniques can with be also combined, altion methods can only measure relative movement to first though in this paper the fusion other localization and previous positions. A special of this group is though methods is this not directly described. tion methods can measure relative movement to in paper the fusion with other localization tion other methods can only only measure relativecase movement to first first though in this paper the fusion with other localization and other previous positions. A special case of this group is methods is is not directly directly described. incremental relativepositions. localization, wherecase the of new estimated and other A this group and other previous previous positions. A special special case this group is is methods methods is not not directly described. described. incremental relative localization, where the of new estimated position is always relative only to the previous one. incremental relative localization, where the new estimated incremental relative localization, where the new estimated position is is always always relative relative only only to to the the previous previous one. one. position 2. INTRODUCTION position is always onlysatellite to the system previous(GNSS) one. can Often used Global relative navigation 2. INTRODUCTION INTRODUCTION Often used Global navigation satellite system (GNSS) can 2. be used anywhere on the planet where the (GNSS) signal from Often used Global navigation satellite system can 2. INTRODUCTION Often used Global navigation satellite system (GNSS) can be used used anywhere anywhere ondetectable. the planet planet where where the the signal from Odometric module realization which is described in this multiple satellites ison this condition be the signal from be used anywhere ondetectable. the planetAlthough where the signal from Odometric module realization which is described in this multiple satellites is Although this condition does not allow use indoors and in similarly closed enviarticle is amodule preparation step for further extension and multiple satellites is detectable. Although this condition Odometric realization which is in multiple satellites isindoors detectable. Although this condition Odometric realization which is described described in this this does not allow use and in similarly closed enviarticlefusion is aamodule preparation step for further extension and ronments. Visual odometry, on the other hand, can also data with equipped GNSS module. The module is does not allow use indoors and in similarly closed enviarticle is preparation step for further extension and does not allow useodometry, indoors and in similarly closed enviarticle is a with preparation step for module. further The extension and ronments. Visual on the other hand, can also data fusion equipped GNSS module is measure position in absolute coordinates if we create some realized on a mobile robot with two drive wheels viz Fig. 1. ronments. Visual odometry, on the other hand, can also data fusion with equipped GNSS module. The module is ronments. Visual in odometry, on the otherif we hand, cansome also data fusion with equipped GNSS module. The module is measure position absolute coordinates create realized on a mobile robot with two drive wheels viz Fig. 1. measure measure position position in in absolute absolute coordinates coordinates if if we we create create some some realized realized on on aa mobile mobile robot robot with with two two drive drive wheels wheels viz viz Fig. Fig. 1. 1.

Copyright © 2016, 2016 IFAC 346 Hosting by Elsevier Ltd. All rights reserved. 2405-8963 © IFAC (International Federation of Automatic Control) Copyright © 2016 IFAC 346 Copyright © 2016 IFAC 346 Peer review under responsibility of International Federation of Automatic Copyright © 2016 IFAC 346Control. 10.1016/j.ifacol.2016.12.063

2016 IFAC PDES October 5-7, 2016. Brno, Czech Republic Daniel Davidek et al. / IFAC-PapersOnLine 49-25 (2016) 346–351

On each drive wheel, there is incremental optical encoder 1 which are connected to the STM32F4 development board via created extension board. The robot is also equipped with GPS unit 2 , which serves for the purpose of this article as the source of time-tag. Firmware running on the MCU is taking care of the position and probability calculations. It also communicates with PC using developed protocol via UART transmission line transferred by a Bluetooth module. On the PC developed visualization and control program shows and stores localization data.

347

R + b/2

=C

x1 , y1 ∆θ R

pL pC

In following sections, the used theoretical background is deduced.

pR y 40

b x x 0 , y0

θ0

55

67.5

20

4

Fig. 2. Geometric relations between wheel paths and path angles for non-equal constant wheel velocities

[cm]

Fig. 1. Thr robot wheel (right) and undercarriage (left) scheme with marked zero coordinates (x0 , y0 )

The robot is represented by a center point position and yaw orientation. The representing center point is a projection of drive wheels’ axis center point to the plane surface on which the robot stands as it can be seen in Fig. 1. Equations (1), (2) and (3) formulate relations between wheel paths and travelled path angles depicted in 2. They are valid if the following conditions are met: • Each wheel has point contact with the floor. • There is no slippage. • Angular acceleration is zero for both wheels thus angular velocities are constants but not equal. pR + pL 2 pR − pL ∆θ = b pL b R= pR − p L b pR + pL pC b C =R + = = 2 2 p R − pL ∆θ

(1) (2) (3) (4)

In the used model each wheel circumscribe a curve which is almost circular for small angular acceleration. Both circle segments have common centers of curvature. We are neglecting the angular acceleration due to the high sampling frequency and low acceleration of the used motors. 1 2

If the velocities are exactly equal both wheels travels along a line. Radius R from (3) is infinite therefore (1), (2) and (3) are not used. Wheel path pL is calculated from the encoder ticks tL . It is propotional to the wheel radius rL and ticks per revolution TL viz (5).

3. GEOMETRIC REPRESENTATION

pC =

x0 , y0

L stands for left wheel and R for the right one. The equations for the right wheel are analogous. pL =

2πrL tL TL

(5)

Position update calculation is performed periodically. It is based on the length s (6) depicted in Fig. 4. As the period of calculations is high relatively to the movement we can assume sufficiently small angles and thus deduce formula valid for small angles (7) from (4). s =2C sin(∆θ/2) pC s= sin(∆θ/2) ∆θ/2

(6) (7)

Cartesian position components update can be calculated as in (8) for non-equal wheel velocities and (9) for the equal velocities. The lower index 0 is used for the previous variable value (k −1) and the lower index 1 symbolizes the new calculated variable value (k).   x1 = y =  1 θ1 =

x0 + s cos(θ0 + ∆θ/2) y0 − s sin(θ0 + ∆θ/2) ∆θ + θ0

(8)

After de substitution of s in (8) we get (10) which is used for the calculations as its variables can be directly

ENC HEDS 5500 - Avago Technologies BD982 - Trimble

347

2016 IFAC PDES 348 October 5-7, 2016. Brno, Czech Republic Daniel Davidek et al. / IFAC-PapersOnLine 49-25 (2016) 346–351

y

3.1 Probablistic model Fundamentally the localization error using the odometry only is increasing with time and path traveled, as only relative measurements from the previously localized position are available. This is due the influences of two kinds Borenstein et al. (1996):

∆θ x1 , y1

θ1

θ0

∆θ

s

• Systematic errors Unequal wheel diameters, misalignment of wheels, finite encoder resolution and sampling rate. • Non-Systematic errors Travel over uneven floors, unexpected objects on the floor and wheel slippage due to slippery floors, skidding, non-point contact with the floor.

∆θ/2

x0 , y0 θ0

There are many models for localization probability calculation. Vast summary can be found in Kramer and Kandel (2012).

x

Fig. 3. Path update s and other geometric quantities

y

Because of the implementation where the MCU processes the localization data and sends them to PC, extended Kalman filter was preferred over particle filters and other methods as it uses lower data bandwidth on dispatch.

R + b/2 x1 , y1

Kalman filter The most effective linear filter for linear systems with the white Gaussian noise is Kalman filter. The Kalman filter is also the ideal linear filter for nonlinear systems as it is proved in Simon (2006).

∆θ

x0 , y0 s cos α

Discrete Kalman filter (DKF) assumes linear system given by the definition (12) with description in table 1.

s sin α

s



xk = zk =

θ0 + ∆θ/2 = α

Axk−1 + Buk−1 + wk Cxk + vk

(12)

Table 1. State equation variables key

x Fig. 4. Path update s cartesian projections (θ0 in Fig. 3) calculated from the measured wheel paths . As the frequency of calculations can be set very high (depends on the used MCU architecture) goniometric function multiplication can be replaced by its subtraction as in (11). Although the relative calculation error is higher, because we subtract two numbers of common magnitude Limpouch (1999). Therefore the equations can be interchanged in the firmware.

   x1 = y1 =   θ1 =

 x1 = y =  1 θ1 =

 x1 = y =  1 θ1 =

x0 + pC cos(θ0 ) y0 + pC sin(θ0 ) θ0

pC ∆θ/2 sin(∆θ/2) cos(θ0 + ∆θ/2) pC ∆θ/2 sin(∆θ/2) sin(θ0 + ∆θ/2)

x0 + y0 − ∆θ + θ0



pC ∆θ  sin(θ0 pC ∆θ cos(θ0

x0 + y0 − ∆θ + θ0

 + ∆θ) − sin(θ0 )  + ∆θ) − cos(θ0 )

(9)

(10)

(11)

348

x u z A B C w v

... ... ... ... ... ... ... ...

state vector input variables vector measured variables vector system matrix system to input bonding matrix observation matrix input noise vector measurement noise vector

Kalman filter works over probabilistically expressed system. The system is represented via its average value x ˆk and its covariance matrix Pk . In the first phase of Kalman filtering there is an update of the current state as can be seen in (13) and in the second phase it updates the system by inclusion of new measurements as in (14) Santana et al. (2008).  − x ˆk = Ak xk−1 + Bk uk−1 T P− k = Ak Pk−1 Ak + Wk  − T − T − Kk = Pk Ck (Ck Pk Ck + Vk ) 1 − − ˆk + Kk (zk − Ck x ˆk ) x ˆ =x  k Pk = (I − Kk Ck )P− k

(13)

(14)

The upper index minus indicates apriori variables which hold a value before the k-step measurement inclusion. No

2016 IFAC PDES October 5-7, 2016. Brno, Czech Republic Daniel Davidek et al. / IFAC-PapersOnLine 49-25 (2016) 346–351

349

upper index indicates aposteriori variables which take into account the current k-step measurement. Kk is a Kalman filter gain matrix.

apriori probability is taken as the aposteriori probability (26) Kiriy and Buehler (2002) .

The system noise is represented by covariance matrices 15. It is presumed the noise is white and uncorrelated Kiriy and Buehler (2002).

P = P− k

p(wk ) = N (0, Q) p(vk ) = N (0, R)

(15) (16)

Because of the system non-linearities we employ the Extended Kalman Filter (EKF). EKF models the nonlinearity by partial differentials around the current state.  xk = g(xk−1 , uk−1 ) + wk zk = h(xk ) + vk



x ˆ− k = g(xk−1 , uk−1 ) T P− k = Gk Pk−1 Gk + Wk

 − T − T − Kk = Pk Hk (Hk Pk Hk + Vk ) 1 − − ˆk + Kk (zk − h(ˆ xk )) x ˆ =x  k − Pk = (I − Kk Hk )Pk

After de substitution non-linear system function p using (11) in shape of column vector and its linearization we get following expressions:

Gk =



1 0 0 1 0 0

pC (cos (∆θ ∆θ pC ∆θ (sin (∆θ

Dk =

(18)

(19)

Matrix G is a linearized system model (20) and H is linearized measurement vector (21). ∂g(xk−1 , uk−1 ) Gk = (20) ∂xk−1 ∂h(xk−1 ) (21) Hk = ∂xk In compliance with (Sebastian Thrun (2005), Santana et al. (2008), Larsen et al.) we include encoder error εtL into encoder output as in (22) thus we must adapt the system (23) and EKF prediction equations (24).  tˆL = tL ± εtL (22) tˆR = tR ± εtR  xk = g(xk−1 , uk−1 , ε) zk = h(xk ) + vk

(23)

x ˆ− k = p(xk−1 , uk−1 , 0) T T P− k = Gk Pk−1 Gk + Dk Γk Dk

(24)



3.2 Used formulae for calculation in odometric module

(17)

The non-linear functions g and h in (17) represents system and measurement model. EKF prediction and actualization step functions are adjusted accordingly (18), (19).

The Γ is sensor noise covariance matrix and Dk is Jacobian matrix mapping odometric noise into state space. ∂p(xk−1 , uk−1 , 0) (25) Dk = ∂uk−1 In our system where external absolute measurement is not implemented and the odometry encoder measurement is part of the system model, only prediction phase of EKF is used. In this case, where there are no measurements the 349

(26)



+ θk ) − cos (θk )) + θk ) − sin (θk )) 1



 D11 D12 D12 D22 , kde aL /b aR /b

(27)

(28)

 pR  D11 = abL [−C cos β + (∆θ) 2 (sin β − sin θ1 )]    pL a R  = [ C cos β − D  12  b (∆θ)2 (sin β − sin θ1 )]   pR aR D12 = b [−C sin β − (∆θ) 2 (cos β − cos θ1 )] p aL L  D22 = b [ C sin β + (∆θ)2 (cos β − cos θ1 )]    pC   C  = ∆θ   β = ∆θ + θk

(29)

Encoder sensor noise covariance matrix Γk converts perrevolution encoder tick error σTL/R relatively to tick count tL/R over per-revolution encoder ticks TL/R .  2  σtL 0 , kde (30) Γk = 2 0 σtR tL TL tR = σTR TR

σtL = σTL

(31)

σtR

(32)

4. EXPERIMENTS 4.1 Gear measurement To find out the maximal number of misinterpreted ticks from encoders the backlash of drive wheel gear was measured. Also, tick per revolution and gear ratio were measured. All outcomes from gear measurement are available in table 2 Table 2. Gear measurement outcomes (average ± standard deviation) wheel ticks per revolution TL/R backlash per revolution gear ratio

left 8582 ± 4 278 ± 14 16.76 : 1

right 8581 ± 3 120 ± 30 ←

4.2 Scale factor measurement For nominal wheel diameter systematic error reduction, scale factor measurement was performed. Mobile robot was

2016 IFAC PDES 350 October 5-7, 2016. Brno, Czech Republic Daniel Davidek et al. / IFAC-PapersOnLine 49-25 (2016) 346–351

driven along a straight line of known length ten times and from odometric measured vs real length difference the scale factor ES was calculated using (33). ES =

Lreal lenght

(33)

Lcalc by odometry rL/R = rL/R ES

Where L is square side length, b is drive wheel base and (cB , cL , cR ) are correct factors. Table 3. Calculated UMBmark correct factors cB 0.99987 0.99980

UMB + scalefactor ES UMB only

cL 0.99995 0.99995

cR 1.00005 1.00005

(34) 4.4 GPS referenced odometric measurement

4.3 UMBmark measurement For systematic error from left and right wheel diameter and wheelbase effective value difference UMBmark measurement Borenstein and Feng (1994) was carried out. In this experiment was the mobile robot drive wheel axis center situated over one square corner with a side over five meters as can be seen in Fig. 5. Its orientation was aligned with one of the square sides. The robot was driven along each side of the square with 90◦ turns in corners afterward. On arrival at the starting position, the odometrically measured position was recorded. The measurement was repeated five times in both clockwise (CW) and counterclockwise (CCW) directions. If the odometric measurement is exactly correct the recorded ending position (XCW/CCW , YCW/CCW ) should be exactly 0 in both x and y-axis.

For odometry precision evaluation simultaneous GPS and odometric localization were carried out on flat moderately tilted surface depicted on Fig. 6. The tilt angles are depicted greater than in reality. As it is written at the end of this chapter the localization error coming from the tilted surface from the non-level surface is insignificant against the GPS localization error. Stationary base station referential GPS antenna was situated on the border of the floor. The mobile robot has two GPS antennas for position and yaw, pitch angles measurement registered via NMEA0183:PTNL,VGK and NMEA-0183:PTNL,AVR messages. Coordinate systems odometric data center point frontal localisation antenna base station referential antenna localisation antenna base antenna

Up

auxilary antenna

V

K

H α2

L1

L2

α1 D1 D2

Fig. 6. GPS referenced measurement surface slope profile Fig. 5. UMBmark measurement square frame From the UMBmark measurement correcting factors, values can be calculated via (36). When both directions (CW/CCW) are in the lower index it means the calculations are analogous for each direction. As it is concluded in Borenstein and Feng (1994) values calculated from the measurement conducted on a flat surface can be used to correct the systematic errors.  x ˆCW/CCW = n1 Σn1 Xi,CW/CCW yˆCW/CCW = n1 Σn1 Yi,CW/CCW   α       β        ED

  cB         c L   c

R

=



x ˆCW +ˆ xCCW −4L  −ˆ xCCW average xˆCW−4L

= average

L + b sin(β/2) L − b sin(β/2) π = π+α 2 = ED + 1 = ED cL =

−ˆ yCCW ; yˆCW−4L

;

yˆCW +ˆ yCCW −4L

(35) 

Trajectories As it is apparent from Fig. 6 the floor surface has a cross section of two slopes with different angles. The slope angles were calculated from GPS measurements. The robot was driven along two oval trajectories (less than 20 m and 40 m long) afterward with both GPS and odometric localization online. The first did not cross the D1 distance and the second was closer than D2 distance. The recorded data was afterward processed in MATLAB. The calculated maximal errors of localization position (37) were one order smaller (table 4) than those of GPS position (2 - 4 mm), therefore tilting of measured data was not needed nor performed. 

 (36)

αi ∆i

(37)

Table 4. Calculated slope variables and errors trajectory distance height slope angle position error viz (37)

350

Li = atan D i 1−cos αi = Li cos αi

i Di Li αi ∆i

1 (=A) 1, 8295 m 0, 0264 m 0.8267◦ 0.1905 mm

2 (=B) 3, 4685 m 0, 0431 m 0.7119◦ 0.2675 mm

2016 IFAC PDES October 5-7, 2016. Brno, Czech Republic Daniel Davidek et al. / IFAC-PapersOnLine 49-25 (2016) 346–351

∆i

REFERENCES

Di

Li

αi Di Fig. 7. Maximal position error calculation ∆i scheme   ex,i = ODOMx,i − GPSx,i ey,i = ODOM (38)  y,i − GPSy,i   Σ = n e 2 + e 2 j x,i y,i i Σj Σ1 Σj EXYj = n Qj =

(39) (40)

Table 5. Outcomes for both trajectories and different correction factors

traj. A B

351

set (j) ES UMBmark Qj EXYj [m] Qj EXYj [m]

1 no no 1 0.11833 1 0.31069

2 no yes 1.4704 0.17400 0.99253 0.30837

3 yes no 1.399 0.16555 0.98603 0.30635

4 yes yes 0.94033 0.11127 0.97835 0.30396

5. CONCLUSION Calculations were performed to determine the lowest error correction parameters. The measured odometry localization error from GPS data was added up across the whole trajectory Σj in (38). To compare each correction method the error was normalized to trajectory localization error of not corrected parameters to get variable Qj (39) (lower is better). To get the average odometry localization error over the whole trajectory in meters EXYj we can use (40) (lower is better). EXYj is directly tied to the trajectory length as the localization error would steadily increase without the EKF measurement step. From the table 5 we can see that the localization error is the lowest when using both UMBmark and scale factor parameter correction on both trajectories. The next step to improve mobile robot localization would be GPS position data addition to the EKF measurement step to correct the odometry localization non-systematic errors, also to verify the consistency of the used noise models. ACKNOWLEDGEMENTS The completion of this paper was made possible by the grant No. FEKT-S-14-2429 - The research of new control methods, measurement procedures and intelligent instruments in automation - financially supported by the Internal science fund of Brno University of Technology and by the project Centre for Applied Cybernetics 3 (TE01020197) under the Technology Agency of the Czech Republic. 351

Borenstein, J., Everett, H.R., and Feng, L. (1996). ”Where am I?” – Systems and Methods for Mobile Robot Positioning. p.130-131. URL http://www-personal. umich.edu/~johannb/Papers/pos96rep.pdf. Borenstein, J. and Feng, L. (1994). Umbmark – a method for measuring, comparing, and correcting odometry errors in mobile robots. Technical report, University of Michigan. URL http://www-personal.umich.edu/ ~johannb/Papers/umbmark.pdf. Kiriy, E. and Buehler, M. (2002). Three-state extended kalman filter for mobile robot localization. Technical report. URL http://kom.aau.dk/group/05gr999/ reference_material/filtering/eKf-3state.pdf. Kramer, J. and Kandel, A. (2012). On accurate localization and uncertain sensors. International Journal of Intelligent Systems, 27(5), 429–456. doi:10.1002/int. 21530. URL http://dx.doi.org/10.1002/int.21530. Larsen, T., Hansen, K., Andersen, N., and Ravn, O. (????). Design of kalman filters for mobile robots, evaluation of the kinematic and odometric approach. In Proceedings of the 1999 IEEE International Conference on Control Applications (Cat. No.99CH36328). Institute of Electrical & Electronics Engineers (IEEE). doi:10.1109/cca. 1999.801027. URL http://dx.doi.org/10.1109/CCA. 1999.801027. Limpouch, J. (1999). Sireni chyb ve vypoctech: Zaokrouhlovaci chyby. URL http://kfe.fjfi.cvut. cz/~limpouch/numet/foluvux/node6.html. Moreira, M.A.G., Machado, H.N., Mendonca, C.F.C., and Pereira, G.A.S. (2007). Mobile robot outdoor localization using planar beacons and visual improved odometry. In Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, 2468–2473. doi:10.1109/IROS. 2007.4399384. Santana, A.M., Souza, A.A.S., Britto, R.S., Alsina, P.J., and Medeiros, A.A.D. (2008). Localization of a mobile robot based in odometry and natural landmarks using extended kalman filter. Technical report. doi:10.13140/ 2.1.2081.8882. URL http://dx.doi.org/10.13140/2. 1.2081.8882. Sebastian Thrun, Wolfram Burgard, D.F. (2005). Probabilistic Robotics. The MIT Press. URL http://www.ebook.de/de/product/3701211/ sebastian_thrun_wolfram_burgard_dieter_fox_ probabilistic_robotics.html. Simon, D. (2006). Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches. Wiley. URL https: //books.google.cz/books?id=UiMVoP\_7TZkC.