Unmanned aerial vehicle visual signal time-frequency demodulation and pose resolving method oriented to high-dynamic illumination environment

Abstract

The invention discloses an unmanned aerial vehicle visual signal time-frequency demodulation and pose calculation method for a high-dynamic illumination environment, and aims to solve the problems of weak strong light interference resistance, insufficient pose calculation robustness and single control strategy in the prior art. The method comprises the following steps: firstly, collecting a continuous image sequence, constructing a pixel-level time domain signal, extracting a target light spot with a specific carrier frequency through frequency domain transformation, and filtering out strong light interference; resolving a pose through a PnP algorithm and calculating a re-projection residual error, dynamically adjusting a Kalman filter to observe a noise covariance according to the pose and the re-projection residual error, and outputting optimal state estimation; and finally, realizing loose / tight coupling dual-mode control switching according to the distance and the residual error. The method significantly improves the positioning signal-to-noise ratio and robustness in a high-dynamic illumination environment, gives consideration to the flight safety and control precision of the unmanned aerial vehicle, and is suitable for the navigation task of the unmanned aerial vehicle in a complex illumination scene.

Description

Technical Field

[0001] This invention relates to the field of UAV visual positioning, and specifically to a method for time-frequency demodulation and pose calculation of UAV visual signals for highly dynamic lighting environments. Background Technology

[0002] Current UAV visual positioning technology mainly relies on spatial domain analysis of image brightness. Conventional methods typically employ single-frame image processing techniques, such as global thresholding or edge detection, to extract the geometric center of the bright spot as a feature point, and then use the PnP (Perspective-n-Point) algorithm to calculate the pose.

[0003] Existing technologies only analyze the spatial features (such as brightness and shape) of a single frame image. In high dynamic range (HDR) environments such as backlighting of windows or metallic reflections, the brightness of background noise (such as sunlight or specular reflections) is often higher than that of the target signal, causing threshold segmentation to fail and making it impossible to distinguish between the background and the target.

[0004] Traditional PnP (Positioning and Positioning) calculations are purely geometric and mathematical processes. As long as the number of input points meets the requirements (such as 3 or 4 points), the result can be output, but it is impossible to determine whether the result is caused by outliers. Once an outlier occurs, the calculated pose will change drastically.

[0005] Existing navigation algorithms typically only output position coordinates, with the onboard flight controller providing full control. When visual positioning quality deteriorates (e.g., due to excessive distance or jitter in the calculation), the control weights cannot be dynamically adjusted, easily leading to drone oscillations. Summary of the Invention

[0006] This invention proposes a time-frequency demodulation and pose calculation method for UAV visual signals in highly dynamic lighting environments. The method includes the following steps: S1. Construct a pixel-level time-domain signal sequence, perform frequency domain transformation on the time-domain signal, calculate the spectral energy amplitude at a preset carrier frequency, generate a target feature mask based on the energy threshold, and extract a target spot with a high signal-to-noise ratio. S2. Perform PnP pose calculation based on an asymmetric geometric distribution model and calculate the reprojection error; S3. Construct the observation noise covariance matrix of the Kalman filter based on the reprojection error, and perform optimal state estimation; S4. Design a dual-mode switching function based on the confidence level of the state estimation and the distance constraint to generate loosely coupled (position correction) or tightly coupled (attitude takeover) dual-mode control commands.

[0007] In the preferred embodiment, the construction of pixel-level temporal signal sequences in step S1 differs from traditional methods that directly process single-frame images. The time window for data collection in this method Continuity within A sequence of frame images. Assume the image resolution is... For any pixel on the image plane Construct its time-domain brightness signal sequence :

[0008] in Because background light (such as sunlight) is typically constant or changes at a low frequency over a short period of time, while the drone's active light source operates at a specific carrier frequency... Modulate (e.g., 500Hz).

[0009] Perform Discrete Fourier Transform (DFT) on the signal sequence: ; Calculate the target frequency Corresponding spectral energy amplitude :

[0010] in, ; Set energy judgment threshold Generate a binarized feature mask :

[0011] Using the above formula, regardless of the background pixel brightness No matter how high (even if it reaches the saturation value of 255), as long as it does not possess frequency... Its fluctuation characteristics, its amplitude It will approach 0, thus mathematically eliminating strong light interference.

