A trajectory tracking control method for small agile fixed-wing UAV with roll decoupling

Abstract

The present invention discloses a trajectory tracking control method for a small agile fixed-wing UAV with roll decoupling. According to the characteristics of the small agile fixed-wing UAV, a roll decoupling position controller that does not rely on the UAV model is designed to achieve position tracking when the roll is limited by obstacle avoidance and cannot participate in position tracking. In view of the problem of large attitude deviation span, an attitude controller that performs hierarchical control according to the size of attitude deviation is proposed, and attitude controllers for large-angle maneuvers and small-angle changes are designed respectively. Under the premise of ensuring tracking accuracy, the present invention can realize the lateral flight of the fixed-wing UAV through obstacles.

Description

Technical Field

[0001] The invention belongs to the field of unmanned aerial vehicle control, and in particular relates to a trajectory tracking control method for a small agile fixed-wing unmanned aerial vehicle with roll decoupling. Background Art

[0002] In complex low-altitude environments such as urban buildings, plateaus and mountains, environmental obstacles are complexly distributed, and the flight channels formed by obstacles are relatively narrow laterally, which poses a major challenge to the autonomous flight of drones. Laterally narrow flight channels require drones to fly sideways at a large angle to cross obstacles, which places high demands on the lateral maneuverability of drones. Small agile fixed-wing drones have the characteristics of a large thrust-to-weight ratio, strong rudder effect, and a low aspect ratio. They retain the efficient cruising capability of fixed-wing drones and have the maneuverability of rotary-wing drones. Therefore, the excellent side-flying capability of small agile fixed-wing drones can be used to improve the safety of drones in laterally narrow flight channels.

[0003] Trajectory tracking control is one of the key core technologies for drones to achieve autonomous flight. Fixed-wing drone trajectory tracking faces the difficulties of fixed-wing drones being difficult to accurately model, underactuated, and having strong nonlinearity. Complex low-altitude environments require drones to maneuver with large roll angles, which poses a greater challenge to the trajectory tracking of fixed-wing drones. At present, researchers' research on fixed-wing drone maneuvering flight usually does not consider environmental constraints, but focuses on the realization of maneuvering actions; some researchers have studied the tracking tasks of fixed-wing drones in obstacle environments, but usually the environmental obstacles are simple or the drones are in a coordinated flight state. Therefore, a trajectory tracking method is needed that can enable fixed-wing drones to fly sideways through obstacles while ensuring tracking accuracy. Summary of the invention

[0004] To achieve the above object, the present invention provides a trajectory tracking control method for a small agile fixed-wing UAV with roll decoupling, comprising the following steps:

[0005] The position error of the UAV during trajectory tracking is calculated based on the gravity normal system, and the reference speed for adjusting the speed of convergence of the UAV to the trajectory is obtained based on the UAV position error calculation;

[0006] Subtract the reference speed from the speed of the drone in the inertial system to obtain a speed error;

[0007] Based on the rate of change of the reference speed and the speed error, a proportional-integral speed control law about the inertia speed is constructed, and a desired acceleration command is obtained based on the speed control law;

[0008] Based on the expected acceleration command, a reference thrust of the propeller of the UAV is obtained, and the thrust control of the UAV is performed based on the reference thrust;

[0009] The reference attitude of the UAV is determined based on the nonlinear vector projection method, the roll angle attitude is added to the reference attitude to obtain the reference attitude of the UAV roll decoupling, and the UAV attitude is controlled by adopting an error classification control method.

[0010] Preferably, the expression of the reference speed is:

[0011]

[0012] In the formula, v com represents the desired velocity of the trajectory, d p represents a positive constant, v t Indicates the speed in the asymptotic direction.

[0013] Preferably, the expression of the speed in the progressive direction is:

[0014] v t =d p i s -k s e s i s -k c e c j s -k h e h k s

[0015] In the formula, k s Represents the positive constant coefficient of the control trajectory tracking tangent direction error, k c represents the positive constant coefficient of the lateral error of the control trajectory tracking, k d represents the positive constant coefficient of the longitudinal error of the control trajectory tracking, d p represents a positive constant coefficient that controls the speed at which the drone converges to the trajectory. k s =(i s ) × j s , k i represents the basis vectors of the inertial frame, Represents the three-dimensional space trajectory of the drone.

[0016] Preferably, the expression of the acceleration command is:

[0017]

