CN108536096A - The three-D profile control method and device of task based access control polar coordinate system - Google Patents
The three-D profile control method and device of task based access control polar coordinate system Download PDFInfo
- Publication number
- CN108536096A CN108536096A CN201810321613.3A CN201810321613A CN108536096A CN 108536096 A CN108536096 A CN 108536096A CN 201810321613 A CN201810321613 A CN 201810321613A CN 108536096 A CN108536096 A CN 108536096A
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- new task
- profile
- point
- polar coordinate
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B19/00—Programme-control systems
- G05B19/02—Programme-control systems electric
- G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
- G05B19/408—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by data handling or data format, e.g. reading, buffering or conversion of data
- G05B19/4086—Coordinate conversions; Other special calculations
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/35—Nc in input of data, input till input file format
- G05B2219/35356—Data handling
Landscapes
- Engineering & Computer Science (AREA)
- Human Computer Interaction (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Feedback Control In General (AREA)
Abstract
The disclosure proposes a kind of three-D profile control method of task based access control polar coordinate system, includes the following steps：The kinetics equation of XYZ triaxial movement platforms is established under world's cartesian coordinate system；According to desired profile traces, new task polar coordinate system is established based on three-D profile track, and calculates corresponding new task polar coordinate system to world's cartesian coordinate system coordinate conversion relation；System dynamics equation under world's cartesian coordinate system is converted to the error dynamics equation under new task polar coordinate system；It is fed back based on feedforward compensation, Set scale derivative controller, and decouples the error dynamics equation.The disclosure has the beneficial effect that：By way of coordinate transform to new task polar coordinate system, it is that a two dimension decouples control problem by three-D profile control errors problem dimensionality reduction, the complexity that design difficulty and controller parameter to obtain simplified three-D profile controller are adjusted, effectively improves the technique effect of the control accuracy of three-D profile.
Description
Technical field
This disclosure relates to the three of the three-D profile control method of servo-drive system more particularly to a kind of task based access control polar coordinate system
Tie up contour outline control method, and the device based on the above method.
Background technology
Usually the purpose of tracing control be in order to reduce the tracking error between physical location and target location, to
So that object follows the track movement of a pre-planning.However, when weighing the quality that machined part surface is processed, wheel
Wide error is often more generally useful used.The tracing control for being directly based upon tracking error is tended not in machined part surface
It is upper to obtain preferable profile errors.In order to meet the processed quality of machined part surface, the tracking based on profile errors
Control is necessary.
There is the contour outline control method of two class mainstreams at present.A kind of method is cross-linked control method, and another kind of is base
In the control method of world's cartesian coordinate system.
The former is by calculating and estimating profile errors in process, to control the gain of each axis controller,
And then the process of control lathe.Apparatus structure due to realizing the above method is simple, and institute's method described above is more universal.Example
Such as the patent of invention of Publication No. CN101114166A《A kind of contour outline control method of complicated track》And Publication No.
The patent of invention of CN102854840A《Based on PREDICTIVE CONTROL and cross-linked straight drive XY platform contours control methods》Needle respectively
Cross-linked control method is used to two axis servo-drive systems, to the profile of direct compensation system on the basis of single shaft control
Error, to improve machining accuracy.However, for traditional uniaxiality tracking control structure, profile errors are only a kind of compensation, and
And this method does not account for system dynamics model.
The latter is that profile Properties Control is directed under world coordinate system.Specifically, this kind of method based on coordinate system transformation
To in processing track feed motion (being moved along reference contours track) and Contour extraction move (with orbit tracking movement
The vertical movement in direction) decoupling, it is respectively controlled as two independent controlled quentity controlled variables.There is no directly against wheel for the above method
Wide error is controlled, but this concept of task coordinate system is introduced in control in order to which direct estimation control error is gone forward side by side
Row profile controls, to be much improved on control performance.For example, being processed for two-dimensional silhouette, Publication No. CN103760816A
Patent of invention《The servo-drive system contour outline control method of task based access control polar coordinate system》It is built by the osculating circle at desired trajectory
A task polar coordinate system is found, and current actual positions are estimated to the most short position of osculating circle, by osculating circle for osculating circle
Radial direction as profile performance indicator, using the tracking angle of osculating circle as feeding property index, to by profile performance and into
Decoupling control is carried out to performance.However, method used in aforementioned invention is carried out both for XY axis kinetic control systems
Two-dimensional silhouette control.Two-dimensional silhouette is controlled, it, can be very square since profile errors and tracking error are all in osculating plane
Just it realizes the decoupling of profile performance and feeding property, but three-D profile is controlled, profile errors are not often closely flat
In face；So being controlled for three-D profile, to realize that the decoupling of profile performance and feeding property is just needed in three-dimensional system of coordinate
Each coordinate direction is controlled, that is, needs that three groups of parameters are adjusted.Since parameter regulation is cumbersome, so this increases
The application difficulty of three-D profile control method is added.
Invention content
The purpose of the disclosure be solve the prior art three-D profile control application in parameter regulation it is excessive and it is cumbersome not
Foot, provides a kind of three-D profile control method and device of task based access control polar coordinate system.It is above-mentioned under the new task polar coordinate system
Scheme, which can obtain, only needs two groups of control parameters of adjusting to can be realized to the profile performance of Three-dimension process and the drop of feeding property
The effect of dimension and decoupling control.
To achieve the goals above, the disclosure uses technical solution below：
It proposes a kind of three-D profile control method of task based access control polar coordinate system, includes the following steps：
S100 the kinetics equation of XYZ triaxial movement platforms) is established under world's cartesian coordinate system, wherein Mei Geyun
Moving axis is set as second-order linearity dynamic system；
S200) according to desired profile traces, new task polar coordinate system is established based on three-D profile track, and calculate corresponding
New task polar coordinate system is to world's cartesian coordinate system coordinate conversion relation；
S300) system dynamics equation under world's cartesian coordinate system is converted to the error under new task polar coordinate system
Kinetics equation；
S400 it) is based on feedforward compensation to feed back, Set scale-derivative controller, and decouples the error dynamics equation.
In a preferred embodiment, the kinetics equation is following second-order linearity power in each movement axis direction
System：
Wherein gi(s) be each axis transmission function, mi,ci,ki∈ R respectively represent quality, damping and the gain of each axis
Coefficient, and the kinetics equation is expressed as matrix form under world's cartesian coordinate systemWherein M, C
∈R3×3Respectively represent the quality constant coefficient diagonal matrix and damping constant coefficient diagonal matrix of each axis, w, u ∈ R3It respectively represents defeated
The position vector and control vector entered.
Further, in above-described embodiment of the disclosure, new task polar coordinate system is established by following sub-step：
S201 the current location point and set point of actual profile track) are measured by the encoder in servo-drive system；
S202 it) is based on profile method of estimation and obtains estimation point；
S203 the first line) is determined based on current location point and estimation point, and the is determined based on set point and estimation point
Two lines；
S204) crosspoint between the perpendicular bisector and the perpendicular bisector of the second line of the first line of setting is new task polar coordinates
The pole of system, the direction of pole to set point are the polar axis direction of new task polar coordinate system；
Wherein, the distance of pole to set point is radius rd, around the counter clockwise direction angle of pole it is new along polar axis direction
The polar angle θ of task coordinate system, polar angle of the set point under new task polar coordinate system are θd。
Still further, in above-described embodiment of the disclosure, for arbitrary point (r, θ), new task polar coordinate system to generation
The coordinate conversion relation T of boundary's cartesian coordinate systemnwFor
Wherein, w is position of the point (r, θ) in world's cartesian coordinate system,It is spin matrix, and estimates
The tangent line rector of pointThe normal line vector of estimation pointAnd the binormal vector of estimation pointIt is calculated according to following formula
Wherein,V=[0rd0]T。wEIt is that estimation point is sat in the world
Position in mark system.It is spin matrix of equal value.It is translation matrix of equal value.VectorIt is from estimation
Vector of the point to current location point.VectorIt is the vector from set point to estimation point.
Still further, in above-described embodiment of the disclosure, step S300) include following sub-step：
S301 first derivatives of the w about time t) is calculated according to following formulaAnd second dervative
S302) the first derivative by w about time tAnd second dervativeThe kinetics equation is substituted into world's flute card
Matrix form under your coordinate system, is converted to the error dynamics equation under new task polar coordinate system
Wherein
Still further, in above-described embodiment of the disclosure, in step S400) in based under new task polar coordinate system
Proportional-plus-derivative governing equation set by error dynamics equation is：
Wherein,KvAnd KpPoint
It is not the proportionality coefficient matrix and differential coefficient matrix of positive definite symmetric matrices form,
Wherein kvr、kvθ、kprAnd kpθIt is coefficient to be adjusted.
Still further, in above-described embodiment of the disclosure, in step S400) in the decoupling error dynamics equation
Including following sub-step：
S401) respectively by proportionality coefficient matrix KvWith differential coefficient matrix KpThe error substituted under new task polar coordinate system is dynamic
Mechanical equationObtain error relationship formula
S402) by kvr=2 ξr(2πfr)、kvθ=2 ξθ(2πfθ)、kpr=(2 π fr)2And kpθ=(2 π fθ)2It is updated to error
After relational expression, Laplace transform is executed with tuning coefficient k to error relationship formulavr、kvθ、kprAnd kpθ。
Secondly, the disclosure also proposes a kind of three-D profile control device of task based access control polar coordinate system, comprises the following modules：
Initialization module, the kinetics equation for establishing XYZ triaxial movement platforms under world's cartesian coordinate system；First modulus of conversion
Block, for according to desired profile traces, establishing new task polar coordinate system based on three-D profile track, and calculate corresponding new task
Polar coordinate system is to world's cartesian coordinate system coordinate conversion relation；Second conversion module, being used for will be under world's cartesian coordinate system
System dynamics equation be converted to the error dynamics equation under new task polar coordinate system；Decoupling module, for based on feedforward
Compensation Feedback, Set scale-derivative controller, and decouple the error dynamics equation.Wherein, in world's cartesian coordinate system
The lower each kinematic axis of the kinetics equation for establishing XYZ triaxial movement platforms is set as second-order linearity dynamic system.
In a preferred embodiment, the kinetics equation is following second-order linearity power in each movement axis direction
System：
Wherein gi(s) be each axis transmission function, mi,ci,ki∈ R respectively represent quality, damping and the gain of each axis
Coefficient, and the kinetics equation is expressed as matrix form under world's cartesian coordinate systemWherein M, C
∈R3×3Respectively represent the quality constant coefficient diagonal matrix and damping constant coefficient diagonal matrix of each axis, w, u ∈ R3It respectively represents defeated
The position vector and control vector entered.
Further, in above-described embodiment of the disclosure, the first conversion module further includes following submodule：Measure mould
Block, current location point and set point for measuring actual profile track by the encoder in servo-drive system；Estimation module is used
In based on profile method of estimation acquisition estimation point；Link module, for determining the first line based on current location point and estimation point,
And the second line is determined based on set point and estimation point；Module is built, the perpendicular bisector and second for the first line to be arranged connect
Crosspoint between the perpendicular bisector of line is the pole of new task polar coordinate system, and the direction of pole to set point is new task polar coordinates
The polar axis direction of system；Wherein, the distance of pole to set point is radius rd, along polar axis direction around the counter clockwise direction angle of pole
For the polar angle θ of new task coordinate system, polar angle of the set point under new task polar coordinate system is θd。
Still further, in above-described embodiment of the disclosure, the first conversion module is according to following coordinate conversion relation Tnw
Arbitrary point (r, θ) is transformed into new task polar coordinate system to world's cartesian coordinate system
Wherein, w is position of the point (r, θ) in world's cartesian coordinate system,It is spin matrix, and estimates
The tangent line rector of pointThe normal line vector of estimation pointAnd the binormal vector of estimation pointIt is calculated according to following formula
Wherein,V=[0rd0]T。wEIt is that estimation point is sat in the world
Position in mark system.It is spin matrix of equal value.It is translation matrix of equal value.VectorIt is from estimating
Vector of the enumeration to current location point.VectorIt is the vector from set point to estimation point.
Still further, in above-described embodiment of the disclosure, the second conversion module includes following submodule：
Derivation module, for calculating first derivatives of the w about time t according to following formulaAnd second dervative
Conversion module, for the first derivative by w about time tAnd second dervativeThe kinetics equation is substituted into exist
Matrix form under world's cartesian coordinate system is converted to the error dynamics equation under new task polar coordinate system
Wherein
Still further, in above-described embodiment of the disclosure, based on the mistake under new task polar coordinate system in decoupling module
Proportional-plus-derivative governing equation set by differential mechanical equation is：
Wherein,KvAnd KpPoint
It is not the proportionality coefficient matrix and differential coefficient matrix of positive definite symmetric matrices form,
Wherein kvr、kvθ、kprAnd kpθIt is coefficient to be adjusted.
Still further, in above-described embodiment of the disclosure, decoupling module includes following submodule：
Module is substituted into, for respectively by proportionality coefficient matrix KvWith differential coefficient matrix KpIt substitutes under new task polar coordinate system
Error dynamics equationObtain error relationship formula
Conversion module is used for kvr=2 ξr(2πfr)、kvθ=2 ξθ(2πfθ)、kpr=(2 π fr)2And kpθ=(2 π fθ)2Generation
Enter to after error relationship formula, Laplace transform is executed with tuning coefficient k to error relationship formulavr、kvθ、kprAnd kpθ。
Finally, the disclosure also discloses a kind of computer readable storage medium, is stored thereon with computer instruction, the instruction
It is realized when being executed by processor such as the step of any one of aforementioned the method.
The disclosure has the beneficial effect that：By way of coordinate transform to new task polar coordinate system, by three-D profile error
Control problem dimensionality reduction is that a two dimension decouples control problem, to obtain design difficulty and the control of simplified three-D profile controller
The complexity of device parameter regulation effectively improves the technique effect of the control accuracy of three-D profile.In addition, the coordinate system transformation method
It is applicable not only in three-D profile error tracing control, applies also for any one existing profile errors method of estimation.
Description of the drawings
Fig. 1 show the system construction drawing of the three-D profile control method for the task polar coordinate system for executing the disclosure；
Fig. 2 show the step flow chart of the three-D profile control method of the task polar coordinate system of the disclosure；
Fig. 3 show new task polar coordinate system under normal circumstances to the transformation between world's cartesian coordinate system coordinate
Relation schematic diagram；
Fig. 4~8 show new task polar coordinate system under special circumstances between world's cartesian coordinate system coordinate
Transformation relation schematic diagram；
Fig. 9 is the three-D profile control system architecture figure based on new task polar coordinate system；
Figure 10 is the three-D profile controller architecture figure based on new task polar coordinate system；
Figure 11 show the function structure chart of the three-D profile control device of the task polar coordinate system of the disclosure；
Figure 12 is shown under space ellipse profile, using method of disclosure change radial error control frequency when angle
The experimental result of error frequency situation；
Figure 13 is shown under space ellipse profile, radial when changing radial error and controlling frequency using method of disclosure
The experimental result of error frequency situation.
Specific implementation mode
The technique effect of the design of the disclosure, concrete structure and generation is carried out below with reference to embodiment and attached drawing clear
Chu, complete description, to be completely understood by the purpose, scheme and effect of the disclosure.It should be noted that the case where not conflicting
Under, the feature in embodiment and embodiment in the disclosure can be combined with each other.The identical attached drawing mark used everywhere in attached drawing
Note indicates same or analogous part.
Fig. 1 show the system construction drawing of the three-D profile control method for the task polar coordinate system for executing the disclosure.This public affairs
It opens using three industrial axis glass-engraving CNC machines as operating platform, illustrates the three-D profile based on new task polar coordinate system
The effect of control method.
According to one embodiment of the disclosure, operating platform uses common computer as the host computer of system, is responsible for
Execute the motion planning of lathe.Specifically, it may be usedController of the microscale experiment case as system real time kinematics
To drive lathe servo-driver, and the signal of encoder on servo-driver is acquired simultaneously.All servo-drivers are all in speed
It works under degree pattern, and position closed loop is carried out to workpiece to be processed according to the profile of planning by controller.
With reference to method flow diagram shown in Fig. 2, in one embodiment according to the disclosure, task based access control polar coordinate system
Three-D profile control method includes the following steps：
S100 the kinetics equation of XYZ triaxial movement platforms) is established under world's cartesian coordinate system；
S200) according to desired profile traces, new task polar coordinate system is established based on three-D profile track, and calculate corresponding
New task polar coordinate system is to world's cartesian coordinate system coordinate conversion relation；
S300) system dynamics equation under world's cartesian coordinate system is converted to the error under new task polar coordinate system
Kinetics equation；
S400 it) is based on feedforward compensation to feed back, Set scale-derivative controller, and decouples the error dynamics equation.
Specifically, in the system shown in figure 1, three axis of three axis numerically controlled machine are designed to mutually orthogonal, and are transported
Moving axis is overlapped with world's cartesian coordinate system.The kinetic model of each kinematic axis may be generally viewed as a second-order linearity dynamics
System：
Wherein gi(s) be each axis transmission function, mi,ci,ki∈ R respectively represent the quality of each axis, damping and gain
Coefficient.Because each kinematic axis of three axis numerically controlled machine is decoupling, the kinetic model of each axis can be by independence
Identification.In one embodiment, sweep sine be used to encourage each axis of lathe, so as to useSystem
Identification toolbox reads the output of each axis.Correspondingly, the kinetics equation of three axis numerically controlled machine is in world's cartesian coordinate system
Under can be write as matrix form：
Wherein M, C ∈ R3×3It is constant coefficient diagonal matrix, and respectively represents the quality and damping matrix of each axis, and w, u ∈
R3Respectively represent the position vector and control vector of input.
With reference to transformation relation schematic diagram shown in Fig. 3, in one embodiment of the disclosure, built based on three-D profile track
Vertical new task polar coordinate system includes following sub-step：
S201 current location the point A and set point D of actual profile track) are measured (i.e. by the encoder in servo-drive system
Ideal position point in desired profile traces)；
S202 it) is based on profile method of estimation and obtains estimation point E；
S203 the first line EA) is determined based on current location point A and estimation point E, and is based on set point D and estimation point E
Determine the second line ED；
S204) crosspoint between the perpendicular bisector and the perpendicular bisector of the second line ED of the first line EA of setting is new task pole
The pole O of coordinate system (nTPCF).Behind the position for calculating estimation point E, set point D and pole O, it is based on above-mentioned three position
A space circle can be obtained, and establishes new task polar coordinate system in the space circle.Wherein, pole O is to the side of set point D
To the polar axis direction for new task polar coordinate system.The distance of pole O to set point D is radius rd, along polar axis direction around pole O's
Counter clockwise direction angle is the polar angle θ of new task coordinate system, and polar angles of the set point D under new task polar coordinate system is θd。
Optionally, estimation point E can by following is intended to the embodiment of infinite profile errors method of estimation by explanation and
It obtains.Standard estimation used herein only obtains a kind of method of estimation point E specific locations, and the disclosure does not limit estimation point
E can also be obtained by existing other methods.
Enable profile traces c in three dimensionsd(t) it is expressed as：
cd(t)=[xd(t)yd(t)zd(t)]T,
Wherein, cd(t) it can be indicated using arc length parameters s.To c near set point D in desired profile tracesd
(s) make Taylor expansion, to obtain the expansion of the positions set point D：
On the other hand, Frenet-Serret frame formula are applied at set point D, can be obtained：
Above-mentioned frame formula substitution expansion can be obtained：
Because of s3Item is in (4) formula relative to s2Influence of the item to profile errors is very small, it is possible to will contain s3Item
It is omitted altogether, to obtain cd(s) standard estimator：
The wherein coefficient of (t, n, b) itemIndicate in desired profile traces some set point
Coordinate in Frenet frames.Closest approach in the true location point of profile to estimated profile traces is denoted as E points, and uses
(tE,nE,bE) indicate coordinate position of the E points in Fernet frames.Wherein, E point coordinates is exactly t in the standard estimation formulas,
Coordinate of the coefficient of n, b, i.e. the E point in Frenet coordinate systems
In the case of certain special, the transformation relation schematic diagram with reference to shown in Fig. 4~8, for following five kinds of special feelings
Condition sets the value of profile errors and tracking error：
(1) current location point A overlaps (as shown in Figure 4) with pole O, then using current location point A as new task polar coordinates
The pole O of system, profile errors are equal to estimation point E to the distance of current location point A at this time, and tracking error is the first line EA at this time
With the angle of the second line ED；
(2) current location point A is overlapped with pole O, conllinear with estimation point E and set point D, and current location point A be located at estimate
Between enumeration E and set point D lines (as shown in Figure 5), profile errors are equal to the distance of estimation point E to current location point A at this time,
Tracking error is π；
(3) current location point A is overlapped with pole O, conllinear with estimation point E and set point D, and current location point A is located at the
Except two line ED (as shown in Figure 6), profile errors are estimation point E to the distance of current location point A, tracking error 0 at this time；
(4) current location point A is overlapped with pole O, and estimation point E overlaps (as shown in Figure 7) with set point D, and profile misses at this time
Difference is estimation point E to the distance of current location point A, tracking error 0；
(5) current location point A, pole O and estimation point E overlap (as shown in Figure 8), and profile errors are 0 at this time, tracking error
For set point D to the distance of current location point A.
After setting up new task coordinate system, coordinate conversion relation of the new task polar coordinate system to world's cartesian coordinate system
TnwIt can be determined by going out the local Frenet coordinate systems of foundation in set point E.Illustratively illustrate to establish below with reference to Fig. 4
Journey can refer to following manner foundation for other special circumstances.
As shown in figure 4, at estimation point E establish as next part Frenet coordinate systems (that is,Coordinate system)：
Wherein,For the tangent line rector of estimation point E,For the normal line vector of estimation point E,For estimation point E binormal to
Amount.Above-mentioned three is unit vector.On the one hand, world's cartesian coordinate system andTransformational relation between coordinate system is such as
Under：
Wherein,It is spin matrix, wEIt is positions of the estimation point E in world coordinate system,Indicate fromCoordinate system to world coordinate system transformation matrix,Indicate from world coordinate system toThe transformation matrix of coordinate system.Separately
On the one hand,The transformational relation that coordinate system is transformed into task polar coordinate system is as follows：
Above formula can be written asWhereinV=[0rd0]T, J is such as
The form of lower matrix：
In conjunction with above-mentioned both sides conversion formula, the coordinate transform of new task polar coordinate system to world's cartesian coordinate system is closed
It is TnwIt can be written as following form：
Wherein, w is position of the point (r, θ) in world's cartesian coordinate system,It is spin matrix of equal value,It is translation matrix of equal value.
Further, according to the coordinate conversion relation T of above-mentioned new task polar coordinate system to world's cartesian coordinate systemnw, generation
System dynamics equation under boundary's cartesian coordinate system can be exchanged into the error dynamics equation under new task polar coordinate system.Specifically
Ground can first ask w about the first derivative of time tAnd second dervative
Then, the first derivative by w about time tAnd second dervativeThe kinetics equation is substituted into world's flute card
Matrix form under your coordinate system, is converted to the error dynamics equation under new task polar coordinate system
Wherein
Based on the error dynamics equation under above-mentioned new task polar coordinate system, the proportional-plus-derivative governing equation of setting is：
Wherein,Indicate the position of the current location point A under new task polar coordinate system,
Indicate the position of the set point D under new task polar coordinate system,
In aforementioned proportion-differential governing equation,It is feedforward compensation term.KvAnd KpIt is the ratio of positive definite symmetric matrices form respectively
Example coefficient matrix and differential coefficient matrix,
Aforementioned proportion coefficient matrix and differential coefficient matrix are substituted into respectivelyCan obtain as
Lower error relationship formula
Then k is enabledvr=2 ξr(2πfr), kvθ=2 ξθ(2πfθ), kpr=(2 π fr)2, kpθ=(2 π fθ)2, and above formula is executed
Laplace transform obtains：
In this way, by adjusting k thereinvr、kvθ、kprAnd kpθ, you can the design realization for completing entire controller is dynamic to error
The decoupling control of mechanical model.Specifically, to obtain critical damping dynamically, in one embodiment of the disclosure, damped coefficient
ξrAnd ξθIt can be both configured to 1, and parameter f is determined by data fit approach according to actual test datarAnd fθ.Art technology
Personnel can select data fit approach commonly used in the art to determine that above-mentioned parameter, the disclosure do not limit this according to actual conditions
It is fixed.Therefore, the system of three-D profile control method of the entire power based on above-described embodiment based on new task polar coordinate system and
The structure of controller can refer to Fig. 9 and structure chart shown in Fig. 10.Wherein εrIt is radial contour error, εθIt is angle profile errors.
TwfIt indicates from world coordinate system to the transformation of local Frenet coordinate systems.
Function structure chart shown in 1 referring to Fig.1, in one embodiment according to the disclosure, task based access control polar coordinate system
Three-D profile control device include following module：Initialization module, for establishing tri- axis of XYZ under world's cartesian coordinate system
The kinetics equation of motion platform；First conversion module, for according to desired profile traces, being established based on three-D profile track new
Task polar coordinate system, and corresponding new task polar coordinate system is calculated to world's cartesian coordinate system coordinate conversion relation；Second turn
Block is changed the mold, is moved for the system dynamics equation under world's cartesian coordinate system to be converted to the error under new task polar coordinate system
Mechanical equation；Decoupling module, for being fed back based on feedforward compensation, Set scale-derivative controller is dynamic to decouple the error
Mechanical equation.
Specifically, in one embodiment of the disclosure, three axis of three axis numerically controlled machine be designed to it is mutually orthogonal,
And kinematic axis is overlapped with world's cartesian coordinate system.It is dynamic that the kinetic model of each kinematic axis may be generally viewed as a second-order linearity
Mechanical system：
Wherein gi(s) be each axis transmission function, mi,ci,ki∈ R respectively represent the quality of each axis, damping and gain
Coefficient.Because each kinematic axis of three axis numerically controlled machine is decoupling, the kinetic model of each axis can be by independence
Identification.In one embodiment, sweep sine be used to encourage each axis of lathe, so as to useSystem
Identification toolbox reads the output of each axis.Correspondingly, the kinetics equation of three axis numerically controlled machine is in world's cartesian coordinate system
Under can be write as matrix form：
Wherein M, C ∈ R3×3It is constant coefficient diagonal matrix, and respectively represents the quality and damping matrix of each axis, and w, u ∈
R3Respectively represent the position vector and control vector of input.
Further, in the above embodiment of the present invention, the first conversion module further includes following submodule：Measure mould
Block, current location point A and set point D for measuring actual profile track by the encoder in servo-drive system；Estimation module,
Estimation point E is obtained for being based on profile method of estimation；Link module, for determining first based on current location point A and estimation point E
Line EA, and the second line ED is determined based on set point D and estimation point E；Module is built, for being arranged in the first line EA
Crosspoint between vertical line and the perpendicular bisector of the second line ED is the pole O of new task polar coordinate system.Calculate estimation point E,
Behind the position of set point D and pole O, a space circle can be obtained based on above-mentioned three position, and built in the space circle
Vertical new task polar coordinate system.Wherein, pole O to the direction of set point D be new task polar coordinate system polar axis direction.Pole O is arrived
The distance of set point D is radius rd, along polar axis direction around pole O counter clockwise direction angle be new task coordinate system polar angle θ,
Polar angles of the set point D under new task polar coordinate system is θd。
Optionally, estimation point E can by following is intended to the embodiment of infinite profile errors method of estimation by explanation and
It obtains.Standard estimation used herein only obtains a kind of method of estimation point E specific locations, and the disclosure does not limit estimation point
E can also be obtained by existing other methods.
Enable profile traces c in three dimensionsd(t) it is expressed as：
cd(t)=[xd(t) yd(t) zd(t)]T,
Wherein, cd(t) it can be indicated using arc length parameters s.To c near set point D in desired profile tracesd
(s) make Taylor expansion, to obtain the expansion of the positions set point D：
On the other hand, Frenet-Serret frame formula are applied at set point D, can be obtained：
Above-mentioned frame formula substitution expansion can be obtained：
Because of s3Item is in (4) formula relative to s2Influence of the item to profile errors is very small, it is possible to will contain s3Item
It is omitted altogether, to obtain cd(s) standard estimator：
The wherein coefficient of (t, n, b) itemIndicate in desired profile traces some set point
Coordinate in Frenet frames.Closest approach in the true location point of profile to estimated profile traces is denoted as estimation point E, and
Use (tE,nE,bE) indicate coordinate positions of the estimation point E in Fernet frames.Wherein, the coordinate of estimation point E is exactly the mark
T in quasi- estimation formulas, n, the coefficient of b, i.e. coordinates of the estimation point E in Frenet coordinate systems
In the case of certain special, the transformation relation schematic diagram with reference to shown in Fig. 4~8, for following five kinds of special feelings
Condition sets the value of profile errors and tracking error：
(1) current location point A overlaps (as shown in Figure 4) with pole O, then using current location point A as new task polar coordinates
The pole O of system, profile errors are equal to estimation point E to the distance of current location point A at this time, and tracking error is the first line EA at this time
With the angle of the second line ED；
(2) current location point A is overlapped with pole O, conllinear with estimation point E and set point D, and current location point A be located at estimate
Between enumeration E and set point D lines (as shown in Figure 5), profile errors are equal to the distance of estimation point E to current location point A at this time,
Tracking error is π；
(3) current location point A is overlapped with pole O, conllinear with estimation point E and set point D, and current location point A is located at the
Except two line ED (as shown in Figure 6), profile errors are estimation point E to the distance of current location point A, tracking error 0 at this time；
(4) current location point A is overlapped with pole O, and estimation point E overlaps (as shown in Figure 7) with set point D, and profile misses at this time
Difference is estimation point E to the distance of current location point A, tracking error 0；
(5) current location point A, pole O and estimation point E overlap (as shown in Figure 8), and profile errors are 0 at this time, tracking error
For set point D to the distance of current location point A.
After setting up new task coordinate system, the first conversion module can be by going out the local Frenet of foundation in set point E
Coordinate system, determine new task polar coordinate system to world's cartesian coordinate system coordinate conversion relation Tnw.Below with reference to Fig. 4 examples
Property illustrate to establish process, for other special circumstances, can refer to following manner foundation.
As shown in figure 4, at estimation point E establish as next part Frenet coordinate systems (that is,Coordinate system)：
Wherein,For the tangent line rector of estimation point E,For the normal line vector of estimation point E,For estimation point E binormal to
Amount.Above-mentioned three is unit vector.On the one hand, world's cartesian coordinate system andTransformational relation between coordinate system is such as
Under：
Wherein,It is spin matrix, wEIt is positions of the estimation point E in world coordinate system,Indicate fromCoordinate system to world coordinate system transformation matrix,Indicate from world coordinate system toThe transformation matrix of coordinate system.Separately
On the one hand,The transformational relation that coordinate system is transformed into task polar coordinate system is as follows：
Above formula can be written asWhereinV=[0rd0]T, J is such as
The form of lower matrix：
In conjunction with above-mentioned both sides conversion formula, the coordinate transform of new task polar coordinate system to world's cartesian coordinate system is closed
It is TnwIt can be written as following form：
Wherein, w is position of the point (r, θ) in world's cartesian coordinate system,It is spin matrix of equal value,It is translation matrix of equal value.
Further, according to the coordinate conversion relation T of above-mentioned new task polar coordinate system to world's cartesian coordinate systemnw, the
Two conversion modules include following submodule, and new task is converted to the system dynamics equation realized under world's cartesian coordinate system
Error dynamics equation under polar coordinate system：
Derivation module, for calculating first derivatives of the w about time t according to following formulaAnd second dervative
Conversion module, for the first derivative by w about time tAnd second dervativeThe kinetics equation is substituted into exist
Matrix form under world's cartesian coordinate system is converted to the error dynamics equation under new task polar coordinate system
Wherein
Decoupling module is controlled based on the error dynamics equation under above-mentioned new task polar coordinate system, the proportional-plus-derivative of setting
Equation is：
Wherein,Indicate the position of the current location point A under new task polar coordinate system,
Indicate the position of the set point D under new task polar coordinate system,
In aforementioned proportion-differential governing equation,It is feedforward compensation term.KvAnd KpIt is the ratio of positive definite symmetric matrices form respectively
Example coefficient matrix and differential coefficient matrix,
Substitution module in decoupling module substitutes into aforementioned proportion coefficient matrix and differential coefficient matrixIt can obtain error relationship formula
Then by the conversion module in decoupling module by kvr=2 ξr(2πfr), kvθ=2 ξθ(2πfθ), kpr=(2 π fr)2,
kpθ=(2 π fθ)2It is updated to error relationship formula, and Laplace transform is executed to error relationship formula and is obtained：
In this way, by adjusting k thereinvr、kvθ、kprAnd kpθ, you can the design realization for completing entire controller is dynamic to error
The decoupling control of mechanical model.Specifically, to obtain critical damping dynamically, in one embodiment of the disclosure, damped coefficient
ξrAnd ξθIt can be both configured to 1, and parameter f is determined by data fit approach according to actual test datarAnd fθ.Art technology
Personnel can select data fit approach commonly used in the art to determine that above-mentioned parameter, the disclosure do not limit this according to actual conditions
It is fixed.
In one embodiment of the disclosure, following space ellipse profile is as test curve：
Wherein, the parameter of test curve is respectively a=b=50mm, c=20mm, ω=1rad/s, the period of desired profile
For 2 π s, damped coefficient ξrAnd ξθIt is both configured to 1, and determines by data fit approach to obtain parameter fr=80Hz, fθ=60Hz.
Test result as shown in figure 12, when changing radial error control frequency, angular error frequency does not change substantially.It is similar
Ground, test result as shown in fig. 13 that, when changing angular error and controlling frequency, radial error frequency does not also change.On
It states two test results and shows that radial error control frequency and angular error control frequency are independent from each other, therefore disclosure institute
The method of proposition is decoupling.
In another embodiment of the disclosure, common spatially spiral line is bent as test in another contour machining
Line.The parameter of the test curve is as follows：
Coefficient ξr、ξθ、frAnd fθUsing data identical with previous embodiment.Following table is shown based on above-mentioned test curve,
The disclosure proposes that the three-D profile control method of task based access control polar coordinate system and the three-D profile based on task with traditional coordinate system are missed
Poor comparison diagram.Wherein, εnTPCFIt is the three-D profile error of the three-D profile control method of task based access control polar coordinate system, εTCFIt is base
In the three-D profile error of task with traditional coordinate system, εfIt is root mean square profile errors, and εrIt is radial contour error.It can from following table
Method to find out task based access control polar coordinate system that the disclosure is proposed controls error relative to task with traditional coordinate system improving
It all improves a lot in terms of profile errors.
| εf | εr | εTCF | εnTPCF | |
| Maximum value | 0.05 | 0.046 | 0.05 | 0.014 |
| Average value | 0.045 | 0.015 | 0.045 | 0.0029 |
It should be appreciated that the embodiment of the present invention can by computing device, hardware and software combination or pass through storage
Computer instruction in non-transitory computer-readable memory is effected or carried out.The method can use standard program
Technology-include realized in computer program configured with the non-transitory computer-readable storage media of computer program, wherein
Configured in this way storage medium make computer operated in a manner of specific and is predefined-according in a particular embodiment describing
Method and attached drawing.Each program can be realized with the programming language of level process or object-oriented with logical with computer system
Letter.However, if desired, the program can be realized with compilation or machine language.In addition, the program can be in programming for this purpose
It is run on application-specific integrated circuit.
Although description of the invention is quite detailed and especially several embodiments are described, it is not
Any of these details or embodiment or any specific embodiments are intended to be limited to, but it is by reference to appended that should be considered as
Claim considers that the prior art provides the possibility explanation of broad sense for these claims, to effectively cover the present invention
Preset range.In addition, with the foreseeable embodiment of inventor, present invention is described above, its purpose is to be provided with
Description, and those equivalent modifications that the present invention can be still represented to the unsubstantiality change of the present invention still unforeseen at present.
Claims (9)
1. a kind of three-D profile control method of task based access control polar coordinate system, which is characterized in that include the following steps：
S100 the kinetics equation of XYZ triaxial movement platforms) is established under world's cartesian coordinate system, wherein each kinematic axis
It is set as second-order linearity dynamic system；
S200) according to desired profile traces, new task polar coordinate system is established based on three-D profile track, and is calculated corresponding newly appointed
Polar coordinate system of being engaged in is to world's cartesian coordinate system coordinate conversion relation；
S300) system dynamics equation under world's cartesian coordinate system is converted to the error dynamics under new task polar coordinate system
Learn equation；
S400 it) is based on feedforward compensation to feed back, Set scale-derivative controller, and decouples the error dynamics equation.
2. according to the method described in claim 1, it is characterized in that, the second-order linearity of each kinematic axis of the kinetics equation
Dynamic system is
Wherein gi(s) be each axis transmission function, mi,ci,ki∈ R respectively represent the quality of each axis, damping and gain coefficient；
The kinetics equation is expressed as matrix form under world's cartesian coordinate systemWherein M, C ∈ R3×3Point
The quality constant coefficient diagonal matrix and damping constant coefficient diagonal matrix of each axis, w, u ∈ R are not represented3Respectively represent the position of input
Set vector sum control vector.
3. according to the method described in claim 2, it is characterized in that, new task polar coordinate system is established by following sub-step：
S201 the current location point (A) and set point (D) of actual profile track) are measured by encoder in servo-drive system；
S202 it) is based on profile method of estimation and obtains estimation point (E)；
S203 the first line (EA)) is determined based on current location point (A) and estimation point (E), and based on set point (D) and estimation
Point (E) determines the second line (ED)；
S204) crosspoint between the perpendicular bisector and the perpendicular bisector of the second line (ED) of the first line of setting (EA) is new task pole
The pole (O) of coordinate system；
Wherein, pole (O) arrives set point to the polar axis direction that the direction of set point (D) is new task polar coordinate system, pole (O)
(D) distance is radius rd, along polar axis direction around the polar angle θ that the counter clockwise direction angle of pole (O) is new task coordinate system, give
It is θ to pinpoint the polar angle of (D) under new task polar coordinate systemd。
4. according to the method described in claim 3, it is characterized in that, for arbitrary point (r, θ), new task polar coordinate system to the world
The coordinate conversion relation T of cartesian coordinate systemnwFor
Wherein, w is position of the point (r, θ) in world's cartesian coordinate system,It is spin matrix, and estimation point
(E) tangent line rectorThe normal line vector of estimation point (E)And the binormal vector of estimation point (E)It is calculated according to following formula
V=[0 rd 0]T, wEIt is estimation point (E) in world coordinate system
In position, andIt is spin matrix,It is translation matrix.
5. according to the method described in claim 4, it is characterized in that, step S300) include following sub-step：
S301 first derivatives of the w about time t) is calculated according to following formulaAnd second dervative
S302) the first derivative by w about time tAnd second dervativeThe kinetics equation is substituted into sit in world Descartes
Matrix form under mark system, is converted to the error dynamics equation under new task polar coordinate system
Wherein
6. according to the method described in claim 5, it is characterized in that, in step S400) in based under new task polar coordinate system
Proportional-plus-derivative governing equation set by error dynamics equation is：
Wherein,KvAnd KpIt is respectively
The proportionality coefficient matrix and differential coefficient matrix of positive definite symmetric matrices form,
Wherein kvr、kvθ、kprAnd kpθIt is coefficient to be adjusted.
7. according to the method described in claim 6, it is characterized in that, in step S400) in the decoupling error dynamics equation
Including following sub-step：
S401) respectively by proportionality coefficient matrix KvWith differential coefficient matrix KpSubstitute into the error dynamics under new task polar coordinate system
EquationObtain error relationship formula
S402) by kvr=2 ξr(2πfr)、kvθ=2 ξθ(2πfθ)、kpr=(2 π fr)2And kpθ=(2 π fθ)2It is updated to error relationship formula
Afterwards, Laplace transform is executed with tuning coefficient k to error relationship formulavr、kvθ、kprAnd kpθ。
8. a kind of three-D profile control device of task based access control polar coordinate system, which is characterized in that comprise the following modules：
Initialization module, the kinetics equation for establishing XYZ triaxial movement platforms under world's cartesian coordinate system；
First conversion module, for according to desired profile traces, establishing new task polar coordinate system based on three-D profile track, and count
Corresponding new task polar coordinate system is calculated to world's cartesian coordinate system coordinate conversion relation；
Second conversion module, for the system dynamics equation under world's cartesian coordinate system to be converted to new task polar coordinate system
Under error dynamics equation；
Decoupling module, for being fed back based on feedforward compensation, Set scale-derivative controller, and decouple the error dynamics side
Journey.
9. a kind of computer readable storage medium, is stored thereon with computer instruction, it is characterised in that the instruction is held by processor
The step of method as described in any one of claim 1 to 7 is realized when row.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810321613.3A CN108536096B (en) | 2018-04-11 | 2018-04-11 | Three-dimensional contour control method and device based on task polar coordinate system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810321613.3A CN108536096B (en) | 2018-04-11 | 2018-04-11 | Three-dimensional contour control method and device based on task polar coordinate system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108536096A true CN108536096A (en) | 2018-09-14 |
| CN108536096B CN108536096B (en) | 2020-12-29 |
Family
ID=63480852
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810321613.3A Active CN108536096B (en) | 2018-04-11 | 2018-04-11 | Three-dimensional contour control method and device based on task polar coordinate system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108536096B (en) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2006187045A (en) * | 2004-12-24 | 2006-07-13 | Mitsubishi Heavy Ind Ltd | Controller for permanent magnet type synchronous motor |
| CN101114166A (en) * | 2007-09-13 | 2008-01-30 | 暨南大学 | Contour outline control method for complicated track |
| CN102854840A (en) * | 2012-09-24 | 2013-01-02 | 沈阳工业大学 | Direct-driven XY table profile control method based on predictive control and cross coupling |
| CN103760816A (en) * | 2013-12-30 | 2014-04-30 | 哈尔滨工业大学深圳研究生院 | Servo system contour control method based on task polar coordinate system |
| CN104777465A (en) * | 2014-01-09 | 2015-07-15 | 江南大学 | Random extended object shape and state estimation method based on B spline function |
| CN105003537A (en) * | 2015-07-14 | 2015-10-28 | 广东省自动化研究所 | Air-suspending vibration signal inhibition-based method and system |
| CN106774163A (en) * | 2016-12-08 | 2017-05-31 | 哈尔滨工业大学深圳研究生院 | High-precision three-dimensional contour outline control method and device |
| CN107273852A (en) * | 2017-06-16 | 2017-10-20 | 华南理工大学 | Escalator floor plates object and passenger behavior detection algorithm based on machine vision |
-
2018
- 2018-04-11 CN CN201810321613.3A patent/CN108536096B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2006187045A (en) * | 2004-12-24 | 2006-07-13 | Mitsubishi Heavy Ind Ltd | Controller for permanent magnet type synchronous motor |
| CN101114166A (en) * | 2007-09-13 | 2008-01-30 | 暨南大学 | Contour outline control method for complicated track |
| CN102854840A (en) * | 2012-09-24 | 2013-01-02 | 沈阳工业大学 | Direct-driven XY table profile control method based on predictive control and cross coupling |
| CN103760816A (en) * | 2013-12-30 | 2014-04-30 | 哈尔滨工业大学深圳研究生院 | Servo system contour control method based on task polar coordinate system |
| CN104777465A (en) * | 2014-01-09 | 2015-07-15 | 江南大学 | Random extended object shape and state estimation method based on B spline function |
| CN105003537A (en) * | 2015-07-14 | 2015-10-28 | 广东省自动化研究所 | Air-suspending vibration signal inhibition-based method and system |
| CN106774163A (en) * | 2016-12-08 | 2017-05-31 | 哈尔滨工业大学深圳研究生院 | High-precision three-dimensional contour outline control method and device |
| CN107273852A (en) * | 2017-06-16 | 2017-10-20 | 华南理工大学 | Escalator floor plates object and passenger behavior detection algorithm based on machine vision |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108536096B (en) | 2020-12-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111590594B (en) | Robot trajectory tracking control method based on visual guidance | |
| Lei et al. | Accuracy enhancement of five-axis CNC machines through real-time error compensation | |
| Wang et al. | Newton-ILC contouring error estimation and coordinated motion control for precision multiaxis systems with comparative experiments | |
| Fu et al. | Accuracy enhancement of five-axis machine tool based on differential motion matrix: geometric error modeling, identification and compensation | |
| Bi et al. | Identification and compensation of geometric errors of rotary axes on five-axis machine by on-machine measurement | |
| Bohez | Compensating for systematic errors in 5-axis NC machining | |
| Li et al. | Dual sliding mode contouring control with high accuracy contour error estimation for five-axis CNC machine tools | |
| CN109773786A (en) | A kind of industrial robot plane precision scaling method | |
| CN105509671B (en) | A kind of robot tooling center points scaling method using plane reference plate | |
| CN109454281B (en) | Method for calibrating propeller workpiece coordinate system in robot milling | |
| Zhang et al. | Design and implementation of hybrid force/position control for robot automation grinding aviation blade based on fuzzy PID | |
| CN107450473A (en) | A kind of calculating of CFXYZA types five-axle number control machine tool rotary shaft geometric error, compensation and its verification method | |
| CN108527373A (en) | The parameter measurement of mechanical arm and discrimination method and device, terminal, storage medium | |
| WO1982004336A1 (en) | Tool diameter correcting method for numerical control device | |
| Hu et al. | Accurate three-dimensional contouring error estimation and compensation scheme with zero-phase filter | |
| Lei et al. | NURBS-based fast geometric error compensation for CNC machine tools | |
| CN109366220A (en) | A kind of workpiece localization method and system | |
| CN101666619A (en) | Method for calculating absolute coordinates of work piece | |
| CN107457785B (en) | Robot position compensation method based on joint feedback | |
| CN110716497B (en) | Registration method based on plane reference constraint and margin constraint | |
| CN107806825A (en) | The line lathe space geometry error measure discrimination method of three face five based on plane grating | |
| Yang et al. | A high accuracy on-line estimation algorithm of five-axis contouring errors based on three-point arc approximation | |
| CN107589720A (en) | A kind of equivalent plane cross-coupling control method | |
| CN108436915A (en) | Dual robot motion control method | |
| Wang et al. | Design and implementation of five-axis transformation function in CNC system |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |