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 unmanned aerial vehicle (UAV) perception and path planning, and in particular to a method and system for unmanned aerial vehicle (UAV) non-light perception path planning based on an area array sensor. Background Art

[0002] Today's cities are developing rapidly. In order to meet the needs of industrial production and people's lives, a large number of cables and pipes are laid underground in cities and factories. In order to protect the cables, a special channel for laying these cables, the cable trench, is constructed. The movable space inside the cable trench is extremely narrow, with a cross-sectional size of only about 40cm×50cm. After the laying is completed, the trench will begin to cap, and an inspection port will be opened every few hundred meters, which brings great difficulties to cable inspection.

[0003] At present, manual inspection is the main method used for cables in cable trenches. If manual inspection fails, the inspection can only 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. The work is cumbersome and prone to repeated 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 lightless 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, in the research on inspection in narrow environments, there are still many challenges in obstacle perception and path planning, such as the small size of obstacles, the large working range of ToF array sensors, and the inability to use GPS positioning. Traditional perception and planning methods rely on lidar or visual point clouds for real-time mapping, which has a large amount of calculation and is difficult to deploy on small mobile platforms; traditional planning methods rely on potential field methods, which have a large amount of calculation and high planning delay, and cannot meet real-time needs. Therefore, designing a method that can perceive the surrounding environment in real time and perform fast path planning in narrow and dark environments has become an urgent need in the industry to ensure that drones can perform inspection tasks safely and efficiently in complex environments and expand the scope of cable inspection. Summary of the invention

[0005] In view of the deficiencies in the prior art, the purpose of the present invention is to provide a method and system for drone dark-light perception path planning based on area array sensors, which can still perform perception in a dark and narrow environment, is particularly suitable for the detection of 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) non-light sensing path planning based on an array sensor, comprising: Step S1: Arrange a number of ToF array sensors on the drone in a circular direction to provide all-round coverage of the drone's surroundings, preset a starting point and a target point, and control the drone to start from the starting point; Step S2: Obtain obstacle depth information of each ToF 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 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 drone speed, outputting a second optimized trajectory, and the drone tracks 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, a number of depth points are obtained on the obstacle within the detection angle range, and the spatial position of each depth point relative to the corresponding ToF area array sensor is obtained as the obstacle depth information of the ToF area array sensor; A homogeneous transformation matrix is ​​used to characterize 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 array sensor and the homogeneous transformation matrix of the ToF 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 the depth points relative to the corresponding ToF area array sensor in the width and height directions, It is The vertical distance from the depth point to the corresponding ToF array sensor plane, is the number of all depth points, For the The distance from a depth point to the corresponding ToF array sensor in the X direction, For the The distance from a depth point to the corresponding ToF array sensor in the Y direction, For the The distance from each depth point to the corresponding ToF 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, and based on the first edge intersection, the second edge intersection is found to generate a path with the minimum cost in the corresponding grid as a 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: Assume that the grid is a unit cell with side length 1, and set the unit cost of the grid 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 unit cost of the area closer to the obstacle is larger. 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 at a position a certain distance from the obstacle, and generating a collision-free guiding trajectory; Step S403: obtaining control points on the first track segment, presetting a number of insertion points, and constructing a number of vectors along the direction of the vertical tangent of the curve track, the number of which is the same as the number of insertion points, wherein the starting point of the vector is on the curve track and the end point is on the guide track; 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; Step S405: Generate 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 point 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 unmanned aerial vehicles based on an array sensor, comprising: The deployment module is used to arrange a number of ToF array sensors on the drone in a circular manner to provide all-round coverage of the drone's surroundings, preset a starting point and a target point, and control the drone to start from the starting point; The omnidirectional obstacle information acquisition module is used to obtain the obstacle depth information of each ToF 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 is used to perform trajectory collision detection based on omnidirectional obstacle information, acquire a first trajectory segment in the initial path that collides with the obstacle, replan the first trajectory segment to generate a second trajectory segment that does not collide with the obstacle, and obtain a first optimized trajectory after replacing the first trajectory segment in the initial path with the second trajectory segment; 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 comprises 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 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 unmanned aerial vehicle dark-light perception path planning method 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 plurality of ToF array sensors in a circular pattern on the UAV to provide all-round coverage of the surroundings of the UAV, and can obtain the surrounding environment obstacle information in real time. Combined with path planning, safe obstacle avoidance and movement can be achieved. When encountering an obstacle that cannot be passed, operations such as hovering and returning can be performed, thereby improving safety.

