A virtual fence safety control method for underactuated quadrotor drones

Abstract

The present invention discloses a virtual fence safety control method for an underactuated quadrotor drone. The method includes establishing a rigid body dynamics model based on the structural characteristics of the underactuated quadrotor drone; defining error expressions for the state and control quantities of the underactuated quadrotor drone using Lie theory; deriving the error state dynamics equation of the underactuated quadrotor drone based on the error expressions for the state and control quantities of the underactuated quadrotor drone; designing a trajectory tracking controller based on a linear quadratic regulator based on the error state dynamics equation of the underactuated quadrotor drone; and designing a quadratic programming safety filter based on an exponential barrier function to safely filter the output of the trajectory tracking controller to ensure that the underactuated quadrotor drone flies within a safe area similar to the virtual fence. The method has fast response speed, simplified system tuning parameters, and good trajectory tracking performance.

Description

Technical Field

[0001] The present invention belongs to the field of aircraft safety technology, and in particular relates to a virtual fence safety control method for an under-actuated quadrotor unmanned aerial vehicle (UAV). Background Art

[0002] In the field of drone control, the linear quadratic regulator (LQR), a well-known feedback controller, has been increasingly used by researchers in quadrotor control. Recently, researchers have utilized Lie theory to address the evolution of the robot's state along a manifold, overcoming the shortcomings of most previous LQR control schemes, which treat Euler angles, quaternions, and rotation matrices used to represent attitude as vector spaces and thus neglect their manifold properties. This approach allows for better tracking of the intended trajectory. Furthermore, in the field of drone safety control, when upper-level trajectory planning errors cause the drone to stray from the safe zone, an online quadratic programming (QP) strategy combined with an exponentially controlled barrier function (ECBF) exhibits multiple minima, making it difficult to track the intended trajectory under certain conditions. This strategy, with its dynamic feasibility constraints for nonlinear systems of any relative degree and forward invariance, allows the quadrotor to maintain a high control execution frequency while maintaining safe flight and tracking the intended trajectory as closely as possible. Summary of the Invention

[0003] In response to the above technical problems, the present invention provides a virtual fence safety control method for an under-actuated quadrotor drone.

[0004] The technical solution adopted by the present invention to solve the technical problem is:

[0005] A virtual fence safety control method for an underactuated quadrotor drone, the method comprising the following steps:

[0006] S100: Establish a rigid body dynamics model based on the structural characteristics of an underactuated quadrotor drone;

[0007] S200: Based on the rigid body dynamics model, the error expressions of the state and control variables of the underactuated quadrotor UAV are defined using Lie theory.

[0008] S300: Derivation of the error state dynamics equation of the under-actuated quadrotor UAV based on the error expressions of the state and control variables of the under-actuated quadrotor UAV;

[0009] S400: Design a trajectory tracking controller based on a linear quadratic regulator based on the error state dynamics equation of an underactuated quadrotor drone.

[0010] S500: Design a quadratic programming safety filter based on an exponential barrier function to safely filter the output of the trajectory tracking controller to ensure that the underactuated quadrotor drone flies within a safe area similar to a virtual fence.

[0011] Preferably, S100 specifically includes:

[0012]

[0013]

[0014]

[0015] Where m is the mass of the UAV; the subscript I represents the inertial coordinate system, and the subscript B represents the body coordinate system; is the rotation matrix from the world coordinate system to the body coordinate system; g is the gravitational acceleration; They are the drone state quantities inertial position, body velocity and attitude, where the drone attitude is expressed in the form of unit quaternion; They are The first differential of is the quaternion Hamiltonian operator; f, are the thrust and angular velocity of the drone control input respectively; e3 = [0 0 1] T is the base vector.

[0016] Preferably, S200 specifically includes:

[0017]

[0018]

[0019]

[0020]

[0021]

[0022] The following definitions are given about quaternions Operations:

[0023]

[0024] Where: are the scalar part and vector part of the quaternion q respectively; They are the position error, body velocity error and attitude error of the UAV in the inertial coordinate system respectively; are the thrust error and angular velocity error of the UAV control input respectively; are the desired position, velocity and attitude of the UAV in the inertial coordinate system respectively; are the reference thrust and angular velocity, respectively.

[0025] Preferably, S300 specifically includes:

[0026]

[0027]

[0028]

[0029] Where × is defined as:

[0030]

[0031] Where: They are The first-order differential of .

[0032] Preferably, S400 specifically includes:

[0033]

