A robot path planning method based on hybrid algorithm

Abstract

The invention discloses a robot path planning method based on a hybrid algorithm, which relates to the field of robot technology, including: using a D-H parameter method to establish a robot forward kinematics model; using a numerical iteration method to establish an inverse kinematics model to solve the target joint position; using an improved artificial potential field algorithm to plan a path, and calculating the joint speed and acceleration required to complete the planned path; judging in real time whether the robot is trapped in a local minimum; calculating the joint speed and acceleration required to complete the path planned by the improved fast random exploration tree algorithm; judging in real time whether the robot encounters a dynamic obstacle during operation; judging in real time whether the robot reaches the target joint position. The problem that the artificial potential field algorithm is prone to being trapped in a local minimum is solved by the fast random exploration tree algorithm, and the problem that the fast random exploration tree algorithm is not suitable for real-time planning of obstacle avoidance in a dynamic environment is solved by the artificial potential field algorithm.

Description

Technical Field

[0001] The present invention relates to the technical field of robotic arms, and in particular to a robotic arm path planning method based on a hybrid algorithm. Background Art

[0002] After entering the new century, the world economy has developed rapidly, and industrialization at home and abroad has developed very rapidly. The traditional industrial model of manual operation has low production efficiency and cannot meet the demand. As an important production service tool for industrial production, robotic arms are involved in more production and service scenarios. In industrial production, industrial robots (robotic arms) have been successfully used on a large scale in the automotive manufacturing industry, and are also the most widely used automated mechanical equipment in the field of robotics. Industrial robots have replaced manual labor in some high-risk and highly repetitive industrial operations, which has greatly improved production efficiency while reducing the occurrence of safety accidents. Not only that, industrial robots also play a role in logistics, agriculture, space, and medical care.

[0003] The traditional model can no longer meet the requirements, and motion planning has become the basis of robot arm motion control. Motion planning is to set the starting point and target point of the robot, and then the robot can calculate a motion trajectory that meets the constraints in the specified environment through the motion planning algorithm, so that the robot can reach the target point. In the new industrial scenario, the robot arm has active physical perception capabilities, and people and robot arms work together. The robot arm can complete the task while ensuring the safety of personnel in the presence of personnel, and carry out production operations efficiently. With the increasing frequency of human-machine collaboration, it is necessary to ensure the efficiency of the robot arm's production operations while ensuring the safety of the collaborative personnel. Therefore, in complex human-machine interactive collaborative scenarios, it is very necessary to develop a motion planning algorithm with good obstacle avoidance effect and high reliability. Summary of the invention

[0004] In order to solve the above technical problems, a robot arm path planning method based on a hybrid algorithm is provided. This technical solution solves the problems raised in the above background technology.

[0005] In order to achieve the above purpose, the technical solution adopted by the present invention is:

[0006] A robot arm path planning method based on a hybrid algorithm, comprising:

[0007] Based on the current joint position of the robot arm, the forward kinematics model of the robot arm is established using the DH parameter method;

[0008] Initialize the algorithm, obtain the target point information, use the numerical iteration method to establish the inverse kinematics model, and solve the target joint position and posture;

[0009] Using the improved artificial potential field algorithm to plan a path, calculating the joint speed and acceleration required to complete the planned path, and the robotic arm continuously iterates according to the joint speed and acceleration;

[0010] Determine in real time whether the robot arm is trapped in the local minimum. If so, use the improved fast random exploration tree algorithm to get out of the trap. If not, no output is made.

[0011] The joint speed and acceleration required to complete the path planned by the improved fast random exploration tree algorithm are calculated, and the robot arm continuously iterates according to the joint speed and acceleration;

[0012] Determine in real time whether the robot arm encounters a dynamic obstacle during operation. If so, switch to the improved artificial potential field algorithm for dynamic obstacle avoidance. If not, no output is made.

[0013] Determine in real time whether the robot arm has reached the target joint position. If so, the output is that it has reached the destination and the algorithm ends. If not, continue to run the hybrid algorithm.

[0014] Preferably, the method of establishing a forward kinematics model of the robot arm using the DH parameter method based on the current joint posture of the robot arm specifically includes:

[0015] Establish a spatial rectangular coordinate system for each connecting rod and number them in sequence;