[0018] In the formula, the function clip(·) represents the clipping operation, k v Represents the speed error proportional gain parameter, which is a constant greater than zero; k virepresents the speed error integral gain parameter, which is also a constant greater than zero; η represents the integral separation control parameter. v Exceeding the critical error e v0 When , no integral operation is performed, and the integral is separated; e vI Indicates the integral saturation control term. When the error integral is greater than E i When , the integral error saturates and no longer increases.

[0019] Preferably, the expression of the reference thrust is:

[0020] T=m||a v ||

[0021] In the formula, m represents the mass of the drone, a v represents the acceleration produced by the desired propeller thrust.

[0022] Preferably, the reference posture of the drone is expressed as:

[0023]

[0024] In the formula, k i represents the basis vectors of the inertial frame, represents the vector representing the direction in which the drone's nose is expected to point.

[0025] Preferably, the expression of the reference attitude of the UAV roll decoupling is:

[0026]

[0027] In the formula, represents the reference attitude of the UAV, φ c Indicates a roll command.

[0028] Preferably, the attitude is controlled in stages based on the size of the attitude error, and the staged control method is: determine whether the attitude angle deviations of the three axes are all greater than π / 4, and when they are all greater than π / 4, perform large-angle maneuvering attitude control in a special orthogonal group space; when they are not all greater than π / 4, perform small-angle error attitude control in the Euler angle.

[0029] Compared with the prior art, the present invention has the following advantages and technical effects:

[0030] The present application provides a trajectory tracking control method for a small agile fixed-wing UAV with roll decoupling. The method can perform model-free speed control of the UAV according to the position error in the gravity normal system and the speed error in the inertial system to obtain the desired propeller thrust. The method can use elevators and rudders to achieve position tracking according to the strong lateral maneuverability of the small agile fixed-wing UAV, that is, determine the reference attitude of roll decoupling by a nonlinear vector projection method. The method can perform graded control according to the size of the attitude error to achieve large-angle maneuvering and small-angle control in SO(3) and Euler angle space, respectively. The stability of the position controller and the attitude controller can be proved according to the Lyapunov stability theory. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] The drawings constituting a part of the present application are used to provide a further understanding of the present application. The illustrative embodiments and descriptions of the present application are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0032] Figure 1 Schematic diagram of the relationship between the inertial system, the mechanical system and the gravity normal system in an embodiment of the present invention;

[0033] Figure 2 Schematic diagram of nonlinear vector projection according to an embodiment of the present invention. DETAILED DESCRIPTION

[0034] It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0035] It should be noted that the steps shown in the flowcharts of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and that, although a logical order is shown in the flowcharts, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0036] Embodiment 1

[0037] This embodiment provides a rolling decoupled small agile fixed-wing UAV trajectory tracking control method, including the following steps:

[0038] The position error of the UAV during trajectory tracking is calculated based on the gravity normal system, and the reference speed for adjusting the speed of convergence of the UAV to the trajectory is obtained based on the UAV position error calculation;

[0039] Subtract the reference speed from the speed of the drone in the inertial system to obtain a speed error;

[0040] Based on the rate of change of the reference speed and the speed error, a proportional-integral speed control law about the inertia speed is constructed, and a desired acceleration command is obtained based on the speed control law;

[0041] Based on the expected acceleration command, a reference thrust of the propeller of the UAV is obtained, and the thrust control of the UAV is performed based on the reference thrust;

[0042] The reference attitude of the UAV is determined based on the nonlinear vector projection method, the roll angle attitude is added to the reference attitude to obtain the reference attitude of the UAV roll decoupling, and the UAV attitude is controlled by adopting an error classification control method.

[0043] Furthermore, the reference speed is calculated by calculating the position error of trajectory tracking based on the gravity normal system, and using the position error to design a reference speed command that can adjust the speed at which the UAV converges to the trajectory.

[0044] Consider the following three-dimensional space trajectory that satisfies the kinematic constraints of a small agile fixed-wing UAV in an inertial system:

[0045]

[0046] It is a trajectory that is twice differentiable and whose first-order derivative function is not zero. Gravity normal system The basis vectors can be expressed as:

[0047]

[0048] where k i Inertial system The basis vectors, gravity normal system, inertial system and unmanned aircraft system The relationship between Figure 1 As shown; the skew symmetric operator is defined as:

[0049]

[0050] Therefore, the rotation matrix from the inertial system to the gravity normal system is:

[0051]

[0052] Define the position error for trajectory tracking:

[0053]

