A motion planning method and system considering dynamic constraints of quadrotor unmanned aerial vehicle

Abstract

The present invention discloses a motion planning method considering the dynamic constraints of a quad-rotor unmanned aerial vehicle. An optimized motion trajectory is obtained by establishing a trajectory planning problem model of a quad-rotor unmanned aerial vehicle. The motion trajectory is optimized by using a B-spline curve parameterization so that one-dimensional polynomial coefficients are converted into control points with physical meaning in space. At the same time, a search tree and a state tree are designed to improve the search process so that the control points corresponding to the B-spline curve meet the dynamic constraints in the region, thus avoiding the complex steering function design in the traditional method, ensuring the quality of the motion trajectory, and improving the performance of the algorithm. Correspondingly, the present invention provides a motion planning system considering the dynamic constraints of a quad-rotor unmanned aerial vehicle.

Description

Technical Field

[0001] The present invention relates to the field of robot motion planning, and in particular to a motion planning method and system considering the dynamic constraints of a quad-rotor unmanned aerial vehicle. Background Art

[0002] In recent years, drones, especially quad-rotor drones, have been used to complete different tasks, such as aerial photography, circuit inspection, and rescue, due to their small size, good maneuverability, and strong hovering ability. However, the current drone missions mainly rely on the instructions of the ground control station, and all kinds of planning and decision-making tasks during the flight are completed by the pilot, which usually results in the drone being unable to complete the mission efficiently and limits the application of drones. Realizing autonomous flight of drones in complex environments is of great help in improving or even solving the above problems.

[0003] In order to ensure that planning is completed within a limited time, traditional methods usually only require that the path does not collide with obstacles during path search, and consider constraints such as speed and acceleration during optimization. Although this is an efficient strategy, there is a big problem, that is, the geometric path obtained at this time may not necessarily produce a feasible trajectory after trajectory optimization, which leads to planning failure. Kinematic and dynamic path search can solve this problem to a certain extent. This method considers the dynamic constraints of the drone itself during path search and directly generates a path that the drone can execute. However, this type of method needs to solve the optimal control problem in each iteration, which is computationally intensive. Summary of the invention

[0004] Purpose of the invention: The purpose of the present invention is to provide a motion planning method and system that takes into account the dynamic constraints of a quadrotor drone, fully utilizing the properties of the B-spline curve, combining the B-spline curve with a fast exploration random tree algorithm to process the dynamic constraints of the drone, improving the search process by simultaneously expanding the search tree and the state tree, and directly searching for executable paths during motion planning to improve the practicability of the algorithm.

[0005] Technical solution: The present invention provides a motion planning method considering the dynamic constraints of a quad-rotor drone, comprising the following steps:

[0006] 1) Use a polynomial to represent the motion trajectory of a quad-rotor drone, and establish a quad-rotor drone motion trajectory model based on the quad-rotor drone motion trajectory. The quad-rotor drone motion trajectory model is as follows:

[0007]

[0008] in, represents the system state variable, represents the configuration space, is the first-order derivative of x(t), represents the system input, represents the set of permissible input controls, represents the position of the quadcopter in the inertial coordinate system, x, y, z represent the coordinate axes in the inertial coordinate system, is the first-order derivative of p(t), is the second-order derivative of p(t), is a one-dimensional polynomial; represents the system matrix, represents the input matrix, 0 3 represents the third-order zero matrix, I 3 represents the third-order identity matrix;

[0009] 2) Establish a quadrotor UAV trajectory planning problem model based on the quadrotor UAV motion trajectory model;

[0010] It is known that quadcopters are 0 The state x(t 0 )=x 0 and in t f The state x(t f )=x f ,The trajectory planning problem model of the quadrotor drone is as follows:

[0011]

[0012]

[0013] x(t 0 )=x 0 ,x(t f )=x f

[0014]

[0015]

[0016] in Represents free space, represents the obstacle area, v max represents the maximum speed of the quadrotor drone along each axis of the inertial coordinate system, a max Represents the maximum acceleration of the quadrotor drone along each axis of the inertial coordinate system; For p μ The first derivative of (t), For p μ The second derivative of (t);

[0017] 3) Use B-spline curve to parameterize the motion trajectory of the quadrotor drone and obtain the B-spline curve control points corresponding to the motion trajectory;

[0018] By m+1 nodes {u 0 ,u 1 ,...,u m}, n+1 control points {p 0 ,p 1 ,...,p n The k-order uniform B-spline curve determined by} is defined as follows:

[0019]

[0020] Where N i,k (u) is the B-spline curve basis function, is the i-th B-spline curve control point, The number of nodes, the number of control points and the degree of the B-spline curve satisfy m=n+k+1; is a constant greater than zero;

[0021] 4) Design a search algorithm to search and select B-spline curve control points that meet the dynamic constraints;

[0022] 5) Solve the position of the quadrotor drone corresponding to the control point of the B-spline curve. The position of the quadrotor drone is the final quadrotor drone trajectory. Output the final quadrotor drone motion trajectory to complete the trajectory planning problem.

[0023] Further, in step 4), a search algorithm is designed to search and select B-spline curve control points that meet the dynamic constraints, including the following steps:

[0024] 4.1) Define the graph Construct the search tree and define the graph Constructing the state tree, is the geometric trajectory formed by k+1 B-spline control points, for The corresponding B-spline curve control point set; is the quadrotor UAV trajectory segment corresponding to the B-spline curve control point, are the starting and ending points of the corresponding quadrotor drone trajectory segment; Nodes in and Nodes in Correspondingly, i = 0, 1, 2, ..., N;

[0025] 4.2) Initialize the graph and

[0026] If the control points of the B-spline curve are known to be p j-k ,p j-k+1 ,...p j , then in u∈[u j ,u j+1 ) is as follows:

[0027] c(u)=c j (s) = P j T M k T b(s)

[0028] in b is a vector function of s, s is parameterized for u, reflecting the relationship between u and the node interval [u j ,u j+1 ) position, M k is a k+1-order constant matrix uniquely determined by k, and the control point matrix of the B-spline curve is

