Obstacle avoidance path planning method and system for multi-degree-of-freedom robotic arm

Abstract

The present invention provides a multi-degree-of-freedom robot arm obstacle avoidance path planning method and system, which relates to the field of robot obstacle avoidance technology. The present invention uses forward kinematics to solve the transformation relationship and three-dimensional coordinates of each joint of the robot arm, generates a workspace of the robot arm, models obstacles in the workspace, generates a corresponding obstacle space, establishes an obstacle avoidance space and generates a preliminary global path, optimizes the preliminary path, calculates the movement speed, acceleration and joint limit torque of the robot arm, comprehensively generates the maximum obstacle avoidance angle, compares the obstacle avoidance angle of each joint with the maximum obstacle avoidance angle, screens extreme joint angles and counts their number. Redundant degrees of freedom are calculated and a threshold is set. If the threshold is exceeded, the obstacle avoidance is successful and the path planning is effective. The present invention achieves effective obstacle avoidance of the robot arm in a complex environment by establishing the obstacle avoidance space of the robot arm, optimizing the path, and managing and evaluating the redundant degrees of freedom.

Description

Technical Field

[0001] The present invention relates to the technical field of robot obstacle avoidance, and in particular to a multi-degree-of-freedom robot arm obstacle avoidance path planning method and system. Background Art

[0002] At present, robot arm obstacle avoidance technology has made significant progress in many fields. First, the rapid development of sensor technology, such as lidar, depth cameras and force sensors, enables robot arms to perceive the environment more accurately and detect static and dynamic obstacles in real time. Secondly, path planning algorithms have been continuously optimized, combined with heuristic search algorithms and sampling algorithms, to improve the efficiency and reliability of path planning.

[0003] In the prior art, publication number CN110125943B discloses a multi-degree-of-freedom robot obstacle avoidance path planning method, which obtains an initial path by adopting a robot path planning method; collects real-time information of each joint in the initial path at each preset time; mathematically models and maps the real-time information in the initial path, randomly selects a path, calculates feasibility and fitness according to preset constraints, marks the path with the largest fitness as a qualified path, repeats until qualified, and records the qualified path as a second-generation path set; crosses, mutates, and fuses the second-generation path set; obtains a third-generation path set, repeats until the number of iterations is N; and takes the path with the largest fitness in the last iteration as the final target path.

[0004] Insufficient existing technology:

[0005] The existing technology has problems with insufficient computing efficiency and real-time performance. As the degrees of freedom of the robotic arm increase, the computational complexity of the path planning algorithm also increases. Especially in complex environments, the number of obstacles increases, resulting in a large amount of computing resources required for path generation, evaluation and optimization. It is impossible to determine the workspace to which the optimized path belongs, and the real-time performance is not strong.

[0006] The existing technology does not take into account the operating parameters of the robot arm during obstacle avoidance. By selecting random paths, it is impossible to determine the movement efficiency and safety of the robot arm in a dynamically changing environment, and it is difficult to adjust the path in time to cope with sudden obstacles or environmental changes.

[0007] Therefore, it is necessary to provide a multi-degree-of-freedom robotic arm obstacle avoidance path planning method and system to solve the above problem.

[0008] The above information disclosed in this Background section is only for enhancement of understanding of the background of the present disclosure and therefore it may contain information that does not constitute the prior art that is already known to one of ordinary skill in the art. Summary of the invention

[0009] The purpose of the present invention is to provide a multi-degree-of-freedom robotic arm obstacle avoidance path planning method and system to solve the problems raised in the above background technology.

[0010] To achieve the above object, the present invention provides the following technical solutions:

[0011] A multi-degree-of-freedom robot arm obstacle avoidance path planning method, the specific steps include:

[0012] Furthermore, the transformation relationship of each joint of the robot arm is solved based on the following method:

[0013] Based on forward kinematics, the geometric relationship of the kinematic chain is used to solve the problem. Four parameters associated with the robot arm are used. The four parameters are rotation angle, connecting rod length, connecting rod offset and torsion angle. For the rotation angle, connecting rod length, connecting rod offset and torsion angle, a homogeneous transformation matrix is ​​used to represent the transformation relationship between joint i and joint i-1. For a robot with n joint points, the homogeneous transformation matrix of each joint is multiplied in turn by the step-by-step product method to finally obtain the transformation matrix of the robot arm relative to the base. The logic is as follows:

[0014]

[0015] Among them, θ i represents the rotation angle of joint i, a i represents the length of the connecting rod connected to joint i, d i represents the offset of the link connected to joint i, α i represents the torsion angle between two adjacent links connected by joint i, Represents the homogeneous transformation matrix from joint i to joint i-1, where i is the index of the joint point;

[0016]

[0017] in, represents the transformation matrix of the robot arm relative to the base, Represents the homogeneous transformation matrix from joint n to joint n-1.

[0018] Furthermore, the three-dimensional coordinates of each joint are solved, and the working space of the robotic arm is generated according to the transformation relationship of each joint of the robotic arm and the three-dimensional coordinates, and the formula based on this is:

[0019] The formula for calculating the three-dimensional coordinates of each joint is:

[0020] x i =α i cosθ i +α i+1 cos(θ i+θ i+1 )

[0021] y i =α i sinθ i +α i+1 sin(θ i +θ i+1 )

[0022] z i =α i tanθ i +α i+1 tan(θ i +θ i+1 )

[0023] Among them, x i ,y i 、z i Represents the horizontal coordinate, vertical coordinate, and vertical coordinate of joint i, α i+1 represents the torsion angle between two adjacent links connected by joint i+1, θ i+1 Represents the rotation angle of joint i+1;

[0024] The working space of the robot arm is generated by using the transformation relationship of each joint of the robot arm and the three-dimensional coordinates, and the method is based on:

[0025] Calculate the coordinates of the end effector and use it as the reference point, traverse the angle combinations of all related nodes, discretize the rotation angle of each joint point, use forward kinematics to calculate the transformation relationship between adjacent joint points for each discretized angle set, record all the calculated coordinate positions to generate a space set to represent the workspace of the robot arm, based on the formula:

[0026]

[0027]

[0028] in, represents the angle set after the rotation angle is discretized. Represents the coordinates of the end effector, 1 in the coordinate represents the degree of freedom of the end effector, and P represents the spatial set used to represent the working space of the robotic arm.

[0029] Furthermore, an obstacle space corresponding to the working space of the robot arm is generated, and an obstacle avoidance space of the robot arm is established, according to the method:

[0030] According to the coordinates and size of each obstacle, its geometric volume is defined in the workspace of the robot. The geometric tools are used to represent the volume set occupied by each obstacle. These sets occupy corresponding areas in the workspace of the robot. The area is the obstacle space. Then the obstacle space is mapped to the workspace of the robot. The geometric difference between the workspace and the obstacle space is calculated through Boolean operations to obtain the obstacle avoidance space of the robot. The formula is as follows:

[0031]

[0032] Where F represents the obstacle avoidance space of the robot arm, W represents the working space of the robot arm, and V(O m ) represents the volume geometry of the mth obstacle, O m represents the mth obstacle, and q represents the total number of obstacles.

[0033] Furthermore, a heuristic search algorithm is used to generate a preliminary global path, based on the following method:

[0034] A heuristic search algorithm is used to estimate the cost from the current node to the target node to select the optimal path. A specific valuation function is used to put the node's neighbor nodes into an open list starting from the starting point, calculate their total path cost, and select the next expanded node based on the minimum total path cost. The termination condition is that when the target node is expanded, the search ends and the preliminary global path is found. The formula is:

[0035] f(j)=g(j)+h(j)

[0036]

[0037] h(j)=|x j -x goal |+|y j -y goal |+|z j -z goal |

[0038] Among them, f(j) represents the total path cost from the starting point to the target node and passing through node j, g(j) represents the actual cost from the starting point to node j, h(j) represents the heuristic estimate from node j to the target node, that is, the estimated cost, expressed as Manhattan distance, x goal ,y goal 、z goal Indicates the horizontal coordinate, vertical coordinate, and vertical coordinate of the target node, x j ,y j 、z j Represents the horizontal coordinate, vertical coordinate, and vertical coordinate of path node j;

[0039] Minimize the obstacle avoidance path length and maximize the safety distance, select the obstacle avoidance path with the least obstacles on the road, and meet the minimum value of this constraint condition. The formula is:

[0040] L = f(j) min

[0041]

[0042] Where L represents the minimum length of the obstacle avoidance path, l k represents the kth point on the minimum length L of the obstacle avoidance path, d(l k , O m ) represents the kth point l on the minimum length L of the obstacle avoidance path k Minimum distance to all obstacles, d min It represents the minimum safety distance, and k represents the index of the point on the minimum length L of the obstacle avoidance path.

[0043] Furthermore, the speed, acceleration and limit torque of the joint motion of the robot arm are calculated based on the following formula:

[0044]

[0045] Where v(t) represents the velocity of the robot at time t, a(t) represents the acceleration of the robot at time t, I represents the moment of inertia, and θ i+1 -θ i represents the difference between the rotation angle of joint i+1 and the rotation angle of joint i in the path, τ max Indicates the ultimate torque of the robot arm.

[0046] Furthermore, the maximum obstacle avoidance angle is generated based on the formula:

[0047] τ dynamic (t) = τ max +δ*v(t)+ε*a(t)

[0048]

[0049] K i =θ i +Δμ i

[0050] Among them, τ dynamic (t) represents the dynamic torque of the robot at time t, δ and ε represent the weights of the robot velocity and acceleration on the dynamic torque, respectively, and K max (t) represents the maximum obstacle angle of the robot at time t, K i represents the obstacle avoidance angle of joint i, θ i represents the rotation angle of joint i, Δμi Indicates the angle that needs to be increased or decreased to ensure obstacle avoidance.

[0051] Furthermore, each joint obstacle avoidance angle is compared with the maximum obstacle avoidance angle to screen out extreme joint angles and count their number. The logic is as follows:

[0052]

