A robotic wire laying method

Abstract

The present invention provides a robotic wire placement method, belonging to the field of wire placement, comprising: planning a wire placement processing trajectory using CAM software; taking into account S-shaped acceleration and deceleration while ensuring robot speed uniformity to obtain an S-shaped curve, thereby obtaining a secondary processing trajectory; using trajectory points of the secondary processing trajectory to obtain the angle values ​​of each joint, and limiting the maximum speed of each joint to obtain the optimal speed value of each joint, thereby replanning the optimal joint trajectory, obtaining the robot's final processing trajectory, and generating a processing G-code; based on the robot's joint positions, speeds, and accelerations corresponding to the final processing trajectory, obtaining the driving torque of each robot joint at different positions, and storing the driving torque in a second code; and having a numerical control system synchronously run the processing G-code and the second code. The present invention can achieve high-precision wire placement control.

Description

Technical Field

[0001] The present invention belongs to the field of wire laying, and in particular relates to a robot wire laying method. Background Art

[0002] Robotic wire laying refers to the process of laying metal mesh, iron wire and other materials using robots or automated equipment. In traditional wire laying operations, manual positioning, metering, cutting and laying are usually required, which takes up a lot of manpower and time, and it is difficult to ensure laying accuracy and efficiency. Therefore, the use of robotic wire laying technology can improve laying accuracy and efficiency. In the existing technology, a CNC system is usually used to control the robot to realize wire laying processing, but it is highly dependent on professional CAM software. Although the CAM software dedicated to wire laying can realize trajectory planning, process simulation, speed planning and post-processing functions, it has poor integration with the CNC system and cannot perform torque feedforward compensation. The actual control effect is still not ideal. Summary of the Invention

[0003] The purpose of the present invention is to provide a robotic wire laying method that can achieve high-precision wire laying control effects.

[0004] The present invention is achieved through the following technical solutions:

[0005] A robotic wire laying method comprises the following steps:

[0006] Step S1: planning a wire placement processing trajectory using CAM software, where the processing trajectory includes discrete trajectory points in a Cartesian coordinate system, robot postures corresponding to each discrete trajectory point, and robot speed and acceleration;

[0007] Step S2: The numerical control system considers the S-shaped acceleration and deceleration form according to the machining trajectory while ensuring the uniformity of the robot speed, obtains an S-shaped curve representing the displacement-time relationship, discretizes the machining S-shaped curve according to the interpolation cycle, and obtains the secondary machining trajectory;

[0008] Step S3: Using the trajectory points of the secondary processing trajectory, the inverse kinematics algorithm is used to solve the angle values ​​of each joint of the robot, and the maximum speed of the joint is limited to obtain the optimal speed value of each joint. The optimal joint trajectory is re-planned based on the optimal speed value of each joint, and the final processing trajectory of the robot is obtained according to the joint trajectory using the forward kinematics algorithm, and the processing G code is generated;

[0009] Step S4: Obtain the driving torque of each joint of the robot at different positions according to the position, velocity, and acceleration of each joint of the robot corresponding to the final processing trajectory, and put the driving torque into the second code;

[0010] Step S5: The CNC system synchronously runs the machining G code and the second code. The G code controls the robot to move along the final machining trajectory. The second code serves as a supplement to the G code and performs torque feedforward compensation on the robot within the same interpolation cycle.

[0011] Furthermore, in step S1, the discrete trajectory points of the processing trajectory are represented as ,in,( x i , y i ,z i ) represents the position coordinates of discrete trajectory points, ( θ x,i , θ y,i , θ z,i ,) represents the attitude angle of discrete trajectory points, v i represents the speed of the robot end, a i Indicates the acceleration of the robot end.

[0012] Furthermore, in step S2, the S-shaped curve is represented as ,in, , , , 0≤ t < t 1 is the acceleration stage, t 1≤ t < t 2 is the uniform speed stage, t 2≤ t ≤ T In the deceleration phase, T Indicates the processing time of the S-shaped curve from the initial acceleration to the final deceleration to 0. s ( t ) indicates the corresponding time t The displacement length of the machining.

