Trajectory Planning Method for Unmanned Helicopter

Abstract

The present invention provides a trajectory planning method for an unmanned helicopter, including global planning and local planning; in global planning, the sampling-based path planning algorithm, i.e., the RRT* algorithm, is used to obtain a smooth and collision-free initial minimized Snap trajectory, and this trajectory is used as the default path for subsequent local planning; in local planning, the result of global planning is used as the default path for actual flight, a section is taken from the result of global planning as the initial path for flight and a polynomial curve is used to fit the initial path, converting the trajectory planning problem into a quadratic programming problem, and finally decoupling the polynomial coefficients and the segment times, and by solving the segment times, a real-time trajectory is obtained. The present invention can achieve real-time trajectory planning for an unknown environment, ensure autonomous avoidance of obstacles during flight, and generate a dynamically feasible real-time trajectory.

Description

Technical Field

[0001] The present invention relates to the field of real-time trajectory planning and control law design for unmanned helicopters, and specifically to a trajectory planning method for unmanned helicopters. Background Art

[0002] Trajectory planning is of great significance for the application of unmanned helicopters, such as in scenarios like fruit condition monitoring, pesticide spraying, post-disaster search and rescue, and logistics transportation. In the current field of unmanned helicopter motion planning, two common planning algorithms are generally adopted:

[0003] One is to combine path planning and trajectory control. A collision-free path from the starting point to the ending point is planned using a path planning algorithm, and then the on-board flight control system is used to track the generated path in real time to ensure that the unmanned helicopter flies within the flight envelope. Currently, common path planning methods include: A* algorithm, RRT* algorithm, heuristic search algorithm, etc. However, this planning method inadequately considers the flight dynamics constraints of the unmanned helicopter during the trajectory planning stage. In order to ensure that the unmanned helicopter always remains within the flight envelope, only a relatively conservative path can be generated.

[0004] The other is to transform the trajectory planning problem into an optimal control problem, discretize the flight process, directly optimize the control quantities at each discrete point, and generate the final flight trajectory by solving the optimal control problem. Common numerical solution methods include: direct transformation method, direct shooting method, etc., which are often applied to the trajectory optimization of helicopter landing and the trajectory planning after a single engine failure of a helicopter. The problem with this method is that it needs to repeatedly call the flight dynamics model, which takes a long time to calculate, and a trade-off needs to be made between the calculation time and the calculation accuracy. Therefore, it is not applicable to the real-time trajectory planning of unmanned helicopters and cannot achieve the obstacle avoidance function.

[0005] Currently, there is no reported research on the real-time trajectory planning of unmanned helicopters with obstacle avoidance. Summary of the Invention

[0006] In order to solve the problems of the prior art, the present invention provides a trajectory planning method for unmanned helicopters, which realizes the real-time trajectory planning of an unknown environment, ensures autonomous obstacle avoidance during the flight process, and generates a dynamically feasible real-time trajectory.

[0007] The present invention provides a trajectory planning method for an unmanned helicopter, including global planning and local planning; in global planning, the sampling-based path planning algorithm RRT* algorithm is adopted. According to the prior environment information and the task points issued by the user, intermediate waypoints are supplemented to form a path without obstacles. Then, the differential flatness theory is used to smooth this path to obtain a smooth and collision-free initial minimized Snap trajectory, which is used as the default path for subsequent local planning; in local planning, the result of global planning is used as the default path for actual flight. A section is taken from the result of global planning as the initial path for flight and a polynomial curve is used to fit the initial path; the obstacle avoidance function for obstacles is realized by calculating the obstacle penalty function, the curvature penalty function, and the deviation penalty function to obtain a feasible path. Several waypoints are selected from it, and the waypoints are used as the constraints of trajectory planning. The trajectory is represented by a piecewise polynomial. Using the differential flatness theory, the trajectory planning problem is converted into a quadratic programming problem. Finally, the polynomial coefficients and the piecewise time are decoupled, and by solving the piecewise time, the real-time trajectory is obtained.

[0008] The specific steps of the global planning are as follows:

[0009] First, use the RRT* algorithm to search for a collision-free feasible path;

[0010] Then, based on three assumptions:

