A launch vehicle servo mechanism jam fault diagnosis method and system

Abstract

The application discloses a kind of launch vehicle servo mechanism jam fault diagnosis methods, including calculating swing angle deviation according to swing angle information;Servo swing angle telescopic sign is determined according to swing angle deviation;Whether the swing angle of servo mechanism is abnormal is judged according to servo swing angle telescopic sign;If the swing angle of servo mechanism is normal, it is considered that servo mechanism does not occur jam fault;If the swing angle of servo mechanism is abnormal, it is further judged whether the deviation of rocket body attitude angle is abnormal;If the deviation of rocket body attitude angle is normal, it is considered that servo mechanism does not occur jam fault;If the deviation of rocket body attitude angle is abnormal, it is considered that servo mechanism occurs jam fault.The application can realize the quick diagnosis servo mechanism jam fault, further realizes swing angle reconstruction, reaches the effect of stable control, high reliability of launch vehicle under fault condition.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of launch vehicle attitude control system, and particularly relates to a launch vehicle servo mechanism jam fault diagnosis method and system. BACKGROUND

[0002] The servo mechanism is an execution mechanism of the launch vehicle attitude control system, and the control system drives the swing of the engine nozzle through the extension and retraction of the servo mechanism, so as to realize the control of the pitch, yaw and roll attitude angles of the rocket during flight. Once the servo mechanism related fault occurs, the consequences will be fatal, which may cause a large attitude deviation, deviate from the predetermined orbit, or even lead to attitude instability. Therefore, the fault diagnosis of the servo fault is particularly important. If the servo fault can be accurately diagnosed and remedial measures are taken, the failure of the rocket can be prevented.

[0003] The servo mechanism jam fault is the main fault mode of the launch vehicle hydraulic servo mechanism. The existing technology has more researches on the control command redistribution after the occurrence of the jam fault, such as pseudo-inverse method, linear programming method, fixed point method, etc. However, the methods for diagnosing and detecting the jam fault are relatively less. Some scholars propose a reference model based on-servo system on-orbit fault diagnosis processing system and method. This method can diagnose many types of faults, but the diagnosis time is long and the efficiency is low. Some scholars propose an extended multiple model adaptive estimation fault method, but this method needs to establish various fault models, and the processing ability for unknown faults is general. SUMMARY

[0004] The present application aims to overcome the above-mentioned defects, and provides a launch vehicle servo mechanism jam fault diagnosis method and system. The present application solves the technical problems of difficult diagnosis of the servo mechanism jam fault and low diagnosis efficiency, and further leads to control failure. The present application can quickly diagnose the servo mechanism jam fault, further realizes the swing angle reconstruction, and achieves the effects of stable control of the launch vehicle under fault conditions and high reliability.

[0005] To achieve the above-mentioned application purposes, the present application provides the following technical solutions:

[0006] A launch vehicle servo mechanism jam fault diagnosis method, comprising:

[0007] calculating a swing angle deviation according to swing angle information; the swing angle information comprises a servo reference model output and a servo mechanism swing angle feedback;

[0008] determining a servo swing angle extension and retraction flag according to the swing angle deviation;

[0009] judging whether the servo mechanism swing angle is abnormal according to the servo swing angle extension and retraction flag;

[0010] If the servo mechanism swing angle is normal, it is considered that the servo mechanism does not have a jam fault.

[0011] If the servo mechanism swing angle is abnormal, it is further determined whether the missile attitude angle deviation is abnormal;

[0012] If the missile attitude angle deviation is normal, it is considered that the servo mechanism does not have a jamming failure;

[0013] If the missile attitude angle deviation is abnormal, it is considered that the servo mechanism has a jamming failure.

[0014] Further, the servo reference model output is the value output by the servo reference model after receiving the swing angle command; the servo reference model is a servo characteristic simulation transfer function;

[0015] The servo mechanism swing angle feedback is the actual output value of the servo mechanism after receiving the swing angle command, which is a sensor measurement value.

[0016] Further, the swing angle deviation = servo reference model output - swing angle feedback.

[0017] Further, the method for determining the servo swing angle expansion and contraction flag according to the swing angle deviation comprises:

[0018] The swing angle deviation is compared with a servo working threshold M1;

[0019] When the swing angle deviation > M1, the servo swing angle expansion and contraction flag is set to 1;

[0020] When the swing angle deviation < -M1, the servo swing angle expansion and contraction flag is set to -1;

[0021] When -M1≤ swing angle deviation ≤ M1, the servo swing angle expansion and contraction flag is set to 0.

[0022] Further, the method for determining whether the servo mechanism swing angle is abnormal according to the servo swing angle expansion and contraction flag comprises:

[0023] S1 in the current shot, it is determined whether the servo swing angle expansion and contraction flag is 0; if the servo swing angle expansion and contraction flag is 0, it is considered that the current shot servo mechanism swing angle is normal, and the next shot returns to step S1; if the servo swing angle expansion and contraction flag is not 0, it enters step S2;

[0024] S2 determines whether the servo swing angle expansion and contraction flag of the current shot and the servo swing angle expansion and contraction flags of the previous n consecutive shots are the same, if they are the same, it enters step S3, if they are different, it is considered that the current shot servo mechanism swing angle is normal, and the next shot returns to step S1;

[0025] S3 judges whether the servo mechanism swing angle feedbacks of the current beat and the continuous n beats before the current beat are all same; if the servo mechanism swing angle feedbacks of the current beat and the continuous n beats before the current beat are all same, it is considered that the servo mechanism swing angle of the current beat is abnormal; if the servo mechanism swing angle feedbacks of the current beat and the continuous n beats before the current beat are not all same, it is considered that the servo mechanism swing angle of the current beat is normal, and the next beat returns to step S1.

[0026] n is an integer greater than or equal to 2.

[0027] Further, in step S3, the method for judging whether the servo mechanism swing angle feedbacks of the current beat and the continuous n beats before the current beat are all same includes:

[0028] The servo mechanism swing angle feedback of the current beat is subtracted from the servo mechanism swing angle feedbacks of the continuous n beats before the current beat respectively, and it is judged whether the obtained each difference is less than a set threshold M2; when all the differences are less than the set threshold M2, it is considered that the servo mechanism swing angle feedbacks of the current beat and the continuous n beats before the current beat are all same, otherwise it is considered that the servo mechanism swing angle feedbacks of the current beat and the continuous n beats before the current beat are not all same.

[0029] Further, the method for judging whether the servo mechanism swing angle is abnormal according to the servo swing angle extension flag further includes:

[0030] In step S3, when it is considered that the servo mechanism swing angle of the current beat is abnormal, the fault count is added by 1, and it is judged whether the fault count exceeds a set threshold M3; when the fault count exceeds the set threshold M3, it is further judged whether the arrow body attitude angle deviation of the current beat is abnormal; when the fault count does not exceed the set threshold M3, the next beat returns to step S1.

[0031] When it is considered that the servo mechanism swing angle of the current beat is normal, the fault count is cleared.

[0032] Further, the method for further judging whether the arrow body attitude angle deviation is abnormal includes:

[0033] It is judged whether the arrow body attitude angle deviation of the current beat exceeds a set threshold M4; when it exceeds the set threshold M4, it is considered that the servo mechanism has a jamming fault, and the servo mechanism fault flag k err_sfi is set to 1; when it does not exceed the set threshold M4, it is considered that the servo mechanism does not have a jamming fault.

[0034] Further, the launch vehicle servo mechanism jamming fault diagnosis method further includes:

[0035] Real-time jamming fault diagnosis is performed on each servo mechanism; when there is a servo mechanism having a jamming fault, the swing angle is reconstructed, and the servo mechanism fault flag k err_sfiThe pseudo-inverse method is used to recalculate the swing angle command, and when there is a stuck fault of more than one servo mechanism, swing angle reconstruction is no longer performed.

[0036] A launch vehicle servo mechanism stuck fault diagnosis system is used to implement the launch vehicle servo mechanism stuck fault diagnosis method, and comprises a three-channel attitude control calculation module, a swing angle distribution module, a fault diagnosis module, a servo mechanism, an inertial measurement unit and a rate gyro measurement module; the fault diagnosis module comprises a servo reference model and a diagnosis module.

