ARTÍCULO ORIGINAL

State estimation technique for a planetary robotic rover

Técnica de estimación de estado para vehículos robóticos planetarios

Jamshed Iqbal^1,2* , Misbahur Rehman-Saad1, Ahsan Malik², Ahmad Mahmood- Tahir³

¹Department of Electrical Engineering and Automation, Aalto University. Otaniementie 17. C.P. 02150. Espoo, Finland.

²Department of Electrical Engineering, COMSATS Institute of Information Technology (CIIT). Park Road, Chak Shahzad. C.P. 44000. Islamabad, Pakistan.

³Postgraduate Programs Directorate, National University of Science and Technology (NUST). H-12. C.P. 44000. Islamabad, Pakistan.

^*Corresponding author: Jamshed Iqbal, e-mail: jamshed.iqbal@comsats.edu.pk

(Received October 28, 2013; accepted August 19, 2014)

Abstract

Given the long traverse times and severe environmental constraints on a planet like Mars, the only option feasible now is to observe and explore the planet through more sophisticated planetary rovers. To achieve increasingly ambitious mission objectives under such extreme conditions, the rovers must have autonomy. Increased autonomy, obviously, relies on the quality of estimates of rover's state i.e. its position and orientation relative to some starting frame of reference. This research presents a state estimation approach based on Extended Kalman Filter (EKF) to fuse distance from odometry and attitude from an Inertial Measurement Unit (IMU), thus mitigating the errors generated by the use of either system alone. To simulate a Sun-sensor based approach for absolute corrections, a magnetic compass was used to give absolute heading updates. The technique was implemented on MotherBot, a custom-designed skid steered rover. Experimental results validate the application of the presented estimation strategy. It showed an error range within 3% of the distance travelled as compared to about 8% error obtained from direct fusion.

Keywords:Mobile rovers, state estimation, planetary exploration, Extended Kalman Filter

Resumen

Dados los largos tiempos de recorrido y las severas limitaciones medioambientales en un planeta como Marte, ahora la única opción factible es observar y explorar el planeta a través de vehículos planetarios más sofisticados. Para lograr los objetivos de misiones cada vez más ambiciosas bajo tales condiciones extremas, los vehículos deben tener autonomía. Una mayor autonomía, obviamente, depende de la calidad de estimaciones del estado del vehículo, por ejemplo, su posición y su orientación relativa a un punto de referencia inicial. Esta investigación presenta un método de estimación de estado basado en el Filtro Extendido Kalman (EKF) para combinar la distancia de la odometría y la orientación dada por Unidad de Medida Inercial (IMU), y atenuando así los errores generados por el uso individual de cualquier de estos sistemas. Para simular un método basado en sensores solares para correcciones absolutas, se usó un compás magnético para dar datos absolutos de las actualizaciones de dirección. La técnica fue implementada en MotherBot, un vehículo especialmente diseñado con dirección deslizante. Los resultados experimentales validaron la aplicación de la estrategia de estimación propuesta. Se mostró un rango de error dentro del 3% de la distancia recorrida en comparación con un error aproximado del 8% obtenido de la combinación directa.

Palabras clave:Vehículos móviles, estimación de estado, exploración planetaria, Filtro Extendido de Kalman

Introduction

Recent trend because of technological advancements in robotics is substituting human with robots or helping humans to operate in environments where their access is not conveniently possible [1]. Human entry into most of the planets like Mars has not been yet possible. Consequently, autonomous robotic rovers are major sources of information about many planets. Challenges like navigation, localization and obstacle avoidance have been successfully addressed and overcome for structured indoor environment. However, more work needs to be done for outdoor environments to address the associated challenges and constraints imposed by such environments [2]. The task of designing a navigation system for a planetary rover can be divided into the following sub- tasks: Goal assignment, Path planning, State estimation/Localization and Hazard avoidance. State estimation is a critical part of navigation system for any autonomous rover, which gives knowledge of position and orientation of a rover during its traversal, relative to a certain frame of reference. This information is used both by the ground operators and onboard flight software to make numerous decisions and predictions regarding path planning, pointing of instruments, antennae and cameras, orientation of solar panels and to perform various safety checks on the system [3].

