An online estimation method for external force and external moment of unmanned system based on error state Kalman filter

Abstract

The present application belongs to the technical field of unmanned aerial vehicle state estimation, and particularly relates to an online estimation method for external force and external torque of an unmanned system based on error state Kalman filtering. The external force estimation method provided by the present application can be run in real time and online, and the running frequency is consistent with that of an inertial measurement unit, which can be as high as several hundred hertz. The present application provides an online estimation method for external force and external torque of a quad-rotor unmanned aerial vehicle based on error state Kalman filtering, which can estimate the external force and external torque simultaneously without relying on external devices such as motion capture systems, RTK or GPS to provide position information, and only needs to use the inertial measurement unit, motor speed meter, attitude feedback on the quad-rotor unmanned aerial vehicle and the accurately modeled dynamics model of the quad-rotor unmanned aerial vehicle.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of unmanned aerial vehicle state estimation, and relates to an online estimation method for external force and external torque of an unmanned system based on error state Kalman filtering, in particular to an online estimation method for external force and external torque of a quadrotor unmanned aerial vehicle based on error state Kalman filtering. BACKGROUND

[0002] In recent years, due to the small size, flexible action, small volume and increasingly strong performance of quadrotor aircraft, they are used to perform various tasks: among them, they either need to physically interact with the environment, such as inspection and cargo transportation, or need to fly close to other quadrotor aircraft, including formation flight and multi-robot cooperation to perform tasks. In these scenarios, the quadrotor unmanned aerial vehicle may experience strong external force and external torque, which are difficult to model but greatly affect the dynamic behavior of the quadrotor aircraft. Accurate estimation of external force and external torque and appropriate reaction to them are crucial for the quadrotor aircraft to safely and effectively complete the given task. SUMMARY

[0003] Therefore, the present application provides an online estimation method for external force and external torque of an unmanned system based on error state Kalman filtering, which can accurately estimate the external force and external torque received by the quadrotor unmanned aerial vehicle in real time online without relying on external devices such as motion capture systems, RTK or GPS to provide position information, only using the inertial measurement unit, motor speed meter, attitude feedback on the quadrotor unmanned aerial vehicle and the accurately modeled quadrotor dynamics model, with the help of the error state Kalman filtering method.

[0004] The technical solutions of the present application are as follows:

[0005] An online estimation method for external force and external torque of an unmanned system based on error state Kalman filtering, the steps of the method comprising

[0006] Step one, constructing a system dynamics model of the quadrotor unmanned aerial vehicle;

[0007] Step two, constructing a measurement model of the quadrotor unmanned aerial vehicle;

[0008] Step three, constructing a real state dynamics model, a nominal state dynamics model and an error state dynamics model of the quadrotor unmanned aerial vehicle according to the system dynamics model of the quadrotor unmanned aerial vehicle constructed in step one;

[0009] Step 4: Discretize the real state dynamic model constructed in Step 3 using the explicit Euler method to obtain the real state discrete dynamic equation; discretize the nominal state dynamic model constructed in Step 3 using the explicit Euler method to obtain the nominal state discrete dynamic equation; and discretize the error state dynamic model constructed in Step 3 using the explicit Euler method to obtain the error state discrete dynamic equation.

[0010] Step 5: Based on the discrete dynamic equations of the real state, the discrete dynamic equations of the nominal state, the discrete dynamic equations of the error state obtained in Step 4, and the measurement model constructed in Step 2, calculate the Jacobian and substitute the calculated Jacobian into the Kalman filter formula for the error state to obtain the recursive formula for state estimation. Extract the terms of external force and external torque from the obtained recursive formula for state estimation to complete the online estimation of external force and external torque of the quadcopter UAV.

[0011] In step one, the method for constructing the system dynamics model of the quadcopter UAV is as follows:

