A method for dynamic obstacle avoidance based on KD tree, and a storage medium and an electronic device thereof

Abstract

The application discloses a method for dynamic obstacle avoidance based on a KD tree and a storage medium and an electronic device thereof, which comprises the following steps: combining and generating a KD tree point cloud graph after processing a sensor point cloud obtained by a robot; transposing a preset robot footprint to a preset trajectory point pose of the robot in the KD tree point cloud graph; searching for cloud points near the center of the robot footprint in the KD tree point cloud graph by using radius search and / or k-nearest neighbor search, and obtaining searched point clouds; traversing the searched point clouds, and judging whether the searched point clouds are inside the robot footprint; if yes, it is indicated that the robot collides with an obstacle; otherwise, the robot will not collide with the obstacle; the above technical scheme can help the robot to realize more accurate dynamic obstacle avoidance, and greatly improves the passability and safety of the robot.

Description

Technical Field

[0001] This invention relates to the fields of automatic control and robot navigation technology, and in particular to a method for dynamic obstacle avoidance based on KD tree, its storage medium, and electronic equipment. Background Technology

[0002] Existing robots primarily use cost maps for perception and control. The core principle of cost maps is to discretize the map into a grid map and assign a cost value to each grid cell. The idea behind grid maps is to divide a working environment into many grids horizontally and vertically according to a preset resolution, where obstacle occupancy within the same grid cell is considered uniform. This allows a finite number of grid cells to represent the obstacle situation on a finite-sized plane. Dynamic obstacle avoidance for robots involves predicting whether a future trajectory point pose is likely to collide with an obstacle, thereby achieving obstacle avoidance. Traditional grid map dynamic obstacle avoidance methods transpose a preset robot footprint (a robot footprint, also known as a collision box, is a polygon composed of coordinate points) to the preset trajectory point pose in the grid map; then, they sequentially check whether each grid cell containing the robot footprint contains an obstacle. If any grid cell contains an obstacle, the robot collides with that obstacle. The accuracy of robot dynamic obstacle avoidance is affected by the resolution of the grid map. If the grid map resolution is low, there are fewer grids in the grid map, meaning each grid area is large. This will result in a larger error when judging collision obstacles (by default, obstacles within a grid are fully occupied). The lower the resolution, the larger the error. If the grid map resolution is increased, the amount of grid map data will increase exponentially, consuming a huge amount of storage space and bringing considerable computational complexity, making it difficult for general CPU processors to handle. In addition, the grid occupied by obstacles and the grid of safe areas in the grid map have the same space overhead, resulting in a lot of wasted performance.

[0003] A laser SLAM adaptive resolution grid map construction method (patent number CN 111381245 B) involves: initially defining three resolutions (high, medium, and low) as Rh, Rm, and Rl respectively, and defining parameters A, B, and N; acquiring laser radar point cloud information; detecting straight line segments and arc segments in the point cloud information; counting the total number of detected straight line segments and arc segments (C1) and the number of point cloud segments not included in the straight line segments and arc segments (C2); first automatically setting the map resolution R* based on the values ​​of C1 and C2, and then adjusting it based on the point cloud distance information to obtain the map resolution R; and updating the map. This technical solution uses data collected by laser radar to determine the density of obstacles in the current environment, and then combines this with the distance information from obstacles to the robot to adjust the map resolution in real time. Applying this grid map can improve the accuracy of the robot's dynamic obstacle avoidance while effectively reducing storage space and saving computing resources. It solves the problem that the time and space overhead of the grid occupied by obstacles and the grid of the safe area in the grid map are the same, resulting in significant performance waste. However, the above grid map still consumes a large amount of storage space and has relatively high computational complexity. Summary of the Invention

[0004] Therefore, a dynamic obstacle avoidance method based on KD-tree is needed to solve the problems of low accuracy, high storage space consumption, and long time required to determine whether there are obstacles in all grids where the robot's footprint is located, which are problems of traditional grid maps.

