A hovering method and system for firefighting drones based on laser guidance and trusted fusion

Abstract

This invention discloses a hovering method and system for firefighting drones based on laser guidance and reliable fusion, relating to the field of drone hovering control technology. By deploying laser guidance devices in the high-altitude firefighting operation area, a stable spatial guidance reference is established to guide the drone into the target hovering area and continuously receive coded laser beams. Based on this, laser reception features are extracted and a link reliability factor is calculated to assess the reliability of laser deviation observation information and suppress anomalies. Simultaneously, the laser deviation observations are aligned with the attitude, velocity, and altitude information output by the inertial navigation unit using a time reference and coordinate consistency correction to form a fused data set. Through adaptive reliable fusion and consistency constraints, a hovering error vector for hovering control is constructed, driving the flight control unit to dynamically adjust the drone's hovering position, thereby improving the stability and reliability of drone hovering positioning during high-altitude firefighting operations.

Description

Technical Field

[0001] This invention relates to the field of drone hovering control technology, specifically to a hovering method and system for firefighting drones based on laser guidance and trusted fusion. Background Technology

[0002] During high-altitude firefighting operations, drones need to maintain stable hovering for extended periods in high-altitude, strong airflow, and smoke-dust-affected environments to ensure that the firefighting equipment accurately targets the area.

[0003] Existing drone hovering and positioning technologies largely rely on inertial navigation systems, satellite positioning systems, or visual feature-based positioning methods. Inertial navigation systems are prone to accumulating errors during prolonged hovering; satellite positioning is susceptible to obstruction or signal instability in complex high-altitude environments; and visual positioning methods are highly dependent on ambient lighting, smoke density, and scene characteristics, making it difficult to maintain continuous and stable operation at fire scenes. Furthermore, while some existing technologies incorporate external guidance or auxiliary positioning methods, in practical applications they often lack dynamic evaluation mechanisms for the reliability of guidance links, making it difficult to promptly identify and suppress abnormal observation information.

[0004] Meanwhile, in terms of multi-source information fusion, existing hovering control methods typically employ fixed weights or simple filtering to fuse information from different sensors. This fails to adequately consider the varying degrees of reliability of different information sources in complex environments, making the fusion results susceptible to abnormal data. Consequently, this can lead to unstable hovering positioning or even control failure. Therefore, it is necessary to propose a UAV hovering positioning method and system that can introduce an external stable guidance benchmark and combine link reliability assessment with reliable multi-source information fusion. Summary of the Invention

[0005] This invention addresses the shortcomings of existing technologies by proposing a hovering method and system for firefighting drones based on laser guidance and trusted fusion.

[0006] The technical solution to achieve the objective of this invention is as follows:

[0007] On the one hand, the hovering method for firefighting drones based on laser guidance and trusted fusion includes the following steps:

[0008] The laser guidance device is controlled to emit coded laser beams to form a spatial guidance reference. The UAV is then controlled to enter the target hovering area to continuously receive the coded laser beams, while simultaneously acquiring the UAV's attitude, speed, and altitude information.

[0009] The encoded laser beam is demodulated, its modulation frequency characteristics are analyzed and laser reception characteristics are extracted. Based on the spatial position change of the laser beam in the airborne sensing coordinate system, a lateral deviation observation consisting of lateral offset and offset direction is constructed.

[0010] Based on extended Kalman filtering, the lateral deviation observation is fused with UAV attitude, velocity and altitude information to output a hovering error vector.

[0011] The hovering error vector is mapped and calculated based on the preset hovering control law to generate hovering correction control quantity to drive the UAV to perform hovering position adjustment.

[0012] Furthermore, after extracting the laser receiving features, the calculation of the link reliability factor is also included, including:

[0013] The laser receiving characteristics are obtained and the calculation is performed to obtain the laser receiving characteristics, including laser receiving intensity, receiving continuity and receiving stability. Among them, laser receiving intensity is used to characterize the received amplitude level of the coded laser signal at the current spatial location, receiving continuity is used to characterize the continuous receiving state of the laser signal in the time series, and receiving stability is used to characterize the degree of fluctuation of laser receiving intensity in the continuous time series.

[0014] The laser receiving intensity, receiving continuity, and receiving stability are normalized and fused to form a link reliability factor that characterizes the reliability of the current laser guidance link.

[0015] Furthermore, the construction of lateral bias observations includes:

[0016] Based on the characteristics of laser reception, the spatial position change of laser in the airborne sensing coordinate system is analyzed to determine the reference position and real-time reception position of laser in the airborne sensing coordinate system.

[0017] Based on the relative spatial relationship between the reference position and the real-time receiving position, the lateral offset of the UAV relative to the spatial guidance reference is calculated. At the same time, the corresponding offset direction is determined according to the directional distribution of the lateral offset in the lateral plane of the airborne sensing coordinate system. The lateral offset and the offset direction together constitute the lateral deviation observation that characterizes the lateral deviation state of the UAV.

[0018] Furthermore, it also includes performing credibility modeling processing on lateral deviation observations based on link credibility factors to generate observation credibility factors, including:

[0019] Based on the link reliability factor, the lateral deviation observations are subjected to reliability weighting to reflect the impact of the laser guidance link status on the reliability of lateral deviation observations;

[0020] Based on the completed confidence weighting process, anomaly suppression processing is performed by combining the changes of lateral bias observations in continuous time series to generate an observation confidence factor that characterizes the reliability of the current lateral bias observation.

[0021] Furthermore, prior to multi-source information fusion processing, time reference alignment and coordinate consistency correction of multi-source information are also included, including:

[0022] Timestamps are assigned to lateral deviation observations, attitude information, velocity information, and altitude information respectively to unify the time base, and alignment processing is performed on the timestamps of each information source.

[0023] Based on attitude information, an attitude transformation matrix is ​​constructed from the airborne sensing coordinate system to the unified reference coordinate system. Coordinate consistency correction is performed on the lateral deviation observation to transform it from the airborne sensing coordinate system to the unified reference coordinate system, thereby obtaining the deviation candidate quantity.

[0024] The motion compatibility of the candidate deviation quantities is checked by combining the velocity information, the rate of change of the candidate deviation quantities is calculated, and the compatibility of the candidate deviation quantities with the lateral velocity output by the inertial navigation unit is judged. The hovering horizontal difference is then selected as the laser deviation observation input for the extended Kalman filter.

[0025] Furthermore, the multi-source information fusion processing based on extended Kalman filtering includes:

[0026] The observation noise covariance matrix of the extended Kalman filter is dynamically adjusted based on the observation reliability factor, and the system noise covariance matrix is ​​dynamically adjusted based on the link reliability factor to achieve adaptive reliable fusion.

[0027] Using the hovering horizontal difference as the laser deviation observation input, attitude information and velocity information as state prediction auxiliary quantities, and height information as the vertical observation input, the hovering error vector containing the lateral horizontal error component and the vertical height error component is output through prediction and update iterative loop.

[0028] Further processing of high-level information includes:

[0029] Based on altitude information and a preset hovering altitude benchmark, calculate the altitude deviation of the UAV relative to the target hovering altitude in the current hovering state;

[0030] Mean filtering anomaly suppression processing is performed on the height deviation to remove instantaneous impulse noise that occurs during the height measurement process. Height deviations that exceed the preset height error anomaly threshold are discarded and re-acquired to obtain the hovering height error after anomaly suppression processing. This is used as the vertical observation input for the extended Kalman filter and participates in the multi-source fusion calculation of the hovering error vector together with the lateral deviation observation.

[0031] Furthermore, the generation of hover correction control values ​​includes:

[0032] Based on the preset hovering control law, proportional-integral-derivative control law mapping calculations are performed on the lateral horizontal error component and the vertical height error component in the hovering error vector to generate lateral hovering correction and vertical hovering correction.

[0033] Amplitude limiting processing is performed on the lateral hovering correction amount and the vertical hovering correction amount respectively, and the correction amount exceeding the preset correction amount amplitude threshold is clipped to the maximum value of the threshold.

[0034] The lateral hovering correction value, after amplitude limitation, is combined with the vertical hovering correction value to form the hovering correction control value, which is input into the flight control unit and integrated with the attitude control signal and altitude hold control signal to drive the UAV power system to perform hovering position adjustment.

[0035] Furthermore, the extended Kalman filter parameters and hovering correction control variables are continuously and dynamically updated throughout the entire hovering operation of the UAV, including:

[0036] During the drone hovering process, the observation noise covariance matrix and system noise covariance matrix of the extended Kalman filter are dynamically updated based on the link confidence factor and observation confidence factor calculated in real time. When the link confidence factor is lower than the confidence threshold, the covariance matrix parameters of the previous frame are used.

[0037] Based on the updated filtering parameters, the multi-source information fusion process is re-executed to generate a new hovering error vector. The hovering correction control quantity is then re-mapped, calculated, and updated to drive the UAV to continuously adjust its hovering position.

[0038] Secondly, a hovering system for firefighting drones based on laser guidance and trusted fusion includes an coded laser emitting module, a laser receiving module, an inertial navigation unit, an EKF fusion processing unit, and a flight control unit.

[0039] The coded laser emission module controls the laser guidance device to emit a coded laser beam toward the target hovering area, and the coded laser beam is generated by frequency modulation or phase modulation.

[0040] The laser receiving module acquires the coded laser beam received by the two-dimensional photodetector array structure and performs demodulation processing, extracts the modulation frequency features and laser receiving features, and constructs the lateral deviation observation based on the spatial position change of the laser beam. The two-dimensional photodetector array structure is deployed on the UAV body.

[0041] The inertial navigation unit collects the attitude and velocity information of the UAV, and simultaneously controls the altitude measuring device to determine the altitude information;

[0042] The EKF fusion processing unit is based on extended Kalman filtering. It adjusts the observation noise covariance matrix and the system noise covariance matrix with the link confidence factor and the observation confidence factor, and performs multi-source information fusion on the lateral deviation observation, attitude information, velocity information and altitude information to output the hovering error vector.

[0043] The flight control unit uses the hovering control law to map the hovering error vector into a hovering correction control quantity and drives the UAV to adjust its hovering position.

[0044] Compared with the prior art, the significant advantages of this invention are:

[0045] 1. A space guidance benchmark is constructed by laser guidance, and link credibility factor and observation credibility factor are introduced to dynamically quantify and evaluate the reliability of laser guidance link and the effectiveness of deviation observation, so as to suppress and screen unreliable observations and improve the reliability of hovering positioning information from the source.

[0046] 2. By integrating laser deviation observation with inertial navigation, speed, and altitude information through time alignment and consistency constraints, a hovering error vector is formed and used for hovering control. This enables the hovering positioning process to adapt to environmental changes and takes into account the coordinated control of horizontal and vertical errors, significantly enhancing the stability and robustness of UAV hovering. Attached Figure Description

[0047] Figure 1 A flowchart of a hovering method for firefighting drones based on laser guidance and trusted fusion;

[0048] Figure 2 This is a flowchart of the laser guidance and link reliability assessment process in this invention;

[0049] Figure 3 This is a schematic diagram of the laser guidance device layout and the establishment of the space guidance reference in this invention;

[0050] Figure 4 This is a flowchart of the multi-source information alignment and hover level difference filtering process in this invention. Detailed Implementation

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

[0052] Example 1

[0053] like Figure 1 As shown, this invention discloses a hovering method for firefighting drones based on laser guidance and trusted fusion, comprising the following steps:

[0054] S1: Deploy a laser guidance device in the high-altitude firefighting operation area to emit coded laser beams along a preset spatial direction and form a spatial guidance reference; control the UAV to enter the target hovering area, so that the airborne laser sensing device continuously receives the coded laser beams, extracts the corresponding laser reception characteristics, and calculates the link reliability factor based on the laser reception intensity, continuity and stability parameters.

[0055] S2: Based on the laser receiving characteristics, the spatial position change of the encoded laser beam in the airborne sensing coordinate system is analyzed to obtain the lateral offset and corresponding offset direction of the UAV relative to the spatial guidance reference. The lateral offset and offset direction constitute the lateral deviation observation. Based on the link credibility factor, the lateral deviation observation is subjected to credibility weighting and anomaly suppression processing to generate the observation credibility factor corresponding to the current guidance state.

[0056] S3: During the hovering process of the UAV, the attitude and velocity information output by the inertial navigation unit are acquired simultaneously, as well as the altitude information output by the altitude measurement device; the attitude information, velocity information, altitude information, and lateral deviation observation are assigned timestamps and time reference alignment processing is performed to form a fused data group; based on the fused data group, the coordinate consistency correction of the lateral deviation observation is performed based on the attitude information to obtain the deviation candidate quantity, and the motion compatibility verification of the deviation candidate quantity is performed in combination with the velocity information to select the hovering horizontal difference that passes the verification.

[0057] S4: Based on the observation confidence factor, the hovering level difference is subjected to extended Kalman filtering (EKF) adaptive confidence fusion processing to obtain the confidence level error. At the same time, the hovering height error is calculated based on the altitude information as the vertical observation input of the extended Kalman filter. The confidence level error and the hovering height error are integrated by consistency constraint according to the link confidence factor to form a hovering error vector for hovering control.

[0058] S5: The hovering error vector is mapped and calculated according to the preset hovering control law to obtain the lateral hovering correction and the vertical hovering correction. The lateral hovering correction and the vertical hovering correction are combined to form the hovering correction control. The hovering correction control is input into the flight control unit and works in conjunction with attitude control and altitude hold control to drive the UAV to perform hovering position adjustment. During hovering, the extended Kalman filter parameters and the hovering correction control are dynamically updated based on the link confidence factor and the observation confidence factor.

[0059] like Figure 2 As shown, in step S1, a spatial reference is established by deploying a laser guidance device, and the UAV is controlled to receive laser signals to extract features and calculate the link reliability factor, including:

[0060] S101: Deployment and calibration of laser guidance devices.

[0061] Based on the scope of the high-altitude firefighting operation area, the altitude of the target hovering area, and the distribution of surrounding obstacles, determine the deployment points, height, and emission angle of the laser guidance device. The deployment points are selected to be unobstructed, free from strong electromagnetic interference, and fully cover the target hovering area. The deployment height is not lower than the lowest altitude of the target hovering area, and the emission angle is calibrated to the preset direction to ensure that the coded laser beam covers all preset hovering points in the target hovering area.

[0062] A pulsed laser guidance device is installed at the deployment point. The laser guidance device uses a pulsed laser emitter, and the wavelength of the emitted coded laser beam is selected in the range of 532nm~1064nm. The laser pulse frequency is set to 10kHz~50kHz, and the laser beam divergence angle is controlled within 0.1mrad~0.5mrad. The laser output power is dynamically adjusted according to the target hovering distance to ensure that the airborne laser sensing device can stably receive the coded laser beam within the target hovering area. The coded laser beam is generated by frequency modulation or phase modulation. The modulation frequency is uniquely set within the preset frequency range, so that the coded laser beam has an identifiable frequency mark to distinguish it from natural light interference and smoke scattering light.

[0063] The laser guidance device undergoes spatial reference calibration. A high-precision total station is used to measure the installation coordinates of the laser guidance device. The installation coordinates are then input into the control module of the laser guidance device. The control module sets the emission direction parameters of the coded laser beam based on the installation coordinates, causing the coded laser beam to be emitted along a preset spatial direction and forming a spatial guidance reference. The spatial guidance reference adopts a three-dimensional laser reference plane or a laser reference axis. The three-dimensional laser reference plane is formed by the intersection of coded laser beams emitted by at least three laser guidance devices, while the laser reference axis is formed by a continuous coded laser beam emitted by a single laser guidance device. After calibration, the emission parameters of the laser guidance device are locked to prevent the emission direction from deviating during operation.

