A Composite Control Method Based on Model Predictive Control and a Disturbance Observer for the Acquisition, Tracking, and Pointing System

Abstract

Nonlinear disturbances, such as friction, backlash, and base vibration, are the main factors restricting the further improvement of the dynamic performance of the Acquisition, Tracking and Pointing (ATP) system. However, the modeling and compensation of the time-varying nonlinearities are still challenging. In this paper, a compound control algorithm based on model predictive control (MPC) and a Proportional Multiple-Integral State-Augmented Kalman Filter (PMISAKF) disturbance observer is proposed to improve the disturbance rejection performance. Specifically, a PMISAKF is used to estimate the system’s state and external disturbances in real-time. These observed disturbances are then treated as known external inputs at the current or future time points and incorporated into the predictive model of MPC. This allows the optimization process to take these external factors into consideration, enabling the calculation of an optimal control strategy and achieving high pointing and tracking accuracy for nonlinear time-varying disturbances. Experimental results show that the proposed method can effectively improve the system’s anti-interference ability and tracking accuracy.

1. Introduction

The Acquisition, Tracking and Pointing (ATP) system refers to a system that is capable of precisely controlling equipment or mechanisms to make minute adjustments or undertake positioning in a specified direction. It typically possesses features such as high-precision pointing, stable control, and rapid response. It has been widely applied in many manned and unmanned platforms for receiving and transmitting information with seeker devices such as antennas, IR/UV sensors, optical devices, etc. for reconnaissance, prewarning, guidance, communication, and other functions [1,2,3]. The development of the new-generation ATP system has put forward higher requirements for pointing accuracy, servo bandwidth, compact structure, and lightweight design. Among them, the servo bandwidth represents the system’s ability to follow rapidly changing commands. However, nonlinear disturbances, such as friction, backlash, mass unbalance, and carrier disturbance, restrict further improvement of the performance of the ATP system. As a result, disturbance rejection performance becomes a key controller property for achieving high pointing and tracking accuracy.

Nonlinear disturbances can be roughly divided into two categories: time-invariant and time-varying disturbances. For time invariant disturbances, scholars have proposed various control algorithms, such as sliding-mode control [4,5,6] and internal mode control [7,8]. They have been proven to be very useful in practical systems. However, for time-varying disturbances, such as friction, backlash, and carrier disturbance, it is difficult to effectively suppress them using the above controllers.

In order to suppress the nonlinear time-varying disturbances, scholars have conducted extensive research. The compensation methods can be mainly divided into two categories: disturbance-model-based and observer-based. The basic concept of a disturbance-model-based compensation method is to design accurate nonlinear disturbance models and then compensate them with a feed-forward compensator. Cong et al. [9] designed an improved Stribeck friction model and acquired the parameters identified by a genetic algorithm. By optimizing the friction model, the chattering phenomenon and limit cycle caused by changes in the motion direction of the inertial stability platform and friction overcompensation were effectively improved. In a similar vein, Li et al. [10] proposed a novel pre-stiction friction model which is a modified LuGre model, including the pre-sliding, the sliding and the pre-stiction regimes, that can characterize friction behaviors in the pre-stiction regime. Liao et al. [11] proposed a new friction model of a harmonic gear servo system, which is related to both its velocity and position. The drawback of the disturbance-model-based compensation method is that it is difficult to construct a disturbance model with sufficient accuracy, especially when coupled with other random disturbances.

To address these disturbances without a specific structure, disturbance observers have been extensive studied. The most widely used ones include disturbance observer (DOB), Kalman filter, extended state observer (ESO), active disturbance rejection control (ADRC), generalized proportional integral observer (GPIO), etc. [12,13,14,15]. Although the disturbance-observer-based compensation methods mentioned above ensure that the estimated disturbances asymptotically follow the actual disturbance, its observation performance is weak when the disturbance is complex and has no specific structure. According to Li et al. [7], disturbances without a specific structure can be considered as time-varying disturbances, which can be approximated by a finite-order time polynomial function, so the several orders derivatives of the disturbances can be considered as augmented states for simultaneous estimation. Therefore, the proportional multiple-integral (PMI) observer has been investigated for estimating such time-varying disturbances [16,17,18,19] and good compensation results have been achieved. On this basis, Jiang et al. [20] fulfilled the PMI observer in the State-Augmented Kalman filter framework (PMISAKF). The PMISAKF observer is designed based on the time series polynomial function of time-varying disturbances. It can acquire a better disturbance estimation result and fully consider the measurement noise, process noise, and mobility characteristics of the estimated disturbance in the discrete system. Li et al. [21] investigate the mapping relationship between internal and external disturbances and servo performance by establishing a high-precision dynamics model of the servo device. The control method is also a combination of a PI controller and PMISAKF. However, They directly compensate the disturbance observation values into the system, which can easily cause system instability during high-frequency or instantaneous disturbances.

In recent years, many scholars have conducted research on model predictive control (MPC) to address the time-varying, nonlinear, and uncertain characteristics of external disturbances. MPC can solve optimal control problems in a finite time domain without the need for precise models through time-domain rolling iterative optimization, enabling it to handle uncertainty problems caused by model mismatch and unknown disturbances [22,23,24]. The disturbance observer-based technique is a promising method for improving the robustness of MPC by estimating and compensating disturbances simultaneously [25]. Examples of disturbance observer-based MPC include [26,27]. Emami et al. [26] developed an adaptive model-based predictive controller for attitude and trajectory tracking of a vertical take-off and landing aircraft, and two observers were introduced to simultaneously estimate the model uncertainty and external disturbance; additionally, time-varying trajectory tracking was carried out via simulation, and the experiment was not presented. Wang et al. [27] presents an improved equivalent input disturbance observer-based model predictive control (IEIDO-based MPC) scheme for position tracking control of a magnetic levitation system. The IEIDO was first proposed to estimate both the system states and the time-varying equivalent input disturbance. It effectively demonstrates that MPC based on a disturbance observer has emerged as a promising strategy for enhancing the system’s capability in mitigating disturbances. It is crucial to note that, while the magnetic levitation system necessitates sub-millimeter servo accuracy, its applicability to the ATP system, which demands even higher tracking and pointing accuracy, remains an open question that warrants further validation.