[0013] Furthermore, in step S3, the robot j The optimal velocity value of each joint is expressed as ,in, Indicates the robot's j Joint angle values, Indicates the robot's j The speed limit of each joint, Indicates the robot's j The acceleration limit of each joint, Indicates the robot'sj The jerk limit value of each joint, Indicates the current processing time T The remaining path length under , 1≤ j ≤ J , J is the number of joints.

[0014] Furthermore, in step S3, planning the optimal joint trajectory specifically includes: j The initial position of each joint is based on the j The optimal velocity value of each joint is calculated j The position of the joint at the next point, and so on to get the j The optimal joint trajectory of each joint.

[0015] Furthermore, in step S4, a dynamics algorithm model is established based on Newton-Euler dynamics using Matlab software. The dynamic parameters of the dynamics algorithm model are obtained using an identification algorithm. The position, velocity and acceleration of each joint of the robot corresponding to the final processing trajectory are transmitted to the dynamics algorithm model. The kinematics extrapolation from the first joint to the second joint is used to obtain the dynamic parameters of the dynamics algorithm model. J The joint iteratively calculates the velocity and acceleration of each link of the robot, and then uses kinematic inference to get the velocity and acceleration of each link from the first link. J The interaction forces and torques between the robot's links are iteratively calculated from the joint to the first joint to obtain the driving torque of each joint at different positions.

[0016] Furthermore, the robot is a six-joint robot.

[0017] Furthermore, the interpolation period is 1 ms.

[0018] The present invention has the following beneficial effects:

[0019] 1. The present invention first plans the wire laying processing trajectory using CAM software, and then considers the form of S-type acceleration and deceleration while ensuring the uniformity of the robot speed, obtains an S-type curve representing the displacement-time relationship, discretizes the processing S-type curve according to the interpolation cycle, and obtains a secondary processing trajectory. Then, using the trajectory points of the secondary processing trajectory, the angle values ​​of each joint of the robot are solved by the inverse kinematics algorithm, and the maximum speed limit of the joint is performed to obtain the optimal speed value of each joint. The optimal joint trajectory is re-planned based on the optimal speed value of each joint, and the final processing trajectory of the robot is obtained according to the joint trajectory through the forward kinematics algorithm, and the processing G code is generated. Then, according to the position and speed of each joint of the robot corresponding to the final processing trajectory, the optimal speed value of each joint is obtained. and acceleration, obtain the driving torque of each joint of the robot at different positions, and put the driving torque into the second code. Finally, the CNC system runs the processing G code and the second code synchronously. The G code controls the robot to move along the final processing trajectory. The second code, as a supplement to the G code, performs torque feedforward compensation on the robot within the same interpolation cycle. The uniformity of wire laying and the flexibility of the wire laying process are guaranteed through secondary trajectory planning and speed planning. The maximum speed limit of the joint ensures that the robot can process at the highest operating speed. The G code and the second code are dual-code controlled to realize torque feedforward compensation of the real-time position, thereby ensuring the contour accuracy of the wire laying and finally obtaining a high-precision wire laying control effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] The present invention will be further described in detail below with reference to the accompanying drawings.

[0021] Figure 1 Flowchart of the present invention.

[0022] Figure 2 Detailed flow chart of the present invention.

[0023] Figure 3 This is a schematic structural diagram of the six-joint robot of the present invention. DETAILED DESCRIPTION

[0024] like Figure 1 and Figure 2 As shown, the robotic wire laying method includes the following steps:

[0025] Step S1: planning a wire placement processing trajectory using CAM software, where the processing trajectory includes discrete trajectory points in a Cartesian coordinate system, robot postures corresponding to each discrete trajectory point, and robot speed and acceleration;

[0026] Specifically, in this embodiment, the Figure 3 The six-joint robot shown in the figure has servo drives on each joint of the robot to drive its movements. J 1 is the robot waist rotation joint (base joint), J2 is the robot's shoulder swing joint (upper arm joint), J 3 is the robot's elbow swing joint (forearm joint), J 4 is the wrist rotation joint, J 5 is the wrist swing joint, J 6 is the wrist twist joint. The machining trajectory planned by CAM software i The discrete trajectory points are represented as ,in,( x i , y i ,z i ) represents the position coordinates of discrete trajectory points, ( θ x,i , θ y,i , θ z,i ,) represents the attitude angle of discrete trajectory points, v i represents the speed of the robot end, a i Indicates the acceleration of the robot end.