Many common navigational strategies used for state estimation of vehicles on the earth cannot be used in case of most planetary surfaces. Radio based navigation is not feasible for controlling a planetary vehicle. The planetary systems, in general, do not have a GPS system. Moreover, weak magnetic field in most of the planets makes magnetic sensor based approaches futile. The current need for higher autonomy on planetary rovers owing to massive time lags for communication also puts higher accuracy requirements on their state estimation capabilities. This in turn requires combining data from múltiple sensors. Hidalgo et al. presented a complete methodology to characterize and model sensors for aid in planetary exploration [4]. Fusing odometry data with INS can compensate the errors exhibited by these two types of sensors. The errors in odometry result due to wheel slippage, surface deformation or changes in radii of wheels. On the other hand, position estimation of a slow moving rover demands accelerometers of higher sensitivity, which exhibit considerable sensor noise.

The scientific literatee reports several navigational and localization strategies for mobile robots in general as well as specifically for planetary rovers. Azizi at al. determined position and orientation of a mobile robot based on odometry and a single axis gyroscope [5]. The approach, based on sensor fusion, has been implemented on Pioneer 2 robot. Thein et al. presented a Celestial Navigation (CelNav) approach for Lunar surface with an in-depth analysis of the relationship between position accuracy and noise of astronomical equipment [6] . Other approaches for celestial navigation are based on observation of sun, earth, and fixed stars and then using the altitude difference method [7], using star tracker to aid rover odometry [8] and using Qualitative Relational Mapping (QRM)[9] .

With a focus on Mars, the present research is aimed at combining INS and odometry signals to realize a mordí robust estimation system [10]. This paper first elaborates the custom- developed rover for this research and then presents pre 1 iminary results and analysis. Finally, it comments on conclusion and highlights future research avenues.

System Design

The custom-developed system to validate the proposed estimation strategy consists mainly of a mobile robotic rover and a dedicated laptop simulating the control station. The sensory system of the robot comprised of IMU and motor position encoders. The designed algorithm combines odometry and IMU data to estimate the rover's state.

Hardware

MotherBot, a simple track-driven tank-like rover realized by researchers of Automation Lab. of Aalto University to conduct planetary research has been used in this work. The robot tracks are actuated with DC motors. Power to the motors is provided by an onboard 12V battery. The rover can navigate in straight forward/backward direction as well as in place turns using proper switching of the motors. Figure 1a and b shows block diagram and actual hardware of Mother Bot.

The optical incremental encoders (HEDS 5600) have been used to estímate the distance travelled by the rover. The encoders have been serially interfaced with the laptop using an Atmel based data-acquisition circuit that delivers the right and left encoder counts along with the direction of motion of each wheel at a maximum frequency of 4Hz. Figure 2 shows the encoder placement on the rover and the data-acquisition circuit.

An automotive grade 3DM-G IMU by Microstrain, consisting oftri-axial magnetometers, gyroscopes and accelerometers along with high-resolution ADCs and DACs has been used. The IMU processor combines the high frequency response of the rate gyroscopes and the long-term low frequency response of the accelerometers and magnetometers. By processing the data in this way it can correct for gyroscope bias drifting and provide robust orientation measurement. In this work the data used is in the form of Euler angles, calculated referenced to a North-East-Down frame. The IMU is mounted at a height of about 1.5m above the ground surface and the trials with the rover have been performed outdoors in a car park to cope the variations due to magnetic field of the motors and surroundings.

Algorithm

Combining Odometry with gyro heading data resulted in non-linear equations. Thus the proposed strategy for estimation is based on EKF [11]. The nominal vehicle motion is described by straight drives or turn-in-place motions. The position updates of the rover are given by (1-2)

where x and y represent the rover position and ψ is the heading or Yaw angle referenced to the North aligned frame. μ_R and μ_L are the right and left wheel incremental distances respectively. To account for errors in odometry, a zero white Gaussian noise w is added to both x and y position and incremental wheel distance μ.

