Mechanical arm autonomous obstacle avoidance planning method based on improved artificial potential field method

Abstract

The invention discloses a mechanical arm autonomous obstacle avoidance planning method based on an improved artificial potential field method, and relates to the field of mechanical arm path planning and obstacle avoidance, in particular to the mechanical arm autonomous obstacle avoidance planning method based on the improved artificial potential field method. The objective of the invention is to solve the problem of low efficiency in an obstacle avoidance planning process of an existing method. The method comprises the following steps: constructing a forward kinematics transformation matrix from a base coordinate system to an end effector of the mechanical arm; a collision-free feasible path of the tail end of the mechanical arm from a starting point to a target point is solved by considering obstacles of a static part; key path points are extracted; the key path points containing the mechanical arm joint angle information are sequentially stored in a path point queue from the starting point to the end point to serve as temporary target points for improving the artificial potential field method; and key path points are selected from the path point queue to serve as current temporary target points, and an improved artificial potential field method and a collision early warning mechanism are used for guiding the mechanical arm to advance towards the temporary path points till the mechanical arm moves to the target points.

Description

Technical Field

[0001] The present invention relates to the field of robot arm path planning and obstacle avoidance, and in particular to a robot arm autonomous obstacle avoidance planning method based on an improved artificial potential field method. Background Art

[0002] Under the concept of intelligent manufacturing, robotic arms are widely used in industrial scenarios such as welding, assembly, and handling to improve production efficiency. Robotic arms usually perform repetitive tasks in a single work environment instead of humans through offline manual teaching or hard coding, and leave the remaining work to humans. They are usually placed in a protective fence to work and isolate themselves from people. However, under the trend of flexible and customized production, this production model is gradually insufficient to meet the needs. More and more work tasks require workers and robotic arms to work closely together in a shared space. The workspace of the robotic arm has also changed from a single structured environment to a complex unstructured environment. This requires it to have good autonomous obstacle avoidance and planning capabilities to improve production efficiency while ensuring the safety of the environment and workers.

[0003] At present, there are many effective solutions to the obstacle avoidance planning problem of mobile robots. However, due to the high-dimensional characteristics of robotic arms, path planning algorithms that perform well in the field of mobile robots, such as the A* algorithm and the DWA algorithm, are difficult to transplant and use to solve the obstacle avoidance planning problem of robotic arms. In addition, most of the existing research on the obstacle avoidance planning problem of robotic arms is conducted in a static environment using a plan-first, then execute strategy, which does not have good responsiveness to environmental changes and dynamic obstacles.

[0004] The artificial potential field method is a local path planning algorithm based on a virtual potential field. The target point generates an attractive potential field on the robotic arm, while obstacles generate a repulsive potential field on the robotic arm. Ultimately, the robotic arm moves along the descending gradient of the combined potential field. The artificial potential field method offers significant advantages in obstacle avoidance planning for robotic arms due to its concise mathematical description and high scalability in high dimensions. However, traditional artificial potential field methods suffer from limited foresight, local minima, and relatively limited dynamic response capabilities. Summary of the Invention

[0005] The purpose of the present invention is to solve the problem of low efficiency of the obstacle avoidance planning process of the existing methods and to propose an autonomous obstacle avoidance planning method for a manipulator based on an improved artificial potential field method.

[0006] The specific process of a robotic arm autonomous obstacle avoidance planning method based on the improved artificial potential field method is as follows:

[0007] Step 1: Construct the transformation matrix between the joint coordinate systems of adjacent joints of the robotic arm;

[0008] Based on the transformation matrix between the joint coordinate systems of adjacent joints of the manipulator, construct the forward kinematic transformation matrix from the base coordinate system to the i-th joint coordinate system of the manipulator, where i = 1, 2, 3, 4, 5, 6;

[0009] Without considering the operation of the end effector of the manipulator, the positive kinematic transformation matrix from the base coordinate system to the sixth joint coordinate system of the manipulator is used as the positive kinematic transformation matrix from the base coordinate system to the end effector of the manipulator;

[0010] Step 2: Based on the forward kinematic transformation matrix from the base coordinate system constructed in step 1 to the end effector of the manipulator, consider some static obstacles and use the improved RRT* algorithm to solve a collision-free feasible path from the starting point to the target point of the end effector of the manipulator;

[0011] Step 3: Based on the feasible path obtained in step 2, use the pruning strategy to delete redundant path points and extract key path points;

[0012] Step 4: Based on the key path points in step 3, obtain the robot arm joint angle corresponding to each key path point;

[0013] The key path points containing the robot arm joint angle information are stored in the path point queue in order from the starting point to the end point, which serves as the temporary target point of the improved artificial potential field method.

[0014] Step 5. Select a key path point from the path point queue in step 4 as the current temporary target point, and use the improved artificial potential field method and collision warning mechanism to guide the robot arm to move towards the temporary path point until it reaches the target point.

[0015] The beneficial effects of the present invention are:

[0016] The purpose of the present invention is to address the shortcomings of the existing technology and provide a method for autonomous obstacle avoidance planning of a robotic arm based on an improved artificial potential field method. By introducing an improved RRT* algorithm, the foresight of the robotic arm's obstacle avoidance planning process is improved, and the local minimum problem that is prone to occur in the traditional artificial potential field method is avoided. At the same time, a virtual potential field for predicting obstacles is introduced to improve the efficiency of the obstacle avoidance planning process and enhance the robotic arm's responsiveness to dynamic obstacles during the obstacle avoidance planning process. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 Flowchart of the present invention;

[0018] Figure 2 This is a collision warning flow chart;

[0019] Figure 3 This is map information of Example 1;

[0020] Figure 4 This is a global path diagram of Example 1;

[0021] Figure 5 Planning a complete path for obstacle avoidance in Example 1;

[0022] Figure 6 This is the map information of Example 2;

[0023] Figure 7 Planning a complete path for obstacle avoidance in Example 2;

[0024] Figure 8a This is the angle-time image of joint 1 during the obstacle avoidance planning process of the robotic arm in Example 2;

[0025] Figure 8b This is the angle-time image of joint 2 during the robot arm's obstacle avoidance planning process in Example 2;

[0026] Figure 8c This is the time image of joint 3 angles during the obstacle avoidance planning process of the robot arm in Example 2;

[0027] Figure 8d This is the time image of the four joint angles during the obstacle avoidance planning process of the robot arm in Example 2;

[0028] Figure 8e This is the time image of the joint 5 angles during the obstacle avoidance planning process of the robot arm in Example 2;

[0029] Figure 8f This is the time image of the six joint angles during the obstacle avoidance planning process of the robotic arm in Example 2;

[0030] Figure 8g This is the x, y, and z position-time image at the end of the robot arm's obstacle avoidance planning process in Example 2. DETAILED DESCRIPTION

[0031] Specific implementation method 1: Combination Figure 1 This embodiment describes a method for autonomous obstacle avoidance planning of a robotic arm based on an improved artificial potential field method. The specific process is as follows:

[0032] Step 1: Construct the transformation matrix between the joint coordinate systems of adjacent joints (i-th joint and i-1-th joint) of the robot arm (for example, the coordinate transformation from the joint 2 coordinate system to the joint 3 coordinate system);

[0033] Based on the transformation matrix between the joint coordinate systems of adjacent joints of the manipulator, construct the forward kinematic transformation matrix from the base coordinate system to the i-th joint coordinate system of the manipulator, where i = 1, 2, 3, 4, 5, 6;

