A method and system for judging the flight status of an unmanned aerial vehicle

Abstract

The present invention discloses a method for judging the flight state of a drone, the method comprising: S1: measuring the distance, azimuth and altitude parameters of the drone relative to a ground radar, and constructing a measurement vector of the drone; S2: using Kalman filtering to filter the measurement vector and predict the real flight state; S3: calculating the yaw angle, pitch angle and roll angle of the drone and constructing a six-dimensional flight state vector; S4: constructing a user instruction sequence and calculating the actual flight state change after the drone executes the i-th instruction; S5: calculating the ideal flight state change after the drone executes the i-th instruction, and comparing it with the actual flight state change output by step S4 to evaluate the flight state of the drone. The present invention adopts Kalman filtering to accurately predict the real flight state of the drone; establishes a corresponding relationship between the user instruction and the flight state change, and evaluates the flight state of the drone in steps, which is conducive to intuitively judging whether the flight state of the drone meets the instruction requirements.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicles, and in particular to a method and system for determining the flight status of an unmanned aerial vehicle. Background Art

[0002] Drones are widely used in many fields such as aerial photography, express delivery, pesticide spraying, etc., and play an important role. Drones perform flight control by pre-setting or receiving user commands in real time, mainly including: taking off and returning according to the commands, flying to a designated location, and stabilizing the flight attitude.

[0003] The flight state of a drone is controlled by user commands. Existing research on the judgment of the flight state of a drone focuses on using algorithms or models to improve the accuracy of target state estimation, thereby making flight state judgments. There is little analysis combined with the user command sequence, and there is no one-to-one matching of the user command sequence with the drone flight state, so as to judge the drone flight state from the perspective of the user command. Therefore, the present invention provides a method for judging the flight state of a drone, which can effectively execute the user command by judging whether the current flight state is consistent with the user command expectation, thereby improving the control performance of the drone. Summary of the invention

[0004] In view of this, the present invention provides a method and system for judging the flight status of a UAV, the purpose of which is to: improve the prediction accuracy of the flight parameters of the UAV through filtering estimation, establish a corresponding relationship between user instructions and the change in the flight status of the UAV to study the execution of user instructions, and judge whether the flight status of the UAV meets the instruction requirements by comparing the actual flight status change of the UAV with the ideal flight status change, thereby realizing intuitive and effective judgment of the flight status of the UAV.

[0005] To achieve the above object, the present invention provides a method for determining the flight status of a drone, comprising the following steps:

[0006] S1: Use the distance, azimuth and altitude parameters of the UAV relative to the ground radar to construct the UAV measurement vector;

[0007] S2: Use Kalman filtering to filter the measurement vector of the UAV and predict the actual flight status of the UAV;

[0008] S3: Calculate the yaw angle, pitch angle and roll angle of the UAV through the predicted real flight state of the UAV, and construct the six-dimensional flight state vector of the UAV;

[0009] S4: Construct a user command sequence and calculate the actual flight state change after the drone executes the i-th command;

[0010] S5: Calculate the ideal flight state change after the UAV executes the i-th instruction, and compare it with the actual flight state change output in step S4 to evaluate the UAV flight state.

[0011] As a further improvement method of the present invention:

[0012] Optionally, in step S1, using the distance, azimuth and altitude parameters of the UAV relative to the ground radar includes:

[0013] A1: The ground radar receives the echo signal ech from the UAV at time k and decomposes the echo signal ech into wavelet coefficients of different scales:

[0014]

[0015] Among them, Wvc(achi,b) represents the wavelet coefficient of ech, achi is the scale parameter, b is the translation parameter, ψ * is the complex conjugate of the wavelet function;

[0016] Threshold processing is performed on the wavelet coefficient Wvc(achi,b) to remove the high-frequency coefficients containing noise and obtain the wavelet coefficient Wsig(achi,b) containing only the target;

[0017] Using Wsig(achi,b), the denoised drone echo signal dec is obtained through signal reconstruction;

[0018] A2: Select the reference blade of the drone and establish the phase compensation factor

[0019] phc=exp(j·amp·cos(ang·m+inp))

[0020] Wherein, phc represents the reference blade phase compensation factor, exp represents the exponential function, j represents the imaginary sign, amp represents the amplitude coefficient vector, and ang represents the angular velocity coefficient vector;

[0021] Perform phase compensation on the drone echo signal to obtain the compensated echo signal:

[0022] dec′=dec·phc

[0023] Where, dec′ represents the echo signal after compensation;

