CN100451891C - Dynamic forward-looking processing method of small program segment and implementing device - Google Patents

Abstract

The invention discloses the dynamic forward-looking method and implementation equipment for small program pieces, which uses human-machine interface, task coordination, movement control and programmable controller. It loads the movement control and the programmable controller in the system kernel space, and loads the human-machine interface and the task coordination in the system user space, and communicates with each other through the system sharing communications buffer; the movement control includes two parts of movement planning and axis control; its characteristics are: the movement planning uses dynamic forward-looking method to process the speed planning to the small program pieces interpreted by the task coordination part, and forms the speed set value in processing, and implements the machine servo axis smooth movement by the axis control in the processing. The invention can effectively addresses the speed smooth problem and the pointed transition problem in the small segment processing, thereby improving the surface machining quality of the workpiece.

Description

The dynamic prediction disposal route and the implement device of small program segment

Technical field

The present invention relates to the velocity process technology of numerical control device, specifically a kind of disposal route that CAM is produced the work procedure process velocity of forming by small program segment.Adopt the numerical control device of this method can realize that the dynamic prediction of small program segment process velocity handles, solve smooth problem of speed and wedge angle transition problem in the little line segment processing effectively, thereby improve the suface processing quality of workpiece.

Background technology

The processing of complex profile is the main application of high-performance numerically-controlled machine.In present numerical control device, because the restriction of cost and system processing power, device only has straight line and circular arc processing capacity usually.Therefore, for the processing of implement device to complex profile, must adopt earlier CAD system carry out the solid model design, again by the CAM system according to processing technology, convert three-dimensional model to form job sequence, process processing by numerical control device by program segment (straight-line segment or arc section).Common speed and precision in order to reach the program segment defined, device will carry out the acceleration, deceleration processing to each program segment, to guarantee that cutter is through the specified position of program segment.

And in the processing of complex profile, in order to ensure shape and the precision by straight line or circular fitting workpiece, the path displacement of the program segment defined that CAM produces is very short usually, therefore also claims small program segment (referring to the millimeter level).If with each small program segment is that unit carries out acceleration, deceleration control, these small program segments will cause the frequent start-stop of motor on the one hand, and too short processing displacement simultaneously also can cause " starvation of data " in the velocity process, can't reach the speed of regulation.In fact the Machining Complicated Profile program of CAM generation has a notable attribute, and the small program segment of promptly finishing a certain continuous machining path has identical speed of feed, and formed tangential velocity angle satisfies corresponding fitting precision requirement between program segment.Therefore, adopt the prediction treatment technology, by to finishing the speed of feed unified planning of a certain continuous machining path program segment, and realize the connection processing of speed between different machining paths, can realize speed smooth problem and the wedge angle transition problem of mini line segment processing in handling effectively.

At present the prediction processing capacity in the numerical control device has become the important indicator of estimating its performance, and state, inside and outside many high-performance numerical control devices all are furnished with the function corresponding assembly as the product of Siemens, Fa Nake company.These methods are mainly static method, promptly by opening up the program segment buffer zone in device, and realize the unified planning of process velocity with the program segment read ahead technique.Therefore, hop count is handled in the prediction of program segment becomes the important indicator of estimating its performance.Yet the job sequence of the complex profile that CAM generated is on a grand scale usually.Medium finished surface with 500 * 500mm is an example.With step-length 0.5mm, line-spacing 1mm processing, its program hop count then is ten thousand sections of 500 * 500/0.5=50.Huge program hop count, make static method can't perfect foresight processing the track situation and produce " chain rupture ", thereby cause the discontinuous of processing medium velocity.Simultaneously because of the dynamic perfromance of device controlling object, and user's work in-process repaiies the accents interactive operation of etc.ing to feeding and spindle speed, and prediction processing itself is an individual process dynamically.Adopt the static treatment method,, can't adjust dynamically the data of having planned according to the dynamic perfromance of device because of the program segment of handling has been stored in the buffer zone.

Summary of the invention

Problem at the existence of the small program segment prediction of existing numerical control device disposal route, the purpose of this invention is to provide a kind of can be according to dynamic perfromance the small program segment that the data of having planned are revised of device dynamically look forward to the prospect disposal route and implement device, the present invention is by the decision condition and the speed planning method of introducing " program chain " and " program chain " identification, and the speed connection processing between " program chain " realizes the dynamic prediction processing of small program segment process velocity; Based on the method, the present invention has adopted PC platform development numerical control implement device is to realize the stepless control of small program segment speed of feed in the process.

To achieve these goals, technical scheme of the present invention is as follows:

Adopt man-machine interface, task coordinate, motion control and Programmable Logic Controller, motion control and Programmable Logic Controller are loaded on the kernel spacing of system, man-machine interface, task coordinate partly load on the user's space of system, pass through the shared communication buffer district mutual communication of system to each other; Motion control comprises motion planning and axle control two parts; Wherein motion planning adopts dynamically prediction velocity process method, and the small program segment through the task coordinate partial interpretation is carried out speed planning, forms the speed value of setting in the process, is realized the smooth motion of lathe axis servomotor in the process by the axle control section.

Described dynamic prediction velocity process method is by " program chain " identification, and three steps of speed connection processing are formed between " program chain " speed planning and " program chain ".Described " program chain " is for satisfying the continuous program section of device speed and accuracy requirement.Based on the feature of CAM generation complex profile work procedure, two adjacent program segments can form " program chain ".The decision condition of described " program chain " identification is: 1) have identical program segment speed is set; 2) when cutter passed through the angle of two program segments linkings so that speed to be set, speed was no more than the setting value of device; 3) when cutter passed through the angle of two program segments connections so that speed to be set, error was no more than the setting value of device; Based on described decision condition, the identification of " program chain " can form some " program chains " to the program segment " cluster " of continuous arrival in the method; " cluster " of program segment is a dynamic process.

Described speed planning processing procedure serves as to handle unit with " program chain ", and based on the requirement of small program segment processing to workpiece surface finish, processing procedure adopts " S type " rate curve disposal route.Many at the constraint of " S type " rate curve staging treating method, the problem that calculated amount is big, processing procedure adopts the disposal route based on cubic function; Specifically be by adopting boost phase and the decelerating phase match of cubic function to " S type " rate curve, and then boost phase in formation " program chain " velocity process, at the uniform velocity the interpolation step number and the moving displacement in stage, decelerating phase; When " program chain " only comprised a program segment, what realized was traditional speed planning method; When " program chain " when comprising a plurality of program segment, can be because of it to the program segment unified planning that arrives in advance, what finished is that the prediction of process velocity is handled; In velocity process, because the arrival of program segment is a dynamic process.If the program segment of follow-up arrival satisfies the decision condition of current " program chain ", and current " program chain " processing is not as yet to the decelerating phase, then, prevent " chain rupture " phenomenon that produces in the static treatment method by revising with the interpolation step number in decelerating phase to planning at the uniform velocity.

