4.1

Signal preprocessing

Some preprocessing is required to obtain useful measurement data. For Valkyrie, the angle of each leg joint is sensed by measuring the travel of its hydraulic actuator with an LVDT, and then computing a transformation through the joint linkage. This mapping does not account for flexion of the joint linkage when loaded or backlash when a joint changes direction. To account for these effects, we preprocess the joint angle measurements using the correction model, previously introduced by Koolen et al.² The model assumes linear compliance at the joint level,

where is the raw joint angle measurement, τ_i is the measured joint torque, and K_i is the joint-level stiffness. In practice, we limit the magnitude of the modification term, |τ_i/K_i|, to 0.1 rad. We used K_i = 10000 Nm/rad for all joints except the hip yaw joints, where we used K_i = 7000.²

For Valkyrie similar preprocessing carried out, with different correction terms which are a function of the mass properties of the robot.

4.2

Process model

The state of the floating base of the robot is described by

where ^bω_b ∈ ℝ³ and ^bv_b ∈ ℝ³ are the angular and linear velocities, ^wR_b is the floating-base rotation matrix, and ^wp_b = [p_x,p_y,p_z]^T is the base position. The angular and linear velocities are expressed in the base (pelvis) frame b, while the position and orientation of the pelvis are expressed in a fixed world frame w (as indicated by the leading superscripts above).

As the IMU provides accurate measurements of the angular velocity ^bω_b, we use the measured values directly and do not incorporate ^bω_b in the EKF state. In addition to the floating-base state, the EKF also estimates gyro and acceleration biases, denoted b_ω and b_a respectively. This leads us to define the global state of the EKF as the tuple

The fact that the rotation matrix ^wR_b is an overparameterization of orientation leads to constraints on the global state that cannot be handled in a standard EKF setting. To overcome this problem, we maintain orientation uncertainty in terms of a perturbation rotation in body frame represented by a minimal set of coordinates.

Omitting superscripts and subscripts and summarizing,³ the true orientation of the floating base is represented by the rotation matrix R. The estimated orientation, R̂, is related to it through

where χ ∈ ℝ³ is the rotation error in exponential coordinates relative to the body frame and R(χ) = e^χ̂ is the corresponding perturbation rotation matrix. Here, ·̂ denotes the skew-symmetric matrix such that ·̂ x = · × x for any x ∈ ℝ³, so that e^χ̂ is the rotation matrix corresponding to a rotation of |χ| about the axis defined by χ.

Table 2.

Contribution of various sensors to the filtered state estimate. Modes of integration found to be useful are marked ✓ and those not used here (for a variety of reasons) are indicated ✘.

4.2.1

Inputs

Similar to angular velocity, the acceleration ^ba_b is measured accurately by the IMU. We treat both ^bω_b and ^ba_b as inputs to the process model rather than measurements in the EKF - a common approach. Hence, the input vector for the EKF can be written as

Following the linearization derivations from,³ Euler integration is used to produce the a priori state and covariance.

4.3

Leg Kinematic Measurement model

As with many floating base state estimation algorithms,^4,5 our approach to using the leg kinematics assumes that the robot’s stance foot maintains stationary contact with the ground during part of the gait. This allows instantaneous velocity and position measurements of the robot’s floating base to be inferred via forward kinematics. Given the accuracy of the IMU orientation sensors, we choose to use joint sensing to measure only the position and velocity of the pelvis. Of course, perfectly clean and stable ground contact is seldom achieved in practice. However, we assert that for short periods (on the order of the sample time of our sensors) these assumptions are reasonable.

An estimate ^w p̂_b [k] of the position of the floating base at time step k can be computed using the current floating base orientation estimate in the world frame, ^wR_b [k], and the previous estimated position of the stationary foot, ^wp_f [k - 1],

where ^wp_b/f [k] denotes the position of the floating base with respect to the foot, which can be computed using the filtered leg joint positions and the floating base orientation estimate, ^wθ_b [k].

Two types of filter measurement could be formulated using this position estimate. The simplest approach would be to directly apply (6) as a position measurement within the EKF. However, because of the inaccuracy in joint sensing and because the robot’s foot does not always remain motionless after initial contact, we use the difference between consecutive position estimates over a short period of time to create a velocity measurement of the pelvis frame,

where h = 3 msec is the time step duration. This approach is more attractive because each resultant observation is a separate measurement of the robot’s velocity and does not accumulate a history of, for example, the effects of non-ideal ground contact or the footplate rolling or sliding. The influence of an erroneous velocity is transient and quickly corrected by subsequent observations. Using both measurement types together has been explored in related work,⁶ but we avoid doing so as it raises the possibility of creating inconsistencies, particularly when combined with position measurements derived from the LIDAR module (presented in the following section).

To use the leg kinematics for state estimation, it is necessary to determine which of the feet is most likely to be in stationary contact with the ground. We use a Schmitt trigger with a threshold of 575 N to classify contact forces sensed by the robot’s 3-axis foot force-torque sensors and detect whether either foot is in contact. A simple state machine then decides which foot is most reliably in contact and thus will provide the basis for kinematic measurements. The output of the foot contact classifier is demonstrated in the upper plot in Figure 2.