[0064] S102: Control of the drone entering the target hovering area.

[0065] Flight commands are sent to the UAV via the ground control terminal to control it to fly along a preset path from the take-off and landing point; during flight, the inertial navigation unit obtains the UAV's current position information in real time, compares the current position information with the preset coordinate range of the target hovering area, and determines the flight position.

[0066] When the drone reaches the outer boundary of the target hovering area, reduce its flight speed to... Adjust the attitude so that the receiver of the airborne laser sensing device is aligned with the direction of the coded laser emission; start the sensing device, set the receiving sensitivity to match the laser output power, and start the continuous receiving mode.

[0067] Control the drone to slowly enter the target area and monitor the coded laser reception status in real time: if the coded laser beam can be received for more than 3 consecutive seconds and the signal reception intensity meets the preset initial threshold, the entry is considered successful. For example, the initial threshold can be set to 500mV. Control the drone to stop horizontal flight and enter the hovering preparation state; otherwise, adjust the position and attitude and try again until the conditions are met.

[0068] refer to Figure 3 , Figure 3 This is a schematic diagram illustrating the layout of the laser guidance device and the establishment of the space guidance reference provided in this embodiment.

[0069] Figure 3 The left side shows the deployment points of at least two laser guidance devices and their emission direction calibration process; the two laser guidance devices include laser guidance device 1 and laser guidance device 2; before operation, the installation coordinates of the laser guidance devices are measured using a total station, and the installation coordinates are input into the control module of the laser guidance devices. The control module sets the emission direction parameters of the coded laser beam based on the installation coordinates, so that the coded laser beam is emitted along the preset spatial direction and forms a spatial guidance reference. After calibration, the emission parameters of the laser guidance devices are locked to avoid emission direction deviation.

[0070] Figure 3 The target hovering area is shown in the middle with a dashed box. Multiple preset hovering points are set up in the target hovering area. The coded laser beam emitted by the laser guidance device covers the target hovering area and provides a spatial reference for the UAV's hovering and positioning in the form of a "spatial guidance reference".

[0071] Figure 3 The upper right shows the control process of the UAV entering the target hovering area: The ground control terminal sends flight commands to the UAV, controls the UAV to fly along the preset path, and reduces the flight speed and adjusts the attitude when it approaches the outer boundary of the target hovering area so that the receiver of the airborne laser sensing device is aligned with the direction of the coded laser emission. Then, the continuous receiving mode is started and the coded laser receiving status is monitored in real time during the process of entering the target area. When the continuous reception is satisfied and the intensity meets the preset initial threshold, the entry is determined to be successful and the hovering preparation state is entered.

[0072] It should be noted that, Figure 3 This is for illustrative purposes only, intended to help understand the process structure and data organization, and does not represent the precise working state in actual operation.

[0073] S103: Extraction of laser receiving features.

[0074] The airborne laser sensing device continuously receives coded laser beams, converts the optical signals into electrical signals, and after filtering and amplification, demodulates the received coded laser beams, analyzes their modulation frequency characteristics, extracts the modulation frequency identifier, and verifies it against a preset frequency. For example, the preset frequency can be set to 1kHz~10kHz. Only when the verification is successful is the core laser reception feature extracted, including: the coordinates of the laser spot center. Area of ​​light spot Receive strength Signal continuity and stability This provides a basis for subsequent deviation calculations.

[0075] The center coordinates of the laser spot are extracted using a combination of threshold segmentation and centroid method. First, a grayscale threshold for the electrical signal is set; for example, this threshold can be set to 128. The processed electrical signal is then segmented using the threshold, identifying areas with grayscale values ​​greater than or equal to the threshold as laser spot regions and removing background noise. After determining the pixel range of the laser spot, the center coordinates of the laser spot are calculated using the centroid method, as shown in the following formula:

[0076] 、 ,

[0077] in, This represents the total number of pixels in the light spot. For the first Each light spot pixel coordinate, For the corresponding pixel grayscale value, this coordinate is the initial position of the coded laser beam in the airborne sensing coordinate system.

[0078] Combined with the pixel resolution of the airborne laser sensing device Calculate the area of ​​the light spot. Set the light spot area threshold range , These are the maximum and minimum values ​​of the set spot area, respectively. If the data is identified as an abnormal spot, the data set is removed and re-extracted.

[0079] Real-time acquisition and calculation of laser receiving characteristic parameters, including:

[0080] Received strength The light intensity detection module collects data every 10ms, taking data per unit time. The average value within, i.e. ,in, unit of time Number of times within, For the first Secondary received signal strength acquisition value;

[0081] Continuity : Count the number of frames of continuously received valid coded laser signals and calculate the duration of continuous reception. , ;

[0082] stability : Calculate the variance of the received intensity per unit time. The smaller the variance, the better the signal stability.

[0083] S104: Calculation of link trust factor.

[0084] Set the weight coefficients for the three core parameters to satisfy... Where: Received strength weight Continuity weight Stability weights Dynamically adjust based on the complexity of the working environment: increase when environmental interference is significant. Increase when the environment is stable .

[0085] The three core parameters are normalized by mapping the measured values ​​to the [0,1] interval to eliminate the influence of dimensions, thus obtaining the normalized values ​​of received intensity, continuity, and stability. , , The formula for normalizing the received signal strength is: In the formula 、 These are the preset maximum and minimum received signal strength thresholds, respectively; the continuity normalization formula is... In the formula 、 These are the preset maximum and minimum continuity thresholds, respectively; the stability normalization formula is: In the formula 、 These are the preset maximum and minimum stability thresholds; stability is negatively correlated with variance, and the larger the value after normalization, the better the stability. All the above thresholds are set comprehensively based on actual engineering needs.

[0086] The link reliability factor is calculated using the weighted summation method. The reliability of the laser guidance link is quantified by the formula: ,in, , The closer the value is to 1, the higher the reliability of the link.

[0087] Based on the effective failure range of laser core receiving characteristics and the practical engineering requirements of high-altitude fire suppression, the link reliability factor threshold was determined through comprehensive experimental verification data. Laser loss detection threshold Perform anomaly detection on the calculation results. The laser guidance link was determined to be reliable, and this result was retained. The value is used for subsequent bias observation and fusion processing; if If the link is deemed unreliable, the fused data set is removed, the laser signal is reacquired, and steps S103-S104 are repeated; if If the laser signal is determined to be lost, the abnormal operating condition handling mechanism is immediately triggered, and a laser loss signal is sent to the EKF fusion processing unit and hovering control module. The UAV switches to the backup positioning strategy of inertial navigation + satellite positioning until a valid coded laser signal is received again.

[0088] For example:

[0089] Assume a high-altitude firefighting operation area is The rectangular area, the elevation of the target hovering area There are no tall obstacles in the surrounding area. In accordance with S101 requirements, two unobstructed deployment points were selected. Point A: Coordinates Point B: Coordinates The laser guidance device uses a pulsed laser emitter with an emission wavelength of... pulse frequency divergence angle Output power dynamically adjusted to To ensure Stable reception.

[0090] After calibration with the total station, the emission angle at point A points towards the center of the area. The launch angle at point B is synchronized and calibrated. Using S102, the UAV flies from the takeoff and landing point along a preset path, its speed decreasing upon reaching the outer edge of the area. The receiver sensitivity of the airborne laser sensing device was adjusted to match... Power matching, continuous The laser signal is received stably, indicating successful entry.

[0091] Extract laser receiving characteristics according to S103: unit time Internal collection Average received intensity Continuous reception duration Continuity Reception stability .

[0092] Set the weight according to S104 , , , , , , , , After normalization: , , Link trust factor: .because The link is determined to be trustworthy.

[0093] In step S2, the lateral deviation observation is obtained by analyzing the laser position change, and then weighted and anomaly suppressed based on the link confidence factor to finally generate the observation confidence factor, including:

[0094] S201: Calibration of the airborne sensing coordinate system and setting of the laser position reference.

