Experimental Comparison of Velocity Observers: A Scissored Pair Control Moment Gyroscope Case Study

We consider scissored pair gyroscopes as an auxiliary stabilization system for legged robots. A standard state-feedback control can stabilize the system if all states are available; thus, the velocity estimation becomes the key element for the stabilization. To this end, we implement and experimentally compare a model-free linear differentiator, a model-based full-state linear observer with time-varying gains, and a model-based homogeneous nonlinear differentiator. Moreover, we also show that the considered system cannot be partially linearized via a change of coordinates, and thus is not suitable for a recently reported class of nonlinear observers. The proposed designs are tested on an experimental scissored pair control moment gyroscope setup constructed for a walking robot stabilization.


I. INTRODUCTION
Bipedal walking robots are one of the fastest rising areas of development and research in robotics. The control must provide various modes of movement in a wide range of enviroment (inclined planes, staircases, obstacles of variable height, wet surface, uneven terrain, non-rigid surface etc.). Even more, the control must take into account the wear of the electromechanical and hydraulic components while maintaining the stability.
Designs of bionic robots try to imitate the knee, the hip joint and other articulations by servomotors. Bionic structures require the reproduction of multiple, including physiologically unexplored psychomotor functions, that stabilize the body during the gait. The complexity of muscle and joint systems creates great structural, dynamic and computational difficulties for the synthesis of control signals. A significant disadvantage of the bionic movements is the difficulty of The associate editor coordinating the review of this manuscript and approving it for publication was Tao Wang . accounting for nonlinear effects associated with equipment wear, backlash, and changes in friction characteristics. In the existing implementations bionic mechanisms do not provide the required quality of motion / maneuverability. It is important to note that the bionic robotics comes along with the high cost, exceeding practical applications and reasonable manufacturing prices.
In fact, mimicking the nature is equivalent to chasing (inevitable!) mechanical imprecisions: a human's gait is very precise thanks to the quality of his joints. But if he ever tores one of his ligaments, the stability of the articulations is immediately compromised. We do not want to mimic animals completely: at the moment we do not know how to build effective muscles; backlash in artificial joints is almost inevitable.
We want to develop auxiliary stabilization systems that would allow for a better control. For example, by placing a control moment gyroscope ontop for a robot, we would provide an additional support point without touching the rest of the mechanical system. Control moment gyroscope is a FIGURE 1. We are developing a biped that uses four control moment gyroscopes (highlited in red) as an auxiliary stabilization system. widely used technological device that uses the reaction of a spinning wheel to external torques. Due to the advantages of a large ratio of produced torque to control torque and relatively low power consumption, CMGs have a wide range of applications, including bicycle stabilization [1], vessel stabilisation [2], motorcycle and robot balancing [3], balancing aid for humans and bipedal exoskeletons [4], [5], attitude steering system for the satellites [6] and underwater vehicles [7].
The research problem of this paper is motivated by the walking robot we are currently developing in the Laboratory of Robotics and Mechatronics of the Kuban State University (refer to Fig. 1). This non-anthropomorphic robot has an auxiliary dynamic stabilization system which consists of two scissored pairs of control moment gyroscopes (CMG). The scissored pairs are orthogonal and thus the problem of vertical stabilization of the robot can be considered for each axis separately. Therefore, stabilization of the robot for one axis can be approximated with a simplified prototype. In this paper we consider such a prototype, namely a scissored pair control moment gyroscope inverted pendulum (Fig. 2). Note that the robot has a modular design: the biped is equipped with four identical CMG cubes.
The key element for stabilization of an inverted pendulum is velocity estimation. Note that our robot uses multiple accelerometers to estimate its tilt angles [8], and we rely on soft sensors for velocity estimation. Velocity estimation for mechanical systems is a well-known problem, and there exist numerous results on observers reconstructing the whole state vector, for both linear and nonlinear systems, see, for example, the recent work by Aranovskiy et al. [9] and the references therein for model-based observers and the work [10] for a recent neural-network based observer example. However, a common engineering solution is to consider the velocity estimation problem for each degree of freedom separately rather than to estimate all velocities with a single observer. From the signal processing point of view, this approach can be considered as numerical differentiation, where velocity estimation is seen as online differentiation of a measured position signal, e.g., a first-order difference used in [11]. While differentiator-based velocity observers can be designed model-free, a better performance is typically obtained when observers use (at least partially) available model knowledge: sliding-mode exact differentiators FIGURE 2. Left: inverted pendulum hardware that we study in this paper. The pendulum consists of six bodies: body A (the thrust bearing), body B (the frame), bodies C 1 and C 2 (the single-axis gimbals), and, finally, bodies D 1 and D 2 (the flywheels). Right: corresponding notations. This configuration corresponds to q b = −π/4 and q 1 c = q 2 c = π/2. first proposed by Levant in [12], high-gain differentiators as described by Vasiljevic and Khalil in [13], and differentiators proposed by Perruquetti et al. [14] that ensure finite-time convergence and robustness with respect to measurement noises and uncertainties. In this paper, we aim at practical comparison of some of the discussed observers. The contribution of this paper is two-fold: first, we give a rigorous proof that, despite its simplicity, the mechanical system under consideration does not belong to the class of recently reported systems [9], for which a construction of model-based nonlinear observers is known. Next, we propose a comparison of three velocity observers, namely a model-free differentiator, a model-based linear observer and a model-based nonlinear differentiator. All three observers are tested on the hardware we have built for this purpose, and the experimental results are provided.
The preliminary results of this research are reported in [15]. Extending that work, this manuscript presents i) more challenging mechanical system and motion dynamics with the corresponding hardware, ii) the proof that the system does not belong to the class PLvCC [9], iii) the velocity estimation using an observer with time-varying gains, and iv) detailed and extended experimental studies.
The rest of the paper is organized as follows. In section II we present a model of the considered system. Next, in section III we present the state-feedback controller capable to stabilize the system if all states (including velocities) are measured. Then, in section IV we discuss three velocity observers. Hardware experiments and observers comparison are provided in section V. Finally, possible further research directions are discussed in the concluding section VI.

