Unmanned aerial vehicle state rapid estimation and fault tolerance method based on multiple anchor points and polynomial fitting

Abstract

The invention discloses an unmanned aerial vehicle state rapid estimation and fault tolerance method based on multiple anchor points and polynomial fitting. An unmanned aerial vehicle state estimation algorithm is initialized by using information such as positions of multiple anchor points; based on the current state information of the unmanned aerial vehicle, solving an optimization problem of a cost function related to the positions of the plurality of anchor points, and obtaining an analytic solution expression of current optimal state estimation; in order to improve the operation efficiency, a polynomial fitting method is further utilized to approach an analytical solution; the estimation of the unmanned aerial vehicle on the current state is continuously updated through repeated iteration; the method is suitable for the unmanned aerial vehicle to execute tasks under the condition that the information of the visual sensor is lost, so that the safety and reliability of the unmanned aerial vehicle are remarkably improved.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicles, and in particular to a method for rapid estimation and fault tolerance of unmanned aerial vehicle states based on multiple anchor points and polynomial fitting. Background Art

[0002] State estimation is one of the most important tasks in autonomous cruising of drones. Its importance lies in providing drones with real-time and accurate information about their own position and posture, so as to ensure that drones can fly safely and smoothly according to the preset route. In this process, the state estimation problem usually involves the fusion of multiple sensor information, including inertial, UWB ranging and vision. Inertial sensors, such as gyroscopes, provide the acceleration and speed of drones. UWB ranging technology provides accurate positioning information for drones by measuring the distance between drones and known anchor points. Visual sensors provide drones with rich visual features by capturing image information in the environment, thereby helping drones identify and locate their own positions.

[0003] However, in special environments, due to lighting conditions, occlusion, lack of texture and other reasons, visual sensors cannot obtain reliable information; under such conditions, how to perform state estimation based on inertial and UWB ranging information has become an important research direction.

[0004] In the past, relevant researchers have designed algorithms for positioning through a single UWB anchor point. These algorithms eliminate the steps of pre-installation and calibration of the anchor point. However, it is far from enough to use only a single UWB ranging data with errors for high-precision state estimation, and sometimes it cannot meet the engineering needs in specific situations. The existing multi-anchor solution methods are usually based on geometric solutions and cannot tolerate the failure of some anchor points. Therefore, it is necessary to design a method for rapid estimation and fault tolerance of UAV states based on multiple anchor points and polynomial fitting to solve the above problems. Summary of the invention

[0005] The technical problem to be solved by the present invention is to provide a method for rapid estimation and fault tolerance of UAV state based on multiple anchor points and polynomial fitting. The method aims to solve the problem of insufficient accuracy of the existing technology, can effectively tolerate the functional failure of some anchor points, and can achieve reliable positioning when at least one anchor point is available. The method is used for UAVs to perform special tasks in an environment that lacks visual sensor information but still requires high-precision state estimation, thereby improving the safety of UAV operations.

[0006] In order to achieve the above technical effects, the technical solution adopted by the present invention is:

[0007] A method for rapid estimation and fault tolerance of UAV state based on multiple anchor points and polynomial fitting includes the following steps:

[0008] S1, initialize the parameters of the UAV state estimation algorithm;

[0009] S2, construct a cost function that considers multiple UWB ranging information and obtains an analytical solution expression containing the state information at the next moment;

[0010] S3, use polynomial fitting to approximate the analytical solution and obtain the state at the next moment;

[0011] S4, determine the working status of the drone. If the drone is still operating, return to step S2 to loop and recalculate the status at the next moment; if the drone stops operating, the program stops iterating.

[0012] Preferably, in step S1, the initialization parameters include the drag coefficient matrix μ, the number of anchor points n, the coefficient matrix P, Q, R of the cost function i (i=1, 2, ..., n), sliding window length k w , the initial state vector sequence Anchor point position coordinate vector β, polynomial highest approximation coefficient f1, UWB ranging frequency f2, IMU measurement frequency and adjacent state time interval dt.

[0013] Preferably, in step S2, a cost function considering multiple UWB ranging information is constructed to obtain an analytical solution expression containing the state information at the next moment, including:

[0014] Construct the system state equation:

[0015]

[0016] In the formula, I 3×3 and O 3×3 are the abbreviations of the identity matrix and the zero matrix with row and column dimensions of 3 respectively; μ 3×μ =diag(μ x ,μ y ,μ z ) represents the air resistance coefficient; x represents the drone state vector, which is specifically expressed as Where p represents the position vector of the drone, v represents the velocity vector of the drone; u represents the control input of the system, that is, the acceleration vector of the drone; y represents the measurement output of the system, and h(x) represents that there is a certain functional relationship between the measurement output of the system and the state vector x of the drone. In this method, the measurement value is the multi-anchor distance measurement value, so r1, r2…r n They represent the relative distance values ​​between the n anchor points and the drone respectively;

