A motion planning method for a quadrotor drone

Abstract

A motion planning method for a quad-rotor drone belongs to the field of drone motion planning. The present invention solves the problems that the existing trajectory planning method for optimizing flight time requires global environmental information, the trajectory planning takes a long time, and the planning method has poor robustness. The present invention determines the local planning domain according to the drone center and the detection range of the airborne sensor, and projects the global target end point on the local planning domain to obtain the local target end point; judges whether the line connecting the drone center and the local target end point passes through an obstacle, and if so, finds a replacement point for each obstacle passed through as a waypoint through which the path passes, and plans and optimizes the trajectory between each two adjacent points in the drone center, waypoints, and local target end points; if not, the line connecting the drone center and the local target end point is the path from the drone to the local target end point; repeats the above process until the global target end point is reached. The present invention can be applied to drone motion planning.

Description

Technical Field

[0001] The invention belongs to the technical field of unmanned aerial vehicle motion planning, and in particular relates to a motion planning method for a quad-rotor unmanned aerial vehicle. Background Art

[0002] With the continuous development of UAV technology, its application in various fields is becoming more and more extensive. Therefore, how to carry out effective UAV motion planning has become the focus of current research. For example, the literature (Pham Q.A General, Fast, and Robust Implementation of the Time-Optimal Path Parameterization Algorithm [J]. IEEE Transactions on Robotics, 2013, 30 (6): 1533-1540. DOI: 10.1109 / TRO.2014.2351113.) uses a method based on numerical integration to plan the time-optimal motion trajectory. This method is easy to implement and can explicitly assign the optimal speed to the position coordinates through integration, thereby achieving the effect of optimal motion trajectory time. The advantages of this method in reliability and running time are compared through simulation experiments. However, it does not take into account the limited detection range of the onboard sensors of the UAV during actual operation. The motion planning is carried out under the assumption that the global environmental information is accurately known, which cannot truly realize the real-time flight of the UAV with onboard resources. The paper (Verscheure D, Demeulenaere B, Swevers J, et al. Time-Optimal Path Tracking for Robots: A ConvexOptimization Approach [J]. IEEE TRANSACTIONS ON AUTOMATIC CONTROL AC, 2009, 54 (10): 2318-2327. DOI: 10.1109 / TAC.2009.2028959.) uses the idea of ​​convex optimization to solve the time-optimal trajectory planning problem. By modeling the state space and introducing constraints, the motion planning problem is transformed into a convex optimization problem, so that the consideration of time is added to the optimization cost function to obtain a trajectory planning result with time optimality. However, it makes certain assumptions on the motion model and cost function of the UAV to meet the requirements of the convex function, which limits the scope of use of the motion planning method and reduces the robustness of the method. The paper (Hua Tong. Research on the spatiotemporal trajectory planning method of multi-rotor UAV [D]. Dalian Maritime University, 2022.) adopts the spatiotemporal decoupling method to plan the trajectory for the UAV. The spatial motion planning obtains a smooth trajectory that satisfies the constraints by combining the path search results of the RRT* algorithm with the gradient descent method, and then further shortens the flight time of the spatial trajectory through an additional time cost optimization process. However, it optimizes the trajectory flight time separately. Although a shorter trajectory time allocation result is obtained, the increase in algorithm running time is undoubtedly not conducive to the UAV to complete fast flight.

[0003] In summary, the existing trajectory planning methods for optimizing UAV flight time have the problems of requiring global environmental information, long time required for trajectory planning, and poor robustness of the planning method. It is very necessary to solve the above problems. Summary of the invention

[0004] The purpose of the present invention is to propose a motion planning method for a quadrotor UAV in order to solve the problems that the existing trajectory planning method for optimizing the flight time of the UAV requires global environmental information, the trajectory planning takes a long time, and the planning method has poor robustness.

[0005] The technical solution adopted by the present invention to solve the above technical problems is: a motion planning method for a quad-rotor drone, the method specifically comprising the following steps:

[0006] Step 1: Take the center of the current UAV as the center of the circle and the detection limit distance of the airborne sensor as the radius, and use the obtained circular area as the local planning domain of the UAV;

