A cross-coupling multi-axis synchronous interpolation control method based on PLCopen

Abstract

The present invention discloses a PLCopen-based cross-coupling multi-axis synchronous interpolation control method, encapsulated in a PLCopen motion control function block. The method comprises: determining the coordinates of the starting position of the multi-axis robot's end effector in the Cartesian coordinate system OXY using a forward kinematic transformation based on the acquired initial positions of the joints of each axis of the multi-axis robot; calculating the total interpolation time and real-time theoretical position of the multi-axis robot's end effector using an S-type acceleration and deceleration algorithm based on the coordinates of the starting position and the coordinates of the end point of the target trajectory, where the real-time theoretical position is the coordinate of the current interpolation position; and performing real-time compensation for the interpolation process using a cross-coupling control algorithm until the total interpolation time is completed and the process is terminated. The method can compensate for contour errors generated during the synchronous motion of each axis of the multi-axis robot, improving interpolation accuracy. The method also complies with PLCopen specifications, has strong versatility, and is widely applicable.

Description

Technical Field

[0001] The present invention belongs to the field of multi-axis motion control, and in particular relates to a cross-coupling multi-axis synchronous interpolation control method based on PLCopen. Background Art

[0002] With the continuous advancement of industrial automation technology, motion control is widely used in various multi-axis equipment, such as CNC machine tools and industrial robots. However, the field is plagued by a wide variety of motion control products, each with its own programming language and product specifications. This results in poor compatibility between motion control products from different manufacturers. To address this issue, the current state of the art has proposed a unified programming standard, IEC61131-3, and established the PLCopen motion control function block standard. Using the PLCopen motion control function block standard improves the compatibility, openness, and reusability of motion control systems, reduces development costs, and enables widespread adoption across the entire industrial control field.

[0003] Interpolation is an important part of the motion control system. It densifies the data points of the planned contour trajectory to generate a series of discrete points that approximate the target contour trajectory. The information of these discrete points is sent to the servo driver, which drives the end effector to perform the predetermined action. The quality of the interpolation algorithm will affect the final operation effect.

[0004] In applications where interpolation accuracy is less demanding, open-loop control is often used. However, implementing interpolation requires coordinated, synchronized motion across multiple axes to ensure the end effector follows a preset trajectory. If a single axis's control only considers its own tracking error without considering the errors of other axes, the resulting trajectory is susceptible to significant errors. This is especially true at high speeds. Due to differences in the motion parameters of each axis and external disturbances, there will be a certain degree of contour error between the actual trajectory and the preset ideal trajectory. Therefore, to achieve high-precision interpolation and compensate for the resulting contour errors, a multi-axis synchronous interpolation method based on cross-coupling control using PLCopen is proposed. Summary of the Invention

[0005] The purpose of the present invention is to address the above-mentioned problems and propose a cross-coupling multi-axis synchronous interpolation control method based on PLCopen. The cross-coupling control is used to compensate for the contour errors generated by the synchronous motion of the axes of a multi-axis robot, thereby improving the interpolation accuracy. At the same time, the method complies with the PLCopen specification, has strong versatility, and has a wide range of applications.

[0006] To achieve the above object, the technical solution adopted by the present invention is:

[0007] The present invention proposes a PLCopen-based cross-coupling multi-axis synchronous interpolation control method, which is encapsulated in a PLCopen motion control function block and includes the following steps:

[0008] S1. Based on the initial positions of the joints of each axis of the multi-axis robot, the starting position of the end effector of the multi-axis robot in the Cartesian coordinate system OXY is determined by using the forward kinematic transformation. Coordinates ;

[0009] S2, according to the starting position Coordinates and the end point of the target trajectory Coordinates , use the S-type acceleration and deceleration algorithm to calculate the total interpolation time of the end effector of the multi-axis robot and real-time theoretical position, the real-time theoretical position is the current interpolation position Coordinates ;

[0010] S3, use cross-coupling control algorithm to perform real-time compensation during interpolation process until the total interpolation time is completed End the process and use the cross-coupling control algorithm to perform real-time compensation during the interpolation process as follows:

[0011] S31, according to the current interpolation position The theoretical joint angles of each axis of the multi-axis robot are obtained by inverse kinematics transformation;

[0012] S32, the theoretical joint angle of each axis and the actual joint angle output by the corresponding axis are fed back through a first PID controller and then acted on the corresponding axis to reach the corresponding position;

[0013] S33, according to the position reached by the corresponding axis, the actual position of the end effector of the multi-axis robot in the Cartesian coordinate system OXY is obtained through the positive kinematic transformation Coordinates ;

[0014] S34, establish the actual position during interpolation With the current interpolation position The contour error between The formula to calculate the contour error in real time ;

[0015] S35, according to the contour error The formula corresponds to , calculate the corresponding X-axis cross-coupling gain and the cross-coupling gain of the Y axis ;

[0016] S36, contour error Input the cross-coupling controller and set the cross-coupling gain of the X-axis , cross-coupling gain of the Y axis The output u of the cross-coupling controller is compensated to the current interpolated position of the end effector of the multi-axis robot Get the actual input of the multi-axis robot. The actual input of the multi-axis robot satisfies the following formula:

[0017] ;

[0018] Where, is the actual input X-axis coordinate, The Y-axis coordinate actually input.

[0019] Preferably, based on the starting position Coordinates and the end point of the target trajectory Coordinates , use the S-type acceleration and deceleration algorithm to calculate the total interpolation time of the end effector of the multi-axis robot and real-time theoretical position, the real-time theoretical position is the current interpolation position Coordinates , as follows:

[0020] 1) When the target trajectory is a straight line, perform the following operations:

[0021] The straight line segment is obtained from the distance formula between two points Length , and thus calculate the straight line segment The unit vector ,in, is the motion component in the X-axis direction, is the motion component in the Y-axis direction;

