Turntable orientation and attitude estimation method and device based on gyroscope-code disk

Abstract

The present invention discloses a method and device for estimating the azimuth and attitude of a turntable based on a gyroscope-code disk, which belongs to the field of attitude estimation. The method includes: for the estimation of the azimuth and attitude of the turntable at each moment, the following steps are performed: obtaining the turntable azimuth angular velocity measurement value fed back by the gyroscope at the current moment and the turntable azimuth angle measurement value fed back by the code disk to construct the azimuth and attitude measurement matrix at the current moment; fusing the measurement values fed back by the gyroscope and the code disk through the Kalman filter algorithm to obtain the turntable azimuth and attitude estimation value at the current moment; constructing an adaptive method based on residuals to dynamically adjust the process noise covariance matrix and the observation noise covariance matrix to cope with time-varying noise and model errors. This solution combines the advantages of the gyroscope and the code disk, fuses the dynamic prediction of the gyroscope and the absolute observation of the code disk through the Kalman filter algorithm, and adjusts the covariance weight in real time based on the residuals to balance the uncertainty of the system noise, with high accuracy and good real-time performance.

Description

Technical Field

[0001] The present invention relates to the technical field of attitude estimation, and in particular to a method and device for estimating the orientation and attitude of a turntable based on a gyroscope-code disk. Background Art

[0002] Relevant departments primarily monitor, track, and collect evidence using turntables installed at key thoroughfares. However, existing turntables equipped with a single sensor are poorly adaptable to complex environments such as strong winds and heavy rainfall, making it difficult to quickly and accurately estimate the turntable's orientation and attitude. While encoders offer no cumulative error and can provide long-term, stable, and relatively accurate measurement results, they are limited by their sampling rate, have poor high-frequency response, and may experience noise or latency, making them unable to promptly reflect rapid changes in the turntable's orientation and attitude.

[0003] Therefore, there is an urgent need to provide a new turntable orientation and attitude estimation method. Summary of the Invention

[0004] In order to solve the problem that it is difficult to quickly and accurately estimate the turntable's orientation and attitude under complex working conditions when using a single code disk sensor in the traditional method, an embodiment of the present invention provides a turntable orientation and attitude estimation method and device based on a gyroscope-code disk.

[0005] On the one hand, a method for estimating the orientation and attitude of a turntable based on a gyroscope-code disk is provided, the method comprising:

[0006] For the turntable orientation and attitude estimation at each moment, the following operations are performed:

[0007] Obtain the turntable azimuth angular velocity measurement value fed back by the gyroscope and the turntable azimuth angle measurement value fed back by the encoder at the current moment to construct the azimuth attitude measurement matrix at the current moment;

[0008] Based on the azimuth attitude measurement matrix at the current moment, the prior estimate of the azimuth attitude at the current moment, and the process noise covariance matrix and observation noise covariance matrix updated and adjusted at the previous moment, a Kalman filter algorithm is used to fuse the measurement values fed back by the gyroscope and the encoder to obtain an estimated azimuth attitude value of the turntable at the current moment; wherein the azimuth attitude includes the azimuth angular velocity and the azimuth angle;

[0009] The residual is calculated based on the azimuth attitude measurement matrix at the current moment and the azimuth attitude prior estimation value at the current moment, so as to adaptively update and adjust the process noise covariance matrix and the observation noise covariance matrix based on the residual, so as to iteratively estimate the turntable azimuth attitude at the next moment.

[0010] On the other hand, a turntable orientation and attitude estimation device based on a gyroscope and a code disk is provided based on the steps described in any method embodiment of the specification, the device comprising:

[0011] The acquisition unit is used to perform the following operations for the turntable azimuth attitude estimation at each moment: obtaining the turntable azimuth angular velocity measurement value fed back by the gyroscope and the turntable azimuth angle measurement value fed back by the encoder at the current moment, so as to construct the azimuth attitude measurement matrix at the current moment;

[0012] A fusion unit is configured to fuse the measurement values fed back by the gyroscope and the encoder using a Kalman filter algorithm based on the azimuth attitude measurement matrix at the current moment, the prior estimation value of the azimuth attitude at the current moment, and the process noise covariance matrix and observation noise covariance matrix updated and adjusted at the previous moment, to obtain an estimated value of the turntable's azimuth attitude at the current moment; wherein the azimuth attitude includes an azimuth angular velocity and an azimuth angle;