[0034] Without considering the operation of the end effector of the manipulator, the positive kinematic transformation matrix from the base coordinate system to the sixth joint coordinate system of the manipulator is used as the positive kinematic transformation matrix from the base coordinate system to the end effector of the manipulator;

[0035] The present invention considers the obstacle avoidance problem of the manipulator, without considering the operation of the manipulator's end effector. Therefore, the positive kinematic transformation matrix from the base coordinate system to the manipulator's sixth joint coordinate system is used as the positive kinematic transformation matrix from the base coordinate system to the manipulator's end effector.

[0036] Step 2: Based on the forward kinematic transformation matrix from the base coordinate system constructed in step 1 to the end effector of the manipulator, consider some static obstacles and use the improved RRT* algorithm to solve a collision-free feasible path from the starting point to the target point in Cartesian space for the end effector of the manipulator;

[0037] The improved RRT* algorithm considers static obstacles in the robot's workspace for global path planning. Based on the traditional RRT* path planning algorithm, it introduces target bias sampling and pruning strategies.

[0038] Step 3: Based on the feasible path obtained in step 2, use the pruning strategy to delete redundant path points and extract key path points;

[0039] Step 4: Based on the key path points in step 3, obtain the robot arm joint angle corresponding to each key path point;

[0040] The key path points containing the robot arm joint angle information are stored in the path point queue in order from the starting point to the end point, which serves as the temporary target point of the improved artificial potential field method.

[0041] Step 5. Select a key path point from the path point queue in step 4 as the current temporary target point, and use the improved artificial potential field method and collision warning mechanism to guide the robot arm to move towards the temporary path point until it reaches the target point.

[0042] Specific embodiment 2: This embodiment differs from specific embodiment 1 in that: in step 1, the transformation matrix between the joint coordinate systems of adjacent joints (i-th joint and i-1-th joint) of the robot arm is constructed (for example, the coordinate transformation from the joint two coordinate system to the joint three coordinate system).

[0043] Based on the transformation matrix between the joint coordinate systems of adjacent joints of the manipulator, construct the forward kinematic transformation matrix from the base coordinate system to the i-th joint coordinate system of the manipulator, where i = 1, 2, 3, 4, 5, 6;

[0044] Without considering the operation of the end effector of the manipulator, the positive kinematic transformation matrix from the base coordinate system to the sixth joint coordinate system of the manipulator is used as the positive kinematic transformation matrix from the base coordinate system to the end effector of the manipulator;

[0045] The specific process is:

[0046] In the step 1, the working space of the robot arm is described by the joint space and the Cartesian space;

[0047] Definition: The joint space of the robotic arm is a coordinate system that describes the state of the robotic arm, and the angle of each joint is used to represent the state of the robotic arm; the joint space is a multidimensional space composed of all joint variables, and each dimension corresponds to the motion variable of a joint.

[0048] The Cartesian space of the robot arm is defined by the position and posture of the end of the robot arm to describe the state of the robot arm. The position part is the three-dimensional coordinate of the end of the robot arm in space, and the posture part can be expressed by Euler angles, that is, the rotation angle of the end of the robot arm relative to the three axes of the base coordinate system, or by a 3×3 rotation matrix R=[r ij ]express.

[0049] Define the base coordinate system: For the UR5 robot arm in this example, the origin of the base coordinate system is at the center of the first joint axis, the positive direction of the Z axis is vertically upward, the positive direction of the X axis is forward, and the Y axis follows the right-hand rule.

[0050] Define the coordinate system of each joint: Each joint coordinate system follows the DH parameter convention. Taking joint i as an example, the direction of the Zi axis coincides with the rotation axis of joint i, the Xi axis is the direction of the common normal between the Zi axis and the Zi+1 axis, the Yi axis conforms to the right-hand rule, and the origin Oi is located at the intersection of the common normal between the Zi axis and the Zi+1 axis and the Zi axis.

[0051] The specific process is:

[0052] Step 1: Construct the transformation matrix between the joint coordinate systems of adjacent joints (i-th joint and i-1-th joint) of the robot arm (for example, the coordinate transformation from the joint 2 coordinate system to the joint 3 coordinate system); the transformation matrix is ​​expressed as:

[0053]

[0054] Among them, l i is the length of the rod corresponding to the i-th joint, α i is the torsion angle of the rod at the i-th joint, b i is the rod offset corresponding to the i-th joint, θ i is the rod rotation angle of the i-th joint; is the transformation matrix between the joint coordinate system of the i-1th joint and the i-th joint, i = 1, 2, 3, 4, 5, 6; l i , α i 、b i ,θ i The parameters are all known;

[0055] Step 1 and 2: Based on the transformation matrix between the joint coordinate systems of adjacent joints of the manipulator, construct the forward kinematic transformation matrix from the base coordinate system to the i-th joint coordinate system of the manipulator, where i = 1, 2, 3, 4, 5, 6; expressed as:

[0056]

[0057]

[0058]

[0059]

[0060]

[0061]

[0062] in,

[0063] T1 0 Represents the positive kinematic transformation matrix from the base coordinate system to the first joint coordinate system of the manipulator, Represents the positive kinematic transformation matrix T1 from the base coordinate system to the first joint coordinate system of the manipulator 0 middle element;

[0064] Represents the transformation matrix between the joint coordinate system of the k-1th joint and the kth joint, k = 1, 2, 3, 4, 5, 6, k represents the sequence number;

[0065] Represents the positive kinematic transformation matrix from the base coordinate system to the second joint coordinate system of the robotic arm, Represents the positive kinematic transformation matrix from the base coordinate system to the second joint coordinate system of the manipulator middle element;

[0066] Represents the positive kinematic transformation matrix from the base coordinate system to the third joint coordinate system of the manipulator, Represents the positive kinematic transformation matrix T3 from the base coordinate system to the third joint coordinate system of the manipulator 0 middle element;

[0067] Represents the positive kinematic transformation matrix from the base coordinate system to the fourth joint coordinate system of the manipulator, Represents the positive kinematic transformation matrix T4 from the base coordinate system to the fourth joint coordinate system of the manipulator 0 middle element;

[0068] Represents the positive kinematic transformation matrix from the base coordinate system to the fifth joint coordinate system of the manipulator, Represents the positive kinematic transformation matrix T5 from the base coordinate system to the fifth joint coordinate system of the manipulator 0 middle element;

[0069] Represents the positive kinematic transformation matrix from the base coordinate system to the sixth joint coordinate system of the robotic arm, Represents the positive kinematic transformation matrix from the base coordinate system to the sixth joint coordinate system of the manipulator middle element;

[0070] represents the posture of the i-th joint of the robot arm, i = 1, 2, 3, 4, 5, 6;

[0071] Represents the posture R of the i-th joint of the robotic arm i middle element;

[0072] represents the position of the i-th joint in the base coordinate system, that is, the xyz coordinate value of the i-th joint in the base coordinate system, i = 1, 2, 3, 4, 5, 6; the superscript T indicates the transpose;

[0073] π represents matrix multiplication;

[0074] Step 13: The forward kinematic transformation matrix from the base coordinate system to the sixth joint coordinate system of the robotic arm As the forward kinematic transformation matrix from the base coordinate system to the end effector of the manipulator; expressed as:

[0075] Multiply the positive kinematic transformation matrix of the six joint base coordinate systems to the manipulator joint coordinate system to obtain the positive kinematic transformation matrix of the manipulator end effector:

[0076]

[0077] Among them, r 11 、r 12 、r 13 、r 21 、r 22 、r 23 、r 31 、r 32 、r 33 、p x 、p y 、p z Represents the forward kinematic transformation matrix of the robotic arm end effector middle element;