[0029] Calculate the first and second derivatives of the quadcopter position formula with respect to u, and convert u k =t 0 and u m-k =t f Substitute into the quadrotor drone position formula and its first-order derivative and second-order derivative with respect to u, u k is the kth node in the node vector, u m-k is the mkth node in the node vector, and we get p 0 ,p 1 ,...,p k and p n-k ,p n-k+1 ,...,p n , the B-spline curve control point p k Add to As the root node c k (1) = P k T M k T b(1) added to As the root node and At this time, it is an empty set;

[0030] 4.3) In free space Random sampling gets the sampling point p rand ;

[0031] 4.4) Traversal Take the node with the smallest metric function as and confirm The parent node of

[0032] The parent node The formula is as follows:

[0033]

[0034] where ||·|| represents the bi-norm, is a constant greater than zero, cos -1 (·) is the arccosine function, For drones speed at 1000 s;

[0035] 4.5) Determine and exist The corresponding node in and

[0036] 4.6) Solution and p rand The middle node between as well as The corresponding node

[0037] Since the root node There is no parent node. season when hour, The calculation is as follows:

[0038]

[0039] in is the maximum expansion step, ||·|| represents the two-norm, and the known edge The corresponding B-spline curve control points are make get

[0040] 4.7) Determine the edge and edge Is it true? If both are true, the node and edge Add to In and edge Add to Otherwise, return to step 4.3);

[0041] 4.8) Calculation The sub-nodes of the area A satisfy the UAV dynamics constraints;

[0042] The derivative of the k-order B-spline curve is the k-1-order B-spline curve, and the control point q of the B-spline curve of the derivative is i It is obtained by the following formula:

[0043]

[0044] The control points of the B-spline curve are known to be {p i-k ,p i-k+1 ,...p i-1}, then the B-spline curve control point p i The following conditions must be met:

[0045]

[0046]

[0047] 1 of them 3 represents a three-dimensional column vector whose elements are all 1,

[0048] 4.9) Determine in area A Child nodes of calculate exist The corresponding node in

[0049] Random sampling is performed in area A to obtain a set of sampling points Traversal The sampling points in , select the sampling points that make the following cost function take the minimum value as

[0050]

[0051] where b(s) = [s 0 ,s 1 ,...,s k ] T , k is the degree of the B-spline curve, is the control point matrix of the B-spline curve, ρ is a constant greater than zero;

[0052] 4.10) Determine the edge and edge Is it true? If both are true, the node and edge Add to In and edge Add to Otherwise, return to step 4.9);

[0053] 4.11) Judgment Node With the B-spline curve control point p n-k Is the distance less than the threshold? If yes, go to step 4.12), otherwise return to step 4.3);

[0054] like With the B-spline curve control point p n-k Distance less than threshold The figure shows There is a path from its root node to p n-k The geometric path of all nodes on the path together with the B-spline curve control point p 0 ,p 1 ,...,p k-1 ,p n-k ,p n-k+1 ,...,p n Together we determine a path from the drone’s starting state x 0 To the target state x f The trajectory of With the B-spline curve control point p n-k Distance greater than threshold The search process needs to be repeated until the condition is met and the threshold is a constant greater than zero, with p n-k As the center of the ball, A sphere of radius should be completely within to ensure that the resulting trajectory is always safe;

[0055] 4.12) Output the B-spline curve control point p 0 ,p 1 ,...,p n-1 ,p n .

[0056] The present invention provides a motion planning system that considers the dynamic constraints of a quad-rotor drone, comprising a module for establishing a motion trajectory model of a quad-rotor drone, a module for establishing a trajectory planning problem model of a quad-rotor drone, a parameterization module, a search and screening module, and an output module;

[0057] A quad-rotor drone motion trajectory model module is established to establish a quad-rotor drone motion trajectory model based on the quad-rotor drone motion trajectory. The quad-rotor drone motion trajectory model formula is as follows:

[0058]

[0059] in, represents the system state variable, represents the configuration space, is the first-order derivative of x(t), represents the system input, represents the set of permissible input controls, represents the position of the quadcopter in the inertial coordinate system, x, y, z represent the coordinate axes in the inertial coordinate system, is the first-order derivative of p(t), is the second-order derivative of p(t), is a one-dimensional polynomial; represents the system matrix, represents the input matrix, 0 3 represents the third-order zero matrix, I 3 represents the third-order identity matrix;

[0060] A quad-rotor UAV trajectory planning problem model module is established to establish a quad-rotor UAV trajectory planning problem model based on the quad-rotor UAV motion trajectory model;

[0061] It is known that quadcopters are 0 The state x(t 0 )=x 0 and in t f The state x(t f )=x f ,The trajectory planning problem model of the quadrotor drone is as follows:

[0062]

[0063]

[0064] x(t 0 )=x 0 ,x(t f )=x f

[0065]

[0066]

[0067] in Represents free space, represents the obstacle area, v max represents the maximum speed of the quadrotor drone along each axis of the inertial coordinate system, a max Represents the maximum acceleration of the quadrotor drone along each axis of the inertial coordinate system; For p μ The first derivative of (t), For pμ The second derivative of (t);

[0068] The parameterization module is used to parameterize the motion trajectory of the quadrotor drone using the B-spline curve to obtain the B-spline curve control points corresponding to the motion trajectory;

[0069] By m+1 nodes {u 0 ,u 1 ,...,u m}, n+1 control points {p 0 ,p 1 ,...,p n The k-order uniform B-spline curve determined by} is defined as follows:

[0070]

[0071] Where N i,k (u) is the B-spline curve basis function, is the i-th B-spline curve control point, The number of nodes, the number of control points and the degree of the B-spline curve satisfy m=n+k+1; is a constant greater than zero;

[0072] Search and screening module designs search algorithms to search and select B-spline curve control points that meet dynamic constraints;

[0073] The output module is used to solve the position of the quadrotor drone corresponding to the B-spline curve. The position of the quadrotor drone is the final quadrotor drone trajectory, and the final quadrotor drone motion trajectory is output to complete the trajectory planning problem.

[0074] Furthermore, the search and screening module includes a definition unit, an initialization unit, a random sampling unit, a traversal unit, a node determination unit, a corresponding node solution unit, a first judgment unit, a constraint calculation unit, a constraint-internal node calculation unit, a second judgment unit, a threshold comparison unit, and an output unit;

