Mechanical arm obstacle avoidance planning method

Abstract

The invention discloses a mechanical arm obstacle avoidance planning method which comprises the following steps: performing obstacle avoidance planning on a mechanical arm based on an iterative space cutting fast marching tree algorithm to obtain an obstacle avoidance path; the method specifically comprises the following steps that a fused obstacle map is obtained by combining singular points of a mechanical arm and an obstacle map; random sampling is carried out in the map space of the fused obstacle map, and after sampling points are obtained, a domain map is constructed; after sampling is finished, wavefront propagation is carried out from a starting point, a path tree structure is expanded step by step through sampling points, each branch of a path tree is promoted to search in different directions through an iterative space cutting method, direct connection paths are obtained and recorded, the optimal direct connection path is formed through repeated iteration, and finally obstacle avoidance path planning of the mechanical arm is completed. According to the mechanical arm obstacle avoidance planning method, the situation that the algorithm falls into a local optimal solution due to a direct connection strategy is avoided, the situation that the mechanical arm falls into a singular point range in the planning strategy is avoided, the complex motion planning efficiency is improved, and a high-quality obstacle avoidance path is obtained.

Description

Technical Field

[0001] The present invention relates to the field of robots, and in particular to a robot arm obstacle avoidance planning method. Background Art

[0002] As an automatic operating device that imitates the movements of human arms, the robotic arm has the advantages of strong versatility, flexible movement, high stability, and easy control. It is widely used in many fields. In the process of using the robotic arm, it is usually faced with the basic problem of how to avoid obstacles. At the same time, in order to obtain a more intuitive movement trajectory of the end effector of the robotic arm, operators usually choose the Cartesian space trajectory planning of the robotic arm. However, there are singular points of the robotic arm in the Cartesian space trajectory planning, that is, the Jacobian matrix of the corresponding robotic arm position is irreversible, which makes it impossible to accurately control the speed and direction of the end effector. The above challenges will affect the trajectory planning of the robotic arm and limit the development of technology. Therefore, an efficient and fast obstacle avoidance planning algorithm that can avoid singular points is required.

[0003] Obstacle avoidance planning is a crucial component in the field of robotics, and researchers have proposed many algorithms to find the best path for mobile robots. Graph search-based algorithms include Dijkstra and A* as well as improved algorithms, which can find the optimal solution for the path in planning, but the amount of computation increases exponentially when the dimension increases. Optimization-based algorithms include genetic evolutionary algorithms, particle swarm algorithms, etc. These algorithms are sensitive to parameters and are prone to suboptimal solutions. Learning-based algorithms include reinforcement learning and neural networks, which require a large amount of training data or simulation environments to support reliable results and have poor interpretability. Compared with other types of algorithms, sampling-based path planning algorithms rely on their stronger search capabilities, faster speeds, and better robustness to avoid explicitly modeling the entire environment, and are more suitable for robot arm path planning, which has three-dimensional high spatial dimensions and large-scale situations.

[0004] Common sampling-based path planning algorithms include Rapidly Exploring Random Trees (RRT) and Probabilistic Roadmap Method (PRM). The efficiency of RRT algorithm drops sharply when facing a large number of obstacles, and PRM also has many problems such as time-consuming preprocessing. Fast Marching Tree FMT* (Fast Marching Tree) combines the characteristics of RRT* and PRM algorithms. As an asymptotically optimal algorithm, it has extremely high efficiency in higher dimensions and scenarios with high collision check costs, but it still has problems such as redundant exploration and long planning time in complex terrain. Most of the improvement methods are to optimize the growth strategy direction of the marching tree to reduce redundant searches. In the whole process, more complex strategies are often required to encourage the marching tree to grow autonomously in the right direction, which affects the computational complexity of the algorithm and the consumption of computing resources. Summary of the invention

[0005] In view of the above-mentioned defects of the prior art, the technical problem to be solved by the present invention is that the existing robot arm obstacle avoidance planning method has the problems of redundant exploration, long planning time, complex calculation, high consumption of computing resources, and easy to fall into the range of robot arm singular points. The present invention provides a robot arm obstacle avoidance planning method, which avoids the algorithm from falling into the local optimal solution caused by the direct connection strategy, deactivates the corresponding tree nodes and continues to grow the path tree until the target point or the end tree nodes are all deactivated, and avoids the robot arm from falling into the range of singular points in the planning strategy, further improves the efficiency of complex motion planning and obtains high-quality obstacle avoidance paths.

[0006] To achieve the above object, the present invention provides a robot arm obstacle avoidance planning method, which performs obstacle avoidance planning on the robot arm based on an iterative space cutting fast marching tree algorithm to obtain an obstacle avoidance path; the method specifically comprises the following steps:

[0007] Combine the singularity points of the robot and the obstacle map to obtain a fused obstacle map;

