Unmanned aerial vehicle inspection track generation method

Abstract

The invention relates to an unmanned aerial vehicle routing inspection track generation method, which comprises the following steps: firstly obtaining a starting point and an end point in voxel map data, and carrying out unmanned aerial vehicle flight path search by using an optimized path optimization algorithm based on the obtained data, so as to obtain an unmanned aerial vehicle flight path far away from an obstacle. Then, constructing a safe flight corridor of the unmanned aerial vehicle based on the obtained flight path of the unmanned aerial vehicle away from the obstacle, and generating a plurality of initial path points based on the generated safe flight corridor; then, based on the obtained multiple initial path points, a minimum control quantity track is used for track generation, a collision-free unmanned aerial vehicle flight track rotating at the minimum yaw angle is obtained, and track correction is carried out when the unmanned aerial vehicle passes through the inspection target point in the flight process; therefore, the pan-tilt camera on the unmanned aerial vehicle can effectively shoot the inspection target point.

Description

Technical Field

[0001] The present application relates to the field of drone technology, and in particular to a method for generating a drone inspection trajectory. Background Art

[0002] Map representation is indispensable in current drone inspections of indoor substations. However, most current map representation methods are generally composed of large amounts of discrete data. Directly introducing this massive data into the navigation task will cause repeated queries and access to data during the trajectory optimization process, thereby reducing the quality and efficiency of the final trajectory.

[0003] The current solution is to construct safe flight corridors. This approach has the advantage that when generating a flight path, the robot only needs to plan its trajectory within the safe flight corridor, eliminating the need to consider detailed information such as the shape and size of obstacles. This effectively solves the voxel map problem. However, the quality of traditional safe flight corridors depends on the quality of the seeds used to generate them. Poor seed quality can lead to failure in safe flight corridor generation, and subsequently, failure in subsequent trajectory generation. Furthermore, when flying indoors, drones can experience excessive yaw angle rotation due to factors such as obstacle avoidance and mission objectives. However, meaningless rotation consumes energy and can also cause unstable flight. Summary of the Invention

[0004] Based on this, it is necessary to provide a method for generating inspection trajectories for drones, which can cause excessive yaw angle rotation when flying indoors due to reasons such as obstacle avoidance and mission objectives. However, meaningless rotation consumes energy and causes unstable flight.

[0005] This application provides a method for generating a drone inspection trajectory, comprising:

[0006] Acquiring voxel map data, wherein the voxel map data includes a starting point and an end point;

[0007] Based on the starting and ending points in the voxel map data, the optimized path optimization algorithm is used to generate the UAV trajectory and obtain the initial path of the UAV;

[0008] Generate a safe flight corridor based on the drone’s initial path;

[0009] Generate multiple initial path points for drones based on safe flight corridors;

[0010] Based on multiple UAV initial path points, the minimum control amount trajectory is used to generate the trajectory and obtain the preliminary optimized UAV initial path;

[0011] Obtain at least one inspection target point;

[0012] The initially optimized UAV initial path is further optimized based on the inspection requirements of the inspection target point to obtain a further optimized UAV initial path, and the further optimized UAV initial path is defined as the UAV target path;

[0013] Output the drone target path.

[0014] Furthermore, the UAV trajectory is generated using an optimized path optimization algorithm based on the starting point and the end point in the voxel map data to obtain the initial path of the UAV, including:

[0015] Define the center point of each grid in the voxel map as a node;

[0016] A movement cost evaluation function of a node in a voxel map is defined; the expression of the movement cost evaluation function of the node is shown in Formula 1;

[0017] F(n)=G(n)+H(n)+ρJ(n)+μK(n) Formula 1;

[0018] Among them, F(n) is the movement cost evaluation function of node n; G(n) is the movement cost from the starting point to node n when moving along the generated path; H(n) is the movement cost from node n to the end point; J(n) is the obstacle penalty of node n; K(n) is the direction change penalty of node n; ρ is the weight of the obstacle penalty of node n; μ is the weight of the direction change penalty of node n.

[0019] Furthermore, before generating the UAV trajectory using the optimized path optimization algorithm based on the starting point and the end point in the voxel map data to obtain the UAV initial path, the method further includes:

[0020] Calculate the cost of moving from the starting point to the end point and get the moving cost value;

[0021] Get the preset movement cost threshold;

[0022] Determine whether the obtained movement cost value is less than or equal to a preset movement cost threshold;

[0023] If the obtained movement cost value is less than or equal to the preset movement cost threshold, the A* algorithm is used to generate the drone trajectory based on the starting point and end point in the voxel map data to obtain the drone's initial path;

[0024] If the obtained movement cost value is greater than the preset movement cost threshold, the optimized A* algorithm is used to generate the UAV trajectory based on the starting point and end point in the voxel map data to obtain the UAV initial path.

[0025] Furthermore, the method of generating a UAV trajectory using an optimized A* algorithm based on the starting point and the end point in the voxel map data to obtain the UAV initial path further includes:

[0026] Get the starting point and the end point;

[0027] Creating a first list and a second list; the first list is used to store nodes for which movement costs are to be calculated, and the second list is used to store nodes that have been visited;

[0028] Set the starting point as the current processing node;

[0029] Exploring eight directions around the current processing node as exploration directions to obtain multiple nodes, storing the multiple nodes in a first queue, using the current processing node as the parent node of these nodes, and placing the current processing node in a second queue; the exploration step length of the current processing node in the eight directions around the current processing node is three nodes;

[0030] Obtain exploration information in eight directions around the current processing node respectively;

[0031] Marking the exploration direction that touches the obstacle in the exploration information, and storing the marked direction and the three nodes in the marked direction in a second list;

[0032] The exploration nodes in two adjacent exploration directions of the marked exploration direction are stored in the non-omnidirectional exploration obstacle node list;

[0033] Calculate the F(n) value of each node in the second list based on Formula 1 to obtain the F(n) value of each node;

[0034] Filter out the node with the smallest F(n) value in the second list, and use the node with the smallest F(n) value as the new current processing node;

[0035] Determine whether the new current processing node is in the non-omnidirectional exploration obstacle node list;

[0036] If the new current processing node is in the list of non-omnidirectional exploration obstacle nodes, the remaining directions after removing the obstacle directions from the eight directions around the new current processing node are used as exploration directions for exploration until the end point is found;

