Cross coupling multi-axis synchronous interpolation control method based on PLCopen

Abstract

The invention discloses a cross coupling multi-axis synchronous interpolation control method based on PLCopen, which is packaged in a PLCopen motion control function block, and comprises the following steps: determining the coordinates of the initial position of an end effector of a multi-axis robot under a Cartesian coordinate system OXY by utilizing forward kinematics transformation according to the obtained initial position of each axis joint of the multi-axis robot; according to the coordinates of the initial position and the coordinates of the end point of the target track, the total interpolation time and the real-time theoretical position of an end effector of the multi-axis robot are calculated through an S-type acceleration and deceleration algorithm, and the real-time theoretical position is the coordinates of the current interpolation position; and performing interpolation process real-time compensation by adopting a cross coupling control algorithm until the total interpolation time is finished. According to the method, the contour error generated during synchronous movement of all axes of the multi-axis robot can be compensated, the interpolation precision is improved, meanwhile, the PLCopen standard is met, the universality is high, and the application range is wide.

Description

Technical Field

[0001] The 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 development of industrial automation technology, motion control is widely used in various multi-axis equipment, such as CNC machine tools, industrial robots, etc. There are many related control products in the field of motion control. Each manufacturer has its own programming language and product specifications, and the compatibility between motion control products of different manufacturers is very poor. In order to solve this problem, the existing technology proposes a unified programming standard IEC61131-3 and formulates the PLCopen motion control function block standard. The use of the PLCopen motion control function block standard can improve the compatibility, openness and reusability of motion control systems, save development costs, and enable it to be widely used in the entire industrial control field.

[0003] The interpolation function is an important part of the motion control system. It densifies the data points of the planned contour trajectory, generates a series of discrete points that approximate the target contour trajectory, and sends the information of these discrete points 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 not high, open-loop control is often used. However, when implementing the interpolation function, multi-axis coordinated synchronous motion is required to make the end effector run along the preset trajectory. If only the following error of a single axis is considered in the control without considering the errors of other axes, the final running trajectory is prone to large errors, especially when running at high speeds. Due to the differences in the motion parameters of each motion axis and external disturbances, there will be a certain contour error between the actual running trajectory and the preset ideal trajectory. Therefore, in order to achieve high-precision interpolation and compensate for the contour error generated, a cross-coupling control multi-axis synchronous interpolation method based on PLCopen is proposed. Summary of the invention

[0005] The purpose of the present invention is to address the above problems and propose a cross-coupling multi-axis synchronous interpolation control method based on PLCopen. Cross-coupling control is used to realize contour error compensation generated when the axes of a multi-axis robot move synchronously, thereby improving the interpolation accuracy. At the same time, the method complies with the PLCopen specification, has strong versatility and 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. According to the initial positions of the joints of each axis of the multi-axis robot, the starting position P of the end effector of the multi-axis robot in the Cartesian coordinate system OXY is determined by using the forward kinematic transformation. s The coordinates (x s ,y s );

[0009] S2, according to the starting position P s The coordinates (x s ,y s ) and the end point P of the target trajectory e The coordinates (x e ,y e ), the total interpolation time T of the end effector of the multi-axis robot is calculated using the S-type acceleration and deceleration algorithm sum and real-time theoretical position, which is the current interpolation position P i The coordinates (P x ,P y );

[0010] S3, use the cross-coupling control algorithm to perform real-time compensation during the interpolation process until the total interpolation time T is completed sum 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 P i The inverse kinematics transformation obtains the theoretical joint angles of each axis of the multi-axis robot;

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

[0013] S33, according to the position reached by the corresponding axis, the actual position P of the end effector of the multi-axis robot in the Cartesian coordinate system OXY is obtained by positive kinematic transformation a The coordinates (x a ,y a );

[0014] S34, establish the actual position P during interpolation a With the current interpolation position P i The formula of the contour error ε between is used to calculate the contour error ε in real time;

[0015] S35, according to the formula of contour error ε and corresponding to ε=-C x ex +C y e y , calculate the corresponding X-axis cross-coupling gain C x and the cross-coupling gain C of the Y axis y ;

[0016] S36, input the contour error ε into the cross-coupling controller, and set the cross-coupling gain C of the X-axis x , Y-axis cross-coupling gain C y The output u of the cross-coupling controller is compensated to the current interpolation position P of the end effector of the multi-axis robot i Get the actual input of the multi-axis robot. The actual input of the multi-axis robot satisfies the following formula:

[0017]

[0018] In the formula, r x is the actual input X-axis coordinate, r y The Y-axis coordinate actually input.