[0011] 1) During the flight process, the acceleration of the aircraft body in the horizontal plane is relatively small.

[0012] 2) The difference between the current attitude angles θ and φ of the helicopter and the attitude angles during trimming is very small.

[0013] 3) During the flight process, the vertical acceleration in the earth axis system is much smaller than the gravitational acceleration, that is

[0014] Establish a simplified helicopter flight dynamics equation and prove the differential flatness of this flight dynamics equation, that is:

[0015]

[0016]

[0017]

[0018]

[0019]

[0020] In the formula, θ represents the pitch angle of the aircraft body, φ represents the roll angle of the aircraft body, and Indicates the pitch angle and attitude angle during trim. and Indicates the pitch angle rate and angular acceleration rate, and Indicates the roll angular acceleration rate, and Indicates the yaw angle rate and angular acceleration rate. Is an intermediate calculation parameter, and its definition is as follows

[0021]

[0022] In the formula: X, Y, and Z respectively represent the body coordinates in the earth axis system; the superscripts (3) and (4) represent the 3rd and 4th derivatives of the coordinates with respect to time.

[0023] Let the trajectory be p i , and use the function of the square of the fourth derivative of the trajectory with respect to time as the objective function for global planning:

[0024]

[0025] Use a non - linear programming solver to obtain the trajectory p i , and finally extract the path information from the planned trajectory as the result of global planning.

[0026] The specific process of the local planning is as follows:

[0027] First, take the result of global planning as the default path for actual flight, extract a section from the result of global planning as the initial path for flight, and fit the initial path with a polynomial curve;

[0028] Secondly, realize the obstacle avoidance function for obstacles by calculating the obstacle penalty function, curvature penalty function, and deviation penalty function, obtain a feasible path, and select several waypoints from it;

[0029] Use the following formula as the objective function for optimization:

[0030] f total =λ 1 f c +λ 2 f o +λ 3 f s (5)

[0031] In the formula, f c Represents the curvature penalty function term, hoping that there will be no too small turning radius during path planning, and this term is represented by the integral of curvature with respect to time; f o Represents the distance potential function of the trajectory to the obstacle, used to avoid obstacles; f sUsed to limit the distance between the generated local path and the global path; λ 1 , λ 2 , λ 3 represent their weights;

[0032] The integral of curvature over time is used to represent the curvature penalty function of the objective function, and the exponential function is used as the basis function of the objective function, that is:

[0033] f c = ∫c(k)dt

[0034]

[0035] k represents the curvature of a certain point on the path. When k > k 0 , c(k) will rise rapidly. The value of k 0 is determined by the current speed; α and r affect the size and slope of the function;

[0036] The exponential function is selected as the basis function of the obstacle potential function, and at the same time, the exponential function is used to describe the distance between the planned path and the global path:

[0037]

[0038]

[0039] In the formula, d represents the distance from a certain point on the path to a certain obstacle, and d 0 represents the warning range. If d < d 0 , the obstacle potential function will increase rapidly. α and r are parameters for adjusting the size and slope of the function.

[0040] After that, the above waypoints are used as the constraints of the trajectory planning. The trajectory is represented by piecewise polynomials, and the differential flatness theory is used to convert the trajectory planning problem into a quadratic programming problem:

[0041]

[0042] Finally, the polynomial coefficients and the piecewise time are decoupled, and by solving the piecewise time, the real-time trajectory is obtained.

[0043] The beneficial effects of the present invention are as follows:

[0044] 1. The real-time trajectory planning of the unmanned helicopter is realized for the first time.

[0045] 2. Compared with the waypoint planning, the present invention considers the flight dynamics model of the unmanned helicopter in the trajectory planning stage, making the trajectory planning result more reliable.

[0046] 3. Compared with the optimal control method, the present invention improves the solution speed and enables the unmanned helicopter to autonomously avoid obstacles during flight, realizing the real-time obstacle avoidance function during flight. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following will briefly introduce the drawings required in the embodiments. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0048] Figure 1 It is the overall flowchart of a trajectory planning method for an unmanned helicopter provided by the present invention.

[0049] Figure 2 It is a simplified helicopter flight dynamics model used for trajectory planning.

