A motion planning method for quadrotor drone based on radiation field

Abstract

The present invention belongs to the technical field of UAV motion planning, and specifically relates to a motion planning method for a four-rotor UAV based on a radiation field. The method of the present invention can enable the UAV to have an environmental perception function with adaptive learning capabilities, and apply the 3D Gaussian Splatting method to generate a more detailed map with richer information, so that the UAV can better obtain environmental information in a complex environment. Moreover, the path planning algorithm of the present invention uses the heuristic function designed by the expansion method of the state equation and the minimum principle to improve the accuracy and quality of the path search, further improve the optimality of the search path, and output a path that meets the dynamic requirements to reduce the burden of trajectory optimization. Furthermore, the trajectory planning of the present invention samples B-spline curves for a concise cost design, fully utilizes the obstacle occupancy and gradient information of the map, and designs a time reallocation strategy to ensure the executable nature of the trajectory. Based on the above design, the environmental adaptability, reliability and robustness of the UAV motion planning are improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of UAV motion planning, and in particular relates to a motion planning method for a quad-rotor UAV based on radiation field. Background Art

[0002] With the continuous development of drone technology, its application in various fields is becoming increasingly widespread. The advantages of drones in flight capability and maneuverability have greatly expanded the scope of human operations. As their application deepens, people are also demanding more advanced capabilities from drones. For example, in indoor exploration, battlefield collaboration, and environmental scanning, drones are required to possess functions such as autonomous planning and exploration, target identification and tracking, and environmental scanning and recording. All of these high-level tasks require motion planning technology.

[0003] Existing drone motion planning algorithms are generally designed with a known environment as a prerequisite. This results in a low success rate when applied to complex, unknown environments. This is due to, firstly, the poor quality of the initial values provided by the path planning module of these existing algorithms, which prevents the subsequent trajectory planning module from optimizing them. Secondly, these trajectory planning modules fail to consider real-time optimization during flight, resulting in poor on-the-fly calculation capabilities.

[0004] Environmental representation is a prerequisite for motion planning. Existing methods generally employ explicit map representations, such as mesh, voxel, and raster maps. These methods are often used to build offline maps. Subsequent real-time calls can lead to discrepancies between perception and the map, resulting in poor adaptability and learning. Furthermore, real-time mapping using these methods requires significant computational resources, hindering real-time motion planning.

[0005] In summary, in complex and unknown environments, the existing UAV motion planning algorithm has problems such as long mapping time, poor environmental adaptability, and low real-time planning safety performance. In order to further improve the operational capabilities of UAVs, it is very necessary to solve the above problems. Summary of the Invention

[0006] To address the aforementioned technical issues, the present invention provides a radiation field-based motion planning method for a quadrotor drone. This method utilizes a 3D Gaussian Splatting method to create a detailed implicit representation of the environmental map, improving adaptive learning capabilities for diverse environments and providing rich environmental information for motion planning calculations. This method also accelerates the computational speed of drone motion planning and ensures the real-time security of the algorithm output. This method, in turn, ensures the success rate of low-level drone motion planning tasks in complex environments.

[0007] To achieve the above object, the present invention is implemented according to the following technical solutions:

[0008] A motion planning method for a quadrotor drone based on radiation field, comprising the following steps:

[0009] S1 uses the camera on the drone to image environmental obstacles and obtain camera images. The drone is then positioned in real time based on the obtained camera images and camera parameters. The environment map is then created in real time using the 3D Gaussian Splatting method based on the camera images, camera parameters, and the real-time position of the drone.

[0010] S2, starting from the current position of the UAV, searches for a safe and smooth initial path connecting the starting point and the target point in the environment map obtained in step S1;

[0011] S3, performing real-time optimization on the initial path obtained in step S2 to obtain an optimized flight trajectory;

[0012] S4. The speed and acceleration of the UAV in the optimized flight trajectory obtained in step S3 are judged in real time. When the speed or acceleration of the UAV exceeds a set threshold, a time reallocation strategy is implemented for the speed or acceleration that exceeds the executable speed and acceleration of the UAV to extend the flight time and ensure that the UAV can complete the flight mission on time. When the speed and acceleration of the UAV do not exceed the set threshold, the optimized flight trajectory is flown until the target point is reached.

[0013] In step S1, the camera parameters are the camera intrinsic parameters used in image projection;

[0014] The image information is the environment picture taken by the camera in real time;

[0015] The real-time positioning of the aircraft body uses the visual inertial odometry method, which uses the onboard inertial devices and visual information to perform self-positioning of the UAV body;

[0016] The specific process of using the 3D Gaussian Splatting method to build an environment map in real time is as follows:

[0017] Step 1: Extract corner features and edge features from the camera image to obtain point cloud data in three-dimensional space. Based on the real-time position information of the UAV, the point cloud data is converted into the navigation coordinate system of the UAV.

[0018] Step 2: Grid the mission space (hereinafter referred to as the mission space) in the UAV navigation coordinate system. Set the grid range to the size of the mission space to ensure that the grid covers the entire mission space. Set the grid resolution to one-tenth of the diameter of the aircraft or less to ensure the fineness of the map. Divide this space into grid cells of equal size, each grid cell representing a fixed area in the environment.

