Adaptive federated Kalman filtering unmanned aerial vehicle cluster laser communication method based on exponential sliding maximum likelihood estimation

Abstract

The invention discloses a self-adaptive federated Kalman filtering unmanned aerial vehicle cluster laser communication method based on exponential sliding maximum likelihood estimation. The method specifically comprises the steps that firstly, a visual sub-filter and a laser sub-filter are constructed through an ESMLE algorithm, then information of the two sub-filters is fused on the basis of a federated Kalman filter frame, optimal relative position estimation of a friend unmanned aerial vehicle is output, and finally the expected angle of the photoelectric pod is calculated according to the estimation. And a servo mechanism is driven to realize accurate tracking of a target and continuous maintenance of a laser communication link. The method can effectively deal with the problem of visual occlusion, overcomes the defects that a single visual sensor in an existing unmanned aerial vehicle photoelectric tracking system is easily interfered by the environment and lacks a fault isolation mechanism, and improves the stability and robustness of an unmanned aerial vehicle laser communication link under the condition that the sensor fails or the environmental noise is suddenly changed.

Description

Technical Field

[0001] This invention relates to the fields of free-space optical communication and visual servo control technology, and in particular to an adaptive federated Kalman filter-based UAV swarm laser communication method based on exponential sliding maximum likelihood estimation. Background Technology

[0002] With the increasing application of drone swarms in complex electromagnetic environments, establishing communication links with strong anti-interference capabilities and high transmission bandwidth has become crucial for achieving collaborative operations. Traditional drone-to-drone communication mainly relies on radio frequency (RF) technology. However, in environments with strong electromagnetic interference, RF signals are highly susceptible to malicious interference, suppression, or even interception, often leading to communication link interruptions or a sharp increase in data transmission error rates. In contrast, laser communication offers significant advantages such as high transmission rates, narrow beamwidths, strong anti-electromagnetic interference capabilities, and excellent security, making it an ideal solution to replace or enhance traditional RF communication. However, laser communication also places extremely high demands on the pointing accuracy of the optical transceiver. If there are deviations in the relative position estimation between drones or tracking instability, even a small pointing error can cause the communication beam to deviate from the target, resulting in link interruption.

[0003] Currently, position-based visual servoing technology has been widely adopted to achieve precise pointing of target drones. This technology relies on airborne cameras to capture target images and drives the gimbal movement by calculating the relative position. However, in high-speed maneuvering or complex battlefield environments, a single visual servoing system has significant limitations: First, due to the constraints of image processing algorithms and video streaming mechanisms, the visual system suffers from significant end-to-end latency, causing the calculated relative position information to lag behind the actual state; second, in situations such as sudden changes in lighting, target occlusion, or cluttered backgrounds, visual detection algorithms are prone to target loss, false detection, or missed detection, leading to a decrease in the reliability of observation data.

[0004] To address the aforementioned issues, a superior relative position estimation architecture is needed. Utilizing the existing laser communication links between UAVs, a multi-source information fusion method can be introduced. By combining visual observations from the electro-optical pod with navigation status information transmitted back via the laser link, the limitations of a single sensor can be mitigated. However, the reliability and accuracy of each sensor will vary under different environments. Therefore, an adaptive filter needs to be designed to dynamically handle time-varying noise and isolate faults when a sensor fails, thereby continuously outputting the optimal relative position state estimate and providing a stable and high-precision line-of-sight pointing basis for the laser communication servo system. Summary of the Invention

[0005] The purpose of this invention is to provide an adaptive federated Kalman filter-based laser communication method for UAV swarms based on exponential sliding maximum likelihood estimation. This method can dynamically process time-varying noise and achieve fault isolation when a sensor fails, thereby continuously outputting the optimal relative position state estimate and providing a stable and high-precision line-of-sight pointing basis for the laser communication servo system.

[0006] The technical solution to achieve the purpose of this invention is: an adaptive federated Kalman filter-based UAV swarm laser communication method based on exponential moving maximum likelihood estimation, comprising the following steps:

[0007] Step 1: Drone A searches for friendly Drone B in a wide field of view using its onboard vision system and uses an embedded visual target recognition algorithm to determine the pixel coordinates of Drone B in the image plane.

[0008] Step 2: Construct a cooperative tracking model and acquire multi-source observation data. Establish a relative motion state space model for the UAV. Use the visual pixel coordinates of the target obtained in Step 1 to construct a visual observation equation. Use the laser communication link to acquire the navigation state information of the target and construct a laser linear observation equation.

[0009] Step 3: Each sub-filter operates independently, using the exponential sliding maximum likelihood estimation (ESMLE) algorithm to identify the statistical characteristics of the observation noise, adaptively adjusting the filter gain, and performing fault detection and isolation on the current observation data.

[0010] Step 4: Construct a federated Kalman filter, fuse visual and laser sub-filters, and output the optimal relative position estimate;

[0011] Step 5: Calculate the desired angle of the optoelectronic pod using the globally optimal relative position, and drive the servo mechanism to achieve precise target tracking and laser communication link maintenance.

[0012] A UAV cooperative tracking and laser communication control system based on multi-source information fusion is disclosed. This system is used to implement the aforementioned adaptive federated Kalman filter UAV swarm laser communication method based on exponential moving maximum likelihood estimation. The system comprises a first module to a fourth module, the functions of which are as follows:

[0013] The first module is used to control the airborne optoelectronic pod to acquire visual images of the target UAV and calculate its pixel coordinates in the image plane; at the same time, it receives navigation status information, including position and speed, from the target UAV via a laser communication link; and combines the onboard navigation information to construct visual nonlinear observation vectors and laser linear observation vectors respectively.

[0014] The second module constructs a visual sub-filter and a laser sub-filter, and inputs the observation data obtained in the first module into the corresponding sub-filters respectively; each sub-filter uses the exponential sliding maximum likelihood estimation (ESMLE) algorithm to recursively update the observation noise covariance matrix online, and uses the updated noise parameters to perform Kalman filtering measurement updates, outputting local state estimates and covariance.

[0015] The third module constructs a federated Kalman filter as the main filter. The main filter calculates the Mahalanobis distance between the local estimates and global predictions of each effective sub-filter. Based on the magnitude of the Mahalanobis distance, an adaptive information allocation factor is generated. The information of each sub-filter is weighted and fused to output the global optimal relative state estimate. Using the adaptive information allocation factor, the main filter feeds back the fused global optimal state estimate to all sub-filters for state reset.

[0016] The fourth module, the photoelectric pointing control module, uses the globally optimal relative position to calculate the desired pitch and azimuth angles of the photoelectric pod, drives the servo mechanism to perform line-of-sight pointing control, and achieves precise tracking of the target UAV and maintenance of the laser communication link.

[0017] A mobile terminal includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, it implements the aforementioned adaptive federated Kalman filter-based UAV swarm laser communication method based on exponential sliding maximum likelihood estimation.

[0018] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps in the described adaptive federated Kalman filter UAV swarm laser communication method based on exponential moving maximum likelihood estimation.

[0019] A computer program product includes computer instructions for causing a computer to execute the aforementioned adaptive federated Kalman filter-based UAV swarm laser communication method based on exponential moving maximum likelihood estimation.

[0020] Compared with the prior art, the present invention has the following significant advantages: (1) It can dynamically process time-varying noise and realize fault isolation when a certain sensor fails, thereby continuously outputting the optimal relative position state estimate, providing a stable and high-precision line-of-sight pointing basis for the laser communication servo system; (2) Based on the statistical characteristics of the observation noise, it adaptively adjusts the filter gain and realizes the fault judgment of the sub-filter, and then performs information fusion on each sub-filter through the federated filter architecture, thereby significantly improving the robustness of the system in sensor failure and harsh environment while ensuring high-precision relative state estimation. Attached Figure Description

[0021] Figure 1This is a flowchart illustrating an adaptive federated Kalman filter-based UAV swarm laser communication method based on exponential sliding maximum likelihood estimation, according to the present invention.

[0022] Figure 2 This is a schematic diagram illustrating the principle of laser communication between unmanned aerial vehicles in an embodiment of the present invention.

[0023] Figure 3 This is a schematic diagram showing the relationship between the four coordinate systems in an embodiment of the present invention.

[0024] Figure 4 This is a flowchart illustrating the ESMLE adaptive filtering algorithm in an embodiment of the present invention.

[0025] Figure 5 This is a schematic diagram of the structure of the federated Kalman filter in an embodiment of the present invention.

[0026] Figure 6 This is a graph showing the reference position and velocity in an embodiment of the present invention.

[0027] Figure 7 This is a graph of IMU acceleration and angular velocity data collected in an embodiment of the present invention.

[0028] Figure 8 This is a graph of BeiDou navigation data subjected to interference in an embodiment of the present invention.

[0029] Figure 9 This is a comparison chart of the state estimation errors of the two filtering algorithms after adding noise in an embodiment of the present invention.

[0030] Figure 10 This is a graph showing the change in the observed noise trace in ESMLE after adding noise in an embodiment of the present invention.

[0031] Figure 11 This is a comparison curve of the relative position estimation between the federated master filter and each sub-filter in an embodiment of the present invention. Detailed Implementation

[0032] The present invention will now be described in further detail with reference to the accompanying drawings and specific embodiments.

[0033] like Figure 1 As shown, the present invention provides an adaptive federated Kalman filter-based laser communication method for UAV swarms using exponential moving maximum likelihood estimation, comprising the following steps:

[0034] Step 1: Drone A searches for friendly Drone B within a wide field of view using its onboard vision system, and determines the pixel coordinates of Drone B in the image plane using an embedded visual target recognition algorithm, such as... Figure 2 As shown;

[0035] Step 2: Construct a cooperative tracking model and acquire multi-source observation data. Establish a relative motion state space model for the UAV. Use the visual pixel coordinates of the target obtained in Step 1 to construct a visual observation equation. Use the laser communication link to acquire the navigation state information of the target and construct a laser linear observation equation.

[0036] Step 3: Each sub-filter operates independently, using the exponential sliding maximum likelihood estimation (ESMLE) algorithm to identify the statistical characteristics of the observation noise, adaptively adjusting the filter gain, and performing fault detection and isolation on the current observation data.

[0037] Step 4: Construct a federated Kalman filter, fuse visual and laser sub-filters, and output the optimal relative position estimate;

[0038] Step 5: Calculate the desired angle of the optoelectronic pod using the globally optimal relative position, and drive the servo mechanism to achieve precise target tracking and laser communication link maintenance.

[0039] As a specific example, step 2 involves constructing a cooperative tracking model and acquiring multi-source observation data to establish a relative motion state space model for the UAV. The visual pixel coordinates of the target obtained in step 1 are used to construct a visual observation equation. The target's navigation state information is obtained using a laser communication link, and a laser linear observation equation is constructed, as detailed below:

[0040] Step 2.1: Define the navigation coordinate system, body coordinate system, camera coordinate system, and pixel coordinate system, such as... Figure 3 As shown, the details are as follows:

[0041] (1) Navigation coordinate system, i.e., n-system: The North-East-Earth coordinate system is selected as the inertial reference system, with the origin O. n Set at the ground takeoff point, coordinate axis , , They point to due north, due east, and the Earth's center, respectively.

[0042] (2) Body coordinate system, i.e. b system: the origin of the body coordinate system Located at the body's center of mass, The shaft along the machine head points forward. The axis points to the right side of the fuselage. The axis is perpendicular to the fuselage and points downwards; the attitude of the body coordinate system relative to the navigation coordinate system is determined by Euler angles, namely roll angle φ, pitch angle θ, and yaw angle ψ, and the corresponding rotation matrix is ​​denoted as... ;

[0043] (3) Camera coordinate system, i.e., c-frame: the origin of the camera coordinate system Located at the optical center of the photoelectric pod camera, The axis points forward along the optical axis, i.e., in the direction of the line of sight. The axis is parallel to the horizontal direction of the imaging plane. The axis is parallel to the perpendicular direction of the imaging plane; since the installation error of the camera coordinate system relative to the center of the body coordinate system is extremely small, the installation error is negligible; the gimbal rotation of the camera system relative to the body system is determined by the rotation matrix. describe;

[0044] (4) Pixel coordinate system, i.e. two-dimensional coordinate system: The image coordinate system takes the center O′ of the imaging plane as the origin, the x-axis points to the right in the imaging plane, and the y-axis points to the top in the imaging plane; the pixel coordinate system takes the upper left corner of the image as the origin (u,v), and the unit is pixels; the two are transformed by a scaling and translation of the origin. The point P=(X,Y,Z) in three-dimensional space is transformed to the pixel coordinate system through the camera's intrinsic parameter matrix;

[0045] Step 2.2: Based on the coordinate system established in Step 2.1, select relative position, relative velocity, and relative acceleration as the system state vectors:

[0046] (1)

[0047] in, For the filter in State estimate at time 10:00 For friendly drones The relative position vector at time t, For friendly drones The relative velocity vector at time t, Friendly drones The relative acceleration vector at time t;

[0048] Considering that the maneuvers of UAVs in a short period of time are usually smooth, a constant acceleration CA model is used to describe the relative motion dynamics, and the discrete-time state-space equations are established as follows:

[0049] (2)

[0050] in, Here is the state transition matrix. This is process noise; For the filter in State estimate at time -1;

[0051] Step 2.3: Construct a visual sub-filter. The local electro-optical pod outputs images of the friendly UAV in real time. The pixel coordinates of the friendly UAV on the image plane are extracted using a target detection algorithm. Based on the pinhole imaging principle and coordinate transformation relationship, the observation equation is modeled as follows:

[0052] (3)

[0053] in, Let k be the observed pixel coordinates of the target in the image plane at time k. For the state The nonlinear observation equation, Measure noise for each pixel;

[0054] Step 2.4: Construct a laser sub-filter. The friendly UAV periodically transmits its calculated position and velocity back via the laser communication link. UAV A, combining its own position and velocity, calculates the observed values ​​of relative position and relative velocity. The corresponding observation equation is:

[0055] (4)

[0056] in, Let k be the laser communication observation vector at time k. For laser communication observation matrix, Noise measurement for laser communication;

[0057] The laser communication observation vector and laser communication observation matrix of the laser sub-filter are as follows:

[0058] (5)

[0059] (6)

[0060] in, and These represent the position and speed of machine A in the navigation system, respectively. and These represent the position and speed of friendly aircraft B under the navigation system, respectively. It is a 3×3 zero matrix. It is a 3×3 identity matrix.

[0061] As a specific example, each sub-filter described in step 3 operates independently, using the exponential moving maximum likelihood estimation (ESMLE) algorithm to identify the statistical characteristics of observation noise, adaptively adjusting the filter gain, and performing fault detection and isolation on the current observation data, as detailed below:

[0062] Step 3.1: Employ the maximum likelihood estimation method based on the innovation sequence to estimate the time-varying observation noise matrix in real time, as follows:

[0063] Step 3.1.1, Maximum Likelihood Criterion: In order to obtain... The estimated value is obtained by constructing a log-likelihood function, and the innovation at time k is assumed to follow a multivariate normal distribution. The log-likelihood of the new information is:

[0064] (7)

[0065] in, Let k be the observation noise matrix at time k. For time k, regarding The log-likelihood function, Let be the information covariance matrix at time k. Let k be the innovation vector at time k. Let k be the transpose of the innovation vector at time k. For constant terms;

[0066] To find the maximum likelihood solution, for Find the derivative by variation, For a symmetric positive definite matrix, use the matrix differential identity:

[0067] (8)

[0068] in, The sign for partial derivatives;

[0069] Get the The derivative:

[0070] (9)

[0071] The maximum likelihood estimation (MLE) conditions for the observation noise matrix are obtained by solving:

[0072] (10)

[0073] in To obtain the maximum likelihood estimate of the observation noise at time k, For the expected value of the information at time k, The system's observation matrix, Let k be the prior estimation error covariance matrix;

[0074] Step 3.1.2, Exponential Moving Average Strategy: Introduce an exponential moving average strategy to approximate the second moment of the innovation:

[0075] (11)