[0037] If the new current processing node is not in the list of non-omnidirectional exploration obstacle nodes, the eight directions around the new current processing node are used as exploration directions to explore until the end point is found;

[0038] After finding the end point, start from the end point and search for the parent node of each node in turn. Move gradually toward the parent node according to the order in which the child nodes point to the parent node until it reaches the starting point. Connect each node passed during the movement in turn, and use the generated path as the initial path of the drone.

[0039] Furthermore, generating a safe flight corridor based on the initial path of the UAV includes:

[0040] Generate an initial spherical convex hull based on the starting point of the UAV's initial path;

[0041] The initial path of the UAV is used as the search target, the moving direction of the UAV is used as the search direction, and the first point not in the initial spherical convex hull is defined as the center of the sampling area.

[0042] Generate a spherical sampling area based on the center of the sampling area;

[0043] The spherical sampling area is uniformly sampled to obtain multiple sampling points;

[0044] Calculate the score of each sampling point, select the sampling point with the highest score, and use the sampling point with the highest score as the center of the next spherical convex hull;

[0045] Obtain the spherical convex hull formed by the sampling point with the highest score, and define the spherical convex hull as the new initial spherical convex hull;

[0046] Return to executing the search with the drone's initial inspection path as the search target and the drone's movement direction as the search direction. Define the first point not in the initial spherical convex hull as the center of the sampling area, and continue until the last spherical convex hull generated includes the end point of the drone's initial inspection path.

[0047] Furthermore, the calculation formula for the radius of the spherical sampling area is shown in Formula 2;

[0048]

[0049] Among them, r is the radius of the spherical sampling area; o is the coordinate of the center of the spherical sampling area; s l The coordinates of the center of the spherical convex hull generated last time.

[0050] Furthermore, the generation of multiple UAV initial path points based on the safe flight corridor includes:

[0051] Calculate the coordinates of the initial path point of the drone according to formula 3;

[0052]

[0053] Among them, p i is the coordinate of the initial path point of the i-th UAV; is the position of the i-th spherical convex hull; is the position of the i+1th spherical convex hull; is the radius of the convex hull of the i-th sphere; is the radius of the i+1th spherical convex hull; is the vector modulus of the difference between the position of the i-th spherical convex hull and the position of the i+1-th spherical convex hull.

[0054] Furthermore, the minimum control amount trajectory is used to generate trajectories based on the multiple generated UAV initial path points to obtain a preliminarily optimized UAV initial path, including:

[0055] The multiple UAV initial path points are sorted based on the generation order of each convex hull in the safe flight corridor to obtain a sequence table of UAV initial path points;

[0056] Based on the coordinates of the starting point and the coordinates of the UAV initial path point closest to the starting point obtained based on the sequence table of the UAV initial path points, a first optimized path is generated using a minimum control amount trajectory;

[0057] The coordinates of the drone initial path point closest to the starting point obtained based on the drone initial path point sequence table are used as the coordinates of the new starting point;

[0058] Returning to the step of generating a first optimized path using the minimum control amount trajectory by using the coordinates of the starting point and the coordinates of the drone initial path point closest to the starting point obtained based on the drone initial path point sequence table, until each drone initial path point is selected once, thereby obtaining a plurality of first optimized paths;

[0059] The path obtained by sequentially connecting the multiple first optimized paths is defined as the initial path of the preliminarily optimized UAV.

[0060] Furthermore, the method of generating trajectories based on the generated multiple UAV initial path points using a minimum control amount trajectory to obtain a preliminarily optimized UAV initial path further includes:

[0061] Construct a multi-objective optimization polynomial as shown below;

[0062]

[0063]

[0064]

[0065]

[0066] |ψ′|≤|ψ′| max ;

[0067] in, is the jerk of the UAV trajectory at time t; ψ(t) is the maximum angular velocity constraint of the UAV at time t; ρ is the weight of the maximum time tolerance for the UAV to complete the task; The maximum flight speed limit for the drone during flight; The maximum flight acceleration limit of the drone during flight; is the safe flight corridor restriction function, where The i-th trajectory is discretized into a set of several points, τ i is the i-th convex hull, further The maximum time tolerance for the UAV to complete the mission; Share the initial and final state constraints of the intermediate points for this trajectory; Share the initial state constraints of the intermediate point for this trajectory; Share the end state limit of the intermediate point for this trajectory; Share the intermediate point status restriction for this segment of trajectory; The maximum angular velocity limit for the drone gimbal; The maximum angle limit of the gimbal is |ψ′|≤|ψ′| max is the maximum angular velocity constraint of the UAV.

[0068] Furthermore, the inspection requirement based on the inspection target point further optimizes the initially optimized UAV initial path to obtain a further optimized initially optimized UAV initial path, and the further optimized initially optimized UAV initial path is defined as the UAV target path, including:

[0069] Obtaining inspection requirements for each inspection target point, wherein the inspection requirements for the inspection target point include field of view angle requirements and depth of field requirements;

[0070] Select an inspection target point;

[0071] Analyze the inspection target point and obtain the UAV initial path point closest to the inspection target point;

[0072] Analyze the initial path point of the UAV to obtain two first optimized paths adjacent to the initial path point of the UAV;

[0073] Optimizing the two obtained first optimization paths based on the field of view angle requirement and the depth of field requirement to obtain two optimized first optimization paths, and replacing the two first optimization paths with the two optimized first optimization paths;

[0074] Return to select a patrol target point until each patrol target point has been selected once, and obtain the preliminary optimized UAV initial path after further optimization.

[0075] The present application relates to a method for generating a drone inspection trajectory. The method first obtains the starting point and end point in the voxel map data, and uses an optimized path optimization algorithm based on the obtained data to search for the drone flight path, thereby obtaining a drone flight path away from obstacles. Then, a drone safe flight corridor is constructed based on the obtained drone flight path away from obstacles, and multiple initial path points are generated based on the generated safe flight corridor. Then, based on the obtained multiple initial path points, a minimum control amount trajectory is used to generate a trajectory, thereby obtaining a drone flight trajectory with no collision and minimum yaw angle rotation. The trajectory of the drone is then corrected when it passes through the inspection target point during flight, so that the gimbal camera on the drone can effectively capture the inspection target point. BRIEF DESCRIPTION OF THE DRAWINGS

