Unmanned aerial vehicle light-free perception path planning method and system based on area array sensor

Abstract

The invention provides an unmanned aerial vehicle light-free perception path planning method and system based on area array sensors, and the method comprises the steps: arranging a plurality of ToF area array sensors on an unmanned aerial vehicle, presetting a starting point and a target point, controlling the unmanned aerial vehicle to start from the starting point, obtaining the depth information of surrounding obstacles in real time, and carrying out the spatial transformation to obtain the omnidirectional obstacle information of the unmanned aerial vehicle. Generating an initial path by adopting an improved # imgabs0 # algorithm, re-planning a first track section colliding with the obstacle for the initial path to generate a second track section, and replacing the first track section in the initial path with the second track section to generate a first optimized track, based on the feasibility of the speed of the unmanned aerial vehicle, nodes of the first optimized track in time are redistributed, a second optimized track is output, the unmanned aerial vehicle tracks the second optimized track, surrounding obstacle depth information is updated in real time, and the second optimized track is repeatedly obtained until a target point is reached. According to the invention, sensing can be carried out in a dark and narrow environment, and safe obstacle avoidance and advancing are realized.

Description

Technical Field

[0001] The present invention relates to the technical field of drone perception and path planning, and in particular to a method and system for drone path planning in the dark based on an area array sensor. Background Art

[0002] Today's cities are developing rapidly. To meet the needs of industrial production and people's daily lives, a large number of cables and pipes are laid underground in cities and factories. To protect the cables, specialized channels—cable trenches—are constructed to lay these cables. The space available for movement within the cable trenches is extremely narrow, with a cross-sectional area of only approximately 40cm x 50cm. After laying, the trenches are capped, and inspection openings are only opened every few hundred meters, making cable inspection extremely difficult.

[0003] At present, manual inspection is the main method used for cables in cable trenches. If manual inspection fails, the inspection will have to be abandoned. Once a cable abnormality occurs, the cable trench can only be opened by re-digging the ground to check and repair the problematic cables. This work is tedious and prone to repeated and ineffective operations. Brackets for supporting cables extend from both sides of the cable trench. The cables are staggered and the obstacle environment is complex, which not only reduces the manual working space, but also easily causes accidental injuries. Therefore, the development of a drone planning method that can perform omnidirectional obstacle avoidance and automatic inspections in narrow and dark environments is of great significance to improving the industry's intelligence level, ensuring personnel safety, improving inspection efficiency, expanding the inspection scope, and accelerating fault location.

[0004] Currently, research on inspections in confined environments still faces numerous challenges in obstacle perception and path planning, such as the small size of obstacles, the large operating range of ToF area array sensors, and the inability to use GPS high-definition positioning. Traditional perception and planning methods rely on LiDAR or visual detection point clouds for real-time mapping, which is computationally intensive and difficult to deploy on small mobile platforms. Traditional planning methods rely on potential field methods, which are computationally intensive and have high planning latency, failing to meet real-time requirements. Therefore, designing a method that can perceive the surrounding environment in real time and perform rapid path planning in confined, dimly lit environments has become an urgent need within the industry. This is to ensure that drones can safely and efficiently perform inspections in complex environments and expand the scope of cable inspections. Summary of the Invention

[0005] In view of the shortcomings of the existing technology, the purpose of the present invention is to provide a light-free perception path planning method and system for drones based on area array sensors, which can still perceive in lightless and narrow environments, is particularly suitable for detecting internal channels such as cable trenches, and can achieve safe obstacle avoidance and travel.

[0006] In order to solve the above technical problems, the technical solution adopted by the present invention is: A method for unmanned aerial vehicle (UAV) path planning in the dark based on an area array sensor, comprising: Step S1: Arrange several ToF area array sensors in a circular pattern on the drone to provide all-round coverage of the drone's surroundings, preset a starting point and a target point, and control the drone to depart from the starting point; Step S2: Obtain obstacle depth information of each ToF area array sensor relative to surrounding obstacles in real time, and perform spatial transformation on all obstacle depth information to obtain omnidirectional obstacle information of the drone; Step S3: Using improved The algorithm generates the initial path; Step S4: performing trajectory collision detection based on the omnidirectional obstacle information, obtaining a first trajectory segment in the initial path that collides with the obstacle, replanning the first trajectory segment to generate a second trajectory segment that does not collide with the obstacle, and replacing the first trajectory segment in the initial path with the second trajectory segment to obtain a first optimized trajectory; Step S5: reallocating the nodes of the first optimized trajectory in time based on the feasibility of the UAV speed, outputting a second optimized trajectory, and the UAV tracking the second optimized trajectory; Step S6: Repeat steps S2 to S5 until the target point is reached.

