Obstacle avoidance path planning method for explosion-proof humanoid dual-arm robot

Abstract

The present invention belongs to the field of robot obstacle avoidance technology, and specifically relates to an explosion-proof humanoid dual-arm robot obstacle avoidance path planning method, including: obtaining a three-dimensional model of a dual robotic arm; using a sphere to envelop the joints, connecting rods and environmental obstacles of the dual robotic arms; using an improved Bi-RRT algorithm to perform global path planning for the dual robotic arms respectively; judging whether the dual robotic arm global planning path collides, if so, replanning the path and determining the master arm and the slave arm, otherwise, executing the global planning path; judging whether the distance between the obstacle and the joint or connecting rod of any robotic arm is less than a set safety threshold, if the artificial potential field local obstacle avoidance mode is started, otherwise, repeating the above steps until the dual robotic arms complete the path planning respectively and reach the target point. The present invention can achieve obstacle avoidance of static and dynamic obstacles in the workspace, and at the same time solves the collision problem existing in the local obstacle avoidance process between the two arms.

Description

Technical Field

[0001] The invention belongs to the technical field of robot obstacle avoidance, and in particular relates to an obstacle avoidance path planning method for an explosion-proof humanoid dual-arm robot. Background Art

[0002] With the advent of the Industrial 4.0 era, robots entering factories to replace human operations is the current development trend. Humanoid robots can perform some repetitive and dangerous work. When humanoid robots use dual arms to work, collision-free path planning is the first problem that needs to be solved. Traditional path planning algorithms are more concerned with static obstacle avoidance of a single robotic arm, while dual-arm collaboration often uses master-slave planning, that is, the master and slave robotic arms are set manually, and the master arm considers static obstacles in the environment for obstacle avoidance planning. After obtaining the collision-free path of the master arm, the slave arm regards all postures planned by the master arm and obstacles in the environment as obstacles for obstacle avoidance. If a robotic arm is simply fixed as the master arm, planning failure may occur, and master-slave planning is not required in all cases; dynamic obstacles may appear when executing the master-slave planned path, and the robotic arm does not have a dynamic obstacle avoidance function, so there is a risk of collision; when using artificial potential fields for dual robotic arm obstacle avoidance, repulsion and attraction are calculated in Cartesian space, and inverse kinematics calculations need to be frequently performed when updating the position, which takes a lot of time, and the artificial potential field is prone to fall into local optimality, resulting in planning failure. Summary of the invention

[0003] Purpose of the invention: In view of the shortcomings of the prior art, the present invention proposes an obstacle avoidance path planning algorithm for dual robotic arms. On the basis of improving the master-slave strategy, the proposed improved Bi-RRT global planning algorithm can effectively improve the planning efficiency and reduce the calculation time. Combined with the improved artificial potential field local obstacle avoidance method, it can effectively avoid local optimal and frequent calculation of inverse kinematics problems, and can achieve obstacle avoidance of static and dynamic obstacles in the workspace, while solving the collision problem existing in the local obstacle avoidance process between the two arms.

[0004] Technical solution: The obstacle avoidance path planning method of the explosion-proof humanoid dual-arm robot of the present invention comprises:

[0005] Step 1: Obtain a three-dimensional model of the dual robotic arms and establish a forward and inverse kinematics model;

[0006] Step 2: Initialize the dual-arm workspace and set the initial joint angles of the dual-arms and the target joint angle , respectively calculate the positions of each joint and link of the dual manipulator in the world coordinate system at its target joint angle;

[0007] Step 3: Initialize the environmental obstacle information, and use a sphere to envelop the joints, connecting rods and environmental obstacles of the dual robotic arms based on the three-dimensional model and forward and inverse kinematics model;

[0008] Step 4: Use the improved Bi-RRT algorithm to perform global path planning for the dual robotic arms;

[0009] Step 5: Determine whether the dual robot arm global planning path collides. If so, execute step 6; otherwise, execute step 8.

[0010] Step 6: Replan the path and determine the master arm and slave arm. The setting conditions of the master arm and slave arm are: if the target position collides, the robot arm that collides is set as the master arm; if the target position does not collide, the robot arm with a shorter end moving distance is set as the master arm;

