Variable scene-oriented rapid obstacle avoidance path planning method for robot arm

Abstract

The application provides a variable scene-oriented mechanical arm rapid obstacle avoidance path planning method, comprising: representing a mechanical arm configuration in the form of joint space coordinates, obtaining a map sampling point set by using a map sampling point selection strategy based on a random number generator; designing a representation mode of obstacles in a landmark map according to the mapping of joint space coordinates and end coordinates of the mechanical arm in the Cartesian space; designing a multi-type sampling point connection strategy to construct a new landmark map according to the known state change information of obstacles in the scene, obtaining a landmark map containing all state change information of the obstacles; and designing a variable scene-oriented landmark map search strategy based on the A* algorithm according to the obtained landmark map, and completing the mechanical arm path search process. According to the technical scheme provided by the embodiment of the application, the shortest path in the variable scene can be rapidly searched by the mechanical arm under the premise of avoiding collision.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of mechanical arm obstacle avoidance motion planning, and relates to a variable scene-oriented mechanical arm rapid obstacle avoidance path planning method. BACKGROUND

[0002] With the continuous progress of automation technology, the mechanical arm has become a key execution mechanism in many fields such as industrial production, aerospace, etc. However, in the actual working environment, the environment and joint components of the mechanical arm may limit the motion planning of the mechanical arm. Therefore, when the mechanical arm performs a work task, it must consider the mechanical arm body and the obstacles in the environment to quickly plan a collision-free path to ensure safe and efficient completion of the task. Since the time-consuming of obstacle avoidance path planning directly affects the work efficiency of the mechanical arm, in-depth study of the rapid obstacle avoidance method of the mechanical arm and improvement of the efficiency of obstacle avoidance path planning have great theoretical value and practical significance for promoting the wide application of the mechanical arm in industrial production, aerospace and other fields.

[0003] Currently, the mechanical arm obstacle avoidance path planning method mainly includes a search-based method and a sampling-based method. The typical representative of the search-based method is the traditional heuristic search method, which needs to perform real-time collision detection on the mechanical arm during the planning process, which undoubtedly increases the time cost of path planning, and it is difficult to meet the efficiency requirement of the mechanical arm obstacle avoidance path planning.

[0004] The random roadmap method (referred to as PRM algorithm) is a typical sampling-based method, which can greatly improve the planning efficiency compared with the traditional search-based method. The PRM algorithm mainly includes a learning stage and a search stage. The learning stage spends more time on selecting sampling points and constructing a roadmap. The search stage searches for a collision-free path in the constructed roadmap. Since no large amount of collision detection process is needed at this time, the time cost of path planning is low. Although the traditional PRM algorithm has high planning efficiency, it cannot cope with the situation that the working scene changes. When the state of the obstacles in the scene where the mechanical arm is located changes, a large amount of time is needed to construct a new roadmap. Therefore, a new mechanical arm rapid obstacle avoidance path planning method based on the random roadmap method is needed to quickly plan the obstacle avoidance path under the premise of considering that the working scene may change. SUMMARY

[0005] Therefore, the application provides a variable scene-oriented mechanical arm rapid obstacle avoidance path planning method. The method can solve the problem that the traditional random roadmap method cannot be applied to a variable scene, and can still efficiently and safely complete obstacle avoidance path planning when the scene changes.

[0006] The application provides a variable scene-oriented mechanical arm rapid obstacle avoidance path planning method, comprising the following steps:

[0007] Step S1 represents the mechanical arm configuration in the form of joint space coordinates, and uses a map sampling point selection strategy based on a random number generator to obtain a map sampling point set;

[0008] Step S2 designs a representation method of obstacles in a road label map according to the mapping of joint space coordinates and end coordinates of the mechanical arm in the Cartesian space;

[0009] Step S3 designs a multi-type sampling point connection strategy to construct a new road label map according to the known state change information of the obstacles in the scene, and obtains a road label map containing all the change state information of the obstacles;

[0010] Step S4 designs a variable scene-oriented road label map search strategy based on the A* algorithm according to the obtained road label map, and completes the path search process of the mechanical arm.

[0011] In the above method, the step S1 comprises:

[0012] Step S1.1 represents a state of a k-degree-of-freedom mechanical arm in the workspace as a k-dimensional vector:

[0013] q=(θ1,θ2,...,θ k )

[0014] wherein θ i represents the joint angle of the i th joint of the mechanical arm;

[0015] Step S1.2 includes the following steps:

