A Digitalized Gyroscope System Based on a Modified Adaptive Control Method

Abstract

In this work we investigate the possibility of applying the adaptive control algorithm to Micro-Electro-Mechanical System (MEMS) gyroscopes. Through comparing the gyroscope working conditions with the reference model, the adaptive control method can provide online estimation of the key parameters and the proper control strategy for the system. The digital second-order oscillators in the reference model are substituted for two phase locked loops (PLLs) to achieve a more steady amplitude and frequency control. The adaptive law is modified to satisfy the condition of unequal coupling stiffness and coupling damping coefficient. The rotation mode of the gyroscope system is considered in our work and a rotation elimination section is added to the digitalized system. Before implementing the algorithm in the hardware platform, different simulations are conducted to ensure the algorithm can meet the requirement of the angular rate sensor, and some of the key adaptive law coefficients are optimized. The coupling components are detected and suppressed respectively and Lyapunov criterion is applied to prove the stability of the system. The modified adaptive control algorithm is verified in a set of digitalized gyroscope system, the control system is realized in digital domain, with the application of Field Programmable Gate Array (FPGA). Key structure parameters are measured and compared with the estimation results, which validated that the algorithm is feasible in the setup. Extra gyroscopes are used in repeated experiments to prove the commonality of the algorithm.

1. Introduction

MEMS gyroscopes are a kind of angular rate sensor widely used in the fields of navigation, motor vehicles, and mobile devices. They provide inertial angular rate measurements with low cost and low power consumption [1,2]. Recent years have witnessed the development of adaptive control methods for MEMS gyroscopes. The application of an adaptive control algorithm can provide on-line estimation of input angular rate and other parameters, thus it has fine robustness features against parameter variances and disturbances.

Park has proposed an adaptive measurement mode for operating MEMS gyroscopes, and formulated a unified methodology for the synthesis and analysis for the algorithm [3]. The algorithm can realize online estimation and compensation of varied defects and disturbances that influence the behavior of a MEMS gyroscope. He also extended the adaptive control method to mode-tuning and angle measurement applications, which can directly measure the rotation angle without integration of the angular rate [4,5]. John presented the concept of an adaptively controlled single-mass tri-axial angular rate (AR) sensor [6]. The single mass is free to move in three directions, thus a tri-axial perfect oscillator model is chosen as a reference. According to the control law, the coefficients of damping, stiffness and input angular rate can be completely estimated. To ensure the parameter convergence in the continuous domain, a modified trajectory algorithm is presented with the aim to reduce the discretization errors. Fei adopted sliding mode control in MEMS gyroscopes [7,8,9]. A proportional and integral sliding surface is defined and applied to calculate the control forces of different axes. In order to eliminate the chattering, the discontinuous control component is replaced by a smoothing sliding mode component. The algorithm is also updated by the robust sliding mode control method and applied on the tri-axial gyroscope model. Leland developed Lyapunov-based adaptive controllers for MEMS gyroscopes to compensate the uncertainty in the natural frequencies, mode coupling and damping. The controllers are tested under averaged low frequency model and full gyroscope model conditions, respectively [10,11]. Dong presented a new adaptive control method based on Active Disturbance Rejection Control (ADRC), which can precisely estimate and compensate the disturbance on each axis, and is applied on a vibrational beam gyroscope [12].

Most of the reported control methods remained at the simulation stage, and others are implemented on analog circuits. Thus, for the first time, we look into the possibilities of realizing the adaptive control algorithm on a digitalized gyroscope system. Once the algorithm is implemented in the digitalized setup, the measured angular rate will not be affected by variance of the coupling coefficients or the quality factor of the gyroscope, thus the system has better adaptivity for different gyroscopes under different conditions.

The algorithm presented in this paper is based on a model reference adaptive control algorithm, and the adaptive control algorithm is further modified based on previous work. The adaptive law is redesigned to estimate the asymmetric coupling parameters and the reference model is replaced with two PLLs for FPGA implementation. Besides, the rotation mode of the gyroscope is considered in this manuscript and the corresponding elimination section is designed. This paper is organized as follows: in Section 2 the structure diagram and the dynamic characteristic of the gyroscope system are first described, and next in Section 3 the adaptive control method with the robust resistance to z-axis rotation mode is finally presented. Simulation results and the parameter estimation process are then discussed in Section 4, and finally the algorithm implementation on the FPGA-based digitalized gyroscope system is demonstrated in Section 5.

2. Gyroscope System Description

A capacitive silicon micromachined gyroscope is used in this work. The designed z-axis gyroscope is a double decoupled single proof mass gyroscope. It is a two-layer structure which mainly consists of the drive mode components, the sense mode components, the proof mass, the damping elements and the glass substrate, etc. The details of the gyroscope including its packaging, comb fingers, coupling and decoupling springs are displayed in Figure 1a.

Figure 1.

Description of the gyroscope used. (a) SEM photo and packaging details of the gyroscope; (b) Schematic diagram of the gyroscope rotation mode.

The gyroscope is fabricated by the silicon-on-glass (SOG) process. The upper layer of the gyroscope is the silicon structure ICP etched through the single crystal silicon bulk micromachining, while the lower layer is the glass substrate which is anodically bonded with the silicon structure. The gyroscope used in this work has a resonant frequency of 3195.1 Hz in drive mode and 3105.9 Hz in sense mode. The symmetric structure of the gyroscope is especially suitable for the implementation of the adaptive control algorithm proposed in [4]. The detailed structure parameters of the gyroscope in this work are listed in Table 1.

Table 1.

The structure parameters of the gyroscope.

Parameter	Value (unit)
Structure thickness	65 (μm)
Proof mass size	2500 × 2500 (μm2)
Equivalent drive/sense mass	1.39 × 10−6 (kg)
U-shaped spring width	10 (μm)
Drive/sense U-shaped spring length	520 (μm)
Drive/sense beam size	2500 × 70 (μm2)
Comb finger width	4 (μm)
Comb finger length	40 (μm)
Comb finger gap	4 (μm)
Comb finger overlap length	20 (μm)
Drive/sense frequency	3194/3105.6 (Hz)
Q-factor (drive/sense mode)	8256/7839

From the structure model it can be seen that the ideal micromachined gyroscope is a mass-spring-damping system with two independent modes. However, due to the inevitable fabrication errors of the micromachined structure, the actual gyroscope dynamic equation tends to be more complicated. Besides, the fabrication imperfection of comb fingers and beams will contribute to stiffness coupling, stiffness asymmetry, drive force asymmetry and displacement detection asymmetry, etc. These structural errors will lead to undesired coupling of drive and sense modes, and even excite other sub-modes, for instance, the differential mode drive force asymmetry will make the proof mass to rotate around z axis. In such case, mechanical model of three freedom degrees system is considered in this paper to study the dynamic characteristic, as illustrated in Equations (1)–(3), and the schematic diagram of the gyroscope rotation mode, is also depicted in Figure 1b.

The drive mode:

$$m_x\ddot{x}+D_{xx}\dot{x}+D_{xy}\dot{y}+K_{xx}x+K_{xy}y=F_{dx}+2m_p{\Omega }_s\dot{y}$$ (1)

The sense mode:

$$m_y\ddot{y}+D_{yy}\dot{y}+D_{yx}\dot{x}+K_{yy}y+K_{yx}x=F_{dy}-2m_p{\Omega }_s\dot{x}$$ (2)

The rotation mode:

$$\begin{gathered} I_z\left(\ddot{\theta }+D_z\dot{\theta }+K_z\theta \right)=\mathsf{\Sigma }{F}_{\Delta xi}{h}_{xi}+\mathsf{\Sigma }{F}_{\Delta yi}{h}_{yi}+\mathsf{\Sigma }{F}_{xzi}l+\mathsf{\Sigma }{F}_{yzi}l+\mathsf{\Sigma }{F}_{xci}{h}_{xi}+\mathsf{\Sigma }{F}_{yci}{h}_{yi} \\ i=1,2 \end{gathered}$$ (3)

