Adaptive turntable space attitude estimation method and device based on multiple code disks

Abstract

The application discloses a kind of adaptive turntable space attitude estimation method and device based on multiple code disc, belong to the field of attitude estimation.Method includes: for each time's turntable space attitude estimation, all execute: the turntable azimuth angle measurement value of current time absolute code disc feedback and the turntable pitch angular velocity measurement value of incremental code disc feedback are acquired, to construct the space attitude measurement matrix of current time;By Kalman filtering algorithm, the measurement value of absolute code disc and incremental code disc feedback is fused, and the turntable space attitude estimation value of current time is obtained;Residual-based adaptive method is constructed, and process noise covariance matrix and observation noise covariance matrix are dynamically adjusted to cope with time-varying noise and model error.The scheme combines the advantages of two kinds of sensors of absolute code disc and incremental code disc, and adjusts covariance weight based on residual in real time to balance the uncertainty of system noise, with high accuracy and high safety.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of attitude estimation, in particular to a multi-code-disk-based adaptive space attitude estimation method and device for a turntable. BACKGROUND

[0002] In actual supervision and management work, relevant departments mainly monitor, track and collect evidence on target objects through turntable equipment installed at key routes. However, due to factors such as insufficient machining precision of parts, cost constraints and large system errors, the existing turntable equipment cannot accurately estimate the space attitude of the turntable, which causes difficulties for law enforcement departments to collect evidence. In the traditional method, a single code-disk sensor is used to directly feed back the space attitude of the turntable, which has a large system error and is not stable enough to accurately estimate the space attitude of the turntable. Other conventional signal processing methods (such as mean filtering) can only reduce a small amount of error and are not adaptive enough to accurately estimate the space attitude of the turntable.

[0003] Therefore, there is an urgent need to provide a multi-code-disk-based adaptive space attitude estimation method and device for a turntable. SUMMARY

[0004] To solve the problem of large system error and insufficient stability of the traditional single code-disk sensor in complex working conditions, the present application provides a multi-code-disk-based adaptive space attitude estimation method and device for a turntable.

[0005] In one aspect, a multi-code-disk-based adaptive space attitude estimation method for a turntable is provided, which comprises:

[0006] For the space attitude estimation of each moment, the following is performed:

[0007] Obtain the azimuth angle measurement value of the turntable fed back by the absolute code-disk and the pitch angle velocity measurement value of the turntable fed back by the incremental code-disk at the current moment to construct a space attitude measurement matrix at the current moment;

[0008] Based on the space attitude measurement matrix at the current moment, the space attitude priori estimation value for the current moment, and the process noise covariance matrix and the observation noise covariance matrix updated and adjusted at the last moment, the measurement values fed back by the absolute code-disk and the incremental code-disk are fused by using the Kalman filtering algorithm to obtain the space attitude estimation value of the turntable at the current moment;

[0009] Based on the space attitude measurement matrix at the current moment and the space attitude priori estimation value for the current moment, a residual error is calculated to adaptively update and adjust the process noise covariance matrix and the observation noise covariance matrix based on the residual error, so as to iteratively estimate the space attitude of the turntable at the next moment.

[0010] In another aspect, a multi-code-disk based adaptive turntable space attitude estimation device based on the steps of any method embodiment of the specification is provided, the device comprising:

[0011] An acquisition unit is configured to, for each time point of turntable space attitude estimation, acquire a turntable azimuth angle measurement value fed back by an absolute code disk and a turntable pitch angle velocity measurement value fed back by an incremental code disk at the current time point to construct a space attitude measurement matrix at the current time point.

[0012] A fusion unit is configured to fuse the measurement values fed back by the absolute code disk and the incremental code disk based on the space attitude measurement matrix at the current time point, a space attitude prior estimation value at the current time point, and an updated process noise covariance matrix and an observation noise covariance matrix at the previous time point, to obtain a turntable space attitude estimation value at the current time point by using a Kalman filtering algorithm.

[0013] An update unit is configured to calculate a residual based on the space attitude measurement matrix at the current time point and the space attitude prior estimation value at the current time point, to update the process noise covariance matrix and the observation noise covariance matrix adaptively based on the residual, and to iteratively perform turntable space attitude estimation at the next time point.