[0075] Define the unit to define the graph Construct the search tree and define the graph Constructing the state tree, is the geometric trajectory formed by k+1 B-spline control points, for The corresponding B-spline curve control point set; It is composed of the trajectory segments of the quadrotor drone corresponding to the control points of the B-spline curve. is the starting point or ending point of the corresponding quadrotor drone trajectory segment; Nodes in and Nodes in Correspondingly, i = 0, 1, 2, ..., N;

[0076] The initialization unit is used to initialize the graph and

[0077] If the control points of the B-spline curve are known to be p j-k ,p j-k+1 ,...p j , then in u∈[u j ,u j+1 ) is as follows:

[0078] c(u)=c j (s) = P j T M k T b(s)

[0079] in b is a vector function of s, s is parameterized for u, reflecting the relationship between u and the node interval [u j ,u j+1 ) position, M k is a k+1-order constant matrix uniquely determined by k, and the control point matrix of the B-spline curve is

[0080] Calculate the first and second derivatives of the quadcopter position formula with respect to u, and convert u k =t 0 and u m-k =t f Substitute into the quadrotor drone position formula and its first-order derivative and second-order derivative with respect to u, u k is the kth node in the node vector, u m-k is the mkth node in the node vector, and we get p 0 ,p 1 ,...,p k and p n-k ,p n-k+1 ,...,p n , the B-spline curve control point p k Add to As the root node c k (1) = P k T M k T b(1) added to As the root node and At this time, it is an empty set;

[0081] Random sampling unit is used to Random sampling gets the sampling point p rand ;

[0082] The traversal unit is used to traverse Take the node with the smallest metric function as and confirm The parent node of

[0083] The parent node The formula is as follows:

[0084]

[0085] where ||·|| represents the bi-norm, is a constant greater than zero, cos -1 (·) is the arccosine function, For drones speed at 1000 s;

[0086] Determine the node unit to determine and exist The corresponding node in and

[0087] Solve the corresponding node element to solve and p rand The middle node between as well as The corresponding node

[0088] Since the root node There is no parent node. season when hour, The calculation is as follows:

[0089]

[0090] in is the maximum expansion step, ||·|| represents the two-norm, and the known edge The corresponding B-spline curve control points are make get

[0091] The first judgment unit is used to judge the edge and edge Is it true? If both are true, the node and edge Add to In and edge Add to Otherwise, return a random sampling unit;

[0092] The computational constraint unit is used to calculate The sub-nodes of the area A satisfy the UAV dynamics constraints;

[0093] The derivative of the k-order B-spline curve is the k-1-order B-spline curve, and the control point q of the B-spline curve of the derivative is i It is obtained by the following formula:

[0094]

[0095] The control points of the B-spline curve are known to be {p i-k ,p i-k+1 ,...p i-1}, then the B-spline curve control point p i The following conditions must be met:

[0096]

[0097]

[0098] 1 of them 3 represents a three-dimensional column vector whose elements are all 1,

[0099] The calculation constraint node element is used to determine the Child nodes of calculate exist The corresponding node in

[0100] Random sampling is performed in area A to obtain a set of sampling points Traversal The sampling points in , select the sampling points that make the following cost function take the minimum value as

[0101]

[0102] where b(s) = [s 0 ,s 1 ,...,s k ] T , k is the degree of the B-spline curve, is the control point matrix of the B-spline curve, ρ is a constant greater than zero;

[0103] The secondary judgment unit is used to judge the edge and edge Is it true? If both are true, the node and edge Add to In and edge Add to Otherwise, the node element in the calculation constraint is returned;

[0104] The threshold comparison unit is used to determine the node With the B-spline curve control point p n-k Is the distance less than the threshold? If yes, it enters the output unit, otherwise it returns to the random sampling unit;

[0105] like With the B-spline curve control point p n-k Distance less than threshold The figure shows There is a path from its root node to p n-k The geometric path of all nodes on the path together with the B-spline curve control point p 0 ,p 1 ,...,p k-1 ,p n-k ,p n-k+1 ,...,p n Together we determine a path from the drone’s starting state x 0 To the target state x f The trajectory of With the B-spline curve control point p n-k Distance greater than threshold The search process needs to be repeated until the condition is met and the threshold is a constant greater than zero, with p n-k As the center of the ball, A sphere of radius should be completely within to ensure that the resulting trajectory is always safe;

[0106] The output unit is used to output the B-spline curve control point p 0 ,p 1 ,...,p n-1 ,p n .

[0107] Beneficial effect: Compared with the prior art, the present invention has the remarkable feature of obtaining the optimized motion trajectory by establishing a quadrotor UAV trajectory planning problem model, using B-spline curve parameterization to optimize the motion trajectory, so that the one-dimensional polynomial coefficients are converted into control points with physical significance in space, and at the same time designing a search tree and a state tree to improve the search process, so that the control points corresponding to the B-spline curve meet the dynamic constraints in the region, avoiding the complex steering function design in the traditional method, ensuring the quality of the motion trajectory, and improving the performance of the algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0108] Figure 1 It is a schematic diagram of the process of the present invention;

[0109] Figure 2 It is a schematic diagram of the nodes and the relationship between the nodes in the present invention;

[0110] Figure 3 It is a planning map in a two-dimensional environment in the present invention;

[0111] Figure 4 is the trajectory of the drone after the problem planning of the present invention is completed;

[0112] Figure 5 It is the speed size curve of the quad-rotor drone in the present invention;

[0113] Figure 6 It is the acceleration size curve of the quad-rotor drone in the present invention. DETAILED DESCRIPTION

[0114] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0115] Example 1

[0116] The present invention provides a motion planning method considering the dynamic constraints of a quadrotor drone, please refer to Figure 1 As shown, the following steps are included:

[0117] 1) A quad-rotor drone motion trajectory model is established based on the quad-rotor drone motion trajectory. The quad-rotor drone motion trajectory model is as follows:

[0118]

