Robot arm path planning method based on improved a* ant colony fusion artificial potential field

Abstract

The application discloses a mechanical arm path planning method based on improved A* ant colony fusion artificial potential field, and belongs to the technical field of surgical mechanical arms. The improved A* ant colony is used for single search by improved A* ant colony prediction cost H(m), a relatively optimal path is quickly planned, the pheromone rule and path selection probability of the ant colony algorithm are improved, the improved A* ant colony is fused with the improved bidirectional ant colony algorithm to realize traversal of an optimal path in a dynamic process, the artificial potential field method is combined to realize optimization of a mechanical arm operation path, a smoothness at a jump point is realized by a cubic B-spline interpolation function, an optimal path is output, and the requirements of high-efficiency obstacle avoidance, good smoothness, few turning points and fast search of the teleoperation bronchoscope mechanical arm path planning are met. The application can control the operation path of the surgical mechanical arm, meet the continuity requirements of acceleration and speed during operation, improve the stability and smoothness of operation, and solve the surgical mechanical arm path planning problem.

Description

Technical Field

[0001] This invention discloses a robotic arm path planning method based on an improved A* ant colony fusion artificial potential field, belonging to the field of surgical robotic arm technology. Background Technology

[0002] Surgical navigation systems are an important component of computer-assisted surgery. They correlate preoperative images of the patient with intraoperative anatomical structures, track the bronchoscope tip sensor in real time, and display and update its position on the patient's image as the bronchial tip. By observing the relative position of the bronchoscope tip to the lesion site in the patient's image, the surgeon navigates the bronchoscope to reach the target location in the bronchioles.

[0003] The path planning method provides sensor location guidance, determining the bronchial opening from which the sensor should be inserted, at what angle, whether the target point has been reached, the expected path of the bronchoscope, and whether the angle needs to be adjusted for sampling. It also maintains airway patency by treating the airway mucosa as an obstacle during path planning. Among numerous path planning methods, the ant colony algorithm is widely used due to its strong robustness, positive feedback mechanism, and ease of implementation.

[0004] A* ant colony algorithm combines heuristic search with breadth-first search to find obstacle avoidance paths in planar or spatial obstacle environments. It is relatively effective when obstacle information is known and is suitable for global planning. However, it lacks local planning capabilities in certain environments, such as in environments containing obstacles, where it is difficult to avoid obstacles and the planned path may become a broken line. The APF algorithm is suitable for local planning and can achieve certain obstacle avoidance functions, but the planned path may not be optimal and may encounter arrival problems and getting stuck in local problems.

[0005] In complex tracheal environments, path searching for robotic arms during medical history taking and quarantine is challenging. Conventional path planning methods are prone to getting stuck in local optima, necessitating new approaches to address this problem. Summary of the Invention

[0006] To overcome the shortcomings of existing path planning methods in the application of remotely operated bronchoscope robotic arms, this invention proposes a robotic arm path planning method based on an improved A* ant colony fusion artificial potential field. Leveraging the rapid global path planning capabilities of A* ant colonies and their ease of integration with other methods, this invention combines A* ant colony fusion with artificial potential field methods. A "reward-based" strategy is used to update global pheromones, allowing pheromones on the current optimal path to be dynamically adjusted based on the current optimal solution, avoiding getting trapped in local optima. Ant colonies occupying obstacle space are not processed; instead, a bidirectional ant colony search mechanism is employed to shorten the distance between the starting point and the target point, improving the initial effectiveness of the ant colony and accelerating the convergence speed.

[0007] The objective of this invention is achieved through the following technical solution:

[0008] The present invention provides a robotic arm path planning method based on an improved A* ant colony fusion artificial potential field, comprising the following steps:

[0009] Step 1: Construct the motion space model of the robotic arm and initialize the system parameters;

[0010] Determine the starting point S and ending point G of the robotic arm's movement, construct the motion space model of the robotic arm, and transform the motion space model of the robotic arm into a graph structure, where each node in the graph structure represents the state of the robotic arm at a certain position and posture;

[0011] Initialize the robotic arm parameters, including the robotic arm itself, angle, and length;

[0012] The parameters for initializing the ant colony method include the number of ants m, the number of iterations k, the pheromone concentration, pheromone intensity Q, and pheromone volatility coefficient on the path at the initial moment.

[0013] Initialize the parameters related to the A* method;

[0014] Initialize the relevant parameters for the artificial potential field method;

[0015] Step 2: Perform a single-item search using the improved A* method to plan a better path, which includes the following sub-steps;

[0016] Step 2-1: Determine the cost function F(m) of the A* method;

[0017] The A* method selects the search direction using the cost function F(m), expands outward from the starting point, obtains the cost value of each surrounding node by estimating the cost H(m), selects the minimum cost value as the next expansion point, repeats this process until the endpoint is reached, and generates a path from the starting point to the endpoint.

[0018] During the search process, the optimal path with the minimum cost is determined by evaluating the distance between nodes and the estimated remaining distance. The cost function F(m) of the A* method is shown in equation (1):

[0019] F(m)=G(m)+H(m) (1)

[0020] Wherein, the cost function F(m) is the sum of the actual cost and the estimated cost, G(m) is the actual cost from the starting position to node m, H(m) is the estimated cost from the current node m to the target point G, and m is the extended node;

[0021] Step 2-2: Determine the estimated cost H(m) of the A* method;

[0022] The estimated cost H(m) is a distance parameter. Based on the Euclidean distance heuristic, a turning penalty factor is added as the estimated cost to evaluate the operating efficiency of the robot arm. The estimated cost H(m) is shown in equation (2):

[0023]

[0024] Where, (x s ,y s (x) represents the coordinates of the starting position S. g ,y g ) represents the coordinates of the target location G; β is the turning penalty factor, as shown in equation (3):

[0025]

[0026] Where, θ goal The reference angle is the angle between the line connecting the robot's current node and the target node and the horizontal direction, and θ is the angle between the line connecting the robot's current node and the next node and the horizontal direction.

