Mechanical arm transfer path planning method based on multi-station obstacle avoidance graph and inverse kinematics

Abstract

The invention discloses a mechanical arm transfer path planning method based on a multi-station obstacle avoidance diagram and inverse kinematics, which comprises the following steps of: establishing a three-dimensional space model of a machine tool processing mechanism, a reversing mechanism and a charging tray and coordinate data of an environmental obstacle, and constructing an obstacle avoidance path diagram containing an edge weight by adopting an A * algorithm; calculating a minimum cost path set from the slot position of the charging tray to the reversing mechanism and the machine tool processing mechanism, and generating and storing a path library; constructing a six-degree-of-freedom forward kinematics model of the mechanical arm, determining D-H parameters, calling an inverse kinematics iterative algorithm, solving a corresponding joint angle group for node poses in the path library, and generating a joint track; and acquiring a full-load state of the charging tray, a ready state of the reversing mechanism and a machining completion state of the machine tool, and generating a task sequence according to a preset task priority. According to the method, multi-station task switching is achieved, and meanwhile the alignment precision and path planning efficiency of the clamping jaw at the free end of the mechanical arm are improved through joint optimization of an obstacle avoidance graph and inverse kinematics.

Description

Technical Field

[0001] This invention relates to the field of robotic arm transfer technology, and in particular to a robotic arm transfer path planning method based on multi-station obstacle avoidance diagrams and inverse kinematics. Background Technology

[0002] In the production and processing of propellant grains, robotic arms are widely used for transferring propellant grains between loading trays, reversing mechanisms, and machine tool processing mechanisms. However, existing robotic arm transfer systems still face significant technical bottlenecks in complex scenarios involving multiple stations, multiple paths, and multiple tasks running concurrently. Traditional path planning methods often employ fixed trajectories or simple obstacle avoidance strategies, lacking global optimization capabilities for multi-station environments. When the layout of the loading tray slots, reversing mechanisms, and machine tool processing stations is complex, path conflicts can easily occur during the robotic arm's propellant grain handling operations, leading to waiting, congestion, or even misplacement of the loading tray, thereby reducing overall production efficiency. Summary of the Invention

[0003] This invention aims to at least partially solve one of the technical problems in related technologies. To this end, one objective of this invention is to propose a robotic arm transfer path planning method based on multi-station obstacle avoidance maps and inverse kinematics. This method enables multi-station task switching while improving the alignment accuracy and path planning efficiency of the robotic arm's free-end gripper through joint optimization of obstacle avoidance maps and inverse kinematics.

[0004] Firstly, this invention proposes a robotic arm transport path planning method based on multi-station obstacle avoidance maps and inverse kinematics, the steps of which are as follows:

[0005] S1: Establish a three-dimensional spatial model of the machine tool processing mechanism, reversing mechanism and charging tray, as well as the coordinate data of environmental obstacles, generate a multi-station topology map, and use the A* algorithm to construct an obstacle avoidance path map containing edge weights;

[0006] S2: Using the obstacle avoidance path map obtained in step S1, calculate the minimum cost path set from the slot of the charging tray to the reversing mechanism and the machine tool processing mechanism, and generate and store the path library;

[0007] S3: Construct a six-DOF forward kinematics model of the robotic arm, determine the DH parameters, call the inverse kinematics iterative algorithm, calculate the corresponding joint angle group for the node poses in the path library obtained in step S2, and generate the joint trajectory.

[0008] S4: Obtain the full load status of the charging tray, the ready status of the reversing mechanism, and the completed processing status of the machine tool. Generate a task sequence according to the preset task priority, and send the joint trajectory obtained in step S3 to the controller of the robotic arm according to the task sequence.

[0009] Preferably, in step S1:

[0010] S11: In the world coordinate system O-XYZ, measure the three-dimensional coordinates (x, y, z) of the slots of N1 charge trays, the positions of N2 reversing mechanisms, and the positions of N3 machine tool processing mechanisms. i ,y i ,z i ), where i∈[1,N1+N2+N3];

[0011] S12: Construct a minimum bounding box for M static obstacles and record the center coordinates (c kx ,c ky ,c kz ) and volume V k , k∈[1,M];