[0019] Step 3: For each grid cell, traverse the point cloud data within the cell and weight each point using the Gaussian function. The weight of each point is equal to the ratio of the distance from the point to the center of the grid cell to the sum of the distances from all points to the center of the grid cell. Calculate the weighted average of the position coordinates of all points in the grid as the position of the obstacle represented by the grid cell. Traverse all grid cells and combine the calculated obstacle positions into a complete 3D environment map. The Gaussian function formula is:

[0020]

[0021] where d x is the distance from the point to the center of the grid cell in the x-axis direction, d y is the distance from the point to the center of the grid cell in the y-axis direction, d z is the distance from the point to the center of the grid cell in the z-axis direction, and σ is the standard deviation of the Gaussian function.

[0022] In step S2, the specific process of obtaining the initial path is:

[0023] Step (1), add the starting point to the set of nodes to be processed;

[0024] Step (2), determining whether there are still nodes to be processed in the set of nodes to be processed;

[0025] If there are still nodes to be processed in the set of nodes to be processed, the point with the lowest cost is taken as the current node, and it is determined whether the current node is within the distance threshold range centered on the target point. If so, step (6) is executed; otherwise, step (3) is executed.

[0026] If there is no node to be processed in the set of nodes to be processed, the path search is stopped and no feasible path is found;

[0027] Step (3): Using the current state as the initial value, using the state space equation, for a given discrete control input, obtain the extended node that conforms to the dynamics according to the state transfer equation, and store the node in the extended point set of the current node.

[0028] Step (4) calculates the current cost of each candidate node in the extension point set. Determine whether the candidate node already exists in the set of nodes to be processed and can be reached;

[0029] If it is unreachable, skip this candidate node.

[0030] If the candidate node already exists in the set of nodes to be processed and can be reached, the cost of reaching the candidate node from the current node in step (2) is calculated and compared with the cost of the candidate node that already exists in the set of nodes to be processed and represents the same position. The candidate node with the lower current cost is retained in the set of nodes to be processed.

[0031] If the node to be selected is not in the set of nodes to be processed and can be reached, directly calculate its current cost.

[0032] Step (5) uses the Pontryagin minimum principle to solve the optimal control problem to construct a heuristic function, calculates the heuristic cost of each candidate node in the expansion point set, and then returns to step (2);

[0033] Step (6): output the initial path obtained by path planning.

[0034] In step S3, the specific process of trajectory optimization is as follows:

[0035] Step 1: Use B-spline curve to fit the initial path;

[0036] Step 2: Use control points to construct a nonlinear optimization problem that takes into account smoothness, collision safety, and dynamic feasibility. The optimization problem is in the form of:

[0037]

[0038] where f total is the total cost, f s ,f c ,f v ,f a They represent the feasibility cost of smoothness, collision safety, and dynamics (speed, acceleration). k ,Q k+1 ,…,Q n-k is the control point. λ1,λ2,λ3 are the proportions of smoothness, collision safety, and dynamic feasibility costs, λ1+λ2+λ3=1.

[0039] The smoothness cost is in the form of:

[0040]

[0041] where Q i-1 ,Q i ,Q i+1 For the control point.

[0042] The collision safety cost is in the form of:

[0043]

[0044] Where d(Q i ) is the control point Q i Distance to the nearest obstacle, d thr The safety distance threshold is set.

[0045] The feasibility cost of dynamics (speed and acceleration) is:

[0046]

[0047] where v max ,a max The maximum speed and acceleration are limited.

[0048] Step 3: Use a nonlinear optimization solver to solve the nonlinear optimization problem constructed in step 2.

[0049] Step 4: Determine whether the optimized trajectory intersects with the obstacle.

[0050] If the trajectory does not intersect with the obstacle, the trajectory planning outputs a feasible trajectory.

[0051] If the trajectory intersects with an obstacle, step S2 is executed to re-plan the motion.

[0052] In step S4, the specific process of the time reallocation strategy is:

[0053] Step (1) determines whether the speed and acceleration parameters in the feasible trajectory exceed the dynamic limit, and stores the parameters exceeding the dynamic limit in the overspeed set V over With super-accelerated set A over middle.

[0054] Step (2): Calculate the over-limit ratio of the parameters in the overspeed and over-acceleration set, u v =v m / v max with u a =a m / a max . Where u v ,u a is the excess ratio, v m ,a m is the maximum velocity and acceleration in the transfinite set, v max ,a max The maximum speed and acceleration are limited.

[0055] Step (3): using the over-limit ratio parameter, amplify the length of the interval corresponding to the over-limit parameter, and map the maximum speed or acceleration of the corresponding interval to the limited maximum speed and acceleration.

[0056] Step (4): Check whether there are still parameters in the trajectory that exceed the dynamic limits.

[0057] If it still exists, go to step (1);

[0058] If it does not exist, output the optimized flight trajectory.

[0059] Compared with the prior art, the present invention has the following beneficial effects:

[0060] The method of the present invention can enable the UAV to have an environmental perception function with adaptive learning capabilities, and use the 3D Gaussian Splatting method to generate a more detailed map with richer information, so that the UAV can better obtain environmental information in a complex environment. Moreover, the path planning algorithm of the present invention uses the heuristic function designed by the expansion method of the state equation and the minimum principle to improve the accuracy and quality of the path search, further improve the optimality of the search path, and output a path that meets the dynamic requirements to reduce the burden of trajectory optimization. Furthermore, the trajectory planning of the present invention samples the B-spline curve for a simple cost design, fully utilizes the obstacle occupancy and gradient information of the map, and designs a time reallocation strategy to ensure the executable nature of the trajectory. Based on the above design, the environmental adaptability, reliability and robustness of the UAV motion planning are improved.

[0061] A motion planning method for a quad-rotor drone based on a radiation field belongs to the field of drone motion planning. The present invention solves the problem that existing motion planning algorithms require prior environmental information and insufficient environmental details for the perception function to establish a map. The present invention obtains an environmental point cloud based on the drone's onboard sensors and performs body positioning, and uses the above information and camera correction parameters to establish a 3D Gaussian Splatting fine environment; based on the obstacle occupancy and distance gradient information represented by the Gaussian in the established map, the drone motion planning task is completed in two steps: path planning based on Kinodynamic-A* and trajectory planning based on B-spline curves. The map established by the present invention is represented in a probabilistic form, and contains richer environmental details, which is conducive to the engineering implementation of motion planning; the motion planning algorithm designed by the present invention improves the efficiency of path planning and trajectory planning separately, and at the same time, the path planning and trajectory planning are designed to cooperate, thereby improving the computational real-time performance of the motion planning algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] Figure 1 A system block diagram for the application of a motion planning method for a quadrotor drone based on radiation fields.

[0063] Figure 2 This is an example of the effect of state-space expansion.

[0064] Figure 3 This is the effect of time reallocation on the processing of out-of-limit parameters.

[0065] Figure 4 This is a flow chart of an algorithm for motion planning of a quadrotor drone based on radiation field.

[0066] Figure 5 This is the drone platform built in the embodiment.

[0067] Figure 6 Demonstrates the effect of drone motion planning in unknown environments. DETAILED DESCRIPTION

[0068] In order to make the technical solutions, implementation methods and advantages of the present invention more clear, refer to Figure 1 This system diagram shows a radiation-field-based motion planning method for a quadrotor drone. Based on this diagram, the proposed method is deployed on a drone platform, and the implementation process is described using a case study. The system diagram consists of a planner, flight controller, drone, mapping, and positioning. The proposed method involves creating an environmental map and designing a planner.

[0069] Example

[0070] The environment map is constructed using a 3D Gaussian Splatting method that uses radiation fields to represent maps. This algorithm uses 3D Gaussian spheres to represent obstacle objects and introduces an anisotropic 3D Gaussian distribution as a high-quality, unstructured radiation field representation. This enables fast, differentiable rendering using the GPU, allows for anisotropic splatting and fast backpropagation, proposes optimization methods for 3D Gaussian properties, and simultaneously performs adaptive density control, improving the frame rate of the rendered map. This allows the radiation field map to be used for real-time motion planning. Furthermore, because the rendered map contains more detailed map information, it is more conducive to better motion planning and the implementation of higher-level tasks.

[0071] This part requires input of camera images, positioning information and environmental point clouds.

[0072] In order to obtain camera images, it is necessary to install a camera on the drone platform that can capture obstacles in the environment and provide a camera that can process 2D images. In this embodiment, the RealSense D455 camera is used. The camera can provide left and right camera images through the left and right cameras, which is more conducive to the acquisition of environmental information. The camera can also provide inertial IMU information, which can be used for subsequent positioning work.

[0073] In order to obtain positioning information, it is necessary to select positioning-related algorithms. In this embodiment, the visual inertial odometry VINS-Fusion method is used to achieve body positioning. This method requires the single / binocular images of the drone and the inertial IMU information, which can be provided by the RealSense D455 camera. In order to use the above information for high-precision and reliable positioning, it is necessary to write a startup configuration file for the algorithm. The configuration file contains the camera's internal parameters, the transformation matrix between the left and right cameras, and the transformation matrix between the left and right cameras and the IMU respectively. These configuration parameters can be obtained by querying the user manual of the camera device, or by calibrating with a camera calibration tool such as Kalibr. During the positioning process, the VINS-Fusion method will generate an environmental point cloud, which can be used as input for establishing the environmental map.

[0074] After completing the acquisition of the above input information, follow the steps below to create an environment map:

[0075] Step 1: Start the RealSense D455 camera and acquire a camera image. Extract corner and edge features from the camera image to obtain point cloud data in 3D space. Based on the drone's real-time position information, convert this point cloud data into the drone's navigation coordinate system.

[0076] Step 2: Grid the mission space (hereafter referred to as the mission space) in the drone's navigation coordinate system. Set the grid's range to the size of the mission space to ensure that the grid covers the entire mission space. Set the grid's resolution to one-tenth of the drone's diameter or less to ensure the map's precision. Divide this space into equal-sized grid cells, each representing a fixed area in the environment. In this example, a resolution of 0.1m is used, and the range corresponds to the operating area, which is 10×10×3m.

