Flight trajectory tracking method and apparatus

Abstract

This invention belongs to the field of flight control technology and discloses a flight trajectory tracking method and apparatus. The method includes: acquiring preset speed information; determining initial flight speed data based on the preset speed information; smoothing the initial flight speed data based on a discrete maximum tracking function to obtain desired flight speed data; determining a desired position error based on the desired flight speed data; selecting points on the planned trajectory based on the desired position error to determine the coordinate data of the desired trajectory points; smoothing the coordinate data of the desired trajectory points based on a third-order system smoothing strategy to obtain target coordinate data; and sending the target coordinate data to a flight controller so that the flight controller controls the aircraft to perform trajectory tracking based on the target coordinate data. Through this method, even in scenarios lacking speed planning, the aircraft can fly smoothly at a specified speed.

Description

Technical Field

[0001] This invention relates to the field of flight control technology, and in particular to a flight trajectory tracking method and apparatus. Background Technology

[0002] When planning and controlling the trajectory of an aircraft, it is usually based on position and velocity planning, and then control commands are given. The controller usually achieves trajectory tracking through proportional-integral-derivative (PID) or model predictive control (MPC). However, neither of these methods is suitable for scenarios without velocity planning, cannot achieve the goal of the aircraft flying at the set speed, and it is difficult to ensure stable flight attitude.

[0003] The above content is only used to help understand the technical solution of the present invention and does not represent an admission that the above content is prior art. Summary of the Invention

[0004] The main objective of this invention is to provide a flight trajectory tracking method and apparatus, which aims to solve the technical problem in the prior art that, in the absence of speed planning, the aircraft cannot fly at the set speed and it is difficult to ensure stable flight attitude.

[0005] To achieve the above objectives, the present invention provides a flight trajectory tracking method, the method comprising the following steps:

[0006] Obtain preset speed information, and determine initial flight speed data based on the preset speed information;

[0007] Based on the discrete fastest tracking function, the initial flight speed data is smoothed to obtain the desired flight speed data;

[0008] Based on the desired flight speed data, determine the desired position error;

[0009] Based on the expected position error, select points on the planned trajectory points to determine the coordinate data of the expected trajectory points;

[0010] The coordinate data of the desired trajectory points are smoothed using a third-order system smoothing strategy to obtain the target coordinate data.

[0011] The target coordinate data is sent to the flight controller so that the flight controller can control the aircraft to perform trajectory tracking based on the target coordinate data.

[0012] Optionally, the smoothing process of the initial flight speed data based on the discrete fastest tracking function to obtain the desired flight speed data includes:

[0013] Based on the discrete fastest tracking function, a first correspondence is determined between the initial flight speed data, the preset filter factor, the acceleration threshold, and the desired flight speed data;

[0014] The desired flight speed data is determined based on the initial flight speed data, the preset filter factor, the acceleration threshold, and the first correspondence.

[0015] Optionally, determining the desired position error based on the desired flight speed data includes:

[0016] The initial expected position error is determined based on the expected flight speed data and the square root control coefficient.

[0017] When the initial expected error is less than or equal to a preset error threshold, the expected position error is determined to be the initial expected position error.

[0018] Optionally, after determining the initial expected position error based on the expected flight speed data and the square root control coefficient, the method further includes:

[0019] When the initial expected error is greater than the preset error threshold, a second correspondence is obtained between the square root control coefficient, the acceleration threshold, the expected flight speed data, and the expected position error;

[0020] The desired position error is obtained based on the square root control coefficient, the acceleration threshold, the desired flight speed data, and the second correspondence.

[0021] Optionally, the step of selecting points on the planned trajectory points based on the desired position error and determining the coordinate data of the desired trajectory points includes:

[0022] Obtain the planned trajectory points and the current position, and determine the expected error point on the planned trajectory points based on the expected position error and the current position;

[0023] When the expected error point is located between the current position and the first trajectory point in the planned trajectory points, a third correspondence is obtained between the expected position error, the coordinate data of the first trajectory point, the coordinate data of the current position, and the coordinate data of the expected trajectory point.

[0024] The coordinate data of the desired trajectory point are obtained based on the expected position error, the coordinate data of the first trajectory point, the coordinate data of the current position, and the third correspondence.

[0025] Optionally, after obtaining the planned trajectory point and the current position, and determining the expected error point on the planned trajectory point based on the expected position error and the current position, the method further includes:

[0026] When the first trajectory point coincides with the current position or the current position and the first trajectory point meet a preset numerical condition, the coordinate data of the desired trajectory point is determined to be the coordinate data of the first trajectory point.

[0027] Optionally, after obtaining the planned trajectory point and the current position, and determining the expected error point on the planned trajectory point based on the expected position error and the current position, the method further includes:

[0028] When the expected error point is located between the second trajectory point and the last trajectory point in the planned trajectory points, the initial interval distance is determined based on the interval distance between the current position and the first trajectory point;

[0029] Obtain the distance between the planned trajectory points and initialize the loop variable;

[0030] A distance threshold is determined based on the initial interval distance and the trajectory point interval distance;

[0031] When the expected position error is less than or equal to the distance threshold, a fourth correspondence is obtained between the expected position error, the initial interval distance, the coordinate data of the planned trajectory point, and the coordinate data of the expected trajectory point.

[0032] The coordinate data of the desired trajectory point are obtained based on the expected position error, the initial interval distance, the coordinate data of the planned trajectory point, and the fourth correspondence.

[0033] Optionally, after determining the distance threshold based on the initial interval distance and the trajectory point interval distance, the method further includes:

[0034] When the expected position error is greater than the distance threshold, the loop variable is updated;

[0035] When the loop variable is greater than the number of trajectory points, the coordinate data of the desired trajectory point is determined to be the coordinate data of the last trajectory point;

[0036] When the loop variable is less than or equal to the number of trajectory points, the initial interval distance is updated to the distance threshold, and the process returns to the step of determining the distance threshold based on the initial interval distance and the trajectory point interval distance.

[0037] Optionally, the desired trajectory points include horizontal desired trajectory points in the horizontal direction and vertical desired trajectory points in the vertical direction. The smoothing process of the coordinate data of the desired trajectory points based on a third-order system smoothing strategy to obtain target coordinate data includes:

[0038] Obtain the coordinate data of the horizontal desired trajectory point and the coordinate data of the vertical desired trajectory point;

[0039] Based on the third-order system smoothing strategy, a fifth correspondence is determined between the coordinate data of the desired trajectory point, the natural frequency of the position loop, the complex frequency, and the smoothed coordinate data.

[0040] Based on the coordinate data of the desired horizontal trajectory point, the natural frequency of the position loop, the complex frequency, and the fifth correspondence, the smooth coordinate data in the horizontal direction are determined.

[0041] Based on the coordinate data of the vertical desired trajectory point, the natural frequency of the position loop, the complex frequency, and the fifth correspondence, the smooth coordinate data in the vertical direction are determined;

[0042] The target coordinate data is determined based on the smoothed coordinate data in the horizontal direction and the smoothed coordinate data in the vertical direction.

[0043] Furthermore, to achieve the above objectives, the present invention also proposes a flight trajectory tracking device, the flight trajectory tracking device comprising:

[0044] A speed smoothing module is used to acquire preset speed information and determine initial flight speed data based on the preset speed information;

[0045] The speed smoothing module is also used to smooth the initial flight speed data based on the discrete fastest tracking function to obtain the desired flight speed data;

[0046] The position determination module is used to determine the desired position error based on the desired flight speed data;

[0047] The position determination module is also used to select points on the planned trajectory points based on the expected position error, and determine the coordinate data of the expected trajectory points;

[0048] The position determination module is also used to smooth the coordinate data of the desired trajectory point based on a third-order system smoothing strategy to obtain target coordinate data;

[0049] The trajectory tracking module is used to send the target coordinate data to the flight controller, so that the flight controller can control the aircraft to perform trajectory tracking based on the target coordinate data.

[0050] In this invention, preset speed information is acquired, and initial flight speed data is determined based on this information. The initial flight speed data is then smoothed using a discrete maximum tracking function to obtain desired flight speed data. Based on this desired flight speed data, a desired position error is determined. Based on this desired position error, points are selected on the planned trajectory points to determine the coordinate data of the desired trajectory points. The coordinate data of the desired trajectory points is then smoothed using a third-order system smoothing strategy to obtain target coordinate data. This target coordinate data is then sent to the flight controller, enabling the flight controller to control the aircraft to perform trajectory tracking based on the target coordinate data. Compared to traditional control schemes, where the aircraft cannot fly at the set speed and it is difficult to maintain a stable flight attitude in scenarios without speed planning, this invention calculates the required error based on the characteristics of the controller generating a specific speed, and then performs point selection. Simultaneously, the output position command is smoothed to design a stable flight trajectory for the aircraft, ensuring stable attitude during flight. Even in scenarios with only position trajectory points and lacking speed planning, the aircraft can still fly at the specified speed with a relatively stable attitude. Attached Figure Description

[0051] Figure 1 This is a flowchart illustrating the first embodiment of the flight trajectory tracking method of the present invention;

[0052] Figure 2 This is a schematic diagram of the overall process of an embodiment of the flight trajectory tracking method of the present invention;

[0053] Figure 3 This is a flowchart illustrating the second embodiment of the flight trajectory tracking method of the present invention;

[0054] Figure 4 This is a schematic diagram of the trajectory where the expected error point is located between the current position and the first trajectory point, according to an embodiment of the flight trajectory tracking method of the present invention.

[0055] Figure 5 This is a flowchart illustrating the third embodiment of the flight trajectory tracking method of the present invention;

[0056] Figure 6 This is a schematic diagram of a trajectory where the expected error point is located between the second trajectory point and the last trajectory point, according to an embodiment of the flight trajectory tracking method of the present invention.

[0057] Figure 7 This is a structural block diagram of the first embodiment of the flight trajectory tracking device of the present invention.

[0058] The realization of the objective, functional features and advantages of the present invention will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0059] It should be understood that the specific embodiments described herein are for illustrative purposes only and are not intended to limit the scope of the invention.

[0060] This invention provides a flight trajectory tracking method, referring to... Figure 1 , Figure 1 This is a flowchart illustrating a first embodiment of a flight trajectory tracking method according to the present invention.

[0061] In this embodiment, the flight trajectory tracking method includes the following steps:

[0062] Step S10: Obtain preset speed information, and determine the initial flight speed data based on the preset speed information.

[0063] It should be noted that the execution subject in this embodiment is a flight trajectory tracking device. The flight trajectory tracking device is used to design a stable flight trajectory for the aircraft in the absence of speed planning, so that the aircraft can fly at a specified speed with a relatively stable attitude.

[0064] It is understood that the preset speed information refers to the flight speed set by the user in advance, that is, the speed specified by the user for the aircraft, which is obtained from the ground station. On the premise of ensuring that it does not exceed the flight envelope, different speed values ​​can be set for different flight segments. This embodiment does not impose any restrictions on this and can be set according to actual needs.

[0065] It should be understood that after obtaining the user-set flight speed, the aircraft can be considered to have roughly three stages during flight: acceleration from zero to the set speed, speed changes during flight segments, and deceleration to zero, thereby obtaining the initial speed command, which is the initial flight speed data.

[0066] Step S20: Based on the discrete fastest tracking function, smooth the initial flight speed data to obtain the desired flight speed data.

[0067] It should be noted that directly using the initial speed command cannot guarantee the stability of the aircraft during flight. Therefore, this embodiment smooths the initial speed command to smooth the acceleration. The smoothing of acceleration corresponds to a smaller amplitude and frequency of changes in the aircraft's attitude angle, thereby improving flight stability. The expected flight speed data is the speed command obtained after smoothing, which is the required / expected speed.

[0068] Further, step S20 includes: determining a first correspondence between the initial flight speed data, the preset filter factor, the acceleration threshold, and the desired flight speed data according to the discrete fastest tracking function; and determining the desired flight speed data according to the initial flight speed data, the preset filter factor, the acceleration threshold, and the first correspondence.

[0069] It is understandable that the acceleration threshold refers to the maximum acceleration of the aircraft, the preset filter factor refers to the set filter factor, and the smoothing process adopts the form of a discrete maximum speed tracking function. First, it is necessary to determine the calculation relationship between the initial flight speed data, the preset filter factor, the acceleration threshold, and the desired flight speed data according to the form of the discrete maximum speed tracking function, that is, the first correspondence relationship, which can be expressed by the following expression:

[0070]

[0071]

[0072] In the formula, V_c is the speed command (initial flight speed data), and V_sp is the smoothed speed command (desired flight speed data). Let a_max be the derivative of the velocity, h be the preset filter factor, and u be the intermediate variable. By substituting the relevant data into the calculation formula corresponding to the first relationship mentioned above, the desired flight speed data can be obtained.

[0073] In practice, after obtaining the user-set flight speed, the process of accelerating from zero to the set speed, changing the speed of the flight segment, and decelerating to zero is smoothed to ensure smooth acceleration and flight stability.

[0074] Step S30: Determine the desired position error based on the desired flight speed data.

[0075] It should be understood that the desired position error refers to the required position error, that is, the error between the desired current position and the desired position. In this embodiment, the required error is calculated based on the characteristics of the controller generating a specific speed. The controller can be a square root controller or a proportional controller, and the choice can be made according to actual needs. This embodiment does not impose any restrictions on this.

[0076] Furthermore, if a square root controller is used, step S30 includes: determining an initial expected position error based on the expected flight speed data and the square root control coefficient; and determining the expected position error as the initial expected position error when the initial expected error is less than or equal to a preset error threshold.

[0077] It should be noted that the square root control coefficient refers to the control parameter of the square root controller. The specific value can be set according to the actual situation; this embodiment does not impose any restrictions. When using a square root controller, if a certain error is exceeded, its output increases according to the square root function curve, making the output more reasonable. The preset error threshold is a set error threshold; the specific value can be set according to the actual situation; this embodiment does not impose any restrictions. According to the design principle of the square root controller, if the error between the current position and the desired position (position error) is less than or equal to the preset error threshold, the following relationship holds:

[0078]

[0079] V_cmd = Kp * dist_error

[0080] In the formula, V_cmd represents the output velocity of the position loop, Kp represents the square root control coefficient, a_max represents the acceleration threshold, dist_threshold represents the preset error threshold, and dist_error represents the position error.

[0081] It is understandable that if the expected position error is greater than the preset error threshold, then the following relationship holds:

[0082]

[0083] In the formula, V_cmd represents the output velocity of the position loop, Kp represents the square root control coefficient, a_max represents the acceleration threshold, and dist_error represents the position error.

[0084] It should be understood that, in order for the aircraft to fly at a set speed, a point must be selected from a given series of trajectory points, with a distance equal to the position error from the aircraft's current position. Therefore, it is necessary to calculate the required position error (expected position error) based on different situations. In this case, V_sp = V_cmd, where V_sp represents the expected flight speed data, and V_cmd represents the position loop output speed. Based on the above calculation formula for the position loop output speed, the expected position error can be inferred. First, initialize the expected position error using the following calculation formula:

[0085]

[0086] In the formula, dist_error_sp represents the desired position error, V_sp represents the desired flight speed data, and Kp represents the square root control coefficient. The desired position error at this point is the initialized desired error, i.e., the initial desired position error. After determining the initial desired position error, the error range is judged and the value of the desired position error is updated. Judging the error range is the process of comparing the initial desired position error with a preset error threshold. If the initial desired position error is less than or equal to the preset error threshold, the value of the desired position error is updated using the following formula:

[0087]

[0088] In the formula, dist_error_sp represents the expected position error, V_sp represents the expected flight speed data, and Kp represents the square root control coefficient. The expected position error at this time is the final determined expected position error. It can be found that when the initial expected position error is less than or equal to the preset error threshold, the value of the initial expected position error is the same as the value of the final determined expected position error.

[0089] It should be noted that when the initial expected error is greater than the preset error threshold, a second correspondence is obtained between the square root control coefficient, the acceleration threshold, the expected flight speed data and the expected position error, and the expected position error is obtained based on the second correspondence.

[0090] Understandably, if the initial expected position error is greater than the preset error threshold, the value of the expected position error is updated using the following formula (second correspondence):

[0091]

[0092] At this point, dist_error_sp represents the desired position error, V_sp represents the desired flight speed data, Kp represents the square root control coefficient, and a_max represents the acceleration threshold. Substituting these values, the desired position error is obtained.

[0093] Furthermore, if a proportional controller is used, step S30 includes: determining the initial desired position error based on the desired flight speed data and the proportional control coefficient.

[0094] It should be understood that the proportional control coefficient refers to the control coefficient of the proportional controller. The specific value can be set according to the actual situation; this embodiment does not impose any restrictions on this. Since the output of the proportional controller has a linear relationship with the error, it is generally suitable for operating conditions with small errors. According to the design principle of the proportional controller, the following relationship exists:

[0095] V_cmd=K*dist_error

[0096] In the formula, V_cmd represents the output speed of the position loop, K represents the proportional control coefficient, and dist_error represents the position error. Then, by inversely calculating the desired position error, the following relationship is obtained:

[0097]

[0098] In the formula, dist_error_sp represents the desired position error, V_sp represents the desired flight speed data, and K represents the proportional control coefficient. Substituting the relevant values, the desired position error is obtained.

[0099] In practice, the required position error is calculated by back-calculating based on the characteristics of the controller generating a specific speed.

[0100] Step S40: Based on the expected position error, select points on the planned trajectory points and determine the coordinate data of the expected trajectory points.

[0101] It should be noted that the planned trajectory points are a series of location trajectory points given in the planning process. They are usually updated at a fixed frequency and interval and obtained from the planning end. The desired trajectory points are the trajectory points at the desired location, selected from this series of location trajectory points. When selecting points, the desired trajectory points corresponding to the desired location are determined based on the desired location error.

[0102] It is understandable that point selection is usually performed separately in the horizontal and vertical directions. Therefore, the desired trajectory points are also determined separately in the horizontal and vertical directions. That is to say, the desired trajectory points include horizontal desired trajectory points in the horizontal direction and vertical desired trajectory points in the vertical direction. The coordinate data of the horizontal and vertical desired trajectory points are two-dimensional. The coordinate data is the coordinate of the trajectory point. For example, in the plane direction, the coordinate data is the XY coordinate. In this embodiment, the coordinate data can be regarded as the coordinate of the control position command.