[0027] Steps 2-3: Decomposition of the next node of the estimated cost H(m);

[0028] The estimated cost H(m) affects the path search speed of the A* method. A smaller H(m) will result in too many child nodes being formed during the search process, reducing the computation speed and search efficiency. When H(m) is equal to the actual cost from node m to the destination G, the A* method will not form additional child nodes during the search, and the search efficiency will be improved accordingly.

[0029] The estimated cost H(m) from the current node m to the target point G is the cost d from the current node m to the next node n. mn The cost d from the next node n to the target node G nG The sum is shown in equation (4):

[0030] H(m)=d mn +d nG (4)

[0031] Further, the cost d from the current node to the next node in the estimated cost is analyzed. mn The cost d from the next node to the target node nG A weight adjustment factor is set so that the estimated cost changes with the search node. The improved heuristic function is shown in equations (5) and (6).

[0032]

[0033]

[0034] Where, η mnThe improved heuristic function is λ, which is the path balance adjustment factor.

[0035] Step 3: The optimal path is traversed in the dynamic process by combining the A* method and the bidirectional ant colony method;

[0036] Using the angle of rotation of a bronchoscope as an ant, in the pre-selection, the path planned by the A* method is used as the element in set A. The element in set A of each population of ants is used as the target point and the path is searched. At the same time, the shortest path is continuously selected.

[0037] Each population will compare its own shortest path to find the shortest path, which is the appropriate angle for the rotation of each segment of the bronchoscope robotic arm.

[0038] Specifically, it includes the following sub-steps:

[0039] Step 3-1: Determine the initial pheromone allocation strategy;

[0040] An unbalanced distribution strategy is adopted so that the pheromone concentration of the better path planned by the A* method is higher than that of other paths, avoiding the initial blind search caused by the balanced distribution of the initial pheromone concentration in the ant colony method, reducing deadlock, and accelerating the convergence speed and optimization ability.

[0041] Step 3-2: Perform a bidirectional search using the bidirectional ant colony method;

[0042] The bidirectional ant colony method is used for bidirectional search. Starting from the beginning and the end point at the same time, the search range is expanded in the direction of the other until the two search paths meet or cannot be expanded further, thus improving the efficiency of path search.

[0043] The pheromones released by the two groups of ants cannot be recognized by each other, and because the ants traveling in opposite directions have different destinations, their estimated costs are different, as shown in equation (7):

[0044]

[0045] Among them, D m Let x be the distance from node m to the destination; s ,y s (x) is the starting coordinate, (x) g ,y g () represents the coordinates of the target point;

[0046] If pheromones from different ant colonies appear in the same grid at a certain moment, ants from the two groups will meet, resulting in a newly constructed path. The corrected A* mathematical expression is shown in equation (8):

[0047]

[0048] in, L represents the amount of pheromone left by the k-th ant as it travels from node m to node n; Q represents the total amount of pheromone left by the ant along its path in one pass; L represents the total amount of pheromone left by the ant in one pass. k Let be the total length of the path traveled by the k-th ant during this search for the target;

[0049] Step 3-3: Determine the pheromone update strategy;

[0050] By using a coefficient σ, different treatment methods are applied to different ants, which accelerates the convergence speed of the ant colony method and makes the path search more instructive.

[0051] When the length of the path ants travel is greater than the average length of all ants in each generation, the pheromone concentration of these ants is reduced; conversely, the pheromone concentration is increased so that subsequent ants gradually move toward the optimal path.

[0052] Local pheromone updates are used. Each ant in each generation updates the pheromone of a node after passing through it. The update method is shown in equation (9):

[0053] τ mn (t+1)=(1-ρ)τ mn (t)+Δτ mn (t) (9)

[0054] Where ρ is the pheromone evaporation rate parameter, used to control the degree of pheromone retention, with a value range of [0, 1], and Δτ mn τ represents the pheromone concentration per unit distance along the path from node m to node n. mn It is the sum of the pheromone concentrations of all ants along the path from node m to node n;

[0055] The pheromone rules are shown in equations (10) and (11):

[0056]

[0057]

[0058] Where σ is the pheromone regulation factor, a constant representing the intensity of the pheromone increment, and L ave L is the average path length of all ants in each generation. k For the path taken by each ant;

[0059] Steps 3-4: Determine the state transition probability to identify obstacles;

[0060] In the path selection probability, obstacle rejection weights are used, and heuristic factors are employed to modify heuristic information. This ensures that ants obtain effective obstacle-free paths during the search process and increases path selection diversity and probability. As shown in equation (12):

[0061]

[0062] in, Let $k$ be the reciprocal of the weight of the ant $k$ position it moves from free grid $f$ to obstacle grid $o$, and $k$ be the path selection probability. Inversely proportional, as shown in equation (13), d(f,o) is the repulsion distance from the free grid f to the barrier grid o, as shown in equation (14):

[0063]

[0064]

[0065] in, Let f be the number of times ant k visits grid f, which increases with each visit of ant k to the grid;

[0066] Step 4: Optimize the path by incorporating the artificial potential field method;

[0067] Based on the path planning using the bidirectional ant colony method in step 3A*, the force field between the current position and the target position of the robotic arm is determined. According to the direction of the resultant force of the force field, the path of the robotic arm is adjusted to allow it to bypass obstacles and approach the target position. This specifically includes the following sub-steps:

[0068] Step 4-1: Determine the attractive and repulsive forces of the obstacle using the artificial potential field method;

[0069] The artificial potential field method involves the existence of repulsive forces around obstacles, attractive forces around key target points, and collision ranges around obstacles, with movement occurring under the influence of these two potential fields.

[0070] The relationship between the gravitational potential field P and the distance between the moving robotic arm and the key target point is shown in Equation (15):

[0071]

[0072] Where μ is the scaling factor and d1 is the Euclidean distance;

[0073] The corresponding gravitational force I is shown in equation (16):

[0074]

[0075] in, The symbol is gradient;