[0050] Figure 3 It represents the set of feasible paths obtained by the RRT* algorithm.

[0051] Figure 4 It represents the shortest path after pruning.

[0052] Figure 5 It represents the global trajectory after smoothing.

[0053] Figure 6 It is a schematic diagram of local environment information processing, indicating the range of local environment information input for each local planning.

[0054] Figure 7 It is the flowchart of local trajectory real-time planning.

[0055] Figure 8 It is the prior map used for global planning before starting flight.

[0056] Figure 9 It is the real environment during actual flight.

[0057] Figure 10 It is a comparison diagram of the local planning trajectory and the global planning trajectory.

[0058] Figure 11 It is the image of speed versus time obtained by splicing.

[0059] Figure 12 It is the image of attitude angle versus time obtained by splicing.

[0060] Figure 13 It is the image of angular rate versus time obtained by splicing.

[0061] Figure 14 It is the angular acceleration vs. time image obtained by splicing. Specific embodiments

[0062] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0063] As Figure 1 shown, the overall process of a trajectory planning method for an unmanned helicopter provided by the present invention is as follows:

[0064] Before the start of the flight mission, first, the sampling-based path planning algorithm, the RRT* algorithm, is used to generate a smooth and collision-free initial minimized Snap trajectory for the given mission objective and prior environment information, and this trajectory is used as the default path for subsequent local planning. During the actual flight of the unmanned helicopter, there may be errors between the prior environment information and the actual situation. At this time, based on the environment information updated at a certain frequency, a local planning algorithm is used to generate a real-time feasible trajectory that satisfies the dynamic constraints and is collision-free.

[0065] Global planning

[0066] First, global planning is carried out. The first step of global planning is to use the RRT* algorithm to supplement intermediate waypoints according to the prior environment information and the mission points issued by the user, so as to form a path without obstacles. The RRT algorithm is an incremental construction method. During the construction process, the algorithm continuously randomly generates state points in the search space. If the point is in a collision-free position, the nearest node in the search tree is found as the reference node, and the reference node is used as the starting point to extend towards the random node with a certain step length. The position where the end point of the extension line is located is regarded as a valid node and added to the search tree. This growth process of the search tree continues until the distance between the target node and the search tree is within a certain range and then terminates. Subsequently, the search algorithm finds the shortest path connecting the starting point to the end point in the search tree. The RRT* algorithm adds two steps of reselecting the parent node and rewire on the basis of the RRT algorithm, making the RRT* algorithm asymptotically optimal.

[0067] After supplementing the intermediate waypoints through the RRT* algorithm, the second step of the global planning begins, namely trajectory smoothing. Here, the theory of differential flatness needs to be introduced. The theory of differential flatness is a concept proposed by Filess et al. for nonlinear systems. D. Mellinger et al. proved that the dynamic model of a quadrotor is differentially flat under the assumption of neglecting aerodynamic drag, that is: there exists a set of input variables that can represent the state variables and control variables of the quadrotor system by this set of input variables and their finite-order derivatives. We call this set of input variables the flat output. In this way, we transform the original optimal control problem in the state space into a nonlinear programming problem in the flat output space, reducing the dimension of the planning space and thus reducing the difficulty of the optimal control problem. ZHAO DI et al. deduced the differential flatness property of a helicopter on this basis and applied it to the trajectory optimization during helicopter landing.

[0068] Assume that during trajectory planning, the forces acting on the helicopter at a certain moment are as Figure 2 shown. The intermediate coordinate system X V Y V Z V is obtained by rotating the earth axis system around the Z axis by an angle ψ, and then rotating around the Y and Z axes by angles θ and φ to obtain the body axis system X B Y B Z B . In the trimmed state, the resultant force acting on the airframe except for gravity is denoted as (hereinafter referred to as "resultant force"), and the resultant force in any motion state is denoted as F. The angular differences between it and are represented by σ and α.

[0069] We introduce three assumptions:

[0070] 1) During flight, the acceleration in the horizontal plane is small.

[0071] 2) The differences between the current attitude angles θ and φ of the helicopter and the attitude angles during trimming are very small.