[0012] S13: The center of each slot in the loading tray, each station of the reversing mechanism, and each station of the machine tool processing mechanism are the graph nodes v. i This will satisfy the Euclidean distance d ij ≤d max node v i v j Connect and generate a directed edge e. ij ,in

[0013] S14: Calculate edge weight w ij ;

[0014] w ij =αd ij +βρ ij

[0015] Where α and β are preset constants, and ρ ij The obstacle density coefficient;

[0016]

[0017] S15: Using the node set v = {v i}, Edge set e = {e ij} and weight set w = {w ij Construct a multi-station topology graph G = (v, e, w), and apply the A* algorithm to G to search for the minimum cost obstacle avoidance path from the slot of the charging tray to the reversing mechanism and the machine tool processing station.

[0018] Preferably, in step S2:

[0019] S21: Apply the multi-station topology graph G = (v,e,w) obtained in step S1, and the nodes s of each slot of the loading tray in the multi-station topology graph G = (v,e,w). m (m∈[1,N1]) serves as the source node, and the reversing mechanism node tn (n∈[1,N2]) and machine tool machining mechanism node t n (n∈[N2+1,N2+N3]) are used as the target node to construct source-target pairs (s m ,t n );

[0020] S22: For each source-target pair (s m ,t n On the topology graph G, use Dijkstra's algorithm to find the minimum cost path p. mn =[V0=s m V1,……V L =t n ];

[0021] S23: Calculate path cost in The edge weights are those determined in step S1;

[0022] S24: For each minimum cost path p mn and the corresponding path cost C mn Store to path library Λ={p mn C mn};

[0023] S25: Press C on the paths in the path library Λ mn Sort the paths from smallest to largest to generate the minimum cost path set from the charge tray slot to the reversing mechanism station and the machine tool processing mechanism station.

[0024] Preferably, in step S3:

[0025] S31: Define the DH parameter set for each joint of the six-DOF robotic arm.

[0026] Θ={a i ,α i ,d i ,θ i |i=1......6}

[0027] Where a is the link length, α is the link twist angle, d is the joint offset, and θ is the joint angle, a single-joint homogeneous transformation matrix is ​​established:

[0028]

[0029] S32: Calculate the positive kinematic cumulative matrix T6 = A1A2A3A4A5A6 according to the joint number to obtain the end pose function f(q);

[0030] S33: For each obstacle avoidance path p in step S2 mn =[V0,V1,......,VL Extract node V j The three-dimensional coordinates (x) of (j=0......L) i ,y i ,z i And define the Euler angle group. Let x, y, and z represent the rotation angles about the x, y, and z axes, respectively. cθ j =cosθ j sθ j =sinθ j ,cψ j =cosψ j ,sψ j =sinψ j Construct the target pose matrix:

[0031]

[0032] S34: The joint angle vector is solved using an iterative inverse kinematics algorithm;

[0033] q j =[θ 1j θ 2j θ 3j θ 4j θ 5j θ 6j ]

[0034] Make f(q) j )≈P j Iteration formula:

[0035]

[0036] Where r is the number of iterations and λ is the step size coefficient. It is the Jacobian pseudo-inverse matrix;

[0037] S35: Transform the node sequence {q0,q1,......,q} L}Integrated into joint trajectory Q mn And store it in the joint trajectory library;

[0038] S36: Set the joint trajectory Q mn The controller sent to the robotic arm executes the drive path.

[0039] Preferably, in step S4:

[0040] S41: Define the state vector σ = [b1b2b3] T Where b1 represents the fully loaded charge tray, b2 represents the ready reversing mechanism, and b3 represents the completed machine tool processing state; ω1>ω2>ω3>0

[0041] S42: Define the weight vector ω = [ω1ω2ω3] T ,satisfy;

[0042] S43: Calculate the task priority index Π=ω T σ

[0043] S44: Treat tasks τ according to task priority index Π. j Sort the data in descending order (j = 1, ..., R) to obtain the task sequence T = {τ1, τ2, ..., τ}. R};

[0044] S45: For each task τ in the task sequence T j Call the minimum cost path p in the path library Λ mn and its corresponding joint trajectory Q mn Generate execution instruction I j =(Q mn ,τ j );

[0045] S46: Execute instructions I1 to I2 in the task sequence order. R The commands are sequentially sent to the controller of the robotic arm to drive the robotic arm to execute the corresponding joint trajectories.

