A mechanical arm obstacle avoidance path planning method based on an improved ant colony algorithm

Abstract

The application provides a mechanical arm obstacle avoidance path planning method based on an improved ant colony algorithm, and relates to the fields of robot path planning and intelligent optimization algorithm. The method gives a non-uniform metric structure induced by a metric tensor G(q) to the joint configuration space, G(q) is composed of three parts of a normalized inertia matrix W I , a singularity gradient cross product term and an obstacle distance gradient cross product term; the local geodesic distance is calculated by using the average of the metric tensor of the two end nodes, all edge weights are pre-calculated and cached in the PRM graph construction stage. The ant colony uses the reciprocal of the geodesic distance as the heuristic function, uses the normalized value of the trace increment of the metric tensor to drive the non-uniform pheromone evaporation, and uses the weighted sum of the geodesic length cost and the inertia weighted velocity mutation penalty as the path comprehensive cost; the hornet algorithm performs pre-search and completes the non-uniform initialization of the pheromone under the geodesic metric. The method effectively improves the path safety, continuity and dynamic adaptability.

Description

Technical Field

[0001] This invention relates to the field of robot path planning and intelligent optimization algorithms, specifically a robotic arm obstacle avoidance path planning method based on an improved ant colony algorithm. Background Technology

[0002] Robotic arm path planning is one of the core technologies in industrial automation and intelligent manufacturing. Its goal is to plan an optimal motion path for the robotic arm from the initial configuration to the target configuration, while meeting joint motion constraints and obstacle avoidance requirements. Ant colony optimization (ACO), as a classic swarm intelligence algorithm, has been widely adopted in the field of robot path planning due to its strong global search capability and parallelism.

[0003] However, existing ant colony algorithm-based robotic arm path planning methods have the following prominent problems in engineering practice:

[0004] First, there is the problem of singularity attraction traps. When performing ant colony search in the joint configuration space, traditional methods apply a uniform pheromone evaporation coefficient to all nodes, without distinguishing between singular configuration regions and normal configuration regions. Since joint increments near singular configurations are typically small, ants tend to frequently traverse these nodes, leading to a continuous accumulation of pheromones in the singular configuration neighborhood, forming a strong positive feedback loop. In subsequent iterations, the ant colony is continuously guided to singular regions, ultimately planning a path that extremely closely approximates the singular configuration. This causes drastic changes in joint speed during actual robotic arm execution, triggering controller limiting or even emergency stop protection. Existing literature on singularity avoidance generally remains at the post-processing stage after path planning, failing to fundamentally eliminate the singularity attraction effect during the ant colony search process.

[0005] Second, there is a lack of constraints on the continuity of joint velocity. Existing ant colony algorithms typically use only joint spatial distance or end effector displacement as evaluation indicators for heuristic functions and state transition probabilities, neither of which can predict sudden changes in angular velocity between nodes in a continuous path. Ant colonies tend to select path nodes with small joint increments, but if the joint movement direction of adjacent nodes suddenly reverses, the robotic arm will generate high-frequency chatter during execution.

[0006] Third, there is the problem of low pheromone initialization efficiency. Traditional ant colony algorithms use a uniform pheromone initialization strategy, where ants rely primarily on heuristic functions for blind exploration in the early iteration stages, resulting in a low effective path discovery rate and slow convergence speed. Although some literature has proposed non-uniform initialization methods based on greedy strategies or known solutions, the mechanism for fast feasible path pre-search for high-dimensional joint configuration spaces remains imperfect.

[0007] The three problems mentioned above share a common technical root: existing methods treat singular configuration avoidance, joint velocity continuity assurance, and pheromone initialization efficiency as independent modules within the Euclidean joint configuration space. Each module achieves its function through separate penalty terms or weighting coefficients, lacking organic connection between them. Adjusting the parameters of a single module often has unpredictable effects on the performance of other modules: for example, increasing the singularity penalty coefficient may weaken the relative weight of the velocity smoothing term; accelerating pheromone evaporation to repel dangerous areas may cause effective path information in narrow channels to disappear prematurely. This parameter coupling problem limits the engineering applicability of existing methods in complex scenarios, making it difficult to achieve stable performance improvements by adjusting the parameters of a single module. Summary of the Invention

[0008] To address the above problems, this invention proposes a robotic arm obstacle avoidance path planning method based on an improved ant colony algorithm, comprising the following steps:

[0009] S1. Construct an obstacle geometry model of the robotic arm's workspace, approximating each link of the robotic arm with a capsule-like envelope, and using the PRM presampling strategy to discretize the 3D workspace into a set of candidate nodes for joint configuration space. And sparse connectivity graph; after the PRM graph is constructed, a metric tensor is assigned to the joint configuration space. The induced non-uniform metric structure, wherein the metric tensor is defined as:

[0010] ;

[0011] in The normalized joint equivalent inertia diagonal matrix is ​​automatically obtained from the URDF file and satisfies the following conditions. , ; , The weighting coefficients are positive real numbers; For nodes The operability index is defined by the ratio of the minimum to the maximum singular value of the Jacobian matrix. It is a singularity index; This is the normalized value of the minimum distance between each link capsule and the nearest obstacle; , For positive scalar functions that depend on the current configuration; , They are respectively and For joint angle vectors The gradient column vector is approximated using the finite difference method; By strictly positive definite matrix It consists of two positive semi-definite outer product terms, is globally strictly positive definite, and its minimum eigenvalue satisfies ;

[0012] For any pair of adjacent nodes in the connected graph The mean of the tensor is measured at both ends. Define the local geodesic distance approximation:

[0013] ;

[0014] The and the geodesic distance from each candidate node to the target node After the PRM diagram is constructed, it is pre-calculated and cached for direct reading in subsequent steps;

[0015] S2. The longhorn beetle whisker search algorithm is invoked to perform a pre-search within the configuration space. The distance between nodes is measured using the local geodesic distance pre-calculated and cached in step S1. Obtain an initial feasible path; perform non-uniform initialization of the ant colony pheromones based on this path: assign values ​​to the pheromones of nodes traversed by the pre-searched path. The remaining nodes are assigned values , ;

[0016] S3, using the pre-calculated cache from step S1 Constructing heuristic functions:

[0017] ;

[0018] in To prevent division by zero regularization of small values; the heuristic function comprehensively reflects the node Information on operability, obstacle spacing, and inertia-weighted joint distance is used to automatically assign smaller heuristic values ​​to nodes in singular configuration neighborhoods and obstacle-dense areas; all nodes' operability, obstacle spacing, and inertia-weighted joint distance information are used to automatically assign smaller heuristic values ​​to nodes in singular configuration neighborhoods and obstacle-dense areas; The calculation is completed and cached in one step, and the table is directly looked up during ant colony iteration;

[0019] S4. The ant constructs the path iteratively based on the following state transition probabilities:

[0020] ;

[0021] in , These are the influence indices of pheromones and heuristic functions, respectively. For ants The set of nodes currently allowed to be accessed; the allocation of pheromone increments is determined by the comprehensive path cost, which is a weighted average of geodesic length cost and velocity direction change penalty cost:

[0022] ;

[0023] in , The weighting coefficients are positive real numbers; Inertia-weighted joint velocity mutations:

[0024] ;