The innovation of this paper lies in proposing a PMISAKF-MPC controller which significantly enhances the system’s capacity to counteract nonlinear and time-varying disturbances. Specifically, a PMISAKF is an algorithm that combines predictive modeling, incremental state-space representation, and adaptive Kalman filtering techniques. It utilizes the system’s dynamic model and observational data to estimate and track the system’s state and external disturbances in real-time through filtering and prediction steps. The disturbance estimates obtained through a PMISAKF can be regarded as known external inputs at the current or future time points. By incorporating time-varying disturbance information into the predictive model of MPC, the optimization process can take these external factors into account and compute the optimal control strategy accordingly. MPC continuously updates its control strategy through iterative optimization processes based on new observational data and disturbance estimates. This iterative nature allows MPC to flexibly address system uncertainties and external disturbances, achieving high tracking and pointing accuracy under nonlinear time-varying disturbances. Furthermore, it helps the MPC controller maintain system stability and performance in the presence of time-varying disturbances, enhancing the robustness and adaptability of the control.

This paper is organized as follows. A dual-inertia dynamics model of the system is developed and the disturbances are analyzed in Section 2. Then, the PMISAKF-MPC strategy is described. In Section 4, the results of the comparison experiments are analyzed. Section 5 concludes this paper.

2. Dynamic Modeling

A high-precision ATP system is crucial for maintaining a stable line of sight (LOS) in inertial space. Its primary function is to isolate various multi-source disturbances, including friction, backlash, and carrier disturbance, ensuring that the LOS remains unperturbed and precisely aligned towards its intended target. The structure of a typical two-axis ATP system is shown in Figure 1. The azimuth/elevation axis is derived by the electromechanical actuator (EMA), which is mainly composed of key components such as the driver controller, motor, reducer, encoder, and gear reducers.

The EMA usually adopts two driving methods, namely direct motor driving or indirect reducer driving. The direct motor driving method avoids the impact of transmission chain factors on servo performance by directly coupling the motor with the load. It can achieve good control accuracy and a working bandwidth, but this method requires a high output torque in the motor, which limits its application in situations where space size and weight requirements are strict. The indirect reducer driving method can reduce the volume and weight of the servo mechanism to a certain extent. However, this transmission method inevitably has nonlinear factors such as friction, backlash, etc. These nonlinear factors exist in the system in the form of disturbances. Along with external disturbances, it restricts the improvement of the servo performance of the ATP system. As a result, the disturbance rejection performance becomes a key controller property in achieving precise positioning, particularly in regard to the indirect reducer driving method.

2.1. Modeling of the ATP System

The ATP SYSTEM with an indirect reducer driving method usually consists of an AC servo motor, a gear reducer, and a load. Considering the elastic components of the transmission mechanism, we can use the dual-inertia model to represent it,