[0095] Calibrate the airborne sensing coordinate system corresponding to the airborne laser sensing device The origin of the coordinate system Coinciding with the receiving center of the airborne laser sensing device, The plane is parallel to the horizontal plane of the drone's fuselage, i.e., the transverse plane. Axis perpendicular to The plane is pointed upwards from the drone, i.e., vertically. This clearly distinguishes the coordinates of the horizontal plane and the vertical plane, ensuring the accuracy of subsequent calculations of the horizontal offset.

[0096] Based on the spatial guidance reference established in step S101, the laser beam is extracted in the airborne sensing coordinate system. Reference position coordinates in If the spatial guidance reference is a three-dimensional laser reference plane, the equation of the reference plane is transformed to the airborne sensing coordinate system through coordinate transformation, and the coordinates of the intersection point of the laser beam and the reference plane are solved as the reference position. If it is a laser reference axis, the coordinates of the intersection point of the projection of the reference axis in the airborne sensing coordinate system are solved as the reference position. This reference position is the ideal receiving position of the laser beam in the airborne sensing coordinate system when the UAV hovers accurately.

[0097] S202: Analysis of the spatial position change of laser in airborne sensing coordinate system.

[0098] Based on the laser receiving features extracted in step S103, the actual position coordinates of the laser spot center in the airborne sensing coordinate system of consecutive frames are obtained in real time. ,in, , The number of consecutively acquired frames, with a value range of... To ensure the stability of position change analysis, the acquisition frequency is kept consistent with the laser pulse frequency, i.e. .

[0099] Calculate the position change of the laser spot center in consecutive frames, and obtain the lateral position change respectively. Change in position in the plane , and vertical Change in position along the axis The vertical position change is used to assist in the subsequent height error judgment, while this step focuses on the analysis of the lateral position change.

[0100] Horizontal position change of consecutive frames , Smoothing filtering is performed, and a moving average filtering algorithm is used to eliminate random noise interference, resulting in a stable mean value of the lateral position change. , .

[0101] S203: Calculation of lateral offset and offset direction, and construction of lateral deviation observation.

[0102] Calculate the lateral offset of the UAV relative to the space guidance reference. Based on the mean of the filtered lateral positional changes, the Euclidean distance formula is used to calculate, i.e. , The magnitude of the lateral offset directly reflects the degree to which the drone deviates laterally from the ideal hovering position.

[0103] Calculate the lateral offset direction Using airborne sensing coordinate system With the positive axis direction as the reference and clockwise as the positive angle direction, the formula for calculating the offset direction is: , Simultaneously define the offset direction identifier, when The time is the first quadrant offset. The time is the second quadrant offset, and so on, to determine the specific direction of the UAV's lateral offset.

[0104] By lateral offset With offset direction Together they constitute the lateral deviation observations characterizing the lateral deviation state of the UAV. Its vector expression is , T Indicated by transpose, this observation fully characterizes the lateral deviation of the UAV relative to the space guidance reference, providing core data support for subsequent deviation correction.

[0105] S204: Perform confidence weighting on the lateral deviation observations based on the link confidence factor.

[0106] The link trust factor calculated in step S104 is called. , The larger the value, the more reliable the laser guidance link, and the greater the lateral deviation observation. The higher the credibility of a data point, the greater its weight in subsequent processing.

[0107] Set weighting coefficients Weighting coefficients and link credibility factor Positive correlation, satisfying By strengthening the weight differentiation of credibility factors through squared relationships, high-credibility observations are given higher weights. And it satisfies the normalization condition. , These are untrusted weights, used as backup for subsequent anomalies.

[0108] Observations on lateral deviation Perform confidence-weighted processing to obtain the weighted lateral bias observation. Its specific component is the weighted lateral offset of the UAV. The lateral offset direction of the UAV after weighted processing By using weighted processing, the impact of low-confidence observations is reduced, thereby improving the accuracy of subsequent bias processing.

[0109] S205: Perform anomaly suppression processing on the weighted lateral deviation observations.

[0110] Set the horizontal offset anomaly threshold Abnormal threshold range with offset direction ,in The value range is set according to the target hovering positioning accuracy requirements. mm, the higher the positioning accuracy. The smaller the value; the smaller the minimum anomaly threshold in the offset direction. Maximum abnormal threshold in offset direction When the offset direction changes abruptly, it is determined to be an abnormal direction.

[0111] Construct anomaly judgment criteria for the weighted lateral deviation observations. Perform anomaly detection: If This is determined to be an abnormal offset, indicating that the drone's lateral deviation exceeds the reasonable range; if the offset direction... Jump angle ≥ The direction is determined to be abnormal, indicating severe interference in the laser receiving signal; if the link reliability factor is... The result was directly determined to be an abnormality in the observation, indicating that the reliability of the laser guidance link was extremely low. The threshold of 0.3 was verified by high-altitude fire extinguishing engineering experiments. Under this threshold, the guidance link had failed and the observation was of no reference value.

[0112] Anomaly suppression processing is performed using an adaptive threshold pruning method: for anomalies in offset, Cut to ,Right now For cases of directional anomalies, the average of the offset directions from the previous three frames is used to replace the current value. For anomalies caused by excessively low link confidence factors, the current observation is discarded and replaced with a valid observation from the previous frame to ensure that the abnormal observation does not affect subsequent processing.

[0113] S206: Generate an observation confidence factor corresponding to the current guidance state.

[0114] Observational confidence factor Used to quantify the reliability of lateral deviation observations under the current guidance state, its value is determined by the link reliability factor. Together with the observed bias after anomaly suppression, it determines whether the following conditions are met. , The larger the value, the more reliable the lateral bias observations after weighting and anomaly suppression.

[0115] Construct a computational model for the observation reliability factor, combined with the link reliability factor. Offset after anomaly suppression With ideal offset This refers to the offset in the ideal hovering state, which is determined by the accuracy of the actual application scenario. The calculation formula is:

[0116] ,

[0117] in, As the initial observation confidence factor, The attenuation coefficient has a range of values. This is used to adjust the influence of the offset on the observation confidence factor. The calculated observation confidence factor is normalized to ensure that its value is within a certain range. Within the interval, the final output is the observation confidence factor, which characterizes the reliability of the current lateral bias observation. .

[0118] For example:

[0119] Calibrate the airborne sensing coordinate system according to S201 The origin coincides with the laser receiving center. The plane is parallel to the horizontal plane of the fuselage. The spatial guidance reference is the three-dimensional reference plane formed by the intersection of laser beams at points A and B. After transformation to the airborne coordinate system, the reference position coordinates are... pixel resolution Continuous data collection according to S202 The center position of the laser spot in frame 30, real-time position. Calculate the change in lateral position Combined with pixel resolution conversion , Similarly, After moving average filtering Combined with pixel resolution conversion , Similarly, .

[0120] Calculate the lateral offset according to S203:

[0121] Offset direction This represents the fourth quadrant offset, specifically the lateral deviation observation. .

[0122] Call S204 Weighting coefficients After weighting , Set according to S205. No issues found. Set the ideal offset using S206. attenuation coefficient Initial observation confidence factor: After normalization .

[0123] like Figure 4 As shown, in step S3, multi-source sensor information is acquired synchronously, fused into a data set after time alignment, and then the hovering level error is corrected and calculated. A reliable hovering level difference is then selected through motion compatibility verification, including:

[0124] S301: Synchronous acquisition and preprocessing of multi-source information.

[0125] After the drone enters the hovering preparation state, it activates the inertial navigation unit and altitude measurement device, maintaining synchronous operation with the onboard laser sensing device to ensure the timing consistency of multi-source information acquisition. The acquisition frequency is uniformly set to match the laser pulse frequency. This avoids data misalignment caused by differences in collection frequency.

