Deep Multi-Modal Contact Estimation for Invariant Observer Design on Quadruped Robots
Abstract: This work reports on developing a deep learning-based contact estimator for legged robots that bypasses the need for physical contact sensors andtakes multi-modal proprioceptive sensory data from joint encoders, kinematics,and an inertial measurement unit as input.
Unlike vision-based state estimators,proprioceptive state estimators are agnostic to perceptually degraded situationssuch as dark or foggy scenes.
For legged robots, reliable kinematics and contactdata are necessary to develop a proprioceptive state estimator.
While some robotsare equipped with dedicated contact sensors or springs to detect contact, somerobots do not have dedicated contact sensors, and the addition of such sensors isnon-trivial without redesigning the hardware.
The trained deep network can accurately estimate contacts on different terrains and robot gaits and is deployed alonga contact-aided invariant extended Kalman ﬁlter to generate odometry trajectories.
The ﬁlter performs comparably to a state-of-the-art visual SLAM system.
Introduction
Legged robots can traverse uneven terrains.
This unique capability gives legged robots the potentialto conduct scientiﬁc exploration in extreme environments, execute rescue missions in hazardousscenes and can help humans in everyday tasks [1].
To accomplish these goals, knowledge of therobot’s current state is necessary.
While joint angles can be directly measured using encoders,the robot’s pose and velocity require additional measurements and a mathematically sound datafusion method.
Methods for state estimation rely on visual and proprioceptive information.
Visionbased methods take advantage of the rich information provided by cameras and LiDARs.
However,these methods might suffer from failure due to illumination changes or extreme environments suchas snowstorms or foggy scenes [2].
On the other hand, proprioceptive state estimators take onlyinternal measurements and are thus agnostic to perceptually degraded situations.
Moreover, unlikevision sensors that typically operate with low frequency (e.g., 10 − 60 Hz ), proprioceptive sensorsusually operate at a high update rate (e.g., 100 − 2000 Hz ).
This high-frequency information providesodometry estimates for localization and mapping tasks [3, 4, 5] and it is also crucial for controllersand planners to maintain stability and execute planned policies [6]. The former application is thefocus of this work.
Proprioceptive state estimators often fuse the measurements from an Inertial Measurement Unit(IMU) with leg odometry.
Leg odometry, in particular, uses kinematics and contact data to updatethe state.
Reliable measurements of kinematic and contact information become critical in this case.
Not all legged robots are equipped with dedicated contact sensors or springs to detect contact [7, 8].
The addition of dedicated contact sensors is non-trivial and often leads to a redesign of the hardware.
In this paper, we develop a deep learning-based contact estimator that does not require dedicated sensors and instead uses joint encoders, kinematics, and IMU data.
We create contact data sets usingan MIT Mini Cheetah robot [8] on eight different terrains.
The trained network can correctly estimate contact events across different terrains and robot gaits.
We further deploy the contact estimatoralong with a contact-aided invariant extended Kalman ﬁlter (InEKF) [9] and show that the resultingodometry trajectory is comparable to a state-of-the-art visual SLAM algorithm (used as a proxy for ground truth).
The contributions of this work are as follows: 1) Open-source quadruped contactdata sets recorded using an MIT Mini Cheetah, 2)
A lightweight multi-modal deep learning-basedcontact estimator that mitigates the need for physical contact sensors and gives reliable estimationson different terrains with different robot gaits, 3) A quadruped version of the contact-aided invariant
EKF compatible with Lightweight Communications and Marshalling (LCM) [10, 11] interface.
Thedata sets and software will be available for download after receiving the ﬁnal decision, and 4) Experimental results for contact-aided state estimation using an MIT Mini Cheetah on eight differentterrains.
Related Work
A thorough literature review on robot state estimation, including legged robots and contact-aidedapproaches, is available by Hartley et al.
In this section, we focus on model-based and datadriven methods for contact estimation.
Model-Based Contact Estimation.
Model-based approaches utilize physics models of the robotto estimate foot contacts.
Traditionally, researchers segment the touchdown event of robot legs orprosthetic legs by thresholding on the estimated Ground Reaction Force (GRF) from the generalequation of motion [12, 13, 14].
Although this method can detect touchdown events, the estimated
GRF is often noisy and unreliable.
De Luca et al.
[15], Haddadin et al.
[16] proposed a Generalized Momentum (GM) method for detecting contact events on robot manipulators.
This GM-basedmethod is, in fact, a ﬁltered version of the work of Focchi et al.
Although GM-based methodsmitigate the noise problem in GRF estimation, an empirical threshold on the cut-off frequency isstill required.
Hwangbo et al.
[17] ﬁrst introduces a probabilistic representation of the contact state and uses amethod similar to a Hidden Markov Model (HMM) to fuse the dynamics and kinematics for contactestimation.
They adopt a Monte-Carlo sampling algorithm to compute the transition model andverify their method against GM-based methods.
Jenelten et al.
[18] expands the HMM method andfocuses on slippage detection.
They demonstrate ANYmal [19], a quadruped robot, walking stablyon slippery ground with their approach.
The above two methods aim to detect contact as early aspossible for the controller to maintain stability; however, we aim to detect reliable contacts for stateestimation on various terrains.
Since the mentioned works do not evaluate using a state estimator, itis unclear how their methods assist state estimation.
Bledt et al.
[20] leverages both the GM-based methods and the probabilistic representation of contactstates.
They use a Kalman ﬁlter to fuse the gait phase scheduler information from the controller withthe GRF estimated using the GM method.
They demonstrate that this Kalman ﬁltering approachcan correctly estimate contacts and help the controller reduce the bouncing event upon touchdown.
However, this method assumes the leg phase to be periodic as it uses the gait scheduler informationin the prediction step of the Kalman ﬁlter.
It could experience a loss in performance when the phaseis heavily violated.
Data-Driven Contact Estimation.
Data-driven approaches take advantage of the rapid development of recent machine learning techniques.
Camurri et al.
[21] uses logistic regression to learn the
GRF threshold for contact detection.
This work compares against heuristic-based thresholding on
GRF using a base state estimator.
The result shows that the logistic regression classiﬁer can doublethe performance of the state estimator.
However, compared to deep learning methods, the performance of a logistic regression classiﬁer gets saturated as the number of data increases [22, 23].
Thismethod requires a speciﬁc training procedure for different gait, loading conditions of the robot, andindividual terrain properties.
Rotella et al.
[24] uses a fuzzy C-means clustering for the probability of contacts in all six endeffector degrees of freedom.
They integrate the contact estimator with a base state estimator andshow their approach performs considerably better than implementations that are purely based onmeasured normal force.
However, this method assumes contact wrench sensors and additional IMUare available at each end-effector.
Furthermore, this method was only tested in simulation.
Its performance on real robots remains unknown.
Piperakis et al.
[25] proposes an unsupervised learningmethod for humanoid gait phase estimation.
The authors employ Gaussian Mixture Models (GMMs)for clustering and show the accuracy by comparing to the ground-truth data and leg odometry.
However, this work also assumes the availability of wrench/force sensors at each end-effector, and theclustering result is affected by the gait and data density.
The above methods either assume the availability of wrench/force sensors or are restricted by thenature of simple regression and are thus unable to generalize to different scenarios.
In contrast, ourwork proposes a multi-modal deep learning-based contact estimator that does not require contactsensors and can generalize well to different gaits and terrain properties.
Moreover, as more databecomes available, the network performance can be improved.
Background on Contact-Aided Invariant Extended Kalman Filtering
We adopt the contact-aided invariant Kalman ﬁltering approach [9] that is well-suited for leggedrobot state estimation problems.
State Representation.
We wish to estimate the orientation, velocity, and position of the body (IMU)frame in the world frame at any time stamp t , which are represented as R t ∈ SO(3) , v t ∈ R 3 , andp t ∈ R 3 , respectively.
We deﬁne the state X t ∈ SE L +2 (3) as X t :=
( R t , v t , p t , d lt ) , whered lt ∈ R 3 is the position of a contact foot in the world frame.
When a contact event is detected forleg l , d lt is calculated via the forward kinematics and the current body state, and then augmentedinto the state to enforce the no-slip (zero-velocity) condition.
Similarly, if the contact constraintbreaks, the associated contact position d lt will be marginalized (removed) from the state to allowfoot movement.
The matrix Lie group, SE L +2 (3) is an extension of SE(3) and it was introducedby Barrau [26].
When a new contact event is detected, the position of the corresponding foot is computed using¯ d t = ¯ p t + ¯ R t h p
(˜ α t ) , where h p (˜ α t ) is the foot position relative to the body frame computed bythe forward kinematics (the bar notation denotes estimated variables).
The number L indicates thenumber of legs that is currently having contact with the ground (here L ≤ 4 ).
Process Model.
We assume the IMU measurements, i.e., angular velocity and linear acceleration,are corrupted by white Gaussian noise:
[Equation]
where GP represents a Gaussian process and δ ( t − t ′ ) is the Dirac delta function.
To handle smallfoot slippage, we model the contact velocity noise in the body frame via a white Gaussian noise [27]w vt ∼ GP (0 3 , 1 , Σ v δ ( t − t ′ )) .
The process model of the individual term in the state becomes:
[Equation]
where ( · ) × denotes a 3 × 3 skew symmetric matrix, g is the gravity vector, ˜
α t is the encoder measurements, and h R (˜ α t ) is the orientation of the contact frame in IMU (body) frame calculated byforward kinematics.
Measurement Model.
In the ﬁlter update step, we use the contact constraint calculated earlier alongwith forward kinematics from current encoder measurements to correct the state.
We assume themeasurements from joint encoders are corrupted by white Gaussian noise: ˜ α t = α + w αt , w αt ∼ 128 N (0 3 , 1 , Σ α δ ( t − t ′ )) .
The assumption that the contact point is ﬁxed in the world frame during a contact event leads to h p (˜ α t ) =
R T t ( d t − p t )
+ J p
(˜ α t ) w αt , where J p ( ˜ α t ) is the analytical Jacobian of the forward kinematics function.
Given this measurement model, we use the right-invariantcorrection model described by Hartley et al.
Implementation on Mini Cheetah.
The implementation by Hartley et al.
[9] is generalizable andopen-sourced.
In this work, we modify the open-sourced software for the MIT Mini Cheetah [8]by integrating the forward kinematics functions generated using FROST library [28].
An LCMinterface is created for the ﬁlter to communicate with the robot.
Furthermore, because the Mini
Cheetah is not equipped with contact sensors, we develop a contact estimation framework to detectthe contact events for the robot, as detailed in the next section.
Contact Estimation
In this section, we will discuss our deep learning approach for contact estimation.
We deﬁne ourcontact state for each leg l ∈ { RF, LF, RH, LH } as C =
[ c RF c LF c RH c LH ] and c l ∈ 141 { 0 , 1 } .
Here, 0 denotes no contact, and 1 indicates a ﬁrm contact with the ground.
Depending onthe robot’s motion, the contact state vector C can have totally 16 different states.
(From all feet arein the air to all feet are in contact with the ground).
We formulate it as a classiﬁcation problem inour deep neural network and aim at estimating the correct contact state C given the input data.
Inorder to follow typical classiﬁcation pipelines, we map our contact state vector C to 16 differentstates S ∈ { 0 , 1 , . .
. , 15 } by treating C as a binary value with 4 digits and using binary to decimalconversion as the function.
For example, contact state C i =
[0 1 1 0] is mapped to S i = 6 .
4.1 Preprocessing of the Input Data
The contact estimation network takes sensor measurements from joint encoders, IMU, and kinematics as input.
For a synchronized time n , the sensor measurements are concatenated as a 54 × 1 arrayz n = [ q n ˙ q n a n ω n p fn v fn ] , whereq n =
[ q RF 1 n q RF 2 n q RF 3 n q LF 1 n . .
q LH 3 n ] ˙ q n = [ ˙ q RF 1 n ˙ q RF 2 n ˙ q RF 3 n ˙ q LF 1 n . . .
˙ q LH 3 n ] a n = [ a xn a yn a zn ] , ω n = [ ω xn ω yn ω
zn ]
p fn =
[ p RFxn p RFyn p RFzn p LFxn . . .
p LHzn ] v fn =
[ v RFxn v RFyn v RFzn v LFxn . . .
v LHzn ] .
Here, q T n is a 12 × 1 vector containing all the joint encoder measurements (rad) at time n , ˙ q T n is a12 × 1 vector with joint angular velocity (rad / sec) , a n holds the linear accelerations (m / sec) fromthe IMU in the IMU frame, ω n contains the angular velocity (rad / sec) in the IMU frame, p T fn is a12 × 1 vector with foot positions calculated from forward kinematics, and v T fn is a 12 × 1 vector thatcarries the linear velocities of each foot.
It is worth noticing that both p fn and v fn are represented inthe robot’s hip frame.
To infer the relationship between data across the time domain, for data pointat time n , we create a window with size w and append previous measurements within this windowinto a 2D array D n =
[ z T n − w z T n − w + . . .
z T n ] T .
D n is a w × 54 array.
(In our case, we usew = 150 .)
Each time, the network takes D n as input and estimates the contact state S n as output.
4.2 Network Architecture
The contact estimation network consists of 2 blocks of convolutions and 3 fully connected layers, asshown in Figure 2.
Each block contains 2 1-dimensional convolution layers and a 1-dimensional maxpooling.
The convolution layers are designed to extract deep features from the input data.
We choosea 1-dimensional kernel to increase computational efﬁciency in terms of memory usage and run time.
The kernel moves along the time domain, and the data is padded to preserve dimensions.
ReLUis employed as the nonlinear activation function for the convolution layer.
The second convolutionlayer is applied with a dropout mechanism to prevent overﬁtting.
At the end of each block, a 1dimensional max-pooling layer is added to downsample the data.
The second block is connected to 3 fully connected layers that convert the deep features into the 16classes we deﬁned earlier.
We also employ the dropout mechanisms in the ﬁrst 2 fully connected [Table] layers to prevent overﬁtting.
Finally, we formulate a classiﬁcation problem using the cross entropyloss as L [Equation]
P [Equation]
[Equation]
P j is the probability output from the network of state j , and P i isthe probability of the ground truth state.
The detailed network architecture is listed in the Appendix.
4.3 Contact Data Set
We create open-source contact data sets using an MIT Mini Cheetah robot.
We record all the proprioceptive sensor measurements from the robot as LCM logs.
Recorded measurements includejoint encoders data, foot positions and velocities, IMU measurements, and estimated joint torquesfrom an MIT controller [29].
The IMU measurements are received at 1000 Hz , while other dataare recorded at 500 Hz .
All data are upsampled to match the IMU frequency after being recorded.
Around 1,000,000 data points are collected from various terrains, including asphalt road, concrete,forest, grass, middle pebbles, small pebbles, rock road, and sidewalk.
To give the network negativeexamples of non-contact scenarios, we also record several sequences of the robot walking in the airby holding the robot up and giving the same controller command.
(i.e., not having contact with theground while operating the same gait.)
The grass data sets are collected inside an outdoor facility equipped with a motion capture system,which we utilize as a proxy for ground truth trajectory.
Markers are attached to each foot and therobot body for the motion capture system to record the absolute foot positions and body pose in theworld frame.
For the rest of the data sets, in addition to the proprioceptive sensors, we also record
RGB-D images with an Intel D455 camera mounted on top of the robot.
We use these images in astate-of-the-art visual SLAM system, ORB SLAM2 [30], to generate the approximate ground truthtrajectories.
Figure 3b shows the picture of each terrain.
The concrete data sets are recorded in alab environment with polished concrete.
The forest data sets are collected in a forest with randomleaves, woods, and plants scattered around the ground.
The middle pebbles data sets are recorded onpebbles of size ranging from 3 to 7 cm.
The small pebbles data sets are recorded on pebbles rangingfrom 0 .
5 to 2 cm.
Rock road consists of dirt and rocks of random sizes.
It is worth mentioning thatthe contact data sets contain data from 3 different gaits and different robot loading. To label ground truth contact points, we run an ofﬂine pre-processing algorithm with manual reﬁnements.
As shown in the Appendix, the pre-processing algorithm computes the local minimum andmaximum value using the data points both in the future and past around current time stamp.
Thus,it is unable to run online on the robot. On top of that, we also notice that this process creates falsepositives. Speciﬁcally, after inspecting slow-motion videos of Mini Cheetah’s walking patterns, weobserve a bouncing effect on the robot’s foot upon touch down. This results in a sudden change offoot height.
Because of this, we manually remove labels where a sudden change of the foot heighttake place. Figure 3c shows an example of labeled ground truth contacts overlapped with foot height. As seen in the ﬁgure, the initial "jump" in the signal is removed from the ground truth contact list. [Figure]
[Table]
Experimental Results
We train and evaluate our contact network on the contact data sets discussed in the previous section.
Some sequences are reserved for testing.
The rest of the data are split into validation, testing, andtraining sets with the ratio of 15% , 15% , and 70% , respectively.
Table 1 lists the number of databeing used from each terrain.
A window size of 150 is used to allow the network to infer fromthe time domain.
Each data point is normalized along the time domain to prevent scaling issues.
We shufﬂed the training data to ensure generalizability in the model and reduce overﬁtting.
We setthe batch size to 30 and use 10 − 4 as the learning rate.
The training process took 1.5 hours on an
NVIDIA RTX 3090 GPU for 30 epochs.
We note that after about seven epochs, the network canconverge.
5.1 Contact Estimation
We now present the evaluation of the trained contact network.
We evaluate the performance of thenetwork in two ways.
First, 16 class denotes the accuracy in terms of 16 contact states S we deﬁnedearlier.
This case is a harsher way to evaluate the network since it requires all four legs to be correctsimultaneously.
We also compute the accuracy of the individual leg by comparing the estimatedcontact state of each leg with ground truth contacts individually.
On average, the trained networkachieves 87 .
54% of accuracy in terms of 16 contact states and 96 .
21% in each leg individually.
Detailed accuracy across different terrain is listed in the Appendix.
In this experiment, we compare the accuracy of the proposed method with other contact detection methods with the Mini Cheetah robot.
We implement a model-based approach as proposedin [12, 13, 14]. In this method, the estimated ground reaction force is computed via the generalequation of motion with a low-pass ﬁlter. A ﬁxed threshold is then set to detect the contact event.
Because we don’t have direct access to the motor current on the Mini Cheetah robot, we use thetorque command from the controller as an approximation to the actual torque on the actuators. Wealso obtain the gait cycle command from the MIT controller [29] to serve as one contact segmentation method.
Table 2 lists the accuracy of above methods on two test sequence.
We can see the proposed methodachieves the highest accuracy on both sequences and the GRF thresholding has the worst perfor-
[Figure]
mance.
The gait cycle method does not remove the false positive signal when gait cycle is violated.
5.3 Contact-Aided Invariant Extended Kalman Filter
The estimated contacts are used with the contact-aided invariant extended Kalman ﬁlter describedin Section 3 on both grass and concrete test sets.
The estimated contacts are converted into LCMmessages along with IMU and encoder measurements.
The InEKF takes in the LCM messages andestimates the pose of the robot accordingly.
We also run the ﬁlter with the ground truth contact datato serve as a reference.
Figure 4a and 4b show the trajectories from the InEKF with different contact sources on a concreteloop sequence.
Qualitatively compared to the baseline contact detectors, the resulting trajectory withthe proposed contact estimation has smaller drifts from the trajectory with ground truth contacts,especially in the height (y) axis. 5.4 Cassie-Series Robot Experiment
We also perform an experiment on a Cassie-series bipedal robot, developed by Agility Robotics, toshow the generalizability of the proposed framework in different robots and training label sources. The robot is equipped a spring on each leg, which serves as a contact detection sensor. The contactstate is deﬁned as C = [ c L c R ] .
The decimal contact states becomes S ∈ { 0 , 1 , . . Similar to MiniCheetah, we concatenate the input data as z n = [ q n ˙ q n a n ω n p fn v fn ] . As the datafrom Cassie is recorded at a higher frequency, a window size of 600 is used in this experiment.
The data is recorded on polished concrete with a controller from [31]. Around 1 , 300 , 000 datapoints are recorded. Two sequences are reserved as test sets, and two are used in training.
Weobtained two sources of ground truth contacts in this experiment - one from spring deﬂections; theother from kinematics. Spring contact labels are obtained by thresholding on the GRF computedfrom the spring deﬂection.
We assume this is a more reliable contact source as it comes from aphysical sensor. The kinematics contacts are generated following the same procedure describedin Section 4.3. We train the network on two ground truth contacts separately and evaluate bothnetworks against spring contacts.
Table 3 lists the accuracy of the two networks.
We can see that when trained with ground truthcontacts from spring deﬂection, the network can achieve above 98% of accuracy.
When it is trainedwith the labels generated with kinematics, although a little bit of accuracy is lost, it still maintainabove 95% accuracy on both leg.
This indicates training with the labels generated from the processdescribed in Section 4.3 only sacriﬁce a small amount of network accuracy. This experiment alsoshows the generalizability of the proposed framework. We demonstrate the proposed framework canachieve above 95% of accuracy on two distinct robot as long as one collect enough data for training.
[Table] [Table]
5.5 Ablation Study and Runtime
We perform an ablation study on different network architectures to study how the performancechanges.
Table 4 lists the average performance of different network structure on the test sets.
Each"block" consists of 2 convolution layers followed by ReLU activation function and 1 max pooling,as described in Figure 2.
In the table, 2 Blocks is the proposed network detailed in Section 4.
Block refers to the network with only the ﬁrst block and the fully connected layers of the proposednetwork.
As for 4 Blocks, we added two additional blocks with 256 and 512 channels before thefully connected layers.
Conv-Pool-Conv-Pool consists of only 2 convolution layers of sizes 64 and128 and the fully connected layers.
Each convolution layer is followed by a ReLU function and onemax pooling.
From the table, we can see that 2 Blocks has the best performance among all otherstructures.
However, 1 Block maintains a smaller network while showing a slightly lower accuracy.
The inference speed on an NVIDIA RTX 3090 GPU is approximately 1100 Hz. The inferencespeed on an NVIDIA Jetson AGX Xavier, which we equipped on the robot, is around {+
+}
Hz after TensorRT optimization.
The frequency of the encoder measurements on Mini Cheetah is 500 Hz.
This means the proposed contact estimator run at real-time without delay on the robot.
The InEKFruns at 2000 Hz on an Intel i7-8750H CPU.
Discussion and Limitations
The developed contact estimator can capture contact events for a quadruped robot withoutforce/contact sensors.
However, the contacts are modeled as binary values, and the “quality” ofcontacts is not modeled in our contact estimation network.
As a result, a perfect contact is alwaysassumed when a contact event is detected.
Furthermore, the covariance matrix of the contact measurements is set heuristically in the InEKF, which along with the uncertainties in the kinematicmodels, could be the potential sources of estimation error in the InEKF.
In the future, we wish toexpand the contact detection network to have a covariance estimation of the current contact state.
This approach will allow the state estimator to treat contact information as a sensor measurementwith an online uncertainty update and potentially decrease the drift in the state estimation.
We formulated the problem as a classiﬁcation problem with 16 different contact states.
This modelcan potentially be further improved by employing a multi-task learning approach [32, 33].
Thisapproach allows to independently segment the contact events for each foot.
Conclusion
We developed a multi-modal deep learning-based contact estimation method that does not requirecontact/force sensors and works well with different robot gaits on distinct terrains.
We presentopen-source contact data sets that require minimal supervision for labeling contact events of an MIT
Mini Cheetah robot.
We show that the trained contact network can achieve 94% of accuracy acrossdifferent terrains.
The estimated contacts are employed in a contact-aided invariant state observerfor quadruped robots, and the resulting trajectory is comparable to a modern visual SLAM system.