[0014] In another aspect, a computer device is provided, the computer device comprising a memory and a processor, the memory being configured to store a computer program, and the processor being configured to execute the computer program stored on the memory to implement the steps of the above method.

[0015] In another aspect, a computer readable storage medium is provided, the storage medium storing a computer program, the computer program being executed by a processor to implement the steps of the above method.

[0016] In another aspect, a computer program product is provided, comprising a computer program, the computer program being executed by a processor to implement the steps of the above method.

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

[0018] Although the absolute code disc adopts the brushless motor with high precision, it cannot be self-locked without power supply, and it is unsafe in standby state when measuring the pitch angle of the turntable, which can cause the turntable to be knocked down, and the pitch precision cannot be guaranteed. Although the incremental code disc is equipped with a brush motor, it has lower precision and is economical and suitable, but it can maximize the safety of the turntable in the standby state in the pitch direction. By combining the advantages of the absolute code disc and the incremental code disc, the azimuth measurement of the absolute code disc and the pitch angle measurement of the code disc are fused through the Kalman filtering algorithm, so that the system can achieve the effect of relying on the measurement result when the state is stable and relying on the prediction result when the state is unstable. At the same time, the covariance weight is adjusted in real time based on the residual error to balance the uncertainty of the system noise, which can greatly improve the accuracy and safety of the turntable space attitude estimation under complex working conditions. BRIEF DESCRIPTION OF DRAWINGS

[0019] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiments or the prior art description. Obviously, the drawings described below are some embodiments of the present application, and other drawings can also be obtained by those skilled in the art without creative labor.

[0020] Figure 1 It is a flow chart of a self-adaptive turntable space attitude estimation method based on multiple code discs provided by an embodiment of the present application.

[0021] Figure 2 It is a structure diagram of a self-adaptive turntable space attitude estimation device based on multiple code discs provided by an embodiment of the present application.

[0022] Figure 3 It is a hardware architecture diagram of a computer device provided by an embodiment of the present application. DETAILED DESCRIPTION

[0023] In order to make the purpose, technical solutions and advantages of the embodiments of the present application more clear, the technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are some embodiments of the present application, not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor are within the protection scope of the present application.

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

[0025] Please refer to Figure 1 The self-adaptive turntable space attitude estimation method based on multiple code discs provided by the embodiments of the present application comprises:

[0026] Step 100: For each time point of the turntable space attitude estimation, the absolute code disc feedback turntable azimuth angle measurement value and the incremental code disc feedback turntable pitch angle velocity measurement value at the current time point are obtained to construct a space attitude measurement matrix at the current time point;

[0027] Step 102: Based on the space attitude measurement matrix at the current time point, the space attitude prior estimation value at the current time point, and the process noise covariance matrix and the observation noise covariance matrix updated and adjusted at the last time point, the measurement values fed back by the absolute code disc and the incremental code disc are fused by using the Kalman filtering algorithm to obtain the turntable space attitude estimation value at the current time point.

[0028] Step 104: Based on the space attitude measurement matrix at the current time point and the space attitude prior estimation value at the current time point, a residual error is calculated to update and adjust the process noise covariance matrix and the observation noise covariance matrix based on the residual error, so as to iteratively perform the turntable space attitude estimation at the next time point.

[0029] In the embodiment of the application, although the absolute code disc adopts the brushless motor which has high precision, it cannot be self-locked without power supply, and it is unsafe in the standby state when measuring the pitch angle of the turntable, which may cause the turntable to be dropped, so the pitch precision cannot be guaranteed. Although the incremental code disc is provided with the brush motor which has lower precision and is economical and suitable, the safety in the standby state of the turntable in the pitch direction can be maximally guaranteed. By combining the advantages of the two kinds of sensors of the absolute code disc and the incremental code disc, the azimuth measurement of the absolute code disc and the pitch angle measurement of the code disc are fused by using the Kalman filtering algorithm, the system can achieve the effect of relying on the measurement result when the system is stable and relying on the prediction result when the system is understable, and the covariance weight is adjusted in real time based on the residual error to balance the uncertainty of the system noise, so the accuracy and safety of the turntable space attitude estimation under complex working conditions can be greatly improved.

[0030] The following describes Figure 1 the execution mode of each step.

[0031] For step 100:

[0032] In the embodiment, the value directly fed back by the absolute code disc is the turntable azimuth angle measurement value θ a,t , and the value directly fed back by the incremental code disc is the turntable pitch angle velocity measurement value ω e,t . In the embodiment, the parameters directly measured by the two kinds of code discs are used to construct the space attitude measurement matrix, the turntable azimuth angle velocity is not calculated from the turntable azimuth angle measurement value, and the turntable pitch angle is not calculated from the turntable pitch angle velocity measurement value to construct the space attitude measurement matrix, so the error caused by conversion can be avoided, and the problem of poor space attitude estimation accuracy caused by low precision and reliability of the space attitude measurement matrix can be avoided.

[0033] For step 102:

[0034] In some embodiments, step 102 can include steps S1-S5:

[0035] Step S1, based on the last time's turntable space attitude estimation value, determine the space attitude priori estimation value for the current time.

[0036] In this step, the space attitude priori estimation value Θ t|t-1 may be expressed as:

[0037] Θ t|t-1 = F * Θ t-1|t-1

[0038] Wherein, the state transition matrix Δt is the sampling time interval, Θ t-1|t-1 is the last time's turntable space attitude estimation value.

[0039] It should be noted that the initial time's turntable space attitude estimation value Wherein, θ0 is the absolute code disc feedback initial azimuth angle of the turntable, θ e,0 is the incremental code disc feedback initial pitch angle of the turntable.

[0040] Step S2, obtain the last time's updated process noise covariance matrix and the last time's covariance matrix, and determine the predicted covariance matrix.

[0041] In some embodiments, the predicted covariance matrix is calculated by the following way:

[0042] P t|t-1 = F * P t-1|t-1 * F T + Q t

[0043] Wherein,

[0044]

[0045] In the formula, P t|t-1 is the predicted covariance matrix, F is the state transition matrix, F T is the transpose of the state transition matrix, P t-1|t-1 is the last time's covariance matrix, Q t is the current process noise covariance matrix, P 0|0 is the initial covariance matrix, σ a,θ is the initial azimuth angle error used to represent the absolute code disc accuracy, σ e,θ is the initial pitch angle error used to represent the incremental code disc accuracy, σ a,ω and σ e,ωrespectively, initial azimuth angle velocity error and initial pitch angle velocity error, which can be set to a large value (such as 1 rad / s) to quickly converge, Δt is a sampling time interval, q a,θ and q a,ω respectively, absolute code disc azimuth angle integral error and azimuth angle velocity disturbance, q e,θ and q e,ω respectively, incremental code disc pitch angle integral error and pitch angle velocity disturbance.

[0046] In the embodiment, by constructing the process noise covariance matrix based on the absolute code disc azimuth angle integral error and the azimuth angle velocity disturbance, and the incremental code disc pitch angle integral error and the pitch angle velocity disturbance, and constantly updating and adjusting the process noise covariance matrix based on the residual, the prediction covariance weight can be adjusted in real time, and the system process noise can be balanced.

[0047] In step S3, the observation noise covariance matrix updated and adjusted at the last time is obtained, so as to update the Kalman gain matrix at the current time based on the prediction covariance matrix and the obtained observation noise covariance matrix.

[0048] In the embodiment of the application, the Kalman gain matrix at the current time is calculated by the following formula:

[0049] K t =P t|t-1 *H T *(H*P t|t-1 *H T +R t ) -1

[0050] wherein,

[0051]

[0052] In the formula, K t is the Kalman gain matrix at the current time, P t|t-1 is the prediction covariance matrix, H is the observer, R t is the current observation noise covariance matrix, R0 is the initial observation noise covariance matrix, r θ is the angle measurement variance of the absolute code disc, r ω is the angle velocity measurement variance of the incremental code disc.

[0053] In the embodiment, the observation noise covariance matrix is constructed based on the angular velocity measurement variance of the incremental encoder and the angular measurement variance of the absolute encoder, and the observation noise covariance matrix is constantly updated based on the residual, so as to adjust the value of the Kalman gain matrix in real time, dynamically adjust the weight of the two sensor data on the final estimation through the Kalman gain matrix, balance the system observation noise, and enable the system to achieve the effect of relying on the measurement result when the state is stable and relying on the predicted result when the state is unstable.

