Augmentation of Enhanced HDR Utilizing Encoder to Estimate Yaw-Angle of Cleaning Robots

Abstract

This paper proposes an improved algorithm for augmenting the enhanced HDR by utilizing encoder readings to achieve further minimized drift on the yaw-angle estimation of cleaning robots. The primary idea of the proposed approach consists of two phases: 1) classifying the manoeuvre (stop, straight and curving) being carried out using logic as a function of encoder readings; 2) controlling the Attenuator gain, resetting the Binary I-controller and switching Integrator in the enhanced HDR structure, taking into account the identified manoeuvring conditions. The proposed algorithm is applied to a cleaning robot and the experimental data demonstrate the effectiveness of this approach.

Keywords

Yaw-angle Estimation MEMS Gyro Encoder Cleaning Robot Enhanced HDR (Heuristic Drift)

1. Introduction

Autonomous cleaning robots have increasingly gained popularity due to their time saving function and for reducing household labour. Developing a low-cost self-localization technology for cleaning robots is one of the most important obstacles in further popularizing these robot products. However, trade-offs exist between cost and quality, and how these factors influence their performance.

Although diverse self-localization technologies such as sensor-based, mark-based and stereo vision-based technologies are being studied [1 –3], in general, these systems are expensive [4 –6]. On the other hand, the relative localization via yaw-angle estimation with inertial sensors (i.e., low-cost MEMS gyro) holds a considerable advantage in that it is entirely self-contained inside the robot and is easy to accomplish in real-time. Additionally, it results in lower installation and operating costs for the entire system [5,6]. However, as yaw-angle estimation relies on the integration of rate signals from gyros that have relatively large drift errors, unbounded yaw-angle errors can occur, which consequently causes unbounded position errors [7 –11].

Recently, studies have been conducted that applied specific techniques to certain systems (e.g., zero-velocity update [ZUPT] and heuristic drift reduction [HDR] for pedestrian navigation) [12]. This presented the possibility of implementing low-cost systems using only gyro sensors. In addition, the enhanced HDR (hereafter referred to as an e-HDR) structure, which adaptively controls the filter gain for updating the gyro drift error more accurately, had been adopted in wheeled vehicles (or mobile robots) [13 –17]. However, e-HDR has some shortcomings in certain situations, e.g., 1) when true rates are less than the threshold in the HDR process; 2) when the measured rates are highly contaminated with noise even in the stationary state. In such conditions, the estimated bias is accumulated over time. This shortcoming is primarily due to inability to discriminate its own manoeuvre, as e-HDR utilizes gyro as its sole sensor.

To improve on previous studies involving e-HDR and to achieve further minimized drift on yaw-angle estimation, this paper examines a simple but effective filtering method that augments the e-HDR by utilizing encoder readings. The main idea of the proposed approach consists of two phases; 1) classifying the manoeuvre (stop, straight and curving) being carried out through logic as a function of encoder readings; 2) controlling the Attenuator gain, resetting the Binary I-controller and switching Integrator in the e-HDR structure, taking into account the identified manoeuvring conditions. By applying this strategy, the shortcomings of the e-HDR can be overcome and the drift on yaw-angle estimation can be minimized, regardless of time.

A sensor kit was created based on a MEMS gyro (MPU6050, InvenSense Co. [18]) and a microcontroller (ATmega328, Atmel Co. [19]). The proposed algorithm was embedded into the kit and its effectiveness demonstrated through precise rate-table tests and actual manoeuvring tests on a commercial cleaning robot.

2. Heuristic Drift Reduction (HDR) Filter

2.1. The pure HDR filter

The gyro's bias drifts are a major cause of performance degradation due to the accumulation of integration errors during yaw-angle (ψ) estimations. To estimate and compensate for bias drifts, HDR in the form of closed loop control systems were presented [12]. A block diagram of the filters are shown in Fig. 1

Figure 1.

Block diagram of pure HDR filters