[0011] Step 7: The slave arm regards the master arm's planned path and static obstacles as static obstacles, and replans using the improved Bi-RRT algorithm;

[0012] Step 8: Use steps 1 to 7 to execute the global planning path;

[0013] Step 9: determine whether the distance between the obstacle and the joint or connecting rod of any robot arm is less than the set safety threshold. If so, start the artificial potential field local obstacle avoidance mode; otherwise, execute step 10;

[0014] Step 10: Repeat steps 4 to 9 until the two robotic arms complete the path planning and reach the target point.

[0015] To further improve the above technical solution, the positions of the joints and connecting rods of the dual robotic arms at their target joint angles in the world coordinate system in step 2 include: The position of the joint is , No. The connecting rod center is ; The other robot arm in the dual robot arm is at its target joint angle The position of the joint is , No. The connecting rod center is .

[0016] Furthermore, a sphere is used to envelop the joints and connecting rods of the robot arm, and the sphere radius is and .

[0017] Furthermore, the improved Bi-RRT algorithm in step 4 includes:

[0018] Step 4.1: Build two random search trees at the starting point and the target point respectively , ;

[0019] Step 4.2: In the random tree , Find the nodes closest to each other and set distance The nearest node is , distance The nearest node is , taking the midpoint of the line connecting the current node and the target point as the center of the circle and the Euclidean distance between the target point and the center of the circle as the radius to establish a random point exploration area, and the generated random sampling points are generated by the target gravity. Move towards the target point to form new random sampling points ;

[0020] Step 4.3: Find random trees Middle distance The nearest point , and randomly sample points Direction expansion to obtain new nodes ;

[0021] Step 4.4, judgment Along Whether there is a collision with an obstacle during the movement, if so, return to step 4.2, otherwise Add to Random Tree middle;

[0022] Step 4.5: In the random tree Find and Newly added nodes The closest node , and towards direction, if it can be expanded with Direct connection means that a collision-free path is planned. If a collision occurs with an obstacle during the expansion process, the expansion is stopped and the execution continues;

[0023] Step 4.6: Determine whether the number of iterations exceeds the maximum number of iterations. If so, it means that the collision-free path has not been successfully found. If it does not exceed the maximum number of iterations, then swap and , then skip to step 4.2.

[0024] Furthermore, the calculation formula of the random point exploration area is:

[0025]

[0026] Among them, W represents the global space, Region represents the actual exploration space, Represents the node closest to the target point in the random tree;

[0027] The calculation formula of the target gravity is:

[0028]

[0029] in, represents the gravitational coefficient, Indicates the target point;

[0030] The new node The calculation formula is:

[0031]

[0032] In the formula, represents a new random sampling point, Represents the distance in the random tree The nearest node, Indicates the size of the expansion step.

[0033] Furthermore, the robot arm is in the current joint angle The distance between a joint or link and the center of the obstacle is calculated as follows:

[0034]

[0035] In the formula, The current position of the robot arm Link or joint position coordinates; is the obstacle center coordinate, is the radius of the spherical obstacle, is the radius of the robot arm link or joint;

[0036] When an obstacle appears and the distance between any joint or link of the robot arm Less than the set safety threshold , the robot arm starts the artificial potential field local obstacle avoidance mode.

[0037] Furthermore, the artificial potential field local obstacle avoidance mode in step 9 includes:

[0038] Step 9.1, calculate the repulsive force field currently exerted on the robot arm;

[0039] Step 9.2, calculate the gravitational force of the target point on the robotic arm;

[0040] Step 9.3, update the robot arm joint angles.

[0041] Further, the step 9.1 comprises:

[0042] Calculate the repulsive force field of obstacles on each joint link of the current robot arm:

[0043]

[0044] In the formula, is the gain coefficient of the repulsive field, is the distance between the current position of each joint link of the robot and its target position, is the distance between the i-th joint or link of the robot and the center of the obstacle at the current joint angle, To set safety thresholds;

[0045] The calculation formula for calculating the distance between each joint and connecting rod of the dual robotic arm and the corresponding repulsive field is:

[0046]

[0047] In the formula, The distance between each joint link of the current robot arm and the joint link of another robot arm;

[0048] Then, the repulsive field currently exerted on the robot arm is for:

[0049]

[0050] The derivation of the repulsion function is:

[0051]

[0052] Calculate the Jacobian matrix of the robot Map the repulsive force to the joint space:

[0053] .

[0054] Furthermore, the calculation formula of the gravitational force of the target point on the robotic arm is:

[0055]

[0056] In the formula, and They are the current joint angle and target joint angle of the robot arm respectively;

[0057] Furthermore, the calculation formula for updating the joint angle of the robot arm is:

[0058]

[0059] In the formula, is the iteration step length, is the repulsive force mapped to the joint space through the robot Jacobian matrix J.

[0060] Beneficial effect: Compared with the prior art, the advantages of the present invention are: the present invention provides a dual-manipulator obstacle avoidance path planning method, improves the Bi-RRT algorithm, enhances its directionality and rapidity, and effectively improves the efficiency of global path planning; improves the master-slave strategy, increases the conditions for judging the master and slave, and reduces redundant calculations; introduces an improved artificial potential field method, converts the repulsive field in the Cartesian space into the repulsive force in the joint space, avoids frequent calculations of inverse kinematics, realizes dynamic obstacle avoidance in the joint space, improves the real-time performance of the algorithm, can avoid the occurrence of local optimal situations, effectively solves the collision problem caused by dynamic obstacles when executing the global planned path, and can avoid collisions between manipulators while avoiding local obstacles. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 It is a schematic diagram of the overall process of the present invention;

[0062] Figure 2 It is a specific flow chart of the improved Bi-RRT algorithm of the present invention;

[0063] Figure 3 It is a schematic diagram of the improved Bi-RRT path planning algorithm exploring random points in the target-guided exploration space;

[0064] Figure 4 It is a schematic diagram of the improved Bi-RRT path planning algorithm in forming new random points using target gravity;

[0065] Figure 5 It is a schematic diagram of the setting conditions of the master and slave arms;

[0066] Figure 6 yes Figure 5 A partial enlarged view of the boxed area;

[0067] Figure 7 It is a comparison of the Bi-RRT algorithm before and after the improvement of the present invention. DETAILED DESCRIPTION

[0068] The technical solution of the present invention is described in detail below with reference to the accompanying drawings, but the protection scope of the present invention is not limited to the embodiments.

[0069] Embodiment 1: The present invention proposes an obstacle avoidance path planning algorithm for dual robotic arms, such as Figure 1 As shown, the specific steps are as follows.

[0070] Step 1: Obtain the 3D model of the dual robotic arms and establish the forward and inverse kinematics model according to their joint types.

[0071] Step 2: Initialize the dual-arm workspace and give the initial joint angles of the dual-arm , target joint angle , calculate the position of each joint link of the dual manipulator in the world coordinate system at each target joint angle, and according to the kinematic model, the position of the first manipulator The position of each joint in the world coordinate system at its target joint angle is expressed as , the first robot arm The position of the center of each link in the world coordinate system is expressed as , the position of the connecting rod center is determined by the average value of the positions of the two adjacent joints, is the target state, which is the position or posture that the robot arm is expected to reach in the end. In motion planning, the target state is used to calculate the repulsive field, attractive field and obstacle avoidance strategy. The position of each joint in the world coordinate system at its target joint angle is expressed as , No. The position of the center of each link in the world coordinate system is expressed as .

[0072] Step 3: Initialize the environmental obstacle information. Based on the three-dimensional model and the forward and inverse kinematics model, a sphere is used to envelop the robot joints, connecting rods, and environmental obstacles for obstacle distance calculation and collision detection. The enveloping method is: the diameter of each joint, the length of the connecting rod, and the length of the obstacle are used as the sphere diameter to form a sphere. The sphere radius is and The two radii are the diameter of each joint of the robot arm and the length of the connecting rod, which can be directly obtained through physical measurement or three-dimensional model.

