Erecting control method, device and equipment based on Kalman filtering

Abstract

The invention provides an erection control method, device and equipment based on Kalman filtering, and the method comprises the steps: carrying out the time updating of Kalman filtering according to a posterior state result at a previous moment, and obtaining a prior state result at a current moment; whether the corresponding angle sensors are in an expected state at the current moment or not is judged according to the measurement residual errors of the two angle sensors at the current moment, one angle sensor is installed on the erecting frame, and the other angle sensor is installed on the launching pad; and if at least one angle sensor is in an expected state at the current moment, performing measurement updating of Kalman filtering according to the measurement information of the corresponding angle sensor at the current moment and the prior state result at the current moment, and obtaining a posterior state result at the current moment. According to the method and the device, the adverse effect of oil cylinder shaking, sensor damage or accidental abnormity on the Kalman filtering estimation result can be reduced, so that the control precision of the erecting action is improved, and the control time is shortened.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of erecting control, in particular to an erecting control method, device and equipment based on Kalman filtering. BACKGROUND

[0002] The erecting control technology is widely used in launch systems such as rockets and missiles, and plays a crucial role. The erecting control aims to accurately convert the launch object from a horizontal state to a vertical launch state, and the entire process has very high requirements for control accuracy, stability and reliability.

[0003] When using a traditional hydraulic system to perform the erecting action, a hydraulic pump and a proportional valve are needed to control the extension and retraction amount of the erecting cylinder to realize the adjustment of the erecting stand angle. In the related art, a Kalman filtering algorithm is used to combine the erecting angle predicted based on the erecting stand motion model and the erecting angle measured by the angle sensor installed on the erecting stand to obtain a more accurate erecting angle. However, it is found in actual use that the accuracy of the Kalman filtering estimation result is low in some links in the control process, resulting in low control accuracy and long control time of the erecting action. SUMMARY

[0004] The present application provides an erecting control method, device and equipment based on Kalman filtering, which can solve the technical problem of low accuracy of Kalman filtering estimation result in some links in the control process in the prior art.

[0005] In a first aspect, an erecting control method based on Kalman filtering is provided, which comprises: performing time update of Kalman filtering according to the posterior state result of the last moment to obtain the prior state result of the current moment, wherein the state result comprises an erecting angle; determining whether the corresponding angle sensor is in an expected state at the current moment according to the measurement residual of the two angle sensors at the current moment, wherein one angle sensor is installed on the erecting stand and the other angle sensor is installed on the launch platform; if at least one angle sensor is in an expected state at the current moment, performing measurement update of Kalman filtering according to the measurement information of the corresponding angle sensor at the current moment and the prior state result of the current moment to obtain the posterior state result of the current moment.

[0006] Further, in an embodiment, the step of performing time update of Kalman filtering according to the posterior state result of the last moment to obtain the prior state result of the current moment comprises: calculating the global prior state vector of the current moment according to the state transition matrix of the current moment and the global posterior state vector of the last moment; For the two angle sensors, the following steps are performed: Based on the current state transition matrix, the process noise covariance matrix and the posterior state covariance matrix of the angle sensor at the previous time, the prior state covariance matrix of the angle sensor at the current time is calculated. The step of performing Kalman filtering measurement updates based on the measurement information of the corresponding angle sensor at the current moment and the prior state result at the current moment, to obtain the posterior state result at the current moment if at least one angle sensor is in the expected state at the current moment, includes: If both angle sensors are in the expected state at the current moment, then preset operations are performed on the two angle sensors respectively to obtain the posterior state vector and posterior state covariance matrix of the two angle sensors at the current moment. The global posterior state vector and global posterior state covariance matrix of the two angle sensors at the current moment are obtained by fusing the posterior state vector and posterior state covariance matrix of the two angle sensors at the current moment. The information allocation factor of the two angle sensors at the current moment is calculated based on the posterior state covariance matrix of the two angle sensors at the current moment. If only one angle sensor is in the expected state at the current moment, then a preset operation is performed on that angle sensor to obtain the posterior state vector and posterior state covariance matrix of that angle sensor at the current moment. The posterior state vector of that angle sensor at the current moment is used as the global posterior state vector at the current moment. The posterior state covariance matrix of that angle sensor at the current moment is multiplied by the first adjustment coefficient to obtain the global posterior state covariance matrix at the current moment. The information allocation factor of the two angle sensors at the previous moment is used as the information allocation factor of the two angle sensors at the current moment. Based on the global posterior state covariance matrix and global process noise covariance matrix at the current moment, as well as the information allocation factors of the two angle sensors at the current moment, the new process noise covariance matrix and posterior state covariance matrix of the two angle sensors at the current moment are calculated. The preset operations include: The Kalman gain of the angle sensor at the current moment is calculated based on the prior state covariance matrix, measurement matrix, and measurement noise covariance matrix of the angle sensor at the current moment. The posterior state vector of the angle sensor at the current moment is calculated based on the global prior state vector, the Kalman gain, measurement matrix, and measurement vector of the angle sensor at the current moment. The posterior state covariance matrix of the angle sensor at the current moment is calculated based on the Kalman gain, measurement matrix, and prior state covariance matrix of the angle sensor at the current moment.

[0007] Furthermore, in one embodiment, after the step of determining whether the corresponding angle sensor is in the expected state at the current moment based on the measurement residuals of the two angle sensors at the current moment, the method further includes: If neither of the two angle sensors is in the expected state at the current moment, then the global prior state vector at the current moment is used as the global posterior state vector at the current moment. The sum of the prior state covariance matrices of the two angle sensors at the current moment is multiplied by the second adjustment coefficient to obtain the global posterior state covariance matrix at the current moment. The information allocation factor of the two angle sensors at the previous moment is used as the information allocation factor of the two angle sensors at the current moment.