[0013] An updating unit is used to calculate a residual based on the azimuth attitude measurement matrix at the current moment and a priori estimation value of the azimuth attitude at the current moment, so as to adaptively update and adjust the process noise covariance matrix and the observation noise covariance matrix based on the residual, so as to iteratively estimate the turntable azimuth attitude at the next moment.

[0014] On the other hand, a computer device is provided, comprising a memory and a processor, wherein the memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory to implement the steps of the above-mentioned method.

[0015] On the other hand, a computer-readable storage medium is provided, wherein 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.

[0016] On the other hand, a computer program product is provided, comprising a computer program, wherein the computer program implements the steps of the above method when executed by a processor.

[0017] The technical solution provided by the present invention can at least bring the following beneficial effects:

[0018] By combining the advantages of the gyroscope and the code disk, and fusing the dynamic prediction of the gyroscope and the absolute observation of the code disk through the Kalman filter algorithm, the system can achieve the effect of relying on the gyroscope at high frequency and relying on the code disk at low frequency. At the same time, the covariance weight is adjusted in real time based on the residual to balance the uncertainty of the system noise, which can greatly improve the accuracy and real-time performance of the turntable's azimuth and attitude estimation under complex working conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0020] Figure 1 This is a flow chart of a turntable orientation and attitude estimation method based on a gyroscope-code disk provided by one embodiment of the present invention;

[0021] Figure 2 This is a structural diagram of a turntable orientation and attitude estimation device based on a gyroscope and a code disk, provided in one embodiment of the present invention;

[0022] Figure 3 This is a hardware architecture diagram of a computer device provided by one embodiment of the present invention. DETAILED DESCRIPTION

[0023] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0024] The specific implementation of the above concept is described below.

[0025] Please refer to Figure 1 The embodiment of the present invention provides a method for estimating the orientation and attitude of a turntable based on a gyroscope and a code disk, the method comprising:

[0026] Step 100: For the turntable orientation and attitude estimation at each moment, execute:

[0027] Obtain the turntable azimuth angular velocity measurement value fed back by the gyroscope and the turntable azimuth angle measurement value fed back by the encoder at the current moment to construct the azimuth attitude measurement matrix at the current moment;

[0028] Step 102: Based on the current azimuth attitude measurement matrix, the current azimuth attitude prior estimate, and the updated and adjusted process noise covariance matrix and observation noise covariance matrix at the previous moment, a Kalman filter algorithm is used to fuse the measurement values of the gyroscope and the encoder feedback to obtain the current turntable azimuth attitude estimate; wherein the azimuth attitude includes the azimuth angular velocity and the azimuth angle;

[0029] Step 104: Calculate the residual based on the azimuth attitude measurement matrix at the current moment and the azimuth attitude prior estimation value at the current moment, and adaptively update the adjustment process noise covariance matrix and the observation noise covariance matrix based on the residual to iteratively estimate the turntable azimuth attitude at the next moment.

[0030] In an embodiment of the present invention, by combining the advantages of the two sensors, the gyroscope and the code disk, and fusing the dynamic prediction of the gyroscope and the absolute observation of the code disk through the Kalman filter algorithm, the system can achieve the effect of high-frequency dependence on the gyroscope and low-frequency dependence on the code disk. At the same time, the covariance weight is adjusted in real time based on the residual to balance the uncertainty of the system noise, which can greatly improve the accuracy and real-time performance of the turntable azimuth and attitude estimation under complex working conditions.

[0031] Described below Figure 1 How to perform the steps shown.

[0032] For step 100:

[0033] In this implementation, the MEMS gyroscope can be used to directly obtain the turntable azimuth angular velocity measurement value. , use the code disk to directly obtain the turntable azimuth angle measurement value , to obtain the current moment's orientation and attitude measurement matrix .

[0034] Regarding step 102:

[0035] In some implementations, step 102 may include steps S1-S5:

[0036] Step S1: Determine a priori estimated value of the turntable's orientation and attitude at the current moment based on the estimated value of the turntable's orientation and attitude at the previous moment.

[0037] In this step, the prior estimate of the orientation and attitude at the current moment is It can be expressed as:

[0038]