[0073] Step 4: The two robotic arms improve the Bi-RRT path planning algorithm separately for global planning. The specific process of improving the Bi-RRT algorithm is as follows: Figure 2 As shown, the specific steps are as follows:

[0074] Step 4.1: Build two random search trees at the starting point and the target point respectively , ;

[0075] Step 4.2: In the random tree , Find the nodes closest to each other and set distance The nearest node is , distance The nearest node is of . A random point exploration area is established with the midpoint of the line connecting the current node and the target point as the center and the Euclidean distance between the target point and the center as the radius, so that the exploration random sampling points are always within the area, reducing the exploration of useless nodes and improving the algorithm convergence speed. At the same time, the algorithm's reverse exploration capability is retained to prevent the algorithm from falling into a local minimum value, such as Figure 3 In order to improve the search efficiency and accelerate the convergence process of the algorithm, the generated random sampling points are Move towards the target point to form new random sampling points ,like Figure 4 shown.

[0076] The target guided search space calculation formula is as follows:

[0077]

[0078] In the formula, represents the global space, represents the actual exploration space, Represents the node closest to the target point in the random tree.

[0079] The target gravity calculation formula is as follows:

[0080]

[0081] In the formula, represents the gravitational coefficient, Indicates the target point.

[0082] Step 4.3: Finding random trees Middle distance The nearest point , and randomly sample points Direction expansion to obtain new nodes ,

[0083]

[0084] In the formula, represents a new random sampling point, Represents the distance in the random tree The nearest node, Indicates the size of the expansion step.

[0085] Step 4.4: Determine the closest point Along the new node Whether there is a collision with an obstacle during the movement, if so, return to step 4.2, otherwise Add to Random Tree and continue to execute downward;

[0086] Step 4.5: In the random tree Find and Newly added nodes The closest node , and towards direction, if it can be expanded with Direct connection means that a collision-free path is planned. If a collision occurs with an obstacle during the expansion process, the expansion will be stopped and the execution will continue;

[0087] Step 4.6: Determine whether the number of iterations exceeds the maximum number of iterations. If so, it means that the collision-free path has not been successfully found. If it does not exceed the maximum number of iterations, then swap and , then jump to step 4.2 to execute the algorithm.

[0088] Step 5: Determine whether the two robot arms' separate global planning paths collide. If so, proceed to the next step; otherwise, jump to step 8.

[0089] Step 6: Since both arms are planned separately using the improved Bi-RRT algorithm, there is randomness. If a collision occurs, planning failure may occur without further judgment of the master-slave condition: When the target posture of one of the robotic arms collides with the planned path of the other robotic arm, such as Figure 5 , Figure 6 As shown in the figure, if the blue track robot arm is set as the main arm and the red track robot arm is set as the slave arm for obstacle avoidance, the slave arm cannot generate a collision-free path because the planned path of the main arm collides with the target position of the slave arm. Therefore, it is necessary to set the robot arm that collides at the target position as the main arm, that is, the red track robot arm is the main arm; the blue track robot arm is set as the slave arm for replanning; if the planned path of the two arms does not collide at the target position of the two arms, the robot arm with the shorter end moving distance is specified as the main arm.

[0090] Step 7: The slave arm regards the master arm’s planned path and the static obstacle as static obstacles, and replans using the improved Bi-RRT algorithm to generate another collision-free path.

[0091] Step 8: Execute the global planning path, using the global operation of steps 1-7, which generally refers to the global logic execution from steps 1-7.

[0092] Step 9: Use the forward and inverse solution kinematic model and collision detection envelope model of the dual manipulator:

[0093] For the robot arm at the current joint angle The distance between a joint or link and the center of the obstacle:

[0094]

[0095] In the formula, The current position of the robot arm Link or joint position coordinates, is the obstacle center coordinate, is the radius of the spherical obstacle, is the radius of the robot link or joint.

[0096] When an obstacle appears and the distance between any joint or link of the robot arm Less than the set safety threshold , the robot arm starts the artificial potential field local obstacle avoidance mode, and the specific steps are as follows.

[0097] Step 9.1, calculate the repulsive force field of the obstacle on each joint link of the current robot arm,

[0098]