[0016] The coordinate system is initially located in the initial coordinate system, and is rotated by a first rotation angle around the X axis of the initial coordinate system so that the Z axis of the initial coordinate system is parallel to the Z axis of the next coordinate system;

[0017] The coordinate system is translated along the X axis of the initial coordinate system by a first translation amount so that the Z axis of the initial coordinate system is collinear with the Z axis of the next coordinate system;

[0018] The coordinate system is rotated by a second rotation angle around the Z axis of the initial coordinate system so that the X axis of the initial coordinate system is parallel to the X axis of the next coordinate system;

[0019] The coordinate system is translated along the Z axis of the initial coordinate system by a second translation amount, and has now been transformed to the next coordinate system;

[0020] The homogeneous transformation matrix between the coordinate systems of link i-1 and link i is obtained through homogeneous transformation;

[0021] Based on the current joint posture of the robot arm, a forward kinematics model of the robot arm is established by using the DH parameter method, and the DH parameters of each connecting rod are determined, wherein the DH parameters include a first rotation angle, a second rotation angle, a first translation amount, and a second translation amount;

[0022] The homogeneous transformation matrix is:

[0023] ,

[0024] In the formula, is the homogeneous transformation matrix between the coordinate systems of link i-1 and link i, is the first rotation angle, is the second rotation angle, is the first translation, is the second translation.

[0025] Preferably, the algorithm is initialized, information of the target point is obtained, an inverse kinematics model is established using a numerical iteration method, and the target joint position and posture are solved specifically including:

[0026] Get the target point information and mark it as the end coordinate of the end link;

[0027] Calculate the relative elevation angle between link i-1 and link i according to the relative elevation angle iteration formula;

[0028] Solve the target joint pose based on the relative elevation angle between each link;

[0029] The relative elevation angle iteration formula is:

[0030] ,

[0031] In the formula, is the relative elevation angle between link i-1 and link i, is the coordinate of the end of link i, is the length of connecting rod i.

[0032] Preferably, the use of the improved artificial potential field algorithm to plan a path, calculating the joint speed and acceleration required to complete the planned path, and the continuous iterative operation of the robotic arm according to the joint speed and acceleration specifically includes:

[0033] Based on the target joint posture, a segmented attractive potential field is constructed;

[0034] According to the segmented attraction potential field, the attraction linear velocity is calculated;

[0035] According to the moving speed of the obstacle, the obstacle's range of action is calculated;

[0036] Based on the obstacle moving speed and obstacle action range, an improved repulsive potential field is constructed;

[0037] According to the improved repulsive potential field, the repulsive linear velocity of each obstacle is calculated;

[0038] Calculate the combined velocity of the robot's attraction linear velocity and each repulsion linear velocity, and output it as the joint velocity;

[0039] Based on the real-time joint velocity changes of the robot arm, calculate and output its real-time acceleration;

[0040] The segmented attractive potential field is:

[0041] ,

[0042] Where U a is the segmented attractive potential field, K a To set the attractive potential field gain, is the distance between the end effector and the target point, is to set the distance constant, S is the adjustment factor;

[0043] The attraction linear velocity is:

[0044] ,

[0045] Where V a is the linear velocity of attraction, is the gradient of the piecewise attractive potential field, that is , the direction is negative;

[0046] The obstacle range is:

[0047] ,

[0048] In the formula, is the range of action of the ith obstacle, , are the maximum and minimum ranges of the obstacle, V0 is the obstacle speed corresponding to the maximum range, V i is the speed of the i-th obstacle;

[0049] The improved repulsive potential field is:

[0050] ,

[0051] Where U i is the improved repulsive potential field of the ith obstacle, K r To set the repulsive potential field gain, is the minimum distance between the robot and the ith obstacle, , are the static and dynamic obstacle factors, respectively;

[0052] The repulsive linear velocity is:

[0053] ,

[0054] Where V ri is the repulsive linear velocity of the ith obstacle, For U i right The partial differential of .

[0055] Preferably, the real-time determination of whether the robot arm is trapped in a local minimum specifically includes:

[0056] Get the real-time joint speed of the robot arm;

