CN115390512B - Flexible gantry double-drive system and electromechanical combined decoupling motion control method thereof - Google Patents

Abstract

The present invention relates to the field of motor drive and motion control, in particular, to a flexible gantry double drive system and its electromechanical combined decoupling motion control method, including a flexible support device, a linear motor, a linear guide rail and a beam. The flexible support device includes a deflector Flexible hinges and rotary flexible hinges use flexible hinge devices to realize sliding connection and flexible support of beams and linear guide rails. The invention is based on the gantry double-drive motion platform of the permanent magnet synchronous linear motor, divides the motion of the gantry double-drive into a translation circuit and a rotation circuit, and controls them separately, and solves the synchronization problem in the motion control of the gantry double-drive with an asymmetric flexible support structure It improves the synchronization performance and convergence speed of the dual-drive motion, realizes effective suppression of internal coupling and external disturbances, and improves the positioning accuracy of the gantry dual-drive motion platform.

Description

Flexible gantry double drive system and its electromechanical joint decoupling motion control method

technical field

The invention relates to the field of motor drive and motion control, in particular to a flexible gantry double drive system and an electromechanical combined decoupling motion control method thereof.

Background technique

As an industrial device, the gantry double-drive motion platform has important uses in the fields of precision instruments, high-end machine tools, and semiconductor equipment. The mechanical coupling introduced by the beam makes the dual-drive synchronous control difficult, especially the coupling problem of the system, which will seriously affect the positioning accuracy of the gantry dual-drive system. In addition, various unknown external disturbances further affect the dual-drive synchronization performance of the system.

Most of the existing gantry double-drive systems use rigid support structures, relying on the system adjustment accuracy and master-slave axis synchronization accuracy to control the influence of the internal force of the beam, which cannot be completely suppressed under complex working conditions. "Research on the Design and Modeling of Partially Replaceable Flexible Joints on Double-Drive Gantry Platforms" published a flexible support joint for double-sided thin wall beams. Its disadvantage is that it cannot overcome the left and right vibration of the beam in the span direction. There is displacement, and at the same time, it is easy to generate left and right oscillations, which affects the overall accuracy of the system.

The existing gantry double-drive algorithm mostly adopts the idea of double-sided synchronization and suppresses asynchronous interference as the guiding ideology, such as the article "Synchronization Control of Double-drive System of Gantry Machine Tool Based on Disturbance Observer". However, it is impossible to realize the complete synchronization of double-drive Yes, the emergence of two-sided asynchrony can lead to rapid changes in the system model.

The invention fully considers the consistency of the mechanical model and the electrical model, realizes the combined decoupling of the electromechanical combination of the double drive of the gantry through the design of the flexible support and the design of the control algorithm, and overcomes various defects of the existing gantry servo system.

Contents of the invention

Aiming at the defects in the prior art, the object of the present invention is to provide a flexible gantry double drive system and its electromechanical combined decoupling motion control method.

A flexible gantry double drive system provided according to the present invention includes a flexible support device, a linear motor, a linear guide rail and a beam;

The flexible supporting device includes a twisted flexible hinge and a rotary flexible hinge. The two sets of linear motors are respectively installed on the two linear guide rails arranged in parallel at intervals. The rotary flexible hinge is connected with two sets of linear motors, the deflection flexible hinge provides relative rotation in the horizontal plane of the beam and relative displacement along its span direction, and the rotary flexible hinge provides relative rotation in the horizontal plane of the beam. relative rotation;

The movement control of the end of the crossbeam connected with the twisted flexible hinge adopts translation loop control, wherein the translation loop follows the position-velocity cascade control mode, and the movement control of the end of the crossbeam connected with the rotary flexible hinge The rotation loop control is adopted, wherein the rotation loop follows the position single-stage control mode, and the control quantity controlled by the translation loop and the control quantity controlled by the rotation loop are processed as the current loop control setting, and the current loop control adopts vector control Decoupled control under the framework.

In some embodiments, the torsionally flexible hinge includes a first beam connecting frame, a first line rail connecting block, a first vertical stop piece, and a first longitudinal stop piece;

The first beam connecting frame is a hollow structural frame, the upper and lower end surfaces of the first line rail connecting block are respectively connected to the upper and lower beams of the first beam connecting frame through the first vertical stop piece, and the first The left and right sides of the line rail connection block are respectively connected to the left and right beams of the first crossbeam connecting frame through the first longitudinal stop piece, and one end of the crossbeam is located on the left and right beams of the first crossbeam connecting frame;

The first vertical stopper restricts the relative displacement between the first beam connecting frame and the second line rail connecting block in the vertical direction, and the first longitudinal stopper restricts the first beam connecting frame The relative displacement of the line rail connection block in the sliding direction of the beam.

In some embodiments, the rotary flexible hinge includes a second beam connecting frame, a second line rail connecting block, a second vertical stop piece, a second longitudinal stop piece, a transverse stop piece and a transverse stop plate;

The second beam connecting frame is a hollow structural frame, and the upper and lower end faces of the second line rail connecting block are respectively connected to the upper and lower beams of the second beam connecting frame through the second vertical stop piece. The left and right sides of the rail connection block are respectively connected to the left and right beams of the second beam connecting frame through the second longitudinal stop piece, and the transverse stop plate is respectively connected to the second beam through the transverse stop piece The upper and lower beams of the frame and the second line rail connecting block are connected,