[0019] Preferably, according to the starting position P s The coordinates (x s ,y s ) and the end point P of the target trajectory e The coordinates (x e ,y e ), the total interpolation time T of the end effector of the multi-axis robot is calculated using the S-type acceleration and deceleration algorithm sum and real-time theoretical position, which is the current interpolation position P i The coordinates (P x ,P y ), as follows:

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

[0021] The straight line segment P is obtained from the distance formula between two points. s P e Length Thus, the straight line segment P is calculated s P e The unit vector Where dx is the motion component in the X-axis direction, and dy is the motion component in the Y-axis direction;

[0022] According to the straight line segment P s P e The length L and the straight line segment P s P e The unit vector Use the S-type acceleration and deceleration algorithm to calculate the total interpolation time Tsum 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 P can be obtained i The coordinates (P x ,P y ):

[0024]

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

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

[0027] The total angular displacement ω is calculated according to the running direction of the arc trajectory, 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 P s P e The length L = ωR, and the arc segment P s P e Radius Use the S-type acceleration and deceleration algorithm to calculate the total interpolation time T sum 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 P can be obtained i The coordinates (P x ,P y )for:

[0034]

[0035] in,

[0036] θ i =S(t) / R;

[0037] The angle θ of the arc trajectory at the current time t t The calculation of is as follows:

[0038] If the arc trajectory is clockwise, then

[0039] θ t =θ s -θ i ;

[0040] If the arc trajectory is counterclockwise, then

[0041] θ t =θ s +θ i ;

[0042] In the formula, θ i is the angular displacement of the arc from time zero to the current time t, and S(t) is the displacement at the current time t.

