Robot collision avoidance motion path planning method and related devices
By constructing a grid map occupancy map and generating a diffusion strategy to create a robot navigation path, and combining potential distance features and lateral gradient constraints, the collision constraint problem in real-time robot path planning is solved, achieving safe and reliable obstacle avoidance capabilities.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- WUHAN HUANYU ZHIXING TECH CO LTD
- Filing Date
- 2026-04-15
- Publication Date
- 2026-05-29
AI Technical Summary
Existing technologies cannot meet the real-time path planning requirements of robots, especially in complex dynamic environments where it is difficult to strictly guarantee hard collision constraints and dynamic feasibility.
By constructing a grid map occupancy map, combining it with a diffusion strategy to generate an initial navigation path, and calculating potential distance features based on the robot's own footprint, lateral gradient constraints are added to avoid obstacles, thus generating a navigation path that follows vehicle kinematics and dynamics constraints.
It enables robots to avoid obstacles in complex environments in real time, enhances the safety and robustness of motion planning, and generates feasible trajectories that follow physical constraints.
Smart Images
Figure CN122111022A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of robot motion control technology, and in particular to a method and device for planning a robot collision avoidance motion path. Background Technology
[0002] With the continuous development of robotics technology, robots are being used in more and more places. How to ensure that robots do not collide during autonomous operation is an important research topic for the autonomous operation control of robots.
[0003] Traditional robot motion planning typically faces two core challenges: real-time performance and collision constraint satisfaction. While Model Predictive Control (MPC) methods can strictly satisfy collision and dynamic constraints, they suffer from high computational complexity and generate only single-modal trajectories, making them ill-suited for handling uncertainties in complex dynamic environments and prone to getting trapped in local optima. End-to-end planning methods based on diffusion models, while generating multimodal trajectories and exhibiting good generalization ability, struggle to strictly guarantee hard collision constraints because they mimic the distribution of learning samples, and the generated trajectories may suffer from dynamic infeasibility. Furthermore, diffusion models typically have slow inference speeds, making them unsuitable for real-time robot planning.
[0004] This shows that existing technologies cannot meet the real-time path planning needs of robots. Summary of the Invention
[0005] In view of this, it is necessary to provide a robot collision avoidance motion path planning method and related device to solve the problem that the existing technology cannot meet the real-time path planning requirements of robots.
[0006] To address the aforementioned problems, in a first aspect, the present invention provides a robot collision avoidance motion path planning method, comprising: A grid map occupancy map is constructed based on the grid map occupancy information within the robot's preset range. An initial navigation path is generated by combining the grid map occupancy map with the preset navigation trajectory using a diffusion strategy. Based on the robot's own footprint, the potential distance characteristics between the robot and each obstacle are calculated when the robot moves along the initial navigation path. The potential distance characteristics are used to characterize the collision risk between the robot and the obstacle. When the potential distance feature indicates that the collision risk between the robot and the obstacle is greater than the preset risk threshold, a lateral gradient constraint is added to the diffusion strategy based on the potential distance feature to obtain a navigation path that can avoid the obstacle.
[0007] In one possible implementation, a grid map occupancy map is constructed based on grid map occupancy information within a preset range of the robot, including: Collect terrain point cloud data within a preset range of the robot, and construct a raster map based on the terrain point cloud data; The grid cells in the grid map whose point cloud count exceeds a preset point cloud count threshold are identified as occupied grid cells. The maximum height of the terrain point cloud in each occupied grid cell is used as the occupancy value of the occupied grid cell to construct a grid map occupancy map.
[0008] In one possible implementation, an initial navigation path is generated using a diffusion strategy, combining a grid map occupancy map and a preset navigation trajectory, including: Extract the spatial occupancy features of the raster map occupancy map and the navigation features of the preset navigation trajectory, and fuse the spatial features and navigation features to obtain a noisy trajectory; The hidden features of the noisy trajectory are extracted. The hidden features, space occupancy features, navigation features, and preset diffusion time step conditions are used as inputs to the set diffusion denoising model to perform diffusion denoising on the noisy trajectory and obtain the initial navigation path.
[0009] In one possible implementation, the potential distance characteristics between the robot and each obstacle as it moves along the initial navigation path are calculated based on the robot's own footprint, including: The robot's movement area is determined based on its location and its own footprint. Construct the minimum distance constraint inequality from each obstacle to the boundary of the area occupied by the robot when it moves along the initial trajectory; By using a pre-defined neural network to solve the minimum distance constraint inequality, the potential distance characteristics between the robot and each obstacle when moving along the initial navigation path are obtained.
[0010] In one possible implementation, the minimum distance constraint inequality is:
[0011]
[0012] in, Obstacles The minimum distance between the robot's movement area and the area it occupies, where G is the robot's rotation angle. The translation distance of the robot. Indicates obstacles Position in the robot coordinate system and Let the rotation and translation matrices be the robot's own position. Location of each obstacle The potential distance characteristics relative to the robot's own footprint. Indicates the correlation of collisions. Represents distance features, and This represents the generalized form of inequality constraints.
[0013] In one possible implementation, when the potential distance feature indicates a collision risk between the robot and an obstacle greater than a preset risk threshold, a lateral gradient constraint is added to the diffusion strategy based on the potential distance feature to obtain a navigation path that can avoid the obstacle, including: Based on the potential distance features, the collision-related distance values between the robot and obstacles are calculated. When the collision-related distance values are within the preset distance range, an exponential collision penalty function is constructed. The gradient of the exponential collision penalty function is solved to obtain the lateral gradient constraint. The lateral gradient constraint is added to the diffusion strategy, and the gradient guidance of the collision classifier is used to generate a navigation path that can avoid obstacles.
[0014] The formula for the collision classifier is:
[0015] in, The diffusion model uses conditional scores based on score matching to learn the trajectory sample distribution. The diffusion model is based on the unconditional scores of the learning trajectory sample distribution, which are obtained by matching scores. is the classifier gradient guided by collision constraints, and w is a hyperparameter controlling the size of the guidance.
[0016] Secondly, the present invention also provides a robot collision avoidance motion path planning device, comprising: The initial navigation path generation module is used to construct a grid map occupancy map based on the grid map occupancy information within the robot's preset range, and generate an initial navigation path by combining the grid map occupancy map and the preset navigation trajectory using a diffusion strategy. The collision risk calculation module is used to calculate the potential distance characteristics between the robot and each obstacle when the robot moves along the initial navigation path based on the robot's own footprint. The potential distance characteristics are used to characterize the collision risk between the robot and the obstacle. The navigation path correction module is used to add lateral gradient constraints to the diffusion strategy based on the potential distance features when the collision risk between the robot and the obstacle is greater than a preset risk threshold, so as to obtain a navigation path that can avoid the obstacle.
[0017] Thirdly, the present invention also provides a robot, characterized in that it includes a memory, a processor, and a motion module, wherein... Memory, used to store programs; The processor, coupled to the memory, is used to execute the program stored in the memory to implement the steps in the robot collision avoidance motion path planning method of any of the above implementations; The motion module is used to move along a trajectory under the control of the processor.
[0018] Fourthly, the present invention also provides a computer-readable storage medium for storing a computer-readable program or instruction, which, when executed by a processor, can implement the steps in the robot collision avoidance motion path planning method described above.
[0019] The beneficial effects of this invention are as follows: The robot collision avoidance motion path planning method provided by this invention constructs a grid map occupancy map based on grid map occupancy information within a preset range of the robot, and generates an initial navigation path using a diffusion strategy by combining the grid map occupancy map and a preset navigation trajectory. By generating the initial navigation path by combining grid map occupancy information and a preset navigation trajectory, the method directly utilizes raw point cloud data to construct a grid map and maps it to the robot's trajectory generation, eliminating error-prone intermediate representations and complex coding modules, thus avoiding error propagation from the source. Based on the robot's own footprint, the method calculates the potential distance characteristics between the robot and each obstacle when moving along the initial navigation path. These potential distance characteristics are used to characterize the robot's position. Collision risk between humans and obstacles: Based on the robot's own footprint, the potential distance characteristics between the robot and each obstacle are determined when the robot moves along the initial navigation path. When the potential distance characteristics indicate that the collision risk between the robot and the obstacle is greater than a preset risk threshold, a lateral gradient constraint is added to the diffusion strategy based on the potential distance characteristics to obtain a navigation path that can avoid obstacles. A physically feasible trajectory that follows vehicle kinematics and dynamics constraints is generated. When there is a potential collision risk between the predicted trajectory and the obstacle, the system automatically generates and adds a virtual repulsive field gradient to guide the sampling process to actively deviate from the danger zone. This achieves an active obstacle avoidance capability similar to real-time force feedback, which greatly enhances the safety of robot motion planning. Attached Figure Description
[0020] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.
[0021] Figure 1 A flowchart illustrating a robot collision avoidance motion path planning method provided in an embodiment of the present invention; Figure 2 This is a flowchart illustrating a method for constructing a raster map occupancy map according to an embodiment of the present invention. Figure 3 A flowchart illustrating an initial navigation path generation method provided in an embodiment of the present invention; Figure 4 A flowchart illustrating a method for determining potential distance features provided in an embodiment of the present invention; Figure 5 A schematic diagram of robot movement provided in an embodiment of the present invention; Figure 6 A flowchart illustrating a navigation path correction method provided in an embodiment of the present invention; Figure 7 This is a schematic diagram of the structure of a robot collision avoidance motion path planning device provided in an embodiment of the present invention; Figure 8 This is a schematic diagram of the structure of a robot provided in an embodiment of the present invention. Detailed Implementation
[0022] Preferred embodiments of the present invention will now be described in detail with reference to the accompanying drawings, which form part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, but are not intended to limit the scope of the present invention.
[0023] In the description of the embodiments of the present invention, unless otherwise stated, "multiple" means two or more. "And / or" describes the relationship between related objects, indicating that there can be three relationships. For example, A and / or B can represent three situations: A exists alone, A and B exist simultaneously, and B exists alone.
[0024] The terms "first," "second," etc., used in the embodiments of this invention are for descriptive purposes only and should not be construed as indicating or implying their relative importance or implicitly specifying the number of technical features indicated. Therefore, a technical feature defined with "first" or "second" may explicitly or implicitly include at least one of that feature.
[0025] In this document, the term "embodiment" means that a particular feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of the invention. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a mutually exclusive, independent, or alternative embodiment. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described herein can be combined with other embodiments.
[0026] A specific embodiment of the present invention, such as Figure 1 As shown, a method for planning a robot's collision avoidance motion path is disclosed, including: S101, construct a grid map occupancy map based on the grid map occupancy information within the robot's preset range, and generate an initial navigation path by combining the grid map occupancy map and the preset navigation trajectory using a diffusion strategy.
[0027] In this embodiment of the invention, it is first necessary to obtain grid map occupancy information within a preset range of the robot. The preset range can be set according to the robot's sensor detection distance, movement speed, and safe braking distance, for example, a circular area with a radius of five to twenty meters centered on the robot's current position, or a rectangular area based on the actual working scene. A grid map is a map representation method that discretizes a continuous environment into cells of the same size. Each grid cell is assigned an occupancy value, which indicates whether there is an obstacle at that grid cell location. The occupancy value can be obtained by: real-time scanning by the robot's lidar, combined with simultaneous localization and mapping algorithms to construct a two-dimensional or three-dimensional occupancy grid map; or by extracting data of the corresponding range from a pre-stored global map.
[0028] Furthermore, after obtaining the grid map occupancy information, a grid map occupancy map is constructed. This occupancy map is essentially a two-dimensional matrix, where each element corresponds to a grid cell in the environment. The value of the element indicates whether the grid cell is occupied and the maximum height of the obstacle occupying that grid cell.
[0029] Furthermore, the preset navigation trajectory can be a coarse path pre-calculated by the global path planner based on the global map, a sequence of critical path points manually specified by the operator, or an experience trajectory recorded during the robot's previous operation. The preset navigation trajectory provides macroscopic directional guidance from the current position to the target position, but it may not be smooth enough or fully consider the details of local obstacles.
[0030] Furthermore, the path is treated as a sequence of path points using a diffusion strategy, with each path point corresponding to a two-dimensional coordinate. Initially, the path points on the preset navigation trajectory can be used as the initial path point set. Then, through multiple iterations, each path point diffuses to its neighboring path points and surrounding free grid areas, making the overall path gradually smoother and tending to avoid highly occupied areas in the occupancy map, thus obtaining the initial navigation trajectory.
[0031] S102, calculate the potential distance characteristics between the robot and each obstacle when the robot moves along the initial navigation path based on the robot's own footprint. The potential distance characteristics are used to characterize the collision risk between the robot and the obstacle.
[0032] In this embodiment of the invention, the robot's self-occupied area refers to the region covered by the robot's orthographic projection on a horizontal plane. For common wheeled robots, this region can be rectangular, circular, or a convex polygon. This self-occupied area can be obtained directly from the robot's design drawings or by obtaining the coordinates of a set of contour points through offline measurement. To calculate the potential distance characteristics between the robot and each obstacle as it moves along the initial navigation path, multiple discrete robot poses need to be predicted along the path. Each pose includes the robot's position coordinates and orientation angle. The orientation angle can be determined by using the direction of the line connecting two adjacent path points as the robot's forward direction, or by smoothing the kinematic constraints of the robot to obtain the expected orientation of each sampled point. For each sampled pose, based on the current self-occupied area of the robot, the shortest distance between each point on the robot's contour and surrounding obstacles is calculated. The obstacle information here comes from the grid map occupancy map in the aforementioned embodiment, i.e., all grids marked as occupied. When calculating the shortest distance, a classic geometric distance algorithm can be used. For example, for a rectangular body, calculate the distance from the four sides of the rectangle to the nearest occupied grid cell; for a polygonal body, calculate the nearest distance from each vertex of the polygon to the occupied grid cell, as well as the perpendicular distance from each side to the occupied grid cell, and then take the minimum value among all distances.
[0033] S103, when the potential distance feature indicates that the collision risk between the robot and the obstacle is greater than the preset risk threshold, a lateral gradient constraint is added to the diffusion strategy based on the potential distance feature to obtain a navigation path that can avoid the obstacle.
[0034] In this embodiment of the invention, it is first necessary to determine whether the collision risk represented by the potential distance features is greater than a preset risk threshold. Since the potential distance features are multi-dimensional, the preset risk threshold can also be a multi-dimensional threshold vector, or a fusion function can be used to map the multi-dimensional features into a scalar risk value. For example, a rule-based judgment logic can be set: if the global minimum distance is less than the safe distance threshold, or the minimum distance in any direction is less than the directional safety threshold, or the distance change rate is negative and the absolute value is greater than the rapid approach threshold, or the distance to any key point of the vehicle body is less than the key point safety threshold, then the collision risk is determined to exceed the limit. When the collision risk is determined to be greater than the preset risk threshold, this invention proposes to add a lateral gradient constraint to the original diffusion strategy. The so-called lateral gradient constraint refers to a correction force or correction term that deviates the guide path perpendicular to the robot's forward direction. Unlike the traditional diffusion strategy, which only relies on the global repulsive force of the occupancy map, the lateral gradient constraint is specifically designed for the direction of the currently detected collision risk. Based on the directional minimum distance calculated in the aforementioned embodiment, the direction with the greatest risk is determined. For example, if the minimum distance in the left region is significantly less than the minimum distance in the right region, and there is an obstacle on the left, then the collision risk mainly comes from the left. Therefore, lateral gradient constraints should guide the path to shift to the right to increase the safe distance on the left. Conversely, if the risk comes from the right, the path should shift to the left. If the risk comes from the front and the rate of change of distance is negative, lateral gradient constraints can guide the path to slow down appropriately or simultaneously produce a lateral shift.
[0035] The robot collision avoidance motion path planning method provided by this invention constructs a grid map occupancy map based on grid map occupancy information within a preset range of the robot. It then generates an initial navigation path using a diffusion strategy, combining the grid map occupancy map with a preset navigation trajectory. By generating the initial navigation path by combining grid map occupancy information and the preset navigation trajectory, it directly utilizes raw point cloud data to construct a grid map, which is then mapped to the robot's trajectory generation. This eliminates error-prone intermediate representations and complex coding modules, preventing error propagation from the source. Based on the robot's own footprint, it calculates the potential distance features between the robot and each obstacle as it moves along the initial navigation path. These potential distance features characterize the robot's interaction with obstacles. The system detects collision risks by determining the potential distance characteristics between the robot and obstacles along the initial navigation path based on the robot's own footprint. When the potential distance characteristics indicate a collision risk between the robot and obstacles greater than a preset risk threshold, a lateral gradient constraint is added to the diffusion strategy based on the potential distance characteristics to obtain a navigation path that can avoid obstacles. This generates a physically feasible trajectory that follows vehicle kinematics and dynamics constraints. When there is a potential collision risk between the predicted trajectory and obstacles, the system automatically generates and adds a virtual repulsive field gradient to guide the sampling process to actively deviate from the danger zone. This achieves active obstacle avoidance capability similar to real-time force feedback, greatly enhancing the safety of robot motion planning.
[0036] In some possible embodiments of the present invention, such as Figure 2 As shown, a grid map occupancy map is constructed based on the grid map occupancy information within the robot's preset range, including: S201: Collect terrain point cloud data within a preset range of the robot and construct a raster map based on the terrain point cloud data; S202, the grid cells in the grid map whose point cloud count is greater than the preset point cloud count threshold are identified as occupied grid cells, and the maximum height of the terrain point cloud in each occupied grid cell is used as the occupancy value of the occupied grid cell to construct a grid map occupancy map.
[0037] In this embodiment of the invention, a 3D point cloud within a certain range is acquired using radar mounted on a robot. The input point cloud within a range of -5 meters to 5 meters is converted into a 200x200 grid image with the shape [B, C, H, W], where B is the batch size, C represents the number of channels in the grid image (if a 2D grid is used, the number of channels is 1), and H and W represent the grid size of 200x200. First, the position coordinates of each point in the grid are calculated. For example, taking the point cloud center (0, 0) as the origin, the coordinates of the top-left corner of the grid are (5, -5) meters. The position of each point in the grid is obtained by subtracting the coordinates of the top-left corner from each point and dividing by the grid size of 0.05. Then, the distribution of each point in the grid is determined by whether it exceeds the grid boundary. If the number of points in the grid exceeds a certain threshold, such as 20, the grid is considered occupied; otherwise, it is considered noise. Finally, the height value of the point with the highest height in each grid is used as the occupancy value for that grid, and the value of an unoccupied grid is 0. Through this transformation, the original input point cloud is converted into a 1x200x200 raster map.
[0038] In some possible embodiments of the present invention, such as Figure 3 As shown, an initial navigation path is generated using a diffusion strategy, combining the grid map occupancy map and the preset navigation trajectory, including: S301, extract the spatial occupancy features of the raster map occupancy map and the navigation features of the preset navigation trajectory, and fuse the spatial features and navigation features to obtain a noisy trajectory; S302, extract the hidden features of the noisy trajectory, and use the hidden features, space occupancy features, navigation features and preset diffusion time step conditions as input to the set diffusion denoising model to perform diffusion denoising on the noisy trajectory to obtain the initial navigation path.
[0039] In this embodiment of the invention, when generating the initial navigation path, a small neural network is first used to encode the raster map obtained in the aforementioned embodiment, outputting a 192-dimensional feature map, namely the encoded conditional feature Q. fThen, an attention mechanism is used to fuse conditional features with trajectory points to enhance scene perception. Additional navigation information is also injected for fusion; this navigation information is represented as trajectory points on the critical route, and navigation features Q are extracted using a multilayer perceptron (MLP) network. n and the diffusion time step condition Q t In combination, the adaptive layer normalization module guides the trajectory generation process, ensuring that trajectory planning and navigation are aligned. Figure 1 To.
[0040] Furthermore, a diffusion denoising strategy is employed to generate the initial navigation trajectory, using a Dit (diffusion transformer) network architecture throughout the process. First, the noisy trajectory is preprocessed and mapped to hidden layer features. Each state considers only the sine and cosine of the trajectory point's coordinates and orientation angle, e.g., considering the trajectory point distribution over the next 8 steps. Then, a multi-layered transformer-based DitBlock is used to progressively learn and remove noise. The input to each DitBlock includes the hidden layer features of the noisy trajectory and the conditional features Q-factors encoded with scene information after cross-attention. f Contextual features, navigation features Q n With diffusion time step condition Q t The combined features and attention mask are used to limit the scope of the attention mechanism. Finally, a FinalLayer layer maps the features processed by the DitBlock layers back to the final output space. The FinalLayer layer incorporates layer normalization, linear transformation, and adaptive mechanisms, enabling the model to better capture features and make effective predictions. Noise learning is a reverse diffusion process, learning the denoising process through several diffusion steps (e.g., 10 steps), with each step using the Dit network to predict noise. The neural network learning process involves learning how to remove noise at each diffusion step to restore a distribution close to the original data. The ultimate goal is to learn the data distribution from noisy trajectories to generate trajectories, using a score-based loss from the diffusion model. The log probability density gradient of the noise distribution p(x) of the trajectory at diffusion time t is used as the loss score. During the learning process, this gradient will explore in the direction of higher probability density to learn the distribution of trajectory samples and generate the initial navigation trajectory.
[0041] In some possible embodiments of the present invention, such as Figure 4 As shown, the potential distance characteristics between the robot and each obstacle are calculated based on the robot's own footprint as it moves along the initial navigation path, including: S401, Determine the robot's movement area based on the robot's position and its own footprint; S402, Construct the minimum distance constraint inequality from each obstacle to the boundary of the area occupied by the robot when it moves along the initial trajectory; S403 uses a pre-set neural network to solve the minimum distance constraint inequality, and obtains the potential distance characteristics between the robot and each obstacle when the robot moves along the initial navigation path.
[0042] In this embodiment of the invention, during robot path planning, a feasible region of a convex polygon is defined based on the robot's length and width. The robot's range of motion is limited by the inequality Gx≤h, where x represents the robot's position coordinates, and G and h reflect the robot's rotation and translation, thereby ensuring that the robot avoids collisions with obstacles during movement. To calculate the minimum distance between the robot body and surrounding obstacles, it can be represented as a strongly dual problem, as follows:
[0043]
[0044] in, Obstacles The minimum distance between the robot's movement area and the area it occupies, where G is the robot's rotation angle. For the robot's translation angle, Indicates obstacles Position in the robot coordinate system and Let be the rotation and translation matrices of the robot's own position.
[0045] By observing this strongly dual problem, we can find that each point Both with a pair and Related. and The geometric meaning of such as Figure 5 As shown, the minimum distance can be seen. From obstacle point The decision is made based on the edge closest to the robot itself. Edges associated with collisions have positive values, while edges unrelated to collisions have values of 0. The larger the value, the stronger the correlation of the collisions. For This represents the normal vector of the plane separating the obstacle point from the robot itself. Therefore... and This can be understood as the matching and distance from each obstacle point to its nearest robot edge. Distance calculation is sparsified by pruning mismatched edges for each obstacle point. Solving this convex optimization problem yields the latent distance features of each obstacle point. and .
[0046] Furthermore, traditional convex optimization problems have high computational complexity, and real-time applications become impossible when the number of obstacle points is too large. To accelerate the solution of latent distance features, the mapping from obstacle points to latent distance features can be implemented using a neural network. The first layer of the network is a 1x32 fully connected layer that reads the positions of M obstacle points in batches, and then represents them through layer normalization and the hyperbolic tangent function. The update process can be understood as a single-step gradient descent update projected onto the L2 norm. The second layer of the network is also a 32x32 fully connected layer, followed by a modified linear unit activation representation. The update process can be understood as a single-step gradient update projected onto a semi-positive definite cone by the robot. By alternately repeating the first and second layers, a 32x1 fully connected layer is finally output, used for output... The loss function is calculated by constructing the mean squared error using the optimal solution obtained from solving the graph optimization function and the network solution learned by the neural network. This ensures... High precision in different vector directions, and The mean squared error is also used to construct the loss. The neural network used for latent distance feature calculation does not require collecting large amounts of real-world data for training; it is only affected by the robot's shape [G, h]. For a given robot model [G, h], the training process randomly generates the positions of M points from a specific range for each axis. M optimal values are obtained by solving the graph optimization problem. This constructs the labeled dataset T={ for neural network training. By iterating through 5000 training epochs, with a batch size of 256 per epoch, and using an optimizer at 5e... -5 The learning rate is set to 0.5 and the decay rate is set to 0.5 to update the network parameters. Notably, the output of this neural network is not a simple distance value, but a set of latent distance features that sparsely encode the collision correlations from the robot body to all obstacle points, as well as the normal vector relationships.
[0047] In some possible embodiments of the present invention, such as Figure 6 As shown, when the potential distance feature indicates a collision risk between the robot and an obstacle greater than a preset risk threshold, a lateral gradient constraint is added to the diffusion strategy based on the potential distance feature to obtain a navigation path that can avoid the obstacle, including: S601 calculates the collision-related distance value between the robot and the obstacle based on the potential distance features. When the collision-related distance value is within the preset distance value range, an exponential collision penalty function is constructed. S602, the gradient of the exponential collision penalty function is solved to obtain the lateral gradient constraint. The lateral gradient constraint is added to the diffusion strategy, and the gradient guidance of the collision classifier is used to generate a navigation path that can avoid obstacles.
[0048] In this embodiment of the invention, based on the potential distance features obtained in the foregoing embodiments, collision-related distance values are generated, and the conversion relationship is as follows: clip_distances = max(1-distance / CLIP_DISTANCE, 0) When distance is greater than or equal to CLIP_DISTANCE, the penalty is 0, indicating that the distance is far enough that no repulsive force is needed. When distance is close to 0 or negative, the penalty is close to 1, indicating a serious risk of collision. When d is between (0, CLIP_DISTANCE), the penalty decays linearly. A negative exponential collision penalty is then constructed, which guides the gradient of the position to form a repulsive field reward. The gradient of this reward is calculated to obtain the constraint gradient, which is then superimposed on the noise prediction of the diffusion model. To ensure trajectory smoothness, only the lateral gradient of the trajectory is penalized, and a 1D convolution with a Gaussian kernel is used for smoothing constraints to avoid trajectory jitter; no penalty is applied to the longitudinal gradient. Finally, the calculated gain is multiplied by the trajectory obtained at time step t, and this multiplication is used as the gradient score guided by the classifier. This guides the diffusion generation to actively avoid obstacles during sampling, and through continuous iteration and denoising, a trajectory that conforms to smoothness and obstacle avoidance preferences is generated.
[0049] The specific process is shown in the following formula:
[0050] in, The diffusion model uses conditional scores based on score matching to learn the trajectory sample distribution. The diffusion model is based on the unconditional scores of the learning trajectory sample distribution, which are obtained by matching scores. The classifier gradient is guided by collision constraints. w These are hyperparameters used to control the boot size. When w When = 0, it is an unconditional model of the diffusion model learning the sample distribution; as = 0, it becomes an unconditional model. w As the size increases, the sample will become more and more towards The direction of increase is generated, meaning it increasingly meets the collision constraint condition c.
[0051] This invention directly utilizes raw point cloud data to construct conditional features of a 2D grid context and potential obstacle distance features, mapping them to robot trajectory generation. This eliminates error-prone intermediate representations and complex coding modules, preventing error propagation at the source. This significantly improves the perception and decision-making accuracy and overall robustness of the motion planning system in complex and noisy environments. By considering the robot model, a physically feasible trajectory that follows robot kinematics and dynamics constraints is generated. This trajectory process is interpretable; its generation mechanism explicitly transforms high-level driving semantics into low-level optimization objectives. Through gradient guidance and other techniques, it dynamically applies influence during the inference process, ensuring that the final trajectory is not only effective but also conforms to the rational expectations of the human operator and the physical limits of the robot. By introducing a gradient-based real-time guidance mechanism during sampling, efficient coordination of safety and comfort is achieved. When a potential collision risk exists between the predicted trajectory and obstacles, the system automatically generates and attaches a virtual repulsive field gradient, guiding the sampling process to actively deviate from the danger zone. This achieves active obstacle avoidance capabilities similar to real-time force feedback, greatly enhancing the safety of robot motion planning. Meanwhile, the guidance mechanism also takes the smoothness constraints such as lateral jitter of the trajectory as optimization targets. By applying smooth guidance in each step of denoising sampling, it can effectively suppress drastic changes and frequent oscillations of the trajectory, thereby generating a smooth and comfortable planning experience.
[0052] To better implement the robot collision avoidance motion path planning method in this embodiment of the invention, based on the robot collision avoidance motion path planning method, correspondingly, as follows: Figure 7 As shown, this embodiment of the invention also provides a robot collision avoidance motion path planning device. The robot collision avoidance motion path planning device 700 includes: The initial navigation path generation module 701 is used to construct a grid map occupancy map based on the grid map occupancy information within the robot's preset range, and generate an initial navigation path by combining the grid map occupancy map and the preset navigation trajectory using a diffusion strategy. The collision risk calculation module 702 is used to calculate the potential distance characteristics between the robot and each obstacle when the robot moves along the initial navigation path based on the robot's own footprint. The potential distance characteristics are used to characterize the collision risk between the robot and the obstacle. The navigation path correction module 703 is used to add lateral gradient constraints to the diffusion strategy based on the potential distance features when the collision risk between the robot and the obstacle is greater than a preset risk threshold, so as to obtain a navigation path that can avoid the obstacle.
[0053] The robot collision avoidance motion path planning device 700 provided in the above embodiments can realize the technical solutions described in the above robot collision avoidance motion path planning method embodiments. The specific implementation principles of each module or unit can be found in the corresponding content in the above robot collision avoidance motion path planning method embodiments, and will not be repeated here.
[0054] like Figure 8 As shown, the present invention also provides a robot 800. The robot 800 includes a processor 801, a memory 802, and a display 803. Figure 8 Only some components of robot 800 are shown, but it should be understood that it is not required to implement all of the components shown, and more or fewer components may be implemented instead.
[0055] In some embodiments, processor 801 may be a central processing unit (CPU), a microprocessor, or other data processing chip, used to run program code stored in memory 802 or process data, such as the robot collision avoidance motion path planning method of the present invention.
[0056] In some embodiments, processor 801 may be a single server or a group of servers. The server group may be centralized or distributed. In some embodiments, processor 801 may be local or remote. In some embodiments, processor 801 may be implemented on a cloud platform. In some embodiments, the cloud platform may include private cloud, public cloud, hybrid cloud, community cloud, distributed cloud, internal cloud, multi-cloud, etc., or any combination thereof.
[0057] In some embodiments, memory 802 may be an internal storage unit of robot 800, such as a hard disk or memory of robot 800. In other embodiments, memory 802 may also be an external storage device of robot 800, such as a plug-in hard disk, smart media card (SMC), secure digital (SD) card, flash card, etc. equipped on robot 800.
[0058] Furthermore, the memory 802 may include both internal storage units of the robot 800 and external storage devices. The memory 802 is used to store application software and various types of data installed on the robot 800.
[0059] In some embodiments, the motion module 803 can be the moving legs of a multi-legged robot, the moving wheels of a wheeled robot, etc.
[0060] In some embodiments, when the processor 801 executes the robot collision avoidance motion path planning program in the memory 802, the following steps may be implemented: A grid map occupancy map is constructed based on the grid map occupancy information within the robot's preset range. An initial navigation path is generated by combining the grid map occupancy map with the preset navigation trajectory using a diffusion strategy. Based on the robot's own footprint, the potential distance characteristics between the robot and each obstacle are calculated when the robot moves along the initial navigation path. The potential distance characteristics are used to characterize the collision risk between the robot and the obstacle. When the potential distance feature indicates that the collision risk between the robot and the obstacle is greater than the preset risk threshold, a lateral gradient constraint is added to the diffusion strategy based on the potential distance feature to obtain a navigation path that can avoid the obstacle.
[0061] It should be understood that when the processor 801 executes the robot collision avoidance motion path planning program in the memory 802, in addition to the functions mentioned above, it can also perform other functions, as can be found in the description of the corresponding method embodiments above.
[0062] Accordingly, this application also provides a computer-readable storage medium for storing computer-readable programs or instructions. When the programs or instructions are executed by a processor, they can implement the steps or functions of the robot collision avoidance motion path planning method provided in the above-described method embodiments.
[0063] Those skilled in the art will understand that all or part of the processes of the methods described in the above embodiments can be implemented by a computer program instructing related hardware, and the program can be stored in a computer-readable storage medium. The computer-readable storage medium may be a disk, optical disk, read-only memory, or random access memory, etc.
[0064] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.
Claims
1. A method for planning a robot's collision avoidance motion path, characterized in that, include: A grid map occupancy map is constructed based on the grid map occupancy information within the robot's preset range. An initial navigation path is generated by combining the grid map occupancy map with the preset navigation trajectory using a diffusion strategy. Based on the robot's own footprint, the potential distance characteristics between the robot and each obstacle are calculated when the robot moves along the initial navigation path. The potential distance characteristics are used to characterize the collision risk between the robot and the obstacle. When the potential distance feature indicates that the collision risk between the robot and the obstacle is greater than a preset risk threshold, a lateral gradient constraint is added to the diffusion strategy based on the potential distance feature to obtain a navigation path that can avoid the obstacle.
2. The robot collision avoidance motion path planning method according to claim 1, characterized in that, The step of constructing a grid map occupancy map based on grid map occupancy information within a preset range of the robot includes: Collect terrain point cloud data within a preset range of the robot, and construct a raster map based on the terrain point cloud data; The grid cells in the grid map whose point cloud count exceeds a preset point cloud count threshold are identified as occupied grid cells, and the maximum height of the terrain point cloud in each occupied grid cell is used as the occupancy value of the occupied grid cell to construct a grid map occupancy map.
3. The robot collision avoidance motion path planning method according to claim 1, characterized in that, The step of generating an initial navigation path using a diffusion strategy by combining the grid map occupancy map and the preset navigation trajectory includes: Extract the spatial occupancy features of the grid map occupancy map and the navigation features of the preset navigation trajectory, and fuse the spatial features and the navigation features to obtain a noisy trajectory; The hidden features of the noisy trajectory are extracted, and the hidden features, space occupancy features, navigation features, and preset diffusion time step conditions are used as inputs to the set diffusion denoising model to perform diffusion denoising on the noisy trajectory to obtain the initial navigation path.
4. The robot collision avoidance motion path planning method according to claim 1, characterized in that, The calculation of the potential distance characteristics between the robot and each obstacle when the robot moves along the initial navigation path based on its own footprint includes: The robot's movement area is determined based on its location and its own footprint. Construct the minimum distance constraint inequality from each obstacle to the boundary of the area occupied by the robot when it moves along the initial trajectory; The minimum distance constraint inequality is solved using a pre-defined neural network to obtain the potential distance characteristics between the robot and each obstacle when the robot moves along the initial navigation path.
5. The robot collision avoidance motion path planning method according to claim 4, characterized in that, The minimum distance constraint inequality is: in, Obstacles The minimum distance between the robot's movement area and the area it occupies, where G is the robot's rotation angle. The translation distance of the robot. Indicates obstacles Position in the robot coordinate system and Let the rotation and translation matrices be the robot's own position. and Location of each obstacle The potential distance characteristics relative to the robot's own footprint. Indicates the correlation of collisions. Represents distance features, and This represents the generalized form of inequality constraints.
6. The robot collision avoidance motion path planning method according to claim 4, characterized in that, When the potential distance feature indicates that the collision risk between the robot and the obstacle is greater than a preset risk threshold, a lateral gradient constraint is added to the diffusion strategy based on the potential distance feature to obtain a navigation path that can avoid the obstacle, including: Based on the potential distance features, the collision-related distance value between the robot and the obstacle is calculated. When the collision-related distance value is within a preset distance value range, an exponential collision penalty function is constructed. The gradient of the exponential collision penalty function is solved to obtain the lateral gradient constraint. The lateral gradient constraint is added to the diffusion strategy, and the collision classifier gradient guidance is used to generate a navigation path that can avoid the obstacle.
7. The robot collision avoidance motion path planning method according to claim 6, characterized in that, The formula for the collision classifier is: in, The diffusion model uses conditional scores based on score matching to learn the trajectory sample distribution. The diffusion model is based on the unconditional scores of the learning trajectory sample distribution, which are obtained by matching scores. is the classifier gradient guided by collision constraints, and w is a hyperparameter controlling the size of the guidance.
8. A robot collision avoidance motion path planning device, characterized in that, include: The initial navigation path generation module is used to construct a grid map occupancy map based on the grid map occupancy information within a preset range of the robot, and generate an initial navigation path by combining the grid map occupancy map and the preset navigation trajectory using a diffusion strategy. The collision risk calculation module is used to calculate the potential distance characteristics between the robot and each obstacle when the robot moves along the initial navigation path based on the robot's own footprint. The potential distance characteristics are used to characterize the collision risk between the robot and the obstacle. The navigation path correction module is used to add a lateral gradient constraint to the diffusion strategy based on the potential distance feature when the potential distance feature indicates that the collision risk between the robot and the obstacle is greater than a preset risk threshold, so as to obtain a navigation path that can avoid the obstacle.
9. A robot, characterized in that, Includes memory, processor, and motion module, among which, The memory is used to store programs; The processor, coupled to the memory, is used to execute the program stored in the memory to implement the steps in the robot collision avoidance motion path planning method according to any one of claims 1 to 7. The motion module is used to move along a trajectory under the control of the processor.
10. A computer-readable storage medium, characterized in that, Used to store computer-readable programs or instructions, which, when executed by a processor, can implement the steps in the robot collision avoidance motion path planning method according to any one of claims 1 to 7.