[0076] The relationship between the repulsive potential field K and the distance between the moving robotic arm and the obstacle is shown in equation (17):

[0077]

[0078] Where, λ rej Here, d is the repulsion coefficient, r is the maximum influence range of the obstacle, and d is the maximum repulsion range. mo Let T be the shortest distance between node m and obstacle node o, and the corresponding repulsive force T is shown in equation (18):

[0079]

[0080] Step 4-2: Determine the repulsive force function;

[0081] The distance between the moving robotic arm and the target key point is increased in the repulsion function, and the repulsion gain is modified to reduce the influence of the repulsion, so that the moving robotic arm reaches the target key point. The repulsion function T is shown in equation (19):

[0082]

[0083] Where, d me The distance from the robotic arm to the target key point, including Euclidean distance, with coefficients. It is a constant, taking the value (0,1);

[0084] The repulsive force is determined by the negative gradient; when 0 < d < r, F rej It consists of two parts, F rej =F obs→m +F m→e As shown in equations (20) and (21) respectively:

[0085]

[0086]

[0087] The combined potential field E, obtained by superimposing the gravitational and repulsive potential fields, is shown in equation (22):

[0088] E = P + K (22)

[0089] The resultant force H on the robotic arm is shown in equation (23):

[0090]

[0091] Step 4-3: Step outside the local problem;

[0092] The problem of getting caught in a local state occurs when forces are in equilibrium, and the opposing forces of attraction and repulsion cancel each other out, causing the robotic arm to stop moving.

[0093] The next optimal node generated by the A* method is used as a local virtual target point. A new gravitational force is generated through this virtual target point, breaking the local equilibrium and guiding the robotic arm towards the new virtual target key point. This guides the robotic arm out of the local optimum and continues moving towards the target point. The new gravitational force F... vir As shown in equation (24):

[0094]

[0095] Where: φ is the gain coefficient, which increases with the increase of the number of obstacles, |D n -D vir | 2 The distance from the robotic arm to the key point of the virtual target;

[0096] When the virtual target point is reached, D vir =0, the net force on the moving robotic arm is F = F rej +F att +F vir ;

[0097] Step 5: Integrate the A* ant colony method with the artificial potential field method;

[0098] The A* ant colony method is used for global planning, and the artificial potential field method is used for local planning to achieve the optimal global planning and the obstacle avoidance capability of local planning.

[0099] The cost of searching for key points includes the total cost from the starting point to the key point and the improved heuristic function H(m), as well as the gravitational and repulsive force values ​​of the artificial potential field method.

[0100] The comprehensive cost function F(m) for each key point in the A* ant colony algorithm is shown in equation (25):

[0101] F(m)=G(m)+H(m)+P(m) (25)

[0102]

[0103] Where P(m) is the potential energy at critical node m, d(m,g) is the Euclidean distance from critical node m to target critical point g, b represents the position of the obstacle, and d(m,b) is the Euclidean distance from critical node m to obstacle b. att k rej These are the coefficients of attraction and repulsion, respectively;

[0104] Repeat steps 3 to 5 iteratively until the maximum number of iterations is reached, and then obtain the path with the optimal time and path length.

[0105] Step 6: Construct a cubic B-spline interpolation curve to smooth out jump points;

[0106] The fusion of the A* ant colony method and the artificial potential field method can significantly reduce path length and planning time. The path is smoothed by using a cubic uniform B-spline curve, which achieves path smoothing, eliminates sharp inflection points at jump points, optimizes the curve and meets the requirements of robot movement acceleration and speed continuity, thereby improving the stability and smoothness of the surgical robotic arm operation.

[0107] Among them, the B-spline curve is a generalized form of the Bezier curve, obtained by approximating a polygon. For spatial location-time series {θ} i ,t i}, i = 0, 1, ..., n, and its general form is shown in equation (27):

[0108]

[0109] Among them, Q i (,i=0,1,...,n) are the coordinates of the given control points, N i,k (t) is the k-th degree standardized B-spline basis function, as shown in equation (28):

[0110]

[0111] in, k is the order of the B-spline function, i is the sequence number of the B-spline function, and let 0 / 0 = 0, 0 ≤ t ≤ 1, i = 0, 1, ..., k-1;

[0112] When k=3, the obstacle avoidance position-time series point is used as the model value point to determine the control point through the obstacle avoidance position-time series point in the obstacle space, and a cubic B-spline interpolation curve is established to ensure that the smoothed obstacle avoidance trajectory passes through the key nodes on the obstacle avoidance path selected by the A* method. The i-th segment of the curve is shown in Equation (29):

[0113]

[0114] The key nodes on the planned obstacle avoidance path are selected by the fusion method of A* ant colony method and artificial potential field method, and the optimal planned path is output by smoothing the key nodes using cubic B-spline curves.

[0115] It also includes step 7, which controls the running path of the surgical robotic arm based on the optimal planning path obtained in step 6, to meet the requirements of acceleration and speed continuity during operation, improve the stability and smoothness of operation, make the planned path scheme safer and more efficient, meet actual needs, and solve the problem of surgical robotic arm path planning.

[0116] Beneficial effects:

[0117] 1. The present invention provides a robotic arm path planning method based on an improved A* ant colony fusion artificial potential field. By performing a single-item search using the improved A* method, a better path can be planned, improving the initial effectiveness of the ant colony and accelerating the convergence speed.

[0118] 2. The present invention provides a robotic arm path planning method based on an improved A* ant colony fusion artificial potential field. By fusing the A* method with the bidirectional ant colony method, and adopting a global pheromone update formula with a merit-based reward strategy, the search speed is improved, and the optimal path is traversed in the dynamic process.

[0119] 3. The present invention provides a robotic arm path planning method based on an improved A* ant colony method fused with an artificial potential field. This method integrates the improved A* ant colony method with the artificial potential field method, enhances the obstacle avoidance capability of local planning, achieves optimal planning of global obstacle avoidance path, and achieves better dynamic obstacle avoidance effect.