[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 T sum The calculation is as follows:

[0045] Since the running time of the acceleration stage, uniform acceleration stage, deceleration stage, uniform speed stage, acceleration and deceleration stage, uniform deceleration stage, and deceleration and deceleration stage corresponds to T 1 、T 2 、T 3 、T 4 、T 5 、T 6 、T 7 , and satisfy:

[0046]

[0047] T 3 =t 3 -t 2 =T 1

[0048] T 4 =t 4 -t 3

[0049]

[0050] T 7 =t 7 -t 6 =T 5

[0051] The speeds of the acceleration phase, uniform acceleration phase, deceleration phase, uniform speed phase, acceleration and deceleration phase, uniform deceleration phase, and deceleration and deceleration phase correspond to v 1 、v 2 、v 3 、v 4 、v 5 、v 6 、v 7 , and satisfy:

[0052]

[0053] v 2 =v 1 +A max T 2

[0054] v 3 =V max

[0055] v 4 =V max

[0056]

[0057] v 6 =v 5 -D max T 6

[0058] v 7 =v e

[0059] The displacements 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 s in sequence. 1 、s 2 、s 3 、s 4 、s 5 、s 6 、s 7 , and satisfy:

[0060]

[0061] S a =s 3

[0062]

[0063] s 4 =s 3 +v 3 T 4

[0064]

[0065]

[0066] s 7 =L

[0067] In summary, we can solve T 4 , the formula is as follows:

[0068]

[0069] Thus, the end time t of the acceleration phase, uniform acceleration phase, deceleration phase, uniform speed phase, acceleration and deceleration phase, uniform deceleration phase and deceleration and deceleration phase can be calculated. 1 ,t 2 ,t 3 ,t 4 ,t 5 ,t 6 ,t 7 , get the total interpolation time T sum =t 7 ;

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

[0071]

[0072] Where S(t) is the displacement at the current time t, J max is the maximum acceleration, A max is the maximum acceleration, D max is the maximum deceleration, V max is the maximum speed, v s is the starting speed, v e is the terminal velocity, which is zero, L is the total displacement, S a is the total displacement of the acceleration section, S d is the total displacement of the deceleration section. The acceleration section includes the acceleration stage, the uniform acceleration stage and the deceleration stage. The deceleration section includes the acceleration and deceleration stage, the uniform deceleration stage and the deceleration and deceleration stage.

[0073] Preferably, the starting angle θ of the arc trajectory is calculated using the atan2 function s and the end angle θ e Before, also perform the following operations:

[0074] Determine whether |P is satisfieds P 0 -P e P o |<σ, if not satisfied, it is considered not to be an arc trajectory, an error warning is issued and the process ends; if satisfied, it is considered to be an arc trajectory, and the atan2 function is used to calculate the starting angle θ of the arc trajectory s and the end angle θ e , where P s P 0 is the starting position P s The coordinates (x s ,y s ) to the center P of the arc trajectory o The coordinates (x 0 ,y o ) distance, P e P o is the end point P of the arc trajectory e The coordinates (x e ,y e ) to the center P o The coordinates (x 0 ,y o ), σ is the preset minimum value.

[0075] Preferably, the actual position P during the interpolation process is established a With the current interpolation position P i The formula for the contour error ε between is used to calculate the contour error ε in real time, as follows:

[0076] because,

[0077] e x =P x -x a ;

[0078] e y =P y -y a ;

[0079] In the formula, e x is the X-axis direction error in the Cartesian coordinate system OXY, e y is the Y-axis direction error in the Cartesian coordinate system OXY;

[0080] When the target trajectory is a straight line trajectory, the formula for the contour error ε is as follows:

[0081] ε=-e x sinθ l +e y cosθ l ;

[0082] In the formula, θ lIt is the angle between the straight line trajectory and the positive direction of the X axis in the Cartesian coordinate system OXY;

[0083] When the target trajectory is a circular arc trajectory, the formula for the contour error ε is as follows:

[0084]

[0085] in,

[0086]

[0087] By expanding the formula of the contour error ε of the arc trajectory with Taylor formula, we can get:

[0088]

[0089] In the formula, f(e x ,e y ,θ t ) represents a higher-order term, that is, an expansion of the second order or higher;

[0090] Ignore the higher-order terms f(e x ,e y ,θ t ), the formula of contour error ε is simplified as:

[0091]

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

[0093]

[0094] Where u(i) is the output of the cross-coupling controller in the i-th interpolation period, k p is the proportionality coefficient, k i is the integration coefficient, k d is the differential coefficient, T is the interpolation period, ε(i) is the contour error of the i-th interpolation period, ε(i-1) is the contour error of the i-1-th interpolation period, 1≤i≤T sum / T.

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

[0096] For a straight line trajectory:

[0097] C x = sinθ l , C x = cosθ l

[0098] For circular arc trajectories:

[0099]

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

[0101] 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 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 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;

[0102] When the target trajectory is an arc trajectory, the PLCopen motion control function block is an arc interpolation function block. The input pins of the arc 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;

[0103] Among them, the axis group pin AxisGroup is used to specify the ID of the axis required to participate in the movement, 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 V max , that is, the speed in the uniform speed stage, the acceleration pin Acceleration is used to specify the maximum acceleration A maxThe deceleration pin Deceleration is used to specify the maximum deceleration D max , the jerk pin Jerk is used to specify the maximum jerk J max The cache mode pin BufferMode is the cache mode between two instructions. The completion mark 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 interrupted status 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. The error code pin Error is used to output the error code.

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

[0105] During the interpolation motion process, this method obtains the actual position of the end effector in the Cartesian space after forward kinematic transformation based on the feedback data of the actual position of each axis of the multi-axis robot, 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 by inverse kinematics to obtain the joint angle of each axis of the multi-axis robot, and then the corresponding axis is issued after the output of the PID controller to realize closed-loop control, which 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 the industrial robot control, has good versatility and a wide range of applications. BRIEF DESCRIPTION OF THE DRAWINGS

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

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

[0108] Figure 3 The following are the schematic diagrams (a) and (b) of calculating the contour error of a straight line trajectory and the contour error of a circular arc trajectory in step S3 of the present invention;

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

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

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

[0112] It should be noted that, unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art in the technical field 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.

[0113] 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:

[0114] S1. According to the initial positions of the joints of each axis of the multi-axis robot, the starting position P of the end effector of the multi-axis robot in the Cartesian coordinate system OXY is determined by using the forward kinematic transformation. s The coordinates (x s ,y s ).

[0115] S2, according to the starting position P s The coordinates (x s ,y s ) and the end point P of the target trajectory e The coordinates (x e ,y e ), the total interpolation time T of the end effector of the multi-axis robot is calculated using the S-type acceleration and deceleration algorithm sum and real-time theoretical position, which is the current interpolation position P i The coordinates (P x ,P y ).

[0116] In one embodiment, according to the starting position P s The coordinates (x s ,y s ) and the end point P of the target trajectory e The coordinates (x e ,y e ), the total interpolation time T of the end effector of the multi-axis robot is calculated using the S-type acceleration and deceleration algorithm sum and real-time theoretical position, which is the current interpolation position P i The coordinates (P x ,Py ), as follows:

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

[0118] The straight line segment P is obtained from the distance formula between two points. s P e Length Thus, the straight line segment P is calculated s P e The unit vector Where dx is the motion component in the X-axis direction, and dy is the motion component in the Y-axis direction;

[0119] According to the straight line segment P s P e The length L and the straight line segment P s P e The unit vector Use the S-type acceleration and deceleration algorithm to calculate the total interpolation time T sum And obtain the displacement and time relationship of the end effector of the multi-axis robot;

[0120] According to the principle of linear interpolation, the current interpolation position P can be obtained i The coordinates (P x ,P y ):

[0121]

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

[0123] Use the atan2 function to calculate the starting angle θ of the arc trajectory s and the end angle θ e , where the starting angle θ s P is the center of the arc trajectory o Point to the starting position P s The angle between the vector and the positive direction of the X-axis in the Cartesian coordinate system OXY, the end angle θ e P is the center of the arc trajectory o Point to the end point P of the arc trajectory e The angle between the vector and the positive direction of the X-axis in the Cartesian coordinate system OXY, the starting angle θ s and the end angle θ e The angle range is (-π,π];

[0124] The total angular displacement ω is calculated according to the running direction of the arc trajectory, as follows:

[0125] If the arc trajectory is clockwise, then

[0126]

[0127] If the arc trajectory is counterclockwise, then

[0128]

[0129] According to the arc segment P s P e The length L = ωR, and the arc segment P s P e Radius Use the S-type acceleration and deceleration algorithm to calculate the total interpolation time T sum And obtain the displacement and time relationship of the end effector of the multi-axis robot;

[0130] According to the arc interpolation principle, the current interpolation position P can be obtained i The coordinates (P x ,P y )for:

[0131]

[0132] Among them, θ i =S(t) / R;

[0133] The angle θ of the arc trajectory at the current time t t The calculation of is as follows:

[0134] If the arc trajectory is clockwise, then θ t =θ s -θ i ;

[0135] If the arc trajectory is counterclockwise, then θ t =θ s +θ i ;

[0136] In the formula, θ i is the angular displacement of the arc from time zero to the current time t, and S(t) is the displacement at the current time t.

[0137] In one embodiment, 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:

[0138] Total interpolation time T sum The calculation is as follows:

[0139] Since the running time of the acceleration stage, uniform acceleration stage, deceleration stage, uniform speed stage, acceleration and deceleration stage, uniform deceleration stage, and deceleration and deceleration stage corresponds to T 1 、T 2 、T 3 、T4 、T 5 、T 6 、T 7 , and satisfy:

[0140]

[0141]

[0142] T 3 =t 3 -t 2 =T 1

[0143] T 4 =t 4 -t 3

[0144]

[0145] T 7 =t 7 -t 6 =T 5

[0146] The speeds of the acceleration phase, uniform acceleration phase, deceleration phase, uniform speed phase, acceleration and deceleration phase, uniform deceleration phase, and deceleration and deceleration phase correspond to v 1 、v 2 、v 3 、v 4 、v 5 、v 6 、v 7 , and satisfy:

[0147]

[0148] v 2 =v 1 +A max T 2

[0149] v 3 =V max

[0150] v 4 =V max

[0151]

[0152] v 6 =v 5 -D max T 6

[0153] v 7 =ve

[0154] The displacements 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 s in sequence. 1 、s 2 、s 3 、s 4 、s 5 、s 6 、s 7 , and satisfy:

[0155]

[0156] S a =s 3

[0157]

[0158] s 4 =s 3 +v 3 T 4

[0159]

[0160] s 7 =L

[0161] In summary, we can solve T 4 , the formula is as follows:

[0162]

[0163] Thus, the end time t of the acceleration phase, uniform acceleration phase, deceleration phase, uniform speed phase, acceleration and deceleration phase, uniform deceleration phase and deceleration and deceleration phase can be calculated. 1 ,t 2 ,t 3 ,t 4 ,t 5 ,t 6 ,t 7 , get the total interpolation time T sum =t 7 ;

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

[0165]

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

[0167]

[0168] Where V(t) is the velocity at the current time t, S(t) is the displacement at the current time t, and J max is the maximum acceleration, A max is the maximum acceleration, D max is the maximum deceleration, V max is the maximum speed, v s is the starting speed, v e is the terminal velocity, which is zero, L is the total displacement, S a is the total displacement of the acceleration section, S d is the total displacement of the deceleration section. The acceleration section includes the acceleration stage, the uniform acceleration stage and the deceleration stage. The deceleration section includes the acceleration and deceleration stage, the uniform deceleration stage and the deceleration and deceleration stage.

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

[0170] Determine whether |P is satisfied s P 0 -P e P o |<σ, if not satisfied, it is considered not to be an arc trajectory, an error warning is issued and the process ends; if satisfied, it is considered to be an arc trajectory, and the atan2 function is used to calculate the starting angle θ of the arc trajectory s and the end angle θ e , where P s P 0 is the starting position P s The coordinates (x s ,y s ) to the center P of the arc trajectory o The coordinates (x 0 ,y o ) distance, P e P o is the end point P of the arc trajectory e The coordinates (x e ,y e ) to the center P o The coordinates (x 0 ,y o ), σ is the preset minimum value.

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

[0172] S31, according to the current interpolation position P iThe inverse kinematics transformation obtains the theoretical joint angles of each axis of the multi-axis robot;

[0173] S32, the theoretical joint angle of each axis and the actual joint angle output by the corresponding axis fed back are applied to the corresponding axis through the first PID controller to reach the corresponding position;

[0174] S33, according to the position reached by the corresponding axis, the actual position P of the end effector of the multi-axis robot in the Cartesian coordinate system OXY is obtained by positive kinematic transformation a The coordinates (x a ,y a );

[0175] S34, establish the actual position P during interpolation a With the current interpolation position P i The formula of the contour error ε between is used to calculate the contour error ε in real time;

[0176] S35, according to the formula of contour error ε and corresponding to ε=-C x e x +C y e y , calculate the corresponding X-axis cross-coupling gain C x and the cross-coupling gain C of the Y axis y ;

[0177] S36, input the contour error ε into the cross-coupling controller, and set the cross-coupling gain C of the X-axis x , Y-axis cross-coupling gain C y The output u of the cross-coupling controller is compensated to the current interpolation position P of the end effector of the multi-axis robot i Get the actual input of the multi-axis robot. The actual input of the multi-axis robot satisfies the following formula:

[0178]

[0179] In the formula, r x is the actual input X-axis coordinate, r y The Y-axis coordinate actually input.

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

[0181] Specifically, the contour error is compensated to the Cartesian space theoretical position (current interpolation position P) planned in the interpolation algorithm through the cross-coupled controller (CCC). i ), through the first PID controller and the second PID controller (i.e. Figure 5 The PID in the control algorithm can further improve the interpolation accuracy. Figure 5 As shown, the theoretical position P in Cartesian space is calculated in real time by the linear interpolation principle or circular interpolation principle. i (P x ,P y ), the inverse kinematics transformation obtains the theoretical joint angle of each axis. In this embodiment, there are two axes as controlled objects, which are denoted as Axis1 and Axis2 respectively, and the corresponding theoretical joint angle is θ 1 and θ 2 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 out by the encoder), the actual position P in the Cartesian space is obtained through the forward kinematic transformation. a , and then calculate the tracking error e in the X-axis direction x and the tracking error e in the Y-axis direction y , and the cross-coupling gain C of the X-axis x , Y-axis cross-coupling gain C y Forming the formula ε=-C x e x +C y e y , and the corresponding X-axis cross-coupling gain C is obtained according to the formula of the previously calculated contour error ε x and the cross-coupling gain C of the Y axis y , combined with the output u of the cross-coupling controller to compensate for the current interpolation position P of the end effector of the multi-axis robot i Get the actual input of the multi-axis robot.