[0007] Project the global target endpoint onto the local planning domain to obtain the local target endpoint of the UAV;

[0008] Step 2: Determine whether the line connecting the center of the drone and the local target endpoint passes through an obstacle;

[0009] If the line connecting the center of the drone and the local target endpoint passes through an obstacle, execute step 3;

[0010] If the line connecting the center of the drone and the local target end point does not pass through an obstacle, the line connecting the center of the drone and the local target end point is used as the path from the drone to the local target end point; and go to step 4;

[0011] Step 3: Find replacement points for each obstacle passed by, and use the replacement points found as waypoints for the path. Perform preliminary path planning for the navigation route between every two adjacent points among the current drone position point, waypoints and local target end points, and then optimize the path of the preliminary path planning results.

[0012] Then go to step 4;

[0013] Step 4: Determine whether the global target destination has been reached;

[0014] If the global target end point has been reached, the motion planning ends;

[0015] If the global target end point has not been reached, return to step 1.

[0016] Furthermore, the specific method for finding the replacement point is:

[0017] For any obstacle passed, draw a perpendicular line between the obstacle and the local target end point, and select any replacement point on the perpendicular line;

[0018] Similarly, find a replacement point for each obstacle passed.

[0019] Furthermore, the specific process of the preliminary path planning is as follows:

[0020] Step 1, initialize the number of iterations i = 1;

[0021] Step 2: Use the RRT* algorithm to calculate the point X start With point X goal Plan the path between i ; Among them, X start and X goal are two adjacent points;

[0022] Step 3: Determine whether the stop condition of path planning is met;

[0023] If the stop condition of path planning is not met, set i=i+1 and continue to step 4;

[0024] If the stopping condition of path planning is reached, the planning result obtained in the last iteration is used as the preliminary path planning result;

[0025] Step 4: Set the path length l planned in the i-1th iteration i-1 As the length of the major axis of the ellipse, As the length of the minor axis of the ellipse, C min Representative point X start With point X goal The straight-line distance between points X start With point X goal As a point on the major axis of the ellipse, and point X start With point X goal Symmetric about the center of the ellipse;

[0026] The interior of the ellipse is taken as the sampling area. According to the determined sampling area, the path is replanned using the RRT* algorithm. The length of the planned path is recorded as l i ;

[0027] And return to step 3.

[0028] Furthermore, a cost optimization step is added to the RRT* algorithm, and the cost optimization method is (1) or (2):

[0029] (1) Within the set range, select the parent node with the smallest path time cost for the newly extended node;

[0030] (2) Within the set range, the newly extended node is used as the parent node of the existing nodes in the tree to rewire the tree structure.

[0031] Furthermore, the stopping condition of the path planning is that the difference in path lengths between two iterative plans is less than a set threshold or the set maximum number of iterations is reached.

[0032] Furthermore, the path optimization is performed on the preliminary path planning result; specifically:

[0033] Step (1), according to the set time step, forwardly integrate from the initial state at the maximum acceleration on the position-velocity plane until the maximum velocity curve when the touch does not violate the constraint is obtained;

[0034] Step (2), starting from the state where the constraint is violated, finding the integral switching point along the maximum speed curve when the constraint is not violated;

[0035] Step (3), at the integration switching point, first integrate backward with the integration start acceleration and then integrate backward with the minimum acceleration until it intersects with the integral curve obtained in step (1);

[0036] Step (4): reset the state at the integral switching point to the initial state and return to step (1).

[0037] Furthermore, the method for finding the integral switching point is: a point that satisfies one of the following conditions is the integral switching point;

[0038] Condition (1): Discontinuity on the maximum velocity curve when the constraint is not violated;

[0039] Condition (2): The point where the tangent vector of the maximum velocity curve is in the same direction as the tangent vector of the integral curve corresponding to the acceleration limit value when the constraint is not violated;

[0040] Condition (3): Determine the zero inertia point s * , at point s * There is a constraint variable a k (s * )=0, if the zero inertia point s * The constraint variable b k (s * )≤0, then the point s * It is not an integral switching point. If the zero inertia point s * The constraint variable b k (s * )>0, then calculate the zero inertia point s * The speed at

