Adaptive sampling and multi-channel extension method for robot path planning

Abstract

The application belongs to the technical field of mechanical arm motion planning, and provides a kind of adaptive sampling and multi-channel expansion mechanical arm path planning method, first, the environment obstacle set is constructed, the point to obstacle distance, the clearance configuration and the segment level minimum clearance are established, and the unified safety criterion is constructed;Subsequently, according to the approximation state of bidirectional search tree, the sampling window is adaptively updated, and the random reference sample is generated in combination with the stagnation detection mechanism;On this basis, the hierarchical multi-channel expansion mechanism of direct pointing expansion, bypass expansion, potential field wall expansion and global bottom expansion is used to generate candidate new nodes;When a feasible new node is generated, it is connected to the current search tree and real meeting detection is carried out, if the double trees are successfully connected, path backtracking, splicing, simplification and full path safety review are further executed, and the final executable path is output.The application can improve the safety, search efficiency and environmental adaptability of the path planning of the mechanical arm in the complex limited environment.

Description

Technical Field

[0001] This invention belongs to the field of robotic arm motion planning technology, specifically relating to an adaptive sampling and multi-channel expansion robotic arm path planning method. Background Technology

[0002] With the development of robotics technology, multi-degree-of-freedom robotic arms are increasingly widely used in rehabilitation training, assisted therapy, and other scenarios. Especially in tasks such as bedside rehabilitation training, assistive movement of patient limbs, and collaborative operation of treatment equipment, robotic arms often need to operate in environments with limited space, dense obstacles, and high safety requirements. In such scenarios, robotic arms not only need to avoid fixed obstacles such as walls, equipment, and bed railings, but also need to consider the constraints of the surrounding safety zone to avoid potential collision risks to the patient. Therefore, high demands are placed on the safety, stability, and adaptability to complex environments of path planning methods.

[0003] Path planning is a crucial step for ensuring safe movement in robotic arms. In existing technologies, Rapid Expanding Random Trees (RRT) and its improved algorithms (such as RRT* and bidirectional RRT) are widely used in robotic arm path planning due to their suitability for high-dimensional space searches. Bidirectional RRT improves search efficiency to some extent by simultaneously expanding the search tree from both the start and end points. However, in rehabilitation and training scenarios, existing bidirectional RRT methods still have some shortcomings, such as inconsistent safety criteria, insufficient adaptability of sampling and expansion strategies to complex and constrained spaces, and inadequate verification of path reliability after connecting two trees. Therefore, it is difficult to simultaneously achieve a balance between safety, search efficiency, and stability requirements in clinical application environments.

[0004] Therefore, it is necessary to propose a robotic arm path planning method for rehabilitation training and assisted therapy scenarios to improve the safety, effectiveness and feasibility of path search in complex and constrained environments. Summary of the Invention

[0005] To address the aforementioned technical problems, this invention provides an adaptive sampling and multi-channel expansion method for robotic arm path planning, thereby resolving the issues in the prior art. The technical solution adopted by this invention is as follows: An adaptive sampling and multi-channel expansion method for robotic arm path planning includes: Step S1: Construct a unified security judgment system and feasibility criteria; initialize the bidirectional search tree; Step S2: At the start of each round of cyclic search, update the sampling state of this round based on the search progress of the current bidirectional search tree and generate random reference samples; Step S3: Based on a unified security determination system and feasibility criteria, perform multi-channel expansion in a fixed order to generate feasible nodes step by step; Step S4: Connect the generated feasible new node to the current expanded tree of the bidirectional search tree and perform a real meeting detection; if no real meeting is detected, return to step S3 to continue expansion; if a real meeting is detected, proceed to step S5. Step S5: Perform backtracking and splicing on the bidirectional search tree after the actual meeting to obtain the initial path; perform final review based on a unified safety judgment system and feasibility criteria, and output the final executable joint path after the review is passed.

[0006] Furthermore, in step S1, a unified security determination system and feasibility criteria are constructed, including: Step S1-1: Unify the environment and obstacle set: Map all obstacles that pose a collision risk to the robotic arm to the same workspace coordinate system to form an obstacle set; Step S1-2: Define the shortest distance from a point to an obstacle: Calculate the Euclidean shortest distance from any point in the workspace to the surface of each obstacle in the obstacle set, and take the minimum value as the shortest distance from the point to the obstacle set. Step S1-3: Define the configuration clearance function. The minimum value of the shortest distance from all points in the envelope point set occupied by the links of the robotic arm in the workspace under the current posture of the robotic arm to the obstacle set is used as the output value of the configuration clearance function. The output value of the configuration clearance function is used to measure the safety of the overall posture of the robotic arm to the obstacle. Step S1-4: Set the global minimum safe clearance threshold and discrete detection step size; Steps S1-5: Unifying the minimum clearance at the segment level and feasibility criteria: Based on the continuous movement process of the robotic arm, between the starting and ending configurations of the robotic arm's movement, the number of discrete configuration points is determined according to the distance between the two points and the discrete detection step size. Linear interpolation sampling is performed on the line connecting the two points in the joint space to obtain each discrete configuration point. The configuration clearance function value of all discrete configuration points is calculated and the minimum value is taken as the minimum clearance at the segment level for the line connecting the two points, which is used to characterize the safety margin at the most dangerous position in the line connecting the two points. The final feasibility criterion is: a connecting line is defined as a safe and feasible path segment if and only if the minimum clearance at the segment level is not lower than the minimum safe clearance threshold, as expressed as:

[0007] in: Indicates from arrive Candidate connections; and Configure any two postures of the robotic arm; The minimum safe clearance threshold; Indicates from arrive The minimum clearance of the entire line segment under discrete sampling.

[0008] Furthermore, in step S1, the bidirectional search tree is initialized, including: reading the starting point configuration and ending point configuration of the robotic arm movement, which are respectively used as the root nodes of the starting point tree and the ending point tree in the bidirectional search tree, and the configuration clearance function values ​​of the starting point configuration and the ending point configuration are not lower than the minimum safe clearance threshold.

[0009] Furthermore, step S2 includes: Step S2-1: Construct the tracking target and baseline distance: Take one of the bidirectional search trees as the current expansion tree and the other as the opposite tree; first, select a tracking target configuration from the opposite tree to guide the expansion direction of the current expansion tree and measure the approximation of the two trees; at the same time, construct the baseline distance with the distance between the root of the current expansion tree and the tracking target configuration. Step S2-2: Define the nearest distance and the current search progress: Calculate the Euclidean distance from all nodes in the current expanded tree to the target being tracked, and take the minimum value as the nearest distance from the current expanded tree to the target being tracked; calculate the current search progress based on the baseline distance and the nearest distance. The search progress is used to characterize the degree of approximation between the current expanded tree and the adjacent tree. Step S2-3: Constructing a sliding window and stagnation detection: Based on the single-step progress increment of each search round, the average progress is obtained by averaging the increments of the most recent rounds; then, a stagnation flag is generated through an indicator function. Step S2-4, Sampling Center Shift and Scale Shrinkage: Based on the current search progress and stagnation indicators, determine the sampling center and sampling scale for this round of sampling; Step S2-5, Sampling Window Set Construction and Trimming: Based on the robotic arm joint structure, combined with the sampling center and sampling scale, construct the set of valid sampling windows for this round; Step S2-6, Random Reference Point Generation: Construct low-difference sequence points and uniform random vectors, and linearly map them to the legal range of the current sampling window to obtain the covering samples and random perturbation samples within the window; then, through convex combination, the covering samples and random perturbation samples are weighted and fused to obtain random reference points.

[0010] Furthermore, in steps S2-3: when the average advance in the most recent rounds is lower than a preset stagnation threshold, the current search is determined to be in a stagnant state; when the average advance is greater than or equal to the stagnation threshold, the current search is determined to be in a normal advance state.