[0078] Posture of the robotic arm end effector

[0079] Among them, r 11 、r 12 、r 13 、r 21 、r 22 、r 23 、r 31 、r 32 、r 33 Represents an element in R;

[0080] [p x p y p z ] T It represents the position of the end effector of the robot arm in the base coordinate system, that is, the xyz coordinate value of the end effector of the robot arm in the base coordinate system.

[0081] Other steps and parameters are the same as those in the first embodiment.

[0082] Specific embodiment three: This embodiment differs from specific embodiments one or two in that: in step two, based on the forward kinematic transformation matrix from the base coordinate system constructed in step one to the end effector of the manipulator, an improved RRT* algorithm is used to solve a collision-free feasible path from the starting point to the target point of the end effector of the manipulator in Cartesian space, taking into account static obstacles;

[0083] Step 2 uses the location mentioned in step 1 [p x p y p z ] T , the initial end position of the robot arm is the starting point in step 2. Then, in the task, there will be an end state of the robot arm. Similarly, the robot forward kinematic transformation matrix is ​​used to obtain the end position of the robot arm at the end, which is the end point in step 2.

[0084] The improved RRT* algorithm considers static obstacles in the robot's workspace for global path planning. Based on the traditional RRT* path planning algorithm, it introduces target bias sampling and pruning strategies.

[0085] The specific process is:

[0086] Step 21: Add the path starting point to the empty tree structure T′ to obtain the initial current tree structure T′;

[0087] The starting point of the path is the position of the end effector of the robot arm in the base coordinate system obtained in step 1;

[0088] Step 2: Randomly sample a node X in Cartesian space rand , the specific sampling strategy is as follows:

[0089]

[0090] Among them, X goal is the target point, and P is the probability of selecting the target point as a random sampling point;

[0091] For example, if P is 0.2, there is a 20% probability of sampling at the target point and an 80% probability of random sampling;

[0092] Step 2 and 3: Find the distance node X in the current tree structure rand The nearest node X nearest :

[0093]

[0094] Where T′ represents all nodes in the current tree structure; X represents any node in the current tree structure;

[0095] From X nearest Towards X rand Expand one step in the direction to generate a new node X new :

[0096] X new =Steer(X nearest ,X rand )

[0097] Among them, the Steer() function is used to limit the expansion step size;

[0098] Step 24: Determine the slave node X nearest To node X new Is there a collision (node ​​X new Whether it is within the obstacle range, collision will occur, no collision will occur);

[0099] If there is a collision, discard node X new , return to step 22 and resample;

[0100] If there is no collision, then new Select the node with the smallest path cost within a certain radius (set) as X new The parent node of new Add the tree structure and execute step 25; the specific process is:

[0101] 1) X new All nodes X within a certain radius (set) near Expressed as:

[0102] X near ={X i ∈T′|||X i -X new || <d}

[0103] Among them, X i Indicates that in X new Any node in the current tree structure within a certain radius (set) (at this time X new Not in the current tree structure, X i in the current tree structure);

[0104] X near Contains X new All nodes within a certain radius (set);

[0105] d is the custom sampling radius;

[0106] 2) For the current tree structure in X new Any node X within a certain radius (set) i Calculate the path cost, X i ∈X near ;

[0107] Cost i (X new )=Cost(X i )+Cost(X i ,X new )

[0108] Among them, Cost(X i ) represents the distance from the starting point to the node X i The path cost value;

[0109] Cost(X i ,X new ) indicates that from X i to X new The path cost value;

[0110] Cost i (X new ) means from the starting point to X first i Node to X new The total cost of the node's path (in X i For X new Parent node is X new Total cost value of

[0111] 3) Select the node with the smallest path cost as X new The parent node of newAdd to the tree structure and update the tree structure (the parent-child node relationship is also part of the tree structure. Although the nodes have not changed, the connection method has changed);

[0112] Step 25: Determine X in the tree structure new Other nodes X within a certain radius (set) i The parent node of node X i Connect to the parent node;

[0113] The specific process is:

[0114] If the following equation is satisfied, update node X i The parent node is X new , update the tree structure (change node X i With parent node X new Connections, parent-child node relationships are also part of the tree structure. Although the nodes have not changed, the connection method has changed).

[0115] If the following formula is not satisfied, then the node X i The parent node remains unchanged;

[0116] Cost(X new )+Cost(X new ,X i ) <Cost(X i )

[0117] Among them, Cost(X new ) represents the distance from the starting point to the node X new The path cost value;

[0118] Cost(X new ,X i ) represents the slave node X new To node X i The path cost value;

[0119] Cost(X i ) represents the distance from the starting point to the node X i The path cost value;

[0120] Step 26: Repeat steps 22 to 25 until a feasible path from the starting point to the target point is found.

[0121] Other steps and parameters are the same as those in the first or second embodiment.

[0122] Specific embodiment 4: This embodiment differs from any one of specific embodiments 1 to 3 in that: in step 3, based on the feasible path obtained in step 2, a pruning strategy is used to delete redundant path points and extract key path points; the specific process is as follows:

[0123] 1) Let m = 1;

[0124] 2) Determine whether there is a collision between the mth path point and the last path point M in the feasible path;

[0125] If there is a collision, execute 3);

[0126] If there is no collision, delete the path points between the mth and Mth path points, connect the mth and Mth path points, update the path, and execute 4);

[0127] 3) Let M = M-1 ， Repeat step 2);

[0128] 4) Let m = m + 1, and repeat step 2) until m is the end point of the path, and then obtain the key path point.

[0129] It can effectively remove redundant path points, generate a simpler critical path, and improve the efficiency and stability of path execution.

[0130] The other steps and parameters are the same as those in the first to third embodiments.

[0131] Specific embodiment 5: This embodiment differs from any one of specific embodiments 1 to 4 in that: in step 4, based on the key path points in step 3, the robot arm joint angle corresponding to each key path point is obtained;

[0132] The key path points containing the robot arm joint angle information are stored in the path point queue in order from the starting point to the end point, which serves as the temporary target point of the improved artificial potential field method.

[0133] The specific process is:

[0134] Step 41: According to the starting pose of the key path point obtained in step 3 and the ending pose of the key path point obtained in step 3, perform pose interpolation on the key path points other than the starting point and the ending point in step 3 (the matrix of step 1 contains position and pose, and the position except the starting point and the ending point is obtained by step 3. The starting pose and the ending pose of the key path point are obtained by step 1. The pose includes position and pose. The starting point and the ending point will not be pruned in step 3) to obtain the pose of each key path point;

[0135] Step 42: Based on the position and posture of each key path point obtained in step 41 and the forward kinematic transformation matrix of the end effector of the manipulator Use the analytical method to find the inverse solution and obtain the robot arm joint angle corresponding to each key path point (the rod rotation angle θ of the i-th joint i );

[0136] Step 43: Store the key path points containing the robot arm joint angle information into the path point queue in order from the starting point to the end point as temporary target points of the improved artificial potential field method.

[0137] Other steps and parameters are the same as those in Specific Embodiments 1 to 4-1.

[0138] Specific embodiment 6: This embodiment differs from any one of specific embodiments 1 to 5 in that: in step 5, a key path point is selected from the path point queue of step 4 as the current temporary target point, and the improved artificial potential field method and collision warning mechanism are used to guide the robot arm to move toward the temporary path point until it reaches the target point;

[0139] The specific process is:

