A trajectory planning method based on point cloud image fusion obstacle abstract graph

Abstract

The application discloses a kind of trajectory planning methods based on point cloud image fusion obstacle abstract graph, comprising: obtaining point cloud data and image data;Based on the point cloud data and image data, utilize point cloud image-graph network to generate the graph describing obstacle, wherein the point cloud image-graph network is the network that training is completed, and the loss of network in training process is calculated based on the prediction error of node position and the prediction error of node occupation state;Based on the graph describing obstacle, utilize trajectory library-based planning algorithm to carry out trajectory planning.This method uses the technical means of multi-sensor fusion, combines point cloud data and image data to obtain obstacle information, overcomes the technical problem of insufficient single-sensor data information, uses deep learning technology to predict unknown environment outside local perception area, can more comprehensively perceive environment, realizes efficient prediction of long-range obstacle.

Description

Technical Field

[0001] This invention belongs to the field of trajectory planning technology, and particularly relates to a trajectory planning method based on point cloud image fusion obstacle abstraction map. Background Technology

[0002] In recent years, with the rapid advancement of robotics technology, increasingly diverse application scenarios have placed more stringent demands on robots' navigation and obstacle avoidance capabilities. Based on differences in the degree of environmental information they can grasp, robot path planning technology can be divided into two main categories: local path planning and global path planning, specifically including:

[0003] (1) Local path planning: Local path planning is a method for making navigation decisions based on the surrounding environment information of the robot's current location. This method mainly focuses on the robot's action strategy in the short term, and generates a short-term, local path by analyzing the obstacles and environmental features around the robot in real time.

[0004] (2) Global path planning: Global path planning is a method for generating paths based on a deep understanding of the overall environment. Global path planning aims to generate an optimal or near-optimal path for the robot from the starting point to the destination, while considering avoiding obstacles and satisfying other constraints (such as shortest path, lowest energy consumption, etc.).

[0005] When unmanned systems enter unknown environments, if no pre-built map is available or the environment map is too large, the system can typically only rely on local path planning. However, most airborne sensors, such as LiDAR and depth cameras, can only provide surface information, often leading to insufficient field of view, limited field of view angle, and occlusion issues. This restricts the perception area and creates unknown regions. Insufficient understanding of these unknown environments can affect planning performance, leading to blind planning or even dead ends.

[0006] For path planning problems in unknown regions, current traditional techniques mainly employ methods such as... Figure 1 The diagram illustrates two strategies for handling unknown areas. One is a more conservative approach, treating the unknown area as impassable, allowing the planning technology to generate a safer but slower trajectory. The main drawback of this strategy is that it narrows the planning area, leading to a decrease in the unmanned system's movement speed. The other is a more aggressive approach, treating the unknown area as traversable, thus generating a faster but more dangerous trajectory. The main disadvantage of this strategy is that if the unknown area is actually impassable, the drone may suddenly stop or even collide, which is extremely dangerous. Moreover, neither strategy considers using information from the known area to infer the occupancy of the unknown area, making it difficult to achieve ideal results in complex environments.

[0007] In addition to the two traditional strategies for handling unknown regions, some techniques attempt to use deep learning to predict unknown regions. For example, ... Figure 2 As shown, Unet (a U-shaped encoder-decoder network structure) is used to predict a map expressed as a raster map. The input to Unet is a raster map consisting of known occupied areas, where some information about unknown areas is missing due to occlusion and other factors. Through the inference output of Unet, we can obtain a more complete raster map. This more complete map is then used for collision detection in planning techniques to help the planning techniques achieve better results.

[0008] Although this deep learning-based technique for predicting unknown regions has taken into account using information from known regions to infer the environment, it still has many shortcomings:

[0009] 1. In a given area, the portion actually containing obstacles often occupies only a small portion. Therefore, using this technique to represent maps with a dense grid may lead to a waste of computational resources. For larger-scale or more detailed prediction tasks, this technique may struggle to meet the real-time computational requirements of unmanned systems.

[0010] 2. The aforementioned technique operates only at the map level, producing a more complete map that is directly used for collision detection in the planning algorithm. However, the purpose of predicting unknown areas is to improve the performance of the planning algorithm. This technique is not designed for integration with specific planning algorithms to output results more suitable for their use.