[0008] Performing random sampling in the map space of the fused obstacle map to obtain sampling points, wherein the sampling points are used for growing a path tree;

[0009] After obtaining a sufficient number of sampling points, perform a domain search, calculate the domain radius of each sampling point, build a domain graph, and obtain the neighboring sampling points of each sampling point from the domain graph for subsequent use;

[0010] After sampling, from the starting point x start Start wavefront propagation, use sampling points to gradually expand the path tree structure, use the iterative space cutting method to prompt each branch of the path tree to search in different directions, obtain the nodes in the domain graph as potential parent nodes and record the direct path, repeatedly iterate to form the optimal direct path, and finally complete the obstacle avoidance path planning of the robot arm.

[0011] Furthermore, the singularity points of the robot and the obstacle map are combined to obtain a fused obstacle map, which specifically includes:

[0012] Calculate the singularity cost of each point in the original obstacle map space and calculate the geometric perception singularity index;

[0013] Generate a cost map of the robot's singular points based on the geometric perception singularity index, and mark the locations with high singularity costs as obstacles;

[0014] Finally, it is merged with the obstacle map to obtain a fused obstacle map.

[0015] Further, the current operating ellipsoid M(q)=JJ is calculated using the Riemann metric T The distance from the reference ellipsoid Σ, the geometric perception singularity index ξ,

[0016]

[0017] where Σ is the smallest sphere containing the operating ellipsoid (Σ = kI), where k is a constant greater than the square of the largest singular value:

[0018]

[0019] The geometric perception singularity index ξ decreases as the singular value of the robotic arm decreases, thereby detecting the singular points of the robotic arm.

[0020] Further, random sampling is performed in the map space of the fused obstacle map to obtain sampling points, and the sampling points are used for the growth of the path tree, specifically including:

[0021] Using the method of space cutting to correct the initial free search space to obtain an effective search space, so that the sampling points are only generated in the effective search space;

[0022] Within the effective search space, sampling points are obtained by progressive sampling, and additional sampling points are randomly generated near the space cutting line and obstacles.

[0023] Further, using the method of space cutting to correct the initial free search space to obtain an effective search space, specifically including the following steps:

[0024] Randomly generate a starting point A and an ending point B, first check the straight line L between the two points AB , and perform collision detection; if no collision occurs, the straight line L between the two points Ab is the optimal path;

[0025] If a collision occurs, keep the slope of the straight line between the two points unchanged, generate a translated space cutting line according to the set step size, and then perform collision detection on the part of the space cutting line in the effective search space. Here, the initial effective search space in the sampling stage is the initial free search space; if a collision occurs, continue to increase the step size for collision detection in this direction until the translated cutting line no longer intersects the effective search space; if no collision occurs, stop the translation in this direction and continue the translation search of the space cutting line in the other direction. Finally, find the space cutting lines in both directions. The new boundary formed by the two space cutting lines does not collide with the obstacles and contains the starting point A and the ending point B, completing the space cutting and obtaining the cut effective search space.

[0026] Further, progressive sampling specifically includes pseudo-random sampling in the corrected search space, that is, the effective search space, and adding additional sampling points randomly generated near the space cutting line and obstacles. During sampling, the spatial information of the sampling points themselves is recorded simultaneously to obtain the sampling points and the states of the sampling points;

[0027] According to the sampling points and sampling point states, the surrounding space of the sampling points recorded inside the obstacle is further pseudo-randomly sampled, and finally all sampling points not covered by obstacles are retained as valid sampling points.

[0028] Furthermore, after obtaining a sufficient number of sampling points, a neighborhood search is performed, the neighborhood radius of each sampling point is calculated, a neighborhood graph is constructed, and neighbor sampling points of each sampling point are obtained from the neighborhood graph for subsequent use; specifically, after obtaining a sufficient number of sampling points, a KD tree is used to accelerate the neighborhood search and calculate the neighborhood radius of each sampling point:

[0029]

[0030] Where γ is a constant, d is the spatial dimension, and N is the total number of sampling points;

[0031] After calculating the neighborhood radius of each sampling point, connections are established with other sampling points within the neighborhood radius. Each sampling point has its own set of neighbors, and the construction of the neighborhood graph is finally completed.

[0032] Furthermore, after the sampling is completed, from the starting point x start Start wavefront propagation, use sampling points to gradually expand the path tree structure, at each path tree node x i Maintain a mask matrix mask i , to record the feasible area of ​​the tree branch growth of the current node; in the process of path tree expansion, each time the node with the lowest cost is selected from the priority queue H as the expansion, and the child node will also inherit the mask matrix mask from the parent node parent , and continue to dynamically update the mask through the space cutting strategy child =SpaceCut(mask parent ,x i ,x go ), completing the iterative space cutting method.

