Inertial measurement system stable loop redundant sensor instability recovery method

Abstract

The invention provides an inertial measurement system stable loop redundant sensor system instability recovery method, and belongs to the field of inertial measurement. The method comprises the following steps: firstly, for a three-frame four-axis stable platform, carrying out physical modeling analysis and designing a non-linear integral sliding mode controller capable of resisting random disturbance; then, configuring a redundant sensor for the stabilized platform; according to the rotation relation between the frames of the stabilized platform, coordinate transformation between the frame systems is carried out, and the data relation between the sensors is obtained; and finally, considering multi-party signal sources after the redundant sensors are added, positioning a fault, setting a signal substitution scheme, and carrying out multiple redundant backup on data required by platform stability maintenance. And a non-linear integral sliding mode controller with a rapid instability recovery characteristic is used to verify the replaced signal. According to the method, a fiber-optic gyroscope, an MEMS gyroscope, a grating ruler and other sensors are used for data fusion, coordinate system transformation and a signal replacement strategy are combined, a fault sensor is accurately positioned, and signal replacement is carried out.

Description

Technical Field

[0001] This invention belongs to the field of inertial measurement, specifically a method for recovering from instability in a redundant sensor system with a stable loop in an inertial measurement system. Background Technology

[0002] An inertial platform provides a spatial inertial coordinate system reference through its stabilization loop. When the external environment is overloaded or a component of the stabilization loop fails, the loop may become unstable and diverge. In the event of an accident, the platform's ring frame will rapidly rotate and fall from its original stable position. The coupling between the platform ring frame and the gyroscope, at high speeds, can damage not only the gyroscope and accelerometer but also the internal connecting wires, resulting in a fatal malfunction.

[0003] To avoid losses caused by platform collapse, stable loop instability recovery control technology is undoubtedly a reliable and effective platform protection method.

[0004] The stabilization loop of an inertial platform contains a variety of sensors. When a sensor fails, the control loop will fail, resulting in loop instability. For unavoidable sensor failures, it is necessary to locate the fault and restore the signal to ensure that the platform can quickly return to its initial position after instability, thereby completing the instability recovery work.

[0005] To address this, a method for fault location and signal replacement of redundant sensor systems is proposed. Two sets of redundant sensors are configured on the inertial platform to ensure multiple signal sources. The fault location is determined in a timely manner by comparing the signals from the redundant sensors, and the faulty sensor signal is immediately replaced by the signal from the other redundant sensors after processing, thus ensuring the normal operation of the stable loop. Summary of the Invention

[0006] This invention addresses the instability of a three-frame four-axis platform caused by the failure of some sensors in its stabilization loop. It proposes a method for restoring instability using a redundant sensor system in the stabilization loop of an inertial measurement system. By employing a redundant sensor system and deriving the mathematical relationship between the angular velocities of the four axes, the mathematical relationship between the signals of each sensor is obtained. Based on this, fault location and signal replacement are performed, thereby completing the instability recovery.

[0007] The instability recovery method for the redundant sensor system in the stabilization loop of the inertial measurement system comprises the following steps:

[0008] Step 1: Perform physical modeling analysis and design a nonlinear integral sliding mode controller for the three-frame four-axis stabilized platform;

[0009] The modeling equations for the inertial platform are:

[0010]

[0011] Where J is the platform's moment of inertia. and These are the angular velocity and angular acceleration of the platform, respectively, K m This is the gyroscope torque coefficient, R is the motor resistance, u is the control variable, and K is the control torque coefficient. b It is the back electromotive force coefficient, ω is the relative angular velocity between the platform and the gimbal, and T is the back electromotive force coefficient. c T represents the Coulomb friction torque. s ω represents the maximum static friction. s σ is the Stribeck angular velocity, σ is the coefficient of viscous friction, m is the mass of the platform, g is the acceleration due to gravity, and a is the angular velocity of the platform. z and a x These are the acceleration components along the z-axis and x-axis, respectively, θ b The angle between the gravitational acceleration vector and the radial velocity vector of the center of gravity about the platform is represented by r0, which is the distance between the center of mass of the inertial platform and the center of rotation. d It is an external random disturbance torque.

[0012] The equations for the nonlinear integral sliding mode controller are:

[0013]

[0014] Where λ is the uncertainty error parameter, and e and Let k be the position error and its derivative. p k i k and ε are control parameters, and t is time. Let g(e) be the second derivative of the position quantity, g(e) be a nonlinear function, and s be a sliding surface function.

[0015] Step 2: Configure redundant sensors for the three-frame four-axis stabilization platform;

[0016] The three-frame four-axis stabilization platform includes an inner frame, an outer frame, and a follower frame. The three frames correspond to three axis systems, plus the platform axis system, for a total of four axis systems. A sensor is placed on each frame, and the relationship between the sensor signals is derived by the rotational relationship between the axis systems.

[0017] On the stage, three fiber optic gyroscopes, namely A1, A2 and A3, are installed along the three axes of the stage: roll axis, azimuth axis and pitch axis. MEMS gyroscope B1 is installed on the inner frame directly connected to the stage, MEMS gyroscope B2 is installed on the outer frame connected to the inner frame, and MEMS gyroscope B3 is installed on the follower frame connected to the outer frame.

[0018] A grating ruler C1 is installed between the stage body and the inner frame to measure the relative angle between the stage system and the inner frame system in the roll axis direction. A grating ruler C2 is installed between the inner frame and the outer frame to measure the relative angle between the inner frame system and the outer frame system in the yaw axis direction. A grating ruler C3 is installed between the outer frame and the servo frame to measure the relative angle between the outer frame system and the servo system in the pitch axis direction. Simultaneously, three MEMS gyroscopes D1, D2, and D3 are added to the stage body as redundant sensors for the fiber optic gyroscopes A1, A2, and A3, respectively.

[0019] Step 3: Perform layer-by-layer coordinate transformation on the rotational relationship between each frame of the three-frame four-axis stabilization platform to obtain the relationship between the sensor data on the frame and the sensor data on the platform.

[0020] The specific steps are as follows:

[0021] Step 301: Establish four coordinate systems for the three-frame four-axis stable platform: OX1Y1Z1 is the platform coordinate system, OX2Y2Z2 is the inner frame coordinate system, OX3Y3Z3 is the outer frame coordinate system, and OX4Y4Z4 is the follower frame coordinate system.

[0022] Step 302: Perform the first-level coordinate transformation between the platform system and the internal frame system;

[0023] The inner frame rotates only about the Z-axis of the platform system, and the platform system rotates about the z1 axis by θ. z The coordinate transformation is as follows:

[0024]

[0025] in, These represent the absolute angular velocities of the platform system along the x, y, and z axes, respectively. The absolute angular velocities θ and y are the x-axis, y-axis, and z-axis of the inner frame system, respectively. z The z-axis rotation angle between the platform and the inner frame. The relative angular motion rate between the platform system and the inner frame system.

[0026] The relationship of angular velocity along the z1 axis is as follows:

[0027]

[0028] The final relationship between the sensor data on the frame and the sensor data on the platform is obtained as follows:

[0029] B1-d(C1)=A1

[0030] Wherein, B1 corresponds to the signal output value of the redundant MEMS gyroscope B1 in the inner frame, A1 is the signal output value of the fiber optic gyroscope A1 on the z-axis, and d(C1) corresponds to the signal differential value of the grating ruler C1 on the z-axis.

[0031] Step 303: Perform the second-level coordinate transformation between the inner frame system and the outer frame system;

[0032] The outer frame rotates only about the Y-axis of the inner frame, and the inner frame is rotated about the y2 axis by θ. y The coordinate transformation is as follows:

[0033]

[0034] in, These represent the absolute angular velocities along the x, y, and z axes of the outer frame system, respectively, and θ. y The y-axis rotation angle between the inner and outer frames. The relative angular motion rate between the inner frame system and the outer frame system.