[0053] Among them, Q represents the logical value for judging whether each joint obstacle avoidance angle is an extreme joint angle. When Q=1, it means that the obstacle avoidance angle of joint i is an extreme joint angle; when Q=0, it means that the obstacle avoidance angle of joint i is not an extreme joint angle.

[0054] Furthermore, according to the number of extreme joint angles, the redundant degrees of freedom are calculated, and a redundant degrees of freedom threshold is set. If the redundant degrees of freedom exceed the threshold, the obstacle avoidance is successful and the path planning is valid. The logic is as follows:

[0055] r = JN

[0056] if

[0057] if

[0058] Among them, r represents the redundant degree of freedom, r max represents the redundant degree of freedom threshold, J represents the overall degree of freedom of the robot arm, and N represents the number of extreme joint angles.

[0059] The present invention further provides a multi-degree-of-freedom robot arm obstacle avoidance path planning system, wherein the path planning system is used to execute any one of the multi-degree-of-freedom robot arm obstacle avoidance path planning methods described above, comprising:

[0060] A workspace generation module, the workspace generation module is used to solve the transformation relationship of each joint of the robot arm and the three-dimensional coordinates of each joint based on the principle of forward kinematics, and generate the workspace of the robot arm according to the transformation relationship of each joint of the robot arm and the three-dimensional coordinates;

[0061] An obstacle avoidance space establishment module, the obstacle avoidance space establishment module is used to model the volume of obstacles in the working space of the robot arm, generate an obstacle space corresponding to the working space of the robot arm, and establish the obstacle avoidance space of the robot arm;

[0062] A path optimization module, wherein the path optimization module is used to generate a preliminary global path using a heuristic search algorithm in the established obstacle avoidance space, and optimize the preliminary global path, wherein the optimization includes minimizing the path length and maximizing the safety distance, and selecting an obstacle avoidance path with the least obstacles on the path;

[0063] A dynamic performance analysis module, which is used to calculate the speed, acceleration and limit torque of the joint movement of the robot arm during movement, and to comprehensively generate a maximum obstacle avoidance angle using the speed, acceleration and limit torque of the robot arm;

[0064] A redundant degree of freedom evaluation module is used to compare the obstacle avoidance angle of each joint with the maximum obstacle avoidance angle to screen out extreme joint angles and count their number. Based on the number of extreme joint angles, the redundant degrees of freedom are calculated and a redundant degree of freedom threshold is set. If the redundant degrees of freedom exceed the threshold, the obstacle avoidance is successful and the path planning is valid.

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

[0066] The present invention can improve the efficiency of path planning by adopting a heuristic search algorithm to optimize the path length and safety distance. Combining forward kinematics and homogeneous transformation matrix, the working space and obstacle avoidance space of the robot arm are quickly solved, solving the problems of long calculation time and slow response in complex environments in the prior art. In addition, by introducing a path optimization module, the path length is minimized and the safety distance is maximized, which greatly improves the safety of the movement and overcomes the defect of insufficient safety consideration in the previous technology.

[0067] The present invention enhances the flexibility of the robot arm in complex and dynamic environments through dynamic performance analysis and redundant degree of freedom evaluation, and path smoothing ensures the continuity of motion, solving the problem of insufficient path smoothness in the prior art. At the same time, through the evaluation of extreme joint angles and the ability to flexibly adjust the path, the ability to cope with obstacles has also been significantly improved, effectively addressing the shortcomings of the prior art in environments with dense obstacles;

[0068] The present invention realizes the path planning optimization and safe obstacle avoidance functions of the robot arm in a complex environment through the method steps of workspace generation, obstacle avoidance space establishment, path optimization, dynamic performance analysis and redundant degree of freedom evaluation. BRIEF DESCRIPTION OF THE DRAWINGS

[0069] Figure 1 It is a schematic diagram of the overall method flow of the present invention.

[0070] Figure 2 It is a schematic diagram of the system module flow of the present invention. DETAILED DESCRIPTION

[0071] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with specific embodiments.

[0072] It should be noted that, unless otherwise defined, the technical terms or scientific terms used in the present invention should be understood by people with ordinary skills in the field to which the present invention belongs. The words "first", "second" and similar words used in the present invention do not indicate any order, quantity or importance, but are only used to distinguish different components. "Include" or "comprise" and similar words mean that the elements or objects appearing before the word include the elements or objects listed after the word and their equivalents, without excluding other elements or objects. "Connect" or "connected" and similar words are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. "Up", "down", "left", "right" and the like are only used to indicate relative positional relationships. When the absolute position of the described object changes, the relative positional relationship may also change accordingly.

[0073] Example:

[0074] See also Figure 1 , a multi-degree-of-freedom robot arm obstacle avoidance path planning method, the specific steps include:

[0075] Step 1: Solve the transformation relationship of each joint and the three-dimensional coordinates of each joint of the robotic arm based on the principle of forward kinematics, and generate the working space of the robotic arm according to the transformation relationship of each joint and the three-dimensional coordinates of the robotic arm;

[0076] Step 2: Modeling is performed in the workspace of the robotic arm according to the volume of the obstacle, an obstacle space corresponding to the workspace of the robotic arm is generated, and an obstacle avoidance space of the robotic arm is established;