II. MODEL DESCRIPTION
The considered inverted pendulum is shown in Fig. 2. In this paper we follow the notations from the manual of the model 750 control moment gyroscope commercialized by Educational Control Products company [16]. The pendulum consists of six bodies: body A (the thrust bearing, shown in green), body B (the frame, shown in blue), VOLUME 8, 2020 bodies C 1 and C 2 (the single-axis gimbals, shown in red and orange), and, finally, bodies D 1 and D 2 (the flywheels, shown in brown and black). We associate a basis with each of the bodies: We assume that all the bodies are symmetric; so the inertia matrices are diagonal. Let us denote by diag(I c , J c , K c ) the inertia matrix (w.r.t the center of mass) of the bodies C 1 and C 2 ; and by diag(I d , J d , K d ) the inertia matrix of the bodies D 1 and D 2 . In the same manner, we denote the inertia matrix of the body A as diag(I a , J a , K a ). As for the body B, the frame consists of two identical cubes, and we denote by I b , J b and K b the principal moments of inertia of each half of the frame w.r.t. the center of mass of each cube.
We denote by q a the angle of rotation between the ''north'' direction and the body A; the angle of the body B with respect to the vertical is denoted as q b ; the angle of the bodies C 1 and C 2 with respect to the body B are denoted as q 1 c and q 2 c ; and the angle of the bodies D 1 and D 2 with respect to the body C 1 and C 2 are denoted as q 1 d and q 2 d . The configuration shown in Fig. 2 corresponds to the angles q b = −π/4 and q 1 c = q 2 c = π/2. Note that the equilibrium position of an actual robot depends on the configuration of the legs and may be subject to external disturbances; therefore, for our cube we model the equilibrium point as q b = −π/4−e, where e is the unknown (small) bias. The notations and the corresponding hardware parameters are summarized in the Table 1.
With these definitions, the vectors of angular velocities can be found as: 1 where R AB , R BC and R CD are the transformations between the bases of corresponding bodies: Then the parallel axis theorem allows us to write the total kinetic energy of the system as: The potential energy is given by P = √ 2mlg cos(q b + π/4 + e). Therefore the Lagrangian can be written as L = T − P. The idea is to apply torques τ 1 c , τ 2 c , τ 1 d and τ 2 d to the bodies C 1 , C 2 , D 1 and D 2 , respectively; thus we can find the Euler-Lagrange equations: These equations are very cumbersome, but we will shortly simplify the system. To this end, we introduce the following hypotheses. 21696 VOLUME 8, 2020 Hypothesis 1: A scissored pair control moment gyroscope implies a symmetric control for the gimbals, the idea behind it is to cancel parasitic impact on the body A. Thus we enforce the following constraints: Hypothesis 2: The initial conditions are also symmetric for the bodies C 1 , C 2 and D 1 , D 2 : The initial conditions of the body A are q a (0) =q a (0) = 0. Hypothesis 3: For control moment gyroscope systems, the nominal operation mode assumes |q d | max(|q a |, |q b |, |q c |), and regulation of the velocityq d is performed by the means of local motor controllers. We assume that these controllers provide fast and accurate velocity regulation ensuring thatq d is constant and equals to the nominal velocity, see Table 1, thusq d ≈ 0. The symmetry of the initial conditions and the control introduced in Hypotheses 1 and 2 results in the following symmetry of the signals, allowing us to reduce the cumbersomeness of the equations of motion: Going further, it is easy to see that Hypothesis 1 (symmetric control of the scissored pair) and the initial conditions (Hypothesis 2), imply that the body A is not subject to any torque: q a (t) =q a (t) = 0.
The above assumptions allow to simplify the system (1) and write it down explicitly: where . In this expression we use the rotational symmetry K d = I d of the disk D.
In practice it means that our scissored pair (Fig. 2) can be thought of as a single gimbal 1D inverted pendulum (refer to Fig. 3). If these Hypotheses 1-2 are violated, than the approximation error will appear as un unknown function on the left-hand side of the simplified model (2). However, the experimental studies (Section V) show that the imposed assumptions are reasonable, thus allowing for for satisfactory velocity estimation.
This simplified system contains only three bodies B, C and D, thus greatly simplifying the analysis. Note that even if the constraints allow us to analyze the 1D pendulum, all the experiments presented in the Section V are performed on the hardware shown in Fig. 2.
Moreover, in our hardware the gimbals C 1 and C 2 are controlled by the means of servo drives that ensure velocity tracking. In this case, the velocity ω c can be considered as an input signal. Under these assumptions the system (2) can be rewritten asq