[0039] Among them, the state transfer matrix , is the sampling time interval of the gyroscope, It is the estimated value of the turntable's orientation and attitude at the previous moment.

[0040] It should be noted that the estimated value of the turntable orientation at the initial moment is ,in, It is the initial orientation angle of the turntable fed back by the encoder.

[0041] Step S2: Obtain the updated and adjusted process noise covariance matrix at the last moment and the covariance matrix at the last moment to determine a prediction covariance matrix.

[0042] In some embodiments, the prediction covariance matrix is calculated as follows:

[0043]

[0044] in,

[0045]

[0046]

[0047]

[0048] Where, is the prediction covariance matrix, is the state transition matrix, is the transpose of the state transfer matrix, is the covariance matrix of the previous moment, is the current process noise covariance matrix, is the initial covariance matrix, is the initial angle error used to characterize the accuracy of the code disk, is the initial angular velocity error used to characterize the gyroscope bias, is the sampling time interval of the gyroscope, is the initial process noise covariance matrix, is the integral error of the code disk turntable angle, is the angular velocity disturbance of the gyroscope turntable.

[0049] In this embodiment, the process noise covariance matrix is constructed based on the gyroscope turntable angular velocity disturbance and the code disk turntable angle integral error, and the process noise covariance matrix is continuously updated and adjusted based on the residual to adjust the prediction covariance weight in real time and balance the system process noise.

[0050] Step S3: Obtain the observation noise covariance matrix that was updated and adjusted at the previous moment, so as to update the Kalman gain matrix at the current moment based on the prediction covariance matrix and the obtained observation noise covariance matrix.

[0051] In the implementation of the present invention, the Kalman gain matrix at the current moment is calculated by the following formula:

[0052]

[0053] in,

[0054]

[0055]

[0056] Where, is the Kalman gain matrix at the current moment, is the prediction covariance matrix, is the observer, is the current observation noise covariance matrix, is the initial observation noise covariance matrix, is the angle measurement variance of the code disk, is the angular velocity measurement variance of the gyroscope.

[0057] In this embodiment, an observation noise covariance matrix is constructed based on the angular velocity measurement variance of the gyroscope and the angle measurement variance of the code disk, and the observation noise covariance matrix is continuously updated and adjusted based on the residual to adjust the value of the Kalman gain matrix in real time. In this way, the weights of the two sensor data on the final estimate are dynamically adjusted through the Kalman gain matrix to balance the system observation noise, so that the system can achieve the effect of relying on the gyroscope at high frequency and relying on the code disk at low frequency.

[0058] Step S4, based on the current moment's azimuth attitude prior estimation value, the current moment's azimuth attitude measurement matrix and the current moment's Kalman gain matrix, the measurement values of the gyroscope and the encoder feedback are integrated to obtain the current moment's turntable azimuth attitude estimation value.

[0059] In this embodiment of the present invention, the estimated value of the turntable's orientation and attitude at the current moment is determined by the following formula:

[0060]

[0061] in,

[0062]

[0063]

[0064]

[0065] Where, is the estimated value of the turntable's orientation and attitude at the current moment, is the prior estimate of the orientation and attitude at the current moment, is the Kalman gain matrix at the current moment, is the orientation and attitude measurement matrix at the current moment, is the observer, is the turntable azimuth angle measurement value fed back by the encoder at the current moment, is the turntable azimuth angular velocity measurement value fed back by the gyroscope at the current moment, is the estimated value of the turntable orientation at the initial moment, It is the initial orientation angle of the turntable fed back by the encoder.

[0066] In this embodiment, a residual-based adaptive method is used to adjust the covariance weight in real time to balance the uncertainty of the system noise. When a complex environment with large interference noise such as strong winds and heavy rain occurs, the observation noise covariance matrix and the process noise covariance matrix can be adjusted based on the residual to adjust the values in the Kalman gain matrix to achieve the weight trust adjustment of the two sensor measurement values, and output the turntable azimuth and attitude estimation value at the current moment, so that the system can achieve the effect of high-frequency dependence on the gyroscope and low-frequency dependence on the code disk. It can output turntable azimuth and attitude estimation values with different emphases under different environmental interference intensities, so that the estimation of the turntable azimuth and attitude can adapt to different complex environments, focusing on accuracy when the environmental interference is small, and focusing on speed and reliability when the environmental interference is large.

