Multi-rotor aircraft system based on wireless direction finding and guiding control method

Abstract

The invention discloses a multi-rotor aircraft system based on wireless direction finding and a guidance control method, and the aircraft system comprises a satellite integrated navigation module, a visual processing module, a wireless direction finding module and a flight controller module. The system can identify the target orientation in the radiation source interference environment to provide software and hardware support for the abutting flight task of the multi-rotor aircraft. The guidance control method based on the system comprises four steps of autonomous takeoff, fault-tolerant navigation processing, wireless direction-finding detection and abutting-in control, a visual processing module in a fault-tolerant navigation algorithm can output information such as real-time positioning and speed to a flight controller, and pose information of the unmanned aerial vehicle is solved in real time; navigation positioning data can be provided for the unmanned aerial vehicle without satellite positioning signals, and the reliability of flight operation is guaranteed; therefore, the unmanned aerial vehicle finds and comes into the vicinity of the wireless interference device so as to find the interference source, the failure of the flight task of the unmanned aerial vehicle caused by the interference source is avoided, and the safety is better guaranteed.

Description

Technical Field

[0001] The present invention relates to a UAV guidance and control technology, in particular to a multi-rotor aircraft system and a guidance and control method based on wireless direction finding, and belongs to the technical field of multi-rotor aircraft autonomous flight control. Background Art

[0002] With the rapid development of science and technology, drones are widely used in military and civilian fields. At the same time, drone countermeasures and technologies have also been developed accordingly. These technologies include electromagnetic interference, laser systems, and radio frequency interference, etc., with the purpose of causing drones to lose control or land. The most typical device is a jammer, which uses high-power equipment to send interference signals to flying drones in a directional manner, blocking the satellite positioning of drones and the communication between drones and ground stations, forcing drones to land or even crash. At present, the purchase of countermeasures is not restricted. If a legally flying drone is persecuted by criminals using countermeasures, it will not only lead to the failure of the drone's flight mission, but may also cause certain economic losses and personal injuries.

[0003] In the prior art, UAV anti-interference technologies include frequency hopping communication, spread spectrum communication, anti-interference antennas, encrypted communication, and intelligent identification and interference suppression. These technologies can improve the anti-interference capability of UAV systems and ensure their stable operation in complex environments, which is of great significance for avoiding wireless countermeasures. The invention patent with application number 202410148369.0 discloses a self-learning anti-interference control method for multi-rotor aircraft in a dynamic disturbance environment. It combines the feedforward PID control method with meta-learning and adaptive control, and can utilize the information accumulated during the previous flight process. It can adapt faster when the disturbance conditions continue to change, improve the generalization ability of the flight system for new tasks, and thus improve the stability and anti-interference ability of the aircraft under unknown environmental disturbances.

[0004] In actual application, the difficulty of anti-interference is that it is difficult to identify and find the position of the wireless countermeasure device in time. Therefore, how to make a multi-rotor aircraft fly in a wireless interference environment and find the wireless countermeasure device is a technical problem that needs to be solved urgently in the prior art. Summary of the invention

[0005] In order to overcome the deficiencies in the prior art, the purpose of the present invention is to provide a multi-rotor aircraft system and a guidance and control method based on wireless direction finding, so that the multi-rotor aircraft can detect and approach the wireless countermeasure device in a wireless interference environment, thereby ensuring the safe flight of the UAV.

[0006] In order to achieve the above object, the present invention adopts the following technical solution:

[0007] The present invention first discloses a multi-rotor aircraft system based on wireless direction finding, comprising:

[0008] The satellite integrated navigation module fuses the collected UAV flight data and outputs the position, speed and angle data to the flight controller module;

[0009] The visual processing module performs image algorithm calculations on the collected real-time images and outputs real-time posture data to the flight controller;

[0010] The wireless direction finding module receives the target signal through the direction finding antenna, and after being processed by the direction finding host, outputs the detection status and azimuth data of the target signal to the flight controller;

[0011] The flight controller module is connected to the satellite integrated navigation module, the visual processing module and the wireless direction finding module through the RS422 serial port to calculate the desired rotational torque of the UAV to control the UAV to move near the wireless jammer.

[0012] Preferably, the aforementioned satellite combined navigation module is composed of an inertial sensor unit, a positioning unit, a magnetometer unit and a barometer unit; the visual processing module is composed of an airborne computing unit and a camera unit; and the wireless direction finding module is composed of a wireless direction finding host and a direction finding antenna.