[0012] Define the coordinate system fixed to the center of mass of the quadcopter UAV as follows: (x, y, z axes are respectively) The inertial frame fixed to the ground is ε (the x, y, and z axes are ε). x ε y ε z ), for The linear velocity of the system relative to the ε system is The following is a representation of the system. for The angular velocity of the system relative to the ε system in The representation under the system, R∈SO(3) is The rotation of the system relative to the ε system is denoted as e3 = [0, 0, 1]. T The magnitude of gravitational acceleration is g = 9.8 m / s². 2 The mass of the drone is m, and its inertia tensor matrix is ​​I. The inertia product I is ignored. xy I xz I yz Only retain the moment of inertia I xx I yy I zz That is, I = diag(I) xx I yy I zz ), k f k τ k flap k m u represents the aerodynamic parameters of the propeller. i Let I be the rotational speed of propeller i (i = 1, 2, 3, 4). iThe current of the motor i, The position vector of the propeller i to the center of mass of the body, D = diag(k dx , k dy , k dz ) is the aerodynamic drag coefficient of the unmanned aerial vehicle, ∈ i represents the rotating direction of the propeller (clockwise is +1, counterclockwise is -1);

[0013] The external force received by the unmanned aerial vehicle includes gravity G B , propeller thrust aerodynamic drag and other external forces to be estimated

[0014]

[0015] wherein

[0016] G B = mgR T e3

[0017]

[0018]

[0019] The external moment received by the unmanned aerial vehicle is

[0020]

[0021] wherein

[0022]

[0023]

[0024]

[0025] According to Newton's equation in the B system, we have

[0026]

[0027] According to Euler's equation in the B system, we have

[0028]

[0029] Substitute the combined external force and the combined external moment into the Newton-Euler equation, and since the propeller power model, motor speed measurement and other errors will cause the existence of process noise, therefore, introduce zero-mean Gaussian white noise in the equation to consider the influence of these uncertain factors;

[0030]

[0031]

[0032] wherein,

[0033] Rotations are represented using unit quaternions;

[0034]

[0035] Further, since the specific type or characteristics of the external force and external moment are not known in advance, the existing work has verified through experiments that modeling the external force as a random walk process helps to estimate the unknown and changing force, so the external force and external moment are modeled as a random walk process;

[0036]

[0037]

[0038] wherein,

[0039] The state vector is defined as

[0040]

[0041] System dynamics model is

[0042]

[0043]

[0044]

[0045]

[0046]

[0047] The method for constructing the measurement model of the quad-rotor unmanned aerial vehicle in step two is:

[0048] The sensors for the measurement model include an inertial measurement unit and an attitude sensor, wherein the inertial measurement unit contains an accelerometer and an angular velocity meter;

[0049] The accelerometer measures the representation of the combined external force in B except for gravity, and the measurement result also contains measurement noise (modeled as Gaussian white noise), and the measurement equation of the accelerometer is

[0050]

[0051] wherein

[0052] The angular velocity meter measures the angular velocity of the aircraft body relative to the inertial system in the aircraft body, and similar to the accelerometer, the measurement result contains measurement noise, and the measurement equation of the angular velocity meter is ω m = ω + v ω

[0053] wherein

[0054] The current attitude quaternion of the unmanned aerial vehicle is obtained by the attitude sensor, the attitude error is modeled as Gaussian white noise, and the measurement equation of the attitude is

[0055]

[0056] wherein

[0057] The finally obtained observation equation y = h (x, u, n) is

[0058]

[0059] ω m = ω + v ω

[0060]

[0061] In step three, the real state vector, the nominal state vector, and the error state vector are defined as

[0062]

[0063] The three state vectors satisfy the relationship

[0064] v t = v + δ v

[0065]

[0066] ω t = ω + δ ω

[0067]

[0068]

[0069] The real state satisfies the motion equation Directly substitute x t into the equation to obtain

[0070]

[0071]

[0072]

[0073]

[0074]

[0075] For the equations of motion By simultaneously calculating the expectations from both sides, the nominal state-motion equations can be obtained.

[0076]

[0077]

[0078]

[0079]

[0080]

[0081] State relation Substitute into the equations of motion of the real state Substitute into the nominal state-motion equations Simplifying and omitting second-order terms, we obtain the error state motion equation.

[0082]

[0083]

[0084]

[0085]

[0086]

[0087] Discretizing the nominal state motion equation and the error state motion equation yields the corresponding discrete motion equations x, respectively. k+1 =f d (x k ,u k )and Where k+1 represents the next time step, and k represents the current time step;

[0088] In step four, the Jacobian is calculated:

[0089]

[0090] The iterative process for state estimation using the ESKF formula is as follows:

[0091] First, predict the nominal state x at the next time step. k And calculate the corresponding covariance matrix P.

[0092] x k =f d (x k u k )

[0093]

[0094] Where Q is the covariance matrix of the noise in the process model;