[0076] in, Forgetting factor, The larger the value, the more sensitive the system is to current information and the faster it responds, but the smoothness decreases; conversely, the stability increases. Let k be the exponential moving average estimate of the information covariance at time k. Let $\frac{k}{k-1}$ be the exponential moving average estimate of the new information covariance. The exponential moving average estimate of the expected value of the innovation at time k

[0077] According to the observed noise R in equation (11) k From the maximum likelihood estimation, the recursive update formula for the observation noise covariance matrix is ​​derived:

[0078] (12)

[0079] in, Let k be the exponential moving average estimate of the observation noise covariance matrix at time k;

[0080] Again, based on equation (12), and simplifying to take the direct recursive form, the final adaptive observation noise update formula is obtained as follows:

[0081] (13)

[0082] in, This is the exponential moving average estimate of the observation noise covariance matrix at time k-1;

[0083] Equation (13) retains the statistical rationality of the maximum likelihood estimation, while avoiding the risks of step size sensitivity, convergence difficulties, or loss of positive definiteness of the observation noise covariance matrix due to iteration errors that may occur in the traditional gradient descent method. It also has an intuitive physical meaning: when the observation noise suddenly increases, the innovation magnitude increases dramatically, driving the exponential moving average estimate of the observation noise covariance matrix to increase rapidly, thereby reducing the weight of the measurement data in the Kalman gain at that moment and suppressing the pollution of the state estimation by noise; conversely, when the environment is stable, the exponential moving average estimate of the observation noise covariance matrix gradually converges to the true noise level, ensuring the optimality of the estimation.

[0084] Step 3.2: Establish a fault detection mechanism based on chi-square test to eliminate the influence of outliers caused by target occlusion or false detection during visual servoing.

[0085] Construct the following chi-square test statistic:

[0086] (14)

[0087] in, Let k be the chi-square test statistic at time k;

[0088] Under the fault-free assumption It follows a chi-square distribution with m degrees of freedom. Set the threshold corresponding to the significance level. ,if This is then determined to be a measurement anomaly;

[0089] Step 3.3: Combining the ESMLE noise adaptive estimation and chi-square fault detection mechanisms from steps 3.1 and 3.2, an ESMLE filtering algorithm with fault isolation and noise adaptive capabilities is established, such as... Figure 4 As shown, the complete process is as follows:

[0090] Step 3.3.1: Time Update. Based on the posterior estimate at time k-1, predict the current state and covariance using the system state model from Step 2.1.

[0091] (15)

[0092] in, This is the prior estimate of the state at time k. Here is the state transition matrix. This is the posterior estimate of the state at time k-1. Let k be the prior estimation error covariance matrix. Let be the posterior estimation error covariance matrix at time k−1. Process noise covariance matrix;

[0093] Step 3.3.2: Innovation calculation and detection statistic construction. Utilize the sensor measurement values ​​at the current time. If no measurement values ​​are available, proceed directly to step 3.3.3. If measurement values ​​are available, calculate the innovation vector and its prediction covariance matrix. For now, use the noise covariance from the previous time step.

[0094] (16)

[0095] in, Let k be the innovation vector at time k. Let k be the sensor observation value at time k. Let k be the prediction covariance matrix of the innovation vector at time k. This is the exponential moving average estimate of the observation noise covariance matrix at time k-1.

[0096] Construct the chi-square test statistic:

[0097] (17)

[0098] Step 3.3.3, Fault Judgment and Branch Processing: Based on the preset fault detection threshold... A binary decision is made regarding the measurement quality:

[0099] Scenario A: If Then determine the current observation This is an outlier; to prevent filter divergence, this measurement is isolated, and the predicted value is directly output as the posterior estimate, while maintaining the noise statistical characteristics unchanged:

[0100] (18)

[0101] in, This is the posterior estimate of the state at time k; Let be the posterior estimation error covariance matrix at time k; Let k be the exponential moving average estimate of the observation noise covariance matrix at time k;

[0102] Scenario B: If If the current observation is deemed valid, ESMLE noise adaptive update is first performed to calculate the observation noise covariance at the current time. :

[0103] (19)

[0104] in, Forgetting factor;

[0105] Based on the updated Recalculate the new information covariance With Kalman gain :

[0106] (20)

[0107] in, The Kalman gain at time k;

[0108] Finally, the measurements and updates of the state and covariance are completed:

[0109] (twenty one)

[0110] in, Let be the posterior estimation error covariance matrix at time k. It is the identity matrix;

[0111] Step 3.3.4: Calculate the posterior estimate at time k. , and adaptive noise matrix As prior information for the next moment, the next round of filtering continues.

[0112] As a specific example, step 4 describes constructing a federated Kalman filter, fusing visual and laser sub-filters, and outputting the optimal relative position estimate, as follows:

[0113] Step 4.1: Visual observation and laser communication data differ significantly in sampling frequency, accuracy characteristics, and environmental sensitivity. To ensure high system fault tolerance while achieving optimal state estimation, information fusion is performed using a federated Kalman filter, such as...Figure 5 As shown, each sub-filter first uses the ESMLE filtering algorithm to block the upload of abnormal data through hard threshold logic;

[0114] Step 4.2: Using Mahalanobis distance, the estimation quality of the sub-filter is measured by the difference in estimates between the sub-filter and the main filter, thereby adaptively adjusting the corresponding information factor weights.

[0115] The information allocation factor after introducing Mahalanobis distance is:

[0116] (twenty two)