[0119] in, represents the system state variable, represents the configuration space, is the first-order derivative of x(t), represents the system input, represents the set of permissible input controls, represents the position of the quadcopter in the inertial coordinate system, x, y, z represent the coordinate axes in the inertial coordinate system, is the first-order derivative of p(t), is the second-order derivative of p(t), is a one-dimensional polynomial; represents the system matrix, represents the input matrix, 0 3 represents the third-order zero matrix, I 3 Represents the third-order identity matrix.

[0120] 2) Establish a quadrotor UAV trajectory planning problem model based on the quadrotor UAV motion trajectory model.

[0121] It is known that quadcopters are 0 The state x(t 0 )=x 0 and in t f The state x(t f )=x f In order to solve the optimal motion trajectory in terms of time and control cost, the trajectory planning model formula of the quadrotor drone is as follows:

[0122]

[0123]

[0124] x(t 0 )=x 0 ,x(t f )=x f

[0125]

[0126]

[0127] in Represents free space, represents the obstacle area, v max represents the maximum speed of the quadrotor drone along each axis of the inertial coordinate system, a max Represents the maximum acceleration of the quadrotor drone along each axis of the inertial coordinate system; For p μ The first derivative of (t), For p μ The second derivative of (t).

[0128] Assume that the altitude of the drone is always 0, and the initial state of the drone is x(t 0 ) = [10,10,0,0,0,0]T , the target state x(t f ) = [750,750,0,0,0,0] T , planning start time t 0 =0, the maximum speed of the drone v max =30, maximum acceleration a max =35.

[0129] 3) Use B-spline curve to parameterize the motion trajectory of the quadrotor drone and obtain the B-spline curve control points corresponding to the motion trajectory.

[0130] By m+1 nodes {u 0 ,u 1 ,...,u m}, n+1 control points {p 0 ,p 1 ,...,p n The k-order uniform B-spline curve determined by} is defined as follows:

[0131]

[0132] Where N i,k (u) is the B-spline curve basis function, is the i-th B-spline curve control point, The number of nodes, the number of control points and the degree of the B-spline curve satisfy m=n+k+1; is a constant greater than zero.

[0133] 4) Please refer to Figure 2 and Figure 3 As shown, a search algorithm is designed to search and select B-spline curve control points that meet the dynamic constraints.

[0134] 4.1) Define the graph Construct the search tree and define the graph Constructing the state tree, is the geometric trajectory formed by k+1 B-spline control points, for The corresponding B-spline curve control point set; It is composed of the trajectory segments of the quadrotor drone corresponding to the control points of the B-spline curve. is the starting point or ending point of the corresponding quadrotor drone trajectory segment; Nodes in and Nodes in Corresponding, i=0,1,2,...,N, it is known that and The control point matrix can be determined Ri Substitute into formula (4) to obtain the position of any point on the corresponding trajectory segment, except the root node and Any node in has only one parent node.

[0135] 4.2) Initialize the graph and

[0136] If the control points of the B-spline curve are known to be p j-k ,p j-k+1 ,...p j , then in u∈[u j ,u j+1 ) is as follows:

[0137] c(u)=c j (s) = P j T M k T b(s)(4)

[0138] in b is a vector function of s, s is parameterized for u, and represents u in the node interval [u j ,u j+1 ) position, M k is a k+1-order constant matrix uniquely determined by k, and the control point matrix of the B-spline curve is

[0139] Calculate the first and second derivatives of formula (4) for u, and replace u k =t 0 and u m-k =t f Substitute into the quadrotor drone position formula and its first-order derivative and second-order derivative with respect to u, u k is the kth node in the node vector, u m-k is the mkth node in the node vector, and we get p 0 ,p 1 ,...,p k and p n-k ,p n-k+1 ,...,p n , the B-spline curve control point p k Add to As the root node c k (1) = P k T M k T b(1) added to As the root node and This is an empty set.

[0140] According to the initial state x of the drone 0 Solve for the control point p 0 ,p 1 ,...,p k , according to the drone target state x f Solve for the control point p n-k ,p n-k+1 ,...,p n ; Since the initial and final velocities of the drone are both zero, p 0 =p 1 =p 2 =p 3 =[10,10,0] T , p n-3 =p n-2 =p n-1 =p n =[750,750,0] T .

[0141] 4.3) In free space Random sampling gets the sampling point p rand .

[0142] In order to speed up the search, non-completely random sampling is used here, and the probability that the sampling point is the target point is set to 0.2.

[0143] 4.4) Traversal Take the node with the smallest metric function as and confirm The parent node of

[0144] To ensure the overall smoothness of the trajectory, The choice of considers two factors, one is and p rand distance, and secondly, the drone The velocity direction and p rand The following two parts of the metric function reflect the above two factors respectively. The formula is as follows:

[0145]

[0146] Among them, ||·|| represents the bi-norm, is a constant greater than zero, cos -1 (·) is the arccosine function, For drones The speed of time.

[0147] 4.5) Determine and exist The corresponding node in and

[0148] 4.6) Solution and p rand The middle node between as well as The corresponding node

[0149] Since the root node There is no parent node. season when hour, The calculation is as follows:

[0150]

[0151] in is the maximum expansion step, ||·|| represents the two-norm, and the known edge The corresponding B-spline curve control points are make get

[0152] 4.7) Determine the edge and edge Is it true? If both are true, the node and edge Add to In and edge Add to Otherwise, return to step 4.3).

[0153] 4.8) Calculation The child nodes of satisfy the area A of the UAV dynamics constraints.

[0154] The derivative of the k-order B-spline curve is the k-1-order B-spline curve, and the control point q of the B-spline curve of the derivative is i It is obtained by the following formula:

[0155]

[0156] The control points of the B-spline curve are known to be {p i-k ,p i-k+1 ,...p i-1}, then the B-spline curve control point p i The following conditions must be met:

[0157]

[0158]

[0159] 1 of them 3 Represents a three-dimensional column vector whose elements are all 1. Substituting this into formula (8) and formula (9) can determine area A.

[0160] 4.9) Determine in area A Child nodes of calculate exist The corresponding node in

[0161] Random sampling is performed in area A to obtain a set of sampling points Traversal The sampling points in , select the sampling points that make the following cost function take the minimum value as