[0120] 4. The present invention provides a robotic arm path planning method based on an improved A* ant colony fusion artificial potential field. The path is smoothed by cubic B-spline interpolation curves, making the planned obstacle avoidance path safer and more efficient, and improving the stability and smoothness of the surgical robotic arm operation. Attached Figure Description

[0121] Figure 1 This is a flowchart of a robotic arm path planning method based on an improved A* ant colony fusion artificial potential field according to the present invention.

[0122] Figure 2 This is a comparison chart of energy loss for the cost function H(m) in the embodiments;

[0123] Figure 3 This is a schematic diagram illustrating the optimal path in terms of time and path length in the embodiment;

[0124] Figure 4 This is a schematic diagram of the path after being smoothed by three B-spline curves in the embodiment;

[0125] Figure 5 This is a comparison diagram of different path planning methods in the embodiments. Detailed Implementation

[0126] To better illustrate the purpose and advantages of the present invention, the invention will be further described below in conjunction with the accompanying drawings and examples.

[0127] Example 1:

[0128] This embodiment applies a robotic arm path planning method based on an improved A* ant colony fusion artificial potential field, as described in this invention. First, a relatively optimal path is planned using a single-direction search with the improved A* method. Then, the optimal path is traversed dynamically by fusing the improved A* method with the improved bidirectional ant colony method. The robotic arm's operational path is optimized using an artificial potential field. Smoothing at jump points is achieved through cubic B-spline interpolation curves, and the optimal path is output. Figure 1 As shown, the specific steps include:

[0129] Step 1: Construct the motion space model of the robotic arm and initialize the system parameters;

[0130] In this embodiment, a simulation method is used. The simulation environment is Windows 10 64-bit, MATLAB 2019b, i7-9750H CPU, and 16GB of memory.

[0131] Determine the starting point S and ending point G of the robotic arm's movement, construct the motion space model of the robotic arm, and transform the motion space model of the robotic arm into a graph structure, where each node in the graph structure represents the state of the robotic arm at a certain position and posture;

[0132] In this embodiment, a 30×30 obstacle grid environment map is established. Grids with obstacles cannot be passed through, while grids without obstacles can be passed through. To ensure the safety of the robotic arm during movement, the size of the obstacles is divided according to half the area occupied by the robotic arm. Grids that do not fill a whole grid are treated as one grid.

[0133] The matrix composed of 0s and 1s represents the map where the robotic arm needs to find the optimal path, that is, the path optimization effect diagram in a 30×30 grid environment. 0 indicates that the grid can be passed, and 1 indicates that the grid is an obstacle and cannot be passed.

[0134] Initialize the robotic arm parameters, including the robotic arm itself, angle, and length;

[0135] The parameters for initializing the ant colony method include the number of ants m, the number of iterations k, the pheromone concentration, pheromone intensity Q, and pheromone volatility coefficient on the path at the initial moment.

[0136] Initialize the parameters related to the A* method;

[0137] Initialize the relevant parameters for the artificial potential field method;

[0138] Step 2: Perform a single-item search using the improved A* method to plan a better path, which includes the following sub-steps;

[0139] Step 2-1: Determine the cost function F(m) of the A* method;

[0140] The A* method selects the search direction using the cost function F(m), expands outward from the starting point, obtains the cost value of each surrounding node by estimating the cost H(m), selects the minimum cost value as the next expansion point, repeats this process until the endpoint is reached, and generates a path from the starting point to the endpoint.

[0141] During the search process, the optimal path with the minimum cost is determined by evaluating the distance between nodes and the estimated remaining distance. The cost function F(m) of the A* method is shown in equation (1):

[0142] F(m)=G(m)+H(m) (1)

[0143] Wherein, the cost function F(m) is the sum of the actual cost and the estimated cost, G(m) is the actual cost from the starting position to node m, H(m) is the estimated cost from the current node m to the target point G, and m is the extended node;

[0144] Step 2-2: Determine the estimated cost H(m) of the A* method;

[0145] The estimated cost H(m) is a distance parameter. Based on the Euclidean distance heuristic, a turning penalty factor is added as the estimated cost to evaluate the operating efficiency of the robot arm. The estimated cost H(m) is shown in equation (2):

[0146]

[0147] Where, (x s ,y s (x) represents the coordinates of the starting position S. g ,y g ) represents the coordinates of the target location G; β is the turning penalty factor, as shown in equation (3):

[0148]

[0149] Where, θ goal The reference angle is the angle between the line connecting the robot's current node and the target node and the horizontal direction, and θ is the angle between the line connecting the robot's current node and the next node and the horizontal direction.

[0150] Steps 2-3: Decomposition of the next node of the estimated cost H(m);

[0151] The estimated cost H(m) affects the path search speed of the A* method. A smaller H(m) will result in too many child nodes being formed during the search process, reducing the computation speed and search efficiency. When H(m) is equal to the actual cost from node m to the destination G, the A* method will not form additional child nodes during the search, and the search efficiency will be improved accordingly.

[0152] The estimated cost H(m) from the current node m to the target point G is the cost d from the current node m to the next node n. mn The cost d from the next node n to the target node G nG The sum is shown in equation (4):

[0153] H(m)=d mn +d nG (4)

[0154] Further, the cost d from the current node to the next node in the estimated cost is analyzed. mn The cost d from the next node to the target node nG A weight adjustment factor is set so that the estimated cost changes with the search node. The improved heuristic function is shown in equations (5) and (6).

[0155]

[0156]

[0157] Where, η mn The improved heuristic function is λ, which is the path balance adjustment factor.

[0158] Step 3: The optimal path is traversed in the dynamic process by combining the A* method and the bidirectional ant colony method;

[0159] Using the angle of rotation of a bronchoscope as an ant, in the pre-selection, the path planned by the A* method is used as the element in set A. The element in set A of each population of ants is used as the target point and the path is searched. At the same time, the shortest path is continuously selected.

[0160] Each population will compare its own shortest path to find the shortest path, which is the appropriate angle for the rotation of each segment of the bronchoscope robotic arm.