Figure 2.

Left: Evolution of foot force signals for the left (green) and right (red) foot during two steps. Lower plots show raw velocity, filter output, ground truth and error relative to VICON. Right: Localization accuracy for a variety of walking experiments. Position drift on left scale (in meters), yaw drift on right (in degrees). Error (versus ground truth) of the BDI state estimator (blue), our kinematic-only EKF (green), and LIDAR (red) estimators are shown.

Figure 3.

For LIDAR localization, the robot first collects a static point cloud of its environment which is used for subsequent localization. In our experiments we could localize the robot walking backward and forward over the illustrated walking course even though the robot had no rear facing sensors.

In the specific case of walking up stairs, the controller needs to use the toe of the trailing foot to push the robot forward and upward while the foot is not in stationary contact. While the design of the state estimator is almost entirely independent of the walking controller, as we increased the speed of locomotion we needed to feedback the active contact points from the controller to the state estimator in this situation.

We also classify other events in the gait cycle. Striking contact is determined when a force of 20-30N is maintained for more than 5msec. Breaking contact is determined when force falls below 275N. Because these impacts create unrealistic measurements, the EKF integrates these measurements with higher measurement covariance. We note that when the robot is in a double support stance, information from both legs could be leveraged to provide additional kinematic measurements. For simplicity we currently neglect this information.

Figure 2 contains a number of plots comparing (1) the raw pelvis velocity measurements inferred from kinematics with (2) the output of our integrating filter and (3) the velocity estimated from VICON motion capture. Using only raw leg position signals, the typical pelvis velocity standard deviation measured while standing stationary was 7.6 cm/sec. Adding joint-level filters reduced this to 2.3 cm/sec. Integrating filtered leg joint position into the EKF further reduced the error standard deviation to 1.4 cm/sec.

Finally, as mentioned in Section 4.3 the revolute joints states are filtered separately. The compliance model mentioned above is again used in preprocessing, before extended Kalman Filtering.

4.4

Yaw Bias Measurement Model

An additional challenge when developing a system for autonomy is Yaw Bias drift. The IMU gyroscopes contain bias resulting in an inability to directly measure rotation drift without directly sensing it.

To detect this bias we introducted a detector which used occasional observations of the robot being stationary to capture the orientation and to infer that all subsequent rotation is dur to the rotation bias. By slowly evolving this estimate, an accurate and stable rotation bias can be directly estimated and removed.

However, inherently this bias is much lower for the Atlas robot (with a tactical grade IMU) versus the MEMS IMU on the Valkyrie robot. We estimate that the former drifts at a rate 10 times less than the MEMS IMU.

6.1

Gaussian Particle Filter

We first adapted the approach originally proposed by Bry et al.³ using a Gaussian Particle Filter (GPF), which was presented in.⁸

In typical operation, the robot is first commanded to stand still for about 30 seconds while it collects a full 3D point cloud of its environment (see Figure 4). This cloud is then converted into a probabilistic occupancy grid (OctoMap)⁹ against which efficient localization comparisons later performed during locomotion. While the MAV experiments presented in³ required offline mapping with a separate sensor, our legged humanoid and actuated LIDAR with 30 m range permit the map to be constructed immediately prior to operation and immediately utilized while walking. Furthermore, if the robot were to approach the map boundary, on-line construction of a new map could easily be performed during operation.

Figure 4.

Left: Initially the point clouds are analysed to remove clutter. Right: Subsequently the resultant point clouds are aligned to a single reference cloud. This results in objects in the scene, such as a valve, being consistent across each observation. These results are from data collected in the DRC Finals by the MIT team.

Since the LIDAR is fundamentally a planar 2D sensor, only a subset of the state vector (namely x, y and yaw in the rotating sensor plane) is observable at any given instant. We therefore partition the full state vector into observable and unobservable sub-states, and use a GPF to incorporate each laser measurement over the observable variables. The particle filter samples are weighted according to the proposed sub-state likelihood, which is computed by comparing the LIDAR measurements, projected from the sub-state, to the prior map. From these weighted samples a mean and covariance, and in turn an equivalent Kalman measurement update for the full state vector are calculated resulting in a correction to the base position and yaw.

Given the competition environment of the DARPA Robotics Challenge and our inability to test in precisely the DRC conditions (outdoors with a large crowd of moving people), we typically operated without the LIDAR localization module because:

In Section 6.2 we discuss our future plans to improve this important module.

6.2

ICP-based Localization

So as to improve performance in dynamics situations our most recent approach uses Iterative Closest Point (ICP) to retain localization, with a number of significant modificaitons.

We analyze the effect of overlap variations on registration performance and demonstrate that where overlap varies, outlier filtering needs to be tuned accordingly. We define a novel parameter which gives a measure of this overlap. In this context, we propose a strategy for robust non-incremental registration. The pre-filtering module selects planar macro-features from the input clouds, discarding clutter. Outlier filtering is automatically tuned at run-time to allow registration to a common reference in conditions of non-uniform overlap.

In Figure 4 initial results are presented.