The second vertical stop piece restricts the relative displacement between the second beam connection frame and the second line rail connection block in the vertical direction, and the second longitudinal stop piece restricts the first cross beam connection frame With the relative displacement of the second line rail connecting block in the sliding direction of the crossbeam, the transverse stop piece and the transverse stop plate limit the forward and backward movement of the first crossbeam connecting frame and the second line rail connecting block. Relative displacement.

The present invention also provides an electromechanical combined decoupling motion control method for a flexible gantry double drive system, using the flexible gantry double drive system, including the following steps:

Step 100, the controller sets the movement position of the double drive of the gantry;

Step 200, measure the actual displacements of the two shafts of the dual drive, and calculate the system feedback;

Step 300: Divide the dual-drive motion control into translation loop control and rotation loop control, the translation loop control is position-velocity cascade control, and the rotation loop control is angle single-stage control;

Step 400: Using the deflection angle of the beam and the angular velocity of the beam deflection angle as variables of the non-singular terminal sliding mode controller, calculate the angle control quantity of the rotation loop;

Step 500: Process the translational loop control quantity and the rotational loop control quantity as a current loop control setting, and the current loop control adopts decoupling control under the vector control framework;

In some embodiments, the step 200 includes:

The two-axis displacement measurement step: measure the actual displacement of the two axes through the grating ruler, and take the displacement of the main shaft as the translational displacement, and the main shaft is the main shaft of a linear motor on a specified side;

Calculation steps of translational velocity: perform differential calculation on the displacement of the main shaft to obtain the translational velocity of the main shaft;

Calculation steps of the deflection angle of the beam: the deflection angle of the beam is obtained by calculating the displacement of the two axes and the distance between the two parallel axes.

In some embodiments, step 300 includes:

The control steps of the position loop of the translation loop: the position loop control of the translation loop adopts proportional-differential control, the position loop of the translation loop receives the position given command of the motion controller, and the control amount is input to the speed loop;

The speed loop control steps of the translation loop: the speed loop design of the translation loop is based on the model compensation second-order linear expansion state observer decoupling control loop;

Rotational loop angle loop control steps: Rotational loop angle loop design is based on model compensation third-order linear expansion state observer decoupling control loop.

In some implementations, the step of controlling the velocity loop of the translation loop includes:

The dynamic calculation steps of the known model of the translational circuit: it is calculated from two measurable calculations: the translational velocity and the deflection angular velocity of the beam;

Observation steps of the disturbance term based on the extended state observer: design a second-order linear extended state observer with model compensation for the velocity loop of the translational loop, the state variable is the translational velocity, and expand the total disturbance term f _T into a new state for observation, And set its derivative as r _t , the total disturbance item is composed of dynamic and control errors, modeling errors, unmodeled high-order dynamics and external disturbances that are not easy to measure in the actual system, then the state equation of the gantry translation is written as:

y _T = C _T z _T

z₁为平动速度，z₂为总扰动项f_T所作为的新状态，f_T0为模型已知部分，u_T为平动回路速度环输出，y_T为速度环模型输出，b_T0反映控制量与被控量导数之间的关系；in:

z ₁ is the translation velocity, z ₂ is the new state made by the total disturbance item f _T , f _T0 is the known part of the model, u _T is the output of the velocity loop of the translation circuit, y _T is the output of the velocity loop model, and b _T0 reflects The relationship between the control quantity and the derivative of the controlled quantity;

Based on the second-order velocity loop model after expanding the disturbed state into a new state, a second-order linear extended state observer with model compensation is designed:

为观测器输出，L_T为观测器增益矩阵，设置增益矩阵使观测器具备BIBO稳定性，其BIBO为有界输入-有界输出；in:

is the output of the observer, L _T is the gain matrix of the observer, setting the gain matrix makes the observer have BIBO stability, and its BIBO is bounded input-bounded output;

Calculation steps of the control quantity: the control quantity of the position loop is input to the speed loop as the given speed V _Td , following the control law:

Calculate the control quantity u _T ; where: K _T is the speed error gain.

In some implementations, the step of controlling the rotation loop angle loop includes:

The dynamic calculation steps of the known model: the known model dynamics of the rotation circuit is calculated by the center of gravity interference and the Y-axis acceleration interference composed of the disturbance moment brought by the flexible support, the acceleration of the translation ring and the load position. The disturbance moment brought by the flexible support is determined by the beam For the calculation of deflection angle, the center of gravity disturbance composed of the acceleration of the translation ring and the load position is replaced by the X-axis acceleration reference, and the Y-axis acceleration disturbance is replaced by the Y-axis acceleration reference;

Expanding the state observer step; the rotation loop angle ring design has a third-order linear extended state observer with model compensation, the state variables are beam deflection angle and beam deflection angle angular velocity, and the total disturbance term f _R is expanded into a new state for observation, and Let its derivative be r _R , and the total disturbance item is composed of dynamics that are not easy to measure in the actual system, modeling and control errors, unmodeled high-order dynamics, and external disturbances, then the state equation of the gantry angle is written as:

y _R =C _R z _R

为偏转角度，/>

为偏转角角速度，/>

为总扰动项f_R所作为的新状态，f_R0为模型已知部分，u_R为转动回路角度环输出，y_R为角度环模型输出，b_R0反映控制量与被控量导数之间的关系；in:

is the deflection angle, />

is the deflection angular velocity, />

is the new state of the total disturbance item f _R , f _R0 is the known part of the model, u _R is the output of the angle loop of the rotation loop, y _R is the output of the angle loop model, b _R0 reflects the relationship between the control variable and the derivative of the controlled variable relation;