[0011] Furthermore, in steps S2-4, the determination of the sampling center and sampling scale includes: firstly, defining a forward shift function that monotonically increases with the search progress; based on the forward shift function, the root configuration of the current expansion tree, and the tracking target configuration, calculating the sampling center for this round. The sampling center is a 6-dimensional vector in the joint space of the robotic arm, which gradually shifts from the root configuration to the tracking target configuration as the search progress increases; nextly, defining an instantaneous target value for the sampling scale that decreases with the search progress, and ensuring that the instantaneous target value for the sampling scale is not lower than a preset lower limit; when a stagnation state is detected, temporarily amplifying the sampling scale to obtain a stagnation correction scale; the value of the sampling scale for this round is: if it is determined to be a stagnation state, the stagnation correction scale is used; if it is a normal advancement state, the baseline sampling scale is used.

[0012] Furthermore, steps S2-5 include: pre-setting the upper and lower limits of rotation for each joint of the robotic arm, calculating the half-length of the global movable range of each joint, and obtaining the global half-length vector; scaling the global half-length vector using the current sampling scale to obtain the window half-length vector of the current sampling window in each joint dimension; using the sampling center as a reference, calculating the initial upper and lower bounds of the sampling window in combination with the window half-length vector, and then combining the upper and lower limits of rotation of the robotic arm joints to perform element-by-element clipping on the initial upper and lower bounds to obtain the final upper and lower bounds of the sampling window, thereby determining the legal range of the current sampling window set.

[0013] Furthermore, step S3 includes: Step S3-1, Nearest Node Retrieval and Unified Expansion Framework: Based on a random reference point, retrieve the existing node in the current expansion tree that is closest to the random reference point in Euclidean distance, and use it as the neighboring node for this round of expansion; set the single-step expansion step size and unit direction vector, and generate candidate new nodes based on the neighboring nodes, the single-step expansion step size and the unit direction vector; the connection between the candidate new node and the neighboring node satisfies the feasibility criterion. Step S3-2: Construct a direct path to obtain candidate new nodes: Calculate the unit direction from the neighboring node to the tracking target configuration, substitute it into the unified extension framework to generate candidate new nodes for the direct path, and verify whether the connection between the candidate new node and the neighboring node meets the feasibility criterion; if the verification is successful, this round of extension ends and proceeds to step S4; if the verification fails, proceed to step S3-3. Step S3-3: Constructing an outer spherical shell bypass strategy to obtain candidate new nodes: With neighboring nodes as the center, construct a spherical shell candidate set in the 6-dimensional joint space of the robotic arm. Sample several candidate points within the candidate set, and score each candidate point for safety and propulsion consistency. Based on the two scores, construct a comprehensive score, select the candidate point with the highest comprehensive score, calculate the unit bypass direction from the neighboring node to the candidate point, substitute it into the unified expansion framework to generate candidate new nodes for the bypass channel, and verify the feasibility criteria again. If the verification passes, this round of expansion ends; if the verification fails, proceed to step S3-4. Step S3-4: Construct an artificial potential field for wall-hugging channel to obtain candidate new nodes: Construct a continuous directional field based on the combined force of attractive force, repulsive force and tangential force to generate the unit direction for wall-hugging propulsion. Substitute this into the unified extension framework to generate candidate new nodes for the wall-hugging channel and verify the feasibility criteria. If the verification is successful, this round of extension ends; if the verification fails, proceed to step S3-5. Step S3-5, Global Cubble Channel: Generate a global reference point within the legal range of the robotic arm joint limit, calculate the unit catch-up direction from the neighboring node to the global reference point, substitute it into the unified extended framework to generate candidate new nodes for the catch-up channel, and verify the feasibility criterion; if the verification passes, a feasible new node is obtained; if the verification fails, no feasible new node is generated in this round, S3 ends and returns to step S2, and the sampling window and random reference sample for the next round are updated again.

[0014] Furthermore, S4 includes: Step S4-1, New Node Access and Local Neighborhood Optimization: The generated feasible new node is taken as the node to be accessed. A group of candidate neighboring nodes is selected near the node to be accessed. Nodes whose connections to the node to be accessed meet the feasibility criteria are filtered out. Among the filtered nodes, the node with the smallest sum of cumulative path cost from the root to the filtered node and connection cost from the filtered node to the node to be accessed is selected as the parent node of the node to be accessed. After the parent node is selected, the feasible new node is written into the current expanded tree, and the parent pointer and cumulative cost information of the feasible new node are recorded. Step S4-2, True Meeting Detection: In the opposite tree, search for the true node that is closest to the newly joined feasible node, and determine the true meeting of the bidirectional search trees if at least two of the following conditions are met: The distance between a feasible new node and the nearest real node does not exceed the preset rendezvous connection distance threshold; The connecting line segment between the two satisfies the feasibility criterion; If a true meeting is determined, proceed to step S5; if the meeting condition is not met, the current iteration ends, return to step S2, re-update the sampling state and generate random reference samples based on the updated tree structure, and continue to execute steps S3 and S4 to continue expansion.

[0015] Furthermore, step S5 includes: Step S5-1, Path Extraction and Simplification: After the actual meeting, backtrack to the root node along the parent pointers in the extended tree and the opposite side tree respectively to obtain two branch paths: the starting side and the ending side. Reverse the ending side branch and concatenate it with the starting side branch to obtain the complete discrete path from the starting point to the ending point. Perform local smoothing. The smoothed local path segments need to be re-verified. If the feasibility criteria are not met, revert to the unsmoothed version. Step S5-2, Final review and output of the entire path: Perform feasibility criterion review on all adjacent path segments of the final path segment by segment. When the entire path meets the minimum safe clearance threshold, it is output as the final executable joint path.

[0016] The present invention has the following beneficial effects: (1) By establishing a unified safety criterion throughout the entire process, this invention achieves consistent safety verification of the robotic arm path search, tree connection and final path review, which is conducive to improving the reliability of path planning results.

[0017] (2) This invention uses a progress-driven adaptive sampling strategy and a stagnation detection mechanism to enable the sampling range in the robotic arm path planning to be dynamically adjusted with the bidirectional tree approximation state, thereby taking into account both global exploration capability and local focusing capability, and improving search efficiency in complex environments.

[0018] (3) The present invention adopts a hierarchical multi-channel expansion mechanism, which can switch to bypass, potential field wall-hugging and bottom-loop expansion modes in sequence when the robot arm is blocked from direct expansion, thereby enhancing the algorithm's adaptability to obstacle-dense environments and narrow channel scenarios.

[0019] (4) This invention improves the safety and executability of the final path generated by the robotic arm through the real meeting judgment with security verification and the full path review mechanism, and is more suitable for application scenarios with limited space and high safety requirements, such as rehabilitation training and auxiliary treatment. Attached Figure Description

[0020] Figure 1 This is a flowchart of the present invention; Figure 2 A two-dimensional projection diagram showing the temporary enlargement of the sampling window after a pause detection is triggered; Figure 3 A two-dimensional projection diagram illustrating the shift of the window center and the shrinkage of the window scale as the sampling window changes with the search progress. Figure 4 A two-dimensional projection diagram of the bypass strategy of a local high-dimensional spherical shell (hereinafter referred to as "spherical shell"), wherein the candidate domain of the spherical shell appears as a ring shape under two-dimensional projection; Figure 5 A two-dimensional projection diagram of an artificial potential field channel attached to a wall. Detailed Implementation

[0021] The following will be described in conjunction with embodiments of the present invention. Figures 1-5 The technical solutions in the embodiments of the present invention will be clearly and completely described. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Unless otherwise specified, the technical means used in the embodiments are conventional means well known to those skilled in the art.

[0022] This invention addresses the collision-free path planning problem for robotic arms in obstacle-dense environments, aiming to configure a robot at a given starting point. With the destination configuration Under these conditions, a joint space path is generated that satisfies both obstacle avoidance requirements and is executable. Unlike general path searches that only focus on connectivity, this implementation emphasizes a unified safety judgment standard throughout the entire process: any candidate connection between two configurations must satisfy a minimum clearance of no less than a threshold at discrete sampling points. This unified criterion is not only used for the initial search, but is applied throughout all stages, including expansion, access, reconnection, reconnection, pruning of direct connections, smooth replacement, and final full-path review. This avoids inconsistencies where a possibility is initially deemed feasible but later rejected due to different judgment criteria.