[0054] In step S4, the measurement values fed back by the absolute encoder and the incremental encoder are fused based on the spatial attitude prior estimation value at the current moment, the spatial attitude measurement matrix at the current moment and the Kalman gain matrix at the current moment, to obtain the spatial attitude estimation value of the turntable at the current moment.

[0055] In the embodiment of the application, the spatial attitude estimation value of the turntable at the current moment is determined by the following formula:

[0056] Θ t|t = Θ t|t-1 + K t *(Z t -H*Θ t|t-1 )

[0057] Wherein,

[0058]

[0059] In the formula, Θ t|t is the spatial attitude estimation value of the turntable at the current moment, Θ t|t-1 is the spatial attitude prior estimation value at the current moment, K t is the Kalman gain matrix at the current moment, Z t is the spatial attitude measurement matrix at the current moment, H is the observer, θ a,t is the azimuth angle measurement value of the turntable fed back by the absolute encoder at the current moment, ω e,t is the pitch angle velocity measurement value of the turntable fed back by the incremental encoder at the current moment, ω a,t is the azimuth angle velocity estimation value of the turntable, θ e,t is the pitch angle estimation value of the turntable, Θ 0|0 is the spatial attitude estimation value of the turntable at the initial moment, θ a,0 is the initial azimuth angle of the turntable fed back by the absolute encoder, θ e,0 is the initial pitch angle of the turntable fed back by the incremental encoder, which is usually set to zero after self-checking of the equipment.

[0060] Since the spatial attitude measurement matrix Z t is composed of the direct feedback values θ a,t and ω e,t as the measurement values, the spatial attitude estimation value Θt|t In addition to the direct feedback value θ a,t , ω e,t , the azimuth angle speed estimation value ω a,t and the azimuth angle estimation value θ e,t predicted by adjusting the weight with the Kalman gain matrix are also included.

[0061] Therefore, in the embodiment, the residual-based adaptive method is used to adjust the covariance weight in real time, balance the uncertainty of the system noise, and when there is a large wind, heavy rain and other complex environments with large interference noise, the residual-based method can be used to adjust the observation noise covariance matrix and the process noise covariance matrix to adjust the values in the Kalman gain matrix, so as to adjust the weight of the measurement values of the two sensors, output the current time space attitude estimation value of the turntable, and make the system achieve the effect of relying on the measurement value at low frequency and relying on the predicted estimation value at high frequency. The output of the turntable space attitude estimation value can be different in different environmental interference intensities, so that the estimation of the turntable space attitude can adapt to different complex environments, and the measurement value is emphasized when the environmental interference is small, and the predicted estimation value is emphasized when the environmental interference is large.

[0062] Step S5, based on the predicted covariance matrix and the Kalman gain matrix at the current time, updating the covariance matrix at the current time for calling at the next time.

[0063] In this step, the covariance matrix at the current time can be updated in the following way:

[0064] P t|t =(I-K t *H)*P t|t-1

[0065] Wherein, the unit matrix

[0066] For step 104:

[0067] In some embodiments, step 104 can include steps B1-B5:

[0068] Step B1, calculating the forgetting factor d t at the current time.

[0069]

[0070] Wherein, b is the forgetting factor parameter, the value range is usually 0.95≤b≤0.99, and t is the current time for the first cycle.

[0071] Step B2, calculating the residual based on the space attitude measurement matrix at the current time and the space attitude prior estimation value for the current time.

[0072] Specifically:

[0073] e t = Z t -H*Θ t|t-1

[0074] wherein e t is the residual at the current time, Z t is the spatial pose measurement matrix at the current time, H is the observer, and Θ t|t-1 is the spatial pose prior estimate for the current time.

[0075] Step B3, based on the residual and the Kalman gain matrix at the current time, calculate the uncertainty error of the residual, and based on the covariance matrices at the last time and the current time, calculate the covariance variation to update and adjust the process noise covariance matrix in combination with the forgetting factor at the current time, and perform numerical symmetrization and positive definitization processing on the process noise covariance matrix.

[0076] In some embodiments, step B3 can include:

[0077] The process noise covariance matrix is updated and adjusted by the following formula:

[0078] M = K t *e t *e t T *K t T

[0079] N = (P t|t -F*P t-1|t-1 *F T )

[0080]

[0081] The numerical symmetrization and positive definitization processing on the process noise covariance matrix is:

[0082]

