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

Embedded systems; Extended Kalman filter; FPGA; Pose estimation; Quaternion; Stewart platform; Inertial measurement unit; Low-cost sensors; Short periods; Simple geometries; Six degrees of freedom; Stewart platforms; Software; Control and Systems Engineering; Mechanical Engineering; Industrial and Manufacturing Engineering; Artificial Intelligence; Electrical and Electronic Engineering

Abstract :

[en] 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.

Disciplines :

Electrical & electronics engineering

Author, co-author :

Medeiros, Rodrigo Alves ^✱; School of Technology – Pontifical Catholic University of Rio Grande do Sul, PUCRS, Porto Alegre, Brazil

Araujo Pimentel, Guilherme ^✱; Université de Mons - UMONS > Faculté Polytechnique > Service Systèmes, Estimation, Commande et Optimisation ; Université de Mons - UMONS > Faculté Polytechnique > Service Systèmes, Estimation, Commande et Optimisation

Garibotti, Rafael ^✱; School of Technology – Pontifical Catholic University of Rio Grande do Sul, PUCRS, Porto Alegre, Brazil

^✱ These authors have contributed equally to this work.

Language :

English

Title :

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

Publication date :

May 2021

Journal title :

Journal of Intelligent and Robotic Systems

ISSN :

0921-0296

eISSN :

1573-0409

Publisher :

Springer Science and Business Media B.V.

Volume :

102

Issue :

Peer reviewed :

Peer Reviewed verified by ORBi

Additional URL :

https://link.springer.com/content/pdf/10.1007/s10846-021-01377-3.pdf

Research institute :

Research Institute for Energy

Funders :

Coordenação de Aperfeiçoamento de Pessoal de Nível Superior

Funding text :

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

Available on ORBi UMONS :

since 20 December 2022

Statistics

Number of views

2 (0 by UMONS)

Number of downloads

400 (0 by UMONS)

More statistics

Scopus citations^®

Scopus citations^®
without self-citations

OpenCitations

OpenAlex citations

Bibliography

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) DOI: 10.1109/TMECH.2017.2668604
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) DOI: 10.1109/TNNLS.2018.2803827
Markus, E.D., Agee, J.T., Jimoh, A.A.: Flat control of industrial robotic manipulators. Robot. Auton. Syst. 87(C), 226–236 (2017) DOI: 10.1016/j.robot.2016.10.009
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)
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) DOI: 10.1007/s11071-017-3683-y
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) DOI: 10.1016/j.cor.2016.09.017
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) DOI: 10.1016/j.ymssp.2017.11.002
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)
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)
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)
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)
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)
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) DOI: 10.1016/j.actaastro.2016.02.003
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) DOI: 10.1016/j.neucom.2016.10.006
Diebel, J.: Representing attitude: Euler angles, unit quaternions, and rotation vectors. Matrix 58(15), 1–35 (2006)
Debruin, J.: Control systems for mobile satcom antennas. IEEE Control. Syst. Mag. 28(1), 86–101 (2008) DOI: 10.1109/MCS.2007.910205
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)
Stewart, D.: A platform with six degrees of freedom. Inst. Mechan. Eng. 180(1), 371–386 (1965) DOI: 10.1243/PIME_PROC_1965_180_029_02
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) DOI: 10.1109/TBME.2011.2162512
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) DOI: 10.1088/0004-637X/694/2/1610
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)
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) DOI: 10.12989/sem.2014.49.5.645
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) DOI: 10.1109/TC.2015.2485202
Luces, M., Mills, J.K., Benhabib, B.: A review of redundant parallel kinematic mechanisms. J. Intell. Robot. Syst. 86, 75–198 (2017) DOI: 10.1007/s10846-016-0430-4
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) DOI: 10.1007/s10846-018-0850-4
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) DOI: 10.1007/s10846-018-0962-x
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) DOI: 10.1007/BF01257946
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)
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) DOI: 10.1007/s10846-016-0413-5
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)
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) DOI: 10.1109/TIE.2017.2652341
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) DOI: 10.1016/j.mechatronics.2009.06.009
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) DOI: 10.1109/TRO.2010.2050518
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)
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) DOI: 10.1109/TRO.2008.2003281
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) DOI: 10.1007/s10846-014-0092-z
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) DOI: 10.1109/TIP.2014.2380176
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) DOI: 10.1007/s10846-010-9490-z
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) DOI: 10.1109/TRO.2008.2004486
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) DOI: 10.1109/TIE.2014.2314051
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) DOI: 10.1016/j.conengprac.2017.03.006
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)
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)
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)
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) DOI: 10.1109/TIE.2012.2208436
NXP: LPC1800 Series MCUs. Available at https://www.nxp.com/docs/en/brochure/75017637.pdf (2015)
TI: OMAP4 mobile applications platform. Available at https://www.ti.com/lit/ml/swpt034b/swpt034b.pdf (2011)
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)
Olenick, R.P., Apostol, T.M., Goodstein, D.L.: The Mechanical Universe: Introduction to Mechanics and Heat. Cambridge University Press, Cambridge (2008)
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)
Welch, G., Bishop, G.: An introduction to the Kalman filter. In: ACM SIGGRAPH, pp. 19–34 (2001)
Dasgupta, B., Mruthyunjaya, T.: The stewart platform manipulator: A review. Mechan. Mach. Theor. 35(1), 15–40 (2000) DOI: 10.1016/S0094-114X(99)00006-3
Kucuk, S.: Serial and Parallel Robot Manipulators: Kinematics. Dynamics, Control and Optimization. IntechOpen (2012)
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)
Markley, F.L., Crassidis, J.L.: Fundamentals of spacecraft attitude determination and control, vol. 33. Springer, New York (2014)
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)
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) DOI: 10.1109/TCSII.2018.2860122
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) DOI: 10.1049/iet-cdt.2018.5136
Teague, M.R.: Image analysis via the general theory of moments. J. Opt. Soc. Am. 70(8), 920–930 (1980) DOI: 10.1364/JOSA.70.000920