A UAV motion planning method and device

Abstract

The invention relates to the technical field of unmanned aerial vehicles, and discloses a method and device for unmanned aerial vehicle motion planning. The method comprises: calculating total potential energy by using the current position of the unmanned aerial vehicle, the position of a target node, and the nearest obstacle information; calculating the path cost according to the flight route of the unmanned aerial vehicle; calculating the obstacle density within the current flight range of the unmanned aerial vehicle; calculating the total cost value by using the total potential energy, the path cost, the obstacle density, the current position of the unmanned aerial vehicle, and the position of the target node; planning the path of the unmanned aerial vehicle to the target node in a map model by using the total cost value, and obtaining a path to be optimized; mapping the map model, the path to be optimized, and the actual flight status data of the unmanned aerial vehicle to a three-dimensional grid matrix, and obtaining an actual three-dimensional grid matrix; in the actual three-dimensional grid matrix, sequentially performing speed optimization, turning optimization, and battery energy consumption optimization on the mapped path to be optimized, and obtaining a target path; the invention can improve the safe avoidance effect of the unmanned aerial vehicle against dynamic obstacles.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicles, and in particular to a method and device for unmanned aerial vehicle motion planning. Background Art

[0002] This section is intended to provide a background or context to the embodiments of the invention recited in the claims. No admission is made that the description herein is prior art by inclusion in this section.

[0003] UAV motion planning refers to designing a complete set of movement plans for the UAV to ensure that it can complete the scheduled tasks safely and efficiently. Existing UAV motion planning methods, such as A* algorithm, random search method and Dijkstra algorithm, are difficult to achieve real-time and accurate path planning, and have poor effects on complex terrain recognition and safe avoidance of dynamic obstacles. Summary of the invention

[0004] The embodiment of the present invention provides a UAV motion planning method for improving the safety avoidance effect of the UAV against dynamic obstacles and realizing real-time and accurate path planning. The method includes:

[0005] Obtain obstacle point cloud data, perform 3D reconstruction, and obtain a map model;

[0006] According to the map model, obtain the current position of the drone, the target node position, and the nearest obstacle information; the nearest obstacle information includes the maximum value of the distance between the center position of the nearest obstacle and any point on the edge of the nearest obstacle;

[0007] Based on the artificial potential field method, the total potential energy is calculated using the current position of the drone, the target node position, and the nearest obstacle information;

[0008] Obtain the flight route of the drone and calculate the path cost based on the flight route; the path cost reflects the total distance flown;

[0009] According to the maximum distance that the UAV can fly and the current position of the UAV, the flight range is determined in the map model, and the obstacle density within the flight range is calculated; the obstacle density reflects the density of obstacle distribution within the flight range;

[0010] The total potential energy, path cost, obstacle density, current position of the drone, and target node position are input into a preset cost function, and a total cost value is output; the preset cost function is used to calculate the total cost value of the drone flying to the target node;

[0011] Based on the artificial potential field method, the total cost value is used to plan the path of the UAV to the target node in the map model to obtain the path to be optimized;

[0012] Mapping the map model, the path to be optimized, and the actual flight status data of the UAV to a three-dimensional grid matrix to obtain an actual three-dimensional grid matrix for simulating the flight of the UAV;

[0013] In the actual three-dimensional grid matrix, the mapped path to be optimized is optimized in turn for speed, turning, and battery energy consumption to obtain a target path; the target path is used to control the UAV to fly to the target node.

[0014] The embodiment of the present invention further provides a UAV motion planning device, which is used to improve the safety avoidance effect of the UAV against dynamic obstacles and realize real-time and accurate path planning. The device includes:

[0015] The map model building module is used to obtain obstacle point cloud data, perform three-dimensional reconstruction, and obtain a map model;

[0016] The total potential energy calculation module is used to obtain the current position of the drone, the target node position, and the nearest obstacle information according to the map model; the nearest obstacle information includes the maximum value of the distance between the center position of the nearest obstacle and any point on the edge of the nearest obstacle; based on the artificial potential field method, the total potential energy is calculated using the current position of the drone, the target node position, and the nearest obstacle information;

[0017] The path cost calculation module is used to obtain the flight route of the drone and calculate the path cost based on the flight route; the path cost reflects the total distance flown;

[0018] The obstacle density calculation module is used to determine the flight range in the map model according to the maximum distance that the UAV can fly and the current position of the UAV, and calculate the obstacle density within the flight range; the obstacle density reflects the density of obstacle distribution within the flight range;

[0019] A total cost value calculation module is used to input the total potential energy, path cost, obstacle density, current position of the drone, and target node position into a preset cost function, and output a total cost value; the preset cost function is used to calculate the total cost value of the drone flying to the target node;

[0020] The module for determining the path to be optimized is used to plan the path of the UAV to the target node in the map model based on the artificial potential field method and the total cost value to obtain the path to be optimized;

[0021] The target path calculation module is used to map the map model, the path to be optimized, and the actual flight status data of the UAV to a three-dimensional grid matrix to obtain an actual three-dimensional grid matrix for simulating the flight of the UAV; in the actual three-dimensional grid matrix, the mapped path to be optimized is optimized in turn for speed, turning, and battery energy consumption to obtain a target path; the target path is used to control the UAV to fly to the target node.

[0022] An embodiment of the present invention also provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the above-mentioned UAV motion planning method when executing the computer program.

[0023] An embodiment of the present invention further provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the above-mentioned drone motion planning method is implemented.

[0024] An embodiment of the present invention further provides a computer program product, which includes a computer program, and when the computer program is executed by a processor, the above-mentioned drone motion planning method is implemented.

[0025] In the embodiment of the present invention, obstacle point cloud data is obtained, three-dimensional reconstruction is performed, and a map model is obtained; according to the map model, the current position of the drone, the target node position, and the nearest obstacle information are obtained; the nearest obstacle information includes the maximum value of the distance between the center position of the nearest obstacle and any point on the edge of the nearest obstacle; based on the artificial potential field method, the total potential energy is calculated using the current position of the drone, the target node position, and the nearest obstacle information; the flight route of the drone is obtained, and the path cost is calculated according to the flight route; the path cost reflects the total distance flown; according to the farthest distance that the drone can fly and the current position of the drone, the flight range is determined in the map model, and the obstacle density within the flight range is calculated; the obstacle density reflects the distribution of obstacles within the flight range. The method comprises the following steps: determining the density of the distribution of the UAV; inputting the total potential energy, path cost, obstacle density, current position of the UAV and position of the target node into a preset cost function, and outputting a total cost value; the preset cost function is used to calculate the total cost value of the UAV flying to the target node; based on the artificial potential field method, the total cost value is used to plan the path of the UAV flying to the target node in the map model to obtain the path to be optimized; mapping the map model, the path to be optimized and the actual flight status data of the UAV to a three-dimensional grid matrix to obtain an actual three-dimensional grid matrix for simulating the flight of the UAV; in the actual three-dimensional grid matrix, the mapped path to be optimized is optimized in turn for speed, turning and battery energy consumption to obtain a target path; the target path is used to control the UAV to fly to the target node. In the embodiment of the present invention, in the UAV motion planning, the artificial potential field method is first used to plan the path of the UAV to the target node in the map model using the total cost value to obtain the path to be optimized, and then the mapped path to be optimized is optimized in the actual three-dimensional grid matrix in turn for speed optimization, turning optimization, and battery energy consumption optimization to obtain the final target path. The UAV can be safely and accurately controlled to fly according to the set speed, direction, and energy consumption, and the total cost value is calculated using the obstacle density within the flight range of the current position of the UAV. The total cost value can be updated according to the position of the UAV, so that dynamic obstacles can be effectively and safely avoided in real time, thereby realizing real-time, safe, and accurate path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the prior art descriptions. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work. In the drawings:

[0027] Figure 1 Schematic diagram of the process of the UAV motion planning method in an embodiment of the present invention;

[0028] Figure 2 A diagram showing a specific example of a method for planning the motion of a UAV according to an embodiment of the present invention;

[0029] Figure 3 FIG. 4 is another specific example of the UAV motion planning method in an embodiment of the present invention;

[0030] Figure 4 Schematic diagram of a UAV motion planning device in an embodiment of the present invention;

[0031] Figure 5 Schematic diagram of a computer device in an embodiment of the present invention. DETAILED DESCRIPTION

[0032] To make the purpose, technical solution and advantages of the embodiments of the present invention more clear, the embodiments of the present invention are further described in detail below in conjunction with the accompanying drawings. Here, the exemplary embodiments of the present invention and their descriptions are used to explain the present invention, but are not intended to limit the present invention.

[0033] The acquisition, transmission, storage, use, and processing of data in the technical solution of this application comply with the relevant provisions of national laws and regulations.

[0034] At present, it is difficult to achieve efficient and fast path planning and real-time adjustment strategies while improving the safety of drones. When dealing with planning problems with multiple constraints, it is often difficult to balance various constraints.

[0035] In the embodiments of the present invention, facing the essential requirements of unmanned operation and maintenance, according to the first principles thinking method, the spatial position characteristics of the UAV in scenarios such as take-off, inspection, emergency, forced landing, return, and refueling are sorted out, and the UAV only relies on its own current position recognition to complete the fully autonomous task scheduling mode of the inspection mission. A software and hardware architecture that is suitable for the autonomous completion of UAV inspection tasks is constructed, and support is provided for the expansion of the existing UAV task system to form a UAV inspection operation mode and technical method for indoor power equipment.

[0036] In the embodiment of the present invention, the orientation, positioning and navigation technology and obstacle avoidance technology of the UAV without navigation satellite signals are studied. Based on the LIO laser SAM (Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping, a positioning algorithm used in the fields of robots and self-driving cars), and the ORB (Oriented FAST and Rotated BRIEF) visual SLAM and other navigation and control technologies, a motion planning algorithm and path planning algorithm for the UAV are designed, so that it can autonomously plan obstacle avoidance paths according to environmental perception information and avoid collisions with obstacles. The path planning algorithm has the advantages of high efficiency, speed, and real-time performance, and can adapt to dynamic environments. In the embodiment of the present invention, the research and design of the trajectory tracking control technology of the UAV is carried out by combining the three-dimensional space algorithm and the collision detection algorithm.

[0037] In the embodiment of the present invention, a simulated power environment is designed based on environments such as substations, converter stations with strong electric fields, closed tunnels, and narrow spaces, to achieve an autonomous flight demonstration application in a complex indoor environment with strong electromagnetic fields. The safety and efficiency of the drone and its supporting architecture in complex environments such as indoor narrow spaces have been verified.

[0038] Figure 1 FIG. 1 is a flow chart of a method for planning the motion of a UAV according to an embodiment of the present invention. Figure 1 As shown, the method includes:

[0039] Step 101: Obtain obstacle point cloud data, perform three-dimensional reconstruction, and obtain a map model;

[0040] Step 102: According to the map model, the current position of the drone, the position of the target node, and the nearest obstacle information are obtained; the nearest obstacle information includes the maximum value of the distance between the center position of the nearest obstacle and any point on the edge of the nearest obstacle;

[0041] Step 103, based on the artificial potential field method, using the current position of the drone, the target node position, and the nearest obstacle information, calculate the total potential energy;

[0042] Step 104, obtaining the flight route of the UAV, and calculating the path cost according to the flight route; the path cost reflects the total distance flown;

[0043] Step 105: determine the flight range in the map model according to the maximum distance the drone can fly and the current position of the drone, and calculate the density of obstacles within the flight range; the obstacle density reflects the density of obstacle distribution within the flight range;

[0044] Step 106: input the total potential energy, path cost, obstacle density, current position of the drone, and position of the target node into a preset cost function, and output a total cost value; the preset cost function is used to calculate the total cost value of the drone flying to the target node;

[0045] Step 107: Based on the artificial potential field method, the total cost value is used to plan the path of the UAV to the target node in the map model to obtain the path to be optimized;

[0046] Step 108: Map the map model, the path to be optimized, and the actual flight status data of the UAV to a three-dimensional grid matrix to obtain an actual three-dimensional grid matrix for simulating the flight of the UAV;

[0047] Step 109: In the actual three-dimensional grid matrix, the mapped path to be optimized is optimized in speed, turning, and battery energy consumption in turn to obtain a target path; the target path is used to control the UAV to fly to the target node.

[0048] from Figure 1 As can be seen from the process shown, in the embodiment of the present invention, in the UAV motion planning, the artificial potential field method is first used to plan the path of the UAV to the target node in the map model using the total cost value to obtain the path to be optimized, and then the mapped path to be optimized is optimized in the actual three-dimensional grid matrix in turn for speed optimization, turning optimization, and battery energy consumption optimization to obtain the final target path. The UAV can be safely and accurately controlled to fly at the set speed, direction, and energy consumption, and the total cost value is calculated using the obstacle density within the flight range of the current position of the UAV. The total cost value can be updated according to the position of the UAV, so that dynamic obstacles can be effectively and safely avoided in real time, thereby realizing real-time, safe, and accurate path planning.

[0049] First, a map model is constructed. During the implementation, the high-precision measurement module carried by the drone acquires data about the surrounding environment during the flight and constructs a three-dimensional map model.

[0050] Figure 2 FIG. 1 is a specific example of the UAV motion planning method in an embodiment of the present invention. Figure 2 As shown, obtaining obstacle point cloud data, performing three-dimensional reconstruction, and obtaining a map model may include:

[0051] Step 201: Obtain obstacle point cloud data through a laser sensor carried by the drone;

[0052] Step 202: Use high-speed computing hardware and three-dimensional modeling tools to perform three-dimensional reconstruction on the obstacle point cloud data to obtain a map model; the high-speed computing hardware includes: CPU (Central Processing Unit), GPU (Graphics Processing Unit), professional graphics workstation, and cloud computing resources.

[0053] For example, a laser sensor emits a laser pulse, and when the laser hits an obstacle, part of the laser is reflected back. The laser sensor receives the reflected laser and records the timestamp Ot and the emission angle Oθ.

[0054] Calculate the timestamp Ot and the speed of light V to get the obstacle node distance Odi;

[0055] Odi=V×Ot;

[0056] The emission angle Oθ and the obstacle node distance Odi are calculated to obtain the x-axis coordinate xi, the y-axis coordinate yi and the z-axis coordinate zi of the obstacle, which are recorded as point cloud data, wherein the coordinate origin may be the origin of the coordinate of the spatial coordinate system set according to the environment space;

[0057] xi=Odi×cos(Oθ);

[0058] yi=Odi×sin(Oθ);

[0059] zi = Odi × tan(Oθ);

[0060] Afterwards, high-speed computing hardware such as CPU, GPU, professional graphics workstation, cloud computing resources and 3D modeling tools are used for 3D reconstruction, where the 3D modeling tools can be some existing common tools, which are not listed here one by one. When the calculation is performed on the remote processor or cloud computing resources, the hardware resource occupation of the drone itself can be reduced, and the efficiency of the drone motion planning can be improved.

[0061] Afterwards, the UAV motion planning is carried out based on the map model to calculate the total potential energy, path cost, etc.

[0062] When calculating the total potential energy, first obtain the current position of the drone, the target node position, and the nearest obstacle information based on the map model; the nearest obstacle information at least includes the nearest obstacle position information, the nearest obstacle size information (shape, volume, area, etc.), and the maximum value of multiple distances between the nearest obstacle center position and any point on the nearest obstacle edge, etc. Then, based on the artificial potential field method, the total potential energy is calculated using the drone's current position, target node position, and nearest obstacle information.

