Self-adaptive Gaussian mixture mechanical arm path planning method considering collision escape

Abstract

The invention discloses a self-adaptive Gaussian mixture mechanical arm path planning method considering collision escape, and aims to solve the problems of low sampling efficiency, low convergence speed and poor path quality of mechanical arm path planning in a complex environment. The method mainly comprises the following steps: introducing a Gaussian mixture model for dynamic weight adjustment, wherein the model fuses four sampling strategies of target guidance, obstacle avoidance, uniform exploration and collision escape; the weight of each component is adaptively adjusted by evaluating the number of continuous collision times, the distance from the target and the like, so that blind search and heuristic exploration are dynamically balanced, and the sampling efficiency in a complex environment is remarkably improved. In addition, adjacent point search is accelerated by using two-dimensional spatial index, calculation resource allocation is optimized in combination with an early stop mechanism, and the quality of a final path is ensured through path simplification and smoothing. The result shows that the method has remarkable advantages in the aspects of path sampling efficiency, convergence speed and path smoothness.

Description

Technical Field

[0001] The present invention belongs to the field of robotics, and in particular to the field of robot path planning, and in particular to a robot path planning method that considers collision escape and adaptive mixed Gaussian. Background Art

[0002] With the development of robots, robotic arms have been widely used in scenarios such as industrial robots, special operation robots, service consumption robots, and additive manufacturing equipment manufacturing. However, the path planning technology of robotic arms still faces challenges, especially in complex environments. Traditional planning methods often exhibit problems such as low sampling efficiency, slow convergence speed, many redundant nodes, and uneven paths. The Bi-directional Rapidly-exploring Random Tree Star (Bi-RRT*) algorithm significantly improves the sampling efficiency and convergence speed by introducing a bidirectional search strategy based on the asymptotically optimal Rapidly-exploring Random Tree Star (RRT*). Therefore, it is widely used in the field of robotic arm path planning. Despite this, Bi-RRT* still has the following four core problems when applied to robotic arm path planning:

[0003] (1) The sampling strategy lacks guidance and has low exploration efficiency. Bi-RRT* uses uniform random sampling, which results in a large number of invalid sampling points far away from the optimal path in the vast free space area, thereby reducing the convergence speed of the algorithm. In critical areas such as narrow channels, it is difficult to generate effective samples to pass the bottleneck, further weakening the sampling efficiency.

[0004] (2) The expansion step size is fixed and has poor environmental adaptability. This algorithm usually uses a fixed expansion step size. In open areas with sparse obstacles, a smaller fixed step size will limit the growth rate of the random tree and extend the overall planning time. Conversely, in narrow areas with dense obstacles, a larger fixed step size will sharply increase the collision probability, causing the tree expansion to frequently fail and making it difficult to find a feasible path. A single step size is difficult to adapt to the geometric characteristics of different areas in the environment at the same time;

[0005] (3) The asymptotic optimal process converges slowly, and the planning time is lengthy. The Bi-RRT* algorithm does not have global optimality, and its optimal solution is obtained asymptotically through iterative optimization. In the later stages of planning, when the improvement in path length is no longer significant, the algorithm will continue to perform a large number of rewiring operations. Repeated optimization of paths that have stabilized does not effectively utilize computing resources, resulting in unnecessary planning time consumption.

[0006] (4) The generated path is not smooth and does not conform to the kinematic constraints of the robot arm. The path directly generated by traditional Bi-RRT* is composed of a series of broken line segments, which usually contain a large number of sharp corners. Such a path does not meet the kinematic constraints of the robot arm in the real world and cannot be directly executed by the underlying motion controller. Additional post-processing steps are required.

[0007] Patent CN116784975B, "Flexible Puncture Needle Path Planning Method Based on an Improved Bi-RRT Algorithm," introduces a precomputed convergence point to guide tree growth. This approach completely sacrifices the random exploration capability of the RRT algorithm, thereby losing its robustness and probabilistic completeness. Because the convergence point selection lacks awareness of the global layout of obstacles, in complex environments with dense obstacles or recessed structures, this deterministic expansion strategy directs the search to a local minimum and fails. Consequently, the algorithm can only find the first feasible path determined by the initial assumptions, with no guaranteed quality and no ability to find a more optimal path solution.

[0008] The paper "Path Planning for Dragon-Fruit-Harvesting Robotic Arm Based on XN-RRT*Algorithm" introduces a single Gaussian distribution during the sampling process, replacing the traditional RRT* random sampling by dynamically adjusting its sampling center and range. This guides sampling points toward the target area and reduces redundant exploration. However, this method lacks obstacle avoidance measures and the ability to navigate concave areas. When dealing with complex environments such as narrow passages and dense obstacles, it is prone to falling into local optimality, resulting in a significant decrease in overall efficiency. Summary of the Invention

[0009] The present invention provides a robot arm path planning method considering collision escape adaptive Gaussian mixture model (AGMM), aiming to solve the problems of low sampling efficiency, slow convergence speed, many redundant nodes and uneven path in traditional algorithms.

[0010] In order to solve the above problems, the present invention adopts the following technical solutions: introducing a mixed Gaussian model with dynamic weight adjustment, which integrates the four sampling strategies of goal guidance, obstacle avoidance, uniform exploration and collision escape, and balances blind search and heuristic exploration in different areas; combining the adaptive step size mechanism based on local environment perception, so that the algorithm can quickly traverse open areas and conduct detailed exploration in narrow channels; in order to improve computing efficiency, the present invention uses two-dimensional spatial indexing to accelerate node search; through the combined optimization process of path simplification and B-spline smoothing, combined with the early stopping mechanism, it is ensured that a safe, smooth and asymptotically optimal high-quality path is generated within a limited time. Specifically, it includes the following steps:

[0011] S1: Rasterize the map and determine the starting point X of the robot arm movement init and target point X goal , use them as the root nodes of the starting tree and the target tree respectively, initialize other algorithm parameters, and execute S2;

[0012] S2: The loop is terminated when the preset maximum number of iterations is reached or the early stopping mechanism is triggered, otherwise S2.1 is executed;

[0013] S2.1: Calculate whether the difference in the cumulative number of collisions between the starting tree and the target tree exceeds a preset threshold. If the threshold is not reached, randomly select a tree to be expanded and execute S2.2.

[0014] S2.2: First, use AGMM for sampling. The AGMM model consists of a goal-oriented Gaussian component, an obstacle avoidance Gaussian component, a uniform exploration Gaussian component, and a collision escape Gaussian component. The formula of the AGMM model can be expressed as:

[0015]

[0016] Where: k is the number of Gaussian components, x is the sampling point, w i is the weight of the i-th component, is the probability density function of the i-th component, μ i is the mean of the i-th component, ∑ i is the covariance matrix of the i-th component;

[0017] S2.3: Design a goal-oriented component to guide the exploration direction to the target point and accelerate the connection between the two trees. The μ of this Gaussian component is goal is a fixed value, set to the root node coordinate of the target tree, σ goal Used to adaptively adjust the sampling range of the Gaussian component:

[0018] σ goal =σ init_goal ·max(d target / d overall ,γ goal_sigma ) (2)

[0019] Where: d target is the distance from the node closest to the target in the current tree to the target, d overall is the straight-line distance between the starting point and the target point, d target / d overall is the current progress measure, σ init_goal is the initial standard deviation, γ goal_sigma is the minimum scaling factor;

[0020] The goal-oriented component performs extensive exploration when the random tree is far from the goal, and performs fine connection and path optimization when the random tree is close to the goal, executing S2.4;

[0021] S2.4: The obstacle avoidance component explores near obstacles and finds a path around them or through narrow passages. The variance of this component is σ obs_dynamic_val is a fixed value determined by the grid resolution:

[0022] σ obs_dynamic_val =grid×β obs_sigma (3)

[0023] Where: grid is the side length of the grid unit, β obs_sigma is the scaling factor;

[0024] Mean μ obs An adaptive strategy is used to select an obstacle related to the current task. If no obstacle meets the requirements, the closest obstacle is selected as the average:

[0025]

[0026] Where: O radius is the set of coordinates of the center points of the filtered obstacles, o j is the coordinate of the center point of the j-th obstacle, O all The set of coordinates of the center points of all obstacle units on the map, X latest is the coordinate of the node most recently added in the current tree, R filter The radius threshold for preliminary screening, d(X latest ,o j ) is used to calculate the distance from the current node to the obstacle o j The distance, O final is the candidate set filtered by radius and direction, is the direction vector from the latest node to the target, From the latest node to an obstacle o m The direction vector, T dot is the threshold of the cosine value of the angle between two directions, RandomChoice( final ) represents the final mean μ obs From the candidate set O final Randomly select an element from m is the center coordinate of the mth obstacle, To find the latest ,o j )The smallest o j , execute S2.5;

[0027] S2.5: The collision escape component aims to implement a dynamic escape strategy that is activated when the system is trapped due to continuous collisions. It maintains a queue of the most recent collision points and randomly selects a point from the queue as the mean of the Gaussian distribution. Its mathematical form is defined as follows:

[0028] P coll ={p1,p2,…,p k} (7)

[0029] μ coll =RandomChoice(P coll ) (8)

[0030] Where: P coll is the collision point queue, p k is the kth collision point in the queue, μ coll is the sampling center of the collision escape Gaussian component, RandomChoice() is the random selection function;

[0031] Standard deviation σ coll is set to a small fixed value. This setting ensures that the sampling points can be closely around the selected collision center, thereby providing a high-precision local avoidance guidance. Execute S2.6;

[0032] S2.6: The uniform exploration component provides the necessary global exploration capability for the sampling process. The mean μ of this Gaussian component is uniform is fixed at the center of the planning area, and its standard deviation σ uniform is set to a large value to ensure that the sampling points can widely cover the entire planning space, and S2.7 is executed;

[0033] S2.7: Weight adjustment of AGMM involves the weight of the goal-oriented component w G , obstacle avoidance component weight w O , collision escape component weight w C , uniform exploration component weight w U ,The adaptive adjustment of these weights requires a multi-level judgment ,strategy. The priorities are divided from high to low into the ,cases of continuous collision failure, the case of having found a path or being ,close to the target, and the case of being far from the target. ,Execute S2.8;

[0034] S2.8: To determine the continuous collision failure situation, the following conditions must be met:

[0035]