[0067] Step S5: Based on the predicted covariance matrix and the Kalman gain matrix at the current moment, the covariance matrix at the current moment is updated for use at the next moment.

[0068] In this step, the covariance matrix at the current moment can be updated as follows:

[0069]

[0070] Among them, the identity matrix .

[0071] Regarding step 104:

[0072] In some implementations, step 104 may include steps B1-B5:

[0073] Step B1, calculating the residual based on the orientation and attitude measurement matrix at the current moment and the orientation and attitude prior estimation value at the current moment.

[0074] Specifically:

[0075]

[0076] Where, is the residual at the current moment, is the orientation and attitude measurement matrix at the current moment, is the observer, is the prior estimate of the orientation and attitude at the current moment.

[0077] Step B2: Use the sliding window method to estimate the residual covariance.

[0078] Specifically:

[0079]

[0080] Where, is the residual covariance, is the residual at the i-th moment, is the transpose of the residual at the i-th moment, and t is the current moment.

[0081] Step B3: Based on the residual covariance and the Kalman gain matrix at the current moment, the process noise covariance matrix is updated and adjusted, and the process noise covariance matrix is numerically symmetrized and positively definited.

[0082] In some embodiments, step B3 may include:

[0083]

[0084]

[0085] in, is the process noise covariance matrix after initial update adjustment, is the Kalman gain matrix at the current moment, is the residual covariance, is the process noise covariance matrix after numerical symmetry and positive definite processing, the constant , the identity matrix , is a small positive definite matrix.

[0086] In this embodiment, after the residual is used to update the process noise covariance, in order to avoid the loss of symmetry and positive definiteness of the covariance matrix due to numerical calculation errors, the process noise covariance needs to be numerically symmetrized and positively definite to obtain the final updated process noise covariance matrix.

[0087] Step B4: based on the residual covariance and the prediction covariance matrix, update and adjust the observation noise covariance matrix, and perform numerical symmetry and positive definite processing on the observation noise covariance matrix.

[0088] In some embodiments, step B4 may include:

[0089]

[0090]

[0091] Where, is the observation noise covariance matrix after initial update adjustment, is the residual covariance, is the observer, is the prediction covariance matrix, is the observation noise covariance matrix after numerical symmetry and positive definite processing, is a small positive definite matrix.

[0092] In summary, this scheme uses the multi-sensor information fusion method to realize the turntable azimuth and attitude estimation function, which can solve the problem of difficulty in quickly and accurately estimating the turntable azimuth and attitude under complex working conditions. This solution utilizes MEMS gyroscopes and encoders. MEMS gyroscopes offer fast high-frequency response but drift errors, while encoders offer precise measurement results but poor high-frequency response. This solution fully considers model errors and combines the strengths of both sensor types through a Kalman filter algorithm. A residual-based adaptive method adjusts covariance weights in real time to balance system noise uncertainty. In complex environments with high levels of interference noise, such as strong winds or heavy rainfall, the observation noise covariance matrix and the process noise covariance matrix are adjusted based on the residuals, allowing for real-time adjustments to the values in the Kalman gain matrix. This weighting of the two sensor measurements is then adjusted, resulting in an estimated turntable orientation and attitude at the current moment. This system achieves a high-frequency reliance on the gyroscope and a low-frequency reliance on the encoder. This allows for different emphases on the turntable orientation and attitude estimates under varying levels of interference, enabling the turntable orientation and attitude estimation to adapt to diverse environments, prioritizing accuracy in low-interference environments and speed and reliability in high-interference environments. This approach delivers a highly accurate and responsive turntable orientation and attitude estimate regardless of the environment.

[0093] Please refer to Figure 2 The embodiment of the present invention provides a turntable orientation and attitude estimation device based on a gyroscope-code disk, the device comprising:

[0094] The acquisition unit 201 is used to perform the following operations for the turntable azimuth attitude estimation at each moment: obtaining the turntable azimuth angular velocity measurement value fed back by the gyroscope and the turntable azimuth angle measurement value fed back by the encoder at the current moment to construct the azimuth attitude measurement matrix at the current moment;

[0095] A fusion unit 202 is configured to use a Kalman filter algorithm to fuse the measurement values of the gyroscope and the encoder feedback based on the current moment's azimuth attitude measurement matrix, the current moment's azimuth attitude prior estimate, and the process noise covariance matrix and observation noise covariance matrix updated and adjusted at the previous moment, to obtain an estimated value of the turntable's azimuth attitude at the current moment; wherein the azimuth attitude includes an azimuth angular velocity and an azimuth angle;

