Linear motion compensation control method based on digital elevation model

Abstract

The invention discloses a linear motion compensation control method based on a digital elevation model. Relates to the field of automatic control calculation, in particular to a linear motion compensation control method based on a digital elevation model. According to the method, the accurate fixed angle deviation value is iterated according to the Kalman filtering method by combining the pixel deviation angle obtained by the optical flow estimation algorithm based on the ground altitude preposition information provided by the digital elevation model, and meanwhile, the accurate ground altitude is found out. And finally, performing linear motion compensation according to the accurate fixed angle deviation value and the ground altitude. The method comprises the following steps: importing a digital elevation model file; acquiring real-time information; calculating an angular velocity compensation value based on the initial altitude of the target ground; angle offset calculation based on a sparse optical flow estimation algorithm; correcting the fixed deviation value through Kalman filtering; and linear motion compensation of the final altitude based on the true fixed deviation value and the central position of the field of view.

Description

Technical Field

[0001] This invention relates to the field of automatic control technology, and specifically to a linear motion compensation control method based on a digital elevation model. Background Technology

[0002] When an unmanned aerial vehicle (UAV) pod conducts reconnaissance over a specific area, real-time angular velocity compensation is required to stabilize the field of view within the target area. In this process, the calculation of axial velocity and the estimation of the altitude difference between the pod and the ground are crucial. While the axial velocity can be calculated using information from the aircraft's integrated navigation system and the pod's encoder, fixed angular deviations exist due to installation and other factors. Furthermore, the accuracy of ground altitude, as an external input, is difficult to guarantee. Especially when the aircraft's speed-to-altitude ratio is large, both the fixed angular deviation and the estimation error of ground altitude significantly reduce the effectiveness of angular velocity compensation. Therefore, a method is urgently needed that can simultaneously and accurately estimate ground altitude and correct for fixed angular deviations, thereby achieving precise and stable tracking of the target area.

[0003] The Chinese invention patent "A Linear Motion Compensation Control Method Based on Image Matching" (CN117953007B) uses an image matching algorithm to obtain inter-frame pixel offsets, thereby deducing the ground altitude at the center of the field of view. While this method achieves the goal of enabling the UAV pod to automatically and quickly estimate the accurate ground altitude without a passive system, thus enabling precise linear motion compensation, it suffers from several drawbacks. Firstly, there is often a fixed angular deviation between the attitude angle output by the aircraft's integrated navigation system and the zero position of the pod's encoder. Secondly, when the aircraft's speed-to-altitude ratio is high, the pod's line of sight can only remain in one area for a very short time, resulting in very limited data acquisition. This means that simply using inter-frame pixel offsets to deduce ground altitude is insufficient to accurately estimate the result due to the large speed-to-altitude ratio. Furthermore, the fixed angular deviation means that even when the aircraft's speed-to-altitude ratio is not high, the estimated ground altitude is still inaccurate, ultimately significantly reducing the effectiveness of linear motion compensation. Summary of the Invention

[0004] To address the problem in existing linear motion compensation control methods that struggle to simultaneously and accurately estimate ground elevation and correct fixed angle deviations, leading to suboptimal compensation results, this invention proposes a linear motion compensation control method based on a digital elevation model (DEM). This method utilizes ground elevation information provided by the DEM, combined with pixel offset angles derived from optical flow estimation, and then iteratively calculates accurate fixed angle deviations using a Kalman filter, while simultaneously determining the precise ground elevation. Finally, linear motion compensation is performed based on both the accurate fixed angle deviation and the ground elevation.

[0005] The method includes the following steps: S1. According to the flight path, import the digital elevation model file of the area to be measured into the UAV pod. In the digital elevation model file, the initial elevation of the target ground is:

[0006] S2. Real-time acquisition of position and attitude information from the integrated navigation system on the UAV pod: latitude ,longitude ,high Heading angle Pitch angle and roll angle And the attitude information of the pod encoder: azimuth angle and up and down The first character representing linear motion compensation At that moment, , This represents the total time for linear motion compensation; S3, will By substituting the passive positioning algorithm based on the Earth ellipsoid model into the digital elevation model file, the initial altitude of the center of the field of view is obtained.

[0007] S4, according to Solve for the initial platform inertial angular velocity compensation value; S5. Calculate the initial offset angle based on the initial platform inertial angular velocity compensation value. ; S6. Set the initial offset angle to the difference between the observed value and the predicted value, and fix the angle deviation value. As a state variable, the true fixed angle deviation value is solved using the Kalman filter algorithm.