[0095] Subsequently, the Kalman gain matrix K is calculated based on the measurement results y. k and measurement prior h(x) k Obtain the error state quantity Finally, update the covariance matrix P;

[0096] K = P k H T HP k H T +R) -1

[0097]

[0098] P k+1 =(I-KH)P k

[0099] Where R is the covariance matrix of the noise quantity in the measurement model;

[0100] After the update is complete, the error state quantity will be... Combined with nominal state x k The final result of the state estimation, x, is obtained. k+1

[0101]

[0102] Final Reset Ready to start the next cycle

[0103]

[0104] After each loop ends, from x k+1 Extract and These two items are the estimated values ​​of external force and external torque.

[0105] Beneficial effects

[0106] I. This invention employs an error-state Kalman filter-based method to solve the constraint problem of the rotational quaternion having a modulus of 1 involved in the estimation of external forces and external torques.

[0107] Second, this invention relies solely on an inertial measurement unit, a motor tachometer, and an attitude sensor in terms of sensor hardware, without requiring any other external devices.

[0108] Third, the external force estimation method provided by this invention can run online in real time, with an operating frequency consistent with that of the inertial measurement unit, reaching up to several hundred hertz. This invention provides an online estimation method for external forces and torques of a quadrotor UAV based on error state Kalman filtering. It can simultaneously estimate external forces and torques without relying on external devices such as motion capture systems, RTK, or GPS to provide position information, using only the inertial measurement unit, motor tachometer, attitude feedback, and a precisely modeled quadrotor UAV dynamics model on the quadrotor UAV. Attached Figure Description

[0109] Figure 1 The coordinate system of this invention A schematic diagram of the coordinate system ε. Detailed Implementation

[0110] The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0111] This invention provides an online estimation method for external forces and torques of a quadrotor UAV based on error state Kalman filtering, the detailed implementation of which is as follows:

[0112] Step 1: Perform a detailed model of the dynamics of the quadcopter UAV, fully considering the various forces and torques experienced by the UAV during flight. Combine this with quaternion dynamics to represent attitude, and use the UAV's velocity, attitude, angular velocity, external forces, and external torques as the system's state variables to construct a dynamic model of the quadcopter UAV system.

