Skip to main content
SpringerLink
Log in

An Embedded Quaternion-Based Extended Kalman Filter Pose Estimation for Six Degrees of Freedom Systems

  • Regular Paper
  • Published:
Journal of Intelligent & Robotic Systems Aims and scope Submit manuscript

Abstract

This paper proposes a formulation of quaternion-based Extended Kalman Filter pose estimation for six degrees of freedom systems embedded in an FPGA with commercial processors. Our approach uses the fusion of a camera and an inertial measurement unit to estimate simultaneously the position and the orientation of the system of interest. In addition, a Stewart platform is used to validate and evaluate the estimated pose. Although this work considers the use of common low-cost sensors and the use of markers with simple geometry, the results show excellent performance of the developed filter, being able to estimate the pose and orientation with an error below 8.14 mm and 0.63o̱, respectively. Furthermore, the effectiveness of the approach has also been evaluated, showing that the filter is able to converge quickly when the markers are retrieved after a loss of camera data for a short period of time.

This is a preview of subscription content, log in via an institution to check access.

Access this article

Log in via an institution

Price excludes VAT (USA)
Tax calculation will be finalised during checkout.

Instant access to the full article PDF.

Institutional subscriptions

Similar content being viewed by others

Availability of Data and Materials

The source code and raw data used in this work are available at https://github.com/rafaelgaribotti/JINT-D-20-00225.