[0017] Discretize the equation with sampling frequency dt to obtain the discrete control equation:

[0018] x<k+1> =A <k>< / k> x <k>< / k> +B <k>< / k> u <k>< / k>

[0019] y <k+1> =h(x <k+1> )=[r 1<k+1> ,r 2<k+1> ,…,r n<k+1> ] T ;

[0020] Where k represents the kth sampling time step after discretization, and the matrix A <k>< / k> and B <k>< / k> The calculation formula is:

[0021]

[0022] Since the matrix A <k>< / k> and B <k>< / k> The value does not change when the sampling frequency dt is fixed, so it can be abbreviated in the subsequent derivation process, and let A = A <k>< / k> ,B=B <k>< / k> . In addition, the system cost function is:

[0023]

[0024] Where J represents the system error objective function, argmin x Indicates that we want to obtain the variable that minimizes the objective function i means that the width is k w The i-th state in the sliding window of , j represents the measurement error fed back by the j-th ranging unit; e1, e2, e3 represent the prior error, process error and measurement error respectively; where:

[0025]

[0026]

[0027]

[0028] In the formula, It represents the relative distance between the jth ranging unit and the drone at the i-th moment. Represents the relative distance relationship between each distance measuring unit and the first distance measuring unit, that is, the origin set by the system; where, represents the relative distance vector between the jth ranging unit and the origin, It represents the position vector of the UAV in the origin coordinate system at the i-th moment, that is,

[0029] The first item 1 The current state is required to be similar to the state at the previous moment. The second item e 2 The current state is required to be similar to the result obtained by using the state at the previous moment and the state equation. 3·j It is required that the n UWB ranging results are similar to the current state calculation result, p represents the estimated position of the UAV at the i-th moment; let:

[0030]

[0031] In the formula, E represents the error matrix of the system, and W represents the corresponding weight matrix;

[0032] The cost function optimization problem is transformed into:

[0033]

[0034] The specific representation of each matrix is ​​as follows:

[0035]

[0036]

[0037]

[0038]

[0039] In the formula, represents the concatenation of the identity matrix and the zero matrix, represents the concatenation of the identity matrix and the A matrix, They represent the concatenation of C and C matrices respectively, the D matrix represents the correlation of the measured values ​​in the sliding window, and the θ matrix represents the aggregate of the prior estimation, acceleration value, and ranging correlation value;

[0040] By taking partial derivatives, we can get the analytical solution:

[0041]

[0042] at the same time Represents the estimate of the current state at the previous moment, that is, In particular, it can be considered

[0043] The actual IMU sampling frequency is higher than the UWB distance measurement sampling frequency. There is a The measurement of the acceleration is recorded as The pre-integration method is introduced to make full use of IMU measurement data:

[0044] Modify the first formula of the state control equation:

[0045]

[0046] In the formula, the superscript l represents the number of acceleration measurements;

[0047] θ is modified to:

[0048]

[0049] Will The expression is modified to:

[0050]

[0051] In the formula, A l Represents the A matrix corresponding to the lth accelerometer measurement in a set of data.

[0052] Preferably, the specific method of approximating the analytical solution by polynomial fitting and obtaining the state at the next moment is:

[0053] Since the calculation time required to directly obtain the analytical solution is too long, a polynomial with the highest order term of β is used to approximate the analytical solution. The original cost function optimization problem can be rewritten as:

[0054]

[0055] In the formula, Represents the corresponding coefficients of the polynomial that approximates the analytical solution;

[0056] In the t i At this moment, the above matrix is ​​specifically expressed as:

[0057]

[0058] In the formula, α represents the coefficient matrix corresponding to the polynomial approximating the analytical solution, T represents the time interval matrix corresponding to the polynomial approximating the analytical solution, and t l represents the i-th time point, t0 represents the initial time point in the sliding window, Represents the time interval between the i-th time point and the initial time in the sliding window;

[0059] Get the corresponding coefficients of the polynomial that approximates the analytical solution

[0060]

[0061] In the formula, E r represents the process matrix after polynomial approximation, E θ represents the measurement matrix after polynomial approximation;

[0062] Finally, we get an approximate analytical solution Polynomial solution of

[0063] The beneficial effects of the present invention are as follows:

[0064] 1. This method can estimate the state of the UAV in real time with high precision during its motion, and can still work normally when some anchor points fail. It only uses multiple anchor points and IMU to estimate the state of the UAV in real time, and uses the pre-integration method to make full use of IMU and anchor point data with different sampling frequencies. By designing a long-time window cost function and obtaining the optimal solution, the analytical expression of the best state estimation is obtained. By using the polynomial fitting method to approximate the optimal solution, the calculation speed is improved.

[0065] 2. This method can effectively tolerate the failure of some anchor points and achieve reliable positioning when at least one anchor point is available. This method is used for drones to perform special tasks in environments that lack visual sensor information but still require high-precision state estimation, thereby improving the safety of drone operations. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] Figure 1 It is a schematic diagram of the process of the present invention;

[0067] Figure 2 It is a schematic diagram of a specific implementation of the present invention. DETAILED DESCRIPTION

[0068] Embodiment 1:

[0069] like Figure 1 As shown, a method for rapid estimation and fault tolerance of UAV state based on multiple anchor points and polynomial fitting includes the following steps:

[0070] S1, initialize the parameters of the UAV state estimation algorithm;

[0071] S2, construct a cost function that considers multiple UWB ranging information and obtains an analytical solution expression containing the state information at the next moment;

[0072] S3, use polynomial fitting to approximate the analytical solution and obtain the state at the next moment;

[0073] S4, determine the working status of the drone. If the drone is still operating, return to step S2 to loop and recalculate the status at the next moment; if the drone stops operating, the program stops iterating.

[0074] Preferably, in step S1, the initialization parameters include the drag coefficient matrix μ, the number of anchor points n, the coefficient matrix P, Q, R of the cost function i (i=1, 2, ..., n), sliding window length k w , the initial state vector sequence Anchor point position coordinate vector β, polynomial highest approximation coefficient f1, UWB ranging frequency f2, IMU measurement frequency and adjacent state time interval dt.

[0075] Preferably, in step S2, a cost function considering multiple UWB ranging information is constructed to obtain an analytical solution expression containing the state information at the next moment, including:

[0076] Construct the system state equation:

[0077]

[0078] In the formula, I 3×3 and O 3×3 are the abbreviations of the identity matrix and the zero matrix with row and column dimensions of 3 respectively; μ ×3 =diag(μ x ,μ y ,μ z ) represents the air resistance coefficient; x represents the state vector of the drone, which is specifically expressed as x = [p T ,v T ] T , Where p represents the position vector of the drone, v represents the velocity vector of the drone; u represents the control input of the system, that is, the acceleration vector of the drone; y represents the measurement output of the system, and h(x) represents that there is a certain functional relationship between the measurement output of the system and the state vector x of the drone. In this method, the measurement value is the multi-anchor distance measurement value, so r1, r2…r n They represent the relative distance values ​​between the n anchor points and the drone respectively;

[0079] Discretize the equation with sampling frequency dt to obtain the discrete control equation:

[0080] x <k+1> =A <k>< / k> x <k>< / k> +B <k>< / k> u <k>< / k>

[0081] y <k+1> =h(x <k+1> )=[r 1<k+1> ,r 2<k+1< ,…,r n<k+1> ] T ;

[0082] Where k represents the kth sampling time step after discretization, and the matrix A <k>< / k> and B <k>< / k> The calculation formula is:

[0083]

[0084] Since the matrix A <k>< / k> and B <k>< / k> The value does not change when the sampling frequency dt is fixed, so it can be abbreviated in the subsequent derivation process, and let A = A <k>< / k> ,B=B <k>< / k> . In addition, the system cost function is:

[0085]

[0086] Where J represents the system error objective function, argmin x Indicates that we want to obtain the variable that minimizes the objective function i means that the width is k w The i-th state in the sliding window of , j represents the measurement error fed back by the j-th ranging unit; e1, e2, e3 represent the prior error, process error and measurement error respectively; where:

[0087]

[0088]

[0089]

[0090]

[0091]

[0092] In the formula, It represents the relative distance between the jth ranging unit and the drone at the i-th moment. Represents the relative distance relationship between each distance measuring unit and the first distance measuring unit, that is, the origin set by the system; where, represents the relative distance vector between the jth ranging unit and the origin, It represents the position vector of the UAV in the origin coordinate system at the i-th moment, that is,

[0093] The first item 1 The current state is required to be similar to the state at the previous moment. The second item e 2 The current state is required to be similar to the result obtained by using the state at the previous moment and the state equation. 3·j It is required that the n UWB ranging results are similar to the current state calculation result, p represents the estimated position of the UAV at the i-th moment; let:

[0094]