[0008] Further, in one embodiment, the step of fusing the posterior state vectors and posterior state covariance matrices of the two angle sensors at the current moment to obtain the global posterior state vector and global posterior state covariance matrix at the current moment is expressed by the following formula:

[0009]

[0010] in, Let be the global posterior state covariance matrix at time k. Let be the posterior state covariance matrix of the j-th angle sensor at time k. Let be the global posterior vector at time k. Let be the posterior state vector of the j-th angle sensor at time k; The step of calculating the information allocation factor of the two angle sensors at the current moment based on the posterior state covariance matrix of the two angle sensors at the current moment is expressed by the formula:

[0011] in, Let tr() be the information allocation factor for the j-th angle sensor at time k, and let tr() be the trace of the computation matrix. The step of calculating the new process noise covariance matrix and posterior state covariance matrix of the two angle sensors at the current moment based on the global posterior state covariance matrix and global process noise covariance matrix at the current moment, as well as the information allocation factors of the two angle sensors at the current moment, is expressed by the following formula:

[0012]

[0013] in, Let be the global process noise covariance matrix at time k. Let be the process noise covariance matrix of the j-th angle sensor at time k.

[0014] Furthermore, in one embodiment, when both angle sensors are in the expected state at the current moment, a centralized Kalman filter measurement update method or a federated Kalman filter measurement update method is used.

[0015] Furthermore, in one embodiment, an iterative Kalman filter measurement update method is adopted. If only one angle sensor is in the expected state at the current moment, the measurement information of that angle sensor at the current moment is used for each iteration update. If both angle sensors are in the expected state at the current moment, the measurement information of the two angle sensors at the current moment is used alternately for iterative updates.

[0016] Furthermore, in one embodiment, after the step of determining whether the corresponding angle sensor is in the expected state at the current moment based on the measurement residuals of the two angle sensors at the current moment, the method further includes: If neither of the two angle sensors is in the expected state at the current moment, then the prior state result at the current moment is taken as the posterior state result at the current moment.

[0017] Further, in one embodiment, the step of determining whether the corresponding angle sensor is in the expected state at the current moment based on the measurement residuals of the two angle sensors at the current moment includes: If the absolute value of the vertical angle residual in the measurement residual of any angle sensor at the current moment is less than or equal to the angle error threshold, then the angle sensor is determined to be in the expected state at the current moment; otherwise, the angle sensor is determined to be in the unexpected state at the current moment.

[0018] Secondly, embodiments of this application also provide a Kalman filter-based erection control device, the Kalman filter-based erection control device comprising: The time update module is used to perform time updates by Kalman filtering based on the posterior state result of the previous time step to obtain the prior state result of the current time step, wherein the state result includes the erection angle. The status detection module is used to determine whether the corresponding angle sensor is in the expected state at the current moment based on the measurement residual of the two angle sensors at the current moment. One angle sensor is installed on the erecting frame, and the other angle sensor is installed on the launch pad. The measurement update module is used to perform Kalman filtering measurement update based on the measurement information of the corresponding angle sensor at the current time and the prior state result at the current time if at least one angle sensor is in the expected state at the current time, so as to obtain the posterior state result at the current time.

[0019] Thirdly, embodiments of this application also provide a Kalman filter-based erection control device, which includes a processor, a memory, and a Kalman filter-based erection control program stored in the memory and executable by the processor. When the Kalman filter-based erection control program is executed by the processor, it implements the steps of the aforementioned Kalman filter-based erection control method.

[0020] In this application, an angle sensor mounted on the erector frame is used to directly measure the erection angle, while an angle sensor mounted on the launch pad is used to indirectly measure the erection angle. This approach balances the accuracy and stability of the erection angle measurement. After the Kalman filter time update, the measurement residuals of the two angle sensors are used to determine whether the angle sensors are in the expected state. If an angle sensor is in the expected state, then the Kalman filter measurement update is performed. This application reduces the adverse effects of cylinder vibration, sensor damage, or occasional anomalies on the Kalman filter estimation results, thereby improving the control accuracy of the erection action and reducing control time. Attached Figure Description

[0021] Figure 1 This is a flowchart illustrating an embodiment of the erection control method based on Kalman filtering in this application. Figure 2 This is a schematic diagram of the structure of the vertical control actuator in one embodiment of this application; Figure 3 This is a schematic diagram of the Kalman filter process in one embodiment of this application; Figure 4 This is a schematic diagram of the functional modules of an erection control device based on Kalman filtering in one embodiment of this application; Figure 5 This is a schematic diagram of the hardware structure of the erection control device based on Kalman filtering involved in the embodiments of this application. Detailed Implementation

[0022] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present application, and not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present application.

[0023] To make the objectives, technical solutions, and advantages of this application clearer, the embodiments of this application will be described in further detail below with reference to the accompanying drawings.

[0024] In one aspect, embodiments of this application provide a method for erection control based on Kalman filtering.

[0025] Figure 1 A flowchart illustrating a Kalman filter-based erection control method according to an embodiment of this application is shown.

[0026] Reference Figure 1 In one embodiment, the erection control method based on Kalman filtering includes the following steps: S1. Perform time update using Kalman filtering based on the posterior state result of the previous time step to obtain the prior state result of the current time step, where the state result includes the erection angle.

[0027] Specifically, the state results include a state vector and a state covariance matrix. In the scenario of erection control, the state vector includes at least the erection angle and may also include the erection angular velocity.

[0028] For example, the time update formula for Kalman filtering includes:

[0029]

[0030] in, Let be the prior state vector at time k. Let be the posterior state vector at time k-1. Let be the state transition matrix at time k. When the state transition function is linear, the state transition matrix remains constant; when the state transition function is nonlinear, the matrix remains constant. It can be approximated as a pair of state transition functions Jacobian matrix, Let be the prior state covariance matrix at time k. Let be the posterior state covariance matrix at time k-1. Let be the process noise covariance matrix at time k-1. Typically, the process noise covariance matrix remains constant.

[0031] S2. Based on the measurement residuals of the two angle sensors at the current moment, determine whether the corresponding angle sensor is in the expected state at the current moment. One angle sensor is installed on the erecting frame, and the other angle sensor is installed on the launch pad.

[0032] In this embodiment, the expected state is used to characterize that the erection angle measured by the angle sensor is within the normal range and there is no serious inaccuracy. This can be determined based on the measurement residual. It can be understood that if the measurement residual of any angle sensor is too large at the current moment, it indicates that the corresponding angle sensor is seriously inaccurate and is in an unexpected state; otherwise, it is in the expected state.

[0033] Specifically, the formula for calculating the measurement residual is:

[0034] in, Let j be the measurement residual of the j-th angle sensor at time k, where j = 1, 2. Let be the measurement vector of the j-th angle sensor at time k. Let be the measurement matrix of the j-th angle sensor at time k. When the measurement function is linear, the measurement matrix remains constant; when the measurement function is nonlinear... It can be approximated as a pair of measurement functions. The Jacobian matrix.

[0035] Figure 2 A schematic diagram of the erection control actuator is shown in one embodiment of this application.

[0036] Reference Figure 2 During the erection phase, the erection cylinder drives the erection frame to rotate in the forward direction. Figure 2 The launch platform rotates clockwise to change the launcher from a horizontal to a vertical launch position. It is connected to the erector and rotates with the erector to support the launcher. During the tilting phase, the launch platform disconnects from the erector, and the erecting cylinder drives the erector to rotate in the opposite direction. Figure 2 (In a counter-clockwise direction) to avoid the launch drift space of the projectile.

[0037] It can be seen that during the erection phase, the angle of the launch pad and the angle of the erector frame are basically equivalent. The erection angle can be directly measured by the angle sensor installed on the erector frame (hereinafter referred to as the first sensor), and the erection angle can be indirectly measured by the angle sensor installed on the launch pad (hereinafter referred to as the second sensor).

[0038] During the erection phase, the erection cylinder is prone to vibration, which can affect the measurement results of the first and second sensors. Since the launch pad is located closer to the center of rotation and does not directly contact the erection cylinder, the second sensor is less affected.

[0039] Therefore, in this embodiment, the first sensor and the second sensor are used together to measure the erection angle. The first sensor measures the erection angle with higher accuracy when it is in the expected state, and the second sensor is more likely to be in the expected state during the erection stage. Moreover, the multi-sensor scheme can also reduce the adverse effects of sensor damage or occasional abnormalities, thereby taking into account both the accuracy and stability of the erection angle measurement.

[0040] S3. If at least one angle sensor is in the expected state at the current moment, then perform Kalman filtering measurement update based on the measurement information of the corresponding angle sensor at the current moment and the prior state result at the current moment to obtain the posterior state result at the current moment.

[0041] In this embodiment, the measurement update of the Kalman filter is achieved by using the measurement information of the angle sensor in the expected state at the current moment, which can ensure that the posterior state result has higher accuracy than the prior state result.

[0042] For example, the measurement update formula for Kalman filtering includes:

[0043]

[0044]

[0045] in, The Kalman gain at time k, Let k be the measurement noise covariance matrix at time k. Let I be the measurement vector at time k, and let I be the identity matrix.

[0046] It should be noted that, depending on the number of angle sensors in the expected state at the current moment, the measurement update of Kalman filtering is divided into two routes: single sensor and dual sensor. There will be some differences in the operational details, which will not be elaborated here.

[0047] Specifically, the posterior state results are used to provide the erection controller with the control commands for the hydraulic pump and valves.

[0048] Optionally, the erection control algorithm can calculate the control command using the posterior state vector and the posterior state covariance matrix, or it can calculate the control command using only the posterior state vector or the erection angle in the posterior state vector. The choice depends on the control algorithm settings and is not limited in this application.

[0049] Therefore, in this embodiment, a first sensor is used to directly measure the erection angle, while a second sensor is used to indirectly measure it. This approach balances the accuracy and stability of the erection angle measurement. After the Kalman filter time update, the measurement residuals of the two angle sensors are used to determine whether the angle sensors are in the expected state. If an angle sensor is in the expected state, then the Kalman filter measurement update is performed. This embodiment reduces the adverse effects of cylinder vibration, sensor damage, or occasional anomalies on the Kalman filter estimation results, thereby improving the control accuracy of the erection action and reducing control time.