[0023] From the perspective of implementation sequence, this invention can be summarized into five stages. Step S1 first fixes the environmental modeling criteria, establishes the obstacle set representation, the clearance calculation method, and a unique feasibility criterion for the entire text; Step S2 then updates the current sampling window and generates random reference samples based on the approximation state of the bidirectional tree; Step S3 performs multi-channel expansion in a fixed order under a unified criterion, generating feasible new nodes level by level; Step S4 connects the new nodes to the tree structure and determines whether they have truly merged with the opposite tree; Step S5 performs backtracking, splicing, simplification, and final verification on the merged bidirectional tree path, outputting an executable joint path that meets safety constraints. This implementation first establishes a unified safety criterion in Step S1, and then allows subsequent steps to revolve around this criterion.

[0024] like Figure 1 The overall logical structure of this invention can be summarized as follows: First, in step S1, environmental modeling, unified security criterion establishment, and bidirectional search tree initialization are completed; then, the cyclic search phase begins. In step S2, the sampling window for this round is updated according to the current search progress, and random reference samples are generated. In step S3, candidate new nodes are generated in a fixed order of direct expansion—bypass expansion—potential field adhesion—global fallback, and the candidate path segments are checked using the unified security criterion. If no feasible new node is generated, the process returns to step S2 for resampling. If a feasible new node is generated, the process proceeds to step S4, where it is added to the current search tree, and it is checked whether it truly merges with the opposite tree. If they do not merge, the process returns to step S3 to continue expansion. If they successfully merge, the process proceeds to step S5 for path backtracking, splicing, simplification, and final security verification. If the verification fails, the process returns to step S3 to search again. If the verification passes, the final executable path is output. This invention specifically includes the following steps: Step S1: Scene Modeling and Two-Way Initialization Objective: To standardize environmental criteria, establish unified clearance calculation and feasibility judgment standards, and provide a consistent safety verification interface for all subsequent steps. Step S1 of this invention is the foundation for the safety definition of the entire method. Although subsequent steps S2 to S5 undertake different responsibilities such as sampling, expansion, access, convergence, and path output, they must all call the same set of clearance and feasibility definitions given in S1.

[0025] Step S1 specifically includes: Step S1-1: Unify the set of environment and obstacles: First, bed railings, walls, equipment, restricted areas for human access, and other obstacles that may pose a collision risk to the robotic arm are unified into the same workspace coordinate system to form an obstacle set. The unification of this invention is not simply about placing obstacles in the same diagram, but rather requiring them to be consistently invoked and calculated under the same geometric datum. To make subsequent judgments more consistent with real engineering conditions, the link radius, installation calibration error, control tracking error, and necessary safety margin can be uniformly incorporated into the safety expansion processing of the obstacle set; its technical advancement lies in the fact that subsequent steps no longer separately consider ideal geometric collision and engineering uncertainty compensation, but uniformly determine whether the clearance is not less than This reflects safety requirements. The output of step S1-1 is not merely a list of obstacles, but a unified obstacle representation that has incorporated necessary safety redundancy. All subsequent steps, by calling this obstacle set, implicitly consider both environmental geometry and engineering safety margins, thus ensuring consistency in safety assessment criteria.

[0026] Step S1-2: Define the shortest distance from the point to the obstacle: In a unified set of obstacles Next, define any point in the workspace. The closest distance to the set of obstacles. This quantity is the underlying basis for subsequent configuration clearance and segment-level minimum clearance. Defined as follows:

[0027] Parameter description: For workspace points; For the set of obstacles; A single obstacle within a set of obstacles; For point To the obstacle Euclidean nearest distance on the surface. The purpose of equation (1) is to clearly quantify how far a point is from the nearest obstacle. Since the overall safety of the robotic arm is essentially determined by a large number of workspace points, to determine whether a posture is safe, it is necessary to first calculate whether a point is close to an obstacle.

[0028] Step S1-3: Define and configure the net void function : The robotic arm is configured at a certain joint Below, it is not a single point, but an entity with volume and spatial footprint. Therefore, let the robotic arm be configured at its joints... The set of outer envelope points of the connecting rod is as follows This point set is determined by the robotic arm's geometric model and forward kinematics, and already includes the safety expansion shape described in step S1-1. Therefore, the distance from a single point to the obstacle is extended to a safety metric of the overall attitude towards the obstacle, defining a configuration clearance function:

[0029] Parameter description: This is the 6-dimensional joint angle vector of the robotic arm; This represents the set of envelope points occupied by the robotic arm links in the workspace under the current posture. This is the distance from the obstacle to the most dangerous point in the current posture, and all subsequent safety decisions are uniformly invoked through it. In this invention, whenever a judgment is made on whether a discrete configuration point is safe, this clearance function is uniformly invoked, which ensures that the safety judgment criteria are the same regardless of whether the configuration point appears in the sampling expansion stage, the path smoothing stage, or the final verification stage.

[0030] Step S1-4: Define the global minimum safe clearance threshold and the discrete detection step size: After defining the configuration clearance, this invention then sets a global minimum safe clearance threshold as follows: This is used to represent the minimum permissible safety margin; simultaneously, the discrete detection step size is set to... This is used to control the resolution when performing discrete sampling verification on candidate connections.

[0031] in, Its purpose is to provide a unified safety lower bound across the entire text. All subsequent checks on the feasibility of any edge ultimately require verifying whether the clearance at the most dangerous point of that edge is still not less than [a certain value]. The judgment. Its function is to transform a continuous connection into an inspection problem on a finite number of discrete sampling points. The smaller the value, the more detailed the review, and the less likely it is to miss local collisions in narrow channels, but the computational load will also increase. The larger the value, the faster the calculation, but the ability to distinguish small, localized collisions decreases. Therefore, these two quantities determine the lower safety limit and the inspection accuracy, respectively, and together constitute the basic parameters for subsequent segment-level feasibility judgments.

[0032] Steps S1-5: Unifying the minimum clearance at each segment level and determining feasibility criteria: Practice has shown that simply verifying the safety of the starting and ending point configurations is insufficient. During actual robotic arm movement, collisions with obstacles may occur at some point in the middle. Therefore, this invention elevates the safety assessment from a single posture to the entire continuous motion process. Specifically, it first determines the number of sampling points to be checked based on the distance between the two endpoints and the discrete detection step size. Then, it calculates the clearance for each discrete configuration on the segment, and finally takes the minimum value as the segment-level minimum clearance. Thus, the feasibility of a candidate edge no longer depends on the safety of the endpoints, but on whether the uniform threshold requirement is consistently met throughout the entire motion process.

[0033] Therefore, this invention, based on the continuous movement process of the robotic arm, determines the number of discrete sampling points between the initial starting configuration and the final ending configuration of the robotic arm's movement. For any two configurations... and First, based on the joint spatial distance and discrete detection step size between them... Determine how many discrete sampling points need to be checked for this connection:

[0034] Parameter description: It is a norm 2; To round up; This represents the number of discrete check steps for the candidate connection segment.

[0035] Subsequently, linear interpolation sampling is performed on the connection between the current discrete points in joint space to obtain the first... Discrete configuration points:

[0036] Parameter description: From arrive The first online A discrete configuration point; This is the sampling index.

[0037] For each of these discrete configuration points, call the configuration clearance function in equation (2), take the minimum value, and define the segment-level minimum clearance of the connection segment:

[0038] Parameter description: Indicates from arrive The minimum clearance of the entire connected line in the sense of discrete sampling is represented as the safety margin still retained at the most dangerous position in this path.

[0039] Therefore, the only feasibility criterion for this invention is:

[0040] Parameter description: Indicates from arrive Candidate connections; Equation (6) shows that an edge is only valid if the minimum clearance is not less than a certain value during the entire discrete sampling process. Only when it is considered safe and feasible. In this invention, whether it is the new edge extended in step S3, the new connection formed by access and reconnection in step S4, the inter-tree connection when the real meeting occurs, or the pruning of direct connection and smooth replacement in step S5, whenever there is a connection from one configuration to another, equation (6) must be satisfied.