[0140] Step 5.1: Set the key path point k = 1; set the time step Δt = 0.1;

[0141] Step 52: Select the kth key path point from the path point queue as the current temporary target point, delete the kth key path point, and update the queue;

[0142] Step 53: Construct a virtual gravitational field in the joint space of the robotic arm;

[0143] Step 54: Construct a virtual repulsive force field of the robot arm joint in three-dimensional Cartesian space;

[0144] Step 55: Construct a virtual repulsive field of dynamic obstacles;

[0145] Step 5 and 6: In the joint space of the robot arm, each joint θ is adjusted with a fixed step length. i Perform sampling, i = 1, 2, 3, 4, 5, 6, and obtain J groups of joint combinations;

[0146] Calculate the jth joint combination The potential energy value of , j = 1, 2, ..., J, J is the total number of joint combinations;

[0147] Select a joint combination with the smallest potential energy value from the potential energy values ​​of the J joint combinations of the robotic arm as the target joint combination to guide the robotic arm to move; execute step 57;

[0148] The details are as follows:

[0149] 1) Calculate the potential energy value of each joint combination (vector) of the robotic arm;

[0150]

[0151] Among them, U j,total is the potential energy value of the j-th joint combination of the robotic arm (which is a vector);

[0152] Uatt (θ j ) is the virtual gravitational field of the jth joint combination of the robotic arm;

[0153] θ j is the joint angle vector corresponding to the j-th group of joint postures;

[0154] is the jth joint combination θ j The angle value of the first joint in is the jth joint combination θ j The angle value of the second joint in the middle; is the jth joint combination θ j The angle value of the third joint; is the jth joint combination θ j The angle value of the 4th joint in the middle; is the jth joint combination θ j The angle value of the 5th joint; is the jth joint combination θ j The angle value of the 6th joint;

[0155] U rep (O i ) is the virtual repulsive force field of the robot arm joint;

[0156] U rep,vir (O i ) is a virtual repulsive field attached to dynamic obstacles;

[0157] 2) Select the joint combination with the smallest potential energy value from the potential energy values ​​of the J joint combinations of the robotic arm as the target joint combination to guide the movement of the robotic arm;

[0158] For each joint θ, a fixed step size is used i Sampling can obtain many groups of joint combinations (because it is sampling, different sampling results correspond to different joint combinations). Each of these joint combinations can represent the position that the robot arm may move to in the next time step. In other words, in order to select the best one among so many possible positions to move the robot arm, it is necessary to select a group of joint combinations obtained by sampling as the target position for the next time step.

[0159] Step 57: Set up a collision warning mechanism;

[0160] If the collision warning mechanism determines that there is a tendency for the robotic arm to collide with an obstacle, it will issue a danger warning and brake the robotic arm;

[0161] If the collision warning mechanism determines that there is no collision tendency between the robotic arm and the obstacle, a safety prompt is issued and step 58 is executed;

[0162] Step 58: Determine the U corresponding to the joint combination with the minimum potential energy value obtained in step 56. j,total Whether it remains unchanged for 5 consecutive time steps (5 times of looping, 5 times of looping, each loop is 1 time step Δt);

[0163] If it falls into a local minimum, set k = k + 1, execute step 52, select a new temporary target point and update the potential field distribution to jump out of the current local minimum;

[0164] If not, go to step 59;

[0165] Step 59: Calculate the distance between the current temporary target point (known position) and the dynamic obstacle (known position) in real time;

[0166] When the distance is less than the threshold, the current temporary target point is considered to be in a dangerous position, and k=k+1 is set, and step 52 is executed to select a new temporary target point;

[0167] When the distance is greater than or equal to the threshold, the current temporary target point is considered to be in a safe position, and step 50 is executed;

[0168] Step 50: Determine whether the robotic arm has moved to the current temporary target point;

[0169] If not, proceed to steps five and six;

[0170] If it has arrived, determine whether the current temporary target point is the final target point. If not, set k = k + 1 and execute step 52; if so, reach the target point and complete this obstacle avoidance planning task.

[0171] Other steps and parameters are the same as those in Specific Implementations 1 to 5-1.

[0172] Specific embodiment seven: This embodiment differs from any one of specific embodiments one to six in that: in step five-three, a virtual gravitational field is constructed in the joint space of the manipulator;

[0173] The details are as follows:

[0174]

[0175] Among them, 0 is the joint angle vector corresponding to the current posture of the robot arm, θ tempgoal is the robotic arm joint angle vector corresponding to the current temporary target point, ||θ-θ tempgoal || is the distance between the current posture of the robot arm and the target posture defined based on the two-dimensional Euclidean norm in the six-dimensional joint space;

[0176] When the distance between the current posture of the robot arm and the target posture is too far, the excessive gravitational potential energy may cause the repulsive potential field to fail, posing a safety hazard. Therefore, the saturation distance d is introduced.s ; When the distance between the current posture of the robot and the target posture is less than d s When the distance between the current posture of the manipulator and the target posture is higher than d s When , the potential energy curve becomes a linear function, which prevents excessive gravitational potential energy from covering up the repulsive potential energy, causing the repulsive field to fail and causing a collision;

[0177] The details are as follows:

[0178]

[0179] Among them, U att (θ) is the virtual gravitational field;

[0180] d s is the saturation distance;

[0181] θ is the joint angle vector corresponding to the current position of the robot arm, θ=[θ 1 ,0 2 ,θ 3 ,θ 4 ,θ 5 ,θ 6 ],θ 1 is the first joint angle value corresponding to the current posture of the robot arm, θ 2 is the second joint angle value corresponding to the current posture of the robot arm, θ 3 is the third joint angle value corresponding to the current posture of the robot arm, θ 4 is the fourth joint angle value corresponding to the current posture of the robot arm, θ 5 is the fifth joint angle value corresponding to the current posture of the robot arm, θ 6 The sixth joint angle value corresponding to the current posture of the robot arm;

[0182] θ tempgoal is the robotic arm joint angle vector corresponding to the current temporary target point, is the first joint angle value of the robotic arm corresponding to the current temporary target point, is the second joint angle value of the robotic arm corresponding to the current temporary target point, is the third joint angle value of the robotic arm corresponding to the current temporary target point. is the fourth joint angle value of the robotic arm corresponding to the current temporary target point. is the fifth joint angle value of the robotic arm corresponding to the current temporary target point. The sixth joint angle value of the robotic arm corresponding to the current temporary target point;

[0183] ||θ-θ tempgoal|| is the distance between the joint angle vector corresponding to the current position of the manipulator defined based on the two-dimensional Euclidean norm in the six-dimensional joint space and the joint angle vector of the manipulator corresponding to the current temporary target point;

[0184] ε is the gravitational potential field gain coefficient.

[0185] The other steps and parameters are the same as those in the first to sixth embodiments.

[0186] Specific embodiment eight: This embodiment differs from any one of specific embodiments one to seven in that: in step five-four, a virtual repulsive force field of the robot arm joint is constructed in a three-dimensional Cartesian space, specifically as follows:

[0187]

[0188] Among them, U rep (O i ) is the virtual repulsive force field of the robot arm joint;

[0189] O i is the position of the robot arm joint i in the base coordinate system;

[0190] P obs is the position of the obstacle in the base coordinate system;

[0191] η is the repulsive field gain coefficient;

[0192] d(P obs ,O i ) is the distance from the obstacle to the joint i of the robot arm;

[0193] d0 is the maximum influence radius of the obstacle. When the distance between the robot arm joint i and the obstacle exceeds its maximum influence radius, it is no longer affected by the repulsive field.