[0057] Determine whether the real-time joint speed is 0. If so, output that the robot arm is trapped in the local minimum and use the improved fast random exploration tree algorithm to get out of the trap. If not, no output is made.

[0058] Determine whether the real-time joint speed changes periodically. If so, output that the robot arm is trapped in the local minimum and use the improved fast random exploration tree algorithm to escape. If not, no output is made.

[0059] Preferably, the calculating of the joint speed and acceleration required to complete the path planned by the improved fast random exploration tree algorithm, and the continuous iterative operation of the robotic arm according to the joint speed and acceleration specifically includes:

[0060] S101: Map the starting pose and the target pose into the joint space, namely, start and goal, and record start as the root node of the random tree T;

[0061] S102: randomly generate a sampling point rand in the joint space using a random sampling method;

[0062] S103: Find the leaf node near that is closest to rand in the random tree T;

[0063] S104: Use the target point to attract the bias step d, use d to calculate the bias node near' from near to goal, synthesize the growth direction of the node through the bias node near' and rand, and near grows a step in this direction to obtain new;

[0064] S105: Map the sampling point new to the Cartesian space, and use the robot forward kinematics to calculate the distance between the robot arm and the obstacle in the Cartesian space;

[0065] S106: Determine whether the robot arm will collide with the obstacle. If so, discard the sampling point and return to step S102; if not, take near as the parent node of new and add new to T;

[0066] S107: Reselect the parent node and take the neighboring nodes near new as potential parent nodes;

[0067] S108: Select a potential parent node from among the potential parent nodes whose path cost from the potential parent node to the starting point plus the distance from new to the node is the smallest as the new parent node of new;

[0068] S109: Determine whether the distance between new and goal is less than a set threshold. If so, reselect a parent node for goal. If not, do not output.

[0069] S110: Find the path from start to goal from the search tree T;

[0070] S111: Calculate the joint speed and acceleration required to complete the planned path, and the robot arm continues to iterate according to the joint speed and acceleration.

[0071] Preferably, the real-time determination of whether the robot arm encounters a dynamic obstacle during operation specifically includes:

[0072] Get the moving speed of obstacles near the planned path;

[0073] Determine whether the obstacle moving speed is greater than the set threshold. If so, switch to run the improved artificial potential field algorithm for dynamic obstacle avoidance. If not, no output is made.

[0074] Preferably, the real-time determination of whether the robotic arm reaches the target joint posture specifically includes:

[0075] Get robot arm information in real time;

[0076] The inverse kinematics model is established using numerical iteration method;

[0077] Solve the real-time robot arm joint pose;

[0078] Determine whether the real-time robot arm joint posture is the same as the target joint posture. If so, the output has reached the destination and the algorithm ends. If not, continue to run the hybrid algorithm.

[0079] Compared with the prior art, the present invention has the following beneficial effects:

[0080] By improving the fast random exploration tree algorithm, the problems of high path cost, poor path optimization and waste of a large number of sampling points are solved; by improving the artificial potential field algorithm, it can better handle dynamic and static obstacles, greatly enhancing the ability of the robot arm to avoid dynamic obstacles, alleviating the problem of unreachable targets and improving the adaptability of the algorithm; by improving the hybrid algorithm of the fast random exploration tree algorithm and the improved artificial potential field algorithm, the problems that the artificial potential field algorithm is prone to fall into local minima and the fast random exploration tree algorithm is not suitable for real-time planning of obstacle avoidance in dynamic environments are solved. BRIEF DESCRIPTION OF THE DRAWINGS

[0081] Figure 1 It is a flow chart of the robot arm path planning method based on the hybrid algorithm of the present invention;

[0082] Figure 2 A flow chart of the method for establishing a forward kinematics model of a robotic arm according to the present invention;

[0083] Figure 3 A flow chart of the method for establishing an inverse kinematics model of the present invention;

[0084] Figure 4 A flow chart of a method for planning a path using an improved artificial potential field algorithm of the present invention;

[0085] Figure 5 This is a flow chart of a method for determining in real time whether a robot arm is trapped in a local minimum value according to the present invention;

[0086] Figure 6 A flow chart of a method for calculating the joint speed and acceleration required to complete the path planned by the improved fast random exploration tree algorithm of the present invention;

