A kinematics-dynamics based estimator of the center of mass position for anthropomorphic system — A complementary filtering approach

This paper presents an original approach to simply and efficiently estimate the center of mass position of a free-floating base system, like a humanoid robot or a human body. This approach relies on the theory of complementary filtering, which is a popular technique in aerial robotics, but rarely implemented in humanoid robotics. The main idea consists in merging input measurements like the zero-moment point position, the contact forces, etc. which are then filtered according to their reliability in their respective spectral bandwidth. We validate this approach in simulation by (i) comparing the estimated center of mass trajectory with its real value and (ii) showing that the complementary filter offers on average a least reconstruction error than the classic Kalman filter.


I. PROBLEM STATEMENT
Biomechanics and humanoid robotics communities share a common interest in the estimation of Center of Mass (CoM) position.From a biomechanics perspective, it concerns the CoM position of the human body which depends on a very large number of parameters, including soft tissues shapes and densities.These parameters are classically reduced to articular angles coupled to a mass distribution model considering perfectly rigid limbs [1].Nevertheless, the CoM of humans is at the heart of classical biomechanical studies on equilibrium and locomotion [2].Indeed, CoM trajectories constitute a synthetic, mechanically and geometrically relevant motion descriptor [3], and its dynamics carries also information about the contact forces necessary to compensate for the gravity and ensure locomotion.The more accurate is the reconstruction of the CoM trajectory, the more precise will be the extraction of features and phenomena from studied motions.
In Robotics, the CoM of a humanoid robot depends on the configuration of the robot and the dynamical model.Although the modeling error is much lower for humanoid robots than for humans, they are usually extracted from CAD data and may contain discrepancies with the real robot.And, the aging of the robot in addition to material updates and repairs lead the robot parameters to drift from the initial model, and may require a new calibration process [4].Despite that, the CoM is the main control variable for walking motion generation.For instance, on flat ground this control aims mainly to ensure displacement in space while respecting the balance constraints that the center of pressure of the feet, also called Zero Moment Point (ZMP), stays always strictly inside the convex hull The authors (firstname.surname@laas.fr) are with CNRS, LAAS, 7 avenue du colonel Roche, and Univ.de Toulouse, F-31400 Toulouse, France. of the contact surface [5].The modeling errors inducing misestimation of the CoM position which may then endanger the balance of humanoid robots [6].All these modeling errors are due to forward kinematic estimation.Nevertheless, many experimental setups that study human motion and most humanoid robots are equipped with force and moment sensors at contact points that provide dynamics-based, unbiased, ground-truth data on the CoM kinetics.
We have three input signals.The first one comes from the geometry and contains the modeling errors.This signal also suffers from noise coming from improper estimation of the position of the limbs, especially in the case of motion capture.The second signal is the force measurement, which is proportional to CoM accelerations.The third one is the ZMP measurement that we construct using the forces and the moments.The ZMP can be approximated by a linear combination of horizontal positions and accelerations of the CoM.Both last signals suffer from measurement noise.The linear approximation of ZMP, commonly assumed, also introduces an estimation error.
This paper proposes a simple method for multi-sensor data fusion to merge these three signals into an estimator (see Fig. 1).To do so, we take into account an important property that characterizes these signals : they have different spectral distributions of errors and noises.This means that for almost any frequency range of the CoM trajectory, there is a signal providing a better estimation than others.We propose to use data fusion based complementary filtering.This common technique consists in merging input signals that suffer from errors that lie in different bandwidths into one output signal.It is a simple and real-time method that enables to obtain theoretically unbiased, noise-free and non-phase-shifted estimation of the CoM position.
Section II describes the dynamical system linking the input signals to the trajectory of the CoM.Section III introduces our linear complementary filter for three signals.Fig. 2: A sketch representation of the spectral distribution of errors that would emerge from the naive reconstruction of CoM trajectory if we use only one signal (Geometry, Forces and ZMP).The signal with the lowest error is then selected at each frequency bandwidth to constitute minimal-error fusion of these signals.
In Section IV we show how our method behaves against simulated noisy measurements and we compare the performance to estimation by a Kalman filter.In Section V, we compare our method to existing approaches.