[0103] In the specific implementation, point selection operations are performed on a series of position trajectory points in the horizontal and vertical directions according to the desired position error to determine the desired trajectory points and their corresponding coordinates.

[0104] Step S50: Smooth the coordinate data of the desired trajectory point based on the third-order system smoothing strategy to obtain the target coordinate data.

[0105] Further, step S50 includes: acquiring the coordinate data of the horizontal desired trajectory point and the coordinate data of the vertical desired trajectory point; determining the fifth correspondence between the coordinate data of the desired trajectory point, the natural frequency of the position loop, the complex frequency, and the smoothed coordinate data according to the third-order system smoothing strategy; determining the smoothed coordinate data in the horizontal direction according to the coordinate data of the horizontal desired trajectory point, the natural frequency of the position loop, the complex frequency, and the fifth correspondence; determining the smoothed coordinate data in the vertical direction according to the coordinate data of the vertical desired trajectory point, the natural frequency of the position loop, the complex frequency, and the fifth correspondence; and determining the target coordinate data according to the smoothed coordinate data in the horizontal direction and the smoothed coordinate data in the vertical direction.

[0106] It should be understood that the third-order system smoothing strategy refers to the smoothing method using a third-order system. The fifth correspondence refers to the calculation formula for the smoothed coordinate data determined by the third-order system smoothing strategy, which is related to the coordinate data of the desired trajectory point and the natural frequency of the position loop. The smoothed coordinate data is the smoothed coordinate data, which can also be considered as the smoothed position command. The natural frequency of the position loop is the natural frequency of the position loop, which is related to the aircraft performance. The fifth correspondence can be expressed by the following formula:

[0107]

[0108] In the formula, target_sp represents the smoothed coordinate data, target represents the coordinate data of the desired trajectory point, w represents the natural frequency of the position loop, and s represents the complex frequency. Based on this, the calculation relationship for the smoothed coordinate data in the horizontal direction is:

[0109]

[0110] In the formula, target_xy_sp represents the smoothed coordinate data in the horizontal direction, target_xy represents the coordinate data of the desired horizontal trajectory point, w represents the natural frequency of the position loop, and s represents the complex frequency. Substituting the relevant data, the smoothed coordinate data in the horizontal direction can be obtained. Correspondingly, the smoothed coordinate data in the vertical direction can be calculated. This embodiment will not be elaborated further here.

[0111] It should be noted that the smoothed coordinate data in the horizontal direction and the smoothed coordinate data in the vertical direction are both two-dimensional coordinates in a plane. Ultimately, the planar coordinates need to be converted into three-dimensional coordinates, i.e., the target coordinate data.

[0112] In this specific implementation, the coordinate data of the obtained desired trajectory points are smoothed to achieve a smooth flight trajectory, thereby ensuring the stability of the aircraft's attitude during flight.

[0113] Step S60: Send the target coordinate data to the flight controller so that the flight controller controls the aircraft to perform trajectory tracking based on the target coordinate data.

[0114] In practice, the smoothed position command is sent to the flight controller, and the aircraft tracks and responds to the command to achieve trajectory tracking that meets the speed requirements.

[0115] like Figure 2 The overall process diagram shown illustrates the real-time acquisition of aircraft position information, real-time planned trajectory points, and user-set speeds. During acceleration and deceleration, the speeds are smoothed according to the set acceleration. Based on the smoothed speed command and the speed command generation principle of the square root controller, the desired position error is derived. Point selection is performed on a series of trajectory points in the horizontal and vertical directions according to the desired position error. The selected position command points are smoothed using a third-order system. The smoothed position command is then sent to the aircraft controller, where the aircraft tracks and responds to the command, achieving trajectory tracking that meets speed requirements.

[0116] In this embodiment, preset speed information is acquired, and initial flight speed data is determined based on this information. The initial flight speed data is then smoothed using a discrete maximum tracking function to obtain desired flight speed data. Based on this desired flight speed data, a desired position error is determined. Based on this desired position error, points are selected on the planned trajectory points to determine the coordinate data of the desired trajectory points. The coordinate data of the desired trajectory points is then smoothed using a third-order system smoothing strategy to obtain target coordinate data. This target coordinate data is then sent to the flight controller, enabling the flight controller to control the aircraft to track the trajectory based on the target coordinate data. Compared to traditional control schemes, where the aircraft cannot fly at the set speed and it is difficult to ensure stable flight attitude in scenarios without speed planning, this embodiment calculates the required error based on the characteristics of the controller generating a specific speed, and then performs point selection. Simultaneously, the output position command is smoothed to design a stable flight trajectory for the aircraft, ensuring stable attitude during flight. Even in scenarios with only position trajectory points and lacking speed planning, the aircraft can still fly at the specified speed with a relatively stable attitude.

[0117] Reference Figure 3 , Figure 3 This is a flowchart illustrating a second embodiment of a flight trajectory tracking method according to the present invention.

[0118] Based on the above embodiments, step S40 includes:

[0119] Step S401: Obtain the planned trajectory point and the current position, and determine the expected error point on the planned trajectory point based on the expected position error and the current position.

[0120] It should be noted that the current position refers to the aircraft's current position, and the expected error point is the trajectory point with a distance of expected position error from the current position. Based on the expected position error and the current position, the corresponding trajectory point can be found within the planned trajectory points. The expected error point needs to be determined separately in both the horizontal and vertical directions.

[0121] In practical implementation, taking the horizontal direction (XY) as an example, once the desired position error is determined, interpolation will be performed segmentally on a series of trajectory points (planned trajectory points) at fixed distances according to the length of the desired position error to find the XY coordinates of the corresponding control position command.

[0122] Step S402: When the expected error point is located between the current position and the first trajectory point in the planned trajectory points, obtain the third correspondence between the expected position error, the coordinate data of the first trajectory point, the coordinate data of the current position, and the coordinate data of the expected trajectory point.

[0123] It is understandable that the first trajectory point refers to the first planned trajectory point sent, that is, the first trajectory point in the planned trajectory points. The third correspondence refers to the calculation formula for the coordinate data of the expected trajectory point when the expected error point is located between the current position and the first trajectory point. It is related to the expected position error, the coordinate data of the first trajectory point, and the coordinate data of the current position. The calculation formula for the coordinate data of the expected trajectory point (the third correspondence) when the expected error point is located between the current position and the first trajectory point is as follows:

[0124]

[0125] In the formula, target represents the coordinate data of the desired trajectory point, dist_error_sp represents the desired position error, p0 represents the coordinate data of the current position, and p1 represents the coordinate data of the first trajectory point.

[0126] Step S403: Obtain the coordinate data of the desired trajectory point based on the expected position error, the coordinate data of the first trajectory point, the coordinate data of the current position, and the third correspondence.

[0127] It should be understood that, such as Figure 4As shown, taking the horizontal direction as an example, P0 is the current position, P1 to Pn represent a series of planned trajectory points, and Ptarget is the expected error point. If the expected error point in the horizontal direction is located between the current position and the first trajectory point, the calculation formula for the coordinate data of the expected horizontal trajectory point is:

[0128]

[0129] In the formula, target_xy represents the coordinate data of the horizontal expected trajectory point, dist_error_sp represents the expected position error, p0_xy represents the coordinates of the current position in the horizontal direction, and p1_xy represents the coordinates of the first trajectory point in the horizontal direction. Substituting the relevant data, the coordinate data of the horizontal expected trajectory point can be obtained. Correspondingly, when the expected error point in the vertical direction is located between the current position and the first trajectory point, the coordinate data of the vertical expected trajectory point can be calculated in the same way. This embodiment will not be elaborated here.

[0130] In the specific implementation, when the expected error point is located between the current position and the first trajectory point, the coordinate data of the horizontal expected trajectory point and the coordinate data of the vertical expected trajectory point are determined according to the calculation formula of the corresponding expected trajectory point coordinate data.

[0131] It should be understood that when the first trajectory point coincides with the current position or the current position and the first trajectory point meet a preset numerical condition, the coordinate data of the desired trajectory point is determined to be the coordinate data of the first trajectory point.

[0132] It should be noted that the first trajectory point coinciding with the current position means that the coordinates of the first trajectory point coincide with the coordinates of the current position. The preset numerical condition means that the product is zero. The current position and the first trajectory point satisfy the preset numerical condition when the product of the coordinates of the first trajectory point and the coordinates of the current position is zero, that is, |p0*p1|=0, where p0 represents the coordinates of the first trajectory point and p1 represents the coordinates of the current position.

[0133] In the specific implementation, if the coordinates of the first trajectory point in the horizontal direction coincide with the coordinates of the current position, or the product of the coordinates of the first trajectory point and the coordinates of the current position is zero, then the coordinates of the first trajectory point in the horizontal direction are used as the coordinates of the desired horizontal trajectory point; if the coordinates of the first trajectory point in the vertical direction coincide with the coordinates of the current position, or the product of the coordinates of the first trajectory point and the coordinates of the current position is zero, then the coordinates of the first trajectory point in the vertical direction are used as the coordinates of the desired vertical trajectory point.

[0134] In this embodiment, by acquiring the planned trajectory points and the current position, and based on the expected position error and the current position, a desired error point is determined on the planned trajectory points. When the desired error point is located between the current position and a first trajectory point in the planned trajectory points, a third correspondence is obtained between the expected position error, the coordinate data of the first trajectory point, the coordinate data of the current position, and the coordinate data of the desired trajectory point. Based on the expected position error, the coordinate data of the first trajectory point, the coordinate data of the current position, and the third correspondence, the coordinate data of the desired trajectory point is obtained. Compared to using traditional control schemes, in scenarios without speed planning, the aircraft cannot fly at the set speed, and it is difficult to ensure stable flight attitude. This embodiment, based on the characteristics of the controller generating a specific speed, back-calculates the required error and then performs point selection operations according to different situations to determine the desired position coordinates and performs smoothing processing to ensure stable attitude during flight. Even in scenarios with only position trajectory points and lacking speed planning, the aircraft can fly at the specified speed with a relatively stable attitude.

[0135] Reference Figure 5 , Figure 5 This is a flowchart illustrating a third embodiment of a flight trajectory tracking method according to the present invention.

[0136] Based on the above embodiments, after step S401, the method further includes:

[0137] Step S402': When the expected error point is located between the second trajectory point and the last trajectory point in the planned trajectory points, determine the initial interval distance based on the interval distance between the current position and the first trajectory point.

[0138] It should be noted that the second trajectory point refers to the second planned trajectory point sent, that is, the second trajectory point in the planned trajectory points. The last trajectory point refers to the last planned trajectory point sent, that is, the second trajectory point in the planned trajectory points. The interval distance is the difference between the current position and the first trajectory point. The initial interval distance refers to the variable used to calculate the coordinate data. The initial value of the initial interval distance is the interval distance between the current position and the first trajectory point.

[0139] Step S403': Obtain the trajectory point interval distance between the planned trajectory points, initialize the loop variable, and determine the distance threshold based on the initial interval distance and the trajectory point interval distance.

[0140] It is understandable that the trajectory point interval distance refers to the distance between any two planned trajectory points. Since the planned points are updated at a fixed frequency and interval, the distance between any two planned trajectory points is usually the same. The loop variable is the variable i used to calculate the coordinate data, 2≤i≤n, where n is the number of planned trajectory points. The distance threshold is also a variable used to calculate the coordinate data. In this embodiment, the distance threshold = initial interval distance + trajectory point interval distance.

[0141] In the specific implementation, the distance between the current position and the first trajectory point is first recorded as the initial distance. A loop variable i is defined, and the loop starts from the second planned trajectory point. It is then determined whether the expected position error is greater than the distance threshold.

[0142] Step S404': When the expected position error is less than or equal to the distance threshold, obtain the fourth correspondence between the expected position error, the initial interval distance, the coordinate data of the planned trajectory point and the coordinate data of the expected trajectory point, and obtain the coordinate data of the expected trajectory point based on the expected position error, the initial interval distance, the coordinate data of the planned trajectory point and the fourth correspondence.

[0143] It should be understood that the fourth correspondence refers to the formula for calculating the coordinate data of the expected trajectory points when the expected position error is less than or equal to the distance threshold. It is related to the expected position error, the initial interval distance, and the coordinate data of the planned trajectory points, and can be expressed by the following expression:

[0144]

[0145] In the formula, i represents the loop variable, dis_error_sp represents the expected position error, dist represents the initial interval distance, p(i-1) represents the coordinate data of the (i-1)th planned trajectory point, p(i) represents the coordinate data of the ith planned trajectory point, and target represents the coordinate data of the expected trajectory point. Substituting the relevant data, the coordinate data of the expected trajectory point is obtained.

[0146] Further, when the expected position error is greater than the distance threshold, the loop variable is updated; when the loop variable is less than or equal to the number of trajectory points, the initial interval distance is updated to the distance threshold, and the process returns to the step of determining the distance threshold based on the initial interval distance and the trajectory point interval distance.

[0147] It is understandable that the number of trajectory points refers to the number of planned trajectories. When the loop variable is less than or equal to the number of trajectory points, the loop continues. If the expected position error is greater than the distance threshold, the initial interval distance is updated. At this time, the updated initial interval distance equals the distance threshold. The updated initial interval distance is used to determine the new distance threshold, and the loop continues until the last planned trajectory point.

[0148] It should be understood that when the loop variable is greater than the number of trajectory points, the coordinate data of the desired trajectory point is determined as the coordinate data of the last trajectory point. The loop ending occurs when the loop variable is greater than the number of trajectory points. If the loop ends and the desired position error is still greater than the distance threshold, the coordinates of the last planned trajectory point are used as the coordinates of the desired trajectory point. At this point, the initial interval distance is greater than the total distance of all trajectory points.

[0149] It should be noted that, as Figure 6 As shown, taking the horizontal direction as an example, P0 is the current position, P1 to Pn represent a series of planned trajectory points, and Ptarget is the expected error point. If the expected error point in the horizontal direction is located between the second trajectory point and the last trajectory point, first determine whether the expected position error is greater than the distance threshold. If the expected position error is less than or equal to the distance threshold, then the calculation formula for the coordinate data of the horizontal expected trajectory point is:

[0150]

[0151] In the formula, i represents the loop variable, dis_error_sp represents the expected position error, dist represents the initial interval distance, p(i-1)_xy represents the horizontal coordinates of the (i-1)th planned trajectory point, p(i)_xy represents the horizontal coordinates of the ith planned trajectory point, and target_xy represents the coordinate data of the horizontal expected trajectory point. Substituting the relevant data, the coordinate data of the horizontal expected trajectory point can be obtained. If the expected position error is greater than the distance threshold, the distance threshold is updated and the next round of judgment is entered. If the loop ends and the expected position error is still greater than the distance threshold, the coordinates of the last planned trajectory point in the horizontal direction are used as the coordinates of the horizontal expected trajectory point. Correspondingly, the coordinate data of the vertical expected trajectory point can be calculated in the same way in the vertical direction, which will not be described in detail in this embodiment.

[0152] In this embodiment, when the expected error point is located between the second trajectory point and the last trajectory point in the planned trajectory points, an initial interval distance is determined based on the distance between the current position and the first trajectory point. The trajectory point interval distance between the planned trajectory points is obtained, and a loop variable is initialized. Based on the loop variable, the current interval distance is determined in the trajectory point interval distance. A distance threshold is determined based on the current interval distance and the initial interval distance. When the expected position error is less than or equal to the distance threshold, a fourth correspondence between the coordinate data of the planned trajectory points and the coordinate data of the expected trajectory points is obtained. Based on the expected position error, the initial interval distance, the coordinate data of the planned trajectory points, and the fourth correspondence, the coordinate data of the expected trajectory point is obtained. Compared to using traditional control schemes, in scenarios without speed planning, the aircraft cannot fly at the set speed, and it is difficult to ensure stable flight attitude. This embodiment calculates the required error based on the characteristics of the controller generating a specific speed, and then performs point selection operations according to different situations to determine the expected position coordinates and performs smoothing processing to ensure stable attitude during flight. Even in scenarios with only position trajectory points and lacking speed planning, the aircraft can fly at the specified flight speed with a relatively stable attitude.

[0153] Reference Figure 7 , Figure 7 This is a structural block diagram of the first embodiment of the flight trajectory tracking device of the present invention.

[0154] like Figure 7 As shown, the flight trajectory tracking device proposed in this embodiment of the invention includes:

[0155] The speed smoothing module 10 is used to acquire preset speed information and determine initial flight speed data based on the preset speed information.

[0156] The speed smoothing module 10 is also used to smooth the initial flight speed data based on the discrete fastest tracking function to obtain the desired flight speed data.

[0157] The position determination module 20 is used to determine the desired position error based on the desired flight speed data.

[0158] The position determination module 20 is further configured to select points on the planned trajectory points based on the expected position error, and determine the coordinate data of the expected trajectory points.

[0159] The position determination module 20 is also used to smooth the coordinate data of the desired trajectory point based on a third-order system smoothing strategy to obtain the target coordinate data.

[0160] The trajectory tracking module 30 is used to send the target coordinate data to the flight controller so that the flight controller can control the aircraft to perform trajectory tracking based on the target coordinate data.

[0161] In this embodiment, preset speed information is acquired, and initial flight speed data is determined based on this information. The initial flight speed data is then smoothed using a discrete maximum tracking function to obtain desired flight speed data. Based on this desired flight speed data, a desired position error is determined. Based on this desired position error, points are selected on the planned trajectory points to determine the coordinate data of the desired trajectory points. The coordinate data of the desired trajectory points is then smoothed using a third-order system smoothing strategy to obtain target coordinate data. This target coordinate data is then sent to the flight controller, enabling the flight controller to control the aircraft to track the trajectory based on the target coordinate data. Compared to traditional control schemes, where the aircraft cannot fly at the set speed and it is difficult to ensure stable flight attitude in scenarios without speed planning, this embodiment calculates the required error based on the characteristics of the controller generating a specific speed, and then performs point selection. Simultaneously, the output position command is smoothed to design a stable flight trajectory for the aircraft, ensuring stable attitude during flight. Even in scenarios with only position trajectory points and lacking speed planning, the aircraft can still fly at the specified speed with a relatively stable attitude.

