This paper proposes an algorithm for combined contact detection and state estimation for legged robots. The proposed algorithm models the robot's movement as a switched system, in which different modes relate to different feet being in contact with the ground. The key element in the proposed algorithm is an interacting multiple-model Kalman filter, which identifies the currently-active mode defining contacts, while estimating the state. The rationale for the proposed estimation framework is that contacts (and contact forces) impact the robot's state and vice versa. This paper presents validation studies with a quadruped using (i) the high-fidelity simulator Gazebo for a comparison with ground truth values and a baseline estimator, and (ii) hardware experiments with the Unitree A1 robot. The simulation study shows that the proposed algorithm outperforms the baseline estimator, which does not simultaneous detect contacts. The hardware experiments showcase the applicability of the proposed algorithm and highlights the ability to detect contacts.
This work develops a learning-based contact estimator for legged robots that bypasses the need for physical sensors and takes multi-modal proprioceptive sensory data as input. Unlike vision-based state estimators, proprioceptive state estimators are agnostic to perceptually degraded situations such as dark or foggy scenes. While some robots are equipped with dedicated physical sensors to detect necessary contact data for state estimation, some robots do not have dedicated contact sensors, and the addition of such sensors is non-trivial without redesigning the hardware. The trained network can …
Legged Robot State Estimation Using Invariant Neural-Augmented Kalman Filter with a Neural Compensator
Mechatronics, Systems and Control Lab (MSC Lab)（机电系统与控制实验室）；Department of Mechanical Engineering（机械工程系）；Korea Advanced Institute of Science and Technology (KAIST)（韩国科学技术院）
University of Michigan（密歇根大学）；University of California, Berkeley（加州大学伯克利分校）
This paper proposes a state estimator for legged robots operating in slippery environments. An Invariant Extended Kalman Filter (InEKF) is implemented to fuse inertial and velocity measurements from a tracking camera and leg kinematic constraints. {\color{black}The misalignment between the camera and the robot-frame is also modeled thus enabling auto-calibration of camera pose.} The leg kinematics based velocity measurement is formulated as a right-invariant observation. Nonlinear observability analysis shows that other than the rotation around the gravity vector and the absolute position, all…
Robust State Estimation for Legged Robots with Dual Beta Kalman Filter
Tsinghua University（清华大学）；Peking University（北京大学）；SunRising AI Ltd（旭日人工智能有限公司）；College of Engineering, Peking University（北京大学工学院）；School of Vehicle and Mobility, Tsinghua University（清华大学车辆与运载学院）；State Key Laboratory of Intelligent Green Vehicle and Mobility, Tsinghua University（清华大学智能绿色车辆与交通全国重点实验室）；College of Artificial Intelligence, Tsinghua University（清华大学人工智能学院）
Robotic Systems Lab, ETH Zürich（苏黎世联邦理工学院机器人系统实验室）
We present a model predictive controller (MPC) that automatically discovers collision-free locomotion while simultaneously taking into account the system dynamics, friction constraints, and kinematic limitations. A relaxed barrier function is added to the optimization's cost function, leading to collision avoidance behavior without increasing the problem's computational complexity. Our holistic approach does not require any heuristics and enables legged robots to find whole-body motions in the presence of static and dynamic obstacles. We use a dynamically generated euclidean signed distance fi…
College of Engineering, University of Michigan（密歇根大学工程学院）
This paper derives a contact-aided inertial navigation observer for a 3D bipedal robot using the theory of invariant observer design. Aided inertial navigation is fundamentally a nonlinear observer design problem; thus, current solutions are based on approximations of the system dynamics, such as an Extended Kalman Filter (EKF), which uses a system's Jacobian linearization along the current best estimate of its trajectory. On the basis of the theory of invariant observer design by Barrau and Bonnabel, and in particular, the Invariant EKF (InEKF), we show that the error dynamics of the point co…
Arizona State University（亚利桑那州立大学）；Toyota Research Institute of North America（丰田北美研究院）
In this paper, we study Stochastic Control Barrier Functions (SCBFs) to enable the design of probabilistic safe real-time controllers in presence of uncertainties and based on noisy measurements. Our goal is to design controllers that bound the probability of a system failure in finite-time to a given desired value. To that end, we first estimate the system states from the noisy measurements using an Extended Kalman filter, and compute confidence intervals on the filtering errors. Then, we account for filtering errors and derive sufficient conditions on the control input based on the estimated…
Virginia Polytechnic Institute and State University（弗吉尼亚理工学院暨州立大学）
In this work, we present an aided inertial navigation system for an autonomous underwater vehicle (AUV) using an unscented Kalman filter on manifolds (UKF-M). The inertial navigation estimate is aided by a Doppler velocity log (DVL), depth sensor, acoustic range and, while on the surface, GPS. The sensor model for each navigation sensor on the AUV is explicitly described, including compensation for lever arm offsets between the IMU and each sensor. Additionally, an outlier rejection step is proposed to reject measurement outliers that would degrade navigation performance. The UKF-M for AUV nav…
Real-world measurement noise in applications like robotics is often correlated in time, but we typically assume i.i.d. Gaussian noise for filtering. We propose general Gaussian Processes as a non-parametric model for correlated measurement noise that is flexible enough to accurately reflect correlation in time, yet simple enough to enable efficient computation. We show that this model accurately reflects the measurement noise resulting from vision-based Simultaneous Localization and Mapping (SLAM), and argue that it provides a flexible means of modeling measurement noise for a wide variety of …
University of Massachusetts Lowell（马萨诸塞大学洛厄尔分校）；Purdue University（普渡大学）；University of Rhode Island（罗德岛大学）
State estimation for legged locomotion over a dynamic rigid surface (DRS), which is a rigid surface moving in the world frame (e.g., ships, aircraft, and trains), remains an under-explored problem. This paper introduces an invariant extended Kalman filter that estimates the robot's pose and velocity during DRS locomotion by using common sensors of legged robots (e.g., inertial measurement units (IMU), joint encoders, and RDB-D camera). A key feature of the filter lies in that it explicitly addresses the nonstationary surface-foot contact point and the hybrid robot behaviors. Another key featur…
Adaptive Model-Base Control of Quadrupeds via Online System Identification using Kalman Filter
Robotics Innovation Center, German Research Center for Artificial Intelligence (DFKI GmbH)（机器人创新中心，德国人工智能研究中心（DFKI GmbH））；AG Robotik, University of Bremen（机器人技术研究所，不莱梅大学）；Dynamics Division, Department of Mechanics & Maritime Sciences, Chalmers University of Technology（动力学研究所，机械与海洋科学系，查尔姆斯理工大学）
MINES ParisTech, PSL Research University（巴黎高等矿业学院，巴黎文理研究大学）；Safran Tech, Groupe Safran（赛峰科技，赛峰集团）
The present paper introduces a novel methodology for Unscented Kalman Filtering (UKF) on manifolds that extends previous work by the authors on UKF on Lie groups. Beyond filtering performance, the main interests of the approach are its versatility, as the method applies to numerous state estimation problems, and its simplicity of implementation for practitioners not being necessarily familiar with manifolds and Lie groups. We have developed the method on two independent open-source Python and Matlab frameworks we call UKF-M, for quickly implementing and testing the approach. The online reposit…
Many state estimation and control algorithms require knowledge of how probability distributions propagate through dynamical systems. However, despite hybrid dynamical systems becoming increasingly important in many fields, there has been little work on utilizing the knowledge of how probability distributions map through hybrid transitions. Here, we make use of a propagation law that employs the saltation matrix (a first-order update to the sensitivity equation) to create the Salted Kalman Filter (SKF), a natural extension of the Kalman Filter and Extended Kalman Filter to hybrid dynamical syst…
This paper considers the use of two position receivers and an inertial measurement unit (IMU) to estimate the position, velocity, and attitude of a rigid body, collectively called extended pose. The measurement model consisting of the position of one receiver and the relative position between the two receivers is left invariant, enabling the use of the invariant extended Kalman filter (IEKF) framework. The IEKF possesses various advantages over the standard multiplicative extended Kalman filter, such as state-estimate-independent Jacobians. Monte Carlo simulations demonstrate that the two-rece…
TIER IV, Inc.（TIER IV公司）；Massachusetts Institute of Technology（麻省理工学院）；NASA Jet Propulsion Lab, California Institute of Technology（加州理工学院NASA喷气推进实验室）；University of Michigan（密歇根大学）；The University of Tokyo（东京大学）
This paper develops a new nonlinear filter, called Moment-based Kalman Filter (MKF), using the exact moment propagation method. Existing state estimation methods use linearization techniques or sampling points to compute approximate values of moments. However, moment propagation of probability distributions of random variables through nonlinear process and measurement models play a key role in the development of state estimation and directly affects their performance. The proposed moment propagation procedure can compute exact moments for non-Gaussian as well as non-independent Gaussian random…
Zhejiang University（浙江大学）；The Chinese University of Hong Kong, Shenzhen（香港中文大学（深圳））；Southern University of Science and Technology（南方科技大学）
Pose estimation is important for robotic perception, path planning, etc. Robot poses can be modeled on matrix Lie groups and are usually estimated via filter-based methods. In this paper, we establish the closed-form formula for the error propagation for the Invariant extended Kalman filter (IEKF) in the presence of random noises and apply it to vision-aided inertial navigation. We evaluate our algorithm via numerical simulations and experiments on the OPENVINS platform. Both simulations and the experiments performed on the public EuRoC MAV datasets demonstrate that our algorithm outperforms s…
