Rotary table pitching positioning method and device based on gyroscope-coded disc complementary filtering

Abstract

The invention discloses a rotary table pitching positioning method and device based on gyroscope-coded disc complementary filtering, and belongs to the field of rotary table positioning. The method comprises the following steps: determining a pitch angle estimated value of a turntable by utilizing an angular velocity fed back by a gyroscope; obtaining a pitch angle measurement value of the coded disc to the turntable, and judging the coded disc state based on a signal fed back by the coded disc; and on the basis of the coded disc state, performing complementary filtering on the pitch angle estimated value determined by the gyroscope and the pitch angle measured value fed back by the coded disc to obtain a fused pitch angle estimated value. According to the scheme, the advantages of the gyroscope and the coded disc are combined, so that the defect that the system reliability is insufficient when only a single sensor coded disc exists is overcome; and the coded disc state is judged based on the signal fed back by the coded disc, and the measurement results of the gyroscope and the coded disc are fused based on the coded disc state by combining a complementary filtering algorithm, so that the accuracy, real-time performance and reliability of the rotary table pitch angle measurement under the stormy wave condition can be improved.

Description

Technical Field

[0001] The present invention relates to the technical field of turntable positioning, and particularly relates to a turntable pitch positioning method and device based on gyroscope-encoder complementary filtering. Background Art

[0002] With the continuous development of the economic society, the water transportation industry is also constantly progressing. To maintain the efficiency and safety of the water transportation industry, the supervision work of the maritime departments is essential. Currently, in actual supervision, the maritime departments mainly use the turntable equipment installed on the bow of the law enforcement ship to monitor, track, and obtain evidence of the target object. However, the unstable wind and waves on the sea surface are extremely likely to cause obvious pitch deviation of the turntable, resulting in difficulty in accurately measuring the pitch angle of the turntable and causing difficulties for the law enforcement department to obtain evidence. For the measurement of the pitch angle of this turntable, although the encoder can provide relatively accurate measurement results, due to its slow response speed, it cannot timely reflect the rapid change of the pitch angle, and usually only a single encoder sensor is set, and its failure will seriously affect the reliability of the system.

[0003] Therefore, there is an urgent need to provide a new turntable pitch positioning method. Summary of the Invention

[0004] In order to solve the problem that it is difficult to quickly and accurately measure the pitch angle of the turntable under wind and wave conditions and the system reliability is not high when using a single encoder sensor traditionally, the embodiments of the present invention provide a turntable pitch positioning method and device based on gyroscope-encoder complementary filtering.

[0005] On the one hand, a turntable pitch positioning method based on gyroscope-encoder complementary filtering is provided. The method includes: Determining an estimated value of the pitch angle of the turntable by using the angular velocity fed back by the gyroscope; Obtaining the measured value of the pitch angle of the turntable by the encoder, and judging the encoder state based on the signal fed back by the encoder; Based on the encoder state, performing complementary filtering on the estimated value of the pitch angle determined by using the gyroscope and the measured value of the pitch angle fed back by the encoder to obtain a fused estimated value of the pitch angle.

[0006] On the other hand, a turntable pitch positioning device based on gyroscope-encoder complementary filtering according to the steps of any method embodiment of the specification is provided. The device includes: An estimation unit for determining an estimated value of the pitch angle of the turntable by using the angular velocity fed back by the gyroscope; A measurement unit for obtaining the measured value of the pitch angle of the turntable by the encoder, and judging the encoder state based on the signal fed back by the encoder; A fusion unit, configured to perform complementary filtering on the pitch angle estimated value determined by the gyroscope and the pitch angle measured value fed back by the code disk based on the code disk state, so as to obtain a fused pitch angle estimated value.

[0007] On the other hand, a computer device is provided, which includes a memory and a processor. The memory is used to store a computer program, and the processor is used to execute the computer program stored on the memory to implement the steps of the above-mentioned method.

[0008] On the other hand, a computer-readable storage medium is provided. A computer program is stored in the storage medium, and when the computer program is executed by a processor, the steps of the above-mentioned method are implemented.

[0009] On the other hand, a computer program product is provided, including a computer program. When the computer program is executed by a processor, the steps of the above-mentioned method are implemented.

[0010] The technical solution provided by the present invention can at least bring the following beneficial effects: By combining the advantages of two sensors, namely the gyroscope and the code disk, the defect of insufficient system reliability caused by only a single sensor code disk is made up for; and by judging the code disk state based on the signal fed back by the code disk and combining the complementary filtering algorithm to fuse the measurement results of the gyroscope and the code disk based on the code disk state, the accuracy, real-time performance and reliability of the pitch angle measurement of the turntable under wind and wave conditions can be improved. Description of the Drawings