[0050] Further, in one embodiment, the step of performing Kalman filtering time update based on the posterior state result of the previous time step to obtain the prior state result of the current time step includes: The global prior state vector at the current moment is calculated based on the current state transition matrix and the global posterior state vector at the previous moment. For the two angle sensors, the following steps are performed: Based on the current state transition matrix, the process noise covariance matrix and the posterior state covariance matrix of the angle sensor at the previous time, the prior state covariance matrix of the angle sensor at the current time is calculated. The step of performing Kalman filtering measurement updates based on the measurement information of the corresponding angle sensor at the current moment and the prior state result at the current moment, to obtain the posterior state result at the current moment if at least one angle sensor is in the expected state at the current moment, includes: If both angle sensors are in the expected state at the current moment, then preset operations are performed on the two angle sensors respectively to obtain the posterior state vector and posterior state covariance matrix of the two angle sensors at the current moment. The global posterior state vector and global posterior state covariance matrix of the two angle sensors at the current moment are obtained by fusing the posterior state vector and posterior state covariance matrix of the two angle sensors at the current moment. The information allocation factor of the two angle sensors at the current moment is calculated based on the posterior state covariance matrix of the two angle sensors at the current moment. If only one angle sensor is in the expected state at the current moment, then a preset operation is performed on that angle sensor to obtain the posterior state vector and posterior state covariance matrix of that angle sensor at the current moment. The posterior state vector of that angle sensor at the current moment is used as the global posterior state vector at the current moment. The posterior state covariance matrix of that angle sensor at the current moment is multiplied by the first adjustment coefficient to obtain the global posterior state covariance matrix at the current moment. The information allocation factor of the two angle sensors at the previous moment is used as the information allocation factor of the two angle sensors at the current moment. Based on the global posterior state covariance matrix and global process noise covariance matrix at the current moment, as well as the information allocation factors of the two angle sensors at the current moment, the new process noise covariance matrix and posterior state covariance matrix of the two angle sensors at the current moment are calculated. The preset operations include: The Kalman gain of the angle sensor at the current moment is calculated based on the prior state covariance matrix, measurement matrix, and measurement noise covariance matrix of the angle sensor at the current moment. The posterior state vector of the angle sensor at the current moment is calculated based on the global prior state vector, the Kalman gain, measurement matrix, and measurement vector of the angle sensor at the current moment. The posterior state covariance matrix of the angle sensor at the current moment is calculated based on the Kalman gain, measurement matrix, and prior state covariance matrix of the angle sensor at the current moment.

[0051] Figure 3 A schematic diagram of the Kalman filter process in one embodiment of this application is shown.

[0052] Specifically, refer to Figure 3 When updating the prior state results, the global prior state vector is updated directly, and the prior state covariance matrix of each of the two sensors is updated separately. When updating the posterior state results, only the posterior state results of the angle sensors in the expected state are updated. If both angle sensors are in the expected state, the posterior state results of the two angle sensors are fused to obtain the global posterior state result, and the information allocation factor is updated based on the posterior state covariance matrix of the two angle sensors. If only one angle sensor is in the expected state, the posterior state vector of that angle sensor is directly used as the global posterior state vector, and the posterior state covariance of that angle sensor is multiplied by the first adjustment coefficient to obtain the global posterior state covariance, while keeping the value of the information allocation factor unchanged. Finally, the process noise covariance matrix and posterior state covariance matrix of the two angle sensors are readjusted according to the information allocation factor.

[0053] For example, the first adjustment factor can be in the range of 1.2 ≤ α1 ≤ 1.3.

[0054] Specifically, in this embodiment, the formula for calculating the global prior state vector is the same as formula (1), and the formula for calculating the prior state covariance matrix of the two sensors is:

[0055] In this context, parameters with a superscript (j) represent local parameters of the j-th angle sensor, while parameters without a superscript (j) represent global parameters.

[0056] The preset operation formulas include:

[0057]

[0058]

[0059] Further, in one embodiment, the step of fusing the posterior state vectors and posterior state covariance matrices of the two angle sensors at the current moment to obtain the global posterior state vector and global posterior state covariance matrix at the current moment is expressed by the following formula:

[0060]

[0061] in, Let be the global posterior state covariance matrix at time k. Let be the posterior state covariance matrix of the j-th angle sensor at time k. Let be the global posterior vector at time k. Let be the posterior state vector of the j-th angle sensor at time k.

[0062] The step of calculating the information allocation factor of the two angle sensors at the current moment based on the posterior state covariance matrix of the two angle sensors at the current moment is expressed by the formula:

[0063] in, Let tr() be the information allocation factor for the j-th angle sensor at time k, and let tr() be the trace of the computation matrix.

[0064] The step of calculating the new process noise covariance matrix and posterior state covariance matrix of the two angle sensors at the current moment based on the global posterior state covariance matrix and global process noise covariance matrix at the current moment, as well as the information allocation factors of the two angle sensors at the current moment, is expressed by the following formula:

[0065]

[0066] in, Let be the global process noise covariance matrix at time k. Let be the process noise covariance matrix of the j-th angle sensor at time k.

[0067] It should be noted that the update method of the information allocation factor is not limited to the above formula. For example, the trace of the matrix in formula (13) can also be replaced with the F norm of the matrix.

[0068] Optionally, at the initial moment (k=0), the information allocation factors of the two angle sensors can both be set to 1 / 2, or different values ​​can be set according to experience, as long as the sum of the two is equal to 1. For example, the information allocation factor of the second sensor is greater than the information allocation factor of the first sensor.

[0069] Furthermore, in one embodiment, after the step of determining whether the corresponding angle sensor is in the expected state at the current moment based on the measurement residuals of the two angle sensors at the current moment, the method further includes: If neither of the two angle sensors is in the expected state at the current moment, then the global prior state vector at the current moment is used as the global posterior state vector at the current moment. The sum of the prior state covariance matrices of the two angle sensors at the current moment is multiplied by the second adjustment coefficient to obtain the global posterior state covariance matrix at the current moment. The information allocation factor of the two angle sensors at the previous moment is used as the information allocation factor of the two angle sensors at the current moment.

[0070] Continue to refer to Figure 3 In this embodiment, when there is no angle sensor in the expected state, no Kalman filtering measurement update is performed. The global prior state vector is directly used as the global posterior state vector. The sum of the prior state covariance matrices of the two angle sensors is multiplied by the second adjustment coefficient to obtain the global posterior state covariance matrix. The value of the information allocation factor remains unchanged to avoid a decrease in the accuracy of the posterior state result.

