Log In Sign Up

Tilt estimator for 3D non-rigid pendulum based on a tri-axial accelerometer and gyrometer

by   Mehdi Benallegue, et al.
Versailles Saint-Quentin-en-Yvelines University
National Institute of Advanced Industrial Science and Technology

The paper presents a new observer for tilt estimation of a 3-D non-rigid pendulum. The system can be seen as a multibody robot attached to the environment with a ball joint. There is no sensor for the joint position of the sensor. The estimation of tilt, i.e. roll and pitch angles, is mandatory for balance control for a humanoid robot and all tasks requiring verticality. Our method obtains tilt estimations using encoders on other joints and inertial measurements given by an IMU equipped with tri-axial accelerometer and gyrometer mounted in any body of the robot. The estimator takes profit from the kinematic coupling resulting from the pivot constraint and uses the entire signal of accelerometer including linear accelerations. Almost Global Asymptotic convergence of the estimation errors is proven together with local exponential stability. The performance of the proposed observer is illustrated by simulations.


page 1

page 2

page 3

page 4

page 5

page 6


Lyapunov-Stable Orientation Estimator for Humanoid Robots

In this paper, we present an observation scheme, with proven Lyapunov st...

Cerberus: Low-Drift Visual-Inertial-Leg Odometry For Agile Locomotion

We present an open-source Visual-Inertial-Leg Odometry (VILO) state esti...

Hybrid Nonlinear Observers for Inertial Navigation Using Landmark Measurements

This paper considers the problem of attitude, position and linear veloci...

Six-DOF Spacecraft Dynamics Simulator For Testing Translation and Attitude Control

This paper presents a method to control a manipulator system grasping a ...

Bionic Sea Urchin Robot with Foldable Telescopic Actuator

This paper presents a couple of interesting ideas: a telescopic actuator...

I Introduction

One predominant goal of robotics is to be able to perform versatile interactions with the environment. In some cases, contact point constitute a link between the floating base of the robot and the environment, one example is legged locomotion, but also environment-related tasks such as torquing or drilling. Most of these tasks require the contact point to remain at a precise position and not to detach or slip. The observance of such a constraint generates a kinematic coupling allowing to model the robot as a kinematic chain attached to the environment with an unactuated joint. This can be simply summarized as a pendulum with the contact as the pivot point.

One main issue regarding this class of systems is that beside the unactuation, there is usually no direct measurement of the configuration of this pivot. Of course, properly estimating this configuration is of crucial importance in most tasks. Nevertheless, several kinds of sensors are sensitive to this configuration, and may be used to estimate it. The most broadly used ones are tri-axial accelerometer and gyrometer. This set of sensors provides invariant signals relative to different rotations around the gravitational field direction. This means that this orientation, usually called yaw angle, is not observable using this sensing system [benallegue2014humanoids].

Figure 1: Top figures: the robot is attached to the environment through a 3D pivot joint. The encoders only provide the configuration in a local frame called control frame and represented by . Bottom figures: In the world frame the configuration of the real robot is rotated compared to its value in the control frame (light wireframe robot). Only the IMU can provide data about this rotation, . We need to account for encoders in this estimation, to distinguish local kinematics from pivot positions. For example the rotation is here identical in the right and the left lower figures with different IMU configurations.

Nevertheless, in robotics there is often a specific need for a precise estimation of the two other degrees of freedom, which can be referred to as roll and pitch angles, or simply tilt. These two degrees of freedom describe the configuration of the pivot relative to the gravitational field. They are then essential for maintaining balance for humanoid robots


In this paper we provide a state estimator which aims at addressing this problem by providing a state estimator for the tilt of a pendulum which uses an accelerometer and a gyrometer, (i) without neglecting linear accelerations compared to gravity, (ii) well suited for articulated robot, and (iii) with a proven Lyapunov stability. Furthermore, this estimator reaches local exponential stability performances, which makes it particularly suitable for the use as a state feedback for closed-loop control. The idea behind this estimator is close to the recent development of Hua et al [Hua2016automatica], who propose a tilt estimation when having a velocity measurement. The gyrometer and the kinematics constitute the velocity measurement in our case. But the estimator we develop is simpler and has better convergence properties in both theoretical and simulation points of view.