[0034] Where u is the control input obtained by solving the trajectory tracking controller based on the linear quadratic regulator; input for desired drone control; is the UAV state error; K is the gain matrix, which is obtained by minimizing the linear quadratic regulator based on the error state equation, where the cost function of the linear quadratic regulator is Defined as:

[0035]

[0036] Where: is the UAV control input error; Q, R are weight matrices.

[0037] Preferably, the expression of the quadratic programming safety filter based on the exponential barrier function designed in S500 is:

[0038]

[0039]

[0040] in

[0041]

[0042]

[0043]

[0044]

[0045]

[0046]

[0047]

[0048] in

[0049]

[0050] The super ellipsoidal virtual fence safety area is defined as follows:

[0051]

[0052]

[0053] Where vector ω c ,f c Represents the angular velocity and thrust in the x and y axis directions to be solved, f,ω x,y are the initial thrust and angular velocity in the x- and y-axis directions obtained by the linear quadratic regulator; are the angular velocity and thrust in the x and y axes respectively after filtering; f lb ,f ub ,ω lb ,ω ub is the filtered UAV control input ω c ,f c The lower and upper limits of c = [c x ,c y ,c z ] T is the center point of the safe area, d=[d x ,d y ,d z ] T The distance to the safety zone is limited; k z,2 , k z,1 , k ω,1 , k ω,2 , k ω,3 is a real number greater than 0.

[0054] Compared with the prior art, the advantages of the present invention are:

[0055] (1) Short reaction time

[0056] The safety filter uses ECBF to directly minimize the correction of each control input (angular velocity and thrust) solved by LQR, ensuring that the quadcopter can still fly within the safe area in the shortest time even if the upper-level planned trajectory is wrong.

[0057] (2) Simplify system tuning parameters

[0058] The system uses an error-state LQR controller to directly generate and control thrust and angular velocity. Compared with traditional PID controllers, it does not require separate outer-loop controllers for generating thrust and acceleration and inner-loop controllers for generating angular velocity, thus greatly simplifying the system tuning process.

[0059] (3) Good trajectory tracking performance

[0060] Compared with the traditional artificial potential field method (APF), which has the problem of multiple minimum points and makes it difficult to track the predetermined trajectory under certain conditions, the safety filter based on ECBF and QP in this system can ensure that the quadrotor drone has a good trajectory tracking effect on the predetermined trajectory while staying in a safe area. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 This is a flow chart of a virtual fence safety control method for an underactuated quadrotor drone in one embodiment of the present invention;

[0062] Figure 2 This is a system framework diagram corresponding to a virtual fence safety control method for an under-actuated quadrotor drone in one embodiment of the present invention;

[0063] Figure 3 This is a comparison chart between the actual trajectory of the UAV and the reference trajectory in the simulation platform;

[0064] Figure 4 is the UAV position data curve in the simulation platform;

[0065] Figure 5 This is the control input curve before and after filtering of the UAV safety filter in the simulation platform. DETAILED DESCRIPTION

[0066] In order to enable those skilled in the art to better understand the technical solution of the present invention, the present invention is further described in detail below with reference to the accompanying drawings.

[0067] In one embodiment, Figure 1 As shown, a virtual fence safety control method for an underactuated quadrotor drone is characterized in that the method includes the following steps:

[0068] S100: Establish a rigid body dynamics model based on the structural characteristics of an underactuated quadrotor drone.

[0069] In one embodiment, S100 specifically includes:

[0070]

[0071]

[0072]

[0073] Where m is the mass of the UAV; the subscript I represents the inertial coordinate system, and the subscript B represents the body coordinate system; is the rotation matrix from the world coordinate system to the body coordinate system; g is the gravitational acceleration; They are the drone state quantities inertial position, body velocity and attitude, where the drone attitude is expressed in the form of unit quaternion; They are The first differential of is the quaternion Hamiltonian operator; f, are the thrust and angular velocity of the drone control input respectively; e3 = [0 0 1] T is the base vector.

[0074] S200: Based on the rigid body dynamics model, the Lie theory is used to define the error expressions of the state and control variables of an underactuated quadrotor drone.

[0075] In one embodiment, S200 specifically includes:

[0076]

[0077]

[0078]

[0079]

[0080]

[0081] The following definitions are given about quaternions Operations:

[0082]

[0083] Where: are the scalar part and vector part of the quaternion q respectively; They are the position error, body velocity error and attitude error of the UAV in the inertial coordinate system respectively; are the thrust error and angular velocity error of the UAV control input respectively; are the desired position, velocity and attitude of the UAV in the inertial coordinate system respectively; are the reference thrust and angular velocity, respectively.

