Unmanned aerial vehicle GNSS failure hovering control method based on vision

Abstract

The invention belongs to the field of unmanned aerial vehicle control systems, and particularly relates to an unmanned aerial vehicle GNSS failure hovering control method based on vision. According to the invention, the problems of positioning and attitude control under the condition of GNSS failure are solved. Based on visual information and Kalman filtering, stable hovering of the unmanned aerial vehicle in a complex environment is ensured through the steps of automatic mode switching, target selection and tracking, position estimation, attitude control and the like. The method has the characteristics of high precision, low cost and strong adaptability, and has a wide application prospect in the field of autonomous flight and control of unmanned aerial vehicles.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned aerial vehicle control systems, and in particular relates to a vision-based unmanned aerial vehicle GNSS failure hovering control method. Background Art

[0002] In practical applications, drones usually rely on global navigation satellite system (GNSS) signals to obtain accurate position and speed information to ensure precise control of the flight path. However, in some complex environments, GNSS signals are interfered with by various factors or completely fail, resulting in the inability of drones to obtain effective position and speed information. For example, between high-rise buildings in the city, satellite signals may be blocked or multipath reflected, causing signal quality to degrade or even be interrupted. GNSS signals are also easily shielded in underground facilities or densely forested areas. In addition, in military environments or specific areas, GNSS signals may also encounter human interference or shielding, which may cause drones to completely lose their positioning capabilities.

[0003] The loss of GNSS signals can have extremely serious consequences for the safe flight of drones. After losing satellite positioning information, drones will not be able to obtain accurate location information in real time, resulting in the inability to maintain their flight path. Although the inertial navigation system (INS) can provide location information through accelerometer and gyroscope data in a short period of time, due to the cumulative errors of inertial navigation, these errors will expand rapidly as the flight time increases, resulting in serious deviations in the drone's position estimate. This deviation will not only affect the hovering stability of the drone, but may also cause it to deviate from the planned route, and even risk collision or crash in complex environments, which may have catastrophic consequences, especially when the drone is performing critical tasks or flying in dangerous areas.

[0004] In the existing UAV hovering control methods, in addition to GNSS, other sensors are usually relied on to provide position information and attitude control, such as visual sensors, optical flow sensors, lidar, etc. Optical flow sensors estimate the displacement of the UAV by analyzing the movement of continuous images, which has the advantages of low cost and simple operation. However, optical flow technology has great limitations in practical applications, especially when flying at high altitudes. Optical flow sensors need to rely on the texture information of the ground or objects to make effective displacement estimates. At high altitudes, due to the distance from the ground, the detailed texture of the ground cannot be clearly captured, and the data obtained by the optical flow sensor is noisy, resulting in unsatisfactory position control effects. In addition, optical flow sensors are also easily affected by ambient lighting conditions. For example, at night or in an environment with uneven lighting, the positioning performance of optical flow technology will be greatly reduced.

[0005] Although LiDAR technology can provide higher accuracy in complex environments, it is expensive, large in size, and consumes high power, making it unsuitable for long-term flight of small drones. Even if LiDAR is integrated into some high-end drone systems, the complexity of target object tracking and precise hovering control in dynamic environments is still difficult to meet the needs of most commercial drones. Therefore, existing hovering control technologies still have many shortcomings in terms of target object selection, tracking, and maintaining hovering stability, especially when GNSS fails. These technologies are difficult to fully solve the positioning and attitude control problems of drones. Summary of the invention

[0006] The purpose of the present invention is to provide a vision-based UAV GNSS failure hovering control method, which can use visual perception and Kalman filtering technology to achieve high-precision hovering and stable attitude control of the UAV in the event of GNSS failure, thereby ensuring the flight safety of the UAV.

[0007] In order to achieve the above objectives, the first aspect of the present invention is to adopt the following technical solution: a vision-based UAV GNSS failure hovering control method, comprising the following steps:

[0008] 1) When GNSS failure is detected, switch from GNSS positioning mode to visual hovering mode;

[0009] 2) In visual hovering mode, based on the image of the area below captured by the drone’s onboard camera in real time, a target is selected as a reference, and the position and speed of the drone relative to the target are obtained by tracking the visual information of the target;

[0010] 3) The visual information is fused with the horizontal acceleration measured by the UAV through the Kalman filter, and the position and velocity information are estimated through state prediction and correction update; the attitude of the UAV is controlled to achieve stable hovering.

[0011] In a preferred embodiment, step 2) is specifically as follows: when the UAV just enters the visual hovering mode, the image of the lower area captured by the current onboard camera is saved as the "origin frame"; the "origin frame" image is used as a reference coordinate system, in which the horizontal X and Y directions of the body are defined as two mutually perpendicular axes, and the current position of the UAV in the "origin frame" is taken as the coordinate origin; during the visual hovering, the selected target is a number of feature points in the origin frame; the feature points of the image are identified, and the image of the current frame is matched with the origin frame and the previous frame for feature points; the feature point matching results and the pixel displacement ΔPix between the current frame and the previous frame are output; the speed Pix2Vel of the UAV relative to the target is calculated:

[0012]

[0013] Among them, h corresponds to the flight height of the UAV, Vk, Va0, Va1, Va2, Va3 are fitting coefficients; the displacement of the pixel point ΔPix is ​​the average displacement of all feature points;

[0014] The calculation formula of the drone's position PixPos relative to the target is as follows:

[0015]

[0016] Among them, h corresponds to the flight altitude of the UAV, k p 、a p 、b p is the fitting coefficient; ∑ΔPix is ​​the total pixel displacement after the UAV enters the visual hovering mode.

[0017] In step 3), in the Kalman filter, a second-order state model is established and position, velocity, and acceleration are estimated;

[0018] The transfer function between the angle target value and the UAV speed is:

[0019]

[0020] The parameters a0, a1, and b0 are determined by fitting the actual flight data, and the continuous-time model is obtained as follows:

[0021] md_acc'=-a0×md_acc-a1×md_vel+b0×md_θ r

[0022] According to the transfer function between the angle target value and the UAV speed and the continuous time model, the values ​​of parameters a0, a1, and b0 are obtained for the state matrix A and the input matrix B;

[0023] The s in the transfer function represents the complex frequency variable in the Laplace transform; MD_VEL(s) and MD_θ r (s) are the Laplace transform of the UAV velocity in the complex frequency domain and the Laplace transform of the UAV angle target value in the complex frequency domain; MD_VEL (and MD_θ r They are the UAV speed in the complex frequency domain and the UAV angle target value in the complex frequency domain; md_acc, md_vel and md_θ r are the horizontal acceleration, velocity and angle target values ​​of the UAV in the time domain respectively; md_acc′ is the derivative of md_acc;

[0024] The state variables of the second-order state model include the position, velocity, and acceleration of the UAV;

[0025] The state space expression of the second-order state model is:

[0026]

[0027] The state variables of the model are:

[0028]

[0029] y represents the output vector; u represents the target roll angle value or the target pitch angle value; POS is the position, VEL is the velocity, and ACC is the acceleration;

[0030] The state matrix A, input matrix B, and output matrix C are

[0031]

[0032] In step 3), the visual information is the position PixPos of the drone relative to the target and the speed Pix2Vel of the drone relative to the target; the horizontal acceleration measured by the drone is converted by the rotation matrix and input into the Kalman filter; the estimation of the position and velocity information refers to the Kalman filter obtaining and outputting the estimated position POS, estimated velocity VEL and estimated acceleration ACC;

[0033] To control the attitude of the UAV to achieve stable hovering means: to calculate the angle target value based on the estimated position, velocity, acceleration information of the UAV estimated by the Kalman filter and the target velocity refVel calculated by the pixel position sensor, and input the angle target value into the attitude controller for attitude control, so that the UAV can hover stably at the position when it just enters the visual hovering mode.

[0034] In step 3), the visual information is fused with the horizontal acceleration measured by the drone through the Kalman filter, and the position and velocity information are estimated through state prediction and correction update. Specifically,

[0035] Status prediction:

[0036] Covariance prediction: P(k) = ΦP + (k-1)Φ T +Q

[0037] Among them, Φ, Γ and H correspond to the aforementioned matrices A, B and C, that is, Φ = A, Γ = B, H = C; k represents the current moment, k-1 represents the previous moment; X d represents the predicted state of the state variable X; represents the estimated state of the state variable X, including the estimated POS, VEL, and ACC; X d (k) represents the predicted state of the state variable X at the current moment, represents the estimated state of the state variable X at the previous moment, u is the target roll angle value or the target pitch angle value, u(k-1) is the target roll angle value or the target pitch angle value at the previous moment; P is the predicted covariance, P(k) is the predicted covariance at the current moment, P + (k-1) is the estimated covariance of the previous moment, Q is the covariance matrix of the process noise, Q = E[w(t)w T (t)], w(t) is the vector of process noise, corresponding to internal noise; E represents the expected value operator, and t represents time;

[0038] The correction update process of the Kalman filter is as follows:

[0039]

[0040] The covariance update process is: P + (k)=(IK(k)H)P(k)

[0041] X d represents the predicted state of the state variable X, represents the estimated state of the state variable X, represents the estimated output of the state variable; Represents the estimated state of the state variable X at the current moment; is the estimated output of the state variable at the current moment; I is the unit matrix, P + (k) is the estimated covariance at the current moment;