[0083] wherein, is the initial updated and adjusted process noise covariance matrix, M is the uncertainty error of the residual, used to represent the uncertainty error of the residual fed back to the state space through the Kalman gain matrix, K t is the Kalman gain matrix at the current time, e t is the residual, N is the covariance variation, used to reflect the uncaught dynamics, P t-1|t-1 and P t|t are the covariance matrices at the last time and the current time, respectively, F is the state transition matrix, Q t is the historical process noise covariance matrix, and d tis a small positive definite matrix, and is a small positive definite matrix.

[0084] In this embodiment, after updating the process noise covariance using the residual, in order to avoid numerical calculation errors causing the covariance matrix to lose symmetry and positive definiteness, the process noise covariance needs to be numerically symmetrized and positive definite, and the final updated process noise covariance matrix is obtained.

[0085] Step B4, based on the predicted covariance matrix, the predicted uncertainty error is calculated, combined with the square sum of the residual and the forgetting factor at the current time, the observation noise covariance matrix is updated and adjusted, and the observation noise covariance matrix is numerically symmetrized and positive definite.

[0086] In some embodiments, step B4 can include:

[0087] Y = H * P t|t-1 *H T

[0088]

[0089] In the formula, Y is the predicted uncertainty error, which is used to separate high-frequency noise and low-frequency stable model error, is the initial updated observation noise covariance matrix, e t is the residual, H is the observer, P t|t-1 is the prediction covariance matrix, R t+1 is the observation noise covariance matrix after numerical symmetrization and positive definite, ∈I is a small positive definite matrix, R t is the historical observation noise covariance matrix, d t is the forgetting factor at the current time. e t *e t T is the square of the measurement residual, which directly reflects the noise intensity.

[0090] In summary, the scheme realizes the function of space attitude estimation of a turntable by using a multi-code disc information fusion method, and can solve the problem of low accuracy of space attitude estimation of the turntable under complex working conditions. The scheme fully considers model errors, combines the advantages of two types of sensors by using a Kalman filtering algorithm, adjusts the covariance weight in real time by using a residual-based adaptive method, balances the uncertainty of system noise, adjusts the observation noise covariance matrix and the process noise covariance matrix based on the residual when a complex environment with large interference noise such as strong wind and heavy rain occurs, so as to adjust the values in the Kalman gain matrix in real time, adjust the weight of the measurement values of the two types of sensors, output the space attitude estimation value of the turntable at the current time, and make the system depend on the observation result when the system is stable and depend on the prediction result when the state is unstable, so that the output of the space attitude estimation value of the turntable can be different under different environmental interference intensities. Moreover, the adaptive method is used to adjust the covariance weight in real time, so as to balance the uncertainty of system noise, and finally the space attitude estimation technology of the turntable with high accuracy and safety can be obtained.

[0091] For reference Figure 2 The embodiment of the present application provides a kind of adaptive turntable space attitude estimation device based on multiple code disc, and the device comprises:

[0092] The acquisition unit 201 is used for the space attitude estimation of each time of turntable, and is executed: the turntable azimuth angle measurement value of current time absolute code disc feedback and the turntable pitch angle velocity measurement value of incremental code disc feedback are acquired, to construct the space attitude measurement matrix of current time;

[0093] The fusion unit 202 is used for based on the space attitude measurement matrix of current time, the space attitude priori estimation value for current time and the process noise covariance matrix and observation noise covariance matrix updated and adjusted last time, the measurement value of absolute code disc and incremental code disc feedback is fused by using Kalman filtering algorithm, and the space attitude estimation value of current time of turntable is obtained;

[0094] The update unit 203 is used for calculating residual based on the space attitude measurement matrix of current time and the space attitude priori estimation value for current time, to update and adjust the process noise covariance matrix and observation noise covariance matrix based on residual adaptive, to iterate the space attitude estimation of next time of turntable.

[0095] It should be noted that: the device embodiment and the method embodiment described above belong to the same concept, and the specific implementation process is shown in the method embodiment, which will not be repeated here.

[0096] The embodiment of the present application further provides a computer device, comprising a processor and a memory, wherein the memory stores at least one instruction, at least one program, a code set or an instruction set, and the processor loads and executes the at least one instruction, the at least one program, the code set or the instruction set to implement the multi-code-disk-based adaptive rotating table space attitude estimation method provided by any of the above method embodiments.

