Stimuli-Induced Equilibrium Point-Based Algorithm for Motion Planning of a Heavy-Load Servo System

Abstract

To tackle the problems of power saturation and high energy consumption of the heavy-load servo system in a servo process, we propose a motion planning algorithm based on the stimuli-induced equilibrium point (SIEP), named the SIEP-MP algorithm. First, we explore the correlation between various modes of the bionic eye system and the heavy-load servo system through head-eye motion control theory and derive the core formula of the SIEP-MP algorithm from psychological field theory. Then, we design a speed loop of the heavy-load servo system by combining a speed controller and a disturbance observer. Furthermore, we create a position loop of the heavy-load servo system by combining a position controller and a feed-forward controller. We verify the low-pass filtering and range-limiting functions of the SIEP-MP algorithm by building the experimental platform, designing the target trajectory, and setting the control parameters. Experimental results demonstrate similar command filtering, elimination of power saturation, and energy-saving functions compared to low-pass filters, and the algorithm has a better mode-switching performance. The proposed SIEP-MP algorithm can ensure the optimal tracking performance of the heavy-load servo system in different modes through mode switching.

1. Introduction

With the application of small cannons or railguns, weapon stations [1] are gradually evolving into heavy-load weapon stations. Due to the similarity in structural forms and control methods between heavy-load weapon stations and heavy-load robotic arms or large-inertia stabilization platforms, this paper defines them as heavy-load servo systems. Weapon stations are typically designed as coarse-precision systems to achieve high precision and to reject external disturbances [1]. The operational sequence is as follows: Initially, the first-level system leverages its high servo accuracy to search, lock onto, and track the target. Subsequently, the first-level system transmits its position as an instruction to the second-level system. Finally, the second-level system precisely tracks the target based on the instructions from the first-level system. The rationale behind designing heavy-load servo systems as two-level systems is delineated as follows:

1. The first-level system directly propels lightweight loads through a motor, resulting in high mechanical accuracy. Furthermore, this system can isolate the working torque of the second-level system through an independent stable platform. To enhance the control accuracy of the first-level system, ref. [2] implemented an adaptive sliding mode control algorithm based on a neural network to compensate for friction during the motion of the optoelectronic systems. In [3], Chen et. al. employed a time delay and PID control algorithm to achieve high-precision control of the pan-tilt, ensuring precise guidance of the second-level system by the first-level system. Consequently, the first-level system exhibits high control accuracy.

2. The second-level system is propelled by an electric motor and a reducer, introducing more nonlinear elements [4], such as friction and resonance, in the mechanism. To mitigate friction and gaps in the motion of the second-level system, ref. [5] applied a composite control algorithm encompassing a controller, observer, and notch filter for high-precision control of the weapon station. Additionally, to eliminate gaps in the motion process, ref. [6] adopted a synchronous driving method with dual motors for the pitch motion of the telescope, which indicates that the control method of the second-level system is considerably more intricate than that of the first-level system, and enhancing its control accuracy is more challenging.

3. The load inertia of the second-level system is significantly greater than that of the first-level system, leading to more interference torques affecting control accuracy during movement [7]. Power saturation in the second-level system also impacts control accuracy. In [8], Shi employed a resource allocation method to meet the specified tracking accuracy requirement with minimum long-term and short-term power consumption. Consequently, when the secondary system follows the target independently, the photoelectric system fixed on it may lose the target due to these challenges.

The servo system’s optimal performance necessitates the utilization of both control algorithms and motion planning algorithms. An input-shaping technique is employed to eliminate residual vibrations in flexible systems after motion cessation. The main principle involves designing a step instruction as a multi-pulse instruction [9,10] with specified notch points [11]. Various methods use finite impulse response (FIR) filters to plan instructions and filter out noise, equating multi-segment trajectory algorithms with FIR filters in the time domain [12,13] or designing convolution calculations for instruction signals and FIR filters in the frequency domain [14]. An acceleration and cancellation algorithm is employed to mitigate power saturation during system motion, with the core principle being the optimization of the motion profile of instructions (including speed [3,15], acceleration, or jerk [16]). This optimization aims to achieve smooth instruction execution and prevent power saturation. In the context of conserving energy in heavy-load servo systems, numerous studies have focused on optimizing the energy consumption model of instructions by defining objective functions and function variables, such as pseudo spectrum [17], spline interpolation functions [18], and inertia ellipsoid [19]. These algorithms are typically implemented in the instruction planning of single-level systems, ensuring that the single-level system attains specific optimization goals while maintaining tracking accuracy. In contrast, two-level systems can rely on primary systems for target tracking, allowing secondary systems to more effectively achieve optimization goals without the necessity of ensuring tracking accuracy.