II. THE ANTHROPOMORPHIC SYSTEM MODEL
In this section, we briefly describe the equations of the dynamics of a free-floating system with an anthropomorphic structure.The main purpose is to emphasize the link between the measured quantities (i.e. the estimates of the positions of the CoM and ZMP, and the forces) and the under-actuated dynamics, namely the dynamics reduced around the CoM.

A. The under-actuated dynamics
Consider first the Lagrangian dynamics of a n degrees of freedom free-floating based system which makes N contacts with its environment.We name q ∈ Q def = SE(3) × n the configuration vector of the system and q, q its first and second time derivatives.The Lagrangian dynamics reads: where M stands for the mass matrix, b for the centrifugal and Coriolis effects, G for the action of the gravity field.S is a selection matrix which distributes the torque τ over the joints space, J i is the jacobian of contact i and φ i = (f i , ν i ) is the vectorial representation of the unilateral contact wrenches acting on the robot with linear f i and angular ν i components.
This dynamical equation can be split into two parts: the under-actuated dynamics, i.e the dynamics of the free-floating base (denoted by b) and the dynamics of the actuated segments (denoted by a): The first row of ( 2) is the so-called Newton-Euler equation of a moving body, having a mass m, a CoM position c relative to the inertial frame, a linear and angular momenta denoted by p and L respectively.This under-actuated dynamics can be rewritten as: where × denotes the cross product operator, p i is the position of the contact point i relative to the inertial frame and g is the gravity field.

B. The Zero-Moment Point
Under the hypothesis where all contact points lie on the same plane surface, the ZMP horizontal components are defined by [7]: And, if we neglect the variation of the angular momentum and consider the vertical CoM position remains constant, we obtain the so-called cart-table model: which is linear in both variables c x,y and cx,y .

III. THE LINEAR COMPLEMENTARY FILTER
The complementary filter [8] is well known in the field of aerial robotics [9], for example to estimate the attitude of a quad-rotor system by combining the gyroscopic and accelerometer measurements.Unlike the Kalman filter [10] which makes no distinction between the contributions of each measurement in the frequency domain, the complementary filter exploits the influence and the accuracy of each input signal in their respective frequency domain and reconstructs the integrality of the signal by a combination of filtered measurements, with zero-phase-shift.A simple example of complementary filter is derived in Sec.III-A.All along this section, we make use of the following definition: Definition 1 (Linear Complementary Filter): We say that the transfert function Y is the linear complementary filter of the transfert function X if and only if X(s) + Y (s) = 1 for any s ∈ C, s being the Laplace variable.
For our case, we have three signals providing information on the CoM.
[S1] The first one is the geometry-based reconstruction of the CoM.It suffers mainly from biases due to modeling errors of mass distribution.It is also subject to high frequency sensor noise due to motion capture technology or the measurement of the angular position of the joints.The error between this signal and the real position of the CoM lies then in low and high frequency domains.
[S2] The second signal is the CoM acceleration extracted from force measurements.This signal suffers also from sensor noise.The double integration of this signal reduces the high frequency error but generates quadratic drift, visible in low and medium frequencies.[S3] The third signal is the ZMP, which only provides information about the horizontal position of the CoM.It contains high frequency sensor noise, but also carries error due to the linear approximation (6).This approximation assumes a constant CoM height and a negligible variation of the angular momentum.These assumptions are particularly acceptable in low frequency domain, specifically below natural walking rhythm.Similar reasoning can be found in [11], [12].These three considerations are schematized in Fig. 2.
In the following, we gradually design the complementary filters of the CoM vertical and horizontal components.We designate by s the Laplace variable acting in the frequency domain.The Laplace Transform of a temporal signal g(t) is written G(s) and sG(s) corresponds to the Laplace Transform of its time derivative ġ(t). A. Complementary filter of the CoM vertical component 1) Hypothesis: The vertical position c z of the CoM can be observed from two decoupled sources of measurement: • (1) the mass distribution of the anthropomorphic system coupled with the current measure of the positions of links provide an estimation cz • (2) vertical contact force measurements divided by the mass m and subtracted from gravity (cf.Newton equation ( 3)) provide the estimation cz of the vertical acceleration of the CoM.The complementary filter diagram corresponding to those two measurements is in Fig. 3. H z 1 and H z 2 are the linear filters of cz and cz respectively.
2) Design: For the first signal [S1], the noise is mostly located in the high frequency domain.While for the second measurement [S2], due to the double integration process, the noise is mainly concentrated in the low frequency domain.Then s 2 H z 2 is made a high-pass filter 1 in order to filter out the low frequency noise in the double integration processus.We can now set: with τ 1 def = 1 2πf1 the time constant and f 1 the cut-off frequency of the high-pass filter s 2 H 2 .Therefore (7) reads: 1 the s 2 term before H z 2 comes directly from the fact that s 2 C z (s) is the Laplace Transform of cz .
Accordingly, H 1 can be directly computed as the complement of s 2 H 2 , i.e.H 1 def = 1 − s 2 H 2 .So H 1 is of the following form: which is the combination of a low-pass filter and a bandpass filter, both with a unique cutting frequency of value f 1 .
As expected, H 1 acts in the low frequency domain.Fig. 4 represents the bode diagram of the two designed filters H 1 and H 2 .One can remark that both filters depends on the same free variable f 1 , which corresponds to the unique cutting frequency of the low-pass and high-pass filters.The choice of f 1 corresponds to the frequency of sensor noise in the force measurement and link position reconstruction.
Remark: The double integrator corresponding to acceleration measurements imposes the order of the filter to be at least 2; otherwise, the filter H z 1 would be non-causal.