[0041]

[0042] Among them, c k (s * ) is a constraint variable;

[0043] Comparing speeds With zero inertia point s * The corresponding speed on the maximum speed curve when the constraint is not violated. Less than zero inertia point s * The speed on the maximum speed curve when the constraint is not violated is the speed at the zero inertia point s * is the integration switching point, otherwise the zero inertia point s * It is not an integral switching point.

[0044] Furthermore, the integral start acceleration is calculated as follows:

[0045] The integration switching point s * The speed at Then the real number λ is calculated:

[0046]

[0047] Among them, α k is the minimum acceleration at the integral switching point, and ε is an arbitrarily small positive number;

[0048] Then the terminal velocity of the first connecting segment is according to The integrated starting acceleration of the first connecting segment is calculated using the time step;

[0049] Then, the calculation method of the integral starting acceleration of the first connecting segment is adopted, according to The integrated starting acceleration of the second connecting segment is calculated, and so on, until the integrated starting acceleration of the nth connecting segment is calculated.

[0050] The beneficial effects of the present invention are:

[0051] The method of the present invention can ensure that the UAV uses onboard resources to perform real-time planning in an unknown environment, rather than based on a priori known global environmental information, thereby achieving true autonomous flight. Moreover, the path planning algorithm of the present invention can further improve the algorithm's operational efficiency while ensuring the quality of the path, laying the foundation for the rapid flight of the quad-rotor UAV. The present invention adopts a numerical integration algorithm with simple implementation and low computational cost, and is designed for the processing of integral switching, rather than using enumeration to find the switching point, which improves the reliability and robustness of the algorithm while ensuring the optimal trajectory time. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1A flow chart of a motion planning method for a quad-rotor drone of the present invention;

[0053] Figure 2 Find a directional diagram for a safe replacement point;

[0054] Figure 3 It is a schematic diagram of the sampling range of the ellipse;

[0055] Figure 4 It is a schematic diagram of the ellipsoid chain sampling space;

[0056] Figure 5 This is a schematic diagram of rolling time domain planning. DETAILED DESCRIPTION

[0057] Specific implementation method 1: Combination Figure 1 The present embodiment describes a motion planning method for a quad-rotor drone, and the method specifically comprises the following steps:

[0058] Step 1: Take the center of the current UAV as the center of the circle and the detection limit distance of the airborne sensor as the radius, and use the obtained circular area as the local planning domain of the UAV;

[0059] Project the global target endpoint onto the local planning domain to obtain the local target endpoint of the drone (when the global target endpoint is outside the local planning domain, the local target endpoint is the intersection of the line connecting the current drone center and the global target endpoint and the boundary of the local planning domain; when the global target endpoint is within the local planning domain, the local target endpoint is the global target endpoint);

[0060] Step 2: Determine whether the line connecting the center of the drone and the local target endpoint passes through an obstacle;

[0061] If the line connecting the center of the drone and the local target endpoint passes through an obstacle, execute step 3;

[0062] If the line connecting the center of the drone and the local target end point does not pass through an obstacle, the line connecting the center of the drone and the local target end point is used as the path from the drone to the local target end point; and go to step 4;

[0063] Step 3: Find replacement points for each obstacle passed by, and use the replacement points found as waypoints for the path. Perform preliminary path planning for the navigation route between every two adjacent points among the current drone position point, waypoints and local target end points, and then optimize the path of the preliminary path planning results.

[0064] Then go to step 4;

[0065] Step 4: Determine whether the global target destination has been reached;

[0066] If the global target end point has been reached, the motion planning ends;

[0067] If the global target end point has not been reached, return to step 1.

[0068] like Figure 2 As shown in the figure, when a local trajectory collides, a replanning process needs to be designed to guide the trajectory to avoid obstacles at known locations. The replanning process mainly includes:

[0069] 1. Find a safe replacement point for the collision trajectory point; the direction of finding the safe replacement point is perpendicular to the line connecting the collision point and the local target end point;