For IMU sensor, the attitude of the vehicle is defined by the Yaw/heading (Ψ), Roll (Ψ) and Pitch (Φ), which is updated as given by (3)

where the process noise w(κ) is assumed to be a random process that captures the uncertainty due to noise. Hence, the function f(.) giving the updated states at time step (k+1) from time step (κ) can be written as (4)

This can be simply rewritten as (5)

where w(κ) is the state noise vector presumed to be uncorrelated and zero-mean Gaussian with a constant variance Q(κ). The Jacobian matrix of partial derivatives of f(X(k), w(k)) w.r.t. X can be given by (6)

The measurements obtained from the encoders and IMU have been incorporated in the measurement model. The measurement vector for EKF is defined by function h(.) given by (7)

This can be simply rewritten as (8)

where the observation noise vector v(k) is presumed to be uncorrelated and zero-mean Gaussian with a constant variance R(k). The matrix H is the Jacobian matrix of partial derivatives of h(X(k), v(k)) w.r.t. X and is given by (9)

The Jacobian is used if the measurement model has non-linearity but since there is a direct mapping in our measurements and related states, so H takes form of (10)

For applying EKF, Q(k) and R(k) qualitatively represent the confidence placed in the estimated process states and measurements respectively and are given by (11-12)

The initial states of Q and R matrices are determined by (13-14)

For tuning the filter, two matrices called innovation matrices are recorded for each iteration. These are defined by (15-16)

where is an estimate of a priori state and is the estimation error covariance matrix. These can be computed using (17-18)

where X₀ and P₀ are initialized values.

Innovationl is a part of the prediction part of EKF and it utilizes the system and noise models. Whereas Innovation2 is a part of correction part of EKF and utilizes the measurement model. By comparing these two matrices, the difference between system and measurement models can be analyzed and minimized by adjusting the Q and R matrices. Hence, after each run of EKF algorithm, these innovation matrices are examined, and Q and R matrices are adjusted according to the following conditions given in (19-20)

Only the diagonal terms of the innovation matrices are compared with each other. The values of innovation matrices may change from reading to reading. Hence, mean of five to ten readings (n=5 to 10) of Innovation2 is taken when comparing the values. The mean values are then compared to the corresponding R values and first condition is applied. Once R value is obtained, then Innovation 1 is dependent on both R and Q and by applying second condition, a guess of Q value can be obtained. Several adjustment cycles are run in this way until the EKF converges. Once it converges, then suitable changes can be done as per requirements.

Results and Discussion

The presented approach has been implemented on MotherBot. Preliminary results include EKF for Roll, Pitch and Heading measurements and position tracking in case of multi-turn drive trajectory.

Odometry Calibration

For calibration of Odometry, two sketch markers were mounted on each side of the rover and rover was driven in a straight line and encoder ticks were recorded. The marked lines were measured with a measuring tape and ticks/cm for both tracks were then calculated, which were found to be 7.3821 ticks/cm for left track and 7.4125 ticks/cm for right track. Using these values, the average distance from the left and right ticks was calculated to get the theoretical distance travelled by the rover. This distance is then compared with the actual distance as given by the ground truth. Figure 3 shows the errors obtained as a %age of the distance travelled. The errors are random but not substantial. These results can be further used in Kalman filter to obtain better estimates for distance travelled.

Attitude Tracking

The system was initialized with the gyro stabilized values of Roll, Pitch and Yaw directly observed from IMU. These values in themselves have some starting errors, which were included in the initial error covariance matrix (P). Figure 2 shows the performance of the EKF in filtering out the noise from the attitude data while the rover is still stationary. The main motive behind filtering IMU data was to suppress the noise by keeping the deviations within the acceptable limit of 0.05°. As can be seen from figure 4, EKF effectively manages to keep the non-linearities within limit.

After initialization, the performance of the proposed approach to track the changes in attitude was investigated. For Roll measurements, a wooden block with height of 5cm was placed in the path of one of the tracks and Roll tracking of the system was analyzed. Figure 5a shows the corresponding results. The rover climbed over the block at around 150 time index and was over it till around 475 time index and then returned back to the ground. The initial Roll is due to the uneven terrain as tests were conducted outside in a parking space. EKF filtered out noise from straight drive and actively followed the change in Roll and smoothed out any initial disturbances that might have been introduced due to bumping. The accuracy of raw data was about 1.6% with noise getting changed while EKF gave an accuracy of 0.8% which was fairly accurate.