$$\left\{\begin{array}{l}\begin{array}{l}{\dot{\theta }}_{\mathrm{m}}={\omega }_{\mathrm{m}}\\ {\dot{\theta }}_{\mathrm{L}}={\omega }_{\mathrm{L}}\\ J_m{\dot{\omega }}_m=u{\mathrm{K}}_{\mathrm{I}}{\mathrm{K}}_{\mathrm{T}}-{\displaystyle \frac{1}{N}}K\left({\displaystyle \frac{1}{N}}{\theta }_{\mathrm{m}}-{\theta }_{\mathrm{L}}\right)\end{array}\hfill \\ J_L{\dot{\omega }}_L=K\left({\displaystyle \frac{1}{N}}{\theta }_{\mathrm{m}}-{\theta }_{\mathrm{L}}\right)+T_d\hfill \end{array}\right.$$

(1)

where, ${\theta }_m,{\omega }_m$ are the angle and velocity of the AC servo motor, ${\theta }_L,{\omega }_L$ are the angle and velocity of the load, u is the motor voltage, ${\mathrm{K}}_{\mathrm{I}}$ is the voltage conversion coefficient of the drive, ${\mathrm{K}}_{\mathrm{T}}$ is the motor torque coefficient, $N$ and $K$ are the reduction ratio and stiffness of the gear reducer, respectively. $J_m,J_L$ are the inertia of the motor and load, and $T_d$ is the total disturbances torque. The established dual-inertia model of a single axis of an ATP system is shown in Figure 2.

Among them, the nonlinear disturbances, such as friction, backlash, and carrier disturbance, restrict further improvements in the performance of the system. It is necessary to analyze the characteristics of each of them and find the appropriate method to solve them.

2.2. Disturbances Modeling

According to the Ref. [17], the disturbances could be classified into two parts, namely the equivalent torque disturbance of friction, $T_f$, and carrier disturbance, $T_b$. They could be described as

$$\left\{\begin{array}{l}{T}_b=-4{\pi }^2{J}_{m}N(N-1){\displaystyle \sum _{i=1}^{N_b}{M}_i{f}_i^2\sin (2\pi f_{i}t})\hfill \\ T_f={\mu }_b{F}_b{R}_{b}sign({\omega }_{dn})+{\mu }_f{T}_{m}sign({\omega }_{dn}){\displaystyle \frac{(R_{dn}+\Delta r/2)}{(R_{dg}+\Delta r/2)\cos \alpha }}\hfill \end{array}\right.$$

(2)

where, $N_b$, M_i and f_i are the number, amplitude, and frequency of frequency components, respectively. ${\mu }_b$ is the viscous damping friction coefficient of the bearing, $F_b$ is the total equivalent force borne by the bearing, $R_b$ is the bearing gyration radius, and ${\omega }_{dn}$ is the equivalent relative rotation speed of the driven gear. Additionally, ${\mu }_f$ is the constant meshing friction coefficient, $T_m$ is the output torque of the motor, and $R_{dn}$ and $R_{dg}$ are the circle radius of the driven and driving gear, respectively. $\Delta r$ is the displacement increment of the rotation center of the driving gear, α represents the operating pressure angle.

We can divide the disturbance into two parts, namely time-invariable disturbances ${\overline{T}}_d$ and time-varying disturbances ${\tilde{T}}_d$. Equation (2) can be simplified as,

$$\left\{\begin{array}{l}\begin{array}{l}{T}_d={\overline{T}}_d(\omega )+{\tilde{T}}_d(t)\\ {\overline{T}}_d(\omega )={\mu }_{b}sign({\omega }_p)N_b{R}_b\end{array}\hfill \\ {\tilde{T}}_d(t)={\mu }_{g}sign({\omega }_p){\displaystyle \frac{(R_g+\Delta r/2)T_m}{(R_p+\Delta r/2)\cos \alpha }}-4{\pi }^2{J}_{m}N(N-1){\displaystyle \sum _{j=1}^{N_b}{M}_{bj}{f}_{bj}^2\sin (2\pi f_{bj}t})\hfill \end{array}\right.$$

(3)

Time-invariant disturbances can be considered as a stepwise signal that can introduce steady-state errors to the servo system, while the time-varying disturbance is the combination of multiple periodic signals with unknown frequencies and magnitudes. It introduces dynamic errors to the LOS and may lead to system oscillation and dispersion.

3. PMISAKF-MPC Controller

Based on the above analysis, a PMISAKF-MPC controller has been proposed. The structure of the proposed control algorithm is shown in Figure 3. Specifically, the PMISAKF observer is used to estimate time-varying disturbances, and then the rolling iteration optimization mechanism of MPC is utilized to incorporate the obtained estimated time-varying disturbances to predict the optimal control output for the next time step. This enables the system to maintain LOS stability and precise tracking of commands under the influence of friction and carrier disturbances.

3.1. Design of PMISAKF Observer

It can be assumed that the time-varying disturbance ${\tilde{T}}_d$ can be described by the following polynomial function:

$${\tilde{T}}_{\mathrm{d}}=a_0+a_{1}t+a_2{t}^2+\cdot \cdot \cdot +a_{q-1}{t}^{q-1}$$

(4)

where $a_i\left(i\in \left\{0,1,2,\dots ,q-1\right\}\right)$ are constant but unknown coefficients.

The q-order derivation of ${\tilde{T}}_d$ is bounded, so we can extend ${\tilde{T}}_d,{\tilde{T}}_d^{(1)},{\tilde{T}}_d^{(2)},\dots ,{\tilde{T}}_d^{(q-1)}$ as the state for simultaneous estimation, and the PMISAKF observer compensates well for this type of disturbance.

In this section, a PMISAKF observer based on the dual-inertia model is proposed to estimate the time-varying disturbance ${\tilde{T}}_d$. Assuming that the time-invariable disturbance ${\overline{T}}_d$ is perfectly eliminated, recalling Equation (1), the following system can be obtained:

$$\left\{\begin{array}{l}\dot{x}=Ax+Bu+C{\tilde{T}}_d\hfill \\ y=Ex\hfill \end{array}\right.$$

(5)

where,

$$A=\left[\begin{array}{cccc}0& 1& 0& 0\\ {\displaystyle \frac{-K}{N^2{J}_m}}& 0& {\displaystyle \frac{K}{N{J}_m}}& 0\\ 0& 0& 0& 1\\ {\displaystyle \frac{K}{N{J}_L}}& 0& {\displaystyle \frac{-K}{J_L}}& 0\end{array}\right],B=\left[\begin{array}{c}0\\ {\displaystyle \frac{K_{\mathrm{I}}{K}_{\mathrm{T}}}{J_m}}\\ 0\\ 0\end{array}\right],C=\left[\begin{array}{c}0\\ {\displaystyle \frac{1}{J_m}}\\ 0\\ 0\end{array}\right],E={\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]}^{\mathrm{T}}$$

Assuming that ${\tilde{T}}_d$ can be approximated by the polynomial shown in Equation (4), the q-order derivative of the disturbance ${\tilde{T}}_d^{(q)}$ is bounded. To track the time-varying disturbance, we introduce the following parameters:

$${\zeta }_1={\tilde{T}}_d^{(q)},{\xi }_2={\tilde{T}}_d^{(q-1)},\dots ,{\zeta }_i={\tilde{T}}_d^{(q-i+1)},\dots ,{\zeta }_q={\tilde{T}}_d$$

(6)

where ${\tilde{T}}_d^{(i)}$ is the $i$th-order derivative of ${\tilde{T}}_d$.

The PMISAKF observer used to obtain the estimates of velocity and disturbance for Equations (5) and (6) is proposed as follows:

$$\begin{array}{l}\dot{\widehat{x}}=A\widehat{x}+Bu+L_{\omega }(y-E\widehat{x})+C{\widehat{\zeta }}_q,\\ {\dot{\widehat{\zeta }}}_q=L_{\mathrm{I}}^q(y-E\widehat{x})+{\widehat{\zeta }}_{q-1},\\ {\dot{\widehat{\zeta }}}_{q-1}=L_{\mathrm{I}}^{q-1}(y-E\widehat{x})+{\widehat{\zeta }}_{q-2},\\ \hspace{1em}\hspace{1em}\hspace{1em}⋮\\ {\dot{\widehat{\zeta }}}_2=L_{\mathrm{I}}^2(y-E\widehat{x})+{\widehat{\zeta }}_1,\\ {\dot{\widehat{\zeta }}}_1=L_{\mathrm{I}}^2(y-E\widehat{x}).\end{array}$$

(7)

where $\widehat{x}$ and ${\widehat{\zeta }}_i$ are the estimated values of $x$ and ${\zeta }_i$, respectively. The observer gain $L_{\omega }$ and the integral factors of the observer $L_{\mathrm{I}}^i\left(i\in \left\{1,2,\cdots ,q\right\}\right)$ are the parameters to be designed.

The estimate disturbance ${\widehat{\zeta }}_q$ can be expressed as:

$$\begin{array}{ll}{\widehat{\zeta }}_q=& L_{\mathrm{I}}^q{\displaystyle \int (y-E\widehat{x})}\mathrm{d}\tau +L_{\mathrm{I}}^{q-1}{\displaystyle \iint (y-E\widehat{x})}\mathrm{d}{\tau }^2+\cdots \\ & +L_{\mathrm{I}}^2\underset{q-1}{\underbrace{{\displaystyle \iint \cdots }{\displaystyle \int (y-E\widehat{x})}}}\mathrm{d}{\tau }^{q-1}\\ & +L_{\mathrm{I}}^1\underset{q}{\underbrace{{\displaystyle \iint \cdots }{\displaystyle \int (y-E\widehat{x})}}}\mathrm{d}{\tau }^q\end{array}$$

(8)

If $X={\left[x,{\zeta }_1,{\zeta }_2,\cdots ,{\zeta }_q\right]}^{\mathrm{T}}$, the PMISAKF observer used to estimate the defined state can be obtained by combining (7) and (8):

$$\left\{\begin{array}{l}\dot{\widehat{X}}=\overline{\mathit{A}}\widehat{X}+\overline{\mathit{B}}u+\overline{\mathit{L}}{\left[(y-\overline{\mathit{E}}\widehat{X}){\displaystyle \int (y-\overline{\mathit{E}}\widehat{X})}\mathrm{d}\tau \cdots \underset{q-1}{\underbrace{{\displaystyle \iint \cdots }{\displaystyle \int (y-\overline{\mathit{E}}\widehat{X})}}}\mathrm{d}{\tau }^{q-1}\right]}^{\mathrm{T}}\hfill \\ \widehat{y}=\overline{\mathit{E}}\widehat{X}\hfill \end{array}\right.$$

(9)

where

$$\overline{\mathit{A}}=\left[\begin{array}{ccccc}A& 0& \cdots & 0& C\\ 0& 0& \cdots & 0& 0\\ 0& 1& \cdots & 0& 0\\ ⋮& ⋮& \ddots & ⋮& ⋮\\ 0& 0& \cdots & 1& 0\end{array}\right],\overline{\mathit{B}}=\left[\begin{array}{c}B\\ 0\\ 0\\ ⋮\\ 0\end{array}\right],\overline{\mathit{E}}={\left[\begin{array}{c}E\\ 0\\ 0\\ ⋮\\ 0\end{array}\right]}^{\mathrm{T}},\overline{\mathit{L}}=\left[\begin{array}{cccc}{L}_1& L_2& \cdots & L_q\end{array}\right]=\left[\begin{array}{cccc}{L}_{\omega }& 0& 0& 0\\ L_{\mathrm{I}}^1& 0& 0& 0\\ L_{\mathrm{I}}^2& L_{\mathrm{I}}^1& 0& 0\\ ⋮& ⋮& \ddots & 0\\ L_{\mathrm{I}}^q& L_{\mathrm{I}}^{q-1}& \cdots & L_{\mathrm{I}}^1\end{array}\right]$$

Similarly, Equation (9) can also be rewritten in the discrete-time domain with a zero order hold as:

$${\overline{\mathit{A}}}_d=e^{\overline{\mathit{A}}{T}_s},{\overline{\mathit{B}}}_{\mathit{d}}={\displaystyle \underset{0}{\overset{T_s}{\int }}{e}^{{\overline{\mathit{A}}}_d\tau }d\tau }\cdot \overline{\mathit{B}},{\overline{\mathit{E}}}_{\mathit{d}}=\overline{\mathit{E}}$$

(10)

The observation error of the system $e_X=\widehat{X}-X$ can be expressed as

$${\dot{e}}_X=(\overline{\mathit{A}}-L_1\overline{\mathit{E}})e_X$$

(11)

In Equation (5), pair $(A,E)$ is observable, and the following equation holds:

$$\mathrm{rank}\left[\begin{array}{c}s\mathit{I}-\overline{\mathit{A}}\\ \overline{\mathit{E}}\end{array}\right]=\mathrm{rank}\left[\begin{array}{c}s\mathit{I}-A\\ E\end{array}\right]+q$$

(12)

Pair $\left(\overline{\mathit{A}},\overline{\mathit{E}}\right)$ is observable. Therefore, the error model shown in Equation (11) is asymptotically stable after selecting an appropriate $L_1$.

Based on the previously established dynamic model parameters, as well as noise and disturbance covariance values, the gain $L_1$ of the observer can be computed offline iteratively by the following equation until convergence to the steady-state value is reached.

$$\left\{\begin{array}{l}\overline{P}(k|k-1)={\overline{\mathit{A}}}_d\overline{P}(k-1|k-1){\overline{\mathit{A}}}_d^{\mathrm{T}}+\overline{W}{\overline{R}}_w{\overline{W}}^{\mathrm{T}}\hfill \\ L_1(k)=\overline{P}(k|k-1){\overline{\mathit{E}}}_d^{\mathrm{T}}{\left[{\overline{R}}_v+{\overline{\mathit{E}}}_{\mathit{d}}\overline{P}(k|k-1){\overline{\mathit{E}}}_d^{\mathrm{T}}\right]}^{-1}\hfill \\ \overline{P}(k|k)=\left[I-L_1(k){\overline{\mathit{E}}}_{\mathit{d}}\right]\overline{P}(k|k-1)\hfill \end{array}\right.$$

(13)

where $\overline{W}$ is the noise input matrix, ${\overline{R}}_w=\mathrm{diag}\left\{R_u,R_{{\zeta }_1},R_{{\zeta }_2},\cdots ,R_{{\zeta }_q}\right\}$ is the noise variance in the system process noise, including the noise variance in the control voltage and the noise variance in each order disturbance, ${\overline{R}}_v=\mathrm{diag}\left\{R_{{\theta }_m},R_{{\omega }_m},R_{{\theta }_L},R_{{\omega }_L}\right\}$ is the measured noise variance matrix of the angle and the velocity of the motor and the load, $\overline{P}(k-1|k-1)$ is the covariance in the estimation error vector, and $\overline{P}(k|k-1)$ is its prediction to the next iteration.

The values of $R_{{\theta }_m},R_{{\omega }_m},R_{{\theta }_L},R_{{\omega }_L}$ can be obtained by calculating the noise variance in the encoder and the gyro, and $R_u$ is the analog output noise variance in the DA converter. $R_{{\zeta }_1},R_{{\zeta }_2},\cdots ,R_{{\zeta }_q}$ are the variance in the observed disturbance.

$L_1$ can be obtained iteratively offline, and the converged steady-state values are applied to Equation (9) for the real-time estimation of $X$. The ${\widehat{\zeta }}_q={\widehat{\tilde{T}}}_d$ estimated by the PMISAKF observer is converted into a voltage for compensating the time-varying disturbance ${\tilde{T}}_d$ by Equation (14) and is provided to the MPC controller.

$$u_{\mathrm{c}}(k)={\displaystyle \frac{1}{K_{\mathrm{I}}{K}_{\mathrm{T}}}}\left[\begin{array}{llll}0\hfill & \cdots \hfill & 0\hfill & 1\hfill \end{array}\right]\widehat{X}(k)$$

(14)

3.2. Design of MPC

MPC mainly consists of four parts: prediction model, objective function, constraint conditions, and optimization algorithm. If $x={\left[{\theta }_{\mathrm{m}},{\omega }_{\mathrm{m}},{\theta }_{\mathrm{L}},{\omega }_{\mathrm{L}}\right]}^{\mathrm{T}}$, Equation (1) can be converted into the following state space equations:

$$\left\{\begin{array}{l}\dot{x}=Ax+Bu+C{T}_d\hfill \\ y=Ex\hfill \end{array}\right.$$

(15)

where,

$$A=\left[\begin{array}{cccc}0& 1& 0& 0\\ {\displaystyle \frac{-K}{N^2{J}_m}}& 0& {\displaystyle \frac{K}{N{J}_m}}& 0\\ 0& 0& 0& 1\\ {\displaystyle \frac{K}{N{J}_L}}& 0& {\displaystyle \frac{-K}{J_L}}& 0\end{array}\right],B=\left[\begin{array}{c}0\\ {\displaystyle \frac{K_{\mathrm{I}}{K}_{\mathrm{T}}}{J_m}}\\ 0\\ 0\end{array}\right],C=\left[\begin{array}{c}0\\ {\displaystyle \frac{1}{J_m}}\\ 0\\ 0\end{array}\right],E={\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]}^{\mathrm{T}}$$

Equation (15) can be rewritten in the discrete-time domain with a zero order hold as:

$$\left\{\begin{array}{l}x\left(k+1\right)=A_{d}x\left(k\right)+B_{d}u\left(k\right)+C_d{T}_d\left(k\right)\hfill \\ y\left(k\right)=E_d\left(k\right)x\left(k\right)\hfill \end{array}\right.$$

(16)

where, $k$ is the sample counter. The discrete process is shown in Equation (20).

$$A_d=e^{A{T}_s},B_d={\displaystyle \underset{0}{\overset{T_s}{\int }}{e}^{A_d\tau }d\tau }·B,C_d={\displaystyle \underset{0}{\overset{T_s}{\int }}{e}^{A_d\tau }d\tau }·C,E_d=E$$

(17)

where, $T_{\mathrm{s}}$ is the discrete time sampling period. Expanding the state vector to $\eta (k)={\left[\begin{array}{lll}x(k)\hfill & u(k-1)\hfill & T_d(k-1)\hfill \end{array}\right]}^T$, then Equation (16) can be rewritten as:

$$\left\{\begin{array}{l}\eta (k+1)={\stackrel{⌢}{A}}_d\eta (k)+{\stackrel{⌢}{B}}_d\Delta u(k)+{\stackrel{⌢}{C}}_d\Delta T_d(k)\hfill \\ y(k)={\stackrel{⌢}{E}}_d\eta (k)\hfill \end{array}\right.$$

(18)

where ${\stackrel{⌢}{A}}_d=\left[\begin{array}{ccc}{A}_d& B_d& C_d\\ 0_{\mathrm{m}\times \mathrm{n}}& I_{\mathrm{m}}& 0\\ 0& 0& 1\end{array}\right],{\stackrel{⌢}{B}}_d={\left[\begin{array}{ccc}{B}_d& I_{\mathrm{m}}& 0\end{array}\right]}^{\mathrm{T}},{\stackrel{⌢}{C}}_d={\left[\begin{array}{lll}{C}_d\hfill & 0\hfill & 1\hfill \end{array}\right]}^{\mathrm{T}},{\stackrel{⌢}{E}}_d=\left[\begin{array}{lll}{E}_d\hfill & 0\hfill & 0\hfill \end{array}\right]$, $m$ is the dimension of the control parameters, and $n$ is the dimension of the state parameters. $\Delta u(k)$ and $\Delta T_d(k)$ are the variation in control output and disturbance from the previous moment to this moment, respectively.

If we set the prediction domain to $N_p$, the control domain to $N_c$, and $N_c<N_p$, then the future output predicted by the system can be deduced from Equation (18) as:

$$Y(k)=\Phi \eta (k)+\Psi \Delta U+\Gamma \Delta \mathrm{T}$$

(19)

where,

$$\begin{array}{l}Y(k)={\left[\begin{array}{cccc}y(k+1)& y(k+2)& \cdot \cdot \cdot & y\left(k+N_p\right)\end{array}\right]}^{\mathrm{T}}\\ \Phi ={\left[\begin{array}{llll}{\stackrel{⌢}{E}}_d{\stackrel{⌢}{A}}_d\hfill & {\stackrel{⌢}{E}}_d{\stackrel{⌢}{A}}_d^2\hfill & \cdots \hfill & {\stackrel{⌢}{E}}_d{\stackrel{⌢}{A}}_d^{N_{\mathrm{p}}}\hfill \end{array}\right]}^{\mathrm{T}}\\ \Delta U={\left[\Delta u(k)\hspace{1em}\hspace{1em}\Delta u(k+1)\hspace{1em}\hspace{1em}\cdots \hspace{1em}\hspace{1em}\Delta u\left(k+N_{\mathrm{c}}\right)\right]}^{\mathrm{T}}\\ \Delta \mathrm{T}={\left[\Delta T_d(k)\hspace{1em}\hspace{1em}\Delta T_d(k+1)\hspace{1em}\hspace{1em}\cdots \hspace{1em}\hspace{1em}\Delta T_d\left(k+N_{\mathrm{c}}\right)\right]}^{\mathrm{T}}\\ \Psi =\left[\begin{array}{ccccc}{\stackrel{⌢}{E}}_d{\stackrel{⌢}{B}}_d& 0& 0& \cdots & 0\\ {\stackrel{⌢}{E}}_d{\stackrel{⌢}{A}}_d{\stackrel{⌢}{B}}_d& {\stackrel{⌢}{E}}_d{\stackrel{⌢}{B}}_d& 0& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ {\stackrel{⌢}{E}}_d{\stackrel{⌢}{A}}_d^{N_c-1}{\stackrel{⌢}{B}}_d& {\stackrel{⌢}{E}}_d{\stackrel{⌢}{A}}_d^{N_c-2}{\stackrel{⌢}{B}}_d& \cdots & \cdots & {\stackrel{⌢}{E}}_d{\stackrel{⌢}{B}}_d\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ {\stackrel{⌢}{E}}_d{\stackrel{⌢}{A}}_d^{N_{\mathrm{p}}-1}{\stackrel{⌢}{B}}_d& {\stackrel{⌢}{E}}_d{\stackrel{⌢}{A}}_d^{N_{\mathrm{p}}-2}{\stackrel{⌢}{B}}_d& {\stackrel{⌢}{E}}_d{\stackrel{⌢}{A}}_d^{N_{\mathrm{p}}-3}{\stackrel{⌢}{B}}_d& \cdots & {\stackrel{⌢}{E}}_d{\stackrel{⌢}{A}}_d^{N_{\mathrm{p}}-N_c-1}{\stackrel{⌢}{B}}_d\end{array}\right]\\ \Gamma =\left[\begin{array}{ccccc}{\stackrel{⌢}{E}}_d{\stackrel{⌢}{C}}_d& 0& 0& \cdots & 0\\ {\stackrel{⌢}{E}}_d{\stackrel{⌢}{A}}_d{\stackrel{⌢}{C}}_d& {\stackrel{⌢}{E}}_d{\stackrel{⌢}{C}}_d& 0& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ {\stackrel{⌢}{E}}_d{\stackrel{⌢}{A}}_d^{N_c-1}{\stackrel{⌢}{C}}_d& {\stackrel{⌢}{E}}_d{\stackrel{⌢}{A}}_d^{N_c-2}{\stackrel{⌢}{C}}_d& \cdots & \cdots & {\stackrel{⌢}{E}}_d{\stackrel{⌢}{C}}_d\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ {\stackrel{⌢}{E}}_d{\stackrel{⌢}{A}}_d^{N_{\mathrm{p}}-1}{\stackrel{⌢}{C}}_d& {\stackrel{⌢}{E}}_d{\stackrel{⌢}{A}}_d^{N_{\mathrm{p}}-2}{\stackrel{⌢}{C}}_d& {\stackrel{⌢}{E}}_d{\stackrel{⌢}{A}}_d^{N_{\mathrm{p}}-3}{\stackrel{⌢}{C}}_d& \cdots & {\stackrel{⌢}{E}}_d{\stackrel{⌢}{A}}_d^{N_{\mathrm{p}}-N_c-1}{\stackrel{⌢}{C}}_d\end{array}\right]\end{array}$$

Considering that the target of the system is to achieve accurate tracking of the velocity command without exceeding the maximum torque of the motor, the velocity-following residuals and control increments of the system are taken as the state quantities of the objective function. The objective function is then obtained as

$$J(k)={\displaystyle \sum _{i=1}^{N_{\mathrm{p}}}{‖Y(k+i|k)-Y_{ref}(k+i|k)‖}_Q^2}+{\displaystyle \sum _{i=0}^{N_{\mathrm{c}}-1}\Vert \Delta U(k+i){\Vert }_R^2}$$

(20)

where $Y_{ref}(k+i|k)$ is the velocity command, $(k+i|k)$ indicates that the system output at $k$ moment is used to predict the system output at $k+i$ moment, where $i=1,2,\cdot \cdot \cdot ,N_p$. $\Delta U(k+i)$ is the increment of the system control at $k+i$ moment, where $i=0,1,2,\cdot \cdot \cdot ,N_c-1$.

The system needs to constrain the motor voltage and its increments for the control input as follows:

$$\left\{\begin{array}{l}{u}_{\min }\le u(k+i)\le u_{\max }\hfill \\ \Delta u_{\min }\le \Delta u(k+i)\le \Delta u_{\max }\hfill \end{array}\right.$$

(21)

where, $u_{\min }$ and $u_{\max }$ are the minimum and maximum values of motor voltage, respectively. $\Delta u_{\min }$ and $\Delta u_{\max }$ are the minimum and maximum values of motor voltage increments, respectively.

The problem of optimal solution of the objective function is to achieve the minimum of Equation (20) in the prediction domain under the condition that Equation (21) is satisfied. Let $F=\Phi \eta (k)+\Gamma \Delta \mathrm{T}-Y_{ref}$, then Equation (20) can be transformed into a standard quadratic programming problem to be solved as

$$\begin{array}{ll}J& ={\left(Y-Y_{ref}\right)}^T{Q}_Q\left(Y-Y_{ref}\right)+\Delta U^T{R}_R\Delta U\\ & ={(F+\Psi \Delta U)}^T{Q}_Q(F+\Psi \Delta U)+\Delta U^T{R}_R\Delta U\\ & =\Delta U^T\left({\Psi }^T{Q}_Q\Psi +R_R\right)\Delta U+2F^T{Q}_Q\Psi \Delta U+F^T{Q}_{Q}F\\ & =\Delta U^{T}H\Delta U+G\Delta U+P\end{array}$$

(22)

where, $H={\Psi }^{\mathrm{T}}{Q}_Q\Psi +R_R,G=2F^{\mathrm{T}}{Q}_Q\Psi ,P=F^{\mathrm{T}}{Q}_{Q}F,Q_Q=Q\cdot I_{N_{\mathrm{p}}},R_R=R\cdot I_{N_c}$. In the case where $H$ is strictly positive definite, the objective function can be expressed as a quadratic curve of the independent variable $\Delta U$, whose minimum value can be solved by Equation (23), and the final result is shown in Equation (24).

$${\displaystyle \frac{\partial J(\eta (k),\Delta U(k))}{\partial \Delta U(k)}}=H\Delta U+G=0$$

(23)

$$\Delta U=-{\displaystyle \frac{1}{H}}G$$

(24)

The first term of the control input increment $\Delta u(k)=\Delta U(1)$ is taken out to obtain the control input of the system at the current moment as:

$$u(k)=u(k-1)+\Delta u(k)$$

(25)

4. Experimental Results

In order to test the effectiveness of the proposed algorithm, an experimental platform is constructed with an EMA as the control object, and a series of experiments are carried out.

4.1. Experimental Setup

The experimental device has three components: a typical EMA, a load simulation device, and a dSPACE, as shown in Figure 4. The EMA mainly consists of two PMSMs (China, PALKI, model: SPALY80), two drivers (Israel, Elmo, model: P/N: SOL-WHI 20/100PYE), two LSPRs (China, SKGR, model: FABR060-25-S2-P1), and an absolute encoder (China, Yingdong, model: CAPRO-B112050). The load simulation device consists of a permanent magnet synchronous motor (China, OTOI, model: ASM80B1007-30M), an RV reducer (China, Zhenkang, model: ZKRV-20E-161-B), and a torque sensor (China, Daysensor, model: DYN-200). A dSPACE system (Germany dSPACE, DS-1104) is utilized to decode the feedback signals and generate real-time codes for such a system. The sampling rate of the dspace system is 1000 Hz.

4.2. Control Parameters

According to the experimental device parameters, the values of the system state space model can be calculated as:

$$A=\left[\begin{array}{cccc}0& 1& 0& 0\\ -1.6\times {10}^5& 0& 3.4\times {10}^7& 0\\ 0& 0& 0& 1\\ 193.93& 0& -4\times {10}^4& 0\end{array}\right],B=\left[\begin{array}{c}0\\ 3.48\times {10}^3\\ 0\\ 0\end{array}\right],C=\left[\begin{array}{c}0\\ 8.77\times {10}^3\\ 0\\ 0\end{array}\right],E={\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]}^{\mathrm{T}}$$

Considering that the control cycle of the servo system is set to T_s = 1 ms, the state-space model values in the discrete time domain with a zero-order hold are as follows:

$$A_d=\left[\begin{array}{cccc}0.91& 9.7\times {10}^{-4}& 16.72& 5.6\times {10}^{-3}\\ -159.4& 0.92& 3.3\times {10}^4& 16.72\\ 9.5\times {10}^{-5}& 3.2\times {10}^{-8}& 0.98& 9.9\times {10}^{-4}\\ 0.18& 9.5\times {10}^{-5}& -38.64& 0.98\end{array}\right],B_d=\left[\begin{array}{c}1.7\times {10}^{-3}\\ 3.38\\ 2.8\times {10}^{-8}\\ 1.1\times {10}^{-4}\end{array}\right],C_d=\left[\begin{array}{c}4.3\times {10}^{-3}\\ 8.53\\ 7.0\times {10}^{-8}\\ 2.8\times {10}^{-4}\end{array}\right],E_{\mathrm{d}}={\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]}^{\mathrm{T}}$$

Weighing the computing ability of the computer and the performance of the controller, we set $N_{\mathrm{p}}=10,N_{\mathrm{c}}=2$. Then, the specific values of ${\stackrel{⌢}{A}}_d$,${\stackrel{⌢}{B}}_d$,${\stackrel{⌢}{C}}_d$,${\stackrel{⌢}{E}}_d$, $\Phi$,$\Psi$ and $\Gamma$ of MPC can be calculated.

The control objective of MPC is to achieve fast and accurate responses to speed commands and efficient suppression of unknown disturbances. After extensive simulation verification and actual testing, it was found that the weights of the state variables and control variables are as follows: $Q=1,R=0.01$. On the basis of the above values, the matrix of the standard quadratic programming problem is calculated as:

$$H=\left[\begin{array}{cc}0.1095& 0.0077\\ 0.0077& 0.1063\end{array}\right]$$

We can find that the matrix $H$ is strictly positive definite, so the solution of the objective function is reasonably calculated by Equations (23) and (24).

Next, we examine the parameter values of the PMISAKF observer. When $q>3$, the computation of the PMISAKF observer increases sharply; thus, we let $q=3$. Based on the parameters of the dual-inertia state space model of the system, $\overline{\mathit{A}}$,$\overline{\mathit{B}}$,$\overline{\mathit{E}}$, and the discretized ${\overline{\mathit{A}}}_d$,${\overline{\mathit{B}}}_{\mathit{d}}$,${\overline{\mathit{E}}}_{\mathit{d}}$ for the PMISAKF observer can be calculated.

Thus, the variance in the control output signal and the velocity the encoder are:

$$\left\{\begin{array}{l}{R}_{\tilde{u}}={\displaystyle \frac{\delta u}{12}}=2.54\times {10}^{-5}{\mathrm{V}}^2,\\ R_{{\tilde{\omega }}_L}={\displaystyle \frac{\delta {\theta }_L}{12T_s}}=9.98\times {10}^{-4}{(\mathrm{rad}·{\mathrm{s}}^{-1})}^2\end{array}\right.$$

Disturbance variances determine the pole locations of the PMISAKF observer and are directly related to the convergence velocity and the observation noise of the estimated disturbance. Generally, high values for the disturbance variance result in faster estimates but worse noise. The following values of $R_{{\zeta }_3},R_{{\zeta }_2},R_{{\zeta }_3}$ are proven helpful for a better response and tracking performance in the actual system.

$$R_{{\zeta }_3}=1\times {10}^{-5}{\mathrm{N}}^2{\mathrm{V}}^2,R_{{\zeta }_2}=R_{{\zeta }_3}\times {10}^3,R_{{\zeta }_1}=R_{{\zeta }_2}\times {10}^3$$

Finally, the optimal gains of the PMISAKF observer are computed offline as follows:

$$L_1={\left[\begin{array}{ccccccc}0.22& 20.7& 9\times {10}^{-4}& 0.11& 322.4& 21.29& 0.69\end{array}\right]}^{\mathrm{T}}$$

4.3. Analysis of Experimental Results

A series of verification experiments are carried out on the experimental device shown in Figure 4 to evaluate the effectiveness of the control algorithm proposed in this paper. The testing project includes a square tracking experiment, sine tracking experiment, and frequency domain response testing under no-load conditions. The disturbance torque simulation device is used to simulate the base coupling torque and temporary impact torque, and control performance testing is carried out under disturbance conditions. The compared methods include Method 1(PI), Method 2(PI+PMISAKF, proposed in the Ref. [20]), and the proposed compound control algorithm (MPC+PMISAKF). The gains of PI can be obtained through pole assignment of the closed-loop system matrix; this process has been detailed in the Ref. [20].

4.3.1. Verify the Effectiveness of the Disturbance Observer

To verify the effectiveness of the proposed PMISAKF observer based on the dual-inertia model, the disturbance observation accuracy of different observers is compared. The project consists of two categories: one is to input a sine signal speed command to observe the friction torque and the other is to apply base coupling disturbance torque to the model when the speed command is zero to observe the base coupling torque. Three types of disturbance observers have been designed: DOB, a PMISAKF observer based on the single-inertia model proposed in Ref. [20], and a PMISAKF observer based on the dual-inertia model proposed in this paper.

We can operate the system in a velocity closed-loop state and input a sinusoidal signal with an amplitude of 6.28 °/s and a frequency of 1 Hz as the command. Additionally, we can collect input and output shaft torque information through motor current sensors and torque sensors on the RV reducer output shaft and then subtract the input shaft torque value and the ratio of the output shaft torque to the transmission ratio as the actual friction torque value. We can then compare the observation effects of different observers on nonlinear friction torque. The results are shown in Figure 5. The disturbance observation error represents the difference between the actual disturbance and the observation results. It can be found that the peak value has been significantly reduced using the proposed method. This indicates that this method can better characterize the zero crossing characteristics of friction disturbances.

Similarly, we can operate the system in a velocity closed-loop state, set the speed command to zero and add the carrier disturbance at the first second using the load simulation device. W e can then compare the observation effects of different observers, as shown in Figure 6. It could also be found that the disturbance observation error in the proposed method is minimal. The reason is that the multiple integration link in the PMISAKF can accumulate observation errors multiple times, improving the observation accuracy of the PMISAKF. The nominal model for the PMISAKF proposed in this paper is a dual-inertia model, which is closer to the actual system compared to the single-inertia PMISAKF designed based on the first order link and can further improve observation accuracy. Accurate estimation of disturbances can reduce the number of iterations of MPC controllers, which provides convenience for the use of MPC in high dynamic applications.

4.3.2. Velocity Closed-Loop Time Response

A square signal with an amplitude of 6.28°/s and a frequency of 0.5 Hz is applied to the velocity command and the tracking residuals of the different control methods are compared. The experimental results show that the proposed method significantly improves the rapidity of the response while reducing the overshoot, as shown in Figure 7. The velocity tracking residual represents the difference between velocity command and response.

The experiment results are summarized in

Table 1. Comparison of the square wave signal tracking experimental results.

Indicators	Method 1	Method 2	Improvement (%)	Proposed Method	Improvement (%)
Overshoot (%)	49.63	107.34	−116.28	35.46	28.55
Adjustment time (s)	0.123	0.102	17.07	0.061	50.41

. Compared to Method 1, Method 2 reduces the adjustment time by 17.07%, but increases the overshoot by 116.28%. If the load is increased, the overshoot may further increase or even become unstable, which poses a safety hazard. When using the proposed method, not only can the adjustment time be reduced by 50.41% but the overshoot can be reduced by 17%. This indicates that the proposed method can significantly improve the dynamic response performance of the system.

A Sinusoidal signal with an amplitude of 6.28°/s and a frequency of 1 Hz is applied to the velocity command, and the tracking residuals of the different methods are compared. The experimental results are shown in Figure 8, in which the red solid line is Method 1, the blue solid line is Method 2, and the green solid line is proposed method. The equivalent stabilization accuracy is the integration of the velocity tracking residual. We can find that the tracking residual of the proposed method is significantly reduced.

The experiment results are summarized in

Table 2. Comparison of the Sinusoidal wave signal tracking experimental results.

Indicators	Method 1	Method 2	Improvement (%)	Proposed Method	Improvement (%)
Velocity tracking residuals (°/s)	0.56	0.38	32.14	0.19	66.07
Equivalent stabilization accuracy(mrad)	0.83	0.53	36.14	0.24	71.08

. Compared to Method 1, Method 2 reduces the velocity-tracking residuals by 32.14% and the equivalent stabilization accuracy by 36.14%. The proposed method reduces the velocity-tracking residuals by 66.07% and the equivalent stabilization accuracy by 71.08%. The experimental results indicate that the proposed method can further improve the velocity-tracking performance.

4.3.3. Velocity Closed-Loop Frequency Response

In the frequency domain, the velocity closed-loop frequency responses of the system at different control methods are compared, and the results are shown in Figure 9. The system operates in a velocity closed-loop state with no external load applied. Using sine signals of different frequencies with an amplitude of 6.28°/s as velocity commands, the response at different frequencies is obtained through frequency sweeps and plotted into Bode plots. The comparison results show that the Method 2 has a higher bandwidth than Method 1 but also has a larger overshoot. The proposed method improves the closed-loop bandwidth while reducing overshoot. The experiment results are summarized in

Table 3. Comparison of velocity closed-loop frequency response experimental results.

Indicators	Method 1	Method 2	Improvement (%)	Proposed Method	Improvement (%)
Bandwidth (Hz)	17	22	29.41	24	41.18

. Compared to Method 1, Method 2 improves the response bandwidth by 29.41% from 17 Hz to 22 Hz. The proposed method improves the response bandwidth by 41.18% to 24 Hz. This indicates that the system’s ability to resist external disturbances has increased.

4.3.4. Carrier Disturbance Test

Using the load simulation device to simulate the carrier disturbance, we can test the control performance of the EMA under the external disturbance. According to the literature, the carrier disturbance can be approximated by the superposition of multiple sinusoidal signals. We represent it using three principal components, namely 1° and 1 Hz, 2° and 0.5 Hz, and 0.5° and 2 Hz. During the testing process, we set the speed command to zero, apply the carrier disturbance at the first second moment, and observe the speed fluctuations of the system under different control methods. The results are shown in Figure 10. We take the peak-to-peak value of velocity fluctuations as statistical values and record them in

Table 4. Comparison results of carrier disturbance suppression for different controllers.

Indicators	Method 1	Method 2	Improvement (%)	Proposed Method	Improvement (%)
Peak-to-peak value of velocity fluctuation (°/s)	0.48	0.36	25	0.19	60.42

. The experimental results show that, in Method 2, the peak-to-peak value is reduced by 25% and, in the proposed method, this indicator is reduced by 60.42%. These experimental data further verify that the proposed method can effectively improve the system’s anti-interference ability.

5. Conclusions

In this paper, an MPC-PMISAKF is proposed to improve the disturbance rejection performance. Specifically, by leveraging a PMISAKF to estimate the system’s state and external disturbances in real-time, we can incorporate these observed disturbances into the predictive model of MPC as known external inputs at both current and future time points. This incorporation enables the optimization process of MPC to account for these external factors, thereby enabling the computation of an optimal control strategy that achieves more precise control. Experimental results confirmed the effectiveness of the proposed control algorithms. The equivalent stability accuracy has seen an improvement, from 0.83 mrad (with the PI Controller) and 0.53 mrad (with the PI+PMISAKF control algorithm) to 0.24 mrad when employing the composite control algorithm proposed in this paper. Similarly, the peak-to-peak value of velocity fluctuations under carrier disturbance has decreased, from 0.48°/s (with the PI Controller) and 0.36°/s (with the PI+PMISAKF control algorithm) to 0.19°/s when using the composite control algorithm. Additionally, the experimental results of square wave signal tracking and velocity closed-loop frequency response further confirm the effectiveness of the proposed method. These findings solidly verify that the proposed method can significantly enhance the system’s anti-interference capability and tracking accuracy.