Single - turn absolute encoder speed measurement and processing method and system

Abstract

The present invention relates to the field of industrial information automation technology, and particularly relates to a method and system for measuring and processing the speed of a single-turn absolute encoder, including the steps of: S1, initializing the parameters of the single-turn absolute encoder; S2, after initializing the parameters, the encoder continuously outputs angular positions at fixed time intervals, and differentiates the angular positions output twice continuously to obtain the angular velocity; S3, according to the angular velocity obtained in step S2, identifying and correcting the abnormal angular velocity at the zero-crossing of the encoder angular position to obtain the corrected angular velocity; S4, identifying and processing the outliers of the corrected angular velocity in step S3. The present invention also relates to a system for measuring and processing the speed of a single-turn absolute encoder. The present invention can obtain a relatively smooth and accurate angular velocity without additional hardware.

Description

Technical Field

[0001] The present invention relates to the technical field of industrial information automation, and particularly relates to a method and system for measuring and processing the speed of a single-turn absolute encoder. Background Art

[0002] An encoder is a precision displacement measurement sensor that can convert angular displacement or linear displacement into an electrical signal. The former is called a rotary encoder, and the latter is called a linear encoder. Encoders are very important for systems that require precise control of position or speed and have been widely used in the technical fields of industrial information automation such as numerical control machine tools, robots, medical devices, and aerospace.

[0003] Rotary encoders can be roughly divided into two categories: incremental encoders and absolute encoders according to different characteristics. The incremental encoder outputs two-channel quadrature encoding pulses and a zero-phase pulse. The speed magnitude and direction can be obtained by counting the two-channel quadrature pulses and judging the phase pulse. Its advantage is that it is easy to miniaturize, and its disadvantage is that the initial positioning is prone to data loss after power-off. The absolute encoder outputs a binary digital signal, and each angular position has a unique code. The position information will not be lost after the system is powered on and off again, and it has stronger anti-interference ability and is usually applied to systems with more precise requirements for position and speed.

[0004] The angular measurement range of a single-turn absolute encoder is 0 to 360 degrees. When the rotation exceeds 360 degrees, the angle value returns to the origin zero degree. When using the differential method to obtain the speed, there is a large fluctuation at the zero position of the encoder, resulting in discontinuous and inaccurate speed measurement by the differential method. To obtain a continuous and reliable speed, a gyroscope can be installed on the same axis system of the encoder, but this will not only increase the structural volume and weight but also increase the additional cost, which is unacceptable in many cases.

[0005] If only a single-turn absolute encoder is used for speed measurement, especially in scenarios where the encoder needs to pass through the zero point of the rotating shaft system and even needs multi-turn continuous rotation, the continuity and reliability of speed measurement must be considered. Summary of the Invention

[0006] In view of this, it is necessary to provide a method and system for measuring and processing the speed of a single-turn absolute encoder, which can obtain a relatively smooth and accurate angular velocity without additional hardware.

[0007] The present invention provides a method for speed measurement and processing of a single-turn absolute encoder. The method includes the following steps: S1, initializing the parameters of the single-turn absolute encoder; S2, after initializing the parameters, the encoder continuously outputs angular positions at fixed time intervals, and differentiating the angular positions output twice continuously to obtain the angular velocity; S3, according to the angular velocity obtained in step S2, identifying and correcting the abnormal angular velocity at the zero-crossing of the encoder angular position to obtain the corrected angular velocity; S4, identifying and processing the outliers of the corrected angular velocity in step S3.

[0008] Preferably, step S1 includes:

[0009] Defining the data output time interval of the single-turn absolute encoder as T;

[0010] Defining the angle value output in real time by the single-turn absolute encoder as and initializing it to zero, that is, The value range is ;

[0011] Defining the angular positions of two consecutive differentiations in step S2 as the current moment angular position and the previous moment angular position and initializing them to zero, that is, , ;

[0012] Defining the current moment angular position temporary variable of the single-turn absolute encoder and initializing it to zero, that is, ;

[0013] Initializing the signed integer data N with abnormal differential angular velocity at the zero-crossing of the single-turn absolute encoder angular position to zero, that is, N = 0;

[0014] Defining the threshold value of the differential angular velocity abnormality at the zero-crossing of the single-turn absolute encoder angular position as and initializing according to the maximum rotational speed of the actual encoder applied to the device;