[0096] The updating unit 203 is used to calculate the residual based on the azimuth attitude measurement matrix at the current moment and the azimuth attitude prior estimation value at the current moment, so as to adaptively update the adjustment process noise covariance matrix and the observation noise covariance matrix based on the residual, so as to iteratively estimate the turntable azimuth attitude at the next moment.

[0097] In one embodiment of the present invention, the fusion unit 202 is configured to perform:

[0098] Determine a priori estimated value of the turntable's orientation and attitude at the current moment based on the estimated value of the turntable's orientation and attitude at the previous moment;

[0099] Obtain the updated and adjusted process noise covariance matrix at the previous moment and the covariance matrix at the previous moment to determine the predicted covariance matrix;

[0100] Obtaining the observation noise covariance matrix updated and adjusted at the previous moment, so as to update the Kalman gain matrix at the current moment based on the prediction covariance matrix and the obtained observation noise covariance matrix;

[0101] Based on the current moment’s azimuth attitude prior estimate, the current moment’s azimuth attitude measurement matrix, and the current moment’s Kalman gain matrix, the current moment’s turntable azimuth attitude estimate is obtained by fusing the measurement values of the gyroscope and the encoder feedback.

[0102] Based on the predicted covariance matrix and the Kalman gain matrix at the current moment, the covariance matrix at the current moment is updated for call at the next moment.

[0103] In one embodiment of the present invention, the prediction covariance matrix in the fusion unit 202 is calculated as follows:

[0104]

[0105] in,

[0106]

[0107]

[0108]

[0109] Where, is the prediction covariance matrix, is the state transition matrix, is the transpose of the state transfer matrix, is the covariance matrix of the previous moment, is the current process noise covariance matrix, is the initial covariance matrix, is the initial angle error used to characterize the accuracy of the code disk, is the initial angular velocity error used to characterize the gyroscope bias, is the sampling time interval of the gyroscope, is the integral error of the code disk turntable angle, is the angular velocity disturbance of the gyroscope turntable.

[0110] In one embodiment of the present invention, the estimated value of the turntable orientation and attitude at the current moment in the fusion unit 202 is determined by the following formula:

[0111]

[0112] in,

[0113]

[0114]

[0115]

[0116] Where, is the estimated value of the turntable's orientation and attitude at the current moment, is the prior estimate of the orientation and attitude at the current moment, is the Kalman gain matrix at the current moment, is the orientation and attitude measurement matrix at the current moment, is the observer, is the turntable azimuth angle measurement value fed back by the encoder at the current moment, is the turntable azimuth angular velocity measurement value fed back by the gyroscope at the current moment, is the estimated value of the turntable orientation at the initial moment, It is the initial orientation angle of the turntable fed back by the encoder.

[0117] In one embodiment of the present invention, the updating unit 203 is configured to perform:

[0118] Calculate the residual based on the current moment's orientation and attitude measurement matrix and the current moment's orientation and attitude prior estimation value;

[0119] The sliding window method is used to estimate the residual covariance;

[0120] Based on the residual covariance and the Kalman gain matrix at the current moment, the process noise covariance matrix is updated and adjusted, and the process noise covariance matrix is numerically symmetrized and positively definite;

[0121] Based on the residual covariance and prediction covariance matrix, the observation noise covariance matrix is updated and adjusted, and the observation noise covariance matrix is numerically symmetrized and positively definited.

[0122] In one embodiment of the present invention, when the updating unit 203 updates and adjusts the process noise covariance matrix based on the residual covariance and the Kalman gain matrix at the current moment, and performs numerical symmetrization and positive definite processing on the process noise covariance matrix, it is configured to execute:

[0123]

[0124]

[0125] in, is the process noise covariance matrix after initial update adjustment, is the Kalman gain matrix at the current moment, is the residual covariance, is the process noise covariance matrix after numerical symmetry and positive definite processing, is a small positive definite matrix.

[0126] It should be noted that the above-mentioned device embodiment and method embodiment belong to the same concept, and their specific implementation process is detailed in the method embodiment, which will not be repeated here.