[0037] The three-channel attitude control calculation module obtains three-channel control signals according to attitude information, and outputs the three-channel control signals to the swing angle distribution module.

[0038] The swing angle distribution module obtains servo swing angle commands according to the three-channel control signals, and outputs the swing angle commands to the servo mechanism and the servo reference model in the fault diagnosis module.

[0039] The servo mechanism swings according to the swing angle commands, a sensor collects servo swing angles and feeds back to the diagnosis module, the servo swing angles are servo mechanism swing angle feedbacks, the inertial measurement unit and the rate gyro measurement module collect attitude information, and output the attitude information to the three-channel attitude control calculation module.

[0040] The servo reference model outputs information output by the servo reference model to the diagnosis module according to the swing angle commands.

[0041] The diagnosis module calculates swing angle deviations according to the servo reference model output and the servo mechanism swing angle feedbacks, determines servo swing angle expansion and contraction flags according to the swing angle deviations, judges whether the servo mechanism swing angle feedbacks are abnormal according to the servo swing angle expansion and contraction flags, if the servo mechanism swing angle feedbacks are normal, it is considered that the servo mechanism does not have a stuck fault, if the servo mechanism swing angle feedbacks are abnormal, it is further judged whether the missile body attitude angle deviations are abnormal, if the missile body attitude angle deviations are normal, it is considered that the servo mechanism does not have a stuck fault, and if the missile body attitude angle deviations are abnormal, it is considered that the servo mechanism has a stuck fault.

[0042] Compared with the prior art, the present application has at least one of the following beneficial effects:

[0043] (1) The present application creatively proposes a launch vehicle servo mechanism stuck fault diagnosis method, which realizes real-time diagnosis of the servo mechanism stuck fault by using the expansion and contraction flag, and the diagnosis structure is accurate and reliable.

[0044] (2) The expansion and contraction flag calculation process of the present application is similar to the servo working principle, the servo stuck fault diagnosis increases the servo expansion and contraction flag calculation and swing angle action judgment process, and effectively reduces the misjudgment rate.

[0045] (3) The present application designs a continuous multiple shot judgment form, which further improves the reliability of the diagnosis result.

[0046] (4) The application adds the judgment of missile attitude angle anomaly on the basis of swing angle anomaly, which can effectively prevent misjudgment;

[0047] (5) Since the fault mode is strong in pertinence and the diagnosis time is less, the method can early perform fault reconstruction, reduce the attitude deviation caused by servo fault to the system, and even prevent the divergence of attitude. BRIEF DESCRIPTION OF DRAWINGS

[0048] Figure 1 is a servo extension flag calculation schematic diagram in the embodiment of the application;

[0049] Figure 2 is a fault diagnosis flowchart in the embodiment of the application;

[0050] Figure 3 is a control principle block diagram of a launch vehicle in the embodiment of the application. DETAILED DESCRIPTION

[0051] The characteristics and advantages of the application will become more clear and explicit with the following detailed description of the application.

[0052] The special word "exemplary" here means "as an example, embodiment or illustration". Any embodiment described as "exemplary" here is not necessarily interpreted as superior or better than other embodiments. Although various aspects of the embodiments are shown in the drawings, the drawings are not necessarily drawn to scale unless specifically indicated.

[0053] The application provides a launch vehicle servo mechanism stuck fault diagnosis method, which can quickly and accurately diagnose the servo stuck fault during the flight of the rocket, and perform swing angle reconstruction to prevent the divergence of the rocket attitude due to the failure of swing angle control, and realize the stable control of the launch vehicle under the fault condition.

[0054] The launch vehicle servo mechanism stuck fault diagnosis method of the application comprises the following steps:

[0055] 1) Calculate the servo swing angle extension flag according to the swing angle information;

[0056] 2) In the case of continuous extension of the extension flag, judge whether the servo swing angle moves or not, and no movement is considered abnormal;

[0057] 3) Further judge whether the current missile attitude angle deviation is abnormal, and if abnormal, determine that the servo mechanism has stuck fault;