[0126] Simultaneously acquire various types of information and define parameters clearly: attitude information output by the inertial navigation unit, including the UAV pitch angle. Roll angle Heading angle Unit: rad, Attitude information accuracy ≤ denoted as attitude vector The velocity information output by the inertial navigation unit includes the lateral velocity of the UAV in the geodetic coordinate system. 、 and vertical velocity Wherein, the lateral velocity is the component of velocity parallel to the horizontal plane, and the vertical velocity is the component of velocity perpendicular to the horizontal plane, with units of m / s, denoted as the velocity vector. Real-time altitude information of the UAV output by the altitude measurement device Measurement accuracy ≤ The lateral deviation observations generated in step S203 .

[0127] The acquired attitude information Speed ​​information Altitude information and lateral deviation observations Preprocessing is performed separately: Kalman filtering is used to eliminate random drift errors for attitude information; mean filtering is used to remove abnormal impulse noise for velocity, altitude and lateral deviation observations. The size of the filtering window is the same as in step S202 to ensure the stability of the preprocessed data.

[0128] S302: Timestamp assignment and time base alignment processing.

[0129] Preprocessed attitude information Speed ​​information Altitude information and lateral deviation observations Assign UTC timestamps respectively , , , The timestamp precision is in the millisecond range, ensuring that each set of data corresponds to a unique time identifier, which facilitates subsequent time series matching.

[0130] Selecting the time reference for the inertial navigation unit As a unified time reference, the timestamps of velocity information, altitude information, and lateral deviation observations are aligned to eliminate time discrepancies between multi-source information. Set a time deviation threshold. ,like ,in, They are respectively , , The timestamps are corrected using linear interpolation, so that the corrected timestamps are all equal to ;like Discard the abnormal time series data and re-collect and preprocess it.

[0131] The attitude, velocity, altitude, and lateral deviation observations, aligned with the time reference, are combined to form a fused data set. The fused data sets are stored continuously in time series, with each time series corresponding to a complete set of multi-source fused data, providing a unified time series basis for subsequent coordinate correction and verification.

[0132] S303: Based on the coordinate consistency correction of attitude information, the candidate deviation quantity is obtained.

[0133] Criterion for defining the coordinate system: Lateral deviation observation Based on airborne sensing coordinate system Attitude and velocity information are based on the geodetic coordinate system. It is necessary to use the attitude matrix to transform the lateral deviation observations in the airborne sensing coordinate system to the geodetic coordinate system to achieve coordinate consistency.

[0134] Based on attitude vector Construct the attitude transformation matrix from the airborne sensing coordinate system to the ground coordinate system. The formula is:

[0135] .

[0136] Lateral deviation observation Converted to lateral deviation vector in airborne sensing coordinate system The vertical component is 0, and only the lateral deviation is considered. This is achieved through the attitude transformation matrix. Perform a coordinate transformation to obtain the lateral deviation vector in the geodetic coordinate system. .

[0137] Extracting the lateral deviation vector in the geodetic coordinate system The horizontal component, i.e. 、 The axial direction is a candidate quantity for deviation, denoted as . ,in, for Horizontal error in axial direction for The horizontal error in the axial direction directly reflects the degree of horizontal deviation when the drone is hovering.

[0138] S304: Motion compatibility check combining speed information.

[0139] The core criterion for motion compatibility verification is that, while the UAV is hovering, the rate of change of the horizontal error should be consistent with the lateral velocity output by the inertial navigation unit. , Matching means that the rate of change of horizontal error should not exceed the reasonable fluctuation range of horizontal velocity. If the deviation is too large, it indicates that the amount of horizontal error is abnormal and needs to be eliminated.

[0140] Calculate the rate of change of the candidate deviation. The horizontal error is calculated based on the fused data from two consecutive frames, using the following formula:

[0141] 、 ,

[0142] in, Geodetic coordinate system Rate of change of candidate quantities for axial deviation Geodetic coordinate system Rate of change of candidate quantities for axial deviation The data acquisition period is the reciprocal of the acquisition frequency. , This represents the horizontal error of the current frame. , This represents the horizontal error of the previous frame.

[0143] Set motion compatibility verification threshold value range m / s, this threshold is set according to the drone's hovering control accuracy; the higher the accuracy requirement, the smaller the threshold. Verification conditions are established: and .

[0144] Candidate values ​​of current frame offset Execution verification: If both of the above verification conditions are met at the same time, the horizontal error amount is determined to have passed the motion compatibility verification and the data is retained; if either condition is not met, the verification is determined to have failed, the group of horizontal error amounts is removed, marked as abnormal data and recorded.

[0145] S305: Screening and output of hovering level difference.

[0146] For the fused data set of continuous time series, repeat steps S303-S304 to complete the coordinate correction and motion compatibility verification of each set of deviation candidate quantities, and collect all the horizontal error quantities that pass the verification.

[0147] A secondary screening is performed on the horizontal error values ​​that pass the verification to remove isolated outliers: The threshold for the number of consecutive frames that pass the verification is set to 3. If the horizontal error values ​​of a certain horizontal error value pass the verification in the two frames before and after it, then the horizontal error value is retained; if it is isolated data that passes the verification, that is, it fails the verification in the two frames before and after it, then it is determined to be occasional valid data and is removed.

[0148] The remaining horizontal error after the second screening is defined as the hovering horizontal difference that has passed the verification. ,in, Geodetic coordinate system The hovering horizontal difference component in the axial direction, after motion compatibility verification and secondary screening. Geodetic coordinate system The hovering horizontal difference component after being filtered and verified in the same axial direction. It is an index of hover level differences, used to match each set of validated hover level differences with its corresponding observation confidence factor, ensuring a one-to-one correspondence between the time series and attributes of multiple candidate data sets. , The total number of candidate quantities and the hovering horizontal difference all meet the requirements of coordinate consistency and motion compatibility, indicating high reliability.

[0149] For example:

[0150] Synchronously acquire the attitude vector output by the inertial navigation unit according to S301. velocity vector High-altitude information Lateral deviation observations The data showed no abnormalities after preprocessing.

[0151] Assign a timestamp according to S302 and align it to the inertial navigation time reference. Construct the attitude transformation matrix according to S303. , the lateral deviation vector Transform to the geodetic coordinate system to obtain candidate deviation quantities. .

[0152] Calculate the deviation change rate according to S304, and the data acquisition period. Current frame Shaft deviation candidate =0.472mm Shaft deviation candidate = -0.185mm, previous frame The candidate value for axis deviation is 0.468mm, in the previous frame. The candidate value for shaft deviation is -0.183 mm; therefore , ,set up Based on the velocity vector given above ,because and The verification was successful. The hovering level difference was obtained by filtering according to S305. .

[0153] In step S4, the confidence level error is obtained through adaptive confidence fusion, and the height error is calculated. Finally, under consistency constraints, these are integrated into a hovering error vector, including:

[0154] S401: Adaptive credibility fusion processing based on extended Kalman filter (EKF) of observation credibility factor to obtain the credibility level error.

[0155] Obtain the hover level difference output in step S305. Simultaneously acquire the observation confidence factor corresponding to each group of hovering level differences. It is output by step S206 and corresponds one-to-one with the hovering horizontal difference timing.

[0156] The core of extended Kalman filtering is to characterize the uncertainty of the system model through the system noise covariance matrix, such as the error of the UAV's own motion model and inertial navigation drift; and to characterize the uncertainty of the observation data through the observation noise covariance matrix, such as the measurement error of the laser guidance link.

[0157] Through link trust factor Dynamically adjust the system noise covariance matrix by observing the confidence factor. The observation noise covariance matrix is ​​dynamically adjusted to achieve an adaptive fusion logic of "high-confidence information with low noise weight and low-confidence information with high noise weight".

[0158] Before performing EKF fusion on multiple hovering level differences, fusion weights need to be assigned according to the observation confidence of each data set. This is essentially a "priority ranking" of valid candidate data, preventing low-confidence data from occupying too many fusion weights. First, all observation confidence factors are normalized to eliminate the influence of dimensions, resulting in normalized fusion weights. The calculation formula is:

[0159] ,