[0013] More preferably, the aforementioned UAV flight data includes: angular rate, acceleration, position, speed, magnetic heading and barometric altitude.

[0014] More preferably, the aforementioned camera unit is a wide-angle fixed-focus monocular camera.

[0015] The present invention also discloses a multi-rotor aircraft guidance and control method based on wireless direction finding, which adopts the above-mentioned aircraft system and comprises the following steps:

[0016] S1. Autonomous takeoff

[0017] The ground station manually binds or automatically binds the UAV's three-dimensional position Pn, Pe, H and three-axis attitude φ, θ, ψ to the visual processing module after the satellite signal is stable through the flight controller, completing the initial binding of the visual processing module and realizing autonomous takeoff;

[0018] S2. Fault-tolerant navigation processing

[0019] When the satellite signal that the drone relies on is interfered with, the fault-tolerant navigation processing will switch the satellite navigation algorithm to the visual navigation algorithm. The visual navigation algorithm is modularized according to its functions, including: measurement processing module, initialization module, visual inertial sensor fusion module, and global optimization module;

[0020] S3, wireless direction finding detection

[0021] The target of the radiation source is found by global segmented search method:

[0022] 3.1 Based on the current heading angle ψ of the drone, hover and wait for 10 seconds;

[0023] 3.2 If the wireless direction finding device detects the radiation source target, the target heading angle data ψ 目标 The desired heading angle ψ released to the approach control phase d2 ;

[0024] 3.3 If no radiation source target is detected, the target value of the drone's yaw is controlled to ψ+90, and then it hovers and waits for 10 seconds to search for the target radiation source;

[0025] 3.4 If the wireless direction finding device detects the radiation source target, the target heading angle data ψ 目标 The desired heading angle ψ released to the approach control phase d2 ;

[0026] 3.5 If no radiation source target is detected, it means that there is no radiation source target within 3km of the location, and the drone performs the return mission;

[0027] S4, Advance Control

[0028] 4.1 Expected heading angle ψ obtained from wireless detection d2 Calculate the expected northward velocity Vn d and the eastward velocity Ve d , the method to obtain is as follows:

[0029] Ve d =V 总 sin(ψ d2 )

[0030] Vn d =V 总 cos(ψ d2 )

[0031] Where V 总 Approach speed parameters set for the ground station;

[0032] 4.2 Setting the target height H during the approach control phase d2 is the current height of the drone, H d2 =H;

[0033] 4.3 Using feedback control to execute the desired Vn d 、Ve d , H d2 , d2 Target value, making the drone approach the radiation source;

[0034] 4.4 During the approach, if the current radiation source target ψ is found d2 If lost, the drone is controlled to perform the hovering waiting task and continue to perform the wireless detection task;

[0035] 4.5 If the radiation source target is detected in the above step 4.4, continue to execute step 4.1 until the target position is visible through the camera module on the UAV platform, and then the approach control flight mission is terminated.

[0036] Preferably, in the aforementioned step S1, when the following conditions are met, the hovering waiting enters the wireless direction finding detection phase:

[0037] Pn d -Pn≤0.2

[0038] P d -Pe≤0.2

[0039] H d -H≤0.1

[0040] Among them, Pn d 、Pe d , H d are the expected north position, east position and target height of the UAV respectively, and Pn, Pe and H are the real-time three-dimensional positions of the UAV respectively.

[0041] Preferably, the working process of the aforementioned measurement processing module is as follows:

[0042] (1) Output of the gyroscope and accelerometer at a certain moment By integrating, we can get the posture q at a certain moment t , speed v t , position p t , where the gyroscope outputs the angular velocity Contains actual angular velocity w, bias b w With noise n w , expressed as:

[0043] For the attitude quaternion q at a certain moment t It can be expressed as:

[0044]

[0045] in Represents quaternion multiplication, w contains the three-axis angular velocity (w x ,w y ,w z ),Ω represents

[0046]

[0047] Acceleration output by the accelerometer Contains motion acceleration a, bias b a , gravitational acceleration g and noise n a , expressed as:

[0048] For a certain moment, the velocity v t It can be expressed as:

[0049]

[0050] The position p at a certain time t It can be expressed as:

[0051]