[0008] S7, will and Substitute the values ​​into the passive positioning algorithm based on the Earth ellipsoid model in step S3, and perform the iteration of the digital elevation model file to obtain the final altitude of the center position of the field of view.

[0009] S8, according to and Perform linear motion compensation.

[0010] Furthermore, in step S2, the position and attitude information of the integrated navigation system on the UAV pod and the attitude information of the pod encoder meet the timing alignment requirements.

[0011] Furthermore, in step S3, the digital elevation model file is iterated to obtain the initial altitude of the center position of the field of view. Specifically: S31, will Substitute the passive localization algorithm based on the Earth ellipsoid model: Location results obtained: ,in, Query latitude and longitude of the digital elevation model representing the area to be measured; S32, will Substituting into the digital elevation model file from step S1, we obtain Corresponding ground elevation

[0012] S33, Judgment Is it true? If it is true, then... The value increases After that, iteratively execute steps S31-S33; If this condition is not met, then the initial altitude of the center of the field of view is obtained. ,at this time, The value and The values ​​are equal.

[0013] Furthermore, in step S4, the specific steps for solving the initial platform inertial angular velocity compensation value are as follows: S41, based on A passive localization algorithm based on the Earth ellipsoid model is used to solve the problem. At that time, the initial ground latitude and longitude of the center point of the field of view

[0014] S42, according to and the initial latitude and longitude of the pod Solve for the initial distance between the pod and the center point of the field of view. ; S43. Calculate the angular velocity compensation value of the platform's orientation using the following formulas. Angular velocity compensation value of platform pitch As the initial platform inertial angular velocity compensation value:

[0015] in, The initial axial velocity representing spatial orientation. This represents the initial axial velocity of the space pitch.

[0016] Furthermore, in step S5, initial linear motion compensation is performed based on the initial platform inertial angular velocity compensation value, and the initial offset angle is calculated using a sparse optical flow algorithm. .

[0017] Furthermore, in step S6, the initial error propagation matrix parameters are set, and the offset angles calculated at different times are substituted into the Kalman filter algorithm. Through iteration, the value of the state variable is finally stabilized at the true fixed angle deviation value.

[0018] An electronic device includes: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the method described above. A computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the above-described method.

[0019] The beneficial effects of the method described in this invention are as follows: (1) The method of the present invention pre-imports a digital elevation model file, and then uses an optical flow estimation algorithm and a Kalman filtering method to estimate an accurate fixed angle deviation value. At the same time, it queries the accurate ground elevation (the final elevation of the center position of the field of view). It can accurately estimate the ground elevation and correct the fixed angle deviation value. The method of the present invention provides a way to quickly and accurately estimate the fixed angle deviation in the linear motion compensation control process, and in turn optimize the compensation effect (using the real fixed angle deviation value to further refine the final elevation of the center position of the field of view), thereby achieving the purpose of accurate and stable tracking of the target area.

[0020] (2) Compared with the prior art, the method of the present invention can overcome some complex and extreme conditions (such as the aircraft speed-to-altitude ratio being too large, the fixed angle deviation between the aircraft and the pod not being calibrated, etc.), and finally quickly and accurately estimate the fixed angle deviation value and the necessary conditions such as ground altitude, so that the effect of linear motion compensation can meet the requirements at any time and under any conditions. Attached Figure Description

[0021] Figure 1 This is a flowchart of the method described in an embodiment of the present invention; Figure 2 This is a flowchart of the passive linear motion compensation process described in this invention. Detailed Implementation

[0022] The technical solution of the present invention will now be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0023] This embodiment provides a linear motion compensation control method based on a digital elevation model, the flowchart of which is shown below. Figure 1 The method includes the following steps: S1. According to the flight path, import the digital elevation model file of the area to be measured into the UAV pod. In the digital elevation model file, the initial elevation of the target ground is:

[0024] S2. Real-time acquisition of position and attitude information from the integrated navigation system on the UAV pod: latitude ,longitude ,high Heading angle Pitch angle and roll angle And the attitude information of the pod encoder: azimuth angle and pitch angle The first character representing linear motion compensation At that moment, , This represents the total time for linear motion compensation; In step S2, the pod is installed on the UAV's base, and the link is debugged to ensure that the position and attitude information of the aircraft's integrated navigation system can be transmitted to the pod in real time. Simultaneously, the attitude information of the pod's encoder is collected. This step requires timing alignment to strictly ensure the synchronization of information between the aircraft and the pod (meeting timing alignment requirements). Here, we define the time before linear motion compensation begins as time 0, i.e. .