[0022] According to the straight line The length L and the straight line segment The unit vector , use the S-type acceleration and deceleration algorithm to calculate the total interpolation time And obtain the displacement and time relationship of the end effector of the multi-axis robot;

[0023] According to the principle of linear interpolation, the current interpolation position can be obtained Coordinates :

[0024] ;

[0025] 2) When the target trajectory is an arc trajectory, perform the following operations:

[0026] Use atan2 function to calculate the starting angle of the arc trajectory and end angle , where the starting angle The center of the arc trajectory Point to the starting position The angle between the vector and the positive direction of the X axis in the Cartesian coordinate system OXY, the end angle The center of the arc trajectory Point to the end point of the arc trajectory The angle between the vector and the positive direction of the X axis in the Cartesian coordinate system OXY, the starting angle and end angle The angle range is ;

[0027] Calculate the total angular displacement based on the arc trajectory running direction , as follows:

[0028] If the arc trajectory is clockwise, then

[0029] ;

[0030] If the arc trajectory is counterclockwise, then

[0031] ;

[0032] According to the arc segment Length , and the arc segment Radius , use the S-type acceleration and deceleration algorithm to calculate the total interpolation time And obtain the displacement and time relationship of the end effector of the multi-axis robot;

[0033] According to the arc interpolation principle, the current interpolation position can be obtained Coordinates for:

[0034] ;

[0035] in,

[0036] ;

[0037] Arc trajectory at the current moment Angle The calculation of is as follows:

[0038] If the arc trajectory is clockwise, then

[0039] ;

[0040] If the arc trajectory is counterclockwise, then

[0041] ;

[0042] Where, From time zero to the current time The angular displacement of the arc, For the current moment displacement.

[0043] Preferably, the S-shaped acceleration and deceleration algorithm is divided into seven stages: acceleration stage, uniform acceleration stage, deceleration stage, uniform speed stage, acceleration and deceleration stage, uniform deceleration stage, and deceleration and deceleration stage, wherein:

[0044] Total interpolation time The calculation is as follows:

[0045] Since the running time of acceleration stage, uniform acceleration stage, deceleration stage, uniform speed stage, acceleration and deceleration stage, uniform deceleration stage, and deceleration and deceleration stage corresponds to 、 、 、 、 、 、 , and satisfy:

[0046]

[0047]

[0048]

[0049]

[0050]

[0051]

[0052]

[0053] The speeds of the acceleration stage, uniform acceleration stage, deceleration stage, uniform speed stage, acceleration and deceleration stage, uniform deceleration stage, and deceleration and deceleration stage correspond to the following: 、 、 、 、 、 、 , and satisfy:

[0054]

[0055]

[0056]

[0057]

[0058]

[0059]

[0060]

[0061] The displacements of acceleration stage, uniform acceleration stage, deceleration stage, uniform speed stage, acceleration and deceleration stage, uniform deceleration stage, and deceleration and deceleration stage correspond to the following: 、 、 、 、 、 、 , and satisfy:

[0062]

[0063]

[0064]

[0065]

[0066]

[0067]

[0068]

[0069]

[0070]

[0071] In summary, we can solve , the formula is as follows:

[0072]

[0073] Thus, the end time of the acceleration phase, uniform acceleration phase, deceleration phase, uniform speed phase, acceleration and deceleration phase, uniform deceleration phase and deceleration phase can be calculated. 、 、 、 、 、 、 , get the total interpolation time ;

[0074] The relationship between the displacement and time of the end effector of the multi-axis robot is as follows:

[0075]

[0076] Where, For the current moment The displacement, is the maximum jerk, is the maximum acceleration, is the maximum deceleration, is the maximum speed, is the starting speed, is the terminal velocity, which is zero, L is the total displacement, is the total displacement of the acceleration section, is the total displacement of the deceleration stage. The acceleration stage includes the acceleration stage, the uniform acceleration stage and the deceleration stage. The deceleration stage includes the acceleration and deceleration stage, the uniform deceleration stage and the deceleration stage.

[0077] Preferably, the atan2 function is used to calculate the starting angle of the arc trajectory and end angle Before, also perform the following operations:

[0078] Determine whether it is satisfied If it does not meet the requirements, it is considered not an arc trajectory, an error warning is issued and the process ends. If it meets the requirements, it is considered an arc trajectory, and the atan2 function is used to calculate the starting angle of the arc trajectory. and end angle ,in, Starting position Coordinates To the center of the arc trajectory Coordinates distance, The end point of the arc trajectory Coordinates To the center Coordinates distance, The default minimum value.

[0079] Preferably, the actual position during the interpolation process is established With the current interpolation position The contour error between The formula to calculate the contour error in real time , as follows:

[0080] because,

[0081] ;

[0082] ;

[0083] Where, is the X-axis direction error in the Cartesian coordinate system OXY, is the Y-axis direction error in the Cartesian coordinate system OXY;

[0084] When the target trajectory is a straight line trajectory, the contour error The formula is as follows:

[0085] ;

[0086] Where, is the angle between the straight line trajectory and the positive direction of the X axis in the Cartesian coordinate system OXY;

[0087] When the target trajectory is a circular arc trajectory, the contour error The formula is as follows:

[0088]

[0089] in,

[0090] ;

[0091] Contour error of arc trajectory The Taylor formula expansion of the formula can be obtained:

[0092] ;

[0093] Where, It represents the expansion of higher-order terms, i.e., second-order and above;

[0094] Ignore higher-order terms , the contour error The formula is simplified to:

[0095] .

[0096] Preferably, the cross-coupling controller is a second PID controller and satisfies the following formula:

[0097]

[0098] Where, is the output of the cross-coupling controller in the i-th interpolation cycle, is the proportionality coefficient, is the integration coefficient, is the differential coefficient, is the interpolation period, is the contour error of the i-th interpolation cycle, is the contour error of the i-1th interpolation cycle, .