[0162]

[0163] Where b(s) = [s 0 ,s 1 ,...,s k ] T , k is the degree of the B-spline curve, is the control point matrix of the B-spline curve, ρ is a constant greater than zero, and determines After that, Substitute into P and use formula (4) to calculate

[0164] 4.10) Determine the edge and edge Is it true? If both are true, the node and edge Add to In and edge Add to Otherwise, return to step 4.9).

[0165] 4.11) Judgment Node With the B-spline curve control point p n-k Is the distance less than the threshold? If yes, go to step 4.12), otherwise return to step 4.3).

[0166] Threshold Choose according to the actual environment. The more chaotic the environment, the better. The smaller, here

[0167] like With the B-spline curve control point p n-k Distance less than threshold The figure shows There is a path from its root node to p n-k The geometric path of all nodes on the path together with the B-spline curve control point p 0 ,p 1 ,...,p k-1 ,p n-k ,p n-k+1 ,...,p n Together we determine a path from the drone’s starting state x 0 To the target state x f The trajectory of With the B-spline curve control point p n-k Distance greater than threshold The search process needs to be repeated until the condition is met and the threshold is a constant greater than zero, with p n-k As the center of the ball, A sphere of radius should be completely within to ensure that the resulting trajectory is always safe.

[0168] 4.12) Output the B-spline curve control point p 0 ,p 1 ,...,p n-1 ,p n .

[0169] 5) Please refer to Figure 4 As shown, the position of the quadrotor drone corresponding to the B-spline curve is solved. The position of the quadrotor drone is the final quadrotor drone trajectory. The final quadrotor drone motion trajectory is output to complete the trajectory planning problem.

[0170] See also Figure 5 and Figure 6 As shown in Figure 2, the speed and acceleration of the drone did not exceed their maximum values, v max is the maximum speed of the quadrotor drone along each axis, a max is the maximum acceleration of the quadrotor drone along each axis. The maximum actual speed and acceleration in the three-dimensional environment are and

[0171] The output unit is used to output the B-spline curve control point p 0 ,p 1 ,...,p n-1 ,pn .

[0172] Example 2

[0173] Corresponding to the motion planning method considering the dynamic constraints of the quad-rotor drone described in Example 1, this embodiment 2 provides a motion planning system considering the dynamic constraints of the quad-rotor drone, see Figure 1 As shown, it includes a module for establishing a quad-rotor UAV motion trajectory model, a module for establishing a quad-rotor UAV trajectory planning problem model, a parameterization module, a search and screening module, and an output module.

[0174] A quad-rotor drone motion trajectory model module is established to use a polynomial to represent the quad-rotor drone motion trajectory. A quad-rotor drone motion trajectory model is established based on the quad-rotor drone motion trajectory. The quad-rotor drone motion trajectory model formula is as follows:

[0175]

[0176] in, represents the system state variable, represents the configuration space, is the first-order derivative of x(t), represents the system input, represents the set of permissible input controls, represents the position of the quadcopter in the inertial coordinate system, x, y, z represent the coordinate axes in the inertial coordinate system, is the first-order derivative of p(t), is the second-order derivative of p(t), is a one-dimensional polynomial; represents the system matrix, represents the input matrix, 0 3 represents the third-order zero matrix, I 3 Represents the third-order identity matrix.

[0177] A quadrotor UAV trajectory planning problem model module is established to establish a quadrotor UAV trajectory planning problem model based on the quadrotor UAV motion trajectory model to obtain an optimized motion trajectory.

[0178] It is known that quadcopters are 0 The state x(t 0 )=x 0 and in t f The state x(t f )=x f , the model formula of the quadrotor UAV trajectory planning problem is as follows:

[0179]

[0180]

[0181] x(t 0 )=x 0 ,x(t f )=x f

[0182]

[0183]

[0184] in Represents free space, represents the obstacle area, v max represents the maximum speed of the quadrotor drone along each axis of the inertial coordinate system, a max Represents the maximum acceleration of the quadrotor drone along each axis of the inertial coordinate system; For p μ The first derivative of (t), For p μ The second derivative of (t).

[0185] Assume that the altitude of the drone is always 0, and the initial state of the drone is x(t 0 ) = [10,10,0,0,0,0] T , the target state x(t f ) = [750,750,0,0,0,0] T , planning start time t 0 =0, the maximum speed of the drone v max =30, maximum acceleration a max =35.

[0186] The parameterization module is used to use the B-spline curve to parameterize and optimize the motion trajectory, and obtain the B-spline curve control points corresponding to the optimized motion trajectory;

[0187] By m+1 nodes {u 0 ,u 1 ,...,u m}, n+1 control points {p 0 ,p 1 ,...,p n The k-order uniform B-spline curve determined by} is defined as follows:

[0188]

[0189] Where N ik (u) is the B-spline curve basis function, is the i-th B-spline curve control point, The number of nodes, the number of control points and the degree of the B-spline curve satisfy m=n+k+1; is a constant greater than zero.

[0190] See also Figure 2 and Figure 3 As shown, the search and screening module designs a search algorithm to search and select B-spline curve control points that meet the dynamic constraints.

[0191] The search and screening module includes a definition unit, an initial unit, a random sampling unit, a traversal unit, a node determination unit, a corresponding node solution unit, a first judgment unit, a constraint calculation unit, a constraint-internal node calculation unit, a second judgment unit, a threshold comparison unit, and an output unit.

[0192] Define the unit to define the graph Construct the search tree and define the graph Constructing the state tree, is the geometric trajectory formed by k+1 B-spline control points, for The corresponding B-spline curve control point set; It is composed of the trajectory segments of the quadrotor drone corresponding to the control points of the B-spline curve. is the starting point or ending point of the corresponding quadrotor drone trajectory segment; Nodes in and Nodes in Corresponding, i=0,1,2,...,N, it is known that and The control point matrix can be determined R i Substitute into formula (4) to obtain the position of any point on the corresponding trajectory segment, except the root node and Any node in has only one parent node.

[0193] The initial unit is used to initialize the graph and

[0194] If the control points of the B-spline curve are known to be p j-k ,p j-k+1 ,...p j , then in u∈[u j ,u j+1 ) is as follows:

[0195] c(u)=c j (s) = P j T Mk T b(s)(4)

[0196] in b is a vector function of s, s is parameterized for u, and represents u in the node interval [u j ,u j+1 ) position, M k is a k+1-order constant matrix uniquely determined by k, and the control point matrix of the B-spline curve is

[0197] Calculate the first and second derivatives of formula (4) for u, and replace u k =t 0 and u m-k =t f Substitute into the quadrotor drone position formula and its first-order derivative and second-order derivative with respect to u, u k is the kth node in the node vector, u m-k is the mkth node in the node vector, and we get p 0 ,p 1 ,...,p k and p n-k ,p n-k+1 ,...,p n , the B-spline curve control point p k Add to As the root node c k (1) = P k T M k T b(1) added to As the root node and This is an empty set.

[0198] According to the initial state x of the drone 0 Solve for the control point p 0 ,p 1 ,...,p k , according to the drone target state x f Solve for the control point p n-k ,p n-k+1 ,...,p n ; Since the initial and final velocities of the drone are both zero, p 0 =p 1 =p 2 =p 3 =[10,10,0] T , p n-3 =p n-2 =pn-1 =p n =[750,750,0] T .

[0199] Random sampling unit is used to Random sampling gets the sampling point p rand .

[0200] In order to speed up the search, non-completely random sampling is used here, and the probability that the sampling point is the target point is set to 0.2.

[0201] The traversal unit is used to traverse Take the node with the smallest metric function as and confirm The parent node of

[0202] To ensure the overall smoothness of the trajectory, The choice of considers two factors, one is and p rand distance, and secondly, the drone The velocity direction and p rand The following two parts of the metric function reflect the above two factors respectively. The formula is as follows:

[0203]

[0204] Among them, ||·|| represents the bi-norm, is a constant greater than zero, cos -1 (·) is the arccosine function, For drones The speed of time.

[0205] Determine the node unit to determine and exist The corresponding node in and

[0206] Solve the corresponding node element to solve and p rand The middle node between as well as The corresponding node

[0207] Since the root node There is no parent node. season when hour, The calculation is as follows:

[0208]

[0209] in is the maximum expansion step, ||·|| represents the two-norm, and the known edge The corresponding B-spline curve control points are make get

[0210] The first judgment unit is used to judge the edge and edge Is it true? If both are true, the node and edge Add to In and edge Add to Otherwise, a random sampling unit is returned.

[0211] The computational constraint unit is used to calculate The child nodes of satisfy the area A of the UAV dynamics constraints.

[0212] The derivative of the k-order B-spline curve is the k-1-order B-spline curve, and the control point q of the B-spline curve of the derivative is i It is obtained by the following formula:

[0213]

[0214] The control points of the B-spline curve are known to be {p i-k ,p i-k+1 ,...p i-1}, then the B-spline curve control point p i The following conditions must be met:

[0215]

[0216]

[0217] 1 of them 3 Represents a three-dimensional column vector whose elements are all 1. Substituting this into formula (8) and formula (9) can determine area A.

[0218] The calculation constraint node element is used to determine the Child nodes of calculate exist The corresponding node in

[0219] Random sampling is performed in area A to obtain a set of sampling points Traversal The sampling points in , select the sampling points that make the following cost function take the minimum value as

[0220]

[0221] where b(s) = [s 0 ,s 1 ,...,s k ] T , k is the degree of the B-spline curve, is the control point matrix of the B-spline curve, ρ is a constant greater than zero, and determines After that, Substitute into P and use formula (4) to calculate

[0222] The secondary judgment unit is used to judge the edge and edge Is it true? If both are true, the node and edge Add to In and edge Add to Otherwise, the node element inside the calculation constraint is returned.

[0223] The threshold comparison unit is used to determine the node With the B-spline curve control point p n-k Is the distance less than the threshold? If yes, it enters the output unit, otherwise it returns to the random sampling unit.

[0224] Threshold Choose according to the actual environment. The more chaotic the environment, the better. The smaller, here

[0225] like With the B-spline curve control point p n-k Distance less than threshold The figure shows There is a path from its root node to p n-k The geometric path of all nodes on the path together with the B-spline curve control point p 0 ,p 1 ,...,p k-1 ,p n-k ,p n-k+1 ,...,p nTogether we determine a path from the drone’s starting state x 0 To the target state x f The trajectory of With the B-spline curve control point p n-k Distance greater than threshold The search process needs to be repeated until the condition is met and the threshold is a constant greater than zero, with p n-k As the center of the ball, A sphere of radius should be completely within to ensure that the resulting trajectory is always safe.

[0226] The output unit is used to output the B-spline curve control point p 0 ,p 1 ,...,p n-1 ,p n .

[0227] See also Figure 4 As shown, the output module is used to solve the position of the quadrotor drone corresponding to the B-spline curve. The position of the quadrotor drone is the final quadrotor drone trajectory, and the final quadrotor drone motion trajectory is output to complete the trajectory planning problem.

[0228] See also Figure 5 and Figure 6 As shown in Figure 2, the speed and acceleration of the drone did not exceed their maximum values, v max is the maximum speed of the quadrotor drone along each axis, a max is the maximum acceleration of the quadrotor drone along each axis. The maximum actual speed and acceleration are and

Claims