[0071] It should be noted that since the second sensor is less affected by cylinder vibration, it is rare for it not to update its measurement during actual control. For example, the probability of the second sensor experiencing occasional abnormalities or damage while the first sensor is severely affected by cylinder vibration is very low.

[0072] For example, the range of the second adjustment factor is 0.75 ≤ α2 ≤ 1.0.

[0073] In particular, in the above embodiments, the prior state covariance is updated separately for different angle sensors. In other embodiments, the global prior state covariance is updated directly using formula (2) by default.

[0074] Furthermore, in one embodiment, when both angle sensors are in the expected state at the current moment, a centralized Kalman filter measurement update method or a federated Kalman filter measurement update method is used.

[0075] Specifically, when using centralized Kalman filtering to update the measurement of two sensors, the measurement information from the two angle sensors is pre-stitched to obtain global measurement information. Then, a global measurement update is performed based on this global measurement information to obtain the posterior state result. The formula for stitching the measurement information includes:

[0076]

[0077]

[0078] in, Let k be the global measurement matrix at time k. Let be the global measurement noise covariance matrix at time k. Let be the measurement noise covariance matrix of the j-th angle sensor at time k. Let be the global measurement vector at time k.

[0079] The formula for global measurement update is the same as formulas (4)-(6).

[0080] Specifically, when using federated Kalman filtering to update the measurement of dual sensors, the measurement information of the two angle sensors is used to update the measurement of a single sensor to obtain the local posterior state result, and then the local posterior state results are fused to obtain the global posterior state result.

[0081] Optionally, the weights used in the fusion calculation can be preset fixed weights or dynamically determined based on the local results of the covariance matrix of the posterior state each time.

[0082] The formulas for single-sensor measurement updates include:

[0083]

[0084]

[0085] Furthermore, in one embodiment, an iterative Kalman filter measurement update method is adopted. If only one angle sensor is in the expected state at the current moment, the measurement information of that angle sensor at the current moment is used for each iteration update. If both angle sensors are in the expected state at the current moment, the measurement information of the two angle sensors at the current moment is used alternately for iterative updates.

[0086] In this embodiment, iterative Kalman filtering is used to update the measurements of single and dual sensors. The difference is that the single sensor uses the same measurement information in each iteration, while the dual sensor alternately uses the measurement information of the two angle sensors in each iteration.

[0087] Specifically, the relevant formulas include:

[0088]

[0089]

[0090] in, Let Kalman gain be the value updated in the i-th iteration at time k. Let be the posterior state covariance matrix updated in the i-th iteration at time k. , Let k be the measurement matrix updated in the i-th iteration at time k. Let be the measurement noise covariance matrix updated in the i-th iteration at time k. Let be the posterior state vector updated in the i-th iteration at time k. , This is the measurement vector updated during the i-th iteration at time k.

[0091] If the number of iterations has not reached the upper limit, and the difference between the posterior state vectors of two adjacent iterations is greater than or equal to the difference threshold, then the next iteration continues; otherwise, the iteration stops, and the posterior state vector and posterior state covariance matrix of the last iteration are used as the posterior state vector and posterior state covariance matrix at the current moment.

[0092] Furthermore, in one embodiment, after the step of determining whether the corresponding angle sensor is in the expected state at the current moment based on the measurement residuals of the two angle sensors at the current moment, the method further includes: If neither of the two angle sensors is in the expected state at the current moment, then the prior state result at the current moment is taken as the posterior state result at the current moment.

[0093] In this embodiment, when there is no angle sensor in the expected state, Kalman filtering measurement update is not performed, and the prior state result is directly used as the posterior state result to avoid a decrease in the accuracy of the posterior state result.

[0094] As an optional implementation, the step of determining whether the corresponding angle sensor is in the expected state at the current moment based on the measurement residuals of the two angle sensors at the current moment includes: If the absolute value of the vertical angle residual in the measurement residual of any angle sensor at the current moment is less than or equal to the angle error threshold, then the angle sensor is determined to be in the expected state at the current moment; otherwise, the angle sensor is determined to be in the unexpected state at the current moment.

[0095] In this embodiment, the absolute value of the vertical angle residual is used as the evaluation standard for the expected state, and the judgment is made from the dimension of the motion result.

[0096] For example, the angle error threshold is set to 1°.

[0097] As another optional implementation, the measurement vector includes the erection angular velocity; The step of determining whether the corresponding angle sensor is in the expected state at the current moment based on the measurement residuals of the two angle sensors at the current moment includes: If the absolute value of the vertical angular velocity residual in the measurement residual of any angle sensor at the current moment is less than or equal to the angular velocity error threshold, then the angle sensor is judged to be in the expected state at the current moment; otherwise, the angle sensor is judged not to be in the expected state at the current moment.

[0098] In this embodiment, the absolute value of the vertical angular velocity residual is used as the evaluation standard for the expected state, and the judgment is made from the dimension of the motion process.

[0099] Secondly, embodiments of this application also provide an erection control device based on Kalman filtering.

[0100] Figure 4 A schematic diagram of the functional modules of an erection control device based on Kalman filtering in one embodiment of this application is shown.

[0101] Reference Figure 4 In one embodiment, the erection control device based on Kalman filtering includes: The time update module 10 is used to perform time update by Kalman filtering based on the posterior state result of the previous moment to obtain the prior state result of the current moment, wherein the state result includes the erection angle. The status detection module 20 is used to determine whether the corresponding angle sensor is in the expected state at the current moment based on the measurement residual of the two angle sensors at the current moment. One angle sensor is installed on the erecting frame and the other angle sensor is installed on the launch pad. The measurement update module 30 is used to perform Kalman filtering measurement update based on the measurement information of the corresponding angle sensor at the current time and the prior state result at the current time if at least one angle sensor is in the expected state at the current time, so as to obtain the posterior state result at the current time.