The section II presents the issue treated in this paper together with the model of the system and the sensors. The section III presents the development of the state estimator. The section IV analyzes the stability of the estimation error. Section V shows the performances of the estimator in simulation, and finally the section VI discusses the results and the properties of this estimator.

Ii Problem statement

The system is a robot linked to the environment through a ball joint called pivot. Without loss of generality we consider that the pivot is located at the origin of the inertial world frame (). The configuration of the pivot is a pure 3D rotation describing a transformation between the global frame and the local frame of the robot, also called control frame (). We represent this rotation by the rotation matrix . For instance, the sensor located at position and orientation in is actually at and has the orientation in . This problem is sketched in Figure 1.

There is no sensor providing the pivot configuration. Instead, the robot is equipped with an IMU consisting in an accelerometer and a gyrometer, both of them are on three axes. The position of this IMU may be not rigidly linked to the ball joint, and can be located in another body of the robot. Since the robot can modify its actuated joint kinematics the IMU may move in the control frame. Therefore, we have to consider its position

, its orientation represented by the orthogonal matrix

, together with their respective first-order time-derivatives and such that , where

is the skew-symmetric operator.

The values of , , and can be obtained through the positions and velocities of the joint encoders and are often the outcome of a motion controller. Therefore, these values are considered to be perfectly known.

The accelerometer provides the sum of the gravitational field and the linear acceleration of the sensor, expressed in the sensor frame. In other words we have


where , , , and

are respectively the accelerometer measurements, the position and the orientation of the IMU, standard gravity constant and a unit vector such that

is the gravitational field.

The gyrometer provides the angular velocity of the IMU, expressed in the sensor frame. In other words


where is the angular velocity vector of the sensor in the global frame such that .

We can see from these equations that the measurements are invariant regarding rotations around the vector aligned with the gravitational field. Therefore, the orientation that can be estimated through this sensing system is incomplete. Nevertheless, we show here that one partial information is observable and consists in , the direction of the gravitational field in the local frame of the robot. This data is the most important variable required to control balance and may be considered as a measure of “verticality” in general.

By replacing and by and respectively and performing time-derivations and identification with (2) and (1) obtain


where is the angular velocity vector of the pendulum such that .

In the following section we develop the state observer for the estimation of .

Iii State estimator

Iii-a State definition

The first variable we define is the pivot angular velocity expressed in the control frame . Replacing this in (3) we have


and since all the rightmost variables are known or measured we may consider as measured.

Let’s define also the following state variables:


with and , with the set is the unit sphere centered at the origin, and defined as

The variable is the state we aim at estimating and cannot be obtained algebraically. On the contrary, the variable is considered measured since we know , and , and is the opposite of the linear velocity of the IMU expressed in the local frame . In this study we use this data to build a tilt estimator able to distinguish gravity from accelerations, similarly to [Hua2016automatica].

By left-multiplying Equation (4) by and replacing the expression of of equation (5) we get


We notice that the left member of equation (8) is the first order time-derivative of . This, together with the time-differentiation of , provide us with the following dynamic equations


The system ((9)) is suitable for the observer synthesis.

Iii-B State-observer and error dynamics:

In order to estimate , we propose the following state-observer


where , are positive scalar gains which verify the condition and and are the estimations of and respectively.

The initial value of should be in . Then the dynamics of the last equation ensures that the norm of this vector remains constant in time. The initial value for on its side could be anywhere in .

We define the following estimation errors and , a time-differentiation of these expressions provide us with the following error dynamics:


To run the analysis of errors, we set . We notice also that and , we obtain this new error dynamics


The nice property of this new dynamics is that it is autonomous and defines a time-invariant ordinary differential equation (ODE) which simplifies drastically the stability analysis. In fact, if one define the state

and the state space with , one can write (12) as where gathers the right-hand side of (12) and defines a smooth vector field on .

Iv Stability analysis

Iv-a Asymptotic stability

Let’s consider the following positive-definite differentiable function


which is radially unbounded over .

Theorem 1