[0097] The embodiment of the present application further provides a computer readable storage medium, wherein the computer readable storage medium stores at least one instruction, at least one program, a code set or an instruction set, and a processor loads and executes the at least one instruction, the at least one program, the code set or the instruction set to implement the multi-code-disk-based adaptive rotating table space attitude estimation method provided by any of the above method embodiments.

[0098] The embodiment of the present application further provides a computer program product, comprising a computer program, wherein a processor of a computer device reads the computer program from a computer readable storage medium, and the processor executes the computer program to enable the computer device to execute the multi-code-disk-based adaptive rotating table space attitude estimation method in any of the above embodiments.

[0099] For the convenience of description, the above apparatus or apparatus is described in various modules or units in terms of functions. Of course, in the implementation of the present application, the functions of each unit can be implemented in the same or multiple software and / or hardware.

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

[0101] Finally, it needs to be pointed out that, in this article, the relationship terms such as first, second, third and fourth, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between the entities or operations. Moreover, the term "include", "contain" or any other variant thereof is intended to cover non-exclusive inclusion, so that the process, method, article or device including a series of elements not only includes those elements, but also includes other elements not explicitly listed or inherent to such process, method, article or device. Without more limitations, the element defined by the statement "including a" does not exclude the presence of other identical elements in the process, method, article or device including the element.

[0102] The above is only the preferred embodiment of the present application, and it should be pointed out that for ordinary skilled in the art, without departing from the principles of the present application, a number of improvements and refinements can also be made, which should be considered as the protection scope of the present application.

Claims

1. An adaptive turntable spatial attitude estimation method based on multiple encoders, characterized in that, include: For each moment of turntable spatial attitude estimation, the following is performed: Obtain the turntable azimuth angle measurement value fed back by the absolute encoder and the turntable pitch angular velocity measurement value fed back by the incremental encoder at the current moment to construct the spatial attitude measurement matrix at the current moment. Based on the spatial attitude measurement matrix at the current moment, the prior estimate of the spatial attitude at the current moment, and the updated and adjusted process noise covariance matrix and observation noise covariance matrix at the previous moment, the Kalman filter algorithm is used to fuse the measurement values ​​fed back by the absolute code disk and the incremental code disk to obtain the estimated value of the turntable spatial attitude at the current moment. The residuals are calculated based on the spatial attitude measurement matrix at the current moment and the prior estimation value of the spatial attitude at the current moment. The process noise covariance matrix and the observation noise covariance matrix are adaptively updated and adjusted based on the residuals to iteratively estimate the turntable spatial attitude at the next moment. The current turntable spatial attitude estimate is calculated as follows: Based on the turntable spatial attitude estimate from the previous moment, determine the prior spatial attitude estimate for the current moment. Obtain the updated and adjusted process noise covariance matrix from the previous time step and the covariance matrix from the previous time step, and determine the prediction covariance matrix. Obtain the observation noise covariance matrix updated and adjusted at the previous time step, and update the Kalman gain matrix at the current time step based on the predicted covariance matrix and the obtained observation noise covariance matrix; Based on the prior estimate of the spatial attitude at the current moment, the spatial attitude measurement matrix at the current moment, and the Kalman gain matrix at the current moment, the measurement values ​​fed back from the absolute code disk and the incremental code disk are fused to obtain the estimated value of the turntable spatial attitude at the current moment. Based on the predicted covariance matrix and the Kalman gain matrix at the current time, update the covariance matrix at the current time for use in the next time step; The step of calculating residuals based on the current spatial attitude measurement matrix and the prior spatial attitude estimate for the current time, and adaptively updating and adjusting the process noise covariance matrix and the observation noise covariance matrix based on the residuals, includes: Calculate the forgetting factor at the current moment; Calculate the residuals based on the spatial attitude measurement matrix at the current moment and the prior estimate of the spatial attitude at the current moment; Based on the residual and the Kalman gain matrix at the current time, the uncertainty error of the residual is calculated, and the covariance change is calculated based on the covariance matrix at the previous time and the current time. The process noise covariance matrix is ​​then updated and adjusted in conjunction with the forgetting factor at the current time, and the process noise covariance matrix is ​​numerically symmetric and positive definite. The uncertainty error of the prediction is calculated based on the predicted covariance matrix. The observation noise covariance matrix is ​​then updated and adjusted by combining the square of the residual and the forgetting factor at the current time. The observation noise covariance matrix is ​​then numerically symmetricized and positive definite.