[0063] In one embodiment, based on the artificial potential field method, the total potential energy is calculated using the current position of the drone, the position of the target node, and the nearest obstacle information, which may include:

[0064] According to the current position of the UAV and the position of the target node, the attraction potential energy between the UAV and the target node is determined;

[0065] According to the current position of the drone and the information of the nearest obstacle, determine the repulsive potential energy between the drone and the nearest obstacle;

[0066] The sum of the attractive potential energy and the repulsive potential energy is taken as the total potential energy.

[0067] Among them, determining the attraction potential energy of the drone and the target node according to the current position of the drone and the target node can include: calculating the first Euclidean distance from the current position of the drone to the target node according to the current position of the drone and the target node; calculating the attraction potential energy of the drone and the target node according to the first Euclidean distance and a preset attraction potential weight; the preset attraction potential weight is used to adjust the attraction potential energy strength.

[0068] For example, the current position of the drone is ( xi 2, yi 2, zi2), the target node position is ( xi 1, yi 1, zi 1) Calculate the target node and the position of the drone to obtain the first Euclidean distance Da from the current position of the drone to the target node:

[0069] ;

[0070] Calculate the preset attraction weights Ka and Da to get the attraction Ua:

[0071] Ua = -Ka × (1 / Da);

[0072] It should be noted that the preset attraction potential weight Ka is used to adjust the strength of the attraction potential energy. It is a custom data range, and the specific numerical value can be determined by the user or system developer according to actual needs.

[0073] Among them, determining the repulsive potential energy between the drone and the nearest obstacle according to the current position of the drone and the nearest obstacle information may include:

[0074] According to the current position of the drone and the position of the nearest obstacle, the second Euclidean distance from the current position of the drone to the nearest obstacle is calculated;

[0075] The repulsive potential energy between the UAV and the nearest obstacle is calculated based on the second Euclidean distance, the maximum value of the distance between the center of the nearest obstacle and any point on the edge of the nearest obstacle, and the preset repulsive potential weight.

[0076] For example, the map model is read to obtain the obstacle size, and the distance between the center position of the nearest obstacle and each point on the edge of the nearest obstacle is calculated to determine the maximum value Rb;

[0077] Similar to the calculation of the first Euclidean distance, the second Euclidean distance Db from the current position of the UAV to the nearest obstacle is calculated based on the current position of the UAV and the position of the nearest obstacle.

[0078] According to the following formula, the repulsive potential energy between the drone and the nearest obstacle is calculated based on the second Euclidean distance Db, the maximum value Rb of the distance between the center of the nearest obstacle and any point on the edge of the nearest obstacle, and the preset repulsive potential weight:

[0079] Ub=Kb×max(0,Db-Rb) 2 ;

[0080] Where Ub is the repulsive potential energy, Kb is the preset repulsive potential weight, which is used to adjust the strength of the repulsive potential, Db is the second Euclidean distance, and Rb is the maximum value of the distance between the center of the nearest obstacle and any point on the edge of the nearest obstacle.

[0081] It should be noted that the preset repulsive potential weight is used to adjust the strength of the repulsive potential energy. It is a custom data range, and the specific numerical value can be determined by the user or system developer according to actual needs.

[0082] Calculate the attractive potential Ua and repulsive potential Ub to get the total potential energy Uc:

[0083] Uc=Ua+Ub.

[0084] Then, in step 104 , the path cost is calculated.

[0085] It should be noted that the calculation order of path cost, total potential energy, and obstacle density is not limited to a fixed order, and the calculation order can be swapped.

[0086] When calculating the path cost, the flight route of the drone can be obtained through the positioning recorder carried by the drone. The positioning recorder records the initial position of the drone and the coordinate points of the path it has flown.

[0087] In one embodiment, the flown route includes a plurality of coordinate points;

[0088] Calculates path costs based on flown routes, which can include:

[0089] Calculate the distance between two adjacent coordinate points in sequence to obtain multiple distance values;

[0090] Outputs the sum of multiple distance values ​​as the path cost.

[0091] In the specific implementation, the distance between two adjacent coordinate points is calculated as the actual cost as follows {A:4, B:1, C:7, D:8} (this is the representation of the dictionary in the computer, A, B, C and D represent the destination, and the path is ABCD). It takes 4 hours from the starting point to A, and 1 hour from B to A. The same applies to the origin to A and A to B as adjacent path points. The first thing to calculate is the cost between the two adjacent points. After that, you only need to add the cost from the origin to A, the cost from A to B, the cost from B to C, and the cost from C to D to get the cost from the origin to D. The total path cost is calculated based on this principle.

[0092] For example, the initial position and the flight path of the drone are acquired by using a positioning recorder to obtain the initial position and the path coordinate set P. The path coordinate set P includes multiple coordinate points that have been flown. Similar to the calculation of the first Euclidean distance, the distance between two adjacent coordinate points is calculated as the actual cost, for example:

[0093] ;

[0094] Wherein, Yo (x13, y13, z13) and Yo (x23, y23, z23) are two adjacent coordinate points, x13, y13, z13, x23, y23, z23 are the specific coordinate point values ​​respectively, and G{Yo (x13, y13, z13), Yo (x23, y23, z23)} represents the actual cost between the two coordinate points.

[0095] Furthermore, the total path cost G can be calculated as follows:

[0096] ;

[0097] In the formula, G (xi2, yi2, zi2) represents the total path cost corresponding to the current position of the UAV, a is the total number of coordinate points in the path coordinate set P, Yo (xi3, yi3, zi3) represents the i-th coordinate point in the path coordinate set P, xi3, yi3, zi3 are the specific coordinate point values ​​respectively.

[0098] In step 105, the flight range is determined in the map model according to the maximum distance that the drone can fly and the current position of the drone, and the obstacle density within the flight range is calculated; the obstacle density reflects the density of obstacle distribution within the flight range.

[0099] For example, by performing a flight test on a fully-charged drone, the maximum distance the drone can fly can be obtained. Based on the maximum distance the drone can fly and the current location of the drone, the flight range can be determined in the map model. This can include: using the current location of the drone as a dot and the maximum distance the drone can fly as a radius, and determining the flight range within the map model.

[0100] In order to calculate the obstacle density clearly and quantitatively, in one embodiment, calculating the obstacle density within the flight range may include:

[0101] Obstacle density within the flight range is calculated using the following formula:

[0102] Yc=Yf / Ym ;

[0103] In the formula, Y is the obstacle density, Yf is the flight range, Y is the number of obstacles within the flight range.

[0104] Then in step 106, the total potential energy, path cost, obstacle density, current position of the drone, and target node position are input into a preset cost function, and the total cost value is output; the preset cost function is used to calculate the total cost value of the drone flying to the target node.

[0105] The total cost value in the embodiment of the present invention takes into account the flight path cost, total potential energy and obstacles around the drone, and can change with the change of the drone's position. Therefore, the drone motion planning in the embodiment of the present invention can avoid dynamic obstacles in real time.

[0106] In one embodiment, the total potential energy, path cost, obstacle density, current position of the drone, and target node position are input into a preset cost function, and the total cost value is output, which may include:

[0107] F = G + Kd × Da + Uc;

[0108] Kd=1 / (Yc+w);

[0109] Where F is the total cost value; G is the path cost; Kd is the current calculation step size, and is the intermediate calculation amount; Yc is the obstacle density; w is the set rated parameter used to avoid division by zero; Da is the first Euclidean distance calculated based on the current position of the drone and the target node position; Uc is the total potential energy.