[0194] Other steps and parameters are the same as those in Specific Embodiments 1 to 7-1.

[0195] Specific embodiment 9: This embodiment differs from any one of specific embodiments 1 to 8 in that: in step 55, a virtual repulsive field for a dynamic obstacle is constructed. The virtual repulsive field established in step 54 cannot interpret its motion information, and therefore has insufficient dynamic response capability. Therefore, the position of the obstacle within a certain period of time in the future can be predicted, and an additional virtual repulsive field is established at the predicted obstacle position, as follows:

[0196]

[0197] When the dynamic obstacle is at low speed or temporarily stationary, the original repulsive field and the predicted obstacle repulsive field may be superimposed, resulting in excessive repulsive potential energy, causing the robot arm to avoid excessively. Therefore, the gain coefficient of the predicted obstacle repulsive potential field must be flexibly set. Therefore, the gain coefficient function η is established. vir (s), as follows:

[0198]

[0199] in:

[0200] η vir (s)=1-e -λs , s=||P obs,vir -P obs ||

[0201] Among them, P obs,vir To predict the position of the obstacle in the base coordinate system;

[0202] d(P obs,vir ,O i ) is the distance from the predicted obstacle to the manipulator joint i in the base coordinate system;

[0203] U rep,vir (O i ) is a virtual repulsive field attached to dynamic obstacles;

[0204] η vir (s) is the gain coefficient function;

[0205] s is the distance between the predicted obstacle position in the base coordinate system and the actual position of the current obstacle in the base coordinate system, defined based on the two-dimensional Euclidean norm;

[0206] λ is the exponential function parameter;

[0207] η vir (s) A saturated exponential growth function is selected, which is sensitive to distance and can provide smooth changes;

[0208] When the predicted obstacle position overlaps with the current obstacle position, it means that the obstacle is temporarily stationary. vir (s) The function value is zero, and the virtual repulsive field has no effect;

[0209] As the distance increases, the obstacle's moving speed increases. At this time, the gain coefficient function value needs to be increased so that the robotic arm can sense the obstacle's movement trend and achieve dynamic obstacle avoidance at a larger angle.

[0210] The other steps and parameters are the same as those in the specific implementation modes 1 to 8-1.

[0211] Specific embodiment 10: This embodiment differs from the specific embodiments 1 to 9 in that a collision warning mechanism is set in step 57, such as Figure 2 As shown; the specific process is:

[0212] 1) Use capsule bounding box to describe the robot arm link configuration;

[0213] Use a spherical bounding box to describe the obstacle configuration and the robot end effector configuration;

[0214] During the process of obstacle avoidance planning of the robot arm, the setting of the repulsive potential field can effectively avoid the collision between the robot arm and the obstacle; however, in order to avoid the occurrence of accidental collisions, a collision warning mechanism needs to be defined. When the robot arm is guided by the improved artificial potential field method and there is a tendency for the robot arm to collide with the obstacle, a warning is issued and necessary measures are taken. The collision warning process is as follows: Figure 2 As shown;

[0215] Calculate the minimum distance between the manipulator and the obstacle: Based on the configuration description characteristics, the distance calculation between the manipulator and the obstacle is converted into the distance calculation between spheres and the distance calculation between the sphere and the capsule;

[0216] 2) Calculate the minimum distance between the end effector of the robotic arm and the obstacle; the specific process is:

[0217] For the distance calculation between spheres, let the centers of the two spheres be A(x A ,y A ,z A ) and B(x B ,y B ,z B ), the radii of the two spheres are R1 and R2 respectively, then the minimum distance d between the two spheres is calculated as follows:

[0218]

[0219] A(x A ,y A ,z A ) is the coordinate of the center of the sphere corresponding to the obstacle, B(x B ,y B ,z B ) is the coordinate of the center of the sphere corresponding to the end effector of the manipulator; R1 is the radius of the sphere corresponding to the obstacle, and R2 is the radius of the sphere corresponding to the end effector of the manipulator;

[0220] 3) Calculate the minimum distance d between the robot arm link i and the obstacle min ; The specific process is:

[0221] The distance calculation between the sphere and the capsule is transformed into the distance problem from a point to a line segment. Let the two end points of the capsule be A′(xA′ ,y A′ ,z A′ ) and B′(x B′ ,y B′ ,z B′ ), the center of the sphere is C′(x C′ ,y C′ ,z C′ ), the sphere and capsule radii are R3 and R4 respectively;

[0222] A′(x A′ ,y A′ ,z A′ ) is the coordinate of the center of the semicircle at one end of the capsule corresponding to the robotic arm link, B′(x B′ ,y B′ ,z B′ ) is the coordinate of the center of the semicircle at the other end of the capsule corresponding to the robotic arm link, C′(x C′ ,y C′ ,z C′ ) is the coordinate of the center of the sphere corresponding to the obstacle;

[0223] 31) When the projection of the sphere's center point C' falls on the line segment A'B', the minimum distance between the sphere and the capsule is as follows:

[0224]

[0225] Where A′C′ represents the line between the capsule endpoint A′ and the sphere center point C′, A′B′ represents the line between the capsule endpoint A′ and the capsule endpoint B′, · represents the dot product of the vectors, R3 represents the radius of the sphere, and R4 represents the radius of the top or bottom of the capsule (the radius of the semicircle at the top or bottom of the capsule).

[0226] The distance d between the robot arm link i and the obstacle min As shown below:

[0227] d min =min(d i ), i=1,2,3,4,5,6

[0228] Among them, d i represents the distance between link i and the obstacle;

[0229] 32) When the projection of the sphere's center point C' falls outside the line segment A'B', the minimum distance between the sphere and the capsule is as follows:

[0230] d i =min(A′C′·A′C′,B′C′·B′C′)-(R3+R4), i=1,2,3,4,5,6

[0231] The distance d between the robot arm link i and the obstacle min As shown below:

[0232] d min =min(d i ), i=1,2,3,4,5,6

[0233] Among them, d i represents the distance between link i and the obstacle;

[0234] 4) Define safety thresholds;

[0235] If the distance d between the end effector of the robot arm and the obstacle or the distance d between the link i and the obstacle min If the value is less than the safety threshold, it is considered that there is a collision tendency between the robot arm and the obstacle, and a danger warning is issued and the robot arm is braked;

[0236] If the distance d between the end effector of the robot and the obstacle and the distance d between the link i and the obstacle min If both are greater than the safety threshold, it is considered that there is no collision tendency between the robotic arm and the obstacle, and a safety prompt is issued.

[0237] The other steps and parameters are the same as those in the specific implementation modes 1 to 9-1.

[0238] The following examples are used to verify the beneficial effects of the present invention:

[0239] Example 1:

[0240] The following uses the robot obstacle avoidance planning in a two-dimensional static and dynamic mixed scenario as an example to further illustrate the technical solution of the present invention. In two-dimensional space, the robot's starting point is [1,1] and the target point is [19,19]. The RRT* algorithm's target point bias sampling probability P is 0.2. The obstacles consist of static obstacles and dynamic obstacles, and dynamic obstacles include uniformly moving obstacles and variable-speed moving obstacles. The obstacle radius is 1, and the repulsive field range is 2.5. Black represents the current obstacle, and gray represents the predicted obstacle. The prediction time is 0.5s. The predicted obstacle gain coefficient function η vir The parameter λ in (s) is set to 1.2, and the two-dimensional spatial map information is as follows Figure 3 shown.