When a plurality of " program chain " occurring, the speed connection processing is based on tangential velocity angle binding occurrence between the initial increment of current chain, follow-up end stopping of chain increment and " program chain " between described " program chain ", utilize " recursion " mechanism that combines with " backstepping ", draw connection speed between " program chain ", with seamlessly transitting of speed between realization " program chain ".

The implement device of the dynamic prediction disposal route of described small program segment, based on isa bus, power by power supply, comprise: display, central controller, machine operation panel, axle control circuit and interface circuit, wherein: central controller is communicated by letter with display, machine operation panel, and communicate by letter with axle control circuit and interface circuit by isa bus, the dynamic prediction handling procedure of small program segment is installed among the FLASH or DOM (electronic hard disc) of central controller, the smoothing processing of implement device process velocity;

Described handling procedure is by program segment queue management, program segment management, program chain management, program chain queue management, trajectory planning.Described program segment queue management receives work procedure, is the interface between motion planning and task coordinate; The program segment management realizes the conversion of program segment data layout; The program chain management realizes the identification of program chain in the work procedure according to the program chain decision condition; The program chain queue management realizes processing that speed between the management of work procedure and program chain is connected in the mode of program chain; Trajectory planning is realized the level and smooth control of process velocity based on the inventive method.

Compared with prior art, the present invention has following advantage:

The one, adaptability is strong.Introduce " program chain " notion, the identification problem that is summed up as " program chain " is handled in the prediction of program segment.Corresponding by being provided with " program chain " decision condition, the method that makes both can realize the speed planning of single block, also can realize the speed planning of multisegment.

The 2nd, carry out the efficient height.Adopt cubic function match " S-type " velocity process curve, avoided the computational complexity of staging treating method.On the basis that guarantees rate smoothing, improved algorithm efficiency.

The 3rd, dynamic is good.Dynamic perfromance at the device controlling object, introduce in the speed planning the at the uniform velocity dynamic correction mechanism of the motion step number in stage, decelerating phase, both avoided " chain rupture " phenomenon in the velocity process, but also in the processing user interactive operations such as accent are repaiied in feeding and spindle speed.

The 4th, rate smoothing degree height.At a plurality of " program chains " that occur in the work procedure,, guarantee the flatness of process medium velocity by introducing " recursion " and " backstepping " treatment mechanism that speed connects between " program chain ".

Description of drawings

Fig. 1 is the inventive method overall construction drawing.

Fig. 2-1 is for finishing the i.e. composition diagram of " program chain " of the continuous small program segment collection of a certain machining path.

Fig. 2-2 is the enlarged drawing of the upper left corner two " program chains " among Fig. 2-1.

Fig. 3-1 is that two small program segments connect formed " program chain " movement locus.

Fig. 3-2 is the speed process decision chart of " program chain " among Fig. 3-1.

Fig. 3-3 is the error process decision chart of " program chain " among Fig. 3-1.

Fig. 4 is the present invention's's " program chain " speed planning method figure.

Fig. 5 is speed connection processing figure between the present invention's " program chain ".

Fig. 6 is the implement device structural drawing of the inventive method.

Fig. 7 is a program structure diagram of the present invention.

Fig. 8 is the program flow diagram of the present invention's " program chain " identification.

Fig. 9 is the present invention's " program chain " speed planning program flow diagram.

Figure 10 is a speed linker process flow diagram between the present invention's " program chain ".

Figure 11 is procedure order figure of the present invention.

Figure 12 is the design sketch of existing method speed planning.

Figure 13 is the design sketch of speed planning of the present invention.

Embodiment

Below in conjunction with accompanying drawing the present invention is described in further detail.

(1) the dynamic prediction disposal route of small program segment

Referring to Fig. 1, adopt man-machine interface, task coordinate, motion control and Programmable Logic Controller, motion control and Programmable Logic Controller are loaded on the kernel spacing of system, man-machine interface, task coordinate partly load on the user's space of system, pass through the shared communication buffer district mutual communication of system to each other; Wherein:

Man-machine interface is used for receiving operational order and work procedure, is used for showing the process of implement device of the present invention, realize between machine operation personnel and implement device of the present invention alternately.

Task coordinate is made up of condition managing and work procedure explanation two parts.Condition managing adopts the finite state machine technology, realizes between machining state, as automatic management and the switching between automatic processing, mode such as manual.Work procedure is explained and is adopted technique of compiling, realizes the decomposition to the Machine-Tool Control order of operational order and work procedure.

Motion control comprises motion planning and axle control two parts, motion planning is according to the program segment disposal route of dynamically looking forward to the prospect, the speed value of setting that generates the lathe axis servomotor is passed to a control section, is realized the smooth motion of lathe axis servomotor in the process by axle control section control device.

Programmable Logic Controller is made up of ladder diagram editor device and Programmable Logic Controller engine two parts.The ladder diagram editor device realizes representing with ladder diagram the editor and the conversion of Machine Tool Electric Appliance control program.The Programmable Logic Controller engine is explained execution to the Machine Tool Electric Appliance control program, thereby finishes Machine Tool Electric Appliance, as the control action of liquid coolant switch, tool changing etc.

The dynamic prediction disposal route of described small program segment is by " program chain " identification, and speed connection processing step is formed between " program chain " speed planning and " program chain ", to solve rate smoothing and the wedge angle transition problem in the small program segment processing.

" program chain " identification:

Described " program chain " is for satisfying the continuous program section of device speed and accuracy requirement; Based on the feature of CAM generation complex profile work procedure, two adjacent program segments can form " program chain "; Shown in Fig. 2-1, " program chain " (has from 1 to 7 " program chain " for the continuous small program segment collection of finishing a certain machining path, machining locus), Fig. 2-2 represents the 4th to 5 " program chain " program segment that machining locus comprised, wherein: the starting point of program chain 4 is to start at from the B end of the AB program segment of program chain 3, specifically comprises BC, CD, DE, EF program segment; Program chain 5 comprises FG, GH program segment.Feature based on CAM generation Machining Complicated Profile program can get the decision condition of " program chain " identification.

The decision condition derivation of " program chain " identification is as follows:

Shown in Fig. 3-1, establishing Pi, Pi+1 is two adjacent in work procedure program segments, and its velocity vector is respectively $\stackrel{\→}{v_i}=(v_{i,x},v_{i,y},v_{i,z}),$ $\stackrel{\→}{v_{i+1}}=(v_{i+1,x},v_{i+1,y},v_{i+1,z}),$ V wherein _{I, x}, v _{I, y}, v _{I, z}With v _{I+1, x}, v _{I+1, y}, v _{I+1, z}Be respectively the projection of velocity vector at x axle, y axle and z axle.