[0041] Step S1-6, Initialization of the bidirectional tree: Read starting point configuration With the destination configuration , respectively serving as the starting point tree With the endpoint tree The root nodes. Since all subsequent expansions are built upon safe root nodes, these two root nodes themselves must first meet the minimum safe clearance requirements:

[0042]

[0043] Parameter description: Configuration for the starting joint; Configuration for endpoint joints; Equations (7) and (8) of the present invention ensure that bidirectional search is under a uniform safety caliber from the outset.

[0044] In step S1 of the present invention, step S1-1 provides a unified obstacle set. Its safety expansion representation; step S1-2 defines the shortest distance from a single point to an obstacle; step S1-3 generalizes it to a single-configuration net clearance function. Steps S1-4 define the minimum safe clearance threshold. With discrete detection step size Steps S1-5 further define the minimum clearance at the segment level. And the unique feasibility criterion (6); finally, steps S1-6 complete the bidirectional tree initialization under this unified criterion.

[0045] Step S2: Update the sampling state and generate random reference points for this round: Objective: Based on the current progress of the bidirectional search, determine the tracking target to be used in each round of expansion before it begins. Sampling Center Sampling window and random reference samples This provides direct input for the current round of the unified multi-channel expansion in step S3. It should be noted that there are two types of windows in this step, but they serve different purposes: the first type is the sliding window length. It only works on the progress sequence. The first type is used to determine whether recent rounds of searching have been insufficient; the second type is the sampling window. It is the joint space. The geometric sampling region actually used in the current round is used to limit the range of random reference samples generated in this round. The former is a statistical observation interval, and the latter is a geometric sampling region. Both serve to generate more stable expansion directions in the future.

[0046] Step S2 specifically includes: Step S2-1: Construct the distance between the tracking target and the baseline: When a tree is in its current expanded state, first select a tracking target configuration from the opposite tree. This is used to guide the expansion direction of the current tree and measure its approximation. The tracking target here is only used for guidance and progress calculation, and is not equivalent to the final actual meeting point. At the same time, a baseline distance is constructed using the distance between the current tree root and the tracking target. Its purpose is to provide a unified and normalized scale for subsequent progress definitions, so that the search status under different start and end point distance scales can be consistently compared.

[0047]

[0048] Parameter description: Configure for tracking targets; Configure the current root tree; This is a correction term, which is a very small positive number to avoid the denominator being 0; The baseline distance is used to normalize subsequent distances to a comparable scale.

[0049] Step S2-2: Define the nearest distance and progress Π; This invention obtains the tracking target Next, we need to determine how far the current tree has progressed relative to it. The most direct approach is to observe which of the existing nodes in the current tree is closest to it. Therefore, the shortest distance from the current tree to the tracked target is defined. :

[0050] Parameter description: Let the set of nodes in the current tree be the set of nodes; Equation (10) takes the distance in the tree. Distance to the nearest node.

[0051] Based on this, define the progress of this round of search. :

[0052] Parameter description: The current search progress Π represents how close the current expanded tree is to the tracked target. ;when Approaching 0 A value close to 1 indicates that the search is nearing the potential meeting region. At this point, sampling should focus more on local areas to improve the efficiency of meeting and traversing narrow channels; when hour If the distance between the two trees is pruned to near 0, it indicates that the two trees are still far apart. In this case, the search should retain a stronger global exploration capability. Equation (11) of this invention is an approximation index. It does not represent the actual path length or the number of tree nodes, but rather represents the degree to which the current tree has approached the target relative to the initial reference distance. The subsequent sampling center in this invention is moved forward and the sampling scale is shrunk, both based on the search progress.

[0053] Step S2-3: Constructing a sliding window and stall detection: If at a certain moment A large sample size does not necessarily indicate that the recent search has been consistently making effective progress. There are exceptions: the current tree may have entered the vicinity of a complex obstacle region, but has not made effective progress in recent rounds. If this exception is not identified, the sampling window may continue to shrink excessively, causing the search to stall. Therefore, step S2 also needs to determine whether effective and continuous progress has occurred in the most recent period.

[0054] First define the first single-step progress increment of the wheel :

[0055] Parameter description: For the first The progress value of each iteration; This represents the amount of progress made by the current wheel relative to the previous wheel. Equation (12) of this invention transforms the current progress state into the progress between two adjacent wheels. If only considering... This itself may lead to special cases of search lag; therefore, this invention introduces... It can directly identify whether recent rounds have been effectively advanced or have essentially stalled.

[0056] Furthermore, regarding the recent The average propulsion is obtained by averaging the increments of the wheels. :

[0057] Parameter description: The length of the sliding window; Indicates recent The average propulsion speed of the wheel. Here, a moving average is used instead of looking at a single speed. This is because the amount of propulsion per round is affected by random sampling fluctuations. If only one round is considered, normal fluctuations can easily be misjudged as stagnation; however, considering the most recent round... The average value of the rounds more stably reflects whether the current search is not progressing enough over a period of time. The above refers to the sliding window length. Effect on In the sequence, the explanation used to determine whether there has been insufficient progress in recent rounds is precisely the meaning of this average progress.

[0058] Then through indicator functions Generate stagnation flag :

[0059] Parameter description: The stagnation threshold; if This indicates that the average advance in recent rounds has fallen below a threshold, suggesting that the current search has entered a state of local obstruction or sluggish progress; if If the reading is positive, it indicates that progress is still normal. If a standstill is detected, this step will temporarily enlarge the sampling window in subsequent scale update phases to broaden the sampling range of the current round and increase the probability of escaping local obstruction. For example... Figure 2 As shown, the search tree starting from the initial configuration enters a concave obstacle region during expansion, resulting in a restricted expansion path. The crosses indicate collision points generated during expansion. Within the normal sampling window, random sampling points are mainly distributed in localized areas, making it difficult to provide effective escape directions, thus causing the search process to stall. Upon detecting a stall, the sampling window is temporarily enlarged to expand the sampling range, allowing random sampling points to cover a wider area, thereby guiding the search tree out of the concave obstacle region and continuing to expand towards the target configuration.

[0060] Step S2-4, Sampling Center Shift and Scale Contraction: Step S2-4 of this invention transforms the abstract progress Π into concrete sampling geometry. As Π increases, the sampling center no longer remains near the tree roots but gradually moves forward towards the target being tracked; simultaneously, the sampling scale gradually contracts from a large global exploration range to a smaller local focus range. The purpose of this design is to maintain coverage in the early stages of the search and improve convergence accuracy and local obstacle avoidance capabilities in the later stages.

[0061] First, define a monotonically increasing forward shift function:

[0062] Parameter description: Forward shift index; .when When smaller, Smaller, the sampling center is more biased towards the tree roots; when When it is large, The sampling center is moved closer to the target as the sample size increases. Equation (15) of this invention converts the current search progress into how much the sampling center should be moved forward. This determines how quickly the curve moves forward. If If, then the forward shift is linear; if In the early stages, the movement is slower, and in the later stages, it is faster. That is to say, It controls when the search clearly shifts from global exploration to local focus.

[0063] Sampling Center for:

[0064] Parameter description: This is a 6-dimensional vector in joint space, representing the location of the center of the current sampling window. and At some point in between, and with The increase gradually moves from near the tree root to near the tracking target. The core concept of this invention (16) is: in the early stage of the search, the center of the window is closer to the current tree root to maintain a sufficient global exploration range; in the later stage of the search, the center of the window... Gradually towards To increase local sampling density near the meeting area, such as... Figure 3 As shown, , , These are the window centers for the three stages shown in the illustration. The sampling window centers gradually move from the root to near the target as the progress progresses, corresponding to equation (16).

[0065] After determining the sampling center, the sampling scale also needs to be determined. Therefore, the instantaneous target value of the sampling scale is defined. :

[0066] Parameter description: This is the lower limit of the scale; The shrinkage coefficient; Follow Increasing while decreasing indicates that as the search gradually approaches the meeting region, the sampling range should decrease from large to small; among which, .