[0241] At the beginning of planning, only static obstacles in the space are considered. The improved RRT* algorithm is used to sample in the workspace, effectively avoiding local minimum points that would cause the robot to fall into an infinite loop during movement, and obtaining a collision-free global path. This global path consists of a series of path points. The pruning strategy is applied to remove redundant path points to generate a simpler global path, such as Figure 4 shown.

[0242] The robot starts to move, and takes each path point on the global path as a temporary target point in turn. The improved artificial potential field method is used to guide the robot to move. The target point generates gravitational potential energy, and the obstacle and the predicted obstacle generate repulsive potential energy. At the same time, the gain coefficient function η vir (s) Flexibly adjust the additional repulsive force field U according to the distance difference between the current obstacle and its predicted position rep,vir Under the action of the combined potential field, the robot responds to the position changes of dynamic obstacles in real time and successfully completes the obstacle avoidance planning task. The travel path is as follows: Figure 5 shown.

[0243] Example 2:

[0244] The following takes the obstacle avoidance planning of the UR5 robot arm in a static and dynamic mixed scenario as an example to further illustrate the technical solution of the present invention. Table 1 is the DH parameter table of the UR5 robot arm:

[0245] In the robot workspace, the initial posture is θ start =[0,0,0,0,0,0], the target posture is θ goal =[2.14,0.63,-0.44,0.88,0,0].

[0246] According to the robot's positive kinematics matrix and the UR5 manipulator DH parameter table, the terminal coordinates corresponding to the initial posture in three-dimensional Cartesian space are [0.8173, -0.1915, -0.0549], and the terminal coordinates corresponding to the target posture are [-0.2762, 0.7868, 0.3681]. The target point bias sampling probability P of the RRT* algorithm is 0.2. The obstacle consists of static obstacles and dynamic obstacles, with static obstacles on the left and dynamic obstacles on the right, described by a spherical bounding box with a radius of 0.1m. The range of the repulsive field is 0.25m, the prediction time is 0.5s, and the obstacle gain coefficient function η is predicted. vir The parameter λ in (s) is set to 1.2, and the manipulator workspace and the initial state of the manipulator are as follows: Figure 6 shown.

[0247] Table 1 UR5 Robot Arm DH Parameters

[0248]

[0249] This example is implemented on the CoppeliaSim simulation platform, using Socket communication and TCP / IP communication protocol to establish a backend obstacle avoidance plan solver and the CoppeliaSim platform UR5 robotic arm. In Cartesian space, an improved RRT* algorithm is used for sampling to plan a collision-free global path for the robotic arm end. The analytical method is used to solve the pruned global path points and the corresponding joint postures are stored in a queue and used as temporary target points in turn. Figure 7 As shown in the figure, an improved artificial potential field method is used to guide the movement of the robotic arm. PID controllers and motion properties are configured for each joint of the UR5 robotic arm on the CoppeliaSim platform. The back-end obstacle avoidance planning solver samples the optimal joint posture for the next step based on the combined potential field and sends it to each joint controller for execution. The robotic arm responds to the surrounding obstacle environment in real time and successfully completes the obstacle avoidance task. The operation process is smooth and fluent. The robotic arm joint position-time image and end position-time image are shown as follows: Figure 8a 、 8b , 8c, 8d, 8e, 8f, and 8g.

[0250] The present invention may have many other embodiments. Without departing from the spirit and essence of the present invention, those skilled in the art may make various corresponding changes and modifications based on the present invention, but these corresponding changes and modifications should all fall within the scope of protection of the claims attached to the present invention.

Claims

1. A method for autonomous obstacle avoidance planning of a robotic arm based on an improved artificial potential field method, characterized by: The specific process of the method is: Step 1: Construct the transformation matrix between the joint coordinate systems of adjacent joints of the robotic arm; Based on the transformation matrix between the joint coordinate systems of adjacent joints of the manipulator, construct the forward kinematic transformation matrix from the base coordinate system to the i-th joint coordinate system of the manipulator, where i = 1, 2, 3, 4, 5, 6; Without considering the operation of the end effector of the manipulator, the positive kinematic transformation matrix from the base coordinate system to the sixth joint coordinate system of the manipulator is used as the positive kinematic transformation matrix from the base coordinate system to the end effector of the manipulator; Step 2: Based on the forward kinematic transformation matrix from the base coordinate system constructed in step 1 to the end effector of the manipulator, consider some static obstacles and use the improved RRT* algorithm to solve a collision-free feasible path from the starting point to the target point of the end effector of the manipulator; Step 3: Based on the feasible path obtained in step 2, use the pruning strategy to delete redundant path points and extract key path points; Step 4: Based on the key path points in step 3, obtain the robot arm joint angle corresponding to each key path point; The key path points containing the robot arm joint angle information are stored in the path point queue in order from the starting point to the end point, which serves as the temporary target point of the improved artificial potential field method. Step 5. Select a key path point from the path point queue in step 4 as the current temporary target point, and use the improved artificial potential field method and collision warning mechanism to guide the robot arm to move towards the temporary path point until it reaches the target point.

2. The method for autonomous obstacle avoidance planning of a robotic arm based on an improved artificial potential field method according to claim 1, characterized in that: In the step 1, a transformation matrix between the joint coordinate systems of adjacent joints of the robotic arm is constructed; Based on the transformation matrix between the joint coordinate systems of adjacent joints of the manipulator, construct the forward kinematic transformation matrix from the base coordinate system to the i-th joint coordinate system of the manipulator, where i = 1, 2, 3, 4, 5, 6; Without considering the operation of the end effector of the manipulator, the positive kinematic transformation matrix from the base coordinate system to the sixth joint coordinate system of the manipulator is used as the positive kinematic transformation matrix from the base coordinate system to the end effector of the manipulator; The specific process is: Step 1: Construct the transformation matrix between the joint coordinate systems of adjacent joints of the robot arm; the transformation matrix is ​​expressed as: Among them, l i is the length of the rod corresponding to the i-th joint, α i is the torsion angle of the rod at the i-th joint, b i is the rod offset corresponding to the i-th joint, θ i is the rod rotation angle of the i-th joint; is the transformation matrix between the joint coordinate system of the i-1th joint and the i-th joint, i = 1, 2, 3, 4, 5, 6; Step 1 and 2: Based on the transformation matrix between the joint coordinate systems of adjacent joints of the manipulator, construct the forward kinematic transformation matrix from the base coordinate system to the i-th joint coordinate system of the manipulator, where i = 1, 2, 3, 4, 5, 6; expressed as: in, Represents the positive kinematic transformation matrix from the base coordinate system to the first joint coordinate system of the manipulator, Represents the positive kinematic transformation matrix from the base coordinate system to the first joint coordinate system of the manipulator middle element; Represents the transformation matrix between the joint coordinate system of the k-1th joint and the kth joint, k = 1, 2, 3, 4, 5, 6 ， k represents the serial number; Represents the positive kinematic transformation matrix from the base coordinate system to the second joint coordinate system of the robotic arm, Represents the positive kinematic transformation matrix from the base coordinate system to the second joint coordinate system of the manipulator middle element; Represents the positive kinematic transformation matrix from the base coordinate system to the third joint coordinate system of the manipulator, Represents the positive kinematic transformation matrix from the base coordinate system to the third joint coordinate system of the manipulator middle element; Represents the positive kinematic transformation matrix from the base coordinate system to the fourth joint coordinate system of the manipulator, Represents the positive kinematic transformation matrix from the base coordinate system to the fourth joint coordinate system of the manipulator middle element; Represents the positive kinematic transformation matrix from the base coordinate system to the fifth joint coordinate system of the manipulator, Represents the positive kinematic transformation matrix from the base coordinate system to the fifth joint coordinate system of the manipulator middle element; Represents the positive kinematic transformation matrix from the base coordinate system to the sixth joint coordinate system of the robotic arm, Represents the positive kinematic transformation matrix from the base coordinate system to the sixth joint coordinate system of the manipulator middle element; represents the posture of the i-th joint of the robot arm, i = 1, 2, 3, 4, 5, 6; Represents the posture R of the i-th joint of the robotic arm i middle element; represents the position of the i-th joint in the base coordinate system, that is, the xyz coordinate value of the i-th joint in the base coordinate system, i = 1, 2, 3, 4, 5, 6; the superscript T indicates the transpose; π represents matrix multiplication; Step 13: The forward kinematic transformation matrix from the base coordinate system to the sixth joint coordinate system of the robotic arm As the forward kinematic transformation matrix from the base coordinate system to the end effector of the manipulator; expressed as: Multiply the positive kinematic transformation matrix of the six joint base coordinate systems to the manipulator joint coordinate system to obtain the positive kinematic transformation matrix of the manipulator end effector: Among them, r 11 、r 12 、r 13 、r 21 、r 22 、r 23 、r 31 、r 32 、r 33 、p x 、p y 、p z Represents the forward kinematic transformation matrix of the robotic arm end effector middle element; Posture of the robotic arm end effector Among them, r 11 、r 12 、r 13 、r 21 、r 22 、r 23 、r 31 、r 32 、r 33 Represents an element in R; [p x p y p z ] T It represents the position of the end effector of the robot arm in the base coordinate system, that is, the xyz coordinate value of the end effector of the robot arm in the base coordinate system.