[0070] 2. Use the safe replacement point as the path waypoint and re-optimize the trajectory.

[0071] In order to realize autonomous flight of UAVs using airborne sensors in unknown environments, the present invention designs a rolling time domain motion planning framework to guide the UAV to continuously plan local trajectories within the limited sensor field of view until the global target endpoint is reached. According to the detection limit of the airborne sensor, the local planning domain of the UAV is set, and then the global target endpoint is projected onto the local planning domain to obtain the local target endpoint. According to the current position of the UAV and the local target endpoint information, path planning and trajectory optimization are continuously performed to generate a local trajectory Φ.

[0072] The drone can only be used within the specified time range T e A part of the local trajectory Φ is executed inside, called T e is the execution domain. When the UAV reaches the boundary position Φ(T e ), the next local trajectory will be planned. To ensure that the UAV can successfully receive the planning result of the next trajectory while executing the current local trajectory, it is necessary to ensure that the execution domain T e Greater than the time of local trajectory planning, as Figure 5 shown.

[0073] Specific implementation method 2: This implementation method is different from the specific implementation method 1 in that the specific method of finding the replacement point is:

[0074] For any obstacle passed, draw a perpendicular line between the obstacle and the local target end point, and select any replacement point on the perpendicular line;

[0075] Similarly, find a replacement point for each obstacle passed.

[0076] The other steps and parameters are the same as those in the first embodiment.

[0077] Specific implementation method three: This implementation method is different from specific implementation methods one or two in that the specific process of the preliminary path planning is:

[0078] Step 1, initialize the number of iterations i = 1;

[0079] Step 2: Use the RRT* algorithm to calculate the point X start With point X goal Plan the path between i ; Among them, X start and X goal are two adjacent points;

[0080] Step 3: Determine whether the stop condition of path planning is met;

[0081] If the stop condition of path planning is not met, set i=i+1 and continue to step 4;

[0082] If the stopping condition of path planning is reached, the planning result obtained in the last iteration is used as the preliminary path planning result;

[0083] Step 4: Set the path length l planned in the i-1th iteration i-1 As the length of the major axis of the ellipse, As the length of the minor axis of the ellipse, C min Representative point X start With point X goal The straight-line distance between points X start With point X goal As a point on the major axis of the ellipse, and point X start With point X goal Symmetric about the center of the ellipse;

[0084] The interior of the ellipse is taken as the sampling area. According to the determined sampling area, the path is replanned using the RRT* algorithm. The length of the planned path is recorded as l i ;

[0085] And return to step 3.

[0086] The other steps and parameters are the same as those in the first or second embodiment.

[0087] Considering the requirement of using onboard resources to perform real-time motion planning in an unknown environment, the path planning algorithm must not only have a fast calculation speed, but also cannot rely too much on global environmental information. Therefore, the path planning algorithm selected in this implementation is the RRT* algorithm. In order to reduce the calculation time of the RRT* algorithm, the method of limiting the sampling range is adopted to simplify the sampling process. Figure 3 As shown, the elliptical surface of the two-dimensional plane is used for explanation:

[0088] X start and Xgoal Set as the local waypoint in path planning, and set C best Set to the length of the local path of the last iteration. According to the sum of the distances from the points in the ellipse to the two foci being less than the major axis, the sampling range is limited to be within the ellipse, which can ensure that the distance cost of the new sampling point is less than the result of the last iteration, that is, each new sampling is an effective sampling that optimizes the result of the last sampling.

[0089] Similarly, in three-dimensional space, the sampling range of each local path is limited by the ellipsoid, such as Figure 4 As shown in the figure, an algorithm sampling space structure of an "ellipsoid chain" is formed, which can significantly reduce the number of sampling times required to find the optimal path and improve the algorithm operation efficiency.

[0090] Specific implementation method 4: This implementation method is different from any one of the specific implementation methods 1 to 3 in that a cost optimization step is added to the RRT* algorithm, and the cost optimization method is (1) or (2):

[0091] (1) Within the set range, select the parent node with the smallest path time cost for the newly extended node;

[0092] (2) Within the set range, the newly extended node is used as the parent node of the existing nodes in the tree to rewire the tree structure.