The time-invariant ODE defined by (12) verifies the following

  1. It admits two equilibrium points namely the origin and .

  2. All trajectories of (12) converge to one of the equilibrium points defined in item 1.

  3. The equilibrium () is locally asymptotically stable with a domain of attraction containing the set

  4. The system (12) is almost globally stable with respect to the origin in the following sense: there exists an open dense subset such that, for every initial condition , the corresponding trajectory converges asymptotically to ().

Let’s prove the four items of the theorem

  1. The equilibria are calculated by solving the equation , where is the nonlinear function describing (12), we get the following


    The trivial solution is and the second solution is calculated if we consider that , so we can write


    We know that , so the only solution of (17) is , which gives from (16) that . This completes the proof of item 1.

  2. The time derivative of (13) in view of (12) yields


    where , one easily verifies that within the gain condition we have if is not an equilibrium. Since (12) is autonomous and is radially unbounded, one can use LaSalle’s invariance theorem. Therefore, every trajectory converges to a trajectory along which .

  3. Since is non-increasing, at , implies that for every . Since the trajectory converges to one of the two equilibrium points, it must be () because this is the only one contained in .

  4. The linearized system around the equilibrium is given by the following dynamics


    with and is a constant matrix having the form


The characteristic polynomial of the matrix is given by


We find that this polynomial has at least one positive root, which is given by


which means that the equilibrium is unstable. This completes the proof of the theorem.

Iv-B Local exponential convergence

From equation (18) we can write the following


In order to find the conditions of exponential convergence, let’s observe the following relations


In the case of at , we can say it exists a fixed such that , since the equilibrium which correspond to is non attractive, so we can write the following


which can be written as


which gives the following inequality


This leads to the local exponential convergence of the errors to the equilibrium .

V Simulations

In this section, we present simulation results showing the effectiveness of the proposed estimator. We generated the signal with trigonometric functions and generated the trajectory of by integration. Figure 2 shows time plot of represented by roll, pitch and yaw angles. We generated the trajectory of by integrating the signal which is the sum of filtered noise and a linear feedback loop to maintain around the value . Finally we generated signals using trigonometric functions and obtained and trajectories by integration. Afterwards we generated the measurement signals for accelerometer and for gyrometer using equations (3) and (4).

We have considered for the simulations the initial conditions for the estimator which correspond to the initial errors and . The parameters of the estimator have been chosen as and , so the condition (

) is verified. We performed two simulation tests, one without considering noise and one with white centered Gaussian noise with standard deviation of

(normalized) added to the three elements of vector measurements and with standard deviation of (normalized) added to the three elements of vector measurements .

Figure 3 on top and bottom shows the evolution of the estimation errors without noise and with noise, respectively. Figure 4 and Figure 5 show the estimation tracking of the variable and the estimation errors with respect to time, without and with noise respectively. We can see that the estimation error converges to zero in about one second. For the noisy case, even if the estimation error shows some sensitivity, we see that the error filters this noise in a relatively efficient way. These two figures, 4 and 5 compare also our results with the observer of Hua et al [Hua2016automatica] with equivalent gains (, with parameters of their estimator), labeled as ‘comparison’. We see clearly that the performances of that estimator are not as good, in both clean and noisy cases, as the presented one, with twice longer convergence times.

Figure 2: Plot showing the values of the orientation of the IMU in the control frame expressed using Euler angles (blue: roll, red: pitch, orange: yaw)
Figure 3: Plot showing the estimation error for . On the top, the case where there is no noise, and on the bottom the noisy case. The colors blue, red, orange represent the three components of this vector respectively.
Figure 4: Estimation of in the case without noise. On the top, three plots showing a comparison between respective components of in blue, its estimation in red and a comparison with Hua’s observer in yellow. On the bottom we see the evolution of our estimation error. The colors blue, red, orange represent the three components of this vector respectively.
Figure 5: Estimation of in the noisy case. On the top, three plots showing a comparison between respective components of in blue and its estimation in red and a comparison with Hua’s observer in yellow. On the bottom we see the evolution of our estimation error . The colors blue, red, orange represent the three components of this vector respectively.

Vi Discussion and conclusion