[0036] Where: Counter collision count Record the number of times a new node was not successfully added, collision boostFor the preset threshold, the RCP queue stores the information of the actual collision point;

[0037] In this case, an escape sampling strategy is adopted to deal with the local dilemma, under which the collision escape component will be given a dominant weight w stuck , thereby guiding the sampling to the nearest collision area for fine exploration, and the weight of the target-oriented component will be based on the scaling factor β stuck In this case, the guidance towards the goal may no longer be effective or even misleading. Similarly, the weights of the uniform exploration component for large-scale exploration and the obstacle avoidance component for bypassing obstacles will also be increased by their respective scaling factors β. obs and β uniform is significantly reduced to avoid guiding the random tree to redundantly expand into invalid areas outside the current dilemma, executing S2.9;

[0038] S2.9: To determine if a path has been found or if the target is very close, the following conditions must be met:

[0039]

[0040] Where: path_found is a Boolean flag, TO_GOAL threshold is the distance threshold;

[0041] In this case, the sampling strategy will shift from broad exploration to focused optimization. To reduce unnecessary global search, the influence of the uniform exploration component will be scaled by a very small factor λ. uniform In addition, in order to accelerate the path convergence and make refined obstacle avoidance adjustments, the weights of the target guidance component and the obstacle avoidance component will be magnified by the factor λ target and λ obs The weight of the collision escape component remains unchanged. This comprehensive adjustment of the weights is intended to focus the algorithm's computing resources on guiding the target of the existing path, reducing unnecessary global exploration, thereby efficiently improving the path quality and accelerating convergence. Execute S2.10;

[0042] S2.10: When judging the situation when the target is far away, the following conditions must be met:

[0043] d target >(d overall ×FAR_GOAL threshold ×1.5) (11)

[0044] Where: FAR_GOAL threshold is the long distance threshold, d target is the long distance threshold, d overall is the total distance;

[0045] In the early stage of exploration, when the random tree is far away from the target, a balanced exploration strategy is maintained, and the weights of all components are kept at their initial values ​​to comprehensively utilize target guidance, local obstacle avoidance, and global exploration information to prevent prematurely falling into the local optimum, and execute S2.11;

[0046] S2.11: Perform nearest neighbor search on the random tree that needs to be expanded and obtain X rand The nearest node X near , then execute S2.12;

[0047] S2.12: Based on Node X near Obstacle distribution within a predefined neighborhood is calculated, and the ratio of the area occupied by obstacles in the area to the total area of ​​the entire area is calculated. The environmental coefficient η is mapped based on this ratio. When obstacles are dense, the η value is small; when obstacles are sparse, the η value is large. The expansion step size is then adjusted according to the η value, and the expansion step size is dynamically expanded:

[0048] Step = Step old × η (12)

[0049] Where: Step old is the step size before adjustment, and Step is the step size after adjustment. This dynamic step size expansion strategy uses a smaller step size in areas with dense obstacles to improve obstacle avoidance capabilities, and uses a larger step size in open areas to accelerate the search process. Execute S2.13;

[0050] S2.13: Calculate X rand With X near The vector of the connection direction and generate a new node X along this direction new , for the new node X new Perform collision detection. If a collision occurs, it is recorded in the recent collision queue RCP and enters the next main loop iteration; otherwise, X new Add it to the current random tree as a new node and execute S2.14;

[0051] S2.14: For new node X new Reselect the parent node and reconnect to make the connection through X new The path is shorter, execute S2.15;

[0052] S2.15: Sampling and expanding the starting tree, then sampling and expanding the target tree, until the two trees form a complete initial path, and then proceed to the next iteration. If a new candidate path is found, calculate its total length. If the path is shorter, update the optimal path and reset the counter; otherwise, add 1 to the counter until the count reaches the stable iteration threshold S. max When , the path is considered to have converged, the main loop is terminated in advance, and S3 is executed;

[0053] S3: Prune the generated random tree. Traverse the nodes of the current tree at a preset iteration interval. If a node is too far away from the initial path, it will be removed from the random tree. If a node is retained, all its ancestor nodes will also be retained to ensure the connectivity of the tree. Execute S4.

[0054] S4: Use the greedy strategy to delete redundant nodes, shorten the path distance, and execute S5;

[0055] S5: Smooth the path. Before outputting the smoothed path, if a collision is detected, the smoothed path is discarded and the path before smoothing is used as the final output result.

[0056] The present invention has the following beneficial effects:

[0057] 1. By introducing a hybrid sampling strategy that combines goal guidance, obstacle avoidance, uniform exploration, and collision escape, the sampling points are concentrated in areas that are more conducive to reaching the target, effectively suppressing the growth of ineffective branches. By evaluating the number of consecutive collisions and the distance to the target, the weights of each component are adaptively adjusted, dynamically balancing blind search and heuristic exploration, significantly improving sampling efficiency in complex environments. The average number of nodes in this method is 831.18, a 76.23% reduction compared to 3496.5 in Bi-RRT*.

[0058] 2. Introducing dynamic step size based on environmental awareness to achieve efficient global exploration. By evaluating the obstacle density around the nearest neighbor node in real time, the expansion step size is dynamically adjusted. This mechanism enables the algorithm to advance rapidly with a larger step size in open areas, while automatically switching to a smaller step size for detailed exploration in complex terrain with dense obstacles, balancing planning speed and success rate. The average path length of this method is 807.56 cm, which is 7.48% shorter than the 872.88 cm of Bi-RRT*.

[0059] 3. Combining a two-dimensional spatial index with an early stopping mechanism significantly shortens planning time. Building on the Bi-RRT* algorithm, this approach utilizes a two-dimensional spatial index to reduce the complexity of neighboring node queries from linear to logarithmic. Furthermore, an early stopping mechanism based on the path convergence state avoids unnecessary iterations after a high-quality solution is found. The average planning time for this method is only 3.71 seconds, a 50.6% reduction compared to Bi-RRT*'s 7.51 seconds. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] Figure 1 This is a flow chart of a robot path planning method considering collision escape and adaptive mixed Gaussian.

[0061] Figure 2This is a schematic diagram of the collision escape Gaussian component sampling of this method;

[0062] Figure 3 Schematic diagram of target-oriented Gaussian component sampling for this method;

[0063] Figure 4 Schematic diagram of Gaussian component sampling for obstacle avoidance in this method;

[0064] Figure 5 Schematic diagram of uniform exploration Gaussian component sampling of this method;

[0065] Figure 6 Schematic diagram of the step size expansion of this method in an obstacle-dense environment;

[0066] Figure 7 Schematic diagram of step size expansion of this method in an environment with sparse obstacles;

[0067] Figure 8 Schematic diagram of the path simplification process of this method;

[0068] Figure 9 is the final path result diagram of RRT*;

[0069] Figure 10 This is the final path result diagram of Bi-RRT*;

[0070] Figure 11 This is the initial path result diagram of this method;

[0071] Figure 12 This is the path simplification result graph of this method;

[0072] Figure 13 This is the curve optimization result diagram of this method;

[0073] Figure 14 This is the result diagram of the starting position of the physical experiment of this method;

[0074] Figure 15 This is the location result diagram of the physical experiment process of this method;

[0075] Figure 16 This is the final position result diagram of the physical experiment of this method. DETAILED DESCRIPTION

[0076] The following specific implementation measures and Figure 1 The present invention is described in detail:

[0077] The present invention relates to a robot path planning method considering collision escape adaptive mixed Gaussian. Based on RRT*, the Bi-RRT* algorithm significantly accelerates the convergence speed of the initial path through bidirectional synchronous search. However, the sampling process of the algorithm lacks guidance, resulting in low exploration efficiency in narrow channels; the expansion step size is fixed, resulting in poor environmental adaptability; the late convergence is lengthy, which wastes computing resources; and the original path is not smooth, making it difficult to directly apply to actual operations. To this end, the present invention proposes a comprehensive optimization method, designs a dynamic weighted mixed Gaussian model, integrates a hybrid sampling strategy of target guidance, collision escape, obstacle avoidance and uniform exploration, and dynamically balances blind search and heuristic exploration; introduces an adaptive step size mechanism based on the complexity of the local environment to maximize the exploration efficiency in different areas; utilizes a two-dimensional spatial index to accelerate node search, and combines the early stopping mechanism to avoid redundant optimization iterations; and ensures the smoothness and high quality of the final path through a post-processing process of path simplification and B-spline curve fitting. Figure 2 Describing the sampling process in a trapped state, after continuous collisions in a narrow channel, the collision escape component is activated, and the sampling is forced to be guided to the vicinity of the collision point for high-precision exploration to find an escape path. Figure 3 Describes the sampling process in the optimization state. When the tree approaches the goal, the goal-oriented component dominates, and its sampling range is adaptively reduced, concentrating the sampling points around the goal for fine path optimization. Figure 4 Describes the obstacle avoidance sampling process. When the tree develops towards an obstacle, the obstacle avoidance component is activated, guiding the generation of sampling points around the obstacle, thereby exploring an effective detour path. Figure 5 Describes the sampling process in the exploration state. In the early planning stage, the goal-oriented component and the uniform exploration component work together to form a broad sampling range to ensure the global exploration capability of the algorithm and prevent premature convergence. Figure 6 Describing an obstacle-dense environment, when the latest node is located in an obstacle-dense area, the calculated obstacle density is higher and the final expansion step size is adaptively reduced, allowing the tree to explore the area in a more cautious and safer manner. Figure 7 In an open environment, when the frontier node of the tree enters an open area with sparse obstacles, the obstacle density within the perception radius is very low, and the expansion step size is adaptively increased, which greatly improves the exploration efficiency of the algorithm in open space. The specific steps are as follows:

[0078] S1: According to the working range of the robot arm, set a two-dimensional array to represent the map, and rasterize the map. According to the position and shape of the obstacle, the corresponding grid is binary marked. The obstacle area is marked as 1 and the feasible area is marked as 0. Determine the starting point X of the robot arm movement init and target point X goal, use them as the root nodes of the starting tree and the target tree respectively, build and initialize the two-dimensional spatial index for each of the two trees, add the root node, initialize other algorithm parameters at the same time, and execute S2;

[0079] S2: The loop is terminated when the preset maximum number of iterations is reached or the early stopping mechanism is triggered, otherwise S2.1 is executed;