[0027] Step S2: The numerical control system considers the S-shaped acceleration and deceleration form according to the machining trajectory while ensuring the uniformity of the robot speed, obtains an S-shaped curve representing the displacement-time relationship, discretizes the machining S-shaped curve according to the interpolation cycle, and obtains the secondary machining trajectory;

[0028] According to the requirements of the wire laying process, the wire laying head needs to ensure uniform speed during the wire laying process and be able to lay the wire efficiently. The wire laying head is set at the end of the robot. The S-shaped curve is a displacement-time function, specifically expressed as ,in, , , , 0≤ t < t 1 is the acceleration stage, t 1≤ t < t 2 is the uniform speed stage, t 2≤ t ≤ T The deceleration stage is the processing time from the start of acceleration to the final deceleration of the S-shaped curve to 0. s ( t ) indicates the corresponding time t The displacement length of the machining.

[0029] Regarding the above s ( t) The functional relationship between displacement and time, the interpolation period is set to 1ms, the displacement value corresponding to each interpolation period is calculated according to the interpolation period, the S-shaped curve is discretized to refine the target trajectory point for each interpolation period, and the secondary processing trajectory is obtained. i The discrete trajectory points are represented as .

[0030] Step S3: Using the trajectory points of the secondary processing trajectory, the inverse kinematics algorithm is used to solve the angle values ​​of each joint of the robot, and the maximum speed of the joint is limited to obtain the optimal speed value of each joint. The optimal joint trajectory is re-planned based on the optimal speed value of each joint, and the final processing trajectory of the robot is obtained according to the joint trajectory through the forward kinematics algorithm, and the processing G code is generated;

[0031] Using the trajectory points of the secondary machining trajectory, the inverse kinematics algorithm is used to solve the joint angle values ​​corresponding to the 1st to 6th joints of the robot. , and calculate the joint velocity value corresponding to each joint according to the Jacobian matrix The specific process of calculating the joint angle value and the joint velocity value is the existing technology. According to the formula Mapping joint velocity vectors to the linear velocity of the robot end effector v and angular velocity w ,in, , is the Jacobian matrix, is the Jacobian matrix The conjugation of .

[0032] The robot's j The optimal velocity value of each joint is expressed as ,in, Indicates the robot's j Joint angle values, Indicates the robot's j The speed limit of each joint, Indicates the robot's j The acceleration limit of each joint, Indicates the robot's j The jerk limit value of each joint, Indicates the current processing time T The remaining path length under j ≤ J , J is the number of joints. Figure 2 In total, J =6 joints J 1- J 6, corresponding to j =1 toj =6 joints.

[0033] The optimal joint trajectory is re-planned according to the optimal speed value, and the optimal joint trajectory is re-planned through forward kinematics and Jacobian matrix. This process is an existing technology. The optimal joint trajectory can be specifically described as follows: j The initial position of each joint is based on the j The optimal velocity value of each joint is calculated j The position of the joint at the next point, and so on to get the j The optimal joint trajectory of each joint.

[0034] Step S4: Obtain the driving torque of each joint of the robot at different positions according to the position, velocity, and acceleration of each joint of the robot corresponding to the final processing trajectory, and put the driving torque into the second code;

[0035] Specifically, Matlab software was used to establish a dynamic algorithm model based on Newton-Euler dynamics. The dynamic parameters of the dynamic algorithm model were obtained using an identification algorithm. The position, velocity, and acceleration of each joint of the robot corresponding to the final processing trajectory were transmitted to the dynamic algorithm model. The kinematic extrapolation from the first joint to the second joint was used to identify the final processing trajectory. J The joint iteratively calculates the velocity and acceleration of each link of the robot, and then uses kinematic inference to get the velocity and acceleration of each link from the first link. J The forces and torques of the robot's interactions between the links are iteratively calculated from the joint to the first joint to obtain the driving torques of each joint at different positions. The process of obtaining the driving torques of each joint at different positions based on the forces and torques of the robot's interactions between the links is an existing technology.