1. A motion planning method considering the dynamic constraints of a quadrotor drone, It is characterized in that The following steps are involved: 1) Establish a quad-rotor UAV motion trajectory model based on the quad-rotor UAV motion trajectory; The motion trajectory model formula of the quadrotor drone is as follows: in, represents the system state variable, represents the configuration space, is the first-order derivative of x(t), represents the system input, represents the set of permissible input controls, represents the position of the quadcopter in the inertial coordinate system, x, y, z represent the coordinate axes in the inertial coordinate system, is the first-order derivative of p(t), is the second-order derivative of p(t), is a one-dimensional polynomial; represents the system matrix, represents the input matrix, 0 3 represents the third-order zero matrix, I 3 represents the third-order identity matrix; 2) Establish a quadrotor UAV trajectory planning problem model based on the quadrotor UAV motion trajectory model; It is known that quadcopters are 0 The state x(t 0 )=x 0 and in t f The state x(t f )=x f ,The model formula of the quadrotor UAV trajectory planning problem is as follows: x(t 0 ) = x 0 , x(t f ) = x f in Represents free space, represents the obstacle area, v max represents the maximum speed of the quadrotor drone along each axis of the inertial coordinate system, a max Represents the maximum acceleration of the quadrotor drone along each axis of the inertial coordinate system; For p μ The first derivative of (t), For p μ The second derivative of (t); 3) Use B-spline curve to parameterize the motion trajectory of the quadrotor drone and obtain the B-spline curve control points corresponding to the motion trajectory; By m+1 nodes {u 0 ,u 1 ,...,u m }, n+1 control points {p 0 ,p 1 ,...,p n The k-order uniform B-spline curve determined by} is defined as follows: Where N i,k (u) is the B-spline curve basis function, is the i-th B-spline curve control point, The number of nodes, the number of control points and the degree of the B-spline curve satisfy m=n+k+1; u 0 ≥0, is a constant greater than zero; 4) Designing a search algorithm to search and select B-spline curve control points that meet the dynamic constraints; including the following sub-steps: 4.1) Define the graph Construct the search tree and define the graph Constructing the state tree, is the geometric trajectory formed by k+1 B-spline control points, for The corresponding B-spline curve control point set; is the quadrotor UAV trajectory segment corresponding to the B-spline curve control point, are the starting and ending points of the corresponding quadrotor drone trajectory segment; Nodes in and Nodes in Correspondingly, i = 0, 1, 2, ..., N; 4.2) Initialize the graph and If the control points of the B-spline curve are known to be p j-k ,p j-k+1 ,...p j , then in u∈[u j ,u j+1 ) is as follows: c(u)=c j (s)=P j T M k T b(s) in b is a vector function of s, s is parameterized for u, reflecting the relationship between u and the node interval [u j ,u j+1 ) position, M k is a k+1-order constant matrix uniquely determined by k, and the control point matrix of the B-spline curve is Calculate the first and second derivatives of the quadcopter position formula with respect to u, and convert u k =t 0 and u m-k =t f Substitute into the quadrotor drone position formula and its first-order derivative and second-order derivative with respect to u, u k is the kth node in the node vector, u m-k is the mkth node in the node vector, and we get p 0 ,p 1 ,...,p k and p n-k ,p n-k+1 ,...,p n , the B-spline curve control point p k Add to As the root node c k (1) = P k T M k T b(1) added to As the root node and At this time, it is an empty set; 4.3) In free space Random sampling gets the sampling point p rand ; 4.4) Traversal Take the node with the smallest metric function as and confirm The parent node of The parent node The formula is as follows: where ||·|| represents the bi-norm, is a constant greater than zero, cos -1 (·) is the arccosine function, For drones speed at 1000 s; 4.5) Determine and exist The corresponding node in and 4.6) Solution and p rand The middle node between as well as The corresponding node Since the root node There is no parent node. season when hour, The calculation is as follows: in is the maximum expansion step, ||·|| represents the two-norm, and the known edge The corresponding B-spline curve control points are make get 4.7) Determine the edge and edge Is it true? If both are true, the node and edge Add to In and edge Add to Otherwise, return to step 4.3); 4.8) Calculation The sub-nodes of the area A satisfy the UAV dynamics constraints; The derivative of the k-order B-spline curve is the k-1-order B-spline curve, and the control point q of the B-spline curve of the derivative is i It is obtained by the following formula: Given that the control points of the B-spline curve are {p i-k , p i-k+1 ,... p i-1}, the control points p i of the B-spline curve need to satisfy the following conditions: 1 of them 3 represents a three-dimensional column vector whose elements are all 1, 4.9) Determine in area A Child nodes of calculate exist The corresponding node in Random sampling is performed in area A to obtain a set of sampling points Traversal The sampling points in , select the sampling points that make the following cost function take the minimum value as in k is the degree of the B-spline curve, is the control point matrix of the B-spline curve, ρ is a constant greater than zero; 4.10) Determine the edge and edge Is it true? If both are true, the node and edge Add to In and edge Add to Otherwise, return to step 4.9); 4.11) Judgment Node With the B-spline curve control point p n-k Is the distance less than the threshold? If yes, go to step 4.12), otherwise return to step 4.3); If The distance to the B-spline curve control point p n-k is less than the threshold Then it indicates that in the figure There exists a geometric path from its root node to p n-k On this path, all the nodes together with the B-spline curve control point p 0 , p 1 ,..., p k-1 , p n-k , p n-k+1 ,..., p n Together determine a trajectory from the initial state x of the UAV 0 to the target state x f If The distance to the B-spline curve control point p n-k is greater than the threshold Then the search process needs to be repeated until this condition is met. The threshold is a constant greater than zero. With p n-k as the center of the sphere and with as the radius, the sphere should be completely within to ensure that the resulting trajectory is always safe; 4.12) Output the B-spline curve control point p 0 ,p 1 ,...,p n-1 ,p n ; 5) Solve the position of the quadrotor drone corresponding to the control point of the B-spline curve. The position of the quadrotor drone is the final quadrotor drone trajectory. Output the final quadrotor drone motion trajectory to complete the trajectory planning problem.