[0095]

[0096] In the formula, E represents the error matrix of the system, and W represents the corresponding weight matrix;

[0097] The cost function optimization problem is transformed into:

[0098]

[0099] The specific representation of each matrix is ​​as follows:

[0100]

[0101]

[0102]

[0103]

[0104] In the formula, represents the concatenation of the identity matrix and the zero matrix, represents the concatenation of the identity matrix and the A matrix, They represent the concatenation of C and C matrices respectively, the D matrix represents the correlation of the measured values ​​in the sliding window, and the θ matrix represents the aggregate of the prior estimation, acceleration value, and ranging correlation value;

[0105] By taking partial derivatives, we can get the analytical solution:

[0106]

[0107] at the same time Represents the estimate of the current state at the previous moment, that is, In particular, it can be considered

[0108] The actual IMU sampling frequency is higher than the UWB distance measurement sampling frequency. There is a The measurement of the acceleration is recorded as The pre-integration method is introduced to make full use of IMU measurement data:

[0109] Modify the first formula of the state control equation:

[0110]

[0111] In the formula, the superscript l represents the number of acceleration measurements;

[0112] θ is modified to:

[0113]

[0114] Will The expression is modified to:

[0115]

[0116] In the formula, A l Represents the A matrix corresponding to the lth accelerometer measurement in a set of data.

[0117] Preferably, the specific method of approximating the analytical solution by polynomial fitting and obtaining the state at the next moment is:

[0118] Since the calculation time required to directly obtain the analytical solution is too long, a polynomial with the highest order term of β is used to approximate the analytical solution. The original cost function optimization problem can be rewritten as:

[0119]

[0120] In the formula, Represents the corresponding coefficients of the polynomial that approximates the analytical solution;

[0121] In the t i At this moment, the above matrix is ​​specifically expressed as:

[0122]

[0123] In the formula, α represents the coefficient matrix corresponding to the polynomial approximating the analytical solution, T represents the time interval matrix corresponding to the polynomial approximating the analytical solution, and t l represents the i-th time point, t0 represents the initial time point in the sliding window, Represents the time interval between the i-th time point and the initial time in the sliding window;

[0124] Get the corresponding coefficients of the polynomial that approximates the analytical solution

[0125]

[0126] In the formula, E r represents the process matrix after polynomial approximation, E θ represents the measurement matrix after polynomial approximation;

[0127] Finally, we get an approximate analytical solution Polynomial solution of

[0128] Embodiment 2:

[0129] like Figure 2 As shown in the figure, it is a specific scene layout diagram; 4 anchor points are placed around the scene, and the anchor point coordinates and drone coordinates are determined by pre-calibration. The drone itself is equipped with IMU and UWB, and the distance between the ground anchor point and the drone is measured through the UWB module. According to the actual situation, relevant parameters are input into the back end, including the drag coefficient matrix, the number of anchor points, the coefficient matrix of the cost function, the sliding window length, the initial state vector sequence, the anchor point position coordinates, the highest polynomial approximation coefficient, the UWB ranging frequency, the IMU measurement frequency, and the time interval between adjacent states. At the same time, the drone starts to move, and at the same time provides the UWB ranging information and IMU information to the back end in real time. The back end continuously updates the current state estimation through the information sent by the drone, so as to achieve the effect of estimating the current drone state.

Claims

1. A method for rapid estimation and fault tolerance of UAV state based on multiple anchor points and polynomial fitting, characterized in that: The following steps are involved: S1, initialize the parameters of the UAV state estimation algorithm; S2, construct a cost function that considers multiple UWB ranging information and obtains an analytical solution expression containing the state information at the next moment; S3, use polynomial fitting to approximate the analytical solution and obtain the state at the next moment; S4, determine the working status of the drone. If the drone is still operating, return to step S2 to loop and recalculate the status at the next moment; if the drone stops operating, the program stops iterating.

2. According to claim 1, a method for rapid estimation and fault tolerance of drone state based on multiple anchor points and polynomial fitting, characterized in that: In step S1, the initialization parameters include the air resistance coefficient matrix μ, the number of anchor points n, the coefficient matrix of the cost function P, Q, R i (i=1, 2, ..., n), sliding window length k w , the initial state vector sequence Anchor point position coordinate vector β, polynomial highest approximation coefficient f1, UWB ranging frequency f2, IMU measurement frequency and adjacent state time interval dt.