[0033] Furthermore, an iterative space cutting method is used to force each branch of the path tree to search in different directions. Each branch of the path tree is restricted to the effective search area represented by its own unique mask matrix. Since the direction vectors formed by the child node and the parent node with the end point are different, the spatial cutting directions completed by the two are different. The effective search area will gradually decrease and improve under the correction of each tree node of the branch. Finally, the iterative space cutting method forces each branch to search in different directions and grow together towards the end point.

[0034] Furthermore, for each branch, an additional list of spatial cutting directions is managed. j , list list jThe spatial cutting direction completed by the previous node is recorded. If a similar direction appears, the spatial cutting stage will be skipped directly to avoid repeated cutting calculations on the same direction by the subsequent nodes of the same branch tree.

[0035] Technical Effects

[0036] The present invention provides a method for obstacle avoidance planning of a robotic arm, which adopts a fast marching tree algorithm of iterative space cutting of an optimization strategy, that is, the growth space of the marching tree is gradually limited by the method of space cutting, forcing the tree to passively grow in the correct direction of the target, and the process of space cutting is accompanied by a direct connection check. The method of the present invention can reduce the search scope of the algorithm while retaining the optimal solution, reduce redundant searches and improve the efficiency of the algorithm. At the same time, the singularity points of the robotic arm are added to the consideration scope of the algorithm, and the singularity index is integrated into the obstacle map to adapt to the obstacle avoidance planning under the robotic arm, so that the algorithm can achieve both good performance and acceptable computational complexity.

[0037] The concept, specific structure and technical effects of the present invention will be further described below in conjunction with the accompanying drawings to fully understand the purpose, characteristics and effects of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 It is a progressive sampling visualization diagram of a robot arm obstacle avoidance planning method according to a preferred embodiment of the present invention;

[0039] Figure 2 It is a schematic diagram of iterative spatial cutting visualization of a method for obstacle avoidance planning of a robot arm according to a preferred embodiment of the present invention;

[0040] Figure 3 It is a schematic diagram of an iterative space cutting correction single tree branch search space of a robot arm obstacle avoidance planning method according to a preferred embodiment of the present invention;

[0041] Figure 4 It is a flow chart of a method for obstacle avoidance planning of a robot arm according to a preferred embodiment of the present invention;

[0042] Figure 5 It is a schematic diagram of an obstacle map of a robot arm obstacle avoidance planning method according to a preferred embodiment of the present invention;

[0043] Figure 6 It is the visualization result of FMT* and ISC-FMT* of a robot arm obstacle avoidance planning method of a preferred embodiment of the present invention;

[0044] Figure 71 is a schematic diagram of an application of a robot arm obstacle avoidance planning method according to a preferred embodiment of the present invention; (a) uterine fibroids magnetic resonance image, (b) treatment plane singular value cost heat map, (c) path planning without considering singular values, (d) path planning considering singular value fusion map;

[0045] Figure 8 It is a schematic diagram of the changes in joint values ​​of a robot arm obstacle avoidance planning method according to a preferred embodiment of the present invention. DETAILED DESCRIPTION

[0046] In order to make the technical problems, technical solutions and beneficial effects to be solved by the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0047] In the following description, specific details such as specific internal procedures and techniques are provided for the purpose of illustration rather than limitation, so as to provide a thorough understanding of the embodiments of the present invention. However, it should be clear to those skilled in the art that the present invention may be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to prevent unnecessary details from obstructing the description of the present invention.

[0048] like Figure 4 As shown, an embodiment of the present invention provides a method for obstacle avoidance planning of a robot arm, which performs obstacle avoidance planning on the robot arm based on an iterative space cutting fast marching tree algorithm to obtain an obstacle avoidance path; specifically, the method includes the following steps:

[0049] Step 100, combining the singularity points of the robot and the obstacle map to obtain a fused obstacle map; the traditional FMT* algorithm only considers obstacle cost and path cost, and for the Cartesian space trajectory planning of the robot, how to avoid the influence of the singularity points on the robot is also an important factor. Therefore, the method provided in the embodiment of the present invention takes the singularity points of the robot into consideration. Specifically, the following steps are included:

[0050] Step 101, calculating the singularity cost of each point in the original obstacle map space and calculating the geometric perception singularity index;

[0051] Traditional indicators (such as operability index and flexibility index k = σ max / σ min ) has defects and cannot fully capture the geometric characteristics of the ellipsoid. Therefore, in the embodiment of the present invention, the Riemann metric is used to calculate the current operating ellipsoid M(q) = JJ T The distance from the reference ellipsoid Σ, the geometric perception singularity index ξ,

[0052]

[0053] where Σ is the smallest sphere that contains the operating ellipsoid (Σ = kI), where k is a constant greater than the square of the largest singular value:

[0054]

[0055] The final calculated result ξ will decrease as the singular value of the robot arm decreases, thereby sensitively detecting the situation where the robot arm is close to singularity.

[0056] Step 102, generating a cost map of the singular points of the robot arm according to the geometric perception singularity index, and marking the positions with high singularity costs as obstacles;

[0057] Step 103, finally merge with the original obstacle map to obtain a fused obstacle map. The original obstacle map is a binary black-and-white map. The singularity cost map sets a threshold for the singularity index. The value above the threshold is 1, and the value below is 0. Finally, a binary black-and-white map is generated. The union of the two maps is 1 to obtain a fused obstacle map.

[0058] Step 200, random sampling is performed in the map space of the fused obstacle map to obtain sampling points, which are used to grow the path tree; specifically, after obtaining the fused obstacle map, random sampling needs to be completed in the map space, and the sampling points are used for the subsequent growth of the path tree. Compared with FMT*, which randomly generates a certain number of sampling points in the entire map space, ISC-FMT* first uses the space cutting method to perform random sampling on the initial free search space χ free Modify so that the sampling points are only in the effective search space χ valid Generate in the middle to limit the growth direction of the path tree.

[0059] In an embodiment of the present invention, a spatial cut is used once at the beginning of the sampling phase to optimize the distribution of sampling points (sampling points will only be generated in the valid search space). In the generation phase of the path tree, a spatial segmentation is also performed each time a new node is included in the path tree to correct the effective search space of the tree branch. The tree branch will only grow in the valid space. At this time, the starting point A here refers to the newly included tree node, and the end point B refers to the final target point.

[0060] The specific steps of space cutting are as follows:

[0061] Randomly generate the starting point A and the end point B, and first check the direct line L between the two points AB , and perform collision detection; sequentially generate cutting lines that spread from the center line directly connecting the starting point A and the end point B to both sides, and complete the collision detection. Assume that the coordinates of the starting point A are (x A ,yA ), the end point B is (x B ,y B ), first check the direct line L between the two AB :

[0062]

[0063] And perform collision detection. Collision detection uses the Bresenham algorithm. The core of checking whether a straight line path is collision-free is to traverse all grid points on the line segment path and check whether each point is occupied by an obstacle.

[0064] If there is no collision, the optimal path from A to B is a straight line. At this time, this straight line is the cutting line in both directions. The cutting lines in both directions overlap. Finally, it is determined whether the cutting lines in both directions are consistent. If they are the same, it means that the two can be directly connected. The optimal path from point A to point B means that the straight line between the two points is the shortest. After finding such an optimal path, there is no need to perform space cutting, and you can directly return to this straight line.

[0065] If a collision occurs, the slope of the AB straight line is kept at m = (y B -y A ) / (x B -x A ) remains unchanged, and the spatial cutting line L after translation is generated according to the set step length △d n , the calculation formula is as follows:

[0066]

[0067] Where n is the current translation round, The translation cutting line is ensured to move along the path normal direction. The generated space cutting line will also be subject to collision detection. If a collision occurs, the step size collision detection will continue to increase in that direction until the translated cutting line no longer intersects with the effective search space (that is, the polygon formed by the cutting line and the boundary of the effective search space has no intersection). If no collision occurs, it means that the space cutting line in that direction has been found, and the translation in that direction is stopped, and the translation search of the space cutting line is repeated in another direction. It should be emphasized that collision detection is performed on part of the effective search space rather than the entire map. Although the effective search space is equivalent to the entire fused obstacle map in the sampling stage, the space cutting method is also used in the subsequent path tree growth stage, so the effective search space will gradually decrease during continuous correction.

[0068] Finally, the space cutting lines in both directions are found. The new boundary formed by the two space cutting lines does not collide with obstacles and contains the starting point A and the end point B. Therefore, the space within the space cutting line is the effective search space χ valid, the space outside the space cutting line is the invalid search area χ invalid At this point, the space cutting is completed and the sampling space after cutting is obtained, that is, the effective search space χ valid .

[0069] In some obstacle maps, advance spatial cutting and direct connection checks will replace inefficient exploration methods. For some maps where there are no obstacles between the starting point and the end point, the best direct connection path is obtained directly without the need to gradually explore using the growth of the path tree.

[0070] At the same time, since space cutting will bring additional narrow paths and reduce the planning rate, the algorithm will randomly generate additional sampling points near the space cutting line and obstacles to make up for the narrow paths caused by space cutting and improve sampling efficiency. In this embodiment, the specific implementation method is progressive sampling, which is as follows.