[0182] In one embodiment, the actual position P during the interpolation process is established. a With the current interpolation position P i The formula for the contour error ε between is used to calculate the contour error ε in real time, as follows:

[0183] because,

[0184] e x =P x -x a ;

[0185] e y =P y -y a ;

[0186] In the formula, e x is the X-axis direction error in the Cartesian coordinate system OXY, ey is the Y-axis direction error in the Cartesian coordinate system OXY;

[0187] When the target trajectory is a straight line trajectory, the formula for the contour error ε is as follows:

[0188] ε=-e x sinθ l +e y cosθ l ;

[0189] In the formula, θ l It is the angle between the straight line trajectory and the positive direction of the X axis in the Cartesian coordinate system OXY;

[0190] When the target trajectory is a circular arc trajectory, the formula for the contour error ε is as follows:

[0191]

[0192] in,

[0193]

[0194] By expanding the formula of the contour error ε of the arc trajectory with Taylor formula, we can get:

[0195]

[0196] In the formula, f(e x ,e y ,θ t ) represents a higher-order term, that is, an expansion of the second order or higher;

[0197] Ignore the higher-order terms f(e x ,e y ,θ t ), the formula of contour error ε is simplified as:

[0198]

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

[0200]

[0201] Where u(i) is the output of the cross-coupling controller in the i-th interpolation period, k p is the proportionality coefficient, k i is the integration coefficient, k d is the differential coefficient, T is the interpolation period, ε(i) is the contour error of the i-th interpolation period, ε(i-1) is the contour error of the i-1-th interpolation period, 1≤i≤T sum / T.

