CN104135212B - Flexibility motion control IP (Intellectual Property) core and implementation method thereof - Google Patents
Flexibility motion control IP (Intellectual Property) core and implementation method thereof Download PDFInfo
- Publication number
- CN104135212B CN104135212B CN201410348942.9A CN201410348942A CN104135212B CN 104135212 B CN104135212 B CN 104135212B CN 201410348942 A CN201410348942 A CN 201410348942A CN 104135212 B CN104135212 B CN 104135212B
- Authority
- CN
- China
- Prior art keywords
- module
- max
- acceleration
- interpolation
- deceleration
- 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.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims abstract description 17
- 230000001133 acceleration Effects 0.000 claims abstract description 182
- 230000036461 convulsion Effects 0.000 claims description 60
- 238000006073 displacement reaction Methods 0.000 claims description 38
- 238000012545 processing Methods 0.000 claims description 29
- 230000003139 buffering effect Effects 0.000 claims description 14
- 238000007781 pre-processing Methods 0.000 claims description 13
- 206010044565 Tremor Diseases 0.000 claims description 12
- JEIPFZHSYJVQDO-UHFFFAOYSA-N ferric oxide Chemical compound O=[Fe]O[Fe]=O JEIPFZHSYJVQDO-UHFFFAOYSA-N 0.000 claims description 10
- 230000000737 periodic effect Effects 0.000 claims description 10
- 230000005540 biological transmission Effects 0.000 claims description 7
- 230000010354 integration Effects 0.000 claims description 6
- 230000001360 synchronised effect Effects 0.000 claims description 5
- 210000001367 artery Anatomy 0.000 claims description 2
- 210000003462 vein Anatomy 0.000 claims description 2
- 239000000047 product Substances 0.000 claims 2
- 239000006227 byproduct Substances 0.000 claims 1
- 230000010363 phase shift Effects 0.000 claims 1
- 238000004080 punching Methods 0.000 claims 1
- 230000008901 benefit Effects 0.000 description 6
- 230000006870 function Effects 0.000 description 6
- 230000008520 organization Effects 0.000 description 6
- 238000010586 diagram Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 102100026816 DNA-dependent metalloprotease SPRTN Human genes 0.000 description 1
- 101710175461 DNA-dependent metalloprotease SPRTN Proteins 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 235000013399 edible fruits Nutrition 0.000 description 1
- 239000000686 essence Substances 0.000 description 1
- 239000003550 marker Substances 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 239000004575 stone Substances 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Landscapes
- Numerical Control (AREA)
Abstract
The invention discloses a flexibility motion control IP (Intellectual Property) core and an implementation method thereof, belonging to the field of motion control. The IP core comprises a acceleration/deceleration module, an interpolation module, a pulse counting module, a pulse generation module, a bus controller and an RAM (Random Access Memory) interface module. The flexibility motion control IP core and the implementation method thereof aim to solve the problems that high speed and high precision cannot be achieved due to incapability of further reducing a control period since a motion control algorithm has a long running period in a general MCU (Micro Controller Unit) or DSP (Digital Signal Processor). The IP core has the characteristics of short control period and high output pulse precision, non-symmetrical linear acceleration and deceleration can be achieved, non-symmetrical S-shaped curve acceleration and deceleration can be achieved, two-axis or three-axis linear interpolation with the linear or the S-shaped curve acceleration and deceleration can be achieved, and two-axis arc interpolation at an uniform speed also can be achieved. Through the adoption of the IP core, motion control relevant algorithm is realized in a Verilog hardware description language soft core mode, the motion control relevant algorithm can be implemented in an FPGA (Field Programmable Gate Array), can be implemented in an ASIC (Application Specific Integrated Circuit) mode, and can be integrated into SoC (System On Chip), the typical working frequency is 100MHz, and feasibility is provided for building a flexibility motion control system.
Description
Technical field
The present invention relates to the research field of motion control, more particularly to a kind of flexibility motion control IP kernel and realization side
Method.
Background technology
General servomotor has three kinds of control modes:Speed control method, direct torque mode, position control mode.
Servo-drive system has inertia and the system for loading as one, either that control mode, and servomotor is required to be added and subtracted
Speed control.When servomotor is from a speed (v1) it is transitioned into another speed (v2), it is necessary to carry out deceleration planning.Mesh
The method of front proposition has linear acceleration and deceleration, Exponential acceleration and deceleration, S curve acceleration and deceleration, trigonometric function acceleration and deceleration.Commonly use in engineering
It is linear acceleration and deceleration and S curve acceleration and deceleration.Linear acceleration and deceleration has and calculates simple, and response is fast, but will produce larger vibration and
Impact, affects fortune control track and lathe life-span, is used for less demanding occasion.S curve acceleration and deceleration can guarantee that acceleration and deceleration process
Flatness, and take full advantage of servo-drive system maximum permission speed and maximum permissible acceleration, efficiency highest, but S curve will
Speed transition process is divided into 7 stages, calculates complicated.After speed planning, need to two axles or the system of three-shaft linkage
Interpolation is carried out, the velocity amplitude of every controlling cycle is assigned to above each axle and is gone, during this, can there is interpolation track mistake
Difference.In the case where system real time can ensure that, the controlling cycle of system is less, and the track that interpolation is obtained is more accurate;And such as
In the case of same trajectory error, the controlling cycle of system is less, and the maximal rate of system is bigger, can improve effect for fruit
Rate.Therefore, in sum, in order to realize the motion control of high-speed, high precision, controlling cycle is as little as possible.Current is open
Acceleration and deceleration curves are calculated more than motion control card using software, software takes the mode of serial arithmetic, and arithmetic speed is slower, in order to
An interpolation operation is completed in each cycle, needs the DSP or MCU of high speed to perform, it is relatively costly, and be difficult further to reduce
Controlling cycle.
With integrated circuit technique development and the appearance of field programmable gate array (FPGA), IC design is sent out
Major transformation.The appearance of hardware description language (HDL) so that the integrated circuit modules of a specific function can be with one section of HDL
In the form of, and it is packaged into the IP kernel with certain function (Intellectual Property core).These IP kernels
Can be general in all kinds of models FPGA that numerous manufacturers provide, and opening for special IC (ASIC) can be transplanted to
Send out.Thus these IP kernels with specific function just become independent patented products.
The content of the invention
Present invention is primarily targeted at overcoming the shortcoming and deficiency of prior art, there is provided a kind of flexibility motion control IP
Core.
Another object of the present invention is to, there is provided a kind of implementation method of flexibility motion control IP kernel.
In order to reach above-mentioned first purpose, the present invention is employed the following technical solutions:
A kind of flexibility motion control IP kernel, the IP kernel include acceleration and deceleration module, interpolation module, pulse counter module,
Pulse generation module, bus control unit and RAM Interface module, the bus control unit respectively with;Acceleration and deceleration module, interpolation mould
Block, pulse counter module connection, the acceleration and deceleration module is connected with interpolation module and RAM Interface module;The interpolation module is again
It is connected with RAM Interface module and pulse counter module;The pulse counter module is also connected with pulse generation module;
The acceleration and deceleration module, the acceleration and deceleration mode for being sent according to CPU selects initial velocity, target velocity, maximum
Speed, peak acceleration and target location parameter, calculate suitable acceleration and deceleration curves, and with the shape of discrete speed values
Formula is stored in RAM;
The interpolation module, for the interpolation parameters sent according to CPU, reading speed numerical value carries out interpolation from RAM,
The corresponding speed values of each axle are exported after interpolation;
The pulse generation module, for each axle speed numerical value sent according to interpolation module, produces respective frequencies
Pulse;
The pulse counter module, for being fed back according to servomotor containing noisy each axle feedback pulse, enters
After row disappears and trembles, input counting module is counted, in the umber of pulse of every controlling cycle output once each spindle motor actual motion;
RAM Interface module, send into RAM controller behind suitable address for the discrete speed values of input to be produced, and is write
Enter RAM, and correct reading address produced according to order, discrete speed values are read from RAM,
The bus control unit, is communicated by bus for controlling this IP with other IP and CPU.
Preferably, the acceleration and deceleration module includes acceleration and deceleration mode selecting module, linear acceleration and deceleration module and sigmoid curve
Acceleration and deceleration module;
The acceleration and deceleration mode selecting module, the acceleration and deceleration mode selection signal for being input into according to higher level's module selects right
The acceleration and deceleration mode answered, acceleration and deceleration parameter corresponding module is input into;
The linear acceleration and deceleration module, for generating the discretization data of linear acceleration and deceleration curve;
The sigmoid curve acceleration and deceleration module, for generating the discretization data of S curve acceleration and deceleration curves.
Preferably, the linear acceleration and deceleration module include acceleration planning module, the first divider, the first multiplier, the
One integration module, the acceleration planning module is connected with first integral module, the first divider, the first multiplier, and described
One divider is also connected with the first multiplier;Maximal rate V_max and peak acceleration A_max of CPU inputs is sent into first and is removed
Musical instruments used in a Buddhist or Taoist mass, calculates t1=V_max/A_max, then result and maximal rate V_max are sent into the first multiplier, calculate S_min=V_
max2/ A_max, S_min is compared with displacement of targets S, if S_min>S, then move to right 1 by V_max, and S_min moves to right 2, t1
1 is moved to right, is compared with S again, until S_min<Till S;For S_min<S and initial velocity is zero, then t1、V_max、A_
Max sends into first integral module, calculates and export per the clock cycle velocity amplitude;If initial velocity (target velocity) is not
Zero, then (decelerating phase accelerates to recalculate boost phase acceleration A _ max'=(V_max-V_start)/V_max × A_max
Degree A_max "=(V_max-V_end)/V_max × A_max), t1, V_max, A_max', A_max " send into first integral mould
Block, calculates and exports per the clock cycle velocity amplitude.
Preferably, the sigmoid curve acceleration and deceleration module include acceleration planning module, data preprocessing module, second
Divider, the second multiplier, Data Post module, second integral module, the data preprocessing module, the second divider,
Data Post module and the second multiplier are linked in sequence;The acceleration planning module respectively with data prediction mould
The connection of block, the second multiplier and second integral module;CPU input maximal rate V_max, peak acceleration A_max and add
Acceleration Jerk sends into data preprocessing module, and them according to A_max/Jerk, two groups of V_max/A_max is sequentially sent to second
Divider, Data Post module is responsible for recognition result and stores t1=A_max/Jerk, t2=V_max/A_max, then
t1、t2, V_max send into the second multiplier, calculate S_min=V_max (t1+t2), S_min is compared with displacement of targets S, such as
Fruit S_min>S, then by t1, V_max, A_max move to right 1, and send into the second multiplier and recalculate S_min and compare with S, directly
To S_min<Till S.For S_min<S and initial velocity (target velocity) is zero, then t1、t2, V_max, A_max, Jerk give
Enter second integral module, calculate and export per the clock cycle velocity amplitude;If initial velocity (target velocity) is not zero,
Recalculate boost phase acceleration Jerk'=(V_max-V_start)/V_max × Jerk (decelerating phase accelerations
Jerk "=(V_max-V_end)/V_max × Jerk), t1、t2, V_max, Jerk', Jerk " send into second integral module, meter
Calculate and export per the clock cycle velocity amplitude.
Preferably, the interpolation module includes interpolation top-level module, linear interpolation module and circular interpolation module;
The interpolation coordinate that the linear interpolation module is used in interpolation parameters is assigned to velocity amplitude above each axle
Go, linear interpolation module include linear interpolation control module, the 3rd multiplier, the 3rd divider, straight line Error processing module with
And straight line output buffer module;Linear interpolation control module first is by (xe-xs)·vi、(ye-ys)·vi、(ze-zs)·viThree groups
Parameter is input into the 3rd multiplier, and is accumulated successively, then by the remainder phase adduction of product and the upper divider of a cycle the 3rd successively
The 3rd divider is sent into, business is sent into into straight line output buffer module, remainder is cached, and then straight line output buffer module is each
Axle speed is sent in pulse generation module;Straight line Error processing module receives the values for actual speed from pulse counter module, and
It is contrasted with the theoretical value of data buffering module, difference is recorded and is input in INTERPOLATION CONTROL OF PULSE module, in next Periodic Compensation;
The circular interpolation module, for carrying out at the uniform velocity two axle circular interpolations, with a large amount of isometric micro lines circular arc is approached,
And velocity amplitude is assigned to above two axles and is gone;The circular interpolation module includes circular interpolation control module, the 4th division
ROM on device, the 4th multiplier, circular arc output buffer module, circle arc error processing module and piece;Circular interpolation control module will
One suitable circular arc etc. point points and corresponding decile angle, θ are found by tabling look-up according to the arc radius R that CPU sends, will
Arc angle α and decile angle, θ send into point number N=α/θ such as the 4th divider, calculating, and per seat is then obtained by tabling look-up
Mark xi,yi, then by a subtraction and multiplication vix=(xi-xi-1) R, viy=(yi-yi-1) R, if there is xi<xi-1、
yi<yi-1, then direction signal DIR is set to into 1, and then circular arc exports buffer module vix,DIRx、viy,DIRySend into pulse to generate
In module;Circle arc error processing module receives the values for actual speed from pulse counter module, and by it and data buffering module
Theoretical value contrast, by difference be input into circular interpolation control module in, in next Periodic Compensation.
Preferably, the pulse generation module includes Error processing module, the 5th divider, allocator module;Interpolation mould
The speed values that block is sended over after the remainder that Error processing module is produced with the upper divider of a cycle the 5th is added, as
Divisor sends into the 5th divider, and the dividend of the 5th divider is system operating frequency, and the business of the 5th divider is frequency dividing ratio,
System clock is divided with this frequency dividing ratio, output frequency is equal to the pulse of input speed value.
Preferably, the pulse counter module includes disappearing trembling module, counting clock synchronization module and counter module;Servo
Driver feed back containing noisy each axle feedback pulse, after carrying out disappearing and trembling, input counting module is counted, and reaches letter
Number arrive after, counting terminates, count results compared with displacement of targets, obtain total displacement error;Arriving signal need to be through synchronous
Module time delay, is allowed to synchronous with the arrival of actual motion;Counting module is in commencing signal, arriving signal, the instruction for controlling clock
Under, the pulse of transmission actual to pulse sending module is counted, and in every controlling cycle once each spindle motor actual motion is exported
Umber of pulse.
In order to reach above-mentioned second purpose, the present invention is employed the following technical solutions:
A kind of implementation method of flexibility motion control IP kernel, comprises the steps:
After S1, system electrification, MicroBlaze soft-core processors start configuration processor, by bus control unit motion control
Parameter processed is dealt into motion control IP kernel;
S2, acceleration and deceleration module select corresponding acceleration and deceleration mode according to acceleration and deceleration mode selection signal, acceleration and deceleration parameter
It is input into corresponding module;
The velocity amplitude each cycle that S3, acceleration and deceleration module are calculated is produced once, is then fed into RAM Interface module, and generation is write
Address, and Memory Controller Hub is sent into together with address, write RAM;
S4, interpolation module are sent to pulse generation module each axle speed, Error processing module present speed numerical value with
After the remainder that the upper divider of a cycle the 5th is produced is added, as divisor the 5th divider, the dividend of the 5th divider are sent into
For motion control IP kernel operating frequency 100MHz, the business of the 5th divider is frequency dividing ratio, 100MHz is carried out with this frequency dividing ratio
Frequency dividing is counted, output frequency is equal to the pulse of input speed value;
S5, send pulse while, servo-driver feed back containing noisy each axle feedback pulse, carry out disappearing trembling
Afterwards, input pulse counting module, is counted by counting module, and after arriving signal arrives, counting terminates, count results and mesh
Marker displacement is compared, and obtains total displacement error;Arriving signal need to be allowed to same with the arrival of actual motion through synchronization module time delay
Step;Counting module commencing signal, arriving signal, control clock instruction under, the pulse of transmission actual to pulse sending module
Counted, in every controlling cycle the umber of pulse of each spindle motor actual motion is exported once.
Preferably, in step S2, if selected acceleration and deceleration mode is linear acceleration and deceleration, acceleration and deceleration parameter input straight line plus-minus
Maximal rate V_max and peak acceleration A_max are sent into the first divider by fast module, linear acceleration and deceleration module first, are calculated
t1=V_max/A_max, then result and maximal rate V_max are sent into the first multiplier, calculate S_min=V_max2/ A_max,
S_min is compared with displacement of targets S, if S_min>S, then move to right 1 by V_max, and S_min moves to right 2, t11 is moved to right, weight
Newly compare with S, until S_min<Till S;For S_min<S and initial velocity is zero, then t1, V_max, A_max send into the
One integration module, calculates and exports per the clock cycle velocity amplitude;If initial velocity (target velocity) is not zero, again
Calculating boost phase acceleration A _ max'=(V_max-V_start)/V_max × A_max (decelerating phase acceleration A _ max "=
(V_max-V_end)/V_max × A_max), t1, V_max, A_max', A_max " send into first integral module, calculate and
A velocity amplitude is exported per the clock cycle;
If selected acceleration and deceleration mode is sigmoid curve acceleration and deceleration, the maximum that sigmoid curve acceleration and deceleration module is first input into CPU
Speed V_max, peak acceleration A_max and acceleration Jerk send into data preprocessing module, them according to A_max/
Two groups of Jerk, V_max/A_max is sequentially sent to the second divider, and Data Post module is responsible for recognition result and stores t1=A_
Max/Jerk, t2=V_max/A_max, then t1、t2, V_max send into the second multiplier, calculate S_min=V_max (t1
+t2), S_min is compared with displacement of targets S, if S_min>S, then by t1, V_max, A_max move to right 1, and send into second and take advantage of
Musical instruments used in a Buddhist or Taoist mass recalculates S_min and compares with S, until S_min<Till S.For S_min<S and initial velocity (target velocity) is
Zero, then t1、t2, V_max, A_max, Jerk send into second integral module, calculate and per the clock cycle export a speed
Value;If initial velocity (target velocity) is not zero, boost phase acceleration Jerk'=(V_max-V_ are recalculated
Start)/V_max × Jerk (decelerating phase acceleration Jerk "=(V_max-V_end)/V_max × Jerk), t1、t2、
V_max, Jerk', Jerk " sends into second integral module, calculates and export per the clock cycle velocity amplitude.
Preferably, in step S4, if selected interpolation mode is linear interpolation, linear interpolation module INTERPOLATION CONTROL OF PULSE module first
By (xe-xs)·vi、(ye-ys)·vi、(ze-zs)·viThree groups of parameters are input into the 3rd multiplier, and are accumulated successively, then will
Product is sequentially sent to the 3rd divider with the remainder phase adduction of the upper divider of a cycle the 3rd, and business is sent into into output buffer module, remaining
Number is cached, and then exports buffer module in each axle speed feeding pulse generation module.Error processing module receive from
The values for actual speed of pulse counter module, and it is contrasted with the theoretical value of data buffering module, difference is recorded and is input into slotting
In mending control module, in next Periodic Compensation;Each axle speed is respectively:
Vx=vi·(xe-xs)/S, Vy=vi·(ye-ys)/S, Vz=vi·(ze-zs)/S;
If selected interpolation mode is circular interpolation, circular interpolation module obtains X, the Y-axis pulse ratio of each cycle by tabling look-up
Example and its direction, then each cycle velocity amplitude can be obtained by a multiplication and a subtraction;INTERPOLATION CONTROL OF PULSE module first is by root
The arc radius R sent according to CPU finds suitable circular arc etc. point points and corresponding decile angle, θ by tabling look-up, by circle
Arc angle α and decile angle, θ send into point number N=α/θ such as the 4th divider, calculating, are then obtained per point coordinates by tabling look-up
xi,yi, then by a subtraction and multiplication vix=(xi-xi-1) R, viy=(yi-yi-1) R, if there is xi<xi-1、yi<
yi-1, then direction signal DIR is set to into 1, and then buffer module is exported vix,DIRx、viy,DIRyIn sending into pulse generation module;
Error processing module receives the values for actual speed from pulse counter module, and by the theoretical value pair of it and data buffering module
Than difference is input in INTERPOLATION CONTROL OF PULSE module, in next Periodic Compensation.
The present invention compared with prior art, has the advantage that and beneficial effect:
1st, the present invention proposes a kind of IP kernel of flexibility motion control, is provided using IP kernel mode, is retouched with Verilog hardware
Predicate says that soft kernel form is realized, can realize in FPGA, together with other peripheral IP kernels, builds based on the motion control of SoPC
System, with flexibility and reconfigurable advantage.
2nd, the present invention calculates acceleration and deceleration curves and interpolation curve using IC regime, can carry out asymmetric straight line and add
Deceleration, asymmetric sigmoid curve acceleration and deceleration, two axles or three axle linear interpolations with straight line or sigmoid curve acceleration and deceleration, at the uniform velocity two
Axle circular interpolation.With fast operation, the little advantage of controlling cycle.Its controlling cycle be T=0.125ms, peak acceleration
For A=125P/T2(pulse each cycle square), maximal rate is 1250P/T (pulse each cycle, i.e. 1MHz).
3rd, there is Error processing module in interpolation module of the invention, can be by the result of the division arithmetic of a upper controlling cycle
Count in next cycle, reduce the cumulative errors of interpolation operation.
4th, the present invention has Error processing module in pulse generation module, and the frequency error that can reduce generation pulse is caused
Accumulative effect.
Description of the drawings
Fig. 1 is general frame figure of the present invention;
Fig. 2 is kinematic parameter definition;
Fig. 3 is the Organization Chart of acceleration and deceleration module;
Fig. 4 is the workflow diagram of linear acceleration and deceleration module;
Fig. 5 is the workflow diagram of sigmoid curve acceleration and deceleration module;
Fig. 6 is the Organization Chart of interpolation module;
Fig. 7 is the Organization Chart of pulse generation module;
Fig. 8 is the Organization Chart of pulse counter module;
Fig. 9 is the Organization Chart of RAM Interface module;
Figure 10 is using the SoC specific embodiment Organization Charts of this IP kernel.
Specific embodiment
With reference to embodiment and accompanying drawing, the present invention is described in further detail, but embodiments of the present invention are not limited
In this.
Embodiment
As shown in figure 1, a kind of flexibility motion control IP kernel of the present embodiment, including acceleration and deceleration module, interpolation module, arteries and veins
Rush counting module, pulse generation module, bus control unit and RAM Interface module, the bus control unit respectively with;Acceleration and deceleration mould
Block, interpolation module, pulse counter module connection, the acceleration and deceleration module is connected with interpolation module and RAM Interface module;It is described to insert
Complementary modulus block is connected with RAM Interface module and pulse counter module again;The pulse counter module is also connected with pulse generation module;
The acceleration and deceleration module, the acceleration and deceleration mode for being sent according to CPU selects initial velocity, target velocity, maximum
Speed, peak acceleration and target location parameter, calculate suitable acceleration and deceleration curves, and with the shape of discrete speed values
Formula is stored in RAM;
The interpolation module, for the interpolation parameters sent according to CPU, reading speed numerical value carries out interpolation from RAM,
The corresponding speed values of each axle are exported after interpolation;
The pulse generation module, for each axle speed numerical value sent according to interpolation module, produces respective frequencies
Pulse;
The pulse counter module, for being fed back according to servomotor containing noisy each axle feedback pulse, enters
After row disappears and trembles, input counting module is counted, in the umber of pulse of every controlling cycle output once each spindle motor actual motion;
RAM Interface module, send into RAM controller behind suitable address for the discrete speed values of input to be produced, and is write
Enter RAM, and correct reading address produced according to order, discrete speed values are read from RAM,
The bus control unit, is communicated by bus for controlling this IP with other IP and CPU.
As shown in Fig. 2 the acceleration and deceleration parameter is included:Acceleration and deceleration mode selection signal, initial velocity, target velocity, most
Big speed, peak acceleration, displacement of targets.
As shown in figure 3, the acceleration and deceleration module includes acceleration and deceleration mode selecting module, linear acceleration and deceleration module and S-shaped
Curve acceleration and deceleration module;
The acceleration and deceleration mode selecting module, the acceleration and deceleration mode selection signal for being input into according to higher level's module selects right
The acceleration and deceleration mode answered, acceleration and deceleration parameter corresponding module is input into;
The linear acceleration and deceleration module, for generating the discretization data of linear acceleration and deceleration curve;As shown in figure 4, described
Linear acceleration and deceleration module includes acceleration planning module, the first divider, the first multiplier, first integral module, the acceleration
Degree planning module be connected with first integral module, the first divider, the first multiplier, first divider also with the first multiplication
Device connects;The course of work is as follows:CPU is input into maximal rate V_max first and peak acceleration A_max send into the first division
Device, calculates t1=V_max/A_max, then result and maximal rate V_max are sent into the first multiplier, calculate S_min=V_
max2/ A_max, S_min is compared with displacement of targets S, if S_min>S, then move to right 1 by V_max, and S_min moves to right 2, t1
1 is moved to right, is compared with S again, until S_min<Till S.For S_min<S and initial velocity (target velocity) is zero, then
t1, V_max, A_max send into first integral module, calculate and per the clock cycle export a velocity amplitude;If initial velocity (mesh
Mark speed) be not zero, then recalculate boost phase acceleration A _ max'=(V_max-V_start)/V_max × A_max and (subtract
Fast stage acceleration A _ max "=(V_max-V_end)/V_max × A_max), t1, V_max, A_max', A_max " send into
Integration module, calculates and exports per the clock cycle velocity amplitude.
The sigmoid curve acceleration and deceleration module, for generating the discretization data of S curve acceleration and deceleration curves;As shown in figure 5,
The sigmoid curve acceleration and deceleration module includes acceleration planning module, data preprocessing module, the second divider, the second multiplication
Device, Data Post module, second integral module, the data preprocessing module, the second divider, Data Post module with
And second multiplier be linked in sequence;The course of work is as follows:CPU is input into maximal rate V_max, peak acceleration A_ first
Max and acceleration Jerk sends into data preprocessing module, and them according to A_max/Jerk, two groups of V_max/A_max is successively
The second divider is sent into, Data Post module is responsible for recognition result and stores t1=A_max/Jerk, t2=V_max/A_max,
Then t1、t2, V_max send into the second multiplier, calculate S_min=V_max (t1+t2), by S_min and displacement of targets S ratios
Compared with if S_min>S, then by t1, V_max, A_max move to right 1, and send into the second multiplier recalculate S_min and with S ratios
Compared with until S_min<Till S.For S_min<S and initial velocity (target velocity) is zero, then t1、t2、V_max、A_max、
Jerk sends into second integral module, calculates and export per the clock cycle velocity amplitude;If initial velocity (target velocity) is no
It is zero, then recalculates that boost phase acceleration Jerk'=(V_max-V_start)/(decelerating phase adds V_max × Jerk
Acceleration Jerk "=(V_max-V_end)/V_max × Jerk), t1、t2, V_max, Jerk', Jerk " send into second integral
Module, calculates and exports per the clock cycle velocity amplitude.
The integration module, its key point is the judgement of deceleration point, for the calculating of boost phase, is sentenced using the time
Disconnected method, linear acceleration and deceleration is according to aforementioned t1, sigmoid curve acceleration and deceleration are according to t1、t2Judge at the uniform velocity point, after reaching at the uniform velocity, record accelerates
The displacement of section, due to accelerating sections and braking section time above symmetrically, so braking section displacement can be obtained by below equation:S_down
=S_up (V_max-V_end)/(V_max-V_start), then be that residual displacement is compared with the difference of total displacement and current displacement,
When residual displacement is exactly equal to braking section displacement S_down, deceleration point arrives.
As shown in fig. 6, the interpolation module includes interpolation top-level module, linear interpolation module and circular interpolation module;
The interpolation top-level module, its function is the interpolation parameters according to CPU inputs, selects corresponding interpolation mode, sends read command
To RAM Interface module;The interpolation coordinate that the linear interpolation module is used in interpolation parameters is assigned to each velocity amplitude
Go above axle, linear interpolation module includes linear interpolation control module, the 3rd multiplier, the 3rd divider, straight line Error processing
Module and straight line output buffer module;Linear interpolation control module first is by (xe-xs)·vi、(ye-ys)·vi、(ze-zs)·
viThree groups of parameters are input into the 3rd multiplier, and are accumulated successively, are then added product with the remainder of the upper divider of a cycle the 3rd
And the 3rd divider is sequentially sent to, and business is sent into into straight line output buffer module, remainder is cached, and then straight line output buffering mould
Block is sent into each axle speed in pulse generation module;Straight line Error processing module receives the actual speed from pulse counter module
Value, and it is contrasted with the theoretical value of data buffering module, difference is recorded and is input in INTERPOLATION CONTROL OF PULSE module, in next cycle
Compensation;Each axle speed is respectively:
Vx=vi·(xe-xs)/S, Vy=vi·(ye-ys)/S, Vz=vi·(ze-zs)/S;
The circular interpolation module, for carrying out at the uniform velocity two axle circular interpolations, with a large amount of isometric micro lines circular arc is approached,
And velocity amplitude is assigned to above two axles and is gone;The circular interpolation module includes circular interpolation control module, the 4th division
ROM on device, the 4th multiplier, circular arc output buffer module, circle arc error processing module and piece;Circular interpolation control module will
One suitable circular arc etc. point points and corresponding decile angle, θ are found by tabling look-up according to the arc radius R that CPU sends, will
Arc angle α and decile angle, θ send into point number N=α/θ such as the 4th divider, calculating, and per seat is then obtained by tabling look-up
Mark xi,yi, then by a subtraction and multiplication vix=(xi-xi-1) R, viy=(yi-yi-1) R, if there is xi<xi-1、
yi<yi-1, then direction signal DIR is set to into 1, and then circular arc exports buffer module vix,DIRx、viy,DIRySend into pulse to generate
In module;Circle arc error processing module receives the values for actual speed from pulse counter module, and by it and data buffering module
Theoretical value contrast, by difference be input into circular interpolation control module in, in next Periodic Compensation.
As shown in fig. 7, the pulse generation module includes Error processing module, the 5th divider, allocator module;Interpolation
The speed values that module is sended over are made after the remainder that Error processing module is produced with the upper divider of a cycle the 5th is added
The 5th divider is sent into for divisor, the dividend of the 5th divider is system operating frequency, and the business of divider is frequency dividing ratio, is used
This frequency dividing ratio is divided to system clock, and output frequency is equal to the pulse of input speed value.
As shown in figure 8, the pulse counter module includes disappearing trembling module, counting clock synchronization module and counter module;
Servo-driver feed back containing noisy each axle feedback pulse, after carrying out disappearing and trembling, input counting module is counted, and is arrived
After arriving up to signal, counting terminates, and count results compared with displacement of targets, obtains total displacement error;Arriving signal need through
Synchronization module time delay, is allowed to synchronous with the arrival of actual motion;Counting module is in commencing signal, arriving signal, control clock
Under instruction, the pulse of transmission actual to pulse sending module is counted, and in every controlling cycle once each spindle motor reality is exported
The umber of pulse of operation.
As shown in figure 9, the RAM Interface module is used to produce send discrete speed values are input into into behind suitable address
RAM controller, writes RAM;Read function:Correct reading address is produced, discrete speed values are read from RAM, send into interpolation
Module.RAM Interface module is made up of following 2 Subordinate modules:Write address generation module, reading address generation module.
Figure 10 gives the SoC specific embodiment figures of employing this IP, and the embodiment is this IP kernel in Xilinx
Realized in the way of SoPC in Spartan 6FPGA, 32 soft cores of MicroBlaze that its RISC CPU is provided for Xilinx
Processor, stone Memory Controller Hub MCB (the Memory Controller that its RAM controller is provided for Spartan6FPGA
Block), its RAM be DDR2SDRAM, the PLB (Processor Local Bus) that its on-chip bus is provided for Xilinx, at that time
The DCM (Digital Clock Manager) that clock module is provided for Xilinx;In the embodiment, motion control IP kernel (Motion
IPcore) running frequency is 100MHz, and MicroBlaze soft-core processors operating frequency is 75MHz, and institute is provided for them by DCM
Need clock, outside 50MHz clocks through DCM produce 100MHz, 75MHz, 600MHz respectively as motion control IP kernel,
The work clock of MicroBlaze soft-core processors, DDR2SDRAM.
Should be as follows based on the SoPC flexibility kinetic control system courses of work:
After system electrification, MicroBlaze soft-core processors start configuration processor, by bus control unit motion control
Parameter is dealt into motion control IP kernel.The acceleration and deceleration parameter is included:Acceleration and deceleration mode selection signal, initial velocity, target velocity,
Maximal rate, peak acceleration, displacement of targets;
Then, acceleration and deceleration module selects corresponding acceleration and deceleration mode according to acceleration and deceleration mode selection signal, and acceleration and deceleration are joined
The corresponding module of number input;
If selected acceleration and deceleration mode is linear acceleration and deceleration, acceleration and deceleration parameter input linear acceleration and deceleration module, straight line plus-minus
Maximal rate V_max and peak acceleration A_max are sent into the first divider by fast module first, calculate t1=V_max/A_max,
Again result and maximal rate V_max are sent into the first multiplier, calculate S_min=V_max2/ A_max, by S_min and target position
Move S to compare, if S_min>S, then move to right 1 by V_max, and S_min moves to right 2, t11 is moved to right, is compared with S again, until
S_min<Till S;For S_min<S and initial velocity is zero, then t1, V_max, A_max send into first integral module, calculate
And a velocity amplitude is exported per the clock cycle;If initial velocity (target velocity) is not zero, recalculates boost phase and add
Speed A_max'=(V_max-V_start)/V_max × A_max (decelerating phase acceleration A _ max "=(V_max-V_end)/
V_max × A_max), t1, V_max, A_max', A_max " send into first integral module, calculate and per the clock cycle output
One velocity amplitude;
If selected acceleration and deceleration mode is sigmoid curve acceleration and deceleration, the maximum that sigmoid curve acceleration and deceleration module is first input into CPU
Speed V_max, peak acceleration A_max and acceleration Jerk send into data preprocessing module, them according to A_max/
Two groups of Jerk, V_max/A_max is sequentially sent to the second divider, and Data Post module is responsible for recognition result and stores t1=A_
Max/Jerk, t2=V_max/A_max, then t1、t2, V_max send into the second multiplier, calculate S_min=V_max (t1
+t2), S_min is compared with displacement of targets S, if S_min>S, then by t1, V_max, A_max move to right 1, and send into second and take advantage of
Musical instruments used in a Buddhist or Taoist mass recalculates S_min and compares with S, until S_min<Till S.For S_min<S and initial velocity (target velocity) is
Zero, then t1、t2, V_max, A_max, Jerk send into second integral module, calculate and per the clock cycle export a speed
Value;If initial velocity (target velocity) is not zero, boost phase acceleration Jerk'=(V_max-V_ are recalculated
Start)/V_max × Jerk (decelerating phase acceleration Jerk "=(V_max-V_end)/V_max × Jerk), t1、t2、
V_max, Jerk', Jerk " sends into second integral module, calculates and export per the clock cycle velocity amplitude.
At the same time, integration module carries out the judgement of deceleration point, for the calculating of boost phase, is judged using the time
Method, linear acceleration and deceleration is according to aforementioned t1, sigmoid curve acceleration and deceleration are according to t1、t2Judge at the uniform velocity point, after reaching at the uniform velocity, record accelerating sections
Displacement, due to accelerating sections and braking section time it is upper symmetrical, so braking section displacement can be obtained by below equation:S_down=
S_up (V_max-V_end)/(V_max-V_start), then be that residual displacement is compared with the difference of total displacement and current displacement, when
When residual displacement is exactly equal to braking section displacement S_down, deceleration point arrives.
The velocity amplitude each cycle that acceleration and deceleration module is calculated is produced once, is then fed into RAM Interface module, and generation writes ground
Location, and Memory Controller Hub is sent into together with address, write RAM.
For two axles or three-shaft linkage are moved, in addition it is also necessary to carry out interpolation operation.Interpolation top-level module is input into according to CPU
Interpolation parameters, selects corresponding interpolation mode, the interpolation parameters that interpolation module sends according to CPU to send read command and connect to RAM
Mouth mold block, produces correct reading address, and discrete speed values are read from RAM, then discrete velocity value is sent back to interpolation module
Interpolation is carried out, the corresponding speed values of each axle are exported Jing after interpolation.
If selected interpolation mode is linear interpolation, linear interpolation module INTERPOLATION CONTROL OF PULSE module first is by (xe-xs)·vi、
(ye-ys)·vi、(ze-zs)·viThree groups of parameters are input into the 3rd multipliers, and are accumulated successively, then will product and the upper cycle the
The remainder phase adduction of three dividers is sequentially sent to the 3rd divider, and business is sent into into output buffer module, and remainder is cached, and then
Output buffer module is sent into each axle speed in pulse generation module.Error processing module receives the reality from pulse counter module
Border velocity amplitude, and it is contrasted with the theoretical value of data buffering module, difference is recorded and is input in INTERPOLATION CONTROL OF PULSE module, under
One Periodic Compensation.Each axle speed is respectively:
Vx=vi·(xe-xs)/S, Vy=vi·(ye-ys)/S, Vz=vi·(ze-zs)/S;
If selected interpolation mode is circular interpolation, circular interpolation module obtains X, the Y-axis pulse ratio of each cycle by tabling look-up
Example and its direction, then each cycle velocity amplitude can be obtained by a multiplication and a subtraction.Its course of work is:Interpolation first
Control module by the arc radius R sent according to CPU found by tabling look-up suitable circular arc etc. point points with it is corresponding etc.
Subangle θ, sends arc angle α and decile angle, θ into point number N=α/θ such as the 4th divider, calculating, then by tabling look-up
Obtain per point coordinates xi,yi, then by a subtraction and multiplication vix=(xi-xi-1) R, viy=(yi-yi-1) R, if deposited
In xi<xi-1、yi<yi-1, then direction signal DIR is set to into 1 (representing reverse), and then buffer module is exported vix,DIRx、viy,
DIRyIn sending into pulse generation module.Error processing module receive from pulse counter module values for actual speed, and by it with
The theoretical value contrast of data buffering module, difference is input in INTERPOLATION CONTROL OF PULSE module, in next Periodic Compensation.
Then, interpolation module is sent to pulse generation module each axle speed, and Error processing module is present speed numerical value
After the remainder produced with the upper divider of a cycle the 5th is added, used as divisor the 5th divider of feeding, the 5th divider is removed
Number is motion control IP kernel operating frequency 100MHz, and the business of divider is frequency dividing ratio, 100MHz is counted with this frequency dividing ratio
Number frequency dividing, output frequency is equal to the pulse of input speed value.
While sending pulse, servo-driver feed back containing noisy each axle feedback pulse, after carrying out disappearing and trembling,
Input pulse counting module, is counted by counting module, and after arriving signal arrives, counting terminates, count results and target
Displacement is compared, and obtains total displacement error (representing with umber of pulse).Arriving signal need to be allowed to and actual fortune through synchronization module time delay
Dynamic arrival synchronization;Counting module commencing signal, arriving signal, control clock instruction under, to pulse sending module reality
The pulse of transmission is counted, and in every controlling cycle the umber of pulse of each spindle motor actual motion is exported once.
Above-described embodiment is the present invention preferably embodiment, but embodiments of the present invention not by above-described embodiment
Limit, other any Spirit Essences without departing from the present invention and the change, modification, replacement made under principle, combine, simplification,
Equivalent substitute mode is should be, is included within protection scope of the present invention.
Claims (9)
1. a kind of flexibility motion control IP kernel, it is characterised in that the IP kernel includes acceleration and deceleration module, interpolation module, pulse
Counting module, pulse generation module, bus control unit and RAM Interface module, the bus control unit respectively with acceleration and deceleration module,
Interpolation module, pulse counter module connection;The acceleration and deceleration module is connected with interpolation module and RAM Interface module;The interpolation
Module is connected with RAM Interface module and pulse counter module again;The pulse counter module is also connected with pulse generation module;Its
In, IP kernel full name is Intellectual Property core, i.e. IP core;
The acceleration and deceleration module, the acceleration and deceleration mode for being sent according to CPU select initial velocity, target velocity, maximal rate,
Peak acceleration and target location parameter, calculate suitable acceleration and deceleration curves, and are protected in the form of discrete speed values
In there is RAM;
The interpolation module, for the interpolation parameters sent according to CPU, reading speed numerical value carries out interpolation, interpolation from RAM
After export the corresponding speed values of each axle;
The pulse generation module, for each axle speed numerical value sent according to interpolation module, produces the pulse of respective frequencies;
The pulse counter module, for being fed back according to servomotor containing noisy each axle feedback pulse, is disappeared
After trembling, input counting module is counted, and in every controlling cycle the umber of pulse of each spindle motor actual motion is exported once;
RAM Interface module, send into RAM controller behind suitable address for the discrete speed values of input to be produced, and is write
RAM, and correct reading address is produced according to order, discrete speed values are read from RAM;
The bus control unit, is communicated by bus for controlling this IP with other IP and CPU;
The pulse generation module includes Error processing module, the 5th divider, allocator module;What interpolation module was sended over
Speed values send into the 5th after the remainder that Error processing module is produced with the upper divider of a cycle the 5th is added as divisor
Divider, the dividend of the 5th divider is system operating frequency, and the business of the 5th divider is frequency dividing ratio, with this frequency dividing ratio pair
System clock is divided, and output frequency is equal to the pulse of input speed value.
2. flexibility motion control IP kernel according to claim 1, it is characterised in that the acceleration and deceleration module includes plus-minus
Fast mode selecting module, linear acceleration and deceleration module and sigmoid curve acceleration and deceleration module;
The acceleration and deceleration mode selecting module, the acceleration and deceleration mode selection signal for being input into according to higher level's module selects corresponding
Acceleration and deceleration mode, acceleration and deceleration parameter corresponding module is input into;
The linear acceleration and deceleration module, for generating the discretization data of linear acceleration and deceleration curve;
The sigmoid curve acceleration and deceleration module, for generating the discretization data of S curve acceleration and deceleration curves.
3. flexibility motion control IP kernel according to claim 2, it is characterised in that the linear acceleration and deceleration module includes
Acceleration planning module, the first divider, the first multiplier, first integral module, the acceleration planning module and the first product
Sub-module, the first divider, the connection of the first multiplier, first divider is also connected with the first multiplier;CPU is input into most
Big speed V_max and peak acceleration A_max send into the first divider, calculate t1=V_max/A_max, then result and maximum
Speed V_max sends into the first multiplier, calculates S_min=V_max2/ A_max, S_min is compared with displacement of targets S, if S_
min>S, then move to right 1 by V_max, and S_min moves to right 2, t11 is moved to right, is compared with S again, until S_min<Till S;It is right
In S_min<S and initial, target velocity is zero, then t1, V_max, A_max send into first integral module, calculate and per when
The clock cycle exports a velocity amplitude;If initial, target velocity is not zero, boost phase acceleration A _ max'=is recalculated
(V_max-V_start)/V_max × A_max, decelerating phase acceleration A _ max "=(V_max-V_end)/V_max × A_
Max, t1, V_max, A_max', A_max " send into first integral module, calculate and per the clock cycle export a speed
Value.
4. flexibility motion control IP kernel according to claim 2, it is characterised in that the sigmoid curve acceleration and deceleration module
Including acceleration planning module, data preprocessing module, the second divider, the second multiplier, Data Post module, second
Integration module, the data preprocessing module, the second divider, Data Post module and the second multiplier are linked in sequence;
The acceleration planning module is connected respectively with data preprocessing module, the second multiplier and second integral module;CPU is defeated
Maximal rate V_max that enters, peak acceleration A_max and acceleration Jerk send into data preprocessing module, they according to
Two groups of A_max/Jerk, V_max/A_max is sequentially sent to the second divider, and Data Post module is responsible for recognition result and is stored
t1=A_max/Jerk, t2=V_max/A_max, then t1、t2, V_max send into the second multiplier, calculate S_min=V_
max·(t1+t2), S_min is compared with displacement of targets S, if S_min>S, then by t1, V_max, A_max move to right 1, and give
Enter the second multiplier to recalculate S_min and compare with S, until S_min<Till S;For S_min<S and initial, target velocity
It is zero, then t1、t2, V_max, A_max, Jerk send into second integral module, calculate and per the clock cycle export a speed
Value;If initial, target velocity is not zero, boost phase acceleration Jerk'=(V_max-V_start)/V_ is recalculated
Max × Jerk, decelerating phase acceleration Jerk "=(V_max-V_end)/V_max × Jerk, t1、t2、V_max、
Jerk', Jerk " sends into second integral module, calculates and export per the clock cycle velocity amplitude.
5. flexibility motion control IP kernel according to claim 1, it is characterised in that the interpolation module includes interpolation top
Layer module, linear interpolation module and circular interpolation module;
The interpolation coordinate that the linear interpolation module is used in interpolation parameters is assigned to velocity amplitude above each axle and goes, directly
Line interpolation module includes linear interpolation control module, the 3rd multiplier, the 3rd divider, straight line Error processing module and straight line
Output buffer module;Linear interpolation control module first is by (xe-xs)·vi、(ye-ys)·vi、(ze-zs)·viThree groups of parameters are defeated
Enter the 3rd multiplier, and accumulated successively, then product and the remainder phase adduction of the divider of a upper cycle the 3rd are sequentially sent to into the
Three dividers, send business into straight line output buffer module, and remainder is cached, and then straight line exports buffer module each axle speed
In sending into pulse generation module;Straight line Error processing module receive from pulse counter module values for actual speed, and by it with
The theoretical value contrast of data buffering module, difference is recorded and is input in INTERPOLATION CONTROL OF PULSE module, in next Periodic Compensation;
The circular interpolation module, for carrying out at the uniform velocity two axle circular interpolations, with a large amount of isometric micro lines circular arc is approached, and
Velocity amplitude is assigned to above two axles and is gone;The circular interpolation module include circular interpolation control module, the 4th divider, the
ROM on four multipliers, circular arc output buffer module, circle arc error processing module and piece;Circular interpolation control module is by basis
The arc radius R that CPU sends finds suitable circular arc etc. point points and corresponding decile angle, θ by tabling look-up, by circular arc
Angle [alpha] and decile angle, θ send into point number N=α/θ such as the 4th divider, calculating, are then obtained per point coordinates x by tabling look-upi,
yi, then by a subtraction and multiplication vix=(xi-xi-1) R, viy=(yi-yi-1) R, if there is xi<xi-1、yi<
yi-1, then direction signal DIR is set to into 1, and then circular arc exports buffer module vix,DIRx、viy,DIRySend into pulse and generate mould
In block;Circle arc error processing module receives the values for actual speed from pulse counter module, and by it and data buffering module
Theoretical value is contrasted, and difference is input in circular interpolation control module, in next Periodic Compensation.
6. flexibility motion control IP kernel according to claim 1, it is characterised in that the pulse counter module includes disappearing
Tremble module, counting clock synchronization module and counter module;What servo-driver fed back feeds back arteries and veins containing noisy each axle
Punching, after carrying out disappearing and trembling, input counting module is counted, and after arriving signal arrives, counting terminates, count results and target position
Phase shift ratio, obtains total displacement error;Arriving signal need to be allowed to synchronous with the arrival of actual motion through synchronization module time delay;Meter
Digital-to-analogue block under commencing signal, arriving signal, the instruction of control clock, count by the pulse of transmission actual to pulse sending module
Number, in every controlling cycle the umber of pulse of each spindle motor actual motion is exported once.
7. the implementation method of the flexibility motion control IP kernel according to any one of claim 1-6, it is characterised in that bag
Include following step:
After S1, system electrification, MicroBlaze soft-core processors start configuration processor, and motion control is joined by bus control unit
Number is dealt into motion control IP kernel;
S2, acceleration and deceleration module select corresponding acceleration and deceleration mode according to acceleration and deceleration mode selection signal, the input of acceleration and deceleration parameter
Corresponding module;
The velocity amplitude each cycle that S3, acceleration and deceleration module are calculated is produced once, is then fed into RAM Interface module, generates write address,
And Memory Controller Hub is sent into together with address, write RAM;
S4, interpolation module are sent to pulse generation module each axle speed, and Error processing module is present speed numerical value and upper one
After the remainder that the divider of cycle the 5th is produced is added, the 5th divider is sent into as divisor, the dividend of the 5th divider is fortune
Dynamic control IP kernel operating frequency 100MHz, the business of the 5th divider is frequency dividing ratio, 100MHz is counted with this frequency dividing ratio
Frequency dividing, output frequency is equal to the pulse of input speed value;
S5, while send pulse, servo-driver feed back containing noisy each axle feedback pulse, after carrying out disappearing and trembling,
Input pulse counting module, is counted by counting module, and after arriving signal arrives, counting terminates, count results and target
Displacement is compared, and obtains total displacement error;Arriving signal need to be allowed to synchronous with the arrival of actual motion through synchronization module time delay;
Under commencing signal, arriving signal, the instruction of control clock, the pulse of transmission actual to pulse sending module is carried out counting module
Count, in every controlling cycle the umber of pulse of each spindle motor actual motion is exported once.
8. the implementation method of flexibility motion control IP kernel according to claim 7, it is characterised in that in step S2, if
Selected acceleration and deceleration mode is linear acceleration and deceleration, then acceleration and deceleration parameter input linear acceleration and deceleration module, and linear acceleration and deceleration module is first
Maximal rate V_max and peak acceleration A_max are sent into into the first divider, t is calculated1=V_max/A_max, then result and
Maximal rate V_max sends into the first multiplier, calculates S_min=V_max2/ A_max, S_min is compared with displacement of targets S, such as
Fruit S_min>S, then move to right 1 by V_max, and S_min moves to right 2, t11 is moved to right, is compared with S again, until S_min<S is
Only;For S_min<S and initial, target velocity is zero, then t1, V_max, A_max send into first integral module, calculate and
A velocity amplitude is exported per the clock cycle;If initial, target velocity is not zero, boost phase acceleration A _ max' is recalculated
=(V_max-V_start)/V_max × A_max, decelerating phase acceleration A _ max "=(V_max-V_end)/V_max × A_
Max, t1, V_max, A_max', A_max " send into first integral module, calculate and per the clock cycle export a speed
Value;
If selected acceleration and deceleration mode is sigmoid curve acceleration and deceleration, the maximal rate that sigmoid curve acceleration and deceleration module is first input into CPU
V_max, peak acceleration A_max and acceleration Jerk send into data preprocessing module, them according to A_max/Jerk, V_
Two groups of max/A_max is sequentially sent to the second divider, and Data Post module is responsible for recognition result and stores t1=A_max/
Jerk, t2=V_max/A_max, then t1、t2, V_max send into the second multiplier, calculate S_min=V_max (t1+t2),
S_min is compared with displacement of targets S, if S_min>S, then by t1, V_max, A_max move to right 1, and send into the second multiplier
Recalculate S_min and compare with S, until S_min<Till S;For S_min<S and initial, target velocity is zero, then t1、
t2, V_max, A_max, Jerk send into second integral module, calculate and per the clock cycle export a velocity amplitude;If initial,
Target velocity is not zero, then recalculate boost phase acceleration Jerk'=(V_max-V_start)/V_max × Jerk,
Decelerating phase acceleration Jerk "=(V_max-V_end)/V_max × Jerk, t1、t2, V_max, Jerk', Jerk " send into
Second integral module, calculates and exports per the clock cycle velocity amplitude.
9. the implementation method of flexibility motion control IP kernel according to claim 7, it is characterised in that in step S4, if
Selected interpolation mode is linear interpolation, and linear interpolation module INTERPOLATION CONTROL OF PULSE module first is by (xe-xs)·vi、(ye-ys)·vi、
(ze-zs)·viThree groups of parameters are input into the 3rd multiplier, and are accumulated successively, then by product and the upper divider of a cycle the 3rd
Remainder phase adduction is sequentially sent to the 3rd divider, and business is sent into into output buffer module, and remainder is cached, and then exports buffering mould
Block is sent into each axle speed in pulse generation module;Error processing module receives the values for actual speed from pulse counter module,
And contrast it with the theoretical value of data buffering module, difference is recorded and is input in INTERPOLATION CONTROL OF PULSE module, mend in next cycle
Repay;Each axle speed is respectively:
Vx=vi·(xe-xs)/S, Vy=vi·(ye-ys)/S, Vz=vi·(ze-zs)/S;
If selected interpolation mode is circular interpolation, circular interpolation module obtained by tabling look-up the X of each cycle, Y-axis impulse ratio and
Its direction, then each cycle velocity amplitude can be obtained by a multiplication and a subtraction;First INTERPOLATION CONTROL OF PULSE module will be according to CPU
The arc radius R of transmission finds suitable circular arc etc. point points and corresponding decile angle, θ by tabling look-up, by arc angle
α and decile angle, θ send into point number N=α/θ such as the 4th divider, calculating, are then obtained per point coordinates x by tabling look-upi,yi, so
Afterwards by a subtraction and multiplication vix=(xi-xi-1) R, viy=(yi-yi-1) R, if there is xi<xi-1、yi<yi-1, then will
Direction signal DIR is set to 1, and then exports buffer module vix,DIRx、viy,DIRyIn sending into pulse generation module;Error processing
Module receives the values for actual speed from pulse counter module, and it is contrasted with the theoretical value of data buffering module, by difference
In input INTERPOLATION CONTROL OF PULSE module, in next Periodic Compensation.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410348942.9A CN104135212B (en) | 2014-07-21 | 2014-07-21 | Flexibility motion control IP (Intellectual Property) core and implementation method thereof |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410348942.9A CN104135212B (en) | 2014-07-21 | 2014-07-21 | Flexibility motion control IP (Intellectual Property) core and implementation method thereof |
Publications (2)
Publication Number | Publication Date |
---|---|
CN104135212A CN104135212A (en) | 2014-11-05 |
CN104135212B true CN104135212B (en) | 2017-04-19 |
Family
ID=51807779
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201410348942.9A Expired - Fee Related CN104135212B (en) | 2014-07-21 | 2014-07-21 | Flexibility motion control IP (Intellectual Property) core and implementation method thereof |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN104135212B (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110471336A (en) * | 2019-08-02 | 2019-11-19 | 上海交通大学 | The servo control system realized based on Verilog |
Families Citing this family (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104597832B (en) * | 2014-12-31 | 2017-04-19 | 浙江中控研究院有限公司 | PLC program scheduler IP core based on AMBA bus |
CN104483906A (en) * | 2015-01-07 | 2015-04-01 | 浙江师范大学 | S-curve acceleration and deceleration control method and device of discrete sampling |
CN104647331B (en) * | 2015-03-23 | 2017-07-21 | 常州米泽智能装备科技有限公司 | A kind of principal and subordinate is servo-actuated teaching industrial robot system |
CN105137817B (en) * | 2015-07-24 | 2018-04-17 | 浙江杰芯科技有限公司 | Control the control program excutive method of the system-on-chip of program processor |
CN106484657A (en) * | 2016-11-18 | 2017-03-08 | 成都嘉纳海威科技有限责任公司 | A kind of reconfigurable signal processor ASIC framework and its reconstructing method |
CN106371406A (en) * | 2016-12-06 | 2017-02-01 | 上海电器科学研究院 | Intelligent control system for underground pipe network welding equipment |
CN107046382B (en) * | 2017-03-24 | 2019-06-11 | 吉林大学 | A kind of iteration speed control method based on trigonometric function |
CN109407621A (en) * | 2018-01-30 | 2019-03-01 | 武汉呵尔医疗科技发展有限公司 | S type acceleration and deceleration motion control method in a kind of sampling mechanical arm interpolation |
CN110160521A (en) * | 2018-02-28 | 2019-08-23 | 北京兴承科技有限公司 | The compound range three-axis attitude resolver of MEMS and calculation method |
JP7020198B2 (en) * | 2018-03-09 | 2022-02-16 | オムロン株式会社 | Controls and control systems |
CN108628259B (en) * | 2018-07-12 | 2021-03-12 | 贵阳合众精密机械制造有限公司 | Acceleration and deceleration movement control method for rigid tapping of drilling and tapping center |
CN108958169B (en) * | 2018-07-27 | 2021-04-06 | 山东大学 | PVT control method based on S curve |
CN110465422B (en) * | 2019-08-29 | 2020-06-19 | 内蒙古大学 | FPGA-based spraying machine motion control system and motion control method thereof |
CN111584160B (en) * | 2020-06-04 | 2021-10-08 | 广州铁路职业技术学院(广州铁路机械学校) | Stranding machine winding displacement system based on wireless communication |
CN112783098B (en) * | 2020-12-28 | 2022-03-25 | 杭州爱科科技股份有限公司 | Multi-axis linkage mechanical motion control method, device and system and readable storage medium |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN1752879A (en) * | 2005-10-31 | 2006-03-29 | 西安交通大学 | Flexible digital type motion control system |
CN201426109Y (en) * | 2009-05-31 | 2010-03-17 | 深圳市雷泰控制技术有限公司 | PCI bus bar type multi-shaft impulse type movement control card |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS59168513A (en) * | 1983-03-16 | 1984-09-22 | Fanuc Ltd | Acceleration and deceleration control system |
-
2014
- 2014-07-21 CN CN201410348942.9A patent/CN104135212B/en not_active Expired - Fee Related
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN1752879A (en) * | 2005-10-31 | 2006-03-29 | 西安交通大学 | Flexible digital type motion control system |
CN201426109Y (en) * | 2009-05-31 | 2010-03-17 | 深圳市雷泰控制技术有限公司 | PCI bus bar type multi-shaft impulse type movement control card |
Non-Patent Citations (1)
Title |
---|
"基于FPGA的运动控制卡的研究与开发";杨雪峰;《中国优秀硕士学位论文全文数据库》;20110615;第6-9、13-15、19、22、26-28、31、34页 * |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110471336A (en) * | 2019-08-02 | 2019-11-19 | 上海交通大学 | The servo control system realized based on Verilog |
Also Published As
Publication number | Publication date |
---|---|
CN104135212A (en) | 2014-11-05 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN104135212B (en) | Flexibility motion control IP (Intellectual Property) core and implementation method thereof | |
CN104899182B (en) | A kind of Matrix Multiplication accelerated method for supporting variable partitioned blocks | |
Gaubert et al. | Modeling and analysis of timed Petri nets using heaps of pieces | |
CN110109491A (en) | Continuous S curve acceleration and deceleration motion control method, system and electronic equipment | |
CN103064343B (en) | Track generating method facing toward complex curved surface processing and track generating system thereof | |
CN105719351B (en) | A kind of method and apparatus of display electronic map | |
CN102809945B (en) | Movement planning method for numerical control processing, movement planner and application thereof | |
CN112783098B (en) | Multi-axis linkage mechanical motion control method, device and system and readable storage medium | |
CN103270468B (en) | The random waveform that low memory uses represents or generates | |
CN103870238B (en) | Pseudo-random sequence generator and its generation method based on FPGA | |
CN103941647A (en) | Flexible accelerating and decelerating control interpolation method for embedded NC equipment | |
CN109947573A (en) | Intelligence suitable for electric system edge calculations accelerates chip | |
CN100511125C (en) | Device for realizing CORDIC algorithm | |
CN104580005A (en) | Method for sending random length message according to flow model accurately | |
CN106970561B (en) | Kinetic control system | |
CN111185909A (en) | Robot operation condition acquisition method and device, robot and storage medium | |
CN104133409B (en) | A kind of symmetry adjustable triangular wave synthesizer | |
CN104777456B (en) | Configurable radar digital signal processing device and its processing method | |
CN102594361B (en) | Audio frequency asynchronous sample rate conversion disposal route | |
CN109407888B (en) | Two-stage filtering processing method for real-time following and debouncing of touch screen coordinate | |
CN103744352A (en) | FPGA-based cubic B-spline curve hardware interpolator | |
CN102353940B (en) | Pulse compression optimization method based on field programmable gate array (FPGA) | |
CN107273099A (en) | A kind of AdaBoost algorithms accelerator and control method based on FPGA | |
CN202309644U (en) | Vehicle vibration signal filtering device based on field programmable gate array (FPGA) | |
GB2503263A (en) | Truncated multiplier |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20170419 |