[0110] It should be noted that the rated parameter is a small positive number used to avoid division by zero. It is a custom data range. The specific value can be determined by the user or system developer according to actual needs. During implementation, the intermediate calculation amount, that is, the current calculation step length Kd, can change dynamically according to the position of the drone.

[0111] Afterwards, in step 107, based on the artificial potential field method, the total cost value is used to plan the path for the UAV to fly to the target node in the map model to obtain the path to be optimized.

[0112] During implementation, the Matplotlib tool (Matplotlib is a popular Python drawing library for creating high-quality charts and visualizations) can be used in combination with the artificial potential field APFA algorithm (Adaptive Particle Filter Algorithm) to process the total cost value, total potential energy, and map model to obtain the path to be optimized.

[0113] In step 108, the map model, the path to be optimized, and the actual flight status data of the UAV are mapped to a three-dimensional grid matrix to obtain an actual three-dimensional grid matrix for simulating the flight of the UAV, and then the path is further optimized in the simulated actual three-dimensional grid matrix.

[0114] Among them, the path to be optimized is mapped to the three-dimensional grid matrix to obtain the corresponding simulation path, and the simulation path is further optimized in the simulation environment, that is, the actual three-dimensional grid matrix, to finally obtain the target path. The specific mapping process is: the flight trajectory of the drone can be obtained according to the path to be optimized, and the flight trajectory of the drone is simulated in the actual three-dimensional grid to obtain the simulation path.

[0115] It should be noted that, in the embodiment of the present invention, simulation refers to digitally displaying the flight trajectory of the drone in an actual three-dimensional grid, simulating the real situation of the drone actually flying according to the flight trajectory of the drone.

[0116] In one embodiment, the grid size of the three-dimensional grid matrix can be obtained in advance as follows:

[0117] Obtain the maximum safe speed, turning radius, navigation accuracy, speed and acceleration of the drone during turning flight;

[0118] Calculate the grid size as follows:

[0119] Dk = (Va × Dc) / Rc;

[0120] Rc=Vb 2 / Aa;

[0121] Where Dk is the grid size, that is, the size of a single grid, Va is the maximum safe speed of the UAV, Dc is the navigation accuracy, Rc is the turning radius, Vb and Aa are the speed and acceleration of the UAV during turning flight, respectively.

[0122] For example, the UAV is tested as follows to obtain the maximum safe speed Va and turning radius Rc:

[0123] Start the UAV to fly, increase the flight speed of the UAV, and use the speed monitoring instrument to obtain the flight parameters of the UAV at different speeds to obtain the flight parameters;

[0124] According to the maximum safe flight speed judgment standard of the UAV, the flight parameters are screened to obtain the flight speed corresponding to the flight parameters that meet the maximum safe flight speed judgment standard of the UAV, which is recorded as the maximum safe speed Va;

[0125] It should be noted that the maximum safe flight speed judgment standard for drones: Through the maximum safe flight speed judgment standard for drones, drone operators and manufacturers can ensure the maximum safe flight speed of drones under various flight conditions, thereby minimizing the risk of accidents and increasing the success rate of flight missions.

[0126] The UAV is made to perform a turning flight, and data is recorded using an external data recording device. The speed and acceleration of the UAV when turning are obtained to obtain the speed Vb and acceleration Aa when turning;

[0127] Calculate the speed and acceleration when turning to get the turning radius Rc=Vb 2 / A;

[0128] Then, the UAV manual is read to obtain the navigation accuracy Dc, the maximum safe speed Va, navigation accuracy Dc and turning radius Rc are calculated, and the grid size Dk=(Va×Dc) / Rc is obtained.

[0129] Afterwards, a three-dimensional grid matrix is ​​constructed according to the grid size;

[0130] Initialize the three-dimensional grid matrix, mark the state of the grid as idle, and obtain an initialized three-dimensional grid matrix;

[0131] It should be noted that initialization is a basic concept in programming and system design. It refers to the process of assigning initial values ​​to variables, data structures or databases when a program is executed or the system starts.

[0132] Get the current UAV space flight range;

[0133] According to the flight range, the initialized three-dimensional grid matrix is ​​mapped to obtain the actual three-dimensional grid matrix.

[0134] The specific steps for obtaining the actual three-dimensional grid matrix are as follows:

[0135] Obstacles within the space flight range are acquired to obtain the position of space obstacles;

[0136] According to the spatial obstacle position, the obstacle is mapped to the initialization grid matrix, the corresponding grid is marked as an obstacle, and the actual three-dimensional grid matrix is ​​obtained.

[0137] Finally, in step 109, in the actual three-dimensional grid matrix, the mapped path to be optimized is optimized in speed, turning, and battery energy consumption in turn to obtain a target path; the target path is used to control the UAV to fly to the target node.

[0138] During implementation, the speed of the mapped path to be optimized, i.e., the simulation path, is adjusted to obtain a primary optimization target path; the turning optimization is performed on the primary optimization target path to obtain a secondary optimization target path; the energy consumption of the secondary optimization target path is optimized to obtain the final target path.

[0139] For example, using speed sensors, timers, direction sensors and other sensors, the flight status data of the drone in the simulated path is acquired to obtain the grid speed Tv and grid time point Tt, and then further optimized. Among them, the grid speed reflects the simulated speed of the drone in the actual three-dimensional grid matrix, and the grid time point Tt is the simulated time point information in the actual three-dimensional grid matrix.

[0140] In one embodiment, the actual flight status data of the drone includes: the initial speed of the drone and the minimum safe distance; the minimum safe distance indicates the distance value that ensures flight safety;

[0141] In the actual three-dimensional grid matrix, the mapped path to be optimized is optimized in turn for speed, turning, and battery energy consumption to obtain the target path, which may include:

[0142] In the actual three-dimensional grid matrix, the distance from the UAV to the nearest obstacle is calculated by curve fitting to obtain the speed calibration coefficient; the speed calibration coefficient is a calculation parameter in the middle, indicating the acceleration of the direction or angle;

[0143] The change speed is calculated according to the following formula; the change speed is used to determine whether to adjust the flight speed of the drone to the change speed according to the preset speed adjustment rule;

[0144] Tv1=Tv0+Ke×(Db-Df);

[0145] Where Tv1 is the speed of change, Tv0 is the initial speed of the UAV, Ke is the speed adjustment coefficient, Db is the second Euclidean distance from the current position of the UAV to the nearest obstacle, and Df is the minimum safe distance;

[0146] It should be noted that the minimum safe distance is a key parameter used to ensure flight safety in drone path planning. It is a custom data range and the specific numerical determination can be adjusted by the user or system developer according to actual needs.

[0147] The speed is optimized according to the preset speed adjustment rules, the changing speed and the minimum safety distance to obtain an optimized path; the preset speed adjustment rules include determining whether to adjust the flight speed of the drone according to the difference between the changing speed and the minimum safety distance.

[0148] For example, when Db is less than Df, the drone speed is adjusted to the change speed Tv1;

[0149] When Db is greater than or equal to Df, the drone can maintain its original speed.

[0150] In one embodiment, in the actual three-dimensional grid matrix, the mapped path to be optimized is sequentially optimized for speed, turning, and battery energy consumption to obtain a target path, which may include:

[0151] Obtaining the grid velocity Tv and grid time point Tt in the actual three-dimensional grid matrix;

[0152] The grid acceleration Ta, grid curvature Tk, and curvature sensitivity Tm can be calculated according to the grid velocity Tv and grid time point Tt as follows:

[0153] Ta = Tv / Tt;

[0154] Tk=|Tv×Ta| / |Tv| 3 ;

[0155] Tm=dTv / dTk;

[0156] The grid is a division interval. The grid acceleration and grid curvature represent the acceleration of the corresponding grid and the curvature of the grid path, which are similar to the acceleration of the function coordinate point and the curvature of the function curve. The curvature sensitivity can reflect the sensitivity of the curvature change.