where x, y is the displacement of the drive axis and sense axis, $m_x$ and $m_y$ is the vibration mass of these two modes, $m_P$ is the value of proof mass, $I_z$ is the rotary moment of inertia of the proof mass around z axis, and $\theta$ is the rotation angle of the z axis. $K_{xx}$, $K_{yy}$ are the normal stiffness coefficients of the drive and sense modes, and $K_{xy}$, $K_{yx}$ are the coupling stiffness coefficients. $D_{xx}$, $D_{yy}$ are the normal damping coefficients of the drive and sense modes, and $D_{xy}$, $D_{yx}$ are the coupling damping coefficients. $F_{dx}$ and $F_{dy}$ are the feedback forces that attached on the drive mode and sense mode, respectively.

As illustrated in Figure 1b, $D_z$ and $K_z$ are the damping and stiffness coefficient of rotation mode. $F_{\Delta xi}$ and $F_{\Delta yi}$ represent the asymmetric drive forces of the drive and sense axes, $F_{xzi}$ and $F_{yzi}$ are the vibration components that couple from the drive and sense modes to the rotation mode. $F_{xci}$ and $F_{yci}$ are the force that calculated to compensate the asymmetric components, $h_{xi}$ and $h_{yi}$ are the arms of the differential drive forces, $i=1,2$ indicates the different sides of the electrodes, $l$ is the dimension parameter of the gyroscope, representing the projection on these two axes of the distance from the beam bearing point to the vibration mass centre:

$$F_{xz}=\left(m_p+2m_1\right){\gamma }_x{\omega }_x^{2}x$$ (4)

$$F_{yz}=\left(m_p+2m_2\right){\gamma }_y{\omega }_y^{2}y$$ (5)

$$F_{\Delta x}=F_{dx}{\lambda }_x$$ (6)

$$F_{\Delta y}=F_{dy}{\lambda }_y$$ (7)

where ${\gamma }_x$ and ${\gamma }_y$ represent the rotation mode coupling coefficients of drive and sense modes, ${\lambda }_x$ and ${\lambda }_y$ are the drive force asymmetric coefficients of drive and sense modes. As depicted in Figure 1b, $m_p$ is the proof mass, $m_1$ and $m_2$ are the masses of the fingers and frames that vibrate in the two modes respectively. Thus $m_p+2m_1$ and $m_p+2m_2$ are equal to $m_x$ and $m_y$ in Equations (1) and (2) respectively. ${\omega }_x$ and ${\omega }_y$ are the resonate frequencies of the drive and sense mode respectively. From the equations of the dynamic characteristics of the gyroscope, it can be concluded that the drive and sense modes are coupled with each other, and the z-axis mode is influenced by different kinds of disturbances and fabrication imperfections.

Due to the existence of the rotation mode, the measured displacements of drive and sense modes are contaminated by the projection of the rotation movement on the other two modes. Assuming $\widehat{x}$, $\widehat{y}$ to be the displacements of x and y that influenced by the rotation mode, ${\delta }_x$ and ${\delta }_y$ are the beam fabrication asymmetric coefficients, then we get:

$$\widehat{x}=cos\theta x-{\delta }_{x}lsin\theta$$ (8)

$$\widehat{y}=cos\theta y+{\delta }_{y}lsin\theta$$ (9)

Considering the control algorithm formation of the drive and sense modes, the gyroscope used in our experiment is double decoupled dual-mode micromachined gyroscope. The drive and sense modes are designed symmetrically, thus the adaptive control algorithm applied on the two modes is also symmetric.

The reference model of the gyroscope is defined as two ideal oscillators, which will remain stable vibration at certain frequencies [4]. The dynamic equations of the ideal reference model can be written as:

$$\ddot{\mathit{q}}+{\mathit{K}}_{ideal}\mathit{q}=0$$ (10)

where the stiffness matrix ${\mathit{K}}_{ideal}$ is:

$${\mathit{K}}_{ideal}=\left[\begin{array}{cc}{K}_{ideal\_x}& 0\\ 0& K_{ideal\_y}\end{array}\right]$$ (11)

The elements on the leading diagonal are the stiffnesses of the two modes, and the vector $\mathit{q}$ is:

$$\mathit{q}=\left[\begin{array}{c}x\\ y\end{array}\right]$$ (12)

Equation (12) represent the displacements of the ideal gyroscope structure.

Considering the coupling terms and the Coriolis force aroused from the input angular rate, the model of the physical gyroscope mechanical structure can be expressed as:

$$\left[\begin{array}{cc}{m}_x& 0\\ 0& m_y\end{array}\right]\left(\ddot{\mathit{q}}+\mathit{D}·\dot{\mathit{q}}+\mathit{K}·\mathit{q}\right)={\mathit{F}}_{\mathit{d}}-2m_p\mathit{\Omega }\dot{\mathit{q}}$$ (13)

In Equation (13) $\mathit{D}$ is the damping matrix, $\mathit{K}$ is the stiffness matrix, and $\mathit{\Omega }$ is the matrix constructed from the input angular rate. ${\mathit{F}}_{\mathit{d}}$ is the vector of the drive force that attached to the drive electrode fingers of the gyroscope.

$$\mathit{D}=\left[\begin{array}{cc}{D}_{xx}& D_{xy}\\ D_{yx}& D_{yy}\end{array}\right]$$ (14)

$$\mathit{K}=\left[\begin{array}{cc}{K}_{xx}& K_{xy}\\ K_{yx}& K_{yy}\end{array}\right]$$ (15)

$${\mathit{F}}_{\mathit{d}}=\left[\begin{array}{c}{F}_x\\ F_y\end{array}\right]$$ (16)

$$\mathit{\Omega }=\left[\begin{array}{cc}0& -{\Omega }_z\\ {\Omega }_z& 0\end{array}\right]$$ (17)

Define R to be the estimation error between the actually stiffness matrix and the reference model:

$$\mathit{R}=\mathit{K}-{\mathit{K}}_{ideal}=\left[\begin{array}{cc}{R}_{xx}& R_{xy}\\ R_{yx}& R_{yy}\end{array}\right]$$ (18)

Due to the fabrication imperfections, small cross-coupling terms still exist between the two modes. In the matrix, the coupling coefficients $D_{xy}$ and $D_{yx}$, $K_{xy}$ and $K_{yx}$ are not necessarily equal to each other, respectively, thus an algorithm that can deal with each components correspondingly is of great importance.

3. The Adaptive Control Algorithm

By comparing the output signal of the reference model and the physical gyroscope, first the tracking error can be obtained. With the proper adaptive law, the estimated parameters are then corrected, thus the parameters are estimated precisely and the feedback control force can be generated through the estimated parameters of the gyroscope, and finally the system is well controlled.

The block diagram of the whole system is shown in Figure 2 , where the differential subtract block compares the velocity outputs with the actual gyroscope and the reference model, and generates an velocity error e, which contains the information of parameter estimation error:

$$\mathit{e}=\mathit{q}-{\mathit{q}}_m$$ (19)

Figure 2.

The block diagram of the adaptive control gyroscope system.

The reference model modification section calculates the estimated stiffness matrix, which is used to control the vibration frequency of the reference model. The feedback control block further generates a direct feedback signal, i.e., the velocity error, which acts as the first feedback signal.

The control law is designed to be implemented in discrete domain. Define the sampling index k, and the sampling time $\Delta t$, the differential subtraction section provides the estimation error $\Delta \mathit{e}={\mathit{e}}_{k+1}-{\mathit{e}}_k$, and multiplied directly by the feedback coefficient $-{\gamma }_0,$ i.e.,

