CN104191428A - Movement path planning method and system based on SCARA - Google Patents

Abstract

The invention provides a movement path planning method and system based on an SCARA. The movement path planning method comprises the steps that an upper computer receives an operation instruction of a user, sets path planning modes of the SCARA and movement parameters corresponding to the path planning modes, and sends the path planning modes and the movement parameters to a movement controller of the SCARA; the movement controller performs operations on an actual movement path of the SCARA according to algorithms corresponding to the path planning modes to obtain actual movement path information of the SCARA and sends the actual movement path information to the SCARA; the SCARA receives the actual movement path information and moves according to the actual movement path information. The movement path planning method and system based on the SCARA have three different movement path algorithms of linear interpolation, circular interpolation and capture in flight and have the advantages of rapid response and high accuracy.

Description

A kind of motion trail planning method and system based on SCARA manipulator

Technical field

The present invention relates to manipulator control technical field, in particular a kind of motion trail planning method and system based on SCARA manipulator.

Background technology

Current, SCARA manipulator is widely used in the industrial circles such as electronics, automobile, plastics, food, and its Major Function has been carrying and assembly work.Along with complexity and the accuracy of processing technology constantly promote, SCARA manipulator on streamline often need to other industrial equipment work compounds, there will be unavoidably the danger bumping with barrier.Therefore, just seem particularly important to existing the movement locus of SCARA manipulator of barrier to plan in working range.

Trajectory planning (Path Planning) refers to the barrier condition of given environment, and starting point and impact point position, require to select a path from starting point to impact point, make SCARA manipulator can pass through safely, without collision all barriers.The path accuracy average out to 0.3mm of existing common SCARA manipulator, its path accuracy is not high as seen, and trajectory planning is consuming time also longer.

Therefore, prior art has yet to be improved and developed.

Summary of the invention

The technical problem to be solved in the present invention is, for the above-mentioned defect of prior art, a kind of motion trail planning method and system based on SCARA manipulator is provided, can effectively solves and adopt the path accuracy of prior art SCARA manipulator not high, the defect of planning time length consuming time.

The technical scheme that technical solution problem of the present invention adopts is as follows:

Based on a motion trail planning method for SCARA manipulator, wherein, described method comprises step:

A, host computer receive user's operational order, and the trajectory planning mode of SCARA manipulator and the kinematic parameter corresponding with described trajectory planning mode are set, and described trajectory planning mode and described kinematic parameter are sent to the motion controller of SCARA manipulator;

B, described motion controller carry out computing according to algorithm corresponding to described trajectory planning mode to the actual motion track of SCARA manipulator, obtain the actual motion trace information of SCARA manipulator, and described actual motion trace information is sent to SCARA manipulator;

C, described SCARA manipulator receive described actual motion trace information, and move according to described actual motion trace information.

The described motion trail planning method based on SCARA manipulator, wherein, the described trajectory planning mode in described steps A is that smooth track planning is caught in linear interpolation trajectory planning, circular interpolation trajectory planning and flight.

The described motion trail planning method based on SCARA manipulator, wherein, in described steps A, in the time that described trajectory planning mode is linear interpolation trajectory planning, described kinematic parameter comprises starting point coordinate, terminal point coordinate and interpolation precision value; In the time that described trajectory planning mode is circular interpolation trajectory planning, described kinematic parameter comprises starting point coordinate, terminal point coordinate, central coordinate of circle, circular interpolation accuracy value and circular interpolation direction; When described trajectory planning mode is that smooth track when planning is caught in flight, described kinematic parameter comprises starting point coordinate, trigger point coordinate, terminal point coordinate, triggering pulsewidth and smoothness.

The described motion trail planning method based on SCARA manipulator, wherein, specifically comprises in described step B:

B1, when described trajectory planning mode be linear interpolation trajectory planning, the DSP in described motion controller carries out computing according to linear interpolation algorithm to the actual motion track of SCARA manipulator, obtain the physical coordinates of multiple interpolated points in the actual motion track of SCARA manipulator, carry out afterwards B4;

B2, when described trajectory planning mode be circular interpolation trajectory planning, the DSP in described motion controller carries out computing according to arc interpolation to the actual motion track of SCARA manipulator, obtain the physical coordinates of multiple interpolated points in the actual motion track of SCARA manipulator, carry out afterwards B4;

B3, when described trajectory planning mode be that smooth track planning is caught in flight, the DSP in described motion controller catches smoothing algorithm according to flight the actual motion track of SCARA manipulator is carried out to computing, obtain the physical coordinates of multiple interpolated points in the actual motion track of SCARA manipulator, carry out afterwards B4;

B4, described DSP transform the physical coordinates of described multiple interpolated points the corner of the corresponding large forearm of SCARA manipulator, and calculate the speed of multiple interpolated points according to predetermined step curve acceleration and deceleration method;

Corner and the speed of the large forearm of SCARA manipulator corresponding described multiple interpolated points are sent to the motion control chip in described motion controller by B5, described DSP, and motion control chip is according to the corner of the large forearm of described SCARA manipulator and speed control SCARA robot movement.

The described motion trail planning method based on SCARA manipulator, wherein, described step B1 specifically comprises:

B11, described DSP obtain the starting point coordinate (X in described kinematic parameter ₀, Y ₀), terminal point coordinate (X _e, Y _e) and interpolation precision value Δ L, and respectively according to Δ X=X _e-X ₀with Δ Y=Y _e-Y ₀obtain horizontal spacing Δ X and the longitudinal pitch Δ Y of terminal point coordinate and starting point coordinate;

B12, judgement | whether Δ X| is more than or equal to | Δ Y|, in the time being more than or equal to, perform step B13, in the time being less than, perform step B14;

B13, according to n ₁=floor (| Δ X|/Δ L+1) calculating the first interpolation frequency n ₁, and according to Δ L _n1=Δ X/n ₁calculate current the first interpolation precision Δ L _n1, and according to Δ X _i=Δ L _n1determine the first horizontal increment Delta X _i, according to Δ Y _i=Δ L _n1* Δ Y/ Δ X determines first longitudinal increment Delta Y _i, according to X _i=X _i-1+ Δ X _idetermine n in actual motion track ₁the abscissa of individual interpolated point, according to Y _i=Y _i-1+ Δ Y _idetermine n in actual motion track ₁the ordinate of individual interpolated point; Wherein, floor function is downward bracket function, and i is that span is [1, n ₁] positive integer;

B14, according to n ₂=floor (| Δ Y|/Δ L+1) calculating the second interpolation frequency n ₂, and according to Δ L _n2=Δ Y/n ₂calculate current the second interpolation precision Δ L _n2, and according to Δ X _j=Δ L _n2* Δ X/ Δ Y determines the second horizontal increment Delta X _j, according to Δ Y _j=Δ L _n2determine second longitudinal increment Delta Y _j, according to X _j=X _j-1+ Δ X _jdetermine n in actual motion track ₂the abscissa of individual interpolated point, according to Y _j=Y _j-1+ Δ Y _jdetermine n in actual motion track ₂the ordinate of individual interpolated point; Wherein, floor function is downward bracket function, and j is that span is [1, n ₂] positive integer.

The described motion trail planning method based on SCARA manipulator, wherein, described step B2 specifically comprises:

B21, described DSP obtain the starting point coordinate (X in described kinematic parameter _a,, Y _a), terminal point coordinate (X _b, Y _b), central coordinate of circle (X _o, Y _o), circular interpolation accuracy value dfstep and circular interpolation direction;