[0067] When stagnation is detected, the smoothed scale is temporarily enlarged to obtain the stagnation correction scale. :

[0068] Parameter description: This is the magnification factor; Ensure the scale does not exceed 1. The scale actually used in this round. Defined as: if Then take Otherwise take The stagnation amplification of this invention is for redefining the actual sampling window used in the current round within S2.

[0069] Step S2-5, Sampling Window Construction and Cutting: In this round of sampling centers and the actual sampling scale in this round Once determined, the geometric sampling area actually used in this round is constructed based on the two. Also, because each joint of the robotic arm has physical upper and lower limits, any sampling window must be constrained within the legal joint range.

[0070] Let the lower and upper limits of the joint be respectively and First, define a global half-length vector. :

[0071] Parameter description: The element-wise half-range represents half of the global movable range of each joint. Equation (20) of this invention provides... This is equivalent to a set of reference half-widths, which describes the maximum scale benchmark available for sampling each joint dimension in a global sense.

[0072] Using the actual scale of this round again Scale it and define the half-length vector of the window. :

[0073] Parameter description: This is the half-width of the sampling window in each joint dimension for this round; The smaller, The smaller the window, the narrower it is; The larger the value, the wider the window. The core idea of ​​this invention (21) is that the value calculated in step S2 is... This is now transformed into a specific geometric scale. Previously... It is still a dimensionless scaling factor; in equation (21), multiply by Then, it is converted into the actual sampled half-width in each joint dimension. Therefore, the aforementioned scale follows... The rule of temporarily enlarging when contraction stalls can be reflected in the window size in this round.

[0074] Since the sampling window cannot exceed the joint limit, element-by-element clipping is required at the window boundaries. (Window lower bound) With the upper realm They are defined as follows:

[0075]

[0076] Parameter description: This indicates taking the larger value for each element. This indicates taking the smallest value for each element; and These are the lower and upper bounds of the sampling window for this round, respectively. Equations (22) and (23) ensure that the sampling window generated in step S2 not only adapts to the progress but also remains valid. Therefore, the sampling area obtained in step S2 will not exceed the joint range that the robotic arm can achieve due to the forward movement of the sampling center or the stagnation of the zoom.

[0077] Therefore, the sampling window set for this round Defined as:

[0078] After completing this step, the next step is to generate... Strictly subject to This constraint ensures that sampling always occurs within a legal region that adapts to changes in progress.

[0079] Step S2-6 Random Reference Point generate: In the sampling window Once this is determined, the final task of step S2 is to generate the current round of random reference samples within this window. This invention does not directly employ a single, purely random sampling method, but rather uses a mixture of two types of samples. This is because path planning requires both coverage and perturbation. Coverage alone would result in overly regular samples, lacking the ability to break out of local structures; perturbation alone might lead to over-clustering of samples, affecting search stability. Therefore, this method generates two types of standardized samples simultaneously in step S2, and then maps them into the current window for mixing.

[0080] make Low-discrepancy sequence points represent relatively uniformly spread coverage samples; let Let be a uniform random vector, representing an exploratory sample with random perturbation; This represents element-wise multiplication. The two classes of samples within the window are then defined as follows:

[0081]

[0082] in, unit hypercube Low-discrepancy sequence points within the window are used to enhance the uniformity of sample distribution within the window; unit hypercube The uniform random vector within is used to provide the necessary random perturbation; Equations (25) and (26) essentially linearly map the standardized samples to the current sampling window. Inside. It should be noted that in equation (25) In the sum (26) These are now actual joint configuration points within the window, rather than standardized points.

[0083] Finally, the final random reference point is obtained using convex combination. :

[0084] Parameter description: For mixed weights; when When it is large, More inclined towards coverage sampling; when When smaller, It leans more towards random perturbation sampling. (Using this consistently throughout the text.) This represents a random reference sample to avoid inconsistencies in sign.

[0085] In step S2 of this invention, the approximation degree of the current tree relative to the tracking target is first determined by equations (9) to (11). Then, use equations (12) to (14) to determine if there is insufficient progress in the most recent rounds; subsequently, use equations (15) to (19) to adjust the progress. The stagnant state was transformed into the sampling center of this round. Compared with the actual sampling scale in this round Then, the valid sampling window for this round is constructed using equations (20) to (24). Finally, random reference samples for this round are generated within this window using equations (25) to (27). In other words, step S2 essentially updates the sampling state and generates random reference samples for this round. At the end of step S2, the core results output to step S3 can be summarized into three items: tracking the target. Sampling window for this round and random reference samples Step S3 proceeds with unified multi-channel expansion in a predetermined order: first, direct targeting; if that fails, detour; if that fails again, the potential field adheres to the wall; if that still fails, then global fallback is initiated.

[0086] Step S3: Multi-channel boot extension In step S3 of this invention, under the unified feasibility criterion (6), the advancement direction is constructed step by step until a new accessible node is obtained. Step S3 is used to actually generate new nodes under the constraints of a unified feasibility criterion. It first determines which node in the tree to start from, and then constructs the direction based on the current channel. Generate candidate new nodes Finally, the segment-level clearance is checked using equation (30). If it fails, the safety criterion is not changed, only the direction generation method is switched.

[0087] Based on step S2, the target for this round of tracking has already been given. Current sampling window and random reference sample S3 directly executes a unified multi-channel expansion process. Its fixed order is: first, retrieve an actual expansion starting point from the current tree. Then, it first attempts to directly navigate through the channel; if the newly generated edge fails to pass the unified segment-level clearance criterion, it switches to circumnavigating around the outer spherical shell; if discrete circumnavigation still cannot form a feasible new edge, it enters the artificial potential field wall-hugging channel, using continuous direction fields for local escape; if this still fails, it activates the global fallback channel to actively change the current search situation. It should be noted that in step S3, only the direction changes between the channels. How to construct it, and ensure that the new node always meets the unified security judgment system and feasibility criteria.

[0088] Step S3 specifically includes: Step S3-1, Nearest Node Retrieval and Unified Extension Framework: At the beginning of step S3, the random reference sample generated in step S2 for this round is first used. In the current tree Find the existing node with the closest Euclidean distance in the search, denoted as . This node is not a special point on the final path, but merely the actual starting point for this round of expansion. This is because step S2 provides... This is merely a reference position that should be prioritized in this round. For the tree to truly grow, it must first be mapped to an executable starting node on the tree. Therefore, we define:

[0089] Parameter description: For the current tree and The nearest node; This indicates selecting the node that minimizes the distance.

[0090] get After that, regardless of which channel is used subsequently, the generation format of the new candidate nodes in this round will remain consistent.

[0091] Let the single-step expansion step size be... Candidate directions are unit vectors ,satisfy Then, the candidate new node is uniformly written as:

[0092] Parameter description: This is a candidate new node for this round; To extend the step size in a single step; The unit direction given for the current channel.

[0093] The above formula shows that, in step S3, all channels essentially provide a direction. Then along that direction from Take a step forward Therefore, the only difference between the channels in step S3 is the direction.

[0094] After candidate nodes are generated, they must be immediately reviewed using the unified segment-level minimum clearance criterion already established in S1. Definition:

[0095] Parameter description: Equation (30) is a direct reuse of Equation (6) in S1 in S3; Indicates from arrive The minimum clearance of this connection at discrete sampling points; This is the global minimum safe clearance threshold. Equation (30) of this invention shows that, regardless of direction... Whether the direction is direct, outer spherical shell, potential field wall, or global bottom, as long as the corresponding candidate edge cannot satisfy equation (30), the direction is considered a failure, the current channel ends and the next channel is switched.

[0096] Step S3-2: Construct a direct channel: Each round of expansion first uses pointing to the tracking target. The direction pointing directly to the unit is taken as the primary direction of advancement, directly from... Pointing to the tracking target ,like Figure 3 As shown. Therefore, the direction pointing directly to the unit is defined as:

[0097] Parameter description: From point to The unit direction. Substituting into equation (29), we can obtain the candidate new nodes under the direct path; then we check whether the candidate edge is safe according to equation (30). If it passes, it means that the current local area allows direct advancement along the target direction, and this round of expansion can end and enter S4; if it fails, it means that taking a step directly from the current node towards the target will touch an obstacle in the middle process or make the clearance lower than the threshold. At this time, we can no longer continue to rush, but must turn to the next channel to detour. The purpose of the direct path is to quickly distinguish whether the current local area can still advance directly towards the target. If it is feasible, then the subsequent complex channels do not need to be activated; if it is not feasible, then step S3 clearly knows that the straight target path is blocked by the local structure, so it is necessary to enter the detour logic.

[0098] Step S3-3: Construct an outer shell bypass strategy to obtain candidate new nodes: When a direct path fails, it indicates that from Directly pointing While the current local direction may not be feasible, this doesn't mean we must completely abandon our goal orientation. A more reasonable strategy at this point is: Within a finite spherical region, search for alternative directions that bypass the current blocking direction while still generally pointing towards the target. For example... Figure 4 ,by Construct a candidate set of spherical shells centered on a 6-dimensional joint space. :

[0099] Parameter description: For the candidate set of outer spherical shells; and These are the inner and outer radii of the spherical shell, respectively, and can usually be determined by... Given, among which With the center radius, The thickness of the spherical shell; at the same time, the candidate points must still be located within the current sampling window. The internal structure satisfies joint limit constraints.

[0100] from Medium sampling Candidate points For each candidate point, the following must be evaluated simultaneously: first, the safety of the candidate point itself; second, whether the detour direction of the candidate point still maintains a certain target advancement trend. Based on this, a safety score is constructed and defined.

[0101] Parameter description: ; Configuration clearance defined for S1; For reference to the upper limit of the net space, used for normalization; Used to limit the score Within the range. The higher the score, the safer the candidate point itself.

[0102] First, calculate the cosine similarity between the candidate direction and the target direction:

[0103] Then map it to Interval:

[0104] Parameter description: The angle between the candidate direction and the target direction; The closer the candidate point is to 1, the closer its detour direction is to still moving towards the target; after mapping .

[0105] Next, a weighted sum is used to construct a comprehensive score:

[0106] Parameter description: A trade-off coefficient between safety and propulsion; The larger the value, the more likely it is to select candidates with a higher safety margin; The smaller the value, the more likely it is to select candidates that still maintain a goal-oriented approach.

[0107] Therefore, the candidate point with the highest score is selected. :

[0108] And based on this, the direction of the outer spherical shell's orbit is constructed:

[0109] Parameter description: The candidate point with the highest overall score for the spherical shell; For the reason point to The unit direction. Then let Substitute into equation (29) and verify again using equation (30). If it passes, it means that step S3 has successfully maintained a certain target propulsion while leaving the blocked direction locally, and this round of expansion ends; if it still fails, it means that the feasible direction cannot be hit by relying on only a limited number of discrete spherical shell candidate points. At this time, we should further enter the potential field wall channel and use the continuous direction field to perform gliding propulsion near the obstacle boundary.

[0110] Step S3-4: Construct an artificial potential field channel to obtain candidate new nodes: When both direct approach and outer spherical shell approaches fail, it indicates that the current local obstacle structure is quite complex: it's neither possible to directly move towards the target nor to bypass it using a small number of discrete candidate points. At this point, S3 no longer relies on a single discrete candidate direction, but instead uses a continuous direction field to generate an executable obstacle-skimming propulsion direction. This direction consists of three forces: attraction, repulsion, and tangential force. The attraction force maintains the overall tendency to propel towards the target and retains attraction to random reference samples to avoid local dead zones; the repulsion force pushes the path away from the obstacle; and the tangential force transfers part of the propulsion trend into the obstacle's tangential plane to reduce push-back oscillations.

[0111] First, define two types of attraction directions. The first type is the attraction direction towards the target being tracked:

[0112] The second type is the direction of attraction toward the random reference sample:

[0113] Therefore, the net attractive force is defined as:

[0114] Parameter description: Let be the attraction vector in joint space; These represent the attraction weights of the tracking target and the random reference, respectively. (Reserved) The purpose is to prevent the directional field from becoming overly rigid in the attraction of a single target, thereby making it easier to form an alternative propulsion trend in local complex areas.

[0115] First, calculate the most dangerous point in the outer envelope of the robotic arm links. :

[0116] Then find the obstacle that minimizes that distance. :

[0117] And find the distance on the surface of the obstacle. nearest point :

[0118] This allows you to define unit directions away from obstacles within the workspace:

[0119] Parameter description: This is currently the most dangerous point in the outer envelope of the robotic arm's linkage; The nearest obstacle; The nearest point on the surface of the obstacle; The direction of the unit away from the obstacle in the workspace.

[0120] Since the expansion in step S3 is performed in joint space, it is also necessary to map the workspace away from the joint space. Let For point The corresponding Jacobian matrix is ​​then defined as follows:

[0121] Parameter description: It is the unit normal direction in the joint space that is farthest from the obstacle.

[0122] Define the current clearance distance as Let the radius of the repulsive force be... This means that the repulsive force only takes effect when the distance to the obstacle is less than this value. Define the repulsive force kernel function:

[0123] Based on this, the repulsion vector is defined as follows:

[0124] Parameter description: This is for repulsive force gain; if the front placement is far from the obstacle, When the target is close to an obstacle, the repulsive force is ineffective; when the target is close to an obstacle, the repulsive force increases rapidly, pushing the search direction away from the vicinity of the obstacle.

[0125] To avoid numerical instability caused by excessively large repulsive force amplitude, it is limited, resulting in:

[0126] Parameter description: Indicates when At that time, the vector is scaled proportionally to the magnitude. ; This represents the upper limit of the repulsive force amplitude.

[0127] Attraction and repulsion alone may still result in being pushed away and then pulled back by the target, therefore a tangential gliding phase is also necessary. for The identity matrix is ​​defined as the projection matrix of the tangent plane.

[0128] The tangential force is then:

[0129] Parameter description: Tangential coefficient; The physical meaning is to project the resultant attractive force onto the plane. Within the tangent plane of the normal direction, the propulsion tendency is more manifested as sliding along the obstacle boundary rather than crashing into the obstacle head-on again.

[0130] Ultimately, as Figure 5 The total directional field is obtained by superimposing the three forces:

[0131] And normalize to obtain the unit direction of the potential field channel attached to the wall:

[0132] Then let Substitute into equation (29) and then check according to equation (30). If it passes, it means that step S3 has found a continuous direction of advancement near the local complex obstacle that neither directly hits the obstacle nor completely loses the target guidance; if it still fails, it means that the current local structure is extremely difficult to solve through local geometric correction. At this time, it is not very meaningful to continue to be limited to the current local area. We should enter the global fallback channel to actively change the search situation.

[0133] Step S3-5, Comprehensive Coverage Channel: When all three channels—direct pointing, outer spherical shell, and potential field adhering to the wall—fail, it indicates that the current local region can no longer rely on local geometric corrections near the target to escape. At this point, step S3 needs to actively expand the field of view, change the search posture, and prevent the algorithm from being trapped in a certain local obstruction region for a long time. Therefore, a global reference point is generated within the joint constraints. (This can be obtained by mapping low-difference sequences across the global joint range), and the catch-all direction is defined accordingly:

[0134] Parameter description: A reference point generated within the globally valid range; For the reason The unit direction pointing to this reference point.

[0135] Then let Substitute into equation (29) and verify again according to equation (30). If it passes, it means that although the current local search failed, a feasible new node was still successfully obtained by changing the global direction; if it still fails, it means that none of the four channels in this round have generated a new edge that meets the unified feasibility criterion. At this time, step S3 ends and returns to step S2. Step S2 updates the sampling window and random reference sample for the next round and starts a new round of expansion. This fallback channel ensures that step S3 will not be stuck for a long time due to local complex structure.