[0022] (2) The present invention adopts an 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 that are dark and narrow, protecting cables and facilitating cable 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 working quality, 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 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 to update, saves more software resources, and can be deployed on a low-cost platform. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] The drawings described herein are used to provide a further understanding of the present invention and constitute a part of this application. The illustrative 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 drawings: Figure 1 This is a flow chart of a method for unmanned aerial vehicle (UAV) non-light sensing path planning based on an area array sensor according to the present invention; Figure 2 It is a 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 An improved 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; Fig. 9 The present invention is a schematic diagram of a non-light sensing path planning system for an unmanned aerial vehicle based on an area array sensor. DETAILED DESCRIPTION

[0027] The technical solutions in the embodiments of the present invention will be described clearly and completely below in conjunction with 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 positions or positional relationships based on the positions 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", and the like are only used for descriptive purposes, and cannot be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, features defined as "first", "second", and the like 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 Embodiment 1 provides a method for drone non-light sensing path planning based on an array sensor, such as Figure 1 As shown, including: Step S1: Arrange a number of ToF array sensors on the drone in a circular direction to provide all-round coverage of the drone's surroundings, preset a starting point and a target point, and control the drone to start from the starting point; Step S2: Obtain obstacle depth information of each ToF 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 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 drone speed, outputting a second optimized trajectory, and the drone tracks the second optimized trajectory; Step S6: Repeat steps S2 to S5 until the target point is reached.

[0030] The present embodiment provides a method for planning a path for drones in the dark based on an array sensor, which does not rely on ambient light sources and can still sense in a dark environment. It is particularly suitable for detecting environments such as cable trenches. The present 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 to ensure that the drone can travel safely in narrow areas. However, the first optimized trajectory is evenly distributed in time, and the drone will still fly at a relatively high speed when passing a corner, which easily weakens the drone's trajectory execution capability. Therefore, the present embodiment reallocates the nodes of the first optimized trajectory in time based on the feasibility of the drone's speed, outputs a second optimized trajectory, and the drone tracks the second optimized trajectory, so that the second optimized trajectory ensures the feasibility of the drone's speed, flies at a lower speed when passing a larger corner, and improves the drone's trajectory execution capability. Compared with the potential field method, the present embodiment does not rely on a potential field map that requires a large amount of calculation and update, saves more software resources, and can be deployed on a low-cost platform.

[0031] The following is a detailed description of a drone dark-light perception path planning method 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 of arranging a plurality of ToF array sensors in a circular direction on a UAV to provide all-round coverage around the UAV is as follows: designing the angle and position of the ToF array sensors according to the size and configuration of the UAV, fixing the ToF array sensors on the frame or protective frame of the UAV, and adjusting the directions of the ToF array sensors so that all ToF array sensors will not interfere with each other and can achieve an all-directional coverage effect, thereby 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 a ToF area array sensor is 45°.

[0033] In step S2 of this embodiment, Figure 3 As shown in FIG. 1 , 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 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 a number of depth points on the obstacle within the detection angle range, and obtain 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 characterize the position and attitude of each ToF 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 of 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 area array depth value in a certain order is specifically 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 the depth point relative to the corresponding ToF 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 the depth point to the corresponding ToF array sensor plane, is the number of depth points, For the The distance from a depth point to the corresponding ToF array sensor in the X direction, For the The distance from a 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 takes the center of the frame as the origin and establishes an ENU (north-east-sky) coordinate system. 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 the 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 test of the embodiment, 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: planning an initial path in several grids near the obstacle, wherein the initial path is composed of a plurality of 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 located 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, and based on the first edge intersection, the second edge intersection is searched to generate a 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, it can be any direction.