[0015] Defining the angular velocity obtained in step S2 as and initializing it to zero, that is, ;

[0016] Defining the corrected angular velocity in step S3 as and initializing it to zero, that is, ;

[0017] Defining the angular velocity The count value of continuous small wild values ​​is M, and is initialized to zero, that is, M=0;

[0018] Define the angular velocity after processing in step S4 as , and initialized to zero, that is .

[0019] Preferably, step S2 comprises:

[0020] Calculate the temporary variable of the current angular position ,Right now ;

[0021] The angular velocity is calculated by the difference method ,Right now .

[0022] Preferably, step S3 comprises:

[0023] like and , then increase the N by 1, that is, N=N+1;

[0024] like and , then subtract 1 from N, that is, N=N-1;

[0025] Update and correct the current angular position ,Right now ;

[0026] Update and correct the angular velocity by difference method ,Right now ;

[0027] Update the previous moment angular position ,Right now .

[0028] Preferably, the step S4 comprises:

[0029] like , then the current angular velocity For abnormal wild values, directly change the current Eliminate and update the angular velocity ,Right now ;

[0030] like , and meets the conditions , then determine the current angular velocity At least isolated small outliers, the current Eliminate and update the angular velocity ,Right now , while incrementing the value of M by 1, i.e., M = M + 1, then determine whether M is greater than or equal to 3: If , then determine that the current angular velocity is a patch micro outlier value, and recalibrate the angular velocity , update the angular velocity , i.e., , while setting the value of M to zero, i.e., M = 0;

[0031] If , and the condition is satisfied, then determine that the current angular velocity is a normal value, and update the angular velocity , i.e., , while setting the value of M to zero, i.e., M = 0.

[0032] The present invention also provides a single - turn absolute encoder speed measurement and processing system, which includes an initialization module, a differential module, a correction module, and a processing module, where:

[0033] The initialization module is used to initialize the parameters of the single - turn absolute encoder;

[0034] The differential module is used to continuously output angular positions of the encoder at a fixed time interval after initializing the parameters, and perform a difference on the angular positions output continuously twice to obtain the angular velocity;

[0035] The correction module is used to identify and correct abnormal angular velocities at the zero - crossing of the encoder angular position according to the angular velocity obtained by the differential module to obtain the corrected angular velocity;

[0036] The processing module is used to identify and process outliers of the angular velocity corrected by the correction module.

[0037] Preferably, the initialization module is specifically used for:

[0038] Define the data output time interval of the single - turn absolute encoder as T;

[0039] Define the angle value output in real - time by the single - turn absolute encoder as , and initialize it to zero, i.e., , and its value is updated according to the encoder angular position output data at time interval T, and the value range is ;

[0040] Define the angular positions of two consecutive differences of the differential module as the current - moment angular position and the previous - moment angular position , and initialize them to zero, i.e., , ;

[0041] Define a temporary variable for the angular position of the single-turn absolute encoder at the current moment , and initialize it to zero, that is ;

[0042] Initialize the signed integer data N of the differential angular velocity anomaly at the zero-crossing of the angular position of the single-turn absolute encoder to zero, that is, N = 0;

[0043] Define the threshold of the differential angular velocity anomaly at the zero-crossing of the angular position of the single-turn absolute encoder as , and according to the maximum rotational speed at which the actual encoder is applied to the device, initialize ;

[0044] Define the angular velocity obtained by the differential module as , and initialize it to zero, that is ;

[0045] Define the angular velocity after correction by the correction module as , and initialize it to zero, that is ;

[0046] Define the angular velocity of the correction module, the continuous small outlier count value is M, and initialize it to zero, that is, M = 0;

[0047] Define the angular velocity after processing by the processing module as , and initialize it to zero, that is .

[0048] Preferably, the differential module is specifically used for:

[0049] Calculate the temporary variable of the angular position at the current moment , that is ;

[0050] Calculate the angular velocity by the differential method, that is .

[0051] Preferably, the correction module is specifically used for:

[0052] If and , then increment the N by 1, that is, N = N + 1;

[0053] If and , then decrement the N by 1, that is, N = N - 1;

[0054] Update and correct the angular position at the current moment , that is ;

[0055] Update and correct the angular velocity by the difference method , that is ;

[0056] Update the angular position at the previous moment , that is .