$${\mathit{\tau }}_0=-{\gamma }_0\Delta \mathit{e}$$ (20)

Meanwhile, the velocity error coupling and displacement error coupling modules are multiplied by the feedback components with the corresponding velocity or displacement signals respectively, then to provide the upgrade amount of different elements in the parameter matrix. In the feedforward control section, the signal that can be used to compensate the coupling and damping disturbances are finally generated, $\mathit{R}, \mathit{D}, \mathit{\Omega }$ are modulated with corresponding sinusoidal waves and sent to the two modes.

Assuming $\widehat{\mathit{R}}, \widehat{\mathit{D}}, \widehat{\mathit{\Omega }}$ to be the estimated value of matrix $\mathit{R}, \mathit{D}, \mathit{\Omega }$, and $\tilde{\mathit{R}}, \tilde{\mathit{D}}, \tilde{\mathit{\Omega }}$ to be the estimation error of the three matrixes, respectively. A positive definite Lyapunov function is chosen as shown in the Appendix B. In the practical conditions, the coupling coefficients on the anti-diagonal of the parameter matrices are not necessarily equal to each other, and the control law will decouple each of the coupling coefficients, respectively. To make the Lyapunov function semi-negative definite, the adaptive control law can be chosen as:

$$\Delta \tilde{\mathit{R}}={\gamma }_R{\mathit{q}}_{\mathit{m}}{\tau }_0^T$$ (21)

$$\Delta \tilde{\mathit{D}}={\gamma }_D\left({\mathit{q}}_{\mathit{m}}\left(k+1\right)-{\mathit{q}}_{\mathit{m}}\left(k\right)\right){\tau }_0^T$$ (22)

$$\Delta \tilde{\mathit{\Omega }}={\gamma }_{\Omega }\left({\mathit{q}}_{\mathit{m}}\left(k+1\right)-{\mathit{q}}_{\mathit{m}}\left(k\right)\right){\tau }_0^T$$ (23)

The Lyapunov stability analysis of the algorithm is illustrated in the Appendix B. The final output angular velocity signal is derived by integral calculation: $\mathit{\Omega }={{\displaystyle \sum }}^{}\Delta \tilde{\mathit{\Omega }}$ elements on the counter-diagonal of the matrix $\mathit{\Omega }$ is the estimated angular velocity.

The reference model modification block estimates the normal stiffness and coupling stiffness of the two gyroscope modes, which will be utilized by the reference model and the feedforward control module.

To maintain the gyroscope working conditions stable, two phase-lock-loop sections are applied in each vibration mode. Each PLL contains a numerical control oscillator (NCO), which generates sinusoidal wave with calculated frequency and phase. Rather than two ideal oscillator models that are illustrated as in Equation (10), the output signal of NCO has a constant amplitude, which will make the algorithm easier for implementation without affecting the Lyapunov stability.

The detailed flow chart of the adaptive control algorithm of the gyroscope system is shown in Figure 3. With two PLLs acting as the reference model, the control error information can be transferred to estimate different parameters through the displacement signal channel and the velocity signal channel. Additional differential detection section is attached at the drive mode to collect rotation angular information so that the least mean square demodulation (LMSD) section can generate signal ${\tau }_r$ to eliminate such an undesirable rotation.

Figure 3.

Detailed flow chart of the modified adaptive gyroscope control algorithm.

To fulfill the designed adaptation law, the feedforward control module is designed to update the parameter estimation and generate the final feedforward signal. The final gyroscope angular rate output is also generated from the module. The total drive signal $\mathit{\tau }$ is attached on the drive fingers of the two modes. It consists of three parts including the feedforward signal, the feedback signal ${\mathit{\tau }}_{\mathbf{0}}$, and the rotation eliminate signal ${\mathit{\tau }}_{\mathit{r}}$, i.e.,

$$\mathit{\tau }=\widehat{\mathit{D}}{\dot{q}}_m+\widehat{\mathit{R}}{q}_m+2\widehat{\mathit{\Omega }}{\dot{q}}_m+{\mathit{\tau }}_0+{\mathit{\tau }}_r$$ (24)

where ${\mathit{\tau }}_{\mathit{r}}$ is defined as:

$${\mathit{\tau }}_r=\left[\begin{array}{c}{F}_{xc}\\ F_{yc}\end{array}\right]$$ (25)

Components of the drive signal have different frequency and phase, in such case the add operation will not severely increase the total drive signal amplitude or make the digital amount overflow. To eliminate the rotation components, the rotation elimination circuit is specially designed. In Equation (3) assume $\theta =0,$ and to cancel off the coupling force components in different modes, then we get:

$$F_{xc}=-F_{\Delta x}-F_{xz}l/h_x$$ (26)

$$F_{yc}=-F_{\Delta y}-F_{yz}l/h_y$$ (27)

Since the frequencies of the asymmetric drive force and the stiffness coupling components are different, the gyroscope is forced to rotate with the mixture of these two frequencies. The demodulation algorithm is applied in both modes to figure out the rotation components of each frequency and generate the compensation forces $F_{xc}$ and $F_{yc}$ of each frequency. Substituting Equations (6) and (7) into Equations (26) and (27), then we get:

$$F_{xc}=-F_{dx}{\lambda }_x-m_p{\gamma }_x{\omega }_x^{2}xl/h_x$$ (28)

$$F_{yc}=-F_{dy}{\lambda }_y-m_p{\gamma }_y{\omega }_y^{2}yl/h_y$$ (29)

By applying small-angle approximation in Equations (8) and (9), it can be written as:

$$\widehat{x}=x-h_x\theta$$ (30)

$$\widehat{y}=y+h_y\theta$$ (31)

The unmatched signals of the drive modes and sense modes are measured from the gyroscope detection pins by signal condition circuits and calculated in the FPGA chip. Although the frequencies of the signals are predictable, the phases of the detected signals remain unknown. Due to the fabrication variation, the coefficients of the gyroscope structure are not measureable, thus a demodulation method is applied to estimate the amplitude of the signal. Here the LMSD algorithm is applied in both modes. The demodulation reference signals are provided by the reference model, the calculated results are multiplied with a pair of quadrature reference signals to generate the appropriate rotation elimination signal ${\tau }_r$.

The rotation angle $\theta$ contains the frequency components of both modes oscillation frequencies. From the detection output port of each mode, the differential signal is demodulated with a pair of quadrature sinusoidal or cosine signals of a certain frequency. Assuming the detected rotation signal is $lsin\theta$, it contains different frequency components of $\omega$ and $\left(\omega +\Delta \omega \right)$, with the corresponding magnitude of A, B, C, E, respectively. By applying the LMSD algorithm, the rotation signal can be easily decomposed and recombined with two quadrature reference sinusoidal signals:

$$lsin\theta =Asin\left(\omega t\right)+Bcos\left(\omega t\right)+Csin\left(\omega t+\Delta \omega t\right)+Ecos\left(\omega t+\Delta \omega t\right)=\left(A+Ccos\Delta \omega t-Esin\Delta \omega t\right)sin\left(\omega t\right)+\left(B+Csin\phi t+Ecos\Delta \omega t\right)cos\left(\omega t\right)$$ (32)

From Equation (32) it can be seen that, although the original frequency components cannot be restored, the force components that are acquired to eliminate the rotation are calculated in real time and can be attached to the drive fingers of the corresponding mode. With the utilization of the rotation elimination circuit, the unexpected rotation of the proof mass can be effectively suppressed to be less than $2\times {10}^{-10}$ rad, compared with the former angular output of $6\times {10}^{-10}$ rad. The rotation angular output and the Lissajous trajectories for tri-modes are depicted in Figure 4 and Figure 5, respectively.