[0202] The relationship between S(t) and time in the continuous time domain can be obtained through the displacement and time relationship of the end effector of the multi-axis robot. Then the continuous time is discretized and accumulated to form the number of interpolation cycles. For each interpolation cycle, the corresponding number is multiplied by the interpolation cycle to get the corresponding moment, thereby obtaining the corresponding value.

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

[0204] For a straight line trajectory:

[0205] C x = sinθ l , C x = cosθ l

[0206] For circular arc trajectories:

[0207]

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

[0209] 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 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 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;

[0210] When the target trajectory is an arc trajectory, the PLCopen motion control function block is an arc interpolation function block. The input pins of the arc 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;

[0211] Among them, the axis group pin AxisGroup is used to specify the ID of the axis required to participate in the movement, 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 V max , that is, the speed in the uniform speed stage, the acceleration pin Acceleration is used to specify the maximum acceleration A max The deceleration pin Deceleration is used to specify the maximum deceleration D max , the jerk pin Jerk is used to specify the maximum jerk J max The cache mode pin BufferMode is the cache mode between two instructions. The completion mark 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 interrupted status 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. The error code pin Error is used to output the error code.

[0212] like Figure 5 As shown, 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.

[0213] Specifically, according to the PLCopen specification and the 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 input pins and output pins of the PLCopen motion control function block are defined according to the interpolation principle. The input pins of the PLCopen motion control function block mainly include the start signal, the parameters required to determine the trajectory, and the parameters related to the speed planning. The output pins include the status information and error information.