Between angle be θ, the peak acceleration a of device _Max, the shape error of device is e, the interpolation cycle of device is Δ t.In order to describe the decision condition of " program chain ", introduce following symbol:

MaxInc represents at the uniform velocity increment (belonging to a kind of displacement increment), i.e. the distance of point-to-point transmission among Fig. 3-1, and A, C represent to adopt the tie point of Pi of the present invention, two program segments of Pi+1, and B represents to adopt the tie point of classic method Pi, two program segments of Pi+1.The displacement increment of an interpolation cycle of Pi, two program segments of Pi+1 is respectively during motion:

$\max {\mathrm{Inc}}_i=\left|\stackrel{\→}{v_i}\right|\×\mathrm{\Δt}$

$\max {\mathrm{Inc}}_{i+1}=\left|\stackrel{\→}{v_{i+1}}\right|\×\mathrm{\Δt}$

Therefore, at the uniform velocity increment maxInc was determined by program segment speed value of setting and interpolation time.

CornerInc represents the acceleration a by device _MaxThe displacement increment of the interpolation cycle in angle between two adjacent program segment Pi, the Pi+1 of decision.Shown in Fig. 3-2, Δ x _n, Δ y _n, Δ z _nRepresent that respectively at the uniform velocity increment maxInc is in the projection of x axle, y axle and z axle, n represents the interpolation step number, then:

$\mathrm{cornerInc}\≤\frac{a_{\max }{\mathrm{\Δt}}^2}{\underset{k\∈x,y,z}{\max }|v_{i+1,k}-v_{i,k}|\cos \mathrm{\α}};$

Wherein $\underset{k\∈x,y,z}{\max }|v_{i+1,k}-v_{i,k}|$ For | v _{I+1, x}-v _{I, y}|, | v _{I+1, y}-v _{I, y}|, | v _{I+1, z}-v _{I, z}| intermediate value the greater, α are itself and the formed angle of respective shaft.

ShapInc represents the displacement increment by the interpolation cycle in angle between two adjacent programs section Pi, Pi+1 of shape error e decision, and shown in Fig. 3-3, BD represents that small program segment connects the shape error that is produced, then:

$\mathrm{shapInc}\≤\frac{4e}{\sqrt{2+\cos \mathrm{\θ}}};$

Based on foregoing description, and if only if maxInc _i=maxInc _I+1, maxInc _i≤ shapInc, maxInc _iDuring≤cornerInc, program segment P _i, P _I+1Can be classified as one " program chain ".

" program chain " speed planning:

Described speed planning processing procedure serves as to handle unit with " program chain ", and based on the requirement of small program segment processing to workpiece surface finish, processing procedure adopts " S type " rate curve disposal route.Many at the constraint of " S type " rate curve staging treating method, the problem that calculated amount is big, processing procedure adopts the disposal route based on cubic function, specifically is by adopting boost phase and the decelerating phase match of cubic function to " S type " rate curve.

As shown in Figure 4, planning will " program chain " motion be divided into acceleration, at the uniform velocity, three stages of deceleration, the interpolation step number in each stage is respectively m, p, q, and with initInc, maxInc and finalInc represent respectively its motion initial period, at the uniform velocity the displacement increment of the unit interpolation time of stage and termination phase (be called for short initial increment, at the uniform velocity increment with stop increment).At big based on staging treating method calculated amount, described processing is with following cubic function match " S type " boost phase of rate curve and the displacement increment in decelerating phase:

ΔD(n)＝a(nΔt) ³+b(nΔt) ²+c(nΔt)+d (1)

Wherein Δ D is a displacement increment, a, and b, c, d are the coefficient of rate curve; N is the interpolation ordinal number.

