Contact-Aided State Estimation on Lie Groups for Legged Robot Mapping and Control

Abstract

Legged robots have the potential to transform the logistics and package delivery industries, become assistants in our homes, and aide in search and rescue. Although many wheeled and flying robots have begun to hit the market, useful walking robots have yet to become a practical reality due to challenging issues in controller design, motion planning, and state estimation. These challenges arise from high degrees of freedom, underactuation, and complex dynamics along with the unstructured nature of the environment in which we want these robots to operate. In particular, state estimation is a crucial component of any mobile robot system. To maintain stability, walking robots often require knowledge of orientation, velocity, joint angles, and local terrain information. Whereas, to plan and execute walking paths, awareness of global pose and map information is needed. Estimation of these states requires consistent fusion of measurements from a variety of sensors.

This thesis focuses on contact-aided state estimation techniques for legged robots. Inertial navigation systems can be used to obtain estimates of pose and velocity by integrating measurements from an inertial measurement unit. However, due to sensor noise and bias, these estimates will quickly drift away from their true values. To reduce or eliminate this drift, additional sensors, such as magnetometers and GPS, can be used to aid these inertial measurements. For legged robots, foot contact and forward kinematic measurements will perform this correction.

First, we develop a contact-aided invariant extended Kalman filter (InEKF) using the theory of Lie groups and invariant observer design. After modeling the robot's state on a Lie group, we show that the error dynamics follows a log-linear autonomous differential equation allowing the observable state variables to be rendered convergent with a domain of attraction that is independent of the system's trajectory. Unlike the standard EKF, neither the linearized error dynamics nor the linearized observation model depend on the current state estimate, which leads to improved convergence properties and a local observability matrix that is consistent with the underlying nonlinear system. Although the robot's global position and yaw remain unobservable after fusion of inertial, kinematic, and contact data, this filter can be executed at high frequencies to provide the feedback controller with real-time orientation and velocity data. Furthermore, since the position/yaw drift is slow, the pose estimate can be used to construct local terrain maps with the help of LiDAR sensors.

Next, we propose a method for contact-aided smoothing using factor graphs, which provide a flexible framework for fusing measurements from multiple sensors to obtain a maximum a posteriori estimate of the robot's entire trajectory. To extend this framework for legged robots, we developed two novel factors. The hybrid contact factor describes how a contact frame moves over time by preintegrating high-frequency inertial-contact data through an arbitrary number of contact switches, while the forward kinematic factor relates this contact frame to the robot's base frame using noisy encoder measurements. Taken together, these factors provide an independent leg odometry measurement that can be added into existing factor graphs to improve state estimation.

Finally, we conclude with ideas and future work on how these state estimates can be used along with gait libraries to design stabilizing feedback controllers as well as global motion planning techniques. All presented algorithms were verified through simulation and experiments results on an ATRIAS- or Cassie-series biped robot.