[0046] Secondly, the present invention proposes a robotic arm transfer system that applies any of the above-mentioned robotic arm transfer path planning methods based on multi-station obstacle avoidance diagrams and inverse kinematics. The robotic arm transfer system includes: a machine tool processing mechanism, a robotic arm, and a reversing mechanism.

[0047] The robotic arm is used to transfer the drug column to be processed in the drug loading tray to the processing station in the machine tool processing mechanism, transfer the drug column with one end processed to the processing station of the reversing mechanism, and put the drug columns with both ends processed back into the drug loading tray in a preset order.

[0048] The machine tool processing mechanism is used to process medicine columns and is located on one side of the robotic arm;

[0049] The reversing mechanism is used to reverse the processed drug column by 180°, so that the robotic arm can transfer the unprocessed part of the other end of the drug column to the processing station of the reversing mechanism, which is located on the side of the robotic arm away from the machine tool processing mechanism.

[0050] Preferably, the reversing mechanism includes a second gripper, a second gripper mounting base, and a third gripper. The second gripper and the third gripper are both located on the side closest to the robotic arm and are staggered. The sides of the second gripper and the third gripper away from the robotic arm are respectively connected to the second gripper mounting base via gripper cylinders. A rotary cylinder is installed on the side of the second gripper mounting base away from the gripper cylinders, and the rotary cylinder is connected to the frame of the robotic arm via a mounting rod.

[0051] Preferably, the free end of the robotic arm is provided with four sets of grippers, and each of the four sets of grippers is connected to the free end of the robotic arm through a gripper mounting seat. The four sets of grippers are arranged in a square, and the square formed by the four sets of grippers is adapted to the square formed by the four adjacent loading chambers of the loading tray.

[0052] Preferably, the machine tool processing mechanism has two sets of drug column processing stations inside.

[0053] The beneficial effects of this invention are:

[0054] (1) By establishing a multi-station topology map in a three-dimensional environment and combining the A* algorithm and Dijkstra's algorithm to achieve global path optimization, the problems of path conflict and low planning efficiency caused by the complex layout of multi-stations in the existing technology are solved.

[0055] (2) By establishing a precise three-dimensional coordinate model of the loading tray, reversing mechanism and machine tool processing mechanism in step S1, and generating a multi-station obstacle avoidance map containing edge weights, the present invention can dynamically calculate the minimum cost path based on obstacle distribution, station distance and safety radius, thereby avoiding interference between the robotic arm and surrounding equipment, drug column or other machinery during the transfer process.

[0056] (3) In step S3, the six-degree-of-freedom forward kinematic equations of the robotic arm are constructed by combining the DH parameter model, and the trajectory of the end joint is optimized by the iterative inverse kinematic algorithm, which realizes the accurate solution of the pose of each node in the obstacle avoidance path. The numerical iteration strategy based on the pseudo-inverse of the Jacobian matrix can dynamically converge to the optimal solution while avoiding singular solutions, effectively reducing the end attitude error.

[0057] (4) In step S4, a dynamic task scheduling mechanism based on task state vector is introduced. By collecting the full load status of the charging plate, the ready status of the reversing mechanism and the processing completion status of the machine tool in real time, and combining the task weight vector to calculate the priority index, the global optimal sorting and distribution of tasks can be realized. This invention can avoid the robotic arm from waiting for a long time or running idle on low priority tasks, thereby improving equipment utilization and production cycle. Attached Figure Description

[0058] In the attached diagram:

[0059] Figure 1 This is a flowchart of the robotic arm transfer path planning method based on multi-station obstacle avoidance diagram and inverse kinematics proposed in this invention.

[0060] Figure 2 This is a schematic diagram of the robotic arm transfer system proposed in this invention;

[0061] Figure 3 This is a schematic diagram of the structure of the loading tray proposed in this invention;

[0062] Figure 4 This is a schematic diagram of the structure of a part of the gripper of the robotic arm proposed in this invention;

[0063] Figure 5 This is a schematic diagram of the reversing mechanism proposed in this invention;

[0064] Figure 6 This is a top view of the robotic arm transfer system proposed in this invention.

[0065] In the diagram: 1-machine tool processing mechanism, 2-robotic arm, 3-reversing mechanism, 4-loading tray;