[0035] Focusing on the angular velocity relationship along the y2 axis, we can obtain the following formula:

[0036]

[0037] The final relationship between the sensor data on the frame and the sensor data on the platform is obtained as follows:

[0038] B2-d(C2)=A3 sin(C1)+A2 cos(C1)

[0039] Wherein, B2 corresponds to the signal output value of the redundant MEMS gyroscope B2 of the outer frame, d(C2) corresponds to the signal differential value of the y-axis grating ruler C2, A3 corresponds to the signal output value of the x-axis stage fiber optic gyroscope A3, A2 is the signal output value of the y-axis stage fiber optic gyroscope A2, and C1 is the signal output value of the z-axis grating ruler C1.

[0040] Step 304: Perform the third-level coordinate transformation between the outer frame system and the following frame system;

[0041] The follower frame rotates only about the X-axis of the outer frame, while the outer frame is rotated about the x3 axis by θ. x The coordinate transformation is as follows:

[0042]

[0043] in, The absolute angular velocities θ and θ of the servo frame system along the x, y, and z axes are respectively. x The x-axis rotation angle between the outer frame and the follower frame. The relative angular motion rate between the outer frame system and the follower frame system.

[0044] The relationship between the angular velocities along the x3 axis can be expressed as follows:

[0045]

[0046] The final relationship between the sensor data on the frame and the sensor data on the platform is obtained as follows:

[0047] B3-d(C3)=cos(C2)cos(C1)A3-cos(C2)sin(C1)A2+sin(C2)(A1+d(C1))

[0048] Wherein, B3 is the signal output value of the redundant MEMS gyroscope B3 of the follower frame, d(C3) is the signal differential value of the grating ruler C3 along the x-axis, C2 is the signal output of the grating ruler C2 along the y-axis, and C3 is the signal output value of the grating ruler C3 along the x-axis.

[0049] Step 4: Pair the relationship between the sensor data on the frame and the sensor data on the platform with the data from the redundant sensors to obtain:

[0050]

[0051] Where D1 is the signal output value of z-axis redundant MEMS gyroscope D1, D2 is the signal output value of y-axis redundant MEMS gyroscope D2, and D3 is the signal output value of x-axis redundant MEMS gyroscope D3.

[0052] Step 5: Based on the data from redundant sensors, locate the fault and set up a signal replacement plan;

[0053] (1) First, focus on the first layer B1-d(C1) = A1 = D1. At this time, the fault diagnosis scheme is a single-axis diagnosis scheme, as follows:

[0054] If B1-d(C1)=D1≠A1, it indicates that fiber optic gyroscope A1 has malfunctioned. The D1 signal is used to replace the A1 signal to complete the instability recovery.

[0055] If B1-d(C1)=A1≠D1, it indicates that MEMS gyroscope D1 has malfunctioned and is not unstable.

[0056] If B1-d(C1)≠A1=D1, then if the frame MEMS gyroscope B1 malfunctions, there will be no instability.

[0057] If the grating ruler C1 malfunctions, the motor will exhibit abnormal operating conditions:

[0058] d(C1)=B1-A1

[0059]

[0060] t0 is the starting point of the time integration.

[0061] The data from the frame MEMS gyroscope B1 and the data from the stage fiber optic gyroscope A1 are subtracted, integrated, and then the initial integral value when the grating ruler is active is added to replace the C1 data. When a single sensor fails, the signal from the grating ruler C1 and the normal output of the angular velocities of gyroscopes A1 / D1 can still be guaranteed.

[0062] (2) Taking into account the data from the second and third layers as well

[0063]

[0064] a. If the gyroscopes A2 / A3 / D2 / D3 on the platform malfunction, then A3 sin(C1) + A2 cos(C1) ≠ D3 sin(C1) + D2 cos(C1).

[0065] If the platform becomes unstable, it indicates a fault in gyroscope A2 or A3. In this case, compare the output signals of gyroscope A2 with those of D2, and A3 with those of D3. The fiber optic gyroscope with unequal values ​​is the faulty one. In this case, D2 / D3 replaces the A2 / A3 signal to complete the instability recovery.

[0066] If the platform does not become unstable, it indicates that gyroscopes D2 / D3 are malfunctioning. In this case, compare the values ​​of A2 with D2 and D3 with D3. MEMS gyroscopes with unequal values ​​are malfunctioning.

[0067] b. The grating ruler C2 on the outer frame is faulty.

[0068] At this point, both the upper and lower equations are unequal, and the motor corresponding to the grating ruler C2 is malfunctioning, causing the platform to become unstable. The data from the grating ruler C2 is then replaced with data from other sensors.

[0069] d(C2)=B2-A3 sin(C1)-A2 cos(C1)

[0070]

[0071] Here, the data from each discrete point is calculated and accumulated to complete the integration operation. The data of the grating ruler C2 when it is in normal condition is used as the integration starting point. The data from the four sensors on the right side of the above equation are calculated, accumulated, and added to the integration starting point to complete the replacement of the grating ruler C2 data.

[0072] c. The MEMS gyroscope B2 on the outer frame malfunctioned.

[0073] At this point, the above equation B2-d(C2)≠A3 sin(C1)+A2 cos(C1)=D3 sin(C1)+D2 cos(C1) remains unaffected and the platform does not become unstable, thus completing the fault location.

[0074] d. The MEMS gyroscope B3 and the grating ruler C3 on the servo frame are malfunctioning.

[0075] B3-d(C3)=cos(C2)cos(C1)A3-cos(C2)sin(C1)A2+sin(C2)(A1+d(C1))

[0076] Since gyroscopes C1, C2, A1, and A2 all have redundant designs, they are robust even if the first two layers fail. Therefore, when the above equation is not equal, it indicates that MEMS gyroscope B3 and grating ruler C3 on the servo frame have failed.

[0077] If the MEMS gyroscope B3 malfunctions, the platform will not become unstable, which can be used to locate the fault.

[0078] If the grating ruler C3 malfunctions, the platform will become unstable, and signal replacement can be performed.

[0079] d(C3)=B3-cos(C2)cos(C1)A3+cos(C2)sin(C1)A2-sin(C2)(A1+d(C1))

[0080]

[0081] Here, the data from each discrete point is calculated and accumulated to complete the integration operation. The data of the grating ruler C3 when it is in normal condition is used as the integration starting point. The data from the six sensors on the right side of the above equation are calculated, accumulated, and added to the integration starting point to complete the replacement of the data of the grating ruler C3.

[0082] Step 6: Verify the substitute signal using a nonlinear integral sliding mode controller.

[0083] The advantages of this invention are:

[0084] This invention presents a method for instability recovery of a redundant sensor system in an inertial measurement system. Based on nonlinear control theory, a nonlinear integral sliding mode controller capable of rapid instability recovery is designed. By nonlinearly processing the closed-loop error, the system instability recovery time is shortened. A redundant sensor instability recovery system is proposed, which ensures multiple signal combinations by placing redundant sensors. It can also perform accurate fault location and signal replacement based on mutual judgment of multiple signal composition methods. Attached Figure Description

[0085] Figure 1 This is a flowchart of the instability recovery method for the redundant sensor system in the stabilization loop of the inertial measurement system according to the present invention;

[0086] Figure 2 This is a schematic diagram of the unbalanced torque of the inertial platform of the present invention;

[0087] Figure 3 This is a schematic diagram of the motor space vector pulse width modulation sector positioning of the present invention;

[0088] Figure 4 This is a schematic diagram showing the sensor placement positions in the redundant sensor system of the present invention;

[0089] Figure 5 This is a schematic diagram of the coordinate system establishment of the three-frame four-axis platform of the present invention;

[0090] Figure 6 This is a schematic diagram of the timing of corner data substitution in the redundant sensor system framework of the present invention;