[0080] S2.1: Dynamically select the tree to be expanded, set the Boolean variable tree_to_expand, and determine whether to switch to a random tree with fewer collisions by calculating whether the difference in the cumulative number of collisions between the starting tree and the target tree exceeds a preset threshold. If the difference in the cumulative number of collisions between the starting tree and the target tree does not reach the threshold, randomly select the tree to be expanded and execute S2.2;

[0081] S2.2: Expand the dynamically selected tree. First, use AGMM for sampling. The AGMM model consists of a goal-oriented Gaussian component, an obstacle avoidance Gaussian component, a uniform exploration Gaussian component, and a collision escape Gaussian component. The formula of the AGMM model can be expressed as:

[0082]

[0083] Where: k is the number of Gaussian components, x is the sampling point, w i is the weight of the i-th component, is the probability density function of the i-th component, μ i is the mean of the i-th component, … i is the covariance matrix of the i-th component;

[0084] S2.3: Design a goal-oriented component to guide the exploration direction to the target point and accelerate the connection between the two trees. The μ of this Gaussian component is goal is a fixed value, set to the root node coordinate of the target tree, σ goal Used to adaptively adjust the sampling range of the Gaussian component:

[0085] σ goal =σ init_goal ·max(d target / d overall ,γ goal_sigma ) (2)

[0086] Where: d target is the distance from the node closest to the target in the current tree to the target, d overall is the straight-line distance between the starting point and the target point, d target / d overall is the current progress measure, σ init_goal is the initial standard deviation, γ goal_sigma is the minimum scaling factor;

[0087] When the random tree is far away from the target, the goal-oriented component target / d overall Close to 1, σ goal For larger values, the sampling points are widely distributed on the map, and a large range of exploration is carried out. When the random tree approaches the target, d target / d overall Reduce, σ goal Then, the sampling points are concentrated near the target, so as to perform fine connection and path optimization and execute S2.4;

[0088] S2.4: The obstacle avoidance component explores near obstacles and finds a path around them or through narrow passages. The variance of this component is σ obs_dynamic_val is a fixed value determined by the grid resolution:

[0089] σ obs_dynamic_val =grid×β obs_sigma (3)

[0090] Where: grid is the side length of the grid unit, β obs_sigma is the scaling factor;

[0091] Mean μ obs An adaptive strategy is used to select an obstacle related to the current task. First, the obstacles are screened to find all obstacles within a preset radius from the latest node of the current tree. Second, the direction of the screened obstacles is filtered. The direction vector from the latest node to the target and the direction vector from the latest node to the obstacle are calculated. The dot product of the two vectors is compared with the set threshold to retain only obstacles in the target direction. Then, an obstacle center is randomly selected from the screened obstacles as the mean. If no obstacles meet the conditions after screening, the globally closest obstacle is selected as the mean:

[0092]

[0093] Where: O radius is the set of coordinates of the center points of the filtered obstacles, o j is the coordinate of the center point of the j-th obstacle, O all The set of coordinates of the center points of all obstacle units on the map, X latest is the coordinate of the node most recently added in the current tree, R filter The radius threshold for preliminary screening, d(X latest ,o j ) is used to calculate the distance from the current node to the obstacle o j The distance, O final is the candidate set filtered by radius and direction, is the direction vector from the latest node to the target, From the latest node to an obstacle o m The direction vector, T dot is the threshold of the cosine value of the angle between two directions, RandomChoice( final ) represents the final mean μ obs From the candidate set O final Randomly select an element from m is the center coordinate of the mth obstacle, To find the latest ,o j )The smallest o j , execute S2.5;

[0094] S2.5: The collision escape component aims to implement a dynamic escape strategy, which is activated when the trapped state is caused by continuous collisions. To achieve escape, a queue of the most recent collision points is maintained and a point is randomly selected from the queue as the mean of the Gaussian distribution. Its mathematical form is defined as follows:

[0095] P coll ={p1,p2,…,p k} (7)

[0096] μ coll =RandomChoice(P coll ) (8)

[0097] Where: P coll is the collision point queue, p k is the kth collision point in the queue, μ coll is the sampling center of the collision escape Gaussian component, RandomChoice() is the random selection function;

[0098] Standard deviation σ coll is set to a small fixed value. This setting ensures that the sampling points are closely around the selected collision center, thus providing a high-precision local avoidance guidance, driving the tree to find a new feasible path in the neighborhood of the failure and execute S2.6;

[0099] S2.6: The uniform exploration component provides the necessary global exploration capability for the sampling process, aiming to prevent the algorithm from falling into local optimality or insufficient exploration due to excessive reliance on the guidance strategy of other components. In order to achieve unbiased sampling over a large range, the mean μ of this Gaussian component is uniform is fixed at the center of the planning area, and its standard deviation σ uniform is set to a large value to ensure that the sampling points can widely cover the entire planning space, and S2.7 is executed;

[0100] S2.7: Weight adjustment of AGMM involves the weight of the goal-oriented component w G , obstacle avoidance component weight w O , collision escape component weight w C , uniform exploration component weight w U ,The adaptive adjustment of these weights requires a multi-level judgment ,strategy. The priorities are divided from high to low into the ,cases of continuous collision failure, the case of having found a path or being ,close to the target, and the case of being far from the target. ,Execute S2.8;

[0101] S2.8: To determine the continuous collision failure situation, the following conditions must be met:

[0102]