2. A motion planning system considering the dynamic constraints of a quadrotor drone, It is characterized in that It includes a module for establishing a quad-rotor UAV motion trajectory model, a module for establishing a quad-rotor UAV trajectory planning problem model, a parameterization module, a search and screening module, and an output module; A quad-rotor drone motion trajectory model module is established to establish a quad-rotor drone motion trajectory model based on the quad-rotor drone motion trajectory. The quad-rotor drone motion trajectory model formula is as follows: in, represents the system state variable, represents the configuration space, is the first-order derivative of x(t), represents the system input, represents the set of permissible input controls, represents the position of the quadcopter in the inertial coordinate system, x, y, z represent the coordinate axes in the inertial coordinate system, is the first-order derivative of p(t), is the second-order derivative of p(t), is a one-dimensional polynomial; represents the system matrix, represents the input matrix, 0 3 represents the third-order zero matrix, I 3 represents the third-order identity matrix; A quad-rotor UAV trajectory planning problem model module is established to establish a quad-rotor UAV trajectory planning problem model based on the quad-rotor UAV motion trajectory model; It is known that quadcopters are 0 The state x(t 0 )=x 0 and in t f The state x(t f )=x f ,The trajectory planning problem model of the quadrotor drone is as follows: x(t 0 )=x 0 ,x(t f )=x f in Represents free space, represents the obstacle area, v max represents the maximum speed of the quadrotor drone along each axis of the inertial coordinate system, a max Represents the maximum acceleration of the quadrotor drone along each axis of the inertial coordinate system; For p μ The first derivative of (t), For p μ The second derivative of (t); The parameterization module is used to parameterize the motion trajectory of the quadrotor drone using the B-spline curve to obtain the B-spline curve control points corresponding to the motion trajectory; By m+1 nodes {u 0 ,u 1 ,...,u m }, n+1 control points {p 0 ,p 1 ,...,p n The k-order uniform B-spline curve determined by} is defined as follows: Where N i , k(u) is the B-spline curve basis function, is the i-th B-spline curve control point, The number of nodes, the number of control points and the degree of the B-spline curve satisfy m=n+k+1; u 0 ≥0, is a constant greater than zero; The search and screening module designs a search algorithm to search and select the B-spline curve control points that meet the dynamic constraints; the search and screening module includes a definition unit, an initialization unit, a random sampling unit, a traversal unit, a node determination unit, a corresponding node solution unit, a first judgment unit, a constraint calculation unit, a constraint internal node calculation unit, a second judgment unit, a threshold comparison unit, and an output unit; Define the unit to define the graph Construct the search tree and define the graph Constructing the state tree, is the geometric trajectory formed by k+1 B-spline control points, for The corresponding B-spline curve control point set; It is composed of the trajectory segments of the quadrotor drone corresponding to the control points of the B-spline curve. is the starting point or ending point of the corresponding quadrotor drone trajectory segment; Nodes in and Nodes in Correspondingly, i = 0, 1, 2, ..., N; The initialization unit is used to initialize the graph and If the control points of the B-spline curve are known to be p j -k,p j -k+1,...p j , then in u∈[u j ,u j+1 ) is as follows: c(u)=c j (s)=P j T M k T b(s) in b is a vector function of s, s is parameterized for u, reflecting the relationship between u and the node interval [u j ,u j+1 ) position, M k is a k+1-order constant matrix uniquely determined by k, and the control point matrix of the B-spline curve is Calculate the first and second derivatives of the quadcopter position formula with respect to u, and convert u k =t 0 and u m-k =t f Substitute into the quadrotor drone position formula and its first-order derivative and second-order derivative with respect to u, u k is the kth node in the node vector, u m-k is the mkth node in the node vector, and we get p 0 ,p 1 ,...,p k and p n-k ,p n-k+1 ,...,p n , the B-spline curve control point p k Add to As the root node c k (1) = P k T M k T b(1) added to As the root node and At this time, it is an empty set; Random sampling unit is used to Random sampling gets the sampling point p rand ; The traversal unit is used to traverse Take the node with the smallest metric function as and confirm The parent node of The parent node The formula is as follows: where ||·|| represents the bi-norm, is a constant greater than zero, cos -1 (·) is the arccosine function, For drones speed at 1000 s; Determine the node unit to determine and exist The corresponding node in and Solve the corresponding node element to solve and p rand The middle node between as well as The corresponding node Since the root node There is no parent node. season when hour, The calculation is as follows: in is the maximum expansion step, ||·|| represents the two-norm, and the known edge The corresponding B-spline curve control points are make get The first judgment unit is used to judge the edge and edge Is it true? If both are true, the node and edge Add to In and edge Add to Otherwise, return a random sampling unit; The computational constraint unit is used to calculate The sub-nodes of the area A satisfy the UAV dynamics constraints; The derivative of the k-order B-spline curve is the k-1-order B-spline curve, and the control point q of the B-spline curve of the derivative is i It is obtained by the following formula: The control points of the B-spline curve are known to be {p i-k ,p i-k+1 ,...p i-1 }, then the B-spline curve control point p i The following conditions must be met: 1 of them 3 represents a three-dimensional column vector whose elements are all 1, The calculation constraint node element is used to determine the Child nodes of calculate exist The corresponding node in Random sampling is performed in area A to obtain a set of sampling points Traversal The sampling points in , select the sampling points that make the following cost function take the minimum value as in k is the degree of the B-spline curve, is the control point matrix of the B-spline curve, ρ is a constant greater than zero; The secondary judgment unit is used to judge the edge and edge Is it true? If both are true, the node and edge Add to In and edge Add to Otherwise, the node element in the calculation constraint is returned; The threshold comparison unit is used to determine the node With the B-spline curve control point p n-k Is the distance less than the threshold? If yes, it enters the output unit, otherwise it returns to the random sampling unit; like With the B-spline curve control point p n-k Distance less than threshold The figure shows There is a path from its root node to p n-k The geometric path of all nodes on the path together with the B-spline curve control point p 0 ,p 1 ,...,p k-1 ,p n-k ,p n-k+1 ,...,p n Together we determine a path from the drone’s starting state x 0 To the target state x f The trajectory of With the B-spline curve control point p n-k Distance greater than threshold The search process needs to be repeated until the condition is met and the threshold is a constant greater than zero, with p n-k As the center of the ball, A sphere of radius should be completely within to ensure that the resulting trajectory is always safe; The output unit is used to output the B-spline curve control point p 0 ,p 1 ,...,p n-1 ,p n ; The output module is used to solve the position of the quadrotor drone corresponding to the B-spline curve. The position of the quadrotor drone is the final quadrotor drone trajectory, and the final quadrotor drone motion trajectory is output to complete the trajectory planning problem.

3. A computer device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, It is characterized in that When the processor executes the computer program, the steps of the method according to claim 1 are implemented.

4. A computer-readable storage medium having a computer program stored thereon, It is characterized in that When the computer program is executed by a processor, the steps of the method according to claim 1 are implemented.

Images