[0066] 21-Gripper 1, 22-Gripper Mounting Base 1;

[0067] 31-Gripper II, 32-Gripper Cylinder, 33-Rotary Cylinder, 34-Gripper Mounting Base II, 35-Mounting Rod, 36-Gripper III. Detailed Implementation

[0068] Reference Figure 1 A method for planning the transfer path of a robotic arm based on multi-station obstacle avoidance maps and inverse kinematics is described, and the method steps are as follows:

[0069] S1: Establish a three-dimensional spatial model of the machine tool processing mechanism 1, the reversing mechanism 3 and the charging tray 4, as well as the coordinate data of environmental obstacles, generate a multi-station topology map, and use the A* algorithm to construct an obstacle avoidance path map containing edge weights.

[0070] Specifically:

[0071] S11: In the world coordinate system O-XYZ, measure the three-dimensional coordinates (x, y, z) of the slots of N1 charge trays 4, the positions of N2 reversing mechanisms 3, and the positions of N3 machine tool processing mechanisms 1. i ,y i ,z i ), where i∈[1,N1+N2+N3];

[0072] S12: Construct a minimum bounding box for M static obstacles and record the center coordinates (c kx ,c ky ,c kz ) and volume Vk , k∈[1,M];

[0073] S13: The center of each slot of the charging tray 4, each station of the reversing mechanism 3, and each station of the machine tool processing mechanism 1 are the graph nodes v. i This will satisfy the Euclidean distance d ij ≤d max node v i v j Connect and generate a directed edge e. ij ,in

[0074] S14: Calculate edge weight w ij ;

[0075] w ij =αd ij +βρ ij

[0076] Where α and β are preset constants, and ρ ij The obstacle density coefficient;

[0077]

[0078] S15: Using the node set v = {v i}, Edge set e = {e ij} and weight set w = {w ij Construct a multi-station topology graph G = (v, e, w), and apply the A* algorithm to the topology graph G to search for the minimum cost obstacle avoidance path from the slot of the charge tray 4 to the reversing mechanism and the machine tool processing station.

[0079] S2: Using the obstacle avoidance path map obtained in step S1, calculate the minimum cost path set from the slot of the charge tray 4 to the reversing mechanism 3 and the machine tool processing mechanism 1, and generate and store the path library;

[0080] Specifically:

[0081] S21: Apply the multi-station topology graph G = (v,e,w) obtained in step S1, and the nodes s of each slot in the charging tray 4 in the multi-station topology graph G = (v,e,w). m (m∈[1,N1]) serves as the source node, and is connected to the 3rd node t of the reversing mechanism. n (n∈[1,N2]) and 1 node t of the machine tool processing mechanism n (n∈[N2+1,N2+N3]) are used as the target node to construct source-target pairs (s m ,t n );

[0082] S22: For each source-target pair (s m ,tn On the topology graph G, use Dijkstra's algorithm to find the minimum cost path p. mn =[V0=s m ,V1,......V L =t n ];

[0083] S23: Calculate path cost in The edge weights are those determined in step S1;

[0084] S24: For each minimum cost path p mn and the corresponding path cost C mn Store to path library Λ={p mn C mn};

[0085] S25: Press C on the paths in the path library Λ mn Sort the paths from smallest to largest to generate the minimum cost path set from slot 4 of the charging tray to station 3 of the reversing mechanism and station of the machine tool processing mechanism.

[0086] S3: Construct a six-degree-of-freedom forward kinematics model for robotic arm 2, determine the DH parameters, call the inverse kinematics iterative algorithm, calculate the corresponding joint angle group for the node poses in the path library obtained in step S2, and generate the joint trajectory.

[0087] Specifically:

[0088] S31: Define the DH parameter set for each joint of the six-DOF robotic arm 2.

[0089] Θ={a i ,α i ,d i ,θ i |i=1......6}

[0090] Where a is the link length, α is the link twist angle, d is the joint offset, and θ is the joint angle, a single-joint homogeneous transformation matrix is ​​established:

[0091]

[0092] S32: Calculate the positive kinematic cumulative matrix T6 = A1A2A3A4A5A6 according to the joint number to obtain the end pose function f(q);

[0093] S33: For each obstacle avoidance path p in step S2 mn =[V0,V1,......,V L Extract node V j The three-dimensional coordinates (x) of (j=0......L) i ,yi ,z i And define the Euler angle group. Let x, y, and z represent the rotation angles about the x, y, and z axes, respectively. cθ j =cosθ j sθ j =sinθ j ,cψ j =cosψ j ,sψ j =sinψ j Construct the target pose matrix:

[0094]

[0095] S34: The joint angle vector is solved using an iterative inverse kinematics algorithm;

[0096] q j =[θ 1j θ 2j θ 3j θ 4j θ 5j θ 6j ]

[0097] Make f(q) j )≈P j Iteration formula:

[0098]

[0099] Where r is the number of iterations and λ is the step size coefficient. It is the Jacobian pseudo-inverse matrix;

[0100] S35: Transform the node sequence {q0,q1,......,q} L}Integrated into joint trajectory Q mn And store it in the joint trajectory library;

[0101] S36: Set the joint trajectory Q mn The controller sent to robotic arm 2 executes the drive path.

[0102] S4: Obtain the full load status of the charging tray, the ready status of the reversing mechanism, and the completed processing status of the machine tool. Generate a task sequence according to the preset task priority, and send the joint trajectory obtained in step S3 to the controller of the robotic arm 2 according to the task sequence.

[0103] Specifically:

[0104] S41: Define the state vector σ = [b1b2b3] TWhere b1 represents the fully loaded charge tray, b2 represents the ready reversing mechanism, and b3 represents the completed machine tool processing state; ω1>ω2>ω3>0

[0105] S42: Define the weight vector ω = [ω1ω2ω3] T ,satisfy;

[0106] S43: Calculate the task priority index Π=ω T σ

[0107] S44: Treat tasks τ according to task priority index Π. j Sort the data in descending order (j = 1...R) to obtain the task sequence T = {τ1, τ2, ..., τ...} R};