[0042] K represents the optimal Kalman gain, which is the process noise covariance Q(Q=E[w(t)w T (t)]) and the measurement noise covariance R(R=E[v(t)v T (t)]) under the condition of minimizing the gain matrix of the estimation error; z(k) represents the observation value at the current moment, including the current position PixPos, velocity Pix2Vel and horizontal acceleration acc of the drone relative to the target; w(t) is the vector of process noise, corresponding to the internal noise; v(t) is the vector of measurement noise, corresponding to the measurement noise, t represents time; E represents the expected value operator; Q is the covariance matrix of process noise; R is the covariance matrix of measurement noise; C is the aforementioned output matrix; K(k) is the optimal Kalman gain at the current moment.

[0043] In step 3), the set target point refPos is compared with the estimated position POS of the drone at the current moment; the position error errorPos is expressed as:

[0044] errorPos=refPos-POS

[0045] The target velocity refVel output by the pixel position controller is:

[0046] refVel=Kp_pos×(0-POS)

[0047] Kp_pos is the proportional gain of the pixel position controller;

[0048] The pixel change rate controller compares the target speed refVel with the current estimated speed VEL output by the Kalman filter, thereby calculating the speed error errorVel = refVel - VEL;

[0049] Calculate the attitude angle control value refAtt output by the pixel change rate controller. Assume that Kp_vel and Kd_vel are the proportional and differential gains of the pixel change rate controller respectively. Then the attitude angle control value refAtt output by the pixel change rate controller is:

[0050] refAtt=Kp_vel×errorVel+Kd_vel×(0-ACC)

[0051] refAtt is the attitude angle control value output by the pixel change rate controller, and the attitude angle control value refAtt(k-1) at the previous moment is the input u(k-1) in the Kalman filter.

[0052] The second aspect of the present invention is to disclose a vision-based UAV GNSS failure hovering control system that adopts the aforementioned control method.

[0053] A third aspect of the present invention is to provide a processor, which is used to run a computer program. When the computer program is running, the vision-based UAV GNSS failure hovering control method as described above is executed.

[0054] A fourth aspect of the present invention is to disclose a UAV, comprising the aforementioned vision-based UAV GNSS failure hovering control system.

[0055] A fifth aspect of the present invention is to provide a computer-readable medium having a computer program stored thereon, wherein the computer program is executed by a processor to implement the vision-based UAV GNSS failure hovering control method as described above.

[0056] The benefits of the present invention are as follows: the present invention solves the positioning and attitude control problems in the case of GNSS failure. Based on visual information and Kalman filtering, the stable hovering of the UAV in a complex environment is ensured through steps such as automatic mode switching, target selection and tracking, position estimation and attitude control. The invention has the characteristics of high precision, low cost and strong adaptability, and has broad application prospects in the field of autonomous flight and control of UAVs. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] Figure 1 : Flowchart of the mode switching of the UAV when GNSS fails;

[0058] Figure 2 :System architecture diagram of vision-based UAV GNSS failure hovering control;

[0059] Figure 3 : Kalman filter state prediction and correction update flow chart. DETAILED DESCRIPTION

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

[0061] like Figure 1 As shown, the present invention realizes GNSS failure detection and mode switching. In normal flight state, the GNSS signal is mainly used to provide the position information and navigation data of the UAV. When the GNSS signal failure is detected (such as signal loss or severe decrease in strength), it will automatically trigger the switch from GNSS positioning mode to visual hovering mode.

[0062] Specifically, the detection of GNSS signal failure specifically means that the GNSS signal is determined to be failed when any of the following conditions are met: GNSS is not connected, the position accuracy (pDop) exceeds 2.8, the number of visible satellites is less than 12, or the validity flag sent by the GNSS sensor to the flight control system indicates that the signal is invalid (which may be caused by factors such as poor signal reception or electromagnetic interference). In the event of GNSS signal failure, the flight control system will automatically switch to visual hover mode. Only when all these conditions are no longer met, that is, the GNSS signal returns to normal, will the flight control system switch back to GNSS positioning mode.

[0063] Figure 1 In the case of visual hovering, turning on the visual hovering function does not mean that the aircraft will switch to the visual hovering mode immediately. After turning on the visual hovering function, the aircraft will switch to the visual hovering mode only when the GNSS signal fails. Figure 1 The original mode refers to the GNSS positioning mode, which is an existing technology. The flight control system defaults to the GNSS positioning mode for the drone, and only switches to the visual hovering mode when the GNSS signal fails.

[0064] Figure 2In the process, the ground station gives the target altitude value and heading value and inputs them into the altitude controller and heading controller respectively. The altitude of the drone measured by the barometer is fed back to the altitude controller, and the heading angle of the drone measured by the inertial measurement unit (IMU) is fed back to the heading controller to realize the altitude and heading control of the drone. When the drone GNSS fails to hover control, the heading angle remains unchanged. The drone inertial measurement unit (IMU) includes a gyroscope, an accelerometer, and a magnetometer. This paragraph is the prior art.