[0102] Furthermore, in one embodiment, the time update module 10 is used for: The global prior state vector at the current moment is calculated based on the current state transition matrix and the global posterior state vector at the previous moment. For the two angle sensors, the following steps are performed: Based on the current state transition matrix, the process noise covariance matrix and the posterior state covariance matrix of the angle sensor at the previous time, the prior state covariance matrix of the angle sensor at the current time is calculated. Measurement update module 30 is used for: If both angle sensors are in the expected state at the current moment, then preset operations are performed on the two angle sensors respectively to obtain the posterior state vector and posterior state covariance matrix of the two angle sensors at the current moment. The global posterior state vector and global posterior state covariance matrix of the two angle sensors at the current moment are obtained by fusing the posterior state vector and posterior state covariance matrix of the two angle sensors at the current moment. The information allocation factor of the two angle sensors at the current moment is calculated based on the posterior state covariance matrix of the two angle sensors at the current moment. If only one angle sensor is in the expected state at the current moment, then a preset operation is performed on that angle sensor to obtain the posterior state vector and posterior state covariance matrix of that angle sensor at the current moment. The posterior state vector of that angle sensor at the current moment is used as the global posterior state vector at the current moment. The posterior state covariance matrix of that angle sensor at the current moment is multiplied by the first adjustment coefficient to obtain the global posterior state covariance matrix at the current moment. The information allocation factor of the two angle sensors at the previous moment is used as the information allocation factor of the two angle sensors at the current moment. Based on the global posterior state covariance matrix and global process noise covariance matrix at the current moment, as well as the information allocation factors of the two angle sensors at the current moment, the new process noise covariance matrix and posterior state covariance matrix of the two angle sensors at the current moment are calculated. The preset operations include: The Kalman gain of the angle sensor at the current moment is calculated based on the prior state covariance matrix, measurement matrix, and measurement noise covariance matrix of the angle sensor at the current moment. The posterior state vector of the angle sensor at the current moment is calculated based on the global prior state vector, the Kalman gain, measurement matrix, and measurement vector of the angle sensor at the current moment. The posterior state covariance matrix of the angle sensor at the current moment is calculated based on the Kalman gain, measurement matrix, and prior state covariance matrix of the angle sensor at the current moment.

[0103] Furthermore, in one embodiment, the Kalman filter-based erection control device further includes a posterior assignment module 40, used for: If neither of the two angle sensors is in the expected state at the current moment, then the global prior state vector at the current moment is used as the global posterior state vector at the current moment. The sum of the prior state covariance matrices of the two angle sensors at the current moment is multiplied by the second adjustment coefficient to obtain the global posterior state covariance matrix at the current moment. The information allocation factor of the two angle sensors at the previous moment is used as the information allocation factor of the two angle sensors at the current moment.

[0104] Further, in one embodiment, the step of fusing the posterior state vectors and posterior state covariance matrices of the two angle sensors at the current moment to obtain the global posterior state vector and global posterior state covariance matrix at the current moment is expressed by the following formula:

[0105]

[0106] in, Let be the global posterior state covariance matrix at time k. Let be the posterior state covariance matrix of the j-th angle sensor at time k. Let be the global posterior vector at time k. Let be the posterior state vector of the j-th angle sensor at time k; The step of calculating the information allocation factor of the two angle sensors at the current moment based on the posterior state covariance matrix of the two angle sensors at the current moment is expressed by the formula:

[0107] in, Let tr() be the information allocation factor for the j-th angle sensor at time k, and let tr() be the trace of the computation matrix. The step of calculating the new process noise covariance matrix and posterior state covariance matrix of the two angle sensors at the current moment based on the global posterior state covariance matrix and global process noise covariance matrix at the current moment, as well as the information allocation factors of the two angle sensors at the current moment, is expressed by the following formula:

[0108]

[0109] in, Let be the global process noise covariance matrix at time k. Let be the process noise covariance matrix of the j-th angle sensor at time k.

[0110] Furthermore, in one embodiment, when both angle sensors are in the expected state at the current moment, a centralized Kalman filter measurement update method or a federated Kalman filter measurement update method is used.

[0111] Furthermore, in one embodiment, an iterative Kalman filter measurement update method is adopted. If only one angle sensor is in the expected state at the current moment, the measurement information of that angle sensor at the current moment is used for each iteration update. If both angle sensors are in the expected state at the current moment, the measurement information of the two angle sensors at the current moment is used alternately for iterative updates.

[0112] Furthermore, in one embodiment, the Kalman filter-based erection control device further includes a posterior assignment module 40, used for: If neither of the two angle sensors is in the expected state at the current moment, then the prior state result at the current moment is taken as the posterior state result at the current moment.

[0113] Furthermore, in one embodiment, the state detection module 20 is used for: If the absolute value of the vertical angle residual in the measurement residual of any angle sensor at the current moment is less than or equal to the angle error threshold, then the angle sensor is determined to be in the expected state at the current moment; otherwise, the angle sensor is determined to be in the unexpected state at the current moment.

[0114] The functions of each module in the above-mentioned Kalman filter-based erection control device correspond to the steps in the above-mentioned Kalman filter-based erection control method embodiment, and their functions and implementation processes will not be described in detail here.

[0115] Thirdly, embodiments of this application provide a Kalman filter-based erection control device, which can be a personal computer (PC), laptop computer, server, or other device with data processing capabilities.