By known conditions $\left\{\begin{array}{c}\mathrm{\&Delta;D}\left(0\right)={\mathrm{\&Delta;<figure-callout\; id="0"\; label="D"\; filenames="C20051004775100211.png,C20051004775100272.png"\; state="\{\{state\}\}">D</figure-callout>}}_0,\mathrm{\&Delta;D}\left(s\right)=\mathrm{\&Delta;}{D}_{\mathrm{end}}\\ \mathrm{\&Delta;}\stackrel{\&CenterDot;}{D}\left(0\right)=0,\mathrm{\&Delta;}\stackrel{\&CenterDot;}{D}\left(s\right)=0\end{array}\right.,$ Can get four coefficients of cubic function:

$a=-2\&times;\frac{{\mathrm{\&Delta;D}}_{\mathrm{end}}-{\mathrm{\&Delta;D}}_0}{{\left(\mathrm{s\&Delta;t}\right)}^2}$

$b=3\&times;\frac{{\mathrm{\&Delta;D}}_{\mathrm{end}}-{\mathrm{\&Delta;D}}_0}{{\left(\mathrm{s\&Delta;t}\right)}^2}---\left(2\right)$

c＝0

d＝ΔD ₀

Δ D in the formula ₀Be the initial displacement increment of motion, promptly initial increment; Δ D _EndDisplacement increment when being the motion end promptly stops increment; S is the interpolation step number, is unknown number.S is more little, and promptly the step number adjusted of speed is few more, rapid change just fast more, and correspondingly acceleration is just big more, can get the interpolation step number thus and be:

$s=\mathrm{ceil}\left[\frac{3|{\mathrm{\&Delta;D}}_{\mathrm{end}}-{\mathrm{\&Delta;<figure-callout\; id="0"\; label="D"\; filenames="C20051004775100211.png,C20051004775100272.png"\; state="\{\{state\}\}">D</figure-callout>}}_0|}{{2a}_{\max }{\mathrm{\&Delta;t}}^2}\right]---\left(3\right)$

By formula (1), (2) and formula (3) can be obtained the total displacement through s interpolation cycle, and expression formula is

$D=\underset{n=1}{\overset{s}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;D}\left(n\right)=(s+1)\&times;\left(\frac{{\mathrm{\&Delta;<figure-callout\; id="0"\; label="D"\; filenames="C20051004775100211.png,C20051004775100272.png"\; state="\{\{state\}\}">D</figure-callout>}}_0+{\mathrm{\&Delta;D}}_{\mathrm{end}}}{2}\right)-{\mathrm{\&Delta;D}}_0---\left(4\right)$

Carry out the speed planning of program chain based on formula (1), (2), (3), (4): be specially:

Boost phase

The interpolation step number is m, accelerates at the uniform velocity increment maxInc from initial increment initInc, and moving displacement is D ₁, with Δ D ₀=initInc, Δ D _End=maxInc substitution formula (2), (3), (4) can get:

The interpolation step number

$m=\mathrm{ceil}\left[\frac{3|\max \mathrm{Inc}-\mathrm{initInc}|}{{2a}_{\max }{\mathrm{\&Delta;t}}^2}\right]$

Moving displacement

${\mathrm{<figure-callout\; id="1"\; label="D"\; filenames="C20051004775100211.png,C20051004775100221.png"\; state="\{\{state\}\}">D</figure-callout>}}_1=(m+1)\&times;\left(\frac{\mathrm{initInc}+\max \mathrm{Inc}}{2}\right)-\mathrm{initInc}$

The displacement increment function coefficients

$a1=-2\&times;\frac{\max \mathrm{Inc}-\mathrm{initInc}}{{\left(\mathrm{m\&Delta;t}\right)}^2}$

$\mathrm{<figure-callout\; id="1"\; label="b"\; filenames="C20051004775100211.png,C20051004775100221.png"\; state="\{\{state\}\}">b</figure-callout>}1=3\&times;\frac{\max \mathrm{Inc}-\mathrm{initInc}}{{\left(\mathrm{m\&Delta;t}\right)}^2}$

c1＝0

d1＝initInc

Decelerating phase

The interpolation step number is q, decelerates to from increment maxInc at the uniform velocity and stops increment finalInc, and moving displacement is D ₃, with Δ D ₀=maxInc, Δ D _End=finalInc substitution formula (2), (3), (4) can draw:

The interpolation step number

$q=\mathrm{ceil}\left[\frac{3|\mathrm{finalInc}-\max \mathrm{Inc}|}{{2a}_{\max }{\mathrm{\&Delta;t}}^2}\right];$

Moving displacement

$D_3=(q+1)\&times;\left(\frac{\mathrm{finalInc}+\max \mathrm{Inc}}{2}\right)-\max \mathrm{Inc};$

The displacement increment function coefficients

$a3=-2\&times;\frac{\mathrm{finalInc}-\max \mathrm{Inc}}{{\left(\mathrm{q\&Delta;t}\right)}^2}$

$b3=3\&times;\frac{\mathrm{finalInc}-\max \mathrm{Inc}}{{\left(\mathrm{q\&Delta;t}\right)}^2};$

c3＝0

d3＝maxInc

Stage at the uniform velocity:

Moving displacement

D=D-D ₁-D ₃, wherein D is " program chain " total displacement length, determines its length according to program segment (as: or straight line or circular arc or helix).

The interpolation step number

p＝(D-D ₁-D ₃)/maxInc

Displacement increment is constant maxInc.

When program chain does not have at the uniform velocity the stage (promptly in the program chain length range, speed can not reach at the uniform velocity increment maxInc and finishes to stop increment finalInc from initial increment initInc), then increment maxInc at the uniform velocity is adjusted at the uniform velocity increment correction maxInc ^*:

When max Inc＞initInc and max Inc＞finalInc:

${\max \mathrm{Inc}}^{*}=-\frac{1}{3}{\mathrm{pa}}_{\max }{t}^2+\frac{1}{6}\sqrt{{\left({2\mathrm{pa}}_{\max }t\right)}^2+18({\mathrm{initInc}}^2+{\mathrm{finalInc}}^2)+12a_{\max }{t}^2(\mathrm{initInc}-\mathrm{finalInc}+2D)}$

When max Inc＞finalInc and max Inc≤initInc:

$\max {\mathrm{Inc}}^{*}=\frac{3[\left({\mathrm{finalIn}{c}^2-\mathrm{initInc}}^2\right)+{2a}_{\max }{t}^2(\mathrm{initInc}-\mathrm{finalInc}+2D)]}{{4\mathrm{pa}}_{\max }{t}^2};$

When max Inc＞initInc and max Inc≤finalInc:

$\max {\mathrm{Inc}}^{*}=\frac{3[({\mathrm{initInc}}^2-{\mathrm{finalInc}}^2)+{2a}_{\max }{t}^2(\mathrm{initInc}-\mathrm{finalInc}+2D)]}{{4\mathrm{pa}}_{\max }{t}^2};$

When max Inc≤initInc and max Inc≤finalInc:

${\max \mathrm{Inc}}^{*}=\frac{1}{3}{\mathrm{pa}}_{\max }{t}^2+\frac{1}{6}\sqrt{{\left({2\mathrm{pa}}_{\max }t\right)}^2+18({\mathrm{initInc}}^2+{\mathrm{finalInc}}^2)+12a_{\max }{t}^2(\mathrm{initInc}+\mathrm{finalInc}-2D)},$

MaxInc is adjusted into maxInc ^*After, can recomputate m, p, q and moving displacement.

When program segment dynamically arrives, satisfy the decision condition of current " program chain ", and current " program chain " processing needs the speed planning of current " program chain " is revised as yet not to the decelerating phase.The length of program segment if newSeg.length represents to arrive, then when its greater than increment maxInc at the uniform velocity, revise at the uniform velocity stage interpolation step number, newly-increased step number is $p^{*}=\mathrm{ceil}\left[\frac{\mathrm{newSeg}.\mathrm{length}}{\max \mathrm{Inc}}\right];$ Otherwise it is classified as the decelerating phase.

Speed connection processing between " program chain ":

If adjacent two program segments can not be classified as one " program chain ", and a plurality of " program chains " appear.The speed connection processing is based on tangential velocity angle binding occurrence between the initial increment of current chain, follow-up end stopping of chain increment and " program chain " between described " program chain ", utilize " recursion " mechanism that combines with " backstepping ", draw connection speed between " program chain ", with seamlessly transitting of speed between realization " program chain ".

As shown in Figure 5, at a plurality of " program chains " that exist in the work procedure, reduce to zero for fear of speed between " program chain ", described " recursive process " from first " program chain ", obtains the accessible maximum increment maxFinalInci that stops after each " program chain " process finishing successively.Described " backstepping process " last program chain from " program chain " formation begins, and successively forward, obtains the maximum initial increment maxInitInci that stops increment each " program chain " for reaching.Described method is by the scanning sequence queue chain, get smaller value among maximum increment maxFinalInci of termination in " program chain " junction and the maximum initial increment maxInitInci as connection speed linkInc, and respectively adjacent " program chain " attribute is revised, respectively initial increment or termination increment are modified to linkInc.

(2) the small program segment implement device of disposal route of dynamically looking forward to the prospect

As shown in Figure 6, dynamically the look forward to the prospect implement device of disposal route of small program segment, based on isa bus, by power supply 4 power supplies, comprise: TFT escope 1, central controller 2, machine operation panel 3, axle control circuit 5 and interface circuit 6, wherein: central controller 2 is communicated by letter with display 1, machine operation panel 3, and communicate by letter with axle control circuit 5 and interface circuit 6 by isa bus, the dynamic prediction handling procedure of small program segment is installed among the FLASH or DOM type storer of central controller 2, realizes the processing of dynamically looking forward to the prospect of small program segment; In order to ensure control device control performance and open nature, the software platform of device adopts the general-purpose operating system RTLinux with real-time extension.

Specific as follows:

TFT escope 1 is of a size of 10.4 ", the display mode of employing 640X480 supports 16 to strengthen looks.

Central controller 2 adopts the industry control integrated circuit board of standard, and CPU adopts the PentiumMMX of Intel, dominant frequency 200MHz, and internal memory 32M supports FLASH and DOM dual mode, memory capacity 64M.

Machine operation panel 3 is interfaces of man-machine interaction, finish keyboard editor, display message and figure and finish to machine operation etc. function.Guidance panel comprises major key plate, function key plate, key switch and LCD pilot lamp, waver, emergency stop switch, loop start, cycle stop push button etc. (being prior art).

Power supply 4 for digital control system provide+5V ,+12V ,-three kinds of direct supplys of 12V.It has high-level efficiency, high reliability, the characteristics of output ripple and low and noise.Power supply unit is mainly formed (for commercial product) by industrial Switching Power Supply T-50B, power board, power input board, power supply electromagnetic interface filter four parts.

Axle control circuit 5 is preferred circuit (a commercial product).It is according to the control requirement of lathe axis servomotor, provide D/A output interface, scrambler input interface, switching signal interface, gauge head signaling interface, display lamp signaling interface, imput output circuit interface, encoder fault to detect interface, isa bus interface, and provide steering logic by field programmable gate array.Wherein the D/A output interface provides the control signal of servo driving, the scrambler input interface detects the position signalling of scrambler in the servomotor, the switching signal interface detects the clear point and the extreme position of lathe axis servomotor, the gauge head signaling interface detects the online position signalling of gauge head, the display lamp signaling interface provides the executing state in the axle control, the imput output circuit interface provides control signal for interface circuit, and encoder fault detects the fault that interface detects scrambler.Circuit produces signal and communicates by the central controller 2 in isa bus interface circuit 6 and the device at last, is finished the speed planning of small program by the software on it.

Interface circuit 6 is preferred circuit (a commercial product), and major function provides 64 inputs/48 output digital interfaces of isolation, plays the information transfering action between device and the machine tool functional component (as: various switches, pilot lamp and relay).The state of various switches in the numerically-controlled machine (as limit switch etc.) will be read in the device by digital input port, and the break-make of pilot lamp on the numerically-controlled machine control panel and relay is then controlled by the digital output port of interface circuit 6.Each passage of interface circuit 6 all is that photoelectricity is isolated, and has strengthened the antijamming capability of system.

As shown in Figure 7, the small program segment handling procedure of dynamically looking forward to the prospect comprises trajectory planning (TrajectoryPlanner), program segment queue management (SegmentQueue), program chain management (SegmentChain), program chain queue management (ChainQueue), program segment management.Program is called by the motion control master routine, wherein: the program segment queue management is the interface of motion planning and task coordinate part, the task coordinate part is passed to the motion planning part by it with work procedure, the program segment management is with the element conversion cost method data structure in the program segment queue management, and its structure comprises following element:

Type: the program segment type, as straight line, circular arc, helix;

StartPosition: the reference position of program segment;

EndPosition: the final position of program segment;

Line: straight line parameter;

HelixRadius: helix parameter;

Circle: circular arc parameter

After the queue management of motion control calling segment receives program segment, convert thereof into the required data structure of this method by the program segment management, the management of calling program chain triggers the operation of trajectory planning thus according to process object---the program chain of decision condition formation trajectory planning then; Trajectory planning plans speed according to " S-type " rate curve, and when having a plurality of program chain, the connection speed of calling program queue chain Management Calculation program chain; Last motion control forms the level and smooth control of cutting speed in the process according to the call by result axle control section of trajectory planning.

" program chain " management is according to " program chain " in the decision condition identification job sequence, its program flow diagram as shown in Figure 8, be specially: the program segment (newseg) that at first from the program segment formation, reads newly, if current " program chain " be empty, then with newly to program segment be that element forms " program chain " otherwise from the program segment (PLlast) of current " program chain " taking-up tail of the queue; If the program segment that newly arrives and the program segment of tail of the queue do not satisfy three conditions of " program chain " identification decision, then with newly to program segment be that element forms " program chain ", identification decision processing end; If the program segment that newly arrives and the program segment of tail of the queue satisfy three conditions of " program chain " identification decision, and current " program chain " process as yet, then will be newly to program segment incorporate current " program chain " into, identification decision processing end; Otherwise the speed planning to current " program chain " is revised; If newly to program segment satisfies two correction conditions of speed (decision condition and current " program chain " that promptly satisfy current " program chain " are processed as yet not to the decelerating phase condition), with newly to program segment incorporate current " program chain " into, and the interpolation step number of " program chain " adjusted, the identification decision processing finishes; Otherwise do not satisfy the speed correction conditions, with newly to program segment be that element forms " program chain ", finish identification and handle.

Trajectory planning realizes that the speed planning of " program chain " in the processing processing is connected with speed." program chain " speed planning program flow diagram is specially as shown in figure 10: take out head of the queue " program chain " from " program chain " formation, judge its current state.If should " program chain " not plan (new program chain) as yet, then call the connection speed between " program chain " queue manager calculating " program chain ", cook up coefficient a, b, c, the d of interpolation step number m, p, q and the displacement increment function in each stage of motion according to described method, the interpolation step number of initial motion planning and the state (that is: this program chain of mark being planned) of mark current " program chain " calculate the displacement increment in each stage then respectively; Otherwise this program chain has been planned the displacement increment that then directly calculates each stage according to motion state respectively; If current motion is in boost phase, according to its its displacement increment of displacement increment function calculation; If current motion is in the at the uniform velocity stage, its displacement increment is a constant; If current motion is in the decelerating phase, according to its its displacement increment of displacement increment function calculation; And then calculate moving displacement by the displacement increment of corresponding sports state, and displacement conversion is become D coordinates value and output, add up the decision condition that the interpolation step number finishes as " program chain " simultaneously.If current " program chain " motion finishes, judge whether whole procedure finishes.If whole job sequence finishes, remove the motion formation, finish motion planning.

" program chain " queue management safeguards " program chain " formation, realizes management to job sequence in the mode of " program chain ", and calculates the connection speed between " program chain ".The program flow diagram of " program chain " queue management is specially as shown in Figure 9: scanning " program chain " formation, and from the beginning recursion is calculated each " program chain " maximum increment that finishes backward; Backstepping calculates each " program chain " maximum initial increment maxInitInci from back to front then; Maximum initial increment between more adjacent at last " program chain " and the maximum increment maxFinalInci that finishes are got its minimum value as the connection speed between " program chain ".

In service at above-mentioned implement device soft, rigid structure, the user can import work procedure by man-machine interface, and work procedure sends to motion control through the work procedure explanation under the control of task coordinate, enter the job sequence motion planning.As shown in figure 11, after the queue management of motion control calling segment receives program segment, convert thereof into the required data structure of this method by the program segment management, the management of calling program chain triggers the operation of trajectory planning thus according to process object---the program chain of decision condition formation trajectory planning then; Trajectory planning plans speed according to " S-type " rate curve, and when having a plurality of program chain the connection speed of calling program queue chain Management Calculation program chain; Last motion control forms the level and smooth control of cutting speed in the process according to the call by result axle control section of trajectory planning.

(3) implementation effect of the present invention:

The topworks of the inventive method and implement device adopts the servo and motor of peace river ∑-2, and the control lathe is the three-axis numerical control milling machine, and the major parameter of device is as follows:

Feed rate F=2500mm/min

Peak acceleration=1000mm/s ²

Interpolation cycle=0.002 second

Servo period=0.0005 second

Task coordinate performance period=0.006 second

The inventive method and implement device Evaluation on effect are processed as the evaluation and test foundation with " R type " curve commonly used in the processing of small program segment.Adopt existing method speed planning curve (horizontal ordinate is unit with the servo period, and ordinate is represented pairing process velocity) as shown in figure 12; And adopting the inventive method and implement device, the speed planning curve is as shown in figure 13.Can obtain as drawing a conclusion from the contrast of rate curve:

1. dynamically the look forward to the prospect implement device acceleration and deceleration number of times of disposal route of small program segment is few, and rate curve is level and smooth.During processing " R type " curve, the present invention only handles through 6 acceleration and deceleration, and classic method needs 26 acceleration and deceleration.

2. dynamically the look forward to the prospect implement device of disposal route of small program segment has realized that the speed between program chain connects, and classic method does not realize that the speed between program chain (or section) is connected, and speed is zero after every section process finishing.

3. the small program segment disposal route process velocity height of dynamically looking forward to the prospect.Because for identical initialization controlled variable, used the time of 220 units during the inventive method implement device processing R type curve, and classic method has been used the time of more than 400 unit.

Claims (9)

1. the dynamic prediction disposal route of a small program segment, adopt man-machine interface, task coordinate, motion control and Programmable Logic Controller, motion control and Programmable Logic Controller are loaded on the kernel spacing of system, man-machine interface, task coordinate partly load on the user's space of system, pass through the shared communication buffer district mutual communication of system to each other; Motion control comprises motion planning and axle control two parts; It is characterized in that: motion planning adopts dynamically prediction disposal route, small program segment through the task coordinate partial interpretation is carried out speed planning, form the speed value of setting in the process, realize the smooth motion of lathe axis servomotor in the process by the axle control section;

Described dynamic prediction disposal route is by " program chain " identification, and the speed connection processing is formed between " program chain " speed planning and " program chain ", and described " program chain " is for satisfying the continuous program section of device speed and accuracy requirement; Based on the feature of CAM generation complex profile work procedure, two adjacent program segments can form " program chain "; The decision condition of described " program chain " identification is: 1) have identical program segment speed is set; 2) when cutter passed through the angle of two program segments connections so that speed to be set, speed was no more than the setting value of device; 3) when cutter passed through the angle of two program segments connections so that speed to be set, error was no more than the setting value of device; Based on described decision condition, " program chain " identifying is to the program segment " cluster " of continuous arrival, some to form " program chains "; Described speed planning serves as to handle unit with " program chain ", processing procedure adopts " S type " rate curve disposal route, specifically be to adopt boost phase and the decelerating phase match of cubic function, form boost phase in " program chain " velocity process, at the uniform velocity the interpolation step number and the moving displacement of stage, decelerating phase cutter " S type " rate curve; In velocity process, if the program segment of follow-up arrival satisfies the decision condition of current " program chain ", and current " program chain " processing is not as yet to the decelerating phase, then, prevent " chain rupture " phenomenon that produces in the static treatment method by revising with the interpolation step number in decelerating phase to planning at the uniform velocity; Speed connection processing between described " program chain ": when a plurality of " program chain " occurring, the speed connection processing is based on tangential velocity angle binding occurrence between the initial increment of current chain, follow-up end stopping of chain increment and " program chain " between described " program chain ", utilize " recursion " mechanism that combines with " backstepping ", draw connection speed between " program chain ", with seamlessly transitting of speed between realization " program chain "; Described " recursion " is from first " program chain ", obtains the accessible maximum increment that stops after each " program chain " process finishing successively; Described " backstepping " last program chain from " program chain " formation begins, and successively forward, obtains the maximum initial increment that stops increment each " program chain " for reaching.