[0005] To achieve the above objectives, the present invention provides a method for dynamic obstacle avoidance based on KD-trees, which includes the following steps:

[0006] The sensor point clouds obtained by the robot are processed and merged to generate a KD tree point cloud map.

[0007] Transpose the preset robot footprint to the preset trajectory point pose in the KD tree point cloud map;

[0008] The searched point cloud is obtained by using radius search and / or k-nearest neighbor search in the KD tree point cloud map to search for cloud points near the center of the robot footprint;

[0009] Determine whether the searched point cloud is inside the robot's footprint; if it is, it means the robot has collided with an obstacle; otherwise, the robot will not collide with an obstacle.

[0010] Furthermore, the process of processing and merging the sensor point clouds obtained by the robot to generate a KD tree point cloud map includes the following steps:

[0011] Filter the multiple point clouds obtained by the robot from the sensors;

[0012] The filtered point clouds are merged to form a point cloud set;

[0013] Import the point cloud collection into a KD tree to generate a tree-structured point cloud map.

[0014] Furthermore, the search center for the radius search in the KD tree point cloud is the robot footprint center, and the search radius is the farthest distance from the robot footprint center to the edge of the robot footprint.

[0015] Furthermore, before determining whether the searched point cloud is inside the robot's footprint after obtaining the searched point cloud, the following steps are also included:

[0016] Count the number of point clouds retrieved;

[0017] Determine if the number of points in the searched point cloud is equal to 0. If it is equal to 0, it means that the robot will not collide with obstacles.

[0018] Furthermore, when it is determined that the number of points retrieved is not equal to 0,

[0019] It also includes obtaining point clouds that are close to the robot's center from the searched point clouds, and determining whether the distance between the point cloud that is close to the robot's center and the robot's center is less than or equal to the closest distance between the edge of the robot's footprint and the center of the robot's footprint. If it is less than or equal to the closest distance between the edge of the robot's footprint and the center of the robot's footprint, it means that the robot has collided with an obstacle.

[0020] Furthermore, when determining whether the searched point cloud is inside the robot's footprint, the pnPoly algorithm is used to determine whether the searched point cloud is inside the robot's footprint, including the following steps:

[0021] From the point cloud obtained by the judgment search, obtain the point cloud of the shortest distance between the robot footprint edge and the robot footprint center to the shortest distance between the robot footprint edge and the robot footprint center;

[0022] Draw a ray from the obtained point cloud and count the number of intersections between this ray and the robot's footprint.

[0023] If the number of intersection points is odd, it means the point cloud is inside the robot's footprint; otherwise, it means the point cloud is outside the robot's footprint.

[0024] Furthermore, before determining whether the searched point cloud is inside the robot's footprint after obtaining the searched point cloud, the following steps are also included:

[0025] Obtain the point cloud that is close to the robot's center from the searched point cloud. Determine whether the distance between the point cloud that is close to the robot's center and the robot's center is less than or equal to the nearest distance between the edge of the robot's footprint and the center of the robot's footprint. If it is less than or equal to the nearest distance between the edge of the robot's footprint and the center of the robot's footprint, it means that the robot has collided with an obstacle.

[0026] Furthermore, when determining whether the searched point cloud is inside the robot's footprint, the pnPoly algorithm is used to determine whether the searched point cloud is inside the robot's footprint, including the following steps:

[0027] The point cloud obtained from the search leads to a ray. The number of intersections between this ray and the robot's footprint is counted.

[0028] If the number of intersection points is odd, it means the point cloud is inside the robot's footprint; otherwise, it means the point cloud is outside the robot's footprint.

[0029] A storage medium storing a computer program that, when executed by a processor, implements the steps of the above-described KD-tree-based dynamic obstacle avoidance method.

[0030] An electronic device includes a storage medium and a processor, wherein a computer program is stored on the storage medium, and the computer program, when executed by the processor, implements the steps of the above-described KD-tree-based dynamic obstacle avoidance method.

