Adaptive EKF target positioning method for optoelectronic pod based on system noise estimation

Abstract

The present invention relates to an electro-optical pod adaptive EKF target positioning method based on system noise estimation, comprising: a UAV mounted with an electro-optical pod for cruise flight, an inertial navigation system installed on the base of the electro-optical pod, the electro-optical pod operated by a ground station so that the center of the field of view locks onto a ground target, and laser ranging is performed to obtain the distance L between the electro-optical pod and the ground target; the system noise Q of the target positioning model is calculated based on the horizontal miss amount and vertical miss amount when the electro-optical pod image tracks the target, and the distance L between the electro-optical pod and the ground target; the state equation and observation equation of the target positioning are established, and the geographic coordinate value of the ground target is calculated using the EKF filtering algorithm based on the longitude and latitude coordinates of the electro-optical pod itself, the distance L between the electro-optical pod and the ground target, and the system noise Q, for tracking the electro-optical pod. The present invention achieves accurate estimation of the target motion state and is used for UAV target reconnaissance and airborne electro-optical pod visual servo tracking.

Description

Technical Field

[0001] The invention relates to an electro-optical pod adaptive EKF target positioning method based on system noise estimation, and belongs to the technical field of airborne electro-optical pod target positioning. Background Art

[0002] Airborne electro-optical pods, integrating visible light cameras, infrared thermal imagers, and laser rangefinders, are crucial payloads for UAVs. As they develop towards multifunctionality and intelligence, they are finding widespread application in a range of fields, including maritime search and rescue, target reconnaissance and surveillance, and battlefield situation assessment. High-precision positioning of key ground targets is crucial for field rescue and laser-guided strikes.

[0003] Currently, active positioning methods based on laser ranging and inertial group attitude angles offer high accuracy. Kalman filtering and its extended algorithms, when applied to target positioning for electro-optical pods, can provide optimal estimated coordinates of ground targets using a linear minimum variance state estimation method. However, the Kalman filter's parameter settings, such as system noise variance and measurement noise variance, significantly impact the filtering results.

[0004] Generally, measurement noise depends on the sensor's inherent measurement error, and measurement noise variance data can be obtained from the sensor's performance manual. However, system noise is often selected based on experience and exhibits a certain degree of randomness. Different system noise variance values ​​significantly impact filtering results. In actual airborne electro-optical pods performing laser ranging and positioning of ground targets, system noise is affected by target ranging distance, optical axis stability, and image tracking accuracy, exhibiting time-varying characteristics. While existing methods such as forgetting factors, expectation maximization, and genetic iteration can adjust system noise variance, these methods are computationally cumbersome and time-consuming, failing to meet the real-time requirements of practical applications. Summary of the Invention

[0005] The technical problem solved by the present invention is to overcome the shortcomings of the existing technology and propose an electro-optical pod adaptive EKF target positioning method based on system noise estimation to achieve accurate estimation of the target motion state for use in UAV target reconnaissance and airborne electro-optical pod visual servo tracking.

[0006] The solution of the present invention is:

[0007] The optoelectronic pod adaptive EKF target positioning method based on system noise estimation includes:

[0008] The UAV is equipped with an optoelectronic pod for cruise flight. An inertial navigation system is installed on the base of the optoelectronic pod. The optoelectronic pod is operated through a ground station to lock the ground target at the center of the field of view. Laser ranging is performed to obtain the distance L between the optoelectronic pod and the ground target.

[0009] The system noise Q of the target positioning model is calculated based on the horizontal miss distance, vertical miss distance and the distance L between the optoelectronic pod and the ground target when the optoelectronic pod image tracks the target;

[0010] The state equation and observation equation for target positioning are established. According to the latitude and longitude coordinates of the optoelectronic pod itself, the distance L between the optoelectronic pod and the ground target, and the system noise Q, the EKF filtering algorithm is used to calculate the geographic coordinates of the ground target for target tracking of the optoelectronic pod.

[0011] Furthermore, the method of operating the optoelectronic pod via the ground station includes:

[0012] The ground station joystick is used to search for and lock onto the ground target, putting the pod in a stable target image tracking state. The pod laser rangefinder is powered on to perform laser ranging on the image-locked ground target, recording the distance L between the electro-optical pod and the ground target, the GPS coordinates of the pod base inertial group, and the horizontal and vertical miss distances of the image tracking.