B. Complementary filter of the CoM horizontal components
In the case of horizontal components estimation, we add the measure of the ZMP position [S3] to the input of the estimator.The complete diagram of the complementary filter of horizontal components is depicted in Fig. 3.
1) Hypothesis: The hypotheses of the CoM vertical component still hold for the case of the horizontal ones.We make the hypothesis the ZMP is a good approximation of the CoM position for very low frequencies.
2) Design: We first write the Laplace Transform of (6): with τ z def = c z g z the time constant of the linearized ZMP model.(10) can be rewritten as: The ZMP dynamics ( 11) is a second order differential equation whose integration reveals one stable (1 + τ z s) and one unstable (1−τ z s) component [13].So, we need to reject this unstable dynamic in order to make the complementary filter stable.Finally, we want that: with τ 2 def = 1 2πf2 the time constant and f 2 the cutting frequency of the low-pass filter of the ZMP.H x,y 3 has the following expression: While H x,y 2 remains unchanged and equal to H z 2 , H x,y 1 is the complementary filter of both s 2 H x,y 2 and (1 + τ z s)(1 − τ z s)H x,y 3 transfert functions, leading to: 3 .H x,y 1 acts as a bandpass filter in a bandwidth around [f 2 ; f 1 ], while H x,y 3 is mainly a third order low-pass filter.Choosing the frequency f 2 is a little bit more subtile than in the case of f 1 .We consider that the linearized ZMP model is only valid for the frequencies below the fondamental frequency of the locomotion pattern.In general, this fondamental frequency is around 1 or 2 Hz.Therefore, we might consider a value of f 2 below 1 Hz.