[0031] Unlike existing technologies, the above-mentioned technical solution generates a KD tree point cloud map from the sensor point cloud obtained by the robot. Compared with the time required to generate a grid map, the time required to generate a KD tree point cloud map is significantly reduced, thus optimizing space overhead. Furthermore, the accuracy of the KD tree point cloud map is of float type, approximately 6-7 significant digits, which is 3-4 orders of magnitude higher than the accuracy of traditional grid maps. By transposing the preset robot footprint onto the robot's current pose or preset trajectory point pose in the KD tree point cloud map, the KD tree point cloud map can be used to search for nearby point clouds of the robot footprint. The searched point clouds are then determined by whether they are within the robot footprint, thus determining whether the robot will collide with an obstacle. Compared with traditional grid maps, the time required to search for nearby point clouds of the robot footprint is also significantly reduced. This helps the robot achieve more accurate dynamic obstacle avoidance, greatly improving the robot's passability and safety. Attached Figure Description

[0032] Figure 1 This is a flowchart illustrating the KD-tree-based dynamic obstacle avoidance method of the present invention;

[0033] Figure 2This is a schematic diagram illustrating the specific process of the KD-tree-based dynamic obstacle avoidance method of the present invention;

[0034] Figure 3 This is a schematic diagram illustrating the specific process of the KD-tree-based dynamic obstacle avoidance method of the present invention. Detailed Implementation

[0035] To explain in detail the technical content, structural features, objectives, and effects of the technical solution, the following description is provided in conjunction with specific embodiments and accompanying drawings.

[0036] In this document, the term "embodiment" means that a specific feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The term "embodiment" appearing in various places throughout the specification does not necessarily refer to the same embodiment, nor does it specifically limit its independence or connection with other embodiments. In principle, in this application, as long as there are no technical contradictions or conflicts, the technical features mentioned in each embodiment can be combined in any way to form corresponding implementable technical solutions.

[0037] Unless otherwise defined, the technical terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application pertains; the use of related terms herein is merely for the purpose of describing particular embodiments and is not intended to limit this application.

[0038] In the description of this application, the term "and / or" is used to describe the logical relationship between objects, indicating that three relationships can exist. For example, A and / or B means: A exists, B exists, and A and B exist simultaneously. Additionally, the character " / " in this document generally indicates that the preceding and following objects have an "or" logical relationship.

[0039] In this application, terms such as “first” and “second” are used only to distinguish one entity or operation from another, and do not necessarily require or imply any actual quantity, hierarchy or order relationship between these entities or operations.

[0040] Unless otherwise specified, the use of terms such as “comprising,” “including,” “having,” or other similar expressions in this application is intended to cover non-exclusive inclusion, which does not exclude the presence of additional elements in a process, method, or product that includes the stated elements, such that a process, method, or product that includes a list of elements may include not only those defined elements but also other elements not expressly listed, or elements inherent to such a process, method, or product.

[0041] Similar to the interpretation in the Patent Examination Guidelines, in this application, expressions such as "greater than," "less than," and "exceeding" are understood to exclude the stated number; expressions such as "above," "below," and "within" are understood to include the stated number. Furthermore, in the description of the embodiments in this application, "multiple" means two or more (including two), and similar expressions related to "multiple" are also interpreted in this way, such as "multiple groups" and "multiple times," unless otherwise explicitly specified.

[0042] In the description of the embodiments of this application, the space-related expressions used, such as "center," "longitudinal," "lateral," "length," "width," "thickness," "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "vertical," "top," "bottom," "inner," "outer," "clockwise," "counterclockwise," "axial," "radial," and "circumferential," indicate the orientation or positional relationship based on the orientation or positional relationship shown in the specific embodiments or drawings. They are only for the purpose of describing the specific embodiments of this application or for the reader's understanding, and do not indicate or imply that the device or component referred to must have a specific position, a specific orientation, or be constructed or operated in a specific orientation. Therefore, they should not be construed as limitations on the embodiments of this application.