[0099] In the formula, is the gain coefficient of the repulsive field, is the distance between the current position of each joint link of the robot and its target position, As a regulating factor, it avoids the situation where the repulsive force is not zero when approaching the target position, resulting in the target being unreachable and the local optimum. It represents the nonlinear contribution of the obstacle distance. The smaller the distance, the greater the potential energy of the repulsive field and the stronger the force.

[0100] In order to avoid the problem of collision between the robotic arms when the robotic arms leave the global planning path during local obstacle avoidance, it is necessary to regard the other robotic arm as an obstacle, calculate the distance between each joint and connecting rod of the two robotic arms, and calculate the corresponding repulsive field ,

[0101]

[0102] In the formula, is the distance between each joint link of the current robot arm and the joint link of another robot arm. The calculation formula is as follows:

[0103]

[0104] In the formula, and They are the current robot arm The joint of the other robot arm joints, and is the sphere envelope radius of the robot joint.

[0105] The repulsive force field currently exerted on the robot arm is for:

[0106]

[0107] The derivation of the repulsion function is:

[0108]

[0109] because To calculate the repulsive force of each joint in Cartesian space, the Jacobian matrix of the robot arm needs to be calculated , mapping the repulsive force to the joint space:

[0110]

[0111] Step 9.2: Calculate the gravitational force of the target point on the robot arm. The calculation formula is as follows:

[0112]

[0113] In the formula, and are the current joint angle and target joint angle of the robot arm respectively.

[0114] Step 9.3, update the joint angle of the robot arm;

[0115]

[0116] In the formula, is the iteration step length.

[0117] Step 10: When the robot arm leaves the range of the obstacle repulsion force, repeat steps 4 to 9 until the robot arm reaches the target point.

[0118] Comparison of Bi-RRT algorithms before and after improvement, using master-slave strategies for planning, such as Figure 7 As shown, the left side is the dual-arm planning path before improvement, and the right side is the planning path after improvement.

[0119] As described above, although the present invention has been shown and described with reference to specific preferred embodiments, it should not be construed as limiting the present invention itself. Various changes in form and details may be made without departing from the spirit and scope of the present invention as defined in the appended claims.

Claims

1. A method for obstacle avoidance path planning of an explosion-proof humanoid dual-arm robot, characterized in that: include: Step 1: Obtain a three-dimensional model of the dual robotic arms and establish a forward and inverse kinematics model; Step 2: Initialize the dual-arm workspace and set the initial joint angles of the dual-arms and the target joint angle , respectively calculate the positions of each joint and link of the dual manipulator in the world coordinate system at its target joint angle; Step 3: Initialize the environmental obstacle information, and use a sphere to envelop the joints, connecting rods and environmental obstacles of the dual robotic arms based on the three-dimensional model and forward and inverse kinematics model; Step 4: Use the improved Bi-RRT algorithm to perform global path planning for the dual robotic arms; The improved Bi-RRT algorithm includes: Step 4.1: Build two random search trees at the starting point and the target point respectively , ; Step 4.2: In the random tree , Find the nodes closest to each other and set distance The nearest node is , distance The nearest node is , taking the midpoint of the line connecting the current node and the target point as the center of the circle and the Euclidean distance between the target point and the center of the circle as the radius to establish a random point exploration area, and the generated random sampling points are generated by the target gravity. Move towards the target point to form new random sampling points ; Step 4.3: Find random trees Middle distance The nearest point , and randomly sample points Direction expansion to obtain new nodes ; Step 4.4, judgment Along Whether there is a collision with an obstacle during the movement, if so, return to step 4.2, otherwise Add to Random Tree middle; Step 4.5: In the random tree Find and Newly added nodes The closest node , and towards If we can expand in the direction of Direct connection means that a collision-free path is planned. If a collision occurs with an obstacle during the expansion process, the expansion is stopped and the execution continues; Step 4.6: Determine whether the number of iterations exceeds the maximum number of iterations. If so, it means that the collision-free path has not been successfully found. If it does not exceed the maximum number of iterations, then swap and , then jump to step 4.2; The calculation formula of the random point exploration area is: ； Among them, W represents the global space, Region represents the actual exploration space, Represents the node closest to the target point in the random tree; The calculation formula of the target gravity is: ； in, represents the gravitational coefficient, Indicates the target point; The new node The calculation formula is: ； In the formula, represents a new random sampling point, Represents the distance in the random tree The nearest node, Indicates the size of the expansion step; Step 5: Determine whether the dual robot arm global planning path collides. If so, execute step 6; otherwise, execute step 8. Step 6: Replan the path and determine the master arm and slave arm. The setting conditions of the master arm and slave arm are: if the target position collides, the robot arm that collides is set as the master arm; if the target position does not collide, the robot arm with a shorter end moving distance is set as the master arm; Step 7: The slave arm regards the master arm's planned path and static obstacles as static obstacles, and replans using the improved Bi-RRT algorithm; Step 8: Use steps 1 to 7 to execute the global planning path; Step 9: determine whether the distance between the obstacle and the joint or connecting rod of any robot arm is less than the set safety threshold. If so, start the artificial potential field local obstacle avoidance mode; otherwise, execute step 10; Step 10: Repeat steps 4 to 9 until the two robotic arms complete the path planning and reach the target point.