III. LINEARIZATION-BASED STABILIZATION
For the system (3), the desired equilibrium is defined as In what follows we say that a control law (locally) stabilizes the system (3) if under this control law the point 0 is (locally) attractive.
To proceed, it is convenient to define a new state variable as a deviation of the actual position from the desired equilibrium, x := q b ω b q c − 0 . Then the system (3) can be rewritten aṡ where u = ω c . The state variable x 3 can be computed through the measurements of the signal q c ; however, since the offset e is not known, the signal x 1 can not be computed. Thus, we define VOLUME 8, 2020 the vector of measurements as Then the control goal is to find a control law that stabilizes (4) at the origin using the measurements y.
A common practice to stabilize a (sufficiently wellbehaved) nonlinear system is to linearize the system around the desired equilibrium. For the system (4) such equilibrium is given by x = 0, u = 0. Let us define Then the linearization of (4) around the origin is given bẏ It is tempting to use the state-feedback static control where K := k 1 k 2 k 3 is the gain vector. The problem, however, is that the equilibrium point would e. Therefore, the stabilization goal |x| → 0 is not achieved under the control law (7). In order to overcome the nonzero steady-state body C angle problem, we add an integral action. To this end, we introduce an auxiliary variable x e defined aṡ x e := 0 − x 3 , where zero stays as the reference for x 3 . Then the extended state-space model is The pair A e , B e is controllable and we can design the extended control law where K e ∈ R 1×4 . Then the closed-loop dynamics is Thus, the state x converges to zero, while the integral action x e ensures the equilibrium offset compensation. However, to implement the control law (8), it is required to estimate the velocity ω b . In the next section we discuss possible velocity observers for the considered system.