[0072] 3) During flight, the vertical acceleration in the earth axis system is much smaller than the gravitational acceleration, that is

[0073] Based on assumption 1), we can obtain:

[0074]

[0075] Expanding the third column of the above formula, we can obtain:

[0076]

[0077] Considering hypothesis 2), it can be approximately considered that:

[0078]

[0079]

[0080] That is to say, the direction of the resultant force F remains unchanged in the body axis system. This is because during flight, the rotor thrust T is always the main component of the resultant force F (neglecting rotor flapping dynamics). Therefore:

[0081]

[0082] Considering hypothesis 3), it can be obtained that:

[0083]

[0084] Select X, Y, and Z as the flat outputs, and use the velocity direction to represent the heading of the unmanned helicopter. The other two attitude angles, angular rates, and angular accelerations of the unmanned helicopter can be expressed as:

[0085]

[0086]

[0087]

[0088]

[0089]

[0090] In the formula:

[0091]

[0092] This proves that the helicopter flight dynamics model satisfies differential flatness under appropriate assumptions. Therefore, we can perform nonlinear programming on X, Y, and Z and then map it back to the state space.

[0093] And we note that the fourth derivative (Snap) of the trajectory p i has a positive correlation with the body attitude angular acceleration. Therefore, a smooth trajectory can be obtained by minimizing the fourth derivative of the trajectory.

[0094]

[0095] Take this trajectory as the result of the global planning. However, in the subsequent real-time planning, we do not consider the time term of the global trajectory and only store it as a path in the on-board computer.

[0096] We assume that there is a four-rotor unmanned helicopter starting from point A, passing through points B, C, and D, and finally reaching point E. Figure 3 Denotes the set of feasible paths obtained by the RRT* algorithm, Figure 4 Denotes the shortest path after pruning, P 1 、P 2 、P 3 Denotes the intermediate waypoints supplemented by RRT*. Figure 5 Denotes the global trajectory after smoothing.

[0097] Local planning

[0098] In the previous section, we obtained the global path based on the mission requirements and prior environmental information. However, it cannot guarantee safety during autonomous flight. Because prior environmental information is often inaccurate, unknown obstacles may appear at any time during flight. Therefore, it is necessary to dynamically adjust the trajectory based on the local environmental information of the unmanned helicopter's current location on the basis of global planning. When the unmanned helicopter is ready to start actual flight, it first discards the previously input prior map, only retains the globally planned trajectory result as the initial path, and instead obtains real-time environmental information through sensors. Figure 6 The dashed circle represents the range of the local map obtained by the sensor when actually flying to point P. We achieve real-time update of the local environmental information inside the algorithm in this way.

[0099] Next, we need to find a method to represent the trajectory. Since the polynomial curve has the advantages of simple form and easy derivation of the polynomial curve coefficient endpoint constraints, we use the polynomial curve to represent the local trajectory. The flowchart of local planning is as Figure 7 shown.

[0100] During the trajectory planning process, we give top priority to the requirements of obstacle avoidance and meeting dynamic constraints. However, these two requirements have two different characteristics: the obstacle penalty function for obstacle avoidance consumes a large amount of computing resources, but whether the trajectory meets the obstacle avoidance requirements is very intuitive, easy to optimize, and has high optimization efficiency; while the penalty function for meeting dynamic constraints has a small amount of calculation, but requires iterative calculation. If the two are optimized together, they will have the disadvantages of both. Currently, in the field of robot trajectory planning, the main idea to solve the above problems is to separately solve the requirements of obstacle avoidance and meeting dynamic constraints: first perform path planning to obtain a path that meets the obstacle avoidance requirements, and then select several waypoints from the path. These waypoints will become the constraint points that the unmanned helicopter must pass through during subsequent trajectory optimization. Generally, as long as these waypoints are close to each other, we can consider that the aircraft will not encounter obstacles during trajectory planning.

[0101] First, perform path planning. We use the following formula as the objective function for optimization:

[0102] f total = λ 1 f c + λ 2 f o + λ 3 f s (18)