[0113] like Figure 1 As shown, the coordinate system fixed to the center of mass of the quadcopter UAV is defined as follows: (x, y, z axes are respectively) The inertial frame fixed to the ground is ε (the x, y, and z axes are ε). x ε y ε z ), for The linear velocity of the system relative to the ε system is The following is a representation of the system. for The angular velocity of the system relative to the ε system in The representation under the system, R∈SO(3) is is the rotation relative to ε, and e3=[0,0,1] T , the gravity acceleration is g=9.8m / s 2 , the mass of the UAV is m, the inertia tensor matrix is I, and the inertia product I xy xz yz , only the inertia matrix I xx yy zz , that is, I=diag(I xx yy zz ), k f , k τ , k flap , k m is the aerodynamic parameter of the propeller, u i is the rotation speed of the propeller i (i=1, 2, 3, 4), I i is the current of the motor i, is the position vector of the propeller i to the center of mass of the UAV, D=diag(k dx dy dz is the aerodynamic drag coefficient of the UAV, ∈ i represents the rotation direction of the propeller (clockwise is +1, and counterclockwise is-1);

[0114] The external force received by the UAV includes gravity G B , propeller thrust aerodynamic drag and other external forces to be estimated

[0115]

[0116] wherein

[0117] G B =mgR T e3

[0118]

[0119]

[0120] The external moment received by the UAV is

[0121]

[0122] wherein

[0123]

[0124] ​​​​​​​​

[0125]

[0126] According to Newton equation in B-frame, we have

[0127]

[0128] According to Euler equation in B-frame, we have

[0129]

[0130] Substitute the total external force and total external torque into the Newton-Euler equation. Meanwhile, the errors of propeller power model and motor speed measurement will lead to the existence of process noise. Therefore, we introduce zero-mean Gaussian white noise into the equation to consider the influence of these uncertain factors

[0131]

[0132]

[0133] where,

[0134] Use unit quaternion to represent rotation

[0135]

[0136] In addition, since the specific type or characteristics of the external force and external torque are unknown in advance, the existing work has verified through experiments that modeling the external force as a random walk process helps to estimate the unknown and changing force. Therefore, the external force and external torque are modeled as a random walk process

[0137]

[0138]

[0139] where,

[0140] Define the state vector of the system as

[0141]

[0142] The dynamics model of the system is

[0143]

[0144]

[0145]

[0146]

[0147]

[0148] Step two, based on the existing sensor model, the measurement model of quadrotor unmanned aerial vehicle system is constructed.

[0149] The sensors for measurement model include inertial measurement unit, attitude sensor, wherein the inertial measurement unit contains accelerometer and gyroscope.

[0150] The accelerometer measures the representation of the total force in B except gravity, and the measurement result also contains measurement noise (modeled as Gaussian white noise). The measurement equation of the accelerometer is

[0151]

[0152] Wherein

[0153] The gyroscope measures the angular velocity of the aircraft body relative to the inertial system in the aircraft body, and similar to the accelerometer, the measurement result contains measurement noise. The measurement equation of the gyroscope is ω m = ω + v ω

[0154] Wherein

[0155] The current attitude quaternion of the unmanned aerial vehicle can be obtained through the attitude sensor, and the attitude error is modeled as Gaussian white noise. The measurement equation of the attitude is

[0156]

[0157] Wherein

[0158] The final observation equation y = h (x, u, n) obtained is

[0159]

[0160] ω m = ω + v ω

[0161]

[0162] Step three, the real state quantity, the nominal state quantity and the error state quantity are introduced, according to the mathematical relationship among the three state quantities, combined with the dynamic model in step one, the real state dynamic model, the nominal state dynamic model and the error state dynamic model of the system are constructed in turn, and the explicit Euler method is used to discretize the three dynamic models, and three corresponding discrete dynamic equations are obtained.

[0163] The real state vector, the nominal state vector, and the error state vector are defined as

[0164]

[0165] The three state vectors satisfy the following relationship

[0166] v t = v + δ v

[0167]

[0168] ω t = ω + δ ω

[0169]

[0170]

[0171] The real state satisfies the motion equation Substitute x t directly into it to get

[0172]

[0173]

[0174]

[0175]

[0176]

[0177] Take the expectation of both sides of the motion equation , and you can get the nominal state motion equation

[0178]

[0179]

[0180]

[0181]

[0182]

[0183] Substitute the state relationship into the real state motion equation and then into the nominal state motion equation Simplify and omit the second order small term, get the error state motion equation

[0184]

[0185]

[0186]

[0187]

[0188]

[0189] Discretize the nominal state motion equation and the error state motion equation, get the corresponding discrete motion equation respectively as k+1 = f d (x k ,u k ) and Where k+1 represents the next time, k represents the current time;

[0190] Step four, the Jacobian of the discrete dynamic equation in step three and the measurement model in step two is calculated, which is substituted into the formula of the error state Kalman filter to get the state estimation recursive formula of the system. The external force and external torque are extracted from the estimated state quantity to get the estimation of the external force and external torque of the quadrotor unmanned aerial vehicle system.

[0191] Calculate the Jacobian

[0192]

[0193] The cyclic iteration process of state estimation using the ESKF formula is as follows:

[0194] First, predict the nominal state x k at the next time, and calculate the corresponding covariance matrix P

[0195] x k ← f d (x k , u k )

[0196]

[0197] Where Q is the covariance matrix of the noise quantity in the process model

[0198] Then, calculate the Kalman gain matrix K, get the error state quantity k from the measurement result y k and the measurement prior h(x k ) Finally, update the covariance matrix P

[0199] K = PH T (HPH T + R) -1

[0200]

[0201] P k+1 = (I - KH)P k

[0202] where R is the covariance matrix of the noise in the measurement model

[0203] After the update, the error state quantity is combined with the nominal state x k to obtain the final result of the state estimation x k+1

[0204]

[0205] Finally, reset and prepare to start the next cycle

[0206]

[0207] After each cycle, extract k+1 and from x , which are the estimates of the external force and the external moment. Using the above process, the estimation of the external force and the external moment of the quad-rotor unmanned aerial vehicle system can be realized.

[0209] To sum up, the above is only a preferred embodiment of the present application, and is not used to limit the protection scope of the present application. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A method for online estimation of external forces and external moments in an unmanned system based on error-state Kalman filtering, characterized in that... The steps of this method include Step 1: Construct a system dynamics model for the quadcopter UAV; Step 2: Construct a measurement model for the quadcopter drone; Step 3: Based on the system dynamics model of the quadcopter UAV constructed in Step 1, construct the real state dynamics model, nominal state dynamics model, and error state dynamics model of the quadcopter UAV. Step four: Discretize the nominal state dynamic model constructed in step three to obtain the nominal state discrete dynamic model; Discretize the error state dynamic model constructed in step three to obtain the error state discrete dynamic model. Step 5: Based on the nominal state discrete dynamic model, the error state discrete dynamic model, and the measurement model constructed in Step 2 obtained in Step 4, calculate the Jacobian and substitute the calculated Jacobian into the error state Kalman filter formula to obtain the state estimation recursive formula. Extract the terms of external force and external torque from the obtained state estimation recursive formula to complete the online estimation of external force and external torque of the quadcopter UAV. In step one, the method for constructing the system dynamics model of the quadcopter UAV is as follows: Define the coordinate system fixed to the center of mass of the quadcopter UAV as follows: The x, y, and z axes are respectively , , An inertial frame fixed to the ground is The x, y, and z axes are respectively , , , for System relative to The linear velocity of the system is The following is a representation of the system. for System relative to The angular velocity of the system is The following is a representation of the system. for System relative to The rotation of the system, remember The magnitude of gravitational acceleration is The mass of the drone is The inertia tensor matrix is Ignoring the product of inertia Only retain the moment of inertia ,Right now , , , , These are the aerodynamic parameters of the propeller. For propeller rotational speed, For motor The current, For propeller The position vector to the body's center of mass Let be the aerodynamic drag coefficient of the UAV. This indicates the direction of propeller rotation, with clockwise being +1 and counterclockwise being -1; =1,2,3,4; The external forces acting on a drone include gravity. propeller thrust Aerodynamic drag And other external forces to be estimated ; in The external torque that the drone will experience is in exist According to Newton's equations, we have exist According to Euler's equations, we have Introduce zero-mean Gaussian white noise into the equation; in, ; Rotation is represented using unit quaternions; External forces and torques are modeled as random walk processes; in, Define the state vector as System dynamics model for In step two, the method for constructing the measurement model of the quadcopter UAV is as follows: The sensors used to measure the model include an inertial measurement unit and an attitude sensor, wherein the inertial measurement unit contains an accelerometer and an angular velocity meter; An accelerometer measures the net external force excluding gravity. The following indicates that the measurement results will also include measurement noise, modeled as Gaussian white noise. The accelerometer's measurement equation is: in An angular velocity meter measures the angular velocity of a system relative to an inertial frame within the system. The measurement results will include measurement noise. The measurement equation for the angular velocity meter is: in The current attitude quaternion of the UAV is obtained through the attitude sensor. The attitude error is modeled as Gaussian white noise, and the attitude measurement equation is: in The final measurement model obtained for In step five, the formula for calculating the Jacobian is: The iterative process for state estimation using the ESKF formula is as follows: First, predict the nominal state at the next time step. And calculate the corresponding covariance matrix. in This is the covariance matrix of the noise level in the process model; Subsequently, the Kalman gain matrix is ​​calculated. According to the measurement results and measurement prior Obtain the error state quantity Finally, update the covariance matrix. ; in It is the covariance matrix of the noise level in the measurement model; After the update is complete, the error state quantity will be... Combined with nominal condition The final result of the state estimation is obtained. : Final Reset Ready to start the next cycle After each loop ends, from Extract and These two items are the estimated values ​​of external force and external torque.

2. The online estimation method for external forces and torques of an unmanned system based on error state Kalman filtering according to claim 1, characterized in that: In step three, the method for constructing the real-state dynamics model, nominal-state dynamics model, and error-state dynamics model of the quadcopter UAV is as follows: Define the true state vector, nominal state vector, and error state vector as follows: The three state vectors satisfy the following relation: The real state dynamic model is , directly Substitute to get For real-state dynamics models By simultaneously calculating the expectation from both sides, the nominal state dynamic model is obtained. State relation Substitute into the real state dynamics model Then substitute it into the nominal state dynamics model The error state dynamic model is obtained. 。 3. The online estimation method for external forces and torques of an unmanned system based on error state Kalman filtering according to claim 1, characterized in that: In step four, the nominal state dynamic model is discretized to obtain the nominal state discrete dynamic model. ; Discretizing the error state dynamics model yields the discrete error state dynamics model as follows: , where k+1 represents the next moment and k represents the current moment.

Images