[0099] Preferably, the cross-coupling gain of the X-axis and the cross-coupling gain of the Y axis The calculation is as follows:

[0100] For a straight trajectory:

[0101] ,

[0102] For circular arc trajectories:

[0103] , .

[0104] Preferably, the PLCopen motion control function blocks are as follows:

[0105] When the target trajectory is a straight line, the PLCopen motion control function block is a linear interpolation function block. The input pins of the linear interpolation function block include the axis group pin AxisGroup, the start pin Execute, the end position pin Position, the velocity pin Velocity, the acceleration pin Acceleration, the deceleration pin Deceleration, the jerk pin Jerk, and the buffer mode pin BufferMode. The output pins include the completion flag pin Done, the command execution status pin Busy, the command control axis status pin Active, the command interrupt pin CommandAborted, the error status pin Error, and the error code pin ErrorID.

[0106] When the target trajectory is a circular arc, the PLCopen motion control function block is a circular interpolation function block. The input pins of the circular interpolation function block include the axis group pin (AxisGroup), the start pin (Execute), the center coordinate pin (AuxPoint), the end coordinate pin (EndPoint), the path direction pin (PathChoice), the speed pin (Velocity), the acceleration pin (Acceleration), the deceleration pin (Deceleration), the jerk pin (Jerk), and the buffer mode pin (BufferMode). The output pins include the completion flag pin (Done), the command execution status pin (Busy), the command control axis status pin (Active), the command interrupt pin (CommandAborted), the error status pin (Error), and the error code pin (ErrorID).

[0107] Among them, the axis group pin AxisGroup is used to specify the ID of the axis required to participate in the motion, the start pin Execute is used to enable to trigger the interpolation motion signal, the center coordinate pin AuxPoint is used to specify the coordinates of the center of the arc trajectory, the end point coordinate pin EndPoint is used to specify the coordinates of the end point of the arc trajectory, the path direction pin PathChoice is used to determine whether the arc trajectory runs clockwise or counterclockwise, the end position pin Position is used to specify the coordinates of the end point of the straight line trajectory, and the speed pin Velocity is used to specify the maximum speed , that is, the speed in the uniform speed stage, the acceleration pin Acceleration is used to specify the maximum acceleration The Deceleration pin is used to specify the maximum deceleration , the acceleration pin Jerk is used to specify the maximum jerk The cache mode pin BufferMode is the cache mode between two instructions, the completion flag pin Done is used to mark the interpolation motion completion status, the instruction execution status pin Busy is used to mark the instruction execution status, the instruction control axis status pin Active is used to mark the motion status of the axis of the multi-axis robot, the instruction interrupt pin CommandAborted is used to mark the status of the interruption when the instruction is not completed, the error status pin Error is used to mark the ID instruction execution status of the axis required to participate in the motion, and the error code pin Error is used to output the error code.

[0108] Compared with the prior art, the present invention has the following beneficial effects:

[0109] During the interpolation motion process, this method uses the feedback data of the actual position of each axis of the multi-axis robot to obtain the actual position of the end effector in the Cartesian space after forward kinematic transformation, calculates the distance between the actual position and the target trajectory in real time to obtain the contour error, and compensates the contour error to the desired interpolation position in real time through the cross-coupling control algorithm. The interpolation position in Cartesian coordinates is transformed through inverse kinematics to obtain the joint angle of each axis of the multi-axis robot, which is then output by the PID controller and sent to the corresponding axis to achieve closed-loop control. This can effectively reduce the contour error and improve the trajectory interpolation accuracy. The interpolation algorithm is encapsulated into a function block that complies with the PLCopen specification, which can be easily transplanted to industrial robot control. It has good versatility and a wide range of applications. BRIEF DESCRIPTION OF THE DRAWINGS

[0110] Figure 1 This is a flow chart of the cross-coupling multi-axis synchronous interpolation control method based on PLCopen of the present invention;

[0111] Figure 2Schematic diagram of the linear interpolation principle (a) and the circular interpolation principle (b) in step S2 of the present invention;

[0112] Figure 3 Schematic diagram (a) of the calculation principle of the contour error of a straight line trajectory and schematic diagram (b) of the calculation principle of the contour error of a circular arc trajectory in step S3 of the present invention;

[0113] Figure 4 The circuit diagram of the linear interpolation function block (a) and the circular interpolation function block (b) of the present invention;

[0114] Figure 5 This is a principle block diagram of the cross-coupling control algorithm in step S3 of the present invention. DETAILED DESCRIPTION

[0115] The following will be combined with the drawings in the embodiments of this application to clearly and completely describe the technical solutions in the embodiments of this application. Obviously, the embodiments described are only part of the embodiments of this application, not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of this application.

[0116] It should be noted that, unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the art in the art of this application. The terms used herein in the specification of this application are only for the purpose of describing specific embodiments and are not intended to limit this application.

[0117] like Figure 1-5 As shown, a cross-coupling multi-axis synchronous interpolation control method based on PLCopen is encapsulated in a PLCopen motion control function block and includes the following steps:

[0118] S1. Based on the initial positions of the joints of each axis of the multi-axis robot, the starting position of the end effector of the multi-axis robot in the Cartesian coordinate system OXY is determined by using the forward kinematic transformation. Coordinates .

[0119] S2, according to the starting position Coordinates and the end point of the target trajectory Coordinates , use the S-type acceleration and deceleration algorithm to calculate the total interpolation time of the end effector of the multi-axis robot and real-time theoretical position, the real-time theoretical position is the current interpolation position Coordinates .

[0120] In one embodiment, based on the starting position Coordinates and the end point of the target trajectory Coordinates , use the S-type acceleration and deceleration algorithm to calculate the total interpolation time of the end effector of the multi-axis robot and real-time theoretical position, the real-time theoretical position is the current interpolation position Coordinates , as follows:

[0121] 1) When the target trajectory is a straight line, perform the following operations:

[0122] The straight line segment is obtained from the distance formula between two points Length , and thus calculate the straight line segment The unit vector ,in, is the motion component in the X-axis direction, is the motion component in the Y-axis direction;

[0123] According to the straight line The length L and the straight line segment The unit vector , use the S-type acceleration and deceleration algorithm to calculate the total interpolation time And obtain the displacement and time relationship of the end effector of the multi-axis robot;

[0124] According to the principle of linear interpolation, the current interpolation position can be obtained Coordinates :

[0125] ;

[0126] 2) When the target trajectory is an arc trajectory, perform the following operations:

[0127] Use atan2 function to calculate the starting angle of the arc trajectory and end angle , where the starting angle The center of the arc trajectory Point to the starting position The angle between the vector and the positive direction of the X axis in the Cartesian coordinate system OXY, the end angle The center of the arc trajectory Point to the end point of the arc trajectory The angle between the vector and the positive direction of the X axis in the Cartesian coordinate system OXY, the starting angle and end angle The angle range is ;

[0128] Calculate the total angular displacement based on the arc trajectory running direction , as follows:

[0129] If the arc trajectory is clockwise, then

[0130] ;

[0131] If the arc trajectory is counterclockwise, then

[0132] ;

[0133] According to the arc segment Length , and the arc segment Radius , use the S-type acceleration and deceleration algorithm to calculate the total interpolation time And obtain the displacement and time relationship of the end effector of the multi-axis robot;

[0134] According to the arc interpolation principle, the current interpolation position can be obtained Coordinates for:

[0135] ;

[0136] in, ;

[0137] Arc trajectory at the current moment Angle The calculation of is as follows:

[0138] If the arc trajectory is clockwise, then ;

[0139] If the arc trajectory is counterclockwise, then ;

[0140] Where, From time zero to the current time The angular displacement of the arc, For the current moment displacement.

[0141] In one embodiment, the S-shaped acceleration / deceleration algorithm is divided into seven stages: acceleration stage, uniform acceleration stage, deceleration stage, uniform speed stage, acceleration / deceleration stage, uniform deceleration stage, and deceleration / deceleration stage, wherein:

[0142] Total interpolation time The calculation is as follows:

[0143] Since the running time of acceleration stage, uniform acceleration stage, deceleration stage, uniform speed stage, acceleration and deceleration stage, uniform deceleration stage, and deceleration and deceleration stage corresponds to 、 、 、 、 、 、 , and satisfy:

[0144]

[0145]

[0146]

[0147]

[0148]

[0149]

[0150]

[0151] The speeds of the acceleration stage, uniform acceleration stage, deceleration stage, uniform speed stage, acceleration and deceleration stage, uniform deceleration stage, and deceleration and deceleration stage correspond to the following: 、 、 、 、 、 、 , and satisfy:

[0152]

[0153]

[0154]

[0155]

[0156]

[0157]

[0158]

[0159] The displacements of acceleration stage, uniform acceleration stage, deceleration stage, uniform speed stage, acceleration and deceleration stage, uniform deceleration stage, and deceleration and deceleration stage correspond to the following: 、 、 、 、 、 、 , and satisfy:

[0160]

[0161]

[0162]

[0163]

[0164]

[0165]

[0166]

[0167]

[0168]

[0169] In summary, we can solve , the formula is as follows:

[0170]

[0171] Thus, the end time of the acceleration phase, uniform acceleration phase, deceleration phase, uniform speed phase, acceleration and deceleration phase, uniform deceleration phase and deceleration phase can be calculated. 、 、 、 、 、 、 , get the total interpolation time ;

[0172] The relationship between the speed and time of the end effector of a multi-axis robot is as follows:

[0173]

[0174] The relationship between the displacement and time of the end effector of the multi-axis robot is as follows:

[0175]

[0176] Where, For the current moment speed, For the current moment The displacement, is the maximum jerk, is the maximum acceleration, is the maximum deceleration, is the maximum speed, is the starting speed, is the terminal velocity, which is zero, L is the total displacement, is the total displacement of the acceleration section, is the total displacement of the deceleration stage. The acceleration stage includes the acceleration stage, the uniform acceleration stage and the deceleration stage. The deceleration stage includes the acceleration and deceleration stage, the uniform deceleration stage and the deceleration stage.

[0177] In one embodiment, the atan2 function is used to calculate the starting angle of the arc trajectory. and end angle Before, also perform the following operations:

[0178] Determine whether it is satisfied If it does not meet the requirements, it is considered not an arc trajectory, an error warning is issued and the process ends. If it meets the requirements, it is considered an arc trajectory, and the atan2 function is used to calculate the starting angle of the arc trajectory. and end angle ,in, Starting position Coordinates To the center of the arc trajectory Coordinates distance, The end point of the arc trajectory Coordinates To the center Coordinates distance, The default minimum value.

[0179] S3, use cross-coupling control algorithm to perform real-time compensation during interpolation process until the total interpolation time is completed End the process and use the cross-coupling control algorithm to perform real-time compensation during the interpolation process as follows:

[0180] S31, according to the current interpolation position The theoretical joint angles of each axis of the multi-axis robot are obtained by inverse kinematics transformation;

[0181] S32, the theoretical joint angle of each axis and the actual joint angle output by the corresponding axis are fed back through a first PID controller and then acted on the corresponding axis to reach the corresponding position;

[0182] S33, according to the position reached by the corresponding axis, the actual position of the end effector of the multi-axis robot in the Cartesian coordinate system OXY is obtained through the positive kinematic transformation Coordinates ;

