A robotic arm path planning and replanning method for dynamic medical scenarios

Abstract

The present invention discloses a method for path planning and replanning of a robotic arm in a dynamic medical scenario, including initial path planning based on a non-uniform sampling adaptive step size RRT* algorithm, and real-time obstacle avoidance of dynamic obstacles based on RRT* combined with a virtual potential field: after obtaining an initial safe collision-free path, a path decision controller is combined to select a motion path for the robotic arm, and the initial path node is judged in real time based on the dynamic obstacle point cloud information whether there are dangerous nodes. If there are dangerous nodes, the local path is replanned. When there are no dangerous nodes, the initial path is executed and it is judged whether the motion planning of the robotic arm is completed until the robotic arm completes the movement from the starting point to the end point. The problem of low efficiency, long planning time, low real-time performance and poor planning ability in a dynamic environment of a traditional path planning algorithm is solved, and a safe and efficient motion path can be generated by the robotic arm under the constraints of static and dynamic obstacles in a medical scenario.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot arm path planning, and in particular relates to a robot arm path planning and re-planning method for dynamic medical scenarios. Background Art

[0002] In the development of the pharmaceutical industry, in order to further increase the speed of drug production, robotic arm operation has become one of the important means to improve efficiency. In the pharmaceutical production environment, there are many pharmaceutical dispensing instruments (incubators, oscillators, etc.) and test tube reagents, which leads to huge challenges for robotic arm movement operations. Finding a collision-free optimal path from the starting point to the target point is an important prerequisite for safely and efficiently achieving pharmaceutical dispensing operations.

[0003] The path planning methods of the robot arm can be divided into offline path planning and online path planning. The main difference between the two is that offline path planning requires the knowledge of the position information of obstacles in the robot arm workspace in advance, while online path planning uses sensors to perceive environmental information during the movement process to perform real-time planning. The online path planning method mainly relies on the artificial potential field method. According to the set motion target and the obstacles perceived in real time, the gravitational field and repulsive field are established to achieve real-time obstacle avoidance in a dynamic environment. This method can well solve the motion path planning operation of the robot arm in an unstructured environment. However, when the repulsive force of the obstacle is the same as the gravitational force of the motion target but in the opposite direction, the robot arm will fall into a local minimum during real-time planning, resulting in the failure of the robot arm path planning. At present, the offline path planning methods can be divided into: planning algorithms based on search methods (such as A* algorithm, Dijkstra algorithm, etc.), planning algorithms based on intelligent bionic methods (ant colony algorithm, simulated annealing algorithm, etc.), planning algorithms based on machine learning methods (graph neural network algorithm, adversarial neural network planning algorithm, etc.) and planning algorithms based on probability sampling methods (RRT algorithm, PRM algorithm, etc.). The robot is an open-chain system with high degrees of freedom. The computational complexity of the planning algorithm based on the search method increases rapidly with the increase of the dimension of the planning space. This type of algorithm is difficult to apply to the path planning of the robot. The planning algorithm based on the intelligent bionic method simulates the evolution of nature to find the optimal solution to the goal. This type of method requires reasonable parameter setting and is prone to fall into the local minimum. In addition, the computational complexity of this type of method will increase sharply with the increase of the dimension. The method based on machine learning is trained by learning with the collected data to obtain the approximate optimal safe and collision-free path of the robot, but this type of method takes a lot of time to train and has poor generalization. The planning method based on probability sampling randomly samples in the planning space and finally forms a feasible path through the connection between nodes. This type of algorithm is suitable for solving path planning problems in high-dimensional space, but the algorithm based on probability sampling is random, inefficient and cannot guarantee the shortest path.

[0004] Based on this, the present invention proposes a robot arm path planning and re-planning method for dynamic medical scenarios. Summary of the invention

[0005] In view of the above technical problems, the present invention provides a robot arm path planning and re-planning method for dynamic medical scenarios.

[0006] The technical solution adopted by the present invention to solve the technical problem is:

[0007] A method for robot arm path planning and replanning in a dynamic medical scenario, the method comprising the following steps:

[0008] S100: Define the three-dimensional working space of the robot arm, determine the positions of the initial point and the target point, use the depth camera to obtain the environmental point cloud information, and track the static obstacle point cloud information and the dynamic obstacle point cloud information respectively;

[0009] S200: Set the initial point position to the initial node of the random tree , the target point position is set as the target node of the random tree ;

