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].
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[Wieber2016].
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 thatis 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.
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].
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 stateand 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 .
The time-invariant ODE defined by (12) verifies the following
It admits two equilibrium points namely the origin and .
All trajectories of (12) converge to one of the equilibrium points defined in item 1.
The equilibrium () is locally asymptotically stable with a domain of attraction containing the set
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
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
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 .
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 .
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 .
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.
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 
. 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, but are often exploited to correct the filtered accelerometer signals using complementary filtering .
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.