A SAKF-Based Composed Control Method for Improving Low-Speed Performance and Stability Accuracy of Opto-Electric Servomechanism

Abstract

The opto-electric servomechanism (OES) plays an important role in obtaining clear and stable images from airborne infrared detectors. However, the inherent torque disturbance and the noisy speed signal cause a significant decline in the inertial stability accuracy and low-speed performances of OESs. Traditional linear control schemes cannot deal with the nonlinear torque disturbance well, and the speed obtained by the finite difference (FD) method cannot effectively balance the tradeoff between the noise filtering and phase delay. Therefore, this paper proposes a strap-down stability control scheme, in the combination of a proportional-integral(PI) controller and a state-augmented Kalman filter (SAKF), where the PI is used to regulate the linear part of the servomechanism, and with the SAKF performing torque disturbance observation and speed estimation simultaneously. The principle and the implementation of the controller are introduced, and the tuning guidelines for the controller parameters are presented as well. Finally, the experimental verifications based on OESs with three transmission types (i.e., the direct-driving, the harmonic-driving, and the rotate vector-driving(RV-driving) OESs) are carried out respectively. The experimental results show that the proposed control scheme can perform better speed observation and torque disturbance compensation for various types of OESs, thus effectively improving the low-speed performance and stability accuracy of the mechanism.

1. Introduction

An opto-electric stable platform (OESP) is an opto-mechatronic device for long-distance target observation. Through an infrared detector or charge-coupled device (CCD) mounted on an OESP, the image of the target can be presented to the operator, as can be seen in Figure 1. OESPs usually work on moving objects such as airplanes or vehicles, and so both optimal low speed performance and inertial stability accuracy are necessary for OESPs in maintaining image quality. The opto-electric servomechanism (OES) is the key component of the OESP to achieve accurate motion. However, the torque disturbance commonly existing in the opto-electric servomechanism (OES) and the low signal-to-noise ratio(SNR) speed signal differentiated by the encoder make it difficult to achieve satisfactory servo performances.

For the purpose of suppressing torque disturbance of an OES, a torque or speed closed loop using a PI controller is always considered. The principle and implementation of a proportional-integral (PI) controller are simple. However, since the PI belongs to the linear control scheme, it seems difficult to effectively deal with the nonlinear torque disturbances, such as friction and torque ripple.

Another well-known approach is to build a torque disturbance observer and perform disturbance compensation according to the output of the observer. Depending on how the observer is built, this approach can be divided into two categories. The first category is to establish dynamic models for each torque disturbance mainly existing in the OES, and to suppress these disturbances one after another using these models. Obviously, an accurate model can compensate for the corresponding torque disturbance quite well. However, if there are multiple types of torque disturbances coexisting in the OES, this method will become complicated; additionally, unless the dynamic model is sufficiently precise, it will lead to a more serious impact on the motion performances of the OES [1,2,3]. Therefore, this method is suitable for servomechanisms under simple working conditions. Another category is to design a disturbance observer based on the nominal model of the OES. Among all the nominal-model-based observers, disturbance observer(DOB) is the most widely used [4]. However, the performance of DOB is subject to the cut-off frequency of its Q filter. If the cut-off frequency is set too high, a severe chattering may happen, causing the OES to not work properly. Additionally, if the cut-off frequency is set too low, the disturbance cannot be effectively observed, resulting in an unsatisfactory disturbance suppression effect. Since the cut-off frequency of the Q filter needs to be determined in reference to the actual characteristics of the OES, it is difficult for the DOB to obtain an optimal observation for the servomechanisms whose parameters change drastically, thus it is usually necessary to combine DOB with other advanced controllers, such as sliding mode controller(SMC) [5], to get a better motion performance.

Due to the limited size and mass of the platform, the OES rarely configures a tachometer for axial speed measurement. Instead, the axial speed is mainly obtained by finite-differentiating the angular data from the encoder mounted on the output side of the OES. However, due to the limited resolution of the encoder, there is a significant quantization error in the obtained speed signal. In order to deal with this problem, a low-pass filter (LPF) is commonly used to handle the differential signal of the encoder [6]. However, the linear LPF brings a severe phase lag and decreases the stability margin while suppressing the noise. Janabi–Sharifi et al. proposed a first-order adaptive windowing method (FOAW), which achieves a tradeoff between the noise suppression and the output rapidity by adaptively adjusting the differential step size of encoder data [7,8]. Jin et al. proposed a parabolic sliding mode filter (PSMF) as well as the adaptive tuning methods for PSMF parameters [9,10,11,12,13,14]. Compared with the Butterworth filter, the PSMF has a larger amplitude-frequency gain as well as a smaller phase lag, so it can achieve a better speed estimation. However, the accuracy of PSMF will be affected when the SNR of the speed signal is low [15]. Moreover, the complicated structure of the PSMF results in a difficult realization, and increases the computational costs of personal computers(PCs) evidently.