[0011] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the following drawings are some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0012] Figure 1 It is a flowchart of a turntable pitch positioning method based on gyroscope-code disk complementary filtering provided by an embodiment of the present invention; Figure 2 It is a structural diagram of a turntable pitch positioning device based on gyroscope-code disk complementary filtering provided by an embodiment of the present invention; Figure 3 It is a hardware architecture diagram of a computer device provided by an embodiment of the present invention. Detailed Embodiments

[0013] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts fall within the scope of protection of the present invention.

[0014] The following describes the specific implementation manners of the above concepts.

[0015] Please refer to Figure 1 , a turntable pitch positioning method based on gyroscope-encoder complementary filtering provided by an embodiment of the present invention, the method includes: Step 100: Determine an estimated value of the pitch angle of the turntable using the angular velocity fed back by the gyroscope; Step 102: Obtain the measured value of the pitch angle of the turntable by the encoder, and judge the encoder state based on the signal fed back by the encoder; Step 104: Based on the encoder state, perform complementary filtering on the estimated value of the pitch angle determined using the gyroscope and the measured value of the pitch angle fed back by the encoder to obtain a fused estimated value of the pitch angle.

[0016] In the embodiments of the present invention, considering that although the gyroscope has drift errors, its response speed is fast, and complementary use with the encoder can greatly improve the system reliability. Therefore, this solution combines the advantages of the two sensors of the gyroscope and the encoder, making up for the defect of insufficient system reliability caused by only a single sensor encoder; and by judging the encoder state based on the signal fed back by the encoder and combining the complementary filtering algorithm to fuse the measurement results of the gyroscope and the encoder based on the encoder state, the accuracy, real-time performance, and reliability of the turntable pitch angle measurement under wind and wave conditions can be improved.

[0017] The following describes Figure 1 the execution manners of the respective steps shown.

[0018] Regarding step 100: In this implementation, an MEMS gyroscope is selected to directly obtain the angular velocity generated by the turntable , and the angular velocity at this time may be due to the combined external forces such as wind and waves.

[0019] The estimated value of the pitch angle in step 100 can be determined by the following formula: Wherein, is the estimated value of the pitch angle at the previous moment, is the sampling time, is the angular velocity fed back by the gyroscope.

[0020] Regarding step 102: In some embodiments, the step of "judging the status of the code disk based on the signals fed back by the code disk" includes steps S1 - S3: Step S1, based on the sampled values of two orthogonal signals fed back by the code disk, calculate the phase difference to determine the signal integrity status value; Step S2, based on the average amplitude of two orthogonal signals fed back by the code disk, determine the signal amplitude status value; Step S3, based on the signal integrity status value and the signal amplitude status value, judge the status of the code disk.

[0021] In some embodiments, the signal integrity status value in step S1 is determined in the following manner: Wherein, is the signal integrity status value, is the phase difference, , are the sampled values of two orthogonal signals fed back by the code disk , of, is the allowable error range value of the phase difference, and N is the number of sampling points of each orthogonal signal.

[0022] In this embodiment, by calculating the phase difference of two orthogonal signals fed back by the code disk, the signal integrity status value of the code disk is judged to realize the quantization of the signal integrity status.

[0023] Due to the phase difference error causing the angular error can be approximated as: Therefore, in some embodiments, the allowable error range value of the phase difference is determined in the following manner: Wherein, is the maximum angular error of the system turntable, which can be measured according to the actual situation of the turntable.

[0024] In some embodiments, the signal amplitude status value in step S2 is determined in the following manner: Wherein, is the signal amplitude status value, , are respectively two orthogonal signals fed back by the code disk , The average amplitude of , are respectively the allowable error range values of the average amplitude of the two orthogonal signals output by the code disk and . N is the number of sampling points of each orthogonal signal.

[0025] In this embodiment, by calculating the average amplitude of the two orthogonal signals fed back by the code disk, the signal amplitude state value of the code disk is judged to realize the quantization of the signal amplitude state.

[0026] Due to the signal amplitude error the resulting angle error can be approximated as: Therefore, in some embodiments, the allowable error range value of the signal average amplitude can be expressed as: where is the maximum angle error of the system turntable, which can be measured according to the actual situation of the turntable.