[0093] The other steps and parameters are the same as those in Specific Embodiments 1 to 3.

[0094] This implementation adds a cost optimization link to make the RRT* algorithm have asymptotic optimality and improve the path quality. The method for setting the range is: select the existing node closest to the new sampling point (sampling step is 0.45 meters), take the selected existing node as the center, and make a circle with a radius r (the value range of r is 0.5 to 1 meter). The obtained circular area is the set range.

[0095] Specific implementation method five: This implementation method is different from any one of specific implementation methods one to four in that: the stopping condition of the path planning is: the difference in path lengths planned by two iterations is less than a set threshold or the set maximum number of iterations is reached.

[0096] The other steps and parameters are the same as those in Specific Embodiments 1 to 4.

[0097] Specific implementation method 6: This implementation method is different from the specific implementation methods 1 to 5 in that: the path optimization is performed on the preliminary path planning result; specifically:

[0098] Step (1), according to the set time step, forwardly integrate from the initial state at the maximum acceleration on the position-velocity plane until the maximum velocity curve when the touch does not violate the constraint is obtained;

[0099] Step (2), starting from the state where the constraint is violated, finding the integral switching point along the maximum speed curve when the constraint is not violated;

[0100] Step (3), at the integration switching point, first integrate backward with the integration start acceleration and then integrate backward with the minimum acceleration until it intersects with the integral curve obtained in step (1);

[0101] Step (4): reset the state at the integral switching point to the initial state and return to step (1).

[0102] Point X start With point X goal As an example, the method of this embodiment is executed until the point X is reached. goal .

[0103] The other steps and parameters are the same as those in Specific Implementation Methods 1 to 5.

[0104] Specific implementation method 7: This implementation method is different from any one of specific implementation methods 1 to 6 in that: the method for finding the integral switching point is: the point that satisfies one of the following conditions is the integral switching point;

[0105] Condition (1): Discontinuity on the maximum velocity curve when the constraint is not violated;

[0106] Condition (2): The point where the direction of the tangent vector of the maximum velocity curve when the constraint is not violated is the same as the direction of the tangent vector of the integral curve corresponding to the acceleration limit value (the maximum acceleration corresponding to the maximum velocity curve when the constraint is not violated is equal to the minimum acceleration, so the limit value here is both the maximum acceleration and the minimum acceleration);

[0107] Condition (3): Determine the zero inertia point s * , at point s * There is a constraint variable a k (s * )=0, if the zero inertia point s * The constraint variable b k (s * )≤0, then the point s * It is not an integral switching point. If the zero inertia point s * The constraint variable b k (s * )>0, then calculate the zero inertia point s * The speed at

[0108]

[0109] Among them, c k (s* ) is a constraint variable;

[0110] Comparing speeds With zero inertia point s * The corresponding speed on the maximum speed curve when the constraint is not violated. Less than zero inertia point s * The speed on the maximum speed curve when the constraint is not violated is the speed at the zero inertia point s * is the integration switching point, otherwise the zero inertia point s * It is not an integral switching point.

[0111]

[0112]

[0113] c(s * )=f(q(s * ))

[0114] In the formula, a k (s * ), b k (s * ) and c k (s * ) are the constraint vectors a(s * )、b(s * ) and c(s * ) is the kth constraint component, q(s * ) represents the preliminary path planning result, A, B and f are matrices that can be calculated based on geometric constraints and dynamic constraints. is q(s * ) with respect to the first-order derivative of s*, is q(s * ) with respect to the second-order derivative of s*.

[0115] The other steps and parameters are the same as those in Specific Embodiments 1 to 6.

[0116] Specific implementation eight: This implementation differs from any one of specific implementations one to seven in that: the calculation method of the integral start acceleration is:

[0117] The integration switching point s * The speed at Then the real number λ is calculated:

[0118]

[0119] Among them, α k is the minimum acceleration at the integral switching point, and ε is an arbitrarily small positive number;