[0157] According to the curvature sensitivity Tm and the preset curvature sensitivity rules, the primary optimization path is optimized for turning to obtain the secondary optimization path; the preset curvature sensitivity rules include: determining whether to delete the coordinate points within the set range of the current position of the drone in the primary optimization path according to the comparison result of the curvature sensitivity Tm and the preset value.

[0158] For example, the default value is 2.

[0159] When the curvature sensitivity Tm is less than 2, no operation is performed;

[0160] When the curvature sensitivity Tm is greater than or equal to 2, the flight trajectory of the drone needs to be deleted;

[0161] It should be noted that the preset value 2 in the curvature sensitivity rule is a custom data range, and the specific numerical value can be adjusted by the user or system developer according to actual needs.

[0162] In one embodiment, the real flight status data of the drone may further include: basic energy consumption, which reflects the average energy consumption of the drone when flying without load;

[0163] In the actual three-dimensional grid matrix, the mapped path to be optimized is optimized in turn for speed, turning, and battery energy consumption to obtain the target path, which may include:

[0164] Obtaining grid energy consumption, flight altitude and load in the actual three-dimensional grid matrix; the grid energy consumption reflects the simulated energy consumption of the UAV in the actual three-dimensional grid matrix;

[0165] The energy consumption coefficient is calculated using the basic energy consumption and grid energy consumption according to the following formula;

[0166] Kg=(Te-Te0) / (Tv 2 +Th+Tw); where Kg is the energy consumption coefficient, Te is the grid energy consumption, Te0 is the basic energy consumption, Tv is the grid speed, Th is the flight altitude, and Tw is the load;

[0167] Regression analysis is performed on grid energy consumption, energy consumption coefficient, grid speed, flight altitude and load to obtain grid speed coefficient, flight altitude coefficient and load coefficient; the grid speed coefficient, flight altitude coefficient and load coefficient can represent the weights of grid speed, flight altitude and load respectively;

[0168] Based on the particle swarm optimization algorithm, the grid velocity coefficient Kv, the flight height coefficient Kh and the load coefficient Kl, the secondary optimization path is optimized to obtain the target path.

[0169] Specifically, Python's SciPy library (the SciPy library includes multiple functional modules such as numerical integration, differential equation solver, optimization algorithm, linear algebra operation, fast Fourier transform, etc.) can be used to calculate the secondary optimization path, grid velocity coefficient Kv, flight altitude coefficient Kh and load coefficient Kl to obtain the final target path.

[0170] You can also choose from a variety of intelligent algorithms such as genetic algorithms and gray wolf optimization. For example, in drone path planning, genetic algorithms can be used to search for the global optimal solution, continuously iterate fitness evaluation factors (the evaluation factors are calculated based on the secondary optimization path, grid speed coefficient Kv, flight altitude coefficient Kh and load coefficient Kl), and find the optimal flight path.

[0171] Figure 3 FIG. 1 is another specific example of the UAV motion planning method in the embodiment of the present invention. Figure 3 As shown, the method includes:

[0172] Step 301: During the flight of the drone, obstacle data is obtained through a laser sensor and transmitted to a remote processor, which constructs a three-dimensional map model; the three-dimensional map model includes three-dimensional models of multiple obstacles; the remote processor then transmits the three-dimensional map model to the drone; the drone and the remote processor are connected through a high-performance communication device;

[0173] Step 302: Calculate the first Euclidean distance Da from the current position of the UAV to the target node according to the current position information of the UAV and the position information of the target node in the three-dimensional map model; calculate the attractive potential energy of the UAV and the target node using the first Euclidean distance Da and the preset attractive potential weight; calculate the second Euclidean distance Db from the current position of the UAV to the nearest obstacle according to the current position information of the UAV and the position information of the nearest obstacle; calculate the repulsive potential energy of the UAV and the nearest obstacle according to the second Euclidean distance Db, the preset repulsive potential weight, and the maximum value Rb of the distance between the center position of the nearest obstacle and any point on the edge of the nearest obstacle; and obtain the total potential energy Uc using the attractive potential energy and the repulsive potential energy;

[0174] Step 303: determine the flight range in the three-dimensional map model according to the maximum distance that the UAV can fly; calculate the obstacle density within the flight range; and calculate the current step length Kd according to the obstacle density and the preset rated parameters;

[0175] Step 304: Obtain the path coordinate points that the UAV has flown through recorded by the positioning recorder; calculate the sum of the actual costs between all two adjacent coordinate points in the path coordinate points that have been flown through, and output it as the total path cost G;

[0176] Step 305, calculate the cost value F based on the total potential energy Uc, the total path cost G, the current step length kd, and the first Euclidean distance Da from the current position of the drone to the target node;

[0177] Step 306: obtain the maximum safe speed Va, turning radius Rc, and navigation accuracy Dc of the pre-tested UAV; determine the grid size Dk according to the maximum safe speed Va, turning radius Rc, and navigation accuracy Dc; and construct a three-dimensional grid matrix according to the grid size Dk;

[0178] Step 307: Process the total potential energy Uc, the cost value F, and the three-dimensional map model based on the APFA algorithm to obtain the path to be optimized;

[0179] Step 308: Map the path to be optimized and obstacles within the flight range to a three-dimensional grid matrix to obtain an actual three-dimensional grid matrix;

[0180] Step 309: digitally simulate and generate the path to be optimized in the actual three-dimensional grid matrix to obtain a simulated path, and optimize the speed, turning, and battery energy consumption of the simulated path to obtain a target path.

[0181] In summary, the embodiments of the present invention: 1) adopt a dynamic step size. Compared with the existing inherent step size, the dynamic step size can adaptively adjust the step size according to the current state of the algorithm or the information in the iteration process, while the static step size remains unchanged during the entire iteration process. Through adaptive adjustment, the dynamic step size can accelerate the convergence speed; 2) achieve efficient response. The embodiments of the present invention obtain the total potential energy Uc of the drone position, the total path cost, etc. through simple calculation steps, and perform speed optimization, turning optimization, and battery energy consumption optimization in turn, which can quickly and accurately further determine the next motion trajectory.

[0182] The present invention also provides a UAV motion planning device, as described in the following embodiments. Since the principle of solving the problem by the device is similar to that of the UAV motion planning method, the implementation of the device can refer to the implementation of the UAV motion planning method, and the repeated parts will not be repeated.

[0183] Figure 4 FIG. 1 is a schematic diagram of a UAV motion planning device according to an embodiment of the present invention. Figure 4 As shown, the device comprises:

[0184] A map model building module 401 is used to obtain obstacle point cloud data, perform three-dimensional reconstruction, and obtain a map model;

[0185] The total potential energy calculation module 402 is used to obtain the current position of the drone, the target node position, and the nearest obstacle information according to the map model; the nearest obstacle information includes the maximum value of the distance between the center position of the nearest obstacle and any point on the edge of the nearest obstacle; based on the artificial potential field method, the total potential energy is calculated using the current position of the drone, the target node position, and the nearest obstacle information;

[0186] The path cost calculation module 403 is used to obtain the flight route of the UAV and calculate the path cost according to the flight route; the path cost reflects the total distance flown;

[0187] The obstacle density calculation module 404 is used to determine the flight range in the map model according to the maximum distance that the UAV can fly and the current position of the UAV, and calculate the obstacle density within the flight range; the obstacle density reflects the density of obstacle distribution within the flight range;

[0188] The total cost value calculation module 405 is used to input the total potential energy, path cost, obstacle density, current position of the drone, and target node position into a preset cost function, and output a total cost value; the preset cost function is used to calculate the total cost value of the drone flying to the target node;