[0108] S45: For each task τ in the task sequence T j Call the minimum cost path p in the path library Λ mn and its corresponding joint trajectory Q mn Generate execution instruction I j =(Q mn ,τ j );

[0109] S46: Execute instructions I1 to I2 in the task sequence order. R The commands are sequentially sent to the controller of robotic arm 2 to drive robotic arm 2 to execute the corresponding joint trajectories.

[0110] Reference Figure 1 and Figure 2 As another embodiment of this application, this embodiment proposes a robotic arm transfer system, which applies any of the above-mentioned robotic arm transfer path planning methods based on multi-station obstacle avoidance diagrams and inverse kinematics. The robotic arm transfer system includes: a machine tool processing mechanism 1, a robotic arm 2, and a reversing mechanism 3.

[0111] The robotic arm 2 is used to transfer the drug column to be processed in the drug loading tray 4 to the processing station in the machine tool processing mechanism 1, transfer the drug column with one end processed to the processing station of the reversing mechanism 3, and put the drug columns with both ends processed back into the drug loading tray 4 in a preset order.

[0112] The machine tool processing mechanism 1 is used to process medicine columns and is located on one side of the robotic arm 2;

[0113] Specifically, the machine tool processing mechanism 1 has two sets of drug column processing stations inside.

[0114] The reversing mechanism 3 is used to reverse the processed drug column by 180°, so that the robotic arm 2 can transfer the unprocessed part of the other end of the drug column to the processing station of the reversing mechanism 3, which is located on the side of the robotic arm 2 away from the machine tool processing mechanism 1.

[0115] Reference Figure 5 The reversing mechanism 3 includes a second gripper 31, a second gripper mounting base 34, and a third gripper 36. The second gripper 31 and the third gripper 36 are both located on the side closest to the robotic arm 2 and are staggered. The sides of the second gripper 31 and the third gripper 36 away from the robotic arm 2 are respectively connected to the second gripper mounting base 34 via gripper cylinders 32. A rotary cylinder 33 is installed on the side of the second gripper mounting base 34 away from the gripper cylinder 32. The rotary cylinder 33 is connected to the frame of the robotic arm 2 via a mounting rod 35.