[0058] 4) If the servo fails, perform swing angle reconstruction, and use the pseudo-inverse method to recalculate the swing angle distribution command;

[0059] In one embodiment, in step 1), the servo mechanism swing angle deviation is calculated according to the swing angle information, and the calculation formula is: swing angle deviation = servo reference model output - swing angle feedback; wherein the swing angle feedback refers to the actual output value of the servo swing angle, which is obtained by sensor measurement; the servo reference model is a servo characteristic simulation transfer function, and the servo reference model output is the value of the swing angle command output by the servo reference model, and the calculation formula is as follows:

[0060] F1α(nT filter ) = α

[0061]

[0062]

[0063] Wherein:

[0064] is the servo reference model transfer function coefficient;

[0065] α is the servo swing angle command;

[0066] n is the servo reference model transfer function order;

[0067] is the current swing servo reference model output value;

[0068] T filter is the filter calculation period, which is the same as the control period.

[0069] According to the working principle of the servo mechanism, when the servo mechanism is working normally, the deviation of the servo swing angle and the swing angle control command is the swing angle deviation, which is used as the input of the servo power amplifier. The power amplifier converts the swing angle deviation into voltage to drive the servo mechanism to extend or retract, so as to realize the consistency of the swing angle output and the command. If the absolute value of the swing angle deviation is greater than the servo working threshold, the servo mechanism extends or retracts; if the absolute value of the swing angle deviation is less than the threshold, the servo mechanism does not extend or retract.

[0070] After calculating the swing angle deviation, the swing angle deviation is compared with the servo working threshold M1 to determine the swing extension flag, and the determination process is shown in Figure 1 : if the swing angle deviation is greater than the threshold M1, the servo extension flag is set to 1, representing "extension"; if the swing angle deviation is less than -M1, the servo extension flag is set to -1, representing "retraction"; if the swing angle deviation is between -M1 and M1, the servo extension flag is set to 0, representing no "extension" or "retraction". The calculated servo extension flag is used as the input of step 2).

[0071] In one specific embodiment, in step 2), when the telescopic flag has a telescopic condition (the telescopic flag is not 0), it is judged whether the swing angle is continuously inaction for a certain number of shots. According to the fault diagnosis flow chart, firstly, it is judged that the swing angle telescopic flag is not 0, which indicates that the telescopic flag is "extension" or "retraction". Then, it is judged whether the next shot telescopic flag is the same as the last shot, and the servo swing angle feedback value is recorded. If the telescopic command is continuously extended or retracted for a plurality of shots, it is represented that the swing angle command continuously outputs a fixed command. Then, it is judged whether the swing angle feedback value changes. If the swing angle feedback value continuously remains unchanged or the change value is less than a set threshold, it is represented that the swing angle is inaction, and it is considered that the current servo mechanism is abnormal. The continuous shot number is a design parameter. If there is one shot discontinuity in the middle, the fault count is cleared.

[0072] In one specific embodiment, in step 3), if the swing angle is judged to be abnormal in step 2), it is further diagnosed whether the missile attitude angle deviation is abnormal. The purpose is to prevent misjudgment and add a layer of protection to the diagnosis. When the fault count is greater than a set threshold, it is judged whether the current missile attitude angle deviation exceeds the attitude deviation threshold. If it does not exceed, it is considered that no servo fault occurs. If it exceeds the threshold, it is determined that a certain servo mechanism has a stuck fault, the servo mechanism fault flag k err_sfi is set to 1, and the fault flag is used as the input of the swing angle reconstruction in step 4).

[0073] In one specific embodiment, in step 4), if the servo fault flag is 1, the swing angle reconstruction is performed, and the pseudo-inverse method is used to recalculate the swing angle distribution command. When the number of servo mechanisms with faults is greater than one, the swing angle reconstruction is no longer performed.

[0074] The swing angle distribution command calculation formula is as follows:

[0075] k sf4_err =[1-k err_sf1 1-k err_sf2 1-k err_sf3 1-k err_sf4 ]

[0076] H sf4_err =H sf4_zc .*(ones(3,1)*k sf4_err )