[0052] (2) For each new image frame obtained by the camera, the minimum pixel interval between adjacent feature points is first set to ensure that the feature points are evenly distributed; then the sparse optical flow algorithm is used to track the existing features, and then the basic matrix model and the random sample consistency algorithm are used to verify the tracking results and eliminate erroneous feature matches; finally, it is determined whether the image is a key frame.

[0053] More preferably, the aforementioned initialization module includes the following processing steps:

[0054] (1) Gyroscope bias calibration

[0055] Minimize the following cost function to get the gyroscope bias b w Initial calibration:

[0056]

[0057] δb w =b w Δt k

[0058] Among them, B represents all frames in the window, Represents the angular velocity bias b w Regarding the Jacobian matrix of constraint γ, Δt k is the time interval between two adjacent frames;

[0059] (2) Solve for velocity, gravity vector, and scale parameters

[0060] From pure vision Then solve the linear least squares problem:

[0061]

[0062] Get the velocity in the object coordinate system of each frame in the window Gravity vector in the camera reference frame and the scale parameter s;

[0063] Among them, the linear measurement model:

[0064]

[0065] I is the identity matrix, For noise.

[0066] Further preferably, the visual inertial sensor fusion module comprises the following processing steps:

[0067] (1) State estimation: A tightly coupled monocular visual odometry based on a sliding window is used for state estimation.

[0068] First, the monocular visual inertial bundle method is used to solve the state quantity. The complete state quantity is expressed as:

[0069] X=[x0,x1,...,x n ,x c ,y0,y1,...,y m ]

[0070] x k =[p k ,v k ,q k ,b a ,b w ],k∈[0,n]

[0071] x c =[p c ,q c ]

[0072] The state vector X includes the states x of all cameras in the sliding window. k , i.e. position p k , rotate q k , speed v k , accelerometer bias b a and gyro bias b w 、External parameters x from camera to inertial sensor c , the inverse depth y of the observed feature point m ;

[0073] Then minimize the sum of the prior and norm of all measurement residuals to obtain the maximum a posteriori estimate:

[0074]

[0075] Among them, z b represents the inertial observation, z lrepresents the visual observation, B is the set of all inertial measurement unit measurements, C is the set of features observed at least twice in the current sliding window, and r B (z b , X) and r C (z l , X) are the residuals of the inertial sensor and the visual measurement, r p , H p is the prior information, and then the maximum a posteriori estimate is nonlinearly optimized to obtain x k ;

[0076] (2) Fault detection and recovery

[0077] Check whether there is a fault. If there is a fault, the system will isolate the abnormal data, directly update the position of the visual inertial odometer through the latest measurement of the inertial measurement unit, and wait for the fault to recover. If the fault is not recovered within 5 seconds, the system will switch back to the initialization state and wait for the system to complete initialization again.

[0078] (3) Relocation

[0079] Add the loop frame to the current sliding window, use the pose of the loop frame as a constant, modify the nonlinear cost function, and add the loop term:

[0080]

[0081] Where L is the observation set of features detected in the loop closure frame, (l, v) refers to the lth feature observed in the loop closure frame v, and q v is the pose of the loop frame v, p v is the position of the loop frame v; if there are multiple loops in the current sliding window, the system will use all loop features from all frames for optimization.

[0082] More preferably, the processing steps of the aforementioned global optimization module are:

[0083] The direct residual of frame i and j is defined as

[0084]

[0085] represents the yaw angle, p represents the position, ^ represents the prior, R represents the rotation matrix, They are the estimates of the roll and pitch angles obtained directly from the monocular visual odometry;

[0086] The entire pose graph is optimized by minimizing the following cost function:

[0087]

[0088] Where S is a set of previous key frames adjacent to the current key frame, and L is a set of loop frames associated with the current key frame.

[0089] The present invention is beneficial in that:

[0090] (1) In the prior art, most multi-rotor aircraft use satellite navigation for navigation, and can only fly automatically for a short time or even not at all in an interference environment. The present invention uses visual navigation combined with IMU data fusion to provide navigation and positioning data for UAVs in the absence of satellite positioning signals, thereby improving the safety and reliability of flight missions.

[0091] (2) The present invention proposes a guidance and control method for discovering a wireless jammer, which enables a UAV to search for and approach the vicinity of a wireless jammer. A wireless direction-finding device is used to detect the relative position of the wireless jammer. The UAV introduces heading and speed control to approach the vicinity of the wireless jammer, which can be used to counter anti-UAV measures.