[0076] Figure 1 A flowchart of a method for generating a drone inspection trajectory according to an embodiment of the present application is provided.

[0077] Figure 2 A schematic diagram of the path exploration of the optimized path optimization algorithm in the drone inspection trajectory generation method provided in one embodiment of the present application.

[0078] Figure 3 A schematic diagram of obstacle location exploration node marking in the drone inspection trajectory generation method provided in one embodiment of the present application.

[0079] Figure 4 A schematic diagram of a safe flight corridor generation method for generating a drone inspection trajectory according to an embodiment of the present application.

[0080] Figure 5 A schematic diagram of generating initial path points for a drone in a method for generating a drone inspection trajectory provided in one embodiment of the present application.

[0081] Figure 6 A schematic diagram of calculating the initial path points of a drone in the drone inspection trajectory generation method provided in one embodiment of the present application.

[0082] Figure 7 This is a schematic diagram of a drone inspection trajectory for a single cabinet in the drone inspection trajectory generation method provided in one embodiment of the present application.

[0083] Figure 8 This is a schematic diagram of multiple cabinet drone inspection trajectories in the drone inspection trajectory generation method provided in one embodiment of the present application.

[0084] Figure 9Schematic diagram of the drone inspection simulation path of the algorithm in Reference 1.

[0085] Figure 10 This is a schematic diagram of the drone inspection simulation path of the algorithm in Reference 2. DETAILED DESCRIPTION

[0086] In order to make the purpose, technical solutions and advantages of this application more clearly understood, the present application is further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain this application and are not intended to limit this application.

[0087] like Figure 1 As shown, in one embodiment of the present application, the method for generating a drone inspection trajectory includes the following steps S100 to S800:

[0088] S100 , obtaining voxel map data, where the voxel map data includes a starting point and an end point.

[0089] Specifically, the starting point refers to the starting point of the drone inspection, which may be a point along the drone's route or the drone's take-off point. The corresponding end point refers to the destination after the drone completes the inspection mission, which may be a point along the drone's route or the drone's landing point.

[0090] S200 , generating a UAV trajectory using an optimized path optimization algorithm based on the starting point and the end point in the voxel map data to obtain an initial UAV path.

[0091] S300, generates a safe flight corridor based on the drone’s initial path.

[0092] S400, generating multiple UAV initial path points based on the safe flight corridor.

[0093] S500: Based on the initial path points of the multiple UAVs, a minimum control amount trajectory is used to generate a trajectory to obtain a preliminarily optimized initial path of the UAV.

[0094] S600: Acquire at least one inspection target point.

[0095] S600: Further optimize the preliminarily optimized initial path of the UAV based on the inspection requirements of the inspection target point to obtain a further optimized initial path of the UAV, and define the further optimized initial path of the UAV as the target path of the UAV.

[0096] S800, output the drone target path.

[0097] In this embodiment, a drone flight path that avoids obstacles is obtained by first acquiring the starting and ending points from the voxel map data and then using an optimized path optimization algorithm to search for the drone's flight path. A safe flight corridor for the drone is then constructed based on the obtained obstacle-free flight path, and multiple initial path points are generated based on the generated safe flight corridor. Trajectory generation is then performed using a minimum control trajectory based on these multiple initial path points, resulting in a collision-free drone flight trajectory with minimal yaw angle rotation. The drone's trajectory is then corrected as it passes through inspection targets during flight, enabling the drone's gimbal camera to effectively capture the inspection targets.

[0098] In one embodiment of the present application, the generation of the UAV trajectory using the optimized path optimization algorithm based on the starting point and the end point in the voxel map data to obtain the UAV initial path includes the following steps S201 to S202:

[0099] S201, defining the center point of each grid in the voxel map as a node.

[0100] S202, defining a movement cost evaluation function of a node in a voxel map; the expression of the movement cost evaluation function of the node is shown in Formula 1;

[0101] F(n)=G(n)+H(n)+ρJ(n)+μK(n) Formula 1;

[0102] Among them, F(n) is the movement cost evaluation function of node n; G(n) is the movement cost from the starting point to node n when moving along the generated path; H(n) is the movement cost from node n to the end point; J(n) is the obstacle penalty of node n; K(n) is the direction change penalty of node n; ρ is the weight of the obstacle penalty of node n; μ is the weight of the direction change penalty of node n.

[0103] Specifically, the calculation formula of J(n) is: Among them, e is a natural constant; the value of σ can be dynamically adjusted according to the effect.

[0104] The calculation formula of x(n) is: in, For node n along d i Direction exploration distance to obstacles; i∈{1,2,3,...,26}.

[0105] The calculation formula of K(n) is: Among them, D n is the direction of travel from the n-1th node to the nth node; D n-1 is the direction of travel from the n-2th node to the n-1th node.

[0106] In one embodiment of the present application, before generating the UAV trajectory using the optimized path optimization algorithm based on the starting point and the end point in the voxel map data to obtain the UAV initial path, the following steps S210 to S250 are also included:

[0107] S210, calculating the movement cost from the starting point to the end point to obtain the movement cost value.

[0108] S220: Obtain a preset movement cost threshold.

[0109] S230: Determine whether the obtained movement cost value is less than or equal to a preset movement cost threshold.

[0110] S240: If the obtained movement cost value is less than or equal to the preset movement cost threshold, the UAV trajectory is generated using the A* algorithm based on the starting point and the end point in the voxel map data to obtain the UAV initial path.

[0111] S250: If the obtained movement cost value is greater than the preset movement cost threshold, the optimized A* algorithm is used to generate the UAV trajectory based on the starting point and the end point in the voxel map data to obtain the UAV initial path.

[0112] In this embodiment, a preset movement cost threshold is first used to calculate the movement cost from the starting point to the end point, and the resulting movement cost is compared with the preset movement cost threshold. If the resulting movement cost is less than or equal to the preset movement cost threshold, the distance between the starting point and the end point is relatively close, and the A* algorithm can be used directly to generate the drone trajectory. If the resulting movement cost is less than or equal to the preset movement cost threshold, the distance between the starting point and the end point is relatively far, and the optimized A* algorithm is selected for drone trajectory generation.