[0087] Figure 7 This is a flow chart of a method for determining in real time whether a robot arm encounters a dynamic obstacle during operation according to the present invention;

[0088] Figure 8 This is a flow chart of the method for determining in real time whether a robotic arm has reached a target joint position of the present invention. DETAILED DESCRIPTION

[0089] The following description is used to disclose the present invention so that those skilled in the art can implement the present invention. The preferred embodiments described below are only examples, and those skilled in the art may think of other obvious variations.

[0090] Reference Figure 1 As shown, a robot arm path planning method based on a hybrid algorithm includes:

[0091] Based on the current joint position of the robot arm, the forward kinematics model of the robot arm is established using the DH parameter method;

[0092] Initialize the algorithm, obtain the target point information, use the numerical iteration method to establish the inverse kinematics model, and solve the target joint position and posture;

[0093] Using the improved artificial potential field algorithm to plan a path, calculating the joint speed and acceleration required to complete the planned path, and the robotic arm continuously iterates according to the joint speed and acceleration;

[0094] Determine in real time whether the robot arm is trapped in the local minimum. If so, use the improved fast random exploration tree algorithm to get out of the trap. If not, no output is made.

[0095] The joint speed and acceleration required to complete the path planned by the improved fast random exploration tree algorithm are calculated, and the robot arm continuously iterates according to the joint speed and acceleration;

[0096] Determine in real time whether the robot arm encounters a dynamic obstacle during operation. If so, switch to the improved artificial potential field algorithm for dynamic obstacle avoidance. If not, no output is made.

[0097] Determine in real time whether the robot arm has reached the target joint position. If so, the output is that it has reached the destination and the algorithm ends. If not, continue to run the hybrid algorithm.

[0098] Reference Figure 2 As shown in the figure, based on the current joint posture of the robot arm, the forward kinematics model of the robot arm is established using the DH parameter method, which specifically includes:

[0099] Establish a spatial rectangular coordinate system for each connecting rod and number them in sequence;

[0100] The coordinate system is initially located in the initial coordinate system, and is rotated by a first rotation angle around the X axis of the initial coordinate system so that the Z axis of the initial coordinate system is parallel to the Z axis of the next coordinate system;

[0101] The coordinate system is translated along the X axis of the initial coordinate system by a first translation amount so that the Z axis of the initial coordinate system is collinear with the Z axis of the next coordinate system;

[0102] The coordinate system is rotated by a second rotation angle around the Z axis of the initial coordinate system so that the X axis of the initial coordinate system is parallel to the X axis of the next coordinate system;

[0103] The coordinate system is translated along the Z axis of the initial coordinate system by a second translation amount, and has now been transformed to the next coordinate system;

[0104] The homogeneous transformation matrix between the coordinate systems of link i-1 and link i is obtained through homogeneous transformation;

[0105] Based on the current joint posture of the robot arm, a forward kinematics model of the robot arm is established by using the DH parameter method, and the DH parameters of each connecting rod are determined, wherein the DH parameters include a first rotation angle, a second rotation angle, a first translation amount, and a second translation amount;

[0106] The homogeneous transformation matrix is:

[0107] ,

[0108] In the formula, is the homogeneous transformation matrix between the coordinate systems of link i-1 and link i, is the first rotation angle, is the second rotation angle, is the first translation, is the second translation.

[0109] The traditional method of establishing the forward kinematics of a robot is to use the DH parameter method to establish a local coordinate system for each link. The advantage of this method is that it is relatively intuitive and easy to understand, and requires relatively few parameters. The overall structure of the robotic arm is composed of the rotating joints of the links, and the links are connected by rotating joint axes or moving joint axes. The end position of the robotic arm can be calculated by knowing the parameters of the links.

[0110] Reference Figure 3 As shown, the algorithm is initialized, the target point information is obtained, and the inverse kinematics model is established using the numerical iteration method. The target joint position is solved specifically including:

[0111] Get the target point information and mark it as the end coordinate of the end link;

[0112] Calculate the relative elevation angle between link i-1 and link i according to the relative elevation angle iteration formula;

[0113] Solve the target joint pose based on the relative elevation angle between each link;

[0114] The relative elevation angle iteration formula is:

[0115] ,

[0116] In the formula, is the relative elevation angle between link i-1 and link i, is the coordinate of the end of link i, is the length of connecting rod i.

[0117] Inverse kinematics analysis is an important basis for motion control and planning of multi-degree-of-freedom manipulators. However, solving the inverse kinematics of multi-degree-of-freedom manipulators is a nonlinear problem. The objective function is a transcendental function, and the number of solutions may be zero or multiple. Traditional methods for solving inverse kinematics include analytical methods, geometric methods, and numerical iteration methods.

[0118] Reference Figure 4 As shown, the improved artificial potential field algorithm is used to plan the path, and the joint speed and acceleration required to complete the planned path are calculated. The robot arm continuously iterates according to the joint speed and acceleration, specifically including:

[0119] Based on the target joint posture, a segmented attractive potential field is constructed;

[0120] According to the segmented attraction potential field, the attraction linear velocity is calculated;

[0121] According to the moving speed of the obstacle, the obstacle's range of action is calculated;

[0122] Based on the obstacle moving speed and obstacle action range, an improved repulsive potential field is constructed;

[0123] According to the improved repulsive potential field, the repulsive linear velocity of each obstacle is calculated;

[0124] Calculate the combined velocity of the robot's attraction linear velocity and each repulsion linear velocity, and output it as the joint velocity;

[0125] Based on the real-time joint velocity changes of the robot arm, calculate and output its real-time acceleration;

[0126] The segmented attractive potential field is:

[0127] ,

[0128] Where U a is the segmented attractive potential field, K a To set the attractive potential field gain, is the distance between the end effector and the target point, is to set the distance constant, S is the adjustment factor;

[0129] The attraction linear velocity is:

[0130] ,

[0131] Where V a is the linear velocity of attraction, is the gradient of the piecewise attractive potential field, that is , the direction is negative;

[0132] The obstacle range is:

[0133] ,

[0134] In the formula, is the range of action of the ith obstacle, , are the maximum and minimum ranges of the obstacle, V0 is the obstacle speed corresponding to the maximum range, V i is the speed of the i-th obstacle;

[0135] The improved repulsive potential field is:

[0136] ,

[0137] Where U i is the improved repulsive potential field of the ith obstacle, K r To set the repulsive potential field gain, is the minimum distance between the robot and the ith obstacle, , are the static and dynamic obstacle factors, respectively;

[0138] The repulsive linear velocity is:

[0139] ,

[0140] Where V ri is the repulsive linear velocity of the ith obstacle, For U i right The partial differential of .

[0141] The attraction potential field of the traditional artificial potential field algorithm is a second-order function, and the attraction speed generated is a first-order linear speed. When the end of the robotic arm is far away from the target point, the attraction speed is very large, which will cause the robotic arm to produce a large joint speed. When the robotic arm runs at a large joint speed, it may not have enough time to slow down when encountering an obstacle, causing the robotic arm to collide with the obstacle and cause a safety accident. For this reason, the construction method of the attraction potential field was redesigned, and a segmented attraction potential field was adopted.

[0142] Reference Figure 5 As shown, real-time judgment of whether the robot arm is trapped in a local minimum specifically includes:

[0143] Get the real-time joint speed of the robot arm;

[0144] Determine whether the real-time joint speed is 0. If so, output that the robot arm is trapped in the local minimum and use the improved fast random exploration tree algorithm to get out of the trap. If not, no output is made.

[0145] Determine whether the real-time joint speed changes periodically. If so, output that the robot arm is trapped in the local minimum and use the improved fast random exploration tree algorithm to escape. If not, no output is made.

[0146] When the robotic arm is running using a hybrid algorithm of the improved fast random exploration tree algorithm and the improved artificial potential field algorithm, it is necessary to detect whether it falls into a local minimum at each control input moment. When it is detected that it falls into a local minimum, the algorithm is switched. The judgment condition for the local minimum is that when the robotic arm runs at a joint speed and acceleration close to 0 for a period of time, the distance between it and the obstacle and the target point remains unchanged, and the robotic arm has not reached the set target point.

[0147] Reference Figure 6 As shown, the joint speed and acceleration required to complete the path planned by the improved fast random exploration tree algorithm are calculated, and the robot arm continuously iterates according to the joint speed and acceleration, specifically including:

[0148] S101: Map the starting pose and the target pose into the joint space, namely, start and goal, and record start as the root node of the random tree T;

[0149] S102: randomly generate a sampling point rand in the joint space using a random sampling method;

[0150] S103: Find the leaf node near that is closest to rand in the random tree T;

[0151] S104: Use the target point to attract the bias step d, use d to calculate the bias node near' from near to goal, synthesize the growth direction of the node through the bias node near' and rand, and near grows a step in this direction to obtain new;

[0152] S105: Map the sampling point new to the Cartesian space, and use the robot forward kinematics to calculate the distance between the robot arm and the obstacle in the Cartesian space;

[0153] S106: Determine whether the robot arm will collide with the obstacle. If so, discard the sampling point and return to step S102; if not, take near as the parent node of new and add new to T;

[0154] S107: Reselect the parent node and take the neighboring nodes near new as potential parent nodes;

[0155] S108: Select a potential parent node from among the potential parent nodes whose path cost from the potential parent node to the starting point plus the distance from new to the node is the smallest as the new parent node of new;

[0156] S109: Determine whether the distance between new and goal is less than a set threshold. If so, reselect a parent node for goal. If not, do not output.

[0157] S110: Find the path from start to goal from the search tree T;

[0158] S111: Calculate the joint speed and acceleration required to complete the planned path, and the robot arm continues to iterate according to the joint speed and acceleration.

[0159] The traditional fast random exploration tree algorithm samples completely randomly in the sampling space, which may lead to a large number of invalid nodes being randomly sampled. The random tree expands too much in other directions, resulting in uncertainty in the algorithm's convergence speed. In addition, the fixed node expansion step makes the algorithm not flexible enough to deal with different environments. Therefore, based on the characteristics of the expansion randomness, the target point is used to guide the growth of the random search tree, so that the node expansion has a certain directionality, reducing the waste of leaf nodes in useless sampling space, reducing the running time of the algorithm, and using an adaptive step size instead of the original fixed expansion step size to improve the applicability of the algorithm.

[0160] Reference Figure 7 As shown, real-time judgment of whether the robot arm encounters a dynamic obstacle during operation specifically includes:

[0161] Get the moving speed of obstacles near the planned path;

[0162] Determine whether the obstacle moving speed is greater than the set threshold. If so, switch to run the improved artificial potential field algorithm for dynamic obstacle avoidance. If not, no output is made.

[0163] By improving the hybrid algorithm of the fast random exploration tree algorithm and the improved artificial potential field algorithm, the problems that the artificial potential field algorithm is prone to fall into local minima and the fast random exploration tree algorithm is not suitable for real-time planning of obstacle avoidance in dynamic environments are solved.

[0164] Reference Figure 8 As shown, real-time judgment of whether the robot arm reaches the target joint posture specifically includes:

[0165] Get robot arm information in real time;

[0166] The inverse kinematics model is established using numerical iteration method;

[0167] Solve the real-time robot arm joint pose;

[0168] Determine whether the real-time robot arm joint posture is the same as the target joint posture. If so, the output has reached the destination and the algorithm ends. If not, continue to run the hybrid algorithm.

[0169] The inverse kinematics model of the robot's joint posture is established using the numerical iteration method, which can effectively and accurately determine whether the robot has reached the target joint posture without human intervention.

[0170] Furthermore, the present solution also proposes a computer-readable storage medium on which a computer-readable program is stored. When the computer-readable program is called, the above-mentioned robot arm path planning method based on the hybrid algorithm is executed.

[0171] It is understandable that the storage medium may be a magnetic medium, such as a floppy disk, a hard disk, or a magnetic tape; an optical medium, such as a DVD; or a semiconductor medium, such as a solid state drive (SSD).

[0172] In summary, the advantages of the present invention are: by improving the fast random exploration tree algorithm, the problems of high path cost, poor path optimization and waste of a large number of sampling points are solved; by improving the artificial potential field algorithm, dynamic and static obstacles can be better handled, while greatly enhancing the ability of the robot arm to avoid dynamic obstacles, the problem of unreachable targets is alleviated and the adaptability of the algorithm is improved; by improving the hybrid algorithm of the fast random exploration tree algorithm and the improved artificial potential field algorithm, the problem that the artificial potential field algorithm is prone to fall into the local minimum and the fast random exploration tree algorithm is not suitable for real-time planning of obstacle avoidance in a dynamic environment is solved.