[0007] Furthermore, the method of step S2 is specifically as follows: Number each ToF area array sensor in a certain order, and set a certain detection angle range for each ToF area array sensor; For each ToF area array sensor, obtain several depth points on the obstacle within the detection angle range, and obtain the spatial position of each depth point relative to the corresponding ToF area array sensor as the obstacle depth information of the ToF area array sensor; A homogeneous transformation matrix is used to represent the position and attitude of each ToF array sensor relative to the drone. Obtain obstacle information of each depth point relative to the drone based on the spatial position of each depth point relative to the corresponding ToF area array sensor and the homogeneous transformation matrix of the ToF area array sensor corresponding to each depth point; All obstacle information at all depth points is aggregated into omnidirectional obstacle information.

[0008] Furthermore, the calculation formula for the spatial position of each depth point relative to the corresponding ToF area array sensor is: ,

[0009] in, 、 They are the horizontal and vertical viewing angles of the ToF area array sensor, 、 Respectively The pixel position coordinates of each depth point relative to the corresponding ToF area array sensor in the width and height directions, It is The vertical distance from each depth point to the corresponding ToF array sensor plane, is the number of all depth points, For the The distance from each depth point to the corresponding ToF array sensor in the X direction, For the The distance from each depth point to the corresponding ToF array sensor in the Y direction, For the The distance from each depth point to the corresponding ToF area array sensor in the Z direction; The calculation formula for obtaining the obstacle information of each depth point relative to the drone based on the spatial position of each depth point relative to the corresponding ToF array sensor and the homogeneous transformation matrix of the ToF array sensor corresponding to each depth point is:

[0010] in, is the homogeneous transformation matrix; The omnidirectional obstacle information is .

[0011] Furthermore, the method of step S3 is specifically as follows: Step S301: generating a voxel map, wherein the voxel map is composed of a plurality of grids; Step S302: Plan an initial path in several grids near the obstacle, wherein the initial path is composed of multiple line segments connected in sequence, each line segment is located in a grid, each line segment includes a first edge intersection and a second edge intersection, and the first edge intersection and the second edge intersection are both on the grid edge line. The first edge intersection of the first line segment is preset, and the first edge intersection of each of the remaining line segments coincides with the second edge intersection of the previous line segment. Based on the first edge intersection, the second edge intersection is searched to generate the path with the minimum cost in the corresponding grid as the line segment.

[0012] Furthermore, a method for finding a second edge intersection point based on the first edge intersection point to generate a path with the minimum cost in the corresponding grid as a line segment is as follows: Assuming the grid is a unit cell with side length 1, the unit cost of the grid edge is set to , the unit cost inside the grid is set to , establish several concentric areas outward from the obstacle and preset the unit cost of each area. The area closer to the obstacle has a larger unit cost. The unit cost of the grid edge is determined according to the area where the grid edge is located. The unit cost inside the grid is determined by the area inside the grid. The value of , then find the first edge intersection To the second edge intersection The minimum cost The calculation formula is:

[0013] in, and The second edge intersection points The first grid endpoint and the second grid endpoint on the grid edge line, First grid endpoint The price, Second grid endpoint The price, is the distance the drone travels along the edge of the grid, The distance the drone travels within the grid, The second edge intersection To the first grid endpoint distance; Based on the distance the drone travels along the edge of the grid The distance the drone travels within the grid Determine the path with the least cost.

[0014] Furthermore, the method of step S4 is specifically as follows: Step S401: uniformly selecting a number of control points in the initial path, and generating a curve trajectory for the control points based on a B-spline curve generation method; Step S402: obtaining the first trajectory segment where the curved trajectory collides with an obstacle, replanning the trajectory at a certain distance from the obstacle, and generating a collision-free guiding trajectory; Step S403: obtaining control points on the first trajectory segment, presetting a number of insertion points, and constructing a number of vectors along the direction of the vertical tangent of the curved trajectory, the number of which is the same as the number of insertion points, with the starting point of the vector being on the curved trajectory and the end point being on the guide trajectory; Step S404: Each vector is translated between two control points, with the starting point of the vector translated to the initial path and the end point pointing to an insertion point; Step S405: generating a second trajectory segment using the insertion point and control points arranged sequentially on the first trajectory segment; Step S406: Repeat steps S402 to S405 until all second trajectory segments are obtained, and concatenate all second trajectory segments with the initial path without all first trajectory segments to obtain a first optimized trajectory.

[0015] Furthermore, the method of step S5 is specifically as follows: Get the first optimization trajectory control points , then the control point Corresponding speed control point The calculation formula is:

[0016] in, is the order of the B-spline curve, For the Time nodes, For the Time nodes, For the control points; Setting the threshold , for the control points Corresponding speed control point Make adjustments when When the new speed control point The calculation formula is:

[0017] in, , is the maximum speed allowed for the drone, The drone will track the speed control point speed; when When the new speed control point The calculation formula is: .

