{'fraction_non_alphanumeric': 0.021974984451661943, 'fraction_numerical': 0.003973464169718748, 'mean_word_length': 4.74265873015873, 'pattern_counts': {'":': 0, '<': 0, '<?xml version=': 0, '>': 5, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Contact Model Fusion for Event-Based Locomotion in Unstructured Terrains

As legged robots are sent into unstructured environments, the ability to robustly manage contact transitions will be a critical skill. This paper introduces an approach to probabilistically fuse contact models, managing uncertainty in terrain geometry, dynamic modeling, and kinematics to improve the robustness of contact initiation at touchdown. A discrete-time extension of the generalized-momentum disturbance observer is presented to increase the accuracy of proprioceptive force control estimates. This information is fused with other contact priors under a framework of Kalman Filtering to increase robustness of the method. This approach results in accurate contact detection with 99.3 % accuracy and a small 4-5ms delay. Using this new detector, an Event-Based Finite State Machine is implemented to deal with unexpected early and late contacts. This allows the robot to traverse cluttered environments by modifying the control actions for each individual leg based on the estimated contact state rather than adhering to a rigid time schedule regardless of actual contact state. Experiments with the MIT Cheetah 3 robot show the success of both the detection algorithm, as well as the Event-Based FSM while making unexpected contacts during trotting.


I. INTRODUCTION
Legged robots offer the promise to negotiate difficult terrain environments beyond the reach of wheeled or tracked vehicles. As robots begin to navigate such terrains, an increasing burden is placed on control systems that are designed to manage contact transitions and achieve solid footholds amidst incomplete and uncertain terrain knowledge. Contact force sensing and its associated feedback mechanisms will play a crucial role in this process. However, conventional force estimation is noisy, with force sensing technology notoriously fragile. Thus, an ability to reliably manage contact transitions through proprioceptive sensing alone is a desirable skill for rough terrain locomotion.
The MIT Cheetah 3 robot is a recently developed platform designed to move through the world using terrain-blind locomotion. Rather than use perception to build a map of the terrain before attempting to cross it, proprioceptive sensing is sought via highly-transparent actuation [1], joint angle readings, and feedback from an Inertial Measurement Unit (IMU). Each of these sensors explicitly provides only perception about the robot's body and leg states rather than the external environment. Work to develop robust control without reliance on perception will ultimately mitigate sensing accuracy requirements upon later integration. With reliable contact detection, an event-based gait switching control strategy was employed on the MIT Cheetah 3 robot to reliably move across unstructured terrains.
In the absence of external force sensing, a now-classic method has become the application of disturbance observers [2] based on the generalized momentum (GM) of the robot. While originally introduced for manipulators, this method has been applied to localize contacts in humanoids [3] and further extended to multi-contact cases through a novel synergy with particle filtering [4]. These methods have also inspired other contact detection strategies in quadrupeds. Hwangbo et al. [5] used inference over a Hidden-Markov Model (HMM) to fuse dynamics, differential kinematics, and kinematics for contact detection [5]. Camurri et al. [6] used approximate inverse-dynamics-based forces as inputs to a contact probability prior for determining when a foothold was secure enough for use in base-velocity estimation.
Despite the solid theoretical grounding of the GM observers, they have yet to be applied for probabilistic contact detection. Work in [5] assumes access to acceleration measurements that may be impossible to obtain in practice, while [6] neglects acceleration effects for the same reason. Interestingly, the main benefit of the GM technique is its ability to skirt the need for acceleration measurements by leveraging properties of the Coriolis matrix. We explore this potential here.
Other work has concentrated on design of control strategies following contact detection in uneven terrain. Carefully crafted state machines [7] have shown great potential in experimental biped walking by encoding adaptive recovery based on the timing of the disturbance within the gait cycle. Other work has focused on designing families of controllers to enable reactive recovery upon touchdown [8]. Reactively modified central pattern generators (CPGs) have also shown an ability for terrain adaptability [9]; however, the kinematic nature of CPGs requires additional mechanisms to manage dynamic balance. An alternative methodology is design control laws that are formally robust to terrain variations (as in [10]), which could be extended to quadrupeds to yet supplement reactive approaches.
The main contribution of this paper is to pursue the GMbased disturbance observer within a unified framework for probablistic contact detection that fuses multiple priors. This provides a simpler solution than the HMM approach in [5] and further demonstrates a sensing delay superior to or equal to all resuls provided in the HMM method. Priors based on kinematics and gait phase increase accuracy of the GM method, while tuning of the filter demonstrates optimality of including all priors with relative equal weight. An alternate derivation of the GM method in discrete-time further facilitates accurate force estimates in computational control loops.
Overall, this provides a method for detecting the contact of a leg on the ground and modifying the control actions for the leg in the absence of a direct force sensor on the foot. The contact detection algorithm allows for a more robust locomotion strategy that detects and handles early and late contacts. With this, the robot is able to safely handle large unexpected terrain changes and obtains stable footholds through the incorporation of low-delay detection.
The remainder of this paper is structured as follows. Section II introduces the overall intrstructure of the control law, motivating the need for proprioceptive contract detection. Section III details an improvement to the GM-based disturbance estimate for discrete-time implementation, and Section IV discusses its fusion with other contact priors for robust contact detection. Section V describes contact detection results in simulations and experiments that are then integrated for reactive gait modification in Section VI. A concluding discussion is offered in Section VII.

II. ROBOT CONTROL
Locomotion control of the MIT Cheetah 3 is defined by a simple gait scheduler that switches the robot between two distinct states, contact and swing. Since legged locomotion is an inherently hybrid form of movement, the robot encounters discrete mode switches as each of the legs enters or leaves contact. The current leg state is defined by the boolean variable, s ∈ 0 = swing, 1 = contact . However, without some way of sensing the leg state, the robot does not know the true values of s and therefore its control may be switched based purely on a phase-dependent scheduler. This initial gait-specific scheduler informs the controller as to what state the legs should be in with a boolean variable, s φ ∈ 0 = swing, 1 = contact .

A. Periodic Phase-Based State Scheduler
We consider a steady state legged locomotion gait as a collection of contact and swing states for each leg with discrete, instantaneous transitions between them. The underlying mechanism behind the leg state switching is a periodic phase-based state scheduler. An overall phase variable, φ ∈ [0, 1), cycles over a defined period depending on the current Currently, the scheduler uses a phase variable that is a linear function of time where t is the current time, t 0 is the start time of the current period, and T is the determined cycle period. The phase variable φ i = φ + φ i,of f set , for each foot, i, is determined for each gait. Figure 2 shows the typical mapping of a trot gait where the dashed periods in the cycle signify scheduled swing states,c φ , and the solid sections are scheduled contact states, c φ . We see that diagonal pairs of legs have the same offset, which is one half cycle away from the other pair. According to the scheduled state for the leg, control actions are different between swing and stance. For the legs in swing, the robot uses an impedance controller to track a calculated swing trajectory. This approach has been used successfully by the MIT Cheetah before [11]. When a leg is scheduled to be in stance, it is used in a balance controller similar to the QP balance controller used in [12]. The forces coming from the QP solver are sent to the leg which is capable of force control through the joint torques on the motors. As such, it is clear that if a leg is scheduled to be in swing but is actually in contact, the controller will continue to forcefully drive the foot into the terrain until the swing phase is done. In the opposite case where the leg is scheduled to be in contact, but it is not, the controller will immediately request for the leg to produce a ground reaction force, causing the leg to stomp into contact mid-swing. Neither situation is desirable and will cause instability during locomotion, but both occur frequently while traversing unstructured terrain and encountering unexpected obstacles.

III. INCREASING THE ACCURACY OF THE MOMENTUM-BASED DISTURBANCE OBSERVER FOR
DISCRETE-TIME IMPLEMENTATION This section describes a contribution to the application of the now-classic generalized-momentum-based (GM-based) disturbance observer [2] in discrete time. The main idea of this method is as follows. Given the torques applied at the joints, trajectory outcomes expected based on a model can be compared to the actual outcomes, with any difference attributed to an external disturbance. As we detail, the classical algorithm based on this principle suffers from drawbacks when implemented in discrete time. These drawbacks arise due to the inconsistency between the derivation of the method (which assumes continuous time implementation) and its actual implementation (which is necessarily discrete time). For motions with high dynamic effects, such as during leg swing, it is found that the classical results provide errant residuals. Through a new derivation of the GMbased disturbance observer fully in discrete time, these errant residuals are shown to be significantly reduced in our results. The section proceeds with a quick review of the methods from [2] followed by our new discrete-time derivation.

A. GM-Based Disturbance Observer in Continuous Time
To begin, consider the standard equations of motion where q ∈ R n d the joint angles, n d the number of degrees of freedom, M ∈ R n d ×n d the mass matrix, Cq ∈ R n d the generalized Coriolis force, g ∈ R n d the generalized gravity force, S ∈ R nj ×n d an actuated joint selector matrix with n j the number of actuated joints, J i ∈ R 3×n d the Jacobian for contact i, and f e i ∈ R 3 the force at contact i. Collecting the external forces into a disturbance vector a filtered version of this disturbance torque τ d can be computed asτ where s is the Laplace variable, λ is the cutoff frequency, andτ d gives an estimate of the disturbance. As in [2],τ d can be computed from where p = Mq the generalized momentum, and C ∈ R n d ×n d is formed through a proper factorization of the Coriolis terms such thatṀ − 2C is skew symmetric [13].
There is an alternate view on this disturbance observer that helps to explain new results in the discrete-time case. Through a change of variables w = λMq −τ d , it follows that This equation is recognized as a low pass filter As a result, the classical disturbance observer obtains filtered disturbance forces through a feedforward from the generalized momentum with the addition of filtered dynamic effects. When implementing these filter equations or integrals in discrete time, it was found that the discretization process introduced modeling errors that appeared as fictitious disturbances. We introduce an alternate derivation fully in discrete time that addresses this error.

B. Discrete Time Equations
Instead, consider applying a discrete time filter to a sampled version of τ d where z the z-domain variable and 1 > γ > 0, with γ monotonically related to the cutoff frequency. Letting w k = (1 − γ)γ k the impulse response of the filter Although this formula contains joint accelerations that aren't measured, the sum can be evaluated using the discrete time analog of intergation by parts known as summation by parts.
For a general sequence f i and g i summation by parts gives Applying this formula with and assumingq 0 = 0, the output of the filter from Eq. (5) is equal to where β = (1 − γ)γ −1 /∆t. Assuming a proper factorization of C such thatṀ = C + C [13], the finite difference of M in (8) can be simplified. As a result the convolution in (8) can be re-expressed as: Thus, it follows that (4) can be written as: To enable comparison with the previous method the cutoff frequency λ is converted to a Z domain pole γ = e −λ∆t  where ∆t is the sampling time. Note this result has a similar structure to (3), however, the feedforward on the generalized momentum is carefully calibrated to be consistent with discrete-time features of implementation. In periods with high dynamic effects, correctly accounting for these effects has been found to be important for accurate force estimates.

C. Computing Estimated Forces
Regardless of the continuous or discrete time derivation, force estimates are computed in a common way. Letting S i the selector matrix for the joints in leg i and J i the leg Jacobian, estimated forces are finally formed at each instant asf i = (S i J T i ) −1 S iτ d which assumes that all disturbance forces on the leg come from the foot. Figure 3 shows the estimated disturbance force during swing in a realistic simulation with sensor noise and discrete-time and quantization effects. Both observers use a cutoff frequency of 15Hz and sample at 1KHz. Over the time 29.35 − 29.72s the RMS error of the continuous time observer (implemented in discrete time) is 8.7N, while the RMS of the full discrete-time observer is 4.1N, improving performance by over a factor of two during swing phase. The leg does not experience large accelerations during stance phases and therefore both observers are accurate. Although these effects are shown for a trotting gait, beneficial aspects of the discrete-time modifications would be expected to increase for more dynamic gaits.

IV. PROBABILISTIC CONTACT MODEL FUSION
Realistically, we cannot simply trust that the controller will adhere to the perfect timing of the scheduler by pulling the foot off the ground or placing it down again on time. Similarly, the ground cannot be assumed to be perfectly flat and may contain unexpected ground height due to rough terrain or unseen objects. Without an external force sensor, we must estimate the forces at the leg from the encoder data, proprioceptive forces, and the dynamics of the leg's model. Encoders will have discretization error and model inaccuracies will cause an external force to show when there is none. None of these single measurements will tell us the leg state, but we can take the information from each and use them to find a better estimate for whether or not a particular foot is likely to be in contact with the ground,ŝ ∈ 0 = swing, 1 = contact . A perfect detection algorithm will have s =ŝ.
The use of various measurements lends itself nicely to using a Kalman Filter to carry out the fusion. By posing the states of the Kalman Filter as the overall estimated probability of the feet being in contact with the ground aŝ We can naturally use probabilistic contact models as priors to fuse the available data. Using the probability of contact as the state allows us to use a simple linear Kalman Filter even though the dynamics involved in the measurements are complex. An advantage of using probabilities rather than discrete, binary contact states in the estimation is the ability to anticipate a change of state as probability grows or shrinks.

A. Prediction Model
The standard prediction equations for the Kalman filter are presented here aŝ Given the gait schedule and the current phase, φ, of a leg, the gait scheduler provides an expected contact state s φ of each leg. Under ideal conditions, we would expect that s and s φ are equivalent and switch simultaneously. However, the robot is subject to possible timing delays in its control system that may cause its leg to take off late, as well as early or late contacts due to inaccurate swing leg trajectory tracking or unforeseen ground height.
To handle these issues, we can build a probabilistic model for the expectation of contact given the scheduled leg state and subphase during stance, φ c , or swing, φc. The probability of contact given the current contact state and subphase percent is chosen to take the form where s φ acts as a switch between swing or stance. The mean, µ, parameters encode the expected subphase value at which the contact state switches while the variance, σ 2 , parameters are determined by the variability in the subphase value at contact. Figure 4 shows the two curves for the probability of contact based on the phase and scheduled state for different variances. Towards the beginning and end of the contact phase it is less likely that the leg is in contact, while it is more likely that the leg is in contact towards the beginning and end of swing. This model can therefore be used as the instantaneous input to the system as which is stacked for each of the robot's legs using their individual scheduled states and phases. Roughly, the following covariance matrix encodes the trust that we place on the accuracy of our phase-based switching model as Since we are simply looking at instantaneous contact detection by fusing the currently available measurements, the state and input matrices are defined to be A k = 0 N and B k = I N (16)

B. Correction: Measurement Models
Knowing that the prediction likely contains inaccuracies in the gait schedule, we can use available measurements to correct the prediction and get a more informed estimate for probability of contact. The standard Kalman Filter correction equations are presented as 1) Measurement Model: Ground Height: In the absence of an environmental perception system, ground height, z g , can be modeled probabilistically. The random variable for the height of the ground is drawn from a Normal Gaussian distribution as Z g ∼ N (µ zg , σ 2 zg ). The defining parameter for average ground height, µ zg , is set to be zero with no external information since we cannot make assumptions about the terrain. Similarly, the variance loosely corresponds to the "roughness" of the terrain. As such, we can use the ground height model to create a belief for the height at which we expect to be in contact with the ground given the position of the foot in the vertical direction, p z . The probability of contact given foot height is defined as (20) Figure 5 shows both the Gaussian model of the ground, as well as the inferred probability of contact given foot height. Each is shown for different parameters corresponding to various roughness estimates. Eventually, the values of µ zg and σ 2 zg can be adapted as we gain information from other sensors or historical footsteps. This model provides the first correction measurement with an associated covariance matrix . . .
as a measure of our belief in the validity of the contact height model.

2) Measurement Model: Contact Force:
The only true indicator of contact is external force felt at the foot. However, the robot is not currently equipped with a direct force sensor. With the force estimate presented in Section III, we can create another simple probabilistic model for the force of contact with Gaussian random variable F c ∼ N (µ fc , σ 2 fc ). Here µ fc is the average force sensed at the initiation of contact and σ 2 fc is a measure of the noise on the estimate. This gives the probability of contact given the estimated foot force as The model and associated contact probability given vertical foot force are shown in Figure 6. This force based probability provides our second measurement The two sets of individual measurements are stacked to form the observation measurement vector used in the Kalman Filter. Similarly, the covariance matrices of each measurement form an overall block diagonal covariance matrix as follows The output matrix, H k , is formed as two stacked N × N identity matrices where N is the number of feet.
By fusing the gait scheduler state expectations with the measurement model probabilities, we can get a better guess for the actual contact state of each leg. Owing to the fact that the Kalman filter is implemented with A k = 0, this fusion process could be derived through a static likelihood maximization with Bayes law. Yet, this process is one preembedded in standard Kalman updates, which may be more accessible in robotics.