[0091] Figure 7 This is a comparison chart of the torque tracking performance of the sliding mode controller and the PID controller of the present invention;

[0092] Figure 8 This is a comparison diagram of the table rotation angle control performance of the sliding mode controller and the PID controller of the present invention;

[0093] Figure 9 This is a data diagram showing the recovery of fiber optic gyroscope failure and instability in the redundant sensor system of this invention.

[0094] Figure 10 This is a data diagram showing the recovery of the grating ruler failure and instability in the redundant sensor system of this invention. Detailed Implementation

[0095] To facilitate understanding and implementation of the present invention by those skilled in the art, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Obviously, the described embodiments are merely some, not all, embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort should fall within the scope of protection of the present invention.

[0096] This invention discloses a method for instability recovery of a redundant sensor system in an inertial measurement system's stable loop. The method utilizes a redundant sensor system to locate the fault and replace signals, thereby achieving instability recovery. A nonlinear integral sliding mode controller is also designed as the instability recovery controller.

[0097] like Figure 1 As shown, the specific steps are as follows:

[0098] Step 1: Perform physical modeling analysis and design a nonlinear integral sliding mode controller for the three-frame four-axis stabilized platform;

[0099] The inertial measurement system (IMS) comprises a stable platform and inner, outer, and follower frames connected sequentially via axes, forming the basic mechanical structure of a three-frame, four-axis platform. The platform is a central circular platform equipped with fiber optic gyroscopes, and grating rulers are installed at the connecting axes of each frame. Redundant MEMS gyroscopes are added to the platform and to each frame to measure the absolute angular velocity of each frame. The redundant sensor signals are used as the basis for judgment and instability recovery, leading to the derivation of fault location and signal replacement schemes.

[0100] A dynamic analysis of the platform of a three-frame four-axis stabilized platform is performed. Since the platform remains stationary under the action of the control loop, the platform rotation angle α is treated as a small angle. A moment analysis is performed on the platform axis. Since the damping moment of the platform axis has a small impact on actual engineering, the damping moment is ignored, resulting in the following dynamic equation:

[0101]

[0102] Where J is the platform's moment of inertia. and These are the angular velocity and angular acceleration of the platform, T. m It is the control torque, T f It is the frictional torque, T g It is the mass imbalance torque, T d It is an external random disturbance torque. The platform's moment of inertia was measured to be J = 0.0717 m·kg. 2 .

[0103] When a platform is operating stably, friction is present on all contact surfaces, such as connecting bearings and servo actuators. The nonlinearity of friction has a significant impact at low platform speeds, potentially leading to intermittent low-speed crawling, steady-state errors, and even system divergence. The Stribeck model, a mathematical model of static friction, has good performance in engineering applications; studies show it can accurately describe the friction between mechanical structures with 90% accuracy. Based on the Stribeck effect of nonlinear friction, a model describing the characteristics of friction was established.

[0104] The Stribeck model is used to describe the frictional torque T. f As shown in equation (2):

[0105]

[0106] Among them, T c T represents the Coulomb friction torque. s This represents the maximum static friction, ε is the relative angular velocity between the platform and the universal joint, and ω is the relative angular velocity between the platform and the universal joint. sσ is the Stribeck angular velocity, and σ is the coefficient of viscous friction.

[0107] This example demonstrates the Coulomb friction torque T obtained through testing. c =0.025 N·m, maximum static friction T s =0.035 N·m, Stribeck angular velocity ω s =0.8 rad / s, viscous friction coefficient σ = 0.002 N·m·s / rad

[0108] When the mass of the inertial platform is not at the center of rotation, the gravitational acceleration and the linear acceleration in the x and z axes will cause a disturbance torque on the center of rotation. The disturbance torque changes with the phase law. Balancing this disturbance torque in the control algorithm can make the control effect more ideal.

[0109] like Figure 2 As shown, the acceleration of the center of mass is decomposed into the x-axis and z-axis, and the torque is taken about the center of rotation. The mass imbalance torque T g The expression is as follows:

[0110] T g =m(ga) z cos(θ) b )r0+ma x sin(θ b )r0 (3)

[0111] Where m is the mass of the platform, g is the acceleration due to gravity, and a z and a x These are the acceleration components along the z-axis and x-axis, respectively, θ b The angle between the gravitational acceleration vector and the radial velocity vector of the center of gravity about the platform is taken as g = 9.8015 m / s² in the experiment. 2 r0 is the distance between the center of mass and the center of rotation of the inertial platform.

[0112] The expressions for the control torque output of the motor are as follows: (4) (5)

[0113] T m =K m i m (4)

[0114]

[0115] Among them, K m It is the gyro torque coefficient, i m It is the current flowing through the motor, K b It is the back electromotive force coefficient, R is the motor resistance, and u is the control quantity.

[0116] In summary, the final modeling equations for the inertial platform can be written as follows:

[0117]

[0118] The following is a summary of some parameters.

[0119]

[0120] The simplified form is as follows

[0121]

[0122] This invention employs a nonlinear integral sliding mode control algorithm to control an inertial platform. Based on stability considerations, a reasonable sliding surface is designed, allowing the state variables to slide along the sliding surface. When the system state variables cross the sliding surface, a switching method is used to adjust the system's control output value, causing the system state to slide along the sliding surface, thus achieving system control. This sliding surface switching method effectively counteracts the influence of model parameters, disturbances, and other factors, improving the overall system robustness and preventing instability issues caused by the nonlinear dynamic characteristics of linear controllers. The design incorporates an integral term to optimize the system's steady-state performance and eliminate steady-state errors. Particularly when operating under low-frequency interference, it effectively ensures the strong robustness and response speed of the integral sliding mode controller.

[0123] For integral sliding surfaces, the integral variable of the integral term is given special consideration. Compared with directly integrating the error, the nonlinear function with the error as the independent variable proposed in this invention can effectively solve the divergence problem: when the system error is small, increasing the error gain will result in a larger integral term output; when the system error is large, it is easy to cause the control signal to be too large, leading to system instability or even integral divergence. Therefore, limiting the error value can ensure that the sufficient control signal is not too large to cause oscillation or poor convergence. This allows for rapid adjustment under small errors and avoids the integral term being too large under large errors, which could cause the integral part of the entire system to fail to converge.

[0124] To design a nonlinear integral sliding mode controller for the stable loop of an inertial measurement system, the nonlinear function of the closed-loop error e should be used as the sliding mode integral term. At the same time, the disturbance error in the actual operation of the inertial measurement system should be set as a random disturbance and introduced into the stable loop control system model of the inertial measurement system. The controller design should be derived using the Lyapunov stability correlation method to resist the disturbance torque.

[0125] The expression for the nonlinear function is shown in equation (10):

[0126]

[0127] Let x1 = α, The state space can be established as shown in equation (11):

[0128] Where, α, These are the table's rotation angle and angular velocity, respectively.

[0129]

[0130] The sliding mode function is selected as follows:

[0131]

[0132] e = x1 - x d (13)

[0133] Differentiating with respect to s, we get:

[0134]

[0135]

[0136] The Lyapunov function is constructed as follows:

[0137]

[0138] The selection convergence rate is:

[0139]

[0140] By combining the equations, we can obtain:

[0141]

[0142] in It is an estimate of θ.

[0143] Let the parameter estimation error be:

[0144]

[0145] Construct the Lyapunov function and find its derivative as follows:

[0146]

[0147] Selecting parameters for adaptive law:

[0148]

[0149] The final nonlinear integral sliding mode controller equation is:

[0150]

[0151] Where λ is the uncertainty error parameter, and e and Let k be the position error and its derivative.p k i k and ε are control parameters, and t is time. Let g(e) be the second derivative of the position quantity, g(e) be a nonlinear function, and s be a sliding surface function.

[0152] Step 2: For the three-frame four-axis stabilization platform, analyze the sources of the required sensor signals and determine the redundant sensor configuration scheme;