[0018] A non-light sensing path planning system for UAV based on area array sensors, comprising: The deployment module is used to arrange several ToF area array sensors on the drone in a circular manner to provide all-round coverage of the drone's surroundings, preset the starting point and target point, and control the drone to depart from the starting point; The omnidirectional obstacle information acquisition module is used to obtain the obstacle depth information of each ToF area array sensor relative to the surrounding obstacles in real time, and perform spatial transformation on all obstacle depth information to obtain the omnidirectional obstacle information of the drone; Initial path acquisition module, used to adopt improved The algorithm generates the initial path; a first optimized trajectory acquisition module, configured to perform trajectory collision detection based on omnidirectional obstacle information, acquire a first trajectory segment in the initial path that collides with an obstacle, replan the first trajectory segment to generate a second trajectory segment that does not collide with the obstacle, and replace the first trajectory segment in the initial path with the second trajectory segment to obtain a first optimized trajectory; A second optimized trajectory acquisition module is used to reallocate the nodes of the first optimized trajectory in time based on the feasibility of the UAV speed, and output a second optimized trajectory, and the UAV tracks the second optimized trajectory; The execution module is used to repeat the omnidirectional obstacle information acquisition module to the second optimized trajectory acquisition module until the target point is reached.

[0019] An electronic device includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, the method for planning a path in the dark for an unmanned aerial vehicle (UAV) based on an area array sensor is implemented.

[0020] A non-transitory computer-readable storage medium stores a computer program, which, when executed by a processor, implements the above-mentioned method for drone dark-light perception path planning based on an area array sensor.

[0021] Compared with the prior art, the present invention has the following advantages and beneficial effects: (1) The present invention arranges a number of ToF array sensors in a circular pattern on the drone to provide all-round coverage of the drone's surroundings. It can obtain real-time information on obstacles in the surrounding environment. Combined with path planning, it can achieve safe obstacle avoidance and movement. When encountering an obstacle that cannot be passed, it can also perform operations such as hovering and returning, thereby improving safety.

[0022] (2) The present invention adopts improved The algorithm generates an initial path and can plan the path in any direction, making it easier for drones to detect complex environments such as cable trenches where there is no light and the area is narrow, protecting the cables and facilitating their management and maintenance.

[0023] (3) The present invention can generate a trajectory that does not collide with obstacles, further protect cables, avoid obstacles, expand the working environment, improve the quality of work, advance the detection range of the cable trench maintenance industry, and propose an operating solution for the cable trench environment that was previously undetectable, providing support for improving the efficiency of cable trench maintenance.

[0024] (4) The path planning of the present invention redistributes nodes in time based on the feasibility of the UAV speed, allowing the UAV to fly at a lower speed when passing through a larger corner, thereby improving the passing capability of the UAV trajectory execution.

[0025] (5) Compared with the potential field method, the efficient path planning method of the present invention does not rely on the potential field map that requires a lot of calculations and updates, saves more software resources, and can be deployed on a low-cost platform. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] The accompanying drawings described herein are used to provide a further understanding of the present invention and constitute a part of this application. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the accompanying drawings: Figure 1 This is a flow chart of a method for unmanned aerial vehicle (UAV) dark-light sensing path planning based on an area array sensor according to the present invention; Figure 2 Schematic diagram of detection by the ToF area array sensor of the present invention; Figure 3 For the surrounding obstacles of the present invention Schematic diagram of depth points; Figure 4 is a schematic diagram of a voxel map grid of the present invention; Figure 5 A schematic diagram of a first edge intersection point to a second edge intersection point on a voxel map grid of the present invention; Figure 6 Improved in one embodiment of the present invention Schematic diagram of the initial path generated by the algorithm; Figure 7 For improved Comparison chart of the algorithm and the original algorithm; Figure 8 A schematic diagram of outputting a first optimization trajectory according to a specific embodiment of the present invention; Figure 9 This is a schematic diagram of a dark-light sensing path planning system for UAVs based on area array sensors according to the present invention. DETAILED DESCRIPTION

[0027] The technical solutions in the embodiments of the present invention will be described clearly and completely below with reference to the accompanying drawings in the embodiments of the present invention.

[0028] In the description of the present invention, it should be understood that the terms "center", "longitudinal", "lateral", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside" and the like indicate orientations or positional relationships based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the system or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as limiting the present invention. In addition, the terms "first", "second", etc. are only used for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, features defined as "first", "second", etc. may explicitly or implicitly include one or more of the features. In the description of the present invention, unless otherwise specified, "multiple" means two or more.

[0029] Example 1 Example 1 provides a method for drone path planning in the dark based on an array sensor. Figure 1 Shown, including: Step S1: Arrange several ToF area array sensors in a circular pattern on the drone to provide all-round coverage of the drone's surroundings, preset a starting point and a target point, and control the drone to depart from the starting point; Step S2: Obtain obstacle depth information of each ToF area array sensor relative to surrounding obstacles in real time, and perform spatial transformation on all obstacle depth information to obtain omnidirectional obstacle information of the drone; Step S3: Using improved The algorithm generates the initial path; Step S4: performing trajectory collision detection based on the omnidirectional obstacle information, obtaining a first trajectory segment in the initial path that collides with the obstacle, replanning the first trajectory segment to generate a second trajectory segment that does not collide with the obstacle, and replacing the first trajectory segment in the initial path with the second trajectory segment to obtain a first optimized trajectory; Step S5: reallocating the nodes of the first optimized trajectory in time based on the feasibility of the UAV speed, outputting a second optimized trajectory, and the UAV tracking the second optimized trajectory; Step S6: Repeat steps S2 to S5 until the target point is reached.

