Aircraft path planning and control method in complex mountainous environment

Abstract

The application discloses a kind of complex mountainous environment under aircraft's flight path planning and control method, comprising the following steps: step one, establish the kinematics and three-dimensional dynamics model of aircraft containing external disturbance;Step two, generate desired flight trajectory satisfying obstacle avoidance constraints;Step three, design preset time extended state observer;Step four, design trajectory tracking controller based on sliding mode control;The application realizes the trajectory planning task of aircraft in complex obstacle environment by converting the safety distance constraint between aircraft and obstacle into a quadratic optimization problem, significantly improves flight safety;Through preset time extended state observer, external disturbance can be converged and accurately estimated within preset time, and disturbance dynamic compensation is realized by cooperating with sliding mode control, significantly reduces the influence of wind, terrain air flow and other disturbances on trajectory tracking, improves system control accuracy and anti-interference robustness, ensures that aircraft completes various tasks such as reconnaissance and rescue in complex mountainous environment stably.

Description

Technical Field

[0001] This invention relates to the field of aircraft planning and control technology, specifically to a method for flight path planning and control of aircraft in complex mountainous environments. Background Technology

[0002] Fixed-wing aircraft achieve long-distance flight through aerodynamics, possessing advantages such as simple structure, high maneuverability, and wide coverage, giving them significant strengths in both military and non-military fields. However, with advancements in aviation and manufacturing technologies, and increasingly complex combat and mission environments, current aircraft trajectory planning and control methods suffer from the following drawbacks: 1. In complex airspace such as mountainous terrain, no-fly zones, and radar detection areas, aircraft struggle to maneuver to meet necessary distance constraints and ensure their safety; 2. During flight, aircraft are susceptible to external disturbances such as wind disturbances, air pressure changes, and terrain-induced airflow disturbances, and existing methods struggle to quickly and in real-time estimate these disturbances. Summary of the Invention

[0003] The purpose of this invention is to provide a method for flight path planning and control of aircraft in complex mountainous environments, so as to solve the problems mentioned in the background art.

[0004] To achieve the above objectives, the present invention provides the following technical solution: a method for trajectory planning and control of an aircraft in a complex mountainous environment, comprising the following steps: Step 1, establishing a kinematic and three-dimensional dynamic model of the aircraft with external disturbances; Step 2, generating a desired flight trajectory that satisfies obstacle avoidance constraints; Step 3, designing a preset time-dilation state observer; Step 4, designing a trajectory tracking controller based on sliding mode control.

[0005] In step one above, a three-dimensional kinematic and three-dimensional dynamic model of the aircraft's center of mass is established to address external disturbances in complex mountainous environments.

[0006] In step two above, a reference aircraft system is constructed based on the model established in step one, taking into account distance constraints and extended classes. Functions and quadratic optimization algorithms are used to generate desired obstacle avoidance trajectories that meet safety distance constraints in complex mountainous environments;

[0007] In step three above, the external disturbance in step one is regarded as an additional state of the aircraft system. An extended state observer is constructed based on a preset time theory to estimate the state of the aircraft system in real time.

[0008] In step four above, the tracking error is defined based on the expected obstacle avoidance trajectory generated in step two, and the extended state observer estimation results obtained in step three are combined with the design of the aircraft trajectory tracking controller based on sliding mode control theory.

[0009] Preferably, in step one, the three-dimensional kinematics are:

[0010]

[0011] The three-dimensional dynamic model is as follows:

[0012]

[0013] in, These are the position coordinates of the aircraft in the inertial coordinate system. , and These are the three-dimensional velocity components of the aircraft in the inertial coordinate system. , and These are, respectively, aircraft speed, trajectory inclination angle, and trajectory deviation angle; , and ballistic coordinate system axis, shaft and Overload on the shaft It is the acceleration due to gravity;

[0014] right , and Find the second derivative, then we have

[0015]

[0016] in,

[0017]

[0018] make Then there is

[0019]

[0020] Wherein, the state vector , inherent term Control input External disturbances .