[0183] S34, establish the actual position during interpolation With the current interpolation position The contour error between The formula to calculate the contour error in real time ;

[0184] S35, according to the contour error The formula corresponds to , calculate the corresponding X-axis cross-coupling gain and the cross-coupling gain of the Y axis ;

[0185] S36, contour error Input the cross-coupling controller and set the cross-coupling gain of the X-axis , cross-coupling gain of the Y axis The output u of the cross-coupling controller is compensated to the current interpolated position of the end effector of the multi-axis robot Get the actual input of the multi-axis robot. The actual input of the multi-axis robot satisfies the following formula:

[0186] ;

[0187] Where, is the actual input X-axis coordinate, The Y-axis coordinate actually input.

[0188] To improve interpolation accuracy, it is necessary to calculate the position deviation between the actual position and the target trajectory in real time and compensate for the position deviation. This method calculates the contour error during the interpolation process in real time and then uses a cross-coupling control algorithm to compensate for the contour error to improve interpolation accuracy.

[0189] Specifically, the contour error is compensated to the Cartesian space theoretical position (current interpolation position) planned in the interpolation algorithm by the cross-coupling controller (CCC). ), through the first PID controller and the second PID controller (i.e. Figure 5 The PID in the cross-coupling control algorithm can further improve the interpolation accuracy. Figure 5 As shown, the theoretical position of Cartesian space is calculated in real time by the linear interpolation principle or circular interpolation principle. , the inverse kinematics transformation obtains the theoretical joint angle of each axis. This embodiment has two axes as controlled objects, which are denoted as Axis1 and Axis2 respectively. The corresponding theoretical joint angles are and The theoretical joint angle of each axis and the actual joint angle of the corresponding axis output by the feedback pass through the first PID controller and act on the controlled objects Axis1 and Axis2 to reach the corresponding position. According to the position reached by the controlled objects Axis1 and Axis2 (the feedback position of the corresponding axis can be read through the encoder), the actual position in the Cartesian space is obtained through the forward kinematic transformation. , and then calculate the tracking error in the X-axis direction and the tracking error in the Y-axis direction , and the cross-coupling gain of the X-axis , cross-coupling gain of the Y axis Forming the formula , and corresponds to the contour error calculated previously The formula for obtaining the corresponding X-axis cross-coupling gain is and the cross-coupling gain of the Y axis , combined with the output u of the cross-coupling controller to compensate for the current interpolated position of the end effector of the multi-axis robot Get the actual input of the multi-axis robot.

[0190] In one embodiment, the actual position during the interpolation process is established With the current interpolation position The contour error between The formula to calculate the contour error in real time , as follows:

[0191] because,

[0192] ;

[0193] ;

[0194] Where, is the X-axis direction error in the Cartesian coordinate system OXY, is the Y-axis direction error in the Cartesian coordinate system OXY;

[0195] When the target trajectory is a straight line trajectory, the contour error The formula is as follows:

[0196] ;

[0197] Where, is the angle between the straight line trajectory and the positive direction of the X axis in the Cartesian coordinate system OXY;

[0198] When the target trajectory is a circular arc trajectory, the contour error The formula is as follows:

[0199]

[0200] in,

[0201] ;

[0202] Contour error of arc trajectory The Taylor formula expansion of the formula can be obtained:

[0203] ;

[0204] Where, It represents the expansion of higher-order terms, i.e., second-order and above;

[0205] Ignore higher-order terms , the contour error The formula is simplified to:

[0206] .

[0207] In one embodiment, the cross-coupling controller is a second PID controller and satisfies the following formula:

[0208]

[0209] Where, is the output of the cross-coupling controller in the i-th interpolation cycle, is the proportionality coefficient, is the integration coefficient, is the differential coefficient, is the interpolation period, is the contour error of the i-th interpolation cycle, is the contour error of the i-1th interpolation cycle, .

[0210] The displacement and time relationship of the end effector of the multi-axis robot can be obtained The relationship with time in the continuous time domain is then discretized and accumulated to form the number of interpolation cycles. For each interpolation cycle, the corresponding number is multiplied by the interpolation cycle to obtain the corresponding moment, thereby obtaining the corresponding value.

[0211] In one embodiment, the cross-coupling gain of the X-axis and the cross-coupling gain of the Y axis The calculation is as follows:

[0212] For a straight trajectory:

[0213] ,

[0214] For circular arc trajectories:

[0215] , .

[0216] In one embodiment, the PLCopen motion control function block is as follows:

[0217] When the target trajectory is a straight line, the PLCopen motion control function block is a linear interpolation function block. The input pins of the linear interpolation function block include the axis group pin AxisGroup, the start pin Execute, the end position pin Position, the velocity pin Velocity, the acceleration pin Acceleration, the deceleration pin Deceleration, the jerk pin Jerk, and the buffer mode pin BufferMode. The output pins include the completion flag pin Done, the command execution status pin Busy, the command control axis status pin Active, the command interrupt pin CommandAborted, the error status pin Error, and the error code pin ErrorID.

[0218] When the target trajectory is a circular arc, the PLCopen motion control function block is a circular interpolation function block. The input pins of the circular interpolation function block include the axis group pin (AxisGroup), the start pin (Execute), the center coordinate pin (AuxPoint), the end coordinate pin (EndPoint), the path direction pin (PathChoice), the speed pin (Velocity), the acceleration pin (Acceleration), the deceleration pin (Deceleration), the jerk pin (Jerk), and the buffer mode pin (BufferMode). The output pins include the completion flag pin (Done), the command execution status pin (Busy), the command control axis status pin (Active), the command interrupt pin (CommandAborted), the error status pin (Error), and the error code pin (ErrorID).

[0219] Among them, the axis group pin AxisGroup is used to specify the ID of the axis required to participate in the motion, the start pin Execute is used to enable to trigger the interpolation motion signal, the center coordinate pin AuxPoint is used to specify the coordinates of the center of the arc trajectory, the end point coordinate pin EndPoint is used to specify the coordinates of the end point of the arc trajectory, the path direction pin PathChoice is used to determine whether the arc trajectory runs clockwise or counterclockwise, the end position pin Position is used to specify the coordinates of the end point of the straight line trajectory, and the speed pin Velocity is used to specify the maximum speed , that is, the speed in the uniform speed stage, the acceleration pin Acceleration is used to specify the maximum acceleration The Deceleration pin is used to specify the maximum deceleration , the acceleration pin Jerk is used to specify the maximum jerk The cache mode pin BufferMode is the cache mode between two instructions, the completion flag pin Done is used to mark the interpolation motion completion status, the instruction execution status pin Busy is used to mark the instruction execution status, the instruction control axis status pin Active is used to mark the motion status of the axis of the multi-axis robot, the instruction interrupt pin CommandAborted is used to mark the status of the interruption when the instruction is not completed, the error status pin Error is used to mark the ID instruction execution status of the axis required to participate in the motion, and the error code pin Error is used to output the error code.

[0220] like Figure 5 As shown in FIG, the multi-axis robot is a two-axis robot and the cross-coupling controller is a PID controller. The interpolation accuracy can be effectively improved by compensating the contour error.

[0221] Specifically, according to the PLCopen specification and target trajectory type, the cross-coupling multi-axis synchronous interpolation control method based on PLCopen proposed in this paper is encapsulated into a PLCopen motion control function block, such as Figure 4 (a) The linear interpolation function block ZMC_MoveLinearAbsolute2D or Figure 4 (b) shows the circular interpolation function block ZMC_MoveCircularAbsolute2D. The PLCopen motion control function block's inputs and outputs are defined based on the interpolation principle. The inputs primarily include the start signal, parameters required for trajectory determination, and parameters related to velocity planning. The outputs include status and error information.

[0222] like Figure 4As shown in (a), when the target trajectory is a linear trajectory, the PLCopen motion control function block is a linear interpolation function block (ZMC_MoveLinearAbsolute2D). The input pins of the linear interpolation function block include the axis group pin AxisGroup, the start pin Execute, the end position pin Position, the speed pin Velocity, the acceleration pin Acceleration, the deceleration pin Deceleration, the jerk pin Jerk, and the buffer mode pin BufferMode; the output pins include the completion flag pin Done, the instruction execution status pin Busy, the instruction control axis status pin Active, the instruction interrupt pin CommandAborted, the error status pin Error, and the error code pin ErrorID. The axis group pin AxisGroup is used to specify the ID of the axis required to participate in the motion. The start pin Execute is used to enable the triggering of the interpolation motion signal. That is, when the rising edge trigger signal is received, the interpolation motion begins to run. The end position pin Position is used to specify the coordinates of the end point of the linear trajectory. The coordinates of the starting position and the coordinates of the end point of the linear trajectory can determine a unique linear trajectory. The speed pin Velocity is used to specify the maximum speed. , that is, the speed in the uniform speed stage, the acceleration pin Acceleration is used to specify the maximum acceleration The Deceleration pin is used to specify the maximum deceleration , the acceleration pin Jerk is used to specify the maximum jerk , the cache mode pin BufferMode is the cache mode between two instructions, which is used to specify the action when multiple instructions are started. According to the PLCopen specification, the cache mode pin BufferMode generally has six cache modes, namely 0: _mcAborting, 1: _mcBuffered, 2: _mcBlendingLow, 3: _mcBlendingPrevious, 4: _mcBlendingNext, 5: _mcBlendingHigh corresponds to interrupt, wait, merge at low speed, merge at previous speed, merge at next speed, and merge at high speed, respectively. Two common modes are set here: 1. mcBuffered waits for the current instruction to complete (waiting for the current instruction to complete before running other instructions); 2. mcBlendingPrevious transitions at the current instruction speed (starting with the current instruction speed as the starting speed to execute the next instruction). The completion flag pin, Done, indicates the completion of the interpolation motion. The instruction execution status pin, Busy, indicates the instruction execution status. For example, it is True when the current instruction is executing. The instruction control axis status pin, Active, indicates the motion status of the axis of a multi-axis robot. For example, it is True when the axis is moving. The command abort pin, CommandAborted, outputs True if the current instruction is not completed and another instruction interrupts the current instruction. The error status pin, Error, indicates the execution status of the ID instruction required for the motion. The error code pin, Error, outputs an error code. If an exception occurs during the execution of the current instruction, the error status pin, Error, is True, and the error code pin, ErrorID, outputs the error code.

[0223] like Figure 4As shown in (b), when the target trajectory is a circular arc trajectory, the PLCopen motion control function block is a circular interpolation function block (ZMC_MoveCircularAbsolute2D). The input pins of the circular interpolation function block include the axis group pin AxisGroup, the start pin Execute, the center coordinate pin AuxPoint, the end point coordinate pin EndPoint, the path direction pin PathChoice, the speed pin Velocity, the acceleration pin Acceleration, the deceleration pin Deceleration, the jerk pin Jerk, and the buffer mode pin BufferMode; the output pins include the completion flag pin Done, the instruction execution status pin Busy, the instruction control axis status pin Active, the instruction interruption pin CommandAborted, the error status pin Error, and the error code pin ErrorID. The Execute pin enables the circular interpolation function block. When the circular interpolation function block receives a rising edge trigger signal, circular interpolation begins. The AuxPoint pin specifies the coordinates of the arc's center, and the EndPoint pin specifies the coordinates of the arc's endpoint. The coordinates of the arc's center and endpoint determine the equation of the circle. The PathChoice pin determines whether the arc runs clockwise or counterclockwise, thereby determining the arc's actual path. The remaining pins have the same functions as the corresponding pins in the linear interpolation function block and are not detailed here.

[0224] The technical features of the above-mentioned embodiments can be combined arbitrarily. In order to make the description concise, not all possible combinations of the technical features in the above-mentioned embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0225] The above-described embodiments merely represent specific and detailed examples of the present application and should not be construed as limiting the scope of the patent application. It should be noted that a person skilled in the art may make various modifications and improvements without departing from the spirit of the present application, and these modifications and improvements fall within the scope of protection of the present application. Therefore, the scope of protection of the present patent application shall be determined by the appended claims.