[0077] F sf4_err =H sf4_err T (H sf4_err H sf4_err T ) -1

[0078] δ=F sf4_err *u

[0079] Wherein:

[0080] k sf4_err is a servo fault flag vector, 1 x 4 dimension;

[0081] H sf4_zc is a normal servo mechanism synthesis matrix, constant, 3 x 4 dimension;

[0082] H sf4_err is a servo mechanism synthesis matrix under servo fault, 3 x 4 dimension;

[0083] F sf4_err is a control distribution matrix after servo fault, 4 x 3 dimension.

[0084] u matrix is an equivalent swing angle matrix;

[0085] δ is a servo swing angle command;

[0086] k err_sf1 , k err_sf2 , k err_sf3 , k err_sf4 are four servo mechanism jamming fault flags respectively;

[0087] Embodiment:

[0088] The method adopted in this embodiment can be divided into the following five steps:

[0089] 1) Create a launch vehicle simulation model, i.e. a launch vehicle servo mechanism jamming fault diagnosis system;

[0090] Take a certain liquid launch vehicle as an example, which is equipped with four servo mechanisms. According to the servo mechanism configuration of the launch vehicle, the servo mechanism synthesis matrix is calculated. Figure 3 Create a launch vehicle simulation model. The launch vehicle is controlled in three channels of pitch, yaw and roll by the missile machine, the servo mechanism swing angle command is calculated through the swing angle distribution matrix, the servo mechanism swings following the swing angle command, the servo swing angle is obtained through the dynamics model to get the launch vehicle attitude information, the attitude information is sampled through the inertial measurement unit and rate gyro to be input to the missile machine for three-channel attitude calculation. The servo fault diagnosis module is included in the launch vehicle simulation model, including servo reference model calculation, servo fault diagnosis and swing angle reconstruction calculation functions.

[0091] 2) Calculate the servo swing angle expansion flag according to the swing angle information;

[0092] The swing angle deviation calculation formula is: swing angle deviation = servo reference model output - swing angle feedback; wherein the servo reference model is a servo characteristic simulation transfer function, and the output is the value of the swing angle command output through the servo reference model; the swing angle feedback refers to the actual output value of the servo swing angle, which is the sensor measurement value.

[0093] After calculating the swing angle deviation, compare the swing angle deviation with the servo working threshold M1 to determine the swing angle extension / retraction indicator. The determination process is as follows: Figure 1 If the swing angle deviation is greater than the threshold M1, the servo extension flag is set to 1, representing "extension"; if the swing angle deviation is less than -M1, the servo extension flag is set to -1, representing "retraction"; if the swing angle deviation is between -M1 and M1, the servo extension flag is set to 0, representing no "extension" or "retraction".

[0094] 3) When the telescopic sign continues to extend or retract, determine whether the swing angle moves; if there is no movement, it is considered abnormal.

[0095] according to Figure 2 The fault diagnosis flowchart first checks if the swing angle extension / retraction flag is not 0, indicating that the flag is either "extended" or "retracted." Next, it checks if the extension / retraction flag in the next frame is the same as the previous frame and records the servo swing angle feedback value. If multiple consecutive frames show the same extension or retraction command, it means the swing angle command is continuously outputting a fixed command. In this case, it checks if the swing angle feedback value changes. If the swing angle feedback value remains unchanged for multiple consecutive frames or the change is less than a set threshold, it means there is no swing angle movement, and the current servo mechanism is considered abnormal. The number of consecutive frames is a design parameter. If there is one discontinuous frame, it indicates that it is not a stuck fault, and the fault count is reset to zero.

[0096] 4) If the previous step determined that the swing angle was abnormal, then further determine whether the current attitude angle deviation of the rocket body is abnormal. If it is abnormal, then it is determined that the servo mechanism has a jamming fault.

[0097] When the fault count in step 3 exceeds the set threshold, it is determined whether the current rocket body attitude angle deviation exceeds the threshold. If it does not exceed the threshold, it is considered that no servo fault has occurred. If it exceeds the threshold, it is determined that a certain servo mechanism has a jamming fault, and the fault flag k of that servo mechanism is set. err_sfi The value is 1, and this flag serves as the input for the next step of swing angle reconstruction.