2. The obstacle avoidance path planning method for an explosion-proof humanoid dual-arm robot according to claim 1, characterized in that: In step 2, the positions of the joints and connecting rods of the dual robotic arms at their target joint angles in the world coordinate system include: The position of the joint is , No. The connecting rod center is ; The other robot arm in the dual robot arm is at its target joint angle The position of the joint is , No. The connecting rod center is ; is the target state.

3. The obstacle avoidance path planning method for an explosion-proof humanoid dual-arm robot according to claim 1, characterized in that: The robot arm joints and connecting rods are enclosed by a sphere, and the sphere radius is and , are the joint diameters and connecting rod lengths of the robot arm respectively.

4. The obstacle avoidance path planning method for an explosion-proof humanoid dual-arm robot according to claim 1, characterized in that: The robot arm is at the current joint angle. The distance between a joint or link and the center of the obstacle is calculated as follows: ； In the formula, The current position of the robot arm Link or joint position coordinates; is the obstacle center coordinate, is the radius of the spherical obstacle, is the radius of the robot arm link or joint; When an obstacle appears and the distance between any joint or link of the robot arm Less than the set safety threshold , the robot arm starts the artificial potential field local obstacle avoidance mode.

5. The obstacle avoidance path planning method for an explosion-proof humanoid dual-arm robot according to claim 4, characterized in that: The artificial potential field local obstacle avoidance mode in step 9 includes: Step 9.1, calculate the repulsive force field currently exerted on the robot arm; Step 9.2, calculate the gravitational force of the target point on the robotic arm; Step 9.3, update the robot arm joint angles.

6. The obstacle avoidance path planning method for an explosion-proof humanoid dual-arm robot according to claim 5, characterized in that: The step 9.1 comprises: Calculate the repulsive force field of obstacles on each joint link of the current robot arm: ； In the formula, is the gain coefficient of the repulsive field, is the distance between the current position of each joint link of the robot and its target position, is the distance between the i-th joint or link of the robot and the center of the obstacle at the current joint angle, To set safety thresholds; The calculation formula for calculating the distance between each joint and connecting rod of the dual robotic arm and the corresponding repulsive field is: ； In the formula, The distance between each joint link of the current robot arm and the joint link of another robot arm; Then, the repulsive field currently exerted on the robot arm is for: ； The derivation of the repulsion function is: ； Calculate the Jacobian matrix of the robot Map the repulsive force to the joint space: 。 7. The obstacle avoidance path planning method for an explosion-proof humanoid dual-arm robot according to claim 6, characterized in that: The calculation formula of the gravitational force of the target point on the robotic arm is: ； In the formula, and are the current joint angle and target joint angle of the robot arm respectively.

8. The obstacle avoidance path planning method for an explosion-proof humanoid dual-arm robot according to claim 7, characterized in that: The calculation formula for updating the robot arm joint angle is: ； In the formula, is the iteration step length, is the repulsive force mapped to the joint space through the robot Jacobian matrix J.

Images