[0214] like Figure 4 As 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 cache 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 to trigger the interpolation motion signal, that is, when the rising edge trigger signal is received, the interpolation motion starts 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, and the speed pin Velocity is used to specify the maximum speed V max , that is, the speed in the uniform speed stage, the acceleration pin Acceleration is used to specify the maximum acceleration A max The deceleration pin Deceleration is used to specify the maximum deceleration D max , the jerk pin Jerk is used to specify the maximum jerk J max, the cache mode pin BufferMode is the cache mode between two instructions, which is used to specify the action when multiple instructions are started. For example, 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, which correspond to interrupt, wait, merge at low speed, merge at the previous speed, merge at the next speed, and merge at high speed. Here, two common modes are set: 1. mcBuffered waits for the current instruction to complete (after waiting for the current instruction to complete Run other instructions again); 2. mcBlendingPrevious transitions at the speed of the current instruction (executes the next instruction with the speed of the current instruction as the starting speed). The completion mark pin Done is used to mark the completion status of the interpolation movement. The instruction execution status pin Busy is used to mark the instruction execution status, such as True when the current instruction is executed. The instruction control axis status pin Active is used to mark the movement status of the axis of the multi-axis robot, such as True when the axis of the multi-axis robot is moving. The instruction interrupt pin CommandAborted is used to output true when the current instruction is not completed and other instructions interrupt the current instruction. The error status pin Error is used to mark the ID instruction execution status of the axis required to participate in the movement. The error code pin Error is used to output the error code. If an exception occurs in the execution of the current instruction, the error status pin Error is True, and the error code pin ErrorID pin outputs the error code.

[0215] like Figure 4As shown in (b), when the target trajectory is a circular 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 interrupt pin CommandAborted, the error status pin Error, and the error code pin ErrorID. The start pin Execute is used to enable the arc interpolation function block. When the arc interpolation function block receives the rising edge trigger signal, the arc interpolation motion starts to run. 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 coordinates of the center of the arc trajectory and the coordinates of the end point of the arc trajectory can determine the equation of the circle. The path direction pin PathChoice is used to determine whether the arc trajectory runs clockwise or counterclockwise, thereby determining the actual running path of the arc trajectory. The functions of the remaining pins are the same as those of the corresponding pins of the linear interpolation function block, and will not be repeated here.

[0216] The technical features of the above-described embodiments may be arbitrarily combined. To make the description concise, not all possible combinations of the technical features in the above-described 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.

[0217] The above-described embodiments only express the more specific and detailed embodiments described in this application, but they cannot be understood as limiting the scope of the patent application. It should be pointed out that for ordinary technicians in this field, several modifications and improvements can be made without departing from the concept of this application, which all belong to the protection scope of this application. Therefore, the protection scope of the patent application shall be based on the attached claims.

Claims

1. A cross-coupling multi-axis synchronous interpolation control method based on PLCopen, characterized in that: The 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: S1. According to the initial positions of the joints of each axis of the multi-axis robot, the starting position P of the end effector of the multi-axis robot in the Cartesian coordinate system OXY is determined by using the forward kinematic transformation. s The coordinates (x s ,y s ); S2, according to the starting position P s The coordinates (x s ,y s ) and the end point P of the target trajectory e The coordinates (x e ,y e ), the total interpolation time T of the end effector of the multi-axis robot is calculated using the S-type acceleration and deceleration algorithm sum and real-time theoretical position, the real-time theoretical position being the current interpolation position P i The coordinates (P x ,P y ); S3, use the cross-coupling control algorithm to perform real-time compensation during the interpolation process until the total interpolation time T is completed sum End the process, the cross-coupling control algorithm is used to perform real-time compensation in the interpolation process as follows: S31, according to the current interpolation position P i The inverse kinematics transformation obtains the theoretical joint angles of each axis of the multi-axis robot; S32, the theoretical joint angle of each axis and the actual joint angle output by the corresponding axis fed back are applied to the corresponding axis through the first PID controller to reach the corresponding position; S33, according to the position reached by the corresponding axis, the actual position P of the end effector of the multi-axis robot in the Cartesian coordinate system OXY is obtained by positive kinematic transformation a The coordinates (x a ,y a ); S34, establish the actual position P during interpolation a With the current interpolation position P i The formula of the contour error ε between is used to calculate the contour error ε in real time; S35, according to the formula of contour error ε and corresponding to ε=-C x e x +C y e y , calculate the corresponding X-axis cross-coupling gain C x and the cross-coupling gain C of the Y axis y ; S36, input the contour error ε into the cross-coupling controller, and set the cross-coupling gain C of the X-axis x , Y-axis cross-coupling gain C y The output u of the cross-coupling controller is compensated to the current interpolation position P of the end effector of the multi-axis robot i The actual input of the multi-axis robot is obtained, and the actual input of the multi-axis robot satisfies the following formula: In the formula, r x is the actual input X-axis coordinate, r y The Y-axis coordinate actually input.

2. The cross-coupling multi-axis synchronous interpolation control method based on PLCopen as claimed in claim 1, characterized in that: According to the starting position P s The coordinates (x s ,y s ) and the end point P of the target trajectory e The coordinates (x e ,y e ), the total interpolation time T of the end effector of the multi-axis robot is calculated using the S-type acceleration and deceleration algorithm sum and real-time theoretical position, the real-time theoretical position being the current interpolation position P i The coordinates (P x ,P y ), as follows: 1) When the target trajectory is a straight line trajectory, perform the following operations: The straight line segment P is obtained from the distance formula between two points. s P e Length Thus, the straight line segment P is calculated s P e The unit vector Where dx is the motion component in the X-axis direction, and dy is the motion component in the Y-axis direction; According to the straight line segment P s P e The length L and the straight line segment P s P e The unit vector Use the S-type acceleration and deceleration algorithm to calculate the total interpolation time T sum 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 P can be obtained i The coordinates (P x ,P y ): 2) When the target trajectory is an arc trajectory, perform the following operations: Use the atan2 function to calculate the starting angle θ of the arc trajectory s and the end angle θ e , where the starting angle θ s P is the center of the arc trajectory o Point to the starting position P s The angle between the vector and the positive direction of the X-axis in the Cartesian coordinate system OXY, the end angle θ e P is the center of the arc trajectory o Point to the end point P of the arc trajectory e The angle between the vector and the positive direction of the X-axis in the Cartesian coordinate system OXY, the starting angle θ s and the end angle θ e The angle range is (-π,π]; The total angular displacement ω is calculated according to the running direction of the arc trajectory, as follows: If the arc trajectory is clockwise, then If the arc trajectory is counterclockwise, then According to the arc segment P s P e The length L = ωR, and the arc segment P s P e Radius Use the S-type acceleration and deceleration algorithm to calculate the total interpolation time T sum 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 P can be obtained i The coordinates (P x ,P y )for: in, i i =S(t) / R; The angle θ of the arc trajectory at the current time t t The calculation of is as follows: If the arc trajectory is clockwise, then i t =θ s -θ i ; If the arc trajectory is counterclockwise, then i t =θ s +θ i ; In the formula, θ i is the angular displacement of the arc from time zero to the current time t, and S(t) is the displacement at the current time t.

3. The cross-coupling multi-axis synchronous interpolation control method based on PLCopen as claimed in 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, among which: The total interpolation time T sum The calculation is as follows: The running time 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 T1, T2, T3, T4, T5, T6, and T7 respectively, and they satisfy: T3=t3-t2=T1 T4=t4-t3 T7=t7-t6=T5 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 v1, v2, v3, v4, v5, v6, and v7, respectively, and satisfy the following requirements: v2=v1+A max T2 v3=V max v4=V max <h2 style=";text-align:left;direction:ltr">v6=v5-D<h2 style=";text-align:left;direction:ltr"> max <h2 style=";text-align:left;direction:ltr"> T6 v7=v e The displacements 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 s1, s2, s3, s4, s5, s6, and s7, respectively, and satisfy: S a =s3 s4=s3+v3T4 s7=L In summary, T4 is solved as follows: Thus, the end times t1, t2, t3, t4, t5, t6, and t7 of the acceleration phase, uniform acceleration phase, deceleration phase, uniform speed phase, acceleration and deceleration phase are calculated, and the total interpolation time T is obtained. sum =t7; The displacement and time relationship of the end effector of the multi-axis robot is as follows: Where S(t) is the displacement at the current time t, J max is the maximum acceleration, A max is the maximum acceleration, D max is the maximum deceleration, V max is the maximum speed, v s is the starting speed, v e is the terminal velocity, which is zero, L is the total displacement, S a is the total displacement of the acceleration section, S d is the total displacement of the deceleration section. The acceleration section includes the acceleration stage, the uniform acceleration stage and the deceleration stage. The deceleration section includes the acceleration and deceleration stage, the uniform deceleration stage and the deceleration and deceleration stage.