[0016] Step S1.2.1 sets the number of map sampling points j to 1;

[0017] Step S1.2.2 sets the joint angle i to 1;

[0018] Step S1.2.3 uses a random number generator rand() to randomly generate a joint angle within the limit of the i th joint angle of the mechanical arm, and the expression is:

[0019] θ i =rand()%(θ i max -θ i min )+θ i min

[0020] wherein θ i max , θ i minrespectively represent the maximum and minimum limit of the i-th joint angle of the robot arm, and % represents the remainder operation;

[0021] Step S1.2.4 initializes a zero vector q of dimension k j , representing the configuration of the k-degree-of-freedom robot arm, and θ i is assigned to the element at the corresponding position of q j , and the expression is:

[0022] q j [i]=θ i

[0023] where q j [i] represents the element at the i-th position of the vector q j ;

[0024] Step S1.2.5 sets i = i + 1, and if i ≤ k, returns to step S1.2.3;

[0025] Step S1.2.6 obtains a set of robot arm configurations q j =(θ1,θ2,...,θ k ), and adds the robot arm configuration q j as a sample point of the landmark map to the set of map sample points P;

[0026] Step S1.2.7 sets j = j + 1;

[0027] Step S1.2.8 sets the maximum number of map sample points as M, and if j ≤ M, returns to step S1.2.2, otherwise skips this step;

[0028] Step S1.2.9 obtains the set of map sample points P:

[0029] P={q1,q2,…,q M}

[0030] where M is set according to task requirements.

[0031] In the above method, the step S2 comprises:

[0032] All sample points q=(θ1,θ2,...,θ k ) in the set of map sample points P are taken out, and collision detection is performed on all sample points in turn, and all sample points that collide with the robot arm itself or obstacles are screened out, and the set of sample points that collide with obstacles in the working environment is obtained:

[0033] S={q1,q2,...,q N}

[0034] where S represents a set of sampling points in which the robot arm collides with obstacles, q i (i = 1, 2, …, N) represents a set of sampling points in which the robot arm collides with obstacles in the working environment, and the set S is used to represent obstacles in the working environment in the roadmap.

[0035] In the above method, the step S3 comprises:

[0036] Step S3.1 divides the obstacles in the working space into fixed obstacles and variable obstacles, and for the variable obstacles, all variable states of the obstacles are represented in the working space.

[0037] Step S3.2 divides the set S of configurations in which the existing robot arm collides with obstacles into a set S1 of configurations in which the robot arm collides with fixed obstacles and a set S2 of configurations in which the robot arm collides with variable obstacles, and S2 represents a set of configurations in which the robot arm collides with various possible states of the variable obstacles:

[0038]

[0039] where represents configurations in which the robot arm collides with variable obstacles A, B, etc. in states a, b, etc.

[0040] Step S3.3 of the roadmap multi-type sampling point connection strategy comprises:

[0041] Step S3.3.1 divides the obtained set P of map sampling points into a set S of collision points in which the robot arm collides with obstacles and a set P0 of safe sampling points in which the robot arm does not collide with obstacles, and divides the set S of collision points into a set S1 of dangerous sampling points in which the robot arm collides with fixed obstacles and a set S2 of threat sampling points in which the robot arm collides with variable obstacles.

[0042] Step S3.3.2 deletes the set S1 of dangerous sampling points in the roadmap, and does not participate in the connection of sampling points in the roadmap, and the set R of remaining sampling points in the roadmap is obtained.

[0043] Step S3.3.3 sets the index i of the roadmap sampling point to 1, and the number of sampling points in the set R is r.

[0044] Step S3.3.4 calculates the distance between the sampling point q i and all sampling points in the set R, and the expression is:

[0045]

[0046] where k is the degree of freedom of the robot arm, q j is a sampling point in the set R, and d j is the distance between the sampling point q i and the sampling point qj the distance between the sampling point q i and all the sampling points in the set R is calculated using the set D i to characterize.

[0047] Step S3.3.5 selects the h sampling points closest to the sampling point q i in the set D i and connects the sampling points, if the sampling point to be connected belongs to the safe sampling point set P0, the corresponding connected edge is defined as a reliable edge, if the sampling point to be connected belongs to the threat sampling point set S2, the corresponding connected edge is defined as an unreliable edge;

[0048] Step S3.3.6 sets i = i + 1, if i ≤ r, returns to step S3.3.4, otherwise skips this step;

[0049] Step S3.3.7 obtains the road map G.