[0071] After the sampling space is corrected, let the corrected search space be The initial sampling point is generated by a pseudo-random sequence: x i ~U(D),i=1,...,b. m additional guide points {x 1 ,...,x m} is also added to the sampling set. At this time, the state marking function of each sampling point is defined as: j =I{obs}(x j )={1 if f(x j )≤0,0otherwise}. Where f(x) is the implicit expression function of the obstacle. When f(x)≤0, it means that the point is inside the obstacle. This makes the initial sampling set S 1 ={(x i ,s i )} contains spatial topological information.

[0072] The second stage targets s in S b = 1 for the internal point of the obstacle {x b} Implement focused sampling. Construct a local sampling space B(x_b)={x|||x-x_b||2≤r} in the surrounding spatial neighborhood, where r is the set radius, and randomly generate a candidate point set S 2 The final valid sampling set is:

[0073] S final ={x∈S 1 ∪S 2 |f(x)>0∧x∈D}

[0074] This process overcomes the problem of insufficient coverage of the initial sampling in the narrow channel area by compensating the probability density near the obstacle boundary. The algorithm achieves adaptive resolution improvement of the obstacle boundary while retaining the initial spatial exploration capability. Compared with conventional uniform sampling or Gaussian sampling, progressive sampling achieves a fast Gaussian sampling-like effect by relying on the sampling point state obtained by the initial sampling. The final sampling effect is as follows: Figure 1 In complex environments, progressive sampling is significantly faster because it reduces invalid sampling while generating more valid sampling points in key areas (around obstacles and around space dividing lines).

[0075] Step 300, after obtaining a sufficient number of sampling points, perform a domain search, calculate the domain radius of each sampling point, construct a domain graph, and obtain the neighboring sampling points of each sampling point from the domain graph for subsequent use;

[0076] Specifically, after obtaining a sufficient number of sampling points, the KD tree is used to accelerate the neighborhood search and calculate the neighborhood radius of each sampling point:

[0077]

[0078] Where γ is a constant, d is the spatial dimension of the obstacle map, and N is the total number of sampling points;

[0079] After calculating the neighborhood radius of each sampling point, connections are established with other sampling points within the neighborhood radius. Each sampling point has its own set of neighbors, and the construction of the neighborhood graph is finally completed.

[0080] The domain map in this step refers to the calculation of the neighborhood radius (r) for each sampling point based on the set parameters (including the total number of sampling points, the spatial dimension of the obstacle map, and the custom coefficient). This dynamic radius setting avoids frequent adjustment of the radius parameter settings under different numbers of sampling points and maps. After that, connections are established with other sampling points within the neighborhood radius. Each sampling point has its own set of neighbors to form a domain map. In the subsequent process of gradually expanding the path tree, only the sampling points in the neighborhood need to be checked as potential parent nodes.

[0081] Each sampling point maintains a list to store all its neighbor sampling points and edge costs. The edge cost refers to the path length from the sampling point to this neighbor sampling point (a direct line without considering obstacles).

[0082] Step 400, after sampling is completed, start from the starting point x startStart wavefront propagation, use sampling points to gradually expand the path tree structure, use the iterative space cutting method to force each branch of the path tree to search in different directions, obtain and record the direct path, iterate repeatedly to form the optimal direct path, and finally complete the obstacle avoidance path planning of the robot arm. Specifically, after the sampling is completed, from the starting point x start Start wavefront propagation, use sampling points, and gradually expand the path tree structure, such as Figure 2 (a)(b) As shown. Each path tree node x i Maintain a mask matrix mask i , to record the feasible area of ​​the tree branch growth of the current node; the mask matrix mask of the starting point start is the effective search area χ obtained by space cutting in the sampling stage valid During the expansion of the path tree, the node with the lowest cost is selected from the priority queue H each time as the expansion, and the child node also inherits the mask matrix mask from the parent node parent , and continue to dynamically update the mask through the space cutting strategy child =SpaceCut(mask parent ,x i ,x goal ), the mask update method here is the same as the space cutting in step 200, completing the iterative space cutting method and reducing the invalid search area. Therefore, each branch of the path tree will be limited to the effective search area represented by its own unique mask matrix, and because the direction vectors formed by each node in a tree branch and the end point are different, the space cutting directions completed by the two are different, so this effective search area will be gradually reduced and improved under the correction of each tree node in the branch, such as Figure 2 (c) and Figure 3 As shown, this iterative space cutting method will eventually prompt each branch to search in different directions and grow together towards the end point.

[0083] It is also worth noting that the spatial distance between the parent node and the child node is related to the search radius r in the domain graph, which is usually very close. This may cause the direction and position of the spatial cutting of adjacent nodes to be repeated. For scenarios where collision detection is expensive, these similar spatial direction cutting will not bring a large degree of correction to the mask matrix of the branch. On the contrary, the time cost will be reduced due to additional collision detection. Therefore, an additional list of spatial cutting directions is managed for each branch. j , list list j Record the spatial cutting direction {θ i}, θ i =arctan((y i -y goal ) / (xi -x goal )). If similar directions appear:

[0084]

[0085] where θ th If the angle threshold is preset, the algorithm directly skips the spatial cutting stage to avoid repeated cutting calculations in the same direction for the same branch tree node in the future.

[0086] In addition, during the growth of the path tree, the calculation of each tree node space cut will also include an additional direct connection check between the current point and the end point, which not only reduces the redundant exploration of the algorithm, but also improves the quality of the path end and reduces the generation of inflection points. Conventional direct connection check algorithms will stop immediately after finding a direct connection, which can greatly improve efficiency, but the generated path often cannot reach the optimal state, destroying the important characteristic of FMT* itself that it is asymptotically optimal. Therefore, ISC-FMT* uses the Bresenham algorithm to traverse the discrete points on the path for direct connection detection. If all points satisfy:

[0087]

[0088] The node is marked as inactive. Inactive nodes no longer participate in subsequent expansion, but their recorded direct paths are still retained as candidate paths, such as Figure 2 (d). If other branches subsequently find a better path (e.g., a shorter path cost), the optimal path record is updated. This strategy retains the asymptotic optimality of FMT* by dynamically covering suboptimal solutions. Finally, the algorithm terminates when a branch reaches the target neighborhood:

[0089] min(x i -x goal )≤∈,x i ∈N active

[0090] ∈ is the set threshold, N active Refers to all non-deactivated points in the path tree, or all terminal nodes are deactivated:

[0091]

[0092] The final optimal path is selected from the direct path and the conventional backtracking path, and the smoothness is optimized through the cubic spline curve. The parameterized equation of the cubic spline can be expressed as:

[0093] p(t)=at 3 +bt 2 +ct+d

[0094] Where a, b, c, d are coefficient vectors, and t∈[0,1] is a parameter. The spline curve must meet boundary conditions (position and velocity continuity of the starting and ending points) to ensure the smoothness and controllability of the robot's motion. Complete the obstacle avoidance path planning for the robot.

[0095] The Iterative Space Cutting Fast Marching Tree Algorithm (ISC-FMT*) in the present invention is compared with the traditional FMT*, RRT*, Informed-RRT* (IRRT*), and PRM* to verify their performance. Figure 5 As shown in the figure, three different obstacle densities of 0.05, 0.25, and 0.5 were tried in a two-dimensional environment, and three different sampling densities of 3000, 4000, and 5000 (RRT* and IRRT* are expressed as the number of iterations). The connection radius of ISC-FMT*, FMT*, and PRM* were set the same, and each algorithm was used in the same experiment for 25 times. All algorithms were run on a 2.50GHz AMD Ryzen 9 7945HX CPU and 16GB memory. The results of FMT* and ISC-FMT* are shown in the figure below. Figure 6 shown.

[0096] Table 1. Test results of all algorithms

[0097]

[0098]

[0099] The final comparative test results are shown in Table 1, where the optimal results are marked in bold. dens There are three obstacle densities: 0.05, 0.25, and 0.5. All test results are the average values ​​of a single obstacle map under three different sampling densities. avg is the average planning time of the algorithm, s t_avg is the time standard deviation, J avg is the average path cost, s J_avg Path standard deviation, Opt is the probability of obtaining a high-quality path, and a high-quality path means that the cost meets the specified threshold condition Trreshold Opt Path.

[0100] From the experimental results of a robot obstacle avoidance planning method of the present invention, the robot path cost obtained by the present invention is better than the method based on the traditional FMT* algorithm, especially in the scene with sparse obstacle density, it is the best among all the methods, which proves that the method can improve the robot path quality effect through iterative spatial cutting and maintain excellent stability. At the same time, the present invention is the best or suboptimal among all the methods in the time consumed by the robot to explore the path, and most of the invalid exploration space can be removed by cyclic spatial cutting. In maps with higher obstacle density, although spatial cutting cannot obtain sufficient benefits, the increase in the robot path planning time compared to the traditional FMT* algorithm can be reduced by adjusting the step size and threshold of spatial cutting in the present invention. In addition, since the present invention adds a direct connection check mechanism, when planning the robot path for certain special maps, a direct connection path between the starting point and the end point can be directly obtained, replacing the conventional exploration method.

[0101] The following will use the application of a robotic arm in surgical obstacle avoidance planning for uterine fibroids to illustrate a robotic arm obstacle avoidance planning method of the present invention.