[0013] Furthermore, the calculation method of the system noise Q in the target positioning model includes:

[0014] Based on the deviation angle between the payload's visual axis and the ground target and the distance between the optoelectronic pod and the ground target, the distance that the laser ranging point deviates from the actual target at the kth moment is calculated as:

[0015]

[0016] Among them L k is the distance between the optoelectronic pod and the ground target at time k, μ xk is the horizontal deviation angle of the visual axis, μ yk is the vertical deviation angle of the visual axis;

[0017] Statistical calculation of the deviation from the target distance [Δγ1, Δγ2...Δγ k ], the system noise Q in the target positioning model can be obtained.

[0018] Furthermore, in the process of tracking ground targets, the sampling points are updated over time in the form of a sliding window, and then the deviation distance data from the target within the sliding window is re-counted and calculated, and the corresponding system noise variance is updated in real time.

[0019] Furthermore, by querying the focal length of the visible light camera at time k, the horizontal field of view angle of the current image is obtained as κ k , the vertical field of view is λ k The total number of pixels in the horizontal and vertical directions of the image are M and N respectively. According to the horizontal miss distance m k and vertical miss distance n kCalculate the angle of the load's visual axis from the target:

[0020]

[0021] Furthermore, the method of establishing the observation equation for target positioning includes:

[0022] At time k, the azimuth attitude angle α and pitch attitude angle β of the target relative to the optoelectronic pod are obtained. Taking the drone pod as the origin, the position of the target in the body coordinate system is (x bk ,y bk ,z bk ), taking the measured values ​​of α, β, and L as observation quantities, the observation equation of the target positioning system is established as follows:

[0023]

[0024] Furthermore, the established state and observation equations have nonlinear characteristics, and the extended Kalman filter algorithm is used to estimate the target position, and h(X k )exist Perform a first-order Taylor series expansion at , omit the second-order and above terms, and the linearized observation equation is:

[0025]

[0026] Among them, k is the observation noise at the kth moment, is the linearized observation matrix.

[0027] Furthermore, based on the linearized state and observation equations, the specific iterative process is as follows:

[0028] State one-step prediction:

[0029]

[0030] One-step prediction mean square error:

[0031]

[0032] Filter gain update:

[0033]

[0034] State estimate update:

[0035]

[0036] State estimation error update:

[0037]

[0038] Among them, Φ k|k-1 Indicates t k-1 The one-step transfer matrix of the time system, Γ k|k-1 Indicates t k-1 The noise driving matrix of the system at time instant, H k represents the linearized observation matrix, Q k-1 Indicates t k-1 The system noise variance matrix at the moment, R k Indicates t k The noise driving matrix of the system at time t.

[0039] Furthermore, when the optoelectronic pod is performing target positioning, the longitude and latitude coordinates of the pod base collected are (N0, W0, H0), and the longitude, latitude, and elevation coordinates of the ground target are calculated according to the following formula (N Tar ,W Tar ,H Tar )for:

[0040]

[0041]

[0042]

[0043] Where Re is the radius of the Earth.

[0044] Furthermore, the expression of the state equation is:

[0045] X k =ΦX k-1 +w k-1

[0046] Among them, X k =[x k ,y k ,z k ] T t k The target state at the moment, Φ is the unit matrix, w k-1 is the system noise.

[0047] The optoelectronic pod adaptive EKF target positioning system based on system noise estimation includes:

[0048] Distance acquisition module between the optoelectronic pod and the ground target: The UAV is equipped with an optoelectronic pod for cruise flight. An inertial navigation system is installed on the base of the optoelectronic pod. The optoelectronic pod is operated through the ground station to lock the ground target at the center of the field of view. Laser ranging is performed to obtain the distance L between the optoelectronic pod and the ground target.

[0049] The system noise determination module of the target positioning model calculates the system noise Q of the target positioning model based on the horizontal miss distance, vertical miss distance and the distance L between the optoelectronic pod and the ground target when the optoelectronic pod image tracks the target;