Claims

1. A cross-coupling multi-axis synchronous interpolation control method based on PLCopen, characterized by: The PLCopen-based cross-coupling multi-axis synchronous interpolation control method is encapsulated in a PLCopen motion control function block and includes the following steps: S1. Based on the initial positions of the joints of each axis of the multi-axis robot, the starting position of the end effector of the multi-axis robot in the Cartesian coordinate system OXY is determined by using the forward kinematic transformation. Coordinates ; S2, according to the starting position Coordinates and the end point of the target trajectory Coordinates , use the S-type acceleration and deceleration algorithm to calculate the total interpolation time of the end effector of the multi-axis robot and real-time theoretical position, the real-time theoretical position is the current interpolation position Coordinates ; S3, use cross-coupling control algorithm to perform real-time compensation during interpolation process until the total interpolation time is completed End the process. The real-time compensation of the interpolation process using the cross-coupling control algorithm is specifically as follows: S31, according to the current interpolation position The theoretical joint angles of each axis of the multi-axis robot are obtained by inverse kinematics transformation; S32, the theoretical joint angle of each axis and the actual joint angle output by the corresponding axis are fed back through a first PID controller and then acted on the corresponding axis to reach the corresponding position; S33, according to the position reached by the corresponding axis, the actual position of the end effector of the multi-axis robot in the Cartesian coordinate system OXY is obtained through the positive kinematic transformation Coordinates ; S34, establish the actual position during interpolation With the current interpolation position The contour error between The formula to calculate the contour error in real time ; S35, according to the contour error The formula corresponds to , calculate the corresponding X-axis cross-coupling gain and the cross-coupling gain of the Y axis , the X-axis cross-coupling gain and the cross-coupling gain of the Y axis The calculation is as follows: For a straight trajectory: ， ； For circular arc trajectories: ， ； Where, is the X-axis direction error in the Cartesian coordinate system OXY, is the Y-axis direction error in the Cartesian coordinate system OXY, is the angle between the straight line trajectory and the positive direction of the X axis in the Cartesian coordinate system OXY, is the arc trajectory at the current moment angle, is the radius of the arc trajectory; S36, contour error Input the cross-coupling controller and set the cross-coupling gain of the X-axis , cross-coupling gain of the Y axis The output u of the cross-coupling controller is compensated to the current interpolated position of the end effector of the multi-axis robot The actual input of the multi-axis robot is obtained, and the actual input of the multi-axis robot satisfies the following formula: ； Where, is the actual input X-axis coordinate, The Y-axis coordinate actually input.