[0012] In a preferred embodiment, the PnP pose calculation in step S2 utilizes the extracted feature point pixel coordinates. Coordinates of 3D feature points pre-set on the drone body Matching is performed. The rotation matrix of the camera coordinate system relative to the world coordinate system is solved using the EPnP algorithm. Translation vector .

[0013] In a preferred embodiment, the calculation of the reprojection error in step S2 is defined as the reprojection residual (ReprojectionError). The calculated pose Substitute the camera projection model into the image plane to calculate the theoretical coordinates of the 3D points projected back onto the image plane. :

[0014] in For the camera intrinsic parameter matrix, The scale factor is used to calculate the total average reprojection error. : ; in, This represents the total number of feature points involved in pose calculation. Indicates the index number of the feature point. Indicates the first The actual observed coordinates of each feature point on the image plane Indicates the first The coordinates of each feature point are theoretically calculated by back-projecting the solved pose (rotation and axis displacement) onto the image plane. express The norm, in this context, represents the Euclidean distance (pixel distance) between the actual observation point and the theoretical projection point.

[0015] In the preferred embodiment, the optimal state estimation in step S3 involves constructing a Kalman filter, with the state vector set as follows: .

[0016] During the update phase, instead of using fixed observation noise, the residuals calculated in step S2 are utilized. Dynamically adjust the observation noise covariance matrix .

[0017] Let the basic observation noise be The adaptive adjustment function is: ; when When the value is small (the solution is accurate), The smaller the value, the more the filter trusts visual observations; when Exceeding the safety threshold (In case of mismatch) It increases exponentially, the filter refuses to update, and it mainly depends on the predicted value. Output.

[0018] In a preferred embodiment, the dual-mode switching function in step S4 is based on the distance between the UAV and the target and the reprojection residual. Its expression is : ; Mode A (Loose Coupling): Output position correction amount The data is sent to the drone flight controller via ROS messages, and then loosely integrated by the flight controller's internal EKF. ; in, P cmd For the target navigation command coordinates, P vis Provides real-time visual positioning coordinates. This is the coordinate system transformation offset.

[0019] Mode B (Tightly Coupled): Directly construct a position-velocity closed-loop PID controller to output the underlying attitude control quantity. (such as roll angle) Pitch angle ) and throttle position :

[0020] in, For attitude control output. This is the position proportional gain coefficient. For the desired position, For the current estimated position, This is the positional deviation. The velocity differential gain coefficient, For the desired speed, For the current real-time speed, This represents the speed deviation.

[0021]

[0022] in, The throttle controls the output. This is the baseline hover throttle value; the balance throttle value required for the drone to maintain constant altitude hovering at its current weight. To highly control the proportional gain coefficient, The desired altitude is the set landing point altitude or the height of the connecting passage. The current real-time height is the Z-axis coordinate measured in real-time by the vision system. This represents the height deviation.

[0023] Compared with the prior art, the beneficial effects of the present invention include: It possesses extremely strong anti-interference capabilities, utilizing the Fourier transform formula. By extracting specific frequency signals, the problem of targets being submerged in strong backlight environments is solved from a mathematical perspective, and the signal-to-noise ratio is significantly improved compared to the traditional brightness threshold method.

[0024] Improved robustness by introducing reprojection residuals The adaptive Kalman filter driven by the system can automatically identify and eliminate visual mismatch data, preventing the drone from losing control due to positioning jumps.

[0025] The control precision is adaptive. Through the dual-mode control law formula, the stability of the airborne flight control is utilized at long distances, while the high-precision closed loop of external vision is utilized at short distances, thus balancing safety and precision throughout the entire docking process. Attached Figure Description

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

[0027] Figure 1 This is an overall flowchart of the method of the present invention.

[0028] Figure 2 This is a flowchart of the image processing and mask generation stages of the method of the present invention.

[0029] Figure 3 This is a flowchart of the pose calculation and adaptive filtering stages of the method of the present invention.

[0030] Figure 4 This is a time-domain fluctuation signal diagram of the method of the present invention.

[0031] Figure 5 This is the spectral energy response diagram of the method of the present invention.