[0043] Unless otherwise expressly specified or limited, the terms "installation," "connection," "linking," "fixing," and "setting," as used in the description of the embodiments of this application, should be interpreted broadly. For example, "connection" can be a fixed connection, a detachable connection, or an integral setting; it can be a mechanical connection, an electrical connection, or a communication connection; it can be a direct connection or an indirect connection through an intermediate medium; it can be the internal connection of two components or the interaction between two components. For those skilled in the art to which this application pertains, the specific meaning of the above terms in the embodiments of this application can be understood according to the specific circumstances.

[0044] See Figures 1 to 3As shown, this invention provides a method for dynamic obstacle avoidance based on KD trees. It generates a KD tree point cloud map from the sensor point cloud obtained by the robot. Compared to generating a grid map, the time required to generate a KD tree point cloud map is significantly reduced, optimizing space overhead. Furthermore, the accuracy of the KD tree point cloud map is of float type, approximately 6-7 significant digits, which is 3-4 orders of magnitude higher than the accuracy of traditional grid maps (whose resolution is mostly 0.05m). By transposing the preset robot footprint onto the robot's preset trajectory point pose in the KD tree point cloud map, the KD tree point cloud map can search for nearby point clouds of the robot footprint to obtain the searched point cloud. By determining whether the searched point cloud is within the robot footprint, it is determined whether the robot will collide with an obstacle. Compared to traditional grid maps, the time required to search for nearby point clouds of the robot footprint is also significantly reduced. Using this method continuously while the robot is moving helps it achieve more accurate dynamic obstacle avoidance, greatly improving the robot's passability and safety.

[0045] The following provides a detailed description of a KD-tree-based dynamic obstacle avoidance method provided by an embodiment of the present invention. See also... Figure 1 As shown, a method for dynamic obstacle avoidance based on KD-trees includes the following steps:

[0046] S1. Process and merge multiple point clouds obtained by the robot's sensors to generate a KD tree point cloud map;

[0047] S2. Transpose the preset robot footprint to the preset trajectory point pose in the KD tree point cloud diagram;

[0048] S3. Use the radius search and / or k-nearest neighbor search in the KD tree point cloud map to find the cloud points near the center of the robot footprint and obtain the searched point cloud.

[0049] S4. Determine whether the searched point cloud is inside the robot's footprint; if it is, it means the robot has collided with the obstacle; otherwise, the robot will not collide with the obstacle.

[0050] The point cloud described above is obtained by sensors scanning the robot while it is moving in the working environment. The point cloud is an array of points on obstacles scanned by the sensors in the working environment. The above processing involves denoising the point cloud obtained by the sensors (the initial point cloud obtained by the sensors is inevitably contaminated by noise during acquisition, processing, and transmission). The above merging refers to merging multiple point clouds, that is, merging arrays of multiple points to form a single point cloud set. The KD-tree mentioned above is short for Multidimensional Binary Search Tree, where k represents the dimension of the data space. A KD-tree is a binary search tree, a tree-shaped data structure that stores instance points in k-dimensional space for fast retrieval. The above generation of the KD-tree point cloud diagram involves constructing a KD-tree from the point cloud set, that is, continuously inserting each point cloud as a node into the KD-tree based on its coordinate information, thus generating a two-dimensional KD-tree point cloud diagram. The robot footprint mentioned above is a preset polygon. Transposing the preset robot footprint to the preset trajectory point pose in the KD tree point cloud diagram results in a polygon composed of coordinate points. The preset trajectory point pose is used to predict and simulate the trajectory point pose to be reached in the next time period. The use of radius search and / or k-nearest neighbor search in the KD tree point cloud diagram to search for cloud points near the robot footprint center means that in some embodiments, radius search is used to search for cloud points near the robot footprint center; in some embodiments, k-nearest neighbor search is used; and in some embodiments, both radius search and k-nearest neighbor search are used to search for cloud points near the robot footprint center, and the union of the two search results is the searched point cloud. In the k-nearest neighbor search, K represents the number of point clouds obtained. By setting the value of K, the K adjacent K point clouds on the KD tree can be searched for near the robot footprint center in the KD tree point cloud diagram. The radius search described above involves setting a search radius to find all cloud points in the KD tree whose distance from the robot's footprint center is less than the search radius.