IV. OBSERVERS DESIGN
A model-based nonlinear observer has been recently proposed in [9] for a class of mechanical systems that are partially linearizable via coordinate changes (PLvCC). The observer can be applied to the mechanical system (2) if it admits a change of coordinates allowing to rewrite the system in such a form that the dynamics is linear in momenta. Then a globally converging exponential momenta observer can be constructed that yields velocity estimation. Unfortunately, despite its simplicity, the considered system does not belong to this class; this claim is rigorously proven in the Appendix .
In this section, we present three designs that are suitable for the considered system, namely a linear model-free differentiator, a linear time-varying observer, and a nonlinear model-based differentiator. The model-free linear differentiator is based on the methods of linear time-invariant system analysis such as Laplace transformation and frequency-domain analysis. This approach is also known as filtered derivative since it can be seen as a serial connection of the (non-realizable) pure differentiator with a low-pass filter. The second approach is motivated by the observability property analysis for linear time-varying systems and Kalman-Bucy filtering. To apply this approach, the considered nonlinear system is rewritten as a linear time-varying one; thus, the estimation error dynamics is also linear and time-varying. Finally, the third method is the nonlinear velocity estimator motivated by the recent advances on homogeneous systems with finite-and fixed-time convergence; the resulting error dynamics of the observer is nonlinear.

A. A LINEAR DIFFERENTIATOR
The simplest way to get the velocity estimation is to use a linear differentiation; however, it is well known that differentiation can be noisy at high frequencies. To this end, a common engineering practice for the velocity estimation is to use a low pass filter together with the differentiator. This can be used to reduce the consequences of noise in the signal but care is needed to ensure that the phase lag does not distort the results. For a 2nd order filter with a time constant τ , the continuous-time transfer function of the model-free differentiator can be written as G(s) = s (τ s+1) 2 , yielding the following state-space realizatioṅ wherex ld 2 is the estimate of x 2 provided by the differentiator.

B. A FULL-ORDER LINEAR TIME-VARYING OBSERVER
One classic solution for linear time-varying systems is to construct a linear time-varying observer in the 21698 VOLUME 8, 2020 wherex lo 2 is the estimate of x 2 provided by the linear observer, the vector of measurements y is defined in (5), C = 1 0 0 0 0 1 and the time-varying symmetric gain matrix H (t) ∈ R 3×3 is the solution of the matrix differential equatioṅ for some H (0) = H 0 > 0, and Q > 0 is the design parameter. It is known (see, e.g., [17], [18]) that the observer (10) ensures exponential convergence if the system is uniformly observable, i.e., there exist T O , δ 1 , and δ 2 , all positive, such that for all t where (·, ·) is the state-transition matrix of the linear time-varying systemẋ = A (y, u)x. This condition is satisfied for the considered system since the pair A , C is observable for all values of u, y 2 .
Define the estimation error asx := x −x. Then in the absence of the bias, i.e., when e = 0, the error dynamics becomesẋ = A − LC x, where L(t) := H (t)C , and the convergence ofx to zero is ensured.
Then is straightforward to verify that the equilibrium of the closed-loop system satisfies x = 0, i.e., the integral action compensates the offset being used in the loop with the state observer even if the observer provides a biased estimate.

C. HOMOGENEOUS FINITE-TIME DIFFERENTIATOR
Following Perruquetti et al. [19], the model-based homogeneous differentiator for the states x 1 , x 2 of the system (4) is constructed aṡ wherex hd 2 is the estimate of x 2 provided by the homogeneous observer, e b =x 1 − y 1 and e b α = |e b | α sign(e b ). In [19] it is shown that if k 1 and k 2 are such that the polynomial s 2 + k 2 s + k 1 is Hurwitz and α ∈ ( 1 2 , 1], then the estimation errorx − x converges to a vicinity of the origin. More precisely, convergence to the vicinity instead of the finite-time convergence to the origin follows from the replacement of x 2 and sin(x 1 ) in f 2 (x, u) in (4) withx 2 and sin(y 1 ) = sin(x 1 + e), respectively. A more detailed analysis of the size of this vicinity can be performed by the means of Lyapunov function analysis in a similar way as in [20]; however, such a result is rather technical and is not presented here for brevity.