[0120] Then the terminal velocity of the first connecting segment is according to The integrated starting acceleration of the first connecting segment is calculated based on the time step (the end position of each connecting segment is calculated according to the integrated starting acceleration of the current connecting segment and the starting position of the current connecting segment, according to the uniformly accelerated linear motion formula);

[0121] Then, the calculation method of the integral starting acceleration of the first connecting segment is adopted, according to The integrated starting acceleration of the second connecting segment is calculated, and so on, until the integrated starting acceleration of the nth connecting segment is calculated (the value of n in the present invention is 3 to 5, and after integrating several segments with the integrated starting acceleration, continue to integrate backward with the minimum acceleration).

[0122] The other steps and parameters are the same as those in Specific Embodiments 1 to 7.

[0123] The path optimization method of the present invention is a time optimal path optimization method based on numerical integration. Compared with other algorithms aimed at time optimization, the method based on numerical integration has the advantages of easy implementation and fast calculation speed, and can meet the requirements of real-time motion planning of quadrotor UAVs. The implementation process of the algorithm is theoretically explained below.

[0124] Assuming that vector q is an n-dimensional vector representing the state of the quadrotor drone system, the following second-order inequality constraint is universal.

[0125]

[0126] Where A(q) is an M×n dimensional matrix, B(q) is an n×M×n dimensional tensor, and f(q) is an M dimensional vector.

[0127] Consider that there is currently a feasible path q(s) (which can be understood as the path to be optimized) represented by the relationship between position and state, where the starting point of the position is specified, that is, s∈[0,send]. The scalar function of time-position can be expressed as:

[0128]

[0129] Differentiating the state vector q with respect to time, we can obtain:

[0130]

[0131] Substituting formula (3) into the second-order inequality constraint, that is, into formula (1), we can obtain:

[0132]

[0133] Formula (4) can be further simplified as:

[0134]

[0135] in:

[0136]

[0137] Take a set of components in the constraint vectors a, b, c in formula (6), namely:

[0138]

[0139] According to the amount a i (s) is positive or negative at position s, inequality (7) can have two forms:

[0140]

[0141] That is, this set of constraints specifies the acceleration boundary values ​​corresponding to the constraints at position s, which can be recorded as the acceleration upper limit β i With the lower limit of acceleration α i . If there is a at position s i (s) = 0, this point is called the zero inertia point and needs to be processed separately.

[0142] Integrating the acceleration boundary values ​​corresponding to all constraints of all non-zero inertia points at a specific position s can be defined as follows:

[0143]

[0144] Therefore, on the specified path q(s(t)), t∈[0,T], the second-order inequality constraint can be equivalently expressed as an acceleration constraint at a specific position and a specific velocity on the position-velocity plane. The form of the acceleration constraint is as follows:

[0145]

[0146] If at a point in the position-velocity plane there is This means that the point satisfies the second-order inequality constraint of the system; otherwise, if there is This means that there is no feasible acceleration value that satisfies the constraints.

[0147] Therefore, the integral curve corresponding to the second-order inequality constraint of the system is defined, that is, the maximum velocity curve (MVC) corresponding to the UAV when it does not violate the constraint is as follows:

[0148]

[0149] At the same time and Considered as two vector fields on the position-velocity plane, the velocity can be integrated according to these two vector fields respectively, and the results are recorded as the integral curve α-profile corresponding to the minimum acceleration and the integral curve β-profile corresponding to the maximum acceleration.

[0150] Combined with the Pontalyakin maximum principle, that is, the time optimality can be achieved when the given performance indicator of the controlled system or motion process takes the maximum or minimum value, the numerical integration algorithm is mainly divided into the following processes: 1. Integrate from the initial state on the position-velocity plane with a fixed step length of maximum acceleration until the constraint is violated, that is, the MVC is touched; 2. Start from the state of violating the constraint and find the integration switching point along the MVC; 3. Integrate backward at the integration switching point with minimum acceleration until it intersects with the original integration curve; 4. Continue to integrate forward with maximum acceleration, and repeat the above process until the terminal state is reached.