[0102] In the sagittal MRI of the patient's uterus, an acoustic path is planned for the focused ultrasound HIFU robot arm from the initial position to the uterine fibroids. The specific MRI image is as follows Figure 7 (a) shows that the red area is the uterine fibroid area, the green area is the pubic area, the yellow area is the rectal area, the white circle in the figure is the starting point, and the white cross is the end point uterine fibroid position. In order to avoid ultrasonic reflection and damage to important normal tissue areas, the sound path needs to avoid the pubic area and the rectal area, that is, the yellow and green areas in the figure during the planning process. The PUMA560 robotic arm is used as a test in the embodiment. The treatment plane is located in the xy plane of the PUMA560 robotic arm z=0.029, the orientation of the HIFU focus is fixed, the starting point coordinates correspond to the robotic arm position (0.3, 0.3, 0.029), and the end point coordinates correspond to the robotic arm position (0.5, 0.65, 0.029), and there is a singular point between the two.

[0103] In this embodiment, the geometric perception singularity index is used to generate the singularity index thermal of the current treatment plane. Figure 7 As shown in (b), the singularity index threshold is set to 90, and the area with a singularity index greater than or equal to the threshold will also be identified as an obstacle by the algorithm, and then the fused obstacle map is obtained based on the original obstacle map.

[0104] First, direct connection detection and space cutting are performed in the fused obstacle map to complete the correction of the sampling space. Progressive sampling is completed in the corrected sampling space. In the first stage, 4000 sampling points are implemented in the obstacle map and the status is recorded. The second stage sampling is implemented at each sampling point inside the obstacle, and 8 sampling points are randomly generated in the circle with a radius of 3. The sampling stage ends.

[0105] ISC-FMT* from starting point x start Start wavefront propagation, use sampling points, and gradually expand the tree structure. In the process of each branch growth, space cutting continuously corrects the effective search space, and completes direct connection detection every time a tree node is accepted. The algorithm ends when all the terminal tree nodes of the final path tree are deactivated or the tree node grows to the vicinity of the target point. Get an optimal sound path, such as Figure 7 (d) as shown.

[0106] Finally, cubic spline is used to smooth the acoustic path interpolation, and the total path duration is set to 2 seconds. The inverse operation is performed according to the PUMA560 robot arm parameters to obtain the angle value changes of the six joints during the path process. The results are as follows: Figure 8 (b) As shown. Without considering the singular point, the path planning is used as Figure 7 (c), the velocity results of the last six joints are as follows Figure 8 (a) Figure 8 (a) It can be observed that although the original path avoided obstacles, due to the absence of singular points, the angle values ​​of the fourth and sixth joints suddenly changed at the end of the first second, resulting in excessive instantaneous speed. This is not allowed during HIFU precision treatment. Figure 8 (b) After considering the singular point obstacle avoidance, the last six joint values ​​are in a stable change process and the obstacles and singular points are avoided, ensuring the safety of HIFU treatment.

[0107] Therefore, the method of the present invention can effectively improve the convergence speed and planning efficiency of the algorithm and improve the path quality in sparse scenarios, and can also improve the path quality and stability in dense scenarios with a slight increase in time cost. In addition, the improvement work on the singularity points of the robot arm also enables the algorithm to have the function of avoiding the singularity points of the robot arm, thereby improving the applicability of the algorithm.

[0108] The preferred specific embodiments of the present invention are described in detail above. It should be understood that a person skilled in the art can make many modifications and changes based on the concept of the present invention without creative work. Therefore, any technical solution that can be obtained by a person skilled in the art through logical analysis, reasoning or limited experiments based on the concept of the present invention on the basis of the prior art should be within the scope of protection determined by the claims.

Claims

1. A robot arm obstacle avoidance planning method, characterized in that: Obstacle avoidance planning is performed on the robot arm based on the iterative space cutting fast marching tree algorithm to obtain the obstacle avoidance path; The specific steps include: Combine the singularity points of the robot and the obstacle map to obtain a fused obstacle map; Performing random sampling in the map space of the fused obstacle map to obtain sampling points, wherein the sampling points are used for growing a path tree; After obtaining a sufficient number of sampling points, a domain search is performed, the domain radius of each sampling point is calculated, a domain graph is constructed, and neighbor sampling points of each sampling point are obtained from the domain graph for subsequent use; After sampling, from the starting point x start Start wavefront propagation, use sampling points to gradually expand the path tree structure, use the iterative space cutting method to make each branch of the path tree search in different directions, and record the direct path during the growth of the path tree. The path tree grows to the end when all tree nodes are inactivated to form the optimal path, and finally complete the obstacle avoidance path planning of the robot arm.

2. A robot arm obstacle avoidance planning method as claimed in claim 1, characterized in that: Combine the singularity points of the robot and the obstacle map to obtain a fused obstacle map, including: Calculate the singularity cost of each point in the original obstacle map space and calculate the geometric perception singularity index; Generate a cost map of the robot's singular points based on the geometric perception singularity index, and mark the locations with high singularity costs as obstacles; Finally, it is merged with the original obstacle map to obtain the fused obstacle map.