2. The cross-coupling multi-axis synchronous interpolation control method based on PLCopen according to claim 1, characterized in that: According to the starting position Coordinates and the end point of the target trajectory Coordinates , use the S-type acceleration and deceleration algorithm to calculate the total interpolation time of the end effector of the multi-axis robot and real-time theoretical position, the real-time theoretical position is the current interpolation position Coordinates , as follows: 1) When the target trajectory is a straight line, perform the following operations: The straight line segment is obtained from the distance formula between two points Length , and thus calculate the straight line segment The unit vector ,in, is the motion component in the X-axis direction, is the motion component in the Y-axis direction; According to the straight line The length L and the straight line segment The unit vector , use the S-type acceleration and deceleration algorithm to calculate the total interpolation time And obtain the displacement and time relationship of the end effector of the multi-axis robot; According to the principle of linear interpolation, the current interpolation position can be obtained Coordinates : ； 2) When the target trajectory is an arc trajectory, perform the following operations: Use atan2 function to calculate the starting angle of the arc trajectory and end angle , where the starting angle The center of the arc trajectory Point to the starting position The angle between the vector and the positive direction of the X axis in the Cartesian coordinate system OXY, the end angle The center of the arc trajectory Point to the end point of the arc trajectory The angle between the vector and the positive direction of the X axis in the Cartesian coordinate system OXY, the starting angle and end angle The angle range is ; Calculate the total angular displacement based on the arc trajectory running direction , as follows: If the arc trajectory is clockwise, then ； If the arc trajectory is counterclockwise, then ； According to the arc segment Length , and the arc segment Radius , The center of the arc trajectory The total interpolation time is calculated using the S-type acceleration and deceleration algorithm. And obtain the displacement and time relationship of the end effector of the multi-axis robot; According to the arc interpolation principle, the current interpolation position can be obtained Coordinates for: ； in, ； Arc trajectory at the current moment Angle The calculation of is as follows: If the arc trajectory is clockwise, then ； If the arc trajectory is counterclockwise, then ； Where, From time zero to the current time The angular displacement of the arc, For the current moment displacement.