[0051] In step S1 above, the process of merging the sensor point clouds obtained by the robot to generate a KD tree point cloud map includes the following steps:

[0052] Filter the multiple point clouds obtained by the robot from the sensors;

[0053] The filtered point clouds are merged to form a point cloud set;

[0054] Import the point cloud collection into a KD tree to generate a tree-structured point cloud map.

[0055] Since the initial point clouds obtained by the sensor are inevitably contaminated by noise during the acquisition, processing, and transmission processes, noise reduction is performed on the multiple point clouds through filtering to make the obtained point clouds represent the actual obstacles. The aforementioned point cloud is an array of points scanned by the sensor on obstacles in the working environment. Merging multiple point clouds refers to combining the arrays of multiple points to form a single point cloud set. When importing the point cloud set into a KD tree to generate a tree-structured point cloud graph, some embodiments use the PCL library to generate the tree-structured point cloud graph, that is, using pcl::KDTreeFLANN::setInputKDTree() to import the point cloud set into the KD tree to generate the tree-structured point cloud graph.

[0056] In radius search, the search radius setting affects both the search time and the time required to determine whether the point cloud is inside the robot footprint. Preferably, the search radius is the farthest distance between the center of the robot footprint and the edge of the robot footprint. This search radius ensures that all point clouds within the robot footprint can be searched, while minimizing search time and the amount of data in the searched point clouds. This results in fewer point clouds being judged later, avoiding the omission of point clouds and preventing robot collisions.

[0057] To minimize the time spent determining whether a collision has occurred, in some embodiments, after obtaining the searched point cloud, before determining whether the searched point cloud is inside the robot's footprint, the number of searched point clouds can be determined. This specifically includes the following steps:

[0058] Count the number of point clouds retrieved;

[0059] Determine if the number of points in the searched point cloud is equal to 0. If it is equal to 0, it means that the robot will not collide with obstacles.

[0060] When the number of searched point clouds is 0, it undoubtedly means that the robot will not collide with obstacles. Therefore, there is no need to continue to determine whether the searched point cloud is inside the robot's footprint. The current preset trajectory point pose determination can be ended, the tree structure point cloud map can be cleared, and the next preset trajectory point pose determination can be restarted, which greatly shortens the obstacle avoidance warning time.

[0061] A further improvement in the above embodiments is that when the number of searched point clouds is not equal to 0,

[0062] This also includes obtaining point clouds that are close to the robot's center from the searched point cloud, and determining whether the distance between these point clouds and the robot's center is less than or equal to the nearest distance between the robot's footprint edge and the robot's footprint center. If it is, it indicates that the robot has collided with an obstacle. If the distance between these point clouds and the robot's center is less than or equal to the nearest distance between the robot's footprint edge and the robot's footprint center, then these point clouds must be inside the robot's footprint and will inevitably collide with an obstacle. This allows the current preset trajectory point pose determination to end, the tree-structured point cloud map to be cleared, and the next preset trajectory point pose determination to begin, significantly shortening obstacle avoidance warning time. Otherwise, if the point cloud close to the robot's center is located between the nearest distance between the robot's footprint edge and the robot's footprint center, it does not necessarily mean it will collide with an obstacle. Therefore, it is only necessary to obtain point clouds that are close to the robot's center from the searched point cloud; the step of determining whether the distance between the searched point cloud and the robot's center is less than or equal to the nearest distance between the robot's footprint edge and the robot's footprint center is unnecessary, thus shortening the determination time.

[0063] See Figure 2 As shown, a further improvement in the above embodiments,