[0161] Specifically, it includes the following sub-steps:

[0162] Step 3-1: Determine the initial pheromone allocation strategy;

[0163] An unbalanced distribution strategy is adopted so that the pheromone concentration of the better path planned by the A* method is higher than that of other paths, avoiding the initial blind search caused by the balanced distribution of the initial pheromone concentration in the ant colony method, reducing deadlock, and accelerating the convergence speed and optimization ability.

[0164] Step 3-2: Perform a bidirectional search using the bidirectional ant colony method;

[0165] The bidirectional ant colony method is used for bidirectional search. Starting from the beginning and the end point at the same time, the search range is expanded in the direction of the other until the two search paths meet or cannot be expanded further, thus improving the efficiency of path search.

[0166] The pheromones released by the two groups of ants cannot be recognized by each other, and because the ants traveling in opposite directions have different destinations, their estimated costs are different, as shown in equation (7):

[0167]

[0168] Among them, D m Let x be the distance from node m to the destination; s ,y s (x) is the starting coordinate, (x) g ,y g () represents the coordinates of the target point;

[0169] If pheromones from different ant colonies appear in the same grid at a certain moment, ants from the two groups will meet, resulting in a newly constructed path. The corrected A* mathematical expression is shown in equation (8):

[0170]

[0171] in, L represents the amount of pheromone left by the k-th ant as it travels from node m to node n; Q represents the total amount of pheromone left by the ant along its path in one pass; L represents the total amount of pheromone left by the ant in one pass. k Let be the total length of the path traveled by the k-th ant during this search for the target;

[0172] Step 3-3: Determine the pheromone update strategy;

[0173] By using a coefficient σ, different treatment methods are applied to different ants, which accelerates the convergence speed of the ant colony method and makes the path search more instructive.

[0174] When the length of the path ants travel is greater than the average length of all ants in each generation, the pheromone concentration of these ants is reduced; conversely, the pheromone concentration is increased so that subsequent ants gradually move toward the optimal path.

[0175] Local pheromone updates are used. Each ant in each generation updates the pheromone of a node after passing through it. The update method is shown in equation (9):

[0176] τ mn (t+1)=(1-ρ)τ mn (t)+Δτ mn (t) (9)

[0177] Where ρ is the pheromone evaporation rate parameter, used to control the degree of pheromone retention, with a value range of [0, 1], and Δτ mn τ represents the pheromone concentration per unit distance along the path from node m to node n. mn It is the sum of the pheromone concentrations of all ants along the path from node m to node n;

[0178] The pheromone rules are shown in equations (10) and (11):

[0179]

[0180]

[0181] Where σ is the pheromone regulation factor, a constant representing the intensity of the pheromone increment, and L ave L is the average path length of all ants in each generation. k For the path taken by each ant;

[0182] Steps 3-4: Determine the state transition probability to identify obstacles;

[0183] In the path selection probability, obstacle rejection weights are used, and heuristic factors are employed to modify heuristic information. This ensures that ants obtain effective obstacle-free paths during the search process and increases path selection diversity and probability. As shown in equation (12):

[0184]

[0185] in, Let $k$ be the reciprocal of the weight of the ant $k$ position it moves from free grid $f$ to obstacle grid $o$, and $k$ be the path selection probability. Inversely proportional, as shown in equation (13), d(f,o) is the repulsion distance from the free grid f to the barrier grid o, as shown in equation (14):

[0186]

[0187]

[0188] in, Let f be the number of times ant k visits grid f, which increases with each visit of ant k to the grid;

[0189] Step 4: Optimize the path by incorporating the artificial potential field method;

[0190] Based on the path planning using the bidirectional ant colony method in step 3A*, the force field between the current position and the target position of the robotic arm is determined. According to the direction of the resultant force of the force field, the path of the robotic arm is adjusted to allow it to bypass obstacles and approach the target position. This specifically includes the following sub-steps:

[0191] Step 4-1: Determine the attractive and repulsive forces of the obstacle using the artificial potential field method;

[0192] The artificial potential field method involves the existence of repulsive forces around obstacles, attractive forces around key target points, and collision ranges around obstacles, with movement occurring under the influence of these two potential fields.

[0193] The relationship between the gravitational potential field P and the distance between the moving robotic arm and the key target point is shown in Equation (15):

[0194]

[0195] Where μ is the scaling factor and d1 is the Euclidean distance;

[0196] The corresponding gravitational force I is shown in equation (16):

[0197]

[0198] in, The symbol is gradient;

[0199] The relationship between the repulsive potential field K and the distance between the moving robotic arm and the obstacle is shown in equation (17):

[0200]

[0201] Where, λ rej Here, d is the repulsion coefficient, r is the maximum influence range of the obstacle, and d is the maximum repulsion range. mo Let T be the shortest distance between node m and obstacle node o, and the corresponding repulsive force T is shown in equation (18):

[0202]

[0203] Step 4-2: Determine the repulsive force function;

[0204] The distance between the moving robotic arm and the target key point is increased in the repulsion function, and the repulsion gain is modified to reduce the influence of the repulsion, so that the moving robotic arm reaches the target key point. The repulsion function T is shown in equation (19):

[0205]

[0206] Where, d me The distance from the robotic arm to the target key point, including Euclidean distance, with coefficients. It is a constant, taking the value (0,1);

[0207] The repulsive force is determined by the negative gradient; when 0 < d < r, F rej It consists of two parts, F rej=F obs→m +F m→e As shown in equations (20) and (21) respectively:

[0208]

[0209]

[0210] The combined potential field E, obtained by superimposing the gravitational and repulsive potential fields, is shown in equation (22):

[0211] E = P + K (22)

[0212] The resultant force H on the robotic arm is shown in equation (23):

[0213]

[0214] Step 4-3: Step outside the local problem;

[0215] The problem of getting caught in a local state occurs when forces are in equilibrium, and the opposing forces of attraction and repulsion cancel each other out, causing the robotic arm to stop moving.