[0011] 3. Typically, when unmanned systems perform tasks in an environment, they can obtain not only the distance to obstacles through sensors such as LiDAR, but also the texture information of obstacles through cameras. This texture information is also an important factor in predicting unknown areas. However, the aforementioned technologies do not take into account the use of this texture information. Summary of the Invention

[0012] To address the problems existing in the prior art, the purpose of this application is to provide a trajectory planning method based on point cloud image fusion obstacle abstract map.

[0013] According to a first aspect of the embodiments of this application, a trajectory planning method based on point cloud image fusion obstacle abstraction map is provided, including:

[0014] Acquire point cloud data and image data;

[0015] Based on the point cloud data and image data, a graph describing the obstacles is generated using a point cloud image-graph network, wherein the point cloud image-graph network is a trained network, and the network loss during training is calculated based on the prediction error of node position and the prediction error of the occupancy state between nodes.

[0016] Based on the graph describing the obstacles, trajectory planning is performed using a trajectory-based planning algorithm.

[0017] Furthermore, in the point cloud image-graph network:

[0018] Based on the input point cloud data and image data, the fully connected layer and the convolutional neural network layer extract point cloud features and image features, respectively. The point cloud data is input in the form of a matrix containing a list of three-dimensional coordinates, and the image data is input in the form of a three-channel matrix.

[0019] Point cloud features and image features are integrated, and a graph describing obstacles is output through a fully connected layer. The node information in the graph describing obstacles represents the two-dimensional node position of the obstacle in the topology, and the edge information represents whether there is an edge between the nodes, that is, whether the node is in an occupied state.

[0020] Furthermore, the graph structure generated by the point cloud image-graph network consists of a matrix containing a list of two-dimensional coordinates of graph nodes and a minimal encoded representation of an adjacency matrix containing graph edge information.

[0021] Furthermore, the loss function of the point cloud image-graph network is:

[0022] TotolLoss=αMSELoss+βBCELoss

[0023] in:

[0024] The prediction error for the node position is

[0025] The prediction error of the occupancy state between nodes is

[0026] y represents the true value of the node position. Here, represents the predicted node position, n is the number of samples in a batch, and z is the ground truth value of the topological edge. Let be the predicted value of the topological edge, and α and β be the weights of the prediction error of the node position and the prediction error of the occupancy state between nodes, respectively.

[0027] Furthermore, based on the graph describing the obstacles, trajectory planning is performed using a trajectory-based planning algorithm, including:

[0028] The location of the obstacle is obtained based on the diagram describing the obstacle;

[0029] Collision detection is performed on all tracks in the track library to exclude tracks that overlap with obstacles;

[0030] The optimal trajectory is selected from the remaining trajectories in the trajectory library based on predetermined indicators.

[0031] According to a second aspect of the embodiments of this application, a trajectory planning device based on point cloud image fusion obstacle abstract map is provided, comprising:

[0032] The acquisition module is used to acquire point cloud data and image data;

[0033] The generation module is used to generate a graph describing obstacles based on the point cloud data and image data using a point cloud image-graph network, wherein the point cloud image-graph network is a trained network, and the network loss during training is calculated based on the prediction error of node position and the prediction error of the occupancy state between nodes.

[0034] The trajectory planning module is used to perform trajectory planning based on the graph describing the obstacles and using a trajectory library-based planning algorithm.

[0035] According to a third aspect of the embodiments of this application, a computer program product is provided, including a computer program / instructions that, when executed by a processor, implement the method described in the first aspect.

[0036] According to a fourth aspect of the embodiments of this application, an electronic device is provided, comprising:

[0037] One or more processors;

[0038] Memory, used to store one or more programs;

[0039] When the one or more programs are executed by the one or more processors, the one or more processors perform the method as described in the first aspect.

[0040] According to a fifth aspect of the embodiments of this application, a computer-readable storage medium is provided that stores computer instructions thereon, which, when executed by a processor, implement the steps of the method as described in the first aspect.

[0041] The technical solutions provided by the embodiments of this application may include the following beneficial effects:

[0042] As can be seen from the above embodiments, this application adopts a multi-sensor fusion technique, combining point cloud data and image data to obtain obstacle information, overcoming the technical problem of insufficient data information from a single sensor. It uses deep learning technology to predict the unknown environment outside the local perception area, enabling more comprehensive perception of the environment and efficient prediction of remote obstacles. The obtained obstacle information is used to assist a trajectory planning algorithm based on a trajectory library to plan the trajectory, avoiding collisions with obstacles, while also having high real-time performance.

[0043] It should be understood that the above general description and the following detailed description are exemplary and explanatory only, and do not limit this application. Attached Figure Description

[0044] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with this application and, together with the description, serve to explain the principles of this application.

[0045] Figure 1 This is a schematic diagram of a traditional strategy for path planning in unknown areas in the background technology.

[0046] Figure 2 This is a schematic diagram of the location region predicted by deep learning in the background technology.

[0047] Figure 3 This is a flowchart illustrating a trajectory planning method based on a point cloud image fusion obstacle abstract map, according to an exemplary embodiment.

[0048] Figure 4 This is a point cloud image-graph network schematic diagram illustrated according to an exemplary embodiment.

[0049] Figure 5 This is a schematic diagram of prediction graph-assisted planning according to an exemplary embodiment.

[0050] Figure 6 This is a prediction graph-assisted planning effect diagram illustrated according to an exemplary embodiment.

[0051] Figure 7 This is a block diagram illustrating a trajectory planning device based on a point cloud image fusion obstacle abstract map, according to an exemplary embodiment.

[0052] Figure 8 This is a schematic diagram of an electronic device according to an exemplary embodiment. Detailed Implementation

[0053] Exemplary embodiments will now be described in detail, examples of which are illustrated in the accompanying drawings. When the following description relates to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with this application.

[0054] The terminology used in this application is for the purpose of describing particular embodiments only and is not intended to be limiting of the application. The singular forms “a,” “the,” and “the” used in this application and the appended claims are also intended to include the plural forms unless the context clearly indicates otherwise. It should also be understood that the term “and / or” as used herein refers to and includes any or all possible combinations of one or more of the associated listed items.

[0055] Figure 3 This is a flowchart illustrating a trajectory planning method based on point cloud image fusion and remote obstacle abstraction map, according to an exemplary embodiment. Figure 3 As shown, the method may include the following steps:

[0056] Step S11: Acquire point cloud data and image data;

[0057] Specifically, local point cloud data with the robot's center as the origin of the coordinate system is obtained from the depth sensor, and first-person view images of the drone are obtained from the image sensor.

[0058] Step S22: Based on the point cloud data and image data, generate a graph describing the obstacles using a point cloud image-graph network, wherein the point cloud image-graph network is a trained network, and the network loss during training is calculated based on the prediction error of node position and the prediction error of the occupancy state between nodes.

[0059] Specifically, this application proposes an innovative neural network model that can effectively process input point cloud and image data and generate a graph describing information about remote obstacles. The neural network is designed primarily to assist in implementing a trajectory library planning algorithm, which has wide applications in autonomous driving, robot navigation, and mobile robots.

[0060] Specifically, such as Figure 4As shown, point cloud data is input as a matrix containing a list of 3D coordinates, while image data is input as a three-channel matrix. The generated graph structure is represented by a matrix containing a list of 2D coordinates of graph nodes and a minimal encoding of the adjacency matrix containing graph edge information (since the graph used is undirected, only half-region information of the adjacency matrix is ​​needed). During the network's operation, features are first extracted from the point cloud and image through fully connected layers and convolutional neural network layers, respectively. These two types of features are then integrated, and finally, another fully connected layer outputs two parts of the graph's information. The resulting graph represents the complete location information of obstacles within the range of the collected point cloud data.