[0189] The module 406 for determining the path to be optimized is used to plan the path of the UAV to the target node in the map model based on the artificial potential field method and the total cost value to obtain the path to be optimized;

[0190] The target path calculation module 407 is used to map the map model, the path to be optimized, and the actual flight status data of the UAV to a three-dimensional grid matrix to obtain an actual three-dimensional grid matrix for simulating the flight of the UAV; in the actual three-dimensional grid matrix, the mapped path to be optimized is optimized in turn for speed, turning, and battery energy consumption to obtain a target path; the target path is used to control the UAV to fly to the target node.

[0191] In one embodiment, the map model construction module is specifically used to:

[0192] Obtain obstacle point cloud data through the laser sensor carried by the drone;

[0193] The obstacle point cloud data is 3D reconstructed using high-speed computing hardware and 3D modeling tools to obtain a map model; the high-speed computing hardware includes: CPU, GPU, professional graphics workstation, and cloud computing resources.

[0194] In one embodiment, the total potential energy calculation module is specifically used for:

[0195] According to the current position of the UAV and the position of the target node, the attraction potential energy between the UAV and the target node is determined;

[0196] According to the current position of the drone and the information of the nearest obstacle, determine the repulsive potential energy between the drone and the nearest obstacle;

[0197] The sum of the attractive potential energy and the repulsive potential energy is taken as the total potential energy.

[0198] In one embodiment, the total potential energy calculation module is specifically used for:

[0199] According to the current position of the UAV and the position of the target node, the first Euclidean distance from the current position of the UAV to the target node is calculated;

[0200] Calculate the attraction potential energy between the UAV and the target node according to the first Euclidean distance and the preset attraction potential weight; the preset attraction potential weight is used to adjust the attraction potential energy strength;

[0201] According to the current position of the drone and the position of the nearest obstacle, the second Euclidean distance from the current position of the drone to the nearest obstacle is calculated;

[0202] According to the following formula, the repulsive potential energy between the drone and the nearest obstacle is calculated based on the second Euclidean distance, the maximum value of the distance between the center of the nearest obstacle and any point on the edge of the nearest obstacle, and the preset repulsive potential weight:

[0203] Ub=Kb×max(0,Db-Rb) 2 ;

[0204] Where Ub is the repulsive potential energy, Kb is the preset repulsive potential weight, which is used to adjust the strength of the repulsive potential, Db is the second Euclidean distance, and Rb is the maximum value of the distance between the center of the nearest obstacle and any point on the edge of the nearest obstacle.

[0205] In one embodiment, the flown route includes a plurality of coordinate points;

[0206] The path cost calculation module is specifically used for:

[0207] Calculate the distance between two adjacent coordinate points in sequence to obtain multiple distance values;

[0208] Outputs the sum of multiple distance values ​​as the path cost.

[0209] In one embodiment, the obstacle density calculation module is specifically used to:

[0210] With the current position of the drone as the dot, the maximum distance the drone can fly is the radius, and the flight range is determined within the range of the map model.

[0211] In one embodiment, the obstacle density calculation module is specifically used to:

[0212] Obstacle density within the flight range is calculated using the following formula:

[0213] Yc=Yf / Ym ;

[0214] In the formula, Y is the obstacle density, Yf is the flight range, Y is the number of obstacles within the flight range.

[0215] In one embodiment, the total cost value calculation module is specifically used to:

[0216] F = G + Kd × Da + Uc;

[0217] Kd=1 / (Yc+w);

[0218] Where F is the total cost value; G is the path cost; Kd is the current calculation step size, and is the intermediate calculation amount; Yc is the obstacle density; w is the set rated parameter used to avoid division by zero; Da is the first Euclidean distance calculated based on the current position of the drone and the target node position; Uc is the total potential energy.

[0219] In one embodiment, the actual flight status data of the drone includes: the initial speed of the drone and the minimum safe distance; the minimum safe distance indicates the distance value that ensures the safety of the drone flight;

[0220] The target path calculation module is specifically used for:

[0221] In the actual three-dimensional grid matrix, the distance from the UAV to the nearest obstacle is calculated by curve fitting to obtain the speed adjustment coefficient;

[0222] It should be noted that, in the present invention, the speed adjustment coefficient is a vector acceleration, that is, it represents the acceleration of the corresponding direction or angle.

[0223] The change speed is calculated according to the following formula; the change speed is used to determine whether to adjust the flight speed of the drone to the change speed according to the preset speed adjustment rule;

[0224] Tv1=Tv0+Ke×(Db-Df);

[0225] Where Tv1 is the speed of change, Tv0 is the initial speed of the UAV, Ke is the speed adjustment coefficient, Db is the second Euclidean distance from the current position of the UAV to the nearest obstacle, and Df is the minimum safe distance;

[0226] The speed is optimized according to the preset speed adjustment rules, the changing speed and the minimum safety distance to obtain an optimized path; the preset speed adjustment rules include determining whether to adjust the flight speed of the drone according to the difference between the changing speed and the minimum safety distance.

[0227] In one embodiment, the target path calculation module is specifically used to:

[0228] Obtain the grid speed and grid time point in the actual three-dimensional grid matrix; the grid speed reflects the simulated speed of the UAV in the actual three-dimensional grid matrix;

[0229] The grid acceleration, grid curvature, and curvature sensitivity are calculated from the grid velocity and grid time point according to the following formula:

[0230] Ta = Tv / Tt;

[0231] Tk=|Tv×Ta| / |Tv| 3 ;

[0232] Tm=dTv / dTk;

[0233] Where Ta is the grid acceleration, Tk is the grid curvature, Tm is the curvature sensitivity, Tv is the grid velocity, and Tt is the grid time point;

[0234] In the embodiment of the present invention, the grid is a division interval, the grid acceleration and the grid curvature represent the acceleration of the corresponding grid and the curvature of the grid path, which are similar to the acceleration of a function coordinate point and the curvature of a function curve, and the curvature sensitivity represents the sensitivity of the curvature change;

[0235] According to the curvature sensitivity and the preset curvature sensitivity rules, the primary optimization path is optimized for turning to obtain the secondary optimization path; the preset curvature sensitivity rules include: according to the comparison result of the curvature sensitivity and the preset value, determining whether to delete the coordinate points within the set range of the current position of the drone in the primary optimization path.

[0236] In one embodiment, the real flight status data of the drone also includes: basic energy consumption;

[0237] The target path calculation module is specifically used for:

[0238] Obtaining grid energy consumption, flight altitude and load in the actual three-dimensional grid matrix; the grid energy consumption reflects the simulated energy consumption of the UAV in the actual three-dimensional grid matrix;

[0239] The energy consumption coefficient is calculated using the basic energy consumption and grid energy consumption according to the following formula;

[0240] Kg=(Te-Te0) / (Tv 2 +Th+Tw); where Kg is the energy consumption coefficient, Te is the grid energy consumption, Te0 is the basic energy consumption, Tv is the grid speed, Th is the flight altitude, and Tw is the load;

[0241] Regression analysis is performed on grid energy consumption, energy consumption coefficient, grid speed, flight altitude and load to obtain grid speed coefficient, flight altitude coefficient and load coefficient;

[0242] Based on the particle swarm optimization algorithm, grid speed coefficient, flight height coefficient and load coefficient, the secondary optimization path is optimized to obtain the target path.

[0243] In one embodiment, the grid size of the three-dimensional grid matrix is ​​obtained in advance as follows:

[0244] Obtain the maximum safe speed, turning radius, navigation accuracy, speed and acceleration of the drone during turning flight;

[0245] The grid size is calculated as follows:

[0246] Dk = (Va × Dc) / Rc;

[0247] Rc=Vb 2 / Aa;

[0248] Where Dk is the grid size, Va is the maximum safe speed of the UAV, Dc is the navigation accuracy, Rc is the turning radius, Vb and Aa are the speed and acceleration of the UAV during turning flight, respectively.