2. by the described small program segment of claim 1 disposal route of dynamically looking forward to the prospect, it is characterized in that: the decision condition derivation of described " program chain " identification is:

If Pi, Pi+1 are two adjacent in job sequence program segments, its velocity vector is respectively ${\stackrel{\&RightArrow;}{v}}_i=(v_{i,x},v_{i,y},v_{i,z}),$ ${\stackrel{\&RightArrow;}{v}}_{i+1}=(v_{i+1,x},v_{i+1,y},v_{i+1,z}),$ V wherein _{I, x}, v _{I, y}, v _{I, z}With v _{I+1, x}, v _{I+1, y}, v _{I+1, z}Be respectively the projection of velocity vector at x axle, y axle and z axle;

Between angle be θ, the peak acceleration a of device _Max, the shape error of device is e, the interpolation cycle of device is Δ t;

MaxInc represents at the uniform velocity displacement increment, and the displacement increment of an interpolation cycle of Pi, two program segments of Pi+1 is respectively during motion:

$\max {\mathrm{Inc}}_i=\left|{\stackrel{\&RightArrow;}{v}}_i\right|\&times;t$

$\max {\mathrm{Inc}}_{i+1}=\left|\stackrel{\&RightArrow;}{v_{i+1}}\right|\&times;t;$