[0103] In the formula, f c represents the curvature penalty function term. We hope that there will be no too small turning radius during path planning, and we use the integral of curvature over time to represent this term; f0 represents the distance potential function of the trajectory to the obstacle, which is used to avoid obstacles; f s is used to limit the distance between the generated local path and the global path; λ 1 , λ 2 , λ 3 represent their weights.

[0104] We use the integral of curvature over time to represent the curvature penalty function of the objective function, and use the exponential function as the basis function of the objective function, that is:

[0105] f c = ∫c(k)dt

[0106]

[0107] k represents the curvature of a certain point on the path. When k > k 0 , c(k) will rise rapidly. The value of k 0 is determined by the current speed. α and r affect the size and slope of the function.

[0108] This patent also selects the exponential function as the basis function of the obstacle potential function:

[0109]

[0110]

[0111] In the formula, d represents the distance from a certain point on the path to a certain obstacle, d 0 represents the warning range. If d < d 0 , the obstacle potential function will increase rapidly. α and r are parameters for adjusting the size and slope of the function.

[0112] Similarly, we also use the exponential function to describe the distance between the planned path and the global path.

[0113] After obtaining the local path, we select several waypoints from the local path. These waypoints divide the path into M segments, and we use M Nth-degree polynomial curves to describe the trajectory we are going to plan. If we directly optimize the trajectory at this time, we need to optimize 3×(N + 1)×M polynomial coefficients, as well as the time allocated for the M piecewise polynomial trajectories. In this way, the computational complexity of the optimization will explode exponentially.

[0114] As we found in the derivation of differential flatness in the previous section, the integral of the square of the fourth derivative of the trajectory with respect to time can make the unmanned helicopter fly smoothly. Moreover, if we only use the integral of the square of the fourth derivative of the trajectory with respect to time as the objective function for optimization, then as long as the time allocated for each polynomial segment is given, the optimization problem can be transformed into a quadratic optimization problem, and at this time the polynomial coefficients can be directly solved.

[0115] Therefore, we only need to optimize the time of each polynomial curve.

[0116] So far, the content of real-time local trajectory planning has been completed. As Figure 8 shown, it is the prior map for global planning before starting the flight. Figure 9 It represents the real environment during actual flight. It can be found that Figure 9 compared with Figure 8 there are some changes: the height of the mountain has become higher, and in addition, there is an extra building (rectangle L 1 ), and a signal tower (rectangle L 2 ). There is an unmanned helicopter that takes off from point A in a hovering state, passes through points B, C, and D, and finally reaches point E and hovers. It is required that the attitude change is small during the flight and there is no excessive control input. Figure 10 This is the final actual flight trajectory. It can be found that the unmanned helicopter avoids the newly added obstacles. The pentagrams in the figure are the task points defined by the user at the beginning, the dashed line is the global trajectory, and the solid line is the actual flight trajectory. Since the task points are just on the newly added obstacles, in order to avoid collisions, the actual flight does not pass through each task point completely. Figure 11 This is the image of the speed over time obtained by splicing. Figure 12 This is the image of the attitude angle over time obtained by splicing. Figure 13 This is the image of the angular rate over time obtained by splicing. Figure 14 This is the image of the angular acceleration over time obtained by splicing. It can be seen that the present invention can realize trajectory planning for unknown environments and generate real-time trajectories that satisfy dynamic constraints.

[0117] Each embodiment in this specification is described in a progressive manner. For the same or similar parts among the embodiments, reference can be made to each other. Each embodiment focuses on the differences from other embodiments. In particular, for the device embodiments, the above description is only the preferred embodiment of the present invention. Since it is basically similar to the method embodiments, the description is relatively simple. For the relevant parts, reference can be made to the partial description of the method embodiments. As mentioned above, the above is only the specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. For any person skilled in the art in the technical field disclosed by the present invention, for those of ordinary skill in the technical field, any changes or substitutions that can be easily thought of without departing from the principle of the present invention should be covered within the protection scope of the present invention. Therefore, the protection scope of the present invention should be subject to the protection scope of the claims.

Claims