Similarly to analyze pitch measurements, a ramp was constructed and rover was driven over it. The rover was forced to stop at the top of ramp for some time and then brought back down from the same side. Figure 5b shows the results of Pitch tracking by IMU and EKF. EKF accuracy in this case was about 0.7% whereas raw data has accuracy of about 1.5% with too much noise to accurately predict it. The bumps can be observed on both sides of the waveform. This corresponds to the interval when the rover struck ramp and got down. These could be related to the rover's tread wheels contact surface area being large and horizontal; it could not smoothly follow the slope unless whole rover was on the ramp itself.

o analyze the Yaw angle measurements, the heading accuracy of IMU and EKF have been observed. The measurements were found to be quite accurate from IMU.EKF filtered out the noise and further enhanced the accuracy. The accuracy of raw data was about 0.9% while that of EKF was found to be 0.2%. This slight improvement of accuracy surely affects the position tracking in long run. Figure 6 shows the corresponding results.

Position Tracking

The position tracking involved combining the odometry data and IMU heading information in the developed EKF approach. These experiments demand a delicate tuning to converge EKF.

MotherBot, being a tracked rover, has a high contact area with ground and thus exhibits unsmooth turning. Thus, it tends to skid while turning and generally the position estimates from encoders are slightly more than the truth value. To account for this, the designed EKF suppresses the distance readings during turn phases, which are identified by change in Yaw/heading angles from IMU. In case of straight drives, the distance estimates are within 0.1% of distance travelled for 10m drive. Figure 7 shows a straight run of about 5m and the EKF position estimate. As can be seen that even for straight drives, there is a change in heading of the rover due to unbalanced wheels. The ground truth was obtained from measuring change in heading and distance travelled using measurement tapes.

Next experiment involves maneuvering the rover over a trajectory having turns. Figure 8(a and b) illustrates EKF performance compared to raw fusion for counter clockwise and clockwise turns respectively. The filter performance to suppress distance during turns can be seen clearly. At each turn, EKF suppressed the distance measurement which translated into change in xy position as compared to the raw data. From the lines being parallel almost, it can be observed that this change is not as much as heading change.

Figure 9 shows errors in xy axis in case of EKF estimated position and normal fusion for a typical run. As can be seen, errors are quite constant for straight runs but change significantly after every turn.

Conclusion

The aim of this research was to develop a state estimation system for a planetary rover. The presented approach is based on EKF based fusion of odometry and attitude data from IMU. An in- house developed track-driven skid-steered rover, MotherBot, was used to validate the strategy. Preliminary results demonstrated feasibility of the technique both in estimating attitude and position. Errors in attitude tracking were found to be within 1% as compared to errors of about 2% without EKF. Position tracking errors for the skid-steered rover was found to be within 3% using EKF based fusion. The presented technique, though having its limitations for slip prone areas, is a very simplistic, energy efficient and robust scheme which can be further enhanced by installing more sensors.

It is planned in near future to integrate the enhanced kinematic model with inertial measurements for irregular terrains. Moreover, implementation of the estimation strategy on more sophisticated planetary platforms ROSA (Robot for Scientific Applications) [12, 13] (shown in Figure 10) and Marsokhod [14]. The later being a wheeled rover, has a far better accuracy in odometry and this should further increase the performance of the estimation system. It is also planned to have some vision based strategy to compensate for odometry limitations in slip prone areas. Image acquisition through a vision system is anticipated to develop an alternate approach for navigation based on relational maps.

Acknowledgements

This research was sponsored by European Commission under Erasmus Mundus program ''SpaceMaster'' Round-I program. The authors would like to thank the commission for their generous support. Special thanks to Dr. Peter Jakubik for his technical guidance. Thanks to Dr. Jorma Selkainaho for help in Kalman filters. We would also like to extend gratitude to Mr. Tomi Ylikorpi for his cooperation during system testing and trials.