Figure 4.

Rotation angular output before and after the elimination circuit.

Figure 5.

The Lissajous trajectories plotted with the output of three modes.

From the simulation result illustrated in Figure 4 and Figure 5, it can be concluded that the algorithm based on the LMSD can effectively suppress the rotation mode of the gyroscope. With the differential movement of exited by the rotation is suppressed, the gyroscope works at a more stable condition and the control and measurement precision is also enhanced.

In terms of different modes displacement detection, the differential mode signals are extracted in the drive and sense modes, while the common mode signal are picked out in the rotation mode. In this case, the asymmetric displacement of drive and sense modes are only generated by the asymmetric drive force, i.e., their rotation angle and linear displacement signals are totally decoupled. Therefore, the rotation elimination circuits are relatively independent of the adaptive control circuits for the drive and sense modes.

Theoretically, the Lyapunov stability of the adaptive algorithm controlled system is not affected by rotation section. The main parameters and coefficients of the simulated system are listed as in Table 2. In the implementation process of the algorithm, apart from the coefficients of the mechanical-electrical interface and the parameter of the gyroscope dynamical model, all the variables and coefficients are calculated or stored in the FPGA chip.

Table 2.

Coefficients and parameters of the simulated system.

Block	Parameter/Coefficient	Symbol	Continuous Domain Value	Digital Domain Value
Feedforward control	Angular rate integral coefficient	${\gamma }_{\Omega }$	0.0087	8
	Damping integral coefficient	${\gamma }_D$	0.0021	8
	Stiffness error integral coefficient	${\gamma }_R$	139.5	1/1024
Reference model	Feedback gain	${\gamma }_0$	$1.9\times {10}^{-6}$	16
Mechanical-electricalInterface	Analog-digital domain gain	$K_{ad}$	$2.07\times {10}^5 \text{LSB}/\mathrm{V}$	-
	Digital-analog domain gain	$K_{da}$	$4.76\times {10}^{-6} \mathrm{V}/\text{LSB}$	-
	Voltage-force coefficient	$K_{vf}$	$2.8\times {10}^{-7} \mathrm{N}/\mathrm{V}$	-
	Displacement-capacitance-voltage coefficient	$K_{dcv}$	$4.4137\times {10}^5 \mathrm{V}/\mathrm{m}$	-
Gyroscope dynamical model	Gyroscope proof mass	$m$	$1.39\times {10}^{-6} \mathrm{k}\mathrm{g}$	-
	Drive mode quality factor	$Q_x$	$8256$	-
	Sense mode quality factor	$Q_y$	$7839$	-
	Drive mode resonating frequency	${\omega }_x$	$3194 \mathrm{H}\mathrm{z}\times$2$\mathsf{\pi }$	-
	Sense mode resonating frequency	${\omega }_y$	$3105.6 \mathrm{H}\mathrm{z}\times$2$\mathsf{\pi }$	-
	Sense-drive stiffness coefficient	$K_{xy}$	$0.1557 \mathrm{N}/\mathrm{m}$	-
	Drive-sense stiffness coefficient	$K_{yx}$	$0.1514 \mathrm{N}/\mathrm{m}$	-
	Sense-drive damping coefficient	$D_{xy}$	$1.08\times {10}^{-7} \mathrm{N}/\mathrm{m}/\mathrm{s}$	-
	Drive-sense damping coefficient	$D_{yx}$	$0.9\times {10}^{-7} \mathrm{N}/\mathrm{m}/\mathrm{s}$	-

Compared with the conventional adaptive control algorithm proposed in [3], the modified adaptive control algorithm proposed in this work has the following improvements: the adaptive law is redesigned to suit the asymmetric condition; rotation elimination section is added to suppress the differential components in the detection signal; the reference model is replaced with two NCOs and the modified adaptive law has a simpler arithmetic structure, the algorithm is more suitable for FPGA implementation.

4. Algorithm Simulation

Simulation of the adaptive control based gyroscope system consists of two steps. The continuous domain simulation verifies the adaptive control algorithm and the digital domain simulation based on the DSP builder tool provides convenient conversion from control algorithm simulation to hardware program language realization.

Simulation system is built in Simulink to investigate the performance of the proposed algorithm. Changing of the gyroscope parameters is simulated to verify the adaptive characteristic of the algorithm. The influences of the coefficients on the gyroscope system performance are also compared in this section.

The value of ${\mathsf{\gamma }}_{\mathrm{R}}$ and ${\mathsf{\gamma }}_{\mathrm{D}}$ will also affect the estimation convergence speed of R and D matrix, respectively, as shown in Figure 6 and Figure 7. From the curves that represent estimation processes with different ${\mathsf{\gamma }}_{\mathrm{R}}$ values, it can be inferred that larger ${\mathsf{\gamma }}_{\mathrm{R}}$ can provide faster stiffness estimation but has larger overshoot and may result in overflow of the digital output signal.

Figure 6.

Convergence process curves of stiffness estimation error matrix R with different ${\mathsf{\gamma }}_{\mathrm{R}}$ values. (a) $R_{xx}$, (b) $R_{xy}$, (c) $R_{yx}$, (d) $R_{yy}$.

Figure 7.

Convergence process curves of damping estimation matrix D with different ${\mathsf{\gamma }}_{\mathrm{D}}$ values. (a) $D_{xx}$, (b) $D_{xy}$, (c) $D_{yx}$, (d) $D_{yy}$.

Like the case of ${\mathsf{\gamma }}_{\mathrm{R}}$ value, ${\mathsf{\gamma }}_{\mathrm{D}}$ also has the influences on the overshoot and estimation time. Moreover, the estimation result of damping components are multiplied by the velocity of the reference model, in such case, the system vibration condition is sensitive to the signal. When ${\mathsf{\gamma }}_{\mathrm{R}}$ value is too large, such as the orange curve in Figure 7d, ripples will emerge in the parameter estimation result, and the amplitude control process is disturbed.

From Figure 8 it can be concluded that ${\mathsf{\gamma }}_{\mathrm{D}}$ will influence the maximum overshoot during the start-up period and ${\mathsf{\gamma }}_{\mathrm{R}}$ value mainly relates with the time consumption of the amplitude control. The adaptive algorithm coefficients${\mathsf{\omega }}_{\mathrm{R}}$, ${\mathsf{\gamma }}_{\mathrm{D}}$ will influence the amplitude control speed of the start-up process, thus a proper combination of these two values is necessary. After a trade-off is made between the control time, overshoot and the proper estimation of parameters that illustrated in Figure 6 and Figure 7, the coefficients are finally determined with a multi-object optimization function [12,13].

Figure 8.

The amplitude control performance with different ${\gamma }_R$ and ${\gamma }_D$ values. (a) Control time, (b) Overshoot.

To further investigate the speed performance of input angular rate estimation, the angular rate estimation function is tested with the ±300°/s step-formed input signal. The angular rate estimation parameter can affect the slew rate and overshoot of the angular rate, as depicted in Figure 9.

Figure 9.

Angular velocity output with step-formed input signal.

With the integral effect, the adaptive control method also has a fine performance against the input noise, from Figure 10 it can be calculated that, the white noise mixed with the input signal are effectively suppressed to obtain the clean output angular rate estimation signal, and specifically the SNR (Signal to noise ratio) has been greatly enhanced from 49 dB to 90 dB.

Figure 10.

Noise suppression of the input angular rate estimation.

In the other hand, ${\gamma }_{\Omega }$ is also simultaneously related with the measurement precision and bandwidth of the gyroscope system. These two characteristics are contradict against each other, thus a trade-off should be made according to the expected application of the gyroscope system.