[0092] (3) The visual processing module in the fault-tolerant navigation algorithm can output real-time positioning and speed information to the flight controller, and solve the UAV's posture information in real time, so that navigation and positioning data can be provided to the UAV in the absence of satellite positioning signals, further ensuring the reliability of flight operations. BRIEF DESCRIPTION OF THE DRAWINGS

[0093] Figure 1 A schematic diagram of the framework structure of a multi-rotor aircraft system based on wireless direction finding according to the present invention;

[0094] Figure 2 An information flow diagram of a multi-rotor aircraft system based on wireless direction finding according to the present invention;

[0095] Figure 3 is a flow chart of the visual processing module of the present invention;

[0096] Figure 4 The present invention is a guidance and control flow chart of a multi-rotor aircraft system based on wireless direction finding. DETAILED DESCRIPTION

[0097] The present invention is described in detail below with reference to the accompanying drawings and specific embodiments.

[0098] Example 1

[0099] See also Figure 1 and Figure 2This embodiment discloses a multi-rotor aircraft system based on wireless direction finding, including: a satellite integrated navigation module, a wireless direction finding module, a visual processing module and a flight controller module. The satellite integrated navigation module is composed of an inertial sensor unit, a positioning unit, a magnetometer unit and a barometer unit; the wireless direction finding module is composed of a wireless direction finding antenna and a wireless direction finding host; and the visual processing module is composed of an airborne computing unit and a camera unit. The satellite integrated navigation module, the visual processing module and the wireless direction finding device are respectively connected to the flight controller through the RS422 serial port to realize data / signal transmission and feedback.

[0100] During operation, the satellite integrated navigation module fuses the angular rate, acceleration, position, speed, magnetic heading and barometric altitude data measured and collected by the inertial sensor unit, positioning unit, magnetometer unit and barometer unit respectively, and outputs the position, speed, angle and other data to the flight controller.

[0101] The camera unit in the visual processing module preferably adopts a wide-angle fixed-focus monocular camera. The onboard computing unit performs image algorithm operations on the collected real-time pictures, and can output real-time posture data to the flight controller through the initial posture data bound by the flight controller.

[0102] The wireless direction-finding module receives the target signal through the direction-finding antenna. After being processed by the direction-finding host, it outputs the detection status and azimuth data of the target signal to the flight controller. The flight controller calculates the desired torque of the UAV based on the speed error and azimuth error to control the UAV to move near the wireless jammer.

[0103] Example 2

[0104] This embodiment discloses a multi-rotor aircraft guidance and control method based on wireless direction finding, which can be divided into four main stages: autonomous takeoff, fault-tolerant navigation processing, wireless direction finding detection, and approach control. Figure 3 As shown, the specific steps are as follows:

[0105] S1. Autonomous takeoff

[0106] The ground station can manually bind or the flight controller can automatically bind the UAV's three-dimensional position Pn, Pe, H and three-axis attitude φ, θ, ψ to the visual processing module after the satellite signal is stable, completing the initial binding of the visual processing module.

[0107] The ground station inputs the target three-dimensional position Pn d 、Pe d , H d , are the desired north position, east position and target height of the UAV, and the desired heading angle is ψ d. Detect the real-time three-dimensional position Pn, Pe, H and current heading angle ψ of the drone. When the following conditions are met, the drone will hover and wait to enter the wireless direction finding detection phase.

[0108] Pn d -Pn≤0.2

[0109] P d -Pe≤0.2

[0110] H d -H≤0.1

[0111] S2. Fault-tolerant navigation processing

[0112] During the entire autonomous flight mission, when the satellite signal that the drone relies on is interfered with, the fault-tolerant navigation processing will switch the satellite navigation algorithm to the visual navigation algorithm to ensure that the drone can still efficiently and safely perform the radiation source target detection flight mission in an environment without satellite signals. The visual navigation algorithm is modularized according to its function, including: measurement processing module, initialization module, visual inertial sensor fusion module, global optimization module, such as Figure 4 As shown, the detailed steps are as follows:

[0113] 1. Measurement and processing module:

[0114] (1) Output of the gyroscope and accelerometer at a certain moment By integrating, we can get the posture q at a certain moment t , speed v t , position p t The gyroscope outputs the angular velocity Contains actual angular velocity w, bias b w With noise n w , which can be expressed as:

[0115] For the attitude quaternion q at a certain moment t It can be expressed as:

[0116]

[0117] in Represents quaternion multiplication, w contains the three-axis angular velocity (w x ,w y ,w z ),Ω represents

[0118]

[0119] Acceleration output by the accelerometer Contains motion acceleration a, bias b a , gravitational acceleration g and noise na , which can be expressed as:

[0120] For a certain moment, the velocity v t It can be expressed as:

[0121]

[0122] The position p at a certain time t It can be expressed as:

[0123]

[0124] (2) For each new image frame obtained by the camera, the minimum pixel interval between adjacent feature points is first set to ensure that the feature points are evenly distributed; then the sparse optical flow algorithm is used to track the existing features, and then the basic matrix model and the random sample consistency algorithm are used to verify the tracking results and eliminate erroneous feature matches; finally, it is determined whether the image is a key frame.

[0125] There are two criteria for selecting keyframes. The first is that if the average disparity of the feature points tracked by the previous keyframe exceeds a certain threshold of 10, the frame is considered a new keyframe. The second is that if the number of tracked feature points is lower than a certain threshold of 100, the frame is considered a new keyframe to prevent the tracked features from being completely lost.

[0126] 2. Initialization module:

[0127] (1) Gyroscope bias calibration:

[0128] First, use pure vision to get the positions of all frames in the sliding window in the visual reference coordinate system attitude and the 3D positions of all landmarks Then use the inertial navigation pre-integration to get two consecutive frames b k and b k+1 The relative constraints between Minimize the following cost function to get the gyroscope bias b w Initial calibration:

[0129]

[0130] δb w =b w Δt k

[0131] Where B represents all frames in the window. Represents the angular velocity bias b w Regarding the Jacobian matrix of constraint γ, Δt k is the time interval between two adjacent frames.

[0132] (2) Solve for velocity, gravity vector, and scale parameters:

[0133] The speed change and position change between two consecutive frames in the object coordinate system can be expressed as:

[0134]

[0135] Represents the rotation matrix of the visual reference coordinate system in the object coordinate system, Represents the rotation matrix of the object coordinate system in the visual reference coordinate system, Represents the gravitational acceleration in the visual reference coordinate system, Indicates that the object is at b k Frame rate.

[0136] Arranged into a linear measurement model, we can get:

[0137]

[0138] Where s is the scale parameter, I is the identity matrix, For noise.

[0139] So first of all, from pure vision Then solve the linear least squares problem:

[0140]

[0141] The speed of the object coordinate system in each frame in the window can be obtained Gravity vector in the camera reference frame and the scale parameter s.

[0142] 3. Visual inertial sensor fusion module:

[0143] (1) State Estimation

[0144] After the estimator is initialized, a tightly coupled monocular visual odometry based on sliding window is used for state estimation.

[0145] First, the monocular visual inertial bundle method is used to solve the state quantity. The state vector X includes the state x of all cameras in the sliding window. k (including position p k , rotate q k , speed v k , accelerometer bias b a and gyro bias b w ), the external parameters x from the camera to the inertial sensor c (Including camera installation position c , installation angle qc ), the inverse depth y of the observed feature point m , the complete state quantity can be expressed as:

[0146] X=[x0,x1,...,x n , x c , y0, y1, ..., y m ]

[0147] x k =[p k , v k ,q k , b a , b w ],k∈[0,n]

[0148] x c =[p c ,q c ]

[0149] Then minimize the sum of the prior and norm of all measurement residuals to obtain the maximum a posteriori estimate:

[0150]

[0151] z b represents the inertial observation quantity, Z l represents the visual observation. B is the set of all inertial measurement unit measurements, and C is the set of features that are observed at least twice in the current sliding window. B (z b , X) and r C (z l , X) are the residuals of the inertial sensor and the visual measurement, r p , H p is the prior information. Then the maximum a posteriori estimate is nonlinearly optimized to obtain x k .

[0152] (2) Fault detection and recovery

[0153] The fault judgment criteria are as follows:

[0154] 1) The number of features in the latest frame is less than a certain threshold of 56;

[0155] 2) There is a large discontinuity between the two most recent estimated positions or rotations.