B22, according to starting point coordinate (X _a,, Y _a), terminal point coordinate (X _b, Y _b), central coordinate of circle (X _o, Y _o) and circular interpolation orientation determination radius R, starting point angle terminal angle and terminal angle and starting point angle is poor and according to calculate circular interpolation times N, and according to calculate current circular interpolation precision Dfstep, according to determine the abscissa of N interpolated point in actual motion track, according to determine the ordinate of N interpolated point in actual motion track; Wherein, floor function is downward bracket function, and m is that span is the positive integer of [1, N].

A movement locus planning system based on SCARA manipulator, wherein, comprising:

Module is set, receive user's operational order for host computer, the trajectory planning mode of SCARA manipulator and the kinematic parameter corresponding with described trajectory planning mode are set, and described trajectory planning mode and described kinematic parameter are sent to the motion controller of SCARA manipulator;

Track obtains and sending module, according to algorithm corresponding to described trajectory planning mode, the actual motion track of SCARA manipulator is carried out to computing for described motion controller, obtain the actual motion trace information of SCARA manipulator, and described actual motion trace information is sent to SCARA manipulator;

Receive and motion module, receive described actual motion trace information for described SCARA manipulator, and move according to described actual motion trace information.

The described movement locus planning system based on SCARA manipulator, wherein, the described described trajectory planning mode arranging in module is that smooth track planning is caught in linear interpolation trajectory planning, circular interpolation trajectory planning and flight.

The described movement locus planning system based on SCARA manipulator, wherein, described track obtain and sending module in the time that described trajectory planning mode is linear interpolation trajectory planning, described kinematic parameter comprises starting point coordinate, terminal point coordinate and interpolation precision value; In the time that described trajectory planning mode is circular interpolation trajectory planning, described kinematic parameter comprises starting point coordinate, terminal point coordinate, central coordinate of circle, circular interpolation accuracy value and circular interpolation direction; When described trajectory planning mode is that smooth track when planning is caught in flight, described kinematic parameter comprises starting point coordinate, trigger point coordinate, terminal point coordinate, triggering pulsewidth and smoothness.

The described movement locus planning system based on SCARA manipulator, wherein, described track obtains and sending module specifically wraps linear interpolation unit, circular interpolation unit, smooth unit, tarnsition velocity acquiring unit and transmitting element are caught in flight, wherein:

Described linear interpolation unit, linear interpolation trajectory planning for working as described trajectory planning mode, the DSP in described motion controller carries out computing according to linear interpolation algorithm to the actual motion track of SCARA manipulator, the physical coordinates that obtains multiple interpolated points in the actual motion track of SCARA manipulator, starts described tarnsition velocity acquiring unit afterwards;

Described circular interpolation unit, circular interpolation trajectory planning for working as described trajectory planning mode, the DSP in described motion controller carries out computing according to arc interpolation to the actual motion track of SCARA manipulator, the physical coordinates that obtains multiple interpolated points in the actual motion track of SCARA manipulator, starts described tarnsition velocity acquiring unit afterwards;

Smooth unit is caught in described flight, that smooth track planning is caught in flight for working as described trajectory planning mode, the DSP in described motion controller catches smoothing algorithm according to flight the actual motion track of SCARA manipulator is carried out to computing, the physical coordinates that obtains multiple interpolated points in the actual motion track of SCARA manipulator, starts described tarnsition velocity acquiring unit afterwards;

Described tarnsition velocity acquiring unit, transforms the physical coordinates of described multiple interpolated points for described DSP the corner of the corresponding large forearm of SCARA manipulator, and calculates the speed of multiple interpolated points according to predetermined step curve acceleration and deceleration method;

Described transmitting element, the motion control chip that for described DSP, the corner of the large forearm of SCARA manipulator corresponding described multiple interpolated points and speed is sent to described motion controller, motion control chip is according to the corner of the large forearm of SCARA manipulator and speed control SCARA robot movement.

A kind of motion trail planning method and system based on SCARA manipulator provided by the present invention, method comprises: host computer receives user's operational order, the trajectory planning mode of SCARA manipulator and the kinematic parameter corresponding with described trajectory planning mode are set, and described trajectory planning mode and described kinematic parameter are sent to the motion controller of SCARA manipulator; Described motion controller carries out computing according to algorithm corresponding to described trajectory planning mode to the actual motion track of SCARA manipulator, obtains the actual motion trace information of SCARA manipulator, and described actual motion trace information is sent to SCARA manipulator; Described SCARA manipulator receives described actual motion trace information, and moves according to described actual motion trace information.The present invention has linear interpolation, circular interpolation, and flight catches three kinds of different motion Trajectory Arithmetics, calculates respectively movement locus interpolated point data separately, and each interpolated point packet, containing position data and speed data, has response fast, the high feature of precision.

Brief description of the drawings

Fig. 1 is the flow chart of the preferred embodiment of the motion trail planning method based on SCARA manipulator of the present invention.

Fig. 2 is the particular flow sheet that obtains actual motion trace information in the motion trail planning method based on SCARA manipulator of the present invention and send.

Fig. 3 is the flow chart that carries out the idiographic flow of linear interpolation in the motion trail planning method based on SCARA manipulator of the present invention.

Fig. 4 is linear interpolation schematic diagram of the present invention.

Fig. 5 is the flow chart that carries out the idiographic flow of circular interpolation in the motion trail planning method based on SCARA manipulator of the present invention.

Fig. 6 is circular interpolation schematic diagram of the present invention.

Fig. 7 a, Fig. 7 b are respectively top view and the side views of the manipulator of SCARA described in the present invention.

Fig. 8 is the schematic diagram of the large forearm angle of SCARA manipulator physical coordinates of the present invention.

Fig. 9 is two sections of movement locus figure of SCARA of the specific embodiment of the invention.

Figure 10 is corner partial enlarged drawing in Fig. 9.

Figure 11 is two sections of movement locus optimization figure of SCARA of the specific embodiment of the invention.

Figure 12 is the vector change algorithm figure of the movement locus of the specific embodiment of the invention.

Figure 13 is the structured flowchart of the movement locus planning system preferred embodiment based on SCARA manipulator of the present invention.

Detailed description of the invention

For making object of the present invention, technical scheme and advantage clearer, clear and definite, developing simultaneously referring to accompanying drawing, the present invention is described in more detail for embodiment.Should be appreciated that specific embodiment described herein, only in order to explain the present invention, is not intended to limit the present invention.

Refer to Fig. 1, Fig. 1 is the flow chart of the preferred embodiment of the motion trail planning method based on SCARA manipulator of the present invention.As shown in Figure 1, the described motion trail planning method based on SCARA manipulator, comprises the following steps:

Step S100, host computer receive user's operational order, the trajectory planning mode of SCARA manipulator and the kinematic parameter corresponding with described trajectory planning mode are set, and described trajectory planning mode and described kinematic parameter are sent to the motion controller of SCARA manipulator.

In embodiments of the invention, user arranges the trajectory planning mode of SCARA manipulator and the kinematic parameter corresponding with described trajectory planning mode by host computer, and send it to the motion controller of SCARA manipulator, namely tell SCARA manipulator where should walk and how this goes to terminal.

Step S200, described motion controller carry out computing according to algorithm corresponding to described trajectory planning mode to the actual motion track of SCARA manipulator, obtain the actual motion trace information of SCARA manipulator, and described actual motion trace information is sent to SCARA manipulator.