[0077] Step 3: For each grid cell, traverse the point cloud data within the cell and weight each point using the Gaussian function. The weight of each point is equal to the ratio of the distance from the point to the center of the grid cell to the sum of the distances from all points to the center of the grid cell. Calculate the weighted average of the position coordinates of all points in the grid as the position of the obstacle represented by the grid cell. Traverse all grid cells and combine the calculated obstacle positions into a complete 3D environment map. The Gaussian function formula is:

[0078]

[0079] where d x is the distance from the point to the center of the grid cell in the x-axis direction, d y is the distance from the point to the center of the grid cell in the y-axis direction, d z is the distance from the point to the center of the grid cell in the z-axis direction, and σ is the standard deviation of the Gaussian function.

[0080] For the established 3D environment map, the motion planning algorithm needs to use the distance to obstacles and distance gradient information in the map. For any position in the map (query point), the Euclidean distance to the nearest obstacle (i.e. the center point of the nearest grid cell) is calculated to represent the obstacle distance:

[0081]

[0082] Among them, x p ,y p ,z p Is the position of the query point on the x, y, z axis, x o ,y o ,z o is the position of the nearest obstacle on the x, y, and z axes.

[0083] Calculate the gradient of the distance field to represent the rate of change of distance with spatial position. Use numerical differentiation methods (such as central difference method) to calculate the partial derivative of the distance function in each dimension to approximate the gradient value. For example, the gradient in the x-axis direction is:

[0084]

[0085] Here, Δx is a small position increment in the x-axis direction.

[0086] Planner design part. This part mainly includes:

[0087] The Kinodynamic-A* algorithm is used to design a path planning function, modeling the quadrotor's dynamic state space and deriving its state transfer equations. The control input is discretized (the degree of discretization is controllable) to obtain more dynamically reachable points, ensuring the natural dynamic smoothness of the search path and reducing the burden of subsequent optimization. Flight energy consumption is selected as the quality criterion for the path, and an optimal control problem is constructed, represented by an energy-based objective function, the drone's state equation, and boundary conditions for the initial and final states. The Pontryagin minimum principle is used to solve the optimal path, where the optimal path energy is expressed as a heuristic function. This Kinodynamic-A* algorithm, based on the traditional A* algorithm, modifies the node expansion and heuristic function selection methods to meet the A* algorithm's path optimality requirements and ensure the existence and optimality of the solution.

[0088] A trajectory planning algorithm is designed using uniform B-splines. The path obtained by path planning is represented by a uniform B-spline curve, transforming the trajectory optimization problem into an optimization problem of B-spline control points. Leveraging the local control properties, convex hull characteristics, and derivative continuity of B-spline curves, cost functions are designed to address trajectory smoothness, collision safety, and dynamic constraints, expressed in terms of control points. Closed-form Jacobian matrices for each cost are then given to improve the speed of problem solving. Using the weighted sum of each cost as the objective function, an unconstrained nonlinear optimization problem is constructed. This problem is convex, guaranteeing the optimality of the solution. Given the dynamically smooth solution provided by path planning, the closed-form Jacobian solution of the cost function ensures a fast trajectory planning solution.

[0089] A time redistribution strategy was designed using non-uniform B-splines to scan the trajectory motion parameters generated by trajectory planning. By stretching the overspeed parameter interval, the overspeed parameters were mapped to a feasible parameter range, ensuring the UAV's ability to execute control signals. Because the position control point, smoothness cost, and collision safety cost are independent of interval length, and the dynamic constraint cost is optimized after overspeed parameter mapping, the physical motion relationship of the output results is guaranteed, maintaining optimality.

[0090] Regarding the above three parts, this embodiment is implemented as follows.

[0091] (1) Path planning method based on Kinodynamic-A*

[0092] Step (1), add the starting point to the set of nodes to be processed;

[0093] Step (2), determining whether there are still nodes to be processed in the set of nodes to be processed;

[0094] If there are still nodes to be processed in the set of nodes to be processed, the point with the lowest cost is taken as the current node, and it is determined whether the current node is within the distance threshold range centered on the target point. If so, step (6) is executed; otherwise, step (3) is executed.

[0095] If there is no node to be processed in the set of nodes to be processed, the path search is stopped and no feasible path is found;

[0096] Step (3): Using the current state as the initial value, using the state space equation, for a given discrete control input, obtain the extended node that conforms to the dynamics according to the state transfer equation, and store the node in the extended point set of the current node. The state space equation is

[0097]

[0098] The state transfer equation is:

[0099]

[0100] In the above two equations, x is the state of the selected drone, including position and velocity; u is the control acceleration input; I3 is a 3×3 unit diagonal matrix; x(t) is the state of the drone at time t; x(0) is the state of the drone at the current moment; τ is the integral variable of the convolution.

[0101] The control input is acceleration. For a quadcopter, the acceptable acceleration input can be set according to the user manual, such as [-a max , a max ], further As the step length, we can get The 2r+1 acceleration control inputs also give (2r+1) 3 Therefore, the accuracy of the environment search can be improved by adjusting r. Figure 2 , based on the state space expansion effect example. In this embodiment, r = 3, a single expansion of 343 nodes to be selected. In specific implementation, you can adjust the settings according to the computing power and search accuracy requirements of your own platform.

