Abstract
Contact interactions are central to robot manipulation and locomotion behaviors. State estimation techniques that explicitly capture the dynamics of contact offer the potential to reduce estimation errors from unplanned contact events and improve closed-loop control performance. This is particularly true in highly dynamic situations where common simplifications like no-slip or quasi-static sliding are violated. Incorporating contact constraints requires care to address the numerical challenges associated with discontinuous dynamics, which make straightforward application of derivative-based techniques such as the Extended Kalman Filter impossible. In this paper, we derive an approximate maximum a posteriori estimator that can handle rigid body contact by explicitly imposing contact constraints in the observation update. We compare the performance of this estimator to an existing state-of-the-art Unscented Kalman Filter designed for estimation through contact and demonstrate the scalability of the approach by estimating the state of a 20-DOF bipedal robot in realtime.
This is a preview of subscription content, log in via an institution to check access.
Access this chapter
Tax calculation will be finalised at checkout
Purchases are for personal use only
Similar content being viewed by others
References
Wahba, G.: A least squares estimate of satellite attitude. SIAM Rev. 7, 409–409 (1965)
Simon, D.: Kalman filtering with state constraints: a survey of linear and nonlinear algorithms. IET Control Theory Appl. 4, 1303–1318 (2010)
Stewart, D.E., Trinkle, J.C.: An implicit time-stepping scheme for rigid body dynamics with inelastic collisions and coulomb friction. Int. J. Numer. Meth. Eng. 39(15), 2673–2691 (1996)
Lowrey, K., Dao, J., Todorov, E.: Real-time state estimation with whole-body multi-contact dynamics: a modified UKF approach. In: Proceedings of the IEEE-RAS International Conference on Humanoid Robots (2016)
Kajita, S., Kanehiro, F., Kaneko, K., Fujiwara, K., Yokoi, K., Hirukawa, H.: Biped walking pattern generation by a simple three-dimensional inverted pendulum model. Adv. Robot. 17, 131–147 (2003)
Stephens, B.J.: State estimation for force-controlled humanoid balance using simple models in the presence of modeling error. In: 2011 IEEE International Conference on Robotics and Automation, pp. 3994–3999, May 2011
Wang, S., Shi, Y., Wang, X., Jiang, Z., Yu, B.: State estimation for quadrupedal using linear inverted pendulum model. In: 2017 2nd International Conference on Advanced Robotics and Mechatronics (ICARM), pp. 13–18, August 2017
Bloesch, M., Hutter, M., Hoepflinger, M.A., Leutenegger, S., Gehring, C., Remy, C.D., Siegwart, R.: State estimation for legged robots - consistent fusion of leg kinematics and IMU (2012)
Rotella, N., Bloesch, M., Righetti, L., Schaal, S.: State estimation for a humanoid robot. In: 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 952–958, September 2014
Xinjilefu, X., Feng, S., Atkeson, C.G.: Dynamic state estimation using quadratic programming. In: 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 989–994, September 2014
Kuindersma, S., Deits, R., Fallon, M., Valenzuela, A., Dai, H., Permenter, F., Koolen, T., Marion, P., Tedrake, R.: Optimization-based locomotion planning, estimation, and control design for Atlas. Auton. Robots 40(3), 429–455 (2016)
Hartley, R., Mangelson, J., Gan, L., Jadidi, M.G., Walls, J.M., Eustice, R.M., Grizzle, J.W.: Legged robot state-estimation through combined forward kinematic and preintegrated contact factors. arXiv:1712.05873 [cs], December 2017
Singh, S.P.N., Waldron, K.J.: A hybrid motion model for aiding state estimation in dynamic quadrupedal locomotion. In: Proceedings of the 2007 IEEE International Conference on Robotics and Automation, pp. 4337–4342, April 2007
Nori, F., Kuppuswamy, N., Traversaro, S.: Simultaneous state and dynamics estimation in articulated structures. In: 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 3380–3386, September 2015
Koval, M.C., Pollard, N.S., Srinivasa, S.S.: Pose estimation for planar contact manipulation with manifold particle filters. Int. J. Robot. Res. 34, 922–945 (2015)
Koval, M.C., Klingensmith, M., Srinivasa, S.S., Pollard, N.S., Kaess, M.: The manifold particle filter for state estimation on high-dimensional implicit manifolds, pp. 4673–4680. IEEE, May 2017
Lowrey, K., Kolev, S., Tassa, Y., Erez, T., Todorov, E.: Physically-consistent sensor fusion in contact-rich behaviors. In: 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 1656–1662, September 2014
Baraff, D.: Fast contact force computation for nonpenetrating rigid bodies, pp. 23–34. ACM Press (1994)
Lemke, C.E., Howson Jr., J.T.: Equilibrium points of bimatrix games. J. Soc. Ind. Appl. Math. 12(2)
Ferris, M.C., Munson, T.S.: Interfaces to PATH 3.0: design, implementation and usage. Comput. Optim. Appl. 12, 207–227 (1999)
Anitescu, M., Potra, F.A.: Formulating dynamic multi-rigid-body contact problems with friction as solvable linear complementarity problems. Nonlinear Dyn. 14(3), 231–247 (1997)
Bullet Collision Detection & Physics Library. http://www.bulletphysics.org/
Smith, R.: Open dynamics engine (2007). http://www.ode.org/
DART: Dynamic animation and robotics toolkit. https://dartsim.github.io/
Gurobi Optimization, Inc.: Gurobi optimizer reference manual. Technical report (2014)
Tedrake, R., The Drake Development Team: Drake: a planning, control, and analysis toolbox for nonlinear dynamical systems (2016)
Yu, K.-T., Bauza, M., Fazeli, N., Rodriguez, A.: More than a million ways to be pushed: a high-fidelity experimental dataset of planar pushing. arXiv:1604.04038 [cs], April 2016
Fazeli, N., Zapolsky, S., Drumwright, E., Rodriguez, A.: Fundamental limitations in performance and interpretability of common planar rigid-body contact models. arXiv:1710.04979 [cs], October 2017
Fazeli, N., Kolbert, R., Tedrake, R., Rodriguez, A.: Parameter and contact force estimation of planar rigid-bodies undergoing frictional contact. Int. J. Robot. Res. 36, 1437–1454 (2017)
Acknowledgements
This work was supported by a Draper Internal Research and Development grant and the National Science Foundation (Grant Number IIS-1657186). Any opinion, findings, and conclusions or recommendations expressed in this material are those of the authors and do not necessarily reflect the views of the National Science Foundation.
Author information
Authors and Affiliations
Corresponding author
Editor information
Editors and Affiliations
Rights and permissions
Copyright information
© 2020 Springer Nature Switzerland AG
About this paper
Cite this paper
Varin, P., Kuindersma, S. (2020). A Constrained Kalman Filter for Rigid Body Systems with Frictional Contact. In: Morales, M., Tapia, L., Sánchez-Ante, G., Hutchinson, S. (eds) Algorithmic Foundations of Robotics XIII. WAFR 2018. Springer Proceedings in Advanced Robotics, vol 14. Springer, Cham. https://doi.org/10.1007/978-3-030-44051-0_28
Download citation
DOI: https://doi.org/10.1007/978-3-030-44051-0_28
Published:
Publisher Name: Springer, Cham
Print ISBN: 978-3-030-44050-3
Online ISBN: 978-3-030-44051-0
eBook Packages: Intelligent Technologies and RoboticsIntelligent Technologies and Robotics (R0)