[0156] If a fault is detected, the system will isolate the abnormal data, directly update the position of the visual inertial odometer through the latest measurement of the inertial measurement unit, and wait for the fault to recover. If the fault is not recovered within 5 seconds, the system will switch back to the initialization state and wait for the system to be re-initialized.

[0157] (3) Relocation

[0158] After determining that the current image is a keyframe, it is matched with the saved keyframe. When the match is successful and the loop is detected, the basic matrix between the current image and the loop candidate image, as well as the three-dimensional position of the feature in the local sliding window and the two-dimensional observation in the loop candidate image, is used for matching verification. If the match exceeds a certain threshold of 70%, we consider the candidate frame to be a correct loop and perform relocalization. During the relocalization process, we add the loop frame to the current sliding window and use the pose of the loop frame as a constant. Therefore, the nonlinear cost function is slightly modified here to add a loop term:

[0159]

[0160] Where L is the observation set of features detected in the loop closure frame, (l, v) refers to the lth feature observed in the loop closure frame v, and q v is the pose of the loop frame v, p v is the position of the loop frame v. If there are multiple loops in the current sliding window, the system will use all loop features from all frames for optimization.

[0161] 4. Global optimization module:

[0162] The direct residual of frame i and j is defined as

[0163]

[0164] represents the yaw angle, p represents the position, ^ represents the prior, and R represents the rotation matrix. It is an estimate of the roll and pitch angles obtained directly from the monocular visual odometry.

[0165] The entire pose graph is optimized by minimizing the following cost function:

[0166]

[0167] Where S is a set of previous key frames adjacent to the current key frame, and L is a set of loop frames associated with the current key frame.

[0168] S3, wireless direction finding detection:

[0169] based on Figure 1 The target heading angle data ψ obtained by the wireless direction finding device 目标 Get the desired heading angle ψ for approach control d2 Specifically, the radiation source is searched by global segmentation in sequence, namely:

[0170] (1) Based on the current heading angle ψ of the UAV, hover and wait for 10 seconds;

[0171] (2) If the wireless direction finding device detects the radiation source target, the target heading angle data ψ 目标 The desired heading angle ψ released to the approach control phase d2 ;

[0172] (3) If no radiation source target is detected, the target value of the drone’s yaw is controlled to be ψ+90, and then the drone hovers and waits for 10 seconds to search for the target radiation source;

[0173] (4) If the wireless direction finding device detects the radiation source target, the target heading angle data ψ 目标 The desired heading angle ψ released to the approach control phase d2 ;

[0174] (5) If no radiation source target is detected, it means that there is no radiation source target within 3 km of the location, and the UAV performs the return mission.

[0175] S4, Advance Control

[0176] (1) The expected heading angle ψ obtained from wireless detection d2 Calculate the expected northward velocity Vn d and the eastward velocity Ve d , the method to obtain is as follows:

[0177] Ve d =V 总 sin(ψ d2 )

[0178] Vn d =V 总 cos(ψ d2 )

[0179] Where V 总 Approach speed parameters set for the ground station.

[0180] (2) Setting the target height H during the approach control phase d2 is the current height of the drone, H d2 =H.

[0181] (3) Using feedback control to execute the desired Vn d 、Ve d , H d2 , d2 Target value, making the drone approach the radiation source.

[0182] (4) During the approach, if the current radiation source target ψ is found d2If lost, the drone is controlled to perform the hovering waiting mission and continue to perform the wireless detection mission.

[0183] (5) If the radiation source target is detected in the above step (4), continue to execute step (1) until the target position is visible through the camera module on the UAV platform, and then end the approach control flight mission.

[0184] In summary, the multi-rotor aircraft system based on wireless direction finding of the present invention can identify the target orientation in an environment with radiation source interference, and provides software and hardware support for the multi-rotor aircraft's approach flight mission. The visual processing module in the fault-tolerant navigation algorithm based on this system can output real-time positioning and speed information to the flight controller, and calculate the position information of the drone in real time. It can provide navigation and positioning data for the drone in the absence of satellite positioning signals, thereby ensuring the reliability of flight operations. At the same time, the guidance and control method based on this system can enable the drone to explore and approach the vicinity of the wireless jammer, thereby discovering the interference source and avoiding the failure of the drone's flight mission caused by the interference source, so that flight safety is better guaranteed.

[0185] The above shows and describes the basic principles, main features and advantages of the present invention. Those skilled in the art should understand that the above embodiments do not limit the present invention in any form, and any technical solution obtained by equivalent replacement or equivalent transformation falls within the protection scope of the present invention.