Based on the third-order angle loop model after expanding the disturbed state into a new state, a third-order linear extended state observer with model compensation is designed:

为观测器输出，L_R为观测器增益矩阵，设置增益矩阵使得观测器具备BIBO稳定性，扩张状态观测器输出扩张状态变量总扰动项观测结果；in:

is the output of the observer, L _R is the gain matrix of the observer, the gain matrix is set so that the observer has BIBO stability, and the extended state observer outputs the observation result of the total disturbance item of the expanded state variable;

Control steps: Control the two-axis synchronous movement beam deflection angle given as 0, follow the control law:

为给定角速度，设定为0。Calculate the control quantity u _R ; where: K _R is the angle error gain, Θ _gd is the given angle, set to 0, D _R is the angular velocity error gain,

For the given angular velocity, set it to 0.

In some implementations, the step of sliding mode control of the rotation loop includes:

Sliding mode function design steps: According to the system rotation equation, regardless of the total disturbance state, the state space equation of the system is:

Design the sliding mode function:

Among them: s is the function of the system state, p and q are both positive odd numbers and have

Design control law design steps: design nonlinear control law according to the designed sliding mode function:

f _Rmax is the upper bound of the total disturbance term, η>0.

In some embodiments, the current loop control step under the vector control framework includes:

Current setting step: calculate the two-axis current setting from the control values of the speed loop of the translation loop and the angle loop of the rotation loop;

Electrical angle calculation steps: After the platform is powered on, find the initial phase of the motor as the zero point for recording the electrical angle, and calculate the electrical angle through the position information measured by the incremental grating ruler;

Current acquisition step: use the Hall sensor to sample and measure the two-phase current of the motor, and calculate the current of the other phase according to the theoretical relationship of the three-phase current;

The step of calculating the current in the two-phase rotating coordinate system: according to the calculated electrical angle, the three-phase current of the motor in the three-phase stationary coordinate system is converted into the current represented by the dq axis of the two-phase rotating coordinate system;

d-axis voltage control steps: the d-axis current given value is 0, proportional-integral control is adopted, and feed-forward compensation is added to eliminate the disturbance of the product of q-axis current and rotational speed;

q-axis voltage control steps: the q-axis current is given by the speed loop of the translation loop and the angle loop of the rotation loop, using proportional-integral control, and adding feed-forward compensation to eliminate the interference of the d-axis current and the flux linkage of the permanent magnet;

The voltage calculation step of the two-phase stationary coordinate system: according to the calculated electrical angle, the current loop control quantity in the two-phase rotating coordinate system is converted into the voltage represented by the αβ axis of the two-phase stationary coordinate system.

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

1. The present invention solves the synchronization problem and coupling problem in the double-drive motion control of the gantry with an asymmetric flexible support structure, improves the synchronization performance and convergence speed of the double-drive motion, realizes effective suppression of internal coupling and external disturbance, and improves the gantry The positioning accuracy of the dual-drive motion platform.

2. The present invention adopts a non-singular terminal sliding mode control strategy, which improves the dynamic response performance of the system.

3. The present invention adopts the current loop control scheme under the vector control framework to realize the decoupling control of the three-phase current of the controlled object motor.

Description of drawings

Other characteristics, objects and advantages of the present invention will become more apparent by reading the detailed description of non-limiting embodiments made with reference to the following drawings:

Fig. 1 is a schematic diagram of the overall structure of the flexible gantry double drive system of the present invention;

Fig. 2 is a structural schematic diagram of the twisted flexible hinge of the present invention;

Fig. 3 is a structural schematic diagram of the rotary flexible hinge of the present invention;

Fig. 4 is a schematic flow chart of the electromechanical combined decoupling motion control method of the flexible gantry double drive system of the present invention;

Fig. 5 is a control schematic diagram of the electromechanical combined decoupling motion control method of the flexible gantry double drive system of the present invention.

Detailed ways

The present invention will be described in detail below in conjunction with specific embodiments. The following examples will help those skilled in the art to further understand the present invention, but do not limit the present invention in any form. It should be noted that those skilled in the art can make several changes and improvements without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

Example 1

The present invention provides a flexible gantry double drive system formed under the mechanical decoupling design method for eliminating internal stress of the beam and guide rail of the gantry double drive platform. The flexible gantry double drive system mainly includes a flexible support device 1, a linear motor 2, The linear guide rail 3 and the cross beam 4, the flexible support device 1 includes a deflection flexible hinge 11 and a rotary flexible hinge 12, the flexible hinge device 1 is used to realize the sliding connection and flexible support of the cross beam 4 and the linear guide rail 3, one side of the cross beam 4 and the linear guide rail 3 The rotary flexible hinge 12 that can realize relative rotation in the horizontal plane is used for connection and support, and the other side of the beam and the linear guide rail are connected and supported by the deflection flexible hinge 11 that can realize relative rotation in the horizontal plane and the relative displacement in the span direction of the beam, so as to realize Elimination of internal force and mechanical decoupling of the beam and guide rail of the gantry double-drive platform.

