Robot fiber placement method

Abstract

The invention provides a robot fiber placement method, which belongs to the field of fiber placement and comprises the following steps: planning a fiber placement processing track by using CAM software; under the condition that the speed uniformity of the robot is guaranteed, an S-shaped acceleration and deceleration form is considered to obtain an S-shaped curve, and then a secondary machining track is obtained; the angle value of each joint is obtained through the track points of the secondary machining track, the maximum speed of the joints is limited to obtain the optimal speed value of each joint, then the optimal joint track is planned again, the final machining track of the robot is obtained, and a machining G code is generated; according to the position, the speed and the accelerated speed of each joint of the robot corresponding to the final machining track, driving torques of each joint of the robot at different positions are obtained, and the driving torques are put into a second code; and the numerical control system synchronously runs and processes the G code and the second code. According to the invention, a high-precision fiber placement control effect can be achieved.

Description

Technical Field

[0001] The present invention belongs to the field of fiber placement, and particularly relates to a robot fiber placement method. Background Art

[0002] Robot fiber placement refers to the process of laying materials such as wire mesh and iron wire using robots or automated equipment. In traditional fiber placement operations, manual positioning, measurement, cutting, and laying are usually required, which takes a large amount of labor and time, and it is difficult to ensure the laying accuracy and efficiency. Therefore, the use of robot fiber placement technology can improve the laying accuracy and efficiency. In the prior art, a numerical control system is usually used to control the robot to achieve fiber placement processing, but it highly depends on professional CAM software. Although the CAM software dedicated to fiber placement can realize functions such as trajectory planning, process simulation, speed planning, and post-processing, its integration with the numerical control system is poor, and torque feedforward compensation cannot be performed, so the actual control effect is still not ideal. Summary of the Invention

[0003] The object of the present invention is to propose a robot fiber placement method that can achieve a high-precision fiber placement control effect.

[0004] The present invention is realized through the following technical solutions: A robot fiber placement method includes the following steps: Step S1: Use CAM software to plan the fiber placement processing trajectory. The processing trajectory includes discrete trajectory points in the Cartesian coordinate system, the corresponding robot poses, robot speeds, and accelerations of each discrete trajectory point; Step S2: The numerical control system, considering the form of S-shaped acceleration and deceleration while ensuring the uniformity of the robot speed according to the processing trajectory, obtains an S-shaped curve representing the displacement-time relationship, and discretizes the processed S-shaped curve according to the interpolation period to obtain a secondary processing trajectory; Step S3: Use the trajectory points of the secondary processing trajectory to solve the joint angle values of the robot through the inverse kinematics algorithm, and perform the maximum joint speed limit to obtain the optimal speed values of each joint. Then, re-plan the optimal joint trajectory based on the optimal speed values of each joint, and obtain the final processing trajectory of the robot through the forward kinematics algorithm according to the joint trajectory, and generate the processing G code; Step S4: According to the positions, speeds, and accelerations of each joint of the robot corresponding to the final processing trajectory, obtain the driving torque of each joint of the robot at different positions, and put the driving torque into the second code; Step S5: The numerical control system synchronously runs the processing G code and the second code. The G code controls the robot to move along the final processing trajectory, and the second code, as a supplement to the G code, performs torque feedforward compensation on the robot within the same interpolation period.

[0005] Further, in the step S1, the discrete trajectory points of the machining trajectory are represented as , where, x i , y i ,z i ) represents the position coordinates of the discrete trajectory point, ( θ x,i , θ y,i , θ z,i ,) represents the attitude angle of the discrete trajectory point, v i represents the speed of the robot end, a i represents the acceleration of the robot end.

[0006] Further, in the step S2, the S-shaped curve is represented as , where, , , , 0 ≤ t < t 1 is the stage of increasing acceleration, t 1 ≤ t < t 2 is the constant speed stage, t 2 ≤ t ≤ T is the stage of decreasing acceleration, T represents the machining time for the S-shaped curve to accelerate from the start to finally decelerate to 0, s ( t ) represents the displacement length machined at the corresponding time t .