CornerInc represents the acceleration a by device _MaxThe displacement increment of the interpolation cycle in angle between two adjacent program segment Pi, the Pi+1 of decision:

$\mathrm{cornerInc}\&le;\frac{a_{\max }{t}^2}{\underset{k\&Element;x,y,z}{\max }|v_{i,k}-v_{i+1,k}|\cos };$

Wherein $\underset{k\&Element;x,y,z}{\max }|v_{i+1,k}-v_{i,k}|$ For | v _{I+1, x}-v _{I, y}|, | v _{I+1, y}-v _{I, y}|, | v _{I+1, z}-v _{I, z}| intermediate value the greater, α are itself and the formed angle of respective shaft;

ShapInc represents the displacement increment by the interpolation cycle in angle between two adjacent programs section Pi, Pi+1 of shape error e decision:

$\mathrm{shapInc}\&le;\frac{4e}{\sqrt{2+\cos }};$

Based on foregoing description, and if only if max Inc _i=max Inc _I+1, max Inc _i≤ shapInc, max Inc _iDuring≤cornerInc, program segment P _i, P _I+1Be classified as one " program chain ".

3. press the dynamic prediction disposal route of the described small program segment of claim 1, it is characterized in that: described speed planning with the motion of " program chain " be divided into acceleration, at the uniform velocity, slowed down for three stages, the interpolation step number in each stage is respectively m, p, q, and represent it respectively at motion initial period, the displacement increment of the unit interpolation time of stage and termination phase at the uniform velocity with initInc, maxInc and finalInc, promptly initial increment, at the uniform velocity increment with stop increment; With as minor function match " S type " boost phase of rate curve and the displacement increment in decelerating phase:

ΔD(n)＝a(nΔt) ³+b(nΔt) ²+c(nΔt)+d (1)

Wherein Δ D is a displacement increment, a, and b, c, d are the coefficient of rate curve, n is the interpolation ordinal number;

By known conditions $\left\{\begin{array}{c}\mathrm{\&Delta;D}\left(0\right)={\mathrm{\&Delta;D}}_0,\mathrm{\&Delta;D}\left(s\right)={\mathrm{\&Delta;D}}_{\mathrm{end}}\\ \mathrm{\&Delta;}\stackrel{\&CenterDot;}{D}\left(0\right)=0,\mathrm{\&Delta;}\stackrel{\&CenterDot;}{D}\left(s\right)=0\end{array}\right.,$ Can get four coefficients of cubic function:

$a=-2\&times;\frac{{\mathrm{\&Delta;D}}_{\mathrm{end}}-{\mathrm{\&Delta;D}}_0}{{\left(\mathrm{s\&Delta;t}\right)}^2}$

$b=3\&times;\frac{{\mathrm{\&Delta;D}}_{\mathrm{end}}-{\mathrm{\&Delta;D}}_0}{{\left(\mathrm{s\&Delta;t}\right)}^2}---\left(2\right)$

c＝0

d＝ΔD ₀

Δ D in the formula ₀Be the initial displacement increment of motion, Δ D _EndDisplacement increment when being the motion end, s is the interpolation step number, is unknown number;

The interpolation step number is:

$s=\mathrm{ceil}\left[\frac{3|{\mathrm{\&Delta;D}}_{\mathrm{end}}-{\mathrm{\&Delta;D}}_0|}{{2a}_{\max }{\mathrm{\&Delta;t}}^2}\right]---\left(3\right)$

By formula (1), (2) and formula (3) can be obtained the total displacement through s interpolation cycle, and expression formula is:

$D=\underset{n=1}{\overset{s}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;D}\left(n\right)=(s+1)\&times;\left(\frac{{\mathrm{\&Delta;D}}_0+{\mathrm{\&Delta;D}}_{\mathrm{end}}}{2}\right)-{\mathrm{\&Delta;D}}_0---\left(4\right)$

Carry out the speed planning of program chain based on formula (1), (2), (3), (4).

4. by the dynamic prediction disposal route of the described small program segment of claim 3, it is characterized in that: the speed planning of described program chain is specific as follows:

Boost phase

The interpolation step number is m, accelerates at the uniform velocity increment max Inc from initial increment init Inc, and moving displacement is D ₁, with Δ D ₀=init Inc, Δ D _End=max Inc substitution formula (2), (3), (4) can get:

The interpolation step number

$m=\mathrm{ceil}\left[\frac{3|\max \mathrm{Inc}-\mathrm{initInc}|}{{2a}_{\max }{\mathrm{\&Delta;t}}^2}\right];$

Moving displacement

$D_1=(m+1)\&times;\left(\frac{\mathrm{initInc}+\max \mathrm{Inc}}{2}\right)-\mathrm{initInc};$

The displacement increment function coefficients

$a1=-2\&times;\frac{\max \mathrm{Inc}-\mathrm{initInc}}{{\left(\mathrm{m\&Delta;t}\right)}^2}$

$b1=3\&times;\frac{\max \mathrm{Inc}-\mathrm{initInc}}{{\left(\mathrm{m\&Delta;t}\right)}^2};$

c1＝0

d1＝initInc

Decelerating phase

The interpolation step number is q, decelerates to from increment max Inc at the uniform velocity and stops increment finalInc, and moving displacement is D ₃, with Δ D ₀=max Inc, Δ D _End=finalInc substitution formula (2), (3), (4) can draw:

The interpolation step number

$q=\mathrm{ceil}\left[\frac{3|\mathrm{finalInc}-\max \mathrm{Inc}|}{{2a}_{\max }{\mathrm{\&Delta;t}}^2}\right];$

Moving displacement

$D_3=(q+1)\&times;\left(\frac{\mathrm{finalInc}+\max \mathrm{Inc}}{2}\right)-\max \mathrm{Inc};$

The displacement increment function coefficients

$a3=-2\&times;\frac{\mathrm{finalInc}-\max \mathrm{Inc}}{{\left(\mathrm{q\&Delta;t}\right)}^2}$

$b3=3\&times;\frac{\mathrm{finalInc}-\max \mathrm{Inc}}{{\left(\mathrm{q\&Delta;t}\right)}^2};$

c3＝0

d3＝max?Inc

Stage at the uniform velocity:

Moving displacement

D=D-D ₁-D ₃, wherein D is " program chain " total length;

The interpolation step number

p＝(D-D ₁-D ₃)/maxInc

Displacement increment is constant max Inc;

When program chain does not have the at the uniform velocity stage, then increment maxInc at the uniform velocity is adjusted at the uniform velocity increment correction maxInc ^*:

When max Inc＞initInc and max Inc＞finalInc:

$\max {\mathrm{Inc}}^{*}=-\frac{1}{3}{\mathrm{pa}}_{\max }{t}^2+\frac{1}{6}\sqrt{{\left(2{\mathrm{pa}}_{\max }t\right)}^2+18({\mathrm{initInc}}^2+{\mathrm{finalInc}}^2)+{12a}_{\max }{t}^2(\mathrm{initInc}-\mathrm{finalInc}+2D)}$

When max Inc＞finalInc and maxInc≤initInc:

$\max {\mathrm{Inc}}^{*}=\frac{3[({\mathrm{finalInc}}^2-{\mathrm{initInc}}^2)+{2a}_{\max }{t}^2(\mathrm{initInc}-\mathrm{finalInc}+2D)}{{4\mathrm{pa}}_{\max }{t}^2};$

When max Inc＞initInc and maxInc≤finalInc:

$\max {\mathrm{Inc}}^{*}=\frac{3[({\mathrm{initInc}}^2-{\mathrm{finalInc}}^2)+{2a}_{\max }{t}^2(\mathrm{initInc}-\mathrm{finalInc}+2D)]}{{4\mathrm{pa}}_{\max }{t}^2};$

When max Inc≤initInc and maxInc≤finalInc:

$\max {\mathrm{Inc}}^{*}=-\frac{1}{3}{\mathrm{pa}}_{\max }{t}^2+\frac{1}{6}\sqrt{{\left(2{\mathrm{pa}}_{\max }t\right)}^2+18({\mathrm{initInc}}^2+{\mathrm{finalInc}}^2)+{12a}_{\max }{t}^2(\mathrm{initInc}+\mathrm{finalInc}-2D)}$

Max Inc is adjusted into max Inc ^*After, can recomputate m, p, q and moving displacement;

When program segment dynamically arrives, satisfy the decision condition of current " program chain ", and current " program chain " processing needs the speed planning of current " program chain " is revised as yet not to the decelerating phase; The length of program segment if newSeg.length represents to arrive, then when its greater than increment maxInc at the uniform velocity, revise at the uniform velocity stage interpolation step number, newly-increased step number is $p^{*}=\mathrm{ceil}\left[\frac{\mathrm{newSeg}\&CenterDot;\mathrm{length}}{\max \mathrm{Inc}}\right];$ Otherwise it is classified as the decelerating phase.

5. press the dynamic prediction disposal route of the described small program segment of claim 1, it is characterized in that: the speed connection processing adopts " recursion " combine with " backstepping " connection speed between mechanism derivation " program chain " between described " program chain ", by the scanning sequence queue chain, get that " program chain " junction is maximum to stop smaller value in increment and the maximum initial increment as connection speed, and respectively adjacent " program chain " attribute is revised, respectively initial increment or termination increment are modified to connection speed.

6. implement device by the dynamic prediction disposal route of the described small program segment of claim 1, it is characterized in that: based on isa bus, power by power supply (4), comprise: display (1), central controller (2), machine operation panel (3), axle control circuit (5) and interface circuit (6), wherein: central controller (2) is communicated by letter with display (1), machine operation panel (3), and communicate by letter with axle control circuit (5) and interface circuit (6) by isa bus, handling procedure is installed in the central controller (2), realizes the processing of dynamically looking forward to the prospect of small program segment;

Described handling procedure comprises trajectory planning, program segment queue management, program chain management, program chain queue management, program segment management, and program is called by the motion control master routine, and wherein: the program segment queue management is the interface of motion planning and task coordinate part; The program segment management is with the element conversion cost method data structure format in the program segment queue management; " program chain " management is " program chain " according to decision condition with the program segment cluster; Trajectory planning realizes that by calling " program chain " queue manager the speed planning of processing " program chain " in the processing is connected with speed; " program chain " queue management safeguards " program chain " formation, realizes management to job sequence in the mode of " program chain ", and calculates the connection speed between " program chain "; Described decision condition is: 1) have identical program segment speed is set; 2) when cutter passed through the angle of two program segments connections so that speed to be set, speed was no more than the setting value of device; 3) when cutter passed through the angle of two program segments connections so that speed to be set, error was no more than the setting value of device.