[0113] like Figure 2 and Figure 3 As shown, in one embodiment of the present application, the UAV trajectory is generated using the optimized A* algorithm based on the starting point and the end point in the voxel map data to obtain the initial path of the UAV, and further includes the following S251 to S263:

[0114] S251, obtaining the starting point and the end point.

[0115] S252, creating a first list and a second list; the first list is used to store nodes for which movement costs are to be calculated, and the second list is used to store nodes that have been visited.

[0116] S253: Set the starting point as the current processing node.

[0117] S254, explore the eight directions around the current processing node as exploration directions to obtain multiple nodes, store the multiple nodes in the first queue, use the current processing node as the parent node of these nodes, and place the current processing node in the second queue; the exploration step length of the current processing node in the eight surrounding directions is three nodes.

[0118] Specifically, the eight directions refer to directly above, upper left, directly left, lower left, directly below, lower right, directly right, and upper right with the current processing node as the center.

[0119] S255, respectively obtain exploration information in eight directions around the current processing node.

[0120] S256: Mark the exploration direction that touches the obstacle in the exploration information, and store the marked direction and the three nodes on the marked direction in a second list.

[0121] S257: Store the exploration nodes in two adjacent exploration directions of the marked exploration direction into a non-omnidirectional exploration obstacle node list.

[0122] Specifically, when an exploration direction is marked, the exploration directions on both sides of it are considered the two adjacent exploration directions of the marked exploration direction. When two adjacent exploration directions are both marked, the two adjacent exploration directions are considered as one direction, and the exploration directions on both sides of it are considered the two adjacent exploration directions of the marked exploration direction.

[0123] S258 , calculating the F(n) value of each node in the second list based on Formula 1 to obtain the F(n) value of each node.

[0124] S259 , filtering out the node with the smallest F(n) value in the second list, and using the node with the smallest F(n) value as a new current processing node.

[0125] S260: Determine whether the obtained new current processing node is in the non-omnidirectional exploration obstacle node list.

[0126] S261: If the obtained new current processing node is in the non-omnidirectional exploration obstacle node list, the remaining directions after removing the obstacle directions from the eight directions around the new current processing node are used as exploration directions for exploration until the end point is found.

[0127] S262: If the obtained new current processing node is not in the non-omnidirectional exploration obstacle node list, eight directions around the new current processing node are used as exploration directions for exploration until the end point is found.

[0128] S263, after finding the end point, start from the end point and search for the parent node of each node in turn, and gradually move toward the parent node according to the order in which the child nodes point to the parent nodes until it moves to the starting point, and connect each node passed through in the movement process in turn, and use the generated path as the initial path of the drone.

[0129] Specifically, if the new current processing node is in the non-omnidirectional exploration obstacle node list, the remaining directions after removing the obstacle directions from the eight directions around the new current processing node are used as exploration directions for exploration until the end point is found.

[0130] If the new current processing node is in the list of non-omnidirectional exploration obstacle nodes;

[0131] The eight directions around the current processing node are used as exploration directions. The remaining directions after removing the obstacle directions are explored to obtain multiple nodes. The multiple nodes are stored in the first queue. The current processing node is used as the parent node of these nodes and the current processing node is placed in the second queue.

[0132] Return to execute the acquisition of exploration information in eight directions around the current processing node;

[0133] Until the end point appears in the explored node.

[0134] Specifically, if the new current processing node is not in the list of non-omnidirectional exploration obstacle nodes, the eight directions around the new current processing node are used as exploration directions for exploration until the end point is found.

[0135] If the new current processing node is not in the list of non-omnidirectional exploration obstacle nodes, return to the step of exploring the eight directions around the current processing node as exploration directions to obtain multiple nodes, store the multiple nodes in the first queue, use the current processing node as the parent node of these nodes, and place the current processing node in the second queue; the exploration step length of the current processing node in the eight directions around the current processing node is three nodes;

[0136] Until the end point appears in the explored node.

[0137] In this embodiment, the method of obtaining the value is mainly discussed in the two-dimensional case, that is, only describing d i, the value selection process of i∈{1,2,3,...,8}, first, when using the A* algorithm to generate the UAV trajectory, it is necessary to determine whether H(n) is less than or equal to the preset movement cost threshold at the first node. The purpose of this judgment is to check whether the end point is near the node. If H(n) is less than or equal to the preset movement cost threshold, the traditional A* algorithm is directly used to generate the UAV trajectory. If H(n) is greater than the preset movement cost threshold, three nodes are explored in the eight directions around the current node, and the explored nodes are stored in the first list. If an obstacle is encountered in a certain direction during the exploration process, the direction is marked, and the three nodes in the marked direction are directly added to the second list. The six nodes in the other two directions adjacent to the marked direction are added to the non-omnidirectional exploration obstacle node list.

[0138] Next, use Formula 1 to calculate the F(n) values ​​of all nodes in the first list and the non-omnidirectional exploration obstacle node list, and filter out the node with the smallest F(n) value. Then determine whether the node with the smallest F(n) value is in the non-omnidirectional exploration obstacle node list. When the node with the smallest F(n) value is in the non-omnidirectional exploration obstacle node list, the node with the smallest F(n) value is used as the current processing node for exploration, and the direction in which obstacles are removed is explored at the same time. When the node with the smallest F(n) value is not in the non-omnidirectional exploration obstacle node list, the node with the smallest F(n) value is used as the current processing node for full exploration in eight directions. Repeat the above exploration steps until the end point is found, thereby completing the exploration of the entire path.

[0139] like Figure 4 As shown, in one embodiment of the present application, generating a safe flight corridor based on the initial path of the drone includes the following steps S301 to S307:

[0140] S301, generating an initial spherical convex hull based on the starting point of the initial path of the UAV.

[0141] Specifically, the radius formula of the initial spherical convex hull is: min(C, dis(p0)), where C is a set constant that limits the maximum value of the spherical radius; dis(p0) is the distance from p0 to the nearest obstacle, and p0 is the coordinate of the starting point.

[0142] After determining the center of the convex hull, the nearest neighbor search of the kd tree data structure is used to obtain the distance d between the point and the obstacle. nearest , which is dis(p0). Because the drone is considered a point mass during planning, obstacles need to be expanded outward by the drone's radius. The radius of the new convex hull is obtained using the following formula. When there are no obstacles in the surrounding area, dis(p0) tends to infinity. In this case, a constant C is selected as the convex hull radius.