Figure 2.

Block diagram of e-HDR filters

The rate of rotation measurement (ω_m) including error factors and the estimated rate (ω_i) are as follows:

{\begin{matrix} ω_{m} = ω_{t r u e} + {\overset{ˉ}{ε}}_{0} + ε_{d} \\ ω_{i} = ω_{m} - {\overset{ˉ}{ε}}_{0} + I_{i} \end{matrix}

(1)

where $ω_{t r u e}$ is the true rate of rotation, ${\overset{ˉ}{ε}}_{0}$ is the fixed bias, ε_d is the bias drift and I_i is the estimated bias drift.

The fixed bias ${\overset{ˉ}{ε}}_{0}$ is estimated by taking the average of ω_m. for approximately 5~15 sec in the perfect stationary condition at the initial activation period of the cleaning robot. The ${\overset{ˉ}{ε}}_{0}$ can be calculated using an average filter in the form of recursive expression, as shown by Eq. 2.

{\overset{ˉ}{ε}}_{0, i} = \frac{i - 1}{i} {\overset{ˉ}{ε}}_{0, i - 1} + \frac{1}{i} ω_{m, i}

(2)

Meanwhile, to remove the bias drift element ( $ε_{d})$ , the estimated bias drift is calculated through the integral controller as follows:

I_{i} = I_{i - 1} - S I G N (ω_{i - 1}) i_{c}

(3)

where i_c is a fixed increment and $S I G N ()$ is a programming function that determines the sign of a number. $S I G N (x)$ returns ‘1’ if x is positive, ‘0’ if $x = 0$ , and ‘-1’ if $x$ is negative.

Theoretically, in a steady state, because of the I-controller, E in Fig. 1 will eventually be zero, which implies that the control signal I will track (but with an opposite sign) slow changes of ε_d with no offset. That is, $I ≅ - ε_{d}$ under ideal conditions [12]. However, this pure HDR filter is only valid for specific application, e.g., pedestrian navigation, which frequently has a zero velocity stationary condition.

2.2. The e-HDR filter

e-HDR gains further performance enhancement during motion, specifically for the mobile vehicle, compared to the pure HDR, through the addition of an ‘attenuator’. That is, the estimated bias drift (I_i) can be applied differently depending on the size of $ω_{i - 1}$ , thereby enhancing the bias drift's estimation capability [13 –17].

Attenuator gain (A_i) is controlled by Eq. 4, shown below.

A_{i} = {\begin{matrix} \begin{matrix} 1 - \frac{| ω_{i - 1} |}{θ_{ω}}, & | ω_{i - 1} | \leq θ_{ω} \end{matrix} \\ \begin{matrix} 0, & | ω_{i - 1} | > θ_{ω} \end{matrix} \end{matrix}

(4)

where $θ_{ω}$ is the threshold of the angular rate.

The entire e-HDR algorithm can now be implemented by:

I_{i} = I_{i - 1} - A_{i} S I G N (ω_{i - 1}) i_{c}

(5)

When compared to the estimation equation for pure HDRs (Eq. 3), it can be seen that bias drifts can be more effectively estimated with attenuator gain (A_i), considering the manoeuvring conditions. However, to estimate the bias drift using e-HDR, the cleaning robot must stop or move straight ( $ω_{s e t} = 0$ ) without fail. In addition, as the stationary or straight state is judged based on the gyro output contaminated by the bias drift (ε_d) and the vibrational component generated when the brush of the cleaning robot rotates (see Fig. 3), accuracy may be low. Moreover, when the cleaning robot conducts a rotation within the threshold, the e-HDR recognizes the movement as drifting bias and incorrectly estimates $I_{i}$ . This can result in error accumulations over time in the yaw-angle estimation. This erroneous phenomenon is described in section 4 via precise rate table tests. Therefore, to overcome the limitations of the e-HDR, it should be improved via accurate manoeuvre discrimination.

Figure 3.

Raw gyro outputs in stationary situations with brush rotations

3. Augmentation of e-HDR Utilizing Encoder

As reviewed earlier, e-HDR has some shortcomings in certain situations, e.g., when: 1) true rates are less than the threshold; 2) the measured rates are highly contaminated with noise even in the stationary condition. In this section, an improved algorithm is presented that augments the e-HDR by utilizing encoder readings. The major concept of the proposed algorithm consists of two phases: 1) classifying the manoeuvre (stop, straight and curving) being carried out through logic as a function of encoder readings; 2) controlling the Attenuator gain, resetting Binary I-controller and switching Integrator in the e-HDR structure, taking into account the identified manoeuvring conditions. A full conceptual block diagram for the proposed augmentation of an e-HDR-utilizing encoder is shown in Fig. 4.