[0098] 5) If a servo malfunctions, the swing angle will be recalculated using a pseudo-inverse method. If the number of servo mechanisms malfunctioning is greater than one, the swing angle will not be recalculated.

[0099] The formula for calculating the swing angle allocation instruction is as follows:

[0100] k sf4_err =[1-k err_sf1 1-k err_sf2 1-k err_sf3 1-k err_sf4 ]

[0101] H sf4_err =H sf4_zc .*(ones(3,1)*k sf4_err )

[0102] F sf4_err = H sf4_err T (H sf4_err H sf4_err T ) -1

[0103] delta = F sf4_err *u

[0104] wherein:

[0105] k sf4_err is a servo fault flag vector, 1x4 dimension;

[0106] H sf4_zc is a normal servo mechanism synthesis matrix, constant, 3x4 dimension;

[0107] H sf4_err is a servo mechanism synthesis matrix under servo fault, 3x4 dimension;

[0108] F sf4_err is a control distribution matrix after servo fault, 4x3 dimension.

[0109] u matrix is an equivalent swing angle matrix;

[0110] delta is a servo swing angle command.

[0111] The present application is described in detail above in conjunction with specific embodiments and exemplary examples, but these descriptions cannot be understood as limiting the present application. Those skilled in the art understand that the technical solutions and embodiments of the present application can be variously replaced, modified or improved without deviating from the spirit and scope of the present application, and these all fall within the scope of the present application. The scope of protection of the present application is subject to the appended claims.

[0112] The contents not described in detail in the specification of the present application are the known technology of those skilled in the art.

Claims

1. A launch vehicle servo mechanism jam fault diagnosis method, characterized by, The method comprises the following steps: According to the swing angle information, a swing angle deviation is calculated; the swing angle information comprises a servo reference model output and a servo mechanism swing angle feedback; According to the swing angle deviation, a servo swing angle telescopic flag is determined; According to the servo swing angle telescopic flag, whether the servo mechanism swing angle is abnormal is judged; If the servo mechanism swing angle is normal, it is considered that the servo mechanism does not have a stuck fault; If the servo mechanism swing angle is abnormal, it is further judged whether the missile attitude angle deviation is abnormal; If the missile attitude angle deviation is normal, it is considered that the servo mechanism does not have a stuck fault; If the missile attitude angle deviation is abnormal, it is considered that the servo mechanism has a stuck fault; The method for determining the servo swing angle telescopic flag according to the swing angle deviation comprises the following steps: The swing angle deviation is compared with a servo working threshold M1; When the swing angle deviation > M1, the servo swing angle telescopic flag is set as 1; When the swing angle deviation < -M1, the servo swing angle telescopic flag is set as -1; When -M1 <= swing angle deviation <= M1, the servo swing angle telescopic flag is set as 0; The method for judging whether the servo mechanism swing angle is abnormal according to the servo swing angle telescopic flag comprises the following steps: S1: in the current beat, it is judged whether the servo swing angle telescopic flag is 0; if the servo swing angle telescopic flag is 0, it is considered that the servo mechanism swing angle in the current beat is normal, and the next beat returns to step S1; if the servo swing angle telescopic flag is not 0, step S2 is entered; S2: it is judged whether the servo swing angle telescopic flag in the current beat and the servo swing angle telescopic flags in the previous n beats are all same; if they are same, step S3 is entered; if they are different, it is considered that the servo mechanism swing angle in the current beat is normal, and the next beat returns to step S1; S3: it is judged whether the servo mechanism swing angle feedbacks in the current beat and the previous n beats are all same; if they are all same, it is considered that the servo mechanism swing angle in the current beat is abnormal; if they are not all same, it is considered that the servo mechanism swing angle in the current beat is normal, and the next beat returns to step S1; n is an integer greater than or equal to 2.

2. The method of claim 1, wherein, The servo reference model output is a value output by a servo reference model after receiving a swing angle instruction; the servo reference model is a servo characteristic simulation transfer function; The servo mechanism swing angle feedback is an actual output value of a servo mechanism after receiving a swing angle instruction, which is a sensor measurement value.