[0160] in, For the first The observational reliability factor of the hovering level difference data. If a certain hovering level difference corresponds to If a candidate is determined to be a low-confidence candidate, its weight is directly adjusted. Setting it to 0 removes the fusion contribution of this candidate quantity, retaining only high-reliability data to participate in subsequent EKF fusion, thus reducing the interference of low-quality data from the source.

[0161] The normalized fusion weights calculated above are used as follows: As a prerequisite, multiple sets of hovering level differences After performing weighted preprocessing, the preprocessed observation data is substituted into the EKF framework, which has undergone dynamic adjustment of the noise matrix, and a "prediction-update" iterative process is executed:

[0162] Prediction phase: Based on the velocity information of the inertial navigation unit, the horizontal position of the UAV is predicted. Combined with the adjusted system noise covariance matrix, the prediction error covariance is calculated to ensure that the error weights of the system model match the link reliability.

[0163] Update phase: The weighted hovering level difference is used as the observation input, and the Kalman gain is calculated by combining it with the adjusted observation noise covariance matrix. The prediction results are then corrected to ensure that the weights of the observation data match their own credibility.

[0164] Finally, the EKF iteration results are integrated using a weighted summation method to obtain the confidence level error. The specific calculation formula is as follows:

[0165] ,

[0166] ,

[0167] in, In the geodetic coordinate system Confidential horizontal error in the axial direction. In the geodetic coordinate system The axial direction confidence level error is calculated; the fused confidence level error is smoothed using the mean filtering algorithm consistent with step S301 to eliminate random noise generated during the fusion process and ensure the stability of the confidence level error; simultaneously, an abnormal threshold for the confidence level error is set. The value range is set to mm, this value range represents the error tolerance for drone hovering control in engineering applications. If If the threshold requirement is met, the fusion process of S401 will be re-executed.

[0168] S402: Calculate the hovering height error based on altitude information.

[0169] Define the hovering height benchmark and preset the target hovering height. This reference is consistent with the preset height of the target hovering area in step S101, and is input into the UAV flight control unit in advance by the ground control terminal as a reference reference for altitude error calculation.

[0170] The preprocessed real-time altitude information of the UAV obtained in step S301 is called. Calculate the initial hovering height error. The calculation formula is:

[0171] ,

[0172] in, This indicates that the drone is hovering above the target altitude. This indicates that the drone is below the target hovering altitude. This indicates that the drone is at the ideal hovering altitude.

[0173] Initial hovering height error Anomaly suppression and preprocessing are performed, consistent with the preprocessing logic in step S301: Mean filtering is used to remove impulse noise, and the filter window size is... The frame yields the filtered height error. Set an abnormal height error threshold. value range mm, this threshold range is set based on the actual measurement accuracy of the altitude measuring device, combined with the vertical error tolerance under complex environments such as strong airflow at high altitudes. If If the altitude error is deemed abnormal, this set of data is discarded, and altitude information is recollected and recalculated; if This data is retained as the final hovering height error. ,Right now The hovering height error is used as the vertical observation input for the extended Kalman filter (EKF), and together with the lateral deviation observation, it participates in the multi-source fusion calculation of the hovering error vector.

[0174] S403: Perform consistency constraint integration based on the link trust factor to form a hovering error vector.

[0175] The link trust factor calculated in step S104 is called. Clarify its role in consistency constraints: Used to adjust the confidence level error. Error with hovering height The weighting percentage The closer the value is to 1, the more reliable the laser guidance link is, the higher the weight of both, and the stronger the constraint strength. The closer the value is to 0.5, which is the critical value for link trustworthiness, the weight should be appropriately reduced to improve the constraint tolerance.

[0176] Construct consistency constraint weights based on link trust factors Set the horizontal error constraint weights respectively Weights with height error constraints ,in, , , To ensure that the weight allocation is positively correlated with the reliability of the link, and that the horizontal error constraint weight is slightly higher than the altitude error constraint weight, this setting is to meet the core requirements of hovering during high-altitude firefighting, i.e., horizontal positioning accuracy takes precedence over altitude positioning accuracy; if If the link is untrusted, the effective constraint weights of the previous frame are used directly to avoid constraint failure caused by the untrusted link.

[0177] Perform consistency constraint checks to ensure that the deviation between the confidence level error and the hovering height error is within a reasonable range, and construct the constraint conditions:

[0178] ,

[0179] in, The consistency constraint threshold has a range of values. If the constraint is met, the two are considered to be in good consistency, and subsequent integration is performed; if the constraint is not met, the reliable horizontal error and hovering height error are corrected a second time, and the verification is repeated until the condition is met.

[0180] The confidence level error quantity verified by consistency constraints Error with hovering height The process is integrated to form a hovering error vector for hovering control. The vector expression is:

[0181] ,

[0182] The first two components of the hovering error vector are the lateral horizontal error components of the UAV, and the third component is the vertical altitude error component. These components can be directly input into the hovering control law in step S5 for subsequent hovering correction calculations, ensuring hovering positioning accuracy and stability.

[0183] In step S5, the hovering error vector is mapped to a hovering correction control quantity, which is then input to the flight control unit to collaboratively adjust the UAV's hovering position. The extended Kalman filter (EKF) parameters and hovering correction control quantity are dynamically updated in conjunction with the link confidence factor and observation confidence factor, including:

[0184] S501: Preset hover control law and complete parameter calibration.

[0185] Based on the hovering characteristics of high-altitude firefighting drones, the preset hovering control law adopts the PID (proportional-integral-derivative) control law. This control law can realize the linear mapping from the hovering error vector to the hovering correction amount, effectively suppressing error accumulation and improving hovering response speed.

[0186] Define the input and output of the hovering control law: the input is the hovering error vector output in step S403. The output quantities are the lateral hovering correction quantity and the vertical hovering correction quantity, which correspond to the lateral position adjustment quantity and the vertical height adjustment quantity of the UAV, respectively.

[0187] Calibrate the PID control law parameters, setting the horizontal and vertical PID parameters respectively: Horizontal PID parameter: proportional coefficient Integral coefficient Differential coefficients Vertical PID parameter: proportional coefficient Integral coefficient Differential coefficients The PID parameters are set based on the core requirements of prioritizing horizontal positioning accuracy and ensuring smooth vertical altitude adjustment to prevent overshoot when the high-altitude firefighting drone is hovering. These parameters are also comprehensively set considering the anti-interference requirements of complex environments such as strong high-altitude airflow and the execution response characteristics of the drone's power system. The parameter calibration principles are: increasing the proportional and derivative coefficients in strong high-altitude wind environments to enhance anti-interference capabilities; and decreasing the integral coefficient when the environment is stable to avoid overshooting of the correction.

[0188] S502: Calculate lateral and vertical hovering corrections based on hovering control laws.

[0189] Extracting the hovering error vector lateral error components , and vertical error components All units are in mm, ensuring that the dimensions of the error components match those of the correction quantities.

[0190] Calculate the lateral hovering correction using a lateral PID control law. 、 Perform mapping calculations separately to obtain Lateral hovering correction in axial direction and Lateral hovering correction in axial direction The calculation formula is:

[0191] ,

[0192] ,

[0193] in, , This represents the time from the start of the hovering correction to the current calculation time. Inside, 、 The integral term of the axial direction error component is used to eliminate static error; , These are the differential terms of the error components, used to suppress overshoot of the correction and improve response stability.

[0194] Calculate the vertical hovering correction using a vertical PID control law. Perform mapping calculations to obtain the vertical hovering correction amount. The calculation formula is:

[0195] ,

[0196] The calculated lateral hovering correction amount 、 Vertical hovering correction Perform amplitude limiting processing and set the correction amplitude threshold: lateral correction threshold. mm, vertical correction threshold mm, to avoid excessive correction amount causing sudden changes in drone attitude and unstable hovering; if the correction amount exceeds the preset correction amount amplitude threshold, it will be trimmed to the corresponding maximum value to ensure a smooth correction process.