[0153] When the control loop of the three-frame four-axis platform is operating, it requires angular velocity data from fiber optic gyroscopes and angle data from grating rulers to achieve stable control. The integral of the angular velocity data from the fiber optic gyroscopes serves as the error in the control loop, directly affecting its operation. The motor utilizes space vector pulse width modulation (SPWM). The three-phase inverter generates different voltage vectors through six switching devices, thereby controlling the motor's current and magnetic field to ensure precise operation. The core of SPWM is to synthesize the target voltage within one PWM cycle by appropriately selecting these eight voltage vectors, thus controlling the motor's speed and direction.

[0154] The eight voltage vectors divide the voltage vector space into six sectors, such as... Figure 3 As shown, the rotor angle of the motor determines its current sector. The grating ruler monitors this angle in real time and transmits it to the control system. Based on the current sector, the system selects the appropriate voltage vector combination to control the inverter's switching, thereby achieving precise motor drive. It is clear that the motor requires angle data provided by the grating ruler to achieve normal control.

[0155] The three-frame four-axis stabilization platform includes an inner frame, an outer frame, and a follower frame. The three frames correspond to three axis systems, plus the platform axis system, for a total of four axis systems. A sensor is placed on each frame, and the relationship between the sensor signals is derived by the rotational relationship between the axis systems.

[0156] The basic architecture of the three-axis platform is as follows Figure 4 As shown, A, B, C, and D represent fiber optic gyroscopes, frame-redundant MEMS gyroscopes, grating rulers, and stage-redundant MEMS gyroscopes, respectively. The subscripts 1, 2, and 3 in the lower right corner represent the roll axis, azimuth axis, and pitch axis, respectively.

[0157] The system consists of four parts from the inside out. The innermost gray platform has three fiber optic gyroscopes, A1, A2 and A3, installed along the roll axis, azimuth axis and pitch axis respectively. The inner frame directly connected to the platform has a MEMS gyroscope B1 installed. The outer frame has a MEMS gyroscope B2 installed. The outermost servo frame has a MEMS gyroscope B3 installed.

[0158] A grating ruler C1 is installed between the stage body and the inner frame to measure the relative angle between the stage system and the inner frame system along the roll axis. A grating ruler C2 is installed between the inner frame and the outer frame to measure the relative angle between the inner frame system and the outer frame system along the yaw axis. A grating ruler C3 is installed between the outer frame and the servo frame to measure the relative angle between the outer frame system and the servo system along the pitch axis. Simultaneously, three MEMS gyroscopes in three directions are added to the stage body for redundancy. D1, D2, and D3 serve as redundant sensors for fiber optic gyroscopes A1, A2, and A3, respectively. Theoretically, the outputs of A and D should be identical, except for differences in accuracy.

[0159] Step 3: Perform layer-by-layer coordinate transformation on the rotational relationship between each frame of the three-frame four-axis stabilization platform to obtain the relationship between the sensor data on the frame and the sensor data on the platform.

[0160] For the physical constraints between the frames of a three-frame four-axis stabilization platform, the mathematical relationship between angular velocities is analyzed and combined with sensor measurements to propose a correlation relationship between sensor data. The scheme adopts a progressive transformation method, using the rotational relationship between the frames of the four-frame three-axis platform to complete the derivation of the relationship between the sensor data.

[0161] The specific steps are as follows:

[0162] Step 301, as follows Figure 5 As shown, four coordinate systems are established for the three-frame four-axis stable platform: OX1Y1Z1 is the platform coordinate system, OX2Y2Z2 is the inner frame coordinate system, OX3Y3Z3 is the outer frame coordinate system, and OX4Y4Z4 is the follower frame coordinate system.

[0163] Based on the platform's physical structure, it can be observed that the platform system and the inner frame system share only one Z-axis rotation angle, the inner frame system and the outer frame system share only one Y-axis rotation angle, and the outer frame system and the servo frame system share only one X-axis rotation angle. Since the sensors on each frame measure the absolute angular velocity within their respective systems, coordinate transformation can be used to establish a connection between the sensor data on each frame and the sensor data on the platform body.

[0164]

[0165] Step 302: Perform the first-level coordinate transformation between the platform system and the internal frame system;

[0166] The inner frame rotates only about the Z-axis of the truss system. Therefore, there is only a rotational relationship between the inner frame system and the truss system based on a Z-axis rotation angle. Rotating the truss system about the z1 axis by θ... z The coordinate transformation is as follows:

[0167]

[0168] in, These represent the absolute angular velocities of the platform system along the x, y, and z axes, respectively. The absolute angular velocities θ and y are the x-axis, y-axis, and z-axis of the inner frame system, respectively. z The z-axis rotation angle between the platform and the inner frame. The relative angular motion rate between the platform system and the inner frame system.

[0169] Since the sensor-related angular velocity relationships are all along the z1 axis, we only need to focus on the angular velocity relationships along the z1 axis:

[0170]

[0171] Substituting the sensor codes into the expression, we finally obtain the relationship between the sensor data on the frame and the sensor data on the platform:

[0172] B1-d(C1)=A1 (25)

[0173] Wherein, B1 corresponds to the signal output value of the redundant MEMS gyroscope B1 in the inner frame, A1 is the signal output value of the fiber optic gyroscope A1 on the z-axis, and d(C1) corresponds to the signal differential value of the grating ruler C1 on the z-axis.

[0174] Step 303: Perform the second-level coordinate transformation between the inner frame system and the outer frame system;

[0175] The outer frame rotates only about the Y-axis of the inner frame. Therefore, there is only a rotational relationship between the inner frame system and the platform system, with a rotation angle of θ around the y2 axis. y The coordinate transformation is as follows:

[0176]

[0177] in, These represent the absolute angular velocities along the x, y, and z axes of the outer frame system, respectively, and θ. y The y-axis rotation angle between the inner and outer frames. The relative angular motion rate between the inner frame system and the outer frame system.

[0178] Since the sensor-related angular velocity relationships are all along the y2 axis, we only need to focus on the angular velocity relationships along the y2 axis, and thus obtain the following equation:

[0179]

[0180] Substituting the sensor codes into the expression, we finally obtain the relationship between the sensor data on the frame and the sensor data on the platform:

[0181] B2-d(C2)=A3 sin(C1)+A2 cos(C1) (28)

[0182] Wherein, B2 corresponds to the signal output value of the redundant MEMS gyroscope B2 of the outer frame, d(C2) corresponds to the signal differential value of the y-axis grating ruler C2, A3 corresponds to the signal output value of the x-axis stage fiber optic gyroscope A3, A2 is the signal output value of the y-axis stage fiber optic gyroscope A2, and C1 is the signal output value of the z-axis grating ruler C1.

[0183] Step 304: Perform the third-level coordinate transformation between the outer frame system and the following frame system;

[0184] The follower frame rotates only about the X-axis of the outer frame; therefore, there is only a rotational relationship between the inner frame system and the platform system based on a single X-axis rotation angle. Rotating the outer frame system by θ about the x3 axis... x The coordinate transformation is as follows:

[0185]

[0186] in, The absolute angular velocities θ and θ of the servo frame system along the x, y, and z axes are respectively. x The x-axis rotation angle between the outer frame and the follower frame. The relative angular motion rate between the outer frame system and the follower frame system.

[0187] Since the sensor-related angular velocity relationships are all along the x3 axis, we only need to focus on the angular velocity relationships along the x3 axis, and thus obtain the following equation:

[0188]

[0189] Substituting the sensor codes, we finally obtain the relationship between the sensor data on the frame and the sensor data on the platform:

[0190] B3-d(C3)=cos(C2)cos(C1)A3-cos(C2)sin(C1)A2+sin(C2)(A1+d(C1)) (31)

