A linear motion compensation control method based on digital elevation model

Abstract

The application discloses a linear motion compensation control method based on a digital elevation model. The application relates to the field of automatic control calculation, and particularly relates to a linear motion compensation control method based on a digital elevation model. The application provides ground elevation front information based on a digital elevation model, combines a pixel offset angle obtained by using an optical flow estimation algorithm, iteratively obtains an accurate fixed angle deviation value according to a Kalman filtering method, and simultaneously finds an accurate ground elevation. Finally, linear motion compensation is carried out according to the accurate fixed angle deviation value and the ground elevation. The method comprises the following steps: importing a digital elevation model file; collecting real-time information; calculating an angular velocity compensation value based on an initial ground elevation of a target; calculating an angle offset based on a sparse optical flow estimation algorithm; correcting a fixed deviation value by using a Kalman filtering method; and performing linear motion compensation based on a final elevation of a real fixed deviation value and a field center position.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of automatic control, in particular to a linear motion compensation control method based on a digital elevation model. BACKGROUND

[0002] When a UAV pod is used to survey a specific area, the angular velocity needs to be compensated in real time to stabilize the field of view in the target area. In this process, the calculation of axial velocity and the estimation of the height difference between the pod and the ground are critical. The calculation of axial velocity can be completed by using the information of the aircraft's integrated navigation system and the pod encoder, but there is a fixed angular deviation due to installation and other reasons. On the other hand, the accuracy of the ground elevation as external information input is also difficult to guarantee. Especially when the aircraft speed is high, the fixed angular deviation and the estimation error of the ground elevation will greatly reduce the compensation effect of the angular velocity. Therefore, there is an urgent need for a method that can accurately estimate the ground elevation and correct the fixed angular deviation to achieve precise and stable tracking of the target area.

[0003] Chinese invention patent "Linear motion compensation control method based on image matching" (CN117953007B) uses image matching algorithm to obtain inter-frame pixel offset, thereby inversely deducing the ground elevation of the field of view center. Although this method can achieve the effect of automatically and quickly estimating the accurate ground elevation of the UAV pod without power supply, and then performing precise linear motion compensation, it still has some problems. For example, 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 encoder. When the speed-height ratio of the aircraft is very large, the boresight of the pod can only stay in a small area for a very short time, so only a small amount of data can be obtained, which makes it difficult to accurately estimate the result. Moreover, due to the fixed angular deviation, even when the speed-height ratio of the aircraft is not large, the estimated ground elevation is not accurate, which greatly reduces the effect of linear motion compensation. SUMMARY

[0004] In order to solve the problem that it is difficult to estimate the ground altitude and correct the fixed angle deviation value in the existing linear motion compensation control method, and the final compensation effect is poor, the application provides a linear motion compensation control method based on a digital elevation model, which provides ground altitude pre-information based on a digital elevation model, combines the pixel offset angle obtained by an optical flow estimation algorithm, and iteratively obtains an accurate fixed angle deviation value according to a Kalman filtering method, and simultaneously finds an accurate ground altitude. Finally, linear motion compensation is performed according to the accurate fixed angle deviation value and the ground altitude.

[0005] The method comprises the following steps:

[0006] S1, according to the flight route, the digital elevation model file of the to-be-measured area is imported into the unmanned aerial vehicle pod, and the target ground initial altitude in the digital elevation model file is:

[0007] S2, real-time collection of position and attitude information of the combined navigation system on the unmanned aerial vehicle pod: latitude , longitude , height , heading angle , pitch angle and roll angle and the attitude information of the pod encoder: azimuth angle and pitch represents the i-th moment of linear motion compensation, , , represents the total moment of linear motion compensation;

[0008] S3, substituting

[0009] into the passive positioning algorithm based on the earth ellipsoid model, the digital elevation model file is iterated to obtain the initial altitude of the center position of the field of view

[0010] S4, according to , the initial platform inertial angular velocity compensation value is solved;