2. The method as described in claim 1, characterized in that, The predicted covariance matrix is ​​calculated as follows: in, In the formula, To predict the covariance matrix, Here is the state transition matrix. This is the transpose of the state transition matrix. Let be the covariance matrix of the previous time step. The current process noise covariance matrix is... The initial covariance matrix, The initial azimuth angle error is used to characterize the absolute accuracy of the code disk. The initial pitch angle error is used to characterize the accuracy of the incremental encoder. and These are the initial azimuth angular velocity error and the initial pitch angular velocity error, respectively. The sampling time interval, and These are the azimuth angle integral error and azimuth angular velocity disturbance of the absolute code disk, respectively. and These are the incremental encoder pitch angle integral error and pitch angular velocity disturbance, respectively.

3. The method as described in claim 1, characterized in that, The current turntable spatial attitude estimate is determined by the following formula: in, In the formula, This is the estimated spatial attitude of the turntable at the current moment. For the prior estimate of the spatial attitude at the current moment, Let Kalman gain be the value at the current time step. This is the spatial attitude measurement matrix at the current moment. For the observer, This is the measured value of the turntable's azimuth angle as fed back by the absolute encoder at the current moment. This is the measured value of the turntable pitch angular velocity fed back by the incremental encoder at the current moment. This is an estimated value for the azimuth angular velocity of the turntable. This is an estimated value for the turntable's pitch angle. This represents the estimated spatial attitude of the turntable at the initial moment. The initial orientation angle of the turntable is based on feedback from the absolute encoder. The initial pitch angle of the turntable is fed back by the incremental encoder.

4. The method as described in claim 1, characterized in that, The uncertainty error of the residual is calculated based on the residual and the Kalman gain matrix at the current time, and the covariance change is calculated based on the covariance matrices at the previous and current times. This is then combined with the forgetting factor at the current time to update and adjust the process noise covariance matrix. The process noise covariance matrix is ​​then numerically symmetric and positive definite, including: The process noise covariance matrix is ​​updated and adjusted using the following formula: The process noise covariance matrix is ​​numerically symmetric and positive definite. in, Let M be the initial updated and adjusted process noise covariance matrix, and M be the uncertainty error of the residuals. Let Kalman gain be the value at the current time step. Let N be the residual, and N be the change in covariance. and These are the covariance matrices of the previous time step and the current time step, respectively. Here is the state transition matrix. The historical process noise covariance matrix, Forgetting factor at the current moment, The process noise covariance matrix is ​​the result of numerical symmetry and positive definite processing. It is a small positive definite matrix.

5. An adaptive turntable spatial attitude estimation device based on multiple encoders, used to implement the steps of the method described in any one of claims 1-4, characterized in that, include: The acquisition unit is used for the spatial attitude estimation of the turntable at each moment. It performs the following: acquires the measured value of the turntable azimuth angle fed back by the absolute encoder and the measured value of the turntable pitch angular velocity fed back by the incremental encoder at the current moment, so as to construct the spatial attitude measurement matrix at the current moment. The fusion unit is used to fuse the measured values ​​fed back from the absolute code disk and the incremental code disk based on the spatial attitude measurement matrix at the current time, the prior estimate of the spatial attitude at the current time, and the updated and adjusted process noise covariance matrix and observation noise covariance matrix at the previous time, using the Kalman filter algorithm to obtain the estimated value of the turntable spatial attitude at the current time. The update unit is used to calculate the residual based on the spatial attitude measurement matrix at the current time and the prior estimate of the spatial attitude at the current time, so as to adaptively update and adjust the process noise covariance matrix and the observation noise covariance matrix based on the residual, and iteratively perform turntable spatial attitude estimation at the next time step.

6. A computer device, characterized in that, The computer device includes a memory and a processor. The memory is used to store computer programs, and the processor is used to execute the computer programs stored in the memory to implement the steps of the method according to any one of claims 1-4.

7. 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 described in any one of claims 1-4.

8. A computer program product, characterized in that, Includes a computer program, which, when executed by a processor, implements the steps of the method according to any one of claims 1-4.

Images