[0030] This embodiment provides a method for planning a path for drones in the dark based on an array sensor. The method does not rely on ambient light sources and can still perform perception in a dark environment. It is particularly suitable for detecting environments such as cable trenches. This embodiment plans an initial path according to a global goal and generates a first optimized trajectory based on the initial path that does not collide with obstacles, ensuring that the drone can safely travel in narrow areas. However, the first optimized trajectory is evenly distributed in time, and the drone will still fly at a higher speed when passing a corner, which easily weakens the drone's trajectory execution capability. Therefore, this embodiment reallocates the nodes of the first optimized trajectory in time based on the feasibility of the drone's speed and outputs a second optimized trajectory. The drone tracks the second optimized trajectory, so that the second optimized trajectory ensures the feasibility of the drone's speed and flies at a lower speed when passing a larger corner, thereby improving the drone's trajectory execution capability. Compared with the potential field method, this embodiment does not rely on a potential field map that requires a lot of calculations and updates, saves more software resources, and can be deployed on a low-cost platform.

[0031] The following is a detailed description of a method for drone dark-light perception path planning based on an area array sensor provided in this embodiment.

[0032] In step S1 of this embodiment, Figure 2 As shown in FIG, a method for arranging several ToF area array sensors in a circular manner on a UAV to provide all-round coverage around the UAV is as follows: design the angle and position of the ToF area array sensors according to the size and configuration of the UAV, fix the ToF area array sensors on the frame or protective frame of the UAV, and adjust the direction of the ToF area array sensors so that all ToF area array sensors will not interfere with each other and can achieve all-directional coverage, forming a complete obstacle detection area, such as Figure 2 As shown, in a specific implementation of this embodiment, the interval angle of the ToF area array sensor is designed to be 45°, and the detection range of one ToF area array sensor is 45°.

[0033] In step S2 of this embodiment, Figure 3 As shown in the figure, it is a schematic diagram of several depth points on the surrounding obstacles. The method of obtaining the obstacle depth information of the surrounding obstacles to each ToF area array sensor in real time and performing spatial transformation on all obstacle depth information to obtain the omnidirectional obstacle information of the drone is as follows: Step S201: numbering each ToF area array sensor in a certain order, and setting a certain detection angle range for each ToF area array sensor; Step S202: For each ToF array sensor, obtain several depth points on the obstacle within the detection angle range, and obtain the depth of each The spatial position of the depth point relative to the corresponding ToF area array sensor is used as the obstacle depth information of the ToF area array sensor; Step S203: using a homogeneous transformation matrix to represent the position and attitude of each ToF area array sensor relative to the drone; Step S204: Based on each The spatial position of the depth point relative to the corresponding ToF array sensor and the homogeneous transformation matrix of the ToF array sensor corresponding to each depth point obtain the obstacle information of each depth point relative to the drone; Step S205: All obstacle information at all depth points are aggregated into omnidirectional obstacle information.

[0034] In step S201 of this embodiment, the method for obtaining the serial number of each ToF area array sensor and the corresponding array depth value in a certain order is as follows: for each ToF area array sensor, the direction facing the ToF area array sensor is set as the Z-axis direction, the surface of the ToF area array sensor is set as the coordinate origin, and a right-handed coordinate system is established. At this time, the depth information returned by the ToF area array sensor is the converted depth information, that is, the vertical distance from the obstacle to the ToF area array sensor plane , according to the vertical distance The relative position of the ToF array sensor corresponding to the pixel can be calculated to obtain the corresponding spatial position. The calculation formula for the spatial position of a depth point relative to the corresponding ToF area array sensor is: ,

[0035] in, 、 They are the horizontal and vertical viewing angles of the ToF area array sensor, 、 Respectively The pixel coordinate position of each depth point relative to the corresponding ToF area array sensor in the width and height directions, It is The vertical distance from each depth point to the corresponding ToF array sensor plane, is the number of depth points, For the The distance from each depth point to the corresponding ToF array sensor in the X direction, For the The distance from each depth point to the corresponding ToF array sensor in the Y direction, For the The distance from a depth point to the corresponding ToF area array sensor in the Z direction.

[0036] Then the set of spatial positions of all depth points is .

[0037] In step S202 of this embodiment, the relative position of each ToF array sensor relative to the frame is also known. The drone establishes an ENU (North-East-Sky) coordinate system with the center of the frame as the origin. The relative position and attitude of each ToF array sensor can be calculated using a 4×4 homogeneous transformation matrix To express:

[0038] in, is the rotation matrix around the X axis, is the rotation matrix around the Y axis, is the rotation matrix around the Z axis.