[0191] Wherein, B3 is the signal output value of the redundant MEMS gyroscope B3 of the follower frame, d(C3) is the signal differential value of the grating ruler C3 along the x-axis, C2 is the signal output of the grating ruler C2 along the y-axis, and C3 is the signal output value of the grating ruler C3 along the x-axis.

[0192] Step 4: Establish the relationship between the sensor data on the frame and the sensor data on the platform, and pair it with the data from redundant sensors;

[0193] The fault-tolerant control and multi-sensor fusion fault diagnosis scheme, based on the aforementioned system, adds three MEMS gyroscopes in three directions on the platform for redundancy. D1, D2, and D3 serve as redundant sensors for A1, A2, and A3, respectively. Theoretically, the outputs of A and D should be identical, except for differences in accuracy. In summary, under normal platform operation, the following mathematical relationship should exist: For the fault location scheme, a voting mechanism is employed, combined with the platform's instability state, to complete the fault location.

[0194] After substituting the sensor data numbers into the angular velocity relationship, the relationships between the sensor data are as follows:

[0195]

[0196] Where D1 is the signal output value of z-axis redundant MEMS gyroscope D1, D2 is the signal output value of y-axis redundant MEMS gyroscope D2, and D3 is the signal output value of x-axis redundant MEMS gyroscope D3.

[0197] Step 5: Based on the data from redundant sensors, locate the fault and set up a signal replacement plan;

[0198] (1) First, focus on the first layer B1-d(C1) = A1 = D1. At this time, the fault diagnosis scheme is a single-axis diagnosis scheme, as follows:

[0199] If B1-d(C1)=D1≠A1, it indicates that fiber optic gyroscope A1 has malfunctioned. The D1 signal is used to replace the A1 signal to complete the instability recovery.

[0200] If B1-d(C1)=A1≠D1, it means that MEMS gyroscope D1 has malfunctioned. Since MEMS gyroscopes do not directly participate in the closed loop, there is no instability.

[0201] If B1-d(C1)≠A1=D1, then if the frame MEMS gyroscope B1 malfunctions, there will be no instability.

[0202] If the grating ruler C1 malfunctions, the motor will exhibit abnormal operating conditions:

[0203]

[0204] t0 is the starting point of the time integration.

[0205] As shown in Equation 12, the difference between the data from the frame MEMS gyroscope B1 and the data from the stage fiber optic gyroscope A1 can be integrated, and the initial integrated value when the grating ruler is active can be added to replace the C1 data. When a single sensor fails, the signal from the grating ruler C1 and the normal output of the angular velocity of gyroscopes A1 / D1 can still be guaranteed.

[0206] (2) Taking into account the data from the second and third layers as well

[0207]

[0208] a. If the gyroscopes A2 / A3 / D2 / D3 on the platform malfunction, then A3 sin(C1) + A2 cos(C1) ≠ D3 sin(C1) + D2 cos(C1).

[0209] If the platform becomes unstable, it indicates a malfunction in gyroscopes A2 / A3. In this case, the output signals of gyroscopes A2 and D2, and A3 and D3, are compared. The fiber optic gyroscope with unequal values ​​is the malfunctioning one. In this situation, D2 / D3 replaces the A2 / A3 signal to complete the instability recovery.

[0210] If the platform does not become unstable, it indicates that gyroscopes D2 / D3 are malfunctioning. In this case, compare the values ​​of A2 with D2 and D3 with D3. MEMS gyroscopes with unequal values ​​are malfunctioning.

[0211] b. The grating ruler C2 on the outer frame is faulty.

[0212] At this point, both the upper and lower equations in equation (35) are unequal, and the motor corresponding to grating ruler C2 is malfunctioning, causing the platform to become unstable. The data of grating ruler C2 is then replaced by data from other sensors.

[0213]

[0214] Here, the data from each discrete point is calculated and accumulated to complete the integration operation. The data of the grating ruler C2 when it is in normal condition is used as the integration starting point. The data from the four sensors on the right side of the above equation are calculated, accumulated, and added to the integration starting point to complete the replacement of the grating ruler C2 data.

[0215] c. The MEMS gyroscope B2 on the outer frame malfunctioned.

[0216] At this point, the above equation B2-d(C2)≠A3 sin(C1)+A2 cos(C1)=D3 sin(C1)+D2 cos(C1) remains unaffected and the platform does not become unstable, thus completing the fault location.

[0217] d. The MEMS gyroscope B3 and the grating ruler C3 on the servo frame are malfunctioning.

[0218] B3-d(C3)=cos(C2)cos(C1)A3-cos(C2)sin(C1)A2+sin(C2)(A1+d(C1)) (36)

[0219] Since gyroscopes C1, C2, A1, and A2 all have redundant designs, they are robust even if the first two layers fail. Therefore, when the above equation is not equal, it indicates that MEMS gyroscope B3 and grating ruler C3 on the servo frame have failed.

[0220] If the MEMS gyroscope B3 malfunctions, the platform will not become unstable, which can be used to locate the fault.

[0221] If the grating ruler C3 malfunctions, the platform will become unstable, and signal replacement can be performed.

[0222]

[0223] Here, the data from each discrete point is calculated and accumulated to complete the integration operation. The data of the grating ruler C3 when it is in normal condition is used as the integration starting point. The data from the six sensors on the right side of the above equation are calculated, accumulated, and added to the integration starting point to complete the replacement of the data of the grating ruler C3.

[0224] The above is a fault location and signal replacement scheme, which can complete the fault location and unstable signal recovery of all sensors. The number of damaged sensors in each layer is greater than or equal to 1, and the normal replacement of the C2 signal is guaranteed even if five sensors C1, C2, A1, A2, and A3 are damaged. The whole system has strong robustness.

[0225] In this invention, four sensors are equipped for each axis. The grating ruler and fiber optic gyroscope are essential sensors, while the MEMS gyroscopes on the stage and frame are redundant. The fiber optic gyroscope measures the angular velocity of the stage, and its integral value serves as the error in the control loop; therefore, a MEMS gyroscope with the same function as the fiber optic gyroscope is provided for redundancy. Although the MEMS gyroscope on the frame is also a redundant sensor, its function is not entirely the same as the other sensors. It not only plays a role in comprehensive fault diagnosis but also serves an indispensable role in signal replacement. The MEMS gyroscope on the frame is not merely a simple data backup redundancy; as a signal source, it can construct three consecutive equal equations, thus enabling fault diagnosis using a voting mechanism. Moreover, it is a cleverly designed and crucial sensor in this scheme.

[0226] (1) Replacement of fiber optic gyroscope failure

[0227] The hardware code enables signal replacement in case of fiber optic gyroscope or grating ruler failure. When the fiber optic gyroscope fails, switching the status identifier allows for timely connection of the MEMS gyroscope to the control loop, completing the closed loop. When the grating ruler fails, the angular velocity signal from the fiber optic gyroscope is integrated and superimposed on the zero point of the grating ruler data to restore the grating ruler's signal output, ensuring the motor's sector detection function is accurate.

[0228] This function relies on AT commands and is implemented through three main modules: fault injection, state switching, and controller parameter adjustment.

[0229] When simulating fiber optic gyroscope failure, it's crucial to avoid actually damaging the gyroscope; therefore, a fault signal injection method is employed. When a fiber optic gyroscope malfunctions, it's equivalent to superimposing erroneous data onto the normal output signal. Since this erroneous data can be decomposed into sine waves of different frequencies, the method of superimposing a sine wave deviation onto the normally measured data of the fiber optic gyroscope is chosen to inject the fault data. When the injected fault frequency is 0, it can be considered a mode-hopping fault in the fiber optic gyroscope.

[0230] The amplitude and frequency of the injected sinusoidal bias can be adjusted using AT commands, and the instability recovery procedure can be initiated. When the injected sinusoidal bias frequency is 0, the system displays a mode-hopping fault; when the injected sinusoidal bias frequency is not 0, the system displays a gyroscope jitter fault. By repeatedly injecting sinusoidal signals with different amplitudes and frequencies into the fiber optic gyroscope, the real-world scenario of a fiber optic gyroscope malfunction can be simulated.