[0025] and middle Sharing the same URDF parsing results; pheromone increment ( (where pheromone intensity constant)

[0026] S5, with Metric tensor relative to fundamental metric The normalized value of the additional trace contribution is used as a local metric for the distortion. Drives non-uniform pheromone evaporation:

[0027] ;

[0028] node The volatility coefficient is:

[0029] ;

[0030] in Based on the basic volatility coefficient, The adjustment coefficient for the distorted variable. This is the upper limit of the volatility coefficient; for all... Node pairs with endpoint Pheromones are updated using the following formula:

[0031] ;

[0032] This causes the pheromone evaporation rate in the neighborhood of unusual configurations and areas with dense obstacles to be higher than in normal areas;

[0033] S6. Determine the termination condition: If the current iteration count reaches the preset maximum iteration count. , or continuous The change in the overall cost of the globally optimal path within the next iteration is less than the preset convergence threshold. If the result is positive, output the optimal collision-free path obtained in the current iteration; otherwise, return to step S4.

[0034] The normalized joint equivalent inertia matrix mentioned in step S1 The method of obtaining it is: from the robotic arm's URDF file. <inertial>The tags are used to analyze the moment of inertia tensors of each link. With its traces The equivalent scalar moment of inertia, normalized to:

[0035] ;

[0036] The Automatically adaptable robotic arms to different configurations; high-inertia joints have larger corresponding dimensions. This causes the angle of the joint to change at geodesic distances. The higher metric cost in the middle leads to a preference for paths with high inertia and small joint displacements at the global path optimization level.

[0037] The scalar function described in step S1 , The definitions are as follows:

[0038] ;

[0039] in , The shape coefficient is a positive real number, and its weighting coefficient is the same as that of the outer product term in claim 1. , They are mutually independent; the function varies with singularity. Elevation or barrier spacing Decreasing and monotonically increasing, making The weights of the corresponding outer product terms adaptively increase with the degree of risk; when and hour, , , Degenerate into The dominant inertia-weighted near-Euclidean metric does not interfere with the ant colony's search behavior in normal areas.

[0040] The gradient described in step S1 and The finite difference method is used for approximate calculation: for the ... For each joint component, an adaptive step size is used:

[0041] ;

[0042] If the difference does not exceed the joint limit Using central difference:

[0043] ;

[0044] ;

[0045] in For the first Unit vectors along each coordinate axis; one-sided difference is used near the joint limit; the gradient and... , Pre-compute and cache the candidate node set in the PRM graph, and do not repeat the computation in the main loop of ant colony iteration.

[0046] The cost function of the pre-search phase of the beetle whisker search algorithm in step S2 is defined as follows:

[0047] ;

[0048] in With nodes Place At the target node Calculation of the mean metric tensor; The absolute minimum distance between each link capsule and the nearest obstacle; This is the safety distance threshold; The collision penalty coefficient is used to make the pre-search path geometrically optimal in the configuration space induced by the metric tensor. The resulting initial path naturally deviates from singular configurations and obstacle-dense regions, providing a more directional prior for the non-uniform initialization of pheromones.

[0049] In step S5, the volatility coefficient at each node satisfies the following condition: in normal regions far from singular configurations and obstacles, , In the singular configuration neighborhood ( ) or areas with dense obstacles ( ) place, or exponential growth made Monotonically increasing, Increase as it goes until it reaches its upper limit. Pheromones decay more rapidly at such nodes, inhibiting ant colonies from converging on dangerous areas; Required and The calculation is cached during the pre-calculation in step S1 and read directly during the iteration, without increasing the computational overhead of the main loop.

[0050] It also includes a dynamic obstacle response step: acquiring the current position of the obstacle at a preset frequency and recalculating each candidate node. Cache and trigger middle Automatic updates ensure that the volatile coefficient at the node is maintained. Synchronously responds to obstacle movement without additional calculations; reduces the minimum distance to obstacles by more than a threshold. The nodes perform forced local pheromone evaporation, retaining the coefficient. For nodes whose spacing increases beyond a threshold, pheromone protection enhancement is performed according to the following formula:

[0051] ;

[0052] If the overall cost of the optimal path increases by more than a preset percentage threshold within at least three consecutive iterations, the beetle search algorithm is triggered to perform an emergency local search on the collision path segment. The emergency search cost function is based on the current frame. Updated The metric involves assigning pheromone values ​​to the corresponding nodes in the search results to repair the initial values, and then re-performing collision detection and verification.

[0053] When the number of degrees of freedom of the robotic arm increases to a redundant configuration, the number of joints... At that time, the Jacobian matrix For underdetermined matrices, the operability index is changed to... ( Calculate the ratio of the minimum to the maximum singular value of a square matrix; The dimensions automatically expand with the number of joints; The dimensions expand accordingly, and the positive definiteness is determined by each Strict guarantees are in place; a null space utilization margin penalty is further introduced into the state transition probability, with the minimum normalized margin between each joint and the joint limit being used:

[0054] ;

[0055] Characterizes the degree of redundancy degradation; adds a zero-space margin penalty term to the path cost function. Weight The extensions increase adaptively with the iteration process; the above extensions do not need to be re-derived for a specific number of degrees of freedom and can be directly adapted to robotic arms with six or more degrees of freedom.

[0056] Compared with the prior art, the present invention has the following beneficial effects:

[0057] I. Singularity avoidance is upgraded from an independent penalty mechanism to a metric geometry mechanism, eliminating singularity attraction traps. This invention constructs a non-uniform configuration space metric induced by a metric tensor G(q), embedding singularity configuration avoidance constraints within the metric structure. In G(q)... The term amplifies the geodesic distance along the singular gradient direction within the neighborhood of the singular configuration, making the configuration nodes at the singular configuration more aligned with the heuristic function. Automatic switching has a higher cost and a naturally lower transition probability; at the same time, it measures distorted variables. The non-uniform evaporation mechanism driven by the pheromone accelerates the decay of pheromones in the neighborhood of singular configurations, preventing stable accumulation of pheromones near these configurations. Both mechanisms emerge naturally from the unified definition of G(q), requiring no independent penalty coefficient control, eliminating singular attraction traps at the metric geometry level, and can be stably reproduced under different random initialization conditions.

[0058] II. Velocity continuity is jointly guaranteed by the metric infrastructure and path-level penalties, with a unified physical source. This invention guarantees joint velocity continuity through two complementary mechanisms. First, the normalized inertia matrix in G(q) Firstly, the angular displacement of high-inertia joints carries a greater metric cost in geodesic distance calculations, naturally favoring paths with smaller displacements for high-inertia joints at the global path optimization level. Secondly, the path cost function includes a penalty term for sudden changes in velocity direction. The inertia weights are used to capture the direction reversal information of adjacent path segments by the included angle of the three nodes. and Both methods share the same URDF analysis results, have a unified physical source, and require no additional parameter calibration. They suppress joint velocity chatter from two levels: local geometry of the placement points (edge-by-edge measurement) and overall path trajectory (consistency of cross-edge directions), respectively, ensuring velocity continuity from the planning mechanism rather than the post-processing stage of the path.

[0059] Third, the BAS pre-search obtains a directional initial path under the metric tensor-induced configuration space metric, significantly improving the early convergence speed. This invention utilizes the beetle whisker search algorithm to perform pre-search under the metric tensor-induced non-uniform configuration space metric, and the distance term in the cost function uses geodesic distance. This makes the pre-search path more costly in the neighborhood of singular configurations and areas with dense obstacles. The resulting initial path naturally deviates from the dangerous area and is geometrically superior to the pre-search results guided by Euclidean algorithm. After non-uniform initialization of pheromones based on this path, the ant colony obtains global directional guidance in the early iteration stage, significantly improving the effective path discovery rate, greatly reducing the number of iterations required for the main body to converge, and improving the overall planning efficiency.

[0060] Furthermore, this invention proposes a mechanism for accurate identification of failed nodes and forced local pheromone evaporation, which performs targeted pheromone compression only on path segments that fail due to obstacle movement, while fully preserving the remaining valid path information and avoiding global restarts.

[0061] This invention also possesses excellent degree-of-freedom scalability. The core formulas of this invention have structural scalability in the scenario of redundant degree-of-freedom robotic arms: the inertia weight coefficient is automatically obtained through URDF file parsing; the operability metric and singularity index are automatically adapted under non-square matrix Jacobian; and the composite dynamic bias factor further introduces a null space utilization co-quantum term to avoid redundancy degradation configuration. Attached Figure Description

[0062] The accompanying drawings, which are included to provide a further understanding of this application and form part of this application, illustrate exemplary embodiments and are used to explain this application, but do not constitute an undue limitation of this application. Obviously, the drawings described below are merely some embodiments of the present invention, and those skilled in the art can obtain other drawings based on these drawings without creative effort. In the drawings:

[0063] Figure 1 This is an overall flowchart of a robotic arm obstacle avoidance path planning method based on an improved ant colony algorithm.

[0064] Figure 2 A comparison of the three-dimensional trajector trajector in a robotic arm obstacle avoidance path planning method based on an improved ant colony algorithm;

[0065] Figure 3 This is the convergence curve of the mean cost of the optimal path as a function of the number of iterations;

[0066] Figure 4 A line graph showing the distribution of the inertia-weighted joint velocity mutation at each path node in the optimal path along the node number;

[0067] Figure 5 This is a curve showing the synchronous change of key path nodes with the perception frame number in a dynamic obstacle scenario. Detailed Implementation

[0068] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0069] It should be noted that the following detailed descriptions are exemplary and intended to provide further explanation of this application. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application pertains.

[0070] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the exemplary embodiments according to this application. As used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise. Furthermore, it should be understood that when the terms "comprising" and / or "including" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0071] Example 1

