A kind of three axis Cartesian robot space circular arc interpolation methods
Technical field
The invention belongs to technical field of robot control, in particular to a kind of three axis Cartesian robot space circular arcs
Interpolating method.
Background technique
In current robot control field application, for example, the dispensing of three axis Cartesian robots, the tasks such as engraving, track
Planning is the key point of completion task.Circular interpolation is one of trajectory planning main task, usually only to several on motion path
A key point goes out intermediate point according to interpolation the characteristics of track, to realize the high-precision motion control of colleges and universities.These systems need
Accomplish to handle in real time, needs a kind of method that algorithm is simple, calculation amount is low, arithmetic speed is fast, interpolation coordinate precision is high.
The prior art need to judge interpolation direction, and judgment method is complicated and multistep is needed to judge, the present invention passes through specific method
Vector constructs specific auxiliary vector, avoids this process of the judgement in interpolation direction, eliminates unnecessary calculating.
Existing judgement benefit circular arc, which corresponds to central angle and the technology of two vector angle relationships, to be had: calculating separately starting point to process
The angle then summation of point, passing point to middle stop, needs three steps to calculate, and be not suitable for passing point and starting point or terminating point
The case where angle is more than π;Judged by mixed product, is calculated complicated;Or by Kinematics analysis parametric equation, need to calculate anti-three
Angle function.
Summary of the invention
In order to solve the technical issues of above-mentioned background technique proposes, the present invention is intended to provide a kind of three axis rectangular co-ordinate machines
People's space circular arc interpolation method simplifies algorithm, reduces calculation amount, improves arithmetic speed and precision.
In order to achieve the above technical purposes, the technical solution of the present invention is as follows:
A kind of three axis Cartesian robot space circular arc interpolation methods, comprising the following steps:
(1) it is specified any not conllinear three in three axis Cartesian robot motion profiles by teaching machine or host computer
Point: starting point P1, intermediate passing point P2With terminating point P3;
(2) by P1、P2、P33 points substitute into center of circle O, radius R analytic expression, this analytic expression by 3 points it is coplanar and 3 points to justify
The heart is equidistant the two constraint solvings and obtains;
(3) the central angle W for needing the circular arc of interpolation is calculated;
(4) P is calculated1、P2、P3The specific normal vector N of plane where 3 points, and specific auxiliary is constructed according to normal vector N
Vector M;
(5) parametric equation of interpolated point is showed;
(6) interpolation number I is calculated;
(7) parameter value for calculating i-th interpolation, is substituted into the parametric equation of step (5), obtains i-th interpolation
Point coordinate, i value since 0;
(8) judge interpolation terminating point, if i < I, return step (7) is otherwise transferred to step (9);
(9) calculated result is sent to the executing agency of three axis Cartesian robots.
Further, in step (2), if P1Coordinate be (x1,y1,z1), P2Coordinate be (x2,y2,z2), P3Seat
It is designated as (x3,y3,z3), the coordinate of center of circle O is (ox,oy,oz), then center of circle O, the analytic expression of radius R are as follows:
Wherein:
A1=y1*z2-y1*z3-z1*y2+z1*z2+y2*z3-z2*z2
B1=-x1*z2+x1*z3+z1*x2-z1*z1-x2*z3+z1*z2
C1=x1*y2-x1*z2-y1*x2+y1*z1+x2*z2-z1*y2
D1=-x1*y2*z3+x1*z2*z2+x2*y1*z3-z1*y1*z2-x2*z2*z1+z1*y2*z1;
A2=2* (x2-x1)
B2=2* (y2-y1)
C2=2* (z2-z1)
D2=x1 2+y1 2+z1 2-x2 2-y2 2-z2 2;
A3=2* (z1-x1)
B3=2* (z2-y1)
C3=2* (z3-z1)
D3=x1 2+y1 2+z1 2-z1 2-z2 2-z3 2。
Further, in step (3), whenWhen, need the circular arc of interpolation for semicircle, W=180 °;
WhenWhen, needing the circular arc of interpolation is minor arc,When
When, needing the camber line of interpolation is major arc,
Further, in step (4), normal vectorAuxiliary vector
Further, in step (5), the parametric equation of interpolated point is as follows:
In above formula, (ox,oy,oz) be center of circle O coordinate,θ is parameter
Equation independent variable, indicate the line in this interpolated point and the center of circle withAngle.
Further, in step (7), the parameter value of i-th interpolationIt is substituted into parametric equation, is obtained
To the point coordinate (x of i-th interpolationi,yi,zi):
Further, in step (6), interpolation numberWherein V is interpolation rate, i.e., each period passes through
The length of space circular arc.
By adopting the above technical scheme bring the utility model has the advantages that
(1) needing the amount asked such as center of circle O, radius R in the present invention has analytic expression, solves simply, quickly, simplifies calculation
Method;
(2) present invention is constructed specific auxiliary vector, is avoided the judgement in interpolation direction, subtracted by specific normal vector
Step is lacked;
(3) present invention can acquire the central angle of interpolation circular arc by the judgement of superiority and inferiority arc, sentence without complicated mixed product
It is disconnected, improve arithmetic speed;
(4) present invention indicates interpolated point with parametric equation, and each period solves absolute position, avoids accumulated error;
(5) interpolated point of the invention is only indicated by one group of parametric equation, reduces size of code.
Detailed description of the invention
Fig. 1 is flow chart of the method for the present invention;
Fig. 2 is interpolation example schematic of the invention;
Fig. 3 is of the invention four kinds along interpolation situation schematic diagram counterclockwise.
Specific embodiment
Below with reference to attached drawing, technical solution of the present invention is described in detail.
The three axis Cartesian robot space circular arc interpolation method of one kind that the present invention designs, as shown in Figure 1, detailed process
It is as follows.
S01 specifies arbitrarily not conllinear: starting point P at 3 points by teaching machine or other host computers1(1,2,4), pass through
Point P2(2,1,3), terminating point P3(6,6,6).
S02, by 3 points of substitution center of circle O (ox,oy,oz), the analytic expression of radius R, this analytic expression is coplanar by 3 points, 3 points to
The two equal constraint solvings of circle center distance obtain;
A1=y1*z2-y1*z3-z1*y2+z1*z2+y2*z3-z2*z2=2;
B1=-x1*z2+x1*z3+z1*x2-z1*z1-x2*z3+z1*z2=-7;
C1=x1*y2-x1*z2-y1*x2+y1*z1+x2*z2-z1*y2=9;
D1=-x1*y2*z3+x1*z2*z2+x2*y1*z3-z1*y1*z2-x2*z2*z1+z1*y2*z1=-24;
A2=2* (x2-x1)=2;B2=2* (y2-y1)=- 2;C2=2* (z2-z1);-2
D2=x1 2+y1 2+z1 2-x2 2-y2 2-z2 2=7;
A3=2* (z1-x1)=10;B3=2* (z2-y1)=8;C3=2* (z3-z1)=4;
D3=x1 2+y1 2+z1 2-z1 2-z2 2-z3 2=-87;
S03 calculates central angle W corresponding to arc length;
Because vector angle range is [0, π], center of circle angular region is [0,2 π], it is therefore desirable to the circular arc of interpolation needed for judging
For major arc or minor arc, method is as follows;
Judge vectorAngle,
When, the camber line of required interpolation is major arc,
S04 calculates specific normal vector N, constructs specific auxiliary vector M;
Normal vector calculation formula are as follows:
Auxiliary vector calculation formula are as follows:
Construction can guarantee auxiliary vector on interpolation direction in this way, so as to avoid the interpolation walking direction of conventional method,
Simplify calculation amount.
S05 shows the parametric equation of interpolated point;
Parametric equation indicates to need unitization vector
Wherein θ is parametric equation independent variable, physical significance be present interpolation point and the center of circle line withAngle, θ
=[0, W].
S06 calculates interpolation number I by interpolation rate V;
V refers to the length by space circular arc in each period, can enable interpolation week number
S07 substitutes into the coordinate that S05 calculates interpolated point according to interpolation parameters value;
Above formula (xi,yi,zi) be i-th interpolation point coordinate, as shown in table 1:
Table 1
i |
x |
y |
z |
0 |
1 |
2 |
4 |
1 |
1.107972297 |
1.816744143 |
3.833473823 |
2 |
1.234129431 |
1.642593264 |
3.669988221 |
3 |
1.377740384 |
1.478556477 |
3.510490508 |
… |
… |
… |
… |
50 |
6 |
6 |
6 |
S08, the judgement of interpolation terminating point;
Judge i≤50, "Yes" then returns to S07, and "No" then terminates interpolation.
Calculated result is transmitted to the executing agency of three axis Cartesian robots by S09.
As shown in Fig. 2, being interpolated point in figure, indicates circular interpolation starting point P1, ※ expression circular interpolation passing point
P2, ☆ expression circular interpolation terminating point P3, zero indicates the center of circle O of circular interpolation.It generates from starting point, passing point arrives terminating point
It is expected to comply fully with interpolation for curve.
Fig. 3 shows 4 kinds of possible types that interpolation encounters, wherein (a) is major arc P counterclockwise1(1,2,4),P2(2,1,
3),P3(6,6,6) situation (b) is minor arc P counterclockwise1(1,2,4),P2(2,1,3),P3(4,2,2) situation is (c) clockwise
Major arc P1(6,6,6),P2(2,1,3),P3(1,2,4) situation (d) is minor arc P clockwise1(4,2,2), P2(2,1,3),P3(1,
2,4) situation.It is found that the present invention, only with one group of analytic expression, completes the interpolation of all situations, eliminates without judging interpolation direction
Deterministic process.
Embodiment is merely illustrative of the invention's technical idea, and this does not limit the scope of protection of the present invention, it is all according to
Technical idea proposed by the present invention, any changes made on the basis of the technical scheme are fallen within the scope of the present invention.