Claims

1. A multi-rotor aircraft system based on wireless direction finding, characterized in that: include: The satellite integrated navigation module fuses the collected UAV flight data and outputs the position, speed and angle data to the flight controller module; The visual processing module performs image algorithm calculations on the collected real-time images and outputs real-time posture data to the flight controller. The wireless direction finding module receives the target signal through the direction finding antenna, and after being processed by the direction finding host, it outputs the detection status and azimuth angle data of the target signal to the flight controller. The flight controller module is connected to the satellite integrated navigation module, the visual processing module and the wireless direction finding module through the RS422 serial port to calculate the desired rotational torque of the UAV to control the UAV to move near the wireless jammer.

2. A multi-rotor aircraft system based on wireless direction finding according to claim 1, characterized in that: The satellite integrated navigation module is composed of an inertial sensor unit, a positioning unit, a magnetometer unit and a barometer unit; the visual processing module is composed of an airborne computing unit and a camera unit; and the wireless direction finding module is composed of a wireless direction finding host and a direction finding antenna.

3. A multi-rotor aircraft system based on wireless direction finding according to claim 1, characterized in that: The UAV flight data includes: angular rate, acceleration, position, speed, magnetic heading and barometric altitude.

4. A multi-rotor aircraft system based on wireless direction finding according to claim 2, characterized in that: The camera unit is a wide-angle fixed-focus monocular camera.

5. A multi-rotor aircraft guidance and control method based on wireless direction finding, characterized in that: The aircraft system according to any one of claims 1 to 4 comprises the following steps: S1. Autonomous takeoff The ground station manually binds or automatically binds the UAV's three-dimensional position Pn, Pe, H and three-axis attitude φ, θ, ψ to the visual processing module after the satellite signal is stable through the flight controller, completing the initial binding of the visual processing module and realizing autonomous takeoff; S2. Fault-tolerant navigation processing When the satellite signal that the drone relies on is interfered with, the fault-tolerant navigation processing will switch the satellite navigation algorithm to the visual navigation algorithm. The visual navigation algorithm is modularized according to its functions, including: measurement processing module, initialization module, visual inertial sensor fusion module, and global optimization module; S3, wireless direction finding detection The target of the radiation source is found by global segmented search method: 3.1 Based on the current heading angle ψ of the drone, hover and wait for 10 seconds; 3.2 If the wireless direction finding device detects the radiation source target, the target heading angle data ψ 目标 The desired heading angle ψ released to the approach control phase d2 ; 3.3 If no radiation source target is detected, the target value of the drone's yaw is controlled to ψ+90, and then it hovers and waits for 10 seconds to search for the target radiation source; 3.4 If the wireless direction finding device detects the radiation source target, the target heading angle data ψ 目标 The desired heading angle ψ released to the approach control phase d2 ; 3.5 If no radiation source target is detected, it means that there is no radiation source target within 3km of the location, and the drone performs the return mission; S4, Advance Control 4.1 Expected heading angle ψ obtained from wireless detection d2 Calculate the expected northward velocity Vn d and the eastward velocity Ve d , the method to obtain is as follows: And d =V 总 sin(ψ d2 ) Vn d =V 总 cos(ψ d2 ) Where V 总 Approach speed parameters set for the ground station; 4.2 Setting the target height H during the approach control phase d2 is the current height of the drone, that is, H d2 =H; 4.3 Using feedback control to execute the desired Vn d 、Ve d , H d2 , d2 Target value, making the drone approach the radiation source; 4.4 During the approach, if the current radiation source target ψ is found d2 If lost, the drone is controlled to perform the hovering waiting task and continue to perform the wireless detection task; 4.5 If the radiation source target is detected in the above step 4.4, continue to execute step 4.1 until the target position is visible through the camera module on the UAV platform, and then the approach control flight mission is terminated.

6. A multi-rotor aircraft guidance and control method based on wireless direction finding according to claim 5, characterized in that: In step S1, when the following conditions are met, the system hovers and waits to enter the wireless direction finding detection phase: Pn d -Pn≤0.2 On d -Pe≤0.2 H d -H≤0.1 Among them, Pn d 、Pe d , H d are the expected north position, east position and target height of the UAV respectively, and Pn, Pe and H are the real-time three-dimensional positions of the UAV respectively.