[0024] A3: Perform a two-dimensional Fourier transform on dec′ to obtain the range-Doppler image rd, perform a one-dimensional constant false alarm detection on rd, and obtain the distance rang of the UAV relative to the ground radar at time k k ;

[0025] A4: Perform a three-dimensional Fourier transform on dec′ to obtain the spatial frequency domain response in different directions. Use the extreme value detection method to obtain the azimuth angle azi of the UAV relative to the ground radar at time k k ;

[0026] A5: Calculate the height of the drone relative to the ground at time k k :

[0027] high k =imw·gsd·Fr / (sw·100)

[0028] Among them, imw represents the width of the image taken by the drone, gsd represents the ground resolution, Fr represents the real focal length of the drone camera, and sw represents the real width of the drone camera.

[0029] Optionally, in step S1, constructing a measurement vector of the drone includes:

[0030] S101: With the ground radar as the coordinate origin, the UAV's forward direction as the x-axis direction, the direction perpendicular to the UAV's forward direction on the horizontal plane as the y-axis direction, and the vertical direction as the z-axis direction, calculate the UAV's position at time k:

[0031]

[0032]

[0033] meaz k =high k

[0034] Among them, meax k represents the measured position of the drone on the x-axis at time k, meay k represents the measured position of the drone on the y-axis at time k, meaz k represents the measured position of the drone on the z-axis at time k;

[0035] S102: Constructing the UAV's measurement vector

[0036] measure k =[meax k ,meay k ,meaz k ]

[0037] Among them, measure represents the measurement vector of the drone.

[0038] Optionally, in step S2, Kalman filtering is used to predict the actual flight parameters of the drone, including:

[0039] S201: Establish the real state vector of the drone at time k:

[0040] state k =[pox k ,poy k ,poz k ,vx k ,vy k ,vz k ,accex k ,accey k ,accez k ]

[0041] Among them, state k represents the real state vector of the drone at time k, pox k represents the real position of the drone on the x-axis at time k, poy k represents the real position of the drone on the y axis at time k, poz k represents the real position of the drone on the z axis at time k, vx k represents the real speed of the drone on the x-axis at time k, vy k represents the real speed of the drone on the y axis at time k, vz k represents the real speed of the drone on the z axis at time k, accex k represents the actual acceleration of the drone on the x-axis at time k, accey k represents the actual acceleration of the drone on the y-axis at time k, accez k represents the real acceleration of the drone on the z-axis at time k;

[0042] S202: Predicting the actual flight status of a drone

[0043] state k+1|k =F·state k

[0044] P k+1|k =FP k F T +Q k

[0045] Among them, state k+1|k represents the real state vector prediction vector of the UAV at time (k+1), F represents the state transfer matrix of the UAV motion, and F T represents the transpose of matrix F, P k represents the covariance matrix value at time k, P k+1|k represents the predicted value of the covariance matrix at time (k+1), Q k represents the k-time process noise covariance matrix;

[0046] S203: Calculate Kalman gain

[0047] Kalz k+1 =P k+1|k H T (HP k+1|k H T +R k+1 ) -1

[0048] Kalz k+1 represents the Kalman gain at time (k+1), R k+1 represents the observation noise covariance, (HP k+1|k H T +R k+1 ) -1 Indicates HP k+1|k H T +R k+1 Find the inverse matrix;

[0049] S204: The actual state of the drone at time (k+1) k+1 Updated to:

[0050] state k+1 =state k+1|k +Kalz k+1 (measure k+1 -H·state k+1|k )

[0051] state k+1 =[pox k+1 ,poy k+1 ,poz k+1 ,vx k+1 ,vy k+1 ,vz k+1 ,accex k+1 ,accey k+1 ,accez k+1 ]

[0052] Among them, pox k+1 represents the real position of the drone on the x-axis at time (k+1), poy k+1 represents the real position of the drone on the y axis at time (k+1), poz k+1 represents the real position of the drone on the z-axis at time (k+1), vx k+1 represents the real speed of the drone on the x-axis at time (k+1), vy k+1 represents the real speed of the drone on the y axis at time (k+1), vz k+1 represents the real speed of the drone on the z-axis at time (k+1), accexk+1 represents the actual acceleration of the drone on the x-axis at time (k+1), accey k+1 represents the actual acceleration of the drone on the y-axis at time (k+1), accez k+1 represents the actual acceleration of the drone on the z-axis at time (k+1), measure k+1 Represents the measurement vector of the UAV at time k+1.

[0053] Optionally, in step S3, using the predicted real flight parameters of the drone to construct a flight state vector of the drone includes:

[0054] S301: Calculate the yaw angle of the drone at time k k , pitch angle k , roll angle roll k :