[0021] Preferably, step two specifically includes the following steps:

[0022] 2.1 Constructing the kinematic model of the reference aircraft system:

[0023]

[0024] in, To reference the desired trajectory position coordinates of the aircraft, , and The three-dimensional velocity components of the desired trajectory, , and These are the reference aircraft speed, trajectory inclination angle, and trajectory deflection angle, respectively.

[0025] 2.2 Calculating the State of the Reference Aircraft System: Define the desired state vector of the reference aircraft system as... Assuming the reference aircraft flies at a constant speed, the position vector of the next waypoint on the flight path is... ,but It can be calculated in the following way:

[0026]

[0027]

[0028]

[0029] in, for The initial value;

[0030] 2.3 Establish obstacle avoidance distance constraints: Define the distance variable as:

[0031]

[0032] in, For the reference aircraft position vector, Let be the obstacle's position vector. This is the safe distance threshold;

[0033]

[0034] in, Based on the safe distance, For reference velocity components, This is for simulating step size;

[0035] The constraint is: ensure that the distance between the reference aircraft and the obstacle is greater than a safe threshold, i.e. Combined with extended classes function , According to the definition of the control barrier function, for Establish the following constraints:

[0036]

[0037] 2.4 Derivation of the linearized form of the constraints: Based on the reference kinematic model, we can obtain:

[0038]

[0039] in,

[0040]

[0041] Will Recorded as , can be obtained

[0042]

[0043] in, ;

[0044] 2.5 Quadratic Optimization to Generate the Desired Trajectory: A quadratic optimization algorithm is used to generate the system state that satisfies the obstacle avoidance constraints. The specific algorithm is as follows:

[0045]

[0046] in, It is regulation , and Weight matrix of the three channel constraints; Three-dimensional vector The first in One variable, ; and Let these be the minimum and maximum values ​​of the desired speed, respectively. The desired obstacle avoidance trajectory can be obtained by substituting it into the kinematic model of the reference aircraft.

[0047] Preferably, step three specifically includes the following steps:

[0048] 3.1 Establishing an extended state model for the aircraft: incorporating external disturbances Considered as an additional state of the aircraft system ,make The extended state model of the aircraft is then:

[0049]

[0050]

[0051] 3.2 Design of a preset time observer structure: to control the input and the speed of the aircraft system As input, design the following preset time-dilation state observer:

[0052]

[0053]

[0054] in, ; For system bandwidth; and They are respectively and Observed values; It is a preset time dynamic gain that satisfies

[0055]

[0056] in, , and , This is the preset convergence time.

[0057] Preferably, step four specifically includes the following steps:

[0058] 4.1 Define trajectory tracking error: based on the desired obstacle avoidance trajectory generated in step two. Define trajectory tracking error ,in, This refers to the actual location of the aircraft.

[0059] 4.2 Design of linear sliding surfaces: Select the following linear sliding surfaces:

[0060]

[0061] in, ;

[0062] 4.3 Constructing a sliding mode controller: The sliding mode controller is designed as follows:

[0063]

[0064] in, .

[0065] Compared with the prior art, the beneficial effects of the present invention are as follows: In use, the present invention combines the control obstacle function and the invariant set theory to transform the safe distance constraint between the aircraft and the obstacle into a quadratic optimization problem that can be solved in real time, thus efficiently solving the trajectory planning problem in complex mountainous multi-obstacle environments; by designing a sliding mode controller that integrates a preset time-dilation state observer, it can quickly estimate and dynamically compensate for bounded external disturbances in real time, taking into account both the accuracy of trajectory tracking and the robustness of the system against interference. Attached Figure Description

[0066] Figure 1 This is a flowchart of the method of the present invention;

[0067] Figure 2 A three-dimensional obstacle avoidance trajectory diagram for the aircraft;

[0068] Figure 3A graph showing the distance between the aircraft and the obstacle;

[0069] Figure 4 A graph showing the velocity observation effect of the extended state observer;

[0070] Figure 5 The graph shows the effect of interference observation on the extended state observer. Detailed Implementation

