CN117021101A - Multi-arm path planning method for belt conveyor dismounting robot - Google Patents

Abstract

The invention discloses a multi-arm path planning method for a belt conveyor disassembly robot, which comprises the following steps: the method is used for solving the path planning problem of the arm of the belt conveyor dismantling robot in the dismantling task, and comprises the steps of establishing a kinematic model of the belt conveyor dismantling task; the traditional RRT algorithm is improved, a node weight function is introduced in the node updating process of the traditional RRT algorithm to guide the generation of new nodes in the exploration process, the obstacle avoidance capability in the path planning process and the exploration non-directionality are improved, and invalid sampling points are effectively reduced; according to the method, the mechanical arm joint angle group in the joint space is used for representing the mechanical arm position information, and the mechanical arm joint angle group is applied to the RRT algorithm after the improvement is introduced, so that complicated operation of inverse kinematics solution is avoided, and the planning efficiency is improved; and finally, an active and passive dual-tree expansion method is provided, the active exploration is carried out on the main mechanical arm by using a path planning algorithm, the passive verification is carried out on the auxiliary mechanical arm, the exploration process and the verification process are carried out simultaneously, and the multi-arm cooperative motion is realized.

Description

A multi-arm path planning method for belt conveyor disassembly robots

Technical field

The invention relates to a multi-arm path planning algorithm for belt conveyor disassembly tasks, is mainly used in path planning of multi-manipulators, and belongs to the field of robotic technology.

Background technique

The belt conveyor is an important transportation equipment in underground coal mines. It needs to be disassembled at any time as the mining surface advances, the working surface withdraws and its own wear and tear increases. Existing disassembly mainly relies on manual work, which has problems such as difficulty, low efficiency, and prone to danger. Research and development of intelligent disassembly equipment for belt conveyors is of great significance to improving the safety and efficiency of underground production; currently some lifting machinery Equipment, multi-degree-of-freedom robotic arms, etc. can all perform disassembly operations. However, for larger and heavier objects such as belt conveyors, dual robotic arm operations have more advantages than single robotic arms, but there are attendant consequences. The main reason is the difficulty of robot arm path planning due to task complexity, motion coupling, and obstacle avoidance of multiple robot arms. Therefore, the path planning problem of multi-robot arms has become an important research issue for scholars. Therefore, it is of great significance to conduct path planning research on multi-robot arms.

Path planning is a key technology to ensure the safe operation of robotic arms. At present, domestic and foreign scholars have proposed many path planning algorithms such as artificial potential field method, ant colony algorithm, A* algorithm, genetic algorithm and RRT algorithm. Among them, the RRT algorithm is an efficient, flexible and suitable path planning algorithm for various scenarios. Due to its advantages of fast exploration, simple implementation, strong adaptability, suitability for complex environments and strong scalability, it is convenient for the robot arm to operate in various scenarios. Path planning under high-dimensional space and complex constraints has been widely used in the field of robot path planning in recent years. However, the traditional RRT algorithm also has inherent flaws. Its global random sampling will lead to a waste of computing resources, the algorithm converges slowly, and the generated path is not smooth, making it difficult to be directly executed by the robot. Therefore, improving the traditional RRT algorithm is of great significance to the research of robot path planning. .

Contents of the invention

Aiming at the path planning problem of the belt conveyor disassembly robot's arms in the disassembly task, the present invention proposes a multi-arm path planning algorithm, which is improved on the traditional RRT algorithm and introduces node weights in the node update process of the traditional RRT algorithm. function to guide the generation of new nodes during the exploration process, which can improve the obstacle avoidance ability and non-directional exploration in the path planning process, and effectively reduce invalid sampling points; the present invention uses the robot arm joint angle group in the joint space to represent the robot arm position Information is introduced into the improved RRT algorithm, which avoids the cumbersome calculations of inverse kinematics solution and improves planning efficiency. Finally, an active and passive dual-tree expansion method is proposed. The main manipulator uses a path planning algorithm to actively explore, from The mechanical arm is used for passive verification, and the exploration process and verification process are carried out simultaneously, realizing multi-arm coordinated movement. See description below for details:

A multi-arm path planning method for a belt conveyor disassembly robot, including the following steps:

Step 1. Establish a kinematic model for the belt conveyor disassembly task;

Step 2. Initialize the initial pose and target pose of the robotic arm, and determine the basic information about obstacles in the environment;

Step 3. Based on the traditional RRT algorithm, use the Sobol sequence to generate sampling points, and introduce the node weight function to expand new nodes;

Step 4. Introduce active and passive expansion, and achieve coordinated planning of multiple robot arms through master robot arm sampling and slave robot arm verification;

Step 5. Use the polynomial fitting method based on the least squares method to optimize the path to make the robot arm movement smoother.

Further, step 2 specifically includes the following content:

An active-passive extended RRT algorithm is proposed to explore the path planning of dual manipulators. The master manipulator R uses an active growing random tree T _R , and the slave manipulator L uses a passive growing random tree T _{L , and randomly grows a random tree T L} in the joint space. The tree is expanded to define the initial joint angle of the main robotic arm as x _start , the target position as x _goal , the initial joint angle of the slave robotic arm as x′ _start , and the target position as x′ _goal , and use them as the starting point and target point. Add to random trees T _R and _TL respectively.

Further, step 3 specifically includes the following content:

Multi-arm joint space path planning usually involves higher dimensions. Because the dimensions are too high, the calculation complexity increases, which makes it difficult for the manipulator to complete path planning within the specified time; although RRT sampling in their respective joint spaces can complete the path Planning, but coordination between robotic arms cannot be achieved; the specific steps in step 3 include:

S31. Compared with the pseudo-random sequence, the Sobol sequence can make the distribution of sampling points more uniform while ensuring randomness. The sampling point x _sample is defined in the joint space, consisting of the joint angles of the robotic arm, and is generated by the Sobol sequence. The node with the smallest Euclidean distance from the sampling point in the random tree is regarded as the nearest point, and the point is recorded as x _nearest ;

S32. Introduce the node weight function, and use this function to adaptively adjust the expansion weight in the direction of the random sampling point and the target point; in the absence of obstacles, give the target point a higher weight to guide the random tree towards the target point. direction expansion, and in the case of obstacles, random sampling points are given a higher weight,

Guide the random tree to bypass obstacles; the expanded formula of the new sampling point is:

in, is the weight of the direction of the sampling point, determined by the following formula,

The initial value of m is set to 0, the initial value of n is set to 1, k ₁ ∈ (0,1) reflects the change of the direction weight of the sampling point, k ₂ ∈ (0,1) represents the weight of the target point direction, when the environment is complex and obstacles When the number is large, the value of k ₂ should be smaller, otherwise it should be larger.

Further, step 4 specifically includes the following content:

S41. Substitute the update node x _new of the random tree in the joint space into the forward kinematics solution of the main manipulator R to obtain the node information of the Cartesian coordinate system;

S42. The obstacles encountered by the dismantling robot arm in the work space are mainly connecting rod brackets and crushed coal materials. The rectangular envelope box method and the spherical envelope box method are used for collision detection respectively;

S43. Determine whether each joint of the robotic arm collides with an obstacle. If not, add x _new to the actively growing random tree, and define x _nearest as the parent node of x _new ; if so, delete the node and return to reselect the node. ;

S44. Substitute x′ _new in the passively grown random tree corresponding to the update node x _new in the actively growing random tree into the forward kinematics solution of the mechanical arm L;

S45. Use the envelope box method to perform collision detection to determine whether each joint of the robotic arm collides with an obstacle. If not, add x′ _new to the random tree T _L , and define x′ _nearest as x′ _new Parent node; if so, remove x _new from _TR , remove x′ _new from T _L , and return to reselect the node.

The beneficial effects of the technical solution provided by the present invention are:

1) Improvements have been made to the traditional RRT algorithm. In the node update process of the traditional RRT algorithm, the node weight function is introduced to guide the generation of new nodes during the exploration process, which can improve the obstacle avoidance ability and non-directional exploration during the path planning process, and is effective Reduce invalid sampling points;

2) The present invention uses the mechanical arm joint angle group in the joint space to represent the mechanical arm position information, and applies it to the improved RRT algorithm, which avoids the cumbersome calculation of inverse kinematics solution and improves planning efficiency;

3) An active and passive dual-tree expansion method is proposed. The main robotic arm uses a path planning algorithm to actively explore, and the slave robotic arm performs passive verification. The exploration process and verification process are carried out simultaneously, achieving coordinated movement of multiple robotic arms.

Description of the drawings

Figure 1 is a framework diagram of the multi-arm path planning method for a belt conveyor disassembly robot in the present invention;

Figure 2 is a schematic diagram of the new node generation process provided by the present invention;

Figure 3 is a schematic diagram of the active and passive dual tree expansion sampling process provided by the present invention;

Figure 4 is a schematic diagram of the belt conveyor disassembly robot model used in the present invention;

Figure 5 is a schematic diagram of the end path result of the robotic arm in the longitudinal beam process in the embodiment;

Figure 6 is a graph showing changes in the angles of each joint of the robotic arm R during the longitudinal beam process in the embodiment;

Figure 7 is a graph showing changes in the angles of each joint of the robotic arm L during the longitudinal beam process in the embodiment;

Figure 8 is a diagram showing the displacement change results of the end of the robotic arm R in the longitudinal beam process in the embodiment;

Figure 9 is a diagram showing the displacement change results of the end of the robotic arm L during the longitudinal beam process in the embodiment;

Figure 10 is a schematic diagram of the end path result of the robotic arm in the H frame process in the embodiment;

Figure 11 is a graph showing changes in angles of each joint of the H-frame process robot arm R in the embodiment;

Figure 12 is a graph showing changes in angles of each joint of the robotic arm L in the H-frame process in the embodiment;

Figure 13 is a diagram showing the displacement change results of the end of the robotic arm R in the H frame process in the embodiment;

Figure 14 is a diagram showing the displacement change results of the end of the robot arm L in the H frame process in the embodiment.

Detailed ways

In order to make the purpose, technical solutions and advantages of the present invention clearer, the present invention will be described in detail below with reference to the accompanying drawings.

A multi-arm path planning method for belt conveyor disassembly robots. The algorithm framework is shown in Figure 1. The specific methods include:

Step 1. Establish a kinematics model for the belt conveyor disassembly robot shown in Figure 4;

Step 2. Initialize the initial pose and target pose of the robotic arm, and determine the basic information about obstacles in the environment;

Step 3. Based on the traditional RRT algorithm, use the Sobol sequence to generate sampling points, and introduce the node weight function to expand new nodes. The new node generation process is shown in Figure 2;

Step 4. Introduce active and passive expansion, and achieve coordinated planning of multiple robot arms through master robotic arm sampling and slave robotic arm verification. The active and passive dual-tree expansion sampling process is shown in Figure 3;

Step 5. Use the polynomial fitting method based on the least squares method to optimize the path to make the robot arm movement smoother.

Step 2 specifically includes the following:

An active-passive extended RRT algorithm is proposed to explore the path planning of dual manipulators. The master manipulator R uses an active growing random tree T _R , and the slave manipulator L uses a passive growing random tree T _{L , and randomly grows a random tree T L} in the joint space. The tree is expanded to define the initial joint angle of the main robotic arm as x _start , the target position as x _goal , the initial joint angle of the slave robotic arm as x′ _start , and the target position as x′ _goal , and use them as the starting point and target point. Add to random trees T _R and _TL respectively.

Step 3 specifically includes the following:

Multi-arm joint space path planning usually involves higher dimensions. Because the dimensions are too high, the calculation complexity increases, which makes it difficult for the manipulator to complete path planning within the specified time; although RRT sampling in their respective joint spaces can complete the path Planning, but coordination between robotic arms cannot be achieved; the specific steps in step 3 include:

S31. Compared with the pseudo-random sequence, the Sobol sequence can make the distribution of sampling points more uniform while ensuring randomness. The sampling point x _sample is defined in the joint space, consisting of the joint angles of the robotic arm, and is generated by the Sobol sequence. The node with the smallest Euclidean distance from the sampling point in the random tree is regarded as the nearest point, and the point is recorded as x _nearest ;

S32. Introduce the node weight function, and use this function to adaptively adjust the expansion weight in the direction of the random sampling point and the target point; in the absence of obstacles, give the target point a higher weight to guide the random tree towards the target point. direction expansion, and in the case of obstacles, random sampling points are given higher weights to guide the random tree to bypass obstacles; the expansion formula of new sampling points is:

in, is the weight of the direction of the sampling point, determined by the following formula,

The initial value of m is set to 0, the initial value of n is set to 1, k ₁ ∈ (0,1) reflects the change of the direction weight of the sampling point, k ₂ ∈ (0,1) represents the weight of the target point direction, when the environment is complex and obstacles When the number is large, the value of k ₂ should be smaller, otherwise it should be larger.

Step 4 specifically includes the following:

S41. Substitute the update node x _new of the random tree in the joint space into the forward kinematics solution of the main manipulator R to obtain the node information of the Cartesian coordinate system;

S42. The obstacles encountered by the dismantling robot arm in the work space are mainly connecting rod brackets and crushed coal materials. The rectangular envelope box method and the spherical envelope box method are used for collision detection respectively;

S43. Determine whether each joint of the robotic arm collides with an obstacle. If not, add x _new to the actively growing random tree, and define x _nearest as the parent node of x _new ; if so, delete the node and return to reselect the node. ;

S44. Substitute x′ _new in the passively grown random tree corresponding to the update node x _new in the actively growing random tree into the forward kinematics solution of the mechanical arm L;

S45. Use the envelope box method to perform collision detection to determine whether each joint of the robotic arm collides with an obstacle. If not, add x′ _new to the random tree T _L , and define x′ _nearest as x′ _new Parent node; if so, remove x _new from _TR , remove x′ _new from T _L , and return to reselect the node.

The method of the invention is aimed at the disassembly task of the belt conveyor and belongs to the field of robots. In order to enable those skilled in the art to better understand the present invention, the multi-arm path planning algorithm of the belt conveyor disassembly robot will be described in detail below with reference to specific embodiments.

Take the longitudinal beam disassembly task as an example. The longitudinal beam disassembly process is completed by two robotic arms on the same side. Take the robotic arm ① and robotic arm ② in Figure 4 as an example to simulate. The robotic arm ② is defined as the main arm, denoted as R. Define robotic arm ① as the slave arm, denoted as L. The active and passive extended RRT algorithm is used to simulate the longitudinal beam process, and the end path of the robotic arm is shown in Figure 5. The joint angles of the robotic arms are shown in Figures 6 and 7. The displacements of the two robotic arms in the x, y, and z directions are as follows As shown in Figure 8 and Figure 9, the angles of the joints of the two robotic arms change the same. The end paths of the robotic arms are the same in the x and z directions, and a distance of 1.4m is maintained in the y direction. The robotic arms can complete the disassembly and placement of the longitudinal beams. The work verified the feasibility of the invention.

The disassembly process of the H frame is completed by the two robotic arms on the opposite side. This section takes the robotic arm ② and the robotic arm ④ as examples for simulation. The robotic arm ② is defined as the master arm, denoted by R, and the robotic arm ④ is defined as the slave arm, denoted by R. Make L. The active and passive extended RRT algorithm is used to simulate the H-frame process, and the end path of the robotic arm is shown in Figure 10. The joint angle of the robotic arm is shown in Figures 11 and 12. The displacements of the two robotic arms in the x, y, and z directions are as follows As shown in Figure 13 and Figure 14, the joint angles of the two robotic arms change in opposite directions. The obtained end paths of the robotic arms are the same in the y and z directions and are symmetrical about x=0 in the x direction. The robotic arms can complete the work of knocking down the H frame. Verification The feasibility of the present invention.