1. A trajectory planning method for an unmanned helicopter, characterized in that: it includes global planning and local planning; in global planning, the sampling-based path planning algorithm RRT* algorithm is adopted. According to the prior environmental information and the task points issued by the user, intermediate waypoints are supplemented to form a path without obstacles. Then, the differential flatness theory is used to smooth this path to obtain a smooth and collision-free initial minimized Snap trajectory, which is used as the default path for subsequent local planning; in local planning, the result of global planning is used as the default path for actual flight. A section is taken from the result of global planning as the initial path for flight and a polynomial curve is used to fit the initial path; the obstacle avoidance function for obstacles is realized by calculating the obstacle penalty function, curvature penalty function and deviation penalty function to obtain a feasible path. Several waypoints are selected from it, and the waypoints are used as the constraints of trajectory planning. The trajectory is represented by a piecewise polynomial. Using the differential flatness theory, the trajectory planning problem is converted into a quadratic programming problem. Finally, the polynomial coefficients and the piecewise time are decoupled, and the real-time trajectory is obtained by solving the piecewise time; The specific steps of the global planning are as follows: First, use the RRT* algorithm to search for a collision-free feasible path; Then, based on three assumptions: 1) During the flight process, the acceleration of the aircraft in the horizontal plane is relatively small; 2) The differences between the current attitude angles θ and φ of the helicopter and the attitude angles during trimming are very small; ​ 3) During flight, the vertical acceleration in the geocentric coordinate system is much smaller than the gravitational acceleration, that is A simplified helicopter flight dynamics equation is established and the differential flatness of this flight dynamics equation is proved, that is: Where: θ represents the pitch angle of the airframe, φ represents the roll angle of the airframe, and represent the pitch angle and attitude angle during trimming, and represent the pitch rate and angular acceleration rate, and represent the roll angular acceleration rate, and represent the yaw rate and angular acceleration rate, a ⊥ 、 j ⊥ 、 s ⊥ 、 are intermediate calculation parameters, and their definitions are as follows: where: X, Y, Z respectively represent the body coordinates in the earth axis system; the superscripts (3), (4) represent the 3rd and 4th derivatives of the coordinates with respect to time; Let the trajectory be p i , and use the square of the fourth derivative of the trajectory as a function of time as the objective function for global planning: Using a non - linear programming solver, the trajectory p is obtained i , and finally the path information is extracted from the planned trajectory as the result of the global planning; The specific process of the local planning is as follows: First, the result of global planning is used as the default path for actual flight. A section is taken from the result of global planning as the initial path for flight and a polynomial curve is used to fit the initial path; Secondly, the obstacle avoidance function for obstacles is realized by calculating the obstacle penalty function, curvature penalty function and deviation penalty function to obtain a feasible path, and several waypoints are selected from it; After that, the above waypoints are used as the constraints of trajectory planning. The trajectory is represented by a piecewise polynomial. Using the differential flatness theory, the trajectory planning problem is converted into a quadratic programming problem; Finally, the polynomial coefficients and the piecewise time are decoupled, and the real-time trajectory is obtained by solving the piecewise time.

2. The trajectory planning method for an unmanned helicopter according to claim 1, characterized in that: The selection of the several waypoints is specifically as follows: The following formula is used as the optimization objective function: f total = λ 1 f c + λ 2 f o + λ 3 f s (5) where f c represents the curvature penalty function term, which hopes that there will be no too small turning radius during the path planning process, and this term is represented by the integral of curvature over time; f o represents the distance potential function of the trajectory to the obstacle, which is used to avoid obstacles; f s is used to limit the distance between the generated local path and the global path; λ 1 , λ 2 , λ 3 represent their weights; The curvature penalty function of the objective function is represented by the integral of curvature over time, and the exponential function is used as the basis function of the objective function, that is: k represents the curvature at a certain point on the path. When k > k 0 , c(k) will increase rapidly. The value of k 0 is determined by the current speed; α and r affect the magnitude and slope of the function; The exponential function is selected as the basis function of the obstacle potential function, and at the same time, the exponential function is used to describe the distance between the planned path and the global path; Wherein, d represents the distance from a certain point on the path to a certain obstacle, d 0 represents the warning range. If d < d 0 , the potential function of the obstacle will increase rapidly. α and r are parameters for adjusting the size and slope of the function.

Images