A method and system for controlling the pressure of a laying roller to eliminate the gravity disturbance of a laying mechanism

Abstract

The present invention relates to a method for controlling the pressure of a laying roller to eliminate the gravity disturbance of a laying mechanism, comprising the steps of: determining a relationship model between the roller pressure and the cylinder control parameters of the laying mechanism; determining a set value of the cylinder pressure according to the relationship model; establishing a mathematical model of the gravity disturbance amount of the laying mechanism in different laying postures according to the structural form of the laying mechanism; determining a cylinder pressure compensation value according to the gravity disturbance amount; and compensating the disturbance amount for the set value of the cylinder pressure by using the cylinder pressure compensation value. The present invention also relates to a laying roller pressure control system for eliminating the gravity disturbance of the laying mechanism. The purpose of the method and system for controlling the pressure of the laying roller to eliminate the gravity disturbance of the laying mechanism is to solve the problem of how to improve the control accuracy of the roller pressure.

Description

Technical Field

[0001] The present invention relates to the technical field of automatic fiber placement of composite materials, and particularly relates to a method and system for controlling the pressure of a placement roller to eliminate the gravity disturbance of a placement mechanism. Background Art

[0002] The automatic fiber placement machine for composite materials has the characteristics of high speed and efficiency, high-quality placement, low waste rate, etc. It is the most important advanced composite material automatic forming and manufacturing equipment at present and is the key to realizing the manufacturing of large-size complex-shaped composite material structures. Therefore, it is widely used in the automatic forming of large composite material components in fields such as aerospace.

[0003] During the automatic placement and forming process of the fiber bundle, the placement roller pressure is a crucial process control parameter. The pressure roller of the placement mechanism presses the fiber bundle tightly onto the mold surface under the roller pressure set by the process, playing a role in leveling and compacting the laid layer, connecting between layers, and exhausting air, so as to eliminate the gaps between fibers in the width direction within the layer, increase the interlayer adhesion, and reduce the interlayer gap. The placement roller pressure needs to be set according to the requirements of the placement material and placement process, and the control accuracy is within a certain range (generally within ±5% of the set value). Too small roller pressure will cause problems such as the fiber bundle not being firmly placed and the fiber bundle falling off, while too large placement pressure will cause damage to the pressure roller of the placement mechanism and the fiber bundle itself.

[0004] Therefore, the inventor provides a method and system for controlling the pressure of a placement roller to eliminate the gravity disturbance of a placement mechanism. Summary of the Invention

[0005] (1) Technical Problems to be Solved

[0006] The embodiments of the present invention provide a method and system for controlling the pressure of a placement roller to eliminate the gravity disturbance of a placement mechanism, and solve the technical problem of how to improve the control accuracy of the roller pressure.

[0007] (2) Technical Solutions

[0008] The present invention provides a method for controlling the pressure of a placement roller to eliminate the gravity disturbance of a placement mechanism, including the following steps:

[0009] Determine the relationship model between the roller pressure and the cylinder control parameters of the placement mechanism;

[0010] According to the relationship model, determine the set value of the cylinder pressure;

[0011] According to the structural form of the placement mechanism, establish a mathematical model of the gravity disturbance amount of the placement mechanism in different placement postures;

[0012] According to the gravity disturbance amount, determine the cylinder pressure compensation value;

[0013] Compensate the disturbance amount of the cylinder pressure set value by using the cylinder pressure compensation value.

[0014] Further, the relationship model between the roll pressure and the cylinder control parameters of the laying mechanism is specifically as follows:

[0015] Determine the relationship model between the roll pressure and the cylinder control parameters according to the piston diameter, piston rod diameter, pressure in the rod chamber and pressure in the non-rod chamber of the cylinder.

[0016] Further, determining the cylinder pressure set value according to the relationship model is specifically as follows:

[0017] Determine the cylinder pressure set value according to the roll pressure, the piston diameter, the piston rod diameter and the pressure in the rod chamber.

[0018] Further, establishing a mathematical model of the gravity disturbance amount of the laying mechanism in different laying postures according to the structural form of the laying mechanism specifically includes the following steps:

[0019] When the wire laying mechanism is a machine tool type wire laying machine, determine the vector direction of the tip point of the laying mechanism through the multi-body kinematics principle according to the structural form of the machine tool and the real-time coordinates of each swing angle axis;

[0020] Calculate the mathematical model of the gravity disturbance amount of the laying mechanism according to the decomposition amount of the gravity of the laying mechanism in the vector direction of the tip point.

[0021] Further, determining the vector direction of the tip point of the laying mechanism through the multi-body kinematics principle according to the structural form of the machine tool and the real-time coordinates of each swing angle axis specifically includes:

[0022] When the wire laying mechanism is a machine tool type AC structure wire laying machine, determine the vector direction of the tip point according to the coordinate values of the A axis and the C axis;

[0023] When the wire laying mechanism is a machine tool type AB structure wire laying machine, determine the vector direction of the tip point according to the coordinate values of the A axis and the B axis.

[0024] Further, establishing a mathematical model of the gravity disturbance amount of the laying mechanism in different laying postures according to the structural form of the laying mechanism specifically includes the following steps:

[0025] When the wire laying mechanism is a robot type wire laying machine, determine the vector direction of the tip point of the laying mechanism through the forward kinematics of the robot according to the DH parameters of the robot and the real-time coordinates of each joint axis;

[0026] Calculate the mathematical model of the gravity disturbance amount of the laying mechanism according to the decomposition amount of the gravity of the laying mechanism in the vector direction of the tip point.

[0027] Furthermore, based on the D-H parameters of the robot and the real-time coordinates of each joint axis, the vector direction of the tool tip point of the laying mechanism is determined through the forward kinematics of the robot, which specifically includes the following steps:

[0028] According to the forward kinematics analysis of the robot, the transformation relationship of the pose of the laying mechanism at the end of the robot relative to the base coordinate system of the robot is calculated;

[0029] Using the homogeneous coordinate transformation method and D-H parameters, the homogeneous coordinate transformation matrix between adjacent joints of the robot is determined;

[0030] The corresponding homogeneous transformation matrices between every two adjacent joints are continuously multiplied to obtain the pose expression of the robot end coordinate system in its base coordinate system;

[0031] Based on the pose expression, the vector direction of the tool tip point is determined.

[0032] Furthermore, based on the mathematical model of the gravity disturbance amount, the cylinder pressure compensation value is determined, specifically:

[0033] Based on the gravity disturbance amount and the piston diameter, the cylinder pressure compensation value is determined.

[0034] Furthermore, the cylinder pressure set value is compensated for the disturbance amount by using the cylinder pressure compensation value, specifically:

[0035] The cylinder pressure compensation value is sent to the pressure controller through PLC communication, and the pressure controller realizes the real-time compensation of the gravity disturbance amount of the fiber placement head.

[0036] The present invention also provides a laying roller pressure control system for eliminating the gravity disturbance of the laying mechanism, including:

[0037] A model determination unit for determining the relationship model between the roller pressure and the cylinder control parameters of the laying mechanism;

[0038] A cylinder pressure determination unit for determining the cylinder pressure set value according to the relationship model;

[0039] A model establishment unit for establishing a mathematical model of the gravity disturbance amount of the laying mechanism in different laying postures according to the structural form of the laying mechanism;

[0040] A compensation value determination unit for determining the cylinder pressure compensation value according to the gravity disturbance amount;

[0041] A disturbance compensation unit for compensating the cylinder pressure set value for the disturbance amount by using the cylinder pressure compensation value.

[0042] (3) Beneficial effects

[0043] In summary, the present invention obtains the laying attitude of the fiber placement head, decomposes the gravity vector of the placement mechanism into the attitude vector of the fiber placement head, calculates the gravity disturbance amount of the placement mechanism, and eliminates the gravity disturbance through the real-time compensation adjustment of the cylinder pressure of the placement mechanism, thereby realizing the high-precision control of the pressure of the placement roller. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following will briefly introduce the drawings required to be used in the embodiments of the present invention. Obviously, the following described drawings are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0045] Figure 1 is a schematic flow chart of a method for controlling the pressure of a placement roller to eliminate the gravity disturbance of a placement mechanism provided by an embodiment of the present invention;

[0046] Figure 2 is a schematic structural diagram of a roller pressure control system of a placement mechanism of a composite material fiber placement device provided by an embodiment of the present invention;

[0047] Figure 3 is a schematic diagram of the decomposition of the gravity of a placement mechanism in the laying attitude provided by an embodiment of the present invention;

[0048] Figure 4 is a schematic flow chart of the compensation of the gravity disturbance amount of a placement mechanism provided by an embodiment of the present invention;

[0049] Figure 5 is a schematic structural diagram of a placement roller pressure control system for eliminating the gravity disturbance of a placement mechanism provided by an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0050] The following will further describe in detail the embodiments of the present invention in conjunction with the drawings and embodiments. The detailed description and drawings of the following embodiments are used to exemplarily illustrate the principles of the present invention, but cannot be used to limit the scope of the present invention, that is, the present invention is not limited to the described embodiments, and covers any modifications, replacements, and improvements of parts, components, and connection methods without departing from the spirit of the present invention.