[0065] Figure 2 In the process, the camera works in the visual hovering mode and obtains images. Combined with the real-time drone altitude measured by the barometer, the microcomputer continuously processes the image data to realize the recognition of feature points, calculate and output the drone's position PixPos relative to the target and the drone's speed Pix2Vel relative to the target. The drone's position PixPos and speed Pix2Vel relative to the target are fed back to the Kalman filter. At the same time, the roll angle, pitch angle target value and acceleration are also used as inputs to the Kalman filter, and the Kalman filter outputs the estimated position POS (fed back to the pixel position controller), estimated speed VEL, and estimated acceleration ACC (fed back to the pixel change rate controller). The pixel position controller calculates the target speed according to the target point output by the microcomputer (the target values ​​in the X and Y directions of this point are both 0, that is, the (0, 0) point, which is the position of the UAV when it just enters the visual hovering mode. The purpose of the failed hovering control method of the present invention is to make the UAV hover stably at this point) and the estimated position output by the Kalman filter; the pixel change rate controller calculates the attitude angle target value (that is, the roll angle target value and the pitch angle target value) according to the received target speed and the estimated speed and estimated acceleration, and the obtained attitude angle target value is fed back to the attitude controller.

[0066] In visual hovering mode, the drone's onboard camera collects real-time image data of the area below the drone, processes the image through a microcomputer, automatically selects a target suitable as a reference, and establishes a local coordinate system with the target. By tracking the target's visual information, the drone can obtain its position and speed relative to the target.

[0067] Specifically, when the drone just enters the visual hovering mode, the image of the lower area captured by the current onboard camera is saved as the "origin frame". This "origin frame" image is used as a reference coordinate system, where the horizontal X and Y directions of the body are defined as two mutually perpendicular axes, with the current position of the drone in the "origin frame" as the coordinate origin. During visual hovering, the selected targets are several feature points in the origin frame. Once the origin frame is captured and saved, it will remain unchanged throughout the visual hovering stage.

[0068] The open source OpenCV library is called to identify the feature points of the image, and the feature points of the current frame are matched with the origin frame and the previous frame. After that, the feature point matching results between the two frames (current frame and previous frame), the displacement ΔPix (X, Y direction) of the pixel point and the timestamp are output. These data are used to calculate the pixel change speed Pix2Vel (that is, the speed of the drone relative to the target), and the calculation formula is as follows:

[0069]

[0070] Among them, h corresponds to the flight altitude of the drone (calculated and fed back by the barometer), Vk, Va0, Va1, Va2, and Va3 are coefficients fitted according to the experimental data of different flight altitudes of the drone during actual flight; usually, 10 meters is a gradient, and 12 sets of data from 10 meters to 120 meters are fitted. The displacement of the pixel point ΔPix is ​​the average displacement of all feature points. At different altitudes, the movement characteristics of the pixels captured by the camera are different; by adding altitude data, the visual speed can be estimated more accurately.

[0071] The calculation formula of the pixel position change PixPos (i.e. the position of the drone relative to the target) is as follows:

[0072]

[0073] Among them, h corresponds to the flight altitude of the UAV, k p 、a p 、b p It is also fitted according to the pixel displacement of the drone at different heights during actual flight. ∑ΔPix is ​​the total pixel displacement after the drone enters the visual hovering mode.

[0074] The present invention establishes a second-order state model and performs Kalman filter position and velocity estimation. The position PixPos of the drone relative to the target, the velocity Pix2Vel of the drone relative to the target, and the acceleration acc (acceleration in this paper is horizontal acceleration) are used as observation values ​​(i.e. z(k)) in the Kalman filter algorithm, and the output of the Kalman filter includes the estimated position (POS), estimated velocity (VEL) and estimated acceleration (ACC).

[0075] In visual hovering mode, the precise positioning of the drone relies on fusing visual information with inertial measurement unit (IMU) data. The visual information includes the position of the drone relative to the target, PixPos, and the velocity of the drone relative to the target, Pix2Vel, and the inertial measurement unit (IMU) data includes acceleration. The acceleration data obtained from the accelerometer is transformed by a rotation matrix so that the acceleration measured by the accelerometer fixed to the fuselage is converted into the body coordinate system (which is the prior art). This step is necessary because it allows the data to be correctly aligned between different reference frames, thereby ensuring the accuracy and reliability of the drone control system.

[0076] The second-order state model proposed in the present invention has state variables including the position (POS), velocity (VEL) and acceleration (ACC) of the drone.

[0077] First, the state matrix and input matrix need to be determined. Fly the drone in GNSS mode and record the target angle (pitch angle or roll angle) and the speed of the drone as experimental data. Combine the experimental data with the transfer function between the angle target value and the speed of the drone and the formula of the continuous-time model to determine a0, a1, and b0, and then determine the state matrix A and input matrix B.

[0078] The transfer function between the angle target value and the UAV speed is set as:

[0079]