[0084] S300: The error state dynamic equation of the under-actuated quadrotor UAV is derived based on the error expressions of the state and control quantities of the under-actuated quadrotor UAV.

[0085] In one embodiment, S300 specifically includes:

[0086]

[0087]

[0088]

[0089] Where × is defined as:

[0090]

[0091] Where: They are The first-order differential of .

[0092] For the convenience of description, the above error state dynamics equation is referred to as:

[0093]

[0094] S400: Design a trajectory tracking controller based on a linear quadratic regulator according to the error state dynamics equation of an underactuated quadrotor UAV.

[0095] In one embodiment, S400 specifically includes:

[0096]

[0097] Where u is the control input obtained by solving the trajectory tracking controller based on the linear quadratic regulator; input for desired drone control; is the UAV state error; K is the gain matrix, which is obtained by minimizing the linear quadratic regulator based on the error state equation, where the cost function of the linear quadratic regulator is Defined as:

[0098]

[0099] Where: is the UAV control input error; Q, R are weight matrices.

[0100] Since LQR is applicable to LTI systems, the error state dynamics equation of the nonlinear quadrotor UAV must satisfy the following expression:

[0101]

[0102] The present invention approximates the LTI error state system by linearizing the current state, that is, designing the ninth-order state transfer matrix A and the 9×4-dimensional control input matrix B as follows:

[0103]

[0104]

[0105] Right now

[0106]

[0107] The specific matrix items are:

[0108]

[0109]

[0110]

[0111]

[0112]

[0113]

[0114] S500: Design a quadratic programming safety filter based on an exponential barrier function to safely filter the control input obtained by the trajectory tracking controller to ensure that the underactuated quadrotor drone flies within a safe area similar to a virtual fence.

[0115] In one embodiment, the expression of the quadratic programming safety filter based on the exponential barrier function designed in S500 is:

[0116]

[0117] Because according to the rigid body kinematic model of the system, the relationship between position state and angular velocity can be obtained as follows:

[0118]

[0119] Further we can get:

[0120]

[0121] Therefore, the Lie differential forms of h(x,y) can be obtained as follows:

[0122]

[0123]

[0124]

[0125]

[0126] in

[0127]

[0128] At the same time, combined with the relationship between thrust and position state, the Lie differential form of h(z) can be obtained as follows:

[0129]

[0130]

[0131]

[0132] The super ellipsoidal virtual fence safety area is defined as follows:

[0133]

[0134]

[0135] Where vector ω c ,f c Represents the angular velocity and thrust in the x and y axis directions to be solved, f,ω x,y are the initial thrust and angular velocity in the x- and y-axis directions obtained by the linear quadratic regulator; are the angular velocity and thrust in the x and y axes respectively after filtering; f lb ,f ub ,ω lb ,ω ub is the filtered UAV control input ω c ,f c The lower and upper limits of c = [c x ,c y ,c z ] T is the center point of the safe area, d=[d x ,d y ,d z ] T The distance to the safety zone is limited; k z,2 , k z,1 , k ω,1 , k ω,2 , k ω,3 is a real number greater than 0.

[0136] Furthermore, the ROS-PX4-Gazebo simulation platform is used to verify the algorithm, and MAVROS is used to subscribe to the ROS topic containing the current status information of the drone and publish the ROS topic related to the drone control input.

[0137] The LQR weight coefficients in the experiment are designed using the Bryson rule. The CARE equation in the LQR is solved using the Schur decomposition method based on the Lapacke linear algebra library.

[0138] The predetermined circular trajectory designed in the experiment is

[0139] p d =[2cos(0.15t),2sin(0.15t),2] T

[0140] The safety distance is set to d x =d y =1.6,d z =2, the center point of the safe area is set to c x =c y =0,c z =2.

[0141] refer to Figure 3 、 Figure 4 and Figure 5 , shows the experimental results. Figure 3 This is a comparison diagram of the actual trajectory of the UAV and the reference trajectory. The realization represents the expected circular trajectory, and the circular line represents the actual flight trajectory of the UAV. Figure 4 The drone position data curve in the simulation platform shows, from top to bottom, the drone's x-axis coordinate, the planned trajectory's x-axis coordinate, the drone's y-axis coordinate, and the planned trajectory's y-axis coordinate. Figure 5 The control input angular velocity curves for the drone in the simulation platform before and after filtering by the safety filter are shown. From top to bottom, the x-axis angular velocity before filtering, the x-axis angular velocity after filtering, the y-axis angular velocity before filtering, and the y-axis angular velocity after filtering. As can be seen from the above results, after filtering the control input by the safety filter, the drone always remains within the safe zone and maintains its trajectory as closely as possible.