[0057] Preferably, the processing module is specifically configured to:

[0058] If , then the current angular velocity is an abnormal outlier, directly remove the current , and update the angular velocity , that is ;

[0059] If , and the condition is satisfied, then determine that the current angular velocity is at least an isolated small outlier, remove the current , and update the angular velocity , that is , at the same time increase the M by 1, that is M = M + 1, and then determine whether the M is greater than or equal to 3: If , then determine that the current angular velocity is a patch small outlier, re - correct the angular velocity , update the angular velocity , that is , and at the same time set the M value to zero, that is M = 0;

[0060] If , and the condition is satisfied, then determine that the current angular velocity is a normal value, and update the angular velocity , that is , and at the same time set the M value to zero, that is M = 0.

[0061] This application can not only accurately identify the zero - crossing state of the encoder angular position and correct the differential speed, but also identify and process the encoder noise outliers, so as to obtain a relatively smooth and accurate angular velocity without additional hardware. This method is widely applicable to the differential speed measurement of absolute encoders in the field of industrial information automation technology, especially applicable to the continuous differential speed measurement of single - turn absolute encoders. Brief Description of the Drawings

[0062] Figure 1 is a flowchart of the single - turn absolute encoder speed measurement and processing method of the present invention;

[0063] Figure 2 It is a schematic flowchart of the speed measurement and processing method of the single-turn absolute encoder provided by the embodiment of the present invention;

[0064] Figure 3 It is a hardware architecture diagram of the single-turn absolute encoder speed measurement and processing system of the present invention. Specific Embodiments

[0065] In order to make the purpose, technical solutions and advantages of the present invention clearer, the following further describes the present invention in detail with reference to the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention, but do not constitute a limitation to the present invention. Similar elements in different embodiments are labeled with related similar element numbers. In the following embodiments, many detailed descriptions are provided to make the present invention better understood. However, those skilled in the art can easily recognize that some of the features can be omitted in different situations, or can be replaced by other elements, materials, and methods. In some cases, some operations related to the present invention are not shown or described in the specification, in order to avoid the core part of the present invention being overwhelmed by excessive descriptions. For those skilled in the art, it is not necessary to describe these related operations in detail, and they can fully understand the related operations according to the descriptions in the specification and the general technical knowledge in the art.

[0066] It should be noted that, without conflict, the embodiments and features in the embodiments of the present invention can be combined with each other to form various embodiments. At the same time, the steps or actions in the method description can also be adjusted in the order that is obvious to those skilled in the art. Therefore, the various orders in the specification and drawings are only for clearly describing a certain embodiment, and do not mean that they are the necessary order, unless it is stated that a certain order must be followed.

[0067] In the description of the present invention, it should be understood that the terms "center", "longitudinal", "lateral", "length", "width", "thickness", "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", "clockwise", "counterclockwise", etc. indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings. They are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation. Therefore, it should not be construed as a limitation to the present invention. In addition, the terms "first", "second", etc. are only used for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly specifying the quantity of the indicated technical features. Thus, the features defined with "first", "second", etc. may explicitly or implicitly include one or more of such features. In the description of the present invention, unless otherwise stated, the meaning of "a plurality" is two or more.

[0068] In the description of the present invention, it should be noted that unless otherwise clearly defined and limited, the terms "mounted", "connected", "coupled" should be understood in a broad sense. For example, it may be a fixed connection, a detachable connection, or an integral connection; it may be a mechanical connection or an electrical connection; it may be directly connected or indirectly connected through an intermediate medium, and it may be the communication inside two elements. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood through specific situations.

[0069] The present invention will be described in detail below with reference to the drawings and in conjunction with embodiments.

[0070] Refer to Figure 1 shown, which is the operation flow chart of the preferred embodiment of the speed measurement and processing method of the single-turn absolute encoder of the present invention. Please also refer to Figure 2 :

[0071] Step S1, initialize the parameters of the single-turn absolute encoder. Specifically:

[0072] Define the data output time interval of the single-turn absolute encoder as T;

[0073] Define the angle value output in real time by the single-turn absolute encoder as , and initialize it to zero, that is, , and its value is updated according to the time interval T by the encoder angle position output data, and the value range is ;

[0074] Define the angular position of two consecutive differences in step S2 as the angular position at the current moment and the angular position at the previous moment and initialize it to zero, i.e., , ;