[0077] Step 3: In the established obstacle avoidance space, a heuristic search algorithm is used to generate a preliminary global path, and the preliminary global path is optimized. The optimization includes minimizing the path length and maximizing the safety distance, and selecting an obstacle avoidance path with the least obstacles on the road;

[0078] Step 4: Calculate the speed, acceleration and limit torque of the joint movement of the robot arm during movement, and use the speed, acceleration and limit torque of the robot arm to comprehensively generate the maximum obstacle avoidance angle;

[0079] Step 5: Use each joint obstacle avoidance angle to compare with the maximum obstacle avoidance angle to screen out extreme joint angles, and count their number. According to the number of extreme joint angles, calculate the redundant degrees of freedom and set the redundant degrees of freedom threshold. If the redundant degrees of freedom exceed the threshold, the obstacle avoidance is successful and the path planning is valid.

[0080] It should be noted that through forward kinematics, the position and direction of each joint can be calculated, which is crucial for path planning, obstacle avoidance and controlling the robot to perform precise operations in complex environments. This helps ensure that the robot can accurately reach the target position while avoiding collisions with obstacles. By calculating the position of each joint through the transformation matrix, the specific position of the robot in different postures can be determined, which can play a role in precise positioning; by clarifying the workspace, potential obstacles can be identified, obstacle avoidance paths can be designed, and the joint angles can be discretized, which helps to optimize the robot's motion path and improve control accuracy and efficiency.

[0081] Therefore, it is necessary to solve the transformation relationship of each joint of the robot arm, and the method is based on:

[0082] Based on forward kinematics, the geometric relationship of the kinematic chain is used to solve the problem. The four parameters associated with the robot include: rotation angle, connecting rod length, connecting rod offset and torsion angle. The transformation relationship between joint i and joint i-1 is represented by a homogeneous transformation matrix through the rotation angle, connecting rod length, connecting rod offset and torsion angle. For a robot with n degrees of freedom, the homogeneous transformation matrix of each joint is multiplied in turn by the step-by-step product method to finally obtain the transformation matrix of the robot relative to the base. The logic is as follows:

[0083]

[0084] Among them, θ i represents the rotation angle of joint i, a i represents the length of the connecting rod connected to joint i, d i represents the offset of the link connected to joint i, α i represents the torsion angle between two adjacent links connected by joint i, Represents the homogeneous transformation matrix from joint i to joint i-1, where i is the index of the joint point;

[0085]

[0086] in, represents the transformation matrix of the robot arm relative to the base, Represents the homogeneous transformation matrix from joint n to joint n-1.

[0087] It should be noted that by traversing all possible joint configurations, we can fully understand the robot's motion capabilities and coverage. Discretization ensures sufficient accuracy while ensuring computational efficiency. Forward kinematics provides precise three-dimensional coordinates of each joint position, allowing us to accurately map the robot's workspace under different configurations. Recording these positions provides the necessary data support for path planning, obstacle avoidance, and task optimization, ensuring that the robot can complete tasks efficiently and safely in practical applications.

[0088] Therefore, it is necessary to calculate the three-dimensional coordinates of each joint, based on the following formula:

[0089] x i =α i cosθ i +α i+1 cos(θ i +θ i+1 )

[0090] y i =α i sinθ i +α i+1 sin(θ i +θ i+1 )

[0091] z i =α i tanθ i +α i+1 tan(θ i +θ i+1 )

[0092] Among them, x i ,y i 、z i Represents the horizontal coordinate, vertical coordinate, and vertical coordinate of joint i, α i+1 represents the torsion angle between two adjacent links connected by joint i+1, θ i+1 Represents the rotation angle of joint i+1;

[0093] The working space of the robot arm is generated by using the transformation relationship of each joint of the robot arm and the three-dimensional coordinates, and the method is based on:

[0094] Calculate the coordinates of the end effector and use it as the reference point, traverse the angle combinations of all related nodes, discretize the rotation angle of each joint point, use forward kinematics to calculate the transformation relationship between adjacent joint points for each discretized angle set, record all the calculated coordinate positions to generate a space set to represent the workspace of the robot arm, based on the formula:

[0095]

[0096] in, represents the angle set after the rotation angle is discretized. Represents the coordinates of the end effector, where 1 in the coordinates means that the degree of freedom of the end effector is 1, and P represents a spatial set used to represent the working space of the robotic arm.

[0097] It should be noted that determining the obstacle avoidance space of the robot arm is a prerequisite for finding the best obstacle avoidance path. This can ensure that the robot arm will not collide with obstacles during operation, protect the equipment and the environment, and based on the established obstacle avoidance space, it can quickly help plan an optimal path so that the robot arm can avoid obstacles and reach the target position.

[0098] Therefore, it is necessary to generate an obstacle space corresponding to the working space of the robot arm and establish the obstacle avoidance space of the robot arm. The method is as follows:

[0099] According to the coordinates and size of each obstacle, its geometric volume is defined in the workspace of the robot. The geometric tools are used to represent the volume set occupied by each obstacle. These sets occupy corresponding areas in the workspace of the robot. The area is the obstacle space. Then the obstacle space is mapped to the workspace of the robot. The geometric difference between the workspace and the obstacle space is calculated through Boolean operations to obtain the obstacle avoidance space of the robot. The formula is as follows:

[0100]

[0101] Where F represents the obstacle avoidance space of the robot arm, W represents the working space of the robot arm, and V(O m ) represents the volume geometry of the mth obstacle, O m represents the mth obstacle, and q represents the total number of obstacles.

[0102] It should be noted that the heuristic search algorithm is an algorithm that uses heuristic information to guide the search process. The heuristic algorithm can quickly find a feasible path, reduce calculation time, and improve overall efficiency. Through heuristic evaluation, a better path can be found, reducing movement time and energy consumption. By reducing the path length, the operating time and energy consumption of the robot arm can be reduced, and the overall work efficiency can be improved. By smoothing the path, the robot arm can ensure smooth movement, reduce joint wear and mechanical fatigue, thereby extending the life of the equipment. Maximizing the safety distance can increase the distance between the robot arm and obstacles and reduce the risk of collision. At the same time, choosing the path with the least obstacles can improve the efficiency of task completion and reduce the probability of unexpected downtime and failure.

[0103] Therefore, it is necessary to use a heuristic search algorithm to generate a preliminary global path based on the following method:

[0104] A heuristic search algorithm is used to estimate the cost from the current node to the target node to select the optimal path. A specific valuation function is used to put the node's neighbor nodes into an open list starting from the starting point, calculate their total path cost, and select the next expanded node based on the minimum total path cost. The termination condition is that when the target node is expanded, the search ends and the preliminary global path is found. The formula is:

[0105] f(j)=g(j)+h(j)

[0106]

[0107] h(j)=|x j -x goal |+|y j -y goal |+|z j -z goal |

[0108] Among them, f(j) represents the total path cost from the starting point to the target node and passing through node j, g(j) represents the actual cost from the starting point to node j, h(j) represents the heuristic estimate from node j to the target node, that is, the estimated cost, expressed as Manhattan distance, x goal ,y goal 、z goal Indicates the horizontal coordinate, vertical coordinate, and vertical coordinate of the target node, x j ,y j 、z j Represents the horizontal coordinate, vertical coordinate, and vertical coordinate of path node j;

[0109] Minimize the obstacle avoidance path length and maximize the safety distance, select the obstacle avoidance path with the least obstacles on the road, and meet the minimum value of this constraint condition. The formula is:

[0110] L = f(j) min

[0111]

[0112] Where L represents the minimum length of the obstacle avoidance path, l k represents the kth point on the minimum length L of the obstacle avoidance path, d(l k , O m ) represents the kth point l on the minimum length L of the obstacle avoidance path k Minimum distance to all obstacles, d min It represents the minimum safety distance, and k represents the index of the point on the minimum length L of the obstacle avoidance path.

[0113] It should be noted that accurate calculation of speed and acceleration helps to achieve precise control of the robot arm's trajectory and ensure operational stability, while calculating the joint's limit torque can prevent overload operation and avoid damage to the robot arm or safety accidents. By controlling the joint torque within a safe range, mechanical stress and wear are reduced, and the service life of the robot arm is extended.

[0114] Therefore, it is necessary to calculate the speed, acceleration and limit torque of the joint movement of the robot arm in motion, based on the following formula:

[0115]

[0116]

[0117] Where v(t) represents the velocity of the robot at time t, a(t) represents the acceleration of the robot at time t, I represents the moment of inertia, and θ i+1 -θ i represents the difference between the rotation angle of joint i+1 and the rotation angle of joint i in the path, τ max Indicates the ultimate torque of the robot arm.

[0118] It should be noted that the calculation of the maximum obstacle avoidance angle is to ensure that the robot arm can avoid obstacles to the greatest extent possible in complex environments, reduce the risk of collision, protect the safety of equipment and personnel, and find a more effective path based on the maximum obstacle avoidance angle, avoiding unnecessary downtime and adjustments.

[0119] Therefore, it is necessary to generate the maximum obstacle avoidance angle, based on the formula:

[0120] τ dynamic (t) = τ max +δ*v(t)+ε*a(t)

[0121]

[0122] K i =θ i +Δμ i

[0123] Among them, τ aynamic (t) represents the dynamic torque of the robot at time t, δ and ε represent the weights of the robot velocity and acceleration on the dynamic torque, respectively, and K max (t) represents the maximum obstacle angle of the robot at time t, K i represents the obstacle avoidance angle of joint i, θ i represents the rotation angle of joint i, Δμ iIndicates the angle that needs to be increased or decreased to ensure obstacle avoidance; In the above formula, the magnitude relationship between δ and ε is δ<ε, because acceleration directly affects the inertia force of the robot arm. When the robot arm accelerates or decelerates, it will generate additional inertia torque, which often has a more significant impact on the torque than the friction torque at a constant speed. The robot arm requires higher dynamic response capabilities when accelerating, which requires greater torque support. Therefore, in planning, the weight of acceleration is usually set to be greater than the weight of speed; In the above formula, in calculating the obstacle avoidance angle of joint i, the method for determining the angle required to ensure successful obstacle avoidance is to replace Δμ i It is stipulated that in the set interval, the interval can be set to [-Δθ i , +Δθ i ], Δθ i Indicates the maximum allowable adjustment angle range of joint i. For example, the interval can be set to |-15°, +15°|, and the angle should not exceed 1° each time to ensure the accuracy of obstacle avoidance and the smooth movement of the robot arm.