[0103] Where: Counter collision count Records the number of times the random tree fails to add new nodes due to collisions with obstacles when trying to grow. boost It is a preset threshold. When the threshold is reached or exceeded, the algorithm is judged to be in a local dilemma in a certain area. After multiple consecutive collisions, effective sampling cannot be achieved. The RCP queue stores the information of the actual collision points. When the queue is empty, even if the number of consecutive collision failures exceeds the threshold, the actual collision points cannot be used to guide the collision escape component of the AGMM.

[0104] In this case, an escape sampling strategy is adopted to deal with the local dilemma, under which the collision escape component will be given a dominant weight w stuck , thereby guiding the sampling to the nearest collision area for fine exploration, and the weight of the target-oriented component will be based on the scaling factor β stuck In this case, the guidance towards the goal may no longer be effective or even misleading. Similarly, the weights of the uniform exploration component for large-scale exploration and the obstacle avoidance component for bypassing obstacles will also be increased by their respective scaling factors β. obs and β uniform is significantly reduced to avoid guiding the random tree to redundantly expand into invalid areas outside the current dilemma, executing S2.9;

[0105] S2.9: To determine if a path has been found or if the target is very close, the following conditions must be met:

[0106]

[0107] Where: path_found is a Boolean flag. When the starting tree and the target tree are successfully connected, forming a complete path from the starting point to the target point, this flag will be set to True, TO_GOAL thresholdis the distance threshold, used to determine whether the random tree is close to the target;

[0108] In this case, the sampling strategy will shift from broad exploration to focused optimization. To reduce unnecessary global search, the influence of the uniform exploration component will be scaled by a very small factor λ. uniform In addition, in order to accelerate the path convergence and make refined obstacle avoidance adjustments, the weights of the target guidance component and the obstacle avoidance component will be magnified by the factor λ target and λ obs The weight of the collision escape component remains unchanged. This comprehensive adjustment of the weights is intended to focus the algorithm's computing resources on guiding the target of the existing path, reducing unnecessary global exploration, thereby efficiently improving the path quality and accelerating convergence. Execute S2.10;

[0109] S2.10: When judging the situation when the target is far away, the following conditions must be met:

[0110] d target >(d overall ×FAR_GOAL threshold ×1.5) (11)

[0111] Where: FAR_GOAL threshold is 0.50, that is, when the progress is greater than the preset long-distance threshold, the algorithm will maintain a balanced exploration strategy, that is, the weights of all components remain at their initial values. The purpose of this design is to ensure that in the early stages of exploration, the algorithm can comprehensively utilize various sampling information, including both the macro direction provided by goal orientation and the local perception provided by obstacle avoidance, while retaining the necessary uniform exploration to ensure global coverage and prevent the algorithm from falling into the local optimal path too early. After completing the sampling of random points, execute S2.11;

[0112] S2.11: Perform nearest neighbor search on the random tree that needs to be expanded, and use a two-dimensional spatial index to index all nodes of the random tree, so as to quickly query and sample the obtained point X rand The nearest node X near , then execute S2.12;

[0113] S2.12: Based on Node X near Obstacle distribution within a predefined neighborhood is calculated, and the ratio of the area occupied by obstacles in the area to the total area of ​​the entire area is calculated. The environmental coefficient η is mapped based on this ratio. When obstacles are dense, the η value is small; when obstacles are sparse, the η value is large. The expansion step size is then adjusted according to the η value, and the expansion step size is dynamically expanded:

[0114] Step = Step old × η (12)

[0115] Where: Step old is the step size before adjustment, and Step is the step size after adjustment. This dynamic step size expansion strategy uses a smaller step size in areas with dense obstacles to improve obstacle avoidance capabilities, and uses a larger step size in open areas to accelerate the search process. Execute S2.13;

[0116] S2.13: Calculate X rand With X near The vector of the connection direction of , and expand along this direction with a step length Step to generate a new node X new , for the new node X new Perform collision detection, if X new If a collision occurs with an obstacle, the collision point information is recorded in the recent collision queue RCP and the next main loop iteration is entered; otherwise, X new Add it to the current random tree as a new node and execute S2.14;

[0117] S2.14: For new node X new Reselect the parent node and reconnect, and use the two-dimensional spatial index to find the new The set of neighboring nodes within the circle center and the preset radius r is calculated from the root to each neighboring node and then to X new The path cost is chosen as X. new The new parent node of X is reconnected, and this step can optimize the topology of the tree so that new The path is shorter, execute S2.15;

[0118] S2.15: According to the dynamic selection strategy, the selected tree is expanded. The starting tree is sampled and expanded first, and then the target tree is sampled and expanded again. When a certain expansion makes the newly added node in the starting tree close enough to a node in the target tree, the two trees are considered to have met. At this time, the two trees are connected to form a complete initial path, and the next iteration is performed. If a new candidate path is found, its total length is calculated. If the path is shorter, the optimal path is updated and the counter is reset; otherwise, the counter is incremented by 1 until the count reaches the stable iteration number threshold S. max When , the path is considered to have converged, the main loop is terminated in advance, and S3 is executed;

[0119] S3: Prune the generated random tree. After the initial path is generated, traverse the nodes of the current tree at a preset iteration interval. If a node is too far away from the initial path, it will be removed from the random tree. At the same time, if a node is retained, all its ancestor nodes will also be retained to ensure the connectivity of the tree, and then execute S4.