[0075] Define the temporary variable of the angular position of the single-turn absolute encoder at the current moment and initialize it to zero, i.e., ;

[0076] Initialize the signed integer data N of the differential angular velocity anomaly at the zero-crossing of the angular position of the single-turn absolute encoder to zero, i.e., N = 0;

[0077] Define the threshold of the differential angular velocity anomaly at the zero-crossing of the angular position of the single-turn absolute encoder as and initialize it according to the maximum rotational speed at which the actual encoder is applied to the device ;

[0078] Define the angular velocity obtained in step S2 as and initialize it to zero, i.e., ;

[0079] Define the corrected angular velocity in step S3 as and initialize it to zero, i.e., ;

[0080] Define the continuous small outlier count value of the angular velocity in step S3 as M and initialize it to zero, i.e., M = 0;

[0081] Define the processed angular velocity in step S4 as and initialize it to zero, i.e., .

[0082] Step S2: After initializing the parameters, the encoder continuously outputs the angular position at a fixed time interval, and differentiates the angular positions output twice in a row to obtain the angular velocity. Specifically:

[0083] Calculate the temporary variable of the angular position at the current moment , i.e., ;

[0084] Calculate the angular velocity by the differential method, i.e., .

[0085] Step S3: According to the angular velocity obtained in step S2, identify and correct the abnormal angular velocity at the zero-crossing of the angular position of the encoder to obtain the corrected angular velocity. Specifically:

[0086] If it satisfies and ​​​​​​​​​​​​​​​If the condition is met, increment N by 1, i.e., N = N + 1;

[0087] If and the condition is met, decrement N by 1, i.e., N = N - 1;

[0088] Update and correct the current angular position , i.e., ;

[0089] Update and correct the angular velocity by the difference method , i.e., ;

[0090] Update the previous angular position , i.e., .

[0091] Step S4: Identify and process outliers in the corrected angular velocity in step S3. Specifically:

[0092] If , then the current angular velocity is an abnormal outlier. Directly remove the current and update the angular velocity , i.e., ;

[0093] If , and the condition is met, then determine that the current angular velocity is at least an isolated small outlier. Remove the current and update the angular velocity , i.e., , and increment M by 1, i.e., M = M + 1. Then determine whether M is greater than or equal to 3: If , then determine that the current angular velocity is a patchy small outlier. Recalibrate the angular velocity , update the angular velocity , i.e., , and set the value of M to zero, i.e., M = 0;

[0094] If , and the condition is met, then determine that the current angular velocity is a normal value, and update the angular velocity , i.e., , and set the value of M to zero, i.e., M = 0.

[0095] Refer to Figure 3As shown, it is the hardware architecture diagram of the single-turn absolute encoder speed measurement and processing system 10 of the present invention. The system includes: an initialization module 101, a differential module 102, a correction module 103, and a processing module 104. Among them:

[0096] The initialization module 101 is used to initialize the parameters of the single-turn absolute encoder. Specifically:

[0097] Define the data output time interval of the single-turn absolute encoder as T;

[0098] Define the angle value output in real time by the single-turn absolute encoder as , and initialize it to zero, that is , and its value is updated by the encoder angular position output data at time interval T, and the value range is ;

[0099] Define the angular position of the differential module 102 for two consecutive differentials as the angular position at the current moment and the angular position at the previous moment , and initialize it to zero, that is , ;

[0100] Define the temporary variable of the angular position of the single-turn absolute encoder at the current moment, and initialize it to zero, that is ;

[0101] Initialize the signed integer data N with abnormal differential angular velocity at the zero crossing of the angular position of the single-turn absolute encoder to zero, that is N = 0;

[0102] Define the threshold value of the abnormal differential angular velocity at the zero crossing of the angular position of the single-turn absolute encoder as , and according to the maximum rotational speed of the actual encoder applied to the device, initialize ;

[0103] Define the angular velocity obtained by the differential module 102 as , and initialize it to zero, that is ;

[0104] Define the angular velocity after correction by the correction module 103, and initialize it to zero, that is ;

[0105] Define the angular velocity of the correction module 103, and the continuous small outlier count value is M, and initialize it to zero, that is M = 0;

[0106] Define the angular velocity , and initialize it to zero, that is .