[0143] S302, using the initial path of the UAV as the search target and the moving direction of the UAV as the search direction, defining the first point not in the initial spherical convex hull as the center of the sampling area.

[0144] S303: Generate a spherical sampling area based on the center of the sampling area.

[0145] Specifically, the calculation formula for the radius of the spherical sampling area is: r n =d nearest -d dilate , where d nearest Refers to the shortest distance between the drone and the obstacle when the drone is regarded as a point mass; d dilate The distance that the obstacle expands outwards, specifically half of the UAV's wheelbase.

[0146] S304: uniformly sample the spherical sampling area to obtain multiple sampling points.

[0147] S305 , calculating the score of each sampling point, screening out the sampling point with the highest score, and using the sampling point with the highest score as the center of the next spherical convex hull.

[0148] Specifically, the scoring formula is: S core =αS v +βS o +γS r ; Among them, S core is the score of the spherical convex hull formed by the sampling point, S v is the volume of the spherical convex hull formed by the sampling point, S o The overlapping volume of the spherical convex hull formed for the sampling point and the spherical convex hull generated previously; S r is the relationship between the position of the spherical convex hull formed by the sampling point and the area of ​​the inspection point; α is the weight of the volume of the spherical convex hull formed by the sampling point; β is the weight of the overlapping volume of the spherical convex hull formed by the sampling point and the previously generated spherical convex hull; γ is the weight of the relationship between the position of the spherical convex hull formed by the sampling point and the area of ​​the inspection point.

[0149] Further, Where R is the radius of the convex hull of the spherical shape formed by the sampling points.

[0150] in; h2=r-h1.

[0151] where O∈R 3 is the coordinate of the sphere center; P∈R 3 The coordinates of the inspection target.

[0152] S306 , obtaining the spherical convex hull formed by the sampling points with the highest scores, and defining the spherical convex hull as a new initial spherical convex hull.

[0153] S307, return to execute with the initial inspection path of the UAV as the search target and the moving direction of the UAV as the search direction, define the first point not in the initial spherical convex hull as the center of the sampling area, until the last spherical convex hull generated includes the end point of the initial inspection path of the UAV.

[0154] Specifically, after the convex hull is generated, the remaining A* nodes in the convex hull are no longer considered for generating the convex hull until an A* node that is not in the convex hull is found. The first point found that is not in the convex hull is used as the center of the sampling area to generate the sampling area.

[0155] In one embodiment of the present application, the calculation formula for the radius of the spherical sampling area is shown in Formula 2;

[0156]

[0157] Among them, r is the radius of the spherical sampling area; o is the coordinate of the center of the spherical sampling area; s l The coordinates of the center of the spherical convex hull generated last time.

[0158] Specifically, the sampling area is a spherical area: τ(o,r), where o is the center of the sampling area and r is calculated as Formula 2.

[0159] In this embodiment, since the prerequisite for generating a safe flight corridor is that the front and rear convex hulls must intersect and not contain each other, the mathematical expression is given as follows:

[0160]

[0161] and where τ i is the i-th convex hull; τi +1 is the i+1th convex hull.

[0162] So S v 、S o The score cannot be 0. If it is 0, skip this point and resample a point. v and S o The scores of are not 0, then we need to expand the sampling area. The specific calculation method is as follows:

[0163] Where j is the number of iterations of expanding the radius; is the i-th convex hull τ i The radius of the sampling area for the jth update; is the i-th convex hull τ i The radius of the sampling area for the j-1th update; δ>0 is the scale control parameter; ε>0 controls the initial growth rate.

[0164] The inverse of this function is: Its derivative decreases as the number of iterations increases but remains greater than 0, thereby ensuring that the sampling area is continuously expanded but not grown too fast to obtain a convex hull of better quality, and the termination condition of the iteration is reaching the set number of iterations.

[0165] like Figures 5 to 6 As shown, in one embodiment of the present application, generating multiple drone initial path points based on the safe flight corridor includes the following S401:

[0166] S401, calculating the coordinates of the initial path points of the UAV according to Formula 3;

[0167]

[0168] Among them, p i is the coordinate of the initial path point of the i-th UAV; is the position of the i-th spherical convex hull; is the position of the i+1th spherical convex hull; is the radius of the convex hull of the i-th sphere; is the radius of the i+1th spherical convex hull; is the vector modulus of the difference between the position of the i-th spherical convex hull and the position of the i+1-th spherical convex hull.

[0169] Specifically, Among them, p i is the coordinate of the initial path point of the i-th UAV; τ i is the i-th convex hull; τ i+1 is the i+1th convex hull.

[0170] like Figures 7 to 8 As shown, in one embodiment of the present application, the minimum control amount trajectory is used to generate trajectories based on the multiple generated drone initial path points to obtain a preliminary optimized drone initial path, including the following S501 to S505:

[0171] S501 , sorting the obtained multiple UAV initial path points based on the generation order of each convex hull in the safe flight corridor to obtain a sequence table of UAV initial path points.

[0172] S502 , based on the coordinates of the starting point and the coordinates of the drone initial path point closest to the starting point obtained based on the drone initial path point sequence table, a first optimized path is generated using a minimum control amount trajectory.

[0173] S503: The coordinates of the drone initial path point closest to the starting point obtained based on the drone initial path point sequence table are used as the coordinates of the new starting point.

[0174] S504, returning to the execution of the method of using the coordinates of the starting point and the coordinates of the drone initial path point closest to the starting point obtained based on the drone initial path point sequence table to generate a first optimized path using the minimum control amount trajectory, until each drone initial path point is selected once, to obtain multiple first optimized paths.

[0175] S505: A path obtained by sequentially connecting the multiple first optimized paths is defined as a preliminarily optimized initial path of the UAV.

[0176] Specifically, the first optimized path refers to the drone flight path formed between two adjacent drone initial path points or between the starting point and the drone initial path point, or between the drone initial path point and the end point.