[0007] Further, in the step S3, the optimal speed value of the j th joint of the robot is represented as , where, represents the j th joint angle value of the robot, represents the speed limit value of the j th joint of the robot, represents the acceleration limit value of the j th joint of the robot, represents the jerk limit value of the j th joint of the robot, represents the remaining path length at the current machining time T , 1 ≤ j ≤ J , Jis the number of joints.

[0008] Further, in the step S3, planning the optimal joint trajectory specifically includes: at the initial position of the j th joint, calculating the position of the j th joint at the next point according to the optimal speed value of the j th joint at this time, and so on to obtain the optimal joint trajectory of the j th joint.

[0009] Further, in the step S4, using Matlab software to establish a dynamic algorithm model based on Newton-Euler dynamics, the dynamic parameters of the dynamic algorithm model are obtained by an identification algorithm, transmitting the positions, speeds and accelerations of the joints of the robot corresponding to the final machining trajectory to the dynamic algorithm model, iteratively calculating the speeds and accelerations of the links of the robot outward from the first joint to the J th joint through kinematic extrapolation, and then iteratively calculating the forces and torques of the interactions between the links of the robot inward from the J th joint to the first joint through kinematic interpolation to obtain the driving torques of the joints at different positions.

[0010] Further, the robot is a six-joint robot.

[0011] Further, the interpolation period is 1 ms.

[0012] The present invention has the following beneficial effects: 1. First, the present invention uses CAM software to plan the fiber placement processing trajectory. Secondly, considering the S-shaped acceleration and deceleration form while ensuring the uniformity of the robot speed, an S-shaped curve representing the displacement-time relationship is obtained. The S-shaped curve for processing is discretized according to the interpolation period to obtain a secondary processing trajectory. Then, using the trajectory points of the secondary processing trajectory, the joint angle values of the robot are solved through the inverse kinematics algorithm, and the maximum joint speed is limited to obtain the optimal speed values of each joint. Based on the optimal speed values of each joint, an optimal joint trajectory is re-planned. According to the joint trajectory, the final processing trajectory of the robot is obtained through the forward kinematics algorithm, and the processing G-code is generated. Then, according to the positions, speeds, and accelerations of the robot joints corresponding to the final processing trajectory, the driving torques of the robot joints at different positions are obtained and put into the second code. Finally, the numerical control system synchronously runs the processing G-code and the second code. The G-code controls the robot to move along the final processing trajectory, and the second code, as a supplement to the G-code, performs torque feedforward compensation on the robot within the same interpolation period. Through secondary trajectory planning and speed planning, the uniformity of fiber placement and the compliance of the fiber placement process are ensured. Through the maximum joint speed limit, it is ensured that the robot processes at the maximum operating speed. The G-code and the second code are controlled by dual codes to achieve real-time position torque feedforward compensation, thereby ensuring the contour accuracy of fiber placement and finally obtaining a high-precision fiber placement control effect. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0014] Figure 1 is a flowchart of the present invention.

[0015] Figure 2 is a detailed flowchart of the present invention.

[0016] Figure 3 is a schematic structural diagram of a six-joint robot of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0017] As Figure 1 and Figure 2 shown, the robot fiber placement method includes the following steps: Step S1: Use CAM software to plan the fiber placement processing trajectory. The processing trajectory includes discrete trajectory points in the Cartesian coordinate system, the robot poses corresponding to each discrete trajectory point, the robot speed, and acceleration; Specifically, in this embodiment, a six-joint robot as Figure 3 shown is used. Servo drivers for driving their movements are provided on each joint of the robot. Among them, J 1 is the robot waist rotation joint (base joint), J 2 is the robot shoulder swing joint (upper arm joint), J3 is the swinging joint of the robot elbow (forearm joint), J 4 is the rotating joint of the wrist, J 5 is the swinging joint of the wrist, J 6 is the twisting joint of the wrist. The i th discrete trajectory point of the machining trajectory planned by the CAM software is expressed as , where, ([[]] x i , y i ,z i ) represents the position coordinates of the discrete trajectory point, ([[]] θ x,i , θ y,i , θ z,i ,) represents the attitude angle of the discrete trajectory point, v i represents the speed of the robot end, a i represents the acceleration of the robot end.