[0039] In step S203 of this embodiment, the first The calculation formula of the obstacle information of a depth point relative to the drone is: ,in, is a homogeneous transformation matrix, then the omnidirectional obstacle information is , the obstacle information stored in the memory is updated in real time and used in the subsequent local trajectory planning process. In the embodiment test, an arch obstacle with a width of about 12 cm and a height of about 12 cm was built. The information obtained by the ToF area array sensor is as follows Figure 3 As shown, it reflects the actual size of the arch and the area available for passage.

[0040] In step S3 of this embodiment, the improved The algorithm generates the initial path. The specific method is: Step S301: generating a voxel map, wherein the voxel map is composed of a plurality of grids; Step S302: Plan an initial path in several grids near the obstacle, wherein the initial path is composed of multiple line segments connected in sequence, each line segment is located in a grid, each line segment includes a first edge intersection and a second edge intersection, and the first edge intersection and the second edge intersection are both on the grid edge line. The first edge intersection of the first line segment is preset, and the first edge intersection of each of the remaining line segments coincides with the second edge intersection of the previous line segment. Based on the first edge intersection, the second edge intersection is searched to generate the path with the minimum cost in the corresponding grid as the line segment of the grid.

[0041] The path search direction of this algorithm is not limited to the corner direction of the voxel and can be any direction.

[0042] like Figure 4 As shown, other algorithms only consider the movement between two grid endpoints, that is, it must move from one grid endpoint to another adjacent grid endpoint, so there are only 8 directions, while the algorithm in this embodiment allows the first edge intersection on the grid edge To the second edge intersection on any mesh edge ,Therefore, this embodiment can have any direction, can achieve shorter path planning, make the path corners smoother, and have lower requirements on the rotation ability of the robot executing the path.

[0043] For the second edge intersection on the mesh edge , the first edge intersection To the second edge intersection The cost is expressed as:

[0044] in, The first edge intersection To the second edge intersection The distance between them is multiplied by the cost of the line segment's location, The second edge intersection The cost. Figure 4 As shown, the second edge intersection The first grid endpoint is on the grid edge line and the second mesh endpoint , from the first edge intersection To the second edge intersection There are three ways to find the path. The first way is for the drone to follow the first edge intersection. To the first grid endpoint The grid edge travels a distance to the second edge intersection The second method is to fly along the first edge intersection To the second grid endpoint The grid edge travels a distance to the second edge intersection , the third method is the first edge intersection Directly to the second edge intersection .

[0045] Assume that the grid is a unit cell with side length 1, and define The value of the first grid endpoint and the second mesh endpoint Linear interpolation, the calculation formula is:

[0046] in, is from the first grid endpoint To the second edge intersection distance, First grid endpoint The price, Second grid endpoint the price.

[0047] Set the unit cost of the mesh edge to , the unit cost inside the grid is set to , establish several concentric areas outward from the obstacle and preset the unit cost of each area. The area closer to the obstacle has a larger unit cost. The unit cost of the grid edge is determined according to the area where the grid edge is located. The unit cost inside the grid is determined by the area inside the grid. The value of the first edge intersection To the second edge intersection The distance between them multiplied by the cost of the line segment's location The calculation formula is: =

[0048] in, is the distance the drone travels along the edge of the grid, The distance the drone travels within the grid.

[0049] Then find the first edge intersection To the second edge intersection The minimum cost The calculation formula is:

[0050] in, and The second edge intersection points There are first and second grid endpoints on the grid edge line. First grid endpoint The price, Second grid endpoint The price, The second edge intersection To the first grid endpoint distance; Based on the distance the drone travels along the edge of the grid The distance the drone travels within the grid Determine the path with the least cost.

[0051] By introducing the above path cost calculation method into the current planning algorithm, a shorter and smoother path can be planned. First, a map is constructed based on the obstacle information. The obstacles in the map are expanded from the actual obstacles, so they include the actual obstacles. Figure 6 As shown, The starting point is , the black squares are obstacles in the voxel map, and the actual obstacles are the white irregular shapes in the black squares; the conventional algorithm will find the blue path, while the optimized algorithm can find and use the red path as the initial path, which is shorter and has smoother corners.

[0052] Using improved The algorithm generates the initial path result as follows Figure 7 As shown, Figure 7 The black in the middle is the expansion area of the obstacle, and the blue path is Algorithm planning, the red path is the improved Algorithm planning, the improved version can be seen in the upper right corner of the figure The algorithm has a shorter path length, where there are many corners in the voxel map, the improved The algorithm processing is also smoother and more suitable for drone execution.

[0053] In step S4 of this embodiment, Figure 8 As shown, the method for outputting the first optimization trajectory is: Step S401: uniformly select a number of control points in the initial path, and generate a curve trajectory for the control points based on the B-spline curve generation method, such as Figure 8 As shown by the blue points on the red path, the control points of the B-spline curve are obtained to generate the curve trajectory, as shown in Figure 8 The yellow track.