[0136] In step S3 of this invention, a fixed-order multi-channel expansion mechanism is established under the unified feasibility criterion (30). First, the actual expansion starting point is determined from the current tree by equation (28). Then, candidate new nodes are generated uniformly using equation (29). The safety of candidate edges is uniformly verified by equation (30). If the direct direction equation (31) is successful, the target continues to be pursued at the lowest cost. If the direct direction fails, the process switches to the outer spherical shell equation (32) to equation (38), selecting the best candidate from a set of candidates that balance safety and advancement. If the discrete detour still fails, the process switches to the potential field wall-hugging equation (39) to equation (53), continuously advancing near the obstacle boundary through the combined direction of attraction, repulsion, and tangential sliding. If the local continuous correction still fails, the search situation is finally actively changed by the global catch-up equation (54). Throughout S3, only the direction construction method is changed, while the candidate point generation and safety criterion verification remain unified, resulting in a clear structure that is easy to implement in engineering.

[0137] Step S4: Structural integration, local optimization, and actual convergence detection; Objective: To generate a new node in step S3 and pass the unified feasibility criterion review. Formally integrate the new node into the current search tree, perform necessary local path cost optimization while ensuring connection safety, and determine whether the new node has achieved a true union with the opposite tree. Step S4 specifically includes: Step S4-1, New Node Access and Local Neighborhood Optimization: When step S3 generates candidate new nodes And after the connection between it and the starting point of the extension has satisfied the unified segment-level minimum clearance criterion, Treat it as a node to be connected. During connection, it can be... Select a set of neighboring candidate nodes, and retain only those that are similar to... Candidate nodes that can be safely connected are those whose connections also satisfy a unified feasibility criterion. Among these candidate nodes, the cumulative path cost from the root to the candidate node and the path cost from the candidate node to the root are considered. The node with the smaller total cost is selected as the node with the lower connection cost. The parent node. After completing this process, It is officially written into the current tree, and candidate new nodes are recorded. The parent pointer and cumulative cost information are used for subsequent expansion and final path backtracking.

[0138] exist After successful connection, a partial reconnection check can be performed on existing nodes within its neighborhood. If a neighboring node connects via... If, after reconnection, the total cost from the root to the node decreases, and the reconnected edges still satisfy the unified segment-level minimum clearance criterion, then the parent node of the neighboring node is updated to... Without altering the unified safety standards outlined earlier, the tree structure is made to gradually become straighter and less costly in certain local areas, thereby improving the quality of subsequent real convergence and final path extraction.

[0139] Step S4-2, Real Meeting Detection: Finish After the access and local optimization, it is necessary to further determine whether the two trees are truly connected. The tracking target used in steps S2 and S3 above... This is only used to guide the adjustment and expansion direction generation of the sampling window. It can be a representative reference point in the counterpart tree, but it is not equivalent to the actual meeting node that can be directly used for path splicing. Therefore, the actual meeting must be re-retrieved in the counterpart tree. The nearest real node, which simultaneously satisfies the following two conditions: First, The distance between the real node and the real node does not exceed the preset rendezvous connection distance threshold; Second, the connecting line segments between the two still satisfy the unified segment-level minimum clearance criterion.

[0140] Only when both of the above conditions are met is the connection pair recorded and the two trees truly joined, and then proceed to S5; if not, it means that no usable inter-tree connection has been formed yet, this round only completes the access of new nodes, returns to execute S2, re-updates the sampling state and generates random reference samples based on the updated tree structure, and continues to execute steps S3 and S4 to continue expansion.

[0141] Step S5: Path extraction, path simplification, and unified review of the entire path; Objective: To transform the double-tree structure result confirmed in step S4 into a complete joint path that can be directly executed, and to complete the necessary simplification and final output under a unified safety criterion.

[0142] Step S5 specifically includes: Step S5-1, Path Extraction and Simplification: After step S4 confirms the true meeting, backtrack along the parent pointers of the two trees to their respective root nodes to obtain two branch paths: the starting point side and the ending point side. Reverse the ending point side branch and concatenate it with the starting point side branch to obtain the complete discrete path from the starting point to the ending point. Since this path usually still contains redundant intermediate nodes, direct connection pruning can be performed under the unified segment-level minimum clearance criterion constraint. If a smoother trajectory is required in engineering, further local smoothing can be performed. However, the smoothed local path segments still need to be re-verified. If the unified criterion is not met, the system reverts to the unsmoothed version.

[0143] Step S5-2, Final Review and Output of the Entire Path: A unified segment-level minimum clearance check is performed on each of the adjacent path segments of the final path. Only when the entire path fully meets the minimum safety clearance threshold requirement is it output as the final executable joint path. This ensures that all stages, including sampling, expansion, access, convergence, and path simplification, ultimately achieve closed-loop success under the same safety criterion.

[0144] The above embodiments are merely descriptions of preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Any modifications, alterations, alterations, or substitutions made by those skilled in the art to the technical solutions of the present invention without departing from the spirit of the present invention should fall within the protection scope defined by the claims of the present invention.

Claims

1. A robotic arm path planning method with adaptive sampling and multi-channel expansion, characterized in that, include: Step S1: Construct a unified security assessment system and feasibility criteria; Initialize the bidirectional search tree; Step S2: At the start of each round of cyclic search, update the sampling state of this round based on the search progress of the current bidirectional search tree and generate random reference samples; Step S3: Based on a unified security determination system and feasibility criteria, perform multi-channel expansion in a fixed order to generate feasible nodes step by step; Step S4: Connect the generated feasible new node to the current expanded tree of the bidirectional search tree and perform a real meeting detection; if no real meeting is detected, return to step S3 to continue expansion; if a real meeting is detected, proceed to step S5. Step S5: Perform backtracking and splicing on the bidirectional search tree after the actual meeting to obtain the initial path; perform final review based on a unified safety judgment system and feasibility criteria, and output the final executable joint path after the review is passed.

2. The adaptive sampling and multi-channel expansion robotic arm path planning method according to claim 1, characterized in that, In S1, a unified security assessment system and feasibility criteria are established, including: Step S1-1: Unify the environment and obstacle set: Map all obstacles that pose a collision risk to the robotic arm to the same workspace coordinate system to form an obstacle set; Step S1-2: Define the shortest distance from a point to an obstacle: Calculate the Euclidean shortest distance from any point in the workspace to the surface of each obstacle in the obstacle set, and take the minimum value as the shortest distance from the point to the obstacle set. Step S1-3: Define the configuration clearance function. The minimum value of the shortest distance from all points in the envelope point set occupied by the links of the robotic arm in the workspace under the current posture of the robotic arm to the obstacle set is used as the output value of the configuration clearance function. The output value of the configuration clearance function is used to measure the safety of the overall posture of the robotic arm to the obstacle. Step S1-4: Set the global minimum safe clearance threshold and discrete detection step size; Steps S1-5: Unifying the minimum clearance at the segment level and feasibility criteria: Based on the continuous movement process of the robotic arm, between the starting and ending configurations of the robotic arm's movement, the number of discrete configuration points is determined according to the distance between the two points and the discrete detection step size. Linear interpolation sampling is performed on the line connecting the two points in the joint space to obtain each discrete configuration point. The configuration clearance function value of all discrete configuration points is calculated and the minimum value is taken as the minimum clearance at the segment level for the line connecting the two points, which is used to characterize the safety margin of the most dangerous position in the line connecting the two points. The final feasibility criterion is: a connecting line is defined as a safe and feasible path segment if and only if the minimum clearance at the segment level is not lower than the minimum safe clearance threshold, as expressed as: in: Indicates from arrive Candidate connections; and Configure any two postures of the robotic arm; The minimum safe clearance threshold; Indicates from arrive The minimum clearance of the entire line segment under discrete sampling.

3. The adaptive sampling and multi-channel expansion robotic arm path planning method according to claim 2, characterized in that, In step S1, the bidirectional search tree is initialized, including: reading the starting point configuration and ending point configuration of the robotic arm movement, which are respectively used as the root nodes of the starting point tree and the ending point tree in the bidirectional search tree, and the configuration clearance function values ​​of the starting point configuration and the ending point configuration are not lower than the minimum safe clearance threshold.