The torsion flexible hinge 11 mainly includes a first beam connecting frame 111 , a first line rail connecting block 112 , a first vertical stop piece 113 and a first longitudinal stop piece 114 . The first beam connecting frame 11 is a hollow structural frame surrounded by four beams. The four beams are arranged up, down, left, and right. The upper and lower end faces of the first line rail connecting block 112 are connected to the first beam connecting frame through the first vertical stop piece 113 respectively. The upper and lower two beams of 111 are connected, the first vertical stopper 113 is a thin-walled structure, and its thin-walled surface is located in a vertical plane parallel to the sliding direction of the line rail, and the first vertical stopper 113 limits the first beam The relative displacement between the connecting frame 111 and the second line rail connecting block 112 in the vertical direction. The left and right sides of the first line rail connecting block 112 are respectively connected to the left and right two beams of the first beam connecting frame 111 through the first longitudinal stop piece 114. The first longitudinal stop piece is a thin-walled structure, and its thin-walled surface is located at the same In a vertical plane parallel to the sliding direction of the line rail, the first longitudinal stop piece 114 limits the relative displacement of the first cross beam connecting frame 111 and the line rail connecting block 112 in the sliding direction of the cross beam 4 . One end of the crossbeam 4 is located on the left and right two beams of the first crossbeam connecting frame 111, bosses can be set on the left and right two beams of the first crossbeam connecting frame 111, and one end of the crossbeam 4 is located on the boss. The first vertical stop piece 113 and the first longitudinal stop piece 114 in the bias flexible hinge 11 exist in pairs, and there may be one pair or multiple pairs. Preferably, the torsionally flexible hinge 11 is formed by integral manufacturing.

The rotary flexible hinge 12 includes a second beam connecting frame 121 , a second rail connecting block 122 , a second vertical stop piece 123 , a second longitudinal stop piece 124 , a transverse stop piece 125 and a transverse stop plate 126 . The second connecting frame 121 is a hollow frame with the same structure as the first beam connecting frame 111, the second vertical stop piece 123 and the first vertical stop piece 113 and the second vertical stop piece 124 and the first vertical stop piece The sheets 114 are all thin-walled sheets with the same structure, and the transverse stop sheet 125 is also of thin-walled sheet structure. The upper and lower ends of the second line rail connecting block 122 are respectively connected to the upper and lower beams of the second beam connecting frame 121 through the second vertical stop piece 123, and the thin-walled surface of the second vertical stop piece 123 is located parallel to the sliding direction of the line rail. In the vertical plane of , the second vertical stop piece 123 limits the relative displacement of the second beam connecting frame 121 and the second line rail connecting block 122 in the vertical direction. The left and right sides of the second line rail connecting block 122 are respectively connected to the left and right beams of the second beam connecting frame 121 through the second longitudinal stop piece 124, and the thin-walled surface of the second longitudinal stop piece 124 is located In the straight plane, the second longitudinal stop piece 124 limits the relative displacement of the first beam connecting frame 121 and the second line rail connecting block 122 in the sliding direction of the beam 4 . The transverse stop plate 126 is respectively connected with the upper and lower beams of the second beam connecting frame 12 and the second line rail connection block 122 through the transverse stop piece 125. In the plane, the transverse stop piece 125 and the transverse stop plate 126 limit the relative displacement of the first beam connecting frame 121 and the second line rail connecting block 122 in the front-to-back direction. The number of the second vertical stop piece 123 , the second longitudinal stop piece 124 and the transverse stop piece 125 in the rotary flexible hinge 12 is preferably in pairs, and may be one or more pairs. Preferably, the rotary flexible hinge 12 is formed through integrated manufacturing.

Example 2

In this embodiment 2, an electromechanical joint decoupling motion control method for a flexible gantry double drive system formed on the basis of embodiment 1 is adopted. The motion platform divides the movement of the gantry double drive into a translational circuit and a rotational circuit, which are controlled separately, and realize the synchronization and decoupling of the double drive. Loop control, wherein the translation loop follows the position-velocity cascade control mode, and the motion control of the end connected to the crossbeam 4 and the rotary flexible hinge 12 adopts the rotation loop control, wherein the rotation loop follows the position single-stage control mode, and the translation loop control The control quantity and the control quantity of the rotation loop control are processed as the current loop control setting, and the current loop control adopts the decoupling control under the vector control framework. The specific steps include the following:

In step 100, the controller specifies the movement position of the double-drive gantry.

Step 200, measuring the actual displacement of the two shafts of the dual drive, and calculating the system feedback, including the following steps:

Two-axis displacement measurement step: measure the actual displacement of the two axes through the grating ruler, specify the position of the main axis of the linear motor on one side, and take the displacement of the main axis as the translational displacement;

Calculation steps of translational velocity: perform differential calculation on the displacement of the main shaft to obtain the translational velocity of the main shaft;

Calculation steps of the deflection angle of the beam: the deflection angle of the beam is obtained by calculating the displacement of the two axes and the distance between the two parallel axes.

Step 300: Divide the dual-drive motion control into translation loop control and rotation loop control, the translation loop control is position-speed cascade control, and the rotation loop control is position single-stage control, including the following steps:

The control steps of the position loop of the translation loop: the position loop control of the translation loop adopts proportional-differential control, the position loop of the translation loop receives the position given command of the motion controller, and the control amount is input to the speed loop;

The speed loop control steps of the translational loop: the speed loop design of the translational loop is based on the model compensation second-order linear expansion state observer decoupling control loop, including the following:

The dynamic calculation steps of the known model of the translational circuit: it is calculated from two measurable calculations: the translational velocity and the deflection angular velocity of the beam;

Observation steps of the disturbance term based on the extended state observer: design a second-order linear extended state observer with model compensation for the velocity loop of the translational loop, the state variable is the translational velocity, and expand the total disturbance term f _T into a new state for observation, And set its derivative as r _t , the total disturbance item is composed of dynamic and control errors, modeling errors, unmodeled high-order dynamics and external disturbances that are not easy to measure in the actual system, then the state equation of the gantry translation is written as:

y _T = C _T z _T

z₁为平动速度，z₂为总扰动项f_T所作为的新状态，f_T0为模型已知部分，u_T为平动回路速度环输出，y_T为速度环模型输出，b_T0反映控制量与被控量导数之间的关系；in:

z ₁ is the translation velocity, z ₂ is the new state made by the total disturbance item f _T , f _T0 is the known part of the model, u _T is the output of the velocity loop of the translation circuit, y _T is the output of the velocity loop model, and b _T0 reflects The relationship between the control quantity and the derivative of the controlled quantity;

Based on the second-order velocity loop model after expanding the disturbed state into a new state, a second-order linear extended state observer with model compensation is designed:

为观测器输出，L_T为观测器增益矩阵，另外，l₁和l₂为根据实际条件计算的增益值，是无量纲的比例，设置增益矩阵使观测器具备BIBO稳定性，其BIBO为有界输入-有界输出；in:

is the output of the observer, L _T is the gain matrix of the observer, in addition, l ₁ and l ₂ are the gain values calculated according to the actual conditions, which are dimensionless ratios, setting the gain matrix makes the observer have BIBO stability, and its BIBO is bounded input - bounded output;

Calculation steps of the control quantity: the control quantity of the position loop is input to the speed loop as the given speed V _Td , following the control law:

Calculate the control quantity u _T ; where: K _T is the speed error gain.

Rotational loop angle loop control steps: Rotational loop angle loop design is based on model compensation third-order linear expansion state observer decoupling control loop.

The dynamic calculation steps of the known model: the known model dynamics of the rotation circuit is calculated by the center of gravity interference and the Y-axis acceleration interference composed of the disturbance moment brought by the flexible support, the acceleration of the translation ring and the load position. The disturbance moment brought by the flexible support is determined by the beam For the calculation of deflection angle, the center of gravity disturbance composed of the acceleration of the translation ring and the load position is replaced by the X-axis acceleration reference, and the Y-axis acceleration disturbance is replaced by the Y-axis acceleration reference;

Expanding the state observer step; the rotation loop angle ring design has a third-order linear extended state observer with model compensation, the state variables are beam deflection angle and beam deflection angle angular velocity, and the total disturbance term f _R is expanded into a new state for observation, and Let its derivative be r _R , and the total disturbance item is composed of dynamics that are not easy to measure in the actual system, modeling and control errors, unmodeled high-order dynamics, and external disturbances, then the state equation of the gantry angle is written as:

y _R = C _R z _R

为偏转角度，/>

为偏转角角速度，/>

为总扰动项f_R所作为的新状态，f_R0为模型已知部分，u_R为转动回路角度环输出，y_R为角度环模型输出，b_R0反映控制量与被控量导数之间的关系；in:

is the deflection angle, />

is the deflection angular velocity, />

is the new state of the total disturbance item f _R , f _R0 is the known part of the model, u _R is the output of the angle loop of the rotation loop, y _R is the output of the angle loop model, b _R0 reflects the relationship between the control variable and the derivative of the controlled variable relation;

Based on the third-order angle loop model after expanding the disturbed state into a new state, a third-order linear extended state observer with model compensation is designed:

为观测器输出，L_R为观测器增益矩阵，设置增益矩阵使得观测器具备BIBO稳定性，扩张状态观测器输出扩张状态变量总扰动项观测结果；in:

is the output of the observer, L _R is the gain matrix of the observer, the gain matrix is set so that the observer has BIBO stability, and the extended state observer outputs the observation result of the total disturbance item of the expanded state variable;

Control steps: Control the two-axis synchronous movement beam deflection angle given as 0, follow the control law:

为给定角速度，设定为0。Calculate the control quantity u _R ; where: K _R is the angle error gain, Θ _gd is the given angle, set to 0, D _R is the angular velocity error gain,

For the given angular velocity, set it to 0.

Step 400: Using beam deflection angle and beam deflection angular velocity as variables of the non-singular terminal sliding mode controller to calculate the position control quantity of the rotation loop;

Sliding mode function design steps: According to the system rotation equation, regardless of the total disturbance state, the state space equation of the system is:

Design the sliding mode function:

Among them: s is the function of the system state, p and q are both positive odd numbers and have

Design control law design steps: design nonlinear control law according to the designed sliding mode function:

f _Rmax is the upper bound of the total disturbance item, η>0, n and β are proportional coefficients.

Step 500: Process the translational loop control quantity and the rotational loop control quantity as a current loop control setting, and the current loop control adopts decoupling control under the vector control framework;

Current setting step: calculate the two-axis current setting from the control values of the speed loop of the translation loop and the angle loop of the rotation loop;

Electrical angle calculation steps: After the platform is powered on, find the initial phase of the motor as the zero point for recording the electrical angle, and calculate the electrical angle through the position information measured by the incremental grating ruler;

Current acquisition step: use the Hall sensor to sample and measure the two-phase current of the motor, and calculate the current of the other phase according to the theoretical relationship of the three-phase current;

The step of calculating the current in the two-phase rotating coordinate system: according to the calculated electrical angle, the three-phase current of the motor in the three-phase stationary coordinate system is converted into the current represented by the dq axis of the two-phase rotating coordinate system;

d-axis voltage control steps: the d-axis current given value is 0, proportional-integral control is adopted, and feed-forward compensation is added to eliminate the disturbance of the product of q-axis current and rotational speed;