[0050] Ground target geographic coordinate value determination module: establish the state and observation equations for target positioning, and use the EKF filtering algorithm to calculate the geographic coordinate value of the ground target based on the latitude and longitude coordinates of the optoelectronic pod itself, the distance L between the optoelectronic pod and the ground target, and the system noise Q, for target tracking of the optoelectronic pod.

[0051] The beneficial effects of the present invention compared with the prior art are:

[0052] (1) The system noise variance in the current EKF filter application process is generally a fixed value selected through experience, which is random to a certain extent. Different selected values ​​will have a huge impact on the EKF filtering results. At the same time, in the actual process of laser ranging and positioning of ground targets by airborne optoelectronic pods, the system noise will be affected by the target ranging distance, optical axis stability, and image target tracking accuracy, which is a dynamic process. The method of the present invention can always keep the system noise statistics adaptively close to the true value, reducing the risk of divergence of results due to the selection of constant values ​​for system noise in traditional Kalman filtering algorithms, thereby achieving high-precision positioning of ground targets by airborne optoelectronic pods.

[0053] (2) While existing methods such as forgetting factor, expectation maximization, and genetic algorithms can be added to the extended Kalman filter model to adjust the system noise variance, these methods are computationally cumbersome and time-consuming, and cannot meet the real-time requirements of practical applications. The system noise estimation algorithm proposed in this invention only requires statistical calculations of laser ranging values ​​and image tracking miss distances, requiring little computation, making it easy to deploy in embedded systems and highly practical. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 Schematic diagram of the relative positions of the airborne optoelectronic pod and the ground target laser ranging "measurement point" of the present invention;

[0055] Figure 2 Flowchart of the method of the present invention. DETAILED DESCRIPTION

[0056] The present invention will be further described below in conjunction with the embodiments.

[0057] The optoelectronic pod implemented in the present invention has an inertial group sensor installed in its base. The optoelectronic pod adaptive EKF target positioning method based on system noise estimation includes the following steps: Figure 1 、 2 As shown:

[0058] Step 1: Before the drone takes off, the pod needs to be powered on and initialized.

[0059] (1) The inertial group installed on the base of the optoelectronic pod needs to complete the power-on alignment initialization. At the same time, the pitch axis and azimuth axis of the optoelectronic pod servo system rotate to the zero position of the frame angle.

[0060] (2) The UAV cruises to the ground target area and uses the joystick on the ground station console to align the "lock window" in the image field of view with the ground target, causing the pod to enter the visual servo tracking state. When the image "lock window" stably tracks the target, the pod laser rangefinder is powered on and performs laser ranging on the ground target locked in the image, recording the laser ranging value L for a period of time, the GPS coordinates of the pod base inertial group, and the horizontal and vertical miss distances x and y of the image tracking.

[0061] Step 2: Based on the horizontal miss distance x, vertical miss distance y and laser ranging value L when the optoelectronic pod image tracks the target, the system noise Q of the target positioning model within this time period is statistically calculated.

[0062] (1) At time k, the horizontal miss distance x is obtained by sampling k , vertical miss distance y k and laser distance value L k By querying the focal length of the visible light camera, we can know that the horizontal field of view angle of the current image is κ k , the vertical field of view is λ k , the total number of pixels in the horizontal and vertical directions of the image are M and N respectively. According to the horizontal miss distance m k and vertical miss distance n k Calculate the angle of the load's visual axis from the target:

[0063]

[0064]

[0065] Where μ xk is the horizontal deviation angle of the visual axis, μ yk is the vertical deviation angle of the visual axis.

[0066] (2) Based on the deviation angle between the payload's sight axis and the ground target and the laser ranging value, since the angle of the sight axis deviating from the actual target is very small, generally in the order of micro-arc, the distance at which the laser ranging point deviates from the actual target at the kth moment can be calculated as:

[0067]

[0068] Statistical calculation of the deviation from the target distance [Δγ1, Δγ2...Δγ k], the system noise Q in the target positioning model can be obtained k In the process of tracking ground targets, the sampling points will be updated over time in the form of a sliding window, and then the deviation distance data within the sliding window will be recalculated and calculated, and the corresponding system noise variance will be updated in real time.

[0069] Step 3: Establish the state equation and observation equation for target positioning. According to the longitude and latitude coordinates of the optoelectronic pod itself, the laser ranging value L and the system noise Q, the EKF filtering algorithm is used to calculate the geographic coordinates of the ground target.