[0032] Figure 6 This is a logic diagram of the dual-mode control strategy switching method of the present invention. Detailed Implementation

[0033] To verify the effectiveness of the method described in this invention under extreme lighting conditions, data collection and analysis were conducted in a simulated drone window docking scenario. The ambient light intensity was set to [value missing]. (Simulating strong direct sunlight), the active light source (LED) on the drone is set to a specific carrier frequency. Modulate the flashing.

[0034] The present invention discloses a method for time-frequency demodulation and pose calculation of UAV visual signals in highly dynamic lighting environments, the overall flowchart of which is shown below. Figure 1 As shown, the specific steps are as follows: S1. Construct a pixel-level time-domain signal sequence, perform frequency-domain transformation on the time-domain signal, calculate the spectral energy amplitude at a preset carrier frequency, generate a target feature mask based on the energy threshold, and extract the target spot with a high signal-to-noise ratio. The process is as follows: Figure 2 As shown; The system first acquires a continuous sequence of images at a high frame rate, and then selects two typical bright pixels in the field of view, such as... Figure 4 Analysis: In this diagram, pixel A (interference point) corresponds to the area of ​​sunlight reflection on the window glass. Pixel B (target point) corresponds to the area of ​​the actively emitting tag carried by the drone.

[0035] like Figure 4 As shown, the brightness change waveform (8-bit) of these two pixels within a 1.0-second time window is displayed: Pixel A (PixelA-Interference): During this period, the brightness remained at a low level (approximately 40). At that time, affected by changes in the angle of direct sunlight or reflection, the brightness instantly jumps to the saturation zone (approximately 250) and remains thereafter. .

[0036] Pixel B (Target LED): Exhibits a distinct periodic square wave oscillation characteristic. The brightness switches rapidly between low (approximately 30) and high (approximately 200) levels, indicating that the LED is undergoing stable frequency modulation.

[0037] If the traditional spatial domain threshold segmentation method is used (e.g., setting a brightness threshold), ),exist During the specified time period, the brightness of both pixel A and pixel B far exceeds the threshold. At this time, the algorithm will be unable to distinguish between sunlight interference and drone interference, which can easily lead to false detections and incorrect pose calculations.

[0038] For the aforementioned time-domain signal, this invention utilizes pixel-level FFT (Fast Fourier Transform) to convert it into a frequency-domain signal, as shown below. Figure 5 As shown (vertical axis represents amplitude, unit dB): Pixel A Spectrum: such as Figure 5 As shown in the upper subplot, the energy of the interference signal is mainly concentrated in the 0Hz (DC component) and extremely low frequency regions. This is because sunlight reflection is usually constant or slowly changing (e.g., cloud cover, low-frequency wing rotation). As the frequency increases, its energy rapidly decays to the noise floor level (approximately -60dB to -80dB). There are no significant energy peaks at the target carrier frequency.

[0039] Pixel B Spectrum: such as Figure 5 As shown in the lower center subplot, the target signal exhibits an extremely high signal-to-noise ratio across the spectrum. At the preset carrier frequency... At approximately 30Hz in the image, a fundamental peak with an amplitude approaching 200dB appears. Simultaneously, at... Clear harmonic peaks can also be seen at odd harmonic positions (approximately 90Hz, 150Hz).