[0151] Therefore, the processing of the integral switching point on the MVC becomes the focus of the algorithm. It can be proved that there are only three types of switching points: 1. The MVC is discontinuous; 2. The MVC is continuous but not differentiable; 3. The direction of the tangent vector of the integral curve corresponding to the MVC and the acceleration limit value is the same.

[0152] For the first and third types of switching points, their formation conditions can be examined along the MVC. For the second type of switching points, since examining the differentiability will cause discrete errors during implementation, they need to be handled separately.

[0153] It can be proved that the second type of switching point belongs to the zero inertia point, that is, at point s * There is a kth constraint component a at k (s * )=0. Without loss of generality, we can assume that at point s * The left neighbor has a k (s * )<0, correspondingly there is a k (s * )>0. It can be proved by mathematical methods that there must be c zero inertia points on all traversable paths. k (s * )<0.

[0154] So far, among the kth components of the vectors a, b, and c in the inequality constraints, only b k (s * ) positive and negative positions, and the zero inertia point is divided into the following two types: 1. Zero inertia point b k (s * )<0, then this point is not a switching point; 2. Zero inertia point b k (s * )>0, it may be a switching point. Define speed as follows:

[0155]

[0156] Compare this speed with the speed of the maximum speed curve MVC corresponding to the zero inertia point. If If the value is smaller, the zero inertia point can be used as the switching point.

[0157] Another key issue about integral switching is to choose the best starting acceleration for integral starting. Because according to the definition of zero inertia point, due to a k (s * )=0, the integral start acceleration cannot be calculated naturally. If the integral start acceleration is selected arbitrarily, it may cause strong oscillation and the numerical integration algorithm fails to terminate. The following uses the backward integral start on the left side of the switching point as an example to illustrate the method of selecting the integral start acceleration.

[0158] It can be proved mathematically that s * The upper and lower limits of the acceleration at are completely determined by the kth constraint that causes it to become a zero inertia point, that is,

[0159] α=α k ,β=β k (13)

[0160] Assume that there is a line close to s on the position-velocity plane * The vertical line is:

[0161] s=s * -ε (14)

[0162] where ε is an arbitrarily small positive number.

[0163] In s * On the left, the vertical line intersects the α-profile. This is only true if the tangent vector of the α-profile at the intersection with the vertical line points to the fixed point. Only when α-profile is smoothly integrated to the maximum velocity curve MVC.

[0164] The above condition can be expressed in mathematical form as follows: There exists a real number λ such that the following equation holds:

[0165]

[0166] Therefore, the integral start can be started along Integrate a small distance to ensure that it intersects with the existing integral curve, and then return to the original integral curve. So far, the overall process and implementation method of the time optimal trajectory optimization algorithm based on numerical integration have been explained.

[0167] The above calculation examples of the present invention are only used to explain the calculation model and calculation process of the present invention in detail, and are not intended to limit the implementation methods of the present invention. For ordinary technicians in the relevant field, other different forms of changes or modifications can be made based on the above description. It is impossible to list all the implementation methods here. All obvious changes or modifications derived from the technical solution of the present invention are still within the scope of protection of the present invention.

Claims