[0231] The switching between the two gyroscopes is accomplished via status flags. After fault injection, because the fiber optic gyroscope outputs erroneous data, the control loop will issue erroneous instructions based on the erroneous data, causing the platform to become unstable. After a period of time, the status flags are changed, and a check on the status flags is added before the controller instruction function. After the flags are changed, the MEMS gyroscope is connected to the control loop as a sensor. This can simulate the brief instability and gyroscope switching process after the fiber optic gyroscope fails.

[0232] Although fiber optic gyroscopes and MEMS gyroscopes perform the same measurement task, the accuracy and sampling time of the two sensors are not exactly the same. Therefore, the system needs to be adapted to the characteristics of different gyroscopes and even different controller parameters. The specific parameters are adjusted appropriately based on experience during the experiment. This part of the task is also completed in AT commands to ensure that both types of gyroscopes have good control effects when connected to the control loop.

[0233] The above process simulates the instability of a fiber optic gyroscope. By injecting faults, causing brief instability, and switching states, the experimental conditions are made as close as possible to the real-world scenario. Simultaneously, sinusoidal functions with different amplitudes and frequencies are input to the fiber optic gyroscope to ensure the controller maintains platform stability under various external disturbances, enhancing the completeness of the experimental scheme.

[0234] (2) Replacement of faulty grating ruler

[0235] A redundant sensor system is used to achieve real-time, accurate detection and seamless recovery of grating ruler faults. It integrates multi-dimensional angle information provided by redundant sensors and strictly adheres to a pre-defined precise angle relationship model between sensors as the core criterion. Once an abnormal output of the grating ruler is detected, high-fidelity angle correction data is generated in real time by precisely integrating the angular velocity signals acquired by the highly reliable redundant sensors. This data dynamically replaces the failed grating ruler signal. To further improve the accuracy and smoothness of the recovered data, an optimized low-pass filtering algorithm is incorporated to effectively filter out noise interference and significantly optimize dynamic response characteristics. This closed-loop strategy not only ensures the continuity and accuracy of key measurement data but also maximizes the robustness and stable operation of the entire servo control system in the event of a fault, laying a solid foundation for the reliability and performance of the core equipment.

[0236] The formula for substituting grating ruler data is as follows:

[0237] d(C1)=B1-A1

[0238]

[0239] d(C2)=B2-A3 sin(C1)-A2 cos(C1)

[0240]

[0241] d(C3)=B3-cos(C2)cos(C1)A3+cos(C2)sin(C1)A2-sin(C2)(A1+d(C1))

[0242]

[0243] Optical scale signal replacement, such as Figure 5 As shown:

[0244] The system can read sensor data transmitted by the grating ruler and fiber optic gyroscope via the UART interface, and also stores millisecond-level timestamps internally within the microcontroller. Sensor data can be mapped one-to-one with each timestamp and stored for subsequent processing. A circular buffer is used for storage, continuously recording 500 sets of data within the last second, ensuring sufficient storage time for backtracking while controlling memory consumption.

[0245] The fault injection here is also accomplished using AT commands. For example... Figure 6As shown, when the fault command is issued, the fault injection timestamp is recorded as t0. There is a brief delay to simulate the platform instability process. After a period of time, the instability recovery process is initiated, and the time is recorded as t1. The nearest frame angle data point is then searched backwards from this time, and this point is used as the integration starting point, recording its timestamp t2. The nearest recorded angular velocity value is then searched for near t2, and the integration operation on the angular velocity begins from this point. Since the sampling times of the fiber optic gyroscope and the grating ruler are different, the nearest angular velocity can only be found near t2. The sampling time of the fiber optic gyroscope is 2ms, which introduces an angle error of less than or equal to 1ms. Due to the short time, this error can be ignored. The integration process uses rectangular integration, where the angular velocity data of the sampling point is multiplied by the time interval between this point and the next sampling point as the integration value. This rectangular integration method may cause a large integration error. The mean-filtered data value can be used as the integration quantity to reduce the small error caused by rectangular integration.

[0246] Throughout the entire data chain, timestamps are used throughout the entire computation process. Fault injection, backtracking the integration start point, and angular velocity integration are all executed precisely under the drive of timestamps. After finding several time start points, the angular velocity data is first integrated to the current moment, then superimposed on the frame angle integration start point, and this value is sent to the motor as the frame angle data. Subsequently, a frame angle update instruction is added to the angular velocity data storage function. Each time an angular velocity data is read, the frame angle data is updated to ensure data real-time performance.

[0247] The advantage of this invention lies in its effective solution to the error caused by incorrect grating ruler data through circular data storage and time backtracking. The angular velocity integration process can completely cover the instability process. By using time backtracking, it is ensured that the integration starting point is the correct grating ruler data, and the correct fiber optic gyroscope data is integrated from the vicinity of the grating ruler data. This achieves complete coverage of erroneous data, and the instability process will not affect subsequent data, improving data accuracy and ensuring the normal operation of the inertial platform's stabilization loop.

[0248] Step 6: Verify the substitute signal using a nonlinear integral sliding mode controller.

[0249] To verify the effectiveness and superiority of the redundant sensor system proposed in this invention, a verification experiment was conducted using a single-axis inertial platform. The experiment verified the instability recovery effect of the method in the event of fiber optic gyroscope failure and frame angle sensor failure, ensuring that the platform's stabilization loop has good robustness and guaranteeing the ability of the stable control system to recover from failures quickly.

[0250] The specific method is as follows:

[0251] To verify the controller's performance, a Simulink simulation circuit diagram was built. After parameter tuning, disturbance torque was injected into the platform to test the anti-interference torque effect of the instability recovery controller. At the same time, a PID controller was built and its performance was compared with that of a nonlinear integral sliding mode controller.

[0252] The parameters of the nonlinear integral sliding mode controller that can quickly complete instability recovery are as follows:

[0253] k p =40,k I =300, λ=573.6, ε=102, k=418.4

[0254] from Figure 7 It can be seen that the sliding mode controller outperforms the PID controller in terms of output torque tracking speed. Within just 0.1 seconds, the sliding mode controller can stabilize at the position of the disturbance torque, demonstrating its speed and accuracy in tracking disturbance torque. The comparison of the two controllers shows that the sliding mode controller can effectively suppress disturbances in the system, ensuring the system quickly recovers to the desired state, making it suitable for systems with rapid dynamic changes. Although the PID controller can also gradually adjust the torque, its response speed is slower, requiring a longer time to reach a stable state, resulting in a poorer control effect compared to the sliding mode controller. It is precisely the rapid response of the sliding mode controller to disturbance torque that leads to its smaller platform rotation angle.

[0255] from Figure 8 It can be seen that the sliding mode controller outperforms the PID controller in angle control, both in terms of response speed and peak angle control effect. Regarding response speed, the sliding mode controller can stabilize the platform at 0° in just 0.2 seconds, while the PID controller's response time is approximately 0.3 seconds, lagging behind. The platform angle is the parameter that has the greatest impact on navigation. In terms of peak angle, the sliding mode controller's control effect is an order of magnitude stronger than the PID controller. The sliding mode controller can better counteract the interference torque on the platform angle, demonstrating its superiority in this project.

[0256] For a redundant sensor system, four sensors are placed to ensure multiple acquisition methods for the two sets of signals. Simultaneously, multiple sensors can mutually decide on the faulty device and provide a switching scheme. Two sensors are placed on the platform: a high-precision fiber optic gyroscope and a low-precision MEMS gyroscope. Their output signals are denoted as... and A grating ruler is placed between the stage and the outer shell, and its output signal is denoted as α3. A MEMS gyroscope is placed on the outer shell, and its output signal is denoted as α3. in, This represents the angular velocity of the platform, which should normally be equal to the data of the two gyroscopes on the platform.