In step S200, the actual motion trace information of the SCARA manipulator obtaining comprises the physical coordinates of multiple interpolated points in actual motion track, physical coordinates---corresponding polar coordinates and speed with described multiple interpolated points.When described motion controller obtains after the actual motion trace information of SCARA manipulator, send it to SCARA manipulator.

Step S300, described SCARA manipulator receive described actual motion trace information, and move according to described actual motion trace information.

In embodiments of the invention, SCARA manipulator receives after described actual motion trace information, moves according to described actual motion trace information, has guaranteed the accurate motion in each moment of SCARA manipulator, simultaneously can effective avoiding obstacles.

In embodiments of the invention, need first judge described trajectory planning mode, in the time judging that described trajectory planning mode is linear interpolation trajectory planning, calculate the interpolated point of actual motion track by linear interpolation algorithm; In the time judging that inserting described trajectory planning mode is circular interpolation trajectory planning, calculate the interpolated point of actual motion track by arc interpolation; When judging that described trajectory planning mode is that smooth track when planning is caught in flight, calculates the interpolated point of actual motion track by smoothing algorithm.

Embodiment further, as shown in Figure 2, the idiographic flow that obtains actual motion trace information transmission in described step S200 comprises:

Step S201, when described trajectory planning mode be linear interpolation trajectory planning, the DSP in described motion controller carries out computing according to linear interpolation algorithm to the actual motion track of SCARA manipulator, the physical coordinates that obtains multiple interpolated points in the actual motion track of SCARA manipulator, performs step S204 afterwards.

Embodiment further, as shown in Figure 3, the idiographic flow that described step S201 carries out linear interpolation comprises:

Step S2011, described DSP obtain the starting point coordinate (X in described kinematic parameter ₀, Y ₀), terminal point coordinate (X _e, Y _e) and interpolation precision value Δ L, and respectively according to Δ X=X _e-X ₀with Δ Y=Y _e-Y ₀obtain horizontal spacing Δ X and the longitudinal pitch Δ Y of terminal point coordinate and starting point coordinate;

Step S2012, judgement | whether Δ X| is more than or equal to | Δ Y|, in the time being more than or equal to, perform step S2013, in the time being less than, perform step S2014;

Step S2013, according to n ₁=floor (| Δ X|/Δ L+1) calculating the first interpolation frequency n ₁, and according to Δ L _n1=Δ X/n ₁calculate current the first interpolation precision Δ L _n1, and according to Δ X _i=Δ L _n1determine the first horizontal increment Delta X _i, according to Δ Y _i=Δ L _n1* Δ Y/ Δ X determines first longitudinal increment Delta Y _i, according to X _i=X _i-1+ Δ X _idetermine n in actual motion track ₁the abscissa of individual interpolated point, according to Y _i=Y _i-1+ Δ Y _idetermine n in actual motion track ₁the ordinate of individual interpolated point; Wherein, floor function is downward bracket function, and i is that span is [1, n ₁] positive integer;

Step S2014, according to n ₂=floor (| Δ Y|/Δ L+1) calculating the second interpolation frequency n ₂, and according to Δ L _n2=Δ Y/n ₂calculate current the second interpolation precision Δ L _n2, and according to Δ X _j=Δ L _n2* Δ X/ Δ Y determines the second horizontal increment Delta X _j, according to Δ Y _j=Δ L _n2determine second longitudinal increment Delta Y _j, according to X _j=X _j-1+ Δ X _jdetermine n in actual motion track ₂the abscissa of individual interpolated point, according to Y _j=Y _j-1+ Δ Y _jdetermine n in actual motion track ₂the ordinate of individual interpolated point; Wherein, floor function is downward bracket function, and j is that span is [1, n ₂] positive integer.

In order to further illustrate the idiographic flow of above-mentioned linear interpolation, now illustrate by an example.As shown in Figure 4, it is linear interpolation schematic diagram of the present invention, supposes to boning out OA, and wherein O point coordinates is (X ₀, Y ₀), A point coordinates is (X _e, Y _e), moving some N _i-1the coordinate of (being interpolated point) is (X _i-1, Y _i-1), the horizontal spacing Δ X=X of OA distance _e-X ₀, longitudinal pitch Δ Y=Y _e-Y ₀, displacement increment largest component is Δ L, sets Δ L=0.1mm at this.

When | Δ X|>=| when Δ Y|, ask the first interpolation frequency n ₁, n ₁=floor (| Δ X|/Δ L+1), revise Δ L _n1=Δ X/n ₁, Δ X _i=Δ L _n1, Δ Y _i=Δ L _n1* Δ Y/ Δ X; Wherein, floor function is downward bracket function, and i is that span is [1, n ₁] positive integer.

When | Δ X| < | Δ Y|, asks the second interpolation frequency n ₂, n ₂=floor (| Δ Y|/Δ L+1), revise Δ L _n2=Δ Y/n ₂, Δ X _j=Δ L _n2* Δ X/ Δ Y, Δ Y _j=Δ L _n2; Wherein, floor function is downward bracket function, and j is that span is [1, n ₂] positive integer.

Can obtain thus next interpolated point N _i(X _i, Y _i) coordinate figure be (X _i-1+ Δ X, Y _i-1+ Δ Y).

Step S202, when described trajectory planning mode be circular interpolation trajectory planning, the DSP in described motion controller carries out computing according to arc interpolation to the actual motion track of SCARA manipulator, obtain the physical coordinates of multiple interpolated points in the actual motion track of SCARA manipulator, carry out afterwards S204.

Embodiment further, as shown in Figure 5, the idiographic flow that described step S202 carries out circular interpolation comprises:

Step S2021, described DSP obtain the starting point coordinate (X in described kinematic parameter _a,, Y _a), terminal point coordinate (X _b, Y _b), central coordinate of circle (X _o, Y _o), circular interpolation accuracy value dfstep and circular interpolation direction;

Step S2022, according to starting point coordinate (X _a,, Y _a), terminal point coordinate (X _b, Y _b), central coordinate of circle (X _o, Y _o) and circular interpolation orientation determination radius R, starting point angle terminal angle and terminal angle and starting point angle is poor and according to calculate circular interpolation times N, and according to calculate current circular interpolation precision Dfstep, according to determine the abscissa of N interpolated point in actual motion track, according to determine the ordinate of N interpolated point in actual motion track; Wherein, floor function is downward bracket function, and m is that span is the positive integer of [1, N].

In order to further illustrate the idiographic flow of above-mentioned circular interpolation, now illustrate by an example.As shown in Figure 6, it is circular interpolation schematic diagram of the present invention, supposes to be provided with circular arc, and A (X _a, Y _a) and B (X _b, Y _b) be two points on circular arc, known A point, B point, and center of circle O point (X _o, Y _o).Move from A point to B point in the mode of circular interpolation (dfstep=0.1 radian), need to consider A, 2 quadrants of living in rectangular coordinate system of B, when Practical Calculation, be divided into following four kinds of situations:

(1) in the time that A, B are positioned at first quartile:

The angle that starting point A is ordered is ${\mathrm{\&phi;}}_a=\arctan \left(\frac{Y_a-Y_o}{X_a-X_o}\right)*180/\mathrm{pi};$

The angle that terminal B is ordered is ${\mathrm{\&phi;}}_b=\arctan \left(\frac{Y_b-Y_o}{X_b-X_o}\right)*180/\mathrm{pi};$