q-axis voltage control steps: the q-axis current is given by the speed loop of the translation loop and the angle loop of the rotation loop, using proportional-integral control, and adding feed-forward compensation to eliminate the interference of the d-axis current and the flux linkage of the permanent magnet;

The voltage calculation step of the two-phase stationary coordinate system: according to the calculated electrical angle, the current loop control quantity in the two-phase rotating coordinate system is converted into the voltage represented by the αβ axis of the two-phase stationary coordinate system.

In the description of this application, it should be understood that the terms "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", The orientation or positional relationship indicated by "bottom", "inner", "outer", etc. is based on the orientation or positional relationship shown in the drawings, and is only for the convenience of describing the application and simplifying the description, rather than indicating or implying the referred device Or elements must have a certain orientation, be constructed and operate in a certain orientation, and thus should not be construed as limiting the application.

Those skilled in the art know that, in addition to realizing the system, device and each module thereof provided by the present invention in a purely computer-readable program code mode, the system, device and each module thereof provided by the present invention can be completely programmed by logically programming the method steps. The same program is implemented in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, and embedded microcontrollers, among others. Therefore, the system, device and each module provided by the present invention can be regarded as a hardware component, and the modules included in it for realizing various programs can also be regarded as the structure in the hardware component; A module for realizing various functions can be regarded as either a software program realizing a method or a structure within a hardware component.

Specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art may make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. In the case of no conflict, the embodiments of the present application and the features in the embodiments can be combined with each other arbitrarily.

Claims (7)

1. The flexible gantry double-drive system is characterized by comprising a flexible supporting device (1), a linear motor (2), a linear guide rail (3) and a cross beam (4);

the flexible supporting device (1) comprises a deflection flexible hinge (11) and a rotation flexible hinge (12), two groups of linear motors (2) are respectively arranged on two linear guide rails (3) which are arranged in parallel at intervals, two ends of the cross beam (4) are respectively connected with the two groups of linear motors (2) through the deflection flexible hinge (1) and the rotation flexible hinge (12), the deflection flexible hinge (11) provides relative rotation in the horizontal plane of the cross beam (4) and relative displacement along the span direction of the cross beam, and the rotation flexible hinge (12) provides relative rotation in the horizontal plane of the cross beam (4);

the motion control of one end, connected with the deflection flexible hinge (11), of the cross beam (4) adopts translational loop control, wherein the translational loop follows a position-speed cascade control mode, the motion control of one end, connected with the rotation flexible hinge (12), of the cross beam (4) adopts rotary loop control, wherein the rotary loop follows a position single-stage control mode, the control quantity of the translational loop control and the control quantity of the rotary loop control are processed and then used as current loop control setting, and the current loop control adopts decoupling control under a vector control frame;

the translational loop control and the rotational loop control comprise translational loop position loop control, translational loop speed loop control and rotational loop angle loop control;

the translational loop speed loop control comprises:

the step of dynamically calculating a known model of the translation loop: the translational speed and the beam deflection angular speed can be measured and calculated;

a disturbance term observation step based on an extended state observer: the translational loop speed loop designs a second-order linear expansion state observer with model compensation, the state variable is translational speed, and the total disturbance term f is calculated _T Expand to a new state for observation and set the derivative thereof as r _t The total disturbance term consists of dynamic and control errors which are difficult to measure in an actual system, modeling errors, unmodeled high-order dynamic and external disturbance, and a gantry translational state equation is written as follows:

y _T ＝C _T z _T

wherein:

C _T ＝[1 0]，z ₁ is the translational velocity, z ₂ For the total disturbance term f _T New state as f _T0 For the known part of the model, u _T For translational loop velocity loop output, y _T B for velocity loop model output _T0 Reflecting the relationship between the control quantity and the controlled quantity derivative;

based on a second-order velocity loop model after expanding the disturbance state into a new state, designing a second-order linear expansion state observer with model compensation:

L _T ＝[l ₁ l ₂ ] ^T

wherein:

for observer output, L _T Setting a gain matrix for the observer to ensure that the observer has BIBO stability, wherein BIBO is a bounded input-bounded output;

a control amount calculation step: the position loop control quantity is input to the speed loop as a given speed V _Td Following the control law:

calculating a control amount u _T The method comprises the steps of carrying out a first treatment on the surface of the Wherein: k (K) _T Gain for speed error;

the control of the rotating loop angle ring comprises the following steps:

the known model dynamic calculation step: the known model of the rotating loop dynamically calculates the disturbance moment and Y-axis acceleration disturbance, which are formed by the disturbance moment, translational ring acceleration and load position together and are formed by the flexible support, the disturbance moment and Y-axis acceleration disturbance, which are formed by the translational ring acceleration and the load position together, are calculated by the deflection angle of the cross beam, the disturbance moment and Y-axis acceleration disturbance are replaced by the X-axis acceleration setting, and the Y-axis acceleration disturbance is replaced by the Y-axis acceleration setting;

an extended state observer step; the rotating loop angle ring designs a third-order linear expansion state observer with model compensation, state variables are a beam deflection angle and a beam deflection angle angular speed, and the total disturbance item f _R Expand to a new state for observation and set the derivative thereof as r _R The total disturbance term is formed by the dynamic state, modeling and control errors which are not easy to measure in an actual system and is not modeledThe high-order dynamic and external disturbance components, the gantry angle state equation is written as:

y _R ＝C _R z _R

wherein:

C _R ＝[1 0 0]，ζ ₁ zeta is the deflection angle ₂ Zeta is the angular velocity of deflection ₃ For the total disturbance term f _R New state as f _R0 For the known part of the model, u _R For the output of the rotary loop angle ring, y _R For angular ring model output, b _R0 Reflecting the relationship between the control quantity and the controlled quantity derivative;

based on a third-order angle ring model after the disturbance state is expanded to a new state, designing a third-order linear expansion state observer with model compensation:

L _R ＝(β ₁ ，β ₂ ，β ₃ ) ^T

wherein:

for observer output, L _R Setting a gain matrix for the observer so that the observer has BIBO stability and expandsThe state observer outputs the observation result of the total disturbance term of the expansion state variable;

the control step: the deflection angle of the beam for controlling the two-axis synchronous movement is given as 0, and the control law is followed:

calculating a control amount u _R The method comprises the steps of carrying out a first treatment on the surface of the Wherein: k (K) _R For angular error gain Θ _gd For a given angle, set to 0, D _R For the gain of the angular velocity error,

for a given angular velocity, 0 is set.

2. The flexible gantry dual drive system of claim 1, wherein the deflection flexible hinge (11) comprises a first beam connection (111), a first rail connection block (112), a first vertical stop tab (113), and a first longitudinal stop tab (114);

the first beam connecting frame (111) is a hollow structure frame, the upper end face and the lower end face of the first wire rail connecting block (112) are respectively connected with the upper beam and the lower beam of the first beam connecting frame (111) through the first vertical stop piece (113), the left side face and the right side face of the first wire rail connecting block (112) are respectively connected with the left beam and the right beam of the first beam connecting frame (111) through the first vertical stop piece (114), and one end of the beam (4) is located on the left beam and the right beam of the first beam connecting frame (111);

the first vertical stop piece (113) limits the relative displacement of the first beam connecting frame (111) and the first wire rail connecting block (112) in the vertical direction, and the first longitudinal stop piece (114) limits the relative displacement of the first beam connecting frame (111) and the wire rail connecting block (112) in the sliding direction of the beam (4).

3. The flexible gantry dual drive system of claim 1 or 2, wherein the swivel flexible hinge (12) comprises a second cross beam connection (121), a second wire rail connection block (122), a second vertical stop tab (123), a second longitudinal stop tab (124), a transverse stop tab (125), and a transverse stop plate (126);

the second beam connecting frame (121) is a hollow structure frame, the upper and lower end surfaces of the second wire rail connecting block (122) are respectively connected with the upper and lower beams of the second beam connecting frame (121) through the second vertical stop piece (123), the left and right side surfaces of the second wire rail connecting block (122) are respectively connected with the left and right beams of the second beam connecting frame (121) through the second longitudinal stop piece (124), the transverse stop plate (126) is respectively connected with the upper and lower beams of the second beam connecting frame (12) and the second wire rail connecting block (122) through the transverse stop piece (125),

the second vertical stop piece (123) limits the relative displacement of the second beam connecting frame (121) and the second wire rail connecting block (122) in the vertical direction, the second longitudinal stop piece (124) limits the relative displacement of the second beam connecting frame (121) and the second wire rail connecting block (122) in the sliding direction of the beam (4), and the transverse stop piece (125) and the transverse stop plate (126) limit the relative displacement of the second beam connecting frame (121) and the second wire rail connecting block (122) in the front-back direction.

4. An electromechanical combined decoupling motion control method of a flexible gantry double-drive system, which is characterized by adopting the flexible gantry double-drive system as claimed in any one of claims 1-3, comprising the following steps:

step 100, a controller gives a gantry double-drive movement position;

step 200, measuring the actual displacement of the two shafts of the double drive, and calculating the feedback quantity of the system;

step 300: the double-drive motion control is divided into translational loop control and rotational loop control, wherein the translational loop control is position-speed cascade control, and the rotational loop control is angle single-stage control;

step 400: calculating the angle control quantity of the rotating loop by using the beam deflection angle and the beam deflection angular velocity as variables of a nonsingular terminal sliding mode controller;

step 500: the translational loop control quantity and the rotational loop control quantity are processed and then are given as current loop control, and the current loop control adopts decoupling control under a vector control frame;

the step 300 includes:

translational loop position loop control step: the translational loop position loop control adopts proportional-differential control, the translational loop position loop receives a position given instruction of the motion controller, and the control quantity is input to the speed loop;

translational loop speed loop control step: the translational loop speed loop design is based on a model compensation second-order linear expansion state observer decoupling control loop;

and a rotary loop angle ring control step: the design of the angle ring of the rotating loop is based on a model compensation third-order linear expansion state observer decoupling control loop;

the translational loop speed loop control step comprises the following steps:

the step of dynamically calculating a known model of the translation loop: the translational speed and the beam deflection angular speed can be measured and calculated;

a disturbance term observation step based on an extended state observer: the translational loop speed loop designs a second-order linear expansion state observer with model compensation, the state variable is translational speed, and the total disturbance term f is calculated _T Expand to a new state for observation and set the derivative thereof as r _t The total disturbance term consists of dynamic and control errors which are difficult to measure in an actual system, modeling errors, unmodeled high-order dynamic and external disturbance, and a gantry translational state equation is written as follows:

y _T ＝C _T z _T

wherein:

C _T ＝[1 0]，z ₁ for the translation speed, the speed of the translation,z ₂ for the total disturbance term f _T New state as f _T0 For the known part of the model, u _T For translational loop velocity loop output, y _T B for velocity loop model output _T0 Reflecting the relationship between the control quantity and the controlled quantity derivative;