[0054] Among them, p i Indicates the position of the drone in the inertial system; here e s , e c and e h The errors corresponding to the three basis vector directions are the forward trajectory error, the lateral error, and the height error. Considering the velocity in the asymptotic direction:

[0055] vt =d p i s -k s e s i s -k c e c j s -k h e h k s (6)

[0056] Among them, k s k is the positive constant coefficient of the tangent direction error of the control trajectory tracking; c k is the positive constant coefficient of the lateral error of the control trajectory tracking; d is the positive constant coefficient of the longitudinal error of the control trajectory tracking; d p is a positive constant that controls how fast the drone converges to the trajectory. For larger values, the reference speed will be aligned with the trajectory tangent, resulting in a smooth but slow trajectory convergence; smaller values ​​will steer the drone in a direction perpendicular to the trajectory, resulting in fast convergence but larger overshoot.

[0057] The reference speed is:

[0058]

[0059] In the above formula, the overall control strategy depends on the positive constant d p , which controls the speed at which the UAV converges to the trajectory; v com is the speed expected from the trajectory.

[0060] Furthermore, the method for obtaining the reference thrust is:

[0061] The speed error is obtained based on the reference speed command. The speed control law is designed using the reference speed change rate and the speed error to obtain the acceleration command, and then the reference thrust of the UAV propeller is defined.

[0062] The velocity error is defined as:

[0063]

[0064] Among them, v b Indicates the speed of the drone in the aircraft system. The acceleration command is:

[0065]

[0066] in, The function clip(·) represents the clipping operation, k v Represents the speed error proportional gain parameter, which is a constant greater than zero; kvi represents the speed error integral gain parameter, which is also a constant greater than zero; η represents the integral separation control parameter. v Exceeding the critical error e v0 When , no integral operation is performed, and the integral is separated; e vI Indicates the integral saturation control term. When the error integral is greater than E i When , the integral error saturates and no longer increases.

[0067] Then define the reference thrust of the propeller as:

[0068] T=m||a v || (10)

[0069] Where m is the mass of the drone, a v represents the acceleration produced by the desired propeller thrust.

[0070] Furthermore, the method for obtaining the reference attitude of the UAV roll decoupling is:

[0071] The reference attitude of the UAV is determined based on the nonlinear vector projection method. In order to realize the tracking of the roll angle, the attitude of the roll angle is added to the reference attitude of zero roll.

[0072] Reference pose determination problem: Finding a (rotation matrix from the inertial frame to the reference attitude coordinate system), so that i b with a v Align while keeping the lower belly vector k of the aircraft as close as possible b Vector k pointing downward i =[0,0,1] T .

[0073] This is done using a nonlinear vector projection algorithm, which calculates the normal vectors of the two conditions.

[0074]

[0075] Where θ is k i with a v The angle between them, normalized:

[0076]

[0077] The lower abdomen vector closest to the downward direction is k i Perpendicular to and The projection of the plane.

[0078]

[0079] Vector and Proj(k i ) are orthogonal and obey the right-hand rule. Aligning the aircraft's three fuselage axes with these vectors ensures that the aircraft points its thrust to achieve its position control goals while pointing the underbelly as far down as possible. This second condition results in a vector associated with the wing orientation The direction of is always contained in the north-east plane. Figure 2 shown.

[0080] So the reference posture Can be constructed as:

[0081]

[0082] In the formula, k i represents the basis vectors of the inertial frame, represents the vector representing the direction in which the drone's nose is expected to point. The reference attitude represented by equation (15) has a roll angle of zero for Euler angles with a pitch angle less than 90°, because the element Therefore, when the roll angle is always zero, the position controller actually achieves position tracking by controlling the pitch angle and yaw angle to achieve lateral and longitudinal motion. The reference attitude defined here is in the form of zero roll. In order to track the roll angle trace, it is necessary to define an attitude reference with the roll angle added, which contains an independent roll instruction φ c , as shown below:

[0083]

[0084] Furthermore, the attitude is controlled in stages based on the size of the attitude error. The staged control method is as follows: determine whether the attitude angle deviations of the three axes are all greater than π / 4. When they are all greater than π / 4, large-angle maneuvering attitude control is performed in a special orthogonal group space. When they are not all greater than π / 4, small-angle error attitude control is performed in the Euler angle.

[0085] Among them, the attitude control method of SO(3) space large-angle maneuver is as follows:

[0086] Position controller output It represents the rotation matrix from the inertial system to the reference attitude. The rotation matrix from the reference attitude to the machine system can be expressed as