[0042] like Figure 4 As shown, other algorithms only consider the movement between two grid endpoints, that is, they 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 of 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 The distance First grid endpoint The price, Second grid endpoint The cost.

[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 unit cost of the area closer to the obstacle is larger. 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 the first grid endpoint and the second grid endpoint 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 bringing 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 results as follows Figure 7 As shown, Figure 7 The black part 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, such as 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 guiding 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 a new insertion point, obtain the control point 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 curve track that is consistent with the number of insertion points. The starting point of the vector is on the curve 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 beyond the obstacle. We select the corresponding Control Points To Control Points This section of the original yellow track, on which and Select a point at the right angle to construct the first Vector and Vector , 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. Vector Shift to Control Points To Control Points The midpoint of Vector Shift to Control Points and 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 track, using , , , , The five points form a new trajectory to replace the original trajectory, such as Figure 8 The orange trace in Figure 2 shows the

[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 to S405 are repeated continuously to ensure that all trajectories do not collide with obstacles. If the trajectory optimization process cannot be achieved, that is, the trajectory cannot satisfy the robot's safe passage, a hovering trajectory is generated and the corresponding abnormal value is returned. In this way, the trajectory optimization algorithm can generate a trajectory that can be safely driven in 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, which allows the drone to fly at a lower speed when passing a larger corner, thereby improving the passing ability of the drone trajectory execution.

[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 segment 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 Embodiment 2 provides a non-light sensing path planning system for unmanned aerial vehicles based on an array sensor, such as Fig. 9 As shown, including: The deployment module is used to arrange a number of ToF array sensors on the drone in a circular manner to provide all-round coverage of the drone's surroundings, preset a starting point and a target point, and control the drone to start from the starting point; The omnidirectional obstacle information acquisition module is used to obtain the obstacle depth information of each ToF 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 is used to perform trajectory collision detection based on omnidirectional obstacle information, acquire a first trajectory segment in the initial path that collides with the obstacle, replan the first trajectory segment to generate a second trajectory segment that does not collide with the obstacle, and obtain a first optimized trajectory after replacing the first trajectory segment in the initial path with the second trajectory segment; 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 Embodiment 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 non-light perception path planning based on 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 above-mentioned method for drone non-light perception path planning based on area array sensors 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 unmanned aerial vehicle no-light perception path planning based on an array sensor disclosed in an embodiment of the present invention can be applied to a processor or implemented by a processor. The processor may be an integrated circuit chip with signal processing capabilities. In the implementation process, each step of the method for unmanned aerial vehicle no-light perception path planning based on an array sensor can be completed by an integrated logic circuit of hardware in the processor or instructions in the form of software. The above-mentioned processor may be a general-purpose processor, a digital signal processor (DSP, Digital Signal Processor), or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc. The processor can implement or execute the various 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, etc. In combination with the steps of the method disclosed in the embodiment of the present invention, it can be directly embodied as a hardware decoding processor to execute, or it can be executed by a combination of hardware and software modules in the decoding processor. The software module can be located in a storage medium, which is located in a memory. The processor reads the information in the memory and completes the steps of the method for unmanned aerial vehicle no-light perception path planning based on an array sensor provided in an embodiment of the present invention in combination with its hardware.

[0070] In an exemplary embodiment, the electronic device may be implemented by one or more application specific integrated circuits (ASIC), DSP, programmable logic device (PLD), complex programmable logic device (CPLD), FPGA, general purpose processor, controller, microcontroller (MCU), microprocessor, or other electronic components to execute the aforementioned method.

[0071] It can be understood that the memory can be a volatile memory or a non-volatile memory, and can also include both volatile and non-volatile memories. Among them, the non-volatile memory can be a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), a magnetic random access memory (FRAM), a flash memory, a magnetic surface memory, an optical disc, or a compact disc read-only memory (CD-ROM); the magnetic surface memory can be a disk memory or a tape memory. The volatile memory can be a 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 examples of the technical solutions of the present invention. The methods involved in the present invention are not limited to the contents described in the above embodiments, but are subject to the scope defined in the claims. Any modification, supplement or equivalent replacement made by a person skilled in the art based on the embodiment is within the scope of protection required by the claims of the present invention.

Claims

1. A method for drone non-light sensing path planning based on area array sensor, characterized in that: include: Step S1: Arrange a number of ToF array sensors on the drone in a circular direction to provide all-round coverage of the drone's surroundings, preset a starting point and a target point, and control the drone to start from the starting point; Step S2: Obtain obstacle depth information of each ToF 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 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 drone speed, outputting a second optimized trajectory, and the drone tracks the second optimized trajectory; Step S6: Repeat steps S2 to S5 until the target point is reached.

2. The method for drone non-light sensing path planning based on area array sensor according to claim 1 is 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, a number of depth points are obtained on the obstacle within the detection angle range, and the spatial position of each depth point relative to the corresponding ToF area array sensor is obtained as the obstacle depth information of the ToF area array sensor; A homogeneous transformation matrix is ​​used to characterize 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 array sensor and the homogeneous transformation matrix of the ToF 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 non-light sensing path planning based on area array sensor according to claim 2, characterized in that: The calculation formula for the spatial position of each depth point relative to the corresponding ToF 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 the depth point to the corresponding ToF array sensor plane, is the number of all depth points, For the The distance from a depth point to the corresponding ToF array sensor in the X direction, For the The distance from a depth point to the corresponding ToF array sensor in the Y direction, For the The distance from each depth point to the corresponding ToF 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 non-light sensing path planning based on area array sensor according to claim 1 is 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: planning an initial path in several grids near the obstacle, wherein the initial path is composed of a plurality of 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 located 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, and based on the first edge intersection, the second edge intersection is searched to generate a path with the minimum cost in the corresponding grid as the line segment of the grid.

5. The method for drone non-light sensing path planning based on area array sensor according to claim 4 is characterized in that: The method of finding the second edge intersection point based on the first edge intersection point to generate the path with the minimum cost in the corresponding grid as a line segment is: Assume that the grid is a unit cell with side length 1 and set the unit cost of the grid 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 unit cost of the area closer to the obstacle is larger. 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 non-light sensing path planning based on area array sensor 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 at a position a certain distance from the obstacle, and generating a collision-free guiding trajectory; Step S403: obtaining control points on the first track segment, presetting a number of insertion points, and constructing a number of vectors along the direction of the vertical tangent of the curve track, the number of which is the same as the number of insertion points, wherein the starting point of the vector is on the curve track and the end point is on the guide track; 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; Step S405: Generate 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 non-light sensing path planning based on area array sensor according to claim 1 is 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 point 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 non-light sensing path planning system based on an array sensor, characterized in that: include: The deployment module is used to arrange a number of ToF array sensors on the drone in a circular manner to provide all-round coverage of the drone's surroundings, preset a starting point and a target point, and control the drone to start from the starting point; The omnidirectional obstacle information acquisition module is used to obtain the obstacle depth information of each ToF 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 is used to perform trajectory collision detection based on omnidirectional obstacle information, acquire a first trajectory segment in the initial path that collides with the obstacle, replan the first trajectory segment to generate a second trajectory segment that does not collide with the obstacle, and obtain a first optimized trajectory after replacing the first trajectory segment in the initial path with the second trajectory segment; 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 unmanned aerial vehicle non-light perception path planning method based on the 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 non-light perception path planning based on an area array sensor is implemented as described in any one of claims 1 to 7.

Images