based on a second-order velocity loop model after expanding the disturbance state into a new state, designing a second-order linear expansion state observer with model compensation:

L _T ＝[l ₁ l ₂ ] ^T

wherein:

for observer output, L _T Setting a gain matrix for the observer to ensure that the observer has BIBO stability, wherein BIBO is a bounded input-bounded output;

a control amount calculation step: the position loop control quantity is input to the speed loop as a given speed V _Td Following the control law:

calculating a control amount u _T The method comprises the steps of carrying out a first treatment on the surface of the Wherein: k (K) _T Gain for speed error;

the rotating loop angle ring control step comprises the following steps:

the known model dynamic calculation step: the known model of the rotating loop dynamically calculates the disturbance moment and Y-axis acceleration disturbance, which are formed by the disturbance moment, translational ring acceleration and load position together and are formed by the flexible support, the disturbance moment and Y-axis acceleration disturbance, which are formed by the translational ring acceleration and the load position together, are calculated by the deflection angle of the cross beam, the disturbance moment and Y-axis acceleration disturbance are replaced by the X-axis acceleration setting, and the Y-axis acceleration disturbance is replaced by the Y-axis acceleration setting;

an extended state observer step; the rotating loop angle ring designs a third-order linear expansion state observer with model compensation, state variables are a beam deflection angle and a beam deflection angle angular speed, and the total disturbance item f _R Expand to a new state for observation and set the derivative thereof as r _R The total disturbance term consists of dynamic and modeling and control errors which are not easy to measure in an actual system, unmodeled high-order dynamic and external disturbance, and then a gantry angle state equation is written as follows:

y _R ＝C _R z _R

wherein:

C _R ＝[1 0 0]，ζ ₁ zeta is the deflection angle ₂ Zeta is the angular velocity of deflection ₃ For the total disturbance term f _R New state as f _R0 For the known part of the model, u _R For the output of the rotary loop angle ring, y _R For angular ring model output, b _R0 Reflecting the relationship between the control quantity and the controlled quantity derivative;

based on a third-order angle ring model after the disturbance state is expanded to a new state, designing a third-order linear expansion state observer with model compensation:

L _R ＝(β ₁ ，β ₂ ，β ₃ ) ^T

wherein:

for observer output, L _R Setting a gain matrix for the observer so that the observer has BIBO stability, and outputting an observation result of the total disturbance term of the extended state variable by the extended state observer;

the control step: the deflection angle of the beam for controlling the two-axis synchronous movement is given as 0, and the control law is followed:

calculating a control amount u _R The method comprises the steps of carrying out a first treatment on the surface of the Wherein: k (K) _R For angular error gain Θ _gd For a given angle, set to 0, D _R For the gain of the angular velocity error,

for a given angular velocity, 0 is set.

5. The method for controlling the electromechanical joint decoupling motion of a flexible gantry dual drive system according to claim 4, wherein said step 200 comprises:

a two-axis displacement measurement step: measuring the actual displacement of two shafts through a grating ruler, and taking the displacement of a main shaft as translational displacement, wherein the main shaft is the main shaft of a specified linear motor on a certain side;

and a translation speed calculating step: differential calculation is carried out on the displacement of the main shaft to obtain the translational speed of the main shaft;

calculating a beam deflection angle: and calculating the deflection angle of the cross beam through the displacement of the two shafts and the distance between the two parallel shafts.

6. The method for controlling the electromechanical coupling decoupling motion of the flexible gantry dual-drive system according to claim 4, wherein the rotating loop sliding mode control step comprises the following steps:

the design step of the sliding mode function: according to the system rotation equation, the state space equation of the system is as follows, regardless of the total disturbance state:

designing a sliding mode function:

wherein: s is a function of the system state, p and q are both positive and odd numbers and have

Designing a design control rule, namely: designing a nonlinear control law according to the designed sliding mode function:

f _Rmax and eta is more than 0 as the upper bound of the total disturbance term.

7. The method for controlling the electromechanical joint decoupling motion of the flexible gantry dual-drive system according to claim 4, wherein the step of controlling the current loop under the vector control frame comprises the steps of:

a current setting step: calculating the given two-axis current by using the control quantity of the translational loop speed loop and the rotational loop angle loop;

and an electrical angle calculating step: searching an initial phase of a motor as a zero point for recording an electrical angle after the platform is electrified, and calculating the electrical angle through position information measured by an incremental grating ruler;

and a current collection step: using a Hall sensor to sample and measure two-phase current of the motor, and calculating current of the other phase according to the theoretical relation of three-phase current;

and a two-phase rotation coordinate system current calculation step: according to the calculated electrical angle, converting three-phase current of the motor under a three-phase static coordinate system into current represented under the dq axis of a two-phase rotating coordinate system;

d-axis voltage control step: d-axis current given quantity is 0, proportional-integral control is adopted, feedforward compensation is added to eliminate disturbance of q-axis current and rotation speed product;

q-axis voltage control step: the q-axis current is given by a translational loop speed loop and a rotational loop angle loop, proportional-integral control is adopted, and feedforward compensation is added to eliminate the interference of d-axis current and permanent magnet flux linkage;

a two-phase stationary coordinate system voltage calculation step: the current loop control amount in the two-phase rotating coordinate system is converted into a voltage expressed in the alpha beta axis of the two-phase stationary coordinate system according to the calculated electrical angle.

Images