[0010] S300: Initial path planning based on non-uniform sampling adaptive step size RRT* algorithm: A number is randomly generated based on the Gaussian function, and random sampling is performed in the global sampling space or the local sampling space according to the relationship between the number and the preset Gaussian function constraint threshold, and the random sampling points are randomly sampled. Perform adaptive step expansion to generate new nodes ;

[0011] S400: Determine whether the distance between the nearest point cloud of the static obstacle point cloud and the new node meets the collision threshold. If so, move the new node Add a random tree, perform pruning and reconnection, and if the collision threshold is not met, discard the new node; return to S300 and resample until a safe and collision-free initial path is obtained within the preset maximum number of sampling times;

[0012] S500: Real-time obstacle avoidance of dynamic obstacles based on RRT* combined with virtual potential field: After obtaining the initial safe collision-free path, the path decision controller is combined to select the robot arm motion path, and the initial path node is judged in real time based on the dynamic obstacle point cloud information whether there is a dangerous node. If there is a dangerous node, the local path is replanned. When it is judged in real time that there is no dangerous node in the initial path node, the initial path is executed and the robot arm motion planning is judged whether it is completed until the robot arm completes the movement from the initial node. To the target node sports.

[0013] Preferably, in S300, a number is randomly generated based on a Gaussian function, and random sampling is performed in a global sampling space or a local sampling space according to a magnitude relationship between the number and a preset Gaussian function constraint threshold, including:

[0014] S310: Generate a number randomly based on the Gaussian function. When the value is greater than the preset Gaussian function constraint threshold, select the smallest node in the random tree according to the heuristic function. , construct a local sampling space and perform random sampling; the local sampling space is the node selected according to the heuristic function The target node with random tree The constructed cubic sampling space;

[0015] S320: When the value is less than or equal to a preset Gaussian function constraint threshold, random sampling is performed in the global sampling space; wherein the global sampling space is all spatial positions that the robotic arm can reach in the three-dimensional space.

[0016] Preferably, the heuristic function in S310 is:

[0017] ;

[0018] in, represents the actual path length from the initial node of the random tree to the nth node, represents the Euclidean distance from the nth node to the target node of the random tree, As a weighting factor;

[0019] The range of the local sampling space is ,in:

[0020] ;

[0021] ;

[0022] ;

[0023] in, is the smallest node in the random tree selected according to the heuristic function, is the target node of the random tree.

[0024] Preferably, in S300, performing adaptive step expansion according to randomly sampled points to generate new nodes includes:

[0025] S330: According to the randomly sampled points, guide the random tree expansion direction and find the tree node closest to the randomly sampled points ;

[0026] S340: Each sampled point will adaptively set the expansion step size based on the distance between the nearest tree node and the obstacle through the preset step size range [a, b]. , generating a nearest tree node To random sampling point The direction step is the expansion step New Node .

[0027] Preferably, the adaptive step size in S340 is specifically:

[0028] ;

[0029] in, , are the minimum and maximum values ​​of the preset step length range, is a proportionality coefficient, Represents the sum of the distances from the nearest point cloud of all obstacles to the random sampling point.

[0030] Preferably, S400 further includes:

[0031] When the initial path is not generated within the preset maximum number of sampling times, the algorithm is terminated and the path planning fails.

[0032] Preferably, determining whether there is a dangerous node in the planned path in S500 includes:

[0033] Set a radius of The sphere is regarded as the dangerous area of ​​the dynamic obstacle. When the dynamic obstacle moves, the nodes in the initial path that enter the dangerous area of ​​the dynamic obstacle will be regarded as dangerous nodes.

[0034] Preferably, if there is a dangerous node in S500, the local path is replanned, specifically:

[0035] S510: Get the dangerous node in the path, and use the parent node of the first dangerous node in the random tree as the initial node for replanning , take the child node of the last dangerous node in the random tree as the target node ;

[0036] S520: Track the point cloud information of dynamic obstacles and set the initial node As the root node of the tree, Expand and randomly sample to obtain random sampling points ;

[0037] S530: Construct a virtual potential field through the artificial potential field method, target node Gravitational forces on random trees in weight planning , dynamic obstacles and static obstacles produce repulsive forces on the random trees in the replanning and , according to gravity , static obstacle repulsion and dynamic obstacle repulsion Get virtual potential ;

[0038] S540: Based on randomly sampled points , guide the random tree expansion direction and find the tree node closest to the random sampling point , according to the random sampling points , the nearest tree node and virtual potential Composed of random tree expansion strategy to expand and find new nodes A collision check is performed on it. If the collision condition is met, it is added to the local path planning random tree. Otherwise, it is discarded until a final local path is generated within the preset maximum number of sampling times. The path planning is completed based on the available part of the initial path and the generated local path. If the local path is not formed within the preset maximum number of sampling times, the planning fails.

[0039] Preferably, in S530, the virtual potential field gravity F is specifically:

[0040] ;

[0041] Among them, gravity Specifically:

[0042] ;

[0043] in, represents the maximum value of gravity, represents the scale factor, Represents the replanning random tree Node to target node The distance between Node to target node The farther the distance, the bigger it is;

[0044] Static obstacle repulsion and dynamic obstacle repulsion Specifically:

[0045] ;

[0046] ;

[0047] in, is the maximum repulsive force, represents the scale factor, Representation Node To the static obstacle repulsion point The distance between the static obstacle and the random tree node is taken as the repulsion point. , take the point cloud closest to the random tree node of the dynamic obstacle as the repulsion point , Representation Node To the dynamic obstacle repulsion point The distance Indicates the safe distance for generating repulsion; if the node The closer the distance to the repulsion point, the greater the repulsion the obstacle will produce on it.

[0048] Preferably, the new node is generated in S540 in the following manner:

[0049] ;

[0050] in, It is the weight factor used to adjust the weight of the interaction between RRT* and the virtual potential field.

[0051] The above-mentioned method proposes a non-uniform sampling adaptive step size RRT* algorithm and a robotic arm path planning and replanning algorithm combined with RRT* virtual potential field. This method solves the problems of low efficiency, long planning time, low real-time performance and poor planning ability in dynamic environments of traditional path planning algorithms. It can enable the robotic arm to generate a safe and efficient motion path under the constraints of static and dynamic obstacles in medical scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 This is a flow chart of a method for robot arm path planning and re-planning in a dynamic medical scenario in one embodiment of the present invention;

[0053] Figure 2 is a flowchart of initial path planning of a robot arm in one embodiment of the present invention;

[0054] Figure 3 It is a schematic diagram of a local sampling space generated in the initial path planning of a robot arm in one embodiment of the present invention;

[0055] Figure 4 is a schematic diagram of adaptive step length expansion in initial path planning of a robot arm in one embodiment of the present invention;

[0056] Figure 5 is a flow chart of the robot path planning and re-planning decision control in one embodiment of the present invention;

[0057] Figure 6is a flow chart of robot path replanning in one embodiment of the present invention;

[0058] Figure 7 It is a schematic diagram of generating dangerous nodes in the path replanning of a robot arm in one embodiment of the present invention;

[0059] Figure 8 It is a schematic diagram of the expansion of the combination of RRT* and virtual potential field in the robot arm path replanning in one embodiment of the present invention. DETAILED DESCRIPTION

[0060] In order to enable those skilled in the art to better understand the technical solution of the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings.

[0061] In one embodiment, Figure 1 and Figure 2 As shown, a method for robot arm path planning and replanning in a dynamic medical scenario, the method comprising the following steps:

[0062] S100: Define the three-dimensional working space of the robot arm, determine the positions of the initial point and the target point, use the depth camera to obtain the environmental point cloud information, and track the static obstacle point cloud information and the dynamic obstacle point cloud information respectively;

[0063] Specifically, the medical scene refers to the pharmaceutical preparation environment, where the space is small and there are many obstacles. Static obstacles are usually only some fixed instruments and devices, and dynamic obstacles usually refer to culture test tube containers, etc. Point cloud information refers to the point cloud information of static obstacles and dynamic obstacles obtained through the RGB-D information of the depth camera.

[0064] S200: Set the initial point position to the initial node of the random tree , the target point position is set as the target node of the random tree ;

[0065] S300: Initial path planning based on non-uniform sampling adaptive step size RRT* algorithm: A number is randomly generated based on the Gaussian function, and random sampling is performed in the global sampling space or the local sampling space according to the relationship between the number and the preset Gaussian function constraint threshold, and the random sampling points are randomly sampled. Perform adaptive step expansion to generate new nodes ;

[0066] S400: Determine whether the distance between the nearest point cloud of the static obstacle point cloud and the new node meets the collision threshold. If so, move the new node Add random trees, perform pruning and reconnection, and plan the initial path. If the collision threshold is not met, the new node is discarded; return to S300 to resample until a safe and collision-free initial path is obtained within the preset maximum number of sampling times; further, if the initial path is not generated within the preset maximum number of sampling times, the algorithm is terminated and the path planning fails;

[0067] S500: Real-time obstacle avoidance of dynamic obstacles based on RRT* combined with virtual potential field: After obtaining the initial safe collision-free path, the path decision controller is combined to select the robot arm motion path, and the initial path node is judged in real time based on the dynamic obstacle point cloud information whether there is a dangerous node. If there is a dangerous node, the local path is replanned. When it is judged in real time that there is no dangerous node in the initial path node, the initial path is executed and the robot arm motion planning is judged whether it is completed until the robot arm completes the movement from the initial node. To the target node sports.

[0068] The above-mentioned method proposes a non-uniform sampling adaptive step size RRT* algorithm and a robotic arm path planning and replanning algorithm combined with RRT* virtual potential field. This method solves the problems of low efficiency, long planning time, low real-time performance and poor planning ability in dynamic environments of traditional path planning algorithms. It can enable the robotic arm to generate a safe and efficient motion path under the constraints of static and dynamic obstacles in medical scenarios.

[0069] In one embodiment, in S300, a number is randomly generated based on a Gaussian function, and random sampling is performed in a global sampling space or a local sampling space according to a magnitude relationship between the number and a preset Gaussian function constraint threshold, including:

[0070] S310: Generate a number randomly based on the Gaussian function. When the value is greater than the preset Gaussian function constraint threshold, select the smallest node in the random tree according to the heuristic function. , construct a local sampling space and perform random sampling; the local sampling space is the node selected according to the heuristic function The target node with random tree The constructed cubic sampling space;

[0071] S320: When the value is less than or equal to a preset Gaussian function constraint threshold, random sampling is performed in the global sampling space; wherein the global sampling space is all spatial positions that the robotic arm can reach in the three-dimensional space.

[0072] In one embodiment, the heuristic function in S310 is:

[0073] ;

[0074] in, represents the actual path length from the initial node of the random tree to the nth node, represents the Euclidean distance from the nth node to the target node of the random tree, As a weighting factor;

[0075] The range of the local sampling space is ,in:

[0076] ;

[0077] ;

[0078] ;

[0079] in, is the smallest node in the random tree selected according to the heuristic function, is the target node of the random tree.

[0080] Specifically, the sampling space is divided into the global sampling space And the local sampling space , the global sampling space It is all the spatial positions that the robot can reach in three-dimensional space, the local sampling space By node With the target node Construct a cubic sampling space, where It is obtained by searching the random tree through the heuristic function, the local sampling space It will change according to the random tree expansion, and then according to the Gaussian function and its set threshold To determine in which sampling space to generate random samples . Further, the local sampling space diagram is as follows Figure 3 shown.

[0081] The following describes in detail the process of constructing the local sampling space and the process of selecting the sampling points in the sampling point space to generate the sampling points.

[0082] First, the heuristic function is explained as follows:

[0083] ;

[0084] in, From the initial node The actual path length to the nth node, Indicates the nth node to the target node The Euclidean distance of as a weighting factor.

[0085] Then, in each expansion process, the node with the smallest heuristic function in the tree is selected according to the function , generated by and Constructed local sampling space , the range of the sampling space is ,in

[0086] ;

[0087] ;

[0088] .

[0089] Secondly, the sampling point space selection generates the sampling points as follows:

[0090] A number is randomly generated through the Gaussian function. If this number is greater than the set threshold Then randomly sample in global space to generate a sampling point Otherwise, a sampling point is generated by random sampling in the local space. .

[0091] The Gaussian function is defined as follows:

[0092] ;

[0093] in, represents the mean, Represents standard deviation.

[0094] In one embodiment, in S300, adaptive step expansion is performed according to the randomly sampled points to generate new nodes, including:

[0095] S330: According to the randomly sampled points, guide the random tree expansion direction and find the tree node closest to the randomly sampled points ;

[0096] S340: Each sampled point will adaptively set the expansion step size based on the distance between the nearest tree node and the obstacle through the preset step size range [a, b]. , generating a nearest tree node To random sampling point The direction step is the expansion step New Node .

[0097] In one embodiment, the adaptive step size in S340 is specifically:

[0098] ;

[0099] in, , are the minimum and maximum values ​​of the preset step length range, is a proportionality coefficient, Represents the sum of the distances from the nearest point cloud of all obstacles to the random sampling point.

[0100] Specifically, according to the previous step, , guide the random tree expansion direction, and find the distance The nearest tree node , each sampling node will be based on The distance to the obstacle is adaptively extended by setting the step length interval. , the schematic diagram of adaptive step size expansion in initial path planning is as follows Figure 4 As shown, thus generating a arrive The direction step length is New Node , determine the static obstacle point cloud The nearest point cloud If the distance meets the collision threshold, Add to the random tree, otherwise resample. Afterwards, the parent node will be reselected and the connection will be reconnected just like the original RRT* algorithm.

[0101] In one embodiment, Figure 5 As shown, in S500, determining whether there is a dangerous node in the planned path includes:

[0102] Set a radius of The sphere is regarded as the dangerous area of ​​the dynamic obstacle. When the dynamic obstacle moves, the nodes in the initial path that enter the dangerous area of ​​the dynamic obstacle will be regarded as dangerous nodes.

[0103] In one embodiment, Figure 6 and 7 As shown, if there is a dangerous node in S500, the local path is replanned, specifically:

[0104] S510: Get the dangerous node in the path, and use the parent node of the first dangerous node in the random tree as the initial node for replanning , take the child node of the last dangerous node in the random tree as the target node ;

[0105] S520: Track the point cloud information of dynamic obstacles and set the initial node As the root node of the tree, Expand and randomly sample to obtain random sampling points ;

[0106] S530: Construct a virtual potential field through the artificial potential field method, target node Gravitational forces on random trees in weight planning , dynamic obstacles and static obstacles produce repulsive forces on the random trees in the replanning and , according to gravity , static obstacle repulsion and dynamic obstacle repulsion Get virtual potential ;

[0107] S540: Based on randomly sampled points , guide the random tree expansion direction and find the tree node closest to the random sampling point , according to the random sampling points , the nearest tree node and virtual potential Composed of random tree expansion strategy to expand and find new nodes A collision check is performed on it. If the collision condition is met, it is added to the local path planning random tree. Otherwise, it is discarded until a final local path is generated within the preset maximum number of sampling times. The path planning is completed based on the available part of the initial path and the generated local path. If the local path is not formed within the preset maximum number of sampling times, the planning fails.

[0108] In one embodiment, in S530, the virtual potential field gravity F is specifically:

[0109] ;

[0110] Among them, gravity Specifically:

[0111] ;

[0112] in, represents the maximum value of gravity, represents the scale factor, Represents the replanning random tree Node to target node The distance between Node to target node The farther the distance, the bigger it is;

[0113] Static obstacle repulsion and dynamic obstacle repulsion Specifically:

[0114] ;

[0115] ;

[0116] in, is the maximum repulsive force, represents the scale factor, Representation Node To the static obstacle repulsion point The distance between the static obstacle and the random tree node is taken as the repulsion point. , take the point cloud closest to the random tree node of the dynamic obstacle as the repulsion point , Representation Node To the dynamic obstacle repulsion point The distance Indicates the safe distance for generating repulsion; if the node The closer the distance to the repulsion point, the greater the repulsion the obstacle will produce on it.

[0117] In one embodiment, the new node is generated in S540 in the following manner:

[0118] ;

[0119] in, It is the weight factor used to adjust the weight of the interaction between RRT* and the virtual potential field.

[0120] Specifically, Figure 5 and 6 As shown in the figure, after obtaining a planned initial path, a path decision controller is used to choose whether to keep the initial path for the robot to execute or to replan the original path. The dynamic obstacle point cloud is segmented and tracked by the depth camera, and a sphere with a radius of r is used to wrap the dynamic obstacle to represent the danger zone of the obstacle. If a dynamic obstacle moves and the nodes of the original path enter the danger zone of the dynamic obstacle, then these nodes will be regarded as dangerous nodes, such as Figure 7 Dangerous nodes cannot be used in the original path, so they will be partially replanned, and the parent node of the first dangerous node in the tree will be used as the initial node for replanning. , treat the child node of the last node of the dangerous node in the tree as the target node , and re-plan.

[0121] like Figure 8 As shown in the figure, the replanning process uses RRT* combined with the virtual potential field. First, a virtual potential field is constructed. The target node Gravitational forces on random trees in weight planning , obstacles repel the random tree in replanning In the present invention, the point cloud closest to the random tree node of the static obstacle is used as the repulsion point , take the point cloud closest to the random tree node of the dynamic obstacle as the repulsion point Therefore, in the process of expansion, the virtual potential field It is caused by gravity , static obstacle repulsion and dynamic obstacle repulsion Consists of.

[0122] After constructing the virtual potential field, the RRT* expansion strategy is combined with the virtual potential field, so The expansion strategy is based on as well as Obtained by synthesis.

[0123] After satisfying the collision detection, it is added to the random tree and the parent node is reselected and reconnected as in the original RRT* algorithm, and then a new round of sampling and expansion steps in the above method is performed.

[0124] During the replanning process, the random tree expansion strategy mentioned above is used to continuously sample and expand until a final local path is generated. The path decision controller will provide the newly generated local path to the robot for execution, thereby achieving real-time planning in a dynamic obstacle environment.

[0125] If no node in the initial path enters the dynamic obstacle danger zone, the path decision controller will continue to execute the initial path and continuously detect whether any node in the initial path enters the dynamic obstacle danger zone until the robot arm completes the arrive sports.

[0126] Compared with the prior art, the advantages of the present invention are:

[0127] (1) The technical solution of the present invention constrains the sampling space of the random tree in path planning, divides the sampling space into a global sampling space and a local sampling space, uses a heuristic function to select random tree nodes and target nodes to form a local sampling space, and uses a Gaussian distribution function to select the sampling space, thereby accelerating the sampling convergence rate, reducing the number of random tree sampling nodes and ensuring the path quality.

[0128] (2) The technical solution of the present invention sets up a path decision controller. When the initial path is affected by a dynamic obstacle, the path can be replanned, and the available part of the original path is retained. The local path is replanned, thereby reducing the need for the path planned by the traditional planning method to be replanned when it is affected by an obstacle, greatly reducing the planning time and planning complexity. If the initial path is not affected, the path decision controller uses the initial constructed path.

[0129] (3) The technical solution of the present invention utilizes the RRT* algorithm in combination with the virtual potential field during replanning, and sets a proportional factor to change the weight influence of the RRT* algorithm and the virtual potential field in the expansion, thereby reducing the arbitrariness in the RRT* expansion and the local optimal problem that may be caused by the virtual potential field to a certain extent.

[0130] (4) The technical solution of the present invention accelerates the convergence of the random tree and ensures the path quality in the initial path planning. The path replanning can generate a better path and reduce the planning time. The overall design improves the real-time performance, effectiveness and robustness compared with the traditional method.

[0131] The above is a detailed introduction to a robotic arm path planning and replanning method for dynamic medical scenarios provided by the present invention. This article uses specific examples to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the core idea of ​​the present invention. It should be pointed out that for ordinary technicians in this technical field, without departing from the principles of the present invention, several improvements and modifications can be made to the present invention, and these improvements and modifications also fall within the scope of protection of the claims of the present invention.

Claims

1. A robot arm path planning and replanning method for dynamic medical scenarios, characterized in that: The method comprises the following steps: S100: Define the three-dimensional working space of the robot arm, determine the positions of the initial point and the target point, use the depth camera to obtain the environmental point cloud information, and track the static obstacle point cloud information and the dynamic obstacle point cloud information respectively; S200: Set the initial point position to the initial node of the random tree , the target point position is set as the target node of the random tree ; S300: Initial path planning based on non-uniform sampling adaptive step size RRT* algorithm: A number is randomly generated based on the Gaussian function, and random sampling is performed in the global sampling space or the local sampling space according to the relationship between the number and the preset Gaussian function constraint threshold, and the random sampling points are randomly sampled. Perform adaptive step expansion to generate new nodes ; S400: Determine whether the distance between the nearest point cloud of the static obstacle point cloud and the new node meets the collision threshold. If so, move the new node Add a random tree, perform pruning and reconnection, and if the collision threshold is not met, discard the new node; return to S300 and resample until a safe and collision-free initial path is obtained within the preset maximum number of sampling times; S500: Real-time obstacle avoidance of dynamic obstacles based on RRT* combined with virtual potential field: After obtaining the initial safe collision-free path, the path decision controller is combined to select the robot arm motion path, and the initial path node is judged in real time based on the dynamic obstacle point cloud information whether there is a dangerous node. If there is a dangerous node, the local path is replanned. When it is judged in real time that there is no dangerous node in the initial path node, the initial path is executed and the robot arm motion planning is judged whether it is completed until the robot arm completes the movement from the initial node. To the target node sports; S300 includes: S310: Generate a number randomly based on the Gaussian function. When the randomly generated number is greater than the preset Gaussian function constraint threshold, select the smallest node in the random tree according to the heuristic function. , construct a local sampling space and perform random sampling; the local sampling space is the node selected according to the heuristic function The target node with random tree The constructed cubic sampling space; S320: When the randomly generated number is less than or equal to a preset Gaussian function constraint threshold, randomly sampling in the global sampling space; wherein the global sampling space is all spatial positions that the robot arm can reach in the three-dimensional space; S330: According to the randomly sampled points, guide the random tree expansion direction and find the tree node closest to the randomly sampled points ; S340: Each sampled point will adaptively set the expansion step size based on the distance between the nearest tree node and the obstacle through the preset step size range [a, b]. , generating a nearest tree node To random sampling point The direction step is the expansion step New Node ; The specific adaptive step size in S340 is: ； in, , are the minimum and maximum values ​​of the preset step length range, is a proportionality coefficient, Represents the sum of the distances from the nearest point cloud of all obstacles to the random sampling point.

2. The method according to claim 1, characterized in that The heuristic function in S310 is: ； in, represents the actual path length from the initial node of the random tree to the nth node, represents the Euclidean distance from the nth node to the target node of the random tree, As a weighting factor; The range of the local sampling space is ,in: ； ； ； in, is the smallest node in the random tree selected according to the heuristic function, is the target node of the random tree.

3. The method according to claim 2, characterized in that The S400 also includes: When the initial path is not generated within the preset maximum number of sampling times, the algorithm is terminated and the path planning fails.

4. The method according to claim 3, characterized in that In S500, determining whether there is a dangerous node on the planned path includes: Set a radius of The sphere is regarded as the dangerous area of ​​the dynamic obstacle. When the dynamic obstacle moves, the nodes in the initial path that enter the dangerous area of ​​the dynamic obstacle will be regarded as dangerous nodes.

5. The method according to claim 4, characterized in that If there is a dangerous node in S500, the local path is replanned, specifically: S510: Get the dangerous node in the path, and use the parent node of the first dangerous node in the random tree as the initial node for replanning , take the child node of the last dangerous node in the random tree as the target node ; S520: Track the point cloud information of dynamic obstacles and set the initial node As the root node of the tree, Expand and randomly sample to obtain random sampling points ; S530: Construct a virtual potential field through the artificial potential field method, target node Gravitational forces on random trees in weight planning , dynamic obstacles and static obstacles produce repulsive forces on the random trees in the replanning and , according to gravity , static obstacle repulsion and dynamic obstacle repulsion Get virtual potential ; S540: Based on randomly sampled points , guide the random tree expansion direction and find the tree node closest to the random sampling point , according to the random sampling points , the nearest tree node and virtual potential Composed of random tree expansion strategy to expand and generate new nodes A collision check is performed on it. If the collision condition is met, it is added to the local path planning random tree. Otherwise, it is discarded until a final local path is generated within the preset maximum number of sampling times. The path planning is completed based on the available part of the initial path and the generated local path. If the local path is not formed within the preset maximum number of sampling times, the planning fails.

6. The method according to claim 5, characterized in that In S530, the virtual potential field gravity F is specifically: ； Among them, gravity Specifically: ； in, represents the maximum value of gravity, represents the scale factor, Represents the replanning random tree Node to target node The distance between Node to target node The farther the distance, the bigger it is; Static obstacle repulsion and dynamic obstacle repulsion Specifically: ； ； in, is the maximum repulsive force, represents the scale factor, Representation Node To the static obstacle repulsion point The distance between the static obstacle and the random tree node is taken as the repulsion point. , take the point cloud closest to the random tree node of the dynamic obstacle as the repulsion point , Representation Node To the dynamic obstacle repulsion point The distance Indicates the safe distance for generating repulsion; if the node The closer the distance to the repulsion point, the greater the repulsion the obstacle will produce on it.

7. The method according to claim 6, characterized in that The specific method of generating a new node in S540 is as follows: ； in, It is the weight factor used to adjust the weight of the interaction between RRT* and the virtual potential field.

Images