In order to get a better estimation of the torque disturbance as well as the rotational speed, this paper proposes a strap-down stability control scheme in combination of a PI controller and a state-augmented Kalman filter (SAKF). In this scheme, the PI is taken to regulate the linear part of the OES and the SAKF performs torque disturbance observation and speed estimation. Through the controller parameters tuning method provided in the paper, the OES achieves the satisfactory inertial stability accuracy and low-speed performance.

The sections of this paper are organized as follows: Section 2 gives the dynamic model of the OES, which provides the basis for the design of the control scheme. Section 3 introduces the principle of the control algorithm and the structure of the main functional modules. Considering the fact that the control algorithm runs in a digital signal processor (DSP), the discretization representation of the algorithm is solved in Section 4, and the implementation steps of the algorithm are also provided in this section. In Section 5, this algorithm is evaluated based on OESs with different reducers, and in the last section, Section 6, the conclusions of this paper are presented.

2. Dynamic Model

The dynamic model of the OES can be derived from the Newton’s second law as:

$$\{\begin{array}{l}{K}_{\mathrm{t}}\cdot i-M_{\mathrm{f}}=J\cdot \frac{\mathrm{d}{\omega }_{\mathrm{m}}}{\mathrm{d}t}\\ u_{\mathrm{m}}=L_{\mathrm{m}}\cdot \frac{\mathrm{d}i}{\mathrm{d}t}+R\cdot i+K_{\mathrm{e}}\cdot {\omega }_{\mathrm{m}}\\ M_{\mathrm{f}}=B\cdot {\omega }_{\mathrm{m}}+T_{\mathrm{c}}\\ u_{\mathrm{m}}=K_{\mathrm{c}}\cdot K_{\mathrm{g}}\cdot (u-K_{\mathrm{a}}\cdot i)\\ v=K_{\mathrm{r}}\cdot {\omega }_{\mathrm{m}}\\ \theta ={\displaystyle \int v\cdot \mathrm{d}t,}\end{array}$$

(1)

where K_t (N·m/A) is the torque coefficient of the motor, i (A) is the current of the motor, J (kg·m²) is the equivalent moment of inertia on the motor side, ${\omega }_{\mathrm{m}}$ (rad/s) is the motor speed, u_m (V) is the input voltage of the motor, L_m (H) is the inductance of the armature, and R ($\mathsf{\Omega }$) is the armature resistance. K_e (V/rad·s⁻¹) is the back-electromotive force(back-EMF) coefficient, M_f (N·m) is the friction torque, T_c (N·m) is the Coulomb friction torque, B (Nm/rad·s⁻¹) is the viscous friction coefficient, K_c is the gain of the current loop controller, u (V) is the reference input of the current loop, and K_a (V/A) is the current feedback coefficient. v (rad/s) is the speed on the load side, K_r is the transmission ratio, and $\theta$ (rad) is the angular position of the load side.

According to Equation (1), a block diagram representing the speed open-loop OES can be constructed, as shown in Figure 2a, and the transfer function is:

$$U(s)=\frac{L_{\mathrm{m}}s+K_{\mathrm{a}}{K}_{\mathrm{c}}+R}{K_{\mathrm{c}}{K}_{\mathrm{t}}}{T}_{\mathrm{c}}+\frac{L_{\mathrm{m}}J{s}^2+(K_{\mathrm{a}}{K}_{\mathrm{c}}J+RJ+B{L}_{\mathrm{m}})s+K_{\mathrm{a}}{K}_{\mathrm{c}}B+RB+K_{\mathrm{e}}{K}_{\mathrm{t}}}{K_r{K}_{\mathrm{c}}{K}_{\mathrm{t}}}V(s)$$

(2)

where V (s) is the s-domain representation of v.

Considering that the cut-off frequency of the current loop is much larger than the working frequency of the OES, the current loop of model (2) can be equivalent to a proportional component. Then, convert the Coulomb friction torque T_c to the input portion, i.e., $d=\frac{K_a}{K_t}{T}_c$, and a simplified speed open-loop dynamic model can be obtained, as is shown in Figure 2b. The corresponding transfer function is:

$$G_{\mathrm{u}\mathsf{\theta }}(s)=\frac{\Theta (s)}{U(s)}=\frac{K}{(p+s)\cdot s}$$

(3)

$$G_{\mathrm{uv}}(s)=\frac{V(s)}{U(s)}=\frac{K}{p+s}$$

(4)

where $p=\frac{B}{J}$, $K=\frac{K_{\mathrm{r}}{K}_{\mathrm{t}}}{K_{\mathrm{a}}J}$, $\Theta (s)$, $U(s)$ are the s-domain representations of $\theta$ and u respectively.

3. Principle of the Proposed Control Algorithm

The schematic diagram of the control algorithm proposed in this paper is shown in Figure 3. The input is the inertial reference speed ${\omega }_{\mathrm{r}}$, and the output is the actual inertia speed $\omega$ of OES. The module SAKF represents the state-augmented Kalman filter, and the torque disturbance d is observed and compensated based on the voltage u and the rotational speed v differentiated by the encoder. Meanwhile, the SAKF also filters the speed v to obtain a speed estimation result $\widehat{v}$ with a higher SNR for feedback control. The C_v(s) module is a feedback controller. The above control modules SAKF and C_v(s) constitute a speed tracking unit, as shown in the red dashed box in Figure 3. The gyro is then mounted on a carrier with its sensitive axis parallel to the rotational axis for sensing the inertial rotation of the carrier. If the actual speed of the carrier in the inertial space is ${\omega }_{\mathrm{d}}$ and the output of the gyroscope is ${\omega }_{\mathrm{g}}$, then ${\omega }_{\mathrm{d}}={\omega }_{\mathrm{g}}$. In this case, the difference between the reference speed ${\omega }_{\mathrm{r}}$ and ${\omega }_{\mathrm{g}}$ is used as the reference speed v_r of the speed tracking unit, and then the stable pointing of the OES in the inertial space can be realized. So in this controller, the voltage u is the manipulation variation, rotational speed v is the controlled variation, and the reference speed v_r is the setpoint variation.

4. Design of the Control Algorithm

According to the control algorithm principle introduced in Section 3, the SAKF module, and the feedback controller C_v(s) are the three functional modules of the algorithm. Therefore, this section will introduce the design process of the three modules in detail.

4.1. SAKF

The SAKF module is designed based on the state space model of the OES, which can be transformed from the OES transfer function (4) as

$$\{\begin{array}{l}\dot{x}=Ax+B(u-d)\\ y=Cx\end{array}$$

(5)

where $x=\left[\begin{array}{l}\theta \\ v\end{array}\right]$, $A=\left[\begin{array}{cc}0& 1\\ 0& -p\end{array}\right]$, $B=\left[\begin{array}{l}0\\ K\end{array}\right]$ and $C=\left[\begin{array}{cc}0& 1\end{array}\right]$.

Since the actual algorithm always runs in a digital signal processor (DSP), the state space model (5) needs to be discretized according to the sampling period T_s. Meanwhile, considering that the SAKF is designed to observe and compensate the disturbance torque, the torque disturbance d is extended to the state variable x of (5) as:

$$\{\begin{array}{l}\left[\begin{array}{l}x(k+1)\\ d(k+1)\end{array}\right]=A_{\mathbf{a}}\cdot \left[\begin{array}{l}x(k)\\ d(k)\end{array}\right]+B_{\mathbf{a}}\cdot u(k)+G\cdot \left[\begin{array}{l}n(k)\\ {\omega }_{\mathrm{n}}(k)\end{array}\right]\\ y(k)=C_{\mathbf{a}}\cdot \left[\begin{array}{l}x(k)\\ d(k)\end{array}\right]+n_{\mathbf{m}}(k)\end{array}$$

(6)

where $A_{\mathbf{a}}=\left[\begin{array}{cc}{A}_{\mathbf{d}}& -B_{\mathbf{d}}\\ 0& I_{1\times 1}\end{array}\right]$, $A_{\mathbf{d}}={\mathrm{e}}^{A{T}_{\mathrm{s}}}$, $B_{\mathbf{d}}={\displaystyle {\int }_0^{T_{\mathrm{s}}}{\mathrm{e}}^{A{T}_{\mathrm{s}}}B\mathrm{d}t}$, $x(k)=\left[\begin{array}{l}\theta (k)\\ v(k)\end{array}\right]$, $I_{1\times 1}$ is a 1×1 unit matrix; $B_{\mathbf{a}}=\left[\begin{array}{l}{B}_{\mathbf{d}}\\ 0\end{array}\right]$, $G=\left[\begin{array}{cc}{B}_{\mathbf{d}}& 0\\ 0& I_{1\times 1}\end{array}\right]$, $C_{\mathbf{a}}=\left[\begin{array}{cc}C& 0\end{array}\right]$, $V=I_{1\times 1}$. ${\omega }_{\mathrm{n}}$ is the difference between d(k + 1) and d(k), which can be regarded as a white noise; meanwhile, n_m(k) is the measurement noise of the speed obtained from encoder, and n(k) is the process noise of the state variable x(k).

According to (5) and (6), a relationship between A_d, B_d and p, K can be derived as:

$$\{\begin{array}{l}{A}_{\mathbf{d}}=\left[\begin{array}{l}\begin{array}{cc}1& \frac{1-{\mathrm{e}}^{-p{T}_{\mathrm{s}}}}{p}\end{array}\\ \begin{array}{cc}0& {\mathrm{e}}^{-p{T}_{\mathrm{s}}}\end{array}\end{array}\right]\\ B_{\mathbf{d}}=\frac{K}{p^2}\left[\begin{array}{l}p{T}_{\mathrm{s}}+{\mathrm{e}}^{-p{T}_{\mathrm{s}}}-1\\ p-p{\mathrm{e}}^{-p{T}_{\mathrm{s}}}\end{array}\right]\end{array}.$$

(7)

Therefore, the estimation result of the state variable [x(k),d(k)]^T can be obtained according to:

$$\left[\begin{array}{l}\widehat{x}(k+1)\\ \widehat{d}(k+1)\end{array}\right]=A_{\mathbf{a}}\left[\begin{array}{l}\widehat{x}(k)\\ \widehat{d}(k)\end{array}\right]+B_{\mathbf{a}}u(k)+L(y(k)-C_{\mathbf{a}}\widehat{x}(k)).$$

(8)

Here, the gain coefficient matrix L in (8) can be obtained by solving

$$L=A_{\mathbf{a}}M{C}_{\mathbf{a}}^{\mathbf{T}}{(R_{\mathbf{r}}+C_{\mathbf{a}}M{C}_{\mathbf{a}}^{\mathbf{T}})}^{-1}$$

(9)

and the Riccati equation

$$A_{\mathbf{a}}M{A}_{\mathbf{a}}^{\mathbf{T}}-A_{\mathbf{a}}M{C}_{\mathbf{a}}^{\mathbf{T}}{\left[R_{\mathbf{r}}+C_{\mathbf{a}}M{C}_{\mathbf{a}}^{\mathbf{T}}\right]}^{-1}{C}_{\mathbf{a}}M{A}_{\mathbf{a}}^{\mathbf{T}}-M+R_{\mathbf{w}}=\mathbf{0}$$

(10)

where R_w is the noise covariance matrix of u(k), and $R_{\mathbf{r}}$ is the noise covariance matrix of the process variable [x(k),d(k)]^T.

4.2. Feedback Control Module

The feedback controller belongs to the classical linear regulator. In order to ensure the stability of the OES, the order of the feedback controller should not be high. For this reason, a PI controller is usually adopted, and it can be expressed as

$$C_v(s)=\frac{K_{\mathrm{P}}s+K_{\mathrm{I}}}{s}$$

(11)

where K_p and K_I represents the proportional gain and integral gain, respectively.

The input of the controller (11) is derived from the error between the reference speed v_r and the speed $\widehat{v}$ observed from the SAKF, then, the output u_C can be obtained using a Tustin transformation with the sampling period T_s as:

$$u_{\mathrm{C}}(k+1)=u_{\mathrm{C}}(k)+K_{\mathrm{P}}(v_{\mathrm{r}}(k+1)-\widehat{v}(k+1))+K_{\mathrm{I}}{T}_{\mathrm{s}}(v_{\mathrm{r}}(k)-\widehat{v}(k)).$$

(12)

5. Implementation of the Proposed Control Algorithm

Since the SAKF and PI are related to each other, they have a strict logical sequence in implementation. Meanwhile, reasonable configuration of the parameters is essential for a satisfactory motion performance. Therefore, this section introduces the implementation and parameter setting methods for this control algorithm.

5.1. Implementation Procedure

The implementation procedure of the control algorithm are as follows:

• Initial state setting for the control algorithm:
  The initial state of the proposed controller includes the pole p and gain K of G_uv, the gain coefficient matrix L, the initial state of (6), namely $\widehat{\theta }(0),\widehat{v}(0),\widehat{d}(0)$, and the initial output of the PI module, i.e., $u_{\mathrm{C}}(0)$. Among them, p and K can be easily obtained through experiment identification. L can be calculated according to (6) to (10), and $\widehat{\theta }(0),\widehat{v}(0),\widehat{d}(0)$ and $u_{\mathrm{C}}(0)$ should be set to 0.
• Observation calculation of speed and torque disturbances:
  By substituting (6) and (7) into (8), the recursive value of $\widehat{v}(k)$ and $\widehat{d}(k)$ can be obtained as:

  $$\widehat{d}(k+1)=\widehat{d}(k)+L_3(v(k)-\widehat{v}(k))$$

  (13)

  $$\widehat{v}(k+1)=\left({\mathrm{e}}^{-p{T}_{\mathrm{s}}}-L_2\right)\widehat{v}(k)+L_2\cdot v(k)-\frac{K\left(1-{\mathrm{e}}^{-p{T}_{\mathrm{s}}}\right)}{p}\left(u(k)-d(k)\right).$$

  (14)
• Output calculation of the feedback controller:
  The recursive output value of the PI, i.e., $u_{\mathrm{C}}(k)$, can be obtained by calculating (12).
• Sum the output value of the SAKF and PI:
  The output of the SAKF and PI are summed to obtain a complete controller output as:

  $$u(k)=\widehat{d}(k)+u_{\mathrm{C}}(k).$$

  (15)
• Update the state variable of the system:
  After step 1 to step 4 are finished, then the output value u(1) can be calculated, and the state observations $\widehat{\theta }(k),\widehat{v}(k),\widehat{d}(k)$ and the controller output value $u_{\mathrm{C}}(k)$ need to be updated according to:

  $$\{\begin{array}{l}\widehat{\theta }(k+1)=\widehat{\theta }(k)\\ \widehat{v}(k+1)=\widehat{v}(k)\\ \widehat{d}(k+1)=\widehat{d}(k)\\ u_{\mathrm{C}}(k+1)=u_{\mathrm{C}}(k)\end{array}.$$

  (16)

Then, each subsequent iteration calculation should be performed in the order of step 2 to step 5.

5.2. Parameter Settings for SAKF

For model (6), assume that its process noise covariance matrix $R_{\mathbf{w}}=\mathrm{diag}\left[{\sigma }_{\theta },{\sigma }_v,{\sigma }_d\right]$, measurement noise covariance matrix $R_{\mathbf{r}}=\mathrm{diag}\left[{\sigma }_{\theta },{\sigma }_v\right]$. Therefore, in order to obtain $R_{\mathbf{w}}$ and $R_{\mathbf{r}}$, it is necessary to obtain ${\sigma }_{\theta },{\sigma }_v,{\sigma }_d$.

Assuming that the resolution of the encoder is $\mathsf{\delta }\theta$ and the encoder’s output value for the actual angle $\theta$ is $\overline{\theta }$, then the difference is $\Delta \theta =\overline{\theta }-\theta \in (-\frac{1}{2}\mathsf{\delta }\theta ,\frac{1}{2}\mathsf{\delta }\theta )$. Considering that the quantization error $\Delta \theta$ is evenly distributed over the interval $(-\frac{1}{2}\mathsf{\delta }\theta ,\frac{1}{2}\mathsf{\delta }\theta )$, so the angle variance is:

$${\sigma }_{\theta }=\mathrm{var}(\Delta \theta )={\displaystyle {\int }_{-\frac{\mathsf{\delta }\theta }{2}}^{\frac{\mathsf{\delta }\theta }{2}}\frac{1}{\mathsf{\delta }\theta }}\Delta {\theta }^2\mathrm{d}(\Delta \theta )=\frac{1}{12}\mathsf{\delta }{\theta }^2.$$

(17)

Similarly, if the speed resolution is $\mathsf{\delta }v$, then the speed variance should be:

$${\sigma }_v=\frac{1}{12}\mathsf{\delta }{v}^2$$

(18)

As the disturbance is not easy to measure and model, the disturbance covariance ${\sigma }_d$ depends on experimental tuning. If ${\sigma }_d$ is set improperly large, the disturbance observation response will be faster and the suppression effect will be better. However, the observation result will be sensitive to the measurement noise, and the stability margin will decrease dramatically. Considering that most of the disturbances are originated by speed, it is reasonable to set the initial value of ${\sigma }_d$ as:

$${\sigma }_d=B\cdot {\sigma }_v$$

(19)

where B is the viscous friction coefficient, and then gradually increase ${\sigma }_d$ until the servo performance and stability margin are both satisfactory.

5.3. Parameter Settings for PI

After the SAKF is properly configured, a rotational speed of high quality can be estimated, and the torque disturbance can be effectively compensated. Therefore, the dynamics of the OES with the SAKF is close to the ideal linear model (4). In this case, the tuning steps of K_P and K_I can be based on the linear model (4).

As the forward transfer function $G_{\mathrm{o}}(s)=G_{\mathrm{uv}}(s)C_v(s)$, where $C_v(s)=\frac{K_{\mathrm{P}}s+K_{\mathrm{I}}}{s}$ and $G_{\mathrm{uv}}(s)=\frac{K}{s+p}$, so

$$G_{\mathrm{o}}(s)=\frac{K(K_{\mathrm{P}}s+K_{\mathrm{I}})}{s(s+p)}.$$

(20)

Then, let $|G_{\mathrm{o}}(\mathrm{j}\omega )|\approx 1$, so one can have

$$K_{\mathrm{P}}{}^2+{\left(\frac{K_{\mathrm{I}}}{{\omega }_{\mathrm{c}}}\right)}^2={\left(\frac{p^2+{\omega }_{\mathrm{c}}{}^2}{K}\right)}^2,$$

(21)

and the open-loop phase angle can be simultaneously obtained as

$$\arg (G_{\mathrm{o}}(\mathrm{j}{\omega }_{\mathrm{c}}))=\arctan \left(\frac{K_{\mathrm{P}}}{K_{\mathrm{I}}}{\omega }_{\mathrm{c}}\right)-\arctan \left(-\frac{p}{{\omega }_{\mathrm{c}}}\right).$$

(22)

Thus the phase margin

$${\theta }_p=\mathsf{\pi }+\arg (G_{\mathrm{o}}(j{\omega }_{\mathrm{c}}))=\mathsf{\pi }+\arctan \left(\frac{K_{\mathrm{P}}}{K_{\mathrm{I}}}{\omega }_{\mathrm{c}}\right)+\arctan \left(\frac{p}{{\omega }_{\mathrm{c}}}\right).$$

(23)

If $\lambda =\lambda ({\theta }_p)=\left|\frac{1}{{\omega }_{\mathrm{c}}}\tan \left({\theta }_p-\mathsf{\pi }-\arctan (\frac{p}{{\omega }_{\mathrm{c}}})\right)\right|$ and ${\omega }_{\mathrm{c}}=2\mathsf{\pi }{f}_{\mathrm{c}}$, then the relationship between K_P, K_I, ${\omega }_{\mathrm{c}}$ and ${\theta }_p$ can be obtained according to (20) and (22) as:

$$\{\begin{array}{l}{K}_{\mathrm{P}}=\frac{\lambda {\omega }_{\mathrm{c}}}{K}\sqrt{\frac{p^2+{\omega }_{\mathrm{c}}{}^2}{1+{\lambda }^2{\omega }_{\mathrm{c}}{}^2}}\\ K_{\mathrm{I}}=\frac{{\omega }_{\mathrm{c}}}{K}\sqrt{\frac{p^2+{\omega }_{\mathrm{c}}{}^2}{1+{\lambda }^2{\omega }_{\mathrm{c}}{}^2}}\end{array}.$$

(24)

Denote the resonant frequency of the OES as f_R, and the damping coefficient at the resonant frequency as ${\xi }_{\mathrm{R}}$, then the closed-loop bandwidth $f_{\mathrm{B}}\le 2f_{\mathrm{R}}{\xi }_{\mathrm{R}}$ [16]. As ${\xi }_{\mathrm{R}}$ ranges between 0.1 to 0.35, it is reasonable to set ${\xi }_{\mathrm{R}}=0.25$, and therefore:

$$f_{\mathrm{B}}\le 0.5f_{\mathrm{R}}$$

(25)

Considering that there is a relationship between the closed-loop bandwidth f_B and the open-loop cut-off frequency f_C as:

$$f_{\mathrm{B}}=2f_{\mathrm{C}}.$$

(26)

Then a relationship between f_C and f_R can be obtained as:

$$f_{\mathrm{C}}\le 0.25f_{\mathrm{R}}.$$

(27)

Here, let $f_{\mathrm{C}}=0.25f_{\mathrm{R}}$, then the relationship between K_P, K_I, and f_R can be obtained by Equation (24) as:

$$\{\begin{array}{l}{K}_{\mathrm{P}}=\frac{\lambda \mathsf{\pi }{f}_{\mathrm{R}}}{2K}\sqrt{\frac{4p^2+{\mathsf{\pi }}^2{f}_{\mathrm{R}}{}^2}{4+{\lambda }^2{\mathsf{\pi }}^2{f}_{\mathrm{R}}{}^2}}\\ K_{\mathrm{I}}=\frac{\mathsf{\pi }{f}_{\mathrm{R}}}{2K}\sqrt{\frac{4p^2+{\mathsf{\pi }}^2{f}_{\mathrm{R}}{}^2}{4+{\lambda }^2{\mathsf{\pi }}^2{f}_{\mathrm{R}}{}^2}}\end{array}.$$

(28)

Therefore, after determining the resonance frequency $f_{\mathrm{R}}$ and the phase margin ${\theta }_p$ of the OES, the satisfactory PI controller parameters can be obtained according to Equation (28).

According to Equations (20) and (28), the open-loop transfer function of the system, $G_{\mathrm{o}}(s)$, can be derived as:

$$G_{\mathrm{o}}(s)=\frac{\lambda \mathsf{\pi }{f}_{\mathrm{R}}}{2}\sqrt{\frac{4p^2+{\mathsf{\pi }}^2{f}_{\mathrm{R}}{}^2}{4+{\lambda }^2{\mathsf{\pi }}^2{f}_{\mathrm{R}}{}^2}}\cdot \frac{1}{s(\mathrm{s}+\mathrm{p})}\cdot (\mathrm{s}+\frac{1}{\lambda }).$$

(29)

Thus the pole of $G_{\mathrm{o}}(s)$ is −p. As $p=\frac{B}{J}\in (0,+\infty )$, so the pole of $G_{\mathrm{o}}(s)$ keeps staying in the left half plane of the zero-pole diagram. Meanwhile, as a feedforward controller, the SAKF module does not play a role in the system stability. So the system remains stable.

6. Experimental Verification

6.1. Introduction of the Experimental Setup

In order to verify the effectiveness of the control algorithm, an experimental setup comprised of a ds1104 [17], a power supply equipment, a swing table, a gyroscope, and three transmission components is built, as shown in Figure 4 and Figure 5. The swing table is controlled by the ds1104 to simulate the flight of aircraft. Meanwhile, the transmission components (i.e., a direct-driving component, a harmonic-driving component, and a RV-driving component) and the gyro are mounted on this swing table. Each component consists of a motor, an encoder or a tachometer, and a reducer. The main attributes of these components are listed in

Table 1. Main parameters of the experimental setup.

Category	Device	Type	Parameter	Value
Inertial sensor	Gyroscope	STIM210	Resolution (°/sec) Range/((°)·s−1)	4.768 × 10−5 ±400
Direct-driving component	Motor	130LCX-2	Rated voltage/V	24
	Tachometer	-	Speed coefficient (r·min−1)	±300
Harmonic-driving component	Motor	12CDT-003	Rated voltage/V	24
	Harmonic reducer	XBS80-100	Transmission ratio	100
	Absolute encoder	BCE90K40-17	Resolution/(°)	0.0027
RV-driving component	Motor	80BL110S50430	Rated voltage/V	24
	RV reducer	RV-201	Transmission ratio	161
	Incremental encoder	EW100049A	Lines	4000

. Meanwhile, both the system and controller parameters of the components are listed in

Table 2. System and controller parameters of the components.

	Direct-Driving Component	Harmonic-Driving Component	RV-Driving Component	Unit
J	3.2 × 10−5	3.44 × 10−5	3.6 × 10−5	Kg m2
B	1.0 × 10−1	1.1 × 10−1	1.2 × 10−1	Nm/(rad s−1)
Kp	4.78 × 10−2	5.26 × 10−2	5.75 × 10−2	Nm/(rad s−1)
KI	6.9	7.5864	8.2722	Nm/rad
${\sigma }_{\theta }$	0	1.85 × 10−14	7.93 × 10−12	rad2
${\sigma }_v$	4.86 × 10−2	1.85 × 10−8	7.93 × 10−6	(rad/s)2
${\sigma }_d$	4.90 × 10−3	2.04 × 10−9	9.8 × 10−7	(Nm)2

.

6.2. Performance Test Method

6.2.1. Low-Speed Motion Performance

In order to effectively evaluate the performance of low-speed motion with the different control methods, the angular fluctuation rate $\epsilon$ can be defined to quantify the low-speed-motion performance as:

$$\epsilon =\frac{\Delta {\theta }_{pp}}{{\theta }_{ref}}$$

(30)

where ${\theta }_{ref}$ represents the variation of the ramp position, and $\Delta {\theta }_{pp}$ is the peak-to-peak value of the tracking error [16], as is shown in Figure 6. Obviously, for the same displacement ${\theta }_{ref}$, if the speed fluctuates more obviously, $\Delta {\theta }_{pp}$ will be larger, and then $\epsilon$ will increase. Ideally, when the component rotates in a constant speed, then $\Delta {\theta }_{pp}=0$ and $\epsilon =0$ exactly.

6.2.2. Inertial Stability Accuracy

Since the OES is used to isolate the motion of the carrier so that the infrared detector can stably point to a fixed direction in the inertial space, the tracking error magnitude between the motion angle of the carrier and the motion angle of OES, denoted as $e$, can reflect the inertia stability accuracy of the OES. When the carrier moves in a sinusoidal function of 1° and 1 Hz, the obtained error magnitude e is used as the criterion for evaluating the stability accuracy, as is shown in Figure 7 [16]. Obviously, the smaller e is, the higher inertial stability accuracy of the OES will be.

6.3. Comparison of Experimental Results

6.3.1. Speed Observation

In order to compare the speed observation performance of the SAKF and the LPF, make the RV-driving component run in torque mode. Set the reference torque to a sine function with amplitude 0.5 Nm and frequency 1 Hz, and use the ds1104 to sample the speed signal by the SAKF and LPF simultaneously. The bandwidth of the 2-order Butterworth LPFs are set equal to the bandwidth of the SAKF, 25 Hz. The result is shown in Figure 8, where the magenta line is the original speed differentiated by encoder, the red line is the speed filtered by the Butterworth LPF, and the blue line is the speed estimated by the SAKF.

As can be seen from Figure 8, the noise from the original speed signal is evidently reduced by both the SAKF and LPF. However, the speed from the LPF has a significant delay, while the delay of the speed estimated by the SAKF is almost negligible. Therefore, comparing with the LPF, the SAKF has a better speed observation performance.

6.3.2. Low-Speed Motion Performance

The PI control method and the proposed composed control method respectively perform speed closed-loop control on the direct drive component, the harmonic drive component, and the RV drive component. Keep the swing table still and set the reference speed v_r to 0.01°/s, then collect the encoder data and plot the angular position value. The typical curve is shown in Figure 9. In Figure 9a,c,e, the red slash represents the motion trajectory envelope when using PI, the blue slash represents the motion trajectory envelope when using the PI + SAKF. Meanwhile, the solid red line represents the actual trajectory by the PI, the solid blue line represents the actual trajectory by the PI + SAKF, and the green line represents the ideal trajectory without any tracking errors. Figure 9b,d,f show the tracking error of the three components when using the PI and the PI + SAKF as the control scheme respectively.

By measuring the fluctuation amplitudes of the respective curves in Figure 9, the low-speed motion performance by the two control schemes can be listed in

Table 3. Comparison of low-speed motion performance.

No.	Category	PI	PI + SAKF
1	Direct-driving component	0.61	0.23
2	Harmonic-driving component	0.09	0.03
3	RV-driving component	0.09	0.01

.

By analyzing the data in Table 3, it can be seen that when the control method proposed in this paper is adopted, all three components can obtain better low-speed motion performance, which means that the proposed control scheme can be applied in various types of OESs and can compensate torque disturbance effectively.

6.3.3. Inertial Stability Accuracy

In order to compare the inertial stability accuracy of the three components by PI and PI + SAKF, make the swing table rotate in a sinusoid reference angle with an amplitude of 1° and a frequency of 1 Hz. Meanwhile, set the inertial reference speed ${\omega }_{\mathrm{r}}$ to 0 for all three components. Then subtract the swing table angle from each component angle to obtain the residual motion of the OESs.

The residual angle of the OESs by two control methods, and the angle of the swing table are shown in Figure 10.

The comparison results of the stability accuracy by PI and PI + SAKF are shown in

Table 4. Comparison of stabilization accuracies.

No.	Category	PI	PI + SAKF
1	Direct-driving component	58%	3%
2	Harmonic-driving component	14%	4%
3	RV-driving component	10%	6%

. It can be seen from Table 4 that, comparing with the PI method, the PI + SAKF can significantly reduce the error magnitude of all three OESs under the same disturbance conditions, which indicates that the proposed control scheme can achieve effective improvement of stability accuracy in a wide range of OESs.

7. Conclusions

In order to effectively compensate the torque disturbance and improve the speed measurement performance, a strap-down stability control algorithm based on the PI + SAKF is proposed. The principle, implementation procedure, and parameter tuning guidelines of the algorithm are given. Then the performance of the control algorithm is verified for three types of OESs, including the direct-driving component, the harmonic-driving component, and the RV-driving component. The experimental results show that a better observation and suppression of torque disturbance can be achieved in all three OESs. Compared with the PI, this composed control scheme can increase the low-speed-motion performance by 62.30% (direct-driving), 66.67% (harmonic-driving), and 88.89% (RV-driving), and increase the stability accuracy by 94.83% (direct-driving), 71.43% (harmonic-driving), and 40.00% (RV-driving), which provides an effective scheme for achieving high inertial stability performance and low-speed performance of opto-electric stability servomechanisms.