[0025] S3, will By substituting the passive positioning algorithm based on the Earth ellipsoid model into the digital elevation model file, the initial altitude of the center of the field of view is obtained.

[0026] In step S3, the digital elevation model file is iterated to obtain the initial altitude of the center position of the field of view. Specifically: S31, will Substitute the passive localization algorithm based on the Earth ellipsoid model: Location results obtained: ,in, Query latitude and longitude of the digital elevation model representing the area to be measured; S32, will Substituting into the digital elevation model file from step S1, we obtain Corresponding ground elevation

[0027] S33, Judgment Is it true? If it is true, then... The value increases After that, iteratively execute steps S31-S33; If this condition is not met, then the initial altitude of the center of the field of view is obtained. at this time, The value and The values ​​are equal.

[0028] S4, according to Solve for the initial platform inertial angular velocity compensation value; In step S4, such as Figure 2 As shown, firstly, The heading angle of the time-integrated navigation system Pitch angle and roll angle relative to the encoder's azimuth angle and pitch angle Convert to initial spatial azimuth and initial pitch angle in space (The values ​​calculated using this method later are denoted as...) The calculation formula is as follows:

[0029]

[0030] Next, using the two spatial pointing angles mentioned above as parameters of the three-dimensional spatial rotation matrix, the velocity in the northeast direction at this time is... Axial velocity converted into spatial pointing angle (The values ​​calculated using this method later are denoted as...) ); By At that time, the latitude and longitude of the integrated navigation system on the drone pod... Initial pointing angle in space and Substituting these values ​​into the passive positioning algorithm program based on the Earth ellipsoid model, the initial ground latitude, longitude, and altitude of the center point of the field of view are obtained.

[0031] By this time and Convert to the geodetic rectangular coordinate system respectively and Finally, according to and Estimate the distance between the two points at this moment. (The distance estimated subsequently based on this method is denoted as...) The specific calculation method is as follows:

[0032]

[0033] Among them, the semi-major axis of the ellipsoid Meters, semi-minor axis of the ellipsoid meters, the first eccentricity of the ellipsoid ellipsoidal radius of curvature .

[0034]

[0035] Finally, the angular velocity compensation value for the platform's orientation is calculated using the following formulas. Angular velocity compensation value of platform pitch As the initial platform inertial angular velocity compensation value:

[0036]

[0037] in, The initial axial velocity representing spatial orientation. This represents the initial axial velocity of the space pitch.

[0038] S5. Calculate the initial offset angle based on the initial platform inertial angular velocity compensation value. ; In step S5, initial linear motion compensation is performed using the initial platform angular velocity compensation value. Starting from the second frame, there will be pixel shift relative to the previous frame (due to poor compensation effect caused by a fixed angle deviation). For this pixel shift, the initial pixel shift amount is obtained using a sparse optical flow estimation algorithm. (The pixel offset between two adjacent frames after this is denoted as...) Then, based on the sparse optical flow estimation algorithm, the offset angle between two adjacent frames is calculated. (The subsequent offset angle is denoted as) ).

[0039] S6. Set the initial offset angle to the difference between the observed value and the predicted value, and a fixed angle deviation value. As a state variable, the true fixed angle deviation value is solved using the Kalman filter algorithm.

[0040] In step S6, the initial error propagation matrix parameters (covariance) are set, and the offset angles calculated at different times are... Substituting the values ​​into the Kalman filter algorithm (program) for iteration, the basic settings are as follows: predict: State variables:

[0041] Covariance: ; in, express The value after the state transition express The value after the state transition This represents the covariance at the current moment. express Covariance after state transition This indicates process noise.

[0042] renew: Kalman gain: ; Update state variables: ; Update the mean squared error: ; in, The root mean square error, This represents the actual value of the deflection angle at the current moment. The difference between the predicted deflection angle and the predicted value is... Therefore, the updated state variable is rewritten as follows:

[0043] Through continuous iteration, when When the change values ​​are all less than 0.01°, it is considered that at this time... The deviation value tends to be consistent with the actual fixed angle value, and the actual fixed angle deviation value is finally determined.