4. The cross-coupling multi-axis synchronous interpolation control method based on PLCopen as claimed in claim 1, characterized in that: The atan2 function is used to calculate the starting angle θ of the arc trajectory. s and the end angle θ e Before, also perform the following operations: Determine whether |P is satisfied s P0-P e P o |<σ, if not satisfied, it is considered not to be an arc trajectory, an error warning is issued and the process ends; if satisfied, it is considered to be an arc trajectory, and the atan2 function is used to calculate the starting angle θ of the arc trajectory s and the end angle θ e , where P s P0 is the starting position P s The coordinates (x s ,y s ) to the center P of the arc trajectory o The coordinates (x0,y o ) distance, P e P o is the end point P of the arc trajectory e The coordinates (x e ,y e ) to the center P o The coordinates (x0,y o ), σ is the preset minimum value.

5. The cross-coupling multi-axis synchronous interpolation control method based on PLCopen as claimed in claim 1, characterized in that: The actual position P during the interpolation process is established a With the current interpolation position P i The formula for the contour error ε between is used to calculate the contour error ε in real time, as follows: because, And x =P x -x a ; and y =P y -and a ; In the formula, e x is the X-axis direction error in the Cartesian coordinate system OXY, e y is the Y-axis direction error in the Cartesian coordinate system OXY; When the target trajectory is a straight line trajectory, the formula for the contour error ε is as follows: e=-e x sinth l +e y cosθ l ; In the formula, θ l It is the angle between the straight line trajectory and the positive direction of the X axis in the Cartesian coordinate system OXY; When the target trajectory is a circular arc trajectory, the formula for the contour error ε is as follows: in, By expanding the formula of the contour error ε of the arc trajectory with Taylor formula, we can get: In the formula, f(e x ,e y ,θ t ) represents a higher-order term, that is, an expansion of the second order or higher; Ignore the higher-order terms f(e x ,e y ,θ t ), the formula of contour error ε is simplified as:

6. The cross-coupling multi-axis synchronous interpolation control method based on PLCopen as claimed in claim 5, characterized in that: The cross-coupling controller is a second PID controller and satisfies the following formula: Where u(i) is the output of the cross-coupling controller in the i-th interpolation period, k p is the proportionality coefficient, k i is the integration coefficient, k d is the differential coefficient, T is the interpolation period, ε(i) is the contour error of the i-th interpolation period, ε(i-1) is the contour error of the i-1-th interpolation period, 1≤i≤T sum / T.

7. The cross-coupling multi-axis synchronous interpolation control method based on PLCopen as claimed in claim 5, characterized in that: The X-axis cross-coupling gain C x and the cross-coupling gain C of the Y axis y The calculation is as follows: For a straight line trajectory: C x =sinθ l ,C x =cosθ l For circular arc trajectories:

8. The cross-coupling multi-axis synchronous interpolation control method based on PLCopen as claimed in 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, and 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; When the target trajectory is an arc trajectory, the PLCopen motion control function block is an arc interpolation function block, and the input pins of the arc interpolation function block include an axis group pin AxisGroup, a start pin Execute, a center coordinate pin AuxPoint, an end point coordinate pin EndPoint, a path direction pin PathChoice, a speed pin Velocity, an acceleration pin Acceleration, a deceleration pin Deceleration, an acceleration pin Jerk, and a buffer mode pin BufferMode; the output pins include a completion flag pin Done, a command execution status pin Busy, a command control axis status pin Active, a command interrupt pin CommandAborted, an error status pin Error, and an 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 movement, 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 V max , that is, the speed in the uniform speed stage, the acceleration pin Acceleration is used to specify the maximum acceleration A max The deceleration pin Deceleration is used to specify the maximum deceleration D max The jerk pin Jerk is used to specify the maximum jerk J max The cache mode pin BufferMode is the cache mode between two instructions, the completion mark 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 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