[0051] It should be noted that, without conflict, the embodiments in the present application and the features in the embodiments can be combined with each other. The following will refer to the drawings and combine the embodiments to detail the present application.

[0052] Figure 1 is a schematic flow chart of a method for controlling the pressure of a placement roller to eliminate the gravity disturbance of a placement mechanism provided by an embodiment of the present invention, as Figure 1As shown, the method may include the following steps:

[0053] S100. Determine the relationship model between the roll pressure and the cylinder control parameters of the laying mechanism;

[0054] S200. Determine the set value of the cylinder pressure according to the relationship model;

[0055] S300. According to the structural form of the laying mechanism, establish a mathematical model of the gravity disturbance amount of the laying mechanism in different laying postures;

[0056] S400. Determine the cylinder pressure compensation value according to the gravity disturbance amount;

[0057] S500. Use the cylinder pressure compensation value to compensate the disturbance amount of the cylinder pressure set value.

[0058] In the above embodiment, based on the multi-body system kinematics theory, a mathematical model of the gravity disturbance amount of the laying mechanism in different laying postures is established, providing a theoretical basis for the high-precision control of the roll pressure during laying. By real-time reading the values of the coordinate axes of the fiber placement machine, calculating the laying posture and the gravity disturbance amount of the laying mechanism, and adjusting the cylinder pressure of the laying mechanism through the control system, the real-time compensation of the roll pressure is realized, improving the control accuracy of the roll pressure during the laying process, which has a very important role in the automatic fiber placement equipment for composite materials.

[0059] Aiming at the characteristics of the laying mechanism of the fiber placement machine, fully considering the influencing factors of the gravity of the laying mechanism on the roll pressure disturbance, a mathematical model of the gravity disturbance amount of the laying mechanism in different postures is established, which is applicable to the fiber placement equipment with vertical AC swing angle structure, vertical AB swing angle structure, horizontal AC swing angle structure, horizontal AB swing angle structure, and robot structure.

[0060] During the laying process of the fiber bundle, according to the characteristics of the material, the roll pressure of each fiber bundle generally needs to be controlled within 80N - 100N. The actuator of the roll pressure mainly consists of the roll pressure cylinder of the laying mechanism, the pressure roller, the guide rail, the proportional valve, etc. By controlling the air pressure of the roll pressure cylinder of the laying mechanism, the output of the roll pressure is realized. However, due to the inherent mass of the laying mechanism connected to the cylinder, the influence of the gravity of the laying mechanism on the roll pressure can reach ±500N in different laying postures of the fiber placement machine, seriously affecting the control accuracy of the roll pressure. Therefore, it is necessary to accurately obtain the laying posture of the fiber placement head, decompose the gravity vector of the laying mechanism into the posture vector of the fiber placement head, calculate the gravity disturbance amount of the laying mechanism, and eliminate the gravity disturbance through the real-time compensation adjustment of the cylinder pressure of the laying mechanism, so as to realize the high-precision control of the roll pressure.

[0061] As an alternative implementation, in step S100, a relationship model between the roller pressure and the cylinder control parameters of the laying mechanism is determined, specifically: based on the piston diameter, piston rod diameter, pressure in the rod chamber, and pressure in the non-rod chamber of the cylinder, a relationship model between the roller pressure and the cylinder control parameters is determined.

[0062] In the above implementation, the laying roller pressure is mainly set by the part laying program for the roller pressure variable F p to determine the roller pressure.

[0063] Among them, the execution system of the laying roller pressure mainly consists of a roller pressing cylinder, an electromagnetic proportional valve, a laying mechanism, etc. By controlling the air pressure of the laying roller pressing cylinder, the output of the laying roller pressure is realized. As Figure 2 shown in the structural schematic diagram of the laying mechanism roller pressure control system, the magnitude of the roller pressure is related to the piston diameter D of the cylinder, the piston rod diameter d, the air pressure P 1 in the cylinder chamber Q 1 (i.e., the non-rod chamber) and the air pressure P 2 in the cylinder chamber Q 2 (i.e., the rod chamber). Among them, P 1 is the main control air pressure for the roller pressure output, and P 2 is the air pressure for the laying mechanism to retract, generally a fixed value of 1 - 2 bar. The relationship between the roller pressure F p and the parameters of the laying roller pressing cylinder is shown in the following formula:

[0064]

[0065] As an alternative implementation, in step S200, based on the relationship model, the cylinder pressure setting value is determined, specifically:

[0066] Based on the roller pressure, piston diameter, piston rod diameter, and pressure in the rod chamber, the cylinder pressure setting value is determined.

[0067] In the above implementation, according to the above formula (1), the process setting value F p of the roller pressure can theoretically be achieved by controlling the cylinder pressure P 1 , and its calculation is as follows:

[0068]

[0069] Under the same control parameters, the laying mechanism connected to the cylinder has an inherent mass M H (generally about 50 kg). In different laying postures of the fiber placement machine, as Figure 4 shown, the gravity of the laying mechanism Seriously affect the control accuracy of the laying roller pressure.

[0070] As an alternative implementation, in step S300, according to the structural form of the laying mechanism, establish a mathematical model of the gravity disturbance amount of the laying mechanism in different laying postures, which specifically includes the following steps:

[0071] S3011. When the wire laying mechanism is a machine tool type wire laying machine, according to the structural form of the machine tool and the real-time coordinates of each swing angle axis, determine the vector direction of the tool tip point of the laying mechanism through the multi-body kinematics principle;

[0072] S3012. Based on the decomposition amount of the gravity of the laying mechanism in the vector direction of the tool tip point, calculate the mathematical model of the gravity disturbance amount of the laying mechanism.

[0073] As an alternative implementation, in step S3011, according to the structural form of the machine tool and the real-time coordinates of each swing angle axis, determine the vector direction of the tool tip point of the laying mechanism through the multi-body kinematics principle, which specifically includes:

[0074] When the wire laying mechanism is a machine tool type AC structure wire laying machine, determine the vector direction of the tool tip point according to the coordinate values of the A axis and the C axis;

[0075] When the wire laying mechanism is a machine tool type AB structure wire laying machine, determine the vector direction of the tool tip point according to the coordinate values of the A axis and the B axis.

[0076] Specifically, establish a coordinate system for the machine tool type wire laying machine. According to the structural form of the machine tool and the reading of the real-time coordinates of each swing angle axis, calculate the vector direction of the TCP (Tool center point) of the laying mechanism according to the multi-body kinematics principle Then through the gravity of the laying mechanism In the decomposition of the laying posture, calculate the gravity disturbance amount F of the laying mechanism G_M .

[0077] The swing angle structural forms of the machine tool type wire laying machine mainly include two structures, AC and AB. According to the corresponding homogeneous transformation matrix, the vector direction of the TCP point of the laying mechanism relative to the machine tool coordinate system can be calculated For the machine tool type wire laying machine, its main structure is divided into two types, vertical and horizontal. Therefore, the gravity of the laying mechanism Has different expressions in their respective machine tool coordinate systems, and its gravity disturbance amount needs to be modeled and calculated separately.

[0078] For the machine tool type AC structure wire laying machine, let α be the coordinate value of the A axis and γ be the coordinate value of the C axis. Therefore, the homogeneous transformation matrix T of the AC swing angle mechanism AC And the vector direction of the TCP point Are calculated as follows:

[0079]

[0080] And determine the homogeneous transformation matrix T of the AC swing angle mechanism AC The third column of which is the TCP point vector direction That is

[0081] For a filament winding machine with a machine tool type AB structure, let α be the coordinate value of the A axis and β be the coordinate value of the B axis. Therefore, the homogeneous transformation matrix T of the AB swing angle mechanism AB and the TCP point vector direction are calculated as follows:

[0082]

[0083]

[0084] And determine the homogeneous transformation matrix T of the AB swing angle mechanism AB The third column of which is the TCP point vector direction That is

[0085] For a machine tool type vertical filament winding machine, the vector of the gravity of its laying mechanism is:

[0086]

[0087] For a machine tool type horizontal filament winding machine, the vector of the gravity of the laying mechanism is:

[0088]

[0089] Therefore, the gravity disturbance of the laying mechanism is The decomposition of which in Let F G_M_VAC be the gravity disturbance of the laying mechanism of a vertical AC structure filament winding machine, F G_M_VAB be the gravity disturbance of the laying mechanism of a vertical AB structure filament winding machine, F G_M_HAC be the gravity disturbance of the laying mechanism of a horizontal AC structure filament winding machine, F G_M_HAB be the gravity disturbance of the laying mechanism of a horizontal AB structure filament winding machine. Then as Figure 3 shown, the relevant calculations are as follows:

[0090]

[0091]

[0092]

[0093]

[0094] As an alternative implementation, in step S300, according to the structural form of the laying mechanism, a mathematical model of the gravity disturbance amount of the laying mechanism in different laying postures is established, which specifically includes the following steps:

[0095] S3021. When the wire laying mechanism is a robotic filament winding machine, according to the D-H parameters of the robot and the real-time coordinates of each joint axis, the vector direction of the tip point of the laying mechanism is determined through the forward kinematics of the robot.

[0096] S3022. Based on the decomposition amount of the gravity of the laying mechanism in the vector direction of the tip point, the mathematical model of the gravity disturbance amount of the laying mechanism is calculated.