[0080] The s in the transfer function represents the complex frequency variable in the Laplace transform. In the expression of the transfer function, MD_VEL(s) and MD_θ r (s) are the Laplace transform of the UAV velocity in the complex frequency domain and the Laplace transform of the target value of the UAV angle (roll angle or pitch angle) in the complex frequency domain. MD_VEL (and MD_θ r (respectively, the target values ​​of the UAV speed in the complex frequency domain and the UAV angle (roll angle or pitch angle) in the complex frequency domain. Parameters a0, a1, and b0 are coefficients in the transfer function, which are obtained by fitting experimental data in MATLAB in order to accurately simulate the dynamic behavior of the UAV.

[0081] Parameters a0, a1, and b0 need to be determined based on actual flight data fitting, and the continuous-time model is:

[0082] md_acc'=-a0×md_acc-a1×md_vel+b0×md_θ r

[0083] md_acc, md_vel, and md_θ rare the horizontal acceleration, velocity and angle target values ​​of the drone in the time domain. These parameters are used to describe the dynamic state of the drone in the Kalman filter algorithm. md_acc′ is the derivative of md_acc.

[0084] The state space expression of the model is:

[0085]

[0086] The state variables of the model are:

[0087]

[0088] The state matrix A, input matrix B, and output matrix C are

[0089]

[0090] The accuracy and efficiency of the algorithm can be ensured by adjusting the output matrix C. y represents the output vector. Bu represents the direct impact of the input on the state variable, B is the input matrix, and u represents the roll angle target value or the pitch angle target value.

[0091] In the Kalman filter algorithm, only the target values ​​of the roll angle (i.e., the lateral roll angle) and the pitch angle are considered, and the heading angle is not involved. For the pitch angle and the roll angle, it is necessary to set the transfer function between the pitch angle target value and the UAV speed, the transfer function between the roll angle target value and the UAV speed, and the continuous-time model of the pitch angle and the continuous-time model of the roll angle. At the same time, the state prediction, state estimation, covariance prediction, covariance update, and state variable estimation output for the roll angle and the pitch angle are also performed independently. For the roll angle and the pitch angle, the parameters a0, a1, and b0 are respectively determined by fitting the actual flight data.

[0092] The input of the Kalman filter is the target value of the roll angle or pitch angle, the position PixPos of the drone relative to the target, the speed Pix2Vel of the drone relative to the target, and the horizontal acceleration acc obtained by the rotation matrix conversion is also input as the observation value, and the output of the Kalman filter includes the estimated position (POS), the estimated speed (VEL) and the estimated acceleration (ACC). The changes in the Kalman filter, the pixel position controller, and the pixel change rate controller in the present invention are mainly the sources of the target value and the feedback value; the target values ​​of the pixel position controllers in the X and Y directions in the visual hovering mode are both 0, and the feedback is the output POS of the Kalman filter, and the output value is the target value of the next level speed controller (i.e., the pixel change rate controller); the feedback of the speed controller is the output VEL of the Kalman filter, and the differential term feedback is the output ACC of the Kalman filter.

[0093] The Kalman filter consists of three main steps: prediction update, correction update, and output estimation. Prediction update: predict the current state based on the previous state estimate and control input. Correction update: use the latest measurement information to update the predicted state to obtain a more accurate estimate of the current state. Output estimate: the estimate to be updated and output based on the calculated Kalman gain and the latest measurement information. State prediction (i.e. prediction update) refers to the prediction of the three key state variables: current position (POS), velocity (VEL), and acceleration (ACC). Covariance prediction refers to the prediction of the uncertainty of these state variables, i.e. the covariance matrix P. In the Kalman filtering process, the Kalman gain K determines the weight distribution between the newly obtained measurement information and the current predicted state. State update (i.e. correction update) updates the estimated values ​​of the three state variables based on the calculated Kalman gain and the latest observations of position, velocity, and acceleration. At the same time, covariance update involves updating the covariance matrix P to reflect the new level of uncertainty in the estimated state.

[0094] Considering the input process noise (w) and measurement noise (v), the discrete-time state-space model can be expressed as:

[0095] Status prediction:

[0096] Covariance prediction: P(k) = ΦP + (k-1)Φ T +Q

[0097] Among them, Φ, Γ and H correspond to the aforementioned matrices A, B and C, that is, Φ = A, Γ = B, H = C. This is to make the model fitted in MATLAB correspond to the model in the Kalman filter. k represents the current moment, and k-1 represents the previous moment. X d represents the predicted state of the state variable X, represents the estimated state of the state variable X (including estimated POS, VEL, and ACC), X d (k) represents the predicted state of the state variable X at the current moment, represents the estimated state of the state variable X at the previous moment (i.e., the estimated POS, VEL, and ACC at the previous moment), u is the attitude angle target value (pitch angle target value or roll angle target value), and u(k-1) is the attitude angle target value at the previous moment. P is the predicted covariance, P(k) is the predicted covariance at the current moment, and P + (k-1) is the estimated covariance of the previous moment, Q is the covariance matrix of the process noise, Q = E[w(t)w T (t)], w(t) is the vector of process noise, corresponding to internal noise; E represents the expected value operator, and t represents time.

[0098] The correction update (i.e. state update) process of the steady-state Kalman filter is as follows:

[0099]

[0100] The covariance update process is: P + (k)=(IK(k)H)P(k)

[0101] X d represents the predicted state of the state variable X, represents the estimated state of the state variable X (including estimated POS, VEL, and ACC), represents the estimated output of the state variable X. Represents the estimated state of the state variable X at the current moment (i.e., the estimated POS, VEL, and ACC at the current moment). is the estimated output of the state variable at the current moment. I is the identity matrix, P + (k) is the estimated covariance at the current moment.

[0102] Where K represents the optimal Kalman gain, which is the process noise covariance Q (Q = E [w (t) w T (t)]) and the measurement noise covariance R(R=E[v(t)v T (t)]) under the condition of minimizing the estimation error. z(k) represents the observation value at the current moment (including the current moment of the drone's position PixPos, velocity Pix2Vel and horizontal acceleration acc relative to the target), k represents the current moment, and k-1 represents the previous moment. w(t) is the vector of process noise, corresponding to internal noise; v(t) is the vector of measurement noise, corresponding to measurement noise, t represents time; E represents the expected value operator. Q is the covariance matrix of process noise. R is the covariance matrix of measurement noise, which is caused by z(k). C is the aforementioned output matrix. K(k) is the optimal Kalman gain at the current moment.

[0103] z(k)-HX d In (k), the current position of the UAV relative to the target PixPos is subtracted from the predicted POS at the current moment, the current velocity of the UAV relative to the target Pix2Vel is subtracted from the predicted VEL at the current moment, and the current horizontal acceleration acc of the UAV is subtracted from the predicted ACC at the current moment.

[0104] The present invention performs attitude control on the unmanned aerial vehicle to achieve a stable hovering target.

[0105] The Kalman filter algorithm is used to fuse visual data with IMU data for state estimation. The filter is updated through prediction and correction, and can still provide relatively accurate position and velocity information estimates for the drone in the event of GNSS failure. This fusion method effectively eliminates noise and uncertainty in sensor data, ensuring the hovering accuracy and stability of the drone.

[0106] According to the UAV position, speed, acceleration information estimated by the Kalman filter and the target speed refVel calculated by the pixel position sensor, the angle target value (attitude angle target value) is calculated; then the attitude controller adjusts the attitude angle (pitch angle, roll angle) of the UAV in real time according to the angle target value and the actual angle value obtained by attitude solution (existing technology). The control of the attitude controller is existing technology, which enables the UAV to maintain a relatively stable hovering state.

[0107] Control: The position control loop (corresponding to the pixel position controller) adopts proportional control, and the speed control loop (corresponding to the pixel change rate controller) adopts PD control. The angle target value is calculated through the position error, the error change rate (i.e., the speed information) and the acceleration.

[0108] The calculation of the position error involves comparing the set target point (refPos) with the estimated position of the drone at the current moment (POS, output by the Kalman filter). The position error (errorPos) can be expressed as:

[0109] errorPos=refPos-POS

[0110] Where refPos is (0, 0).

[0111] The output of the pixel position controller (i.e., the target value refVel of the velocity control loop, i.e., the target velocity) is:

[0112] refVel=Kp_pos×(0-POS)

[0113] All position information in this article, such as POS, includes values ​​in the X and Y directions. The speed control loop corresponds to the pixel change rate controller. The speed control loop compares the target speed (refVel) with the current feedback speed (i.e., the estimated speed VEL at the current moment output by the Kalman filter), thereby calculating the speed error errorVel = refVel-VEL. In order to optimize the response to the speed error, the influence of the acceleration feedback (ACC, i.e., the estimated acceleration ACC at the current moment output by the Kalman filter) also needs to be considered. Kp_pos is the proportional gain of the pixel position controller.

[0114] In the speed control loop, we use PD control to generate the control output, which will be used as the target value refAtt of the angle control loop (corresponding to the attitude angle controller). Let Kp_vel and Kd_vel be the proportional and differential gains of the pixel change rate controller, respectively. The control output of the speed control loop is:

[0115] refAtt=Kp_vel×errorVel+Kd_vel×(0-ACC)

[0116] refAtt is the attitude angle control value output by the pixel change rate controller, and the attitude angle control value refAtt(k-1) at the previous moment is the input u(k-1) in the Kalman filter.

[0117] POS in the errorPos, refVel, and refAtt calculation formulas refers to The estimated position POS in , when C = [1 11], is actually POS in (which is an estimated value), VEL refers to The estimated speed VEL in C = [1 11] is actually VEL in (which is an estimated value), ACC refers to The estimated acceleration ACC in , when C = [11 1], is actually ACC in (which is an estimate).

[0118] Attitude adjustment: The attitude controller adjusts the attitude angle of the UAV in real time according to the received angle target value and the actual angle value obtained by attitude solution, ensuring that the UAV hovers above the target used as a reference and maintains relative stability with the target used as a reference (referring to the relative position of the final UAV's hovering point and the target reference, which is equal to the relative position of the UAV and the target reference when the visual hovering mode is just entered). Specifically, the UAV is made to hover at the position when the visual hovering mode is just entered.

[0119] Example: Automatic hovering switch under GNSS failure

[0120] When a drone enters an area where GNSS signals are interfered with or fail (such as between tall buildings in a city or other similar complex environments) during a flight mission, the system can detect the loss of GNSS signals and take immediate countermeasures. In this case, it automatically switches from a GNSS-dependent navigation mode to a vision-based hovering mode.

[0121] After switching to visual hovering mode, the drone's onboard camera begins to collect real-time image data of the area below. Through image processing, the drone will identify static objects in the environment (such as buildings, road markings, fixed signs, etc.), select an object that is most suitable as a reference target, and establish a local coordinate system relative to the target.

[0122] Once the visual reference is selected, the Kalman filter algorithm is used to fuse the visual information captured by the camera with the data from sensors such as the inertial measurement unit (IMU) and barometer to estimate the real-time position and attitude angle of the drone. The algorithm replaces the physical position and speed of the drone with the position of the pixel and the rate of change of the pixel, ensuring that the drone can still provide more accurate position information and attitude control in the event of GNSS signal failure.

[0123] In order to achieve stable hovering of the drone, the relative position difference between the estimated current position of the drone and the target point (0, 0) is calculated based on the position error, and these errors are processed by the PD control algorithm to generate a real-time attitude angle target value. Finally, through continuous attitude adjustment, it is ensured that the drone can stably hover above the selected target and maintain relative stability, and the hovering attitude can be automatically adjusted even under the influence of external factors such as wind.

[0124] The above embodiments are only used to illustrate the technical solutions 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 solutions obtained by equivalent replacement or equivalent transformation shall fall within the protection scope of the present invention.

Claims

1. A vision-based UAV GNSS failure hovering control method, characterized by: The steps include: 1) When GNSS failure is detected, switch from GNSS positioning mode to visual hovering mode; 2) In visual hovering mode, based on the image of the area below captured by the drone’s onboard camera in real time, a target is selected as a reference, and the position and speed of the drone relative to the target are obtained by tracking the visual information of the target; 3) The visual information is fused with the horizontal acceleration measured by the UAV through the Kalman filter, and the position and velocity information are estimated through state prediction and correction update; the attitude of the UAV is controlled to achieve stable hovering.

2. The vision-based UAV GNSS failure hovering control method according to claim 1 is characterized by: Step 2) is specifically as follows: when the drone just enters the visual hovering mode, the image of the lower area captured by the current onboard camera is saved as the "origin frame"; the "origin frame" image is used as a reference coordinate system, in which the horizontal X and Y directions of the body are defined as two mutually perpendicular axes, and the current position of the drone in the "origin frame" is taken as the coordinate origin; during the visual hovering, the selected target is a number of feature points in the origin frame; the feature points of the image are identified, and the feature points of the current frame image are matched with the origin frame and the previous frame; the feature point matching results and the pixel displacement ΔPix between the current frame and the previous frame are output; the velocity Pix2Vel of the drone relative to the target is calculated: Among them, h corresponds to the flight height of the UAV, Vk, Va0, Va1, Va2, Va3 are fitting coefficients; the displacement of the pixel point ΔPix is ​​the average displacement of all feature points; The calculation formula of the drone's position PixPos relative to the target is as follows: Among them, h corresponds to the flight altitude of the UAV, k p 、a p 、b p is the fitting coefficient; ΣΔPix is ​​the total pixel displacement after the UAV enters the visual hovering mode.