As shown in Figure 11, to meet the gyroscope application requirement, the bandwidth should be more than 40 Hz, and the measurement precision should be higher than 0.1°/s, finally we choose the ${\mathsf{\gamma }}_{\mathsf{\Omega }}$ value as 12.

Figure 11.

Measurement precision and bandwidth with different ${\mathsf{\gamma }}_{\mathsf{\Omega }}$ values.

With the coefficients of the algorithms are determined, the performance against the parameters variation are also simulated. In practical circumstances, the stiffness of working gyroscope may change due to the environmental factors, especially the temperature. In practical condition, the parameters are changed gradually, but to test the estimation speed of the algorithm, the variations are simulated in the form of step switches, as shown in Figure 12 and Figure 13.

Figure 12.

Damping coefficient estimation with switch change at t = 0.5.

Figure 13.

Stiffness coefficient estimation with switch change at t = 0.5 s.

In Figure 12, the quality factors of the gyroscope model vibration modes are changed to 1/10 of their original value, thus the damping coefficients are changed accordingly. In Figure 13, the resonating frequencies of the drive mode and the sense mode are both changed by 10 Hz at 500 ms, i.e., from 3715 Hz to 3725 Hz in drive mode and from 3684 Hz to 3674 Hz in sense mode.

From the simulation curves it can be seen that the algorithm can adapt to the parameter variations robustly and the estimated results converge to the real values after a short transition period.

5. Experimental Validation

Based on a set of digitalized gyroscope system, the modified adaptive control algorithm is programmed in the FPGA chip to control the gyroscope.

5.1. Open-Loop Measurement of the Parameters

To validate the estimation performance of the algorithm, the gyroscope parameters are measured in open-loop testing in advance [14]. As illustrated in Section 2, parameters of the gyroscope are contained in the three modes that coupled with each other. Through frequency sweep and half-power measurement, the resonance frequency ${\mathsf{\omega }}_0$ and quality factor Q can be derived, along with the stiffness and damping coefficient of each mode, i.e., $K_{xx}$, $K_{yy}, D_{xx}$ and $D_{yy}$. The coupling coefficients can be calculated according to the mode-coupling transfer function and the gyroscope coupling output. However, the mechanical and electrical coefficients cannot be measured directly, and thus the drive mode force-voltage transfer functions are not available. To solve this dilemma, the force-displacement transfer functions are transformed into voltage-voltage transfer function and measurement of the unknown coefficients are avoided.

The scale factor ratio of drive and sense modes is measured through exiting the rotation mode of the gyroscope when the common-mode force is attached at the drive mode. Assuming $F_{x }\left(s\right)$ and $F_y\left(s\right)$ to be the resultant forces attached on the drive mode and sense mode, the dynamics equation of the drive and sense modes can be expressed as:

$$\frac{x\left(s\right)}{F_x\left(s\right)}=\frac{1}{m\left(s^2+{\omega }_x/Q_x·s+{\omega }_x^2\right)}$$ (33)

$$\frac{y\left(s\right)}{F_y\left(s\right)}=\frac{1}{m\left(s^2+{\omega }_y/Q_y·s+{\omega }_y^2\right)}$$ (34)

and under such drive force, the displacement of sense mode can be described as:

$$y\left(s\right)s^2+c_{yy}y\left(s\right)s+y\left(s\right)=y\left(s\right)·H_y\left(s\right)=K_{xy}x\left(s\right)+D_{xy}·s·x\left(s\right)$$ (35)

where:

$$H_y\left(s\right)=\frac{1}{m\left(s^2+\frac{{\omega }_y}{Q_y}·s+{\omega }_y^2\right)}$$ (36)

Considering the scale factor ratio of drive and sense modes p, Equation (36) can be written as:

$$\frac{y\left(s\right)}{x\left(s\right)}=p·\frac{U_y\left(s\right)}{U_x\left(s\right)}=K_{xy}·H_y\left(s\right)+D_{xy}·s·H_y\left(s\right)$$ (37)

By frequency sweeping around the resonating frequency, the coupling coefficients can be derived from least square method. The detailed procedure is described as:

$$\frac{y\left(s\right)}{x\left(s\right)}=m+nj$$ (38)

$$H_y\left(s\right)=a+bj$$ (39)

$$s·H_y\left(s\right)=c+dj$$ (40)

where m, n are measured data, and a, b, c, d can be calculated through the gyroscope transfer function.

Assuming the frequency sweeping points are ${\mathsf{\omega }}_1,{\mathsf{\omega }}_2\dots {\mathsf{\omega }}_{\mathrm{n}}$, the corresponding identification equations can be written in matrix format that listed as in Equation (41).

$$\left[\begin{array}{c}\begin{array}{c}m\left({\omega }_1\right)\\ m\left({\omega }_2\right)\end{array}\\ ⋮\\ m\left({\omega }_n\right)\end{array}\right]=\left[\begin{array}{c}\begin{array}{cc}a\left({\omega }_1\right)& c\left({\omega }_1\right)\\ a\left({\omega }_2\right)& c\left({\omega }_2\right)\end{array}\\ \begin{array}{cc}⋮& ⋮\\ a\left({\omega }_n\right)& c\left({\omega }_n\right)\end{array}\end{array}\right]·\left[\begin{array}{c}{K}_{xy}\\ D_{xy}\end{array}\right]$$ (41)

Equation (41) can be written in the form of:

$${\mathit{M}}_{\mathit{n}\times \mathbf{1}}={\mathit{A}}_{\mathit{n}\times \mathbf{2}}·{\mathit{P}}_{\mathbf{2}\times \mathbf{1}}$$ (42)

By operating least square procedure, the calculated parameters are obtained under the meaning of least variance, i.e.:

$$\tilde{\mathit{P}}={\left(\mathit{A}\prime ·\mathit{A}\right)}^{-\mathbf{1}}·\mathit{A}\prime ·\mathit{M}$$ (43)

To measure the coupling coefficients that form the sense mode to the drive mode, the open-loop drive signal is attached on the sense mode and the coupling signal is measured from drive mode. Conducting the same process as illustrated above, $K_{yx}$ and $D_{yx}$ can be calculated. The schematic diagram of the gyroscope system circuit is shown in Figure 14, when used in the open-loop measurement, the digital board is removed and external signals are attached at the drive electrodes of the two modes. A crystal oscillator is used to provide carrier wave of the gyroscope, and capacitances variance signals of the two modes are detected by two diode rings. Since double-sided drive method is applied in the gyroscope, feedback signals are transferred through inverting amplifiers and attached on the opposite drive electrodes.

Figure 14.

Schematic diagram of the gyroscope system circuit.

5.2. Experimental Implementation Result and Comparison

For the convenience of the algorithm implementation, the simulation is performed with DSP builder toolbox, where algorithms of the system are verified and the word length and truncation of each section are determined. Most of the sections in the algorithm can be resolved into basic addition, subtraction, multiplication and integral calculations. The most complex and resource consumption section is the reference model with two NCOs, which are implemented using CORDIC algorithm [15]. As for the calculation of the matrixes, different elements are calculated and transferred separately through different branches. Since the usage of fixed point multiplier will consume large amount of hardware resources in the FPGA and add the power consumption of the digitalized system, most of the constant coefficients are determined as powers of 2, thus the multiplication operation can be realized by the shifting operation.

After the algorithm implementation, values of the parameters estimated program are output from DA chips and recorded, as plotted in Figure 15. In the plots, right vertical axes are labeled with the physical amount equivalent to the digital value system. The dash lines marked with the corresponding curves are the measured values acquired from the open-loop experiment.

Figure 15.

Experimental parameter estimation. (a) Damping values; (b) Stiffness values.