[0055] yaw k =arctan(accez k / sqrt(accex k 2 +accez k 2 ))

[0056] pitch k =arctan(accex k / sqrt(accey k 2 +accez k 2 ))

[0057] roll k =arctan(accey k / sqrt(accex k 2 +accez k 2 ))

[0058] S302: Construct the six-dimensional flight state vector UAV at time k k :

[0059] UAV k =[pox k ,poy k ,poz k ,yaw k ,pitch k ,roll k ].

[0060] Optionally, in step S4, constructing a user instruction sequence and calculating the actual flight state change after the drone executes the i-th instruction includes:

[0061] S401: Construct user instruction sequence:

[0062] User i =[thro i ,yaw_v i ,pitch_v i ,roll_v i ,t i ]

[0063] Among them, i represents the user instruction sequence number, User i Indicates user instructions, thro i Indicates the throttle speed, yaw_v i Indicates the yaw rotation speed, pitch_v i Indicates the pitch rotation speed, roll_v i represents the roll angle rotation speed, t i The time when the user sent the i-th instruction;

[0064] S402: Determine the user instruction sequence User i The corresponding UAV initial flight state vector and final flight state vector:

[0065]

[0066]

[0067] Among them, init i Indicates the initial state of the drone when the user sends the i-th command, end i represents the final state of the drone after executing the ith instruction, c represents the speed of light, It indicates the distance between the UAV and the ground radar when the user sends the i-th instruction. Indicates the distance between the drone and the ground radar when the user sends the i+1th command;

[0068] S403: Calculate init i to the end i The actual flight status change of the drone:

[0069] change=[Δpox,Δpoy,Δpoz,Δyaw,Δpitch,Δroll]

[0070] Among them, Δpox represents the position change of the drone on the x-axis, Δpoy represents the position change of the drone on the y-axis, Δpoz represents the position change of the drone on the z-axis, Δyaw represents the yaw angle change of the drone, Δpitch represents the pitch angle change of the drone, and Δroll represents the roll angle change of the drone.

[0071] Optionally, in step S5, calculating the ideal flight state change after the drone executes the i-th instruction and comparing it with the actual flight state change output in step S4 to evaluate the flight state of the drone includes:

[0072] S501: The ideal flight state change after the drone executes the i-th instruction is expressed as:

[0073] ideal i =[pox′,poy′,poz′,yaw′,pitch′,roll′]

[0074] pox′=thro i ·(t i+1 -t i )

[0075] poy′=thro i ·(t i+1 -t i )·tan(yaw i ·(t i+1 -t i ))

[0076] poz′=thro i ·(t i+1 -t i )·sin(pitch i ·(t i+1 -t i ))

[0077] yaw′=yaw i ·(t i+1 -t i )

[0078] pitch′=pitch i ·(t i+1 -t i )

[0079] roll′=roll i ·(t i+1 -t i )

[0080] Among them, ideal irepresents the ideal state change after the drone executes the ith instruction, pox′ represents the ideal position change on the x-axis after the drone executes the ith instruction, poy′ represents the ideal position change on the y-axis after the drone executes the ith instruction, poz′ represents the ideal position change on the z-axis after the drone executes the ith instruction, yaw′ represents the ideal yaw angle change after the drone executes the ith instruction, pitch′ represents the ideal pitch angle change after the drone executes the ith instruction, and roll′ represents the ideal roll angle change after the drone executes the ith instruction;

[0081] S502: Ideal i Compare each element in the change to evaluate the flight status of the drone:

[0082]

[0083]

[0084] Among them, PosDeter is used to evaluate the position state of the UAV flight. PosDeter=1 indicates that the position state of the UAV flight is correct, PosDeter=0 indicates that the position state of the UAV flight is incorrect, Δd indicates the threshold value of the UAV position deviation, and FreeDeter is used to evaluate the degree of freedom state of the UAV flight. FreeDeter=1 indicates that the degree of freedom state of the UAV flight is correct, FreeDeter=0 indicates that the degree of freedom state of the UAV flight is incorrect, and Δθ indicates the threshold value of the degree of freedom deviation of the UAV.

[0085] The present invention also provides a UAV flight status judgment system, which is characterized by comprising:

[0086] UAV measurement vector module: construct UAV measurement vector;

[0087] UAV flight status filtering module: uses Kalman filtering to predict the actual flight status of the UAV;

[0088] UAV flight state vector module: construct the six-dimensional flight state vector of the UAV;

[0089] The module for calculating the change in the UAV flight state is used to construct a user command sequence and calculate the actual change in the UAV flight state after the command is executed.

[0090] UAV flight status evaluation module: calculates the ideal flight status change after the UAV executes the command, compares it with the actual flight status change, and evaluates the UAV flight status.

[0091] Beneficial Effects

[0092] The present invention realizes accurate prediction of the real flight parameters of the UAV through Kalman filter estimation; establishes a corresponding relationship between user instructions and the change of the UAV's flight state, which is conducive to intuitively displaying the change of the UAV's flight state and quantitatively comparing the actual flight state change of the UAV and the ideal flight state change of the UAV, effectively realizing the judgment of the UAV's flight state.

[0093] The present invention performs signal processing operations of denoising and phase compensation on the echo signal of the UAV, which is conducive to obtaining accurate echo signals, extracting accurate distance, azimuth, and altitude information of the UAV, and constructing accurate measurement vectors; adopts Kalman filtering, fully utilizing the relationship between the position, speed, and acceleration parameters of the UAV for estimation, which is conducive to obtaining accurate filtering results.

[0094] The present invention serializes user instructions, which is conducive to unifying the user instruction format; calculates the ideal flight state change of the drone user instruction, which provides a reference for the actual flight state change of the drone; and the item-by-item comparison method is conducive to comprehensively evaluating the flight state of the drone and performing risk control on the drone. BRIEF DESCRIPTION OF THE DRAWINGS

[0095] Figure 1 A flowchart of a method for determining the flight status of a drone provided by one embodiment of the present invention. DETAILED DESCRIPTION

[0096] The present invention is further described below in conjunction with the accompanying drawings, but the present invention is not limited in any way. Any changes or substitutions made based on the teachings of the present invention belong to the protection scope of the present invention.

[0097] Embodiment 1:

[0098] A method for determining the flight status of a UAV, such as Figure 1 As shown, the following steps are included:

[0099] S1: Use the distance, azimuth and altitude parameters of the UAV relative to the ground radar to construct the UAV measurement vector;

[0100] A1: The ground radar receives the echo signal ech from the UAV at time k and decomposes the echo signal ech into wavelet coefficients of different scales:

[0101]

[0102] Among them, Wvc(achi,b) represents the wavelet coefficient of ech, achi is the scale parameter, b is the translation parameter, ψ * is the complex conjugate of the wavelet function;

[0103] Threshold processing is performed on the wavelet coefficient Wvc(achi,b) to remove the high-frequency coefficients containing noise and obtain the wavelet coefficient Wsig(achi,b) containing only the target;

[0104] Using Wsig(achi,b), the denoised drone echo signal dec is obtained through signal reconstruction;

[0105] A2: Select the reference blade of the drone and establish the phase compensation factor

[0106] phc=exp(j·amp·cos(ang·m+inp))

[0107] Wherein, phc represents the reference blade phase compensation factor, exp represents the exponential function, j represents the imaginary sign, amp represents the amplitude coefficient vector, and ang represents the angular velocity coefficient vector;

[0108] Perform phase compensation on the drone echo signal to obtain the compensated echo signal:

[0109] dec′=dec·phc

[0110] Where, dec′ represents the echo signal after compensation;

[0111] A3: Perform a two-dimensional Fourier transform on dec′ to obtain the range-Doppler image rd, perform a one-dimensional constant false alarm detection on rd, and obtain the distance rang of the UAV relative to the ground radar at time k k ;

[0112] A4: Perform a three-dimensional Fourier transform on dec′(t) to obtain the spatial frequency domain response in different directions. Use the extreme value detection method to obtain the azimuth angle azi of the UAV relative to the ground radar at time k k ;

[0113] A5: Calculate the height of the drone relative to the ground at time k k :

[0114] high k =imw·gsd·Fr / (sw·100)

[0115] Among them, imw represents the width of the image taken by the drone, gsd represents the ground resolution, Fr represents the real focal length of the drone camera, and sw represents the real width of the drone camera.

[0116] S101: With the ground radar as the coordinate origin, the UAV's forward direction as the x-axis direction, the direction perpendicular to the UAV's forward direction on the horizontal plane as the y-axis direction, and the vertical direction as the z-axis direction, calculate the UAV's position at time k:

[0117]

[0118]

[0119] meaz k =high k

[0120] Among them, meax k represents the measured position of the drone on the x-axis at time k, meay k represents the measured position of the drone on the y-axis at time k, meaz k represents the measured position of the drone on the z-axis at time k;

[0121] S102: Constructing the UAV's measurement vector

[0122] measure k =[meax k ,meay k ,meaz k ]

[0123] Among them, measure represents the measurement vector of the drone.