V. EXPERIMENTS AND COMPARISON
As we have already mentioned, even if our assumptions allow us to analyze the 1D pendulum, all the experiments presented in this section are performed on the scissored pair. 2 The hardware for the tests (shown in Fig. 2) is assembled from off-theshelf components. The hardware parameters are summarized in the Table 1. A STM32F746 discovery board was chosen as the main computing unit. We have chosen small brushless motors to drive the wheels D 1 and D 2 , the gimbals C 1 and C 2 are actuated by Dynamixel MX106-R servo motors. We use two accelerometers to measure y 1 as it was proposed in [8], and we have installed a fiber optic gyroscope to have the ground truth while comparing the soft sensors that estimate the signal x 2 .
To compare the considered estimators on the stabilization task, we first tune the gains of the estimators to have comparable open-loop performance. To this end, we utilize the velocity measurement provided by the fiber optic gyroscope and close the loop with the control law (8). Fig. 4(a) provides the angle measurements y 1 and the corresponding control signal u. Note that after two seconds of the experiment an external force was applied for a short period of time. Using the collected data, we have tuned all three soft sensors to fit the best the measured signal x 2 ; estimated velocities are provided in Fig. 4(b). Table 2 summarizes the control gains as well as the parameters of the soft sensors.
At the next step, the previously tuned observers have been compared in the closed-loop stabilization task. To this end, the of fiber optic gyroscope has be disconnected, and the estimatex 2 provided by soft sensors has been used instead of the signal x 2 . The results provided in Fig. 5, 6 and 7 correspond to the velocity estimators (9), (10) and (11), respectively.
The presented experimental results illustrate that the best performance can be obtained using model-based nonlinear differentiator (11). Being model-based, it outperforms the model-free linear differentiator (9), and at the same time it outperforms the model-based linear observer (10). The latter is partially dues to the significant numerical computations required for implementation of the observer (10) as discussed in Remark 1.
Thus, the experimental results support the choice of the model-based nonlinear differentiator (11) as a soft sensor for velocity estimation in the considered application.
Remark 1: It should be emphasized that implementation of the linear time-varying observer (10) in embedded systems is non-trivial; first of all, it is computationally heavy, since the calculation of the gain matrix H (t) requires to solve online several differential equations with quadratic terms that are sensitive to numerical methods. Moreover, for our particular system the gains in the matrix Q (refer to the Table 2) have an asymmetry of six orders of magnitude, thus amplifying the numerical errors.  Note that we have tried to implement a linear time-invariant observer (it can be seen as a particular case of the observer (10)), but we failed to achieve the stabilization, thus we are not reporting these experimental results here. 21700 VOLUME 8, 2020 FIGURE 7. Closed-loop stabilization using observer (11).

VI. CONCLUSION
The theoretical results, obtained and confirmed by experimental studies, can be used to stabilize walking biped robots. The developed nonlinear differentiator is able to stabilize the movement of the robot described in the introduction and shown in Fig. 1.
At a further research direction, we intend to apply the designed nonlinear differentiator for the biped system described in Introduction, Fig. 2. At this step, we also intend to incorporate a friction model into the dynamics of the model-based velocity estimator to reduce the modeling errors in the system.

APPENDIX.
In [21] it is proven that a mechanical system with inertia matrix M and n degrees of freedom is partially linearizable via a change of coordinates (PLvCC) if there exists a full-rank matrix : R n → R n×n such that for all i = 1, . . . , n the matrices B (i) are skew-symmetric, where Here i is the i-th column of , ij is the element of with the indices i and j, and i , j = ∂ j ∂q i − ∂ i ∂q j is the Lie bracket.
The system (2) can be written in the standard mechanical system form as M (q)q + C(q,q)q + G(q) = τ , where the inertia matrix is M (q) = m 1 (q 2 ) 0 0 1 , with m 1 (q 2 ) := To satisfy the PLvCC property, the matrices B (1) and B (2) must be skew-symmetric. The equation B (2) 2,2 = − ∂d ∂q 2 = 0 dictates that d does not depend on q 2 , i.e., it is a function of q 1 only. Now let us focus on the following subsystem: Since m 1 (q 2 ) > 0 for all q 2 , we have: The trivial solution is d = 0 with b = b 0 , but it leads to a singular matrix ; let us check if the system has non-trivial solutions. We can differentiate both equations: The symmetry of second derivatives leads to: Therefore, it leads us to the condition Since m 1 (q 2 ) is a known function, it is easy to verify that this condition does not hold, and therefore the above system does not admit any (non-trivial) solution. Thus we conclude that this mechanical system is not PLvCC. Since 2008, he has been an Associate Professor with the University of Lorraine. Besides the control theory, his research interest includes digital geometry processing. More specifically, he is interested in parameterization techniques, meshing, and reconstruction of objects from 3D point clouds. These applications include oil exploration, plasma physics, biochemistry, and computer-aided design. He is currently the Head of the PIXEL Research Team, INRIA, French National Research Institute for the Digital Sciences. 21702 VOLUME 8, 2020