[0064] When determining whether the searched point cloud is inside the robot's footprint, the pnPoly algorithm is used to determine whether the searched point cloud is inside the robot's footprint, including the following steps:

[0065] From the point cloud obtained by the judgment search, obtain the point cloud of the shortest distance between the robot footprint edge and the robot footprint center to the shortest distance between the robot footprint edge and the robot footprint center;

[0066] Draw a ray from the obtained point cloud and count the number of intersections between this ray and the robot's footprint.

[0067] If the number of intersection points is odd, it means the point cloud is inside the robot's footprint; otherwise, it means the point cloud is outside the robot's footprint.

[0068] After determining whether the distance between the point cloud closest to the robot's center and the robot's center is less than or equal to the nearest distance between the robot's footprint edge and the robot's footprint center, the current preset trajectory point pose determination can be terminated, the tree-structured point cloud map cleared, and the next preset trajectory point pose determination can begin. Otherwise, only the point cloud closest to the robot's center needs to be determined if it lies between the nearest distance between the robot's footprint edge and the robot's footprint center. This reduces the number of point clouds to be determined; only the point clouds whose distance to the robot's center is greater than the nearest distance between the robot's footprint edge and the robot's footprint center need to be determined, eliminating the need to determine all the point clouds. This reduces the time required to determine whether a point cloud is inside the robot's footprint, thus shortening the obstacle avoidance warning time. There are many methods for determining whether a searched point cloud is inside the robot's footprint, as long as it can determine whether the searched point cloud is inside the robot's footprint. See also... Figure 3 As shown, in some embodiments, the distance between the point cloud and each edge of the robot footprint is calculated to obtain the edge closest to the point cloud. The distance between the center of the robot footprint and the point cloud is also calculated. The distance between the center of the robot footprint and the edge closest to the point cloud is then compared. If the distance between the center of the robot footprint and the point cloud is less than or equal to the distance between the center of the robot footprint and the edge closest to the point cloud, it indicates that the point cloud is inside the robot footprint, and the robot will collide with the obstacle. Otherwise, it indicates that the point cloud is outside the robot footprint, and the robot will not collide with the obstacle.

[0069] To minimize the time required to determine if a collision has occurred, in some embodiments, after obtaining the searched point cloud, before determining whether the searched point cloud is inside the robot's footprint, it can be determined whether the distance between the searched point cloud and the robot's center is less than or equal to the closest distance between the edge of the robot's footprint and the center of the robot's footprint. This specifically includes the following steps:

[0070] Obtain the point cloud that is close to the robot's center from the searched point cloud. Determine whether the distance between the point cloud that is close to the robot's center and the robot's center is less than or equal to the nearest distance between the edge of the robot's footprint and the center of the robot's footprint. If it is less than or equal to the nearest distance between the edge of the robot's footprint and the center of the robot's footprint, it means that the robot has collided with an obstacle.

[0071] If the distance between a point cloud instance close to the robot's center and the robot's center is less than or equal to the nearest distance between the robot's footprint edge and the robot's footprint center, then the point cloud instance close to the robot's center must be inside the robot's footprint and will inevitably collide with an obstacle. This allows the current preset trajectory point pose determination to end, the tree-structured point cloud map to be cleared, and the next preset trajectory point pose determination to begin, significantly shortening obstacle avoidance warning time. Otherwise, if the point cloud instance close to the robot's center is located between the nearest distance between the robot's footprint edge and the robot's footprint center, it does not necessarily collide with an obstacle. Therefore, it is only necessary to obtain the point cloud instance close to the robot's center from the searched point cloud; the step of determining whether the distance between the searched point cloud instance and the robot's center is less than or equal to the nearest distance between the robot's footprint edge and the robot's footprint center is unnecessary, thus shortening the determination time.