3. The method for rapid estimation and fault tolerance of drone state based on multiple anchor points and polynomial fitting according to claim 2 is characterized in that: In step S2, a cost function considering multiple UWB ranging information is constructed, and the analytical solution expression containing the state information at the next moment is obtained, including: Construct the system state equation: In the formula, I 3×3 and 0 3×3 are the abbreviations of the identity matrix and the zero matrix with row and column dimensions of 3 respectively; μ ×3 =diag(μ x ,μ y ,μ z ) represents the air resistance coefficient; x represents the state vector of the drone, which is specifically expressed as x = [p T ,v T ] T , Where p represents the position vector of the drone, v represents the velocity vector of the drone; u represents the control input of the system, that is, the acceleration vector of the drone; y represents the measurement output of the system, h(x) represents that there is a certain functional relationship between the measurement output of the system and the state vector x of the drone, and the measurement value is the multi-anchor distance measurement value, r1, r2…r n They represent the relative distance values ​​between the n anchor points and the drone respectively; Discretize the equation with sampling frequency dt to obtain the discrete control equation: x <k+1> =A <k>< / k> x <k>< / k> +B <k> u <k>< / k> < / k> y <k+1> =h(x <k+1> )=[r 1<k+1> ,r 2<k+1> ,…,r n<k+1> ] T ; Where k represents the kth sampling time step after discretization, and the matrix A <k>< / k> and B <k>< / k> The calculation formula is: Since the matrix A <k>< / k> and B <k>< / k> The value does not change when the sampling frequency dt is fixed, so it can be abbreviated in the subsequent derivation process, and let A = A <k>< / k> ,B=B <k>< / k> . In addition, the system cost function is: Where J represents the system error objective function, argmin x Indicates that we want to obtain the variable that minimizes the objective function i means that the width is k w The i-th state in the sliding window of , j represents the measurement error fed back by the j-th ranging unit; e1, e2, e3 represent the prior error, process error and measurement error respectively; where: In the formula, It represents the relative distance between the jth ranging unit and the UAV at the i-th moment. Represents the relative distance relationship between each distance measuring unit and the first distance measuring unit, that is, the origin set by the system; where, represents the relative distance vector between the jth ranging unit and the origin, represents the position vector of the UAV in the origin coordinate system at the i-th moment, that is, The first item 1 The current state is required to be similar to the state at the previous moment. The second item e 2 The current state is required to be similar to the result obtained by using the state at the previous moment and the state equation. 3·j It is required that the n UWB ranging results are similar to the current state calculation result, p represents the estimated position of the UAV at the i-th moment; let: In the formula, E represents the error matrix of the system, and W represents the corresponding weight matrix; The cost function optimization problem is transformed into: The specific representation of each matrix is ​​as follows: In the formula, represents the concatenation of the identity matrix and the zero matrix, represents the concatenation of the identity matrix and the A matrix, and They represent the concatenation of C and C matrices respectively, the D matrix represents the correlation of the measured values ​​in the sliding window, and the θ matrix represents the aggregate of the prior estimation, acceleration value, and ranging correlation value; By taking partial derivatives, we can get the analytical solution: at the same time Represents the estimate of the current state at the previous moment, that is, In particular, it can be considered The actual IMU sampling frequency is higher than the UWB distance measurement sampling frequency. There is a The measurement of the acceleration is recorded as The pre-integration method is introduced to make full use of IMU measurement data: Modify the first formula of the state control equation: In the formula, the superscript l represents the number of acceleration measurements; θ is modified to: Will The expression is modified to: In the formula, A l Represents the A matrix corresponding to the lth accelerometer measurement in a set of data.

4. The method for rapid estimation and fault tolerance of drone state based on multiple anchor points and polynomial fitting according to claim 3 is characterized in that: The specific method of approximating the analytical solution by polynomial fitting and obtaining the state at the next moment is: Since the calculation time required to directly obtain the analytical solution is too long, a polynomial with the highest order term of β is used to approximate the analytical solution. The original cost function optimization problem can be rewritten as: In the formula, Represents the corresponding coefficients of the polynomial that approximates the analytical solution; In the t i At this moment, the above matrix is ​​specifically expressed as: α j ＝[α j0 ，α j1 ，α j2 ，…，α jβ ] T ； In the formula, α represents the coefficient matrix corresponding to the polynomial approximating the analytical solution, T represents the time interval matrix corresponding to the polynomial approximating the analytical solution, and t l represents the i-th time point, t0 represents the initial time point in the sliding window, Represents the time interval between the i-th time point and the initial time in the sliding window; Get the corresponding coefficients of the polynomial that approximates the analytical solution In the formula, E r represents the process matrix after polynomial approximation, E θ represents the measurement matrix after polynomial approximation; Finally, we get an approximate analytical solution Polynomial solution of

Images