[0018] Step S2: According to the machining trajectory, considering the form of S-shaped acceleration and deceleration while ensuring the uniformity of the robot speed, the numerical control system obtains the S-shaped curve representing the displacement-time relationship, and discretizes the machining S-shaped curve according to the interpolation period to obtain the secondary machining trajectory; According to the requirements of the filament winding process, the filament winding head needs to ensure the uniformity of speed during the filament winding process and be able to wind filaments efficiently. The filament winding head is set at the end of the robot. The S-shaped curve is a displacement-time function, specifically expressed as , where, , , , 0 ≤ t < t 1 is the stage of increasing acceleration, t 1 ≤ t < t 2 is the stage of uniform speed, t 2 ≤ t ≤ T is the stage of decreasing acceleration. The machining time of the S-shaped curve from starting to accelerate to finally decelerating to 0, s ( t ) represents the displacement length machined corresponding to the time t .

[0019] For the above s ( t)The functional relationship between displacement and time, with the interpolation period set to 1 ms. Calculate the displacement value corresponding to each interpolation period according to the interpolation period, discretize the S-shaped curve to refine it into the target trajectory points for each interpolation period, so as to obtain the secondary processing trajectory. The i th discrete trajectory point in the secondary processing trajectory is expressed as .

[0020] Step S3: Use the trajectory points of the secondary processing trajectory to solve the joint angle values of the robot through the inverse kinematics algorithm, and perform joint maximum speed limit to obtain the optimal speed values of each joint. Then, re-plan the optimal joint trajectory based on the optimal speed values of each joint, and obtain the final processing trajectory of the robot through the forward kinematics algorithm to generate the processing G code; Use the trajectory points of the secondary processing trajectory to solve through the inverse kinematics algorithm to obtain the joint angle values corresponding to the 1st to 6th joints of the robot , and calculate the joint speed values corresponding to each joint according to the Jacobian matrix . The specific process of calculating the joint angle values and joint speed values is the prior art. According to the formula Map the joint speed vector to the linear velocity v and angular velocity w of the robot end effector, where , is the Jacobian matrix, is the conjugate of the Jacobian matrix .

[0021] The optimal speed value of the j th joint of the robot is expressed as , where represents the j th joint angle value of the robot, represents the speed limit value of the j th joint of the robot, represents the acceleration limit value of the j th joint of the robot, represents the jerk limit value of the j th joint of the robot, represents the remaining path length at the current processing time T , 1 ≤ j ≤ J , J is the number of joints. In Figure 2 , there are a total of J = 6 joints J 1 - J 6, corresponding to the j = 1 to j = 6 joints respectively.

[0022] According to the optimal speed value, the optimal joint trajectory is re-planned, and through forward kinematics and the Jacobian matrix, the optimal joint trajectory is re-planned. This process is the prior art. The planning of the optimal joint trajectory can be specifically described as follows: at the initial position of the j th joint, according to the optimal speed value of the j th joint at this time, calculate the position of the j th joint at the next point, and so on to obtain the optimal joint trajectory of the j th joint.

[0023] Step S4: According to the positions, speeds, and accelerations of the robot joints corresponding to the final machining trajectory, obtain the driving torques of the robot joints at different positions, and put the driving torques into the second code; Specifically, use Matlab software 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. Transmit the positions, speeds, and accelerations of the robot joints corresponding to the final machining trajectory to the dynamic algorithm model. Iteratively calculate the speeds and accelerations of each link of the robot outward from the 1st joint to the J th joint through kinematic extrapolation, and then iteratively calculate the forces and torques of the interaction between the links of the robot inward from the J th joint to the 1st joint through kinematic interpolation to obtain the driving torques of each joint at different positions. Among them, the process of obtaining the driving torques of each joint at different positions according to the forces and torques of the interaction between the links of the robot is the prior art.

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