[0061] This method combines point cloud and image input methods. First, compared to using traditional raster methods, the sparse point cloud used in this application can effectively provide occupied map information, greatly reducing the unnecessary consumption of computational space. Images, as two-dimensional information, have a relatively small impact on computational power. Second, this invention fully considers the respective advantages and disadvantages of point clouds and images in practical application scenarios. For example, a RealSense camera can acquire point cloud and image data. Point clouds can provide distance information of obstacles, but may be distorted at long distances; conversely, images can provide texture information of obstacles and good perception of long distances, but cannot provide distance information. By fusing the features of point clouds and images, this invention can comprehensively capture distance and texture information, optimizing the prediction accuracy for unknown areas. Furthermore, this application outputs map information in the form of a two-dimensional abstract map of remote obstacles. This method is not only lightweight and efficient but also meets the obstacle avoidance requirements of the planning algorithm. The specific implementation of the auxiliary planning algorithm will be described in detail in subsequent sections.

[0062] The training set data is derived from data collection in simulated scenarios. Since existing public datasets lack specific scenarios involving dead ends, we constructed various simulated scenarios, randomly placing the simulated robot in various locations on the map. We then used simulated sensors to generate sensor data, pre-labeling key obstacle nodes as tags. By limiting the range of the sensor point clouds and sampling them to a fixed size, converting the images to a uniform format, and removing useless data, we created the complete dataset.

[0063] It should be noted that the limitation range of the point cloud data in the training set is determined based on the acquisition range of the sensors actually used by the robot. In one embodiment, the sensor used is a RealSense D435i, which has a limited range of 5m. Therefore, the point cloud data in the training set should also be limited to 5m.

[0064] In the graph generated by the point cloud image-graph network, node and edge information represent the two-dimensional node positions of obstacles within the topological structure and the existence of edges between nodes, i.e., whether they are in an occupied state. Since the prediction tasks for these two parts correspond to regression and classification problems respectively, two different types of loss functions are needed to evaluate their prediction accuracy. In one embodiment, MSELoss (Mean Square Error Loss) is used to evaluate the prediction error of node positions, and BCELoss (Binary Cross Entropy Loss) is used to evaluate the prediction error of node occupancy states. Finally, by weighted summing of these two losses, the final total loss function is obtained:

[0065] TotolLoss=αMSELoss+βBCELoss

[0066] in:

[0067]

[0068] y represents the true value of the node position. is the predicted value for the node position, and n is the number of samples in a batch.

[0069]

[0070] z is the truth value of the topological edge. is the predicted value of the topological edge, and n is the number of samples in a batch.

[0071] α and β are the weights of the prediction error of node position and the prediction error of the occupancy state between nodes, respectively.

[0072] S13: Based on the graph describing the obstacles, perform trajectory planning using a trajectory library-based planning algorithm;

[0073] Specifically, the obstacle-description graph described above is an auxiliary prediction technique for trajectory-based planning algorithms (such as "Howard TM, Kelly A. Optimal rough terrain trajectory generation for wheeled mobile robots[J]. The International Journal of Robotics Research, 2007, 26(2):141-166." and "Howard TM, Green CJ, Kelly A, et al. State space sampling of feasible motions for high-performance mobile robot navigation in complex environments[J]. Journal of Field Robotics, 2008, 25(6-7):325-345."). This planning algorithm is widely used in fields such as autonomous driving, robot navigation, and mobile robots. The core concept of this method is to pre-generate and store a trajectory library containing various possible trajectories. In the planning stage, trajectories that overlap with obstacles and cannot pass collision detection are first excluded from the trajectory library. Then, the optimal trajectory is selected according to predetermined indicators, which are set according to actual needs, such as the shortest trajectory, the fewest turns, etc. The advantage of this strategy lies in its ability to pre-generate a trajectory library using substantial computing resources. Then, during real-time operations, it enables rapid path planning by querying and selecting pre-calculated trajectories. This makes trajectory-based planning algorithms particularly suitable for applications with high real-time requirements.