[0102] Step (4) calculates the current cost of each candidate node in the extension point set. Determine whether the candidate node already exists in the set of nodes to be processed and can be reached;

[0103] If it is unreachable, skip this candidate node.

[0104] If the candidate node already exists in the set of pending nodes and is reachable, calculate the cost of reaching the candidate node from the current node in step 2 and compare it with the cost of the candidate node that already exists in the set of pending nodes and represents the same position. The candidate node with the lower current cost is retained in the set of pending nodes.

[0105] If the node to be selected is not in the set of nodes to be processed and can be reached, directly calculate its current cost.

[0106] Step (5), using the Pontryagin minimum principle to solve the optimal control problem to construct a heuristic function, calculate the heuristic cost of each candidate node in the expansion point set, and then return to step (2);

[0107] Taking flight energy as the optimization objective, the optimal control problem is constructed as follows:

[0108]

[0109] stx=[p,υ]x(0)=x c , x(T)=x g ;

[0110] u=a

[0111]

[0112] Where min means minimization; J ∑ is the target energy cost; x is the state of the selected UAV, including position p and velocity v; u is the control acceleration input; u(t) is the control acceleration input at time t; x(0) is the initial state of the UAV; x(T) is the state of the UAV at time T; x c , x g are the initial state and target state of the UAV in the time period T; is the state equation of the UAV; a is the acceleration.

[0113] Construct the Lagrangian function H(x, u, λ):

[0114]

[0115] Where x(t), u(t), and λ(t) represent the state, acceleration control input, and parameters at time t. λ(t) includes λ1(t) and λ2(t).

[0116] In the following solution process, in order to simplify the representation, the time after the variables in the above formula is no longer marked, such as the state is recorded as x.

[0117] Using the canonical equation Then we have:

[0118]

[0119] Desirable:

[0120]

[0121] Among them, α, β are unknown constants, and α=(α x , α y , α z ) T β=(β x , β y , β z ) T .

[0122] Further use the minimum principle By integrating the acceleration and then integrating it again, we can get the velocity and position of the carrier in the state:

[0123]

[0124] Using the boundary condition x(0)=x c , x(T)=x g , the constants α and β in the above formula are:

[0125]

[0126] Where u∈{x,y,z} represents the state index in the three-axis direction, α μ ,β μ is a constant on each axis, p μc ,v μc is the position and velocity of the starting point on each axis, p μg ,v μg is the position and velocity of the target point on each axis.

[0127] The heuristic function result can be obtained as:

[0128]

[0129] where u∈{x,y,z}, α u ,β u is a constant on each axis, J * (T) is the minimum cost.

[0130] Further take the minimum cost value:

[0131]

[0132] Where T h is the interval length corresponding to the optimal energy consumption, h c is the heuristic cost.c As the heuristic cost, the heuristic cost can be calculated for each candidate node in the extension point set.

[0133] Step (6): output the initial path obtained by path planning.

[0134] (2) Trajectory planning method based on uniform B-spline

[0135] Step 1: Use B-spline curve to fit the initial path;

[0136] The mathematical relationship between the position, velocity, and acceleration of the B-spline control points and the spline curve is used to transform the path into a B-spline curve. The corresponding mathematical relationship is:

[0137]

[0138] where Q i , i∈{0, 1, ..., N-2} is the B-spline control point to be found; P j , j∈{0, 1, ..., N-2} is the position of N-1 path nodes; V0, A0 is the velocity and acceleration of the initial node of the path; V N-2 , A N-2 is the velocity and acceleration of the target node of the path; N is the number of nodes in the initial path.

[0139] Based on the above mathematical correspondence, the following linear equations can be constructed to solve the control points. The equations are:

[0140]

[0141] The form of the equation is Ax = b, and N + 3 equations are used to solve N + 2 solutions. The least squares method can be used to solve it, that is,

[0142] x=A T (AA T ) -1 b;

[0143] Step 2: Use control points to construct a nonlinear optimization problem that takes into account smoothness, collision safety, and dynamic feasibility. The optimization problem is in the form of:

[0144]

[0145] where f total is the total cost, f s , f c , f v , f a They represent the feasibility cost of smoothness, collision safety, and dynamics (speed, acceleration). j, j∈{k, ..., nk} is a control point. λ1, λ2, λ3 are the weights of smoothness, collision safety, and dynamic feasibility, λ1+λ2+λ3=1.

[0146] The smoothness cost is in the form of:

[0147]

[0148] where Q i-1 , Q i .Q i+1 For the control point.

[0149] The collision safety cost is in the form of:

[0150]

[0151] Where d(Q i ) is the control point Q i Distance to the nearest obstacle, d thr The safety distance threshold is set.

[0152] The feasibility cost of dynamics (speed and acceleration) is:

[0153]

[0154] where v max , a max The maximum speed and acceleration are limited.

[0155] Step 3: Use a nonlinear optimization solver to solve the nonlinear optimization problem constructed in step 2.

[0156] Step 4: Determine whether the optimized trajectory intersects with the obstacle.

[0157] If the trajectory does not intersect with the obstacle, the trajectory planning outputs a feasible trajectory.

[0158] If the trajectory intersects with an obstacle, step S2 is executed to re-plan the motion.