[0050] In the above method, the step S4 comprises:

[0051] Step S4.1 obtains the current state of the variable obstacle from the working environment;

[0052] Step S4.2 extracts the sampling point set S2' representing all states of the variable obstacle in the threat sampling point set S2, matches the current state of the variable obstacle, and finds the sampling point q danger in the sampling point set S2' representing all states of the variable obstacle that matches the current state of the variable obstacle;

[0053] Step S4.3 converts the sampling point q danger into a dangerous sampling point, and converts all the sampling points in the threat sampling point set S2 into safe sampling points, and converts the threat sampling point set S2 in the road map G into a dangerous sampling point and a safe sampling point set;

[0054] Step S4.4 converts the unreliable edge connected to the dangerous sampling point into a dangerous edge with a weight value of +∞, and converts the unreliable edge not connected to the dangerous sampling point into a reliable edge without changing the weight value;

[0055] Step S4.5 uses the A* algorithm to search for a path with the minimum sum of edge weights from the start point to the end point in the new road map, and completes the fast obstacle avoidance path planning of the robot arm. BRIEF DESCRIPTION OF DRAWINGS

[0056] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings needed to be used in the embodiments will be briefly introduced as follows. Obviously, the drawings in the following description are only some embodiments of the present application, and for those skilled in the art, other drawings can be obtained without creative and laborious work.

[0057] Figure 1 is a flowchart of a variable scene-oriented robot arm rapid obstacle avoidance path planning method provided by an embodiment of the present application;

[0058] Figure 2 is a four-degree-of-freedom robot arm model schematic diagram used in a simulation experiment in the embodiment of the present application;

[0059] Figure 3 is a planning result schematic diagram of a variable obstacle becoming a fixed obstacle in the embodiment of the present application;

[0060] Figure 4 is a planning result schematic diagram of a variable obstacle not becoming a fixed obstacle in the embodiment of the present application.

Specific embodiments

[0061] In order to better understand the technical solutions of the present application, the embodiments of the present application will be described in detail below with reference to the drawings.

[0062] It should be clear that the described embodiments are only some of the embodiments of the present application, but not all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor are within the scope of protection of the present application.

[0063] An embodiment of the present application provides a variable scene-oriented robot arm rapid obstacle avoidance path planning method, please refer to Figure 1 , which is a flowchart of a variable scene-oriented robot arm rapid obstacle avoidance path planning method provided by an embodiment of the present application, as shown in Figure 1 , the method comprises the following steps:

[0064] Step 101, representing the robot arm configuration in the form of joint space coordinates, using a map sampling point selection strategy based on a random number generator, obtaining a set of map sampling points;

[0065] Specifically, first, a state of a k-degree-of-freedom robot arm in a workspace is represented as a k-dimensional vector:

[0066] q=(θ1,θ2,…,θ k )

[0067] Wherein, θ i represents the joint angle of the i th joint of the robot arm;

[0068] Secondly, the selection of the map sampling points is completed based on a random number generator according to the following flow:

[0069] Step 1, let the number of map sampling points j = 1;

[0070] Step 2, let the joint angle i = 1;

[0071] Step 3, use the random number generator rand() to randomly generate a joint angle within the limit of the i-th joint angle of the robot arm, expressed as:

[0072] θ i rand() % (θ i max - θ i min ) + θ i min

[0073] where θ i max , θ i min represent the maximum limit and the minimum limit of the i-th joint angle of the robot arm respectively, and % represents the remainder operation;

[0074] Step 4, initialize a zero vector q j of dimension k, representing the configuration of the k-degree-of-freedom robot arm, and assign θ i to the element at the corresponding position of q j , expressed as:

[0075] q j [i] = θ i

[0076] where q j [i] represents the element at the i-th position of the vector q j

[0077] Step 5, let i = i + 1, if i ≤ k, return to Step 3;

[0078] Step 6, obtain a set of robot arm configurations q j = (θ1, θ2, …, θ k ), and add the robot arm configuration q j as a sampling point of the landmark map to the set of map sampling points P;

[0079] Step 7, let j = j + 1;

[0080] Step 8, let the maximum number of map sampling points be M, if j ≤ M, return to Step 2, otherwise skip this step;

[0081] Step 9, obtain the set of map sampling points P:​

[0082] P = {q1, q2,..., q M}

[0083] Wherein, the selection of M is set according to task requirements.