[0177] In this embodiment, the generated multiple UAV initial path points, the starting point, and the end point are sorted in the order of convex hull generation. Then, a first optimized path is generated using the minimum control trajectory with the starting point and the UAV initial path point closest to the starting point obtained based on the UAV initial path point sequence table. Then, the first optimized path generation step is repeated using the UAV initial path point closest to the starting point obtained based on the UAV initial path point sequence table as the new starting point until the last first optimized path is generated with the end point. The path obtained by sequentially connecting all the obtained first optimized paths is defined as the preliminary optimized UAV initial path.

[0178] like Figures 7 to 8 As shown, in one embodiment of the present application, the trajectory generation based on the multiple generated UAV initial path points is performed using the minimum control amount trajectory to obtain a preliminary optimized UAV initial path, and further includes the following S506:

[0179] S506, constructing a multi-objective optimization polynomial as shown below;

[0180]

[0181] |ψ′|≤|ψ′| max ;

[0182] in, is the jerk of the UAV trajectory at time t; ψ(t) is the maximum angular velocity constraint of the UAV at time t; ρ is the weight of the maximum time tolerance for the UAV to complete the task; The maximum flight speed limit for the drone during flight; The maximum flight acceleration limit of the drone during flight; is the safe flight corridor restriction function, where The i-th trajectory is discretized into a set of several points, τ i is the i-th convex hull, further The maximum time tolerance for the UAV to complete the mission; Share the initial and final state constraints of the intermediate points for this trajectory; Share the initial state constraints of the intermediate point for this trajectory; Share the end state limit of the intermediate point for this trajectory; Share the intermediate point status restriction for this segment of trajectory; The maximum angular velocity limit for the drone gimbal; The maximum angle limit of the gimbal is |ψ′|≤|ψ′| max is the maximum angular velocity constraint of the UAV.

[0183] like Figures 7 to 8 As shown, in one embodiment of the present application, the inspection requirement based on the inspection target point further optimizes the initially optimized UAV initial path to obtain a further optimized initially optimized UAV initial path, and the further optimized initially optimized UAV initial path is defined as the UAV target path, including the following S701 to S706:

[0184] S701 : Obtain inspection requirements for each inspection target point, where the inspection requirements for the inspection target point include field of view angle requirements and depth of field requirements.

[0185] S702: Select an inspection target point.

[0186] S703, analyzing the inspection target point to obtain the UAV initial path point closest to the inspection target point.

[0187] S704: Analyze the initial path point of the drone to obtain two first optimized paths adjacent to the initial path point of the drone.

[0188] S705 , optimizing the two obtained first optimization paths based on the field of view angle requirement and the depth of field requirement to obtain two optimized first optimization paths, and replacing the two first optimization paths with the two optimized first optimization paths.

[0189] S706, returning to select a patrol target point until each patrol target point has been selected once, and obtaining a further optimized preliminary optimized UAV initial path.

[0190] Specifically, define V q ∈R3 is the flight speed of the drone, and the tangent of the current trajectory is generally used as the direction of the vector, P q ∈R 3 is the current center position of the UAV, θ is the angle difference between the yaw angle of the UAV and the yaw angle of the gimbal camera, P q ∈R 3 is the coordinate of the inspection target. Among the above variables, only θ is an unknown variable and the yaw angle of the gimbal can be calculated based on θ and the yaw angle ψ of the drone.

[0191]

[0192] θ is the angle difference between the yaw angle of the drone and the yaw angle of the gimbal camera. ψ is the yaw angle of the drone. w is the position of the inspection target in the world coordinate system. q is the position of the UAV in the world coordinate system. (P w -P q ) y Indicates P w -P q The component of the vector in the y-axis direction is a right-hand coordinate system. The above formula can be used to solve the yaw angle of the gimbal Similarly, by solving the derivative θ' of θ, we can continue to get It is difficult to directly derive θ. We can directly solve its derivative based on the meaning of its derivative:

[0193] θ'=ω;

[0194] V q =ωr;

[0195] r=||P w -P q ||;

[0196] Combining the above formulas, we can get:

[0197]

[0198] Since ψ' is the yaw rotation angular velocity of the UAV, it can be directly determined by the UAV trajectory, so ψ' is also determined by it:

[0199]

[0200] Where ψ' is the yaw angular velocity of the UAV. θ' is the relative angular velocity between the yaw angle of the UAV and the yaw angle of the gimbal camera.

[0201] In vision-based active perception systems, imaging quality is subject to strict optical geometric constraints. Specifically, the target object must satisfy both the field of view and depth of field constraints to avoid target truncation and defocus blur. To achieve this goal, an accurate coordinate system mapping relationship must be established: let the target pose in the global coordinate system be P w =[x w ,y w ,z w ], through the rigid body transformation matrix Map it to the camera coordinate system:

[0202]

[0203] They represent the rotation matrix and translation matrix from the world system to the camera coordinate system, P w Homogeneous coordinate form of P. c The constraints of camera characteristics that need to be satisfied are the field of view angle and focal length constraints, which can be expressed as follows:

[0204]

[0205] After the trajectory is generated by the above optimization conditions, it is necessary to check the trajectory near the target point to meet the requirements. The constraints that need to be met in , if found not to meet the requirements, they will be adjusted, the adjustment and optimization process is as follows;

[0206]

[0207]

[0208] π q is the position penalty function of the target point, q is determined by the positive or negative direction of the target point on the y-axis, where J and K are the field of view and focal length constraints of the gimbal camera. is the vector obtained by taking the absolute value of the y-axis component of the vector. Assume that the trajectory is at time t i to t i+1 The trajectory between.

[0209] Compare this application with the prior art:

[0210] First, the existing technologies are (1) Zhou X, Wang Z, Ye H, et al. Ego-planner: An esdf-free gradient-based local planner for quadrotors [J]. IEEE Robotics and Automation Letters, 2020, 6(2): 478-485. This document is collectively referred to as Document 1; Huang Shicheng. Research on collaborative inspection of multiple drones in indoor substations based on UWB positioning [D]. Fujian: Fuzhou University, 2020. This document is collectively referred to as Document 2.