[0027] In some embodiments, the code disk state is determined in step S3 in the following manner: In the formula, is the code disk state, represents AND, represents OR.

[0028] In this embodiment, by evaluating the signal integrity and signal amplitude based on the two orthogonal signals fed back by the code disk, it is automatically judged whether the code disk is in a normal state. Only when both the signal integrity and signal amplitude are normal, the code disk state is normal, and in other cases, it is abnormal. Incorporating the code disk state into the complementary filtering algorithm of the gyroscope and the code disk can avoid the output of the pitch angle measurement value when the code disk state is abnormal, so as to further improve the accuracy of the turntable pitch positioning.

[0029] Regarding step 104: In some embodiments, step 104 may include: Design a high-pass filter required for fusion and a low-pass filter incorporating the code disk state consideration factors; Input the pitch angle estimated value determined by the gyroscope into the high-pass filter to obtain the filtered pitch angle estimated value; Input the code disk state and the pitch angle measurement value fed back by the code disk into the low-pass filter to obtain the filtered pitch angle measurement value; Sum the filtered pitch angle estimation value and the filtered pitch angle measurement value to obtain the fused pitch angle estimation value.

[0030] In this embodiment, the fusion expression of the fused pitch angle estimation value is: Where, is the fused pitch angle estimation value, is the weight coefficient, is the pitch angle estimation value determined by the gyroscope, is the encoder state, is the pitch angle measurement value feedback by the encoder.

[0031] In this embodiment, the design of the weight coefficient is as follows: In the system can be understood as a high-pass filter, which is used to extract the pitch angle change information of the gyroscope and reduce the drift error; can be understood as a low-pass filter, which is used to smooth the pitch angle measurement value of the encoder and reduce the dynamic error.

[0032] Since the feedback signal of the gyroscope is a high-frequency signal, a high-pass filter is selected to filter the pitch angle estimation value feedback by the gyroscope, so that the gyroscope can play a major positioning role when the wind and waves are large. And the encoder signal is a low-frequency signal, a low-pass filter is selected to filter the pitch angle measurement value feedback by the encoder, so that the encoder can play a major positioning role when the wind and waves are small. By designing the high-pass filter and the low-pass filter, fused pitch angle estimation values with different output emphases can be obtained under different wind and wave intensities, so that the fused positioning of the turntable pitch angle can adapt to different wind and wave environments, emphasizing accuracy when the waves are small and emphasizing speed and reliability when the waves are large. In addition, incorporating the encoder state into the low-pass filter can not only automatically judge the encoder state, but also enable the gyroscope to play a major positioning role when the encoder fails, improving the system reliability.

[0033] In some embodiments, the high-pass filter filters the pitch angle estimation value in the following manner: Substituting gives: In the formula, is the filtered pitch angle estimation value, is the transfer function of the high-pass filter, is the pitch angle estimation value, is the time constant, is the pitch angle estimated value at the previous moment, is the angular velocity fed back by the gyroscope, is the sampling time, is the angular frequency.

[0034] In this embodiment, using a high-pass filter to filter the pitch angle estimated value determined by the gyroscope can enable the gyroscope to play a major positioning role when the wind and waves are large, ensuring the positioning speed and reliability when the waves are large.

[0035] In some embodiments, the low-pass filter filters the pitch angle measurement value in the following manner: Substituting gives: In the formula, is the filtered pitch angle measurement value, is the transfer function of the low-pass filter, is the code disk state, is the pitch angle measurement value fed back by the code disk, is the time constant, is the angular frequency.

[0036] In this embodiment, selecting a low-pass filter to filter the pitch angle measurement value fed back by the code disk can enable the code disk to play a major positioning role when the wind and waves are small, and adding the code disk state can comprehensively improve the accuracy of the fusion output.

[0037] Finally, through summation, the fused pitch angle estimated value is obtained: In the formula, is the time constant, is the angular frequency, is the pitch angle estimated value determined by the gyroscope, is the pitch angle estimated value at the previous moment, is the angular velocity fed back by the gyroscope, is the sampling time, is the code disk state, is the pitch angle measurement value fed back by the code disk.

[0038] In the embodiment of the present invention, the advantages of two types of sensors are combined through a complementary filtering algorithm, avoiding the problem of low system reliability caused by a single encoder sensor. Specifically, the output signal of the gyroscope is input into a high-pass filter, and the output signal of the encoder is input into a low-pass filter and then summed, enabling the system to achieve the effect that when a high-frequency signal is input, the output of the gyroscope dominates the system output, and when a low-frequency signal is input, the output of the encoder dominates the system output. Moreover, when the encoder fails, the output of the gyroscope dominates the system output, and a turntable pitch angle positioning technology with high measurement result accuracy, fast response speed, and high reliability can be obtained.

