Learning Inertial Odometry for Dynamic Legged Robot State Estimation
Anonymous Author(s)
AfﬁliationAddress email
Abstract: This paper introduces a novel proprioceptive state estimator for leggedrobots based on a learned displacement measurement from IMU data.
Recent research in pedestrian tracking has shown that motion can be inferred from inertialdata using convolutional neural networks.
A learned inertial displacement measurement can improve state estimation in challenging scenarios where leg odometry is unreliable, such as slipping and compressible terrains.
Our work learns toestimate a displacement measurement from IMU data which is then fused withtraditional leg odometry.
Our approach greatly reduces the drift of proprioceptivestate estimation, which is critical for legged robots deployed in vision and lidardenied environments such as foggy sewers or dusty mines.
We compared resultsfrom an EKF and an incremental ﬁxed-lag factor graph estimator using data fromseveral real robot experiments crossing challenging terrains.
Our results show areduction of relative pose error by 37 % in challenging scenarios when comparedto a traditional kinematic-inertial estimator without learned measurement.
We alsodemonstrate a 22 % reduction in error when used with vision systems in visuallydegraded environments such as an underground mine.
Introduction
Recent advances in legged robot locomotion have motivated the use of quadruped robots in dull anddirty industrial applications.
To perform these tasks autonomously, accurate state estimation withlimited drift over long periods of time is fundamental.
However, operating in challenging environments such as mines and sewers, dust particles and vapor can severely degrade camera and lidarsystems [1] (Fig. 1).
Proprioceptive methods can work in these environments, as they fuse information only from joint kinematics and Inertial Measurement Unit (IMU) data [2, 3, 4].
However, slipevents or deformation of the robot’s foot or terrain introduce non-Gaussian error that accumulatesover time and leads to unbounded drift
Separately, recent advances in pedestrian tracking using only IMU data with machine learning haveshown promising results [5, 6].
The key insight of these works is that it is possible to learn a motionprior from inertial data which could be robust to changing biases.
We propose a deep neural network capable of learning a motion prior on the locomotion of a leggedrobot for any terrain and in challenging conditions such as slipping.
The network takes a buffer of IMU data and outputs a displacement measurement and covariance which is fused with kinematicinformation.
We demonstrate the application of this measurement in both ﬁltering and factor-graphoptimization contexts to track the motion of an ANYbotics ANYmal quadrupedal robot and compareresults to other learning-based inertial odometry systems [5].
The contributions in this paper can be listed as follows: 1) First kinematics-inertial estimator basedon learned IMU displacement measurements; 2) Integration with both a ﬁlter-based estimator anda factor graph, merging concepts from machine learning and model based methods; 3) Extensivetesting on the ANYmal quadruped platform, including application to ﬁeld experiments of the robotnavigating a mine.
Related Work
In this section, we brieﬂy summarize relevant work on proprioceptive state estimation for leggedrobots (Section 2.1) and related research in learned inertial state estimation (Section 2.2).
2.1 Kinematic-Inertial State Estimation in Legged Robots
Lin et al.
[7] presented the ﬁrst state estimator for legged robots which fused IMU and kinematic datain an Extended Kalman Filter (EKF).
Their method was highly dependent on the assumption thattheir hexapedal robot had always three feet in contact with the ground.
Bloesch et al.
[2] introduceda new ﬁltering approach called the Two-State Implicit Filter (TSIF).
This method employs residualbased modeling of the available information, eliminating the need for an explicit process model.
Thedefault state estimator for the ANYmal robot, which we use in our experiments, is based on TSIF.
More recently, Pronto [8] has been presented as an EKF-based state estimator with the capacity tofuse pose corrections from exteroceptive sensing.
This is done by maintaining a history of states,measurements and covariances, and applying corrections to the past trajectory asynchronously.
Factor graphs have also been used for legged robot state estimation.
Hartley et al.
[9] proposed ahybrid preintegrated contact factor so that multiple foot steps or measurements could be combinedinto a single factor, reducing the number of nodes in the graph.
Wisth et al.
[10] used factor graphsto fuse visual information with inertial and kinematics.
In [11], the same authors improved on thefactor graph formulation by adding feature tracking-based lidar measurements.
2.2 Learned Inertial Navigation
Recent research in inertial navigation systems (INS) has applied machine learning to infer motiondirectly from IMU data — typically focused on tracking a handheld mobile phone.
These methodshave shown impressive performance when compared to traditional inertial-only pedestrian trackingmethods. However, these methods are susceptible to failure if applied outside of the training domain(e.g. , a network trained with pedestrian motion would not generalize to a person on a bicycle).
Chen et al.
[6] introduced IoNet, the ﬁrst data driven INS which estimates odometry using deeprecurrent neural networks (RNNs).
The network was trained to relate buffered data from an IMU into2D displacements and orientation changes.
Yan et al.
[12] proposed RoNIN, a network architecturewhich similarly infers 2D velocity and orientation from raw IMU data.
The ﬁrst approach estimating 3D motion was proposed by Liu et al.
[5], who presented the TLIOﬁltering framework including learned motion estimates from batches of IMU signals.
An EKF wasused to ﬁlter the full 6 DoF state and process updates were performed by traditional IMU integration.
They trained a network with 40 h of pedestrian data to output displacement and covariance from thebuffered IMU data.
These measurements were provided to the EKF as relative position measurements and allowed the ﬁlter to correct for biases online.
Their approach showed impressive resultsfor IMU only navigation but was unable to integrate other sensors such as kinematics or vision.
Currently, the application of inertial learning to robotics is limited.
Brossard et al.
[13] demonstrated
IMU-only state estimation speciﬁcally for wheeled vehicles, heavily exploiting the constrained motion model.
Zhang et al.
[14] trained a network to learn the noise parameters of the IMU as opposedto the motion of the subject.
Their RNN generates rotation, velocity and acceleration terms withbias removed, however this makes integration with other frameworks such as ﬁltering more difﬁcult.
Problem Statement
Our objective is to estimate the state of a legged robot equipped with an IMU, joint sensors (encoders, torque sensors) and, optionally, exteroceptive sensors.
Following standard conventions, wedeﬁne a ﬁxed world frame W aligned with gravity, and two frames attached to the robot: the baseframe B and the IMU frame I .
The transformations between B and I are known.
The robot’s state atthe current time x ( t k ) = x k is deﬁned as follows:
[Equation]
and includes the rotation R WB ∈ SO(3) in world coordinates, the position p W WB ∈ R 3 , the linearvelocity v B WB , and the gyro and accelerometer IMU biases
b g I , b a I .
The IMU measures rotationalvelocity ˜ ω
I WI ∈ R 3 and the proper acceleration ˜ a I ∈ R 3 .
These are corrupted by zero-mean
Gaussian noise η g , η a and by slowly changing biases:
[Equation]
where g W is the gravity vector in world frame.
Without constraining measurements, the biases arenot observable and simple IMU integration will quickly lead to runaway error.
Learned Displacement Measurement
In this section, we describe our learning method including network architecture and loss functions.
Let I ij be a sequence of N consecutive IMU measurements collected from time t i to t j and gravityaligned.
From I ij , we want to predict the robot’s linear position increment ˆ d i ∈ R 3 between t i andt j .
To this end, we deﬁne a nonlinear function F θ that will be approximated by a neural network:
[Equation]
where ˆ u i = (ˆ u xi , ˆ u yi , ˆ u xi ) contains the logarithms of standard deviations from which the covarianceof the displacement ˆ Σ di ∈ R 3 × 3 can be computed:
[Equation]
Following an approach similar to TLIO [5], we approximate F θ with a 1D version of the ResNet18architecture [15].
We also make use of two loss functions, but in contrast to [5] we use a robust
Huber loss kernel to account for outliers in the training set.
4.1 Loss Function
For the ﬁrst 10 epochs, the loss function is deﬁned by the Mean Square Error (MSE):
[Equation]
where n is the number of training samples, ˆ d is the network output, and d is the ground truthdisplacement.
Once the network has begun to converge, the loss function is changed to a robustmodiﬁcation of the Gaussian Maximum Likelihood (GML), which allows the network to learn uncertainty (as done in [16]) but without being over inﬂuenced by outliers. If the MSE loss is notinitially used we ﬁnd the network does not converge.
The negative log-loss of the GML is given by:
[Equation]
(6) where H( (cid:15) k , ˆ Σ dk ) is the sum of the Huber loss h( · , · ) applied to each axis of (cid:15)
k = d k − ˆ d k :
H( (cid:15) k , Σ d k ) =
h( (cid:15) x k , σ x k ) + h
( (cid:15) yk , σ y k )
+ h( (cid:15)
z k , σ z k ) h( (cid:15), σ ) = (cid:26) 1 2 (cid:15) 2 ( σ 2 ) −if | (cid:15) | < δ, δ | (cid:15) | − 12 δotherwise (7) We treat each axis separately because, as shown in later sections, state estimation errors along thez -axis are often quite different from the errors on the xy -plane.
Our loss function resulted in 12%less error computing displacements in test experiments compared to [5].
State Estimation
The network outputs a relative position measurement and uncertainty which is suitable for incorporation into both ﬁltering and smoothing frameworks, as described in the following sections.
5.1 Filtering Estimator
We incorporate the learned displacement measurement into Pronto, an open-source modular leggedrobot estimation framework [8].
As Pronto maintains a history of states, measurements and covariances, it can easily incorporate relative position measurements.
The full system architecture isshown in Fig.
2: the IMU measurements from time t i to t j are accumulated and rotated to be gravityaligned at time t i using the orientation R i from the ﬁlter’s internal state.
Then, they are passed tothe network to estimate displacement and uncertainty.
These are then fused, using the covariance in Eq. 4, into the ﬁlter as a relative position measurement:
[Equation]
where the robot’s position p i at time i is taken from the ﬁlter’s history, while the displacement isfrom the network.
We call our proposed ﬁltering method EKF-IKD (EKF with inertial, kinematic, and learned displacement).
For all training and experiments, we used a buffer size of N = 400 ( 1 s of 400 Hz IMUdata).
Displacement measurements were estimated at 20 Hz , which are overlapping 1 .
5.2 Factor Graph Estimator
In this section, we describe a novel factor graph architecture that incorporates the learned inertialfactor.
In contrast to ﬁlters, factor graph methods re-optimize the full or partial history of states,given all the measurements connected to them, allowing for more accurate and smoother estimates.
Fig.
3 shows the proposed factor graph architecture.
When a new camera keyframe is available, anew node (white circle) is created.
Similarly to [17], we connect consecutive nodes x k and x k +1with preintegrated IMU and leg odometry factors (blue and orange factors), while visual features aremodeled as individual landmarks (gray circles) and factors (yellow factors).
In visually degradedenvironments, the visual landmarks might not be observable for a long period of time (e.g., fromnode x 3 in the example), so it is important to have robust proprioceptive odometry to fall back on.
To this end, we exploit the novel learned displacement factor to further constrain the robot motion(magenta line of Fig. 3).
We call the factor graph implementation of our method FG-VIKD (factorgraph with vision, IMU, kinematics and learned displacement).
The learned displacement measurement d i estimates the position of the robot between the start andend times of the buffer.
The learned factor residual r d can then be computed using ˆ Σ di from Eq. 4:
[Equation]
Fixed-lag optimization is done over the last 5 s of states each time a node is inserted to the graph. Since the IMU buffer is longer than the period between two keyframes, the nodes connected by thelearned factor are not consecutive and instead skip several nodes.
In the typical setup the cameracreates nodes at 20 Hz and the IMU buffer length is 1 s meaning the learned factor would connectx 0 to x 20 , x 1 to x 21 etc. (for simplicity, one every two nodes are connected in the toy example of
Fig. 3).
To provide a high frequency state estimate, we also integrate IMU measurements betweenoptimizations to provide a 400 Hz signal.
Lab Experiments
We performed several experiments with our ANYbotics ANYmal C100 robot as shown in Fig. 4.
The robot was teleoperated over different terrains in the lab: ﬂat ground, soft ground, a slipperysurface and an obstacle.
Ground truth poses were measured using a Vicon motion capture system.
Flat Ground First, we show performance on rigid, ﬂat terrain.
In these conditions, kinematic stateestimate is very accurate, however over long trajectories there is upward Z drift.
Soft Ground Deformable terrain poses a major challenge to legged robot state estimation as thespeciﬁc moment of contact is ill-deﬁned — the foot continues to move and compress the terrainafter a contact is detected.
This additional downward motion results in a hallucinated upward Zdrift.
Some realistic examples of deformable terrain include sand, mud or tall grass.
To reproducethis effect we placed several layers of foam padding on the ﬂoor.
Slipping When a robot slips, such as on wet or loose terrain, certain kinematic assumptions areviolated as the foot is in contact with the terrain but does not have zero velocity.
This results in largespikes in odometry error.
To create slip events in the lab, we placed a common ofﬁce white boardon the ﬂoor and covered it with lubricant (hand sanitizer).
Obstacle Terrain Blind climbing over an uneven obstacle course with ramps.
This experiment wasdesigned to show that our method can generalize to motions on non-planar surfaces.
6.1 Training
For the lab experiments, we collected ten walking sequences for each of the terrains totaling2 h16 min in 40 sequences.
Four sequences were selected as test experiments and the remainingwere split 75:25 into training and validation datasets.
We trained a single model from all the dataand used this same model in all experiments.
[Table] We used the Adam optimizer with an initial learning rate of 10 − 4 and a Huber delta of 1.35.
Traininglasted 200 epochs with the model minimizing validation error used in all lab experiments.
Trainingtook 6 hours on a laptop with an Nvidia Quadro P2000 GPU.
To collect training data we used the Vicon system for training reference.
During the training stepwe remove the biases from the raw IMU signal and inject back random bias values.
In order toestimate the true IMU biases, we create a factor graph where each node has a prior pose factor froma Vicon measurement.
IMU factors connect the nodes allowing the biases to be estimated in batchoptimization for all Vicon measurements.
6.2 Filtering Results
We present here results comparing ﬁltering-based proprioceptive estimator Pronto (which we hereafter refer to as EKF-IK) which is open-source and uses IMU and kinematics and our proposedmethod which incorporates IMU, kinematics and the learned displacement measurement EKF-IKD. We also show the trajectory provided by the default state estimator for the ANYmal robot which wecall TSIF and uses IMU and kinematics as well as the learning-based estimator TLIO.
We evaluated performance using two metrics: absolute pose error (APE) and relative pose error(RPE) – as deﬁned in [19].
APE measures the amount of drift over an entire experiment.
RPEmeasures the drift every X m traveled and is a more ﬁne grained measure – we used 5 m for labexperiments and 10 m for the mine experiment.
In all of the experiments, adding the learned measurement improved the state estimation signiﬁcantly.
As shown in Table 1, in comparison with
EKF-IK, the APE was reduced by 46 % on average ( 51 % on the most challenging terrain: slippingand soft) and the RPE was reduced by 22 % on average and 37 % on the challenging terrains.
Our proposed method out performs TLIO for every experiment which is expected as we use kinematic information.
The TSIF estimator outperforms our method in situations where the terrain isrigid and stable but is on par when slipping or on soft terrain.
We attribute this to the years of ﬁnetuning by the company on this estimator for this particular robot.
As such, we make no claims ofbeating the state-of-the-art, however we point to the signiﬁcant improvement gains to EKF-IK fromthe addition of the learned measurement.
Flat Ground
The results for the ﬁltering methods on ﬂat ground are shown on the left of Fig.
There is a clear upward Z drift for TSIF and EKF-IK which is signiﬁcantly reduced using the learnedmeasurement in EKF-IKD .
Soft Ground
The results for the ﬁltering methods on soft ground are shown on the right of Fig.
The periods where the robot walked on the soft terrain are highlighted in gray.
The upward Z driftfor TSIF and EKF-IK is more pronounced but this is reduced in EKF-IKD. Along the x axis the [Table] 0.24 SLIP.
0.17 SOFT 0. 0.23 OBST. performance is slightly worse indicating scale error introduced by the learned measurement butoverall error is greatly reduced as shown in the RPE plot. Slipping As shown in Fig.
5, periods when the robot is slipping have high error.
The addition ofthe learned measurement in EKF-IKD signiﬁcantly reduces spikes in error, particularly at 60 s .
Ourmethod does slightly better than TSIF in this experiment.
Obstacle
Terrain The obstacle terrain experiment demonstrates that our method can generalize tolocomotion on non-planar terrain.
The upper right plot of Fig.
5 shows that EKF-IKD reduces Zdrift while preserving the true changes in elevation as the robot climbs over the terrain.
In initialexperiments, we found that if the training dataset did not include enough examples of the robotwalking over elevated terrain, the network assumed constant planar elevation.
The resulting stateestimator would then ignore the elevation gain and loss from climbing on terrain.
6.3 Factor Graph Results
We present results comparing two factor graph-based proprioceptive estimators: FG-IKD whichis our proposed factor graph implementation without vision data (simulating camera failure) and FG-IK which is the same factor graph without vision or the learned displacement.
Since there is novision in these cases, nodes are created at a 40 Hz ﬁxed frequency in sync with the IMU.
We demonstrate that by adding the learned factor to this factor graph without any vision, APE wasreduced by 18 % and RPE by 7 % , as shown in Table 2.
The error in these factor graph methods ishigher than the ﬁltering methods because the robot state is intentionally estimated at a lower rate toallow visual data to be incorporated.
Additionally, without landmarks, FG-IK cannot take advantageof the graph structure.
Nevertheless, FG-IKD shows an improvement in proprioceptive estimationimplying if a robot’s vision system were to fail, our method would reduce drift.
Field Experiment: Underground Mine
While the learned displacement measurement can improve proprioceptive state estimation, most deployments of robots do use vision systems.
However, performance of these systems will degradein challenging environments such as underground mines.
In these cases, the learned displacementmeasurement can support the vision-based state estimation system. To demonstrate this, we performed a ﬁeld experiment in which we teleoperated our ANYmal robot through a decommissioned [Table]
limestone mine with large rooms and narrow passageways as shown in Fig. The ﬂoor is mostlyhard with ﬂat and sloped sections, but thick layers of dust and loose debris create slippery areas.
In the mine, the robot used the controller from [20] instead of the default controller. As a result, anew model was trained from lab data as described in Section 6.1, totaling 1 h23 min .
Training datafor the mine was entirely generated in the lab and the resulting model was applied successfully tothe ﬁeld experiment.
7.1 Mine Experiment Results
We compared a classical factor graph-based state estimator VILENS [10]; which uses vision, IMUand kinematics (and hereafter we refer to as FG-VIK); to our method FG-VIKD which uses the samesensors as well as the learned displacement factor.
In the mine, vision sensing is severely affected,forcing FG-VIK to rely on its proprioceptive sensing and, in these situations, the learned factor canhelp keep error low.
Our experiment lasted 20 minutes and over 472 m of walking.
Ground truth was estimated using the robot’s lidar to localize in a prior, high quality, point cloudscan of the area.
Table 3 shows that RPE was reduced by 22 % by adding the learned factor.
We alsoshow TLIO and EKF-IKD as a baselines although they do not use camera sensors.
The trajectory anderror over time are shown in Fig.
6 along with images from the front facing camera at times wherethe drift rate spiked.
These spikes coincide with periods of poor illumination and visual trackingfailure in the dark environment.
The robot lights reﬂected off nearby surfaces triggering the camerato reduce its exposure making the image darker.
The performance improvement would likely bereduced in radically different terrain such as sand or snow; however, this experiment demonstratesgeneralization from the artiﬁcial lab terrains to real-world environments.
Conclusion
In this paper, we introduced a learned displacement measurement derived from inertial sensingwhich can greatly reduce state estimation drift for legged robots in challenging situations.
Weshowed how this measurement can be used in both ﬁltering and factor graph frameworks, and thatthe greatest beneﬁts are when the robot slips or the terrain is deformable.
Extensive experimentswith the ANYmal quadruped robot showed a reduction of 37 % in relative pose error for ﬁlter-basedstate estimation.
We have also shown how the learned displaced measurement can reduce error in avision driven factor graph framework by 22 % in a underground mine.
A video demonstrating theseexperiments is provided as supplementary material.
For future work, inclusion of kinematic data into the network will be explored. Directly using thejoint states as input to the network would likely require a much more complex architecture that mightnot generalize to different robot models. To this end, we seek to incorporate derivative inputs fromthe robot model, such as ground reaction forces or foot velocities.