[0044] S7, will and Substituting these values ​​into the passive localization algorithm based on the Earth ellipsoid model in step S3, we obtain a new passive localization algorithm based on the Earth ellipsoid model.

[0045] Then, the digital elevation model file iteration described in step S3 is performed to obtain the final altitude of the center position of the field of view.

[0046] S8, according to Perform linear motion compensation.

[0047] In step S8, the result obtained in step S6 is... and the result obtained in step S7 Substitute distance In the calculation formula, when the distance at each time point... When the accuracy is relatively high, applying a linear motion compensation program can yield accurate platform angular velocity compensation values ​​during subsequent flight.

[0048]

[0049] The method described in this invention can accurately stabilize the field of view in a specific area by using angular velocity compensation values ​​to control the platform axis in real time.

Claims

1. A linear motion compensation control method based on a digital elevation model, characterized by, The method includes the following steps: S1, according to the flight route, the digital elevation model file of the area to be measured is imported into the unmanned aerial vehicle pod, in the digital elevation model file, the initial altitude of the target ground is: S2, real-time acquisition of position and attitude information of the combined navigation system on the unmanned aerial vehicle nacelle: latitude , longitude , altitude , heading angle , pitch angle and roll angle and the attitude information of the nacelle encoder: azimuth angle and pitch angle represents the first time of linear motion compensation, , , represents the total time of linear motion compensation; S3, will By substituting the passive positioning algorithm based on the Earth ellipsoid model into the digital elevation model file, the initial altitude of the center of the field of view is obtained. S4, according to Solve for the initial platform inertial angular velocity compensation value; S5. Calculate the initial offset angle based on the initial platform inertial angular velocity compensation value. ; S6. Set the initial offset angle to the difference between the observed value and the predicted value, and fix the angle deviation value. As a state variable, the true fixed angle deviation value is solved using the Kalman filter algorithm. S7, will and Substitute the values ​​into the passive positioning algorithm based on the Earth ellipsoid model in step S3, and perform the iteration of the digital elevation model file to obtain the final altitude of the center position of the field of view. S8, according to and Perform linear motion compensation.

2. The linear motion compensation control method based on a digital elevation model according to claim 1, characterized in that, In step S2, the position and attitude information of the integrated navigation system on the UAV pod and the attitude information of the pod encoder meet the timing alignment requirements.

3. The linear motion compensation control method based on a digital elevation model according to claim 2, characterized in that, In step S3, the digital elevation model file is iterated to obtain the initial altitude of the center position of the field of view. Specifically: S31, will Substitute the passive localization algorithm based on the Earth ellipsoid model: Location results obtained: ,in, Query latitude and longitude of the digital elevation model representing the area to be measured; S32, will Substituting into the digital elevation model file from step S1, we obtain Corresponding ground elevation ; S33, Judgment Is it true? If it is true, then... The value increases Meters, iteratively execute steps S31-S33; If this condition is not met, then the initial altitude of the center of the field of view is obtained. at this time, The value and The values ​​are equal.

4. The linear motion compensation control method based on a digital elevation model according to claim 3, characterized in that, In step S4, the specific steps for calculating the initial platform inertial angular velocity compensation value are as follows: S41, based on A passive localization algorithm based on the Earth ellipsoid model is used to solve the problem. At that time, the initial ground latitude and longitude of the center point of the field of view S42, according to and the initial latitude and longitude of the pod Solve for the initial distance between the pod and the center point of the field of view. ; S43. Calculate the angular velocity compensation value of the platform's orientation using the following formulas. Angular velocity compensation value of platform pitch As the initial platform inertial angular velocity compensation value: in, The initial axial velocity representing spatial orientation. This represents the initial axial velocity of the space pitch.

5. The linear motion compensation control method based on a digital elevation model according to claim 4, characterized in that, In step S5, initial linear motion compensation is performed based on the initial platform inertial angular velocity compensation value, and the initial offset angle is calculated using the sparse optical flow algorithm. .

6. The linear motion compensation control method based on a digital elevation model according to claim 5, characterized in that, In step S6, the initial error propagation matrix parameters are set, and the offset angles calculated at different times are substituted into the Kalman filter algorithm. Through iteration, the value of the state variable is finally stabilized at the true fixed angle deviation value. .

7. An electronic device, comprising: A memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that the processor, when executing the computer program, implements the steps of the method as claimed in any one of claims 1 to 6.

8. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by a processor, it implements the steps of the method as described in any one of claims 1 to 6.

Images