[0025] As described above, it is only a preferred embodiment of the present invention, so the scope of implementation of the present invention cannot be limited thereby. That is, equivalent changes and modifications made according to the scope of the patent application of the present invention and the content of the specification should still fall within the scope covered by the patent of the present invention.

Claims

1. A robotic filament placement method, characterized in that: It includes the following steps: Step S1: Use CAM software to plan the fiber placement processing trajectory. The processing trajectory includes discrete trajectory points in the Cartesian coordinate system, the corresponding robot poses, robot speeds, and accelerations for each discrete trajectory point; Step S2: The numerical control system, considering the S-shaped acceleration and deceleration form while ensuring the uniformity of the robot speed according to the processing trajectory, obtains the S-shaped curve representing the displacement-time relationship, discretizes the processed S-shaped curve according to the interpolation period to obtain the secondary processing trajectory; Step S3: Use the trajectory points of the secondary processing trajectory to solve the joint angle values of the robot through the inverse kinematics algorithm, and perform joint maximum speed limit to obtain the optimal speed values for each joint. Then, re-plan the optimal joint trajectory based on the optimal speed values for each joint. According to the joint trajectory, obtain the final processing trajectory of the robot through the forward kinematics algorithm and generate the processing G code; Step S4: According to the positions, speeds, and accelerations of each joint of the robot corresponding to the final processing trajectory, obtain the driving torques of each joint of the robot at different positions, and put the driving torques into the second code; Step S5: The numerical control system synchronously runs the processing G code and the second code. The G code controls the robot to move along the final processing trajectory, and the second code, as a supplement to the G code, performs torque feedforward compensation on the robot within the same interpolation period.

2. The robotic filament placement method according to claim 1, characterized in that: In the step S1, the discrete trajectory points of the machining trajectory are represented as , where, ( x i , y i ,z i ) represents the position coordinates of the discrete trajectory point, ( θ x,i ,θ y,i , θ z,i ,) represents the attitude angle of the discrete trajectory point, v i represents the speed of the robot end, a i represents the acceleration of the robot end.

3. A filament winding method for a robot according to claim 2, characterized in that: In the step S2, the S-shaped curve is expressed as , where , , , 0 ≤ t < t 1 is the stage of increasing acceleration, t 1 ≤ t < t 2 is the stage of constant speed, t 2 ≤ t ≤ T is the stage of decreasing acceleration, T represents the processing time for the S-shaped curve to accelerate from the start to finally decelerate to 0, s ( t ) represents the displacement length processed at the corresponding time t .

4. A filament winding method for a robot according to claim 3, characterized in that: In the step S3, the optimal speed value of the j -th joint of the robot is expressed as , where represents the j -th joint angle value of the robot, represents the speed limit value of the j -th joint of the robot, represents the acceleration limit value of the j -th joint of the robot, represents the jerk limit value of the j -th joint of the robot, represents the remaining path length at the current processing time T , 1 ≤ j ≤ J , J is the number of joints.

5. A robotic filament placement method according to claim 4, wherein: In step S3, planning the optimal joint trajectory specifically includes: at the initial position of the j -th joint, calculating the position of the j -th joint at the next point according to the optimal speed value of the j -th joint at this time, and so on to obtain the optimal joint trajectory of the j -th joint.

6. The robotic fiber placement method according to claim 5, characterized in that: In the step S4, a dynamic algorithm model is established based on Newton-Euler dynamics by using Matlab software. The dynamic parameters of the dynamic algorithm model are obtained by using an identification algorithm. The positions, velocities, and accelerations of the respective joints of the robot corresponding to the final machining trajectory are transmitted to the dynamic algorithm model. The velocities and accelerations of the respective links of the robot are iteratively calculated outward from the first joint to the J joint, and then the forces and torques of the interactions between the links of the robot are iteratively calculated inward from the J joint to the first joint to obtain the driving torques of the respective joints at different positions.

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

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

Images