[0120] S4: Use a greedy strategy to delete redundant nodes by checking whether the straight line connection between any two non-adjacent points in the path is collision-free. If there is no collision, remove the redundant intermediate points to reduce the number of broken line segments in the path and shorten the path distance, and then execute S5;

[0121] S5: Smooth the path and interpolate the simplified path using a cubic B-spline interpolation curve. Before outputting the smoothed path, the smoothed path is checked for collisions again. Because the B-spline interpolation curve does not have inherent obstacle avoidance capabilities, if a collision is detected, the smoothed path is discarded and the path before smoothing is used as the final output result.

[0122] The present invention is simulated using PyCharm software. Figure 8 、 Figure 9 and Figure 10 The original graphs of RRT*, Bi-RRT* and the path planning of the present invention are shown in Figure 1. The coordinate axis unit is cm. In order to further verify the actual effect of the present invention, the specific process is as follows:

[0123] The map size is 640 cm in width and 480 cm in height, the starting coordinates are (10 cm, 10 cm), the end coordinates are (630 cm, 470 cm), and the maximum number of iterations is 5000.

[0124] Set the initial weight, collision escape weight w C Set to 0, the goal-oriented component weight w G Set to 0.4, obstacle avoidance component weight w O Set to 0.3, uniform exploration component weight w U Set to 0.3.

[0125] Set the adaptive weight adjustment factor of the AGMM model. When the continuous collision mechanism is triggered, the dominant weight w of the collision escape component is stuck Set to 0.8; the weight decay factor of other components is β stuck , β obs , β uniform The weight magnification factor λ is used to strengthen the advantage exploration when the path is found or close to the target. target and λ obs are set to 3.0 and 2.5 respectively; at the same time, the factor λ used to suppress global exploration uniform Set to 0.0001.

[0126] Dynamic step size parameters, the basic step size is 7.0cm, the step size scaling factor η is set to 0.3cm in dense obstacle areas and 1.2cm in open areas, set the early stopping mechanism parameters, and optimize the tolerance I max Set to 2.0cm, stable iteration threshold Smax The experiment was repeated 50 times to compare the traditional RRT* method and the Bi-RRT* method. The experimental data are shown in Table 1.

[0127] Table 1 Experimental data of three methods

[0128]

[0129] As shown in Table 1, the improved Bi-RRT* of the present invention reduces the average search time by 53.33%, the average number of nodes by 74.00%, and the average path length by 20.15% compared with the traditional RRT*. Compared with Bi-RRT*, the average search time is reduced by 50.60%, the average number of nodes is reduced by 76.23%, and the average path length is shortened by 7.48%.

[0130] The experimental environment was built on a shelf. A multi-jointed robotic arm, equipped with a gripper at its end, picked up a bottle of water as a payload. The scene used cardboard boxes to create a partitioned obstacle environment, defining the starting and target areas of the task and constraining the robotic arm's motion path.

[0131] This task requires the robotic arm to accurately transport the payload from the starting point, around obstacles, to the final destination. Given that the operating platform is a flat surface with no undulations, this process can be considered a planar path planning problem. Figure 14 、 Figure 15 and Figure 16 The complete operation process is recorded in sequence. Figure 14 is the starting position of the robot arm to grasp the object; Figure 15 is the process position of the robot arm moving in the obstacle avoidance path; Figure 16 The final position of the object when the robot arm successfully places it in the target area.

[0132] The specific implementation scheme described above further illustrates the purpose of the invention, technical solutions and beneficial effects of the present invention. The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the scope of protection of the present invention. Ordinary technicians in this field should understand that any modifications or equivalent replacements in the technical solutions of the present invention are included in the scope of protection of the present invention.

Claims

1. A robot arm path planning method considering collision escape adaptive mixed Gaussian, characterized in that: The following steps are involved: S1: Rasterize the map, determine the starting point and target point, initialize other parameters, and execute S2; S2: The loop is terminated when the preset maximum number of iterations is reached or the early stopping mechanism is triggered, otherwise S2.1 is executed; S2.1: Calculate whether the difference in the cumulative number of collisions between the starting tree and the target tree exceeds a preset threshold. If the threshold is not reached, randomly select a tree to be expanded and execute S2.