3. The vision-based UAV GNSS failure hovering control method according to claim 1 is characterized by: In step 3), in the Kalman filter, a second-order state model is established and position, velocity, and acceleration are estimated; The transfer function between the angle target value and the UAV speed is: The parameters a0, a1, and b0 are determined by fitting the actual flight data, and the continuous-time model is obtained as follows: md_acc'=-a0×md_acc-a1×md_vel+b0×md_θ r According to the transfer function between the angle target value and the UAV speed and the continuous time model, the values ​​of parameters a0, a1, and b0 are obtained for the state matrix A and the input matrix B; The s in the transfer function represents the complex frequency variable in the Laplace transform; MD_VEL(s) and MD_θ r (s) are the Laplace transform of the UAV velocity in the complex frequency domain and the Laplace transform of the UAV angle target value in the complex frequency domain; MD_VEL (and MD_θ r They are the UAV speed in the complex frequency domain and the UAV angle target value in the complex frequency domain; md_acc, md_vel and md_θ r are the horizontal acceleration, velocity and angle target values ​​of the UAV in the time domain respectively; md_acc′ is the derivative of md_acc; The state variables of the second-order state model include the position, velocity, and acceleration of the UAV; The state space expression of the second-order state model is: The state variables of the model are: y represents the output vector; u represents the target roll angle value or the target pitch angle value; POS is the position, VEL is the velocity, and ACC is the acceleration; The state matrix A, input matrix B, and output matrix C are C＝[111]。 4. The vision-based UAV GNSS failure hovering control method according to claim 2 is characterized by: In step 3), the visual information is the position PixPos of the drone relative to the target and the speed Pix2Vel of the drone relative to the target; the horizontal acceleration measured by the drone is converted by the rotation matrix and input into the Kalman filter; the estimation of the position and velocity information refers to the Kalman filter obtaining and outputting the estimated position POS, estimated velocity VEL and estimated acceleration ACC; To control the attitude of the UAV to achieve stable hovering means: to calculate the angle target value based on the estimated position, velocity, acceleration information of the UAV estimated by the Kalman filter and the target velocity refVel calculated by the pixel position sensor, and input the angle target value into the attitude controller for attitude control, so that the UAV can hover stably at the position when it just enters the visual hovering mode.