3. The cross-coupling multi-axis synchronous interpolation control method based on PLCopen according to claim 2, characterized in that: The S-shaped acceleration and deceleration algorithm is divided into seven stages: acceleration stage, uniform acceleration stage, deceleration stage, uniform speed stage, acceleration and deceleration stage, uniform deceleration stage, and deceleration and deceleration stage. The total interpolation time The calculation is as follows: Since the running time of acceleration stage, uniform acceleration stage, deceleration stage, uniform speed stage, acceleration and deceleration stage, uniform deceleration stage, and deceleration and deceleration stage corresponds to 、 、 、 、 、 、 , and satisfy: ； ； ； ； ； ； ； The speeds of the acceleration stage, uniform acceleration stage, deceleration stage, uniform speed stage, acceleration and deceleration stage, uniform deceleration stage, and deceleration and deceleration stage correspond to the following: 、 、 、 、 、 、 , and satisfy: ； ； ； ； ； ； ； The displacements of acceleration stage, uniform acceleration stage, deceleration stage, uniform speed stage, acceleration and deceleration stage, uniform deceleration stage, and deceleration and deceleration stage correspond to the following: 、 、 、 、 、 、 , and satisfy: ； ； ； ； ； ； ； ； ； In summary, we can solve , the formula is as follows: ； Thus, the end time of the acceleration phase, uniform acceleration phase, deceleration phase, uniform speed phase, acceleration and deceleration phase, uniform deceleration phase and deceleration phase can be calculated. 、 、 、 、 、 、 , get the total interpolation time ; The relationship between the displacement and time of the end effector of the multi-axis robot is as follows: ； Where, For the current moment The displacement, is the maximum jerk, is the maximum acceleration, is the maximum deceleration, is the maximum speed, is the starting speed, is the terminal velocity, which is zero, L is the total displacement, is the total displacement of the acceleration section, is the total displacement of the deceleration stage. The acceleration stage includes the acceleration stage, the uniform acceleration stage and the deceleration stage. The deceleration stage includes the acceleration and deceleration stage, the uniform deceleration stage and the deceleration stage.

4. The cross-coupling multi-axis synchronous interpolation control method based on PLCopen according to claim 2, characterized in that: The atan2 function is used to calculate the starting angle of the arc trajectory. and end angle Before, also perform the following operations: Determine whether it is satisfied If it does not meet the requirements, it is considered not an arc trajectory, an error warning is issued and the process ends. If it meets the requirements, it is considered an arc trajectory, and the atan2 function is used to calculate the starting angle of the arc trajectory. and end angle ,in, Starting position Coordinates To the center of the arc trajectory Coordinates distance, The end point of the arc trajectory Coordinates To the center Coordinates distance, The default minimum value.

5. The cross-coupling multi-axis synchronous interpolation control method based on PLCopen according to claim 1, characterized in that: The actual position during the interpolation process is established With the current interpolation position The contour error between The formula to calculate the contour error in real time , as follows: because, ； ； When the target trajectory is a straight line trajectory, the contour error The formula is as follows: ； When the target trajectory is a circular arc trajectory, the contour error The formula is as follows: ； in, ； Where, The center of the arc trajectory coordinates of Contour error of arc trajectory The Taylor formula expansion of the formula can be obtained: ； Where, It represents the expansion of higher-order terms, i.e., second-order and above; Ignore higher-order terms , the contour error The formula is simplified to: 。 6. The cross-coupling multi-axis synchronous interpolation control method based on PLCopen according to claim 5, characterized in that: The cross-coupling controller is a second PID controller and satisfies the following formula: ； Where, is the output of the cross-coupling controller in the i-th interpolation cycle, is the proportionality coefficient, is the integration coefficient, is the differential coefficient, is the interpolation period, is the contour error of the i-th interpolation cycle, is the contour error of the i-1th interpolation cycle, .

7. The cross-coupling multi-axis synchronous interpolation control method based on PLCopen according to claim 1, characterized in that: The PLCopen motion control function blocks are as follows: When the target trajectory is a straight line trajectory, the PLCopen motion control function block is a linear interpolation function block. The input pins of the linear interpolation function block include the axis group pin AxisGroup, the start pin Execute, the end position pin Position, the speed pin Velocity, the acceleration pin Acceleration, the deceleration pin Deceleration, the jerk pin Jerk, and the buffer mode pin BufferMode; the output pins include the completion flag pin Done, the instruction execution status pin Busy, the instruction control axis status pin Active, the instruction interruption pin CommandAborted, the error status pin Error, and the error code pin ErrorID; When the target trajectory is a circular arc trajectory, the PLCopen motion control function block is a circular interpolation function block. The input pins of the circular interpolation function block include the axis group pin AxisGroup, the start pin Execute, the center coordinate pin AuxPoint, the end point coordinate pin EndPoint, the path direction pin PathChoice, the speed pin Velocity, the acceleration pin Acceleration, the deceleration pin Deceleration, the jerk pin Jerk, and the buffer mode pin BufferMode; the output pins include the completion flag pin Done, the command execution status pin Busy, the command control axis status pin Active, the command interruption pin CommandAborted, the error status pin Error, and the error code pin ErrorID. Among them, the axis group pin AxisGroup is used to specify the ID of the axis required to participate in the motion, the start pin Execute is used to enable to trigger the interpolation motion signal, the center coordinate pin AuxPoint is used to specify the coordinates of the center of the arc trajectory, the end point coordinate pin EndPoint is used to specify the coordinates of the end point of the arc trajectory, the path direction pin PathChoice is used to determine whether the arc trajectory runs clockwise or counterclockwise, the end point position pin Position is used to specify the coordinates of the end point of the straight line trajectory, and the speed pin Velocity is used to specify the maximum speed , that is, the speed in the uniform speed stage, the acceleration pin Acceleration is used to specify the maximum acceleration , the deceleration pin Deceleration is used to specify the maximum deceleration The jerk pin is used to specify the maximum jerk The cache mode pin BufferMode is the cache mode between two instructions, the completion flag pin Done is used to mark the completion status of the interpolation motion, the instruction execution status pin Busy is used to mark the instruction execution status, the instruction control axis status pin Active is used to mark the motion status of the axis of the multi-axis robot, the instruction interrupt pin CommandAborted is used to mark the state of being interrupted when the instruction is not completed, the error status pin Error is used to mark the ID instruction execution status of the axis required to participate in the motion, and the error code pin Error is used to output the error code.

Images