The two-tier heavy-load servo system bears resemblance to the human head-eye system, akin to a bionic eye system, where the first-level system corresponds to the eye, and the second-level system aligns with the head. The bionic eye system exhibits scanning mode (saccade), smooth tracking mode (smooth pursuit), and gaze mode (vestibulo-ocular reflex) [20], corresponding to the search mode, following mode, and precision aiming mode of the two-tier system, respectively. The following mode lacks the energy-saving feature present in the smooth tracking mode [21]. Introducing this mode to the heavy-load servo system can enhance its energy-saving capability. Initially, researchers focused on basic investigations of the smooth tracking mode. However, as research progressed, this mode gradually found application in bionic eye systems [22,23,24]. The evolved smooth tracking mode now encompasses frequency and time domain modes. Researchers have employed the variable gain transfer function to implement the frequency-domain mode [25,26,27]. While variable gain transfer functions can emulate the smooth tracking mode, they fall short of achieving the range-limiting function outlined in head-eye motion control theory, which defines the motion range of the eyeball in orbit. In contrast, researchers use piecewise functions to implement the time-domain mode [28,29,30]. Piecewise functions can compensate for the absence of a range-limiting function in frequency domain modes but introduce discontinuity.

To solve the above problems, we propose a motion planning algorithm (SIEP-MP algorithm) based on the stimulus-induced equilibrium point (SIEP) [31,32,33], which can imitate the smooth tracking mode so that the heavy-load servo system has the energy-saving function and has the range-limiting function. The main contributions are listed as follows.

1. The SIEP-MP algorithm can effectively switch the search mode, following mode, and precision aiming mode of the servo system by adjusting parameters.

2. In addition to the filtering function, reducing overshoot and residual vibration of the conventional motion planning algorithm, the SIEP-MP algorithm also has a range-limiting function.

3. The SIEP-MP algorithm achieves better energy savings in the following modes and effectively prevents power saturation during the mode-switching process through the range-limiting function.

4. The SIEP-MP algorithm can effectively solve the problem that the variable gain transfer function method [25] cannot limit the range, as well as the discontinuity problem of the piecewise function method [28].

The organization of this study is as follows: Section 2 introduces the basic concepts and design methods of the SIEP-MP algorithm. Section 3 descibes the construction of a control block diagram for a heavy-load servo system, including the SIEP-MP algorithm. Section 4 verifies the low-pass filtering function, range-limiting function, fast mode-switching function, residual vibration reduction function, power saturation elimination function, and energy-saving function of the SIEP-MP algorithm. Section 5 summarizes the work and discusses its advantages and innovative contribution.

2. Motion Planning Algorithm Design

The SIEP-MP algorithm is a bionic algorithm based on psychological field theory and head-eye motion control theory. We describe the two theories separately.

2.1. Head-Eye Motion Control Theory Application in a Two-Level Heavy-Load Servo System

The head-eye motion control configuration based on monocular vision is shown in Figure 1.

In Figure 1, $\left(p_1,p_2\right)$ represents the coordinates of the intersection point between the boundaries $x=p_1$ and $y=p_2$ of the visual field. $\left(p_1+l_x,p_2+l_y\right)$ denotes the coordinates of the intersection point between the boundaries $x=p_1+l_x$ and $y=p_2+l_y$ of the visual field. $\left(p_3,p_4\right)$ corresponds to the coordinates of the intersection point between the boundaries $x=p_3$ and $y=p_4$ of the eyepit. $\left(p_1+L_x,p_2+L_y\right)$ indicates the coordinates of the intersection point between the boundaries $x=p_3+L_x$ and $y=p_4+L_y$ of the eyepit. These coordinates are defined in the absolute coordinate system. $l_x$ represents the length of the left and right boundaries of the visual field. $l_y$ is the length of the lower and upper boundaries of the visual field. $L_x$ is the length of the left and right boundaries of the eyepit. $L_y$ denotes the length of the lower and upper boundaries of the eyepit. $v_1$ represents the velocity in the x-direction of the visual field, while $v_2$ is the velocity in the y-direction of the visual field. $v_3$ denotes the velocity in the x-direction of the eyepit, and $v_4$ is the velocity in the y-direction of the eyepit.

The human head and eyes are driven by four and six muscles, respectively, which allows for highly flexible movements. However, the coordination of multiple muscles for driving these movements is quite complex. Given the perpendicular orientation of the pointing system in the x- and y-directions, a comprehensive study of the algorithm in the x-direction is essential. The head-eye motion control configuration, based on monocular vision, in the x-direction is depicted in Figure 2.

In Figure 2, $p_0$ is the coordinate of the center point of the visual field in the x-direction. $p_0^{*}$ is the coordinate of the center point of the eye pit in the x-direction. The other coordinates are the same as in Figure 1.

The parameters in Figure 1 and Figure 2 need to satisfy the following conditions:

$$\left\{\begin{array}{c}\begin{array}{c}{l}_x=2D_{x}tan\left({\displaystyle \frac{{\theta }_x}{2}}\right),p_0=p_1+{\displaystyle \frac{l_x}{2}}\hfill \\ p_3\le p_1\le p_1+l_x\le p_3+L_x\hfill \end{array}\hfill \\ L_x=2D_{x}tan\left({\displaystyle \frac{{\Theta }_x}{2}}\right),p_0^{*}=p_3+{\displaystyle \frac{L_x}{2}}\hfill \end{array}\right.$$

(1)

The head-eye motion control theory [34] aims to reduce power output and energy consumption while observing moving targets. If this theory is applied to the heavy-load servo system shown in Figure 3, it can also provide the same advantages.

The head-eye motion control configuration in the x-direction of the heavy-load servo system is shown in Figure 4.

In Figure 4, the heavy-load servo system has actual boundaries on the x-axis. When the first-level system rotates 180° clockwise around the x-axis, the second-level system must follow. Otherwise, the system cannot achieve a full 360° exploration. This lagging following mode can help reduce energy consumption.

The head-eye motion control configuration in the y-direction of the heavy-load servo system is shown in Figure 5.

In Figure 5, the heavy-load servo system lacks physical boundaries along the y-axis. Instead, we establish a virtual boundary to constrain the lag angle between the second-level and first-level systems within a specified range. This feature effectively prevents power saturation during the transition from the following mode to the precision aiming mode.

Within the bionic eye system, during the scanning mode, only the eyes are involved in searching for the target. In the smooth tracking mode, the head lags behind the eyes while tracking the target. In the gaze mode, both the head and eyes track the target without any lag. These three modes can be associated with the search mode, following mode, and precision aiming mode of the heavy-load servo system. However, due to the absence of a clear distinction between the following mode and precision aiming mode, the heavy-load servo system is unable to fully leverage the energy-saving capabilities. To address this limitation, we integrate the smooth tracking mode from the bionic eye system into the heavy-load servo system, utilizing psychological field theory as the foundational framework. The operational modes of both the bionic eye system and the heavy-load servo system are delineated in Figure 6. Unmanned systems, such as unmanned weapon stations, can dynamically switch between modes based on situational awareness. For instance, when an unmanned weapon station fails to detect a target, it operates in search mode. Upon target detection outside the range, the system transitions to the following mode for energy-efficient tracking and readies for a swift switch to the precise aiming mode. When the target enters the effective range, the system engages the precise aiming mode, facilitating timely targeting and engagement.

2.2. SIEP-MP Algorithm Design Based on Psychological Field Theory

The head-eye motion control theory, grounded in psychological field theory, is manifested through the SIEP-MP algorithm. In the x-direction, its workflow unfolds as follows: Upon locking onto the target by the optoelectronic systems, a Mean Target Point (MTP), denoted as $p_{MTP}$, is established. The first-level system initiates tracking of $p_{MTP}$ and establishes the center point of the visual field, $p_0$, through a position encoder. Subsequently, $p_0$ is planned using the SIEP-MP algorithm to formulate the Stimulus-Induced Equilibrium Point (SIEP) denoted as $p_{SIEP}$. Simultaneously, the second-level system commences tracking of $p_{SIEP}$ and establishes the center point of the eyepit, $p_0^{*}$, through the position encoder. Employing a high-precision control strategy for both the first-level and second-level systems ensures accurate tracking of $p_{MTP}$ and $p_{SIEP}$, respectively. Therefore, the above points need to satisfy Equation (2).

$$\left\{\begin{array}{c}{p}_0=p_{MTP}\hfill \\ p_0^{*}=p_{SIEP}\hfill \end{array}\right.$$

(2)

The SIEP-MP algorithm design process, based on the above configuration, is delineated as follows:

1. We establish a stimulation field in the eyepit of the head (second-level system), where the eyepit range is the definition domain of the algorithm. The algorithm has no solution when the $\left(p_{MTP}-p_{SIEP}\right)$ exceeds the eyepit range.

2. When the eye (first-level system) locks the target, the target appears in the visual field, and the visual field boundary stimulates the corresponding eyepit boundary in the x- and y-directions, as shown in Figure 7.

3. We can interpret the stimuli in Figure 7 as “psychological stimuli”. According to psychological field theory [31,35], x negative and x positive stimuli can be written as

$$\left\{\begin{array}{c}{I}_1={\overline{I}}_1+{\displaystyle \frac{{\alpha }_1{l}_x+{\beta }_1{v}_1{t}_s}{1+exp\left(\frac{v_1-v_3}{{\gamma }_1}\right)}}\hfill \\ I_2={\overline{I}}_2+{\displaystyle \frac{{\alpha }_2{l}_x+{\beta }_2{v}_1{t}_s}{1+exp\left(\frac{v_3-v_1}{{\gamma }_2}\right)}}\hfill \end{array}\right.$$

(3)

In Equation (3), ${\overline{I}}_1$ and ${\overline{I}}_2$ are the basal stimuli generated by the visual field boundary $x=p_1$ and the visual field boundary $x=p_1+l_x$, respectively, which are constants and often defined as zero. $t_s$ is the sampling period of the heavy-load servo system. ${\alpha }_1$ and ${\alpha }_2$ are the sensitivity of the eyepit boundary $x=p_3$ and $x=p_3+l_x$ to the visual field size $l_x$, respectively. ${\beta }_1$ is the sensitivity of the eyepit boundary $x=p_3$ to the velocity $v_1$ of the visual field boundary $x=p_1$. ${\beta }_2$ is the sensitivity of the eyepit boundary $x=p_3+L_x$ to the velocity $v_2$ of the visual field boundary $x=p_1+l_x$. ${\gamma }_1$ is the sensitivity of the eyepit boundary $x=p_3$ to the boundary velocity difference $\left(v_1-v_3\right)$. ${\gamma }_2$ is the sensitivity of the eyepit boundary $x=p_3+l_x$ to the boundary velocity difference $\left(v_3-v_1\right)$.

4. To quantify the stimulus force generated by stimuli, we express the stimulus forces $F_1$ caused by stimuli $I_1$ of $\left(p_1-p_3\right)$ and $F_2$ generated by $I_2$ of $\left(p_1+l_x-p_3-L_x\right)$ as

$$\left\{\begin{array}{c}{F}_1={\displaystyle \frac{I_1}{p_1-p_3}}\hfill \\ F_2={\displaystyle \frac{I_2}{p_1+l_x-p_3-L_x}}\hfill \end{array}\right.$$

(4)

5. We consider the eyepit as a whole, and the stimulation forces $F_1$ and $F_2$ on the eyepit boundary form a combined force. The combined force is

$$\sum _{i=1}^2{F}_i={\displaystyle \frac{I_1}{p_1-p_3}}+{\displaystyle \frac{I_2}{p_1+l_x-p_3-L_x}}$$

(5)

6. According to psychological field theory, when the combined force of stimuli is equal to zero, Equation (5) is in equilibrium. Through Equations (1) and (5) and making ${\displaystyle \sum _{i=1}^2}{F}_i=0$, the eyepit center point $p_0$ can be expressed as

$$p_0={\displaystyle \frac{I_1\left(p_1+l_x-\frac{L_x}{2}\right)+I_2\left(p_1+\frac{L_x}{2}\right)}{I_1+I_2}}$$

(6)

Equations (3)–(6) contain the design variables and provide for implementing the process of the SIEP-MP algorithm. From these equations, we know that the stimulus is proportional to the visual field velocity, which can enhance the algorithm’s robustness and prevent the loss of large-sized and high-speed targets in the eyepit. We can also discern that the stimulus force is inversely proportional to the corresponding boundary distance, which can determine the magnitude and direction of the combined force.

7. In psychological field theory [31], the $p_{MTP}$, $p_0$, and $p_0^{*}$ are coincident when the target is stationary. Therefore, we set the boundary sensitivity $\alpha$, boundary velocity sensitivity $\beta$, and boundary velocity difference sensitivity $\gamma$ in Equation (3) as

$$\left[\alpha ,\beta ,\gamma \right]=\left[{\alpha }_1,{\beta }_1,{\gamma }_1\right]=\left[{\alpha }_2,{\beta }_2,{\gamma }_2\right]$$

(7)

8. According to Equation (1), the velocity at the field boundary is equal to the velocity at the center point of the field, and the velocity at the eyepit boundary is equal to the velocity at the center point of the eye. Through Equation (2), we express the boundary velocity difference as

$$\Delta v=v_1-v_3=-\left(v_3-v_1\right)=-\left(v_{SIEP}-v_{MTP}\right)$$

(8)

In Equation (8), $v_{MTP}$ is the velocity of $p_{MTP}$, and $v_{SIEP}$ is the velocity of $p_{SIEP}$.

9. Building upon Equations (7) and (8), and setting ${\overline{I}}_1={\overline{I}}_2=0$, Equation (3) can be expressed as

$$\left\{\begin{array}{c}{I}_1^{*}={\displaystyle \frac{\alpha l_x+\beta v_{MTP}{t}_s}{1+exp\left(\frac{\Delta v}{\gamma }\right)}}\hfill \\ I_2^{*}={\displaystyle \frac{\left(\alpha l_x+\beta v_{MTP}{t}_s\right)exp\left(\frac{\Delta v}{\gamma }\right)}{1+exp\left(\frac{\Delta v}{\gamma }\right)}}\hfill \end{array}\right.$$

(9)

10. Referring to Equations (2) and (4), these can be rewritten as

$$\left\{\begin{array}{c}{F}_1^{*}={\displaystyle \frac{I_1^{*}}{p_{MTP}-\frac{l_x}{2}-p_{SIEP}+\frac{L_x}{2}}}\hfill \\ F_2^{*}={\displaystyle \frac{I_2^{*}}{p_{MTP}+\frac{l_x}{2}-p_{SIEP}-\frac{L_x}{2}}}\hfill \end{array}\right.$$

(10)

11. Utilizing Equation (5) and ${\displaystyle \sum _{i=1}^2}{F}_i^{*}=0$, the quantity $p_{SIEP}$ can be obtained as

$$p_{SIEP}={\displaystyle \frac{\left(l_x-L_x\right)\left[1-exp\left(\frac{\Delta v}{\gamma }\right)\right]}{2\left[1+exp\left(\frac{\Delta v}{\gamma }\right)\right]}}+p_{MTP}$$

(11)

Through the aforementioned steps, the design of the proposed motion planning algorithm can be accomplished. Equation (11) is the core formula of the SIEP-MP algorithm.

3. Servo Control Algorithm Design

The control block diagram of the heavy-load servo system with the SIEP-MP algorithm is shown in Figure 8.

The eliminating power saturation function, low-pass filtering function, and energy-saving function of the SIEP-MP algorithm are applied to the second-level system, so we only introduce the high-precision cascade control algorithm of the second-level system.

The second-level control system of the heavy-load servo system is generally the cascade control system, including the current, speed, and position loop. Considering that the bandwidth of the current loop is approximately ${10}^2$ times wider than that of the speed loop, we can designate it as the electric torque coefficient (ETC) $K_T$ from the motor driver. Next, we will design speed and position loops for the second-level system.

3.1. Speed Loop Design for the Second-Level System of the Heavy-Load Servo System

The speed loop control strategy [5] consists of a proportional-integral (PI) controller and state-augmented Kalman filter (SAKF) observer, where the PI controller is used to ensure the stability of the speed loop, and the SAKF observer is used to estimate and compensate for the disturbance torque. The speed loop of the second-level system is depicted in Figure 9.

In Figure 9, J is the moment of inertia of the second-level system, which can be obtained through 3D (three-dimensional) modeling and experimental testing. N is the transmission ratio from the second-level system’s motor angle to the load angle. $v_0^{*}$ is the speed of the second-level system, $T_d$ is the disturbance torque, and $U_a$ is the control voltage.

Firstly, we design the PI controller.

The output signal of the PI controller is

$$\left\{\begin{array}{c}{U}_{PI}=K_P\left(v_{SIEP}-v_0^{*}\right)+K_I{v}_s\hfill \\ v_s=\underset{0}{\overset{t}{\int }}\left(v_{SIEP}-v_0^{*}\right)d\tau \hfill \end{array}\right.$$

(12)

In Equation (12), $K_P$ and $K_I$ are the proportional control parameter and the integral control parameter of the proportional-integral controller. $\tau$ is the differential element.

According to the performance-constrained proportional integral controller design method [5], we can adjust the servo performance of the speed loop by changing the rise time $t_r$ and damping coefficient ${\xi }_v$ of the closed-loop system transfer function. We can set the parameters of the controller as

$$\left[K_P,K_I\right]=\left[{\displaystyle \frac{6J}{t_r{K}_{T}N}},{\displaystyle \frac{9J}{{\xi }_v^2{t}_r^2{K}_{T}N}}\right]$$

(13)

Next, we design the SAKF observer.

The state space equation of the second-level system is represented as

$$\left\{\begin{array}{c}\dot{x}=Ax+B{U}_a+B_d{T}_d\hfill \\ y=Cx=C{v}_0^{*}\hfill \end{array}\right.$$

(14)

In Equation (14), both $\dot{x}$ and $v_0^{*}$ represent the velocity state variable of the second-level system. $\dot{x}$ is the position state variable of the second-level system. y denotes the output, which can be either the second-level system’s velocity or position. The other parameters in Equation (14) are

$$\left[A,B,B_d,C\right]=\left[0,{\displaystyle \frac{K_{T}N}{J}},{\displaystyle \frac{1}{J}},1\right]$$

(15)

Based on Equation (14), the state expansion observer is

$$\left\{\begin{array}{c}\dot{\widehat{x}}=A\widehat{x}+B{U}_a+K_{\omega }\left(y-C\widehat{x}\right)+B_d{\widehat{T}}_d\hfill \\ {\dot{\widehat{T}}}_d=K_I\left(y-C\widehat{x}\right)+{\widehat{T}}_d\hfill \end{array}\right.$$

(16)

In Equation (16), $K_{\omega }$ represents the speed state variable observer gain and $K_I$ is the disturbance observer gain. Consequently, we construct a new state variable as

$$\tilde{\mathbf{x}}={\left[x{T}_d\right]}^T$$

(17)

To observe the state vector $\tilde{\mathbf{x}}$ based on Kalman filtering theory, we construct an observation equation as

$$\dot{\widehat{\tilde{\mathbf{x}}}}=\tilde{\mathbf{A}}\widehat{\tilde{\mathbf{x}}}+\tilde{\mathbf{B}}{U}_a+{\tilde{\mathbf{K}}}_{\mathbf{o}}\left(y-\tilde{\mathbf{C}}\widehat{\tilde{\mathbf{x}}}\right)$$

(18)

In Equation (18), $\widehat{\tilde{\mathbf{x}}}$ is the estimated value of the state vector $\tilde{\mathbf{x}}$, and the other coefficients are

$$\left\{\begin{array}{c}\left\{\tilde{\mathbf{A}},\tilde{\mathbf{B}}\right\}=\left\{\left[\begin{array}{cc}A& B_d\\ 0& 0\end{array}\right],\left[\begin{array}{c}B\hfill \\ 0\hfill \end{array}\right]\right\}\hfill \\ \left\{\tilde{\mathbf{C}},{\tilde{\mathbf{K}}}_{\mathbf{o}}\right\}=\left\{\left[\begin{array}{c}C\hfill \\ 0\hfill \end{array}\right],\left[\begin{array}{c}{K}_{\omega }\hfill \\ K_I\hfill \end{array}\right]\right\}\hfill \end{array}\right.$$

(19)

To implement the observer in a computer, we provide the calculation formula for the discrete state space equation as

$$\left\{\begin{array}{c}{\tilde{\mathbf{A}}}_{\mathbf{d}}=e^{\tilde{\mathbf{A}}{t}_s}\hfill \\ {\tilde{\mathbf{B}}}_{\mathbf{d}}=\tilde{\mathbf{B}}\underset{0}{\overset{t_s}{\int }}{e}^{{\tilde{\mathbf{A}}}_{\mathbf{d}}\tau }d\tau \hfill \end{array}\right.$$

(20)

To analyze the performance of the observer, we define the observation error as

$$\left\{\begin{array}{c}{\tilde{\mathbf{e}}}_x=\widehat{\tilde{\mathbf{x}}}-\tilde{\mathbf{x}}\hfill \\ {\tilde{\mathbf{e}}}_d={\widehat{T}}_d-T_d\hfill \end{array}\right.$$

(21)

By Equations (14), (16) and (21), we can obtain Equation (22) as

$$\left\{\begin{array}{c}{\dot{\tilde{\mathbf{e}}}}_x=\left(A-K_{\omega }C\right){\tilde{\mathbf{e}}}_x+B_d{\tilde{\mathbf{e}}}_d\hfill \\ {\dot{\tilde{\mathbf{e}}}}_d=-K_{I}C{\tilde{\mathbf{e}}}_x+{\tilde{\mathbf{e}}}_d\hfill \end{array}\right.$$

(22)

We can obtain the state space equation for the observation error ${\tilde{\mathbf{e}}}_x$ through Equation (22) as follows.

$$\left\{\begin{array}{c}\dot{\tilde{\mathbf{E}}}=\left(\tilde{\mathbf{A}}-{\tilde{\mathbf{K}}}_{\mathbf{o}}^{\mathbf{0}}\tilde{\mathbf{C}}\right)\tilde{\mathbf{E}}\hfill \\ \tilde{\mathbf{E}}={\left[{\tilde{\mathbf{e}}}_x{\tilde{\mathbf{e}}}_d\right]}^T\hfill \end{array}\right.$$

(23)

Because the second-level system is a single-input, single-output system with a speed sensor configured, the $\left(A,C\right)$ in Equation (14) is observable. Based on Kalman filtering theory, we can iteratively calculate the Kalman filter gain ${\tilde{\mathbf{K}}}_{\mathbf{o}}^{\mathbf{0}}$ through the model parameters, error variance, and Equation (24).

$$\left\{\begin{array}{c}\tilde{\mathbf{P}}\left(k\left|k-1\right.\right)={\tilde{\mathbf{A}}}_{\mathbf{d}}\left(k\right)+\tilde{\mathbf{W}}{\tilde{\mathbf{R}}}_{\mathbf{P}}{\tilde{\mathbf{W}}}^{\mathbf{T}}\hfill \\ {\tilde{\mathbf{K}}}_{\mathbf{o}}^{\mathbf{0}}\left(k\right)=\tilde{\mathbf{P}}\left(k\left|k-1\right.\right){\tilde{\mathbf{C}}}^{\mathbf{T}}{\left[{\tilde{\mathbf{R}}}_{\mathbf{M}}+\tilde{\mathbf{C}}\left(k\right)\right]}^{-1}\hfill \\ \tilde{\mathbf{P}}\left(k\left|k\right.\right)=\left[\mathbf{I}-{\tilde{\mathbf{K}}}_{\mathbf{o}}^{\mathbf{0}}\left(k\right)\tilde{C}\right]\tilde{\mathbf{P}}\left(k\left|k-1\right.\right)\hfill \\ {\tilde{\mathbf{A}}}_{\mathbf{d}}\left(k\right)={\tilde{\mathbf{A}}}_{\mathbf{d}}\tilde{\mathbf{P}}\left(k-1\left|k-1\right.\right){\tilde{\mathbf{A}}}_{\mathbf{d}}^{\mathbf{T}}\hfill \\ \tilde{\mathbf{C}}\left(k\right)=\tilde{\mathbf{C}}\tilde{\mathbf{P}}\left(k\left|k-1\right.\right){\tilde{\mathbf{C}}}^{\mathbf{T}}\hfill \end{array}\right.$$

(24)

In Equation (24), $\tilde{\mathbf{W}}$ is the noise input matrix; ${\tilde{\mathbf{R}}}_{\mathbf{P}}=diag\left[\begin{array}{cc}{R}_{\tilde{I}}& R_{\tilde{d}}\end{array}\right]$ is the covariance matrix of the process noise, including the control voltage $U_a$, noise variance $R_{\tilde{I}}$, and random disturbance variance $R_{\tilde{d}}$; ${\tilde{\mathbf{R}}}_M=diag\left[R_{\tilde{\omega }}\right]$ is the measurement noise variance matrix, which includes the speed $v_0^{*}$ noise variance $R_{\tilde{\omega }}$; $\tilde{\mathbf{P}}\left(k-1\left|k-1\right.\right)$ is the covariance of the prediction error; $\tilde{\mathbf{P}}\left(k\left|k-1\right.\right)$ is the covariance of the prediction error for the next step. ${\tilde{\mathbf{K}}}_{\mathbf{o}}^{\mathbf{0}}\left(k\right)$ is the value of ${\tilde{\mathbf{K}}}_{\mathbf{o}}^{\mathbf{0}}$ at the k-th iteration.

3.2. Position Loop Design of Second-Level System of the Heavy-Load Servo System

The position loop control strategy consists of a proportional (P) and a feed-forward (FF) controller. The P controller $K_{pp}$ adjusts the position loop to a critical damping system and avoids system overshoot. The FF controller $G_f\left(s\right)$ eliminates static errors after the system stops. The position loop of the second-level system is shown in Figure 10.

We can use the critical proportionality method to obtain the proportional regulator parameters $K_{pp}$ in Figure 10.

From Figure 8 and Equation (13), we can deduce the speed loop model $G_v\left(s\right)$ in Figure 10 as

$$G_v\left(s\right)={\displaystyle \frac{6{\xi }_v^2{t}_{r}s+9}{{\xi }_v^2{t}_r^2{s}^2+6{\xi }_v^2{t}_{r}s+9}}$$

(25)

From Figure 10 and Equation (25), we can write the position loop model $G_p\left(s\right)$ as

$$G_p\left(s\right)={\displaystyle \frac{G_f\left(s\right)G_v\left(s\right)+K_{pp}{G}_v\left(s\right)}{s+K_{pp}{G}_v\left(s\right)}}$$

(26)

From Equation (26), we can deduce the error transfer function $G_{pe}\left(s\right)$ of the position loop as

$$G_{pe}\left(s\right)=1-G_p\left(s\right)={\displaystyle \frac{s-G_f\left(s\right)G_v\left(s\right)}{s+K_{pp}{G}_v\left(s\right)}}$$

(27)

Through making $G_{pe}\left(s\right)=0$, we can obtain that the FF controller $G_f\left(s\right)$ is

$$G_f\left(s\right)={\displaystyle \frac{s}{G_v\left(s\right)}}$$

(28)

From Equation (25), we can transform Equation (28) as

$$G_f\left(s\right)={\displaystyle \frac{{\xi }_v^2{t}_r^2{s}^3+6{\xi }_v^2{t}_r{s}^2+9s}{6{\xi }_v^2{t}_{r}s+9}}$$

(29)

From $K_{pp}$ and $G_f\left(s\right)$, we have designed the position loop control strategy.

4. Experiment and Evaluation

The actual product of the heavy-load servo system tested in our lab is shown in Figure 11.

The first-level system is a high-precision servo system, and its position loop bandwidth is much broader than the second-level system. Therefore, within the position loop bandwidth of the second-level system, the position loop of the first-level system can be equivalent to the coefficient of 1. That is, $p_{MTP}$ and $p_0$ are equal. We can simplify Figure 8, Figure 9, Figure 10, Figure 11 and Figure 12.

4.1. Components and Parameters of the Experimental Platform

Due to the same origin coordinates of the first-level and second-level systems on the y-axis, we can intuitively validate the SIEP-MP algorithm. Therefore, we built the lifting mechanism of the second-level system on the y-axis for algorithm experiments.

We conducted experiments on a dSPACE system and a heavy-load servo system. The schematic and actual system of the experimental platform are shown in Figure 13a and Figure 13b, respectively.

In dSPACE, we use the virtual signal generator module to generate $p_{MTP}$ and the SIEP-MP algorithm program to plan $p_{MTP}$ to generate $p_{SIEP}$. In the second level of the heavy-load servo system, we use a motor driver to receive the control voltage $U_a$ from dSPACE and drive the electric motor. We use an absolute position encoder to provide the angle $p_0^{*}$ and angular velocity $v_0^{*}$.

The component parameters of the experimental platform are shown in

Table 1. Experimental platform component parameters.

Component	Type Number	Parameter	Numeric Value
		Rated Power	750 (W)
		Rated Torque	2.39 (N·m)
AC Motor	ASM80B1007	Rated Speed	3000 (r/min)
		ETC ($K_T$)	0.239
		Encoder-lines	2500
		Sampling Time	1 (ms)
dSPACE	ds1104	Voltage Range	−10∼10 (v)
		Quantization Bits	16 (bit)
		Resolution Ratio	0.1 (°)
Gradienter	LXR-T90	Measuring Accuracy	±0.1 (°)
		Measuring Range	0∼180 (°)
Absolute Encoder	CAPRO -B112050	Protocol	RS232
		Quantization Bits	19 (bit)
		Update Frequency	500 (Hz)
		Baud	57.6 (kb/s)
Electric Cylinder	DSH0506 -250-FL	Rated pressure	3 (kN)
		Rated stroke	230 (mm)
		Position accuracy	±0.02 (mm)
		Screw Lead	4 (mm)
Heavy Load		Load Mass	170 (kg)
		Load Length	1300 (mm)
Lifting Mechanism		A Point	(573, 1074) (mm)
		B Point	(723, 928) (mm)
		Start Point of C	(500, 500) (mm)
		Pitch Angle Range	65 (°)

.

4.2. Design of Target Trajectory, SIEP-MP Algorithm, and Servo Control Algorithm

Firstly, we design the target trajectory.

We demonstrate the low-pass filtering function, range limitation function, eliminating power saturation function, and energy-saving function of the SIEP-MP algorithm by designing the random noise, step signal, chirp signal, and sine signal in the target trajectory.

In the search mode of 0 to 3 s, we set the target trajectory as a random noise signal with a sample period of 0.001 s and an amplitude of 0.01°. In the following mode of 3 to 6 s, we set the target trajectory as a step signal with an amplitude of 8°. In the following mode of 6 to 11.8 s, we set the target trajectory as a chirp signal with a frequency of (0.5–2) Hz and an amplitude of 8°. In the precise aiming mode of 11.8 to 15 s, we set the target trajectory as a sine signal with a frequency of 2 Hz and an amplitude of 8°.

Secondly, we design the SIEP-MP algorithm.

Since the SIEP-MP algorithm is a frequency domain-based motion planning algorithm, we set the Butterworth low-pass filter Equation (30) with the mechanical time constant $T_{sm}$ as the control group of the SIEP-MP algorithm.

$$G_{low}\left(s\right)={\displaystyle \frac{1}{T_{sm}s+1}}$$

(30)

To ensure that Equations (11) and (30) have the same frequency domain characteristics, we set parameters $\gamma$ and $T_{sm}$ in different modes, as shown in

Table 2. Motion planning algorithm parameters.

Parameter	Scanning Mode (0∼3 s)	Following Mode (3∼6 s)	Following Mode (6∼11.8 s)	Precision Aiming Mode (11.8∼15 s)
$\gamma$	0.2	0.2	0.2	4
$\left(l_x-L_x\right)/\phantom{\left(l_x-L_x\right)2}2$	4°	4°	4°	4°
$T_{sm}$	0.1	0.1	0.1	0.005

.

In Table 2, $\left(l_x-L_x\right)/\phantom{\left(l_x-L_x\right)2}2$ is the limiting range, which is the maximum limited distance between $p_{MTP}$ and $p_{SIEP}$ in the SIEP-MP algorithm.

Finally, we design the parameters of the control strategy.

We can set the parameters of the speed loop controller $G_{vel}\left(s\right)$ through the heavy-load inertia $J=21.95\mathrm{kg}·{\mathrm{m}}^2$ of the two-level system. Average transmission ratio $N=282$. ETC $K_T=0.239$. Speed loop rise time $t_r=0.2\mathrm{s}$, and damping coefficient ${\xi }_v=0.707$. According to Equation (13), the speed loop controller is

$$G_{vel}\left(s\right)=K_P+{\displaystyle \frac{K_I}{s}}=9.77+{\displaystyle \frac{146.59}{s}}$$

(31)

After debugging the SAKF observer, the parameters of Equation (24) are

$$\left\{\begin{array}{c}{\tilde{\mathbf{R}}}_{\mathbf{P}}=diag\left(R_{\tilde{u}},R_{wd}\right)=diag\left(7.7\times 10^{-9},1\right)\hfill \\ {\tilde{\mathbf{R}}}_{\mathbf{M}}=\left[R_{\tilde{\omega }}\right]=\left[1.2\times 10^{-5}\right]\hfill \end{array}\right.$$

(32)

4.3. Experiment and Evaluation of the SIEP-MP Algorithm

Based on the parameters in subsection B of Section 4.2, the instructions of the MTP, SIEP, and low-pass filtering point (LFP) are shown in Figure 14.

Figure 14 shows the following three functions of the SIEP-MP algorithm: 1. The SIEP-MP algorithm has a function similar to the low-pass filter, which can filter out high-frequency clutter signals in the instruction (see Figure 14b). 2. The SIEP-MP algorithm can solve the boundary problem in Figure 4, which can constrain the distance between the pre- and post-planning signals within a certain range, while the low-pass filter does not have the above function (see Figure 14c). 3. Due to its range-limiting function, the SIEP-MP algorithm can eliminate power saturation during switching to ensure fast mode switching, which is superior to low-pass filters (see Figure 14d).

Under the instructions in Figure 14, the actual tracking trajectory of the second-level system of the heavy-load servo system under different motion planning algorithms is shown in Figure 15.

Figure 15 shows the following three performances of the SIEP-MP algorithm: 1. The SIEP-MP algorithm and low-pass filter are similar to low-order systems, which can reduce overshoot and residual vibration of the system under step instructions (see Figure 15b). 2. Both the SIEP-MP algorithm and low-pass filter can eliminate the power saturation problem of the second-level system under step instructions. In contrast, unplanned instructions cannot prevent the second-level system from reaching a saturation power of 750 w (see Figure 15c). 3. Compared to unplanned instructions, both the SIEP-MP algorithm and low-pass filter planning instructions can reduce the energy consumption of the second-level system (see Figure 15d).

5. Conclusions

The SIEP-MP algorithm, a bionic innovation drawing from psychological field theory and head-eye motion control principles, excels in target tracking for heavy-load servo systems. It empowers the second-level system to emulate the scanning, smooth pursuit, and gaze modes of a bionic eye during search, following, and precision aiming phases.

In search mode, the algorithm introduces a strategic delay in second-level system tracking, preventing boundary collisions and conserving energy vital for heavy-load operations in constrained spaces. This energy optimization is key without sacrificing safety. During following mode, adjustable delays enable the second-level system to track the first-level system efficiently, striking a balance between energy conservation and responsiveness. The algorithm dynamically fine-tunes these delays according to real-time system dynamics and environmental shifts, enhancing tracking efficiency. The precision aiming mode leverages parameter switching guided by the SIEP-MP algorithm, enabling high-accuracy tasks through adaptive fine-tuning that responds swiftly to target and system changes. The mode transitions are seamless, eliminating overshoot and power surges, with fast switching facilitated by range limitations. This minimizes delays, boosting system agility and stability.

Overall, SIEP-MP’s success stems from its tailored approach to heavy-load servo challenges, integrating energy management, adaptive parameter adjustment, and fluid transitions to ensure precise, efficient target tracking. Its design advancements open new avenues for deploying such systems in intricate environments.