[0074] In this application, the point cloud image-graph network only generates a two-dimensional abstract map of remote obstacles, and then performs equidistant sampling and collision detection calculations on the extended trajectories of trajectories in the trajectory library. Figure 5 As shown, Figure 5 (a) in the image illustrates the situation that common local planning algorithms encounter when faced with a dead end. Because the sensor's perception range is limited, it can only acquire the point cloud at the entrance of the dead end, while the route into the dead end is considered passable, and the robot will enter the dead end and get trapped. Figure 5(b) illustrates the situation where the local planning algorithm using this method encounters a dead end. This method can predict the topology of the dead end and, by extending the original trajectory, allows the route entering the dead end to collide with the predicted topology, thus eliminating routes into the dead end and achieving dead end avoidance. Even in the absence of map details, it can effectively help the planning algorithm avoid distant obstacles. For more refined planning, collision detection can be performed using point clouds directly acquired by sensors and the original trajectory library of the planning algorithm, thereby achieving accurate planning at close range. Therefore, this application helps planning technology avoid obstacles in unknown areas while also ensuring high real-time performance.

[0075] Existing planning technologies often fail to achieve satisfactory results in unfamiliar environments due to factors such as occlusion or insufficient sensor field of view, leading to inadequate obstacle observation. For example, when facing a dead end, an unmanned system may not be able to see the wall at the end of the alley from outside, potentially entering the dead end directly, resulting in wasted resources or becoming trapped and unable to perform its task. This invention can predict obstacles outside the perception area in advance in unfamiliar and perception-limited environments, enabling proactive obstacle avoidance. Furthermore, this invention incorporates specific planning techniques, not only improving planning effectiveness but also meeting real-time requirements. Figure 6 As shown, Figure 6 (a) illustrates a scenario where a drone flies into a dead end and struggles to get out when this method is not used for planning; Figure 6 (b) in the diagram demonstrates the effectiveness of using this method for planning. The drone can predict the topology of dead ends in advance and avoid them.

[0076] Corresponding to the aforementioned embodiments of the trajectory planning method based on point cloud image fusion obstacle abstract map, this application also provides embodiments of the trajectory planning device based on point cloud image fusion obstacle abstract map.

[0077] Figure 7 This is a block diagram of a trajectory planning device based on a point cloud image fusion obstacle abstract map, according to an exemplary embodiment. (Refer to...) Figure 7 The device may include:

[0078] Module 21 is used to acquire point cloud data and image data;

[0079] The generation module 22 is used to generate a graph describing obstacles based on the point cloud data and image data using a point cloud image-graph network, wherein the point cloud image-graph network is a trained network, and the network loss during training is calculated based on the prediction error of node position and the prediction error of the occupancy state between nodes.

[0080] The trajectory planning module 23 is used to perform trajectory planning based on the graph describing the obstacles and using a trajectory library-based planning algorithm.

[0081] Regarding the apparatus in the above embodiments, the specific manner in which each module performs its operation has been described in detail in the embodiments related to the method, and will not be elaborated upon here.

[0082] For the device embodiments, since they basically correspond to the method embodiments, the relevant parts can be referred to in the description of the method embodiments. The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this application according to actual needs. Those skilled in the art can understand and implement this without creative effort.

[0083] A corresponding computer program product includes a computer program / instruction that, when executed by a processor, implements the trajectory planning method based on an obstacle abstraction map fused from point cloud images, as described above.

[0084] Accordingly, this application also provides an electronic device, including: one or more processors; a memory for storing one or more programs; when the one or more programs are executed by the one or more processors, the one or more processors implement the trajectory planning method based on point cloud image fusion obstacle abstraction map as described above. Figure 8 The diagram shown is a hardware structure diagram of any device with data processing capabilities, which is a trajectory planning device based on point cloud image fusion obstacle abstract map provided in an embodiment of the present invention. (Except for...) Figure 8 In addition to the processor, memory, and network interface shown, any data processing device in the embodiment may also include other hardware depending on the actual function of the data processing device, which will not be described in detail here.

[0085] Accordingly, this application also provides a computer-readable storage medium storing computer instructions, which, when executed by a processor, implement the trajectory planning method based on point cloud image fusion obstacle abstraction map as described above. The computer-readable storage medium can be an internal storage unit of any data processing device as described in any of the foregoing embodiments, such as a hard disk or memory. The computer-readable storage medium can also be an external storage device, such as a plug-in hard disk, smart media card (SMC), SD card, flash card, etc., equipped on the device. Furthermore, the computer-readable storage medium can include both internal storage units of any data processing device and external storage devices. The computer-readable storage medium is used to store the computer program and other programs and data required by the data processing device, and can also be used to temporarily store data that has been output or will be output.