[0084] Step 102, according to the mapping of joint space coordinates and Cartesian space robot end coordinates, a kind of road map in the representation of obstacles is designed;

[0085] Specifically, all sample points q = (θ1, θ2,..., θ k ) in the map sample point set P are taken out, collision detection is carried out for all sample points in turn, all sample points colliding with the robot itself or obstacles are screened out, the sample points colliding with the obstacles are represented in set form, and the sample point set colliding with the obstacles in the working environment is obtained:

[0086] S = {q1, q2,..., q N}

[0087] Wherein S represents the sample point set colliding with the obstacles, q i (i = 1, 2,..., N) represents the sample point colliding with the obstacles in the working environment in the set, and the sample point set S is used to represent the obstacles in the working scene in the road map.

[0088] Step 103, according to the known state change information of the obstacles in the scene, a kind of multi-type sample point connection strategy is designed to construct a new road map, and a road map containing all state change information of the obstacles is obtained;

[0089] Specifically, first, the obstacles in the working space are divided into fixed obstacles and variable obstacles, and for the variable obstacles, all variable states of the obstacles are represented in the working space.

[0090] Secondly, the existing set S of robot configurations colliding with the obstacles is divided into set S1 and S2 according to collision with fixed obstacles and collision with variable obstacles, and S2 represents the set of robot configurations colliding with various possible states of variable obstacles:

[0091]

[0092] Wherein represents the configuration of the robot colliding with the variable obstacles A, B, etc. in the states a, b, etc.

[0093] Finally, according to the following process, various types of sample points in the road map are connected:

[0094] Step 1, divide the obtained map sampling point set P into a collision point set S which collides with obstacles and a safe sampling point set P0 which does not collide with obstacles, divide the collision point set S into a dangerous sampling point set S1 which collides with fixed obstacles and a threat sampling point set S2 which collides with variable obstacles;

[0095] Step 2, delete the dangerous sampling point set S1 in the landmark map, and do not participate in the connection of sampling points in the landmark map, and let the remaining sampling point set in the landmark map be R;

[0096] Step 3, let the number of sampling points in the landmark map be i = 1, and the total number of sampling points in the sampling point set R be r;

[0097] Step 4, calculate the distance between the sampling point q i and all sampling points in the sampling point set R, expressed as:

[0098]

[0099] Where k is the degree of freedom of the robot arm, q j is a sampling point in the set R, d j is the distance between the sampling point q i and the sampling point q j , and the distance between the sampling point q i and all sampling points in the sampling point set R is characterized by the set D i ;

[0100] Step 5, select the h closest sampling points to the sampling point q i in the set D i , connect the sampling points, set the weight of the edge after connection to the distance between the sampling points, and if the sampling points to be connected belong to the safe sampling point set P0, define the corresponding connected edge as a reliable edge, and if the sampling points to be connected belong to the threat sampling point set S2, define the corresponding connected edge as an unreliable edge;

[0101] Step 6, let i = i + 1, if i ≤ r, return to step 4, otherwise skip this step;

[0102] Step 7, obtain the landmark map G.

[0103] Step 104, based on the obtained landmark map, design a variable scene-oriented landmark map search strategy based on A* algorithm to complete the robot path search process.

[0104] Specifically, first obtain the current state of the variable obstacle from the working environment;

[0105] Secondly, the sampling point set S2' representing all states of the variable obstacle is taken out from the sampling point set S2 in the threat sampling point set S2, and is matched with the current state of the variable obstacle, and the sampling point q matched with the current state of the variable obstacle is found in the sampling point set S2' representing all states of the variable obstacle danger ;

[0106] Then, the sampling point q danger is converted into a dangerous sampling point, and all other sampling points in the threat sampling point set S2 are converted into safe sampling points, so that the threat sampling point set S2 in the roadmap map G is converted into a dangerous sampling point and a safe sampling point set;

[0107] Then, the unreliable edge connected with the dangerous sampling point is converted into a dangerous edge, and the weight is set to +∞, and the unreliable edge not connected with the dangerous sampling point is converted into a reliable edge, and the weight is not changed;

[0108] Finally, the A* algorithm is used to search for the path with the minimum sum of edge weights from the starting point to the ending point in the new roadmap, and the fast obstacle avoidance path planning of the mechanical arm is completed.

[0109] According to the above method provided by the embodiment of the application, a simulation experiment is performed on a four-degree-of-freedom mechanical arm. Please refer to Figure 2 , which is a four-degree-of-freedom mechanical arm model. The D-H parameters of the mechanical arm are shown in Table 1.