7. A multi-rotor aircraft guidance and control method based on wireless direction finding according to claim 5, characterized in that: The working process of the measurement processing module is as follows: (1) Output of the gyroscope and accelerometer at a certain moment By integrating, we can get the posture q at a certain moment t , speed v t , position p t , where the gyroscope outputs the angular velocity Contains actual angular velocity w, bias b w With noise n w , expressed as: For the attitude quaternion q at a certain moment t It can be expressed as: in Represents quaternion multiplication, w contains the three-axis angular velocity (w x ,w y ,w z ),Ω represents Acceleration output by the accelerometer Contains motion acceleration a, bias b a , gravitational acceleration g and noise n a , expressed as: For a certain moment, the velocity v t It can be expressed as: The position p at a certain time t It can be expressed as: (2) For each new image frame obtained by the camera, the minimum pixel interval between adjacent feature points is first set to ensure that the feature points are evenly distributed; then the sparse optical flow algorithm is used to track the existing features, and then the basic matrix model and the random sample consistency algorithm are used to verify the tracking results and eliminate erroneous feature matches; finally, it is determined whether the image is a key frame.

8. The multi-rotor aircraft guidance and control method based on wireless direction finding according to claim 5, characterized in that: The initialization module includes the following processing steps: (1) Gyroscope bias calibration Minimize the following cost function to get the gyroscope bias b w Initial calibration: δb w =b w Δt k Among them, B represents all frames in the window, Represents the angular velocity bias b w Regarding the Jacobian matrix of constraint γ, Δt k is the time interval between two adjacent frames; (2) Solve for velocity, gravity vector, and scale parameters From pure vision Then solve the linear least squares problem: Get the velocity in the object coordinate system of each frame in the window Gravity vector in the camera reference frame and the scale parameter s; Among them, the linear measurement model: I is the identity matrix, For noise.

9. The multi-rotor aircraft guidance and control method based on wireless direction finding according to claim 5, characterized in that: The visual inertial sensor fusion module includes the following processing steps: (1) State estimation: A tightly coupled monocular visual odometry based on a sliding window is used for state estimation. First, the monocular visual inertial bundle method is used to solve the state quantity. The complete state quantity is expressed as: X=[x0,x1,...,x n ,x c ,y0,y1,...,y m ] x k =[p k ,v k ,q k ,b a ,b w ],k∈[0,n] x c =[p c ,q c ] The state vector X includes the states x of all cameras in the sliding window. k , i.e. position p k , rotate q k , speed v k , accelerometer bias b a and gyro bias b w 、External parameters x from camera to inertial sensor c , the inverse depth y of the observed feature point m ; Then minimize the sum of the prior and norm of all measurement residuals to obtain the maximum a posteriori estimate: Among them, z b represents the inertial observation, z l represents the visual observation, B is the set of all inertial measurement unit measurements, C is the set of features observed at least twice in the current sliding window, and r B (z b , X) and r C (z l , X) are the residuals of inertial sensor and visual measurement, r p , H p is the prior information, and then the maximum a posteriori estimate is nonlinearly optimized to obtain x k ; (2) Fault detection and recovery Check whether there is a fault. If there is a fault, the system will isolate the abnormal data, directly update the position of the visual inertial odometer through the latest measurement of the inertial measurement unit, and wait for the fault to recover. If the fault is not recovered within 5 seconds, the system will switch back to the initialization state and wait for the system to complete initialization again. (3) Relocation Add the loop frame to the current sliding window, use the pose of the loop frame as a constant, modify the nonlinear cost function, and add the loop term: Where L is the observation set of features detected in the loop closure frame, (l, v) refers to the lth feature observed in the loop closure frame v, and q v is the pose of the loop frame v, p v is the position of the loop frame v; if there are multiple loops in the current sliding window, the system will use all loop features from all frames for optimization.

10. The multi-rotor aircraft guidance and control method based on wireless direction finding according to claim 5, characterized in that: The processing steps of the global optimization module are: The direct residual of frame i and j is defined as represents the yaw angle, p represents the position, ^ represents the prior, R represents the rotation matrix, They are the estimates of the roll and pitch angles obtained directly from the monocular visual odometry; The entire pose graph is optimized by minimizing the following cost function: Where S is a set of previous key frames adjacent to the current key frame, and L is a set of loop frames associated with the current key frame.

Images