[0117] in, Let be the Mahalanobis distance between the estimated values ​​of the i-th sub-filter and the main filter (global filter) at time k. The global state estimate of the master filter (global filter) at time k. Let be the local state estimate of the i-th sub-filter at time k. The reciprocal of the covariance matrix used to calculate the Mahalanobis distance. is the adaptive information allocation factor for the i-th sub-filter, and n is the number of effective sub-filters participating in the fusion.

[0118] This invention also provides a UAV cooperative tracking and laser communication control system based on multi-source information fusion. This system is used to implement the aforementioned adaptive federated Kalman filter UAV swarm laser communication method based on exponential moving maximum likelihood estimation. The system includes a first module to a fourth module, the functions of which are as follows:

[0119] The first module is used to control the airborne optoelectronic pod to acquire visual images of the target UAV and calculate its pixel coordinates in the image plane; at the same time, it receives navigation status information, including position and speed, from the target UAV via a laser communication link; and combines the onboard navigation information to construct visual nonlinear observation vectors and laser linear observation vectors respectively.

[0120] The second module constructs a visual sub-filter and a laser sub-filter, and inputs the observation data obtained in the first module into the corresponding sub-filters respectively; each sub-filter uses the exponential sliding maximum likelihood estimation (ESMLE) algorithm to recursively update the observation noise covariance matrix online, and uses the updated noise parameters to perform Kalman filtering measurement updates, outputting local state estimates and covariance.

[0121] The third module constructs a federated Kalman filter as the main filter. The main filter calculates the Mahalanobis distance between the local estimates and global predictions of each effective sub-filter. Based on the magnitude of the Mahalanobis distance, an adaptive information allocation factor is generated. The information of each sub-filter is weighted and fused to output the global optimal relative state estimate. Using the adaptive information allocation factor, the main filter feeds back the fused global optimal state estimate to all sub-filters for state reset.

[0122] The fourth module, the photoelectric pointing control module, uses the globally optimal relative position to calculate the desired pitch and azimuth angles of the photoelectric pod, drives the servo mechanism to perform line-of-sight pointing control, and achieves precise tracking of the target UAV and maintenance of the laser communication link.

[0123] The present invention also provides a mobile terminal, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, it implements the aforementioned adaptive federated Kalman filter UAV swarm laser communication method based on exponential sliding maximum likelihood estimation.

[0124] The present invention also provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps in the adaptive federated Kalman filter UAV swarm laser communication method based on exponential moving maximum likelihood estimation.

[0125] The present invention also provides a computer program product, including computer instructions for causing a computer to execute the aforementioned adaptive federated Kalman filter UAV swarm laser communication method based on exponential moving maximum likelihood estimation.

[0126] Example 1

[0127] The experimental data in this embodiment was collected in the airspace near a university. The experimental platform was a self-developed quadcopter UAV, integrating a BeiDou navigation and positioning system, an IMU (Inertial Measurement Unit), and a data recording module. To simulate the harsh conditions of cooperative tracking in a real environment, this flight experiment was designed with complex operating conditions that included multiple challenges. First, there was a long-endurance cumulative error test, with data acquisition lasting approximately 15 minutes, designed to verify the algorithm's ability to suppress sensor zero-bias drift and cumulative errors. Second, there was a high-dynamic maneuver test, during which the UAV performed frequent large-amplitude eastward turn-around maneuvers between 200 and 400 seconds of flight time to test the filter's dynamic tracking performance in the face of rapid state changes. In addition, the experiment included an environmental disturbance test. At a flight altitude of approximately 65 meters, the UAV encountered strong gusts of wind during the mission execution phase, resulting in significant uncommanded disturbances in the aircraft's attitude, simulating a flight environment under adverse weather conditions.

[0128] The experiment used the post-processing results of an airborne high-precision RTK / INS integrated navigation system as the true state values. The reference position and velocity curves are shown below. Figure 6 (a) and Figure 6 As shown in (b) above, the motion characteristics of the UAV at different flight phases are clearly reflected. Figure 7 As shown, the collected IMU acceleration is as follows: Figure 7 (a) Angular velocity data Figure 7 The data in (b) and GPS location observation data were used as system inputs. The standard KF algorithm and the ESMLE adaptive algorithm were run respectively to evaluate the trajectory tracking effect and error statistics.

[0129] To verify the robustness of the algorithm under sensor failure or strong electromagnetic interference, the experimenter artificially superimposed white noise interference with a standard deviation of 0.5m in each direction in the 600s to 800s interval of the original BeiDou navigation observation data, simulating a failure condition in which the sensor accuracy drops sharply. Figure 8 As shown.

[0130] Figure 9 The dynamic response characteristics of the state estimation errors of the two filtering algorithms during this period are shown. Figure 9 (a) in the figure is a comparison chart of positional errors. Figure 9 Figure (b) shows a comparison of velocity errors. In the initial stage of noise injection, the standard Kalman filter exhibits significant hysteresis and vulnerability. This is due to its gain matrix K... k The system heavily relies on pre-set fixed noise parameters and cannot detect sudden changes in the quality of observed data in real time. As a result, the filter still accepts contaminated measurement information with high confidence. This defect directly leads to serious deviations in state estimation, with position and velocity errors diverging rapidly. The peak position error once exceeded 2.3m, seriously threatening the tracking stability of the system.