[0116] Reference Figure 4 The free end of the robotic arm 2 is provided with four sets of grippers 21. All four sets of grippers 21 are connected to the free end of the robotic arm 2 through gripper mounting bases 22. The four sets of grippers 21 are arranged in a square. The square formed by the four sets of grippers 21 is adapted to the square formed by the four adjacent loading chambers of the loading tray.

Claims

1. A method for planning the transfer path of a robotic arm based on multi-station obstacle avoidance maps and inverse kinematics, characterized in that, The method steps are as follows: S1: Establish a three-dimensional spatial model of the machine tool processing mechanism (1), the reversing mechanism (3) and the charging tray (4) and coordinate data of environmental obstacles, generate a multi-station topology map, and use the A* algorithm to construct an obstacle avoidance path map containing edge weights; S2: Using the obstacle avoidance path map obtained in step S1, calculate the minimum cost path set from the slot of the charge tray (4) to the reversing mechanism (3) and the machine tool processing mechanism (1), and generate and store the path library; S3: Construct a six-degree-of-freedom forward kinematics model of the robotic arm (2), determine the DH parameters, call the inverse kinematics iterative algorithm, obtain the corresponding joint angle group for the node pose in the path library obtained in step S2, and generate the joint trajectory; S4: Obtain the full load status of the charging plate, the ready status of the reversing mechanism, and the completed status of the machine tool processing. Generate a task sequence according to the preset task priority, and send the joint trajectory obtained in step S3 to the controller of the robotic arm (2) according to the task sequence.

2. The robotic arm transfer path planning method based on multi-station obstacle avoidance diagrams and inverse kinematics according to claim 1, characterized in that, In step S1: S11: Under the world coordinate system O-XYZ, measure the three-dimensional coordinates (x, y, z) of the slots of N1 charge trays (4), the positions of N2 reversing mechanisms (3), and the positions of N3 machine tool processing mechanisms (1). i ,y i ,z i ), where i∈[1,N1+N2+N3]; S12: Construct a minimum bounding box for M static obstacles and record the center coordinates (c kx ,c ky ,c kz ) and volume V k , k∈[1,M]; S13: The center of each slot of the loading tray (4), each station of the reversing mechanism (3), and each station of the machine tool processing mechanism (1) are the graph nodes v. i This will satisfy the Euclidean distance d ij ≤d max node v i v j Connect and generate a directed edge e. ij ,in S14: Calculate edge weight w ij ; w ij =αd ij +br ij Where α and β are preset constants, and ρ ij The obstacle density coefficient; S15: Using the node set v = {v i }, Edge set e = {e ij } and weight set w = {w ij Construct a multi-station topology graph G = (v, e, w), and apply the A* algorithm to search for the minimum cost obstacle avoidance path from the slot of the charge tray (4) to the reversing mechanism and the machine tool processing station on the topology graph G.

3. The robotic arm transfer path planning method based on multi-station obstacle avoidance diagrams and inverse kinematics according to claim 2, characterized in that, In step S2: S21: Apply the multi-station topology diagram G = (v,e,w) obtained in step S1, and the slot nodes s of the charging tray (4) in the multi-station topology diagram G = (v,e,w) m (m∈[1,N1]) serves as the source node, and is connected to node t of the reversing mechanism (3). n (n∈[1,N2]) and machine tool processing mechanism (1) node t n (n∈[N2+1,N2+N3]) are used as the target node to construct source-target pairs (s m ,t n ); S22: For each source-target pair (s m ,t n On the topology graph G, use Dijkstra's algorithm to find the minimum cost path p. mn =[V0=s m V1,……V L =t n ]; S23: Calculate path cost in The edge weights are those determined in step S1; S24: For each minimum cost path p mn and the corresponding path cost C mn Store to path library Λ={p mn C mn }; S25: Press C on the paths in the path library Λ mn Sort the data from smallest to largest to generate the minimum cost path set from the charging tray (4) slot to the reversing mechanism (3) station and the machine tool processing mechanism station.

4. The robotic arm transfer path planning method based on multi-station obstacle avoidance diagrams and inverse kinematics according to claim 3, characterized in that, In step S3: S31: Define the DH parameter set for each joint of the six-DOF robotic arm (2). Θ={a i ,a i ,d i ,i i |i=1……6} Where a is the link length, α is the link twist angle, d is the joint offset, and θ is the joint angle, a single-joint homogeneous transformation matrix is ​​established: S32: Calculate the positive kinematic cumulative matrix T6 = A1A2A3A4A5A6 according to the joint number to obtain the end pose function f(q); S33: For each obstacle avoidance path p in step S2 mn =[V0,V1,……,V L Extract node V j The three-dimensional coordinates (x) of (j=0......L) i ,y i ,z i And define the Euler angle group. Let x, y, and z represent the rotation angles about the x, y, and z axes, respectively. cψ j =cosψ j ,sψ j =sinψ j Construct the target pose matrix: S34: The joint angle vector is solved using an iterative inverse kinematics algorithm; q j =[θ 1j i 2j i 3j i 4j i 5j i 6j ] Make f(q) j )≈P j Iteration formula: Where r is the number of iterations and λ is the step size coefficient. It is the Jacobian pseudo-inverse matrix; S35: Transform the node sequence {q0,q1,......,q} L }Integrated into joint trajectory Q mn And store it in the joint trajectory library; S36: Set the joint trajectory Q mn The controller sent to the robotic arm (2) executes the drive path.

5. The robotic arm transfer path planning method based on multi-station obstacle avoidance diagrams and inverse kinematics according to claim 4, characterized in that, In step S4: S41: Define the state vector σ = [b1 b2 b3] T Where b1 represents the fully loaded charge tray, b2 represents the ready reversing mechanism, and b3 represents the completed machine tool processing state; ω1>ω2>ω3>0 S42: Define the weight vector ω = [ω1ω2ω3] T ,satisfy; S43: Calculate the task priority index Π=ω T σ S44: Treat tasks τ according to task priority index Π. j Sort the data in descending order (j = 1, ..., R) to obtain the task sequence T = {τ1, τ2, ..., τ}. R }; S45: For each task τ in the task sequence T j Call the minimum cost path p in the path library Λ mn and its corresponding joint trajectory Q mn Generate execution instruction I j =(Q mn ,τ j ); S46: Execute instructions I1 to I2 in the task sequence order. R The controllers of the robotic arm (2) are sequentially sent to drive the robotic arm (2) to execute the corresponding joint trajectories.