5. The vision-based UAV GNSS failure hovering control method according to claim 2 is characterized by: In step 3), the visual information is fused with the horizontal acceleration measured by the drone through the Kalman filter, and the position and velocity information are estimated through state prediction and correction update. Specifically, Status prediction: Covariance prediction: P(k) = ΦP + (k-1)Φ T +Q Wherein, Φ, Γ and H correspond to the aforementioned matrices A, B and C, that is, Φ = A, Γ = B, H = C; k represents the current moment, k-1 represents the previous moment; X d represents the predicted state of the state variable X; represents the estimated state of the state variable X, including the estimated POS, VEL, and ACC; X d (t) represents the predicted state of the state variable X at the current moment, represents the estimated state of the state variable X at the previous moment, u is the target roll angle value or the target pitch angle value, u(k-1) is the target roll angle value or the target pitch angle value at the previous moment; P is the predicted covariance, P(k) is the predicted covariance at the current moment, P + (k-1) is the estimated covariance of the previous moment, Q is the covariance matrix of the process noise, Q = E[w(t)w T (t)], w(t) is the vector of process noise, corresponding to internal noise; E represents the expected value operator, and t represents time; The correction update process of the Kalman filter is as follows: The covariance update process is: P + (k)=(IK(k)H)P(k) X d represents the predicted state of the state variable X, represents the estimated state of the state variable X, represents the estimated output of the state variable; Represents the estimated state of the state variable X at the current moment; is the estimated output of the state variable at the current moment; I is the unit matrix, P + (k) is the estimated covariance at the current moment; K represents the optimal Kalman gain, which is the process noise covariance Q(Q=E[w(t)w T (t)]) and the measurement noise covariance R(R=E[v(t)v T (t)]) under the condition of minimizing the gain matrix of the estimation error; z(k) represents the observation value at the current moment, including the current position PixPos, velocity Pix2Vel and horizontal acceleration acc of the drone relative to the target; w(t) is the vector of process noise, corresponding to the internal noise; v(t) is the vector of measurement noise, corresponding to the measurement noise, t represents time; E represents the expected value operator; Q is the covariance matrix of process noise; R is the covariance matrix of measurement noise; C is the aforementioned output matrix; K(k) is the optimal Kalman gain at the current moment.