V. IMPLEMENTATION RESULTS
The algorithm was verified both through a realistic dynamics simulation environment developed by the Biomimetic Robotics Lab at MIT as well as through experiments on the MIT Cheetah 3 robot.

A. Simulation
To determine the feasibility of the algorithm and find reasonable values for the Gaussian parameters in the models, a simulation of the robot was run in various scenarios including trotting on flat even ground, on random rough terrain, and up a small step. Table I details the initial parameters used in the contact detection simulation. These parameters were chosen based on intuition and previous experimental data from basic locomotion tests with the robot.
The simulation was run using a slow, long period trotting gait with zero forward velocity and turn rate. The foot height and vertical force measurements during steps for the front foot are shown in Figure 7. The top plot shows the current subphase from 0 to 1, with the wide ramps corresponding to the scheduled stance state, c φ , and the narrow parts to the scheduled swing state,c φ . The second and third plots are the time series plots of the measurements, foot height, p z , and estimated foot force, f z , respectively. The bottom shows the probability of contact that results of the fusion from the models with the ground truth state, s, and estimated state,ŝ, overlaid on top. A hysteric threshold, P c (c), for the probability of contact at which we can reasonably declare contact is defined to givê Using the simulated results, an optimization was run to find the best parameters over various data sets that included flat ground, rough terrain, and a step. The optimization was run offline using fmincon in MATLAB. A cost function was designed as a weighted combination of the squared classifier error and the squared difference between contact probability and leg state with W c >> W n since what we care about most is correct classification and χ being the set of model estimation parameters for all of the contact models. Although the resulting parameters are biased to the tested situations, an effort was made to include a wide variety of terrains to get which shows that the relative weighting of each was similar and therefore all three measurements were beneficial and equally necessary to increase detection robustness.

B. Experimental
The algorithm was implemented on the MIT Cheetah 3 hardware using the same trotting gait as in the simulation described in V-A. Ground truth contact data was determined by a force plate setup that records ground reaction forces over time. The results of the experiment during trotting for the right front foot are shown in Figure 8. During the experiment, contact occurred earlier than expected and at a location higher than expected. Due to this unexpected contact, the foot bounced off of the ground from the large spike in force before touching down again. We also see that during the swing, there is a large amount of noise from the force estimate, likely due to model inaccuracies and the noisy use of differentiated encoder data. Neither measurement nor the gait scheduler could reliably be used to detect contact on its own. This is precisely the power of fusion for the contact detection algorithm. Figure 9 shows that we correctly detect the early contact and are aware that we are not in contact during the bounce although we are scheduled to be in contact according to the gait scheduler. Despite these errors in both the measurements and the controlled gait schedule, the algorithm was able to correctly identify contact 99.3% of the time in this test. A small 4 − 5ms delay during the initial unexpected contact accounts for this slight inaccuracy.

VI. EVENT-BASED GAIT SWITCHING
With the realization of contact detection, we can move away from the timed gait schedule control towards eventbased control. Since the timed gait scheduling used no 6 Fig. 9. Unexpected Foot Contact. Implementation of the algorithm on the actual hardware that shows accurate contact detection with unexpected early contact and foot bounce. feedback to switch between contact and swing states in the legs, there was rigid, forced time-based switches between the state of a leg in swing or stance. This leads to the problems presented in Section II. Under near-perfect conditions these inconsistencies have minimal effect, but in very unstructured terrain with large discrete height changes such as blocks or stairs they can severely impact performance. Having quick, accurate contact detection allows the robot to correct these issues and expand the possible operating situations.
The phased scheduled state, s φ , remains the underlying mechanism for choosing the gait pattern. However, leg control uses the estimated contact state,ŝ, coming from the algorithm. A simple Event-Based Finite State Machine in Figure 10 shows the states and transition conditions.
With the new framework, once the robot's leg is scheduled to be in swing it will continue to follow the desired swing foot trajectory until it detects contact regardless of if it has been scheduled to be in contact during that time. However, as soon as it detects a contact, it will attempt to hold the current foot position rather than following a trajectory. If the algorithm continues to detect contact after some delay time, t > (t delay + t 0 ), where t 0 is the initial time for detected contact, the balance controller will be notified that the leg is available for balance. This intentional delay is meant to prevent fleeting contact from catastrophically affecting the robot's gait by trying to use the leg to balance if it simply makes a slight passing contact or if it slips.
Although a simple change from rigid time-based gait scheduler to an event-based FSM, the robot's robustness to unexpected contact with large height differences has been improved as a result of accurate contact detection. Using a Fig. 11. Touchdown High Speed Comparison. The bottom series shows a touchdown event with no Event-Based FSM where the early contact causes a bounce (highlighted by the red circle) upon touchdown. The top series shows that the Contact Detection notifies the FSM that contact has occurred and switches to the early contact state, mitigating any bounce.
high-speed camera, we captured the touchdown event with and without the event-based switching. Figure 11 features a time series of snapshots that show the results of this implementation on the experimental force plate setup. Once contact occurs, the algorithm holds the current foot position and prevents the bounces observed previously.

VII. CONCLUSION
The work presented in this paper develops a novel approach for event-based legged locomotion control using a probabilistic contact detection algorithm. The timed gait scheduler provides an expectation for state switching at the scheduled state and phase. However, as we accept switching will not happen perfectly on schedule, we make use of available robot states to infer when contact occurs or breaks. Since the robot cannot directly measure force, an accurate discrete-time momentum based observer is derived to estimate external forces on the leg. However, this estimate is still subject to noise so simple probabilistic models are designed for the height and force at which contact with the ground is initiated. These measurement models are fused together to increase the accuracy of the estimated contact state. With the ability to reliably sense contact with the ground, an eventbased switching strategy is employed to deal with early and late contacts. The event-based framework allows the robot to traverse unstructured terrain with obstacles without the use of direct environment perception.