[0054] Step S402: Obtain the first trajectory segment where the curved trajectory collides with an obstacle, replan at a certain distance from the obstacle, and generate a collision-free guidance trajectory. ,like Figure 8 As shown in the purple trajectory, the trajectory starts planning at a unit distance before and after the collision and bypasses the obstacle.

[0055] Step S403: In order to guide the part of the track outside the obstacle, it is necessary to generate new insertion points, obtain the control points on the first track segment, preset a number of insertion points, and construct a number of vectors along the direction of the vertical tangent of the curved track that is the same as the number of insertion points. The starting point of the vector is on the curved track and the end point is on the guide track. For example, Figure 8 As shown, the kth control point on the B-spline curve is , in control points , No. control points , No. control points Two points need to be inserted in order to guide the trajectory outside the obstacle. We select the corresponding control points To control points This section of the original yellow track, on which and Select a point and construct the first vectors Hedi vectors , whose length extends to the guide track ,like Figure 8 Indicated by the green arrow.

[0056] Step S404: Each vector is translated between two control points and the starting point of the vector is translated to the initial path and the end point points to an insertion point. Specifically, the first vector just constructed is translated to the initial path and the end point points to an insertion point. vectors Move to control points To control points The midpoint of vectors Move to control points Hedi control points The midpoint of the vector is the location where the new insertion point is generated, such as Figure 8 middle and shown.

[0057] Step S405: Generate a second track segment using the insertion point and the control points arranged in sequence on the first track segment. arrive This section of the trajectory, using 、 、 、 、 The five points form a new trajectory to replace the original trajectory, such as Figure 8 The orange trace in .

[0058] Step S406: Repeat steps S402 to S405 until all second trajectory segments are obtained, and concatenate all second trajectory segments with the initial path without all first trajectory segments to obtain a first optimized trajectory.

[0059] Steps S402 through S405 are repeated continuously to ensure that all trajectories avoid collisions with obstacles. If the trajectory optimization process fails, meaning the trajectory is not sufficient for the robot to safely pass, a hovering trajectory is generated and the corresponding exception value is returned. This allows the trajectory optimization algorithm to generate a trajectory that can safely navigate narrow areas.

[0060] In this embodiment, the node reallocation process uses a non-uniform B-spline curve, and the time between the control points of the curve is unevenly distributed. This allows the drone to fly at a lower speed when passing through a larger corner, improving the drone's trajectory execution capability.

[0061] In step S5 of this embodiment, each first optimized trajectory control point Each has its corresponding time node , if it represents the time span between two points , then the corresponding speed control point is , for non-uniform B-spline curves, each curve has control points, so we can calculate

[0062] in, is the order of the B-spline curve, is the corresponding time point.

[0063] To ensure the feasibility of the speed, calculate the speed of the above trajectory nodes , when encountering a derivative function beyond feasibility, increase the time to make the flight speed lower: multiply the above speed control point by an operator , the new speed control point is .

[0064] in, is the speed ratio, , is the maximum speed allowed for the drone, The drone will track the speed control point speed. A threshold is also set , so that all , to prevent time from being stretched beyond the range of other nodes.

[0065] Example 2 Example 2 provides a drone dark sensing path planning system based on an array sensor, such as Figure 9 Shown, including: The deployment module is used to arrange several ToF area array sensors on the drone in a circular manner to provide all-round coverage of the drone's surroundings, preset the starting point and target point, and control the drone to depart from the starting point; The omnidirectional obstacle information acquisition module is used to obtain the obstacle depth information of each ToF area array sensor relative to the surrounding obstacles in real time, and perform spatial transformation on all obstacle depth information to obtain the omnidirectional obstacle information of the drone; An initial path acquisition module is used to generate an initial path using an improved algorithm; a first optimized trajectory acquisition module, configured to perform trajectory collision detection based on omnidirectional obstacle information, acquire a first trajectory segment in the initial path that collides with an obstacle, replan the first trajectory segment to generate a second trajectory segment that does not collide with the obstacle, and replace the first trajectory segment in the initial path with the second trajectory segment to obtain a first optimized trajectory; A second optimized trajectory acquisition module is used to reallocate the nodes of the first optimized trajectory in time based on the feasibility of the UAV speed, and output a second optimized trajectory, and the UAV tracks the second optimized trajectory; The execution module is used to repeat the omnidirectional obstacle information acquisition module to the second optimized trajectory acquisition module until the target point is reached.

[0066] Example 3 Example 3 provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, the above-mentioned method for drone dark-light perception path planning based on an area array sensor is implemented.

[0067] Example 4 Embodiment 4 provides a non-transitory computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, the method for planning a path in the dark for a drone based on an area array sensor is implemented.

[0068] The memory in the embodiment of the present invention is used to store various types of data to support the operation of the electronic device. Examples of such data include: any computer program used to operate on the electronic device.