[0216] The next optimal node generated by the A* method is used as a local virtual target point. A new gravitational force is generated through this virtual target point, breaking the local equilibrium and guiding the robotic arm towards the new virtual target key point. This guides the robotic arm out of the local optimum and continues moving towards the target point. The new gravitational force F... vir As shown in equation (24):

[0217]

[0218] Where: φ is the gain coefficient, which increases with the increase of the number of obstacles, |D n -D vir | 2 The distance from the robotic arm to the key point of the virtual target;

[0219] When the virtual target point is reached, D vir =0, the net force on the moving robotic arm is F = F rej +F att +F vir ;

[0220] Step 5: Integrate the A* ant colony method with the artificial potential field method;

[0221] The A* ant colony method is used for global planning, and the artificial potential field method is used for local planning to achieve the optimal global planning and the obstacle avoidance capability of local planning.

[0222] The cost of searching for key points includes the total cost from the starting point to the key point and the improved heuristic function H(m), as well as the gravitational and repulsive force values ​​of the artificial potential field method.

[0223] The comprehensive cost function F(m) for each key point in the A* ant colony algorithm is shown in equation (25):

[0224] F(m)=G(m)+H(m)+P(m) (25)

[0225]

[0226] Where P(m) is the potential energy at critical node m, d(m,g) is the Euclidean distance from critical node m to target critical point g, b represents the position of the obstacle, and d(m,b) is the Euclidean distance from critical node m to obstacle b. att k rej These are the coefficients of attraction and repulsion, respectively;

[0227] Repeat steps 3 to 5 iteratively until the maximum number of iterations is reached, and then obtain the path with the optimal time and path length.

[0228] Step 6: Construct a cubic B-spline interpolation curve to smooth out jump points;

[0229] The fusion of the A* ant colony method and the artificial potential field method can significantly reduce path length and planning time. The path is smoothed by using a cubic uniform B-spline curve, which achieves path smoothing, eliminates sharp inflection points at jump points, optimizes the curve and meets the requirements of robot movement acceleration and speed continuity, thereby improving the stability and smoothness of the surgical robotic arm operation.

[0230] Among them, the B-spline curve is a generalized form of the Bezier curve, obtained by approximating a polygon. For spatial location-time series {θ} i ,t i}, i = 0, 1, ..., n, and its general form is shown in equation (27):

[0231]

[0232] Among them, Q i (,i=0,1,...,n) are the coordinates of the given control points, N i,k (t) is the k-th degree standardized B-spline basis function, as shown in equation (28):

[0233]

[0234] in, k is the order of the B-spline function, i is the sequence number of the B-spline function, and let 0 / 0 = 0, 0 ≤ t ≤ 1, i = 0, 1, ..., k-1;

[0235] When k=3, the obstacle avoidance position-time series point is used as the model value point to determine the control point through the obstacle avoidance position-time series point in the obstacle space, and a cubic B-spline interpolation curve is established to ensure that the smoothed obstacle avoidance trajectory passes through the key nodes on the obstacle avoidance path selected by the A* method. The i-th segment of the curve is shown in Equation (29):

[0236]

[0237] The key nodes on the planned obstacle avoidance path are selected by the fusion method of A* ant colony method and artificial potential field method, and the optimal planned path is output by smoothing the key nodes using cubic B-spline curves.

[0238] In this embodiment, the methods were also compared under the same conditions using simulation. The performance indicators of the different methods are shown in Table 1.

[0239] Table 1

[0240] Optimal path length / m 31.082 28.072 26.224 25.142 Runtime / s 1.8 1.16 0.84 0.53 Number of turning points 26 21 17 13 Expanding the number of nodes 431 396 356 239

[0241] As shown in Table 1, the path planning method of this invention has advantages over other methods in various aspects: compared with the classical genetic algorithm, it reduces the number of expanded nodes by 44%, the number of turning points by 50%, the running time by 70%, and the optimal path length by 19%; compared with the classical ant colony method, it reduces the number of expanded nodes by 40%, the number of turning points by 38%, the running time by 54%, and the optimal path length by 10%; compared with the improved A* ant colony method, it reduces the number of expanded nodes by 32%, the number of turning points by 23%, the running time by 37%, and the optimal path length by 4%. In summary, the method of this invention has high search capability and fast convergence, and can effectively improve search efficiency.

[0242] The simulation path search results are as follows Figure 5 As shown, compared with other methods, the obstacle avoidance path of the robotic arm planned by the fusion of A* ant colony and artificial potential field method is smoother, without jump points and sharp inflection points, and the path length is significantly reduced. It can effectively solve the problems of the ant colony method being prone to getting trapped in local optima and slow convergence speed, improve the actual working efficiency of the teleoperated robotic arm, and improve the motion performance of the teleoperated robotic arm through B spline curve smoothing optimization.

[0243] It also includes step 7, which controls the running path of the surgical robotic arm based on the optimal planning path obtained in step 6, to meet the requirements of acceleration and speed continuity during operation, improve the stability and smoothness of operation, meet actual needs, and solve the surgical robotic arm path planning problem.