[0110] Table 1: DH parameters of the mechanical arm

[0111]

[0112] A simulation environment is built by using C++ language and Qt development framework for test verification. The number of initial sampling points is selected to be 1000, and the experimental running environment is a personal computer with an Intel(R) Core(TM) i5-11400F@2.60GHz processor. This embodiment adopts a two-dimensional plane mode to demonstrate the effectiveness of the improved PRM algorithm. Please refer to Figure 3 , Figure 4 . Figure 3 is a schematic diagram of path planning after the threat obstacle is converted into a fixed obstacle. As can be seen from the diagram, the shortest path avoids the threat obstacle; Figure 4 is a schematic diagram of path planning after the threat obstacle is converted into a fixed obstacle. As can be seen from the diagram, the shortest path avoids the threat obstacle;

[0113] The following two configurations q start and q end are selected as the starting and target configurations of this embodiment, and obstacles affecting the operation of the mechanical arm are added in the mechanical arm motion space, including fixed obstacles and variable obstacles.

[0114] Initial configuration q start : [180°, -21°, 46°, 0°];

[0115] Target configuration q end : [120°, -21°, 46°, 0°];

[0116] After the state of the variable obstacle is changed from A to B, the experimental results are shown in the following table.

[0117] Table 2 Obstacle avoidance path planning results

[0118]

[0119]

[0120] Due to the change of the obstacle state, the result of the manipulator obstacle avoidance path planning also changes, and the planning result of the obstacle state B is subjected to collision detection in the working environment of the obstacle state A. When the manipulator configuration is moved from [180°, -21°, 46°, 0°] to [168°, -21°, 48°, -176°], collision occurs with the obstacle in the state A, and no collision occurs with the obstacle in the state B during the whole process, proving the effectiveness of the algorithm.

[0121] The RRT obstacle avoidance path planning method is used for planning q start , q end , and the planning time is counted, and the experimental results are shown in the following table.

[0122] Table 3 Comparison of planning time of RRT algorithm and improved PRM algorithm

[0123]

[0124] Under the experimental conditions, the improved planning algorithm reduces the planning time of the traditional RRT obstacle avoidance path planning method by 32.19%, proving the efficiency of the algorithm.

[0125] The technical scheme of the embodiment of the application has the following beneficial effects:

[0126] After the variable obstacle is characterized in the roadmap map, when the obstacle state in the working scene of the manipulator changes, it is not necessary to spend a lot of time to construct the roadmap map again, and the shortest path can be directly searched in the constructed roadmap map, and the searched path meets the collision-free requirement.

[0127] The above merely provides the preferred embodiment of the application, and is not used to limit the application. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the application shall fall within the protection scope of the application.

[0128] The contents not described in detail in the specification of the application are the known technology of the skilled in the art.

Claims

1. A variable scene-oriented rapid obstacle avoidance path planning method for a mechanical arm, characterized in that, The method comprises: Step S1 represents the mechanical arm configuration in the form of joint space coordinates, obtains a set of map sampling points using a map sampling point selection strategy based on a random number generator; Step S2 designs a representation method of obstacles in the road map according to the mapping between the joint space coordinates and the end coordinates of the mechanical arm in the Cartesian space; Step S3 designs a multi-type sampling point connection strategy to construct a new road map according to the known state change information of the obstacles in the scene, and obtains a road map containing all state change information of the obstacles; Step S4 designs a road map search strategy for variable scenes based on the A* algorithm according to the obtained road map, and completes the mechanical arm path search process.

2. The method of claim 1, wherein, The step S1 comprises: Step S1.1 represents a certain state of the k-degree-of-freedom mechanical arm in the workspace as a k-dimensional vector: q = (θ1, θ2,..., θ k ) wherein θ i denotes the joint angle of the i-th joint of the robot arm; Step S1.2, the map sampling point selection strategy based on the random number generator comprises: Step S1.2.1 sets the number of map sampling points j = 1; Step S1.2.2 sets the joint angle i = 1; Step S1.2.3 uses a random number generator rand() to randomly generate a joint angle within the limit of the i th joint angle of the mechanical arm, and the expression is: wherein, respectively represent the maximum and minimum limit of the i-th joint angle of the robot arm, and % represents the modulo operation. Step S1.2.4 initializes a k-dimensional zero vector q j , representing the configuration of the k-DOF robot arm, and assigns to the elements of q i corresponding positions the values of θ j , the expression is: q j [i] = θ i where q j [i] denotes the vector q j the element of the ith position; Step S1.2.5 sets i = i + 1, and returns to step S1.2.3 if i ≤ k; Step S1.2.6 obtains a set of robot arm configurations q j = (θ1, θ2,..., θ k ), adds the robot arm configuration q j as a sample point of the landmark map into the map sample point set P; Step S1.2.7 sets j = j + 1; Step S1.2.8 sets the maximum number of map sampling points as M, and returns to step S1.2.2 if j ≤ M, otherwise skips this step; Step S1.2.9 obtains the set of map sampling points P: P = {q1, q2,..., q M} Wherein, the selection of M is set according to the task requirement.