[0142] Compared with the prior art, the advantages of the present invention are:

[0143] (1) Short reaction time

[0144] The safety filter uses ECBF to directly minimize the correction of each control input (angular velocity and thrust) solved by LQR, ensuring that the quadcopter can still fly within the safe area in the shortest time even if the upper-level planned trajectory is wrong.

[0145] (2) Simplify system tuning parameters

[0146] The system uses an error-state LQR controller to directly generate and control thrust and angular velocity. Compared with traditional PID controllers, it does not require separate outer-loop controllers for generating thrust and acceleration and inner-loop controllers for generating angular velocity, thus greatly simplifying the system tuning process.

[0147] (3) Good trajectory tracking performance

[0148] Compared with the traditional artificial potential field method (APF), which has the problem of multiple minimum points and makes it difficult to track the predetermined trajectory under certain conditions, the safety filter based on ECBF and QP in this system can ensure that the quadrotor drone has a good trajectory tracking effect on the predetermined trajectory while staying in a safe area.

[0149] The above is a detailed introduction to the virtual fence safety control method for an under-actuated quadcopter drone provided by the present invention. This article uses specific examples to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the core idea of the present invention. It should be pointed out that for ordinary technicians in this technical field, without departing from the principles of the present invention, several improvements and modifications can be made to the present invention, and these improvements and modifications also fall within the scope of protection of the claims of the present invention.

Claims

1. A virtual fence safety control method for an underactuated quadrotor drone, characterized in that: The method comprises the following steps: S100: Establish a rigid body dynamics model based on the structural characteristics of an underactuated quadrotor drone; S200: Based on the rigid body dynamics model, using Lie theory, define error expressions for state quantities and control quantities of the underactuated quadrotor drone; S300: Derivation of an error state dynamics equation of the under-actuated quadrotor drone based on error expressions of state quantities and control quantities of the under-actuated quadrotor drone; S400: Designing a trajectory tracking controller based on a linear quadratic regulator according to the error state dynamics equation of the underactuated quadrotor drone; S500: Designing a quadratic programming safety filter based on an exponential barrier function to perform safety filtering on the output of the trajectory tracking controller to ensure that the underactuated quadrotor drone flies within a safety area similar to a virtual fence; S100 is specifically: Where m is the mass of the UAV; the subscript I represents the inertial coordinate system, and the subscript B represents the body coordinate system; is the rotation matrix from the world coordinate system to the body coordinate system; g is the gravitational acceleration; They are the drone state quantities inertial position, body velocity and attitude, where the drone attitude is expressed in the form of unit quaternion; They are The first differential of is the quaternion Hamiltonian operator; f, are the thrust and angular velocity of the drone control input respectively; e3 = [0 0 1] T is the base vector; S200 is specifically: The following definitions are given about quaternions Operations: Where: q0, are the scalar part and vector part of the quaternion q respectively; They are the position error, body velocity error and attitude error of the UAV in the inertial coordinate system respectively; are the thrust error and angular velocity error of the UAV control input respectively; are the desired position, velocity and attitude of the UAV in the inertial coordinate system respectively; are the reference thrust and angular velocity, respectively; S300 is specifically: Where × is defined as: Where: They are The first differential of S400 is specifically: Where u is the control input obtained by solving the trajectory tracking controller based on the linear quadratic regulator; input for desired drone control; is the UAV state error; K is the gain matrix, which is obtained by minimizing the linear quadratic regulator based on the error state equation, where the cost function of the linear quadratic regulator is Defined as: Where: is the UAV control input error; Q, R are weight matrices.

2. The method according to claim 1, characterized in that The expression of the quadratic programming safety filter based on the exponential barrier function designed in S500 is: f lb ≤f c ≤f ub . oh lb ≤ω c ≤ω ub . in in The super ellipsoidal virtual fence safety area is defined as follows: Where vector ω c ,f c Represents the angular velocity and thrust in the x and y axis directions to be solved, f,ω x,y are the initial thrust and angular velocity in the x- and y-axis directions obtained by the linear quadratic regulator; are the angular velocity and thrust in the x and y axes respectively after filtering; f lb ,f ub ,ω lb ,ω ub is the filtered UAV control input ω c ,f c The lower and upper limits of c = [c x ,c y ,c z ] T is the center point of the safe area, d=[d x ,d y ,d z ] T The distance to the safety zone is limited; k z,2 , k z,1 , k ω,1 , k ω,2 , k ω,3 is a real number greater than 0.

Images