By comparing the measurement result with the estimated value, the performance of the adaptive algorithm can be evaluated. Parameters Estimated in the program and measured from open-loop method are compared in Table 3. In Figure 15 the measured values are marked with the dash lines in corresponding colors. From Figure 15 and Table 3 it can be concluded that the estimation results of the adaptive control algorithm highly consistent with the open-loop measurement results in the primary values, and less estimation precision in the weak coupling coefficients that has slight influence in the control system, like $D_{xy}\text{ and }{D}_{yx}$.

Table 3.

Comparison between the measurement value and the estimated value.

Parameter	Symbol	Measured Value (Open-Loop)	Estimated Value (Modified Adaptive Control)
Drive mode normal stiffness coefficient	$K_{xx}$	$559.3 \mathrm{N}/\mathrm{m}$	$559.7 \mathrm{N}/\mathrm{m}$
Stiffness coupling coefficient	$K_{xy}$	$0.03 \mathrm{N}/\mathrm{m}$	$0.04 \mathrm{N}/\mathrm{m}$
	$K_{yx}$	$0.155 \mathrm{N}/\mathrm{m}$	$0.17 \mathrm{N}/\mathrm{m}$
Sense mode normal stiffness coefficient	$K_{yy}$	$528.8 \mathrm{N}/\mathrm{m}$	$528.9 \mathrm{N}/\mathrm{m}$
Drive mode normal damping coefficient	$D_{xx}$	$4.10\times {10}^{-6} \mathrm{N}/\mathrm{m}/\mathrm{s}$	$4.4\times {10}^{-6} \mathrm{N}/\mathrm{m}/\mathrm{s}$
Damping coupling coefficient	$D_{xy}$	$2\times {10}^{-7} \mathrm{N}/\mathrm{m}/\mathrm{s}$	$1\times {10}^{-7} \mathrm{N}/\mathrm{m}/\mathrm{s}$
	$D_{yx}$	$1\times {10}^{-7} \mathrm{N}/\mathrm{m}/\mathrm{s}$	$8\times {10}^{-8} \mathrm{N}/\mathrm{m}/\mathrm{s}$
Sense mode normal damping coefficient	$D_{yy}$	$3.92\times {10}^{-6} \mathrm{N}/\mathrm{m}/\mathrm{s}$	$4.1\times {10}^{-6} \mathrm{N}/\mathrm{m}/\mathrm{s}$

To validate the commonality of the algorithm, another two gyroscopes in the same batch are also picked for repeated test using the same digitalized circuit. Parameters of the corresponding gyroscopes are listed in Table 4, and a screenshot of the vibration of the two modes is shown in Figure 16.

Table 4.

Parameters of gyroscope 2 and gyroscope 3.

Parameter	Symbol	Gyroscope 2	Gyroscope 3
Drive mode normal stiffness coefficient	$K_{xx}$	$756.6 \mathrm{N}/\mathrm{m}$	$570.6 \mathrm{N}/\mathrm{m}$
Stiffness coupling coefficient	$K_{xy}$	$1.5\times {10}^{-3} \mathrm{N}/\mathrm{m}$	$1.13\times {10}^{-3} \mathrm{N}/\mathrm{m}$
	$K_{yx}$	$1\times {10}^{-3} \mathrm{N}/\mathrm{m}$	$7.4\times {10}^{-4} \mathrm{N}/\mathrm{m}$
Sense mode normal stiffness coefficient	$K_{yy}$	$744.1 \mathrm{N}/\mathrm{m}$	$549.7 \mathrm{N}/\mathrm{m}$
Drive mode resonating frequency	$f_x$	$3717.31 \mathrm{H}\mathrm{z}$	$3228.96 \mathrm{H}\mathrm{z}$
Drive mode quality factor	$Q_x$	10297	3487
Drive mode normal damping coefficient	$D_{xx}$	$4.48\times {10}^{-6} \mathrm{N}/\mathrm{m}/\mathrm{s}$	$1.15\times {10}^{-5} \mathrm{N}/\mathrm{m}/\mathrm{s}$
Damping coupling coefficient	$D_{xy}$	$1\times {10}^{-8} \mathrm{N}/\mathrm{m}/\mathrm{s}$	$7.9\times {10}^{-8} \mathrm{N}/\mathrm{m}/\mathrm{s}$
	$D_{yx}$	$6\times {10}^{-9} \mathrm{N}/\mathrm{m}/\mathrm{s}$	$7.1\times {10}^{-8} \mathrm{N}/\mathrm{m}/\mathrm{s}$
Sense mode normal damping coefficient	$D_{yy}$	$6.18\times {10}^{-6} \mathrm{N}/\mathrm{m}/\mathrm{s}$	$1.18\times {10}^{-5} \mathrm{N}/\mathrm{m}/\mathrm{s}$
Sense mode resonating frequency	$f_y$	$3684.51 \mathrm{H}\mathrm{z}$	$3167.22 \mathrm{H}\mathrm{z}$
Sense mode quality factor	$Q_y$	8986	4055

Figure 16.

Drive and sense mode signal when controlled by the adaptive algorithm. (a) Gyroscope 2; (b) Gyroscope 3.

The experimental setup established in our work is presented as in Figure 17. The gyroscope system consists of an analog front end, a gyroscope and a digital board based on an FPGA chip. The program is down loaded into an EPCS16 flash chip, so that once the system is supplied with a constant voltage of ±5 V, the algorithm will control the two modes vibrate at constant frequency and amplitude. As can be measured from the oscilloscope, the drive mode frequency is 3105.9 Hz and the sense mode frequency is 3195.1 Hz.

Figure 17.

The modified adaptive control system. (a) Digitalized system with three gyroscopes used; (b) Experimental setup.

As shown in Figure 18, the gyroscope system has scale factor of 136 LSB/°/s, with a nonlinearity of 339 ppm. According to the Allan variance result, bias instability is 13.8°/h, better than 14.9°/h of the digitalized system without adaptive control algorithm [14]. Compared with the output result of the same gyroscope with conventional control algorithm, the improvement of Allan variance can be attributed to the stability of the reference model. The FPGA utilized in our work is Cyclone EP4CE40, and a total of 8125 logic elements (LE) are consumed when the whole modified adaptive control algorithm and AD/DA convertor interface sections are programmed. It can be concluded from the experimental results that the digitalized adaptive control algorithm is successfully applied on the FPGA chip and achieve fine performance. Further enhancement of the performance will be carried out in our future research.

Figure 18.

Performance of the system. (a) Scale factor; (b) Allan variance compared with original system.

6. Conclusions

In this paper we present a digitalized micromachined gyroscope based on a modified adaptive control method. The algorithm can estimate some of the gyroscope key coefficients in real time and make both the drive and sense modes vibrate at constant amplitude, which will enhance the system robustness against environmental variances and disturbances. The rotation mode is also considered in the model, with the corresponding parameters estimated and the common mode vibration amplitude is well controlled, the differential movement exited by the rotation mode is also investigated. Differential detection and LMS demodulation methods are used in the system to eliminate such rotation. Performance including the parameter estimation and the system dynamic characteristics are investigated to choose a set of optimized key coefficients. To validate the commonality of the algorithm, another two gyroscopes are used for repeated test. The digitalized gyroscope system applied with the adaptive control method achieves scale factor of 136 LSB/°/s, with a nonlinearity of 339 ppm, and bias instability of 13.8 °/h by Allan variance.

Comparison of various adaptive control methods are summarized in Table 5. In recent years, different adaptive control methods have been tried on different gyroscopes by some leading researchers. Due to the algorithm complexity, some works still focused on the model building, adaptive control algorithm deduction and simulation verification. Normally, the adopted gyroscope should be designed symmetrically, where z-axis MEMS gyroscope and tri-axial gyroscope can be always utilized. Based on the previous work, the modified adaptive control method is experimentally verified and realized on our digital gyroscope system, which further proves that the adaptive control method is feasible in real application.