[0107] The differential module 102 is used to perform a difference on the angular positions output continuously by the encoder at a fixed time interval after initializing the parameters, so as to obtain the angular velocity. Specifically:

[0108] The differential module 102 calculates the temporary variable of the angular position at the current moment , that is ;

[0109] The differential module 102 calculates the angular velocity by the difference method , that is .

[0110] The correction module 103 is used to identify and correct the abnormal angular velocity at the zero crossing of the encoder angular position according to the angular velocity obtained by the differential module 102, and obtain the corrected angular velocity. Specifically:

[0111] If the conditions of and are satisfied, the correction module 103 increases the N by 1, that is N = N + 1;

[0112] If the conditions of and are satisfied, the correction module 103 subtracts 1 from the N, that is N = N - 1;

[0113] The correction module 103 updates and corrects the angular position at the current moment , that is ;

[0114] The correction module 103 updates and corrects the angular velocity by the difference method , that is ;

[0115] The correction module 103 updates the angular position at the previous moment , that is .

[0116] The processing module 104 is used to identify and process the outliers of the angular velocity corrected by the correction module 103. Specifically:

[0117] If , then the current angular velocity is an abnormal outlier, and the processing module 104 directly eliminates the current , and updates the angular velocity , that is ;

[0118] If and satisfy the condition , then it is determined that the current angular velocity is at least an isolated small outlier value, and the processing module 104 will remove the current , and update the angular velocity , that is , and at the same time increase M by 1, that is M = M + 1, and then determine whether M is greater than or equal to 3: If , then it is determined that the current angular velocity is a patch small outlier value, and the angular velocity is recalibrated , and the angular velocity is updated , that is , and at the same time set the value of M to zero, that is M = 0;

[0119] If and satisfy the condition , then it is determined that the current angular velocity is a normal value, and the processing module 104 updates the angular velocity , that is , and at the same time set the value of M to zero, that is M = 0.

[0120] Although the present invention is described with reference to the current preferred embodiments, those skilled in the art should understand that the above preferred embodiments are only used to illustrate the present invention and are not used to limit the protection scope of the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle scope of the present invention shall be included in the scope of the present invention's protection rights.

Claims

1. A method for measuring speed and processing of a single-turn absolute encoder, characterized in that The method includes the following steps: S1, initialize the parameters of the single-turn absolute encoder; S2, after initializing the parameters, the encoder continuously outputs the angular position at a fixed time interval, and perform a difference on the angular positions output twice continuously to obtain the angular velocity; S3, according to the angular velocity obtained in step S2, identify and correct the abnormal angular velocity at the zero crossing of the encoder angular position to obtain the corrected angular velocity; S4, identify and process the outliers of the corrected angular velocity in step S3; where: Step S1 includes: Define the data output time interval of the single-turn absolute encoder as T; Define the angular value output in real time by the single-turn absolute encoder as , and initialize it to zero, that is , and the value range is ; Define the angular position of two consecutive differences in step S2 as the angular position at the current moment and the angular position at the previous moment , and initialize them to zero, that is , ; Define a temporary variable for the angular position of the single-turn absolute encoder at the current moment , and initialize it to zero, that is ; Initialize the signed integer data N of the differential angular velocity anomaly at the zero crossing of the single-turn absolute encoder angular position to zero, i.e., N = 0; Define the threshold for the differential angular velocity anomaly at the zero crossing of the angular position of the single-turn absolute encoder as , and initialize according to the maximum rotational speed of the actual encoder applied to the device , ; Define the angular velocity obtained in step S2 as , and initialize it to zero, i.e., ; Define the corrected angular velocity in step S3 as , and initialize it to zero, that is ; Step S3 includes: If and , then increment the said N by 1, i.e., N = N + 1; If and , then subtract 1 from the said N, i.e., N = N - 1; Update and correct the current angular position , namely ; Update the corrected angular velocity by the difference method , that is ; Update the angular position at the previous moment , namely .

2. The single-turn absolute encoder speed measurement and processing method according to claim 1, characterized in that Step S1 further includes: Define the angular velocity in step S3 The continuous small wild value count is M and is initialized to zero, i.e., M = 0; Define the angular velocity after being processed in step S4 as , and initialize it to zero, i.e., .