[0069] The method for low-light sensing path planning for drones based on area array sensors disclosed in the embodiments of the present invention can be applied to or implemented by a processor. The processor may be an integrated circuit chip with signal processing capabilities. During implementation, each step of the method for low-light sensing path planning for drones based on area array sensors can be completed by hardware integrated logic circuits or software instructions within the processor. The processor may be a general-purpose processor, a digital signal processor (DSP), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The processor can implement or execute the methods, steps, and logic block diagrams disclosed in the embodiments of the present invention. The general-purpose processor may be a microprocessor or any conventional processor. The steps of the method disclosed in the embodiments of the present invention can be directly executed by a hardware decoding processor or by a combination of hardware and software modules within the decoding processor. The software module may be located in a storage medium located in a memory. The processor reads information from the memory and, in conjunction with its hardware, completes the steps of the method for low-light sensing path planning for drones based on area array sensors provided in the embodiments of the present invention.

[0070] In an exemplary embodiment, the electronic device may be implemented by one or more application-specific integrated circuits (ASICs), DSPs, programmable logic devices (PLDs), complex programmable logic devices (CPLDs), FPGAs, general-purpose processors, controllers, microcontrollers (MCUs), microprocessors, or other electronic components to perform the aforementioned method.

[0071] It is understood that the memory can be volatile memory or non-volatile memory, or can include both volatile and non-volatile memory. Among them, non-volatile memory can be read-only memory (ROM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), electrically erasable programmable read-only memory (EEPROM), magnetic random access memory (FRAM), flash memory, magnetic surface memory, optical disk, or compact disc read-only memory (CD-ROM); magnetic surface memory can be magnetic disk memory or tape memory. Volatile memory can be random access memory (RAM), which is used as an external cache. By way of example and not limitation, many forms of RAM are available, such as static random access memory (SRAM), synchronous static random access memory (SSRAM), dynamic random access memory (DRAM), synchronous dynamic random access memory (SDRAM), double data rate synchronous dynamic random access memory (DDRSDRAM), enhanced synchronous dynamic random access memory (ESDRAM), synchronous link dynamic random access memory (SLDRAM), and direct rambus random access memory (DRRAM).The memories described in the embodiments of the present invention are intended to include, but are not limited to, these and any other suitable types of memories.

[0072] The above embodiments are merely illustrative of the technical solutions of the present invention. The methods of the present invention are not limited solely to those described in the above embodiments, but are subject to the scope defined by the claims. Any modifications, supplements, or equivalent substitutions made by those skilled in the art based on these embodiments are within the scope of protection claimed in the claims.

Claims

1. A method for drone path planning in the dark based on area array sensors, characterized in that: include: Step S1: Arrange several ToF area array sensors in a circular pattern on the drone to provide all-round coverage of the drone's surroundings, preset a starting point and a target point, and control the drone to depart from the starting point; Step S2: Obtain obstacle depth information of each ToF area array sensor relative to surrounding obstacles in real time, and perform spatial transformation on all obstacle depth information to obtain omnidirectional obstacle information of the drone; Step S3: Using improved The algorithm generates the initial path; Step S4: performing trajectory collision detection based on the omnidirectional obstacle information, obtaining a first trajectory segment in the initial path that collides with the obstacle, replanning the first trajectory segment to generate a second trajectory segment that does not collide with the obstacle, and replacing the first trajectory segment in the initial path with the second trajectory segment to obtain a first optimized trajectory; Step S5: reallocating the nodes of the first optimized trajectory in time based on the feasibility of the UAV speed, outputting a second optimized trajectory, and the UAV tracking the second optimized trajectory; Step S6: Repeat steps S2 to S5 until the target point is reached.

2. The method for drone path planning in the absence of light based on area array sensors according to claim 1, characterized in that: The method of step S2 is specifically as follows: Number each ToF area array sensor in a certain order, and set a certain detection angle range for each ToF area array sensor; For each ToF area array sensor, obtain several depth points on the obstacle within the detection angle range, and obtain the spatial position of each depth point relative to the corresponding ToF area array sensor as the obstacle depth information of the ToF area array sensor; A homogeneous transformation matrix is used to represent the position and attitude of each ToF array sensor relative to the drone. Obtain obstacle information of each depth point relative to the drone based on the spatial position of each depth point relative to the corresponding ToF area array sensor and the homogeneous transformation matrix of the ToF area array sensor corresponding to each depth point; All obstacle information at all depth points is aggregated into omnidirectional obstacle information.

3. The method for drone path planning in the dark environment based on area array sensors according to claim 2, characterized in that: The calculation formula for the spatial position of each depth point relative to the corresponding ToF area array sensor is: ， in, 、 They are the horizontal and vertical viewing angles of the ToF area array sensor, 、 Respectively The pixel coordinate position of each depth point relative to the corresponding ToF area array sensor in the width and height directions, It is The vertical distance from each depth point to the corresponding ToF array sensor plane, is the number of all depth points, For the The distance from each depth point to the corresponding ToF array sensor in the X direction, For the The distance from each depth point to the corresponding ToF array sensor in the Y direction, For the The distance from each depth point to the corresponding ToF area array sensor in the Z direction; Based on each The calculation formula for obtaining the obstacle information of each depth point relative to the drone is as follows: in, is the homogeneous transformation matrix; The omnidirectional obstacle information is .