[0011] S5, according to the initial platform inertial angular velocity compensation value, the initial offset angle is calculated;

[0012] S6, the initial offset angle is set as the difference between the observation value and the prediction value, the fixed angle deviation value is the state quantity, and the real fixed angle deviation value is solved by the Kalman filtering algorithm.

[0013] S7, the and into the passive positioning algorithm based on the earth ellipsoid model in step S3, and perform the digital elevation model file iteration to obtain the final altitude of the field of view center position

[0014] S8, according to and linear motion compensation is performed.

[0015] Further, in step S2, the position and attitude information of the combined navigation system on the unmanned aerial vehicle nacelle and the attitude information of the nacelle encoder meet the timing alignment requirements.

[0016] Further, in step S3, the digital elevation model file iteration is performed to obtain the initial altitude of the field of view center position Specifically:

[0017] S31, the is substituted into the passive positioning algorithm based on the earth ellipsoid model:

[0018] to obtain the positioning result: , wherein, represents the digital elevation model query longitude and latitude of the region to be measured;

[0019] S32, the is substituted into the digital elevation model file in step S1 to obtain corresponding ground altitude

[0020] S33, it is judged is established, if yes, the value of is increased by meters, and then steps S31-S33 are iteratively executed.

[0021] If not, the initial altitude of the field of view center position is obtained, at this time, the value of is equal to the value of

[0022] Further, in step S4, the initial platform inertial angular velocity compensation value is solved specifically as follows:

[0023] S41, based on by the passive positioning algorithm based on the earth ellipsoid model, when , the initial ground longitude and latitude of the field of view center point

[0024] S42, according to and the initial longitude and latitude of the nacelle , solve the initial distance between the pod and the center point of the field of view ;

[0025] S43, calculate the angular velocity compensation value of the platform azimuth and the angular velocity compensation value of the platform pitch respectively by the following formula and the angular velocity compensation value of the platform pitch as the initial platform inertial angular velocity compensation value:

[0026]

[0027] wherein, indicates the initial axial velocity of the space azimuth, indicates the initial axial velocity of the space pitch.

[0028] Further, in step S5, according to the initial platform inertial angular velocity compensation value, initial linear motion compensation is performed, and the initial offset angle is calculated by a sparse optical flow algorithm .

[0029] Further, in step S6, the initial error transfer matrix parameter is set, the offset angle calculated at different times is substituted into the Kalman filtering algorithm, and the value of the state quantity is finally stabilized at the real fixed angle deviation value

[0030] An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the above method when executing the computer program.

[0031] A computer readable storage medium stores a computer program, wherein the computer program is executed by a processor to implement the steps of the above method.

[0032] The method has the following advantages:

[0033] (1) The method estimates the accurate fixed angle deviation value by pre-importing the digital elevation model file, and simultaneously queries the accurate ground elevation (the final elevation of the field of view center position), which can accurately estimate the ground elevation and correct the fixed angle deviation value, and the method can quickly and accurately estimate the fixed angle deviation in the linear motion compensation control process, and in turn optimize the compensation effect (use the real fixed angle deviation value to further accurately estimate the final elevation of the field of view center position), so as to achieve the purpose of accurately and stably tracking the target area.

[0034] (2) The method can overcome some complex and extreme conditions (such as too large speed-altitude ratio of the airplane, no calibration of the fixed angle deviation between the airplane and the pod, etc.), finally quickly and accurately estimate the fixed angle deviation value and the ground altitude and other necessary conditions, so that the effect of linear motion compensation can meet the demand at any time and under any condition. BRIEF DESCRIPTION OF DRAWINGS

[0035] Figure 1 The method flowchart of the embodiment of the application is described.

[0036] Figure 2 The passive linear motion compensation flowchart of the application is described. DETAILED DESCRIPTION