[0072] There are many methods for determining whether the searched point cloud is inside the robot's footprint; any method that can determine whether the searched point cloud is inside the robot's footprint is acceptable. See also... Figure 3 As shown, in some embodiments, the distance between the point cloud and each edge of the robot footprint is calculated to obtain the edge closest to the point cloud. The distance between the robot footprint center and the point cloud is also calculated, along with the distance between the robot footprint center and the closest edge of the point cloud. The two distances are compared. If the distance between the robot footprint center and the point cloud is less than or equal to the distance between the robot footprint center and the closest edge of the point cloud, it indicates that the point cloud is inside the robot footprint, meaning the robot will collide with an obstacle. Otherwise, it indicates that the point cloud is outside the robot footprint, and the robot will not collide with an obstacle. See also... Figure 2 As shown, in some embodiments, the pnPoly algorithm is used to determine whether the searched point cloud is inside the robot's footprint. This includes the following steps: A ray is drawn from the searched point cloud, and the number of intersections between this ray and the robot's footprint is counted. If the number of intersections is odd, the point cloud is inside the robot's footprint; otherwise, the point cloud is outside the robot's footprint. Using the pnPoly algorithm helps reduce the computational complexity of determining whether the point cloud is inside the robot's footprint, reduces computation time, and thus shortens the determination time and obstacle avoidance warning time.

[0073] This invention also provides a storage medium storing a computer program that, when executed by a processor, implements the steps of the aforementioned KD-tree-based dynamic obstacle avoidance method. The computer program involved in the embodiments can be stored in a computer device readable storage medium, including but not limited to disks, magnetic tapes, magnetic cards, floppy disks, flash memory, optical disks, optical cards, read-only memory (ROM), random access memory (RAM), erasable programmable ROM (EPROM), and electrically erasable programmable ROM (EEPROM), etc., as well as other biological, physical, or chemical structures capable of performing similar or equivalent functions to the storage media listed above, such as DNA, RNA, proteins, and other units with information storage capabilities. In specific embodiments, the storage medium can be one of the above-mentioned media types or a combination of the above-mentioned media types. In different embodiments, the computer program involved in the embodiments can be centrally stored in a single medium or distributed across multiple media. The memory containing the computer device readable storage medium can be non-volatile memory or random access memory. These computer device readable storage media can be built into the device or connected to the device of the embodiments as an external device or part of an external device. In some embodiments, the memory having the computer device readable storage media is deployed locally; in other embodiments, the memory can be deployed remotely from the processor, for example, as a network-attached memory accessed via RF circuitry or an external port and a communication network, wherein the communication network can be the Internet, one or more intranets, a local area network (LAN), a wide area network (WLAN), a storage area network (SAN), or a suitable combination thereof, as long as it enables computer device access to the memory. Furthermore, the computer programs involved in the embodiments can be stored in plaintext / ciphertext form or designed as training data, which can be integrated and recombined through model training and implicitly stored in the parameter states of deep neural networks or other machine learning models.The processor described in the embodiments of this application can be implemented by hardware, firmware, software, or a combination thereof. It can be a circuit, one or more of an application-specific integrated circuit (ASIC), a digital signal processor (DSP), a digital signal processing device (DSPD), a programmable logic device (PLD), a field-programmable gate array (FPGA), a central processing unit (CPU), a controller, a microcontroller, or a microprocessor. It also includes other physical, biological, or chemical structures that can implement the same or equivalent functions as the processors listed above, such as biological neurons, quantum computing units, DNA computing units, etc., so that the processor can execute some or all of the steps in the computer program or method involved in the various embodiments of this application, or any combination of the steps mentioned therein.

[0074] It should be noted that although the above embodiments have been described herein, this does not limit the scope of patent protection of the present invention. Therefore, any changes and modifications made to the embodiments described herein based on the innovative concept of the present invention, or equivalent structural or procedural transformations made using the content of the present invention's specification and drawings, directly or indirectly applying the above technical solutions to other related technical fields, are all included within the scope of patent protection of the present invention.

Claims