3. A robot arm obstacle avoidance planning method as claimed in claim 2, characterized in that: Use the Riemann metric to calculate the current operating ellipsoid M(q) = JJ T The distance from the reference ellipsoid Σ, the geometric perception singularity index ξ, where Σ is the smallest sphere that contains the operating ellipsoid (Σ = kI), where k is a constant greater than the square of the largest singular value: The geometric perception singularity index ξ decreases as the singular value of the robot arm decreases, thereby detecting the singular points of the robot arm.

4. A robot arm obstacle avoidance planning method as claimed in claim 1, characterized in that: Random sampling is performed in the map space of the fused obstacle map to obtain sampling points, where the sampling points are used for growing the path tree, specifically including: The initial free search space is modified by using the space cutting method to obtain the effective search space, so that the sampling points are only generated in the effective search space; In the effective search space, progressive sampling is used to obtain sampling points, and additional sampling points are randomly generated near the space cutting line and obstacles.

5. A robot arm obstacle avoidance planning method as claimed in claim 4, characterized in that: The initial free search space is modified by using the space cutting method to obtain the effective search space, which specifically includes the following steps: Randomly generate the starting point A and the end point B, and first check the direct line L between the two points AB , and perform collision detection; if no collision occurs, the direct line L between the two points AB This is the optimal path; If a collision occurs, the slope of the straight line between the two points is kept unchanged, and the spatial cutting line after translation is generated according to the set step size. Then, a collision detection is performed on the part of the spatial cutting line in the effective search space. Here, the initial effective search space in the sampling stage is the initial free search space. If a collision occurs, the step size collision detection is continued to be increased in that direction until the translated cutting line no longer intersects with the effective search space. If no collision occurs, the translation in that direction is stopped, and the translation search of the spatial cutting line is continued in another direction. Finally, the spatial cutting lines in both directions are found. The new boundary formed by the two spatial cutting lines does not collide with obstacles and contains the starting point A and the end point B. The spatial cutting is completed, and the effective search space after cutting is obtained.

6. A robot arm obstacle avoidance planning method as claimed in claim 4, characterized in that: The progressive sampling specifically includes pseudo-random sampling in the modified search space, i.e., the effective search space, and adding additional sampling points randomly generated near the space cutting line and obstacles. When sampling, the spatial information of the sampling point itself is recorded to obtain the sampling point and the sampling point status. According to the sampling points and sampling point states, the surrounding space of the sampling points recorded inside the obstacle is further pseudo-randomly sampled, and finally all sampling points not covered by obstacles are retained as valid sampling points.

7. A robot arm obstacle avoidance planning method as claimed in claim 1, characterized in that: After obtaining a sufficient number of sampling points, a neighborhood search is performed, the neighborhood radius of each sampling point is calculated, a neighborhood graph is constructed, and the neighboring sampling points of each sampling point are obtained from the neighborhood graph for subsequent use; specifically, after obtaining a sufficient number of sampling points, a KD tree is used to accelerate the neighborhood search and calculate the neighborhood radius of each sampling point: Where γ is a constant, d is the spatial dimension, and N is the total number of sampling points; After calculating the neighborhood radius of each sampling point, connections are established with other sampling points within the neighborhood radius. Each sampling point has its own set of neighbors, and the construction of the neighborhood graph is finally completed.

8. A robot arm obstacle avoidance planning method as claimed in claim 1, characterized in that: After sampling, from the starting point x start Start wavefront propagation, use sampling points to gradually expand the path tree structure, at each path tree node x i Maintain a mask matrix mask i , to record the feasible area of ​​the tree branch growth of the current node; in the process of path tree expansion, each time the node with the lowest cost is selected from the priority queue H as the expansion, and the child node will also inherit the mask matrix mask from the parent node parent , and continue to dynamically update the mask through the space cutting strategy child =SpaceCut(mask parent ,x i ,x goal ), completing the iterative space cutting method.

9. A robot arm obstacle avoidance planning method as claimed in claim 8, characterized in that: The iterative space cutting method is used to force each branch of the path tree to search in different directions. Each branch of the path tree is restricted to the effective search area represented by its own unique mask matrix. Since the direction vectors formed by the child node and the parent node with the end point are different, the spatial cutting directions completed by the two are different. The effective search area will gradually decrease and improve under the correction of each tree node of the branch. Finally, the iterative space cutting method forces each branch to search in different directions and grow together towards the end point.

10. A robot arm obstacle avoidance planning method as claimed in claim 8, characterized in that: For each branch, an additional list of spatial cutting directions is managed. j , list list j The spatial cutting direction completed by the previous node is recorded. If a similar direction appears, the spatial cutting stage will be skipped directly to avoid repeated cutting calculations on the same direction by the subsequent nodes of the same branch tree.

Images