[0036] Step S5: The CNC system synchronously runs the machining G code and the second code. The G code controls the robot to move along the final machining trajectory. The second code serves as a supplement to the G code and performs torque feedforward compensation on the robot within the same interpolation cycle.

[0037] The above description is merely a preferred embodiment of the present invention and therefore cannot be used to limit the scope of the present invention. In other words, equivalent changes and modifications made according to the scope of the patent application and the contents of the specification should still fall within the scope of the patent of the present invention.

Claims

1. A robotic wire laying method, characterized by: The steps include: Step S1: planning a wire placement processing trajectory using CAM software, where the processing trajectory includes discrete trajectory points in a Cartesian coordinate system, robot postures corresponding to each discrete trajectory point, and robot speed and acceleration; Step S2: The numerical control system considers the S-shaped acceleration and deceleration form according to the machining trajectory while ensuring the uniformity of the robot speed, obtains an S-shaped curve representing the displacement-time relationship, discretizes the machining S-shaped curve according to the interpolation cycle, and obtains the secondary machining trajectory; Step S3: Using the trajectory points of the secondary processing trajectory, the inverse kinematics algorithm is used to solve the angle values ​​of each joint of the robot, and the maximum speed of the joint is limited to obtain the optimal speed value of each joint. The optimal joint trajectory is re-planned based on the optimal speed value of each joint, and the final processing trajectory of the robot is obtained according to the joint trajectory using the forward kinematics algorithm, and the processing G code is generated; Step S4: Obtain the driving torque of each joint of the robot at different positions according to the position, velocity, and acceleration of each joint of the robot corresponding to the final processing trajectory, and put the driving torque into the second code; Step S5: The CNC system synchronously runs the machining G code and the second code. The G code controls the robot to move along the final machining trajectory. The second code serves as a supplement to the G code and performs torque feedforward compensation on the robot within the same interpolation cycle.

2. A robotic wire laying method according to claim 1, characterized in that: In step S1, the discrete trajectory points of the processing trajectory are expressed as ,in,( x i , y i ,z i ) represents the position coordinates of discrete trajectory points, ( θ x,i ,θ y,i , θ z,i ,) represents the attitude angle of discrete trajectory points, v i represents the speed of the robot end, a i Indicates the acceleration of the robot end.

3. The robotic wire laying method according to claim 2, wherein: In step S2, the S-shaped curve is represented as ,in, , , , 0≤ t < t 1 is the acceleration stage, t 1≤ t < t 2 is the uniform speed stage, t 2≤ t ≤ T In the deceleration phase, T Indicates the processing time of the S-shaped curve from the initial acceleration to the final deceleration to 0. s ( t ) indicates the corresponding time t The displacement length of the machining.

4. The robotic wire laying method according to claim 3, wherein: In step S3, the robot j The optimal velocity value of each joint is expressed as ,in, Indicates the robot's j Joint angle values, Indicates the robot's j The speed limit of each joint, Indicates the robot's j The acceleration limit of each joint, Indicates the robot's j The jerk limit value of each joint, Indicates the current processing time T The remaining path length under j ≤ J , J is the number of joints.

5. The robotic wire laying method according to claim 4, characterized in that: In step S3, planning the optimal joint trajectory specifically includes: j The initial position of each joint is based on the j The optimal velocity value of each joint is calculated j The position of the joint at the next point, and so on to get the j The optimal joint trajectory of each joint.

6. The robotic wire laying method according to claim 5, characterized in that: In step S4, Matlab software is used to establish a dynamic algorithm model based on Newton-Euler dynamics. The dynamic parameters of the dynamic algorithm model are obtained by using an identification algorithm. The position, velocity and acceleration of each joint of the robot corresponding to the final processing trajectory are transmitted to the dynamic algorithm model. The kinematic extrapolation from the first joint to the second joint is performed. J The joint iteratively calculates the velocity and acceleration of each link of the robot, and then uses kinematic inference to get the velocity and acceleration of each link from the first link. J The interaction forces and torques between the robot's links are iteratively calculated from the joint to the first joint to obtain the driving torque of each joint at different positions.

7. A robotic wire laying method according to any one of claims 1 to 6, characterized in that: The robot is a six-joint robot.

8. A robotic wire laying method according to any one of claims 1 to 6, characterized in that: The interpolation period is 1 ms.

Images