[0087]

[0088] when Sometimes, there is At this time, the attitude error is zero. A posture error function that can make large attitude deviations converge quickly and stably is adopted:

[0089]

[0090] Among them, Γ a (.) is a skew-symmetric projection operator defined on a square matrix. is a non-crossover operator defined on an antisymmetric matrix,

[0091] The attitude error law (torque generated by the deflection of the rudder surface) designed to make the angular velocity error and attitude error approach zero is:

[0092]

[0093] Where, the gain k md and k mp are all diagonal matrices greater than zero.

[0094] Among them, the posture control method of small angle in Euler angle space is as follows:

[0095] First, convert the rotation matrix from the inertial system to the reference posture into Euler angles (the rotation order is "XYZ" internal rotation)

[0096]

[0097] Then the proportional-integral-differential control law is designed as:

[0098]

[0099] Among them, the integral term is the integral with saturation function; k p , k i and k d is a diagonal matrix greater than zero.

[0100] The above are only preferred specific implementations of the present application, but the protection scope of the present application is not limited thereto. Any changes or substitutions that can be easily thought of by a person skilled in the art within the technical scope disclosed in the present application should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.

Claims

1. A rolling decoupled small agile fixed-wing UAV trajectory tracking control method, characterized in that: The following steps are involved: The position error of the UAV during trajectory tracking is calculated based on the gravity normal system, and a reference speed for adjusting the speed at which the UAV converges to the trajectory is obtained based on the UAV position error calculation; Subtract the reference speed from the speed of the drone in the inertial system to obtain a speed error; Based on the rate of change of the reference speed and the speed error, a proportional-integral speed control law about the inertia speed is constructed, and a desired acceleration command is obtained based on the speed control law; The expression of the reference speed is: In the formula, v com represents the desired velocity of the trajectory, d p represents a positive constant, v t Indicates the speed in the progressive direction; The expression of the speed in the asymptotic direction is: v t =d p i s -k s e s i s -k c e c j s -k h e h k s In the formula, k s Represents the positive constant coefficient of the control trajectory tracking tangent direction error, k c represents the positive constant coefficient of the lateral error of the control trajectory tracking, k h represents the positive constant coefficient of the longitudinal error of the control trajectory tracking, d p represents a positive constant coefficient that controls the speed at which the drone converges to the trajectory. k s =(i s ) × j s , k i represents the basis vectors of the inertial frame, Represents the three-dimensional space trajectory of the drone; Based on the expected acceleration command, a reference thrust of the propeller of the UAV is obtained, and the thrust control of the UAV is performed based on the reference thrust; The expression of the acceleration command is: In the formula, the function clip(·) represents the clipping operation, k v Represents the speed error proportional gain parameter, which is a constant greater than zero; k vi represents the speed error integral gain parameter, which is also a constant greater than zero; η represents the integral separation control parameter. v Exceeding the critical error e v0 When , no integral operation is performed, and the integral is separated; e vI Indicates the integral saturation control term. When the error integral is greater than E i When , the integral error saturates and no longer increases; Determine the reference attitude of the UAV based on the nonlinear vector projection method, add the roll angle attitude to the reference attitude, obtain the reference attitude of the UAV roll decoupling, and control the attitude of the UAV by adopting the error classification control method; The attitude is controlled in stages based on the size of the attitude error. The staged control method is as follows: determine whether the attitude angle deviations of the three axes are all greater than π / 4. When they are all greater than π / 4, large-angle maneuvering attitude control is performed in a special orthogonal group space. When they are not all greater than π / 4, small-angle error attitude control is performed in the Euler angle.

2. The trajectory tracking control method of a small agile fixed-wing UAV with roll decoupling according to claim 1 is characterized in that: The expression of the reference thrust is: T=m||a v || In the formula, m represents the mass of the drone, a v represents the acceleration produced by the desired propeller thrust.

3. The trajectory tracking control method of a small agile fixed-wing UAV with roll decoupling according to claim 1 is characterized in that: The expression of the reference posture of the UAV is: In the formula, k i represents the basis vectors of the inertial frame, A vector representing the direction in which the drone's nose is expected to point.

4. The trajectory tracking control method of a small agile fixed-wing UAV with roll decoupling according to claim 1 is characterized in that: The expression of the reference attitude of the UAV roll decoupling is: In the formula, represents the reference attitude of the UAV, φ c Indicates a roll command.

Images