[0071] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0072] Please see the appendix Figure 1 -Appendix Figure 5 The present invention provides an embodiment of a method for trajectory planning and control of an aircraft in a complex mountainous environment, comprising the following steps: Step 1, establishing a kinematic and three-dimensional dynamic model of the aircraft with external disturbances; Step 2, generating a desired flight trajectory that satisfies obstacle avoidance constraints; Step 3, designing a preset time-dilation state observer; Step 4, designing a trajectory tracking controller based on sliding mode control.

[0073] In step one above, a three-dimensional kinematic and dynamic model of the aircraft's center of mass is established to address external disturbances in complex mountainous environments; the three-dimensional kinematics is as follows:

[0074]

[0075] The three-dimensional dynamic model is as follows:

[0076]

[0077] in, These are the position coordinates of the aircraft in the inertial coordinate system. , and These are the three-dimensional velocity components of the aircraft in the inertial coordinate system. , and These are, respectively, aircraft speed, trajectory inclination angle, and trajectory deviation angle; , and ballistic coordinate system axis, shaft and Overload on the shaft It is the acceleration due to gravity;

[0078] right , and Find the second derivative, then we have

[0079]

[0080] in,

[0081]

[0082] make Then there is

[0083]

[0084] Wherein, the state vector , inherent term Control input External disturbances ;

[0085] In step two above, a reference aircraft system is constructed based on the model established in step one, taking into account distance constraints and extended classes. Using functions and quadratic optimization algorithms, the desired obstacle avoidance trajectory that satisfies safety distance constraints in complex mountainous environments is generated, specifically including the following steps:

[0086] 2.1 Constructing the kinematic model of the reference aircraft system:

[0087]

[0088] in, To reference the desired trajectory position coordinates of the aircraft, , and The three-dimensional velocity components of the desired trajectory, , and These are the reference aircraft speed, trajectory inclination angle, and trajectory deflection angle, respectively.

[0089] 2.2 Calculating the State of the Reference Aircraft System: Define the desired state vector of the reference aircraft system as... Assuming the reference aircraft flies at a constant speed, the position vector of the next waypoint on the flight path is... ,but It can be calculated in the following way:

[0090]

[0091]

[0092]

[0093] in, for The initial value;

[0094] 2.3 Establish obstacle avoidance distance constraints: Define the distance variable as:

[0095]

[0096] in, For the reference aircraft position vector, Let be the obstacle's position vector. This is the safe distance threshold;

[0097]

[0098] in, Based on the safe distance, As a reference velocity component, This is for simulating step size;

[0099] The constraint is: ensure that the distance between the reference aircraft and the obstacle is greater than a safe threshold, i.e. Combined with extended classes function , According to the definition of the control barrier function, for Establish the following constraints:

[0100]

[0101] 2.4 Derivation of the linearized form of the constraints: Based on the reference kinematic model, we can obtain:

[0102]

[0103] in,

[0104]

[0105] Will Recorded as , can be obtained

[0106]

[0107] in, ;

[0108] 2.5 Quadratic Optimization to Generate the Desired Trajectory: A quadratic optimization algorithm is used to generate the system state that satisfies the obstacle avoidance constraints. The specific algorithm is as follows:

[0109]

[0110] in, It is regulation , and Weight matrix of the three channel constraints; Three-dimensional vector The first in One variable, ; and Let these be the minimum and maximum values ​​of the desired speed, respectively. Substituting these values ​​into the kinematic model of the reference aircraft yields the desired obstacle avoidance trajectory.

[0111] In step three above, the external disturbance in step one is regarded as an additional state of the aircraft system. An extended state observer is constructed based on a preset time theory to estimate the state of the aircraft system in real time. Specifically, the steps include:

[0112] 3.1 Establishing an extended state model for the aircraft: incorporating external disturbances Considered as an additional state of the aircraft system ,make The extended state model of the aircraft is then:

[0113]

[0114]

[0115] 3.2 Design of a preset time observer structure: to control the input and the speed of the aircraft system As input, design the following preset time-dilation state observer:

[0116]

[0117]

[0118] in, ; For system bandwidth; and They are respectively and Observed values; It is a preset time dynamic gain that satisfies

[0119]

[0120] in, , and , Preset convergence time;

[0121] In step four above, the tracking error is defined based on the desired obstacle avoidance trajectory generated in step two. Combined with the extended state observer estimation results obtained in step three, the aircraft trajectory tracking controller is designed based on sliding mode control theory. Specifically, the steps include:

[0122] 4.1 Define trajectory tracking error: based on the desired obstacle avoidance trajectory generated in step two. Define trajectory tracking error ,in, This refers to the actual location of the aircraft.

[0123] 4.2 Design of linear sliding surfaces: Select the following linear sliding surfaces:

[0124]

[0125] in, ;

[0126] 4.3 Constructing a sliding mode controller: The sliding mode controller is designed as follows:

[0127]

[0128] in, .

[0129] Experimental example:

[0130] To verify the effectiveness of this method, the following simulation experiment was conducted: Multiple mountain peaks of varying heights (120m-350m) and ranges (600m-900m) were randomly generated on a 5000m×5000m map to simulate a real mountainous environment. Furthermore, the initial conditions for the aircraft were set as follows: , , , , , The control parameters are set as follows: , , , , , ,in, Represents a diagonal matrix. System simulation step size. The observer's preset time parameters The safe distance between the aircraft and the obstacle is set to The simulation results are attached. Figure 2 -Appendix Figure 5As shown in Figure 2, the three-dimensional obstacle avoidance trajectory of the aircraft is illustrated. The yellow square represents the aircraft's starting point, and the yellow pentagram represents its ending point. The dashed line is the trajectory of the reference aircraft obtained through a secondary optimization algorithm, while the solid line represents the aircraft's actual flight trajectory. It can be seen that the aircraft can safely complete its flight mission in mountainous environments and ultimately reach the desired path point. The distance variation curve between the aircraft and obstacles is shown in the attached figure. Figure 3 As shown in the figure, the red line represents the pre-set safe distance. It can be seen that the distance between the aircraft and the obstacle is always greater than the safe distance, meeting the mission requirements. The observation results of the extended state observer are shown in the attached figure. Figure 4 and attached Figure 5 As shown in the observation curves of velocity and disturbance, the observer proposed in this method can guarantee that the error is within the preset time. Convergence was achieved; in summary, the effectiveness of this method has been verified.

[0131] Based on the above, the advantages of this invention are that, when used, this invention combines the control obstacle function and the theory of invariant sets to transform the safety distance constraint between the aircraft and the obstacle into a quadratic optimization problem that can be solved in real time, thus efficiently solving the trajectory planning problem in complex mountainous environments with multiple obstacles; by designing a sliding mode controller that integrates a preset time-dilation state observer, it can quickly estimate and dynamically compensate for bounded external disturbances in real time, taking into account both the accuracy of trajectory tracking and the robustness of the system against interference.

[0132] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the invention can be implemented in other specific forms without departing from its spirit or essential characteristics. Therefore, the embodiments should be considered in all respects as exemplary and non-limiting, and the scope of the invention is defined by the appended claims rather than the foregoing description. Thus, all variations falling within the meaning and scope of equivalents of the claims are intended to be included within the present invention. No reference numerals in the claims should be construed as limiting the scope of the claims.

Claims