[0249] Figure 5 Schematic diagram of a computer device in an embodiment of the present invention. Figure 5 As shown, an embodiment of the present invention further provides a computer device 500, including a processor 501, a memory 502, and a computer program 503 stored in the memory 502 and executable on the processor 501, and the processor 501 implements the above-mentioned UAV motion planning method when executing the computer program 503.

[0250] An embodiment of the present invention further provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the above-mentioned drone motion planning method is implemented.

[0251] An embodiment of the present invention further provides a computer program product, which includes a computer program, and when the computer program is executed by a processor, the above-mentioned drone motion planning method is implemented.

[0252] In the embodiment of the present invention, obstacle point cloud data is obtained, three-dimensional reconstruction is performed, and a map model is obtained; according to the map model, the current position of the drone, the target node position, and the nearest obstacle information are obtained; the nearest obstacle information includes the maximum value of the distance between the center position of the nearest obstacle and any point on the edge of the nearest obstacle; based on the artificial potential field method, the total potential energy is calculated using the current position of the drone, the target node position, and the nearest obstacle information; the flight route of the drone is obtained, and the path cost is calculated according to the flight route; the path cost reflects the total distance flown; according to the farthest distance that the drone can fly and the current position of the drone, the flight range is determined in the map model, and the obstacle density within the flight range is calculated; the obstacle density reflects the distribution of obstacles within the flight range. The method comprises the following steps: determining the density of the distribution of the UAV; inputting the total potential energy, path cost, obstacle density, current position of the UAV and position of the target node into a preset cost function, and outputting a total cost value; the preset cost function is used to calculate the total cost value of the UAV flying to the target node; based on the artificial potential field method, the total cost value is used to plan the path of the UAV flying to the target node in the map model to obtain the path to be optimized; mapping the map model, the path to be optimized and the actual flight status data of the UAV to a three-dimensional grid matrix to obtain an actual three-dimensional grid matrix for simulating the flight of the UAV; in the actual three-dimensional grid matrix, the mapped path to be optimized is optimized in turn for speed, turning and battery energy consumption to obtain a target path; the target path is used to control the UAV to fly to the target node. In the embodiment of the present invention, in the UAV motion planning, the artificial potential field method is first used to plan the path of the UAV to the target node in the map model using the total cost value to obtain the path to be optimized, and then the speed optimization, turning optimization, and battery energy consumption optimization are performed on the path to be optimized in the actual three-dimensional grid matrix to obtain the final target path. The UAV can be safely and accurately controlled to fly according to the set speed, direction, and energy consumption. The total cost value is calculated using the obstacle density within the flight range of the current position of the UAV. The total cost value can be updated according to the position of the UAV, so that dynamic obstacles can be effectively and safely avoided in real time, realizing real-time, safe, and accurate path planning.

[0253] It will be appreciated by those skilled in the art that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Furthermore, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0254] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0255] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0256] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process in the computer or other programmable device. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0257] The specific embodiments described above further illustrate the objectives, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above description is only a specific embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, 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 UAV motion planning method, characterized in that: include: Obtain obstacle point cloud data, perform 3D reconstruction, and obtain a map model; According to the map model, obtain the current position of the drone, the target node position, and the nearest obstacle information; The nearest obstacle information includes the maximum value of the distance between the center position of the nearest obstacle and any point on the edge of the nearest obstacle; Based on the artificial potential field method, the total potential energy is calculated using the current position of the drone, the target node position, and the nearest obstacle information; Obtain the flight route of the drone and calculate the path cost based on the flight route; The path cost reflects the total distance flown; According to the maximum distance the drone can fly and the current location of the drone, the flight range is determined in the map model, and the density of obstacles within the flight range is calculated; The obstacle density reflects the density of obstacle distribution within the flight range; The total potential energy, path cost, obstacle density, current position of the drone, and target node position are input into a preset cost function, and a total cost value is output; the preset cost function is used to calculate the total cost value of the drone flying to the target node; Based on the artificial potential field method, the total cost value is used to plan the path of the UAV to the target node in the map model to obtain the path to be optimized; Mapping the map model, the path to be optimized, and the actual flight status data of the UAV to a three-dimensional grid matrix to obtain an actual three-dimensional grid matrix for simulating the flight of the UAV; In the actual three-dimensional grid matrix, the mapped path to be optimized is optimized in turn for speed, turning, and battery energy consumption to obtain a target path; the target path is used to control the UAV to fly to the target node; Among them, based on the artificial potential field method, the total potential energy is calculated using the current position of the drone, the target node position, and the nearest obstacle information, including: determining the attractive potential energy of the drone and the target node according to the current position of the drone and the target node position; determining the repulsive potential energy of the drone and the nearest obstacle according to the current position of the drone and the nearest obstacle information; and taking the sum of the attractive potential energy and the repulsive potential energy as the total potential energy; Among them, according to the current position of the drone and the position of the target node, the attraction potential energy of the drone and the target node is determined, including: According to the current position of the UAV and the position of the target node, the first Euclidean distance from the current position of the UAV to the target node is calculated; Calculate the attraction potential energy between the UAV and the target node according to the first Euclidean distance and the preset attraction potential weight; the preset attraction potential weight is used to adjust the attraction potential energy strength; According to the current position of the drone and the nearest obstacle information, the repulsive potential energy between the drone and the nearest obstacle is determined, including: According to the current position of the drone and the position of the nearest obstacle, the second Euclidean distance from the current position of the drone to the nearest obstacle is calculated; According to the following formula, the repulsive potential energy between the drone and the nearest obstacle is calculated based on the second Euclidean distance, the maximum value of the distance between the center of the nearest obstacle and any point on the edge of the nearest obstacle, and the preset repulsive potential weight: Ub=Kb×max(0,Db-Rb) 2 ; Where Ub is the repulsive potential energy, Kb is the preset repulsive potential weight, which is used to adjust the strength of the repulsive potential, Db is the second Euclidean distance, and Rb is the maximum value of the distance between the center of the nearest obstacle and any point on the edge of the nearest obstacle.

2. The UAV motion planning method according to claim 1, characterized in that: Obtain obstacle point cloud data, perform 3D reconstruction, and obtain a map model, including: Obtain obstacle point cloud data through the laser sensor carried by the drone; The obstacle point cloud data is 3D reconstructed using high-speed computing hardware and 3D modeling tools to obtain a map model; the high-speed computing hardware includes: CPU, GPU, professional graphics workstation, and cloud computing resources.

3. The UAV motion planning method according to claim 1, characterized in that: The flown route includes a plurality of coordinate points; Calculates path costs based on flown routes, including: Calculate the distance between two adjacent coordinate points in sequence to obtain multiple distance values; Outputs the sum of multiple distance values ​​as the path cost.

4. The UAV motion planning method according to claim 1, characterized in that: According to the maximum distance the drone can fly and the current location of the drone, the flight range is determined in the map model, including: With the current position of the drone as the dot, the maximum distance the drone can fly is the radius, and the flight range is determined within the range of the map model.

5. The UAV motion planning method according to claim 1, characterized in that: Obstacle density within the flight range is calculated, including: Obstacle density within the flight range is calculated using the following formula: Yc=Yf / Ym ； In the formula, Y is the obstacle density, Yf is the flight range, Y is the number of obstacles within the flight range.

6. The UAV motion planning method according to any one of claims 1 to 5, characterized in that: The total potential energy, path cost, obstacle density, current position of the drone, and target node position are input into the preset cost function, and the total cost value is output, including: F = G + Kd × Da + Uc; Kd=1 / (Yc+w); Where F is the total cost value; G is the path cost; Kd is the current calculation step size, and is the intermediate calculation amount; Yc is the obstacle density; w is the set rated parameter used to avoid division by zero; Da is the first Euclidean distance calculated based on the current position of the drone and the target node position; Uc is the total potential energy.