[0037] The technical solutions of the application will be described clearly and completely below with reference to the drawings. Obviously, the described embodiments are part of the embodiments of the application, rather than all the embodiments. Based on the embodiments in the application, all other embodiments obtained by those skilled in the art without creative labor fall within the protection scope of the application.

[0038] The embodiment provides a linear motion compensation control method based on a digital elevation model, and a flowchart of the method is as shown in Figure 1 The method comprises the following steps:

[0039] S1, according to a flight route, a digital elevation model file of a region to be measured is imported into a pod of an unmanned aerial vehicle, in the digital elevation model file, an initial ground altitude of a target is:

[0040] S2, position and attitude information of a combined navigation system on the pod of the unmanned aerial vehicle is collected in real time: latitude , longitude , height , heading angle , pitch angle and roll angle , and attitude information of a pod encoder: azimuth angle and pitch angle represents the i-th moment of linear motion compensation, , , represents the total moment of linear motion compensation;

[0041] In step S2, the pod is installed on the base of the unmanned aerial vehicle, the link is debugged, the position and attitude information of the combined navigation system on the aircraft is ensured to be transmitted into the pod in real time, and the attitude information of the pod encoder is collected. This step requires timing alignment operation to strictly ensure the synchronization of the aircraft and the pod information (to meet the timing alignment requirement). Here, the time before linear motion compensation is started is set as the 0th time, i.e. .

[0042] S3, the

[0043] is substituted into the passive positioning algorithm based on the earth ellipsoid model, the digital elevation model file is iterated, and the initial altitude of the center position of the field of view is obtained

[0044] 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:

[0045] S31, the is substituted into the passive positioning algorithm based on the earth ellipsoid model:

[0046] The positioning result is obtained: , wherein, represents the digital elevation model query longitude and latitude of the region to be measured;

[0047] S32, the is substituted into the digital elevation model file in step S1 to obtain the corresponding ground altitude

[0048] S33, it is judged whether is established, if yes, the value of is increased by meters, and then steps S31-S33 are iterated and executed;

[0049] If not, the initial altitude of the center position of the field of view is obtained At this time, the value of is equal to the value of

[0050] S4, according to , the initial platform inertia angular velocity compensation value is solved;

[0051] In step S4, as shown in Figure 2 , first, the heading angle of the combined navigation system at is substituted into the pitch angle and the 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:

[0052]

[0053]

[0054] 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...) );

[0055] 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.

[0056] 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:

[0057]

[0058]

[0059] 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 .

[0060]

[0061] 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:

[0062]

[0063]

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

[0065] S5. Calculate the initial offset angle based on the initial platform inertial angular velocity compensation value. ;

[0066] 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) ).

[0067] 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.

[0068] 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:

[0069] predict:

[0070] State variables:

[0071] Covariance: ;

[0072] 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 representing process noise.

[0073] update:

[0074] Kalman gain:

[0075] update state:

[0076]

[0077] update covariance:

[0078] where, RMS error, actual value of the deflection angle at the current time, predicted value of the deflection angle, the difference between the two is so the update state is rewritten as:

[0079]

[0080] by constantly iterating, when the change value of is less than 0.01°, it is considered that at this time and the real angle fixed deviation value tends to be consistent, and the final real fixed angle deviation value

[0081] S7, put and into the step S3 based on the earth ellipsoid model passive positioning algorithm, get a new based on the earth ellipsoid model passive positioning algorithm

[0082]

[0083] and perform the digital elevation model file iteration described in step S3, get the final altitude of the center of the field of view

[0084] S8, according to linear motion compensation.

[0085] In step S8, put and into the calculation formula of the distance When the distance at each time is accurate, apply linear motion compensation program, you can get accurate platform angular velocity compensation value in the subsequent flight process

[0086] ​​​

[0087] The method can achieve the purpose of accurately stabilizing the field of view in a specific area by controlling the platform axis in real time by using the angular velocity compensation value.

Claims

1. A linear motion compensation control method based on a digital elevation model, characterized in that, 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: 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; 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