[0086] Other embodiments of this application will readily occur to those skilled in the art upon consideration of the specification and practice of the disclosure herein. This application is intended to cover any variations, uses, or adaptations of this application that follow the general principles of this application and include common knowledge or customary techniques in the art not disclosed herein.

[0087] It should be understood that this application is not limited to the precise structure described above and shown in the accompanying drawings, and various modifications and changes can be made without departing from its scope.

Claims

1. A trajectory planning method based on point cloud image fusion obstacle abstraction map, characterized in that, include: Acquire point cloud data and image data; Based on the point cloud data and image data, a graph describing obstacles is generated using a point cloud image-graph network. In the graph describing obstacles, the node information represents the two-dimensional node position of the obstacle in the topology, and the edge information represents whether there is an edge between the nodes, i.e. whether the node is in an occupied state. The point cloud image-graph network is a trained network, and the network loss during the training process is calculated based on the prediction error of the node position and the prediction error of the occupied state between nodes. Based on the graph describing the obstacles, trajectory planning is performed using a trajectory database-based planning algorithm; In the point cloud image-graph network: Based on the input point cloud data and image data, the fully connected layer and the convolutional neural network layer extract point cloud features and image features, respectively. The point cloud data is input in the form of a matrix containing a list of three-dimensional coordinates, and the image data is input in the form of a three-channel matrix. By integrating point cloud features and image features, a graph describing obstacles is output through a fully connected layer. The graph structure generated by the point cloud image-graph network consists of a matrix containing a list of two-dimensional coordinates of graph nodes and a minimal encoding representation of an adjacency matrix containing graph edge information.

2. The method according to claim 1, characterized in that, The loss function of the point cloud image-graph network is: ， in: The prediction error for the node position is , The prediction error of the occupancy state between nodes is , y represents the true value of the node position. Here, represents the predicted node position, n is the number of samples in a batch, and z is the ground truth value of the topological edge. These are the predicted values ​​for the topological edges. and These are the weights for the prediction error of node position and the prediction error of the occupancy state between nodes, respectively.

3. The method according to claim 1, characterized in that, Based on the obstacle description graph, trajectory planning is performed using a trajectory database-based planning algorithm, including: The location of the obstacle is obtained based on the diagram describing the obstacle; Collision detection is performed on all tracks in the track library to exclude tracks that overlap with obstacles; The optimal trajectory is selected from the remaining trajectories in the trajectory library based on predetermined indicators.

4. A trajectory planning device based on point cloud image fusion obstacle abstract map, characterized in that, include: The acquisition module is used to acquire point cloud data and image data; The generation module is used to generate a graph describing obstacles based on the point cloud data and image data using a point cloud image-graph network. The node information in the graph describing obstacles represents the two-dimensional node position of the obstacle in the topology, and the edge information represents whether there is an edge between the nodes, i.e. whether the node is in an occupied state. The point cloud image-graph network is a trained network, and the network loss during the training process is calculated based on the prediction error of the node position and the prediction error of the occupied state between nodes. The trajectory planning module is used to perform trajectory planning based on the map describing the obstacles and using a trajectory library-based planning algorithm. In the point cloud image-graph network: Based on the input point cloud data and image data, the fully connected layer and the convolutional neural network layer extract point cloud features and image features, respectively. The point cloud data is input in the form of a matrix containing a list of three-dimensional coordinates, and the image data is input in the form of a three-channel matrix. By integrating point cloud features and image features, a graph describing obstacles is output through a fully connected layer. The graph structure generated by the point cloud image-graph network consists of a matrix containing a list of two-dimensional coordinates of graph nodes and a minimal encoding representation of an adjacency matrix containing graph edge information.

5. A computer program product, comprising a computer program / instructions, characterized in that, When the computer program / instructions are executed by the processor, they implement the method as described in any one of claims 1-3.

6. An electronic device, characterized in that, include: One or more processors; Memory, used to store one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the method as described in any one of claims 1-3.

7. A computer-readable storage medium storing computer instructions thereon, characterized in that, When executed by the processor, this instruction implements the steps of the method as described in any one of claims 1-3.

Images