[0257] Under normal circumstances, the high-precision fiber optic gyroscope is connected to the control loop as an angular velocity sensor, and the grating ruler data is provided to the motor balance electrical zero point. If a fault occurs, the following judgment is used to make a decision and perform instability recovery.

[0258] a.

[0259] This indicates that the system is operating normally. High-precision fiber optic gyroscope data is then input into the control loop, and the integrated data is used as the system error to complete closed-loop control.

[0260] b.

[0261] This indicates that the MEMS gyroscope on the platform has malfunctioned. However, the control loop still uses high-precision fiber optic gyroscope data, and platform instability will not occur.

[0262] c.

[0263] This indicates that the fiber optic gyroscope on the platform has malfunctioned, which will cause the control loop to fail and the platform to become unstable. The solution is to disconnect the high-precision fiber optic gyroscope and connect a low-precision MEMS gyroscope to restore the control effect and complete the instability recovery.

[0264] d.

[0265] This indicates that the MEMS gyroscope or grating ruler on the platform is malfunctioning.

[0266] If the MEMS gyroscope on the platform malfunctions, the grating ruler will still provide an electrical zero point to the motor, complete sector judgment, and the control circuit will work normally.

[0267] If the grating ruler malfunctions, the motor will fail and the control circuit will fail. In this case, the original grating ruler data can be obtained by subtracting the data from the fiber optic gyroscope on the platform and integrating the data from the MEMS gyroscope on the outer shell. This data is then provided to the motor to ensure that the motor works normally and completes the instability recovery.

[0268] like Figure 9 As shown, in this simulated fiber optic gyroscope mode-hopping fault, after the instability signal is injected, a step-like signal deviation is superimposed on the fiber optic gyroscope data. Because the fiber optic gyroscope outputs an incorrect signal, the platform becomes unstable, and the frame angle changes. After a period of time, the angular velocity signal source is switched, and the MEMS gyroscope signal is connected to the closed-loop circuit. At this point, the platform begins to stabilize, and the frame angle returns to the equilibrium position. The entire instability recovery process is relatively smooth, and the controller returns the platform to its original position within 1 second after the signal switch.

[0269] Injecting sinusoidal and step faults into the frame angle signal measured by the grating ruler resulted in fluctuations in the frame angle data. After a period of time, an instability recovery process was initiated, tracing back to the moment when the frame angle was not unstable. Using this moment as the integration starting point, the angular velocity data measured by the fiber optic gyroscope was integrated and superimposed onto the grating ruler data at the integration starting point. The effectiveness of the angular velocity integration code was tested by manually cranking the stage. The blue line in the lower box of the figure represents the angular velocity integration value, showing its trend with angular velocity. This experiment verified the effectiveness of the grating ruler signal substitution code.

[0270] like Figure 10 As shown, step form error and sine error are applied to the frame angle data respectively. After the frame angle has erroneous data for a period of time, the instability recovery process is started, integral initialization is performed, and then the initial value of the frame angle and the integral of the angular velocity replace the grating ruler data to complete the instability recovery.

Claims

1. A method for recovering from instability in a redundant sensor system with a stable loop in an inertial measurement system, characterized in that, The specific steps are as follows: Step 1: Perform physical modeling analysis and design a nonlinear integral sliding mode controller for the three-frame four-axis stabilized platform; Step 2: Configure redundant sensors for the three-frame four-axis stabilization platform; The redundant sensors refer to three fiber optic gyroscopes, namely A1, A2 and A3, installed in the system along the three axes of roll axis, azimuth axis and pitch axis respectively. D1, D2 and D3 are redundant sensors corresponding to fiber optic gyroscopes A1, A2 and A3, which are installed in the same position. At the same time, three MEMS gyroscopes are placed in the z-axis of the inner frame, the y-axis of the outer frame and the x-axis of the follower frame. Step 3: Perform layer-by-layer coordinate transformation on the rotational relationship between each frame of the three-frame four-axis stabilization platform to obtain the relationship between the sensor data on the frame and the sensor data on the platform. Step 4: Pair the relationship between the sensor data on the frame and the sensor data on the platform with the data from the redundant sensors to obtain: Wherein, B1 corresponds to the signal output value of the inner frame redundant MEMS gyroscope B1, A1 is the signal output value of the stage's z-axis fiber optic gyroscope A1, and d(C1) corresponds to the signal differential value of the z-axis grating scale C1; D1 is the signal output value of the stage's z-axis redundant MEMS gyroscope D1, B2 corresponds to the signal output value of the outer frame redundant MEMS gyroscope B2, d(C2) corresponds to the signal differential value of the y-axis grating scale C2, A3 corresponds to the signal output value of the x-axis stage fiber optic gyroscope A3, and A2 is... The signal output value of the y-axis stage fiber optic gyroscope A2, C1 is the signal output value of the z-axis grating ruler C1; D2 is the signal output value of the stage y-axis redundant MEMS gyroscope D2, B3 is the signal output value of the follower frame redundant MEMS gyroscope B3, d(C3) is the signal differential value of the x-axis grating ruler C3, C2 is the signal output of the y-axis grating ruler C2, C3 is the signal output value of the x-axis grating ruler C3, and D3 is the signal output value of the stage x-axis redundant MEMS gyroscope D3. Step 5: Based on the data from redundant sensors, locate the fault and set up a signal replacement plan; Step 6: Verify the substitute signal using a nonlinear integral sliding mode controller.

2. The instability recovery method for a redundant sensor system with a stable loop in an inertial measurement system as described in claim 1, characterized in that, In step one, the modeling equation for the inertial platform is: Where J is the platform's moment of inertia. and These are the angular velocity and angular acceleration of the platform, respectively, K. m This is the gyroscope torque coefficient, R is the motor resistance, u is the control variable, and K is the control torque coefficient. b It is the back electromotive force coefficient, ω is the relative angular velocity between the platform and the gimbal, and T is the back electromotive force coefficient. c T represents the Coulomb friction torque. s ω represents the maximum static friction. s σ is the Stribeck angular velocity, σ is the coefficient of viscous friction, m is the mass of the platform, g is the acceleration due to gravity, and a is the angular velocity of the platform. z and a x These are the acceleration components along the z-axis and x-axis, respectively, θ b The angle between the gravitational acceleration vector and the radial velocity vector of the center of gravity about the platform is represented by r0, which is the distance between the center of mass of the inertial platform and the center of rotation. d It is an external random disturbance torque; The equations for the nonlinear integral sliding mode controller are: Where λ is the uncertainty error parameter, and e and Let k be the position error and its derivative. p k i k and ε are control parameters, and t is time. Let g(e) be the second derivative of the position quantity, g(e) be a nonlinear function, and s be a sliding surface function.

3. The instability recovery method for a redundant sensor system with a stable loop in an inertial measurement system as described in claim 1, characterized in that, The three-frame four-axis stabilization platform in step two includes an inner frame, an outer frame, and a follower frame. The three frames correspond to three axis systems plus the platform axis system, for a total of four axis systems. A sensor is placed on each frame, and the relationship between the sensor signals is derived by the rotational relationship between the axis systems. The innermost stage has three fiber optic gyroscopes, A1, A2, and A3, installed along the roll, azimuth, and pitch axes, respectively. The inner frame, which is directly connected to the stage, has a MEMS gyroscope B1 installed, the outer frame, one layer to the outside, has a MEMS gyroscope B2 installed, and the outermost servo frame has a MEMS gyroscope B3 installed. A grating ruler C1 is installed between the stage body and the inner frame to measure the relative angle between the stage system and the inner frame system in the roll axis direction; a grating ruler C2 is installed between the inner frame and the outer frame to measure the relative angle between the inner frame system and the outer frame system in the yaw axis direction; a grating ruler C3 is installed between the outer frame and the servo frame to measure the relative angle between the outer frame system and the servo system in the pitch axis direction; at the same time, three MEMS gyroscopes D1, D2, and D3 are added to the stage body as redundant sensors for fiber optic gyroscopes A1, A2, and A3, respectively.