6. A robotic arm transfer system, characterized in that: The robotic arm transfer system, which uses the robotic arm transfer path planning method based on multi-station obstacle avoidance diagram and inverse kinematics as described in any one of claims 1-5, includes: a machine tool processing mechanism (1), a robotic arm (2), and a reversing mechanism (3). The robotic arm (2) is used to transfer the drug column to be processed in the drug loading tray (4) to the processing station in the machine tool processing mechanism (1), to transfer the drug column processed at one end to the processing station of the reversing mechanism (3), and to put the drug columns processed at both ends back into the drug loading tray (4) in a preset order. The machine tool processing mechanism (1) is used to process medicine columns and is located on one side of the robotic arm (2); The reversing mechanism (3) is used to reverse the processed drug column by 180°, so that the robotic arm (2) can transfer the unprocessed part of the other end of the drug column to the processing station of the reversing mechanism (3), which is located on the side of the robotic arm (2) away from the machine tool processing mechanism (1).

7. A robotic arm transfer system according to claim 6, characterized in that: The reversing mechanism (3) includes a second gripper (31), a second gripper mounting base (34), and a third gripper (36). The second gripper (31) and the third gripper (36) are both located on the side close to the robotic arm (2) and are staggered. The side of the second gripper (31) and the third gripper (36) away from the robotic arm (2) are respectively connected to the second gripper mounting base (34) via a gripper cylinder (32). A rotary cylinder (33) is installed on the side of the second gripper mounting base (34) away from the gripper cylinder (32). The rotary cylinder (33) is connected to the frame of the robotic arm (2) via a mounting rod (35).

8. The robotic arm transfer system according to claim 7, characterized in that: The free end of the robotic arm (2) is provided with four sets of grippers (21). All four sets of grippers (21) are connected to the free end of the robotic arm (2) through gripper mounting base (22). The four sets of grippers (21) are arranged in a square. The square formed by the four sets of grippers (21) is compatible with the square formed by the four adjacent loading chambers of the loading tray.

9. A robotic arm transfer system according to claim 8, characterized in that: The machine tool processing mechanism (1) has two sets of drug column processing stations inside.

Images