[0162] In one embodiment, the speed smoothing module 10 is further configured to determine a first correspondence between the initial flight speed data, the preset filter factor, the acceleration threshold, and the desired flight speed data based on the discrete fastest tracking function;

[0163] The desired flight speed data is determined based on the initial flight speed data, the preset filter factor, the acceleration threshold, and the first correspondence.

[0164] In one embodiment, the position determination module 20 is further configured to determine the initial expected position error based on the expected flight speed data and the square root control coefficient;

[0165] When the initial expected error is less than or equal to a preset error threshold, the expected position error is determined to be the initial expected position error.

[0166] In one embodiment, the position determination module 20 is further configured to, when the initial expected error is greater than the preset error threshold, obtain a second correspondence between the square root control coefficient, the acceleration threshold, the expected flight speed data and the expected position error;

[0167] The desired position error is obtained based on the square root control coefficient, the acceleration threshold, the desired flight speed data, and the second correspondence.

[0168] In one embodiment, the position determination module 20 is further configured to obtain the planned trajectory point and the current position, and determine the expected error point on the planned trajectory point based on the expected position error and the current position;

[0169] When the expected error point is located between the current position and the first trajectory point in the planned trajectory points, a third correspondence is obtained between the expected position error, the coordinate data of the first trajectory point, the coordinate data of the current position, and the coordinate data of the expected trajectory point.

[0170] The coordinate data of the desired trajectory point are obtained based on the expected position error, the coordinate data of the first trajectory point, the coordinate data of the current position, and the third correspondence.

[0171] In one embodiment, the position determination module 20 is further configured to determine the coordinate data of the desired trajectory point as the coordinate data of the first trajectory point when the first trajectory point coincides with the current position or the current position and the first trajectory point satisfy a preset numerical condition.

[0172] In one embodiment, the position determination module 20 is further configured to determine an initial interval distance based on the interval distance between the current position and the first trajectory point when the expected error point is located between the second trajectory point and the last trajectory point in the planned trajectory points;

[0173] Obtain the distance between the planned trajectory points and initialize the loop variable;

[0174] A distance threshold is determined based on the initial interval distance and the trajectory point interval distance;

[0175] When the expected position error is less than or equal to the distance threshold, a fourth correspondence is obtained between the expected position error, the initial interval distance, the coordinate data of the planned trajectory point, and the coordinate data of the expected trajectory point.

[0176] The coordinate data of the desired trajectory point are obtained based on the expected position error, the initial interval distance, the coordinate data of the planned trajectory point, and the fourth correspondence.

[0177] In one embodiment, the expected trajectory points include horizontal expected trajectory points in the horizontal direction and vertical expected trajectory points in the vertical direction. The position determination module 20 is further configured to update the loop variable when the expected position error is greater than the distance threshold.

[0178] When the loop variable is greater than the number of trajectory points, the coordinate data of the desired trajectory point is determined to be the coordinate data of the last trajectory point;

[0179] When the loop variable is less than or equal to the number of trajectory points, the initial interval distance is updated to the distance threshold, and the process returns to the step of determining the distance threshold based on the initial interval distance and the trajectory point interval distance.

[0180] In one embodiment, the position determination module 20 is further configured to acquire the coordinate data of the horizontal desired trajectory point and the coordinate data of the vertical desired trajectory point;

[0181] Based on the third-order system smoothing strategy, a fifth correspondence is determined between the coordinate data of the desired trajectory point, the natural frequency of the position loop, the complex frequency, and the smoothed coordinate data.

[0182] Based on the coordinate data of the desired horizontal trajectory point, the natural frequency of the position loop, the complex frequency, and the fifth correspondence, the smooth coordinate data in the horizontal direction are determined.

[0183] Based on the coordinate data of the vertical desired trajectory point, the natural frequency of the position loop, the complex frequency, and the fifth correspondence, the smooth coordinate data in the vertical direction are determined;

[0184] The target coordinate data is determined based on the smoothed coordinate data in the horizontal direction and the smoothed coordinate data in the vertical direction.

[0185] It should be understood that although the steps in the flowcharts of this application's embodiments are shown sequentially according to the arrows, these steps are not necessarily executed in the order indicated by the arrows. Unless explicitly stated herein, there is no strict order restriction on the execution of these steps, and they can be executed in other orders. Moreover, at least some of the steps in the figures may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily completed at the same time, but can be executed at different times, and their execution order is not necessarily sequential, but can be performed alternately or in turn with other steps or at least some of the sub-steps or stages of other steps.

[0186] It should be noted that the above are merely illustrative examples and do not constitute any limitation on the technical solution of the present invention. In specific applications, those skilled in the art can make settings as needed, and the present invention does not impose any restrictions on this.

[0187] It is understood that the workflow described above is merely illustrative and does not limit the scope of protection of this invention. In practical applications, those skilled in the art can select some or all of the above to achieve the purpose of this embodiment according to actual needs, and no restrictions are imposed here.

[0188] In addition, for technical details not described in detail in this embodiment, please refer to the detour planning method in lane keeping state provided in any embodiment of the present invention, which will not be repeated here.

[0189] Furthermore, it should be noted that, in this document, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or system that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or system. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or system that includes that element.

[0190] The sequence numbers of the above embodiments of the present invention are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments.

[0191] Through the above description of the embodiments, those skilled in the art can clearly understand that the methods of the above embodiments can be implemented by means of software plus necessary general-purpose hardware platforms. Of course, they can also be implemented by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product is stored in a storage medium (such as read-only memory (ROM) / RAM, magnetic disk, optical disk) and includes several instructions to cause a terminal device (which may be a mobile phone, computer, server, or network device, etc.) to execute the methods described in the various embodiments of the present invention.

[0192] The above are merely preferred embodiments of the present invention and do not limit the scope of the patent. Any equivalent structural or procedural transformations made based on the description and drawings of the present invention, or direct or indirect applications in other related technical fields, are similarly included within the scope of patent protection of the present invention.

Claims