7. press the implement device of the dynamic prediction disposal route of the described small program segment of claim 6, it is characterized in that: " program chain " management process is specially: the program segment that at first reads from the program segment formation newly, if current " program chain " is empty, then with newly to program segment be that element forms " program chain ", otherwise take out the program segment of tail of the queue from current " program chain "; If the program segment that newly arrives and the program segment of tail of the queue do not satisfy three conditions of " program chain " identification decision, then with newly to program segment be that element forms " program chain ", identification decision processing end; If the program segment that newly arrives and the program segment of tail of the queue satisfy three conditions of " program chain " identification decision, and current " program chain " process as yet, then will be newly to program segment incorporate current " program chain " into, identification decision processing end; Otherwise the speed planning to current " program chain " is revised; If newly to program segment satisfies the speed correction conditions, with newly to program segment incorporate current " program chain " into, and the interpolation step number of " program chain " is adjusted, the identification decision processing finishes; Otherwise do not satisfy the speed correction conditions, with newly to program segment be that element forms " program chain ", finish identification and handle;

Described speed correction conditions is: the decision condition and current " program chain " that satisfy current " program chain " are processed as yet not to the decelerating phase.

8. press the implement device of the dynamic prediction disposal route of the described small program segment of claim 6, it is characterized in that: the trajectory planning program circuit is specially: take out head of the queue " program chain " from " program chain " formation, judge its current state; If should " program chain " not plan as yet, then call the connection speed between " program chain " queue manager calculating " program chain ", cook up the interpolation step number in each stage of motion and the coefficient of displacement increment function according to described dynamic prediction disposal route, the interpolation step number of initial motion planning and the state of mark current " program chain " calculate the displacement increment in each stage then respectively; Otherwise directly calculate the displacement increment in each stage respectively; If current motion is in boost phase, according to its its displacement increment of displacement increment function calculation; If current motion is in the at the uniform velocity stage, its displacement increment is a constant; If current motion is in the decelerating phase, according to its its displacement increment of displacement increment function calculation; And then calculate moving displacement by the displacement increment of corresponding sports state, and displacement conversion is become D coordinates value and output, add up the decision condition that the interpolation step number finishes as " program chain " simultaneously; If current " program chain " motion finishes, judge whether whole procedure finishes; If whole job sequence finishes, remove the motion formation, finish motion planning.

9 press the implement device of the dynamic prediction disposal route of the described small program segment of claim 6, it is characterized in that: " program chain " queue management flow process is specially: scanning " program chain " formation, and from the beginning recursion is calculated each " program chain " maximum increment that finishes backward; Backstepping calculates each " program chain " maximum initial increment from back to front then; Maximum initial increment between more adjacent at last " program chain " and the maximum increment that finishes are got its minimum value as the connection speed between " program chain ".

Images