[0072] This embodiment uses a six-degree-of-freedom serial robotic arm in an industrial setting as an example, with the joint angles limited to... ( Workspace settings A three-dimensional region containing several static obstacles.

[0073] Each link is approximated by an envelope of a capsule (cylinder with hemispheres at both ends), and the capsule radius is taken as 1.05 times the radius of the circumcircle of the largest cross-section of the corresponding link (with a safety margin). The configuration space is discretized using a PRM pre-sampling strategy, selecting a set of candidate nodes from the configuration points that have passed collision detection. The total number of nodes... Adjacent nodes are separated by Euclidean distance in joint space. Connect the edges to form a sparse graph.

[0074] The Jacobian matrix uses the geometric Jacobian form and has dimensions of . Normalized equivalent inertia of each joint Automatically parsed from URDF files, typical value range is... , The proximal joint inertia is significantly greater than that of the distal joint.

[0075] Key parameters of an ant colony: Number of ants Maximum number of iterations Number of consecutive convergence determinations Convergence threshold ;Pheromone Influence Factor Heuristic function influence factor Basic volatility coefficient upper limit of volatility coefficient ;Metric tensor parameters: , , Function shape coefficient , Measure the adjustment coefficient of distorted variables Path cost weight , pheromone intensity constant Preventing division by zero regularization for small quantities .

[0076] Step S1 Implementation: PRM graph construction and metric tensor G(q) pre-computation; after configuration space discretization and sparse connectivity graph construction are completed, execute the following on all candidate nodes during the PRM graph construction phase: Each component is pre-calculated once, and the results are cached in the node attribute table, so they are not repeatedly called in the main loop of ant colony iteration. The specific calculation steps are as follows.

[0077] Step M1: Normalize the inertia matrix ;

[0078] Read the rotational inertia tensors of the six links from the URDF file. Calculate and normalize the equivalent scalar moment of inertia:

[0079] ;

[0080] Taking the typical value in this embodiment as an example, the normalized result is: , , , , , The total is 1.

[0081] Joint weight matrix This operation will be performed once.

[0082] Step M2: At each candidate node Pre-calculation;

[0083] For the candidate node set Each node , for the One joint ( Take an adaptive step size:

[0084] ;

[0085] If it does not exceed the joint limit Using central difference:

[0086] ;

[0087] in Each call requires... Perform one SVD using the geometric Jacobian matrix; switch to one-sided difference when approaching the joint limit. After pre-calculation, it is cached in the node attribute table, and also cached... For later Used for calculations.

[0088] Step M3: At each candidate node Pre-calculation;

[0089] Using the same step length, for the first One joint:

[0090] ;

[0091] every time The call requires performing a GJK collision distance query on the six connecting capsules, taking the minimum value and dividing it by the reference spacing. (If the value exceeds 1, it is truncated to 1); Each node requires a total of 12 collision distance calculations (6 joints × 2 directions); and Cached in the node attribute table.

[0092] Step M4: Construction of the metric tensor G(q) for each node;

[0093] For each node Calculate the singularity index and scalar weights:

[0094] ;

[0095] ;

[0096] The metric tensor is:

[0097] ;

[0098] Pick , ; for Real symmetric matrix, globally strictly positive definite, minimum eigenvalue ; , , , It is cached together for direct reading in the calculation of volatility coefficient.

[0099] Step M5: Edge weight pre-calculation and caching;

[0100] For each edge in the PRM graph (Joint space Euclidean distance) ), calculate the mean metric tensor and local geodesic distance:

[0101] ;

[0102] The trapezoid is approximately in terms of side length Under the condition that the error order is This is acceptable within the engineering precision range; for Near the drastically changing singular configuration, the region has sparse collision-free nodes and a shorter average side length, further suppressing approximation errors. Furthermore, for all nodes... Pre-compute and cache This is used for direct table lookup in heuristic functions; all edge weights and geodesic distances from nodes to the target are completed in one go during the PRM graph construction phase.

[0103] Step S2 implementation: Longhorn beetle pre-search and non-uniform pheromone initialization;

[0104] (a) BAS pre-search;

[0105] Starting configuration Initial position of the longhorn beetle, target configuration The initial distance must be perceived. With the number of iterations according to Decrease; Step Size The attenuation rates are the same.

[0106] The distance metric between nodes in the pre-search cost function uses the local geodesic distance pre-computed and cached in step S1 (step M5):

[0107] ;

[0108] in The data is retrieved directly from the cache of step M5, without the need for online calculation; , This approach optimizes the pre-search path geometrically within the configuration space induced by the metric tensor, naturally avoiding singular configurations and areas of dense obstacles, and providing a more directional prior path for non-uniform pheromone initialization. The pre-search path, containing approximately 20 configuration space nodes, is obtained after 50 BAS iterations and is validated through collision detection.

[0109] (b) Non-uniform pheromone initialization;

[0110] Pheromones are assigned to the node pairs traversed by the pre-search path. The remaining nodes assign values ​​to pheromones. The non-uniform scaling factor is 8. This is in contrast to traditional uniform initialization. This step increases the effective path discovery rate of the ant colony from about 35% to about 78% (simulation statistical mean) in the first to 30th iterations, significantly shortening the early "blind exploration" stage.

[0111] Step S3; Heuristic function construction; For each candidate node in the PRM graph The heuristic function value is read directly from the cache in step M5:

[0112] ;

[0113] The definition of the metric tensor makes configurations with high singularity more costly in geodesic distances. The same principle applies to configurations with small barrier spacing (where the exponent increases the magnification of the outer product term). (Increase in exponent), high inertia joint large displacement configuration changes from The basic metric contributes a cost; therefore The single quantity comprehensively reflects three types of information: operability, obstacle spacing, and inertia-weighted joint distance, corresponding to the node's As the number of ants decreases automatically, the probability of ants migrating also decreases. (All nodes) This step, the pre-calculation stage, is completed in one go. During iteration, the table is looked up directly, and no online calculation is required.

[0114] It should be noted that traditional pure distance heuristic functions cannot distinguish "seemingly quick" nodes with operability close to zero; this joint heuristic function assigns smaller heuristic values ​​to nodes with low operability in later iterations, causing ants to naturally tend to high operability areas, forming soft singularity avoidance.

[0115] Step S4; Each ant from Starting from the beginning, select the next node on the sparse graph according to the following state transition probabilities:

[0116] ;

[0117] Pick , ; and All methods directly look up tables; there is no need to calculate SVD or collision distance online during the path construction phase. The metric tensor transforms the configuration points of singular configuration neighborhoods and obstacle-dense regions into geodesic points with large distances. Small nodes naturally cause ants to reject such nodes at every step of path construction. Singularity avoidance and obstacle avoidance effects emerge naturally from the metric structure, rather than relying on online computational penalty factors.

[0118] path After construction is complete, calculate the overall path cost:

[0119] ;

[0120] Pick , Wherein the abrupt change in velocity direction:

[0121] ;

[0122] and middle They share the same URDF resolution results and have a unified physical source. The division of labor between the two parts is as follows: pass The metric structure uniformly handles singularity avoidance, obstacle avoidance, and inertia-weighted joint displacement; Independently capture direction reversal information of adjacent path segments As an edge-by-edge metric, it inherently cannot encode path history information across edges; therefore, this item, as a path-level supplement, cannot be omitted. Pheromones increment. ( Only elite ants (the top 5 ants with the lowest overall cost in this iteration) participate in pheromone updates.

[0123] Narrow channel node pheromone compensation; identifying the optimal path that satisfies... For narrow channel nodes, additional overlays are applied to these node pairs:

[0124] ;

[0125] This step prevents non-uniform evaporation mechanisms from occurring in narrow channels (where...). Small cause (Excessive) elimination of effective path pheromones. This compensation step is related to... The division of labor in the volatilization mechanism is clear: middle The first item implements pheromone rejection in dangerous areas at the global level; the second item implements pheromone protection for specific narrow channel nodes that have been verified as safe passage by elite ants to prevent the loss of effective path information due to excessive evaporation. The two items do not have functional overlap.

[0126] Step S5 implementation: After each iteration, calculate the local metric distortion for all nodes. All intermediate quantities are directly read from the cache in step M4 without additional calculation.

[0127] ;

[0128] Always true; normal areas far from singular configurations and obstacles. In the singular configuration neighborhood ( ) or areas with dense obstacles ( ) place, or exponential growth made Monotonically increasing.

[0129] node The volatility coefficient is: ;

[0130] Pick , , The range of values ​​is approximately In areas with unusual configurations and dense obstacles, the volatility coefficient automatically increases, pheromone decays more rapidly, and pheromone accumulation is inhibited in dangerous areas; in normal areas, the volatility coefficient remains at the baseline value. It does not interfere with the stable preservation of pheromones in effective paths.

[0131] For all Node pairs with endpoint Pheromones are updated using the following formula:

[0132] ;

[0133] Step S6: After each iteration, determine the following termination condition: if the current iteration count reaches... , or continuous The comprehensive cost of the globally optimal path within the next iteration The magnitude of the change is lower than the convergence threshold If the iteration fails, the iteration terminates and the optimal collision-free path obtained in the current iteration is output; otherwise, return to step S4 to continue the iteration.

[0134] To systematically verify the method of this invention, a virtual simulation verification system for a six-DOF serial robotic arm was constructed on the MATLAB R2023a platform using the RoboticsSystem Toolbox. The specific construction process is as follows: Taking a standard industrial six-DOF serial robotic arm as the object, the mass, moment of inertia tensor, and DH parameters of each link were parsed from the URDF file. Kinematic model registration was completed in the Robotics System Toolbox, and a forward kinematic mapping was established. With geometric Jacobian matrix The parsing calculation interface; in In a 3D workspace, several cuboid obstacle models are placed according to their physical dimensions. Real-time collision detection between the link capsule and the bounding box of obstacles is achieved based on the GJK algorithm. The configuration space is discretized using a PRM pre-sampling strategy, generating 3000 candidate nodes with a connection distance threshold of 0.35 rad. Node connectivity and collision states are pre-calculated and cached before algorithm execution to ensure the efficiency of the main loop. The simulation environment is completely decoupled from the method of this invention—the simulation platform only provides the kinematic calculation interface and collision detection service; the path planning logic is entirely driven by the algorithm of this invention, without relying on any planner built into the simulation platform, thus ensuring that the verification results truly reflect the performance of the method of this invention.

[0135] In this simulation environment, the standard ACO (control group A), the ACO with only the improved heuristic function (control group B), and the complete method of this invention (experimental group C) were compared. Each group was run independently 50 times with different random seeds, and the statistical results are summarized in Table 1-1.

[0136] Table 1-1 Statistical Results:

[0137] ;

[0138] The five indicators in the table have initially reflected the performance differences of the three methods at the quantitative level. The following sections will describe these differences in detail. Figure 2 , Figure 3 , Figure 4 The above quantitative conclusions are verified by three dimensions: path space morphology, convergence process, and joint continuity. The experimental evidence presented in the three figures and the data in Table 1-1 corroborate each other, jointly supporting the establishment of the three core beneficial effects of this invention.

[0139] See Figure 2 The three-dimensional trajector paths and their superimposed envelopes of the end effectors obtained from 50 independent runs of the three methods in the same simulated workspace are shown in the figure. The envelope is rendered by superimposing 50 trajectories, and the envelope width directly reflects the path consistency of each method under different random initialization conditions. The trajectory of control group A (standard ACO) shows obvious detours in areas with dense obstacles, and some trajectory segments have very small distances from the obstacle surfaces, resulting in a high risk of collision. The envelope widens significantly in this area, indicating that the combination of uniform pheromone evaporation and pure distance heuristic function in standard ACO leads to a lack of perception of singular areas and areas with dense obstacles by the ant colony, and the path convergence results are scattered under different random instances. The trajectory of control group B (improved heuristic function) is generally smoother than that of A, but the path still deviates towards obstacles in narrow passages, and the local narrowing effect of the envelope is limited. The trajectory of experimental group C (this invention) extends smoothly from the starting point along the main diagonal direction of the workspace to the target point, maintaining a visible safe distance from each obstacle throughout the entire process. The envelope narrows uniformly throughout the entire path, and the trajectories in the 50 experiments are highly consistent. The above comparison directly verifies the beneficial effect of the present invention (as described in point one) at the simulation visualization level: metric tensor. Simultaneously affects the heuristic function (By reducing the transfer attraction of singular neighborhood nodes through geodesic distance) and node volatile coefficient (By measuring distorted variables) The collaborative mechanism of accelerating the decay of pheromones in the strange neighborhood enables the ant colony to fundamentally avoid the strange attraction trap, and this avoidance behavior can be stably reproduced under different random initialization conditions.

[0140] See Figure 3 The convergence curves (mean of 50 experiments) of the optimal path cost of the three methods as a function of the number of iterations are shown in the figure. The vertical dashed lines in the figure indicate the number of iterations at which each method reaches the main convergence state. Experimental group C (green) showed a significantly higher rate of path cost reduction in the first 25 iterations than control groups A and B, achieving main convergence at the 89th iteration, while the corresponding nodes for control groups B and A were at the 121st and 148th iterations, respectively. The direct reason for the early rapid convergence is that this invention performs non-uniform initialization of pheromones through the BAS pre-search path—the nodes traversed by the pre-search path are assigned... Compared to uniform initialization conditions In the first ant colony iteration, global directional guidance is provided to the ants, which greatly reduces the number of iterations in the blind exploration phase.

[0141] See Figure 4 The inertia-weighted joint velocity mutation at each path node in the optimal path of the three methods The distribution along the node numbers is shown in the figure. Control group A (blue broken line) exhibits sharp jumps exceeding 2.0 (rad / s estimate) at multiple nodes, such as the 8th and 19th. This is an inevitable consequence of the standard ACO state transition probability being completely unaware of the consistency of movement directions between adjacent nodes—the ant colony accumulates pheromones at minimal joint increment nodes without distinguishing whether that node causes a sharp reversal of the movement direction of adjacent path segments. Control group B (orange broken line) shows a lower peak value, but still exceeds 1.0 (rad / s estimate) at nodes such as the 10th and 12th. This is due to the joint heuristic function. Scoring only on the characteristics of a single node cannot predict the continuous change in motion direction formed by the current candidate node and the two preceding nodes. In the experimental group C (green broken line) of this invention, the abrupt change at each node along the entire path is less than 0.5 (estimated in rad / s), and there are no obvious isolated peaks throughout the entire path; the curve is generally smooth. This result is consistent with the velocity direction abrupt change penalty term in the comprehensive path cost function. The design expectation is completely met: after each ant completes path construction, the overall cost is... Determines the increase in pheromone levels The path of high-speed mutation A path with a larger pheromone concentration receives less pheromone increment, and in subsequent iterations, the pheromone concentration of this type of path remains lower than that of a smooth path, thus reducing the probability of the ant choosing the fluttering node; meanwhile, In metric tensors By assigning greater weight to high-inertia joints with large displacements in the geodesic cost, the path segments with abrupt changes in direction by high-inertia joints are suppressed at the heuristic function level. The two mechanisms synergistically suppress joint velocity chatter from the perspectives of path-level penalty (cross-edge direction consistency) and local metric (edge-by-edge geodesic cost), which precisely corresponds to the improvement in joint velocity continuity described in the second beneficial effect of this invention, which stems from the planning mechanism rather than post-path processing correction.

[0142] Example 2

[0143] This embodiment uses a seven-degree-of-freedom redundant robotic arm (hereinafter referred to as a 7-DOF arm) as an example to illustrate the adaptation process and specific parameter settings of the core formulas of this invention in the 7-DOF case, so as to prove that this method has the ability to structurally extend to changes in joint dimensions, rather than being only applicable to specific degree-of-freedom configurations.

[0144] There is a one-dimensional redundancy between the end effector pose (6D) and joint space (7D) of a 7-DOF arm, which is mathematically represented by the Jacobian matrix of the kinematic positive mapping. The matrix is ​​underdetermined, with a null space dimension of 1. This presents two technical challenges that require specific handling in the algorithm: First, the calculation methods for operability and singularity indices need to be adapted to non-square matrix Jacobians; the condition number definition in the standard 6×6 case cannot be directly applied. Second, redundant degrees of freedom grant the robotic arm multiple equivalent joint configurations at the same end-effector pose. If joint limit constraints are not explicitly addressed, the ant may select joints with configurations close to their limits, leading to a contraction or even degradation of the usable null space. This embodiment provides complete adaptation solutions for both of these problems.

[0145] This embodiment uses the Franka Emika Panda robotic arm as the simulation object. The arm has 7 rotary joints, and the angle limits and maximum angular velocity parameters of each joint are shown in Table 2-1.

[0146] Table 2-1 Joint parameters of Franka Emika Panda:

[0147] ;

[0148] The aforementioned inertia data was obtained by parsing the Panda public URDF file (franka_description package). <inertial>Tag <mass>and <inertia>Tensor acquisition, specifically the extraction steps are: read the inertia tensor of each link. Calculate its trace As an equivalent scalar moment of inertia, it is then normalized over all 7 joints:

[0149] ;

[0150] The normalized inertia weight vector is calculated to be:

[0151] ;

[0152] The dimension of the joint weight matrix is ​​expanded to Least diagonal element Positive definiteness is strictly guaranteed; used for subsequent weighted distance calculation and construction of the G(q) metric tensor.

[0153] The fitness of the G(q) metric tensor in the 7-DOF case is: For underdetermined matrices, the singularity index is changed to... ( Calculation of the ratio of the minimum to the maximum singular value of a square matrix:

[0154] ;

[0155] Equivalent in physical meaning to the six-degree-of-freedom case, the range remains the same. A value close to 0 indicates a near-singular configuration; This is a singularity index.

[0156] gradient Upgrade to 7 dimensions, and use the same adaptive step-size center difference method as in Example 1 for the nth joint (n=1~7):

[0157] ;

[0158] The number of finite difference steps per node is 14 (7 joints × 2 directions); Similarly, each node requires 14 GJK collision distance queries.

[0159] metric tensor The construction formula is:

[0160] ;

[0161] ;

[0162] Pick , , , ; for Real symmetric matrix, minimum eigenvalue The overall system is strictly positive definite.

[0163] The edge weight pre-computation uses the mean metric tensor trapezoidal approximation:

[0164] ;

[0165] The threshold for the distance between adjacent nodes is relaxed to 0.40 rad (7-DOF configuration has a higher spatial dimension, so the threshold is appropriately relaxed to ensure graph connectivity); edge weights and geodesic distances from each node to the target node. Pre-compute and cache during the PRM graph construction phase.

[0166] To address the zero-space degradation problem under joint limit constraints, a minimum normalized margin for each joint distance is introduced for zero-space utilization correction.

[0167] ;

[0168] A value close to 0 indicates that a joint is extremely close to its lower limit, and the usable null space is severely shrunk. For candidate nodes... satisfy At that time, apply soft suppression before normalizing the transition probabilities:

[0169] ;

[0170] Pick , ;when The time correction factor is 1, which does not interfere with normal search. Precompute and cache on the PRM graph node set.

[0171] The state transition probability during the ant path construction phase is:

[0172] ;

[0173] ;

[0174] Pick , Normalization is performed after applying null space correction. The G(q) metric tensor handles singular configurations and obstacle avoidance constraints, while the null space margin correction handles joint limit degradation constraints. The two have clear division of labor and their functions do not overlap.

[0175] Measuring distorted variables at each node:

[0176] ;

[0177] The denominator is 7, meaning the number of joints n=7; at this point, the volatility coefficient is:

[0178] ;

[0179] Pick , , The parameters are consistent with those in Example 1.

[0180] The path cost function is extended with a zero-space margin penalty term based on the two components of Example 1:

[0181] ;

[0182] Pick , , Wherein the abrupt change in velocity direction:

[0183] ;

[0184] Using Pandas' 7-dimensional normalized inertia vector, and... Share the same URDF parsing results; All data comes from a cache lookup in step M5; pheromone increment. ( Only the top 5 elite ants participated in the update. Capture the direction reversal information of adjacent path segments, and... The edge-by-edge metrics of G(q) are complementary; The term imposes an explicit penalty on path nodes where joints approach their limits, which, together with zero-space margin correction, constrains the redundancy degradation configuration.

[0185] Longhorn beetles require pre-search and non-uniform pheromone initialization; starting configuration Initial position of the longhorn beetle, target configuration The direction is the primary search axis. The cost function is:

[0186] ;

[0187] Initial value of distance must be perceived The pre-search path is obtained after 50 iterations, with a step size of 0.07 rad and a step size of 0.07 rad. The nodes along the pre-search path are assigned pheromone values. The remaining nodes are assigned values. Non-uniform proportionality coefficient .

[0188] The ant colony parameters are set as follows; the configuration space dimension is increased by 1 compared to six degrees of freedom, and the total number of candidate nodes is expanded to [number missing]. (To ensure connectivity in the high-dimensional graph), the threshold for the distance between adjacent nodes is relaxed to 0.40 rad; ant count Maximum number of iterations ; , , , G(q) metric tensor parameters: , , , , Zero-space correction parameters: , Preventing division by zero regularization for small quantities .

[0189] Under the ROS / MoveIt simulation platform, the standard ACO (control group A7), the ACO with only the operability heuristic function enabled (control group B7), and the complete method of this invention (experimental group C7) were compared. Each group was run independently 50 times, and the statistical results are shown in Table 2-2.

[0190] Table 2-2 Comparison of Seven-DOF Simulation Results:

[0191] ;

[0192] Experimental results show that introducing zero-space margin correction and After the penalty term, the proportion of joint limit neighborhood nodes decreased from 21% in the control group B7 to 6% in the experimental group C7, demonstrating that the null space correction mechanism has an independent technical contribution in the redundant arm scenario, and does not overlap with the singular avoidance and obstacle avoidance functions of the G(q) metric tensor processing. The proportion of singular neighborhood nodes decreased from 38% to 4%, the maximum velocity mutation decreased from 2.87 to 0.71 rad / s, and the number of convergence iterations decreased from 162 to 97, consistent with the improvement trend of Example 1 in the six-degree-of-freedom case, verifying that the method of the present invention has structural extension capability for joint dimension changes, and each core formula can be directly adapted without re-deriving for specific degrees of freedom.

[0193] Example 3

[0194] Dynamic obstacle scenarios, such as operator movements in a collaborative workbench or workpieces moving on a conveyor belt, present three interdependent technical challenges for ant colony path planning. Existing literature addresses these challenges with a rudimentary approach of simply restarting the ant colony after an obstacle change is detected. One problem is that after an obstacle changes, the "high-quality paths" recorded in historical pheromones may have already traversed the obstacle's interior. Indiscriminately inheriting these paths would mislead subsequent ants into following collision paths. However, a complete reset and restart would discard a large amount of still valid path information, leading to excessively long reconvergence times and failing to meet real-time requirements. Another problem is that an obstacle displacement typically only renders a local segment of the path invalid, while most other node pairs remain valid. Therefore, the algorithm needs to accurately identify "node pairs invalidated due to obstacle changes" and force-evaporate only these pairs, rather than affecting the global pheromone levels.

[0195] And when the obstacle moves, nodes that were originally within the safe distance may quickly enter the danger zone. The value can change drastically between frames. If the volatility coefficient... Failure to track these changes in a timely manner will generate incorrect pheromone gradients around the new obstacle location, leading ants into collision risk zones.

[0196] This embodiment proposes a complete dynamic obstacle response mechanism, which enables rapid and accurate response to obstacle movement without restarting the ant colony.

[0197] The experimental setup is as follows: the robotic arm is a six-degree-of-freedom serial robotic arm.

[0198] Obstacle configuration: 2 static obstacles (cubic prisms, fixed positions); 2 dynamic obstacles: Dynamic obstacle D1: Simulates a human arm, reciprocating in a vertical plane with a simple harmonic motion trajectory, motion period... The movement amplitude is 0.25 m, approximated by a bounding box (0.08 m × 0.08 m × 0.40 m); Dynamic obstacle D2: simulates a conveyor belt workpiece, moving at a constant speed along a horizontal straight line, with a speed of The enclosure dimensions are 0.15 m × 0.15 m × 0.10 m.

[0199] Obstacle position sensing: at a fixed frequency The current position of dynamic obstacles is obtained (simulating the perception output of a depth camera or LiDAR), and the position difference between two adjacent frames is denoted as... .

[0200] Path planning frequency: Each iteration of the ant colony algorithm takes approximately [time missing]. (MATLAB single-threaded, including node graph reconstruction and collision detection, hardware is Intel Core i7-12700H), each obstacle perception update triggers a local pheromone correction (additional time consumption). The two are executed asynchronously, with a total planned refresh rate of approximately To meet the path update frequency requirements in human-machine collaboration scenarios The engineering requirements.

[0201] Each time new obstacle location data arrives, the following failure node identification process is executed:

[0202] Step ① Configure the set of spatial nodes Each node in Recalculate based on the obstacle position in the current frame. (Minimum distance between each link capsule and all obstacle enclosures);

[0203] Step 2: Calculate the change in spacing. ;when (default When marking nodes This is a newly added risk node;

[0204] Step ③ For all pheromone path segments with newly added risk nodes as endpoints (i.e., those containing...) node pairs or Forced localized evaporation will be implemented. ;

[0205] Forced evaporation retention coefficient This involves compressing the pheromone of relevant node pairs to 25% of their original value (instead of reducing it to zero, retaining a small amount of historical information to prevent path vacuum in extreme cases).

[0206] Step 4: Synchronize and update all nodes Caching, used for this round and subsequent iterations. , calculate.

[0207] It should be noted that, The reason for using 0.25 instead of 0 is that if the obstacle moves at high speed and there is a perception delay, completely reducing the pheromone level to zero would create a large area of ​​blank path due to perception errors, causing ants to get stuck in random searches in that area. Retaining 25% effectively weakens the guiding effect of erroneous path information and also preserves the basis for path reconstruction when the obstacle is removed again. Simulation results confirm that... exist The algorithm maintains stability within the range.

[0208] Corresponding to the identification of failed nodes, nodes whose spacing increases due to the distance from obstacles ( The implementation of enhanced pheromone protection includes:

[0209] ;

[0210] in , The upper limit of pheromones (take) This mechanism enables ant colonies to quickly identify and utilize newly opened path resources as obstacles recede and safe passages expand, avoiding missing better paths due to insufficient historical pheromones.

[0211] After the obstacle position is updated in each frame, all nodes Cache is refreshed synchronously. The volatility coefficient follows the formula described in Example 1, which measures the distortion factor:

[0212] ;

[0213] ;

[0214] in After the obstacle position is updated in each frame, Cache refresh synchronously. and It will update automatically. It responds to obstacle movement in real time without any additional steps. When an obstacle approaches a node... Decrease The index increases. rise, The system automatically increases pheromone levels and accelerates evaporation, creating an adaptive repulsion field against dynamic obstacles. The processing time for a single frame of the above-mentioned linkage update is less than 0.2 ms, which does not affect the real-time performance of the main loop.

[0215] In dynamic scenarios, BAS pre-search is not only used for initialization, but also acts as an emergency patch when path crises are triggered:

[0216] Path crisis trigger condition: If the optimal path cost of the elite ant exceeds the threshold in 3 consecutive iterations... If the increase exceeds 30% (indicating that the current optimal path is blocked by obstacles), a local emergency search of BAS is triggered: identifying the start and end nodes of path segments in the current optimal path where collision detection failed. , ; The calculation adopts the comprehensive cost described in Example 1. ,in Use the current frame The updated G(q) is recalculated by looking up the table to ensure that the cost assessment is synchronized with the current obstacle state.

[0217] by This is the starting position of BAS. For the objective, the emergency search cost function is geodesic distance:

[0218] ;

[0219] in With the current frame The updated G(q) calculation automatically reflects the latest obstacle positions; the search is guided by geodesic distance induced by the metric tensor, naturally deviating emergency repair path segments from singular configurations and areas of dense obstacles; a 20-step rapid BAS search is performed, requiring distance awareness. Compared to the initial pre-search, it is smaller, allowing for a finer local search; the new path segment nodes found by BAS are assigned pheromone values. After reconnecting the repaired path segment to the global path, perform collision detection and verification again.

[0220] The aforementioned emergency search trigger frequency is approximately once every 15 to 40 iterations in typical scenarios, with a single execution time of approximately This does not affect the real-time performance of the main loop.

[0221] To ensure the feasibility of the above mechanism, the execution timing of a single planning loop is fully described below (time axis in milliseconds), as shown in Table 3-1:

[0222] Table 3-1 Execution Timing Table:

[0223] ;

[0224] Cycle period approximately Approximately 1 ms is used for dynamic response processing, and approximately 8-9 ms are used for ant colony iteration. The obstacle perception frequency is... Executed asynchronously with the planning loop: Failure node identification is triggered once each frame of obstacle data arrives, and subsequent planning loops use the latest cached data. There is no need to wait for the next frame of sensor data.

[0225] The following is an explanation of the engineering rationale for the key parameters: Regarding The value is based on the following: This value is set to approximately 58% of the safety margin of the connecting rod capsule (0.05 m × 1.03 ≈ 0.052 m). Forced evaporation is triggered when an obstacle enters within 60% of the safety margin, allowing sufficient reaction distance for the algorithm. If the value is set too small (…), it will cause the obstacle to collapse. This will cause minute sensory noises to frequently trigger forced evaporation, disrupting pheromone stability; if the setting is too large ( The failure node identification is too slow and may miss rapidly approaching obstacles.

[0226] about The value is determined based on the speed of motion. Dynamic obstacles (covering the normal speed range of a person's arm movement), with a perception update frequency of 20 Hz, are displaced per frame. The limit is not exceeded. Within this range, historical pheromones have passed through... A single forced evaporation can reduce it to its initial value. This is sufficient to eliminate the guiding effect of erroneous paths without triggering a global pheromone collapse.

[0227] The following are the simulation verification results for dynamic scenarios: The following four schemes are compared using the MATLAB simulation platform, as shown in Table 3-2 below:

[0228] • DA (Baseline): Standard ACO, all restarts after obstacle changes;

[0229] • Solution DB: Standard ACO + Local replanning of collision paths (without modifying pheromones);

[0230] • Solution DC: This invention uses a dynamic mechanism and does not include BAS emergency search;

[0231] • Solution DD: The complete solution of this invention (including BAS emergency search).

[0232] Table 3-2 Comparison of Dynamic Scene Simulation Results:

[0233] ;

[0234] Table 3-2 presents the overall performance differences of the four schemes from the perspective of five statistical indicators. The following further reveals the frame-by-frame operation process of the dynamic response mechanism of the present invention in a single obstacle displacement event.

[0235] See Figure 5 , Figure 5 In the dynamic obstacle scenario of Example 3, taking the approach-away event of dynamic obstacle D1 as an example, the curves showing the synchronous changes of the minimum distance between obstacles (left vertical axis, blue solid line), the forced local evaporation coefficient (right vertical axis, blue dotted line), and the normalized local pheromone concentration (right vertical axis, orange solid line) of the path key node pair with the perception frame number; the horizontal dashed line is the failure node identification trigger threshold.

[0236] Figure 5 The above quantitative conclusions are supported by a temporal dynamic dimension, namely, how the response occurs frame by frame, and whether the causal timing between the triggering conditions and pheromone changes is consistent with the mechanism. The figure uses a complete approach-away event of dynamic obstacle D1 (frame number 0 to 200) as an example to simultaneously draw the minimum distance between obstacles for key nodes on the path. Forced local volatility coefficient and normalized local pheromone concentration, trigger threshold The corresponding critical spacing values ​​are marked with horizontal dashed lines. The evolution of the curve in the figure can be divided into three stages;

[0237] Phase 1 (from frame 0 to frame 75, before the obstacle approaches and triggers): Depend on The volatility coefficient continues to decline, while the base value remains unchanged. The pheromone concentration remains stable. During this stage, the obstacle has not yet entered the trigger zone, and the algorithm runs according to normal ant colony iteration. The pheromone gradient field is not disturbed, demonstrating the complete protection of normal path information when the obstacle distance is sufficient.

[0238] Phase Two (approximately from frame 75 to frame 125, obstacles continue to approach and cross the trigger threshold): Once the threshold is breached (the horizontal dashed line), the failure node identification mechanism is activated, forcing a sharp increase in the local evaporation coefficient. The pheromone concentration is then compressed back to its original value within approximately two sensing frames (about 100 ms). This corresponds to the sharp drop in the orange curve around frame 90 in the figure. It is worth noting that the triggering of the evaporation coefficient and the timing of the spacing decrease are strictly synchronized, and the inter-frame delay is only limited by the sensing refresh rate (20 Hz). This is consistent with the measured results in Table 3-2, where the average DC and DD response times of the scheme are about 48 ms to 51 ms. This indicates that there is no additional delay at the algorithm level in the pheromone suppression response, and the closed-loop timeliness of "sensing is response" is directly verified in the time domain curve.

[0239] Phase 3 (approximately frames 125 to 200, obstacles exit the danger zone): Once the pheromone level rises again and surpasses the trigger threshold once more, the pheromone protection enhancement mechanism is activated:

[0240] ;

[0241] The evaporation coefficient synchronously dropped back to its baseline value, and the pheromone concentration steadily climbed from its extremely low point, recovering to approximately 85% of its original value by around frame 200. This recovery process demonstrates that the invention did not completely reduce the pheromone level to zero during the forced evaporation phase. Instead of 0%, the 25% base pheromone retained acts as a "seed" for path reconstruction after obstacles are removed, enabling the pheromone gradient field to be repaired autonomously within tens of frames without relying on a global restart.

[0242] The difference in path crisis repair success rate between scheme DC and scheme DD further illustrates that relying solely on pheromone evaporation adjustment is limited in exploration efficiency when obstacles completely block narrow passages; BAS emergency search provides candidate repair path segments within 20 steps in a deterministic manner, making up for the inherent shortcomings of ant colony random exploration under time-constrained conditions. The two have a clear division of labor and non-overlapping functions in the dynamic response mechanism.

[0243] The following are special notes on how this dynamic mechanism handles extreme conditions: If an obstacle's speed exceeds the response range; if the dynamic obstacle's speed... At this point, the sensor refresh rate exceeds its coverage, and the displacement in a single frame may exceed the entire safety margin. In this situation, the method degenerates into: forcibly evaporating the pheromones of all node pairs intersecting with the new obstacle bounding box, equivalent to local road segment replanning, and triggering a BAS emergency search, with a response time of approximately... It is still better than a global reboot.

[0244] If an obstacle completely blocks the only feasible path: If the narrow path planned by the path is completely blocked by a dynamic obstacle and there is no alternative path, the BAS emergency search cannot find a repair segment that has passed the collision detection within 20 steps. At this time, the algorithm returns to the "path unreachable" state and keeps the end effector in place (corresponding to the safety stop logic of the upper control system) until the path is reopened.

[0245] If there is a loss of sensing data frames: If two consecutive frames of sensing data fail to arrive (timeout threshold 100 ms), the algorithm will automatically remove all dynamic obstacles. Temporarily set to 0 (the most conservative estimate, considering the obstacle to expand to a range of 0.2 m around the current frame position), at this time By maximizing the pheromone concentration, the pheromone evaporates more rapidly, causing the robotic arm to automatically tend towards a more conservative, wider-spaced path, thus ensuring safety in the event of a sensor failure.

[0246] Example 4

[0247] This embodiment uses a six-degree-of-freedom serial robotic arm as the object. Six obstacles are set in the workspace, including three convex obstacles and three L-shaped groove obstacles. The total volume of the obstacles accounts for about 28% of the workspace, which is a high obstacle density scenario.

[0248] In such scenarios, the set of feasible nodes in the configuration space exhibits a distinct island-like distribution. Ants, starting from the initial configuration, are highly susceptible to getting trapped in local feasible regions and unable to find a complete path to the target configuration. The standard ant colony algorithm typically achieves a path planning success rate of less than 55% in such scenarios, and its convergence speed is significantly reduced due to frequent dead-end backtracking. This embodiment addresses these issues.

[0249] The configuration space node set is presampled using the PRM method, and the total number of nodes is... Threshold for distance between adjacent nodes Initial configuration Target configuration Ant count Maximum number of iterations The main parameters of the ant colony are consistent with those in Example 1.

[0250] The longhorn beetle whisker algorithm is adopted, which undertakes three specific functions in this embodiment, corresponding to different stages of path planning. The functions interact with each other through pheromone states to form a closed-loop cooperation mechanism.

[0251] The first function is multi-starting-point pre-search and hierarchical pheromone seeding. This embodiment addresses the feasible region island problem in high obstacle density scenarios by deploying multiple longhorn beetle individuals in the configuration space to perform parallel pre-search. Specifically, the configuration space is uniformly divided along each joint dimension. From the sub-regions, sub-regions with more than 40% of nodes passing collision detection were selected. There are approximately 12-18 individuals (typically 12-18 in a given scenario). Within each sub-region, a collision-free node is randomly selected as the starting position for each beetle. Each beetle uses the direction from the center of its sub-region towards the target configuration as its initial search axis and independently performs 30 steps of BAS search, requiring distance awareness. Step length The distance metric in the cost function is The starting point G(q) of each sub-region reflects the local singularity and obstacle distribution, which makes each longhorn beetle individual naturally inclined to path segments with high operability and large obstacle spacing in the pre-search stage, thus providing a higher quality initial path segment for stratified pheromone sowing.

[0252] The search trajectory records of each individual longhorn beetle are a set of path segments. For a complete path segment that connects the starting configuration to the target configuration, a pheromone value is assigned. For local path segments that, while not fully connected, cover blank areas of the configuration space (i.e., sparsely connected candidate nodes within that area), assign a value. No value is assigned to segments that have not passed through any valid nodes. The above-mentioned hierarchical pheromone seeding strategy enables the ant colony to have a coarse-grained perception of the feasible domain distribution of the entire configuration space at the initial iteration, rather than accumulating initial pheromones only near the initial configuration, thus fundamentally avoiding the problem of early local locking in high obstacle density scenarios.

[0253] The second function is pheromone gradient fault repair during the mid-iteration phase. During ant colony search, due to high obstacle density, a "fault" phenomenon occurs in the pheromone gradient field: obstacles separate two feasible islands, preventing ants from accumulating path pheromones across the barriers, causing the pheromone gradients of the two islands to converge individually but remain disconnected. This embodiment designs a fault detection and BAS-oriented repair mechanism: every... Each iteration performs a fault detection, specifically by checking whether the optimal ant path starting from the initial configuration can reach the target configuration; if the optimal ant path is at a certain node... If the interruption occurs (the set of allowed access nodes is empty), then... The starting position of the longhorn beetle, with The direction pointing to the target configuration is the search axis. A targeted BAS search is performed, requiring the sensing distance to be reduced to... (Smaller than the pre-search phase, allowing for fine-grained searching in narrow channels), step size Perform 40 steps.

[0254] In BAS-oriented repair search, the cost function adds an attraction term for the current location and the boundary of the feasible region island:

[0255] ;

[0256] in With nodes Place At the target node Calculation of the mean metric tensor; Configure the set of candidate nodes within the target island (by from) (Pre-labeled reverse connectivity analysis of the starting point) This is the fault attraction coefficient. This attraction term causes individual longhorn beetles to simultaneously experience the traction of the target configuration and the attraction of the opposite island boundary during their search, significantly increasing the probability of crossing obstacle barriers. Preserving the Euclidean norm is used to measure the spatial proximity of nodes to the boundary of isolated islands on the opposite side, with low computational cost. After successful repair search, newly discovered cross-fault nodes are assigned pheromone values. Then, perform collision detection verification again; if no connected path is found within 40 steps, then... Nearby Retry after the starting position of the radius random perturbation, up to a maximum of 3 retries.

[0257] The third function is to refine the elite path BAS in the later stages of convergence. When the ant colony iteration enters the later stages ( When the cost of the globally optimal path changes by less than 0.5% over 10 consecutive iterations, the ant colony is considered to have essentially converged. At this point, BAS refinement is performed on the current globally optimal path. The specific steps are as follows: traverse each intermediate node of the optimal path. (excluding start and end nodes), with The location of the longhorn beetle is indicated by the local tangent direction of the path. Using random directions within the normal plane as the search axis, a 10-step BAS refinement search is performed, requiring distance awareness. Step length The refinement cost function is:

[0258] ;

[0259] in , , The three terms correspond to singularity, obstacle spacing, and velocity mutation, respectively, and are independent of path length. At this stage, path length is no longer optimized; the focus is on improving path execution quality. Collision detection is performed immediately after each node is refined. If the collision detection fails after refinement, the original node is retained. After full path refinement is complete, the refined nodes are replaced with the optimal path, and the global optimal path cost is updated.

[0260] After the entire path is refined, based on the comprehensive cost Recalculate the refined path If the cost after refinement is lower than before refinement, then update the globally optimal path; otherwise, retain the original path.

[0261] The refined optimal path nodes are assigned pheromone values. This will further strengthen the guidance in the remaining iterations.

[0262] The collaborative relationship among the three functions is reflected in the following ways: First, the function provides the ant colony with a coarse-grained prior of the global feasible region through hierarchical pheromone seeding, enabling the early exploration of the ant colony to be unrestricted by local isolated areas. Second, when the ant colony encounters a gap in its autonomous search, the deterministic search capability of BAS precisely connects the paths between isolated areas, restoring the global connectivity of the pheromone gradient. Third, after the ant colony has macroscopically converged, BAS refines the elite paths at a microscopic level, compensating for the inherent deficiency of insufficient precision in detailed optimization at the ant colony search level. These three functions correspond to the three levels of "global perception—local connectivity—fine optimization," respectively. The beetle whisker algorithm intervenes at each level with search parameters most suitable for the characteristics of that level, forming a truly complementary advantage with the ant colony algorithm.

[0263] In high obstacle density scenarios, a comparison was made with Example 1 (BAS performs only a single pre-search). Results from 50 independent runs of each method showed that: the path planning success rate increased from 81% in Example 1 to 94%; the average number of convergence iterations decreased from 103 to 76; the proportion of singular neighborhood nodes in the optimal path further decreased from 4.1% to 1.8%; and the average maximum change in joint velocity decreased from... Down to The aforementioned improvements primarily stem from the second function's targeted solution to the islanding problem in high obstacle density scenarios, and the third function's further improvement of the quality of the converged path.< / inertia> < / mass> < / inertial> < / inertial>

Claims

1. A method for obstacle avoidance path planning for a robotic arm based on an improved ant colony algorithm, characterized in that, Includes the following steps: S1. Construct an obstacle geometry model of the robotic arm's workspace, approximating each link of the robotic arm with a capsule-like envelope, and using the PRM presampling strategy to discretize the 3D workspace into a set of candidate nodes for joint configuration space. And sparse connectivity graph; after the PRM graph is constructed, a metric tensor is assigned to the joint configuration space. The induced non-uniform metric structure, wherein the metric tensor is defined as: ； in The normalized joint equivalent inertia diagonal matrix is ​​automatically obtained from the URDF file and satisfies the following conditions. , ; , The weighting coefficients are positive real numbers; For nodes The operability index is defined by the ratio of the minimum to the maximum singular value of the Jacobian matrix. It is a singularity index; This is the normalized value of the minimum distance between each link capsule and the nearest obstacle; , For positive scalar functions that depend on the current configuration; , They are respectively and For joint angle vectors The gradient column vector is approximated using the finite difference method; By strictly positive definite matrix It consists of two positive semi-definite outer product terms, is globally strictly positive definite, and its minimum eigenvalue satisfies ; For any pair of adjacent nodes in the connected graph The mean of the tensor is measured at both ends. Define the local geodesic distance approximation: ； The and the geodesic distance from each candidate node to the target node Pre-calculate and cache the PRM graph after it is built; S2. The longhorn beetle whisker search algorithm is invoked to perform a pre-search within the configuration space. The distance between nodes is measured using the local geodesic distance pre-calculated and cached in step S1. Obtain an initial feasible path; perform non-uniform initialization of the ant colony pheromones based on this path: assign values ​​to the pheromones of nodes traversed by the pre-searched path. The remaining nodes are assigned values , ; S3, using the pre-calculated cache from step S1 Constructing heuristic functions: ； in To prevent division by zero regularization of small values; the heuristic function comprehensively reflects the node The operability, obstacle spacing and inertia-weighted joint distance information at the location are used to automatically assign smaller heuristic values ​​to nodes in the singular configuration neighborhood and obstacle-dense areas. S4. The ant constructs the path iteratively based on the following state transition probabilities: ； in , These are the influence indices of pheromones and heuristic functions, respectively. For ants The set of nodes currently allowed to be accessed; the allocation of pheromone increments is determined by the comprehensive path cost, which is a weighted average of geodesic length cost and velocity direction change penalty cost: ； in , The weighting coefficients are positive real numbers; Inertia-weighted joint velocity mutations: ； and middle Sharing the same URDF parsing results; pheromone increment ; S5, with Metric tensor relative to fundamental metric The normalized value of the additional trace contribution is used as a local metric for the distortion. Drives non-uniform pheromone evaporation: ； node The volatility coefficient is: ； in Based on the basic volatility coefficient, The adjustment coefficient for the distorted variable. This is the upper limit of the volatility coefficient; for all... Node pairs with endpoint Pheromones are updated using the following formula: ； This causes the pheromone evaporation rate in the neighborhood of unusual configurations and areas with dense obstacles to be higher than in normal areas; S6. Determine the termination condition: If the current iteration count reaches the preset maximum iteration count. , or continuous The change in the overall cost of the globally optimal path within the next iteration is less than the preset convergence threshold. If the current iteration is positive, then the optimal collision-free path obtained in the current iteration will be output. Otherwise, return to step S4.

2. The obstacle avoidance path planning method for a robotic arm based on an improved ant colony algorithm according to claim 1, characterized in that, The normalized joint equivalent inertia matrix mentioned in step S1 The method of obtaining it is: parsing the moment of inertia tensors of each link from the URDF file of the robotic arm. With its traces It is the equivalent scalar moment of inertia, and is normalized.

3. The obstacle avoidance path planning method for a robotic arm based on an improved ant colony algorithm according to claim 1, characterized in that, The scalar function described in step S1 , The definitions are as follows: ； in , The shape coefficient is a positive real number, and is related to the above. , They are mutually independent; the function varies with singularity. Elevation or barrier spacing Decreasing and monotonically increasing, making The weights of the corresponding outer product terms adaptively increase with the degree of risk. when and hour, , , Degenerate into The dominant inertia weighted near-Euclidean metric.

4. The obstacle avoidance path planning method for a robotic arm based on an improved ant colony algorithm according to claim 1, characterized in that, The gradient described in step S1 and The finite difference method is used for approximate calculation: for the ... For each joint component, an adaptive step size is used: ； If the difference does not exceed the joint limit Using central difference: ； ； in For the first Unit vectors along each coordinate axis; one-sided difference is used near the joint limit; the gradient and... , Precompute and cache on the candidate node set of the PRM graph.

5. The obstacle avoidance path planning method for a robotic arm based on an improved ant colony algorithm according to claim 1, characterized in that, The cost function of the pre-search phase of the beetle whisker search algorithm in step S2 is defined as follows: ； in With nodes Place At the target node Calculation of the mean metric tensor; The absolute minimum distance between each link capsule and the nearest obstacle; This is the safety distance threshold; This represents the collision penalty coefficient.

6. The obstacle avoidance path planning method for a robotic arm based on an improved ant colony algorithm according to claim 1, characterized in that, In step S5, the volatility coefficient at each node satisfies the following condition: in normal regions far from singular configurations and obstacles, , ; In the neighborhood of singular configurations ( ) or areas with dense obstacles ( ) place, or exponential growth made Monotonically increasing, Increase as it goes until it reaches its upper limit. Pheromones decay more rapidly at such nodes, inhibiting ant colonies from converging on dangerous areas; Required and It is cached during the pre-calculation in step S1.

7. The obstacle avoidance path planning method for a robotic arm based on an improved ant colony algorithm according to claim 1, characterized in that, It also includes a dynamic obstacle response step: acquiring the current position of the obstacle at a preset frequency and recalculating each candidate node. Cache and trigger middle Automatic updates; minimum obstacle spacing reduced beyond a threshold. For nodes, forced local pheromone evaporation is performed; for nodes whose spacing increases beyond a threshold, pheromone protection enhancement is performed according to the following formula: ； If the overall cost of the optimal path increases by more than a preset percentage threshold within at least three consecutive iterations, the beetle search algorithm is triggered to perform an emergency local search on the collision path segment. The emergency search cost function is based on the current frame. Updated The metric involves assigning pheromone values ​​to the corresponding nodes in the search results to repair the initial values, and then re-performing collision detection and verification.

8. The obstacle avoidance path planning method for a robotic arm based on an improved ant colony algorithm according to claim 1, characterized in that, When the number of degrees of freedom of the robotic arm increases to a redundant configuration, the number of joints... At that time, the Jacobian matrix For underdetermined matrices, the operability index is changed to... Calculate the ratio of the minimum to the maximum singular value; The dimensions automatically expand with the number of joints; The dimensions expand accordingly, and positive definiteness is determined by each Strict guarantees are in place; a null space utilization margin penalty is further introduced into the state transition probability, with the minimum normalized margin between each joint and the joint limit being used: ； Characterizes the degree of redundancy degradation; adds a zero-space margin penalty term to the path cost function. Weight It increases adaptively with the iteration process.

Images