[0124] S2: Use Kalman filtering to filter the measurement vector of the UAV and predict the actual flight status of the UAV;

[0125] S201: Establish the real state vector of the drone at time k:

[0126] state k =[pox k ,poy k ,poz k ,vx k ,vy k ,vz k ,accex k ,accey k ,accez k ]

[0127] Among them, state k represents the real state vector of the drone at time k, pox k represents the real position of the drone on the x-axis at time k, poy k represents the real position of the drone on the y axis at time k, poz k represents the real position of the drone on the z axis at time k, vx k represents the real speed of the drone on the x-axis at time k, vy k represents the real speed of the drone on the y axis at time k, vz krepresents the real speed of the drone on the z axis at time k, accex k represents the actual acceleration of the drone on the x-axis at time k, accey k represents the actual acceleration of the drone on the y-axis at time k, accez k represents the real acceleration of the drone on the z-axis at time k;

[0128] S202: Predicting the actual flight status of a drone

[0129] state k+1|k =F·state k

[0130] P k+1|k =FP k F T +Q k

[0131] Among them, state k+1|k represents the real state vector prediction vector of the UAV at time (k+1), F represents the state transfer matrix of the UAV motion, and F T represents the transpose of matrix F, P k represents the covariance matrix value at time k, P k+1|k represents the predicted value of the covariance matrix at time (k+1), Q k represents the k-time process noise covariance matrix;

[0132] For example, if the UAV motion state is uniform linear motion, the state transfer matrix F of the UAV motion is:

[0133]

[0134] The motion state of the drone is uniformly accelerated linear motion, and the state transfer matrix F of the drone motion is:

[0135]

[0136] The motion state of the drone is uniform turning motion, and the state transfer matrix F of the drone motion is:

[0137]

[0138] Among them, ζ represents the angular velocity of the turning motion;

[0139] The motion state of the drone is uniform acceleration turning motion, and the state transfer matrix F of the drone motion is:

[0140]

[0141] S203: Calculate Kalman gain

[0142] Kalz k+1 =P k+1|k H T (HP k+1|k H T +R k+1 ) -1

[0143] Kalz k+1 represents the Kalman gain at time (k+1), R k+1 represents the observation noise covariance, (HP k+1|k H T +R k+1 ) -1 Indicates HP k+1|k H T +R k+1 Find the inverse matrix;

[0144] S204: The actual state of the drone at time (k+1) k+1 Updated to:

[0145] state k+1 =state k+1|k +Kalz k+1 (measure k+1 -H·state k+1|k )

[0146] state k+1 =[pox k+1 ,poy k+1 ,poz k+1 ,vx k+1 ,vy k+1 ,vz k+1 ,accex k+1 ,accey k+1 ,accez k+1 ]

[0147] Among them, pox k+1 represents the real position of the drone on the x-axis at time (k+1), poy k+1 represents the real position of the drone on the y axis at time (k+1), poz k+1 represents the real position of the drone on the z-axis at time (k+1), vx k+1 represents the real speed of the drone on the x-axis at time (k+1), vy k+1 represents the real speed of the drone on the y axis at time (k+1), vz k+1 represents the real speed of the drone on the z-axis at time (k+1), accex k+1represents the actual acceleration of the drone on the x-axis at time (k+1), accey k+1 represents the actual acceleration of the drone on the y-axis at time (k+1), accez k+1 represents the actual acceleration of the drone on the z-axis at time (k+1), measure k+1 Represents the measurement vector of the UAV at time k+1.

[0148] S3: Calculate the yaw angle, pitch angle and roll angle of the UAV through the predicted real flight state of the UAV, and construct the six-dimensional flight state vector of the UAV;

[0149] S301: Calculate the yaw angle of the drone at time k k , pitch angle k , roll angle roll k :

[0150] yaw k =arctan(accez k / sqrt(accex k 2 +accez k 2 ))

[0151] pitch k =arctan(accex k / sqrt(accey k 2 +accez k 2 ))

[0152] roll k =arctan(accey k / sqrt(accex k 2 +accez k 2 ))

[0153] S302: Construct the six-dimensional flight state vector UAV at time k k :

[0154] UAV k =[pox k ,poy k ,poz k ,yaw k ,pitch k ,roll k ].

[0155] S4: Construct a user command sequence and calculate the actual flight state change after the drone executes the i-th command;

[0156] S401: Construct user instruction sequence:

[0157] User i =[thro i ,yaw_v i ,pitch_v i ,roll_v i ,t i ]