Attitude estimation is a topic of active research, especially when IMU signals are used. Accelerometers are at the core of this problem mainly because their signal contains the value of the gravitational field in the frame of the sensor. In static cases, this property allows for algebraic tilt measurements. However, in the dynamic cases, this measurement is mixed with the linear acceleration in an algebraically indistinguishable way. In many works the acceleration is considered negligible compared to gravity field [Zheng2010], and is therefore considered as a noise. Filtering approaches are commonly used to remove this signal [1273529]

. Accelerometers are also commonly used together with gyrometers. Gyrometers provide rotation velocities in the local reference frame. Their signals are commonly merged with accelerometers using Kalman Filtering 

[803999], but are often exploited to correct the filtered accelerometer signals using complementary filtering [4608934].

Several other works rely on the presence of additional data to reconstruct the attitude. For instance, magnetometers [metni2006attitude] or vision [martinelli2012vision] can be used to retrieve redundant attitude signals allowing to reduce the effect of accelerometer errors. Finally, a fusion with external measurements such as GPS [caron2006gps] or landmark relative position [vasconcelos2010nonlinear] allow to better distinguish the linear acceleration from gravitational field measurements and allows to observe the linear part of the kinematics.

We see through this summary that the translational component of the motion of the IMU is commonly considered either as a noise that requires to be deleted or as an independent dynamics which needs to be observed. However, in the specific case of the pendulum, this linear part of the kinematics is coupled with the angular motion which explains the presence of the angular velocity and event angular acceleration in the signals of the accelerometer (see Equation (4)). This enables us to use this signal without any need of filtering and to still be able to reconstruct tilt despite a high level noise level. The translation-rotation coupling is entirely due to the presence of the anchor point of pivot. However, in several works addressing cases similar to pivot link position estimation are still resorting to classical methods where the IMU is considered as an unconstrained floating object, even if the reconstructed attitude are merged with encoder data afterwards [khandelwal2013estimation]. It is worth to note that in addition to orientation, the orientation estimation of a pendulum provides also data on the position of the limbs of the robot, because of the pivot constraint. This relationship allows also to design position controllers on the base of attitude estimators, similarly to hand position compensation presented in [benallegue2014humanoids].

Only few works dealt with attitude estimation taking into account the pivot constraints. One example is the tilt estimation for rigid pendulum around the pivot using multiple accelerometers [Trimpe2010]. This observer was used especially for balancing the reaction wheel cube on edges and corners [Gajamohan2013]. In addition to the requirement of multiple accelerometers at different locations is only limited to rigid pendulum cases. Another work from legged robotics community considers also contact information [Bloesch-RSS-12]. This estimator considers the case of multiple contacts and uses an extended Kalman Filter. The contact information is introduced in the model kinematics but only at the prediction step rather than as a constraint. Their model is intended to take into account the cases of contact slippage, but this variable is not observable using inertial sensors. Another work uses also extended Kalman Filtering for a humanoid robot having flexible contacts with the environment [benallegue2014humanoids]. The contact information was introduced as pseudo measurements in order to allow the pivot constraint to be slightly violated. This observer was extended to take into account the dynamical model of the flexibility [mifsud:hal-01142399]. However the use of extended Kalman filtering only provides the guarantee of optimality around the linearized dynamics around the predicted state and gives no proof of convergence.

To our knowledge, our estimator is the only one providing almost globally convergent tilt estimation for non-rigid pendulum system. In fact the only work we know which can be adapted to these cases is the observer of [Hua2016automatica] where velocity data are used to allow tilt estimation. However, this estimator is more complex than the observer we propose and its convergence properties are weaker, since no minimal set is provided to guarantee the exponential convergence. We could see also that their gain condition for stability lead the system to converge slower than our estimator.

With this kind of estimators the only orientation data missing is the orientation around the gravitational field direction, or yaw angle. This orientation is proven to be out of reach of this measurement system. Therefore, the involvement of other sensors such as magnetometers are necessary to obtain this estimate. The addition of this kind of sensors is the topic of a possible improvement of the presented method.

Finally, the introduction of a model for the dynamics of the pivot could also increase the quality of the observation, specifically by creating coupling between the measurement data of the IMU and other values which are non-observable otherwise. These values include yaw angle without needing additional data, but may go to the estimation of contact forces with the environment [mifsud:hal-01142399]. This is also the topic of next developments regarding this kind of systems.