References

  1. Mattila, J., Koivumäki, J., Caldwell, D.G., Semini, C.: A survey on control of hydraulic robotic manipulators with projection to future trends. IEEE/ASME Trans. Mechatron. 22(2), 669–680 (2017)

    Article  Google Scholar 

  2. Zhang, S., Dong, Y., Ouyang, Y., Yin, Z., Peng, K.: Adaptive neural control for robotic manipulators with output constraints and uncertainties. IEEE Trans. Neural Netw. Learn. Syst. 29(11), 5554–5564 (2018)

    Article  MathSciNet  Google Scholar 

  3. Markus, E.D., Agee, J.T., Jimoh, A.A.: Flat control of industrial robotic manipulators. Robot. Auton. Syst. 87(C), 226–236 (2017)

    Article  Google Scholar 

  4. Sharma, U., Rawal, V., Tayal, V.: An overview on multiple unmanned aerial vehicle control through single controller. Int. J. Comput. Appl. 180(49), 8–12 (2018)

    Google Scholar 

  5. Aboudonia, A., El-Badawy, A., Rashad, R.: Active anti-disturbance control of a quadrotor unmanned aerial vehicle using the command-filtering backstepping approach. Nonlinear Dynam. 90(1), 581–597 (2017)

    Article  Google Scholar 

  6. Xiang, X., Yu, C., Zhang, Q.: Robust fuzzy 3D path following for autonomous underwater vehicle subject to uncertainties. Comput. Oper. Res. 84(C), 165–177 (2017)

    Article  MathSciNet  Google Scholar 

  7. Cai, K., Tian, Y., Liu, X., Fatikow, S., Wang, F., Cui, L., Zhang, D., Shirinzadeh, B.: Modeling and controller design of a 6–DOF precision positioning system. Mech. Syst. Signal Process. 104, 536–555 (2018)

    Article  Google Scholar 

  8. Liu, M., Zhou, H., Pang, A.: Research on motion control system of 6–DOF robotic arm. In: International Conference on Modelling, Identification and Control (ICMIC), pp. 53–61 (2019)

  9. Najm, A.A., Ibraheem, I.K.: Nonlinear PID controller design for a 6–DOF UAV quadrotor system. Eng. Sci. Technol. Int. J. 22(4), 1087–1097 (2019)

    Google Scholar 

  10. Li, J., Bernardos, A.M., Tarrío, P, Casar, J.R.: A combined vision-inertial fusion approach for 6–DOF object pose estimation. In: International Conference on Machine Vision (ICMV), vol. 9445, pp 238–249 (2015)

  11. Xu, D., Li, Y.F.: A new pose estimation method based on inertial and visual sensors for autonomous robots. In: IEEE International Conference on Robotics and Biomimetics (ROBIO), pp. 405–410 (2007)

  12. Ma, T., Song, Z., Xiang, Z., Dai, J.S.: Trajectory tracking control for flexible-joint robot based on extended Kalman filter and PD control. Front. Neurorobot. 13(25), 1–10 (2019)

    Google Scholar 

  13. Dong, G., Zhu, Z.H.: Autonomous robotic capture of non-cooperative target by adaptive extended Kalman filter based visual servo. Acta Astronaut. 122, 209–218 (2016)

    Article  Google Scholar 

  14. Chung, J.H., Pak, J.M., Ahn, C.K., You, S.H., Lim, M.T., Song, M.K.: Particle filtering approach to membership function adjustment in fuzzy logic systems. Neurocomputing 237(10), 166–174 (2017)

    Article  Google Scholar 

  15. Diebel, J.: Representing attitude: Euler angles, unit quaternions, and rotation vectors. Matrix 58(15), 1–35 (2006)

    Google Scholar 

  16. Debruin, J.: Control systems for mobile satcom antennas. IEEE Control. Syst. Mag. 28(1), 86–101 (2008)

    Article  MathSciNet  Google Scholar 

  17. Cardona, M.: A new approach for the forward kinematics of general stewart-gough platforms. In: Proceedings of the 2015 IEEE Thirty Fifth Central American and Panama Convention (CONCAPAN XXXv) (2015)

  18. Stewart, D.: A platform with six degrees of freedom. Inst. Mechan. Eng. 180(1), 371–386 (1965)

    Article  Google Scholar 

  19. Kratchman, L.B., Blachon, G.S., Withrow, T.J., Balachandran, R., Labadie, R.F., Webster, R.J.: Design of a bone-attached parallel robot for percutaneous cochlear implantation. IEEE Trans. Biomed. Eng. 58(10), 2904–2910 (2011)

    Article  Google Scholar 

  20. Ho, P.T.P., Altamirano, P., Chang, C.H., Chang, S.H., Chang, S.W., Chen, C.C., Chen, K.J., Chen, M.T., Han, C.C., Ho, W.M., et al: The Yuan-Tseh Lee array for microwave background anisotropy. Astrophys. J. 694(2), 1610–1618 (2009)

    Article  Google Scholar 

  21. Mello, C.B.: Controle De Trajetória De Uma Plataforma Stewart Para Simulaċão De Transferência De Carga Fora De Porto. Master’s thesis, Federal University of Rio de Janeiro (2011)

  22. de Sousa, R.A., Ferreira, J., de Farias, J.S., Torrão, J, Afonso, D., Martins, M.: SPIF-A: On the development of a new concept of incremental forming machine. Struct. Eng. Mech. 49(5), 645–660 (2014)

    Article  Google Scholar 

  23. Garibotti, R., Butko, A., Ost, L., Gamatié, A, Sassatelli, G., Adeniyi-Jones, C.: Efficient embedded software migration towards clusterized distributed-memory architectures. IEEE Trans. Comput. 65(8), 2645–2651 (2016)

    Article  MathSciNet  Google Scholar 

  24. Luces, M., Mills, J.K., Benhabib, B.: A review of redundant parallel kinematic mechanisms. J. Intell. Robot. Syst. 86, 75–198 (2017)

    Article  Google Scholar 

  25. Navvabi, H., Markazi, A.H.D.: New afsmc method for nonlinear system with state-dependent uncertainty: Application to hexapod robot position control. J. Intell. Robot. Syst. 95, 61–75 (2019)

    Article  Google Scholar 

  26. M’hiri, S.A., Romdhane, N.M.B., Damak, T.: New forward kinematic model of parallel robot par4. J. Intell. Robot. Syst. 96, 283–295 (2019)

    Article  Google Scholar 

  27. Liu, K., Lewis, F., Lebret, G., Taylor, D.: The singularities and dynamics of a stewart platform manipulator. J. Intell. Robot. Syst. 8(3), 287–308 (1993)

    Article  Google Scholar 

  28. Dietmaier, P.: The stewart gough platform of general geometry can have 40 real postures. In: Proceedings of ARK, Strobl, Austria, June 29- July 4, pp 7–16 (1998)

  29. Coronado, E., Maya, M., Cardenas, A., Guarneros, O., Piovesan, D.: Vision-based control of a delta parallel robot via linear camera-space manipulation. J. Intell. Robot. Syst. 85, 93–106 (2017)

    Article  Google Scholar 

  30. Song, S., Kwon, D.: New direct kinematic formulation of 6 D.O.F stewart-gough platforms using the tetrahedron approach. Trans. Control Autom. Syst. Eng. 4(3), 217–223 (2002)

    Google Scholar 

  31. Kumar, P.R., Behera, A.K., Bandyopadhyay, B.: Robust finite-time tracking of stewart platform: a super-twisting like observer-based forward kinematics solution. IEEE Trans. Ind. Electron. 64(5), 3776–3785 (2017)

    Article  Google Scholar 

  32. Yang, C., He, J., Han, J., Liu, X.: Real-time state estimation for spatial six-degree-of-freedom linearly actuated parallel robots. Mechatronics 19(6), 1026–1033 (2009)

    Article  Google Scholar 

  33. Rendon-Mancha, J., Cardenas, A., Garcia, M., Gonzalez-Galvan, E., Lara, B.: Robot positioning usingcamera-space manipulation with a linear camera model. IEEE Trans. Robot. 26(4), 726–733 (2010)

    Article  Google Scholar 

  34. Zuo, A., Wu, Q.M.J., Gruver, W.A.: Stereo vision guided control of a stewart platform. In: Proceedings of the IEEE Internatinal Symposium on Intelligent Control, pp. 125–130 (2002)

  35. Yoon, Y., Kosaka, A., Kak, A.C.: A new kalman-filter-based framework for fast and accurate visual tracking of rigid objects. IEEE Trans. Robot. 24(5), 1238–1251 (2008)

    Article  Google Scholar 

  36. Araguás, G., Paz, C., Gaydou, D., Paina, G.P.: Quaternion-based orientation estimation fusing a camera and inertial sensors for a hovering UAV. J. Intell. Robot. Syst. 77, 37–53 (2015)

    Article  Google Scholar 

  37. Erdem, A.T., Ercan, A.O.: Fusing inertial sensor data in an extended kalman filter for 3d camera tracking. IEEE Trans. Image Process. 24(2), 538–548 (2015)

    Article  MathSciNet  Google Scholar 

  38. Nützi, G, Weiss, S., Scaramuzza, D., Siegwart, R.: Fusion of IMU and vision for absolute scale estimation in monocular. SLAM J. Intell. Robot. Syst. 61(1-4), 287–299 (2011)

    Article  Google Scholar 

  39. Mirzaei, F.M., Roumeliotis, S.I.: A Kalman filter-based algorithm for IMU-camera calibration: Observability analysis and performance evaluation. IEEE Trans. Robot. 24(5), 1143–1156 (2008)

    Article  Google Scholar 

  40. Du, G., Zhang, P.: Online serial manipulator calibration based on multisensory process via extended kalman and particle filters. IEEE Trans. Ind. Electron. 61(12), 6852–6859 (2014)

    Article  Google Scholar 

  41. Miletović, I., Pool, D., Stroosma, O., van Paassen, M., Chu, Q.: Improved stewart platform state estimation using inertial and actuator position measurements. Control. Eng. Pract. 62, 102–115 (2017)

    Article  Google Scholar 

  42. Alaimo, A., Artale, V., Milazzo, C., Ricciardello, A.: Comparison between euler and quaternion parametrization in uav dynamics. In: 11th International Conference of Numerical Analysis and Applied Mathematics 2013: ICNAAM 2013. AIP Conference Proceedings (2013)

  43. Lee, C., Salcic, Z.: A fully-hardware-type maximum-parallel architecture for Kalman tracking filter in FPGAs. In: International Conference on Information, Communications and Signal Processing (ICICS), vol. 2, pp 1243–1247 (1997)

  44. Bonato, V., Peron, R., Wolf, D.F., de Holanda, J.A.M., Marques, E., Cardoso, J.M.P.: An FPGA implementation for a kalman filter with application to mobile robotics. In: International Symposium on Industrial Embedded Systems (SIES), pp. 148–155 (2007)

  45. Guo, H., Chen, H., Xu, F., Wang, F., Lu, G.: Implementation of EKF for vehicle velocities estimation on FPGA. IEEE Trans. Ind. Electron. 60(9), 3823–3835 (2012)

    Article  Google Scholar 

  46. NXP: LPC1800 Series MCUs. Available at https://www.nxp.com/docs/en/brochure/75017637.pdf (2015)

  47. TI: OMAP4 mobile applications platform. Available at https://www.ti.com/lit/ml/swpt034b/swpt034b.pdf (2011)

  48. Garibotti, R., Reagen, B., Shao, Y.S., Wei, G.Y., Brooks, D.: Using dynamic dependence analysis to improve the quality of high-level synthesis designs. In: IEEE International Symposium on Circuits and Systems (ISCAS), p. 1–4 (2017)

  49. Olenick, R.P., Apostol, T.M., Goodstein, D.L.: The Mechanical Universe: Introduction to Mechanics and Heat. Cambridge University Press, Cambridge (2008)

    MATH  Google Scholar 

  50. Vasquez, X.M.Z.: Posicionamento De Múltiplos Objetos a Partir De Visão Estéreo. Master’s thesis, Pontifical Catholic University of Rio Grande do Sul (2015)

  51. Welch, G., Bishop, G.: An introduction to the Kalman filter. In: ACM SIGGRAPH, pp. 19–34 (2001)

  52. Dasgupta, B., Mruthyunjaya, T.: The stewart platform manipulator: A review. Mechan. Mach. Theor. 35(1), 15–40 (2000)

    Article  MathSciNet  Google Scholar 

  53. Kucuk, S.: Serial and Parallel Robot Manipulators: Kinematics. Dynamics, Control and Optimization. IntechOpen (2012)

  54. de Faria, P.F.S.R., Castro, R.S., Salton, A.T., Flores, J.V.: Quaternion-based dynamic control of a 6–DOF stewart platform for periodic disturbance rejection. In: IEEE Conference on Control Applications (CCA), pp. 1191–1196 (2016)

  55. Markley, F.L., Crassidis, J.L.: Fundamentals of spacecraft attitude determination and control, vol. 33. Springer, New York (2014)

  56. Marins, J.L., Yun, Xiaoping, Bachmann, E.R., McGhee, R.B., Zyda, M.J.: An extended Kalman filter for quaternion-based orientation estimation using MARG sensors. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), vol. 4, pp 2003–2011 (2001)

  57. Garibotti, R., Reagen, B., Shao, Y.S., Wei, G.Y., Brooks, D.: Assisting high-level synthesis improve spmv benchmark through dynamic dependence analysis. IEEE Trans. Circ. Systems II Express Briefs 65(10), 1440–1444 (2018)

    Article  Google Scholar 

  58. Garibotti, R., Ost, L., Butko, A., Reis, R., Gamatié, A., Sassatelli, G.: Exploiting memory allocations in clusterised many-core architectures. IET Comput. Digit. Techniq. 13(4), 302–311 (2019)

    Article  Google Scholar 

  59. Teague, M.R.: Image analysis via the general theory of moments. J. Opt. Soc. Am. 70(8), 920–930 (1980)

    Article  MathSciNet  Google Scholar 