[0131] In contrast, the ESMLE adaptive filtering algorithm proposed in this invention demonstrates a keen noise perception and parameter adjustment capability when dealing with sudden interference. For example... Figure 10 As shown, the observation noise covariance matrix R k The trace value shows a significant step increase at 600s, intuitively reflecting the algorithm's rapid capture of anomalous amplitudes in the innovation sequence. From a filtering mechanism perspective, as R... k The adaptive increase, according to the Kalman gain calculation formula, the gain matrix K kThe automatic convergence is reduced. That is, when the observation quality deteriorates, the filter's dependence on current anomalous observation data is actively reduced, and state recursion is instead made more reliant on the time updates of the system dynamics model. This dynamic trade-off strategy effectively shields against strong noise interference, ensuring the smoothness and convergence of the state estimation results under harsh operating conditions.

[0132] Table 1 summarizes the error data during periods of noise interference. Experimental results show that in areas with strong noise, the root mean square error of the ESMLE algorithm for position estimation decreased by 43.7%, the standard deviation decreased by 42.1%, and the maximum error decreased by 34.7%. This fully verifies that the proposed ESMLE algorithm has excellent robustness in dealing with sudden sensor failures and environmental interference.

[0133] Table 1. Comparison of error statistics between standard KF and ESMLE adaptive filters in the noisy section.

[0134]

[0135] Example 2

[0136] This embodiment aims to examine whether the federated filter can provide stable line-of-sight pointing commands for the servo control of the optoelectronic pod by maintaining the continuity and reliability of relative position estimation when visual observation is interrupted due to occlusion or laser link data is lost.

[0137] The experimental data collection site was located near Gate 6 of a university. A high-definition camera was fixed at the ground takeoff point, serving as the origin of the global coordinate system, to track a UAV performing complex maneuvers in real time. To facilitate the calculation of the relative position between the camera and the UAV, the camera was installed at the same location as the UAV's takeoff point, with due south as the positive direction of the camera coordinate system. The camera's pitch angle was 16.604 degrees, yaw angle was 156.775 degrees, image resolution was 3840*2160, and frame rate was 30fps. In addition, the experiment simultaneously recorded the high-precision navigation status of the target UAV, including position, velocity, and acceleration, transmitted back via a laser communication link, at a sampling frequency of 10Hz. Laser data, as a highly reliable external measurement source, was intended to provide necessary state correction information when visual observation failed.

[0138] The experiment lasted approximately 6 minutes. The system constructed two independent ESMLE sub-filters using visual observation information and laser reception information respectively, and then fused them using a federated filter. To verify the algorithm's fault-tolerant performance, the filtering performance during periods of missing sensor data was analyzed in detail.

[0139] Figure 11Figures (a) to (c) show the relative position estimation error curves of the federated master filter and each sub-filter in the east direction (E), north direction (N), and ground direction (D). By analyzing these curves, we can conclude that:

[0140] (1) Shortcomings of a single sub-filter: From Figure 11 It can be clearly observed that when the visual target is lost (in the gray shaded area) or the laser data is interrupted (in the yellow shaded area), the state estimation error of a single sub-filter diverges rapidly. During the period of visual loss, the position error peak of the visual sub-filter, which relies solely on inertial navigation calculations, once exceeded 16m in the eastward direction, completely losing its tracking capability.

[0141] (2) Advantages of the federated filter: In contrast, the federated filter, i.e., the red line, maintained extremely high estimation accuracy and stability throughout the experiment. This is due to its fusion mechanism: when the visual sub-filter is isolated due to abnormal chi-square detection or no data upload, the main filter uses the high-precision distance information of the laser link to correct the optimal estimate of the friendly UAV's position; and vice versa. Experimental results show that the output curve of the federated filter is smooth and continuous, without any jumps or divergence.

[0142] The above are merely preferred embodiments of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. An adaptive federated Kalman filter-based laser communication method for UAV swarms based on exponential moving maximum likelihood estimation, characterized in that, Includes the following steps: Step 1: Drone A searches for friendly Drone B in a wide field of view using its onboard vision system and uses an embedded visual target recognition algorithm to determine the pixel coordinates of Drone B in the image plane. Step 2: Construct a cooperative tracking model and acquire multi-source observation data. Establish a relative motion state space model for the UAV. Use the visual pixel coordinates of the target obtained in Step 1 to construct a visual observation equation. Use the laser communication link to acquire the navigation state information of the target and construct a laser linear observation equation. Step 3: Each sub-filter operates independently, using the exponential sliding maximum likelihood estimation (ESMLE) algorithm to identify the statistical characteristics of the observation noise, adaptively adjusting the filter gain, and performing fault detection and isolation on the current observation data. Step 4: Construct a federated Kalman filter, fuse visual and laser sub-filters, and output the optimal relative position estimate; Step 5: Calculate the desired angle of the optoelectronic pod using the globally optimal relative position, and drive the servo mechanism to achieve precise target tracking and laser communication link maintenance.