3. The method of claim 1, wherein, The step S2 comprises: All the sampling points q = (θ1, θ2,..., θn) in the set P of map sampling points are taken out k , collision detection is performed on all the sampling points in turn, and all the sampling points colliding with the robot itself or obstacles are screened out. The sampling points colliding with obstacles are represented in a set form, and a set of sampling points colliding with obstacles in the working environment is obtained: S = {q1, q2,..., q N} where S represents a set of sampling points in which the robot arm collides with obstacles, q i (i = 1, 2,..., N) represents a sampling point in which the robot arm collides with obstacles in the working environment, and the set S is used to represent obstacles in the working scene in the roadmap.

4. The method of claim 1, wherein, The step S3 comprises: Step S3.1 divides the obstacles in the workspace into fixed obstacles and variable obstacles, and represents all variable states of the obstacles in the workspace for the variable obstacles; Step S3.2 divides the set S of the existing mechanical arm configurations that collide with the obstacles into set S1 and set S2 according to the collision with the fixed obstacles and the collision with the variable obstacles, and set S2 represents the set of mechanical arm configurations that collide with all possible states of the variable obstacles: wherein represents the configuration in which the robot arm collides with the variable obstacle A, B, etc. in the state a, b, etc. Step S3.3, the multi-type sampling point connection strategy of the road map comprises: Step S3.3.1 divides the obtained set of map sampling points P into a collision point set S that collides with the obstacles and a safe sampling point set P0 that does not collide with the obstacles, and divides the collision point set S into a dangerous sampling point set S1 that collides with the fixed obstacles and a threat sampling point set S2 that collides with the variable obstacles; Step S3.3.2 deletes the dangerous sampling point set S1 in the road map, and does not participate in the connection of the sampling points in the road map, and sets the remaining sampling point set in the road map as R; Step S3.3.3 sets the number of road map sampling points i = 1, and the total number of sampling points in the sampling point set R is r; Step S3.3.4 calculates the sampling point q i the distance to all sampling points in the set R, expressed as: where k is the degree of freedom of the robot arm, q j is the sampling point in set R, d j is the distance between the sampling point q i and the sampling point q j , the distance between the sampling point q i and all sampling points in set R is characterized by set D i ; Step S3.3.5 selects a sampling point q from the set D i the h nearest sampling points to the sampling point q i The h nearest sampling points to the sampling point q are connected, and the weight of the connected edge is the distance between the sampling points. If the sampling point to be connected belongs to the safe sampling point set P0, the corresponding connected edge is defined as a reliable edge. If the sampling point to be connected belongs to the threat sampling point set S2, the corresponding connected edge is defined as an unreliable edge. Step S3.3.6 sets i = i + 1, and returns to step S3.3.4 if i ≤ r, otherwise skips this step; Step S3.3.7 obtains the road map G.

5. The method of claim 4, wherein, The step S4 comprises: Step S4.1 obtains the current state of the variable obstacle from the working environment; Step S4.2 takes out the sampling point set S2' representing all states of the variable obstacle from the sampling point set S2 of the threat, matches the current state of the variable obstacle, and finds the sampling point q in the sampling point set S2' representing all states of the variable obstacle that matches the current state of the variable obstacle danger ; Step S4.3 transforms the sampling point q danger All other sampling points in the threat sampling point set S2 are transformed into safe sampling points. Step S4.4 transforms the unreliable edges connected with the dangerous sampling points into dangerous edges, and sets the weight value as +∞; transforms the unreliable edges not connected with the dangerous sampling points into reliable edges, and does not change the weight value; Step S4.5 searches the path with the minimum sum of edge weight values from the starting point to the ending point in the new road map by using the A* algorithm, and completes the fast obstacle avoidance path planning of the mechanical arm.

Images