[0097] As an alternative implementation, in step S3021, according to the D-H parameters of the robot and the real-time coordinates of each joint axis, the vector direction of the tip point of the laying mechanism is determined through the forward kinematics of the robot, which specifically includes the following steps:

[0098] S30211. According to the forward kinematic analysis of the robot, calculate the transformation relationship of the pose of the laying mechanism at the end of the robot relative to the base coordinate system of the robot.

[0099] S30212. Using the homogeneous coordinate transformation method and D-H parameters, determine the homogeneous coordinate transformation matrix between adjacent joints of the robot.

[0100] S30213. Continuously multiply the homogeneous transformation matrices corresponding to every two adjacent joints to obtain the pose expression of the robot end coordinate system in its base coordinate system.

[0101] S30214. Based on the pose expression, determine the vector direction of the tip point.

[0102] Specifically, establish the coordinate system of the robotic filament winding machine. According to the D-H parameters of the robot and the real-time angles of each joint axis read, through the forward kinematics of the robot, calculate the vector direction of the TCP point of the laying mechanism Then through the gravity of the laying mechanism In the vector direction of the TCP point of the laying mechanism Decompose to calculate the gravity disturbance amount of the laying mechanism.

[0103] To simplify the calculation results, taking the D-H parameters of the KUKA KR480 six-axis robot as an example, calculate the gravity disturbance amount of the laying mechanism.

[0104] Table 1 Robot D-H Parameter Table

[0105]

[0106] According to the forward kinematics analysis of the robot, the transformation relationship of the pose of the end laying mechanism of the robot relative to the base coordinate system of the robot is calculated. Based on the homogeneous coordinate transformation method and the D-H parameter table, where α i-1 is the included angle between two adjacent joint axes of the robot, a i-1 is the link length between two adjacent joint axes, d i is the distance along the common axis direction of two adjacent joint axes, θ i is the joint angle of the robot, then the general expression of the coordinate transformation matrix between adjacent joints of the robot is as follows:

[0107]

[0108] According to the above homogeneous transformation formula and the D-H parameters of the robot, the specific homogeneous transformation matrices between adjacent joints of the robot can be obtained, that is By continuously multiplying the transformation matrices, the following formula can be obtained, that is, the pose expression of the robot end coordinate system in its base coordinate system:

[0109]

[0110] In the formula, n x , n y , n z , a x , a y , a z , b x , b y , b z , p x , p y , p z are the matrix elements of the homogeneous transformation matrix of the six-axis robot, and the calculation results are as follows:

[0111] n x =-cosθ 6 ×(sinθ 5 ×(cosθ 1 ×cosθ 2 ×sinθ 3 +cosθ 1 ×cosθ 3 ×sinθ 2 )-cosθ 5 ×(cosθ 4 ×(cosθ 1 ×cosθ 2 ×cosθ 3 -cosθ 1 ×sinθ 2 ×sinθ 3 )-sinθ1 × sinθ 4 )) + sinθ 6 × (sinθ 4 × (cosθ 1 × cosθ 2 × cosθ 3 - cosθ 1 × sinθ 2 × sinθ 3 ) + cosθ 4 × sinθ 1 );

[0112] n y = - cosθ 6 × (sinθ 5 × (cosθ 2 × sinθ 1 × sinθ 3 + cosθ 3 × sinθ 1 × sinθ 2 ) - cosθ 5 × (cosθ 4 × (cosθ 2 × cosθ 3 × sinθ 1 - sinθ 1 × sinθ 2 × sinθ 3 ) + cosθ 1 × sinθ 4 )) + sinθ 6 × (sinθ 4 × (cosθ 2 × cosθ 3 × sinθ 1 - sinθ 1 × sinθ 2 × sinθ 3 ) - cosθ 1 × cosθ 4 );

[0113] n z = cosθ 6 × (sinθ 5 × (sinθ 2 × sinθ 3 - cosθ 2 × cosθ 3 ) - cosθ 4 × cosθ 5 × (cosθ 2 × sinθ 3+cosθ 3 ×sinθ 2 ))+sinθ 4 ×sinθ 6 ×(cosθ 2 ×sinθ 3 +cosθ 3 ×sinθ 2 );

[0114] a x =sinθ 6 ×(sinθ 5 ×(cosθ 1 ×cosθ 2 ×sinθ 3 +cosθ 1 ×cosθ 3 ×sinθ 2 )-cosθ 5 ×(cosθ 4 ×(cosθ 1 ×cosθ 2 ×cosθ 3 -cosθ 1 ×sinθ 2 ×sinθ 3 )-sinθ 1 ×sinθ 4 ))-cosθ 6 ×(sinθ 4 ×(cosθ 1 ×cosθ 2 ×cosθ 3 -cosθ 1 ×sinθ 2 ×sinθ 3 )+cosθ 4 ×sinθ 1 );