[0124] It should be noted that in the operation of the robotic arm, the extreme joint angles are usually joint points with a degree of freedom of 1. Such joint points often have poor flexibility and are prone to failure. Therefore, it is necessary to find out those joints that are close to or exceed the maximum obstacle avoidance angle, identify potential risk points, avoid possible collisions or failures, and by analyzing the extreme joint angles, adjust and optimize the motion path of the robotic arm to improve the efficiency and safety of the overall operation.

[0125] Therefore, it is necessary to compare the obstacle avoidance angle of each joint with the maximum obstacle avoidance angle to screen out extreme joint angles and count their number. The logic is as follows:

[0126]

[0127] Where Q represents the logical value for judging whether the obstacle avoidance angle of each joint is an extreme joint angle. When Q = 1, it means that the obstacle avoidance angle of joint i is an extreme joint angle; when Q = 0, it means that the obstacle avoidance angle of joint i is not an extreme joint angle; K i represents the obstacle avoidance angle of joint i.

[0128] It should be noted that redundant degrees of freedom provide more path options and increase the possibility of successful obstacle avoidance. By effectively utilizing redundant degrees of freedom, more optimized paths can be found in complex environments, improving the effectiveness of path planning and ensuring that the operational flexibility of the robot arm is maximized while maintaining safety, thereby improving the work efficiency of the robot arm.

[0129] Therefore, it is necessary to calculate the redundant degrees of freedom according to the number of extreme joint angles and set a redundant degrees of freedom threshold. If the redundant degrees of freedom exceed the threshold, the obstacle avoidance is successful and the path planning is valid. The logic is as follows:

[0130] r = JN

[0131] if

[0132] if

[0133] Among them, r represents the redundant degree of freedom, r max represents the redundant degree of freedom threshold, J represents the overall degree of freedom of the robot arm, and N represents the number of extreme joint angles.

[0134] See also Figure 2 The present invention further provides a multi-degree-of-freedom robot arm obstacle avoidance path planning system, wherein the path planning system is used to execute any one of the multi-degree-of-freedom robot arm obstacle avoidance path planning methods described above, comprising:

[0135] A workspace generation module, the workspace generation module is used to solve the transformation relationship of each joint of the robot arm and the three-dimensional coordinates of each joint based on the principle of forward kinematics, and generate the workspace of the robot arm according to the transformation relationship of each joint of the robot arm and the three-dimensional coordinates;

[0136] An obstacle avoidance space establishment module, the obstacle avoidance space establishment module is used to model the volume of obstacles in the working space of the robot arm, generate an obstacle space corresponding to the working space of the robot arm, and establish the obstacle avoidance space of the robot arm;

[0137] A path optimization module, wherein the path optimization module is used to generate a preliminary global path using a heuristic search algorithm in the established obstacle avoidance space, and optimize the preliminary global path, wherein the optimization includes minimizing the path length and maximizing the safety distance, and selecting an obstacle avoidance path with the least obstacles on the path;

[0138] A dynamic performance analysis module, which is used to calculate the speed, acceleration and limit torque of the joint movement of the robot arm during movement, and to comprehensively generate a maximum obstacle avoidance angle using the speed, acceleration and limit torque of the robot arm;

[0139] A redundant degree of freedom evaluation module is used to compare the obstacle avoidance angle of each joint with the maximum obstacle avoidance angle to screen out extreme joint angles and count their number. Based on the number of extreme joint angles, the redundant degrees of freedom are calculated and a redundant degree of freedom threshold is set. If the redundant degrees of freedom exceed the threshold, the obstacle avoidance is successful and the path planning is valid.

[0140] The above formulas are all dimensionless and numerical calculations. The formula is a formula for the most recent real situation obtained by collecting a large amount of data and performing software simulation. The preset parameters in the formula are set by technicians in this field according to actual conditions.

[0141] The above embodiments may be implemented in whole or in part by software, hardware, firmware or any other combination thereof. When implemented by software, the above embodiments may be implemented in whole or in part in the form of a computer program product. Those skilled in the art may appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein may be implemented by electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed by hardware or software methods depends on the specific application and design constraints of the technical solution.

[0142] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, and may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0143] The above description is only a specific implementation manner of the present application, but the protection scope of the present application is not limited thereto. Any technician familiar with the technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application.

Claims