[0158] Among them, i represents the user instruction sequence number, User i Indicates user instructions, thro i Indicates the throttle speed, yaw_v i Indicates the yaw rotation speed, pitch_v i Indicates the pitch rotation speed, roll_v i represents the roll angle rotation speed, t i The time when the user sent the i-th instruction;

[0159] S402: Determine the user instruction sequence User i The corresponding UAV initial flight state vector and final flight state vector:

[0160]

[0161]

[0162] Among them, init i Indicates the initial state of the drone when the user sends the i-th command, end i represents the final state of the drone after executing the i-th instruction, c represents the speed of light, and range ti represents the distance between the UAV and the ground radar when the user sends the i-th command, ti+1 Indicates the distance between the drone and the ground radar when the user sends the i+1th command;

[0163] S403: Calculate init i to the end i The actual flight status change of the drone:

[0164] change=[Δpox,Δpoy,Δpoz,Δyaw,Δpitch,Δroll]

[0165] Among them, Δpox represents the position change of the drone on the x-axis, Δpoy represents the position change of the drone on the y-axis, Δpoz represents the position change of the drone on the z-axis, Δyaw represents the yaw angle change of the drone, Δpitch represents the pitch angle change of the drone, and Δroll represents the roll angle change of the drone.

[0166] S5: Calculate the ideal flight state change after the UAV executes the i-th instruction, and compare it with the actual flight state change output in step S4 to evaluate the UAV flight state;

[0167] S501: The ideal flight state change after the drone executes the i-th instruction is expressed as:

[0168] ideal i =[pox′,poy′,poz′,yaw′,pitch′,roll′]

[0169] pox′=thro i ·(t i+1 -t i )

[0170] poy′=thro i ·(t i+1 -t i )·tan(yaw i ·(t i+1 -t i ))

[0171] poz′=thro i ·(t i+1 -t i )·sin(pitch i ·(t i+1 -t i ))

[0172] yaw′=yaw i ·(t i+1 -t i )

[0173] pitch′=pitch i ·(t i+1 -t i )

[0174] roll′=roll i ·(t i+1 -t i )

[0175] Among them, ideal irepresents the ideal state change after the drone executes the ith instruction, pox′ represents the ideal position change on the x-axis after the drone executes the ith instruction, poy′ represents the ideal position change on the y-axis after the drone executes the ith instruction, poz′ represents the ideal position change on the z-axis after the drone executes the ith instruction, yaw′ represents the ideal yaw angle change after the drone executes the ith instruction, pitch′ represents the ideal pitch angle change after the drone executes the ith instruction, and roll′ represents the ideal roll angle change after the drone executes the ith instruction;

[0176] S502: Ideal i Compare each element in the change to evaluate the flight status of the drone:

[0177]

[0178]

[0179] Among them, PosDeter is used to evaluate the position state of the UAV flight. PosDeter=1 indicates that the position state of the UAV flight is correct, PosDeter=0 indicates that the position state of the UAV flight is incorrect, Δd indicates the threshold value of the UAV position deviation, and FreeDeter is used to evaluate the degree of freedom state of the UAV flight. FreeDeter=1 indicates that the degree of freedom state of the UAV flight is correct, FreeDeter=0 indicates that the degree of freedom state of the UAV flight is incorrect, and Δθ indicates the threshold value of the degree of freedom deviation of the UAV.

[0180] For example, a user sends a sequence of commands to a drone to complete a target mission. We determine the execution result of each command, that is, the actual change in the drone's flight state, by the time the drone receives the command. Based on the difference between the actual execution result of the command and the expected result, we judge the drone's flight state, adjust subsequent commands, and reasonably set the command parameters.

[0181] Embodiment: 2:

[0182] The present invention also provides a UAV flight status judgment system, which includes the following five modules:

[0183] UAV measurement vector module: construct UAV measurement vector;

[0184] UAV flight status filtering module: uses Kalman filtering to predict the actual flight status of the UAV;

[0185] UAV flight state vector module: construct the six-dimensional flight state vector of the UAV;

[0186] The module for calculating the change in the UAV flight state is used to construct a user command sequence and calculate the actual change in the UAV flight state after the command is executed.

[0187] UAV flight status evaluation module: calculates the ideal flight status change after the UAV executes the command, compares it with the actual flight status change, and evaluates the UAV flight status.

[0188] It should be noted that the serial numbers of the above embodiments of the present invention are only for description and do not represent the advantages and disadvantages of the embodiments. And the terms "including", "comprising" or any other variants thereof in this article are intended to cover non-exclusive inclusion, so that a process, device, article or method including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, device, article or method. In the absence of further restrictions, an element defined by the sentence "including a ..." does not exclude the presence of other identical elements in the process, device, article or method including the element.