4. The adaptive sampling and multi-channel expansion robotic arm path planning method according to claim 2, characterized in that, Step S2 includes: Step S2-1: Construct the tracking target and baseline distance: Take one of the bidirectional search trees as the current expansion tree and the other as the opposite tree; first, select a tracking target configuration from the opposite tree to guide the expansion direction of the current expansion tree and measure the approximation of the two trees; at the same time, construct the baseline distance with the distance between the root of the current expansion tree and the tracking target configuration. Step S2-2: Define the nearest distance and the current search progress: Calculate the Euclidean distance from all nodes in the current expanded tree to the target being tracked, and take the minimum value as the nearest distance from the current expanded tree to the target being tracked; calculate the current search progress based on the baseline distance and the nearest distance. The search progress is used to characterize the degree of approximation between the current expanded tree and the adjacent tree. Step S2-3: Constructing a sliding window and stagnation detection: Based on the single-step progress increment of each search round, the average progress is obtained by averaging the increments of the most recent rounds; then, a stagnation flag is generated through an indicator function. Step S2-4, Sampling Center Shift and Scale Shrinkage: Based on the current search progress and stagnation indicators, determine the sampling center and sampling scale for this round of sampling; Step S2-5, Sampling Window Set Construction and Trimming: Based on the robotic arm joint structure, combined with the sampling center and sampling scale, construct the set of valid sampling windows for this round; Step S2-6, Random Reference Point Generation: Construct low-difference sequence points and uniform random vectors, and linearly map them to the legal range of the current sampling window to obtain the covering samples and random perturbation samples within the window; then, through convex combination, the covering samples and random perturbation samples are weighted and fused to obtain random reference points.

5. The adaptive sampling and multi-channel expansion robotic arm path planning method according to claim 4, characterized in that, In steps S2-3: when the average advance in the most recent rounds is lower than the preset stagnation threshold, the current search is determined to be in a stagnant state; when the average advance is greater than or equal to the stagnation threshold, the current search is determined to be in a normal advance state.

6. The adaptive sampling and multi-channel expansion robotic arm path planning method according to claim 4, characterized in that, Step S2-4, determining the sampling center and sampling scale, includes: First, a forward shift function that monotonically increases with the search progress is defined. Based on the forward shift function, the root configuration of the current expanded tree, and the tracking target configuration, the sampling center for this round is calculated. The sampling center is a 6-dimensional vector in the joint space of the robotic arm. As the search progress increases, it gradually shifts from the root configuration to the tracking target configuration. Next, define an instantaneous target value for the sampling scale that decreases as the search progresses, and the instantaneous target value for the sampling scale shall not be lower than a preset lower limit of the scale; When a stagnation state is detected, the sampling scale is temporarily amplified to obtain the stagnation correction scale; The sampling scale for this round is set as follows: if the state is determined to be stagnant, the stagnant correction scale is used; if the state is in normal progress, the baseline sampling scale is used.

7. The adaptive sampling and multi-channel expansion robotic arm path planning method according to claim 5, characterized in that, Steps S2-5 include: The upper and lower limits of rotation for each joint of the robotic arm are pre-defined, and the half-length of the global movable range of each joint is calculated to obtain the global half-length vector. The global half-length vector is scaled using the current sampling scale to obtain the window half-length vector of the current sampling window in each joint dimension. Based on the sampling center, the initial upper and lower bounds of the sampling window are calculated in combination with the window half-length vector. Then, in combination with the upper and lower limits of rotation of the robotic arm joints, the initial upper and lower bounds are clipped element by element to obtain the final upper and lower bounds of the sampling window, thereby determining the legal range of the current sampling window set.

8. The adaptive sampling and multi-channel expansion robotic arm path planning method according to claim 4, characterized in that, Step S3 includes: Step S3-1, Nearest Node Retrieval and Unified Expansion Framework: Based on a random reference point, retrieve the existing node in the current expansion tree that is closest to the random reference point in Euclidean distance, and use it as the neighboring node for this round of expansion; set the single-step expansion step size and unit direction vector, and generate candidate new nodes based on the neighboring nodes, the single-step expansion step size and the unit direction vector; the connection between the candidate new node and the neighboring node satisfies the feasibility criterion. Step S3-2: Construct a direct path to obtain candidate new nodes: Calculate the unit direction from the neighboring node to the tracking target configuration, substitute it into the unified extension framework to generate candidate new nodes for the direct path, and verify whether the connection between the candidate new node and the neighboring node meets the feasibility criterion; if the verification is successful, this round of extension ends and proceeds to S4; if the verification fails, execute S3-3. Step S3-3: Constructing an outer spherical shell bypass strategy to obtain candidate new nodes: With neighboring nodes as the center, construct a spherical shell candidate set in the 6-dimensional joint space of the robotic arm. Sample several candidate points within the candidate set, and score each candidate point for safety and propulsion consistency. Based on the two scores, construct a comprehensive score, select the candidate point with the highest comprehensive score, calculate the unit bypass direction from the neighboring node to the candidate point, substitute it into the unified expansion framework to generate candidate new nodes for the bypass channel, and verify the feasibility criteria again. If the verification passes, this round of expansion ends; if the verification fails, proceed to step S3-4. Step S3-4: Construct an artificial potential field for wall-hugging channel to obtain candidate new nodes: Construct a continuous directional field based on the combined force of attractive force, repulsive force and tangential force to generate the unit direction for wall-hugging propulsion. Substitute this into the unified extension framework to generate candidate new nodes for the wall-hugging channel and verify the feasibility criteria. If the verification is successful, this round of extension ends; if the verification fails, proceed to step S3-5. Step S3-5, Global Cubble Channel: Generate a global reference point within the legal range of the robotic arm joint limit, calculate the unit catch-up direction from the neighboring node to the global reference point, substitute it into the unified extended framework to generate candidate new nodes for the catch-up channel, and verify the feasibility criterion; if the verification passes, a feasible new node is obtained; if the verification fails, no feasible new node is generated in this round, S3 ends and returns to step S2, and the sampling window and random reference sample for the next round are updated again.

9. The adaptive sampling and multi-channel expansion robotic arm path planning method according to claim 8, characterized in that, Step S4 includes: Step S4-1, New Node Access and Local Neighborhood Optimization: The generated feasible new node is taken as the node to be accessed. A group of candidate neighboring nodes is selected near the node to be accessed. Nodes whose connections to the node to be accessed meet the feasibility criteria are filtered out. Among the filtered nodes, the node with the smallest sum of cumulative path cost from the root to the filtered node and connection cost from the filtered node to the node to be accessed is selected as the parent node of the node to be accessed. After the parent node is selected, the feasible new node is written into the current expanded tree, and the parent pointer and cumulative cost information of the feasible new node are recorded. Step S4-2, True Meeting Detection: In the opposite tree, search for the true node that is closest to the newly joined feasible node, and determine the true meeting of the bidirectional search trees if at least two of the following conditions are met: The distance between a feasible new node and the nearest real node does not exceed the preset rendezvous connection distance threshold; The connecting line segment between the two satisfies the feasibility criterion; If a true meeting is determined, proceed to step S5; if the meeting condition is not met, the current iteration ends, return to step S2, re-update the sampling state and generate random reference samples based on the updated tree structure, and continue to execute steps S3 and S4 to continue expansion.

10. The adaptive sampling and multi-channel expansion robotic arm path planning method according to claim 1, characterized in that, Step S5 includes: Step S5-1, Path Extraction and Simplification: After the actual meeting, backtrack to the root node along the parent pointers in the extended tree and the opposite side tree respectively to obtain two branch paths: the starting side and the ending side. Reverse the ending side branch and concatenate it with the starting side branch to obtain the complete discrete path from the starting point to the ending point. Perform local smoothing. The smoothed local path segments need to be re-verified. If the feasibility criteria are not met, revert to the unsmoothed version. Step S5-2, Final Review and Output of the Entire Path: Perform feasibility criterion verification on each of the adjacent path segments of the final path. When the entire path meets the minimum safe clearance threshold, it is output as the final executable joint path.

Images