[0070] (1) The position of the target point in the geodetic rectangular coordinate system is selected as the system state variable, and the state equation and observation equation of the target positioning system are established.

[0071] The expression of the state equation is

[0072] X k =ΦX k-1 +w k-1

[0073] Among them, X k =[x k ,y k ,z k ] T t k The target state at the moment, Φ is the unit matrix, w k-1 is the system noise.

[0074] The airborne optoelectronic imaging system performs multiple measurements on the fixed target and can obtain the azimuth attitude angle α and pitch attitude angle β of the target relative to the optoelectronic platform at time k. The position of the target in the body coordinate system is (x bk ,y bk ,z bk ). Taking the measured values ​​of α, β, and L as the observed quantities, the observation equation is:

[0075] Z k =h(X k )+υ k

[0076] Among them, v k is the measurement noise, which is Gaussian white noise with a mean of 0.

[0077]

[0078] (2) According to the state and observation equation of the system, the UAV uses the optoelectronic pod to perform laser ranging on the ground target during flight, and then completes the positioning calculation. The established system observation equation has nonlinear characteristics, so the extended Kalman filter algorithm is used to estimate the target position. Therefore, h(X k)exist Perform a first-order Taylor series expansion at , omit the second-order and above terms, and the linearized observation equation is:

[0079]

[0080] Among them, among them, is the linearized observation matrix, expressed as the Jacobian matrix.

[0081] Based on the linearized observation equation and state equation, the specific iterative process is as follows:

[0082] State one-step prediction:

[0083]

[0084] One-step prediction mean square error:

[0085]

[0086] Filter gain update:

[0087]

[0088] State estimate update:

[0089]

[0090] State estimation error update:

[0091]

[0092] Among them, Φ k|k-1 Indicates t k-1 The one-step transfer matrix of the time system, Γ k|k-1 Indicates t k-1 The noise driving matrix of the system at time instant, Q k-1 Indicates t k-1 The system noise variance matrix at the moment, R k Indicates t k The noise driving matrix of the system at time t.

[0093] (3) When the optoelectronic pod is performing target positioning, the longitude and latitude coordinates of the pod base collected are (N0, W0, H0). The longitude, latitude, and elevation coordinates of the ground target are calculated according to the following formula (N Tar ,W Tar ,H Tar )for:

[0094]

[0095]

[0096]

[0097] Where Re is the radius of the Earth.

[0098] An electro-optical pod adaptive EKF target positioning system based on system noise estimation is characterized by comprising:

[0099] Distance acquisition module between the optoelectronic pod and the ground target: The UAV is equipped with an optoelectronic pod for cruise flight. An inertial navigation system is installed on the base of the optoelectronic pod. The optoelectronic pod is operated through the ground station to lock the ground target at the center of the field of view. Laser ranging is performed to obtain the distance L between the optoelectronic pod and the ground target.

[0100] The system noise determination module of the target positioning model calculates the system noise Q of the target positioning model based on the horizontal miss distance x and vertical miss distance y when the optoelectronic pod image tracks the target and the distance L between the optoelectronic pod and the ground target;

[0101] Ground target geographic coordinate value determination module: establish the state and observation equations for target positioning, and use the EKF filtering algorithm to calculate the geographic coordinate value of the ground target based on the latitude and longitude coordinates of the optoelectronic pod itself, the distance L between the optoelectronic pod and the ground target, and the system noise Q.

[0102] The present invention proposes to use a combination of laser ranging values ​​and image miss distances to statistically estimate the system noise Q in the extended Kalman filter (EKF) algorithm over a period of time when locating ground targets. Traditional EKF filtering algorithms always use fixed and unchanging empirical values ​​of system noise, which will bring large errors to the output results of the filter and even cause the filter to diverge. However, the use of this method to estimate the system noise Q in target positioning allows the EKF to adaptively select filter parameters that are close to the real ones. After participating in the EKF filtering, it can output a higher ground target positioning accuracy, and the computational complexity is small in embedded systems, which has high practicality.