[0189] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus a necessary general hardware platform, and of course by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present invention is essentially or the part that contributes to the prior art can be embodied in the form of a software product, which is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk) as described above, and includes a number of instructions for a terminal device (which can be a mobile phone, a computer, a server, or a network device, etc.) to execute the methods described in each embodiment of the present invention.

[0190] The above are only preferred embodiments of the present invention, and are not intended to limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made using the contents of the present invention specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A method for determining the flight status of an unmanned aerial vehicle, characterized in that: The method comprises: S1: Calculate the distance, azimuth and altitude parameters of the UAV relative to the ground radar and construct the measurement vector of the UAV; S2: Use Kalman filtering to filter the measurement vector of the UAV and predict the actual flight status of the UAV; S3: Calculate the yaw angle, pitch angle and roll angle of the UAV through the predicted real flight state of the UAV, and construct the six-dimensional flight state vector of the UAV; S4: Construct a user command sequence and calculate the actual flight state change after the drone executes the i-th command; S5: Calculate the ideal flight state change after the UAV executes the i-th instruction, and compare it with the actual flight state change output in step S4 to evaluate the UAV flight state; The step S1 comprises: S101: With the ground radar as the coordinate origin, the UAV's forward direction as the x-axis direction, the direction perpendicular to the UAV's forward direction on the horizontal plane as the y-axis direction, and the vertical direction as the z-axis direction, calculate the UAV's position at time k: money k =high k Among them, meax k represents the measured position of the drone on the x-axis at time k, meay k represents the measured position of the drone on the y-axis at time k, meaz k represents the measured position of the drone on the z-axis at time k, k represents the distance of the UAV relative to the ground radar at time k, high k represents the height of the drone relative to the ground at time k, azi k represents the azimuth of the UAV relative to the ground radar at time k; S102: Constructing the UAV's measurement vector measure k =[meax k ,meay k ,noon k ] Among them, measure k represents the measurement vector of the UAV at time k; The step S2 comprises: S201: Establish the real state vector of the drone at time k: state k =[pox k ,poy k ,poz k ,vx k ,vy k ,vz k ,accex k ,accey k ,accez k ] Among them, state k represents the real state vector of the drone at time k, pox k represents the real position of the drone on the x-axis at time k, poy k represents the real position of the drone on the y axis at time k, poz k represents the real position of the drone on the z axis at time k, vx k represents the real speed of the drone on the x-axis at time k, vy k represents the real speed of the drone on the y axis at time k, vz k represents the real speed of the drone on the z axis at time k, accex k represents the actual acceleration of the drone on the x-axis at time k, accey k represents the actual acceleration of the drone on the y-axis at time k, accez k represents the real acceleration of the drone on the z-axis at time k; S202: Predicting the actual flight status of a drone state k+1|k =F·state k P k+1|k =FP k F T +Q k Among them, state k+1|k represents the real state vector prediction vector of the UAV at time (k+1), F represents the state transfer matrix of the UAV motion, and F T represents the transpose of matrix F, P k represents the covariance matrix value at time k, P k+1|k represents the predicted value of the covariance matrix at time (k+1), Q k represents the k-time process noise covariance matrix; S203: Calculate Kalman gain Kalz k+1 =P k+1|k H T (HP k+1|k H T +R k+1 ) -1 Kalz k+1 represents the Kalman gain at time (k+1), R k+1 represents the observation noise covariance, (HP k+1|k H T +R k+1 ) -1 Indicates HP k+1|k H T +R k+1 Find the inverse matrix; S204: The actual flight state of the drone at time (k+1) k+1 Updated to: state k+1 =state k+1|k +Kalz k+1 (measure k+1 -H·state k+1|k ) state k+1 =[pox k+1 ,poy k+1 ,poz k+1 ,vx k+1 ,vy k+1 ,vz k+1 ,accex k+1 ,accey k+1 ,accez k+1 ] Among them, pox k+1 represents the real position of the drone on the x-axis at time (k+1), poy k+1 represents the real position of the drone on the y axis at time (k+1), poz k+1 represents the real position of the drone on the z-axis at time (k+1), vx k+1 represents the real speed of the drone on the x-axis at time (k+1), vy k+1 represents the real speed of the drone on the y axis at time (k+1), vz k+1 represents the real speed of the drone on the z-axis at time (k+1), accex k+1 represents the actual acceleration of the drone on the x-axis at time (k+1), accey k+1 represents the actual acceleration of the drone on the y-axis at time (k+1), accez k+1 represents the actual acceleration of the drone on the z-axis at time (k+1), measure k+1 Represents the measurement vector of the UAV at time k+1.