1. A method for dynamic obstacle avoidance based on KD-tree, characterized in that, Includes the following steps: The sensor point clouds obtained by the robot are processed and merged to generate a KD tree point cloud map. Transpose the preset robot footprint to the preset trajectory point pose in the KD tree point cloud map; The search point cloud is obtained by using radius search and / or k-nearest neighbor search in the KD tree point cloud map to search for cloud points near the robot footprint center; the search center of the radius search in the KD tree point cloud map is the robot footprint center, and the search radius is the farthest distance from the robot footprint center to the edge of the robot footprint. Determine whether the searched point cloud is inside the robot's footprint; If it is present, it means the robot has collided with an obstacle; Otherwise, the robot will not collide with obstacles; Before determining whether the searched point cloud is inside the robot's footprint after obtaining the searched point cloud, the following steps are also included: Count the number of point clouds retrieved; Check if the number of points in the searched point cloud is equal to 0. If it is, it means the robot will not collide with obstacles. When the number of points retrieved is not equal to 0, It also includes obtaining point clouds that are close to the robot's center from the searched point clouds, and determining whether the distance between the point cloud that is close to the robot's center and the robot's center is less than or equal to the closest distance between the edge of the robot's footprint and the center of the robot's footprint. If it is less than or equal to the closest distance between the edge of the robot's footprint and the center of the robot's footprint, it means that the robot has collided with an obstacle.

2. The method for dynamic obstacle avoidance based on KD-tree according to claim 1, characterized in that, The process of merging the sensor point clouds obtained by the robot to generate a KD tree point cloud map includes the following steps: Filter the multiple point clouds obtained by the robot from the sensors; The filtered point clouds are merged to form a point cloud set; Import the point cloud collection into a KD tree to generate a tree-structured point cloud map.

3. The method for dynamic obstacle avoidance based on KD-tree according to claim 1, characterized in that, When determining whether the searched point cloud is inside the robot's footprint, the pnPoly algorithm is used to determine whether the searched point cloud is inside the robot's footprint, including the following steps: From the point cloud obtained by the judgment search, obtain the point cloud of the shortest distance between the robot footprint edge and the robot footprint center to the shortest distance between the robot footprint edge and the robot footprint center; Draw a ray from the obtained point cloud and count the number of intersections between this ray and the robot's footprint. If the number of intersection points is odd, it means the point cloud is inside the robot's footprint; otherwise, it means the point cloud is outside the robot's footprint.

4. The method for dynamic obstacle avoidance based on KD-tree according to claim 1, characterized in that, Before determining whether the searched point cloud is inside the robot's footprint after obtaining the searched point cloud, the following steps are also included: Obtain the point cloud that is close to the robot's center from the searched point cloud. Determine whether the distance between the point cloud that is close to the robot's center and the robot's center is less than or equal to the nearest distance between the edge of the robot's footprint and the center of the robot's footprint. If it is less than or equal to the nearest distance between the edge of the robot's footprint and the center of the robot's footprint, it means that the robot has collided with an obstacle.

5. The method for dynamic obstacle avoidance based on KD-tree according to claim 1, characterized in that, When determining whether the searched point cloud is inside the robot's footprint, the pnPoly algorithm is used to determine whether the searched point cloud is inside the robot's footprint, including the following steps: The point cloud obtained from the search leads to a ray. The number of intersections between this ray and the robot's footprint is counted. If the number of intersection points is odd, it means the point cloud is inside the robot's footprint; otherwise, it means the point cloud is outside the robot's footprint.

6. A storage medium, characterized in that: The storage medium stores a computer program that, when executed by a processor, implements the steps of the KD-tree-based dynamic obstacle avoidance method as described in any one of claims 1 to 5.

7. An electronic device, characterized in that: It includes a storage medium and a processor, wherein the storage medium stores a computer program, and when the computer program is executed by the processor, it implements the steps of the KD-tree-based dynamic obstacle avoidance method as described in any one of claims 1 to 5.

Images