[0040] The system sets an energy detection threshold (e.g.) Figure 5 (As shown by the dashed line). For pixel A: although its temporal brightness is extremely high ( Figure 4 It reaches 250 in the middle (but in the frequency domain, it is at the characteristic frequency). The amplitude at that location is far below the threshold. Therefore, the algorithm determines pixel A to be background noise and sets its mask value to 0 (removes it). For pixel B: its characteristic frequency... The amplitude at point B is significantly higher than the threshold line. Therefore, the algorithm determines that pixel B is the real target and retains this pixel for subsequent PnP pose calculation.

[0041] S2. Perform PnP pose calculation based on an asymmetric geometric distribution model and calculate the reprojection error; Using the extracted feature point pixel coordinates Coordinates of 3D feature points pre-set on the drone body Matching is performed. The rotation matrix of the camera coordinate system relative to the world coordinate system is solved using the EPnP algorithm. Translation vector .

[0042] In a preferred embodiment, the calculation of the reprojection error in step S2 is defined as the reprojection residual (ReprojectionError). The calculated pose Substitute the camera projection model into the image plane to calculate the theoretical coordinates of the 3D points projected back onto the image plane. :

[0043] in For the camera intrinsic parameter matrix, The scale factor is used to calculate the total average reprojection error. : ; in, This represents the total number of feature points involved in pose calculation. Indicates the index number of the feature point. Indicates the first The actual observed coordinates of each feature point on the image plane Indicates the first The coordinates of each feature point are theoretically calculated by back-projecting the solved pose (rotation and axis displacement) onto the image plane. express The norm, in this context, represents the Euclidean distance (pixel distance) between the actual observation point and the theoretical projection point.

[0044] S3. Construct the observation noise covariance matrix of the Kalman filter based on the reprojection error, and perform optimal state estimation. The process is as follows: Figure 3 As shown; Construct a Kalman filter, with the state vector set as follows: .

[0045] During the update phase, instead of using fixed observation noise, the residuals calculated in step S2 are utilized. Dynamically adjust the observation noise covariance matrix .

[0046] Let the basic observation noise be The adaptive adjustment function is: ; when When the value is small (the solution is accurate), The smaller the value, the more the filter trusts visual observations; when Exceeding the safety threshold (In case of mismatch) It increases exponentially, the filter refuses to update, and it mainly depends on the predicted value. Output.

[0047] S4. Based on the confidence level of the state estimation and the distance constraint, design a dual-mode switching function to generate loosely coupled (position correction) or tightly coupled (attitude takeover) dual-mode control commands. The process is as follows: Figure 6 As shown.

[0048] Based on the distance between the UAV and the target and the reprojection residual Its expression is : ; Mode A (Loose Coupling): Output position correction amount The data is sent to the drone flight controller via ROS messages, and then loosely integrated by the flight controller's internal EKF. ; in, Pcmd For the target navigation command coordinates, P vis Provides real-time visual positioning coordinates. This is the coordinate system transformation offset.

[0049] Mode B (Tightly Coupled): Directly construct a position-velocity closed-loop PID controller to output the underlying attitude control quantity. (such as roll angle) Pitch angle ) and throttle position :

[0050] in, For attitude control output. This is the position proportional gain coefficient. For the desired position, For the current estimated position, This is the positional deviation. The velocity differential gain coefficient, For the desired speed, For the current real-time speed, This represents the speed deviation.

[0051]

[0052] in, The throttle controls the output. This is the baseline hover throttle value; the balance throttle value required for the drone to maintain constant altitude hovering at its current weight. To highly control the proportional gain coefficient, The desired altitude is the set landing point altitude or the height of the connecting passage. The current real-time height is the Z-axis coordinate measured in real-time by the vision system. This represents the height deviation.

Claims

1. A method for time-frequency demodulation and pose calculation of UAV visual signals in highly dynamic lighting environments, characterized in that, Includes the following steps: S1. Acquire high frame rate image sequences of the UAV, construct pixel-level time-domain signal sequences, perform frequency domain transformation on the time-domain signals, calculate the spectral energy amplitude at a preset carrier frequency, and generate a target feature mask based on the energy threshold; S2. Perform PnP pose calculation based on an asymmetric geometric distribution model and calculate the reprojection error; S3. Construct the observation noise covariance matrix of the Kalman filter based on the reprojection error, and perform optimal state estimation; S4. Design a dual-mode switching function based on the confidence level of the state estimation and the distance constraint to generate loosely coupled or tightly coupled dual-mode control commands.

2. The method for time-frequency demodulation and pose calculation of UAV visual signals in highly dynamic lighting environments according to claim 1, characterized in that, The construction of pixel-level temporal signal sequences in step S1 differs from traditional methods that directly process single-frame images. The time window for data collection in this method Continuity within A sequence of frames, with an image resolution of 1. For any pixel point on the image plane Construct its time-domain brightness signal sequence : ； in Since the background light is usually constant or changes at a low frequency over a short period of time, while the drone's active light source operates at a specific carrier frequency... Modulation; Perform Discrete Fourier Transform (DFT) on the signal sequence: ； Calculate the target frequency Corresponding spectral energy amplitude : ； in, ; Set energy judgment threshold Generate a binarized feature mask : ； Using the above formula, regardless of the background pixel brightness How high is it, as long as it doesn't have a frequency? Its fluctuation characteristics, its amplitude It will approach 0, thus mathematically eliminating strong light interference.