The above are only preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention shall be included in the protection of the present invention. within the range.

Claims (4)

1. A multi-arm path planning method for a belt conveyor disassembly robot, characterized by including the following steps: Step 1. Establish a kinematic model for the belt conveyor disassembly task; Step 2. Initialize the initial pose and target pose of the robotic arm, and determine the basic information about obstacles in the environment; Step 3. Based on the traditional RRT algorithm, use the Sobol sequence to generate sampling points, and introduce the node weight function to expand new nodes; Step 4. Introduce active and passive expansion, and achieve coordinated planning of multiple robot arms through master robot arm sampling and slave robot arm verification; Step 5. Use the polynomial fitting method based on the least squares method to optimize the path to make the robot arm movement smoother. 2. A multi-arm path planning method for a belt conveyor disassembly robot according to claim 1, characterized in that step 2 specifically includes: An active-passive extended RRT algorithm is proposed to explore the path planning of dual manipulators. The master manipulator R uses an active growing random tree T _R , and the slave manipulator L uses a passive growing random tree T _{L , and randomly grows a random tree T L} in the joint space. The tree is expanded, and the initial joint angle of the master robot arm is x _start , the target position is x _goal , the initial joint angle of the slave robot arm is x _s ′ _tart , and the target position is x _g ′ _oal , and is used as the starting point and The target points are added to the random trees _TR and _TL respectively. 3. A multi-arm path planning method for a belt conveyor disassembly robot according to claim 2, characterized in that step 3 specifically includes: S31. Compared with the pseudo-random sequence, the Sobol sequence can make the distribution of sampling points more uniform while ensuring randomness. The sampling point x _sample is defined in the joint space, consisting of the joint angles of the robotic arm, and is generated by the Sobol sequence. The node with the smallest Euclidean distance from the sampling point in the random tree is regarded as the nearest point, and the point is recorded as x _nearest ; S32. Introduce the node weight function, and use this function to adaptively adjust the expansion weight in the direction of the random sampling point and the target point; in the absence of obstacles, give the target point a higher weight to guide the random tree towards the target point. direction expansion, and in the case of obstacles, random sampling points are given higher weights to guide the random tree to bypass obstacles; the expansion formula of new sampling points is: in, is the weight of the direction of the sampling point, determined by the following formula, The initial value of m is set to 0, the initial value of n is set to 1, k ₁ ∈ (0,1) reflects the change of the direction weight of the sampling point, k ₂ ∈ (0,1) represents the weight of the target point direction, when the environment is complex and obstacles When the number is large, the value of k ₂ should be smaller, otherwise it should be larger. 4. A multi-arm path planning method for a belt conveyor disassembly robot according to claim 3, characterized in that the step 4 method is as follows: S41. Substitute the update node x _new of the random tree in the joint space into the forward kinematics solution of the main manipulator R to obtain the node information of the Cartesian coordinate system; S42. The obstacles encountered by the dismantling robot arm in the work space are mainly connecting rod brackets and crushed coal materials. The rectangular envelope box method and the spherical envelope box method are used for collision detection respectively; S43. Determine whether each joint of the robotic arm collides with an obstacle. If not, add x _new to the actively growing random tree, and define x _nearest as the parent node of x _new ; if so, delete the node and return to reselect the node. ; S44. Substitute x _n ′ _ew in the passive growth random tree corresponding to the update node x _new in the active growth random tree into the forward kinematics solution of the mechanical arm L; S45. Use the envelope box method to perform collision detection to determine whether each joint of the robotic arm collides with an obstacle. If not, add x _n ′ _ew to the random tree T _L and define x _n ′ _earest as x _n The parent node of ′ _ew ; if so, remove x _new from _TR , remove x _n ′ _ew from _TL , and return to reselect the node.