3. The method for autonomous obstacle avoidance planning of a robotic arm based on an improved artificial potential field method according to claim 2, characterized in that: In step 2, based on the forward kinematic transformation matrix from the base coordinate system constructed in step 1 to the end effector of the manipulator, an improved RRT* algorithm is used to solve a collision-free feasible path from the starting point to the target point of the end effector of the manipulator in Cartesian space, taking into account some static obstacles; The specific process is: Step 21: Add the path starting point to the empty tree structure T′ to obtain the initial current tree structure T′; The starting point of the path is the position of the end effector of the robot arm in the base coordinate system obtained in step 1; Step 2: Randomly sample a node X in Cartesian space rand , the specific sampling strategy is as follows: Among them, X goal is the target point, and P is the probability of selecting the target point as a random sampling point; Step 2 and 3: Find the distance node X in the current tree structure rand The nearest node X nearest : Among them, T′ represents all nodes in the current tree structure; X represents any node in the current tree structure; From X nearest Towards X rand Expand one step in the direction to generate a new node X new : X new =Steer(X nearest ,X rand ) Among them, the Steer() function is used to limit the expansion step size; Step 24: Determine the slave node X nearest To node X new Whether a collision occurred; If there is a collision, discard node X new , return to step 22 and resample; If there is no collision, then new Select the node with the smallest path cost within a certain radius as X new The parent node of new Add the tree structure and execute step 25; the specific process is: 1) X new All nodes within a certain radius X near Expressed as: X near ={X i ∈T′|||X i -X new ‖<d} Among them, X i Indicates that in X new Any node in the current tree structure within a certain radius; X near Contains X new All nodes within a certain radius; d is the custom sampling radius; 2) For the current tree structure in X new Any node X within a certain radius i Calculate the path cost, X i ∈X near ; Cost i (X new )=Cost(X i )+Cost(X i ,X new ) Among them, Cost(X i ) represents the distance from the starting point to the node X i The path cost value; Cost(X i ,X new ) indicates that from X i to X new The path cost value; Cost i (X new ) means from the starting point to X first i Node to X new The total path value of the node; 3) Select the node with the smallest path cost as X new The parent node of new Add to the tree structure and update the tree structure; Step 25: Determine X in the tree structure new Other nodes X within a certain radius i The parent node of node X i Connect to the parent node; the specific process is: If the following equation is satisfied, update node X i The parent node is X new , update the tree structure; If the following formula is not satisfied, then the node X i The parent node remains unchanged; Cost(X new )+Cost(X new ,X i )<Cost(X i ) Among them, Cost(X new ) represents the distance from the starting point to the node X new The path cost value; Cost(X new ,X i ) represents the slave node X new To node X i The path cost value; Cost(X i ) represents the distance from the starting point to the node X i The path cost value; Step 26: Repeat steps 22 to 25 until a feasible path from the starting point to the target point is found.

4. The method for autonomous obstacle avoidance planning of a robotic arm based on an improved artificial potential field method according to claim 3, characterized in that: In step 3, based on the feasible path obtained in step 2, a pruning strategy is used to delete redundant path points and extract key path points. The specific process is as follows: 1) Let m = 1; 2) Determine whether there is a collision between the mth path point and the last path point M in the feasible path; If there is a collision, execute 3); If there is no collision, delete the path points between the mth and Mth path points, connect the mth and Mth path points, update the path, and execute 4); 3) Let M = M-1 ， Repeat step 2); 4) Let m = m + 1, and repeat step 2) until m is the end point of the path, and then obtain the key path point.

5. The method for autonomous obstacle avoidance planning of a robotic arm based on an improved artificial potential field method according to claim 4, characterized in that: In the step 4, based on the key path points in step 3, the robot arm joint angle corresponding to each key path point is obtained; The key path points containing the robot arm joint angle information are stored in the path point queue in order from the starting point to the end point, which serves as the temporary target point of the improved artificial potential field method. The specific process is: Step 41: Based on the starting pose of the key path point obtained in step 3 and the ending pose of the key path point obtained in step 3, perform pose interpolation on the key path points other than the starting pose and the ending pose of the key path point obtained in step 3 to obtain the pose of each key path point; Step 42: Based on the position and posture of each key path point obtained in step 41 and the forward kinematic transformation matrix of the end effector of the manipulator Use analytical methods to find the inverse solution and obtain the robot arm joint angle corresponding to each key path point; Step 43: Store the key path points containing the robot arm joint angle information into the path point queue in order from the starting point to the end point as temporary target points of the improved artificial potential field method.