4. The instability recovery method for a redundant sensor system with a stable loop in an inertial measurement system as described in claim 3, characterized in that, Step three specifically involves: Step 301: Establish four coordinate systems for the three-frame four-axis stable platform: OX1Y1Z1 is the platform coordinate system, OX2Y2Z2 is the inner frame coordinate system, OX3Y3Z3 is the outer frame coordinate system, and OX4Y4Z4 is the follower frame coordinate system. Step 302: Perform the first-level coordinate transformation between the platform system and the internal frame system; The inner frame rotates only about the Z-axis of the platform system, and the platform system rotates about the z1 axis by θ. z The coordinate transformation is as follows: in, These represent the absolute angular velocities of the platform system along the x, y, and z axes, respectively. These represent the absolute angular velocities along the x, y, and z axes of the inner frame system, respectively, θ. z Let Z be the z-axis rotation angle between the platform and the inner frame. The relative angular motion rate between the platform system and the inner frame system; The relationship of angular velocity along the z1 axis is as follows: The final relationship between the sensor data on the frame and the sensor data on the platform is obtained as follows: B1-d(C1)=A1 Step 303: Perform the second-level coordinate transformation between the inner frame system and the outer frame system; The outer frame rotates only about the Y-axis of the inner frame, and the inner frame is rotated about the y2 axis by θ. y The coordinate transformation is as follows: in, These represent the absolute angular velocities along the x, y, and z axes of the outer frame system, respectively, and θ. y The y-axis rotation angle between the inner and outer frames; The relative angular motion rate between the inner frame system and the outer frame system; Focusing on the angular velocity relationship along the y2 axis, we can obtain the following formula: The final relationship between the sensor data on the frame and the sensor data on the platform is obtained as follows: B2-d(C2)=A3 sin(C1)+A2 cos(C1) Step 304: Perform the third-level coordinate transformation between the outer frame system and the following frame system; The follower frame rotates only about the X-axis of the outer frame, while the outer frame is rotated about the x3 axis by θ. x The coordinate transformation is as follows: in, The absolute angular velocities θ and θ of the servo frame system along the x, y, and z axes are respectively. x The x-axis rotation angle between the outer frame and the follower frame; The relative angular motion rate between the outer frame system and the follower frame system; The relationship between the angular velocities along the x3 axis can be expressed as follows: The final relationship between the sensor data on the frame and the sensor data on the platform is obtained as follows: B3-d(C3)=cos(C2)cos(C1)A3-cos(C2)sin(C1)A2+sin(C2)(A1+d(C1)).

5. The instability recovery method for a redundant sensor system with a stable loop in an inertial measurement system as described in claim 4, characterized in that, Step five specifically involves: (1) First, focus on the angular velocity relationship between the platform system and the inner frame system: B1-d(C1)=A1=D1. At this time, the fault diagnosis scheme is a single-axis diagnosis scheme, as follows: If B1-d(C1)=D1≠A1, it indicates that fiber optic gyroscope A1 has malfunctioned. The D1 signal is used to replace the A1 signal to complete the instability recovery. If B1-d(C1)=A1≠D1, it means that MEMS gyroscope D1 has malfunctioned and is unstable. If B1-d(C1)≠A1=D1, then if the frame MEMS gyroscope B1 malfunctions, there will be no instability. If the grating ruler C1 malfunctions, the motor will exhibit abnormal operating conditions: d(C1)=B1-A1 t0 is the starting point of the time integration; The data of the frame MEMS gyroscope B1 is subtracted from the data of the stage fiber optic gyroscope A1 and then integrated. The initial value of the integration when the grating ruler is active is added to replace the data of C1. When a single sensor fails, the signal of the grating ruler C1 and the normal output of the angular velocity of gyroscopes A1 / D1 can still be guaranteed. (2) Taking into account the angular velocity relationships between the inner frame system and the outer frame system, as well as between the outer frame system and the servo frame system, we can further consider the angular velocity relationships between them. a. If the gyroscopes A2 / A3 / D2 / D3 on the platform malfunction, then A3 sin(C1)+A2cos(C1)≠D3 sin(C1)+D2cos(C1). If the platform becomes unstable, it indicates that gyroscope A2 / A3 is malfunctioning. At this time, the output signals of gyroscope A2 and D2, and A3 and D3 are compared. The fiber optic gyroscopes with unequal values ​​are malfunctioning. In this case, D2 / D3 replaces the A2 / A3 signals to complete the instability recovery. If the platform does not become unstable, it indicates that gyroscopes D2 / D3 are malfunctioning. In this case, compare the values ​​of A2 with D2 and D3 with D3. MEMS gyroscopes with unequal values ​​are malfunctioning. b. The grating ruler C2 on the outer frame is faulty; At this point, both the upper and lower equations are unequal, and the motor corresponding to the grating ruler C2 is malfunctioning, causing the platform to become unstable. The data from the grating ruler C2 is then replaced with data from other sensors. d(C2)=B2-A3 sin(C1)-A2 cos(C1) Here, the data from each discrete point is calculated and accumulated to complete the integration operation. The data of the grating ruler C2 when it is in normal condition is used as the integration starting point. The data from the four sensors on the right side of the above formula are calculated and accumulated, and then added to the integration starting point to complete the replacement of the data of the grating ruler C2. c. The MEMS gyroscope B2 on the outer frame malfunctioned. At this point, the above equation B2-d(C2)≠A3 sin(C1)+A2 cos(C1)=D3 sin(C1)+D2cos(C1) remains unaffected and the platform does not become unstable, thus completing the fault location. d. The MEMS gyroscope B3 and the grating ruler C3 on the servo frame are malfunctioning. B3-d(C3)=cos(C2)cos(C1)A3-cos(C2)sin(C1)A2+sin(C2)(A1+d(C1)) Since gyroscopes C1, C2, A1 and A2 all have redundant designs, they are robust even if the first two layers fail. Therefore, when the above equation is not equal, it indicates that MEMS gyroscope B3 and grating ruler C3 on the servo frame have failed. If the MEMS gyroscope B3 malfunctions, the platform will not become unstable, which can be used to locate the fault. If the grating ruler C3 malfunctions, the platform will become unstable, and signal replacement can be performed. d(C3)=B3-cos(C2)cos(C1)A3+cos(C2)sin(C1)A2-sin(C2)(A1+d(C1)) Here, the data from each discrete point is calculated and accumulated to complete the integration operation. The data of the grating ruler C3 when it is in normal condition is used as the integration starting point. The data from the six sensors on the right side of the above formula are calculated and accumulated, and then added to the integration starting point to complete the replacement of the data of the grating ruler C3.

6. The instability recovery method for a redundant sensor system with a stable loop in an inertial measurement system as described in claim 1, characterized in that, The device for recovering instability of a redundant sensor system based on a stabilization loop of an inertial measurement system includes a three-frame four-axis stabilization platform, a fiber optic gyroscope, a MEMS gyroscope, a grating ruler, an A / D converter, a digital low-pass filter, a controller, a D / A converter, and a torque motor. The three-frame four-axis platform is equipped with a set of fiber optic gyroscopes, two sets of redundant MEMS gyroscopes, and a set of grating rulers. The signals are converted into digital signals by an A / D converter, and then pass through a digital low-pass filter to remove high-frequency interference. The filtered digital signals are output to the controller, which performs accurate fault location of the above signals and completes signal replacement. The processed signals are output to a nonlinear integral sliding mode controller that can quickly complete instability recovery. The controller sends control commands to the motor via a D / A converter to complete the platform's instability recovery.

Images