[0115] a y =sinθ 6 ×(sinθ 5 ×(cosθ 2 ×sinθ 1 ×sinθ 3 +cosθ 3 ×sinθ 1 ×sinθ 2 )-cosθ 5 ×(cosθ 4 ×(cosθ 2 ×cosθ 3 ×sinθ 1 -sinθ 1×sinθ 2 ×sinθ 3 )+cosθ 1 ×cosθ 4 ))-cosθ 6 ×(sinθ 4 ×(cosθ 2 ×cosθ 3 ×sinθ 1 -sinθ 1 ×sinθ 2 ×sinθ 3 )-cosθ 1 ×cosθ 4 );

[0116] a z =cosθ 6 ×sinθ 4 ×(cosθ 2 ×sinθ 3 +cosθ 3 ×sinθ 2 )-sinθ 6 ×

[0117] sinθ 5 ×(sinθ 2 ×sinθ 3 -cosθ 2 ×cosθ 3 ))-cosθ 4 ×cosθ 5 ×(cosθ 2 ×sinθ 3 +cosθ 3 ×sinθ 2 );

[0118] b x =cosθ 5 ×(cosθ 1 ×cosθ 2 ×sinθ 3 +cosθ 1 ×cosθ 3 ×sinθ 2 )+sinθ 5 ×(cosθ 4 ×(cosθ 1 ×cosθ 2 ×cosθ 3 -cosθ 1 ×sinθ 2 ×sinθ 3 )-sinθ 1 ×sinθ4 )

[0119] b y = cosθ 5 ×(cosθ 2 ×sinθ 1 ×sinθ 3 - cosθ 3 ×sinθ 1 ×sinθ 2 ) + sinθ 5 ×(cosθ 4 ×(cosθ 2 ×cosθ 3 ×sinθ 1 - sinθ 1 ×sinθ 2 ×sinθ 3 ) + cosθ 1 ×sinθ 4 )

[0120] b z = - cosθ 5 ×(sinθ 2 ×sinθ 3 - cosθ 2 ×cosθ 3 ) - cosθ 4 ×sinθ 5 ×(cosθ 2 ×sinθ 3 + cosθ 3 ×sinθ 2 )

[0121] Therefore, the vector direction of the TCP point of the robotic laying mechanism is as follows: As follows:

[0122]

[0123] For a robotic fiber placement machine, the vector of the gravity of its laying mechanism is: As follows:

[0124]

[0125] Therefore, the gravity disturbance of the laying mechanism is the decomposition of the gravity in the coordinate system. Let F be the gravity disturbance of the laying mechanism of the robotic fiber placement machine. Then, as shown in the figure, the calculation of the gravity disturbance F of the robotic laying mechanism is as follows: In of the coordinate system. Let F G_R be the gravity disturbance of the laying mechanism of the robotic fiber placement machine. Then, as shown in the figure, the calculation of the gravity disturbance F of the robotic laying mechanism is as follows: Figure 3 As shown in the figure, the gravity disturbance F of the robotic laying mechanism G_R is calculated as follows:

[0126]

[0127] As an alternative implementation, in step S400, according to the mathematical model of the gravity disturbance amount, the cylinder pressure compensation value is determined, specifically: according to the gravity disturbance amount and the piston diameter, the cylinder pressure compensation value is determined.

[0128] Specifically, according to the gravity disturbance amount F of the laying mechanism G Calculate the compensation value ΔP of the cylinder pressure of the laying mechanism 1 :

[0129]

[0130] In the formula, F G It should be selected according to the structural form of the fiber placement machine, and the corresponding F G_M_VAC , F G_M_VAB , F G_M_HAC , F G_M_HAB ,

[0131] As an alternative implementation, in step S500, the cylinder pressure set value is compensated for the disturbance amount by using the cylinder pressure compensation value, specifically: the cylinder pressure compensation value is sent to the pressure controller through PLC communication, and the pressure controller realizes the real-time compensation of the gravity disturbance amount of the fiber placement head.

[0132] Specifically, considering the control air pressure P' of the rodless cavity Q of the laying roller pressing cylinder with the gravity disturbance amount F G The control air pressure P' of the rodless cavity Q of the laying roller pressing cylinder with the gravity disturbance amount F 1 Is composed of the theoretical calculated air pressure P of the roller pressure and the compensation air pressure ΔP of the gravity disturbance amount of the laying mechanism, so as to realize the disturbance amount compensation of the laying roller pressure, and the calculation is as follows: 1 The control air pressure P' of the rodless cavity Q of the laying roller pressing cylinder with the gravity disturbance amount F 1 And the compensation air pressure ΔP of the gravity disturbance amount of the laying mechanism 1 Is composed of the theoretical calculated air pressure P of the roller pressure and the compensation air pressure ΔP of the gravity disturbance amount of the laying mechanism, so as to realize the disturbance amount compensation of the laying roller pressure, and the calculation is as follows:

[0133]

[0134] The compensation process of the gravity disturbance amount is as Figure 4 Shown. First, it is judged on the numerical control system whether the fiber placement equipment is a robotic fiber placement machine. If it is a robotic fiber placement machine, the corresponding 6 joint axis coordinate values will be read in real time, and according to the mathematical model of the gravity disturbance amount F of the fiber placement head G_R The numerical value of the gravity disturbance amount of the fiber placement head is calculated in real time. If it is an AB structure horizontal fiber placement machine, the coordinate values of the machine tool A and B axes will be read in real time, and according to the mathematical model of the gravity disturbance amount F of the fiber placement head G_M_HAB The numerical value of the gravity disturbance amount of the fiber placement head is calculated in real time; if it is an AC structure horizontal fiber placement machine, the coordinate values of the machine tool A and C axes will be read in real time, and according to the mathematical model of the gravity disturbance amount F of the fiber placement head G_M_HACA mathematical model is used to calculate the numerical value of the gravity disturbance amount of the fiber placement head in real time. If it is a vertical fiber placement machine with an AB structure, the coordinate values of the A and B axes of the machine tool will be read in real time, and based on the gravity disturbance amount F of the fiber placement head G_M_VAB A mathematical model is used to calculate the numerical value of the gravity disturbance amount of the fiber placement head in real time. If it is a vertical fiber placement machine with an AC structure, the coordinate values of the A and C axes of the machine tool will be read in real time, and based on the gravity disturbance amount F of the fiber placement head G_M_VAC A mathematical model is used to calculate the numerical value of the gravity disturbance amount of the fiber placement head in real time. Finally, the compensation value of the gravity disturbance amount of the fiber placement head is sent to the pressure controller through PLC communication, and the pressure controller is used to realize the real-time compensation of the gravity disturbance amount of the fiber placement head

[0135] Figure 5 FIG. is a schematic structural diagram of a laying roller pressure control system for eliminating the gravity disturbance of a laying mechanism provided by an embodiment of the present invention, as Figure 5 shown, the system may include:

[0136] A model determination unit 100 for determining a relationship model between the roller pressure and the cylinder control parameters of the laying mechanism

[0137] A cylinder pressure determination unit 200 for determining the set value of the cylinder pressure according to the relationship model

[0138] A model establishment unit 300 for establishing a mathematical model of the gravity disturbance amount of the laying mechanism in different laying postures according to the structural form of the laying mechanism

[0139] A compensation value determination unit 400 for determining the cylinder pressure compensation value according to the gravity disturbance amount

[0140] A disturbance compensation unit 500 for compensating the disturbance amount of the cylinder pressure set value by using the cylinder pressure compensation value

[0141] It should be clear that the various embodiments in this specification are described in a progressive manner, and the same or similar parts between the various embodiments can be referred to each other. The key point of each embodiment is to illustrate the differences from other embodiments. The present invention is not limited to the specific steps and structures described above and shown in the figures. And, for the sake of brevity, the detailed description of known method technologies is omitted here

[0142] The above are only the embodiments of the present application and do not limit the present application. For those skilled in the art, the present application can have various changes and modifications without departing from the scope of the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present application shall be included within the scope of the claims of the present application

Claims

1. A method for controlling the pressure of a laying roller to eliminate the gravity disturbance of a laying mechanism, characterized in that, the method comprises the following steps: Determine the relationship model between the roller pressure and the cylinder control parameters of the laying mechanism; Determine the set value of the cylinder pressure according to the relationship model; According to the structural form of the laying mechanism, establish a mathematical model of the gravity disturbance amount of the laying mechanism in different laying postures; Determine the cylinder pressure compensation value according to the gravity disturbance amount; Use the cylinder pressure compensation value to compensate the disturbance amount of the cylinder pressure set value; The step of establishing a mathematical model of the gravity disturbance amount of the laying mechanism in different laying postures according to the structural form of the laying mechanism specifically includes the following steps: When the wire laying mechanism is a machine tool type wire laying machine, according to the structural form of the machine tool and the real-time coordinates of each swing angle axis, determine the vector direction of the tip point of the laying mechanism through the principle of multi-body kinematics; Calculate the mathematical model of the gravity disturbance amount of the laying mechanism according to the decomposition amount of the gravity of the laying mechanism in the vector direction of the tip point.