[0211] Reference 1 uses a voxel map representation method and local obstacle avoidance planning, and makes the yaw angle of the drone turn so that when it reaches the inspection target point, it hovers towards the observation point and takes a photo. Then it continues to move to the next target point through local obstacle avoidance and repeats the above steps. Reference 2 uses a global two-dimensional map and generates MAKLINK connecting lines. It takes the midpoint of each MAKLINK connecting line to generate a global collision-free path map, and then uses the Djikstra algorithm to search for the optimal inspection path based on the position of the inspection target point. Since References 1 and 2 do not add pan-tilt control, the pan-tilt is set to "follow mode" when conducting the algorithm experiment, that is, it always keeps the same direction as the drone's head. Each algorithm was tested 10 times. The SO(3) controller was added to rviz to simulate the effect of the drone flying in a real environment. The maximum flight speed of the drone is limited to 1.5m / s. In the simulated substation map, a 10×5×3m3 cabinet was selected as the inspection target. The site was limited to an area of ​​14×12×5m3. The hardware simulation platform is shown in Table 1.

[0212]

[0213] Table 1 Configuration table of the simulation experiment platform for safe flight corridor generation

[0214] The coordinates of the target points are (11,3,1), (11,5,1), (11,7,1), and the coordinates of the observation points are (10,3,1), (10,5,1), (10,7,1). The experimental results of the shortest path in Reference 1 are shown in the figure below. Figure 9 As shown in the figure, the path effect diagram of document 2 is as follows Figure 10 shown.

[0215] Since the algorithm in Document 2 does not need to generate trajectories, the time spent on trajectory generation is not compared. The average values ​​of the various parameter values ​​from the 10 experimental results are compared, and the comparison results are shown in Table 2. It can be seen from Table 2 that the algorithm of this application has improved in terms of total yaw angle rotation, total path length, and total inspection time. Since Document 1 needs to plan multiple times to find a collision-free trajectory when facing large obstacles, in this scenario, an average of 98.3 times of planning is required to complete the inspection task in each experiment, and there is a situation of rotation in place during planning, so its yaw angle rotation is significantly higher than that of other algorithms.

[0216] However, the proposed algorithm only requires one global planning trajectory and one local trajectory correction in each experiment to generate a global trajectory. Reference 2 only generates MAKLINK connections by modeling the scene to obtain a collision-free inspection path. Therefore, it has no trajectory and its performance remains consistent across experiments.

[0217] In terms of total inspection time, the technical solution of this application reduces this time to 36.08% of that of Reference 1 and 33.41% of that of Reference 2. Reducing the total yaw angle and total path length improves flight stability and saves energy. The total yaw angle is reduced to 11.85% of that of Reference 1 and 34.05% of that of Reference 2. In terms of total path length, the total path length is 80.57% of that of Reference 1 and 48.98% of that of Reference 2. The total time spent on trajectory generation is reduced to 8.63% of that of Reference 1. Reference 2 does not generate a trajectory and is therefore not included in the comparison.

[0218]

[0219] Table 2

[0220] The various technical features of the above-described embodiments can be combined arbitrarily, and the execution order of the method steps is not restricted. In order to make the description concise, not all possible combinations of the various technical features in the above-described embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0221] The above-described embodiments merely represent several implementation methods of the present application. While the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the present application. It should be noted that a person of ordinary skill in the art may make various modifications and improvements without departing from the spirit of the present application, and these modifications and improvements fall within the scope of protection of the present application. Therefore, the scope of protection of the present application shall be determined by the appended claims.

Claims

1. A method for generating a UAV inspection trajectory, characterized in that: The UAV inspection trajectory generation method includes: Acquiring voxel map data, wherein the voxel map data includes a starting point and an end point; Based on the starting and ending points in the voxel map data, the optimized path optimization algorithm is used to generate the UAV trajectory and obtain the initial path of the UAV; Generate a safe flight corridor based on the drone’s initial path; Generate multiple initial path points for drones based on safe flight corridors; Based on multiple UAV initial path points, the minimum control amount trajectory is used to generate the trajectory and obtain the preliminary optimized UAV initial path; Obtain at least one inspection target point; The initially optimized UAV initial path is further optimized based on the inspection requirements of the inspection target point to obtain a further optimized UAV initial path, and the further optimized UAV initial path is defined as the UAV target path; Output the drone target path.

2. The method for generating a drone inspection trajectory according to claim 1, characterized in that: The UAV trajectory is generated using an optimized path optimization algorithm based on the starting point and the end point in the voxel map data to obtain the initial path of the UAV, including: Define the center point of each grid in the voxel map as a node; A movement cost evaluation function of a node in a voxel map is defined; the expression of the movement cost evaluation function of the node is shown in Formula 1; F(n)=G(n)+H(n)+ρJ(n)+μK(n) Formula 1; Among them, F(n) is the movement cost evaluation function of node n; G(n) is the movement cost from the starting point to node n when moving along the generated path; H(n) is the movement cost from node n to the end point; J(n) is the obstacle penalty of node n; K(n) is the direction change penalty of node n; ρ is the weight of the obstacle penalty of node n; μ is the weight of the direction change penalty of node n.

3. The method for generating a UAV inspection trajectory according to claim 2, characterized in that: Before generating the UAV trajectory using the optimized path optimization algorithm based on the starting point and the end point in the voxel map data to obtain the UAV initial path, the method further includes: Calculate the cost of moving from the starting point to the end point and get the moving cost value; Get the preset movement cost threshold; Determine whether the obtained movement cost value is less than or equal to a preset movement cost threshold; If the obtained movement cost value is less than or equal to the preset movement cost threshold, the A* algorithm is used to generate the drone trajectory based on the starting point and end point in the voxel map data to obtain the drone's initial path; If the obtained movement cost value is greater than the preset movement cost threshold, the optimized A* algorithm is used to generate the UAV trajectory based on the starting point and end point in the voxel map data to obtain the UAV initial path.