IV. EXPERIMENTAL RESULTS
In this section, we apply the proposed complementary filter to the case of a simulated humanoid robot walking in straight line.The simulation framework allows: (i) to obtain ground truth measurements, that will be used for the evaluation of the performance of the complementary filter and (ii) to generate noisy model and measurements which will serve as inputs of the filter.We also compare the performance of the designed complementary filter to a more classic Kalman filtering approach, which uses the same kind of measures while assuming that those sensor measures are affected by a white noise.
A. Generation of noisy data 1) Motion generation: We use standard techniques in humanoid robotics to generate the motion of the robot.We first plan a CoM trajectory according to the given foot placements and ZMP reference trajectory [5].Then we generate a whole body trajectory using a second-order generalized inverse kinematics [14]; the following tasks where combined using a strict hierarchy: the feet positions (first priority), the CoM trajectory and a fixed orientation of the pelvis (second priority) and finally a posture task to avoid the drift of actuated joints (third and lowest priority).
2) Generation of noisy measurements: The second-order kinematics produces a control based on q from which we obtain by integration q and q.
These three quantities injected in the right hand side of the unactuated part of the dynamical equation (2) give us the resulting wrench φ c of contact forces.The linear and angular part of the measurement of φ c are then perturbed by a Gaussian colored noise in the high frequency domain with standard deviation σ linear = 10 N and σ angular = 10 Nm, leading to a noisy measurements φc .From φc we obtain the perturbed ZMP position z.
The measurement of the configuration vector q is disturbed by another Gaussian colored noise in the high frequency domain too, with a standard deviation σ configuration = 0.05π.This noise replicates the effects of errors due to motion capture techniques.In addition, we generate an error at the level of the dynamical model of the robot.We add a Gaussian perturbation to the mass distribution of the body and position of the CoM of each link.We make the hypothesis that we know the mass and CoM position of each limb with a precision of 20%.This process aims at generating modeling error for a humanoid robot or for humans due to anthropometric tables.The new dynamical model and the noisy measurements of q enable to generate the geometry-based CoM measurement c.
3) Identification of the mass of the anthropomorphic system: The total mass of the system is directly measurable.It suffices to exploit the forces measurement in static equilibrium (half-sitting position for a humanoid robot or standing rest position for humans), and, by taking the average value of the vertical forces divided by the gravity value, we obtain a good estimate of the total mass.

B. Spectral analysis of measurement errors
Fig. 5 shows the Fourier transform of the errors.The simplest spectral distribution is the error of the force measurement c at the middle of the figure.It is simply the Fourier transform of the noise we added initially, which lies in high frequencies that are partly canceled by our H 2 low-pass filter.At the top of the figure we see the error of geometry-based estimation of the CoM.As expected, the error mainly lies in low and high frequencies.The medium frequencies bandwidth shows a very clean estimation of the CoM position.The bottom part of the figure shows the spectral distribution of the error between the noisy measurement of the ZMP and the perfect linear ZMP of the model.We see clearly that this measurement is reliable only in low frequencies and grows very fast with frequency increase.This is why we fixed the cut-off frequency at 0.4 Hz.We see then that these figures confirm clearly the hypotheses of Fig. 2.

C. Estimation and comparison with KF
The three measurements of the walking trajectory were fed to our complementary filter and to a Kalman filter.The estimation of the complementary filter compared with real values is shown on the top of Fig. 6.We see that the Fig. 5: Fourier transform of the error of each signal.On right, the transform of the error between the real CoM position c and geometry-based estimation c.In the middle, the error between the second CoM time-derivative c and its estimation c.On the left, the transform of the error between the linearized ZMP model ( 6) and the measured values z, defined only in x and y.
tracking in x and y axes is accurate.However, the tracking in z is subject to bias.This is due to the absence of bias correction of ZMP along that axis.On the bottom, the estimation error is displayed for the complementary filter and Kalman filter along the three axes.We see that the error of our complementary filter is always inferior or equivalent to the Kalman filter.We see also that the signal of the complementary filter contains more high frequency noises, that partly due to our choice to take the lowest possible orders for the band-pass filters to keep the simplest possible formulation.We believe that more sophisticated filters can get reduce significantly these artifacts without introducing phase shift.This phenomenon is also due to the fact that there is certainly a small frequency bandwidth where we have no perfectly clean signal.This can be tackled by applying model-based filtering to the estimation, which can also enable to avoid phase-shift, but may be subject to other modeling errors.Finally about this figure, along z direction, we see that Kalman filtering estimation is drifted away from the estimation.This is because the Kalman filter integrates the acceleration signal at all frequencies, introducing drift in medium and low frequency ranges.
We also validate the robustness of our filter with respect to the hypothesis of constant COM altitude, by generating a second walking trajectory whose COM heigh is constrained: it therefore freely derived, mostly in low frequency ranges, which introduces a small error in the frequency range of our ZMP signal.We see in Fig. 6 (right) that along x, this modification only affected the estimation in low frequencies, as expected, whereas along y and z this modification had minor effects.Kalman filter suffered more from this modification, especially along the z direction.
We see in the next section how former studies also considered the sensors fusion for CoM estimation in humanoid robotics and human bio-mechanics communities.We compare the existing approaches with the complementary filtering technique in terms of theoretical guarantees and actual performance.