[0039] Please refer to Figure 2 , an embodiment of the present invention provides a turntable pitch positioning device based on gyroscope-encoder complementary filtering. The device includes: An estimation unit 201 for determining an estimated value of the pitch angle of the turntable by using the angular velocity fed back by the gyroscope; A measurement unit 202 for obtaining a measured value of the pitch angle of the turntable by the encoder and determining the encoder state based on the signal fed back by the encoder; A fusion unit 203 for performing complementary filtering on the estimated value of the pitch angle determined by the gyroscope and the measured value of the pitch angle fed back by the encoder based on the encoder state to obtain a fused estimated value of the pitch angle.

[0040] In an embodiment of the present invention, when the measurement unit 202 determines the encoder state based on the signal fed back by the encoder, it is used for: Calculating a phase difference based on the sampled values of two orthogonal signals fed back by the encoder to determine a signal integrity status value; Determining a signal amplitude status value based on the average amplitude of two orthogonal signals fed back by the encoder; Judging the encoder state based on the signal integrity status value and the signal amplitude status value.

[0041] In an embodiment of the present invention, the signal integrity status value in the measurement unit 202 is determined by the following method: Wherein, is the signal integrity status value, is the phase difference, , are the sampled values of two orthogonal signals fed back by the encoder , of, is the allowable error range value of the phase difference, and N is the number of sampling points of each orthogonal signal.

[0042] In an embodiment of the present invention, the fusion unit 203 is used to perform: Design a high-pass filter required for fusion and a low-pass filter that incorporates considerations of the encoder disk state; Input the pitch angle estimation value determined by the gyroscope into the high-pass filter to obtain a filtered pitch angle estimation value; Input the encoder disk state and the pitch angle measurement value feedback from the encoder disk into the low-pass filter to obtain a filtered pitch angle measurement value; Sum the filtered pitch angle estimation value and the filtered pitch angle measurement value to obtain a fused pitch angle estimation value.

[0043] In an embodiment of the present invention, the high-pass filter in the fusion unit 203 filters the pitch angle estimation value in the following manner: Substituting gives: In the formula, is the filtered pitch angle estimation value, is the transfer function of the high-pass filter, is the pitch angle estimation value, is the time constant, is the pitch angle estimation value at the previous moment, is the angular velocity feedback from the gyroscope, is the sampling time, is the angular frequency.

[0044] In an embodiment of the present invention, the low-pass filter in the fusion unit 203 filters the pitch angle measurement value in the following manner: Substituting gives: In the formula, is the filtered pitch angle measurement value, is the transfer function of the low-pass filter, is the encoder disk state, is the pitch angle measurement value, is the time constant, is the angular frequency.

[0045] It should be noted that: the above device embodiment and the method embodiment belong to the same concept, and the specific implementation process can be seen in the method embodiment, which will not be elaborated here.

[0046] An embodiment of the present application further provides a computer device, which includes a processor and a memory. At least one instruction, at least one program, a code set or an instruction set is stored in the memory, and the at least one instruction, at least one program, the code set or the instruction set is loaded and executed by the processor to implement the turntable pitch positioning method based on gyroscope-encoder complementary filtering provided by the above method embodiments.

[0047] An embodiment of the present application further provides a computer-readable storage medium, on which at least one instruction, at least one program, a code set or an instruction set is stored, and the at least one instruction, at least one program, the code set or the instruction set is loaded and executed by the processor to implement the turntable pitch positioning method based on gyroscope-encoder complementary filtering provided by the above method embodiments.

[0048] An embodiment of the present application further provides a computer program product, which includes a computer program. The processor of the computer device reads the computer program from the computer-readable storage medium, and the processor executes the computer program, so that the computer device executes the turntable pitch positioning method based on gyroscope-encoder complementary filtering in any one of the above embodiments.

[0049] For the convenience of description, when describing the above devices or apparatuses, they are divided into various modules or units according to functions for description. Of course, when implementing the present application, the functions of each unit can be implemented in one or more software and / or hardware.