[0197] S503: Combine hovering correction control values ​​and input them to the flight control unit.

[0198] Lateral hovering correction amount after amplitude limitation 、 Vertical hovering correction Combined, they form a complete hovering correction control quantity. This control quantity contains complete adjustment commands for the drone's lateral and vertical directions, which can be directly used for hovering position adjustments.

[0199] Hover correction control amount The data is input to the UAV flight control unit via the data bus. After receiving the control signal, the flight control unit decodes and amplifies it, converting it into a control signal that can be recognized by the UAV's power system.

[0200] The flight control unit integrates hover correction control signals with attitude control signals and altitude hold control signals, defining the collaborative control logic: the attitude control signal is generated by the attitude vector from the inertial navigation unit. The generator is used to maintain the drone's horizontal position and prevent tilting during lateral corrections; the altitude hold control signal is used to assist in vertical corrections, ensuring that altitude fluctuations do not exceed [a certain threshold] during vertical adjustments. mm; The hovering correction control signal serves as the core adjustment command, taking precedence over the basic attitude and altitude control to ensure that the hovering position quickly converges to the ideal state.

[0201] The flight control unit drives the drone's power system to perform hovering position adjustments: this is achieved by adjusting the speed of the lateral motor. 、 The corresponding lateral position correction is achieved by adjusting the speed of the vertical motor. The corresponding vertical height correction is implemented, and the adjustment status is fed back in real time during the adjustment process to ensure that the correction amount is accurately executed.

[0202] S504: Dynamically update the extended Kalman filter (EKF) parameters and hover correction control based on the link confidence factor and the observation confidence factor.

[0203] Set an update cycle that is consistent with the data acquisition frequency of steps S1-S4. That is, after each set of multi-source data is acquired and a set of hovering error vectors is generated, the fusion weight and hovering correction amount are updated synchronously to ensure that the update sequence is synchronized with the data acquisition sequence and improve the real-time performance of hovering positioning.

[0204] Based on the link confidence factor and observation confidence factor obtained in real time, the observation noise covariance matrix and system noise covariance matrix of the extended Kalman filter are dynamically updated. When the link confidence factor is lower than the confidence threshold, the covariance matrix parameters of the previous frame are used. Based on the updated filter parameters, the multi-source information fusion processing is re-executed to generate a new hovering error vector.

[0205] Call step S104 to calculate the link trust factor in real time Based on the observation confidence factor generated in real time in step S206, the normalized fusion weights in step S401 are first updated. The updated formula is as follows ,in, The current frame observation confidence factor. The updated fusion weights are then used; the consistency constraint weights from step S403 are then updated. , ,in, The current frame link trust factor. The updated horizontal error constraint weights, These are the updated height error constraint weights. If... If the constraint weights of the previous frame are used, the weights will be changed to avoid untrusted links causing sudden changes in weights. Based on the updated filter parameters, the multi-source information fusion process will be re-executed to generate a new hovering error vector. The hovering correction control quantity will be re-mapped, calculated and updated.

[0206] Based on the updated fusion weights and extended Kalman filter (EKF) parameters, the confidence level error calculation in step S401 is re-executed to obtain the updated confidence level error. Then repeat step S403 to generate a new hovering error vector. Finally, the updated hover correction control quantity is obtained by remapping and calculating the PID control law in step S502. Replace the original hover correction control.

[0207] After each update of the Extended Kalman Filter (EKF) parameters, fusion weights, and corrections, the validity of the updated data is verified: if the updated fusion weights satisfy... If the hovering correction is within the amplitude threshold range, the update is deemed valid, and the new weights and corrections are used to perform the next hovering adjustment. If the update is invalid, the current update result is discarded, and the extended Kalman filter (EKF) parameters, weights, and corrections from the previous frame are used. An anomaly is marked and synchronously fed back to the ground control terminal for real-time monitoring by operators.

[0208] Throughout the entire hovering operation of the UAV, step S504 is repeated to achieve real-time and dynamic updates of the extended Kalman filter (EKF) parameters, fusion weights, and hovering corrections. This ensures that the hovering control always adapts to the current laser guidance link status and the UAV's flight status, guaranteeing that the UAV maintains high-precision hovering positioning in the complex environment of high-altitude firefighting, thus meeting the needs of firefighting operations.

[0209] This embodiment establishes a laser spatial guidance reference in the operational area to guide the UAV into the target hovering area and continuously acquire laser guidance information. A link reliability factor is calculated based on laser reception characteristics to dynamically evaluate the guidance link status. On this basis, reliability weighting and anomaly suppression processing are performed on the laser deviation observation information, and time reference alignment and coordinate consistency correction are performed with the attitude, velocity, and altitude information output by the inertial navigation unit to form multi-source fused data. By introducing an adaptive fusion mechanism of observation reliability factor and link reliability factor, consistency constraints are integrated for hovering horizontal error and hovering altitude error to construct a hovering error vector for hovering control. Based on a preset hovering control law, the flight control unit is driven to dynamically correct the UAV's hovering position, enabling the UAV to maintain a stable and reliable hovering positioning state continuously in complex environments such as high-altitude firefighting.

[0210] Example 2

[0211] This invention discloses a hovering system for firefighting drones based on laser guidance and trusted fusion, comprising an coded laser emitting module, a laser receiving array, an inertial navigation unit, an EKF fusion processing unit, and a flight control unit:

[0212] The coded laser emission module emits a coded laser beam with a modulation frequency towards the target hovering area via a laser guidance device. The coded laser beam is generated by frequency modulation or phase modulation, and the modulation frequency is uniquely set within a preset frequency range. The module can dynamically adjust the laser output power according to the range of the high-altitude firefighting operation area and the altitude of the target hovering area. At the same time, it completes the deployment calibration and emission parameter locking of the laser guidance device to ensure that the emitted coded laser beam forms a stable spatial guidance reference and fully covers all preset hovering points in the target hovering area.

[0213] The laser receiving array adopts a two-dimensional photodetector array structure and is deployed on the UAV body to continuously receive coded laser beams from the coded laser emission module. The received coded laser beams are demodulated to analyze their modulation frequency characteristics and laser receiving characteristics. Based on the spatial position change of the coded laser beam in the two-dimensional photodetector array, a lateral deviation observation consisting of lateral offset and offset direction is constructed.

[0214] The inertial navigation unit continuously outputs the attitude and velocity information of the UAV; the EKF fusion processing unit: based on the extended Kalman filter, it performs adaptive reliable fusion processing of multi-source information, and dynamically adjusts the observation noise covariance matrix and the system noise covariance matrix with the link reliability factor and the observation reliability factor; this unit first performs time reference alignment and coordinate consistency correction on the lateral deviation observation output by the laser receiving array, the attitude and velocity information output by the inertial navigation unit, and the altitude information output by the altitude measurement device, and selects the hovering horizontal difference as the laser deviation observation input, and uses the hovering altitude error after anomaly suppression as the vertical observation input; then, using the attitude information and velocity information as state prediction auxiliary quantities, it performs fusion processing on the multi-source information through the prediction and update iterative loop of the extended Kalman filter, and outputs a hovering error vector containing the lateral horizontal error component and the vertical altitude error component.

[0215] The flight control unit maps and calculates the hovering error vector according to the preset hovering control law to obtain the lateral hovering correction and the vertical hovering correction. The lateral hovering correction and the vertical hovering correction are combined to form the hovering correction control quantity. The hovering correction control quantity is input into the flight control unit and works in conjunction with attitude control and altitude hold control to drive the UAV to perform hovering position adjustment. During hovering, the fusion weight and correction quantity are continuously updated based on the link reliability factor and the observation reliability factor.

[0216] The specific functional implementation of each module is described in the relevant content of the hovering method for firefighting drones based on laser guidance and trusted fusion described in Example 1, and will not be repeated here.