3. The method of claim 2, wherein, The swing angle deviation = servo reference model output - swing angle feedback.

4. The method of claim 1, wherein, In step S3, the method for judging whether the servo mechanism swing angle feedbacks in the current beat and the previous n beats are all same comprises the following steps: The servo mechanism swing angle feedback in the current beat is subtracted from the servo mechanism swing angle feedbacks in the previous n beats respectively, and it is judged whether the obtained difference values are all less than a set threshold M2; when they are all less than the set threshold M2, it is considered that the servo mechanism swing angle feedbacks in the current beat and the previous n beats are all same; otherwise, it is considered that the servo mechanism swing angle feedbacks in the current beat and the previous n beats are not all same.

5. The method of claim 1, wherein, The method for judging whether the servo mechanism swing angle is abnormal according to the servo swing angle telescopic flag further comprises the following steps: In step S3, when the current servo mechanism swing angle is considered abnormal, the fault count is incremented by 1, and it is determined whether the fault count exceeds the set threshold M3; when the fault count exceeds the set threshold M3, it is further determined whether the current missile body attitude angle deviation is abnormal; when the fault count does not exceed the set threshold M3, the next time the step S1 is returned. When the current servo mechanism swing angle is considered normal, the fault count is cleared.

6. The method of claim 5, wherein, The method for further determining whether the missile body attitude angle deviation is abnormal comprises: determines whether the current arrow body posture angle deviation exceeds a set threshold M4. If it exceeds the set threshold M4, it is considered that the servo mechanism has a jamming failure, and the servo mechanism failure flag k is set to 1. If it does not exceed the set threshold M4, it is considered that the servo mechanism does not have a jamming failure. err_sfi is 1, and the servo mechanism failure flag k is set to 0.

7. The method of claim 6, wherein, The launch vehicle servo mechanism stuck fault diagnosis method further comprises: Real-time stuck fault diagnosis is performed on each servo mechanism. When stuck fault occurs in one servo mechanism, the yaw angle is reconstructed. According to the servo mechanism fault flag k err_sfi The pseudo-inverse method is used to recalculate the yaw angle command. When stuck fault occurs in more than one servo mechanism, the yaw angle is no longer reconstructed.

8. A launch vehicle servo mechanism jam fault diagnosis system, comprising: The launch vehicle servo mechanism stuck fault diagnosis method for realizing any one of claims 1-7 comprises a three-channel attitude control calculation module, a swing angle distribution module, a fault diagnosis module, a servo mechanism, an inertial measurement unit and a rate gyro measurement module; the fault diagnosis module comprises a servo reference model and a diagnosis module; The three-channel attitude control calculation module obtains three-channel control signals according to the attitude information, and outputs the three-channel control signals to the swing angle distribution module; The swing angle distribution module obtains each servo swing angle command according to the three-channel control signals, and outputs the swing angle command to the servo mechanism and the servo reference model in the fault diagnosis module; The servo mechanism swings according to the swing angle command, the sensor collects the servo swing angle and feeds back to the diagnosis module, and the servo swing angle is the servo mechanism swing angle feedback; the inertial measurement unit and the rate gyro measurement module collect the attitude information and output the attitude information to the three-channel attitude control calculation module; The servo reference model outputs information according to the swing angle command output by the diagnosis module, i.e. the servo reference model output; The diagnosis module calculates the swing angle deviation according to the servo reference model output and the servo mechanism swing angle feedback; determines the servo swing angle expansion and contraction flag according to the swing angle deviation; determines whether the servo mechanism swing angle is abnormal according to the servo swing angle expansion and contraction flag; if the servo mechanism swing angle is normal, it is considered that the servo mechanism does not occur stuck fault; if the servo mechanism swing angle is abnormal, it is further determined whether the missile body attitude angle deviation is abnormal; if the missile body attitude angle deviation is normal, it is considered that the servo mechanism does not occur stuck fault; if the missile body attitude angle deviation is abnormal, it is considered that the servo mechanism occurs stuck fault.

Images