1. A multi-degree-of-freedom robot arm obstacle avoidance path planning method, characterized in that: The specific steps include: Step 1: Solve the transformation relationship of each joint and the three-dimensional coordinates of each joint of the robotic arm based on the principle of forward kinematics, and generate the working space of the robotic arm according to the transformation relationship of each joint and the three-dimensional coordinates of the robotic arm; Step 2: Modeling is performed in the workspace of the robotic arm according to the volume of the obstacle, an obstacle space corresponding to the workspace of the robotic arm is generated, and an obstacle avoidance space of the robotic arm is established; Step 3: In the established obstacle avoidance space, a heuristic search algorithm is used to generate a preliminary global path, and the preliminary global path is optimized. The optimization includes minimizing the path length and maximizing the safety distance, and selecting an obstacle avoidance path with the least obstacles on the road; Step 4: Calculate the speed, acceleration and limit torque of the joint movement of the robot arm in motion, and use the speed, acceleration and limit torque of the robot arm to comprehensively generate the maximum obstacle avoidance angle; Step 5: Use each joint obstacle avoidance angle to compare with the maximum obstacle avoidance angle to screen out extreme joint angles, and count their number. According to the number of extreme joint angles, calculate the redundant degrees of freedom and set the redundant degrees of freedom threshold. If the redundant degrees of freedom exceed the threshold, the obstacle avoidance is successful and the path planning is valid.

2. A multi-degree-of-freedom robot arm obstacle avoidance path planning method according to claim 1, characterized in that: The method for solving the transformation relationship of each joint of the robotic arm is based on: Based on forward kinematics, the geometric relationship of the kinematic chain is used to solve the problem. Four parameters associated with the robot arm are used. The four parameters are rotation angle, connecting rod length, connecting rod offset and torsion angle. For the rotation angle, connecting rod length, connecting rod offset and torsion angle, a homogeneous transformation matrix is ​​used to represent the transformation relationship between joint i and joint i-1. For a robot with n joint points, the homogeneous transformation matrix of each joint is multiplied in turn by the step-by-step product method to finally obtain the transformation matrix of the robot arm relative to the base. The logic is as follows: Among them, θ i represents the rotation angle of joint i, a i represents the length of the connecting rod connected to joint i, d i represents the offset of the link connected to joint i, α i represents the torsion angle between two adjacent links connected by joint i, Represents the homogeneous transformation matrix from joint i to joint i-1, where i is the index of the joint point; in, represents the transformation matrix of the robot arm relative to the base, Represents the homogeneous transformation matrix from joint n to joint n-1.

3. A multi-degree-of-freedom robot arm obstacle avoidance path planning method according to claim 2, characterized in that: Solve the three-dimensional coordinates of each joint, and generate the working space of the robotic arm according to the transformation relationship of each joint of the robotic arm and the three-dimensional coordinates. The formula is: The formula for calculating the three-dimensional coordinates of each joint is: x i =a i cosθ i +a i+1 cos(θ i +θ i+1 ) y i =a i sinth i +a i+1 sin(θ i +θ i+1 ) z i =a i tanθ i +a i+1 tan(θ i +θ i+1 ) Among them, x i ,y i 、z i Represents the horizontal coordinate, vertical coordinate, and vertical coordinate of joint i, α i+1 represents the torsion angle between two adjacent links connected by joint i+1, θ i+1 Represents the rotation angle of joint i+1; The working space of the robot arm is generated by using the transformation relationship of each joint of the robot arm and the three-dimensional coordinates, and the method is based on: Calculate the coordinates of the end effector and use it as the reference point, traverse the angle combinations of all related nodes, discretize the rotation angle of each joint point, use forward kinematics to calculate the transformation relationship between adjacent joint points for each discretized angle set, record all the calculated coordinate positions to generate a space set to represent the workspace of the robot arm, based on the formula: in, represents the angle set after the rotation angle is discretized. Represents the coordinates of the end effector, where 1 in the coordinates means that the degree of freedom of the end effector is 1, and P represents a spatial set used to represent the working space of the robotic arm.

4. The method for obstacle avoidance path planning of a multi-degree-of-freedom manipulator according to claim 1, characterized in that: Generate an obstacle space corresponding to the working space of the robot arm and establish an obstacle avoidance space of the robot arm, according to the method: According to the coordinates and size of each obstacle, its geometric volume is defined in the workspace of the robot. The geometric tools are used to represent the volume set occupied by each obstacle. These sets occupy corresponding areas in the workspace of the robot. The area is the obstacle space. Then the obstacle space is mapped to the workspace of the robot. The geometric difference between the workspace and the obstacle space is calculated through Boolean operations to obtain the obstacle avoidance space of the robot. The formula is as follows: Where F represents the obstacle avoidance space of the robot arm, W represents the working space of the robot arm, and V(O m ) represents the volume geometry of the mth obstacle, O m represents the mth obstacle, and q represents the total number of obstacles.