7. The UAV motion planning method according to claim 1, characterized in that: The actual flight status data of the UAV includes: the initial speed of the UAV and the minimum safe distance; the minimum safe distance indicates the distance value that ensures the flight safety of the UAV; In the actual three-dimensional grid matrix, the mapped path to be optimized is optimized in turn for speed, turning, and battery energy consumption to obtain the target path, including: In the actual three-dimensional grid matrix, the distance from the UAV to the nearest obstacle is calculated by curve fitting to obtain the speed calibration coefficient; the speed calibration coefficient represents the acceleration of the direction or angle; The change speed is calculated according to the following formula; the change speed is used to determine whether to adjust the flight speed of the drone to the change speed according to the preset speed adjustment rule; Tv1=Tv0+Ke×(Db-Df); Where Tv1 is the speed of change, Tv0 is the initial speed of the UAV, Ke is the speed adjustment coefficient, Db is the second Euclidean distance from the current position of the UAV to the nearest obstacle, and Df is the minimum safe distance; The speed is optimized according to the preset speed adjustment rules, the changing speed and the minimum safety distance to obtain an optimized path; the preset speed adjustment rules include determining whether to adjust the flight speed of the drone according to the difference between the changing speed and the minimum safety distance.

8. The UAV motion planning method according to claim 7, characterized in that: In the actual three-dimensional grid matrix, the mapped path to be optimized is optimized in turn for speed, turning, and battery energy consumption to obtain the target path, including: Obtain the grid speed and grid time point in the actual three-dimensional grid matrix; the grid speed reflects the simulated speed of the UAV in the actual three-dimensional grid matrix; The grid acceleration, grid curvature, and curvature sensitivity are calculated from the grid velocity and grid time point according to the following formula: Ta = Tv / Tt; Tk=|Tv×Ta| / |Tv| 3 ; Tm=dTv / dTk; Where Ta is the grid acceleration, Tk is the grid curvature, Tm is the curvature sensitivity, Tv is the grid velocity, and Tt is the grid time point; the grid acceleration and grid curvature represent the grid acceleration and grid path curvature, and the curvature sensitivity reflects the sensitivity of curvature change; According to the curvature sensitivity and the preset curvature sensitivity rules, the primary optimization path is optimized for turning to obtain the secondary optimization path; the preset curvature sensitivity rules include: according to the comparison result of the curvature sensitivity and the preset value, determining whether to delete the coordinate points within the set range of the current position of the drone in the primary optimization path.

9. The UAV motion planning method according to claim 8, characterized in that: The actual flight status data of the UAV also includes: basic energy consumption; the basic energy consumption reflects the average energy consumption of the UAV when flying without load; In the actual three-dimensional grid matrix, the mapped path to be optimized is optimized in turn for speed, turning, and battery energy consumption to obtain the target path, including: Obtaining grid energy consumption, flight altitude and load in the actual three-dimensional grid matrix; the grid energy consumption reflects the simulated energy consumption of the UAV in the actual three-dimensional grid matrix; The energy consumption coefficient is calculated using the basic energy consumption and grid energy consumption according to the following formula; Kg=(Te-Te0) / (Tv 2 +Th+Tw); where Kg is the energy consumption coefficient, Te is the grid energy consumption, Te0 is the basic energy consumption, Tv is the grid speed, Th is the flight altitude, and Tw is the load; Regression analysis is performed on grid energy consumption, energy consumption coefficient, grid speed, flight altitude and load to obtain grid speed coefficient, flight altitude coefficient and load coefficient; the grid speed coefficient, flight altitude coefficient and load coefficient represent the weights of grid speed, flight altitude and load respectively; Based on the particle swarm optimization algorithm, grid speed coefficient, flight height coefficient and load coefficient, the secondary optimization path is optimized to obtain the target path.

10. The UAV motion planning method according to claim 1, characterized in that: The grid size of the three-dimensional grid matrix is ​​obtained in advance as follows: Obtain the maximum safe speed, turning radius, navigation accuracy, speed and acceleration of the drone during turning flight; Calculate the grid size as follows: Dk = (Va × Dc) / Rc; Rc=Vb 2 / Aa; Where Dk is the grid size, Va is the maximum safe speed of the UAV, Dc is the navigation accuracy, Rc is the turning radius, Vb and Aa are the speed and acceleration of the UAV during turning flight, respectively.

11. A UAV motion planning device, characterized in that: include: The map model building module is used to obtain obstacle point cloud data, perform three-dimensional reconstruction, and obtain a map model; The total potential energy calculation module is used to obtain the current position of the drone, the target node position, and the nearest obstacle information based on the map model; The nearest obstacle information includes the maximum value of the distance between the center position of the nearest obstacle and any point on the edge of the nearest obstacle; Based on the artificial potential field method, the total potential energy is calculated using the current position of the drone, the target node position, and the nearest obstacle information; The path cost calculation module is used to obtain the flight route of the drone and calculate the path cost based on the flight route; the path cost reflects the total distance flown; The obstacle density calculation module is used to determine the flight range in the map model according to the maximum distance the drone can fly and the current position of the drone, and calculate the obstacle density within the flight range; The obstacle density reflects the density of obstacle distribution within the flight range; A total cost value calculation module is used to input the total potential energy, path cost, obstacle density, current position of the drone, and target node position into a preset cost function, and output a total cost value; the preset cost function is used to calculate the total cost value of the drone flying to the target node; The module for determining the path to be optimized is used to plan the path of the UAV to the target node in the map model based on the artificial potential field method and the total cost value to obtain the path to be optimized; The target path calculation module is used to map the map model, the path to be optimized, and the actual flight status data of the UAV to a three-dimensional grid matrix to obtain an actual three-dimensional grid matrix for simulating the flight of the UAV; in the actual three-dimensional grid matrix, the mapped path to be optimized is sequentially optimized in terms of speed, turning, and battery energy consumption to obtain a target path; the target path is used to control the UAV to fly to the target node; The total potential energy calculation module is specifically used to: determine the attractive potential energy between the drone and the target node according to the current position of the drone and the position of the target node; determine the repulsive potential energy between the drone and the nearest obstacle according to the current position of the drone and the nearest obstacle information; and take the sum of the attractive potential energy and the repulsive potential energy as the total potential energy; Among them, the total potential energy calculation module is specifically used for: According to the current position of the UAV and the position of the target node, the first Euclidean distance from the current position of the UAV to the target node is calculated; Calculate the attraction potential energy between the UAV and the target node according to the first Euclidean distance and the preset attraction potential weight; the preset attraction potential weight is used to adjust the attraction potential energy strength; According to the current position of the drone and the position of the nearest obstacle, the second Euclidean distance from the current position of the drone to the nearest obstacle is calculated; According to the following formula, the repulsive potential energy between the drone and the nearest obstacle is calculated based on the second Euclidean distance, the maximum value of the distance between the center of the nearest obstacle and any point on the edge of the nearest obstacle, and the preset repulsive potential weight: Ub=Kb×max(0,Db-Rb) 2 ; Where Ub is the repulsive potential energy, Kb is the preset repulsive potential weight, which is used to adjust the strength of the repulsive potential, Db is the second Euclidean distance, and Rb is the maximum value of the distance between the center of the nearest obstacle and any point on the edge of the nearest obstacle.

12. A computer device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the drone motion planning method according to any one of claims 1 to 10 is implemented.

13. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the drone motion planning method according to any one of claims 1 to 10 is implemented.

14. A computer program product, characterized in that The computer program product comprises a computer program, and when the computer program is executed by a processor, the drone motion planning method according to any one of claims 1 to 10 is implemented.

Images