2. The adaptive federated Kalman filter UAV swarm laser communication method based on exponential moving maximum likelihood estimation according to claim 1, characterized in that, Step 2 is described in detail below: Step 2.1: Define the navigation coordinate system, body coordinate system, camera coordinate system, and pixel coordinate system; (1) Navigation coordinate system, i.e., n-system: The North-East-Earth coordinate system is selected as the inertial reference system, with the origin O. n Set at the ground takeoff point, coordinate axis , , They point to due north, due east, and the Earth's center, respectively. (2) Body coordinate system, i.e. b system: the origin of the body coordinate system Located at the body's center of mass, The shaft along the machine head points forward. The axis points to the right side of the fuselage. The axis is perpendicular to the fuselage and points downwards; the attitude of the body coordinate system relative to the navigation coordinate system is determined by Euler angles, namely roll angle φ, pitch angle θ, and yaw angle ψ, and the corresponding rotation matrix is ​​denoted as... ; (3) Camera coordinate system, i.e., c-frame: the origin of the camera coordinate system Located at the optical center of the photoelectric pod camera, The axis points forward along the optical axis, i.e., in the direction of the line of sight. The axis is parallel to the horizontal direction of the imaging plane. The axis is parallel to the perpendicular direction of the imaging plane; since the installation error of the camera coordinate system relative to the center of the body coordinate system is extremely small, the installation error is negligible; the gimbal rotation of the camera system relative to the body system is determined by the rotation matrix. describe; (4) Pixel coordinate system, i.e. two-dimensional coordinate system: The image coordinate system takes the center O′ of the imaging plane as the origin, the x-axis points to the right in the imaging plane, and the y-axis points to the top in the imaging plane; the pixel coordinate system takes the upper left corner of the image as the origin (u,v), and the unit is pixels; the two are transformed by a scaling and translation of the origin. The point P=(X,Y,Z) in three-dimensional space is transformed to the pixel coordinate system through the camera's intrinsic parameter matrix; Step 2.2: Based on the coordinate system established in Step 2.1, select relative position, relative velocity, and relative acceleration as the system state vectors: （1） in, For the filter in State estimate at time 10:00 For friendly drones The relative position vector at time t, For friendly drones The relative velocity vector at time t, Friendly drones The relative acceleration vector at time t; Considering that the maneuvers of UAVs in a short period of time are usually smooth, a constant acceleration CA model is used to describe the relative motion dynamics, and the discrete-time state-space equations are established as follows: （2） in, Here is the state transition matrix. This is process noise; For the filter in State estimate at time -1; Step 2.3: Construct a visual sub-filter. The local electro-optical pod outputs images of the friendly UAV in real time. The pixel coordinates of the friendly UAV on the image plane are extracted using a target detection algorithm. Based on the pinhole imaging principle and coordinate transformation relationship, the observation equation is modeled as follows: （3） in, Let k be the observed pixel coordinates of the target in the image plane at time k. For the state The nonlinear observation equation, Measure noise for each pixel; Step 2.4: Construct a laser sub-filter. The friendly UAV periodically transmits its calculated position and velocity back via the laser communication link. UAV A, combining its own position and velocity, calculates the observed values ​​of relative position and relative velocity. The corresponding observation equation is: （4） in, Let k be the laser communication observation vector at time k. For laser communication observation matrix, Noise measurement for laser communication; The laser communication observation vector and laser communication observation matrix of the laser sub-filter are as follows: （5） （6） in, and These represent the position and speed of machine A in the navigation system, respectively. and These represent the position and speed of friendly aircraft B under the navigation system, respectively. It is a 3×3 zero matrix. It is a 3×3 identity matrix.

3. The adaptive federated Kalman filter UAV swarm laser communication method based on exponential moving maximum likelihood estimation according to claim 2, characterized in that, Step 3 is as follows: Step 3.1: Employ the maximum likelihood estimation method based on the innovation sequence to estimate the time-varying observation noise matrix in real time, as follows: Step 3.1.1, Maximum Likelihood Criterion: In order to obtain... The estimated value is obtained by constructing a log-likelihood function, and the innovation at time k is assumed to follow a multivariate normal distribution. The log-likelihood of the new information is: （7） in, Let k be the observation noise matrix at time k. For time k, regarding The log-likelihood function, Let be the information covariance matrix at time k. Let k be the innovation vector at time k. Let k be the transpose of the innovation vector at time k. For constant terms; To find the maximum likelihood solution, for Find the derivative by variation, For a symmetric positive definite matrix, use the matrix differential identity: （8） in, The sign for partial derivatives; Get the The derivative: （9） The maximum likelihood estimation (MLE) conditions for the observation noise matrix are obtained by solving: （10） in To observe the maximum likelihood estimate of the noise at time k, For the expected value of the information at time k, The system's observation matrix, Let k be the prior estimation error covariance matrix; Step 3.1.2, Exponential Moving Average Strategy: Introduce an exponential moving average strategy to approximate the second moment of the innovation: （11） in, Forgetting factor, The larger the value, the more sensitive the system is to current information and the faster it responds, but the smoothness decreases; conversely, the stability increases. Let k be the exponential moving average estimate of the information covariance at time k. Let $\frac{k}{k-1}$ be the exponential moving average estimate of the new information covariance. The exponential moving average estimate of the expected value of the innovation at time k According to the observed noise R in equation (11) k From the maximum likelihood estimation, the recursive update formula for the observation noise covariance matrix is ​​derived: （12） in, Let k be the exponential moving average estimate of the observation noise covariance matrix at time k; Again, based on equation (12), and simplifying to take the direct recursive form, the final adaptive observation noise update formula is obtained as follows: （13） in, This is the exponential moving average estimate of the observation noise covariance matrix at time k-1; Step 3.2: Establish a fault detection mechanism based on chi-square test to eliminate the influence of outliers caused by target occlusion or false detection during visual servoing. Construct the following chi-square test statistic: （14） in, Let be the chi-square test statistic at time k; Under the fault-free assumption It follows a chi-square distribution with m degrees of freedom. Set the threshold corresponding to the significance level. ,if This is then determined to be a measurement anomaly; Step 3.3: Combining the ESMLE noise adaptive estimation and chi-square fault detection mechanism from steps 3.1 and 3.2, an ESMLE filtering algorithm with fault isolation and noise adaptive capabilities is established. The complete process is as follows: Step 3.3.1: Time Update. Based on the posterior estimate at time k-1, predict the current state and covariance using the system state model from Step 2.