1. A flight trajectory tracking method, characterized in that, The flight trajectory tracking method includes: Obtain preset speed information, and determine initial flight speed data based on the preset speed information; Based on the discrete fastest tracking function, the initial flight speed data is smoothed to obtain the desired flight speed data; Based on the expected flight speed data, the expected position error is determined, specifically including: based on the expected flight speed data and the square root control coefficient, the initial expected position error is determined; when the initial expected position error is less than or equal to a preset error threshold, the expected position error is determined as the initial expected position error. Based on the expected position error, a point is selected on the planned trajectory point to determine the coordinate data of the expected trajectory point. Specifically, this includes: obtaining the planned trajectory point and the current position; determining the expected error point on the planned trajectory point based on the expected position error and the current position; when the expected error point is located between the current position and a first trajectory point among the planned trajectory points, obtaining a third correspondence between the expected position error, the coordinate data of the first trajectory point, the coordinate data of the current position, and the coordinate data of the expected trajectory point; and obtaining the coordinate data of the expected trajectory point based on the expected position error, the coordinate data of the first trajectory point, the coordinate data of the current position, and the third correspondence. The coordinate data of the desired trajectory points are smoothed using a third-order system smoothing strategy to obtain the target coordinate data. The target coordinate data is sent to the flight controller so that the flight controller can control the aircraft to perform trajectory tracking based on the target coordinate data.

2. The method as described in claim 1, characterized in that, The process of smoothing the initial flight speed data based on the discrete fastest tracking function to obtain the desired flight speed data includes: Based on the discrete fastest tracking function, a first correspondence is determined between the initial flight speed data, the preset filter factor, the acceleration threshold, and the desired flight speed data; The desired flight speed data is determined based on the initial flight speed data, the preset filter factor, the acceleration threshold, and the first correspondence.

3. The method as described in claim 1, characterized in that, After determining the initial expected position error based on the expected flight speed data and the square root control coefficient, the method further includes: When the initial expected position error is greater than the preset error threshold, a second correspondence is obtained between the square root control coefficient, the acceleration threshold, the expected flight speed data, and the expected position error. The desired position error is obtained based on the square root control coefficient, the acceleration threshold, the desired flight speed data, and the second correspondence.

4. The method as described in claim 1, characterized in that, After obtaining the planned trajectory point and the current position, and determining the expected error point on the planned trajectory point based on the expected position error and the current position, the method further includes: When the first trajectory point coincides with the current position or the current position and the first trajectory point meet a preset numerical condition, the coordinate data of the desired trajectory point is determined to be the coordinate data of the first trajectory point.

5. The method as described in claim 1, characterized in that, After obtaining the planned trajectory point and the current position, and determining the expected error point on the planned trajectory point based on the expected position error and the current position, the method further includes: When the expected error point is located between the second trajectory point and the last trajectory point in the planned trajectory points, the initial interval distance is determined based on the interval distance between the current position and the first trajectory point; Obtain the distance between the planned trajectory points and initialize the loop variable; A distance threshold is determined based on the initial interval distance and the trajectory point interval distance; When the expected position error is less than or equal to the distance threshold, a fourth correspondence is obtained between the expected position error, the initial interval distance, the coordinate data of the planned trajectory point, and the coordinate data of the expected trajectory point. The coordinate data of the desired trajectory point are obtained based on the expected position error, the initial interval distance, the coordinate data of the planned trajectory point, and the fourth correspondence.

6. The method as described in claim 5, characterized in that, After determining the distance threshold based on the initial interval distance and the trajectory point interval distance, the method further includes: When the expected position error is greater than the distance threshold, the loop variable is updated; When the loop variable is greater than the number of trajectory points, the coordinate data of the desired trajectory point is determined to be the coordinate data of the last trajectory point; When the loop variable is less than or equal to the number of trajectory points, the initial interval distance is updated to the distance threshold, and the process returns to the step of determining the distance threshold based on the initial interval distance and the trajectory point interval distance.

7. The method according to any one of claims 1 to 6, characterized in that, The desired trajectory points include horizontal desired trajectory points in the horizontal direction and vertical desired trajectory points in the vertical direction. The coordinate data of the desired trajectory points are smoothed using a third-order system smoothing strategy to obtain target coordinate data, including: Obtain the coordinate data of the horizontal desired trajectory point and the coordinate data of the vertical desired trajectory point; Based on the third-order system smoothing strategy, a fifth correspondence is determined between the coordinate data of the desired trajectory point, the natural frequency of the position loop, the complex frequency, and the smoothed coordinate data. Based on the coordinate data of the desired horizontal trajectory point, the natural frequency of the position loop, the complex frequency, and the fifth correspondence, the smooth coordinate data in the horizontal direction are determined. Based on the coordinate data of the vertical desired trajectory point, the natural frequency of the position loop, the complex frequency, and the fifth correspondence, the smooth coordinate data in the vertical direction are determined; The target coordinate data is determined based on the smoothed coordinate data in the horizontal direction and the smoothed coordinate data in the vertical direction.

8. A flight trajectory tracking device, characterized in that, The flight trajectory tracking device includes: A speed smoothing module is used to acquire preset speed information and determine initial flight speed data based on the preset speed information; The speed smoothing module is also used to smooth the initial flight speed data based on the discrete fastest tracking function to obtain the desired flight speed data; The position determination module is used to determine the desired position error based on the desired flight speed data; The position determination module is also used to select points on the planned trajectory points based on the expected position error, and determine the coordinate data of the expected trajectory points; The position determination module is also used to smooth the coordinate data of the desired trajectory point based on a third-order system smoothing strategy to obtain target coordinate data; The trajectory tracking module is used to send the target coordinate data to the flight controller, so that the flight controller can control the aircraft to perform trajectory tracking based on the target coordinate data; The position determination module is also used to determine the initial expected position error based on the expected flight speed data and the square root control coefficient; When the initial expected position error is less than or equal to a preset error threshold, the expected position error is determined to be the initial expected position error; The location determination module is further configured to obtain the planned trajectory point and the current position, and determine the expected error point on the planned trajectory point based on the expected position error and the current position; When the expected error point is located between the current position and the first trajectory point in the planned trajectory points, a third correspondence is obtained between the expected position error, the coordinate data of the first trajectory point, the coordinate data of the current position, and the coordinate data of the expected trajectory point. The coordinate data of the desired trajectory point are obtained based on the expected position error, the coordinate data of the first trajectory point, the coordinate data of the current position, and the third correspondence.

Images