2. The method for determining the flight status of a drone according to claim 1, wherein: The step S3 comprises: S301: Calculate the yaw angle of the drone at time k k , pitch angle k , roll angle roll k : yaw k =arctan(accez k / sqrt(accex k 2 +accez k 2 )) pitch k =arctan(accex k / sqrt(accey k 2 +accez k 2 )) roll k =arctan(accey k / sqrt(accex k 2 +accez k 2 )) S302: Construct the six-dimensional flight state vector UAV at time k k : UAV k =[pox k ,poy k ,poz k ,yaw k ,pitch k ,roll k ]。 3. A method for determining the flight status of an unmanned aerial vehicle according to claim 2, characterized in that: The step S4 comprises: S401: Construct user instruction sequence: User i =[thro i ,yaw_v i ,pitch_v i ,roll_v i ,t i ] Among them, i represents the user instruction sequence number, User i Indicates user instructions, thro i Indicates the throttle speed, yaw_v i Indicates the yaw rotation speed, pitch_v i Indicates the pitch rotation speed, roll_v i represents the roll angle rotation speed, t i The time when the user sent the i-th instruction; S402: Determine the user instruction sequence User i The corresponding UAV initial flight state vector and final flight state vector: Among them, init i Indicates the initial state of the drone when the user sends the i-th command, end i represents the final state of the drone after executing the ith instruction, c represents the speed of light, It indicates the distance between the UAV and the ground radar when the user sends the i-th instruction. Indicates the distance between the UAV and the ground radar when the user sends the i+1th command; S403: Calculate init i to the end i The actual flight status change of the drone: change=[Δpox,Δpoy,Δpoz,Δyaw,Δpitch,Δroll] Among them, Δpox represents the position change of the drone on the x-axis, Δpoy represents the position change of the drone on the y-axis, Δpoz represents the position change of the drone on the z-axis, Δyaw represents the yaw angle change of the drone, Δpitch represents the pitch angle change of the drone, and Δroll represents the roll angle change of the drone.

4. The method for determining the flight status of a drone as claimed in claim 3, characterized in that: The step S5 comprises: S501: The ideal flight state change after the drone executes the i-th instruction is expressed as: ideal i =[pox′,poy′,poz′,yaw′,pitch′,roll′] pox′=thro i ·(t i+1 -t i ) poy′=thro i ·(t i+1 -t i )·tan(yaw i ·(t i+1 -t i )) poz′=thro i ·(t i+1 -t i )·sin(pitch i ·(t i+1 -t i )) yaw′=yaw i ·(t i+1 -t i ) pitch′=pitch i ·(t i+1 -t i ) roll′=roll i ·(t i+1 -t i ) Among them, ideal i represents the ideal state change after the drone executes the ith instruction, pox′ represents the ideal position change on the x-axis after the drone executes the ith instruction, poy′ represents the ideal position change on the y-axis after the drone executes the ith instruction, poz′ represents the ideal position change on the z-axis after the drone executes the ith instruction, yaw′ represents the ideal yaw angle change after the drone executes the ith instruction, pitch′ represents the ideal pitch angle change after the drone executes the ith instruction, and roll′ represents the ideal roll angle change after the drone executes the ith instruction; S502: Ideal i Compare each element in the change to evaluate the flight status of the drone: Among them, PosDeter is used to evaluate the position state of the UAV flight. PosDeter=1 indicates that the position state of the UAV flight is correct, PosDeter=0 indicates that the position state of the UAV flight is incorrect, Δd indicates the threshold value of the UAV position deviation, and FreeDeter is used to evaluate the degree of freedom state of the UAV flight. FreeDeter=1 indicates that the degree of freedom state of the UAV flight is correct, FreeDeter=0 indicates that the degree of freedom state of the UAV flight is incorrect, and Δθ indicates the threshold value of the degree of freedom deviation of the UAV.

5. A UAV flight status judgment system, characterized in that: include: UAV measurement vector module: construct UAV measurement vector; UAV flight status filtering module: uses Kalman filtering to predict the actual flight status of the UAV; UAV flight state vector module: construct the six-dimensional flight state vector of the UAV; The module for calculating the change in the UAV flight state is used to construct a user command sequence and calculate the actual change in the UAV flight state after the command is executed. UAV flight status evaluation module: calculates the ideal flight status change after the UAV executes the command, compares it with the actual flight status change, and evaluates the UAV flight status; To realize a method for determining the flight status of a UAV as described in any one of claims 1 to 4.

Images