Figure 4.

A block diagram of the augmentation of the e-HDR-utilizing encoder

3.1. Classifying manoeuvres

In this study, encoders were used to discriminate the cleaning robot's manoeuvres. Even though diverse error factors generally exist in low-grade encoders installed in cleaning robots, identifying the cleaning robot's manoeuvres such as stopping, driving straight and turning can be considered valid. Additionally, the discrimination of such manoeuvres via encoder readings is more apparent than those via gyros, as shown in Table 1 [5].

Table 1.

Discrimination effectiveness for mobile robot manoeuvres

Manoeuvring condition	Encoder	Gyro
Stationary	High	Medium
Straight	High	Low
Turning	Medium	High

3.2. Manoeuvring discrimination logic via encoders

The logic for manoeuvring discrimination through encoders is shown in Table 2, where $n_{i_L}$ and $n_{i_R}$ are the counted values of the pulses from the left (L) and right (R) encoders, respectively, and $Δ | n_{i} | = | n_{i_L} - n_{i_R} |$ is an absolute value of the difference of pulses between the left and right encoder. In stationary situations, the counted numbers of signals from the two encoders are ‘0’ and thus, apparently, $Δ | n_{i} |$ should be ‘0’. On the other hand, in turning situations, $n_{i_L}$ and $n_{i_R}$ are not ‘0’, and $Δ | n_{i} |$ exceeds the threshold (δ_E). The outputs ( $C_{E})$ from the manoeuvring discrimination logic are returned as 0 in cases of stationary situations, 1 in cases of driving straight situations and 2 in cases of turning situations, as shown in Table 2.

Table 2.

Definition of manoeuvring discrimination functions through encoders

Manoeuvring condition	$n_{i_L}$	$n_{i_R}$	$Δ \| n_{i} \|$	$C_{E}$
Stationary	$= 0$	$= 0$	$= 0$	0
Straight (including swaying)	$\neq 0$	$\neq 0$	$\leq δ_{E}$	1
Turning (including curving)	$\neq 0$	$\neq 0$	$\geq δ_{E}$	2

3.3. Refining the attenuator

Attenuator gain (A_i) control via the manoeuvring discrimination logic indicated above is shown in Eq. 6 below:

A_{i} = {\begin{matrix} \begin{matrix} 1, & i f C_{E} = 0 \end{matrix} \\ \begin{matrix} 1 - \frac{| ω_{i - 1} |}{θ_{ω}}, & i f C_{E} = 1 \land | ω_{i - 1} | < θ_{ω} \end{matrix} \\ \begin{matrix} 0, & E l s e \end{matrix} \end{matrix}

(6)

Through this refined attenuator, which utilizes encoder readings, the manoeuvring conditions with angular rates lower than the threshold are considered as swaying motion, i.e., unlikely e-HDR.

3.4. Resetting I-controller

The purpose of resetting the I-controller is to nullify the accumulated error in estimated bias drift ( $I_{i - 1}$ ) when cleaning robots are discriminated in a stationary condition, seemingly via encoder readings ( $C_{E} = 0$ ). To this end, the bias drift is initialized with $ω_{m, i}^{'}$ as shown in Fig. 4 and Eq. 5 is refined as:

I_{i} = {\begin{matrix} \begin{matrix} ω_{m, i}^{'} - A_{i} S I G N (ω_{i - 1}) i_{c}, & i f C_{E} = 0 \end{matrix} \\ \begin{matrix} I_{i - 1} - A_{i} S I G N (ω_{i - 1}) i_{c}, & E l s e \end{matrix} \end{matrix}

(7)

3.5. Switching logic for integration

During operation of the cleaning robot, yaw-angles do not change in the stationary condition; thus, the integration process in such a condition is set to halt in order to further minimize the accumulation of integration errors. Therefore, integration is performed only when $C_{E} = 1 or 2,$ as shown in Eq. 8.

ψ_{i} = {\begin{matrix} \begin{matrix} ψ_{i - 1}, & C_{E} = 0 \end{matrix} \\ \begin{matrix} ψ_{i - 1} + ω_{i} Δ t, & E l s e \end{matrix} \end{matrix}

(8)

The set parameters for implementing the proposed algorithm are shown in Table 3.

Table 3.

Parameters of the proposed algorithm

Parameter	Symbol	Value	Units
LPF cut-off frequency	$ω_{c}$	15	rad/sec
Nominal increment	$i_{c}$	0.0001	deg/sec
Turn threshold	$θ_{ω}$	0.7	deg/sec
Encoder threshold	$δ_{E}$	20	#Pulse

4. Experimental Results

For experimental verification, a sensor kit (uploaded with the proposed algorithm estimation filter) that satisfied the specifications of the electric interface and the size of the commercial cleaning robot (VR20H9050UW, Samsung) was designed and fabricated in-house (see Table 4). The MCU (micro controller unit) of the inertial sensor kit was based on ATmega328 (Atmel Co.) and the MPU6050 z-axis gyro sensor (InvenSense Co.) was adopted as the MEMS gyro. An RB-35GM (D&J WITH Co.), installed in the cleaning robot, was used as an encoder.

Table 4.

Specifications of the yaw-angle estimation sensor kit

Item	Specification
Bias stability	36 deg/h
Angle random walk	3 deg/ $\sqrt{h}$
Rate random walk	18 deg/h/ $\sqrt{H z}$
Power source	2.37-3.46 V
Size	4 ×4 ×0.9 mm
Communication mode	RS-232
Encoder channel	2 ch

4.1. Precise rate table test

To verify the effectiveness of the proposed algorithm compared to e-HDR in certain situations (when the input rate is less than the threshold ( $\approx 0.5 d e g / s$ ) for a period of time, i.e., $C_{E} = 1 \land | ω_{i - 1} | < θ_{ω}$ ), a precise rate table test (rate resolution $0.0001 d e g / s$ ) was performed, as shown in Fig. 5. As shown in Fig. 6(a) and (b), for this period, both ‘proposed algorithm’ and ‘e-HDR’ made estimations by cognizing the rate of rotation as bias drift. However, when the motion was completely stopped (when $C_{E} = 0$ ), we were able to confirm that the yaw-angle error had not accumulated in the proposed algorithm (by resetting the I_i to 0), while the yaw-angle error of e-HDR diverged over time (due to $I_{i} = - 0.5 d e g / s$ ), as shown in Fig. 6(c). Furthermore, it can be seen that the yaw-angle is completely stable in the phase where $C_{E} = 0,$ according to the ‘switching logic for integration’, which was not the case for e-HDR.

Figure 5.

Precise rate table test and sensor kit

Figure 6.

Comparisons between the proposed algorithm and e-HDR: (a) yaw rate; (b) estimated bias; (c) yaw-angle

4.2. Manoeuvring test

For verification through actual manoeuvring tests, the sensor kit was installed on a commercial cleaning robot (VR20H9050UW, Samsung). Manoeuvring tests consisting of driving straight and turning were repeatedly conducted for approximately 300 sec on the 3 m× 1 m track on which the reference equipment motion capture system (Raptor-4, Motion Analysis Co. [20]) was installed (see Fig. 7). Sensor kit data and reference equipment data were obtained via Bluetooth and Ethernet, respectively, at 50 Hz (see Fig. 8).

Figure 7.

Test arena

Figure 8.

Block diagram of sensor kit and data acquisition

Fig. 9(a) shows the X-Y trajectory for when the robot is in auto cleaning mode. The cleaning robot divides the 3m× 1 m track into section ‘A’ and ‘B’, and conducts the cleaning process (wall following, then a zig-zag pattern) separately. The black, dashed line represents wall following, while the red line represents movement via the zig-zag pattern. Fig. 9(b) and (c) show the trajectory X and Y versus time, respectively.

Figure 9.

Trajectory versus time: (a) X-Y; (b) X vs. T; (c) Y vs. t

The results of the estimated yaw-angle for both proposed algorithm and e-HDR can be seen in Fig. 10. Fig. 10(b) and (c) show that while the e-HDR showed an increase in errors over time ( $\geq 15 d e g$ ) compared to the true yaw-angle, the proposed algorithm showed a well-bounded error ( $\leq 3 d e g$ ), regardless of time. The results of the cleaning robot manoeuvring tests were repeated three times and are summarized in Table 5.

Figure 10.

Results of the manoeuvring tests: (a) yaw-angle; (b) enlarged (276~300s); (c) error

Table 5.

Quantitative experimental results

(Unit:deg)
Trial	e-HDR	Proposed
Trial	RMS(STD)	RMS(STD)
Test I	11.98 (5.97)	2.53 (1.85)
Test II	16.84 (8.29)	2.74 (1.37)
Test III	17.41 (10.25)	1.29 (1.14)
Average	15.41 (8.17)	2.19 (1.46)

5. Conclusion

This paper proposed an improved algorithm that augments the e-HDR by utilizing encoder readings to achieve further minimized drift on the yaw-angle estimation of cleaning robots. The primary idea of the proposed approach consisted of two phases: 1) classifying the manoeuvre (stop, straight and curving) being carried out through logic as a function of encoder readings; 2) controlling the Attenuator gain, resetting Binary I-controller and switching Integrator in the e-HDR structure, taking into account the identified manoeuvring conditions. The proposed algorithm was implemented in a sensor kit, which was self-fabricated for this study and its effectiveness was demonstrated through precise rate-table tests and actual manoeuvring tests on a commercial cleaning robot. By applying this strategy, the shortcomings of the e-HDR can be overcome and the drift on yaw-angle estimation can be well-bounded, regardless of time and minimized ( $\leq 3 d e g$ ).

Footnotes

6. Acknowledgements

This work (Grants No.C0333549) was supported by Business for Cooperative R&D between Industry, Academy, and Research Institute, fund in the Korea Small and Medium Business Administration in 2015.

References

Olson

C. F.

. Probabilistic Self-Localization for Mobile Robots. IEEE Robotics and Automation Society. 2000; (Feb 2000)DOI: 10.1109/70.833191.

Zhou

Wei

Tan

. Mobile robot self-localization based on global visual appearence features. IRCA ′03; 14–19 Sept. 2003; Taiplei, Taiwan. IEEE; 2003. p. 1271–1276.

Jang

G. J.

Kim

S. H.

Lee

W. H.

Kweon

I. S.

. Color landmark based self-localization for indoor mobile robots. In: ICRA ′02; 11–15 May 2002; Washington, DC, USA. IEEE; 2002. p. 1037–1042. DOI: 10.1109/ROBOT.2002.1013492.

Borenstein

Feng

. Measurement and correction of systematic odometry errors in mobile robots. IEEE Trans. on Robotics and Automation. December 1996; 1(6):pp.869–880.

Cecco

M. D.

. Sensor fusion of inertial-odometric navigation as a function of the actual maneuvers of autonomous guided vehicles. Institute of Physics Publishing, Meas. Sci. Technol. 2003; 14(5):pp. 643–653.

Kim

J. M.

Kim

Y. T.

Kim

S. S.

. Indoor localization for mobile robot using extended Kalman filter. Journal of Fuzzy Logic and IntelligentSystems. 2008; 18(5):pp. 706–711.

Hong

S. K.

Moon

Ryuh

. Angle measurements for mobile robots with filtering of short-term noise in inertial sensors. Transactions of the Institute of Measurement & Control. May 2009; 33(6): 650–664. DOI: 10.1177/0142331209342194.

Hong

S. K.

Park

. Minimum-drift heading measurement using a MEMS gyro for indoor mobile robots. Sensors. 2008; 8(11):7287–7299. DOI: 10.3390/s8117287.

Hong

S. K.

Bae

. Improvement of Heading Error Using a Wavelet De-noising Filter for Indoor Mobile Robots: Application to MEMS Gyro. Journal of Institute of Control, Robotics and Systems. 2008; 14(9):893–897. DOI: 10.5302/J.ICROS.2008.14.9.893.

10.

Hong

S. K.

Bae

. Fuzzy logic based performance augmentation of MEMS gyro for mobile robots. Journal of Intelligent & Fuzzy Systems: Applications in Engineering and Technology. 2008; 19(6):393–398.

11.

Borenstein

Feng

. Gyrodometry: a new method for combining data from gyros and odometry in mobile robots. In: Robotics and Automation; 22–28 Apr 1996; Minneapolis, MN. IEEE; 1996. p. 423–428 vol.1. DOI: 10.1109/ROBOT.1996.503813.

12.

Borenstein

Ojeda

Kwanmuang

. Heuristic Reduction of Gyro Drift for Personnel Tracking Systems. Journal of Navigation. 2008; 62(1):41–58. DOI: http://dx.doi.org/10.1017/S0373463308005043.

13.

Borenstein

Ojeda

. Heuristic reduction of gyro drift in gyro-based vehicle tracking. SPIE Defense + Security. 2009; 7305. DOI: 10.1117/12.816918.

14.

Borenstein

Ojeda

. Heuristic reduction of gyro drift in vehicle tracking applications. Int. J. of Vehicle Autonomous Systems. 2009; 2(1/2):pp. 78–98.

15.

Qian

Xia

Liu

Peng

. Fuzzy Heuristic Reduction of Gyro Drift in Gyro-Based Mobile Robot Tracking. ICIECS ′09; 19–20 Dec. 2009; Wuhan. IEEE; 2009. p. 1–4. DOI: 10.1109/ICIECS. 2009. 5363297.

16.

Gao

Zhao

. An integrated land vehicle navigation system based on context awareness. in GPS Solutions. Springer. 2015. p. 1–16. DOI: 10.1007/S10291-015-0460-6.

17.

Zhang

Guo

Zhang

Song

L. M. M.

Zhang

. A navigation algorithm aided by multi-motion constraints of vehicle based on unified model. CGNCC; 8–10 Aug. 2014; Yantai. IEEE; 2014. p. 749–755. DOI: 10.1109/CGNCC.2014.7007305.

18.

MPU-6000 and MPU-6050 Product Specification Revision 3.4 [Lnternet]. Available from: http://www.invensense.com/products/motion-tracking/6-axis/mpu-6050/. Accessed: Sep 2015.

19.

Atmel AVR 8-bit and 32-bit Mcrocontrollers [Internet]. Available from: http://www.atmel.com/products/microcontrollers/avr/default.aspx?tab=documents. Accessed: Sep 2015.

20.

Motion Analysis. Specifications [Internet] Available from: http://www.motionanalysis.com/html/animation/specifications.html. Accessed: Sep 2015.