[0217] The above description is merely a preferred embodiment of the present invention. The scope of protection of the present invention is not limited to the above embodiments. All technical solutions falling within the scope of the present invention's concept are within the scope of protection of the present invention. It should be noted that for those skilled in the art, any improvements and modifications made without departing from the principles of the present invention should also be considered within the scope of protection of the present invention.

Claims

1. A hovering method for firefighting drones based on laser guidance and trusted fusion, characterized in that, Includes the following steps: The laser guidance device is controlled to emit coded laser beams to form a spatial guidance reference. The UAV is then controlled to enter the target hovering area to continuously receive the coded laser beams, while simultaneously acquiring the UAV's attitude, speed, and altitude information. The encoded laser beam is demodulated, its modulation frequency characteristics are analyzed and laser reception characteristics are extracted. Based on the spatial position change of the laser beam in the airborne sensing coordinate system, a lateral deviation observation consisting of lateral offset and offset direction is constructed. Based on extended Kalman filtering, the lateral deviation observation is fused with UAV attitude, velocity and altitude information to output a hovering error vector. The hovering error vector is mapped and calculated based on a preset hovering control law to generate a hovering correction control quantity to drive the UAV to adjust its hovering position. The process of extracting laser reception features also includes calculating the link reliability factor, including: The laser receiving characteristics are obtained and the calculation is performed to obtain the laser receiving characteristics, including laser receiving intensity, receiving continuity and receiving stability. Among them, laser receiving intensity is used to characterize the received amplitude level of the coded laser signal at the current spatial location, receiving continuity is used to characterize the continuous receiving state of the laser signal in the time series, and receiving stability is used to characterize the degree of fluctuation of laser receiving intensity in the continuous time series. The laser receiving intensity, receiving continuity, and receiving stability are normalized and fused together to form a link reliability factor characterizing the reliability of the current laser guidance link. Based on the link confidence factor, confidence modeling is performed on the lateral deviation observations to generate observation confidence factors, including: Based on the link reliability factor, the lateral deviation observations are subjected to reliability weighting to reflect the impact of the laser guidance link status on the reliability of lateral deviation observations; Based on the completed confidence weighting process, anomaly suppression processing is performed by combining the changes of lateral bias observations in continuous time series to generate an observation confidence factor that characterizes the reliability of the current lateral bias observation.

2. The hovering method for firefighting drones based on laser guidance and trusted fusion as described in claim 1, characterized in that, The construction of lateral deviation observations includes: Based on the characteristics of laser reception, the spatial position change of laser in the airborne sensing coordinate system is analyzed to determine the reference position and real-time reception position of laser in the airborne sensing coordinate system. Based on the relative spatial relationship between the reference position and the real-time receiving position, the lateral offset of the UAV relative to the spatial guidance reference is calculated. At the same time, the corresponding offset direction is determined according to the directional distribution of the lateral offset in the lateral plane of the airborne sensing coordinate system. The lateral offset and the offset direction together constitute the lateral deviation observation that characterizes the lateral deviation state of the UAV.

3. The hovering method for firefighting drones based on laser guidance and trusted fusion as described in claim 1, characterized in that, Prior to multi-source information fusion processing, time reference alignment and coordinate consistency correction of multi-source information are also included, including: Timestamps are assigned to lateral deviation observations, attitude information, velocity information, and altitude information respectively to unify the time base, and alignment processing is performed on the timestamps of each information source. Based on attitude information, an attitude transformation matrix is ​​constructed from the airborne sensing coordinate system to the unified reference coordinate system. Coordinate consistency correction is performed on the lateral deviation observation to transform it from the airborne sensing coordinate system to the unified reference coordinate system, thereby obtaining the deviation candidate quantity. The motion compatibility of the candidate deviation quantities is checked by combining the velocity information, the rate of change of the candidate deviation quantities is calculated, and the compatibility of the candidate deviation quantities with the lateral velocity output by the inertial navigation unit is judged. The hovering horizontal difference is then selected as the laser deviation observation input for the extended Kalman filter.

4. The hovering method for firefighting drones based on laser guidance and trusted fusion as described in claim 3, characterized in that, Multi-source information fusion processing based on extended Kalman filtering includes: The observation noise covariance matrix of the extended Kalman filter is dynamically adjusted based on the observation reliability factor, and the system noise covariance matrix is ​​dynamically adjusted based on the link reliability factor to achieve adaptive reliable fusion. Using the hovering horizontal difference as the laser deviation observation input, attitude information and velocity information as state prediction auxiliary quantities, and height information as the vertical observation input, the hovering error vector containing the lateral horizontal error component and the vertical height error component is output through prediction and update iterative loop.

5. The hovering method for firefighting drones based on laser guidance and trusted fusion as described in claim 1, characterized in that, Processing of altitude information includes: Based on altitude information and a preset hovering altitude benchmark, calculate the altitude deviation of the UAV relative to the target hovering altitude in the current hovering state; Mean filtering anomaly suppression processing is performed on the height deviation to remove instantaneous impulse noise that occurs during the height measurement process. Height deviations that exceed the preset height error anomaly threshold are discarded and re-acquired to obtain the hovering height error after anomaly suppression processing. This is used as the vertical observation input for the extended Kalman filter and participates in the multi-source fusion calculation of the hovering error vector together with the lateral deviation observation.

6. The hovering method for firefighting drones based on laser guidance and trusted fusion as described in claim 1, characterized in that, The generation of hover correction control values ​​includes: Based on the preset hovering control law, proportional-integral-derivative control law mapping calculations are performed on the lateral horizontal error component and the vertical height error component in the hovering error vector to generate lateral hovering correction and vertical hovering correction. Amplitude limiting processing is performed on the lateral hovering correction amount and the vertical hovering correction amount respectively, and the correction amount exceeding the preset correction amount amplitude threshold is clipped to the maximum value of the threshold. The lateral hovering correction value, after amplitude limitation, is combined with the vertical hovering correction value to form the hovering correction control value, which is input into the flight control unit and integrated with the attitude control signal and altitude hold control signal to drive the UAV power system to perform hovering position adjustment.

7. The hovering method for firefighting drones based on laser guidance and trusted fusion as described in claim 1, characterized in that, This also includes continuously and dynamically updating the extended Kalman filter parameters and hovering correction control variables throughout the entire hovering operation of the UAV, including: During the drone hovering process, the observation noise covariance matrix and system noise covariance matrix of the extended Kalman filter are dynamically updated based on the link confidence factor and observation confidence factor calculated in real time. When the link confidence factor is lower than the confidence threshold, the covariance matrix parameters of the previous frame are used. Based on the updated filtering parameters, the multi-source information fusion process is re-executed to generate a new hovering error vector. The hovering correction control quantity is then re-mapped, calculated, and updated to drive the UAV to continuously adjust its hovering position.

8. A hovering system for firefighting drones based on laser guidance and trusted fusion, used to implement the hovering method for firefighting drones based on laser guidance and trusted fusion as described in any one of claims 1-7, characterized in that, Includes an coded laser transmitting module, a laser receiving module, an inertial navigation unit, an EKF fusion processing unit, and a flight control unit: The coded laser emission module controls the laser guidance device to emit a coded laser beam toward the target hovering area, and the coded laser beam is generated by frequency modulation or phase modulation. The laser receiving module acquires the coded laser beam received by the two-dimensional photodetector array structure and performs demodulation processing, extracts the modulation frequency features and laser receiving features, and constructs the lateral deviation observation based on the spatial position change of the laser beam. The two-dimensional photodetector array structure is deployed on the UAV body. The inertial navigation unit collects the attitude and velocity information of the UAV, and simultaneously controls the altitude measuring device to determine the altitude information; The EKF fusion processing unit is based on extended Kalman filtering. It adjusts the observation noise covariance matrix and the system noise covariance matrix with the link confidence factor and the observation confidence factor, and performs multi-source information fusion on the lateral deviation observation, attitude information, velocity information and altitude information to output the hovering error vector. The flight control unit uses the hovering control law to map the hovering error vector into a hovering correction control quantity and drives the UAV to adjust its hovering position.

Images