1. A motion planning method for a quad-rotor drone, characterized in that: The method specifically comprises the following steps: Step 1: Take the center of the current UAV as the center of the circle and the detection limit distance of the airborne sensor as the radius, and use the obtained circular area as the local planning domain of the UAV; Project the global target endpoint onto the local planning domain to obtain the local target endpoint of the UAV; Step 2: Determine whether the line connecting the center of the drone and the local target endpoint passes through an obstacle; If the line connecting the center of the drone and the local target endpoint passes through an obstacle, execute step 3; If the line connecting the center of the drone and the local target end point does not pass through an obstacle, the line connecting the center of the drone and the local target end point is used as the path from the drone to the local target end point; and go to step 4; Step 3: Find replacement points for each obstacle passed by, and use the replacement points found as waypoints for the path. Perform preliminary path planning for the navigation route between every two adjacent points among the current drone position point, waypoints and local target end points, and then optimize the path of the preliminary path planning results. The path optimization of the preliminary path planning result is specifically as follows: Step (1), according to the set time step, forwardly integrate from the initial state at the maximum acceleration on the position-velocity plane until the maximum velocity curve when the touch does not violate the constraint is obtained; Step (2), starting from the state where the constraint is violated, find the integral switching point along the maximum speed curve when the constraint is not violated; Step (3), at the integration switching point, first integrate backward with the integration start acceleration and then integrate backward with the minimum acceleration until it intersects with the integral curve obtained in step (1); Step (4), resetting the state at the integral switching point to the initial state, and returning to step (1); The method for finding the integral switching point is as follows: a point that satisfies one of the following conditions is the integral switching point; Condition (1): Discontinuity on the maximum velocity curve when the constraint is not violated; Condition (2): The point where the tangent vector of the maximum velocity curve is in the same direction as the tangent vector of the integral curve corresponding to the acceleration limit value when the constraint is not violated; Condition (3): Determine the zero inertia point s * , at point s * There is a constraint variable a k (s * )=0, if the zero inertia point s * The constraint variable b k (s * )≤0, then the point s * It is not an integral switching point. If the zero inertia point s * The constraint variable b k (s * )>0, then calculate the zero inertia point s * The speed at Among them, c k (s * ) is a constraint variable; Comparing speeds With zero inertia point s * The corresponding speed on the maximum speed curve when the constraint is not violated. Less than zero inertia point s * The speed on the maximum speed curve when the constraint is not violated is the speed at the zero inertia point s * is the integration switching point, otherwise the zero inertia point s * It is not an integral switching point; Then go to step 4; Step 4: Determine whether the global target destination has been reached; If the global target end point has been reached, the motion planning ends; If the global target end point has not been reached, return to step 1.

2. The motion planning method of a quad-rotor drone according to claim 1, characterized in that: The specific method for finding the replacement point is: For any obstacle passed, draw a perpendicular line between the obstacle and the local target end point, and select any replacement point on the perpendicular line; Similarly, find a replacement point for each obstacle passed.

3. The motion planning method of a quad-rotor drone according to claim 2, characterized in that: The specific process of the preliminary path planning is as follows: Step 1, initialize the number of iterations i = 1; Step 2: Use the RRT* algorithm to calculate the point X start With point X goal Plan the path between i ; Among them, X start and X goal are two adjacent points; Step 3: Determine whether the stop condition of path planning is met; If the stop condition of path planning is not met, set i=i+1 and continue to step 4; If the stopping condition of path planning is reached, the planning result obtained in the last iteration is used as the preliminary path planning result; Step 4: Set the path length l planned in the i-1th iteration i-1 As the length of the major axis of the ellipse, As the length of the minor axis of the ellipse, C min Representative point X start With point X goal The straight-line distance between points X start With point X goal As a point on the major axis of the ellipse, and point X start With point X goal Symmetric about the center of the ellipse; The interior of the ellipse is taken as the sampling area. According to the determined sampling area, the path is replanned using the RRT* algorithm. The length of the planned path is recorded as l i ; And return to step 3.

4. The motion planning method of a quad-rotor drone according to claim 3, characterized in that: The RRT* algorithm adds a cost optimization step, and the cost optimization method is (1) or (2): (1) Within the set range, select the parent node with the smallest path time cost for the newly extended node; (2) Within the set range, the newly extended node is used as the parent node of the existing nodes in the tree to rewire the tree structure.

5. The motion planning method for a quad-rotor drone according to claim 4, characterized in that: The stopping condition of the path planning is: the difference in path lengths between two iterations is less than a set threshold or the set maximum number of iterations is reached.

6. The motion planning method for a quad-rotor drone according to claim 5, characterized in that: The calculation method of the integral starting acceleration is: The integration switching point s * The speed at Then the real number λ is calculated: Among them, α k is the minimum acceleration at the integral switching point, and ε is an arbitrarily small positive number; The terminal velocity of the first connecting segment is according to The integrated starting acceleration of the first connecting segment is calculated using the time step; Then, the calculation method of the integral starting acceleration of the first connecting segment is adopted, according to The integrated starting acceleration of the second connecting segment is calculated, and so on, until the integrated starting acceleration of the nth connecting segment is calculated.

Images