1. A method for trajectory planning and control of an aircraft in a complex mountainous environment, comprising the following steps: Step 1: Establish the kinematics and three-dimensional dynamics model of the aircraft with external disturbances; Step 2: Generate the desired flight trajectory that satisfies obstacle avoidance constraints; Step 3: Design a preset time-dilation state observer; Step 4: Design a trajectory tracking controller based on sliding mode control; Its characteristics are: In step one above, a three-dimensional kinematic and three-dimensional dynamic model of the aircraft's center of mass is established to address external disturbances in complex mountainous environments. In step two above, a reference aircraft system is constructed based on the model established in step one, taking into account distance constraints and extended classes. Functions and quadratic optimization algorithms are used to generate desired obstacle avoidance trajectories that meet safety distance constraints in complex mountainous environments; In step three above, the external disturbance in step one is regarded as an additional state of the aircraft system. An extended state observer is constructed based on a preset time theory to estimate the state of the aircraft system in real time. In step four above, the tracking error is defined based on the expected obstacle avoidance trajectory generated in step two, and the extended state observer estimation results obtained in step three are combined with the design of the aircraft trajectory tracking controller based on sliding mode control theory. Step two specifically includes the following steps: Step 2.1 Construct the kinematic model of the reference aircraft system: ， in, To reference the desired trajectory position coordinates of the aircraft, , and The three-dimensional velocity components of the desired trajectory, , and These are the reference aircraft speed, trajectory inclination angle, and trajectory deflection angle, respectively. Step 2.2 Calculate the reference aircraft system state: Define the desired state vector of the reference aircraft system as follows: Assuming the reference aircraft flies at a constant speed, the position vector of the next waypoint on the flight path is... ,but It can be calculated in the following way: ， ， ， in, for The initial value; Step 2.3 Establish obstacle avoidance distance constraints: Define the distance variable as: ， in, For the reference aircraft position vector, Let be the obstacle's position vector. This is the safe distance threshold; ， in, Based on the safe distance, As a reference velocity component, This is for simulating step size; The constraint is: ensure that the distance between the reference aircraft and the obstacle is greater than a safe threshold, i.e. Combined with extended classes function , According to the definition of the control barrier function, for Establish the following constraints: ， Step 2.4 Derivation of the linearized form of the constraints: Based on the reference kinematic model, we can obtain: ， in, ， Will Recorded as , can be obtained ， in, ; Step 2.5 Secondary Optimization to Generate the Desired Trajectory: A secondary optimization algorithm is used to generate the system state that satisfies the obstacle avoidance constraints. The specific algorithm is as follows: ， in, It is regulation , and Weight matrix of the three channel constraints; Three-dimensional vector The first in One variable, ; and Let these be the minimum and maximum values ​​of the desired speed, respectively. Substituting these values ​​into the kinematic model of the reference aircraft yields the desired obstacle avoidance trajectory. Step three specifically includes the following steps: Step 3.1 Establish the extended state model of the aircraft: incorporate external disturbances Considered as an additional state of the aircraft system ,make The extended state model of the aircraft is then: ， ， Step 3.2 Design the preset time observer structure: to control the input and the speed of the aircraft system As input, design the following preset time-dilation state observer: ， ， in, ; For system bandwidth; and They are respectively and Observed values; ; and These are the ballistic inclination angle and the ballistic deviation angle, respectively. For gravitational acceleration; inherent term Control input External disturbances ; It is a preset time dynamic gain that satisfies ， in, , and , This is the preset convergence time.

2. The method for trajectory planning and control of an aircraft in a complex mountainous environment according to claim 1, characterized in that: In step one, the three-dimensional kinematics are as follows: ， The three-dimensional dynamic model is as follows: ， in, These are the position coordinates of the aircraft in the inertial coordinate system. , and These are the three-dimensional velocity components of the aircraft in the inertial coordinate system. For the speed of the aircraft; , and ballistic coordinate system axis, shaft and Overload on the shaft; right , and Find the second derivative, then we have ， in, ， make Then there is ， Wherein, the state vector , inherent term Control input External disturbances .

3. The method for trajectory planning and control of an aircraft in a complex mountainous environment according to claim 1, characterized in that: Step four specifically includes the following steps: Step 4.1 Define trajectory tracking error: based on the desired obstacle avoidance trajectory generated in Step 2. Define trajectory tracking error ,in, This refers to the actual location of the aircraft. Step 4.2 Design the linear sliding surface: Select the following linear sliding surface: ， in, ; Step 4.3 Constructing the sliding mode controller: The sliding mode controller is designed as follows: ， in, .

Images