5. The method for obstacle avoidance path planning of a multi-degree-of-freedom manipulator according to claim 1, characterized in that: A heuristic search algorithm is used to generate a preliminary global path based on the following method: A heuristic search algorithm is used to estimate the cost from the current node to the target node to select the optimal path. A specific valuation function is used to put the node's neighbor nodes into an open list starting from the starting point, calculate their total path cost, and select the next expanded node based on the minimum total path cost. The termination condition is that when the target node is expanded, the search ends and the preliminary global path is found. The formula is: f(j)=g(j)+h(j) h(j)=|x j -x goal |+|y j -y goal |+|z j -z goal | Among them, f(j) represents the total path cost from the starting point to the target node and passing through node j, g(j) represents the actual cost from the starting point to node j, h(j) represents the heuristic estimate from node j to the target node, that is, the estimated cost, expressed as Manhattan distance, x goal ,y goal 、z goal Indicates the horizontal coordinate, vertical coordinate, and vertical coordinate of the target node, x j ,y j 、z j Represents the horizontal coordinate, vertical coordinate, and vertical coordinate of path node j; Minimize the obstacle avoidance path length and maximize the safety distance, select the obstacle avoidance path with the least obstacles on the road, and meet the minimum value of this constraint condition. The formula is: L=f(j) min Where L represents the minimum length of the obstacle avoidance path, l k represents the kth point on the minimum length L of the obstacle avoidance path, d(l k , O m ) represents the kth point l on the minimum length L of the obstacle avoidance path k Minimum distance to all obstacles, d min It represents the minimum safety distance, and k represents the index of the point on the minimum length L of the obstacle avoidance path.

6. A multi-degree-of-freedom robot arm obstacle avoidance path planning method according to claim 5, characterized in that: The formula for calculating the velocity, acceleration and limit torque of the joint motion of the robot arm is: Where v(t) represents the velocity of the robot at time t, a(t) represents the acceleration of the robot at time t, I represents the moment of inertia, and θ i+1 -θ i represents the difference between the rotation angle of joint i+1 and the rotation angle of joint i in the path, τ max Indicates the ultimate torque of the robot arm.

7. A multi-degree-of-freedom robot arm obstacle avoidance path planning method according to claim 6, characterized in that: The maximum obstacle avoidance angle is generated based on the formula: t dynamic (t)=τ max +δ*v(t)+ε*a(t) K i =θ i +Dm i Among them, τ dynamic (t) represents the dynamic torque of the robot at time t, δ and ε represent the weights of the robot velocity and acceleration on the dynamic torque, respectively, and K max (t) represents the maximum obstacle angle of the robot at time t, K i represents the obstacle avoidance angle of joint i, θ i represents the rotation angle of joint i, Δμ i Indicates the angle that needs to be increased or decreased to ensure obstacle avoidance.

8. A multi-degree-of-freedom robot arm obstacle avoidance path planning method according to claim 7, characterized in that: The obstacle avoidance angle of each joint is compared with the maximum obstacle avoidance angle to screen out extreme joint angles and count their number. The logic is as follows: Among them, Q represents the logical value for judging whether each joint obstacle avoidance angle is an extreme joint angle. When Q=1, it means that the obstacle avoidance angle of joint i is an extreme joint angle; when Q=0, it means that the obstacle avoidance angle of joint i is not an extreme joint angle.

9. A multi-degree-of-freedom robot arm obstacle avoidance path planning method according to claim 8, characterized in that: According to the number of extreme joint angles, the redundant degrees of freedom are calculated and a redundant degrees of freedom threshold is set. If the redundant degrees of freedom exceed the threshold, the obstacle avoidance is successful and the path planning is valid. The logic is as follows: r = JN If r ≥ r max Obstacle avoidance is successful and path planning is effective If r <r max Insufficient redundant degrees of freedom, need to re-plan the path Among them, r represents the redundant degree of freedom, r max represents the redundant degree of freedom threshold, J represents the overall degree of freedom of the robot arm, and N represents the number of extreme joint angles.

10. A multi-degree-of-freedom robot arm obstacle avoidance path planning system, the path planning system is used to execute the multi-degree-of-freedom robot arm obstacle avoidance path planning method according to any one of claims 1 to 9, comprising: A workspace generation module, wherein the workspace generation module solves the transformation relationship of each joint of the robot arm and the three-dimensional coordinates of each joint based on the principle of forward kinematics, and generates a workspace of the robot arm according to the transformation relationship of each joint of the robot arm and the three-dimensional coordinates; An obstacle avoidance space establishment module, the obstacle avoidance space establishment module is used to model the volume of obstacles in the working space of the robot arm, generate an obstacle space corresponding to the working space of the robot arm, and establish the obstacle avoidance space of the robot arm; A path optimization module, wherein the path optimization module is used to generate a preliminary global path using a heuristic search algorithm in the established obstacle avoidance space, and optimize the preliminary global path, wherein the optimization includes minimizing the path length and maximizing the safety distance, and selecting an obstacle avoidance path with the least obstacles on the path; A dynamic performance analysis module, which is used to calculate the speed, acceleration and limit torque of the joint movement of the robot arm during movement, and to comprehensively generate a maximum obstacle avoidance angle using the speed, acceleration and limit torque of the robot arm; A redundant degree of freedom evaluation module is used to compare the obstacle avoidance angle of each joint with the maximum obstacle avoidance angle to screen out extreme joint angles and count their number. Based on the number of extreme joint angles, the redundant degrees of freedom are calculated and a redundant degree of freedom threshold is set. If the redundant degrees of freedom exceed the threshold, the obstacle avoidance is successful and the path planning is valid.

Images