[0116] Figure 5 A schematic diagram of the hardware structure of the erection control device based on Kalman filtering involved in the embodiment of this application is shown.

[0117] Reference Figure 5 In this embodiment of the application, the erection control device based on Kalman filtering may include a processor, a memory, a communication interface, and a communication bus.

[0118] The communication bus can be of any type and is used to interconnect the processor, memory, and communication interface.

[0119] The communication interface includes input / output (I / O) interfaces, physical interfaces, and logical interfaces for interconnecting devices within the Kalman filter-based erection control device, as well as interfaces for interconnecting the Kalman filter-based erection control device with other devices (such as other computing devices or user equipment). Physical interfaces can be Ethernet interfaces, fiber optic interfaces, ATM interfaces, etc.; user equipment can be displays, keyboards, etc.

[0120] Memory can be various types of storage media, such as random access memory (RAM), read-only memory (ROM), non-volatile RAM (NVRAM), flash memory, optical storage, hard disk, programmable ROM (PROM), erasable PROM (EPROM), electrically erasable PROM (EEPROM), etc.

[0121] The processor can be a general-purpose processor, which can call the Kalman filter-based erection control program stored in memory and execute the Kalman filter-based erection control method provided in the embodiments of this application. For example, the general-purpose processor can be a central processing unit (CPU). The method executed when the Kalman filter-based erection control program is called can be referred to in the various embodiments of the Kalman filter-based erection control method of this application, and will not be repeated here.

[0122] Those skilled in the art will understand that Figure 5 The hardware structure shown does not constitute a limitation of this application and may include more or fewer components than shown, or combine certain components, or have different component arrangements.

[0123] It should be noted that the sequence numbers of the embodiments in this application are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments.

[0124] The terms "comprising" and "having," and any variations thereof, in the specification, claims, and accompanying drawings of this application are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or apparatus that includes a series of steps or units is not limited to the listed steps or units, but may optionally include steps or units not listed, or may optionally include other steps or units inherent to such process, method, product, or apparatus. The terms "first," "second," and "third," etc., are used to distinguish different objects, etc., and do not indicate a sequence, nor do they limit "first," "second," and "third" to different types.

[0125] In the description of the embodiments of this application, terms such as "exemplary," "for example," or "for instance" are used to indicate examples, illustrations, or explanations. Any embodiment or design described as "exemplary," "for example," or "for instance" in the embodiments of this application should not be construed as being more preferred or advantageous than other embodiments or designs. Specifically, the use of terms such as "exemplary," "for example," or "for instance" is intended to present the relevant concepts in a concrete manner.

[0126] In the description of the embodiments of this application, unless otherwise stated, " / " means "or". For example, A / B can mean A or B. The "and / or" in the text is merely a description of the relationship between related objects, indicating that there can be three relationships. For example, A and / or B can mean: A exists alone, A and B exist simultaneously, and B exists alone. In addition, in the description of the embodiments of this application, "multiple" means two or more.

[0127] In some processes described in the embodiments of this application, multiple operations or steps are included in a specific order. However, it should be understood that these operations or steps may not be executed in the order they appear in the embodiments of this application, or they may be executed in parallel. The sequence number of the operation is only used to distinguish different operations, and the sequence number itself does not represent any execution order. In addition, these processes may include more or fewer operations, and these operations or steps may be executed sequentially or in parallel, and these operations or steps may be combined.

[0128] Through the above description of the embodiments, those skilled in the art can clearly understand that the methods of the above embodiments can be implemented by means of software plus necessary general-purpose hardware platforms. Of course, they can also be implemented by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk) as described above, and includes several instructions to cause a terminal device to execute the methods described in the various embodiments of this application.

[0129] The above are merely preferred embodiments of this application and do not limit the patent scope of this application. Any equivalent structural or procedural transformations made using the content of this application's specification and drawings, or direct or indirect applications in other related technical fields, are similarly included within the patent protection scope of this application.

Claims

1. A method for controlling erection based on Kalman filtering, characterized in that, The erection control method based on Kalman filtering includes: The time update is performed by Kalman filtering based on the posterior state result of the previous moment to obtain the prior state result of the current moment, where the state result includes the erection angle. Based on the measurement residuals of the two angle sensors at the current moment, determine whether the corresponding angle sensor is in the expected state at the current moment. One angle sensor is installed on the erecting frame, and the other angle sensor is installed on the launch pad. If at least one angle sensor is in the expected state at the current moment, then a Kalman filter measurement update is performed based on the measurement information of the corresponding angle sensor at the current moment and the prior state result at the current moment to obtain the posterior state result at the current moment.