6. The method for autonomous obstacle avoidance planning of a robotic arm based on an improved artificial potential field method according to claim 5, characterized in that: In the step 5, a key path point is selected from the path point queue of step 4 as the current temporary target point, and the improved artificial potential field method and the collision warning mechanism are used to guide the robot arm to move toward the temporary path point until it reaches the target point; The specific process is: Step 5.1: Set the key path point k = 1; set the time step Δt = 0.1; Step 52: Select the kth key path point from the path point queue as the current temporary target point, delete the kth key path point, and update the queue; Step 53: Construct a virtual gravitational field; Step 54: Construct a virtual repulsive force field for the robotic arm joints; Step 55: Construct a virtual repulsive force field for dynamic obstacles; Step 5 and 6: For each joint θ with a fixed step length i Perform sampling, i = 1, 2, 3, 4, 5, 6, and obtain J groups of joint combinations; Calculate the jth joint combination The potential energy value of , j = 1, 2, ..., J, J is the total number of joint combinations; Select the joint combination with the smallest potential energy value from the J joint combinations of the robotic arm as the target joint combination to guide the robotic arm to move; execute steps 5 and 7; specifically as follows: 1) Calculate the potential energy value of each joint combination of the robotic arm; Among them, U j,total is the potential energy value of the j-th joint combination of the robotic arm (which is a vector); U att (θ j ) is the virtual gravitational field of the jth joint combination of the robotic arm; θ j is the joint angle vector corresponding to the j-th group of joint postures; is the jth joint combination θ j The angle value of the first joint in is the jth joint combination θ j The angle value of the second joint in the middle; is the jth joint combination θ j The angle value of the third joint; is the jth joint combination θ j The angle value of the 4th joint in the middle; is the jth joint combination θ j The angle value of the 5th joint in the middle; is the jth joint combination θ j The angle value of the 6th joint; U rep (O i ) is the virtual repulsive force field of the robot arm joint; U rep,vir (O i ) is a virtual repulsive field attached to dynamic obstacles; 2) Select the joint combination with the smallest potential energy value from the potential energy values ​​of the J joint combinations of the robotic arm as the target joint combination to guide the movement of the robotic arm; Step 57: Set up a collision warning mechanism; If the collision warning mechanism determines that there is a tendency for the robotic arm to collide with an obstacle, it will issue a danger warning and brake the robotic arm; If the collision warning mechanism determines that there is no collision tendency between the robotic arm and the obstacle, a safety prompt is issued and step 58 is executed; Step 58: Determine the U corresponding to the joint combination with the minimum potential energy value obtained in step 56. j,total Whether it remains unchanged for 5 consecutive time steps; If yes, set k=k+1 and execute step 52; If not, go to step 59; Step 59: Calculate the distance between the current temporary target point and the dynamic obstacle in real time; When the distance is less than the threshold, the current temporary target point is considered to be in a dangerous position, and k=k+1 is set, and step 52 is executed; When the distance is greater than or equal to the threshold, the current temporary target point is considered to be in a safe position, and step 50 is executed; Step 50: Determine whether the robotic arm has moved to the current temporary target point; If not, proceed to steps five and six; If it has arrived, determine whether the current temporary target point is the final target point. If not, set k = k + 1 and execute step 52; if so, reach the target point and complete this obstacle avoidance planning task.

7. The method for autonomous obstacle avoidance planning of a robotic arm based on an improved artificial potential field method according to claim 6, characterized in that: The virtual gravitational field is constructed in step 53; the details are as follows: Among them, U att (θ) is the virtual gravitational field; d s is the saturation distance; θ is the joint angle vector corresponding to the current position of the robot arm, θ=[θ 1 ,θ 2 ,θ 3 ,θ 4 ,θ 5 ,θ 6 ],θ 1 is the first joint angle value corresponding to the current posture of the robot arm, θ 2 is the second joint angle value corresponding to the current posture of the robot arm, θ 3 is the third joint angle value corresponding to the current posture of the robot arm, θ 4 is the fourth joint angle value corresponding to the current posture of the robot arm, θ 5 is the fifth joint angle value corresponding to the current posture of the robot arm, θ 6 The sixth joint angle value corresponding to the current posture of the robot arm; θ tempgoal is the robotic arm joint angle vector corresponding to the current temporary target point, is the first joint angle value of the robotic arm corresponding to the current temporary target point, is the second joint angle value of the robotic arm corresponding to the current temporary target point, is the third joint angle value of the robotic arm corresponding to the current temporary target point. is the fourth joint angle value of the robotic arm corresponding to the current temporary target point. is the fifth joint angle value of the robotic arm corresponding to the current temporary target point. The sixth joint angle value of the robotic arm corresponding to the current temporary target point; ||θ-θ tempgoal || is the distance between the joint angle vector corresponding to the current position of the manipulator defined based on the two-dimensional Euclidean norm in the six-dimensional joint space and the joint angle vector of the manipulator corresponding to the current temporary target point; ε is the gravitational potential field gain coefficient.

8. The method for autonomous obstacle avoidance planning of a robotic arm based on an improved artificial potential field method according to claim 7, characterized in that: The virtual repulsive force field of the robotic arm joint is constructed in step 54, specifically as follows: Among them, U rep (O i ) is the virtual repulsive force field of the robot arm joint; O i is the position of the robot arm joint i in the base coordinate system; P obs is the position of the obstacle in the base coordinate system; η is the repulsive field gain coefficient; d(P obs ,O i ) is the distance from the obstacle to joint i of the robot arm; d0 is the maximum impact radius of the obstacle.

9. The method for autonomous obstacle avoidance planning of a robotic arm based on an improved artificial potential field method according to claim 8, characterized in that: The virtual repulsive force field of the dynamic obstacle is constructed in step 55, specifically: in: η vir (s)=1-e -λs ,s=||P obs,vir -P obs || Among them, P obs,vir To predict the position of the obstacle in the base coordinate system; d(P obs,vir ,O i ) is the distance from the predicted obstacle to the manipulator joint i in the base coordinate system; U rep,vir (O i ) is a virtual repulsive field attached to dynamic obstacles; η vir (s) is the gain coefficient function; s is the distance between the predicted obstacle position in the base coordinate system and the actual position of the current obstacle in the base coordinate system, defined based on the two-dimensional Euclidean norm; λ is the exponential function parameter.

10. The method for autonomous obstacle avoidance planning of a robotic arm based on an improved artificial potential field method according to claim 9, characterized in that: In step 57, a collision warning mechanism is set; the specific process is as follows: 1) Use capsule bounding box to describe the robot arm link configuration; Use a spherical bounding box to describe the obstacle configuration and the robot end effector configuration; 2) Calculate the minimum distance between the end effector of the robotic arm and the obstacle; the specific process is: Assume that the centers of the two spheres are A(x A ,y A ,z A ) and B(x B ,y B ,z B ), the radii of the two spheres are R1 and R2 respectively, then the minimum distance d between the two spheres is calculated as follows: 3) Calculate the minimum distance d between the robot arm link i and the obstacle min ; The specific process is: Assume that the two ends of the capsule are A′(x A′ ,y A′ ,z A′ ) and B′(x B′ ,y B′ ,z B′ ), the center of the sphere is C′(x C′ ,y C′ ,z C′ ), the sphere and capsule radii are R3 and R4 respectively; 31) When the projection of the sphere's center point C' falls on the line segment A'B', the minimum distance between the sphere and the capsule is as follows: Where A′C′ represents the line between the capsule endpoint A′ and the sphere center point C′, A′B′ represents the line between the capsule endpoint A′ and the capsule endpoint B′, · represents the dot product of the vectors, R3 represents the radius of the sphere, and R4 represents the radius of the top or bottom of the capsule. The distance d between the robot arm link i and the obstacle min As shown below: d min =min(d i ),i=1,2,3,4,5,6 Among them, d i represents the distance between link i and the obstacle; 32) When the projection of the sphere's center point C' falls outside the line segment A'B', the minimum distance between the sphere and the capsule is as follows: d i =min(A′C′·A′Cv,B′C′·B′C′)-(R3+R4),i=1,2,3,4,5,6 The distance d between the robot arm link i and the obstacle min As shown below: d min =min(d i ),i=1,2,3,4,5,6 Among them, d i represents the distance between link i and the obstacle; 4) Define safety thresholds; If the distance d between the end effector of the robot arm and the obstacle or the distance d between the link i and the obstacle min If the value is less than the safety threshold, it is considered that there is a collision tendency between the robot arm and the obstacle, and a danger warning is issued and the robot arm is braked; If the distance d between the end effector of the robot and the obstacle and the distance d between the link i and the obstacle min If both are greater than the safety threshold, it is considered that there is no collision tendency between the robotic arm and the obstacle, and a safety prompt is issued.

Images