Acknowledgements

2. Khan, A., Riaz, S. & Iqbal, J. (2013). Surface estimation of a pedestrian walk for outdoor use of power wheelchair based robot. Life Sci J., 10(3), 1697-1704.         [ Links ]

3. Ali, K., Vanelli, C., Biesiadecki, J., Maimone, M., Cheng, Y., Martin, A. & Alexander, J. (2005). Attitude and position estimation on theMars exploration rovers. Proceedings of the IEEE International Conference on Systems, Man and Cybernetics. Hawaii, USA. 2005. pp. 20-27.         [ Links ]

4. J. Hidalgo, P. Pantelis, J. Kohler, J. Del-Cerro, A. Barrientes. ''Improving planetary rover attitude estimation via MEMS sensor characterization''. Sensors. Vol. 12. 2012. 2219-2235        [ Links ]

5. F. Azizi, N. Houshangi. Mobile robot position determination using data from gyro and odometry. Proceedings of the Canadian Conference on Electrical and Computer Engineering. Ontario, Canada 2004. pp. 719-722.         [ Links ]

6. M. Thein, D. Quinn, D. Folta. Celestial Navigation (CelNav): Lunar surface navigation. Proceedings of the AIAA/AAS Astrodynamics Specialist Conference and Exhibition. Hawaii, USA. 2008. DOI: 10.2514/6.2008-6759. Available on: http://arc.aiaa. org/doi/pdf/10.2514/6.2008-6759        [ Links ]

7. P. Benjamin. ''Celestial navigation on the surface of Mars''. NASA STI/Recon Technical Report. AD- A392455; USNA-1531-2. Vol. 1. 34 pages. 2001. Naval Academy. Annapolis, USA. Available on: http://www.dtic.mil/dtic/tr/fulltext/u2/a392455.pdf. Accessed: July, 2014.         [ Links ]

8. J. Gammell, C. Tong, P. Berczi, S. Anderson, T. Barfoot, J. Enright. Rover odometry aided by a star tracker. Proceedings of the IEEE Aerospace Conference. Montana, USA. 2013. pp. 1-10.         [ Links ]

9. M. McClelland, M. Campbell, T. Estlin. Qualitative relational mapping for planetary rovers. Proceedings of Workshop on Intelligent Robotic Systems AAAI. Washington, USA. 2013. pp. 110-113.         [ Links ]

10. M. Saad. State estimation technique for a planetary rover. Thesis Helsinki University of Technology. Espoo, Finland. 2007. pp 1-65. Available on online at http://autsys.aalto.fi/en/Publications/M34834. Accessed: July, 2014.         [ Links ]

11. E. Baumgartner, H. Aghazarian, A.Trebi. Rover localization results for the FIDO rover. Proceedings of the SPIE 4571, Sensor Fusion and Decentralized Control in Robotic Systems IV. Boston, USA. 2001. pp. 34-44.         [ Links ]

12. J. Iqbal, S. Heikkila, A. Halme. Tether tracking and control of ROSA robotic rover. Proceedings of IEEE International Conference on Control, Automation, Robotics and Vision. Hanoi, Vietnam. 2008. pp. 689-693.         [ Links ]

13. J. Iqbal. Tether tracking and control of ROSA robotic rover. Thesis Helsinki University of Technology. Espoo, Finland. 2007. pp. 1-93. Available on: at http:// autsys.aalto.fi/en/Publications/M34826 Accessed: October 27, 2013.         [ Links ]

14. J. Hakenberg. Mobility and autonomous reconfiguration of Marsokhod. Thesis Helsinki University of Technology, Espoo, Finland. 2008. pp. 1-58. Available on: http://autsys.aalto.fi/en/ Publications/M36440. Accessed: October 27, 2013.         [ Links ]

15. J. Iqbal, S. Riaz, A. Khan, H. Khan. ''A novel track- drive mobile robotic framework for conducting projects on robotics and control systems''. Life Sci .Vol. 10. 2013. pp. 130-137.         [ Links ]