[0103] The system noise variance in current EKF filter applications is typically fixed and selected empirically. This introduces a degree of randomness, and different values ​​can significantly impact the EKF filtering results. Furthermore, in actual laser ranging and positioning of ground targets by an airborne electro-optical pod, system noise is dynamically affected by target ranging distance, optical axis stability, and image tracking accuracy. The proposed method consistently maintains an adaptive approximation of the system noise statistic to its true value, reducing the risk of diverging results caused by selecting a constant value for system noise in traditional Kalman filtering algorithms. This approach enables high-precision positioning of ground targets by airborne electro-optical pods.

[0104] Although the present invention has been disclosed above in terms of preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art may make possible changes and modifications to the technical solutions of the present invention by using the methods and technical contents disclosed above without departing from the spirit and scope of the present invention. Therefore, any simple modifications, equivalent changes and modifications made to the above embodiments based on the technical essence of the present invention without departing from the content of the technical solutions of the present invention shall fall within the scope of protection of the technical solutions of the present invention.

Claims

1. An adaptive EKF target positioning method for an optoelectronic pod based on system noise estimation is characterized by: include: The UAV is equipped with an optoelectronic pod for cruise flight. An inertial navigation system is installed on the base of the optoelectronic pod. The optoelectronic pod is operated through a ground station to lock the ground target at the center of the field of view. Laser ranging is performed to obtain the distance L between the optoelectronic pod and the ground target. The system noise Q of the target positioning model is calculated based on the horizontal miss distance, vertical miss distance and the distance L between the optoelectronic pod and the ground target when the optoelectronic pod image tracks the target; The state equation and observation equation for target positioning are established. Based on the longitude and latitude coordinates of the optoelectronic pod itself, the distance L between the optoelectronic pod and the ground target, and the system noise Q, the EKF filtering algorithm is used to calculate the geographic coordinates of the ground target for target tracking of the optoelectronic pod. The calculation method of the system noise Q in the target positioning model includes: Based on the deviation angle between the payload's visual axis and the ground target and the distance between the optoelectronic pod and the ground target, the distance that the laser ranging point deviates from the actual target at the kth moment is calculated as: Among them L k is the distance between the optoelectronic pod and the ground target at time k, μ xk is the horizontal deviation angle of the visual axis, μ yk is the vertical deviation angle of the visual axis; Statistical calculation of the deviation from the target distance [Δγ1, Δγ2...Δγ k ], the system noise Q in the target positioning model can be obtained; By querying the focal length of the visible light camera at time k, the horizontal field of view angle of the current image is obtained as κ k , the vertical field of view is λ k The total number of pixels in the horizontal and vertical directions of the image are M and N respectively; according to the horizontal miss distance m k , current image horizontal field angle κ k , the total number of pixels M in the horizontal direction of the image, and the μ xk Horizontal deviation angle of the visual axis; according to the vertical miss distance n k , the vertical field angle of the current image λ k , the total number of pixels N in the vertical direction of the image, and the μ yk Vertical deviation angle of the boresight.

2. The optoelectronic pod adaptive EKF target positioning method based on system noise estimation according to claim 1 is characterized in that: Methods for operating the electro-optical pod via a ground station include: The ground station joystick is used to search for and lock onto the ground target, putting the pod in a stable target image tracking state. The pod laser rangefinder is powered on to perform laser ranging on the image-locked ground target, recording the distance L between the electro-optical pod and the ground target, the GPS coordinates of the pod base inertial group, and the horizontal and vertical miss distances of the image tracking.

3. The optoelectronic pod adaptive EKF target positioning method based on system noise estimation according to claim 2 is characterized in that: In the process of tracking ground targets, the sampling points are updated over time in the form of a sliding window, and then the deviation distance data from the target within the sliding window is re-counted and calculated, and the corresponding system noise variance is updated in real time.

4. The optoelectronic pod adaptive EKF target positioning method based on system noise estimation according to claim 3 is characterized in that: By querying the focal length of the visible light camera at time k, the horizontal field of view angle of the current image is obtained as κ k , the vertical field of view is λ k The total number of pixels in the horizontal and vertical directions of the image are M and N respectively. According to the horizontal miss distance m k and vertical miss distance n k Calculate the angle of the load's visual axis from the target:

5. The optoelectronic pod adaptive EKF target positioning method based on system noise estimation according to claim 1 is characterized in that: Methods for establishing observation equations for target positioning include: At time k, the azimuth attitude angle α and pitch attitude angle β of the target relative to the optoelectronic pod are obtained. Taking the drone pod as the origin, the position of the target in the body coordinate system is (x bk ,y bk ,z bk ), taking the measured values ​​of α, β, and L as observation quantities, the observation equation of the target positioning system is established as follows:

6. The optoelectronic pod adaptive EKF target positioning method based on system noise estimation according to claim 5 is characterized in that: The established state and observation equations have nonlinear characteristics. The extended Kalman filter algorithm is used to estimate the target position. k )exist Perform a first-order Taylor series expansion at , omit the second-order and above terms, and the linearized observation equation is: Among them, k is the observation noise at the kth moment, is the linearized observation matrix, X k t k The target state at the moment.

7. The optoelectronic pod adaptive EKF target positioning method based on system noise estimation according to claim 6, characterized in that: Based on the linearized state and observation equations, the specific iterative process is as follows: State one-step prediction: One-step prediction mean square error: Filter gain update: State estimate update: State estimation error update: Among them, Φ k|k-1 Indicates t k-1 The one-step transfer matrix of the time system, Γ k|k-1 Indicates t k-1 The noise driving matrix of the system at time instant, H k represents the linearized observation matrix, Q k-1 Indicates t k-1 The system noise variance matrix at time , R k Indicates t k The noise driving matrix of the system at time t.

8. The optoelectronic pod adaptive EKF target positioning method based on system noise estimation according to claim 1 is characterized in that: When the optoelectronic pod is performing target positioning, the longitude and latitude coordinates of the pod base collected are (N0, W0, H0). The longitude, latitude and elevation coordinates of the ground target are calculated according to the following formula (N Tar ,W Tar ,H Tar )for: Where Re is the radius of the Earth.

9. The optoelectronic pod adaptive EKF target positioning method based on system noise estimation according to claim 1, characterized in that: The expression of the state equation is: X k =ΦX k-1 +w k-1 Among them, X k =[x k ,y k ,z k ] T t k The target state at the moment, Φ is the unit matrix, w k-1 is the system noise.

10. The optoelectronic pod adaptive EKF target positioning system based on system noise estimation is characterized by: include: Distance acquisition module between the optoelectronic pod and the ground target: The UAV is equipped with an optoelectronic pod for cruise flight. An inertial navigation system is installed on the base of the optoelectronic pod. The optoelectronic pod is operated through the ground station to lock the ground target at the center of the field of view. Laser ranging is performed to obtain the distance L between the optoelectronic pod and the ground target. The system noise determination module of the target positioning model calculates the system noise Q of the target positioning model based on the horizontal miss distance, vertical miss distance and the distance L between the optoelectronic pod and the ground target when the optoelectronic pod image tracks the target; Ground target geographic coordinate value determination module: establishes the state and observation equations for target positioning, and uses the EKF filtering algorithm to calculate the geographic coordinate value of the ground target based on the longitude and latitude coordinates of the optoelectronic pod itself, the distance L between the optoelectronic pod and the ground target, and the system noise Q, for target tracking of the optoelectronic pod; The calculation method of the system noise Q in the target positioning model includes: Based on the deviation angle between the payload's visual axis and the ground target and the distance between the optoelectronic pod and the ground target, the distance that the laser ranging point deviates from the actual target at the kth moment is calculated as: Among them L k is the distance between the optoelectronic pod and the ground target at time k, μ xk is the horizontal deviation angle of the visual axis, μ yk is the vertical deviation angle of the visual axis; Statistical calculation of the deviation from the target distance [Δγ1, Δγ2...Δγ k ], the system noise Q in the target positioning model can be obtained; By querying the focal length of the visible light camera at time k, the horizontal field of view angle of the current image is obtained as κ k , the vertical field of view is λ k The total number of pixels in the horizontal and vertical directions of the image are M and N respectively; according to the horizontal miss distance m k , current image horizontal field angle κ k , the total number of pixels M in the horizontal direction of the image, and the μ xk Horizontal deviation angle of the visual axis; according to the vertical miss distance n k , the vertical field angle of the current image λ k , the total number of pixels N in the vertical direction of the image, and the μ yk Vertical deviation angle of the boresight.

Images