Table 5.

Comparison of different gyroscope control methods in recent years.

Institute	Year	Control Method	Gyroscope Used	Simulation and Experiment
University of California at Berkeley [3]	2000	Adaptive control strategy	z-axis MEMS gyroscope	Algorithm simulation
RMIT University [6]	2006	Tri-axial adaptively controlled algorithm	Single-mass tri-axial AR sensor	Modeled fabrication and algorithm simulation
University of Louisiana at Lafayette [7]	2009	Sliding mode control	z-axis MEMS gyroscope	Algorithm simulation
Cleveland State University [12]	2008	Active Disturbance Rejection Control	Vibrational beam gyroscope	Analog circuit implementation
This work	2015	Modified Adaptive control	z-axis MEMS gyroscope	Simulation and FPGA digital circuit implementation

Future research of the adaptive method controlled gyroscope system may focus on the further enhancement of the performance and application of the adaptive control method on other types of gyroscopes for instance, ring, disk or hemispherical shell gyroscopes.

Appendix A

Nomenclature

Symbol	Nomenclature	Symbol	Nomenclature
${\mathit{K}}_{ideal}$	Ideal Stiffness matrix	${\mathit{\tau }}_0$	Feedback section output vector
$\theta$	Z axis rotation angle	$\mathit{\tau }$	Total drive signal matrix
$\mathit{q}$	Displacement vector	${\mathit{\tau }}_{\mathit{r}}$	Rotation elimination signal vector
$x$	Drive mode displacement	$m_p$	Gyroscope proof mass
$\mathrm{y}$	Sense mode displacement	$m_x$	Drive mode vibration mass
$\mathit{F}$	Feedback force matrix	$m_y$	Sense mode vibration mass
$F_x$	Drive mode feedback force	$Q_x$	Drive mode quality factor
$F_y$	Sense mode feedback force	$Q_y$	Sense mode quality factor
$\mathit{D}$	Damping matrix	${\gamma }_{\Omega }$	Angular rate integral coefficient
$\mathit{K}$	Stiffness matrix	${\gamma }_D$	Damping integral coefficient
$\mathit{R}$	Stiffness estimation error matrix	${\gamma }_R$	Stiffness error integral coefficient
$\mathit{\Omega }$	Angular rate matrix	${\gamma }_0$	Feedback gain
$\mathit{e}$	Displacement estimation error vector	$V$	Lyapunov function

Appendix B

The Lyapunov stability proof process is illustrated as follows:

$$V=\frac{1}{2}\left(\Delta {\mathit{e}}^T{\gamma }_0\Delta \mathit{e}+{\mathit{e}}^T{\gamma }_0\mathit{K}\mathit{e}+tr\{{\gamma }_R^{-1}\tilde{\mathit{R}}{\tilde{\mathit{R}}}^T+{\gamma }_D^{-1}\tilde{\mathit{D}}{\tilde{\mathit{D}}}^T+{\gamma }_{\Omega }^{-1}\tilde{\mathit{\Omega }}{\tilde{\mathit{\Omega }}}^T\}\right)$$ (B1)

By taking differential operation on the Lyapunov function, it can be derived that:

$$\Delta V=V_{k+1}-V_k={\gamma }_0\Delta {\mathit{e}}^T\left(\Delta {\mathit{e}}_{k+1}-\Delta {\mathit{e}}_k\right)+{\gamma }_0{\left(\Delta {\mathit{e}}_{k+1}-\Delta {\mathit{e}}_k\right)}^T{\mathit{K}}_m\mathit{e}+tr\left\{{\gamma }_R^{-1}{\tilde{\mathit{R}}}_k\Delta {\tilde{\mathit{R}}}^T+{\gamma }_D^{-1}{\tilde{\mathit{D}}}_k\Delta {\tilde{\mathit{D}}}^T+{\gamma }_{\Omega }^{-1}{\tilde{\mathit{\Omega }}}_k\Delta {\tilde{\mathit{\Omega }}}^T\right\}$$ (B2)

$$\Delta \tilde{\mathit{R}}={\tilde{\mathit{R}}}_{k+1}-{\tilde{\mathit{R}}}_k$$ (B3)

$$\Delta \tilde{\mathit{D}}={\tilde{\mathit{D}}}_{k+1}-{\tilde{\mathit{D}}}_k$$ (B4)

$$\Delta \tilde{\mathit{\Omega }}={\tilde{\mathit{\Omega }}}_{k+1}-{\tilde{\mathit{\Omega }}}_k$$ (B5)

Assuming the gyroscope controlled by the modified control law is under a stable condition, and the control signal is composed of the feedback and feed forward outputs, i.e.:

$$\ddot{\mathit{q}}+\mathit{D}\dot{\mathit{q}}+\mathit{K}\mathit{q}=\widehat{\mathit{D}}{\dot{\mathit{q}}}_m+\widehat{\mathit{R}}{\mathit{q}}_m+2\widehat{\mathit{\Omega }}{\dot{\mathit{q}}}_m-{\gamma }_0\dot{\mathit{e}}-2\Omega \dot{\mathit{q}}$$ (B6)

$$\begin{gathered} \ddot{\mathit{e}}+{\mathit{K}}_m\mathit{e}=\widehat{\mathit{D}}{\dot{\mathit{q}}}_m-\mathit{D}\dot{\mathit{q}}+\widehat{\mathit{R}}{\mathit{q}}_m-\mathit{R}\mathit{q}+2\widehat{\mathit{\Omega }}{\dot{\mathit{q}}}_m-2\mathit{\Omega }\dot{\mathit{q}}-{\gamma }_0\dot{\mathit{e}} \\ =\widehat{\mathit{D}}{\dot{\mathit{q}}}_m-\mathit{D}\dot{\mathit{q}}+\widehat{\mathit{R}}{\mathit{q}}_m-\mathit{R}\mathit{q}+2\widehat{\mathit{\Omega }}{\dot{\mathit{q}}}_m-2\mathit{\Omega }\dot{\mathit{q}}-{\gamma }_0\dot{\mathit{e}} \\ =\tilde{\mathit{D}}{\dot{\mathit{q}}}_m+\tilde{\mathit{R}}{\mathit{q}}_m+2\tilde{\mathit{\Omega }}{\dot{\mathit{q}}}_m-{\gamma }_0\dot{\mathit{e}} \end{gathered}$$ (B7)

After left multiplication by ${\gamma }_0\Delta {\mathit{e}}^T$ on both sides of Equation (B7), and written in discrete time domain, it can be derived that:

$$\begin{array}{c}{\gamma }_0\Delta {\mathit{e}}^T\left(\Delta {\mathit{e}}_{k+1}-\Delta {\mathit{e}}_k\right)+{\gamma }_0\Delta {\mathit{e}}^T{\mathit{K}}_m\mathit{e}\hfill \\ ={\gamma }_0\Delta {\mathit{e}}^T\left(\tilde{\mathit{D}}\left(\mathit{q}\left(k+1\right)-\mathit{q}\left(k\right)\right)+\tilde{\mathit{R}}\mathit{q}\left(k\right)+2\tilde{\mathit{\Omega }}(\mathit{q}\left(k+1\right)-\mathit{q}\left(k\right)\right)-{\gamma }_0^2\Delta {\mathit{e}}^T\Delta \mathit{e}\hfill \end{array}$$ (B8)

Substituting Equation (B8) into Equation (B2), the differential Lyapunov function can be written as:

$$\begin{gathered} \Delta V={\gamma }_0\Delta {\mathit{e}}^T\left(\Delta {\mathit{e}}_{k+1}-\Delta {\mathit{e}}_k\right)+{\gamma }_0\Delta {\mathit{e}}^T{K}_m\mathit{e}+tr\left\{{\gamma }_R^{-1}{\tilde{\mathit{R}}}_k\Delta {\tilde{\mathit{R}}}^T+{\gamma }_D^{-1}{\tilde{\mathit{D}}}_k\Delta {\tilde{\mathit{D}}}^T+{\gamma }_{\Omega }^{-1}{\tilde{\mathit{\Omega }}}_k\Delta {\tilde{\mathit{\Omega }}}^T\right\} \\ =-{\tau }_0\left(\left(\tilde{\mathit{D}}\left({\mathit{q}}_{\mathit{m}}\left(k+1\right)-{\mathit{q}}_{\mathit{m}}\left(k\right)\right)+\tilde{\mathit{R}}{\mathit{q}}_{\mathit{m}}\left(k\right)+2\tilde{\mathit{\Omega }}({\mathit{q}}_{\mathit{m}}\left(k+1\right)-{\mathit{q}}_{\mathit{m}}\left(k\right)\right)\right) \\ -{\gamma }_0^2\Delta e^T\Delta \mathit{e}+tr\left\{{\gamma }_R^{-1}{\tilde{\mathit{R}}}_k\Delta {\tilde{\mathit{R}}}^T+{\gamma }_D^{-1}{\tilde{\mathit{D}}}_k\Delta {\tilde{\mathit{D}}}^T+{\gamma }_{\Omega }^{-1}{\tilde{\mathit{\Omega }}}_k\Delta {\tilde{\mathit{\Omega }}}^T\right\} \end{gathered}$$ (B9)

To judge whether Equation (B9) is negative semi-definite, the first term should be combined with the third term and convert into the formula of matrix trace, taking ${\mathit{\tau }}_0\tilde{\mathit{R}}\mathit{q}$ as example:

$$\begin{gathered} {\mathit{\tau }}_0\tilde{\mathit{R}}{\mathit{q}}_m=\left[\begin{array}{cc}{\tau }_x& {\tau }_y\end{array}\right]\left[\begin{array}{cc}{\tilde{R}}_{11}& {\tilde{R}}_{12}\\ {\tilde{R}}_{21}& {\tilde{R}}_{22}\end{array}\right]\left[\begin{array}{c}{q}_{mx}\\ q_{my}\end{array}\right] \\ ={\tilde{R}}_{11}{\tau }_x{q}_{mx}+{\tilde{R}}_{21}{\tau }_y{q}_{mx}+{\tilde{R}}_{12}{\tau }_x{q}_{my}+{\tilde{R}}_{22}{\tau }_y{q}_{my} \\ =tr\left\{\left[\begin{array}{c}{\tilde{R}}_{11}{\dot{q}}_{mx}+{\tilde{R}}_{12}{\dot{q}}_{my}\\ {\tilde{R}}_{21}{\dot{q}}_{mx}+{\tilde{R}}_{22}{\dot{q}}_{my}\end{array}\right]\left[\begin{array}{cc}{\tau }_x& {\tau }_y\end{array}\right]\right\}=tr\left\{{\gamma }_R^{-1}\tilde{\mathit{R}}{\gamma }_R{\mathit{q}}_m{\tau }_0^T\right\} \end{gathered}$$ (B10)

It can be seen that under the condition of asymmetric coupling coefficients, Equation (B1) still exists. Similarly the following equations can also be proved:

$${\tau }_0\tilde{\mathit{D}}\left({\mathit{q}}_{\mathit{m}}\left(k+1\right)-{\mathit{q}}_{\mathit{m}}\left(k\right)\right)=tr\left\{{\gamma }_D^{-1}\tilde{\mathit{D}}{\gamma }_D\left({\mathit{q}}_{\mathit{m}}\left(k+1\right)-{\mathit{q}}_{\mathit{m}}\left(k\right)\right){\tau }_0^T\right\}$$ (B11)

$${\tau }_0\tilde{\mathit{\Omega }}\left({\mathit{q}}_{\mathit{m}}\left(k+1\right)-{\mathit{q}}_{\mathit{m}}\left(k\right)\right)=tr\left\{{\gamma }_{\Omega }^{-1}\tilde{\mathit{\Omega }}{\gamma }_{\Omega }\left({\mathit{q}}_{\mathit{m}}\left(k+1\right)-{\mathit{q}}_{\mathit{m}}\left(k\right)\right){\tau }_0^T\right\}$$ (B12)

Substituting Equations (B10)–(B12) into Equation (B9):

$$\Delta V=-{\gamma }_0^2\Delta {\mathit{e}}^T\Delta \mathit{e}+tr\left\{\tilde{\mathit{R}}\left({\gamma }_R^{-1}\Delta {\tilde{\mathit{R}}}^T-{\mathit{q}}_{\mathit{m}}{\tau }_0^T\right)+\tilde{\mathit{D}}\left({\gamma }_D^{-1}\Delta {\tilde{\mathit{D}}}^T-\left({\mathit{q}}_{\mathit{m}}\left(k+1\right)-{\mathit{q}}_{\mathit{m}}\left(k\right)\right){\tau }_0^T\right)+\tilde{\mathit{\Omega }}\left({\gamma }_{\Omega }^{-1}\Delta {\tilde{\mathit{\Omega }}}^T-\left({\mathit{q}}_{\mathit{m}}\left(k+1\right)-{\mathit{q}}_{\mathit{m}}\left(k\right)\right){\mathit{\tau }}_{\mathbf{0}}^{\mathit{T}}\right)\right\}$$ (B13)

By applying the adaptive law:

$$\tilde{\mathit{R}}={\gamma }_R{\mathit{q}}_{\mathit{m}}{\tau }_0^T$$ (B14)

$$\Delta \tilde{\mathit{D}}={\gamma }_D\left({\mathit{q}}_{\mathit{m}}\left(k+1\right)-{\mathit{q}}_{\mathit{m}}\left(k\right)\right){\tau }_0^T$$ (B15)

$$\Delta \tilde{\mathit{\Omega }}={\gamma }_{\Omega }\left({\mathit{q}}_{\mathit{m}}\left(k+1\right)-{\mathit{q}}_{\mathit{m}}\left(k\right)\right){\tau }_0^T$$ (B16)

Thus:

$$tr\left\{\tilde{\mathit{R}}\left({\gamma }_R^{-1}\Delta {\tilde{\mathit{R}}}^T-{\mathit{q}}_{\mathit{m}}{\tau }_0^T\right)+\tilde{\mathit{D}}\left({\gamma }_D^{-1}\Delta {\tilde{\mathit{D}}}^T-\left({\mathit{q}}_{\mathit{m}}\left(k+1\right)-{\mathit{q}}_{\mathit{m}}\left(k\right)\right){\tau }_0^T\right)+\tilde{\mathit{\Omega }}\left({\gamma }_{\Omega }^{-1}\Delta {\tilde{\mathit{\Omega }}}^T-\left({\mathit{q}}_{\mathit{m}}\left(k+1\right)-{\mathit{q}}_{\mathit{m}}\left(k\right)\right){\mathit{\tau }}_{\mathbf{0}}^{\mathit{T}}\right)\right\}=0$$

$\Delta V=-{\gamma }_0^2\Delta {\mathit{e}}^T\Delta \mathit{e}\le 0$, and the system is proofed to be Lyapunov stable.

Author Contributions

Dunzhu Xia convinced the modified algorithm and realized the programming; Yiwei Hu finalized the simulation and performed the experiments for validation; Peizhen Ni designed the PCB board and debugged the circuit system.

Conflicts of Interest

The authors declare no conflict of interest.