4. The method for drone path planning in the absence of light based on area array sensors according to claim 1, characterized in that: The method of step S3 is specifically as follows: Step S301: generating a voxel map, wherein the voxel map is composed of a plurality of grids; Step S302: Plan an initial path in several grids near the obstacle, wherein the initial path is composed of multiple line segments connected in sequence, each line segment is located in a grid, each line segment includes a first edge intersection and a second edge intersection, and the first edge intersection and the second edge intersection are both on the grid edge line. The first edge intersection of the first line segment is preset, and the first edge intersection of each of the remaining line segments coincides with the second edge intersection of the previous line segment. Based on the first edge intersection, the second edge intersection is searched to generate the path with the minimum cost in the corresponding grid as the line segment of the grid.

5. The method for drone path planning in the absence of light based on area array sensors according to claim 4, characterized in that: The method for finding the second edge intersection point based on the first edge intersection point to generate the minimum cost path in the corresponding grid as a line segment is: Assuming the grid is a unit cell with side length 1, the unit cost of the grid edge is set to , the unit cost inside the grid is set to , establish several concentric areas outward from the obstacle and preset the unit cost of each area. The area closer to the obstacle has a larger unit cost. The unit cost of the grid edge is determined according to the area where the grid edge is located. The unit cost inside the grid is determined by the area inside the grid. The value of , then find the first edge intersection To the second edge intersection The minimum cost The calculation formula is: in, and The second edge intersection points The first grid endpoint and the second grid endpoint on the grid edge line, First grid endpoint The price, Second grid endpoint The price, is the distance the drone travels along the edge of the grid, The distance the drone travels within the grid, The second edge intersection To the first grid endpoint distance; Based on the distance the drone travels along the edge of the grid The distance the drone travels within the grid Determine the path with the least cost.

6. The method for drone path planning in the absence of light based on area array sensors according to claim 1, characterized in that: The method of step S4 is specifically as follows: Step S401: uniformly selecting a number of control points in the initial path, and generating a curve trajectory for the control points based on a B-spline curve generation method; Step S402: obtaining the first trajectory segment where the curved trajectory collides with an obstacle, replanning the trajectory at a certain distance from the obstacle, and generating a collision-free guiding trajectory; Step S403: obtaining control points on the first trajectory segment, presetting a number of insertion points, and constructing a number of vectors along the direction of the vertical tangent of the curved trajectory, the number of which is the same as the number of insertion points, with the starting point of the vector being on the curved trajectory and the end point being on the guide trajectory; Step S404: Each vector is translated between two control points, with the starting point of the vector translated to the initial path and the end point pointing to an insertion point; Step S405: generating a second trajectory segment using the insertion point and control points arranged sequentially on the first trajectory segment; Step S406: Repeat steps S402 to S405 until all second trajectory segments are obtained, and concatenate all second trajectory segments with the initial path without all first trajectory segments to obtain a first optimized trajectory.

7. The method for drone path planning in the absence of light based on area array sensors according to claim 1, characterized in that: The method of step S5 is specifically as follows: Get the first optimization trajectory control points , then the control point Corresponding speed control point The calculation formula is: in, is the order of the B-spline curve, For the Time nodes, For the Time nodes, For the control points; Setting the threshold , for the control points Corresponding speed control point Make adjustments when When the new speed control point The calculation formula is: in, is the speed ratio, , is the maximum speed allowed for the drone, The drone will track the speed control point speed; when When the new speed control point The calculation formula is: 。 8. A drone dark sensing path planning system based on area array sensors, characterized by: include: The deployment module is used to arrange several ToF area array sensors on the drone in a circular manner to provide all-round coverage of the drone's surroundings, preset the starting point and target point, and control the drone to depart from the starting point; The omnidirectional obstacle information acquisition module is used to obtain the obstacle depth information of each ToF area array sensor relative to the surrounding obstacles in real time, and perform spatial transformation on all obstacle depth information to obtain the omnidirectional obstacle information of the drone; Initial path acquisition module, used to adopt improved The algorithm generates the initial path; a first optimized trajectory acquisition module, configured to perform trajectory collision detection based on omnidirectional obstacle information, acquire a first trajectory segment in the initial path that collides with an obstacle, replan the first trajectory segment to generate a second trajectory segment that does not collide with the obstacle, and replace the first trajectory segment in the initial path with the second trajectory segment to obtain a first optimized trajectory; A second optimized trajectory acquisition module is used to reallocate the nodes of the first optimized trajectory in time based on the feasibility of the UAV speed, and output a second optimized trajectory, and the UAV tracks the second optimized trajectory; The execution module is used to repeat the omnidirectional obstacle information acquisition module to the second optimized trajectory acquisition module until the target point is reached.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the program, the method for drone non-light perception path planning based on an area array sensor is implemented as described in any one of claims 1 to 7.

10. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method for drone dark-light perception path planning based on an area array sensor is implemented as described in any one of claims 1 to 7.

Images