Download references

Funding

This study was financed in part by the Coordenação de Aperfeiçoamento de Pessoal de Nível Superior - Brasil (CAPES) - Finance Code 001.

Author information

Authors and Affiliations

  1. School of Technology – Pontifical Catholic University of Rio Grande do Sul, PUCRS, Av. Ipiranga, 6681, Porto Alegre, Brazil

    Rodrigo Alves Medeiros, Guilherme Araujo Pimentel & Rafael Garibotti

  2. Systems, Estimation, Control and Optimization Department, University of Mons, UMONS, 31 Boulevard Dolez, Mons, Belgium

    Guilherme Araujo Pimentel

Authors
  1. Rodrigo Alves Medeiros
    View author publications

    You can also search for this author in PubMed Google Scholar

  2. Guilherme Araujo Pimentel
    View author publications

    You can also search for this author in PubMed Google Scholar

  3. Rafael Garibotti
    View author publications

    You can also search for this author in PubMed Google Scholar

Contributions

All authors contributed to the study and development of this work, with the following responsible for the sections: Introduction: Rodrigo Medeiros and Rafael Garibotti; Literature Review: all authors; Background: Rodrigo Medeiros and Guilherme Pimentel; Algorithm’s implementation: Rodrigo Medeiros; Development flow: all authors; Experimental setup and data collection: Rodrigo Medeiros; Analysis and discussion of results: all authors; Writing - original draft preparation: Rodrigo Medeiros and Rafael Garibotti; Writing - review and editing: all authors; Supervision: Rafael Garibotti and Guilherme Pimentel.

Corresponding author

Correspondence to Rafael Garibotti.

Ethics declarations

Conflict of Interests

The authors declare that in addition to funding, there is no conflict of interest or competing interests.

Additional information

Publisher’s Note

Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.

Rights and permissions

Reprints and permissions

About this article

Check for updates. Verify currency and authenticity via CrossMark

Cite this article

Medeiros, R.A., Pimentel, G.A. & Garibotti, R. An Embedded Quaternion-Based Extended Kalman Filter Pose Estimation for Six Degrees of Freedom Systems. J Intell Robot Syst 102, 18 (2021). https://doi.org/10.1007/s10846-021-01377-3

Download citation

  • Received:

  • Accepted:

  • Published:

  • DOI: https://doi.org/10.1007/s10846-021-01377-3

Keywords

Search

Navigation