[0159] (3) Trajectory planning method based on uniform B-spline

[0160] Step 1: Use B-spline curve to fit the path output by path planning.

[0161] Step 2: Use the control points to construct a nonlinear optimization problem that takes into account smoothness, collision safety, and dynamic feasibility. The optimization problem is in the form of:

[0162] Step 3: Use a nonlinear optimization solver to solve the nonlinear optimization problem constructed in step 2. Solving this optimization problem requires using the Jacobian matrix of each cost function to update the optimization variables.

[0163]

[0164] where f total is the total cost; f s , f c , f v , f a They represent the feasibility cost of smoothness, collision safety, and dynamics (speed, acceleration) respectively; x is the optimization variable; Q k , Q k+1 ,...,Q n-k is the control point; k is the degree of the selected spline, and the number of control points of the spline curve is n+k; λ1, λ2, λ3 are the proportions of the smoothness, collision safety, and dynamic feasibility costs, and λ1+λ2+λ3=1.

[0165] In this embodiment, considering that the dynamic constraints will be further adjusted later, the optimization focuses on smoothness and collision safety, and λ1, λ2, and λ3 are set to 0.4, 0.5, and 0.1 respectively. The settings can be adjusted as needed in other application scenarios.

[0166] The smoothness cost is in the form of:

[0167]

[0168] where f s is the smoothness cost; Q i-1 , Q i , Q i+1 is the control point, k is the degree of the selected spline, and the spline curve has n+k control points in total.

[0169] For a single control point Q i The smoothness cost is:

[0170]

[0171] where f s is the smoothness cost; Q i-1 , Q i , Q i+1 is the control point, k is the degree of the selected spline, and the spline curve has n+k control points in total.

[0172] Therefore, the Jacobian matrix corresponding to the smoothness cost is:

[0173]

[0174] where f s is the smoothness cost; x is the optimization variable; Q k-1 , Q k ,...,Q n-k is the control point, k is the degree of the selected spline, and the spline curve has n+k control points in total.

[0175] The collision safety cost is in the form of:

[0176]

[0177] where f c is the collision safety cost; d(Q i ) is the control point Q i Distance to the nearest obstacle; d thr The safety distance threshold is set.

[0178] The collision safety cost Jacobian matrix is:

[0179]

[0180] where f c is the collision safety cost; x is the optimization variable; d(Q i ) is the control point Q i Distance to the nearest obstacle, d thr is the set safety distance threshold, k is the degree of the selected spline, and the number of control points of the spline curve is n+k.

[0181] The feasibility cost of dynamics (speed and acceleration) is:

[0182]

[0183] where f v , f a is the feasibility cost of speed and acceleration; v max , a max is the maximum speed and acceleration limit; V iμ , A iμ are the velocity and acceleration represented by the trajectory control points; μ∈{x, y, z} represents the three axes; k is the degree of the selected spline, and the number of control points of the spline curve is n+k.

[0184] The dynamics (velocity and acceleration) feasibility cost Jacobian matrix is:

[0185] calculate Equivalent to calculation f a Similarly, let the dynamic limit loss cost be Q i Taking the derivative, the dynamics limitation loss Jacobian matrix is:

[0186]

[0187] Where Δu is the length of the node interval of each spline segment, and the matrix on the right is already in the expanded form, which is recorded as So about V m , m∈[k,nk-1], the expansion form of each item is:

[0188]

[0189] Comparing the derivation process of the Jacobian equation with speed limit, the Jacobian equation with acceleration limit is similar to:

[0190]

[0191] Where Δu is the length of the node interval of each spline segment, and the matrix on the right is already in the expanded form, which is recorded as About A e , the expanded form of the term e∈[k,nk-2] is:

[0192]

[0193] Using the cost functions represented by the control points and their corresponding Jacobian matrices, this embodiment uses the Gauss-Newton method to achieve trajectory optimization. In theory, any optimization method that can solve unconstrained nonlinear optimization problems can be used to solve this problem, and the choice can be made based on the actual results.

[0194] Step 4: Determine whether the optimized trajectory intersects with the obstacle.

[0195] If the trajectory does not intersect with the obstacle, the trajectory planning outputs a feasible trajectory.

[0196] If the trajectory intersects with an obstacle, step S2 is executed to re-plan the motion.

[0197] (4) Time reallocation strategy based on non-uniform B-spline

[0198] Step (1): Scan the velocity and acceleration parameters in the trajectory and store the parameters that exceed the dynamic limit in the overspeed set Y over With super-accelerated set A over The speed and acceleration parameters can be obtained from the position control point and the interval length:

[0199]

[0200] where Q m , Q m+1 is the position control point, V m , Ve , V e+1 is the velocity control point, Ae is the acceleration control point, u m+k+1 ,u m+1 ,u e+k+2 ,u e+1 is the knot vector, and k is the degree of the spline curve.

[0201] Step (2): Calculate the over-limit ratio of the parameters in the overspeed and over-acceleration set, u v =v m / v max with u a =a m / a max . Where u v ,u a is the excess ratio, v m , a m is the maximum velocity and acceleration in the transfinite set, v max , a max The maximum speed and acceleration are limited.