3. The method for time-frequency demodulation and pose calculation of UAV visual signals for highly dynamic lighting environments according to claim 1, characterized in that, The method for calculating the PnP pose in step S2 is to use the extracted feature point pixel coordinates. Coordinates of 3D feature points pre-set on the drone body The matching is performed, and the rotation matrix of the camera coordinate system relative to the world coordinate system is solved using the EPnP algorithm. R Translation vector .

4. The method for time-frequency demodulation and pose calculation of UAV visual signals for highly dynamic lighting environments according to claim 1, characterized in that, The calculation of reprojection error in step S2 defines the reprojection residual. The calculated pose Substitute the camera projection model into the image plane to calculate the theoretical coordinates of the 3D points projected back onto the image plane. : ； in For the camera intrinsic parameter matrix, Using the scale factor, calculate the total average reprojection error. : ； in, This represents the total number of feature points involved in pose calculation. Indicates the index number of the feature point. Indicates the first The actual observed coordinates of each feature point on the image plane Indicates the first The coordinates of each feature point are theoretically calculated by back-projecting the solved pose (rotation and axis displacement) onto the image plane. express The norm, in this context, represents the Euclidean distance (pixel distance) between the actual observation point and the theoretical projection point.

5. The method for time-frequency demodulation and pose calculation of UAV visual signals for highly dynamic lighting environments according to claim 1, characterized in that, The optimal state estimation in step S3 involves constructing a Kalman filter, with the state vector set as follows: Let the basic observation noise be... The adaptive adjustment function is: ； when When the value is small (the solution is accurate), The smaller the value, the more the filter trusts visual observations; when Exceeding the safety threshold (In case of mismatch) It increases exponentially, the filter refuses to update, and it mainly depends on the predicted value. Output.

6. The method for time-frequency demodulation and pose calculation of UAV visual signals for highly dynamic lighting environments according to claim 5, characterized in that, When constructing the Kalman filter, instead of using fixed observation noise, the residual calculated in step S2 is used. Dynamically adjust the observation noise covariance matrix .

7. The method for time-frequency demodulation and pose calculation of UAV visual signals for highly dynamic lighting environments according to claim 1, characterized in that, The dual-mode switching function in step S4 is based on the distance between the UAV and the target and the reprojection residual. Its expression is : ； If the distance between the drone and the target is greater than the safe distance or the reprojection residual is greater than the set threshold, the loosely coupled mode is used; if the distance between the drone and the target is less than the safe distance and the reprojection residual is less than the set threshold, the tightly coupled mode is used.

8. The method for time-frequency demodulation and pose calculation of UAV visual signals for highly dynamic lighting environments according to claim 7, characterized in that, The loosely coupled mode is the output position correction amount. The data is sent to the drone flight controller via ROS messages, and then loosely integrated by the flight controller's internal EKF. ； in, P cmd For the target navigation command coordinates, P vis Provides real-time visual positioning coordinates. This is the coordinate system transformation offset.

9. A method for time-frequency demodulation and pose calculation of UAV visual signals for highly dynamic lighting environments according to claim 7, characterized in that, The tightly coupled mode directly constructs a position-velocity closed-loop PID controller, outputting the underlying attitude control quantity. (such as roll angle) Pitch angle ) and throttle position : ； in, For attitude control output. This is the position proportional gain coefficient. For the desired position, For the current estimated position, This is the positional deviation. The velocity differential gain coefficient, For the desired speed, For the current real-time speed, This represents the speed deviation.

10. A method for time-frequency demodulation and pose calculation of UAV visual signals for highly dynamic lighting environments according to claim 9, characterized in that, The throttle amount The expression is: ； in, The throttle controls the output. This is the baseline hover throttle value; the balance throttle value required for the drone to maintain constant altitude hovering at its current weight. To highly control the proportional gain coefficient, The desired altitude is the set landing point altitude or the height of the connecting passage. The current real-time height is the Z-axis coordinate measured in real-time by the vision system. This represents the height deviation.

Images