1. （15） in, This is the prior estimate of the state at time k. Here is the state transition matrix. This is the posterior estimate of the state at time k-1. Let k be the prior estimation error covariance matrix. Let be the posterior estimation error covariance matrix at time k−1. Process noise covariance matrix; Step 3.3.2: Innovation calculation and detection statistic construction. Utilize the sensor measurement values ​​at the current time. If no measurement values ​​are available, proceed directly to step 3.3.

3. If measurement values ​​are available, calculate the innovation vector and its prediction covariance matrix. For now, use the noise covariance from the previous time step. （16） in, Let k be the innovation vector at time k. Let k be the sensor observation value at time k. Let K be the prediction covariance matrix of the innovation vector at time k. This is the exponential moving average estimate of the observation noise covariance matrix at time k-1; Construct the chi-square test statistic: （17） Step 3.3.3, Fault Judgment and Branch Processing: Based on the preset fault detection threshold... A binary decision is made regarding the measurement quality: Scenario A: If Then determine the current observation This is an outlier; to prevent filter divergence, this measurement is isolated, and the predicted value is directly output as the posterior estimate, while maintaining the noise statistical characteristics unchanged: （18） in, This is the posterior estimate of the state at time k; Let be the posterior estimation error covariance matrix at time k; Let k be the exponential moving average estimate of the observation noise covariance matrix at time k; Scenario B: If If the current observation is deemed valid, ESMLE noise adaptive update is first performed to calculate the observation noise covariance at the current time. : （19） in, Forgetting factor; Based on the updated Recalculate the new information covariance With Kalman gain : （20） in, The Kalman gain at time k; Finally, the measurements and updates of the state and covariance are completed: （21） in, Let be the posterior estimation error covariance matrix at time k. It is the identity matrix; Step 3.3.4: Calculate the posterior estimate at time k. , and adaptive noise matrix As prior information for the next moment, the next round of filtering continues.

4. The adaptive federated Kalman filter UAV swarm laser communication method based on exponential moving maximum likelihood estimation according to claim 3, characterized in that, Step 4 is as follows: Step 4.1: Visual observation and laser communication data are fused through a federated Kalman filter. Each sub-filter first uses the ESMLE filtering algorithm to block the upload of abnormal data through hard threshold logic. Step 4.2: Using Mahalanobis distance, the estimation quality of the sub-filter is measured by the difference in estimates between the sub-filter and the main filter, thereby adaptively adjusting the corresponding information factor weights. The information allocation factor after introducing Mahalanobis distance is: （22） in, Let be the Mahalanobis distance between the estimates of the i-th sub-filter and the main filter at time k. The global state estimate of the master filter at time k. Let be the local state estimate of the i-th sub-filter at time k. The reciprocal of the covariance matrix used to calculate the Mahalanobis distance. is the adaptive information allocation factor for the i-th sub-filter, and n is the number of effective sub-filters participating in the fusion.

5. An adaptive federated Kalman filter-based UAV swarm laser communication system based on exponential moving maximum likelihood estimation, characterized in that, This system is used to implement the adaptive federated Kalman filter UAV swarm laser communication method according to any one of claims 1 to 4. The system includes a first module to a fourth module, wherein the functions of each module are as follows: The first module is used to control the airborne optoelectronic pod to acquire visual images of the target UAV and calculate its pixel coordinates in the image plane; at the same time, it receives navigation status information, including position and speed, from the target UAV via a laser communication link; and combines the onboard navigation information to construct visual nonlinear observation vectors and laser linear observation vectors respectively. The second module constructs a visual sub-filter and a laser sub-filter, and inputs the observation data obtained in the first module into the corresponding sub-filters respectively; each sub-filter uses the exponential sliding maximum likelihood estimation (ESMLE) algorithm to recursively update the observation noise covariance matrix online, and uses the updated noise parameters to perform Kalman filtering measurement updates, outputting local state estimates and covariance. The third module constructs a federated Kalman filter as the main filter. The main filter calculates the Mahalanobis distance between the local estimates and global predictions of each effective sub-filter. Based on the magnitude of the Mahalanobis distance, an adaptive information allocation factor is generated. The information of each sub-filter is weighted and fused to output the global optimal relative state estimate. Using the adaptive information allocation factor, the main filter feeds back the fused global optimal state estimate to all sub-filters for state reset. The fourth module, the photoelectric pointing control module, uses the globally optimal relative position to calculate the desired pitch and azimuth angles of the photoelectric pod, drives the servo mechanism to perform line-of-sight pointing control, and achieves precise tracking of the target UAV and maintenance of the laser communication link.

6. A mobile terminal, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the adaptive federated Kalman filter UAV swarm laser communication method based on exponential sliding maximum likelihood estimation as described in any one of claims 1 to 4.

7. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the steps in the adaptive federated Kalman filter UAV swarm laser communication method based on exponential sliding maximum likelihood estimation as described in any one of claims 1 to 4.

8. A computer program product, characterized in that, Includes computer instructions for causing a computer to execute the adaptive federated Kalman filter UAV swarm laser communication method based on exponential moving maximum likelihood estimation as described in any one of claims 1 to 4.

Images