[0244] The above detailed description further illustrates the purpose, technical solution, and beneficial effects of the invention. It should be understood that the above description is only a specific embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A robotic arm path planning method based on an improved A* ant colony fusion artificial potential field, characterized in that: Leveraging the advantages of A* ant colony's rapid global path planning capability and ease of integration with other methods, this study combines A / ant colony and artificial potential field methods. A "reward-based" strategy is used to update global pheromones, allowing pheromones on the current optimal path to be dynamically adjusted based on the current optimal path, avoiding getting trapped in local optima. Ants occupying obstacle space are not processed. A bidirectional search mechanism is employed to shorten the distance between the starting point and the target point, improving the initial effectiveness of the ant colony and accelerating convergence. The steps include: Step 1: Construct the motion space model of the robotic arm and initialize the system parameters; Determine the starting point S and ending point G of the robotic arm's movement, construct the motion space model of the robotic arm, and transform the motion space model of the robotic arm into a graph structure, where each node in the graph structure represents the state of the robotic arm at a certain position and posture; Initialize the robotic arm parameters, including the robotic arm itself, angle, and length; The parameters for initializing the ant colony method include the number of ants m, the number of iterations k, the pheromone concentration, pheromone intensity Q, and pheromone volatility coefficient on the path at the initial moment. Initialize the parameters related to the A* method; Initialize the relevant parameters for the artificial potential field method; Step 2: Perform a single-item search using the improved A* method to plan a better path, which includes the following sub-steps; Step 2-1: Determine the cost function F(m) of the A* method; The A* method selects the search direction using the cost function F(m), expands outward from the starting point, obtains the cost value of each surrounding node by estimating the cost H(m), selects the minimum cost value as the next expansion point, repeats this process until the endpoint is reached, and generates a path from the starting point to the endpoint. During the search process, the optimal path with the minimum cost is determined by evaluating the distance between nodes and the estimated remaining distance. The cost function F(m) of the A* method is shown in equation (1): F(m)=G(m)+H(m) (1) Wherein, the cost function F(m) is the sum of the actual cost and the estimated cost, G(m) is the actual cost from the starting position to node m, H(m) is the estimated cost from the current node m to the target point G, and m is the extended node; Step 2-2: Determine the estimated cost H(m) of the A* method; The estimated cost H(m) is a distance parameter. Based on the Euclidean distance heuristic, a turning penalty factor is added as the estimated cost to evaluate the operating efficiency of the robot arm. The estimated cost H(m) is shown in equation (2): Where, (x s ,y s (x) represents the coordinates of the starting position S. g ,y g ) represents the coordinates of the target location G; β represents the turning penalty factor, as shown in equation (3): Where, θ goal The reference angle is the angle between the line connecting the robot's current node and the target node and the horizontal direction, and θ is the angle between the line connecting the robot's current node and the next node and the horizontal direction. Steps 2-3: Decomposition of the next node of the estimated cost H(m); The estimated cost H(m) affects the path search speed of the A* method. A smaller H(m) will result in too many child nodes being formed during the search process, reducing the computation speed and search efficiency. When H(m) is equal to the actual cost from node m to the destination G, the A* method will not form additional child nodes during the search, and the search efficiency will be improved accordingly. The estimated cost H(m) from the current node m to the target point G is the cost d from the current node m to the next node n. mn The cost d from the next node n to the target node G nG The sum is shown in equation (4): H(m)=d mn +d nG ∞ (4) The estimated cost d is the cost from the current node to the next node. mn The cost d from the next node to the target node nG A weight adjustment factor is set so that the estimated cost changes with the search node. The improved heuristic functions are shown in equations (5) and (6): Where, η mn The improved heuristic function is λ, which is the path balance adjustment factor. Step 3: The optimal path is traversed in the dynamic process by combining the A* method and the bidirectional ant colony method; Using the angle of rotation of a bronchoscope as an ant, in the pre-selection, the path planned by the A* method is used as the element in set A. The element in set A of each population of ants is used as the target point and the path is searched. At the same time, the shortest path is continuously selected. Each population will compare its own shortest path to find the shortest path, which is the appropriate angle for the rotation of each segment of the bronchoscope robotic arm. Specifically, it includes the following sub-steps: Step 3-1: Determine the initial pheromone allocation strategy; An unbalanced distribution strategy is adopted so that the pheromone concentration of the better path planned by the A* method is higher than that of other paths, avoiding the initial blind search caused by the balanced distribution of the initial pheromone concentration in the ant colony method, reducing deadlock, and accelerating the convergence speed and optimization ability. Step 3-2: Perform a bidirectional search using the bidirectional ant colony method; The bidirectional ant colony method is used for bidirectional search. Starting from the beginning and the end point at the same time, the search range is expanded in the direction of the other until the two search paths meet or cannot be expanded further, thus improving the efficiency of path search. The pheromones released by the two groups of ants cannot be recognized by each other, and because the ants traveling in opposite directions have different destinations, their estimated costs are different, as shown in equation (7): Among them, D m Let x be the distance from node m to the destination. s ,y s (x) is the starting coordinate, (x) g ,y g () represents the coordinates of the target point; If pheromones from different ant colonies appear in the same grid at a certain moment, ants from the two groups will meet, resulting in a newly constructed path. The corrected A* mathematical expression is shown in equation (8): in, Let L be the number of pheromones left by the k-th ant as it travels from node m to node n, Q be the total amount of pheromones left by the ant along its path in one pass, and L be the total amount of pheromones left by the ant in one pass. k Let be the total length of the path traveled by the k-th ant during this search for the target; Step 3-3: Determine the pheromone update strategy; By using a coefficient σ, different treatment methods are applied to different ants, which accelerates the convergence speed of the ant colony method and makes the path search more instructive. When the length of the path ants travel is greater than the average length of all ants in each generation, the pheromone concentration of these ants is reduced; conversely, the pheromone concentration is increased so that subsequent ants gradually move toward the optimal path. Local pheromone updates are used. Each ant in each generation updates the pheromone of a node after passing through it. The update method is shown in equation (9): t mn (t+1)=(1-ρ)τ mn (t)+Δτ mn (9) Where ρ is the pheromone evaporation rate parameter, used to control the degree of pheromone retention, and its value ranges from [0, 1]. mn Let Δτ be the pheromone on the path from node m to node n. mn It is the sum of the pheromone concentrations of all ants along the path from node m to node n; The pheromone rules are shown in equations (10) and (11): Where σ is the pheromone regulation factor, a constant representing the intensity of the pheromone increment, and L ave L is the average path length of all ants in each generation. k For the path taken by each ant; Steps 3-4: Determine the state transition probability to identify obstacles; In the path selection probability, obstacle rejection weights are used, and heuristic factors are employed to modify heuristic information. This ensures that ants obtain effective obstacle-free paths during the search process and increases path selection diversity and probability. As shown in equation (12): in, Let $k$ be the reciprocal of the weight of the ant $k$ position it moves from free grid $f$ to obstacle grid $o$, and $k$ be the path selection probability. Inversely proportional, as shown in equation (13), d(f,o) is the repulsion distance from the free grid f to the barrier grid o, as shown in equation (14): in, Let f be the number of times ant k visits grid f, which increases with each visit of ant k to the grid; Step 4: Optimize the path by incorporating the artificial potential field method; Based on the path planning using the bidirectional ant colony method in step 3A*, the force field between the current position and the target position of the robotic arm is determined. According to the direction of the resultant force of the force field, the path of the robotic arm is adjusted to allow it to bypass obstacles and approach the target position. This specifically includes the following sub-steps: Step 4-1: Determine the attractive and repulsive forces of the obstacle using the artificial potential field method; The artificial potential field method involves the existence of repulsive forces around obstacles, attractive forces around key target points, and collision ranges around obstacles, with movement occurring under the influence of these two potential fields. The relationship between the gravitational potential field P and the distance between the moving robotic arm and the key target point is shown in Equation (15): Where μ is the scaling factor and d1 is the Euclidean distance; The corresponding gravitational force I is shown in equation (16): in, The symbol is gradient; The relationship between the repulsive potential field K and the distance between the moving robotic arm and the obstacle is shown in equation (17): Where, λ rej Here, d is the repulsion coefficient, r is the maximum influence range of the obstacle, and d is the maximum repulsion range. mo Let T be the shortest distance between node m and obstacle node o, and the corresponding repulsive force T is shown in equation (18): Step 4-2: Determine the repulsive force function; The distance between the moving robotic arm and the target key point is increased in the repulsion function, and the repulsion gain is modified to reduce the influence of the repulsion, so that the moving robotic arm reaches the target key point. The repulsion function T is shown in equation (19): Where, d me The distance from the robotic arm to the target key point, including Euclidean distance, with coefficients. It is a constant, taking the value (0,1); The repulsive force is determined by the negative gradient; when 0 < d < r, F rej It consists of two parts, F rej =F obs→m +F m→e As shown in equations (20) and (21) respectively: The combined potential field E, obtained by superimposing the gravitational and repulsive potential fields, is shown in equation (22): E = P + K (22) The resultant force H on the robotic arm is shown in equation (23): Step 4-3: Step outside the local problem; The problem of getting caught in a local state occurs when forces are in equilibrium, and the opposing forces of attraction and repulsion cancel each other out, causing the robotic arm to stop moving. The next optimal node generated by the A* method is used as a local virtual target point. A new gravitational force is generated through this virtual target point, breaking the local equilibrium and guiding the robotic arm towards the new virtual target key point. This guides the robotic arm out of the local optimum and continues moving towards the target point. The new gravitational force F... vir As shown in equation (24): Where: φ is the gain coefficient, which increases with the increase of the number of obstacles, |D n -D vir | 2 The distance from the robotic arm to the key point of the virtual target; When the virtual target point is reached, D vir =0, the net force on the moving robotic arm is F = F rej +F att +F vir ; Step 5: Integrate the A* ant colony method with the artificial potential field method; The A* ant colony method is used for global planning, and the artificial potential field method is used for local planning to achieve the optimal global planning and the obstacle avoidance capability of local planning. The cost of searching for key points includes the total cost from the starting point to the key point and the improved heuristic function H(m), as well as the gravitational and repulsive force values ​​of the artificial potential field method. The comprehensive cost function F(m) for each key point in the A* ant colony algorithm is shown in equation (25): F(m)=G(m)+H(m)+P(m) (25) Where P(m) is the potential energy at critical node m, d(m,g) is the Euclidean distance from critical node m to target critical point g, b represents the position of the obstacle, and d(m,b) is the Euclidean distance from critical node m to obstacle b. att k rej These are the coefficients of attraction and repulsion, respectively; Repeat steps 3 to 5 iteratively until the maximum number of iterations is reached, and then obtain the optimal path in terms of time and path length. Step 6: Construct a cubic B-spline interpolation curve to smooth out jump points; The fusion of the A* ant colony method and the artificial potential field method can significantly reduce path length and planning time. The path is smoothed by using a cubic uniform B-spline curve, which achieves path smoothing, eliminates sharp inflection points at jump points, optimizes the curve and meets the requirements of robot movement acceleration and speed continuity, thereby improving the stability and smoothness of the surgical robotic arm operation. Among them, the B-spline curve is a generalized form of the Bezier curve, obtained by approximating a polygon. For spatial location-time series {θ} i ,t i }, i = 0, 1, ..., n, and its general form is shown in equation (27): Among them, Q i (,i=0,1,...,n) are the coordinates of the given control points, N i,k (t) is the k-th degree standardized B-spline basis function, as shown in equation (28): in, k is the order of the B-spline function, i is the sequence number of the B-spline function, and let 0 / 0 = 0, 0 ≤ t ≤ 1, i = 0, 1, ..., k-1; When k=3, the obstacle avoidance position-time series point is used as the model value point to determine the control point through the obstacle avoidance position-time series point in the obstacle space, and a cubic B-spline interpolation curve is established to ensure that the smoothed obstacle avoidance trajectory passes through the key nodes on the obstacle avoidance path selected by the A* method. The i-th segment of the curve is shown in Equation (29): The key nodes on the planned obstacle avoidance path are selected by the fusion method of A* ant colony method and artificial potential field method, and the optimal planned path is output by smoothing the key nodes using cubic B-spline curves.

2. The robotic arm path planning method based on an improved A* ant colony fusion artificial potential field as described in claim 1, characterized in that: It also includes step 7, which controls the running path of the surgical robotic arm based on the optimal planning path obtained in step 6, to meet the requirements of acceleration and speed continuity during operation, improve the stability and smoothness of operation, make the planned path scheme safer and more efficient, meet actual needs, and solve the problem of surgical robotic arm path planning.

Images