4. The method for generating a UAV inspection trajectory according to claim 3, characterized in that: The method further includes: generating a UAV trajectory using an optimized A* algorithm based on the starting point and the end point in the voxel map data to obtain the UAV initial path; Get the starting point and the end point; Creating a first list and a second list; the first list is used to store nodes for which movement costs are to be calculated, and the second list is used to store nodes that have been visited; Set the starting point as the current processing node; Exploring eight directions around the current processing node as exploration directions to obtain multiple nodes, storing the multiple nodes in a first queue, using the current processing node as the parent node of these nodes, and placing the current processing node in a second queue; the exploration step length of the current processing node in the eight directions around the current processing node is three nodes; Obtain exploration information in eight directions around the current processing node respectively; Marking the exploration direction that touches the obstacle in the exploration information, and storing the marked direction and the three nodes in the marked direction in a second list; The exploration nodes in two adjacent exploration directions of the marked exploration direction are stored in the non-omnidirectional exploration obstacle node list; Calculate the F(n) value of each node in the second list based on formula 1 to obtain the F(n) value of each node; Filter out the node with the smallest F(n) value in the second list, and use the node with the smallest F(n) value as the new current processing node; Determine whether the new current processing node is in the non-omnidirectional exploration obstacle node list; If the new current processing node is in the list of non-omnidirectional exploration obstacle nodes, the remaining directions after removing the obstacle directions from the eight directions around the new current processing node are used as exploration directions for exploration until the end point is found; If the new current processing node is not in the list of non-omnidirectional exploration obstacle nodes, the eight directions around the new current processing node are used as exploration directions to explore until the end point is found; After finding the end point, start from the end point and search for the parent node of each node in turn. Move gradually toward the parent node according to the order in which the child nodes point to the parent node until it reaches the starting point. Connect each node passed during the movement in turn, and use the generated path as the initial path of the drone.

5. The method for generating a UAV inspection trajectory according to claim 4, characterized in that: Generating a safe flight corridor based on the initial path of the UAV includes: Generate an initial spherical convex hull based on the starting point of the UAV's initial path; The initial path of the UAV is used as the search target, the moving direction of the UAV is used as the search direction, and the first point not in the initial spherical convex hull is defined as the center of the sampling area. Generate a spherical sampling area based on the center of the sampling area; The spherical sampling area is uniformly sampled to obtain multiple sampling points; Calculate the score of each sampling point, select the sampling point with the highest score, and use the sampling point with the highest score as the center of the next spherical convex hull; Obtain the spherical convex hull formed by the sampling point with the highest score, and define the spherical convex hull as the new initial spherical convex hull; Return to executing the search with the drone's initial inspection path as the search target and the drone's movement direction as the search direction. Define the first point not in the initial spherical convex hull as the center of the sampling area, and continue until the last spherical convex hull generated includes the end point of the drone's initial inspection path.

6. The method for generating a UAV inspection trajectory according to claim 5, characterized in that: The calculation formula of the radius of the spherical sampling area is shown in Formula 2; Where r is the radius of the spherical sampling area; o is the coordinate of the center of the spherical sampling area; s l The coordinates of the center of the spherical convex hull generated last time.

7. The method for generating a UAV inspection trajectory according to claim 6, characterized in that: The generating of multiple UAV initial path points based on the safe flight corridor includes: Calculate the coordinates of the initial path point of the drone according to formula 3; Among them, p i is the coordinate of the initial path point of the i-th UAV; is the position of the i-th spherical convex hull; is the position of the i+1th spherical convex hull; is the radius of the convex hull of the i-th spherical shape; is the radius of the i+1th spherical convex hull; is the vector modulus of the difference between the position of the i-th spherical convex hull and the position of the i+1-th spherical convex hull.

8. The method for generating a UAV inspection trajectory according to claim 7, characterized in that: The method of generating trajectories based on the multiple generated UAV initial path points using the minimum control amount trajectory to obtain a preliminarily optimized UAV initial path includes: The multiple UAV initial path points are sorted based on the generation order of each convex hull in the safe flight corridor to obtain a sequence table of UAV initial path points; Based on the coordinates of the starting point and the coordinates of the UAV initial path point closest to the starting point obtained based on the sequence table of the UAV initial path points, a first optimized path is generated using a minimum control amount trajectory; The coordinates of the drone initial path point closest to the starting point obtained based on the drone initial path point sequence table are used as the coordinates of the new starting point; Returning to the step of generating a first optimized path using the minimum control amount trajectory by using the coordinates of the starting point and the coordinates of the drone initial path point closest to the starting point obtained based on the drone initial path point sequence table, until each drone initial path point is selected once, thereby obtaining a plurality of first optimized paths; The path obtained by sequentially connecting the multiple first optimized paths is defined as the initial path of the preliminarily optimized UAV.

9. The method for generating a UAV inspection trajectory according to claim 8, characterized in that: The method of generating trajectories based on the generated multiple UAV initial path points using a minimum control amount trajectory to obtain a preliminarily optimized UAV initial path further includes: Construct a multi-objective optimization polynomial as shown below; ψ′|≤ψ′| max ; in, is the jerk of the UAV trajectory at time t; ψ(t) is the maximum angular velocity constraint of the UAV at time t; ρ is the weight of the maximum time tolerance for the UAV to complete the task; The maximum flight speed limit for the drone during flight; The maximum flight acceleration limit of the drone during flight; is the safe flight corridor restriction function, where The i-th trajectory is discretized into a set of several points, τi is the i-th convex hull, and further The maximum time tolerance for the UAV to complete the mission; Share the initial and final state constraints of the intermediate points for this trajectory; Share the initial state constraints of the intermediate point for this trajectory; Share the end state limit of the intermediate point for this trajectory; Share the intermediate point status restriction for this segment of trajectory; The maximum angular velocity limit for the drone gimbal; The maximum angle limit of the gimbal is |ψ′|≤|ψ′| max is the maximum angular velocity constraint of the UAV.

10. The method for generating a UAV inspection trajectory according to claim 9, wherein: The inspection requirement based on the inspection target point further optimizes the initially optimized UAV initial path to obtain a further optimized initially optimized UAV initial path, and defines the further optimized initially optimized UAV initial path as the UAV target path, including: Obtaining inspection requirements for each inspection target point, wherein the inspection requirements for the inspection target point include field of view angle requirements and depth of field requirements; Select an inspection target point; Analyze the inspection target point and obtain the UAV initial path point closest to the inspection target point; Analyze the initial path point of the UAV to obtain two first optimized paths adjacent to the initial path point of the UAV; Optimizing the two obtained first optimization paths based on the field of view angle requirement and the depth of field requirement to obtain two optimized first optimization paths, and replacing the two first optimization paths with the two optimized first optimization paths; Return to select a patrol target point until each patrol target point has been selected once, and obtain the preliminary optimized UAV initial path after further optimization.

Images