[0127] An embodiment of the present application also provides a computer device, which includes a processor and a memory, wherein the memory stores at least one instruction, at least one program, code set or instruction set, and the at least one instruction, at least one program, code set or instruction set is loaded and executed by the processor to implement the gyroscope-code disk-based turntable azimuth and attitude estimation method provided in the above-mentioned method embodiments.

[0128] An embodiment of the present application also provides a computer-readable storage medium, on which is stored at least one instruction, at least one program, code set or instruction set, and the at least one instruction, at least one program, code set or instruction set is loaded and executed by a processor to implement the gyroscope-code disk-based turntable azimuth and attitude estimation method provided in the above-mentioned method embodiments.

[0129] An embodiment of the present application also provides a computer program product, which includes a computer program. A processor of a computer device reads the computer program from a computer-readable storage medium, and the processor executes the computer program, so that the computer device executes any of the gyroscope-code disk-based turntable orientation and attitude estimation methods in the above-mentioned embodiments.

[0130] For the convenience of description, the above devices or apparatuses are described as being divided into various modules or units according to their functions. Of course, when implementing the present application, the functions of each unit can be implemented in the same or multiple software and / or hardware.

[0131] Through the description of the above embodiments, it can be seen that those skilled in the art can clearly understand that the present application can be implemented by means of software plus the necessary general hardware platform. Based on this understanding, the technical solution of the present application, or the part that contributes to the prior art, can be embodied in the form of a software product, which can be stored in a storage medium such as ROM / RAM, a magnetic disk, an optical disk, etc., and includes a number of instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods of each embodiment of the present application or certain parts of the embodiments.

[0132] Finally, it should be noted that, in this document, relational terms such as first, second, third, and fourth are used only 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 "comprises," "comprising," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus comprising a series of elements includes not only those elements, but also other elements not explicitly listed, or elements inherent to such process, method, article, or apparatus. In the absence of further limitations, an element defined by the phrase "comprising a ..." does not exclude the presence of additional identical elements in the process, method, article, or apparatus comprising the element.

[0133] The above is only a preferred embodiment of the present application. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present application. These improvements and modifications should also be regarded as the scope of protection of the present application.

Claims