[0202] Step (3): using the over-limit ratio parameter, amplify the length of the interval corresponding to the over-limit parameter, and map the maximum speed or acceleration of the corresponding interval to the limited maximum speed and acceleration.

[0203] The mapping method is as follows:

[0204]

[0205] The definition range of m and e is the same as above. Q represents the position control point, V represents the velocity control point, and A represents the acceleration control point. The first digit of the subscripts represents the index, and the second digit represents the coordinate axis.

[0206] Step (4): Check whether there are still parameters in the trajectory that exceed the dynamic limits.

[0207] If it still exists, go to step (1);

[0208] If it does not exist, the motion trajectory parameters are output to the controller.

[0209] The effect of time reallocation on the processing of over-limit parameters is shown in Figure 3 ,For the processing effect of the speeding parameters after time redistribution, the trajectory remains unchanged and the speed is limited to within 2m / s.

[0210] After completing the design of the above algorithm, according to Figure 4 , an algorithm flow chart for motion planning of a quadrotor drone based on radiation field, the task is implemented according to the following steps:

[0211] S1 uses the camera on the drone to image environmental obstacles and obtain camera images. The drone is then positioned in real time based on the obtained camera images and camera parameters. The environment map is then created in real time using the 3D Gaussian Splatting method based on the camera images, camera parameters, and the real-time position of the drone.

[0212] S2, starting from the current position of the UAV, searches for a safe and smooth initial path connecting the starting point and the target point in the environment map obtained in step S1;

[0213] S3, performing real-time optimization on the initial path obtained in step S2 to obtain an optimized flight trajectory;

[0214] S4. The speed and acceleration of the UAV in the optimized flight trajectory obtained in step S3 are judged in real time. When the speed or acceleration of the UAV exceeds a set threshold, a time reallocation strategy is implemented for the speed or acceleration that exceeds the executable speed and acceleration of the UAV to extend the flight time and ensure that the UAV can complete the flight mission on time. When the speed and acceleration of the UAV do not exceed the set threshold, the optimized flight trajectory is flown until the target point is reached.

[0215] In summary, the above environment map building and planner design require hardware equipment including cameras and computing platforms. In order to realize the UAV motion planning task, additional flight control unit and UAV power kit are required. In this embodiment, RealSense D455 camera, Nvidia Jetson Orin NX computing platform, Pixhawk4 6C Mini flight control unit, SFP250 power kit are used to build the following Figure 5 , as shown in the UAV platform built in the embodiment. In order to ensure the safe execution of the radiation field-based quadcopter motion planning task, the aircraft's remote control needs to be configured before starting the flight. For details, please refer to the relevant guidelines for UAV flight.

[0216] Finally, for the test of the quadcopter UAV motion planning method based on the radiation field in the unknown environment of the UAV design in this embodiment, the test results are as follows: Figure 6 The following figure demonstrates the effectiveness of drone motion planning in an unknown environment. The blue curve represents the flight trajectory, which is highly smooth and maintains a safe distance from known obstacles. The colored areas are explicit representations of obstacle occupancy. This experiment validated the safety, feasibility, and robustness of the proposed algorithm.

[0217] It should be noted that the radiation field-based quadcopter UAV motion planning method of the present invention is not limited to the hardware devices designed in the embodiments. It only requires that the rotorcraft be equipped with a positioning module, a control module, a computing module, and a vision module. It can be extended to multi-rotor UAVs such as quadcopters and hexacopters, and positioning methods such as laser and optical flow. The quadcopter UAV platform hardware designed by the present invention has the advantages of small size and lightweight. Compared with traditional mapping and UAV motion planning algorithms, the present invention provides a more detailed environmental map for motion planning, while improving the computational efficiency of motion planning and enhancing the UAV's operational capabilities in unknown and complex environments.