[0173] The above shows and describes the basic principles, main features and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The above embodiments and descriptions only describe the principles of the present invention. The present invention may be subject to various changes and improvements without departing from the spirit and scope of the present invention. These changes and improvements fall within the scope of the present invention. The scope of protection claimed by the present invention is defined by the attached claims and their equivalents.

Claims

1. A robot arm path planning method based on a hybrid algorithm, characterized in that: include: Based on the current joint position of the robot arm, the forward kinematics model of the robot arm is established using the DH parameter method; Initialize the algorithm, obtain the target point information, use the numerical iteration method to establish the inverse kinematics model, and solve the target joint position and posture; Using the improved artificial potential field algorithm to plan a path, the joint speed and acceleration required to complete the planned path are calculated, and the robotic arm continuously iterates according to the joint speed and acceleration; Determine in real time whether the robot arm is trapped in the local minimum. If so, use the improved fast random exploration tree algorithm to get out of the trap. If not, no output is made. The joint speed and acceleration required to complete the path planned by the improved fast random exploration tree algorithm are calculated, and the robot arm continuously iterates according to the joint speed and acceleration; Determine in real time whether the robot arm encounters a dynamic obstacle during operation. If so, switch to the improved artificial potential field algorithm for dynamic obstacle avoidance. If not, no output is made. Determine in real time whether the robot arm has reached the target joint position. If so, output that it has reached the destination and the algorithm ends. If not, continue to run the hybrid algorithm. The step of using the improved artificial potential field algorithm to plan a path, calculating the joint speed and acceleration required to complete the planned path, and the robot arm continuously iterating according to the joint speed and acceleration specifically includes: Based on the target joint posture, a segmented attractive potential field is constructed; According to the segmented attraction potential field, the attraction linear velocity is calculated; According to the moving speed of the obstacle, the obstacle's range of action is calculated; Based on the obstacle moving speed and obstacle action range, an improved repulsive potential field is constructed; According to the improved repulsive potential field, the repulsive linear velocity of each obstacle is calculated; Calculate the combined velocity of the robot's attraction linear velocity and each repulsion linear velocity, and output it as the joint velocity; Based on the real-time joint velocity changes of the robot arm, calculate and output its real-time acceleration; The segmented attractive potential field is: ， Where U a is the segmented attractive potential field, K a To set the attractive potential field gain, is the distance between the end effector and the target point, is to set the distance constant, S is the adjustment factor; The attraction linear velocity is: , Where V a is the linear velocity of attraction, is the gradient of the piecewise attractive potential field, that is , the direction is negative; The obstacle range is: , In the formula, is the range of action of the ith obstacle, , are the maximum and minimum ranges of the obstacle, V0 is the speed of the obstacle corresponding to the maximum range, V i is the speed of the i-th obstacle; The improved repulsive potential field is: , Where U i is the improved repulsive potential field of the ith obstacle, K r To set the repulsive potential field gain, is the minimum distance between the robot and the i-th obstacle, , are the static and dynamic obstacle factors, respectively; The repulsive linear velocity is: , Where V ri is the repulsive linear velocity of the ith obstacle, For U i right The partial differential of The calculating of the joint speed and acceleration required to complete the path planned by the improved fast random exploration tree algorithm, and the continuous iterative operation of the robot arm according to the joint speed and acceleration specifically includes: S101: Map the starting pose and the target pose into the joint space, namely, start and goal, and record start as the root node of the random tree T; S102: randomly generate a sampling point rand in the joint space using a random sampling method; S103: Find the leaf node near that is closest to rand in the random tree T; S104: Use the target point to attract the bias step d, use d to calculate the bias node near' from near to goal, synthesize the growth direction of the node through the bias node near' and rand, and near grows a step in this direction to obtain new; S105: Map the sampling point new to the Cartesian space, and use the robot forward kinematics to calculate the distance between the robot arm and the obstacle in the Cartesian space; S106: Determine whether the robot arm will collide with the obstacle. If so, discard the sampling point and return to step S102; if not, take near as the parent node of new and add new to T; S107: Reselect the parent node and take the neighboring nodes near new as potential parent nodes; S108: Select a potential parent node from among the potential parent nodes whose path cost from the potential parent node to the starting point plus the distance from new to the node is the smallest as the new parent node of new; S109: Determine whether the distance between new and goal is less than a set threshold. If so, reselect a parent node for goal. If not, do not output. S110: Find the path from start to goal from the search tree T; S111: Calculate the joint speed and acceleration required to complete the planned path, and the robot arm continues to iterate according to the joint speed and acceleration.