2. S2.2: The sampling model consists of a goal-oriented Gaussian component, an obstacle avoidance Gaussian component, a uniform exploration Gaussian component, and a collision escape Gaussian component, which can be expressed as: Where: k is the number of Gaussian components, x is the sampling point, w i is the weight of the i-th component, is the probability density function of the i-th component, μ i is the mean of the i-th component, ∑ i is the covariance of the i-th component; S2.3: Design the target-oriented component, the μ of this Gaussian component goal is a fixed value, set to the root node coordinate of the target tree, σ goal Used to adaptively adjust the sampling range of the Gaussian component: s goal =s init_goal ·max(d target / d overall ,c goal_sigma ) (2) Where: d target is the distance to the target, d overall is the straight-line distance between the starting point and the target point, d target / d overall is the current progress measure, σ init_goal is the initial standard deviation, γ goal_sigma is the minimum scaling factor; The goal-oriented component performs extensive exploration when the random tree is far from the goal, and performs fine connection and path optimization when the random tree is close to the goal, executing S2.4; S2.4: The obstacle avoidance component explores near obstacles and finds a path around them or through narrow passages. The variance of this component is σ obs_dynamic_val is a fixed value determined by the grid resolution: s obs_dynamic_val =grid×β obs_sigma (3) Where: grid is the side length of the grid unit, β obs_sigma is the scaling factor. Mean μ obs An adaptive strategy is used to select an obstacle related to the current task. If no obstacle meets the requirements, the closest obstacle is selected as the average: O radius ={o j ∈O all |d(X latest ,o j )≤R filter } (4) Where: O radius is the set of coordinates of the center points of the filtered obstacles, o j is the coordinate of the center point of the j-th obstacle, O all The set of coordinates of the center points of all obstacle units on the map, X latest is the coordinate of the node most recently added in the current tree, R filter The radius threshold for preliminary screening, d(X latest ,o j ) is used to calculate the distance from the current node to the obstacle o j The distance, O final is the candidate set filtered by radius and direction, is the direction vector from the latest node to the target, From the latest node to an obstacle o m The direction vector, T dot is the threshold of the cosine value of the angle between two directions, RandomChoice( final ) represents the final mean μ obs From the candidate set O final Randomly select an element from m is the center coordinate of the mth obstacle, To find the latest ,o j )The smallest o j , execute S2.5; S2.5: The collision escape component aims to implement a dynamic escape strategy that is activated during consecutive collisions. It maintains a queue of the most recent collision points and randomly selects a point from the queue as the mean, which is defined as follows: P coll ={p1,p2,…,p k } (7) μ coll =RandomChoice(P coll ) (8) Where: P coll is the collision point queue, p k is the kth collision point in the queue, μ coll is the sampling center of the collision escape Gaussian component, RandomChoice() is the random selection function; Standard deviation σ coll Set it to a smaller fixed value to ensure that the sampling points can be centered around the collision center and execute S2.6; S2.6: Uniform exploration component provides global exploration for the sampling process, with mean μ uniform is fixed at the center of the region, with a standard deviation σ uniform Set it to a larger value to ensure that the sampling points can cover the entire space and execute S2.7; S2.7: Weight adjustment involves the goal-oriented component weight w G , obstacle avoidance component weight w O , collision escape component weight w C , uniform exploration component weight w U ,The adaptive adjustment of these weights is divided into the ,priority from high to low into the case of continuous collision failure, ,case where the path has been found or is very close to the target, and ,case where the distance to the target is far away, and S2.8 is ,executed; S2.8: To determine the continuous collision failure situation, the following conditions must be met: Where: Counter collision count Record the number of times a new node was not successfully added, collision boost For the preset threshold, the RCP queue stores the information of the actual collision point; In this case, the collision escape component will be given a dominant weight w stuck , the weight of the goal-oriented component will be based on the scaling factor β stuck The weights of the uniform exploration component and obstacle avoidance component are also significantly reduced by their respective scaling factors β obs and β uniform It is significantly reduced and S2.9 is implemented; S2.9: To determine if a path has been found or if the target is very close, the following conditions must be met: Where: path_found is a Boolean flag, TO_GOAL threshold is the distance threshold; The influence of the uniform exploration component is determined by the factor λ uniform is reduced, the weights of the goal-oriented component and the obstacle avoidance component will be reduced by the factor λ target and λ obs The collision escape component weight remains unchanged and S2.10 is executed. S2.10: When judging the situation when the target is far away, the following conditions must be met: d target >(d overall ×FAR_GOAL threshold ×1.5) (11) Where: FAR_GOAL threshold is the long distance threshold, d target For long distance, d overall is the total distance; In the initial stage of exploration, all weights are kept at their initial values ​​to prevent prematurely falling into the local optimum, and S2.11 is executed; S2.11: Perform nearest neighbor search on the random tree to be expanded to obtain the nearest node and execute S2.12; S2.12: Based on Node X near Obstacle distribution in the neighborhood, calculate the ratio of obstacle area to area, and map the environmental coefficient η based on the ratio. Adjust the expansion step size according to the η value, and dynamically expand the step size: Step=Step old ×η (12) Where: Step old is the step size before adjustment, Step is the step size after adjustment, execute S2.13; S2.13: Calculate X rand With X near The vector of the connection direction and generate a new node X along this direction new , for X new Perform collision detection. If a collision occurs, it is recorded in the recent collision queue RCP and enters the next main loop iteration; otherwise, X new Add it to the current random tree as a new node and execute S2.14; S2.14: For new node X new Reselect and reconnect to shorten the path, and execute S2.15; S2.15: Sampling and expanding the starting tree, then sampling and expanding the target tree, until the two trees form a complete initial path, and then proceed to the next iteration. If a new candidate path is found, calculate its total length. If the path is shorter, update the optimal path and reset the counter; otherwise, add 1 to the counter until the count reaches the stable iteration threshold S. max When , terminate the main loop in advance and execute S3; S3: Prune the random tree. If the node is retained, the ancestor is also retained, and S4 is executed. S4: Use the greedy strategy to delete redundant nodes, shorten the path distance, and execute S5; S5: Smooth the path. If a collision is detected, the path before smoothing is used as the final output result.

Images