6. The vision-based UAV GNSS failure hovering control method according to claim 5 is characterized by: In step 3), the set target point refPos is compared with the estimated position POS of the drone at the current moment; The position error errorPos is expressed as: errorPos=refPos-POS The target velocity refVel output by the pixel position controller is: refVel=Kp_pos×(0-POS) Kp_pos is the proportional gain of the pixel position controller; The pixel change rate controller compares the target speed refVel with the current estimated speed VEL output by the Kalman filter, thereby calculating the speed error errorVel = refVel - VEL; Calculate the attitude angle control value refAtt output by the pixel change rate controller. Assume that Kp_vel and Kd_vel are the proportional and differential gains of the pixel change rate controller respectively. Then the attitude angle control value refAtt output by the pixel change rate controller is: refAtt=Kp_vel×errorVel+Kd_vel×(0-ACC) refAtt is the attitude angle control value output by the pixel change rate controller, and the attitude angle control value refAtt(k-1) at the previous moment is the input u(k-1) in the Kalman filter.

7. A vision-based UAV GNSS failure hovering control system using the control method described in any one of claims 1-6.

8. A processor for running a computer program, characterized in that: When the computer program is running, the vision-based unmanned aerial vehicle GNSS failure hovering control method as described in any one of claims 1 to 6 is executed.

9. A drone, characterized in that: Including the vision-based UAV GNSS failure hovering control system as described in claim 7.

10. A computer readable medium having a computer program stored thereon, characterized in that: The computer program is executed by the processor as the vision-based UAV GNSS failure hovering control method as described in any one of claims 1-6.

Images