(2) in the time that A, B are positioned at the second quadrant:

The angle that starting point A is ordered is ${\mathrm{\&phi;}}_a=180+\arctan \left(\frac{Y_a-Y_o}{X_a-X_o}\right)*180/\mathrm{pi};$

The angle that terminal B is ordered is ${\mathrm{\&phi;}}_b=180+\arctan \left(\frac{Y_b-Y_o}{X_b-X_o}\right)*180/\mathrm{pi};$

(3) in the time that A, B are positioned at third quadrant:

The angle that starting point A is ordered is ${\mathrm{\&phi;}}_a=180+\arctan \left(\frac{Y_a-Y_o}{X_a-X_o}\right)*180/\mathrm{pi};$

The angle that terminal B is ordered is ${\mathrm{\&phi;}}_b=180+\arctan \left(\frac{Y_b-Y_o}{X_b-X_o}\right)*180/\mathrm{pi};$

(4) in the time that A, B are positioned at fourth quadrant:

The angle that starting point A is ordered is ${\mathrm{\&phi;}}_a=360+\arctan \left(\frac{Y_a-Y_o}{X_a-X_o}\right)*180/\mathrm{pi};$

The angle that terminal B is ordered is ${\mathrm{\&phi;}}_b=360+\arctan \left(\frac{Y_b-Y_o}{X_b-X_o}\right)*180/\mathrm{pi};$

In the time of arbitrary quadrant in above-mentioned quadrant of A, B, its radius is:

$R=\sqrt{{(X_a-X_o)}^2+{(Y_a-Y_o)}^2};$

In the time that A is clockwise direction circular arc to the circular arc of B, and φ _b-φ _awhen < 0, Δ φ=φ _b-φ _a; In the time that A is clockwise direction circular arc to the circular arc of B, and φ _b-φ _awhen > 0, Δ φ=-(360-φ _b+ φ _a); In the time that A is counter clockwise direction circular arc to the circular arc of B, and φ _b-φ _awhen > 0, Δ φ=φ _b-φ _a; In the time that A is counter clockwise direction circular arc to the circular arc of B, and φ _b-φ _awhen < 0, Δ φ=360+ φ _b-φ _a.

When having obtained starting point coordinate (X _a,, Y _a), terminal point coordinate (X _b,, Y _b), central coordinate of circle (X _o, Y _o), radius R, starting point angle terminal angle and terminal angle and starting point angle is poor after, can be by the interpolation number of times of needs calculate circular interpolation times N.Now need to revise and according to determine the abscissa of N interpolated point in actual motion track, according to determine the ordinate of N interpolated point in actual motion track; Wherein, floor function is downward bracket function, and m is that span is the positive integer of [1, N].

Step S203, when described trajectory planning mode be that smooth track planning is caught in flight, the DSP in described motion controller catches smoothing algorithm according to flight the actual motion track of SCARA manipulator is carried out to computing, the physical coordinates that obtains multiple interpolated points in the actual motion track of SCARA manipulator, performs step S204 afterwards;

Step S204, described DSP transform the physical coordinates of described multiple interpolated points the corner of the corresponding large forearm of SCARA manipulator, and calculate the speed of multiple interpolated points according to predetermined step curve acceleration and deceleration method.

In embodiments of the invention, as shown in Fig. 7 a and Fig. 7 b, it is respectively top view and the side view of the manipulator of SCARA described in the present invention, and Fig. 7 a is placed in plane right-angle coordinate by SCARA and has marked SCARA manipulator large arm brachium L1 and forearm brachium L2, the large spacing scope (A2 of arm, and the spacing scope of forearm (A4, A3) A1).

In step S204, need the physical coordinates of the multiple interpolated points that obtain by interpolation algorithm to be converted into polar coordinates, be also converted into the corner of the large forearm of SCARA manipulator.Be converted into polar idiographic flow in order to further illustrate above-mentioned physical coordinates, now illustrate by an example.

As shown in Figure 8, the schematic diagram of its large forearm angle that is SCARA manipulator physical coordinates of the present invention.As shown in Figure 8, known A, B 2 points, SCARA manipulator need to run to B point from A point, the brachium of its large arm, forearm is respectively L1, L2, when the forearm end of SCARA is during in starting point A, the large initial corner ∠ of arm XOI, the initial corner of forearm are ∠ FIA, are positive direction counterclockwise in this definition,, under right arm pattern, large arm angle is that ∠ XOJ, forearm angle are ∠ GJB; Under left arm pattern, large arm angle is that ∠ XOK, forearm angle are ∠ HKB.In Fig. 8 B1, B2, B3 and B4 represent respectively terminal point coordinate B at first quartile the identifier to fourth quadrant, in like manner G1, G2, G3 and G4 respectively denotation coordination point G at first quartile the identifier to fourth quadrant, H1, H2, H3 and H4 difference denotation coordination point H be the identifier to fourth quadrant at first quartile, J1, J2, J3 and J4 difference denotation coordination point J be the identifier to fourth quadrant at first quartile, and K1, K2, K3 and K4 difference denotation coordination point K be the identifier to fourth quadrant at first quartile.

In embodiments of the invention, determine that according to physical coordinates the first step of the corner of large forearm is according to the judgement of ∠ FIA angle, current left arm or the right arm state of being in, ∠ FIA angle can directly be read by location counter.In the time that IA line is positioned at FI line right side, be right arm pattern, be left arm pattern in the time that IA line is positioned at FI line left side.Second step is according to B point position (X _b, Y _b) and new left and right arms mode computation pI=3.1415926, will be divided into B in four kinds of situations of first quartile-fourth quadrant in calculating formula:

(a) when B point is during at first quartile:

Right arm pattern:

$\left\{\begin{array}{c}\&angle;G_1{J}_1{B}_1=-(\arccos \left(\frac{{L1}^2+{L3}^2-{L2}^2}{2*L1*L3}\right)*180/\mathrm{PI}+\arccos \left(\frac{{L2}^2+{L3}^2-{L1}^2}{2*L2*L3}\right)*180/\mathrm{PI})\\ \&angle;J_1\mathrm{OX}=\arctan \left(\frac{y_b}{x_b}\right)*180/\mathrm{PI}+\arccos \left(\frac{{L1}^2+{L3}^2-{L2}^2}{2*L1*L3}\right)*180/\mathrm{PI}\end{array}\right.$

Left arm pattern:

$\left\{\begin{array}{c}\&angle;H_1{K}_1{B}_1=\arccos \left(\frac{{L1}^2+{L3}^2-{L2}^2}{2*L1*L3}\right)*180/\mathrm{PI}+\arccos \left(\frac{{L2}^2+{L3}^2-{L1}^2}{2*L2*L3}\right)*180/\mathrm{PI}\\ \&angle;H_1\mathrm{OX}=\arctan \left(\frac{y_b}{x_b}\right)*180/\mathrm{PI}-\arccos \left(\frac{{L1}^2+{L3}^2-{L2}^2}{2*L1*L3}\right)*180/\mathrm{PI}\end{array}\right.$

(b) when B o'clock during at the second quadrant:

Right arm pattern:

$\left\{\begin{array}{c}\&angle;G_2{J}_2{B}_2=-(\arccos \left(\frac{{L1}^2+{L3}^2-{L2}^2}{2*L1*L3}\right)*180/\mathrm{PI}+\arccos \left(\frac{{L2}^2+{L3}^2-{L1}^2}{2*L2*L3}\right)*180/\mathrm{PI})\\ \&angle;J_2\mathrm{OX}=180+\arctan \left(\frac{y_b}{x_b}\right)*180/\mathrm{PI}+\arccos \left(\frac{{L1}^2+{L3}^2-{L2}^2}{2*L1*L3}\right)*180/\mathrm{PI}\end{array}\right.$

Left arm pattern:

$\left\{\begin{array}{c}\&angle;H_2{K}_2{B}_2=\arccos \left(\frac{{L1}^2+{L3}^2-{L2}^2}{2*L1*L3}\right)*180/\mathrm{PI}+\arccos \left(\frac{{L2}^2+{L3}^2-{L1}^2}{2*L2*L3}\right)*180/\mathrm{PI}\\ \&angle;H_2\mathrm{OX}=180+\arctan \left(\frac{y_b}{x_b}\right)*180/\mathrm{PI}-\arccos \left(\frac{{L1}^2+{L3}^2-{L2}^2}{2*L1*L3}\right)*180/\mathrm{PI}\end{array}\right.$

(c) when B point is during at third quadrant:

Right arm pattern:

$\left\{\begin{array}{c}\&angle;G_3{J}_3{B}_3=-(\arccos \left(\frac{{L1}^2+{L3}^2-{L2}^2}{2*L1*L3}\right)*180/\mathrm{PI}+\arccos \left(\frac{{L2}^2+{L3}^2-{L1}^2}{2*L2*L3}\right)*180/\mathrm{PI})\\ \&angle;J_3\mathrm{OX}=180+\arctan \left(\frac{y_b}{x_b}\right)*180/\mathrm{PI}+\arccos \left(\frac{{L1}^2+{L3}^2-{L2}^2}{2*L1*L3}\right)*180/\mathrm{PI}\end{array}\right.$

Left arm pattern:

$\left\{\begin{array}{c}\&angle;H_3{K}_3{B}_3=\arccos \left(\frac{{L1}^2+{L3}^2-{L2}^2}{2*L1*L3}\right)*180/\mathrm{PI}+\arccos \left(\frac{{L2}^2+{L3}^2-{L1}^2}{2*L2*L3}\right)*180/\mathrm{PI}\\ \&angle;H_3\mathrm{OX}=180+\arctan \left(\frac{y_b}{x_b}\right)*180/\mathrm{PI}-\arccos \left(\frac{{L1}^2+{L3}^2-{L2}^2}{2*L1*L3}\right)*180/\mathrm{PI}\end{array}\right.$

(d) when B point is during in fourth quadrant:

Right arm pattern:

$\left\{\begin{array}{c}\&angle;G_4{J}_4{B}_4=-(\arccos \left(\frac{{L1}^2+{L3}^2-{L2}^2}{2*L1*L3}\right)*180/\mathrm{PI}+\arccos \left(\frac{{L2}^2+{L3}^2-{L1}^2}{2*L2*L3}\right)*180/\mathrm{PI})\\ \&angle;J_4\mathrm{OX}=\arctan \left(\frac{y_b}{x_b}\right)*180/\mathrm{PI}+\arccos \left(\frac{{L1}^2+{L3}^2-{L2}^2}{2*L1*L3}\right)*180/\mathrm{PI}\end{array}\right.$

Left arm pattern:

$\left\{\begin{array}{c}\&angle;H_4{K}_4{B}_4=\arccos \left(\frac{{L1}^2+{L3}^2-{L2}^2}{2*L1*L3}\right)*180/\mathrm{PI}+\arccos \left(\frac{{L2}^2+{L3}^2-{L1}^2}{2*L2*L3}\right)*180/\mathrm{PI}\\ \&angle;H_4\mathrm{OX}=\arctan \left(\frac{y_b}{x_b}\right)*180/\mathrm{PI}-\arccos \left(\frac{{L1}^2+{L3}^2-{L2}^2}{2*L1*L3}\right)*180/\mathrm{PI}\end{array}\right.$

Wherein, the large arm angle in fourth quadrant is negative angle, has spacing (0, A1) at third quadrant SCARA, cannot directly forward fourth quadrant to by third quadrant, must be through the second quadrant position, then first quartile position arrives.

From above-mentioned condition, if large arm forearm move angle is Δ α, Δ β:

Right arm pattern: $\left\{\begin{array}{c}\mathrm{\&Delta;\&alpha;}=\&angle;G_n\mathrm{OX}-\&angle;\mathrm{FOX}\\ \mathrm{\&Delta;\&beta;}=\&angle;G_n{J}_n{B}_n-\&angle;\mathrm{FIA}\end{array}\right.,(n=\mathrm{1,2,3,4})$

Left arm pattern: $\left\{\begin{array}{c}\mathrm{\&Delta;\&alpha;}=\&angle;H_n\mathrm{OX}-\&angle;\mathrm{FOX}\\ \mathrm{\&Delta;\&beta;}=\&angle;H_n{K}_n{B}_n-\&angle;\mathrm{FIA}\end{array}\right.,(n=\mathrm{1,2,3,4})$

Obtain the anglec of rotation of ordering with respect to A, if B point at first quartile; n=1; N=2 of the second quadrant; Third quadrant is n=3; Fourth quadrant is n=4; Δ α, Δ β is on the occasion of being rotated counterclockwise, for negative value turns clockwise.

In step S204, except the physical coordinates of the multiple interpolated points in actual motion track being converted into the large forearm corner of SCARA manipulator, also need to obtain the speed at each interpolated point place.In embodiments of the invention, calculate the speed of multiple interpolated points by a predetermined step curve acceleration and deceleration method, this algorithm is based on PCL6045 motion control chip, use three grades of caching functions on chip, while writing an interpolation position, can fill in the speed of this interpolation displacement at every turn simultaneously.Little line segment structure information is as follows:

typedef?struct

{

LgInt xl; //x deposits magnifying arm relative position, has been converted into number of pulses

LgInt yl; //y deposits forearm relative position, has been converted into number of pulses

LgInt speed; // speed

}TRunCode；

Acceleration and deceleration step number, by setting parameter (accnum, decnum), can calculate every speed that little line segment is corresponding.When acceleration, each speed value added is accvalue=(MaxVel-StrVel)/accnum; When deceleration, each speed minimizing value is decvalue=(MaxVel-StrVel)/decnum; Wherein MaxVel represents maximal rate, and StrVel represents starting velocity.If total little line segment has m bar, under current little line segment, be designated as n:

When, when n+decnum > m, slow down, V _n=V _n-1-decvalue, is decelerated to initial velocity;

When, when n+decnum <=m, and n < accnum, accelerate V _n=V _n-1+ accvalue, accelerates to maximal rate;

In other situations, be at the uniform velocity, and V _n=V _n-1.

Corner and the speed of the large forearm of SCARA manipulator corresponding described multiple interpolated points are sent to the motion control chip in described motion controller by step S205, described DSP, and motion control chip is according to the corner of the large forearm of described SCARA manipulator and speed control SCARA robot movement.

With a specific embodiment, the flight capture movement method for planning track based on SCARA manipulator of the present invention is described below.

The predetermined path of movement of supposing SCARA manipulator is two sections of movement locus of SCARA as shown in Figure 9, Figure 10 is the corner partial enlarged drawing of Fig. 9, between two track AB and BC, there is turning point as can be seen from Figure 9, in the time of operation, can there is large corner in SCARA, make machine produce vibrations, mechanical component is produced to wearing and tearing.The track that then SCARA orders to C through B trigger point from A point.What basic motion control chip adopted is interpolation mode operation, be that A moves according to linear interpolation mode to B point motion control chip, but it is upper for track is as shown in Figure 9 (due to SCARA frame for movement to embody SCARA, motion control chip adopts interpolation mode operation, but movement locus is but curve).As requested, at B point, SCARA reruns after can not stopping.If do not carry out smoothing algorithm, just there will be large corner, there will be mechanical oscillation at B point, the track after optimizing is as shown in figure 11.

Refer to Figure 12, the vector change algorithm figure that it is movement locus.AB, BC straight line are subdivided into according to speed planning, are divided into T=8 section in this acquiescence, every period is the unit interval, and increasing T can increase the smoothness of track.In the time of A point, direction speed scalar is V _max, direction speed scalar is 0, in the time of C point, direction speed scalar is V _max. direction speed scalar is reduced to 0 gradually; direction speed scalar is increased to gradually as V _max.According to displacement formula S=aT ²/ 2, can ask acceleration a=2 × S/T ²,

S _A1A2＝0.5*a*T ₈ ²-0.5*a*T ₇ ²；

S _A2A3＝0.5*a*T ₇ ²-0.5*a*T ₆ ²；

S _A3A4＝0.5*a*T ₆ ²-0.5*a*T ₅ ²；

S _A4A5＝0.5*a*T ₅ ²-0.5*a*T ₄ ²；

S _A5A6＝0.5*a*T ₄ ²-0.5*a*T ₃ ²；

S _A6A7＝0.5*a*T ₃ ²-0.5*a*T ₂ ²；

S _A7A8＝0.5*a*T ₂ ²-0.5*a*T ₁ ²；

S _a8A9=0.5*a*T ₁ ², acceleration direction is contrary with direction of displacement herein;

S _C8C9＝0.5*a*T ₈ ²-0.5*a*T ₇ ²；

S _C7C8＝0.5*a*T ₇ ²-0.5*a*T ₆ ²；

S _C6C7＝0.5*a*T ₆ ²-0.5*a*T ₅ ²；

S _C5C6＝0.5*a*T ₅ ²-0.5*a*T ₄ ²；

S _C4C5＝0.5*a*T ₄ ²-0.5*a*T ₃ ²；

S _C3C4＝0.5*a*T ₃ ²-0.5*a*T ₂ ²；

S _C2C3＝0.5*a*T ₂ ²-0.5*a*T ₁ ²；

S _c1C2=0.5*a*T ₁ ², acceleration direction is identical with direction of displacement herein;

T in formula _i=i, i ∈ [1,8], due to so coordinate points $P_2=\stackrel{\&RightArrow;}{A_1{A}_2}+\stackrel{\&RightArrow;}{C_1{C}_2}+P_1$

In like manner:

$P_3=\stackrel{\&RightArrow;}{A_2{A}_3}+\stackrel{\&RightArrow;}{C_2{C}_3}+P_2;$

$P_4=\stackrel{\&RightArrow;}{A_3{A}_4}+\stackrel{\&RightArrow;}{C_3{C}_4}+P_3;$

$P_5=\stackrel{\&RightArrow;}{A_4{A}_5}+\stackrel{\&RightArrow;}{C_4{C}_5}+P_4;$

$P_6=\stackrel{\&RightArrow;}{A_5{A}_6}+\stackrel{\&RightArrow;}{C_5{C}_6}+P_5;$

$P_7=\stackrel{\&RightArrow;}{A_6{A}_7}+\stackrel{\&RightArrow;}{C_6{C}_7}+P_6;$

$P_8=\stackrel{\&RightArrow;}{A_7{A}_8}+\stackrel{\&RightArrow;}{C_7{C}_8}+P_7;$

So can calculate P _mpoint position,, is subdivided into by P to two sections of tracks of BC by AB ₁, P ₂, P ₃, P ₄, P ₅, P ₆, P ₇, P ₈, P ₉the track of nine some compositions.The analog track figure being obtained from Figure 12, can realize seamlessly transitting of angle, obtain after coordinate, can to polar conversion method, physical coordinates be converted to large forearm corner data according to physical coordinates in the present invention, and then send in the middle of motion control chip.

Based on above-described embodiment, the present invention also provides a kind of movement locus planning system based on SCARA manipulator, and as shown in figure 13, the described movement locus planning system based on SCARA manipulator, comprising:

Module 100 is set, receive user's operational order for host computer, the trajectory planning mode of SCARA manipulator and the kinematic parameter corresponding with described trajectory planning mode are set, and described trajectory planning mode and described kinematic parameter are sent to the motion controller of SCARA manipulator; As detailed above.

Track obtains and sending module 200, according to algorithm corresponding to described trajectory planning mode, the actual motion track of SCARA manipulator is carried out to computing for described motion controller, obtain the actual motion trace information of SCARA manipulator, and described actual motion trace information is sent to SCARA manipulator; As detailed above.

Receive and motion module 300, receive described actual motion trace information for described SCARA manipulator, and move according to described actual motion trace information; As detailed above.

When concrete enforcement, module 100 is set and is arranged in host computer; Track obtains and sending module 200 is arranged in motion controller, and wherein motion controller mainly comprises dsp processor and motion control chip PCL6045BL; Reception and motion module 300 are arranged in SCARA manipulator.

Embodiment further, in the described movement locus planning system based on SCARA manipulator, the described described trajectory planning mode arranging in module is that smooth track planning is caught in linear interpolation trajectory planning, circular interpolation trajectory planning and flight; As detailed above.

Embodiment further, in the described movement locus planning system based on SCARA manipulator, described track obtain and sending module in the time that described trajectory planning mode is linear interpolation trajectory planning, described kinematic parameter comprises starting point coordinate, terminal point coordinate and interpolation precision value; In the time that described trajectory planning mode is circular interpolation trajectory planning, described kinematic parameter comprises starting point coordinate, terminal point coordinate, central coordinate of circle, circular interpolation accuracy value and circular interpolation direction; When described trajectory planning mode is that smooth track when planning is caught in flight, described kinematic parameter comprises starting point coordinate, trigger point coordinate, terminal point coordinate, triggering pulsewidth and smoothness.

Embodiment further; in the described movement locus planning system based on SCARA manipulator; described track obtains and sending module 200 specifically comprises that linear interpolation unit, circular interpolation unit, flight catches smooth unit, tarnsition velocity acquiring unit and transmitting element, wherein:

Described linear interpolation unit, linear interpolation trajectory planning for working as described trajectory planning mode, the DSP in described motion controller carries out computing according to linear interpolation algorithm to the actual motion track of SCARA manipulator, the physical coordinates that obtains multiple interpolated points in the actual motion track of SCARA manipulator, starts described tarnsition velocity acquiring unit afterwards; As detailed above.

Described circular interpolation unit, circular interpolation trajectory planning for working as described trajectory planning mode, the DSP in described motion controller carries out computing according to arc interpolation to the actual motion track of SCARA manipulator, the physical coordinates that obtains multiple interpolated points in the actual motion track of SCARA manipulator, starts described tarnsition velocity acquiring unit afterwards; As detailed above.

Smooth unit is caught in described flight, that smooth track planning is caught in flight for working as described trajectory planning mode, the DSP in described motion controller catches smoothing algorithm according to flight the actual motion track of SCARA manipulator is carried out to computing, the physical coordinates that obtains multiple interpolated points in the actual motion track of SCARA manipulator, starts described tarnsition velocity acquiring unit afterwards; As detailed above.

Described tarnsition velocity acquiring unit, transforms the physical coordinates of described multiple interpolated points for described DSP the corner of the corresponding large forearm of SCARA manipulator, and calculates the speed of multiple interpolated points according to predetermined step curve acceleration and deceleration method; As detailed above.

Described transmitting element, the motion control chip that for described DSP, the corner of the large forearm of SCARA manipulator corresponding described multiple interpolated points and speed is sent to described motion controller, motion control chip is according to the corner of the large forearm of described SCARA manipulator and speed control SCARA robot movement; As detailed above.

Embodiment further, in the described movement locus planning system based on SCARA manipulator, described linear interpolation unit specifically comprises that first obtains subelement, the first judgment sub-unit, the first linear interpolation subelement and the second linear interpolation subelement, wherein:

First obtains subelement, obtains the starting point coordinate (X of described kinematic parameter for described DSP ₀, Y ₀), terminal point coordinate (X _e, Y _e) and interpolation precision value Δ L, and respectively according to Δ X=X _e-X ₀with Δ Y=Y _e-Y ₀obtain horizontal spacing Δ X and the longitudinal pitch Δ Y of terminal point coordinate and starting point coordinate; As detailed above.

The first judgment sub-unit, for judgement | whether Δ X| is more than or equal to | and Δ Y| starts the first linear interpolation subelement, the second linear interpolation subelement in the time being less than in the time being more than or equal to; As detailed above.

The first linear interpolation subelement, for according to n ₁=floor (| Δ X|/Δ L+1) calculating the first interpolation frequency n ₁, and according to Δ L _n1=Δ X/n ₁calculate current the first interpolation precision Δ L _n1, and according to Δ X _i=Δ L _n1determine the first horizontal increment Delta X _i, according to Δ Y _i=Δ L _n1* Δ Y/ Δ X determines first longitudinal increment Delta Y _i, according to X _i=X _i-1+ Δ X _idetermine n in actual motion track ₁the abscissa of individual interpolated point, according to Y _i=Y _i-1+ Δ Y _idetermine n in actual motion track ₁the ordinate of individual interpolated point; Wherein, floor function is downward bracket function, and i is that span is [1, n ₁] positive integer; As detailed above.

The second linear interpolation subelement, for according to n ₂=floor (| Δ Y|/Δ L+1) calculating the second interpolation frequency n ₂, and according to Δ L _n2=Δ Y/n ₂calculate current the second interpolation precision Δ L _n2, and according to Δ X _j=Δ L _n2* Δ X/ Δ Y determines the second horizontal increment Delta X _j, according to Δ Y _j=Δ L _n2determine second longitudinal increment Delta Y _j, according to X _j=X _j-1+ Δ X _jdetermine n in actual motion track ₂the abscissa of individual interpolated point, according to Y _j=Y _j-1+ Δ Y _jdetermine n in actual motion track ₂the ordinate of individual interpolated point; Wherein, floor function is downward bracket function, and j is that span is [1, n ₂] positive integer; As detailed above.

Embodiment further, in the described movement locus planning system based on SCARA manipulator, described circular interpolation unit specifically comprises that second obtains subelement and circular interpolation subelement, wherein:

Described second obtains subelement, obtains the starting point coordinate (X of described kinematic parameter for described DSP _a,, Y _a), terminal point coordinate (X _b, Y _b), central coordinate of circle (X _o, Y _o), circular interpolation accuracy value dfstep and circular interpolation direction; As detailed above.

Described circular interpolation subelement, for according to starting point coordinate (X _a,, Y _a), terminal point coordinate (X _b, Y _b), central coordinate of circle (X _o, Y _o) and circular interpolation orientation determination radius R, starting point angle and terminal angle and terminal angle and starting point angle is poor and according to calculate circular interpolation times N, and according to calculate current circular interpolation precision Dfstep, according to determine the abscissa of N interpolated point in actual motion track, according to determine the ordinate of N interpolated point in actual motion track; Wherein, floor function is downward bracket function, and m is that span is the positive integer of [1, N]; As detailed above.

In sum, a kind of motion trail planning method and system based on SCARA manipulator provided by the present invention, method comprises: host computer receives user's operational order, the trajectory planning mode of SCARA manipulator and the kinematic parameter corresponding with described trajectory planning mode are set, and described trajectory planning mode and described kinematic parameter are sent to the motion controller of SCARA manipulator; Described motion controller carries out computing according to algorithm corresponding to described trajectory planning mode to the actual motion track of SCARA manipulator, obtains the actual motion trace information of SCARA manipulator, and described actual motion trace information is sent to SCARA manipulator; Described SCARA manipulator receives described actual motion trace information, and moves according to described actual motion trace information.The present invention has linear interpolation, circular interpolation, and flight catches three kinds of different motion Trajectory Arithmetics, calculates respectively movement locus interpolated point data separately, and each interpolated point packet, containing position data and speed data, has response fast, the high feature of precision.

Should be understood that, application of the present invention is not limited to above-mentioned giving an example, and for those of ordinary skills, can be improved according to the above description or convert, and all these improvement and conversion all should belong to the protection domain of claims of the present invention.

Claims (10)

1. the motion trail planning method based on SCARA manipulator, is characterized in that, described method comprises step:

A, host computer receive user's operational order, and the trajectory planning mode of SCARA manipulator and the kinematic parameter corresponding with described trajectory planning mode are set, and described trajectory planning mode and described kinematic parameter are sent to the motion controller of SCARA manipulator;

B, described motion controller carry out computing according to algorithm corresponding to described trajectory planning mode to the actual motion track of SCARA manipulator, obtain the actual motion trace information of SCARA manipulator, and described actual motion trace information is sent to SCARA manipulator;

C, described SCARA manipulator receive described actual motion trace information, and move according to described actual motion trace information.

2. the motion trail planning method based on SCARA manipulator according to claim 1, is characterized in that, the described trajectory planning mode in described steps A is that smooth track planning is caught in linear interpolation trajectory planning, circular interpolation trajectory planning and flight.

3. the motion trail planning method based on SCARA manipulator according to claim 2, it is characterized in that, in described steps A, in the time that described trajectory planning mode is linear interpolation trajectory planning, described kinematic parameter comprises starting point coordinate, terminal point coordinate and interpolation precision value; In the time that described trajectory planning mode is circular interpolation trajectory planning, described kinematic parameter comprises starting point coordinate, terminal point coordinate, central coordinate of circle, circular interpolation accuracy value and circular interpolation direction; When described trajectory planning mode is that smooth track when planning is caught in flight, described kinematic parameter comprises starting point coordinate, trigger point coordinate, terminal point coordinate, triggering pulsewidth and smoothness.

4. the motion trail planning method based on SCARA manipulator according to claim 3, is characterized in that, in described step B, specifically comprises:

B1, when described trajectory planning mode be linear interpolation trajectory planning, the DSP in described motion controller carries out computing according to linear interpolation algorithm to the actual motion track of SCARA manipulator, obtain the physical coordinates of multiple interpolated points in the actual motion track of SCARA manipulator, carry out afterwards B4;

B2, when described trajectory planning mode be circular interpolation trajectory planning, the DSP in described motion controller carries out computing according to arc interpolation to the actual motion track of SCARA manipulator, obtain the physical coordinates of multiple interpolated points in the actual motion track of SCARA manipulator, carry out afterwards B4;

B3, when described trajectory planning mode be that smooth track planning is caught in flight, the DSP in described motion controller catches smoothing algorithm according to flight the actual motion track of SCARA manipulator is carried out to computing, obtain the physical coordinates of multiple interpolated points in the actual motion track of SCARA manipulator, carry out afterwards B4;

B4, described DSP are converted into the physical coordinates of described multiple interpolated points the corner of the corresponding large forearm of SCARA manipulator, and calculate the speed of multiple interpolated points according to predetermined step curve acceleration and deceleration method;

Corner and the speed of the large forearm of SCARA manipulator corresponding described multiple interpolated points are sent to the motion control chip in described motion controller by B5, described DSP, and motion control chip is according to the corner of the large forearm of described SCARA manipulator and speed control SCARA robot movement.

5. the motion trail planning method based on SCARA manipulator according to claim 4, is characterized in that, described step B1 specifically comprises:

B11, described DSP obtain the starting point coordinate (X in described kinematic parameter ₀, Y ₀), terminal point coordinate (X _e, Y _e) and interpolation precision value Δ L, and respectively according to Δ X=X _e-X ₀with Δ Y=Y _e-Y ₀obtain horizontal spacing Δ X and the longitudinal pitch Δ Y of terminal point coordinate and starting point coordinate;

B12, judgement | whether Δ X| is more than or equal to | Δ Y|, in the time being more than or equal to, perform step B13, in the time being less than, perform step B14;

B13, according to n ₁=floor (| Δ X|/Δ L+1) calculating the first interpolation frequency n ₁, and according to Δ L _n1=Δ X/n ₁calculate current the first interpolation precision Δ L _n1, and according to Δ X _i=Δ L _n1determine the first horizontal increment Delta X _i, according to Δ Y _i=Δ L _n1* Δ Y/ Δ X determines first longitudinal increment Delta Y _i, according to X _i=X _i-1+ Δ X _idetermine n in actual motion track ₁the abscissa of individual interpolated point, according to Y _i=Y _i-1+ Δ Y _idetermine n in actual motion track ₁the ordinate of individual interpolated point; Wherein, floor function is downward bracket function, and i is that span is [1, n ₁] positive integer;

B14, according to n ₂=floor (| Δ Y|/Δ L+1) calculating the second interpolation frequency n ₂, and according to Δ L _n2=Δ Y/n ₂calculate current the second interpolation precision Δ L _n2, and according to Δ X _j=Δ L _n2* Δ X/ Δ Y determines the second horizontal increment Delta X _j, according to Δ Y _j=Δ L _n2determine second longitudinal increment Delta Y _j, according to X _j=X _j-1+ Δ X _jdetermine n in actual motion track ₂the abscissa of individual interpolated point, according to Y _j=Y _j-1+ Δ Y _jdetermine n in actual motion track ₂the ordinate of individual interpolated point; Wherein, floor function is downward bracket function, and j is that span is [1, n ₂] positive integer.

6. the motion trail planning method based on SCARA manipulator according to claim 4, is characterized in that, described step B2 specifically comprises:

B21, described DSP obtain the starting point coordinate (X in described kinematic parameter _a,, Y _a), terminal point coordinate (X _b, Y _b), central coordinate of circle (X _o, Y _o), circular interpolation accuracy value dfstep and circular interpolation direction;

B22, according to starting point coordinate (X _a,, Y _a), terminal point coordinate (X _b, Y _b), central coordinate of circle (X _o, Y _o) and circular interpolation orientation determination radius R, starting point angle terminal angle and terminal angle and starting point angle is poor and according to calculate circular interpolation times N, and according to calculate current circular interpolation precision Dfstep, according to determine the abscissa of N interpolated point in actual motion track, according to determine the ordinate of N interpolated point in actual motion track; Wherein, floor function is downward bracket function, and m is that span is the positive integer of [1, N].

7. the movement locus planning system based on SCARA manipulator, is characterized in that, comprising:

Module is set, receive user's operational order for host computer, the trajectory planning mode of SCARA manipulator and the kinematic parameter corresponding with described trajectory planning mode are set, and described trajectory planning mode and described kinematic parameter are sent to the motion controller of SCARA manipulator;

Track obtains and sending module, according to algorithm corresponding to described trajectory planning mode, the actual motion track of SCARA manipulator is carried out to computing for described motion controller, obtain the actual motion trace information of SCARA manipulator, and described actual motion trace information is sent to SCARA manipulator;

Receive and motion module, receive described actual motion trace information for described SCARA manipulator, and move according to described actual motion trace information.

8. the movement locus planning system based on SCARA manipulator according to claim 7, is characterized in that, the described described trajectory planning mode arranging in module is that smooth track planning is caught in linear interpolation trajectory planning, circular interpolation trajectory planning and flight.

9. the movement locus planning system based on SCARA manipulator according to claim 8, it is characterized in that, described track obtain and sending module in the time that described trajectory planning mode is linear interpolation trajectory planning, described kinematic parameter comprises starting point coordinate, terminal point coordinate and interpolation precision value; In the time that described trajectory planning mode is circular interpolation trajectory planning, described kinematic parameter comprises starting point coordinate, terminal point coordinate, central coordinate of circle, circular interpolation accuracy value and circular interpolation direction; When described trajectory planning mode is that smooth track when planning is caught in flight, described kinematic parameter comprises starting point coordinate, trigger point coordinate, terminal point coordinate, triggering pulsewidth and smoothness.

10. the movement locus planning system based on SCARA manipulator according to claim 9; it is characterized in that; described track obtains and sending module specifically comprises that linear interpolation unit, circular interpolation unit, flight catches smooth unit, tarnsition velocity acquiring unit and transmitting element, wherein:

Described linear interpolation unit, linear interpolation trajectory planning for working as described trajectory planning mode, the DSP in described motion controller carries out computing according to linear interpolation algorithm to the actual motion track of SCARA manipulator, the physical coordinates that obtains multiple interpolated points in the actual motion track of SCARA manipulator, starts described tarnsition velocity acquiring unit afterwards;

Described circular interpolation unit, circular interpolation trajectory planning for working as described trajectory planning mode, the DSP in described motion controller carries out computing according to arc interpolation to the actual motion track of SCARA manipulator, the physical coordinates that obtains multiple interpolated points in the actual motion track of SCARA manipulator, starts described tarnsition velocity acquiring unit afterwards;

Smooth unit is caught in described flight, that smooth track planning is caught in flight for working as described trajectory planning mode, the DSP in described motion controller catches smoothing algorithm according to flight the actual motion track of SCARA manipulator is carried out to computing, the physical coordinates that obtains multiple interpolated points in the actual motion track of SCARA manipulator, starts described tarnsition velocity acquiring unit afterwards;

Described tarnsition velocity acquiring unit, transforms the physical coordinates of described multiple interpolated points for described DSP the corner of the corresponding large forearm of SCARA manipulator, and calculates the speed of multiple interpolated points according to predetermined step curve acceleration and deceleration method;

Described transmitting element, the motion control chip that for described DSP, the corner of the large forearm of SCARA manipulator corresponding described multiple interpolated points and speed is sent to described motion controller, motion control chip is according to the corner of the large forearm of SCARA manipulator and speed control SCARA robot movement.