2. The erection control method based on Kalman filtering as described in claim 1, characterized in that, The step of performing Kalman filtering time update based on the posterior state result of the previous time step to obtain the prior state result of the current time step includes: The global prior state vector at the current moment is calculated based on the current state transition matrix and the global posterior state vector at the previous moment. For the two angle sensors, the following steps are performed: Based on the current state transition matrix, the process noise covariance matrix and the posterior state covariance matrix of the angle sensor at the previous time, the prior state covariance matrix of the angle sensor at the current time is calculated. The step of performing Kalman filtering measurement updates based on the measurement information of the corresponding angle sensor at the current moment and the prior state result at the current moment, to obtain the posterior state result at the current moment if at least one angle sensor is in the expected state at the current moment, includes: If both angle sensors are in the expected state at the current moment, then preset operations are performed on the two angle sensors respectively to obtain the posterior state vector and posterior state covariance matrix of the two angle sensors at the current moment. The global posterior state vector and global posterior state covariance matrix of the two angle sensors at the current moment are obtained by fusing the posterior state vector and posterior state covariance matrix of the two angle sensors at the current moment. The information allocation factor of the two angle sensors at the current moment is calculated based on the posterior state covariance matrix of the two angle sensors at the current moment. If only one angle sensor is in the expected state at the current moment, then a preset operation is performed on that angle sensor to obtain the posterior state vector and posterior state covariance matrix of that angle sensor at the current moment. The posterior state vector of that angle sensor at the current moment is used as the global posterior state vector at the current moment. The posterior state covariance matrix of that angle sensor at the current moment is multiplied by the first adjustment coefficient to obtain the global posterior state covariance matrix at the current moment. The information allocation factor of the two angle sensors at the previous moment is used as the information allocation factor of the two angle sensors at the current moment. Based on the global posterior state covariance matrix and global process noise covariance matrix at the current moment, as well as the information allocation factors of the two angle sensors at the current moment, the new process noise covariance matrix and posterior state covariance matrix of the two angle sensors at the current moment are calculated. The preset operations include: The Kalman gain of the angle sensor at the current moment is calculated based on the prior state covariance matrix, measurement matrix, and measurement noise covariance matrix of the angle sensor at the current moment. The posterior state vector of the angle sensor at the current moment is calculated based on the global prior state vector, the Kalman gain, measurement matrix, and measurement vector of the angle sensor at the current moment. The posterior state covariance matrix of the angle sensor at the current moment is calculated based on the Kalman gain, measurement matrix, and prior state covariance matrix of the angle sensor at the current moment.

3. The erection control method based on Kalman filtering as described in claim 2, characterized in that, After the step of determining whether the corresponding angle sensor is in the expected state at the current moment based on the measurement residuals of the two angle sensors at the current moment, the method further includes: If neither of the two angle sensors is in the expected state at the current moment, then the global prior state vector at the current moment is used as the global posterior state vector at the current moment. The sum of the prior state covariance matrices of the two angle sensors at the current moment is multiplied by the second adjustment coefficient to obtain the global posterior state covariance matrix at the current moment. The information allocation factor of the two angle sensors at the previous moment is used as the information allocation factor of the two angle sensors at the current moment.

4. The erection control method based on Kalman filtering as described in claim 2, characterized in that, The step of fusing the posterior state vectors and posterior state covariance matrices of the two angle sensors at the current moment to obtain the global posterior state vector and global posterior state covariance matrix is ​​expressed by the following formula: in, Let be the global posterior state covariance matrix at time k. Let be the posterior state covariance matrix of the j-th angle sensor at time k. Let be the global posterior vector at time k. Let be the posterior state vector of the j-th angle sensor at time k; The step of calculating the information allocation factor of the two angle sensors at the current moment based on the posterior state covariance matrix of the two angle sensors at the current moment is expressed by the formula: in, Let tr() be the information allocation factor for the j-th angle sensor at time k, and let tr() be the trace of the computation matrix. The step of calculating the new process noise covariance matrix and posterior state covariance matrix of the two angle sensors at the current moment based on the global posterior state covariance matrix and global process noise covariance matrix at the current moment, as well as the information allocation factors of the two angle sensors at the current moment, is expressed by the following formula: in, Let be the global process noise covariance matrix at time k. Let be the process noise covariance matrix of the j-th angle sensor at time k.

5. The erection control method based on Kalman filtering as described in claim 1, characterized in that, When both angle sensors are in the expected state at the current moment, a centralized Kalman filter measurement update method or a federated Kalman filter measurement update method is used.

6. The erection control method based on Kalman filtering as described in claim 1, characterized in that, The measurement update method using iterative Kalman filtering is adopted. If only one angle sensor is in the expected state at the current moment, the measurement information of that angle sensor at the current moment is used for each iteration update. If both angle sensors are in the expected state at the current moment, the measurement information of the two angle sensors at the current moment is used alternately for iterative update.

7. The erection control method based on Kalman filtering as described in claim 5 or 6, characterized in that, After the step of determining whether the corresponding angle sensor is in the expected state at the current moment based on the measurement residuals of the two angle sensors at the current moment, the method further includes: If neither of the two angle sensors is in the expected state at the current moment, then the prior state result at the current moment is taken as the posterior state result at the current moment.

8. The erection control method based on Kalman filtering as described in claim 1, characterized in that, The step of determining whether the corresponding angle sensor is in the expected state at the current moment based on the measurement residuals of the two angle sensors at the current moment includes: If the absolute value of the vertical angle residual in the measurement residual of any angle sensor at the current moment is less than or equal to the angle error threshold, then the angle sensor is determined to be in the expected state at the current moment; otherwise, the angle sensor is determined to be in the unexpected state at the current moment.

9. A hoisting control device based on Kalman filtering, characterized in that, The Kalman filter-based erection control device includes: The time update module is used to perform time updates by Kalman filtering based on the posterior state result of the previous time step to obtain the prior state result of the current time step, wherein the state result includes the erection angle. The status detection module is used to determine whether the corresponding angle sensor is in the expected state at the current moment based on the measurement residual of the two angle sensors at the current moment. One angle sensor is installed on the erecting frame, and the other angle sensor is installed on the launch pad. The measurement update module is used to perform Kalman filtering measurement update based on the measurement information of the corresponding angle sensor at the current time and the prior state result at the current time if at least one angle sensor is in the expected state at the current time, so as to obtain the posterior state result at the current time.

10. A hoisting control device based on Kalman filtering, characterized in that, The Kalman filter-based erection control device includes a processor, a memory, and a Kalman filter-based erection control program stored in the memory and executable by the processor, wherein when the Kalman filter-based erection control program is executed by the processor, it implements the steps of the Kalman filter-based erection control method as described in any one of claims 1 to 8.

Images