2. The method for controlling the pressure of a laying roller to eliminate the gravity disturbance of a laying mechanism according to claim 1, characterized in that, the step of determining the relationship model between the roller pressure and the cylinder control parameters of the laying mechanism is specifically: Determine the relationship model between the roller pressure and the cylinder control parameters according to the piston diameter, piston rod diameter, rod chamber air pressure and non-rod chamber air pressure of the cylinder.

3. The method for controlling the pressure of a laying roller to eliminate the gravity disturbance of a laying mechanism according to claim 2, characterized in that, the step of determining the set value of the cylinder pressure according to the relationship model is specifically: Determine the set value of the cylinder pressure according to the roller pressure, the piston diameter, the piston rod diameter and the rod chamber air pressure.

4. The method for controlling the pressure of a laying roller to eliminate the gravity disturbance of a laying mechanism according to claim 1, characterized in that, the step of determining the vector direction of the tip point of the laying mechanism through the principle of multi-body kinematics according to the structural form of the machine tool and the real-time coordinates of each swing angle axis specifically includes: When the wire laying mechanism is a machine tool type AC structure wire laying machine, determine the vector direction of the tip point according to the coordinate values of the A axis and the C axis; When the wire laying mechanism is a machine tool type AB structure wire laying machine, determine the vector direction of the tip point according to the coordinate values of the A axis and the B axis.

5. The method for controlling the pressure of a laying roller to eliminate the gravity disturbance of a laying mechanism according to claim 1, characterized in that, the step of establishing a mathematical model of the gravity disturbance amount of the laying mechanism in different laying postures according to the structural form of the laying mechanism specifically includes the following steps: When the wire laying mechanism is a robot type wire laying machine, determine the vector direction of the tip point of the laying mechanism through the forward kinematics of the robot according to the D-H parameters of the robot and the real-time coordinates of each joint axis; Calculate the mathematical model of the gravity disturbance amount of the laying mechanism according to the decomposition amount of the gravity of the laying mechanism in the vector direction of the tip point.

6. The method for controlling the pressure of a laying roller to eliminate the gravity disturbance of a laying mechanism according to claim 5, characterized in that, According to the D-H parameters of the robot and the real-time coordinates of each joint axis, the vector direction of the tool tip point of the laying mechanism is determined by the forward kinematics of the robot, specifically including the following steps: According to the forward kinematics analysis of the robot, calculate the transformation relationship of the pose of the laying mechanism at the end of the robot relative to the base coordinate system of the robot; Using the homogeneous coordinate transformation method and D-H parameters, determine the homogeneous coordinate transformation matrix between adjacent joints of the robot; Continuously multiply the corresponding homogeneous transformation matrices between every two adjacent joints to obtain the pose expression of the robot end coordinate system in its base coordinate system; According to the pose expression, determine the vector direction of the tool tip point.

7. The laying roller pressure control method for eliminating the gravity disturbance of the laying mechanism according to claim 1, characterized in that, the cylinder pressure compensation value is determined according to the mathematical model of the gravity disturbance amount, specifically: The cylinder pressure compensation value is determined according to the gravity disturbance amount and the piston diameter.

8. The laying roller pressure control method for eliminating the gravity disturbance of the laying mechanism according to claim 1, characterized in that, the disturbance amount compensation is performed on the cylinder pressure setting value by using the cylinder pressure compensation value, specifically: The cylinder pressure compensation value is sent to the pressure controller through PLC communication, and the pressure controller realizes the real-time compensation of the gravity disturbance amount of the fiber placement head.

9. A laying roller pressure control system for eliminating the gravity disturbance of the laying mechanism, characterized in that, including: A model determination unit for determining the relationship model between the roller pressure and the cylinder control parameters of the laying mechanism; A cylinder pressure determination unit for determining the cylinder pressure setting value according to the relationship model; A model establishment unit for establishing a mathematical model of the gravity disturbance amount of the laying mechanism in different laying postures according to the structural form of the laying mechanism; A compensation value determination unit for determining the cylinder pressure compensation value according to the gravity disturbance amount; A disturbance compensation unit for performing disturbance amount compensation on the cylinder pressure setting value by using the cylinder pressure compensation value; The model establishment unit is specifically used for: When the fiber placement mechanism is a machine tool type fiber placement machine, according to the structural form of the machine tool and the real-time coordinates of each swing angle axis, determine the vector direction of the tool tip point of the laying mechanism through the multi-body kinematics principle; According to the decomposition amount of the gravity of the laying mechanism in the vector direction of the tool tip point, calculate the mathematical model of the gravity disturbance amount of the laying mechanism.

Images