1. A turntable orientation and attitude estimation method based on a gyroscope-code disk, characterized in that: include: For the turntable orientation and attitude estimation at each moment, the following operations are performed: Obtain the turntable azimuth angular velocity measurement value fed back by the gyroscope and the turntable azimuth angle measurement value fed back by the encoder at the current moment to construct the azimuth attitude measurement matrix at the current moment; Based on the azimuth attitude measurement matrix at the current moment, the prior estimate of the azimuth attitude at the current moment, and the process noise covariance matrix and observation noise covariance matrix updated and adjusted at the previous moment, a Kalman filter algorithm is used to fuse the measurement values fed back by the gyroscope and the encoder to obtain an estimated azimuth attitude value of the turntable at the current moment; wherein the azimuth attitude includes the azimuth angular velocity and the azimuth angle; Calculating a residual based on the azimuth attitude measurement matrix at the current moment and a priori estimated value of the azimuth attitude at the current moment, so as to adaptively update and adjust the process noise covariance matrix and the observation noise covariance matrix based on the residual, so as to iteratively estimate the turntable azimuth attitude at the next moment; The turntable's current orientation and attitude estimate is calculated as follows: Determine a priori estimated value of the turntable's orientation and attitude at the current moment based on the estimated value of the turntable's orientation and attitude at the previous moment; Obtain the updated and adjusted process noise covariance matrix at the previous moment and the covariance matrix at the previous moment to determine the predicted covariance matrix; Obtaining the observation noise covariance matrix that was updated and adjusted at the previous moment, so as to update the Kalman gain matrix at the current moment based on the prediction covariance matrix and the obtained observation noise covariance matrix; Based on the current moment's azimuth attitude prior estimation value, the current moment's azimuth attitude measurement matrix and the current moment's Kalman gain matrix, the measurement values fed back by the gyroscope and the encoder are integrated to obtain the current moment's azimuth attitude estimation value of the turntable; Based on the predicted covariance matrix and the Kalman gain matrix at the current moment, updating the covariance matrix at the current moment for calling at the next moment; The prediction covariance matrix is calculated as follows: in, Where, is the prediction covariance matrix, is the state transition matrix, is the transpose of the state transfer matrix, is the covariance matrix of the previous moment, is the current process noise covariance matrix, is the initial covariance matrix, is the initial angle error used to characterize the accuracy of the code disk, is the initial angular velocity error used to characterize the gyroscope bias, is the sampling time interval of the gyroscope, is the initial process noise covariance matrix, is the integral error of the code disk turntable angle, is the angular velocity disturbance of the gyroscope turntable; The estimated value of the turntable's orientation and attitude at the current moment is determined by the following formula: in, Where, is the estimated value of the turntable's orientation and attitude at the current moment, is the prior estimate of the orientation and attitude at the current moment, is the Kalman gain matrix at the current moment, is the orientation and attitude measurement matrix at the current moment, is the observer, is the turntable azimuth angle measurement value fed back by the encoder at the current moment, is the turntable azimuth angular velocity measurement value fed back by the gyroscope at the current moment, is the estimated value of the turntable orientation at the initial moment, The initial azimuth angle of the turntable fed back by the encoder; The method comprises calculating a residual based on the azimuth attitude measurement matrix at the current moment and a priori estimated value of the azimuth attitude at the current moment, so as to adaptively update and adjust the process noise covariance matrix and the observation noise covariance matrix based on the residual, including: Calculate the residual based on the current moment's orientation and attitude measurement matrix and the current moment's orientation and attitude prior estimation value; The sliding window method is used to estimate the residual covariance; Based on the residual covariance and the Kalman gain matrix at the current moment, updating and adjusting the process noise covariance matrix, and performing numerical symmetry and positive definite processing on the process noise covariance matrix; Based on the residual covariance and the prediction covariance matrix, updating and adjusting the observation noise covariance matrix, and performing numerical symmetrization and positive definite processing on the observation noise covariance matrix; Based on the residual covariance and the prediction covariance matrix, the observation noise covariance matrix is updated and adjusted, and the observation noise covariance matrix is numerically symmetrized and positively definited, including: Where, is the observation noise covariance matrix after initial update adjustment, is the residual covariance, is the observer, is the prediction covariance matrix, is the observation noise covariance matrix after numerical symmetry and positive definite processing, is a small positive definite matrix.

2. The method according to claim 1, wherein The updating and adjusting process noise covariance matrix based on the residual covariance and the Kalman gain matrix at the current moment, and performing numerical symmetry and positive definite processing on the process noise covariance matrix, includes: in, is the process noise covariance matrix after initial update adjustment, is the Kalman gain matrix at the current moment, is the residual covariance, is the process noise covariance matrix after numerical symmetry and positive definite processing, is a small positive definite matrix.

3. A turntable orientation and attitude estimation device based on a gyroscope and a code disk, used to implement the steps of the method according to any one of claims 1 to 2, characterized in that: include: The acquisition unit is used to perform the following operations for the turntable azimuth attitude estimation at each moment: obtaining the turntable azimuth angular velocity measurement value fed back by the gyroscope and the turntable azimuth angle measurement value fed back by the encoder at the current moment, so as to construct the azimuth attitude measurement matrix at the current moment; A fusion unit is configured to fuse the measurement values fed back by the gyroscope and the encoder using a Kalman filter algorithm based on the azimuth attitude measurement matrix at the current moment, the prior estimation value of the azimuth attitude at the current moment, and the process noise covariance matrix and observation noise covariance matrix updated and adjusted at the previous moment, to obtain an estimated value of the turntable's azimuth attitude at the current moment; wherein the azimuth attitude includes an azimuth angular velocity and an azimuth angle; An updating unit is used to calculate a residual based on the azimuth attitude measurement matrix at the current moment and a priori estimation value of the azimuth attitude at the current moment, so as to adaptively update and adjust the process noise covariance matrix and the observation noise covariance matrix based on the residual, so as to iteratively estimate the turntable azimuth attitude at the next moment.

4. A computer device, characterized in that: The computer device 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 in the memory to implement the steps of any one of the methods described in claims 1-2.

5. A computer-readable storage medium, characterized in that The storage medium stores a computer program, which, when executed by a processor, implements the steps of the method according to any one of claims 1 to 2.

6. A computer program product, characterized in that The method comprises a computer program, which implements the steps of the method according to any one of claims 1 to 2 when the computer program is executed by a processor.

Images