V. DISCUSSIONS
Our CoM estimation approach is part of an active topic both in research on human motion and in humanoid robotics [15].For humanoids, the corrections on the CoM provided by forward kinematics is achieved mainly using various measurement system [16] including force sensors [17], [18].These solutions use mostly Kalman filtering techniques which is agnostic of the frequency domains of each signal.On the other hand, the CoM reconstruction has a longer history in the field of biomechanics [19].Moreover, since few decades, force platforms were already considered for CoM position estimation [20].Most of the methods did not consider the fusion of Force sensors with direct kinematics reconstruction of the CoM [21], [22].
To our best knowledge, the closest published work to our method is the technique by Maus et al [12].The Kinematic CoM estimation was low-pass filtered and the double time-integral of forces was high-pass filtered, before adding the two signals.However, the use of non-complementary filtering removes the guarantee to obtain the totality of the initial signal with zero-phase-shift.Moreover, the double integration of acceleration is not a stable process and this method requires to reset regularly the integral to zero.Instead, our method works for arbitrary durations thanks to the stability of all our filters.Schepers et al [11] developed the same approach as Maus et al, but with ZMP and force measurements.In addition to theoretical guarantees and integration stability issues, this method neglected the horizontal accelerations for the ZMP, which increases the approximation errors.

VI. CONCLUSION AND FUTURE WORKS
We have seen through this paper the design and the implementation of an estimator of the position of the CoM of humans and humanoid robots based on multi-sensor data fusion.This estimator takes profit from the most common sensors available both on-board robots and for human motion recording: the CoM reconstruction provided by the geometrical reconstruction together with a model of mass distribution, the forces that give CoM accelerations, and the ZMP leading to an approximated linear link between horizontal positions and accelerations.The key idea of our method is to consider that these measurements carry noises and errors, but with separated frequency bandwidths for each error.This statement directs our choice to use a complementary filtering technique to merge these signals, specifically because of its particular suitability to merge different bandwidths of signals.
The direct conclusion is that this complementarity in frequency should be exploited when fusing these three measurements.Complementarity filters are very good at that, while it is not straightforward to achieve with common Kalman filters.Despite the solid expertise of our team in using Kalman filtering for this kind of measurements, we were not able to obtain qualitative results with such filter while it was immediate to obtain when implementing a complementarity filter.
The main limitations of our approach lies in the non correction of biases in the vertical component of the CoM, because the ZMP measure [S3] provides only a 2 dimensional information.Actually, this limitation takes its root from the Newton-Euler dynamics: the moment of a force is obtained by a cross product, and the cross product is not an invertible operator.Therefore, one dimension of the CoM position is not observable using the force/torque signal.However, some CoM trajectories may still make the observation possible if we do not ignore the CoM vertical displacement.An interesting perspective is to build an estimator that aims to rebuild the full body dynamics by estimating variation of the angular momentum and considering CoM height variations.This estimator would also exploit a nonlinear version of the complementary filtering techniques in order to increase its accuracy on the parts to keep in each of the input signals.

Fig. 1 :
Fig. 1: The problem of merging measurements for CoM reconstruction in the presence of noises and modeling errors.

Fig. 3 :
Fig. 3: On the left, diagram of the complementary filter for the CoM vertical component.On the right, the diagram for the horizontal components.

Fig. 6 :
Fig.6: On the left, the reconstructed trajectory thanks to the complementary filter.In the middle, error between the ground truth measure of the CoM position and its reconstruction with Kalman and complementary filters.On the right, error between the ground truth measure of the CoM position and its reconstruction with both Kalman and complementary filters, when allowing variations of the CoM vertical position.