[0218] In summary, the above are only preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A motion planning method for a quadrotor drone based on radiation field, characterized in that The steps of the method include: S1, using the camera on the drone to image environmental obstacles, obtain camera images, and locate the drone in real time based on the obtained camera images and camera parameters to obtain the real-time position of the drone, and then build an environmental map in real time based on the camera images, camera parameters and the real-time position of the drone; S2, taking the current position of the UAV as the starting point, obtains the initial path between the starting point and the target point using the environment map obtained in step S1; S3, performing real-time optimization on the initial path obtained in step S2 to obtain an optimized flight trajectory; S4. Real-time determination of the speed and acceleration of the UAV in the optimized flight trajectory obtained in step S3. When the speed or acceleration of the UAV exceeds a set threshold, a time reallocation strategy is implemented for the speed or acceleration that exceeds the executable speed and acceleration of the UAV to extend the flight time so that the UAV can complete the flight mission on time. When the speed and acceleration of the UAV do not exceed the set threshold, the UAV flies according to the optimized flight trajectory until it reaches the target point. In step S2, the method for obtaining the initial path is: Step (1), adding the starting point to the set of nodes to be processed; Step (2), determining whether there are still nodes to be processed in the set of nodes to be processed; If there are still nodes to be processed in the set of nodes to be processed, then the point with the lowest cost in the set to be processed is taken as the current node, and it is determined whether the current node is within the distance threshold range centered on the target point. If so, step (6) is executed; otherwise, step (3) is executed. If there is no node to be processed in the set of nodes to be processed, the path search is stopped; Step (3): using the current state as the initial value, using the state space equation, for a given discrete control input, obtaining the expansion node that conforms to the dynamics according to the state transfer equation, and storing the expansion node in the expansion point set of the current node; Step (4), calculate the current cost of each candidate node in the extension point set, and determine whether the candidate node already exists in the set of nodes to be processed and can be reached; If it is unreachable, skip this candidate node; If the candidate node already exists in the set of nodes to be processed and can be reached, the cost of reaching the candidate node from the current node in step (2) is calculated and compared with the cost of the candidate node that already exists in the set of nodes to be processed and represents the same position. The candidate node with the lower current cost is retained in the set of nodes to be processed. If the node to be selected is not in the set of nodes to be processed and can be reached, directly calculate its current cost; Step (5), use the Pontryagin minimum principle to solve the optimal control problem to construct a heuristic function, calculate the heuristic cost of each candidate node in the expansion point set, and then return to step (2); Step (6), outputting the initial path obtained by path planning; In step S3, the method for real-time optimization of the initial path is: Step 1: Use B-spline curve to fit the initial path; Step 2: Use control points to construct a nonlinear optimization problem that takes into account smoothness, collision safety, and dynamic feasibility; Step 3: Use a nonlinear optimization solver to solve the nonlinear optimization problem constructed in step 2; Step 4: Determine whether the optimized trajectory intersects with the obstacle; If the trajectory does not intersect with the obstacle, the trajectory planning outputs a feasible trajectory; If the trajectory intersects with an obstacle, step S2 is executed to re-plan the motion.

2. The motion planning method for a quadrotor drone based on radiation field according to claim 1, characterized in that: In step S1, a 3D Gaussian Splatting method is used to create an environment map in real time. The specific steps include: Step 1: Extract corner features and edge features from the camera image to obtain point cloud data in three-dimensional space. Based on the real-time position information of the UAV, the point cloud data is converted into the navigation coordinate system of the UAV. Step 2: Grid the mission space in the UAV navigation coordinate system (hereinafter referred to as the mission space). Set the grid range to the size of the mission space to ensure that the grid covers the entire mission space. Set the grid resolution to one-tenth of the aircraft diameter or less to ensure the fineness of the map. Divide this space into grid cells of equal size, with each grid cell representing a fixed area in the environment. Step 3: For each grid cell, traverse the point cloud data within the cell and use the Gaussian function to weight each point. The weight of each point is equal to the ratio of the distance from the point to the center of the grid cell to the sum of the distances from all points to the center of the grid cell. Calculate the weighted average of the position coordinates of all points in the grid as the position of the obstacle represented by the grid cell. Traverse all grid cells and combine the calculated obstacle positions into a complete 3D environment map. The Gaussian function formula is: where d x is the distance from the point to the center of the grid cell in the x-axis direction, d y is the distance from the point to the center of the grid cell in the y-axis direction, d z is the distance from the point to the center of the grid cell in the z-axis direction, and σ is the standard deviation of the Gaussian function.

3. The motion planning method for a quadrotor drone based on radiation field according to claim 1, characterized in that: The optimization problem is in the form: where f total is the total cost, f s ,f c ,f v ,f a They represent the smoothness cost, collision safety cost, dynamic speed cost, and acceleration feasibility cost respectively, Q k ,Q k+1 ,…,Q n-k is the control point, k is the degree of the selected spline, the number of control points of the spline curve is n+k, λ1, λ2, λ3 are the proportions of the cost of smoothness, collision safety, and dynamic feasibility, λ1+λ2+λ3=1.

4. The motion planning method for a quadrotor drone based on radiation field according to claim 3, characterized in that: The smoothness cost is in the form of: where Q i-1 ,Q i ,Q i+1 For the control point.

5. The motion planning method for a quadrotor drone based on radiation field according to claim 3, characterized in that: The collision safety cost is in the form of: Where d(Q i ) is the control point Q i Distance to the nearest obstacle, d thr The safety distance threshold is set.

6. The motion planning method for a quadrotor drone based on radiation field according to claim 3, characterized in that: The feasibility cost of dynamic speed and acceleration is: where v max ,a max The maximum speed and acceleration are limited.

7. The motion planning method for a quadrotor drone based on radiation field according to claim 1, characterized in that: In step S4, the time reallocation strategy is: Step (1) determines whether the speed and acceleration parameters in the feasible trajectory exceed the dynamic limit, and stores the parameters exceeding the dynamic limit in the overspeed set V over With super-accelerated set A over middle; Step (2): Calculate the over-limit ratio of the parameters in the overspeed and over-acceleration set, u v =v m / v max and where u v ,u a is the excess ratio, v m ,a m is the maximum velocity and acceleration in the transfinite set, v max ,a max The maximum speed and maximum acceleration are limited; Step (3), using the over-limit ratio parameter, amplifying the length of the interval corresponding to the over-limit parameter, and mapping the maximum speed or acceleration of the corresponding interval to the maximum speed and acceleration of the limit; Step (4), check whether there are still parameters in the trajectory that exceed the dynamic limit; If it still exists, go to step (1); If it does not exist, output the optimized flight trajectory.

Images