[0050] From the description of the above embodiments, those skilled in the art can clearly understand that the present application can be implemented by means of software plus a necessary general hardware platform. Based on such an understanding, the technical solution of the present application, in essence, or the part that contributes to the prior art can be embodied in the form of a software product. The computer software product can be stored in a storage medium, such as ROM / RAM, magnetic disk, optical disc, etc., and includes several instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods in various embodiments or some parts of the embodiments of the present application.

[0051] Finally, it should also be noted that in this text, relational terms such as first, second, third, and fourth are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the terms "comprising", "including" or any other variant thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device comprising a series of elements not only includes those elements, but also includes other elements not expressly listed, or further includes elements inherent to such process, method, article or device. Without further limitation, an element defined by the statement "comprising an..." does not exclude the presence of additional identical elements in the process, method, article or device comprising the element.

[0052] The above are only the preferred embodiments of the present application. It should be pointed out that for those of ordinary skill in the art, without departing from the principle of the present application, several improvements and refinements can be made, and these improvements and refinements should also be regarded as the protection scope of the present application.

Claims

1. A turntable pitch positioning method based on gyroscope-encoder complementary filtering, characterized in that including: determining an estimated pitch angle of the turntable by using the angular velocity fed back by the gyroscope; obtaining a measured pitch angle of the turntable by the encoder, and judging the encoder state based on the signal fed back by the encoder; performing complementary filtering on the estimated pitch angle determined by using the gyroscope and the measured pitch angle fed back by the encoder based on the encoder state to obtain a fused estimated pitch angle.

2. The method according to claim 1, wherein The judging the encoder state based on the signal fed back by the encoder includes: calculating a phase difference based on the sampled values of two orthogonal signals fed back by the encoder to determine a signal integrity state value; determining a signal amplitude state value based on the average amplitude of two orthogonal signals fed back by the encoder; judging the encoder state based on the signal integrity state value and the signal amplitude state value.

3. The method according to claim 2, wherein The signal integrity state value is determined by the following method: wherein, is the signal integrity status value, is the phase difference, , are the sampled values of two orthogonal signals fed back by the code disk , , is the allowable error range value of the phase difference, and N is the number of sampling points of each orthogonal signal.

4. The method according to any one of claims 1 to 3, characterized in that, The performing complementary filtering on the estimated pitch angle determined by using the gyroscope and the measured pitch angle fed back by the encoder based on the encoder state to obtain a fused estimated pitch angle includes: designing a high-pass filter required for fusion and a low-pass filter incorporating encoder state considerations; inputting the estimated pitch angle determined by the gyroscope into the high-pass filter to obtain a filtered estimated pitch angle; inputting the encoder state and the measured pitch angle fed back by the encoder into the low-pass filter to obtain a filtered measured pitch angle; summing the filtered estimated pitch angle and the filtered measured pitch angle to obtain a fused estimated pitch angle.

5. The method according to claim 4, characterized in that, The high-pass filter filters the estimated pitch angle by the following method: Substituting gives: In the formula, is the estimated pitch angle after filtering, is the transfer function of the high-pass filter, is the estimated pitch angle, is the time constant, is the estimated pitch angle at the previous moment, is the angular velocity feedback from the gyroscope, is the sampling time, is the angular frequency.

6. The method according to claim 4, wherein The low-pass filter filters the measured pitch angle by the following method: Substituting gives: Wherein, is the filtered pitch angle measurement value, is the transfer function of the low-pass filter, is the encoder status, is the pitch angle measurement value, is the time constant, is the angular frequency.

7. A turntable pitch positioning device based on gyroscope-encoder complementary filtering, which is used to implement the steps of the method according to any one of claims 1-6 above, and is characterized in that including: an estimation unit for determining an estimated pitch angle of the turntable by using the angular velocity fed back by the gyroscope; a measurement unit for obtaining a measured pitch angle of the turntable by the encoder and judging the encoder state based on the signal fed back by the encoder; a fusion unit for performing complementary filtering on the estimated pitch angle determined by using the gyroscope and the measured pitch angle fed back by the encoder based on the encoder state to obtain a fused estimated pitch angle.

8. A computer device, characterized in that, The computer device includes a memory and a processor. The memory is used for storing a computer program, and the processor is used for executing the computer program stored on the memory to implement the steps of the method according to any one of claims 1-6 above.

9. A computer-readable storage medium, characterized in that, The storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the method according to any one of claims 1-6 are implemented.

10. A computer program product, characterized in that, including a computer program, and when the computer program is executed by a processor, the steps of the method according to any one of claims 1-6 are implemented.

Images