2. A robot arm path planning method based on a hybrid algorithm according to claim 1, characterized in that: The method of establishing the forward kinematics model of the robot arm using the DH parameter method based on the current joint posture of the robot arm specifically includes: Establish a spatial rectangular coordinate system for each connecting rod and number them in sequence; The coordinate system is initially located in the initial coordinate system, and is rotated by a first rotation angle around the X axis of the initial coordinate system so that the Z axis of the initial coordinate system is parallel to the Z axis of the next coordinate system; The coordinate system is translated along the X axis of the initial coordinate system by a first translation amount so that the Z axis of the initial coordinate system is collinear with the Z axis of the next coordinate system; The coordinate system is rotated by a second rotation angle around the Z axis of the initial coordinate system so that the X axis of the initial coordinate system is parallel to the X axis of the next coordinate system; The coordinate system is translated along the Z axis of the initial coordinate system by a second translation amount, and has now been transformed to the next coordinate system; The homogeneous transformation matrix between the coordinate systems of link i-1 and link i is obtained through homogeneous transformation; Based on the current joint posture of the robot arm, a forward kinematics model of the robot arm is established by using the DH parameter method, and the DH parameters of each connecting rod are determined, wherein the DH parameters include a first rotation angle, a second rotation angle, a first translation amount, and a second translation amount; The homogeneous transformation matrix is: , In the formula, is the homogeneous transformation matrix between the coordinate systems of link i-1 and link i, is the first rotation angle, is the second rotation angle, is the first translation, is the second translation.

3. A robot arm path planning method based on a hybrid algorithm according to claim 2, characterized in that: The algorithm is initialized to obtain the target point information, and the inverse kinematics model is established by using the numerical iteration method to solve the target joint posture. Specifically, the following steps are involved: Get the target point information and mark it as the end coordinate of the end link; Calculate the relative elevation angle between link i-1 and link i according to the relative elevation angle iteration formula; Solve the target joint pose based on the relative elevation angle between each link; The relative elevation angle iteration formula is: , In the formula, is the relative elevation angle between link i-1 and link i, is the coordinate of the end of link i, is the length of connecting rod i.

4. A robot arm path planning method based on a hybrid algorithm according to claim 3, characterized in that: The real-time determination of whether the robot arm is trapped in a local minimum specifically includes: Get the real-time joint speed of the robot arm; Determine whether the real-time joint speed is 0. If so, output that the robot arm is trapped in the local minimum and use the improved fast random exploration tree algorithm to get out of the trap. If not, no output is made. Determine whether the real-time joint speed changes periodically. If so, output that the robot arm is trapped in the local minimum and use the improved fast random exploration tree algorithm to escape. If not, no output is made.

5. A robot arm path planning method based on a hybrid algorithm according to claim 4, characterized in that: The real-time determination of whether the robot arm encounters a dynamic obstacle during operation specifically includes: Get the moving speed of obstacles near the planned path; Determine whether the obstacle moving speed is greater than the set threshold. If so, switch to run the improved artificial potential field algorithm for dynamic obstacle avoidance. If not, no output is made.

6. A robot arm path planning method based on a hybrid algorithm according to claim 5, characterized in that: The real-time determination of whether the robotic arm has reached the target joint position specifically includes: Get robot arm information in real time; The inverse kinematics model is established using numerical iteration method; Solve the real-time robot arm joint pose; Determine whether the real-time robot arm joint posture is the same as the target joint posture. If so, the output has reached the destination and the algorithm ends. If not, continue to run the hybrid algorithm.

Images