3. The speed measurement and processing method of the single-turn absolute encoder according to claim 2, characterized in that Step S2 includes: Calculate the temporary variable of the angular position at the current moment , that is ; Calculating the angular velocity by the difference method That is .

4. The single-turn absolute encoder speed measurement and processing method according to claim 3, characterized in that, Step S4 includes: If , then the current angular velocity is an abnormal outlier, directly remove the current angular velocity , and update the angular velocity , that is ; If , and the condition is satisfied, then it is determined that the current angular velocity is at least an isolated small outlier value. The current angular velocity is removed, and the angular velocity is updated, that is . At the same time, the value of M is incremented by 1, that is M = M + 1. Then it is judged whether the value of M is greater than or equal to 3: If , then it is determined that the current angular velocity is a patch small outlier value, and the angular velocity is recalibrated, and the angular velocity is updated, that is . At the same time, the value of M is set to zero, that is M = 0; If , and the condition is satisfied, then it is determined that the current angular velocity is a normal value, and the angular velocity is updated, that is , and at the same time, the M value is set to zero, that is, M = 0.

5. A single-turn absolute encoder speed measurement and processing system, characterized in that, The system includes an initialization module, a difference module, a correction module, and a processing module, where: The initialization module is used to initialize the parameters of the single-turn absolute encoder; The difference module is used to make the encoder continuously output the angular position at a fixed time interval after initializing the parameters, and perform a difference on the angular positions output twice continuously to obtain the angular velocity; The correction module is used to identify and correct the abnormal angular velocity at the zero crossing of the encoder angular position according to the angular velocity obtained by the difference module to obtain the corrected angular velocity; The processing module is used to identify and process the outliers of the angular velocity corrected by the correction module; where: The specific function of the initialization module is: Define the data output time interval of the single-turn absolute encoder as T; Define the angular value output in real time by the single-turn absolute encoder as , and initialize it to zero, that is . Its value is updated at time intervals T according to the encoder angular position output data, and the value range is ; Define the angular position of two consecutive differentiations of the differential module as the angular position at the current moment and the angular position at the previous moment , and initialize them to zero, that is , ; Define the temporary variable of the angular position of the single-turn absolute encoder at the current moment , and initialize it to zero, that is ; Initialize the signed integer data N of the differential angular velocity anomaly at the zero crossing of the single-turn absolute encoder angular position to zero, i.e., N = 0; Define the threshold of the differential angular velocity anomaly at the zero crossing of the angular position of the single-turn absolute encoder as , and initialize according to the maximum rotational speed of the actual encoder applied to the device ; Define the angular velocity obtained by the differential module as , and initialize it to zero, that is ; The angular velocity corrected by the defined correction module is , and it is initialized to zero, that is ; The specific function of the correction module is: If and , then increment the said N by 1, i.e., N = N + 1; If and , then subtract 1 from the said N, i.e., N = N - 1; Update and correct the current angular position , namely ; Update the corrected angular velocity by the difference method , that is ; Update the previous moment angular position , that is .

6. The single-turn absolute encoder speed measurement and processing system according to claim 5, characterized in that, The specific function of the initialization module also is: Define the angular velocity of the correction module The continuous small outlier count value is M and is initialized to zero, i.e., M = 0; Define the angular velocity processed by the processing module as , and initialize it to zero, that is .

7. The single-turn absolute encoder speed measurement and processing system according to claim 6, characterized in that, The specific function of the difference module is: Calculate the temporary variable of the angular position at the current moment , that is ; Calculating the angular velocity by the difference method That is .

8. The single-turn absolute encoder speed measurement and processing system according to claim 7, characterized in that, The specific function of the processing module is: If , the current angular velocity is an abnormal outlier, directly remove the current angular velocity , and update the angular velocity , that is ; If , and the condition is satisfied, then it is determined that the current angular velocity is at least an isolated small outlier value. The current angular velocity is removed, and the angular velocity is updated, that is . At the same time, the value of M is incremented by 1, that is M = M + 1. Then it is judged whether the value of M is greater than or equal to 3: If , then it is determined that the current angular velocity is a patch small outlier value, and the angular velocity is recalibrated, and the angular velocity is updated, that is . At the same time, the value of M is set to zero, that is M = 0; If , and the condition is satisfied, then it is determined that the current angular velocity is a normal value, and the angular velocity is updated, that is . At the same time, the M value is set to zero, i.e., M = 0.

Images