CN106826829A - A kind of industrial robot fairing trace generator method of Controllable Error - Google Patents

Abstract

A kind of industrial robot fairing trace generator method of Controllable Error, comprises the following steps：S1, generation MOVEB movement instructions, description industrial robot motion track, including tracing point and attitude, and user input tracing point error threshold and action error threshold；High-order B-spline curves, using high-order B-spline interpolation algorithm, are entered row interpolation to tracing point by the interpolation of S2, robot trajectory's point according to track point tolerance chord error threshold high, and the G of robot trajectory's point is realized respectively²Interpolation and G³Interpolation, obtains with continuity high and ensures to meet the interpolation track of track point tolerance chord error requirements high；The interpolation of S3, robot pose, obtains being segmented G²And G³Continuous robot pose curve；S4, the movement locus after tracing point interpolation curve and robot pose curve obtain robot interpolation jointly.Movement locus real-time continuous interpolation of the present invention, calculates simple, efficient, accurate, reduces machine vibration and abrasion.

Description

A kind of industrial robot fairing trace generator method of Controllable Error

Technical field

The invention belongs to industrial robot applied technical field, and in particular to a kind of industrial robot fairing of Controllable Error Trace generator method.

Background technology

Industrial robot is widely used in welding, assemble, carry, polishing, spraying and the complex job such as machining.Making Industrial robot controls the movement locus of robot by movement instruction in industry.The recognizable motion of existing industrial robot refers to Order is main to include linear motion instruction, circular motion instruction and axial coordinate movement instruction, such as in ABB robots respectively with MOVEL, MOVEC and MOVEJ represent that three of the above is instructed.Simple motion track can write axial coordinate movement instruction by teaching machine, mainly It is applied to only require and is accurate to a little, the operation (such as spot welding robot) that track is any between points but the time is most short.It is most of Complicated movement locus needs to be obtained by discrete programming software, and main including instruction and the circular motion instruction of moving along a straight line. But the movement locus of straightway and arc representation only has G in junction⁰Continuity, during robot manipulating task, for essence The necessary reduction of speed of given trace point is really reached, therefore greatly reduces operating efficiency；In addition, speed and acceleration may discontinuously lead Vibration during robot motion is caused, so as to accelerate wear and tear in machines and influence operation quality.

In order to improve the continuity of robot motion track, added in existing movement instruction and approach instruction.Such as Fig. 1 institutes Show, represent that, accurately by given trace point, z10 represents the rounding off of error 10mm using fine instructions in ABB robots, but It is that this approaches instruction and cannot ensure that point tolerance chord error high in track meets and requires；An Chuan robots represent parabola with MOVES Instruction, although parabolic line can accurately pass through given trace point, but cannot ensure that action error meets demand.

From the foregoing, it will be observed that cannot be generated while meeting track point tolerance chord just with existing straight line and circular motion instruction Error high and with movement locus high continuous.Then occur in that some in off-line programing stage interpolated movements rail in existing research Mark and inside control system interpolated movements track method.It is to improve successional preferably square with smooth curve interpolation tracing point Method, but track interpolation need to meet track point tolerance chord error requirements high.Control errors have critically important practical significance, such as Require spraying profile requirement of the pad in the range of the point tolerance of track, in spraying process between 2 points in action in welding process In error range.Two error thresholds are set by user according to processing technology, and any one error is unsatisfactory for all causing product It is off quality.

Existing interpolation method major part only considered tracing point control errors, not consider action control errors：It is such as existing non- Patent document《Industrial robot trajectory planning research based on Based on Interpolating Spline》Pass through three B samples during off-line programing Bar curve interpolation tracing point, it is then that interpolation curve is discrete and post-process as straight line or circular motion are instructed.Although the method is led to Cross the continuity that interpolation improves track, but it is discrete after can also there is error and be unsatisfactory for the deficiency low with continuity.Non- patent text Offer《Real-time Coplanar NURBS Curve Fitting and Interpolation for 6-DOF Robot Arm》In realize a kind of real-time NURBS interpolations of robot control system and discrete algorithm, but the method does not ensure that action is missed Difference, and it is poor by the method stability of matrix inversion calculating control vertex, it is impossible to ensure that action error meets and require.

The fairing interpolation of robot motion track includes track point interpolation and attitude interpolation, the fairing interpolation sum of tracing point The fairing interpolation of small line segment is similar in control Milling Process：Non-patent literature《A real-time look-ahead interpolation methodology with curvature-continuous B-spline transition scheme for CNC machining of short line segments》Propose three axle cutters in a kind of digital control processing The G of track²Continuous bridging algorithm solves the above problems, and can obtain that tracing point smoothly transits and the real-time conformal of continual curvature is inserted Value track.But the bridge joint track that the method is obtained cannot realize high-precision interpolation, point tolerance smaller BridgeCurve in track is shorter, Too short bridge joint track cannot realize the smooth of speed.Non-patent literature《A real-time curvature-smooth interpolation scheme and motion planning for CNC machining of short line segments》The above method is improved, it is proposed that the G of cutter path³Continuous interpolation algorithm, can obtain meeting tracing point mistake Difference and the real-time conforming interpolation track of action error and curvature fairing, can effectively improve processing efficiency and crudy, and Reduce machine vibration.The method achieves preferably application in three-axis numerical control processing, but is not suitable for five-axis robot and industry In robot application.

The attitude of movement locus discontinuously can also influence the precision and efficiency of processing, and existing attitude interpolation method has quaternary number Method, the method based on matrix exponent method and direct interpolation Eulerian angles.Existing non-patent literature《Interpolation method for robot trajectory planning》In the interpolation of robot pose directly realized to Eulerian angles interpolation, but directly Do not have coordinate invariance to Eulerian angles interpolation, Eulerian angles can not continuously correspond to the continuous of tracing point attitude.

In view of robot motion track can not simultaneously meet continuity high and high-precision requirement in the prior art, one is needed badly Plant the generation method of industrial robot fairing movement locus.

The content of the invention

The technical problem to be solved in the present invention is there is poor continuity for robot in the prior art and error is unsatisfactory for Cause inefficiency, off quality and machine vibration problem, there is provided a kind of industrial robot fairing fortune of Controllable Error Dynamic orbit generation method, calculate it is simple, efficiently, it is accurate, robot motion track can simultaneously meet continuity high and high-precision It is required that, industrial robot error (track point tolerance chord error high) meets the conformal movement locus of real-time implementation requirement fairing.

The present invention is adopted the technical scheme that to solve above-mentioned technical problem：

A kind of industrial robot fairing trace generator method of Controllable Error, comprises the following steps：

S1, generation MOVEB movement instructions：MOVEB movement instructions are used for describing industrial robot motion track, including track Point and attitude, and user input tracing point error threshold and action error threshold；

The interpolation of S2, robot trajectory's point：The industrial robot motion track that MOVEB movement instructions are represented imports machine People's control system, using high-order B-spline interpolation algorithm, by high-order B-spline curves according to track point tolerance chord error threshold high Enter row interpolation to tracing point, the G of robot trajectory's point is realized respectively²Interpolation (has G²Continuous cubic Bézier curves) and G³Insert Value (has G³Continuous four B-spline interpolation), obtain with continuity high and guarantee meets track point tolerance chord mistake high The interpolation track that difference is required；

The interpolation of S3, robot pose：Realize the segmentation G of robot pose respectively by B-spline interpolation algorithm²And G³Insert Value, and attitude to the robot trajectory after interpolation carries out unit orthogonalization process, obtains being segmented G²And G³Continuous robot appearance State curve；

Movement locus after S4, generation robot interpolation：The tracing point interpolation curve and step obtained by above-mentioned steps S2 The robot pose curve that rapid S3 is obtained obtains the movement locus after robot interpolation jointly.

By such scheme, the data content of MOVEB movement instructions includes X, Y, Z, I, J, K, U, V, W in the step S1, D, E totally ten one variables, wherein the coordinate of the tracing point under (X, Y, Z) representational tool coordinate system, (I, J, K) represents above-mentioned track The Z axis unit vector of the tool coordinates system set up at point, the X-axis unit vector of (U, V, W) representational tool coordinate system, D, E difference Represent action error threshold and tracing point error threshold.

By such scheme, in the step S2 and step S3, during robot control system parsing MOVEB movement instructions, will One section of continuous path of tracing point error threshold and action error threshold identical constitutes one group and enters row interpolation.

By such scheme, in the step S2, the interpolation of robot trajectory's point specifically includes following steps：

Step one：The decision of primary iteration parameter, obtains one group of robot initial trace point from MOVEB movement instructionsAction error threshold D and tracing point error threshold E；Iterations is designated as k=0, calculates initial changing For tracing pointWith primary iteration action error threshold

Step 2：Specifically include：

1. each inner track point (the not iteration tracing point including head and the tail end points) is set upPlace Bridge joint B-spline curves, in whole step 2, omit iterations k, i.e.,It is equal to P_i,It is equal to d_i(conveniently retouch State)：

The number of times for bridging B-spline curves is p, has 2p+1 control point, respectively A_1,i, A_2,i…A_p,i,Q_i,B_p,i,… B_2,i,B_1,i, knot vector is { 0,0.5,1 }, and the multiplicity of three nodes is respectively p+1, p, p+1；The bridge joint B-spline curves etc. Valency is in two p times symmetrical Bezier curves；

2. set up in Q_iTwo symmetrical p Bezier curves, the control point of two Bezier curves is respectively A_1,i, A_2,i…A_p,i,Q_iAnd Q_i,B_p,i,…B_2,i,B_1,i, wherein A_1,i, A_2,i…A_p,iWith P_i-1P_iCollinearly, B_p,i,…B_2,i,B_1,iWith P_iP_i+1 Collinearly, Q_iIt is A_p,iB_p,iMidpoint；

3. G is divided²And G³Two kinds of situations of interpolation provide guarantee bridge joint B-spline curves G everywhere²Or G³Continuous control point：

Ifh_i=| A_p,iP_i|,θ_i=<T_0,i,T_1,i>

Wherein T_0,iIt is P_i-1P_iUnit vector；T_1,iFor A_p,iB_p,iUnit vector；h_iIt is line segment A_p,iP_iLength；θ_i It is vector T_0,iAnd T_1,iAngle；

I) in G²During interpolation, G²The control vertex of BridgeCurve meets following condition：

Q_i=P_i-h_iT_0,i+h_icosθ_iT_1,i

A_3,i=Q_i-h_icosθ_iT_1,i

A_2,i=A_3,i-h_iT_0,i

A_1,i=A_2,i-β_ih_iT_0,i

Ii) in G³During interpolation, G³The control vertex of BridgeCurve meets following condition：

Q_i=P_i-h_iT_0,i+h_icosθ_iT_1,i

A_4,i=Q_i-h_icosθ_iT_1,i

A_3,i=A_4,i-α_ih_iT_0,i

A_1,i=A_2,i-β_ih_iT_0,i

α_i≥0.7,0<β_i≤0.5

In G²During interpolation, h_i,β_iIt is two BridgeCurve parameters；In G³During interpolation, h_i,α_i,β_iIt is three BridgeCurve ginsengs Number.BridgeCurve parameter determines the position at BridgeCurve control point, following steps 5. in by object function optimize ask Solution is obtained, B_1,i,B_2,i,…,B_p,iObtained by the symmetry of bridge joint B-spline curves；

4. with BridgeCurve more gently for target sets up optimization object function, and conformal and action control errors pacts are provided Beam condition：

Two G of Bezier curve construction²BridgeCurve and G³BridgeCurve is in intersection point Q_iThe maximum curvature at place, and it is maximum Curvature is respectivelyWithTo obtain gentler BridgeCurve, it is desirable to k_g2maxAnd k_g3maxIt is smaller, that is, require step 3. middle bridge joint parameter of curve h_iIt is bigger, therefore object function is defined as to solve bridge joint Parameter of curve h_i, i=1,2 ... N-1's and greatest problem, i.e., as shown in following formula (1)：

To realize conformal constraint, it is desirable to which the BridgeCurve between two tracing points does not exist intersection, wherein G²Continuously And G³Continuously bridge conformal being constrained to：

|P_i-1B_1,i-1|+|A_1,iP_i|≤|P_i-1P_i|, i=2 ... N-1 (2)

|A_1,1P₁|≤|P₀P₁| (3)

|P_N-1B_1,N-1|≤|P_N-1P_N| (4)

To realize action error constraints, P only need to be controlled_iQ_iDistance be not more than iteration action error threshold d_i, i.e.,：

|P_iQ_i|≤d_i (5)

5. solve optimization problem be formula (1) and meanwhile require BridgeCurve parameter meet constraint equation (2), (3), (4), (5) BridgeCurve parameter, is solved, that is, obtains the bridge joint track of iteration tracing point；

Step 3：Calculate the tracing point max value of error Δ of bridge joint track and initial trace point_k, and judge Δ_kWhether it is less than Tracing point error threshold E, the computational methods of track point tolerance are calculating tracing point and two Bezier curve intersection pointsAway from From.Add k subscripts parameter to be equal to k subscript parameters are not added in step 2 in this step 3, be the error inspection of description kth time iteration Survey, do not omit k：

WhereinIt is located atAngular bisector on, according to the symmetry of interpolation curve, by calculatingSize and angular bisector vector obtain, computational methods are：

If Δ_k>E, illustrates that BridgeCurve does not meet tracing point error requirements, updates iteration tracing point into step 4 and changes For action error threshold；Otherwise, interpolation success, exits iteration, derives interpolation track；

Step 4：Iterative parameter is updated, the adjustment vector for making iteration tracing point is：Iteration tracing point after adjustment is： Iteration action error threshold is：Update iterations：K=k + 1, go to step two.

By such scheme, in the step S3, the interpolation of robot pose specifically includes following steps：

Step one：Calculate offset point, noteThe X-axis unit vector of the tool coordinates system at place isZ axis unit vector isWillOne section of fixed range m is offset along X-axis, X-axis skew is obtained Point is designated asWillOne section of fixed range n is offset along Z axis, Z axis offset point is obtained and is designated as

Step 2：To X-axis offset point and Z axis skew point interpolation, X-axis is offset respectively using the method for track point interpolation Point, Z axis offset pointEnter row interpolation, obtain piecewise interpolation curve O (t), R (t)；

Step 3：The attitude of the track after interpolation is calculated, the piecewise interpolation curve for remembering robot trajectory's point is P (t), interpolation X-axis vector afterwards：X (t)=O (t)-P (t), Z axis vector：Z (t)=R (t)-P (t), it is orthogonal to X-axis vector, Z axis vector units Change is processed, and is further calculated unit after interpolation orthogonal X-axis unit vector, Z axis unit vector：

X-axis unit vector X after interpolation^newT () is：

Z axis unit vector Z after interpolation^newT () is：

Z^new(t)=F (t) × X^new(t)×F(t)

Wherein

According to X derived above^new(t), Z^newT the attitude curve after () description interpolation, obtains each track interpolation curve The corresponding attitude information of upper tracing point.

By such scheme, one the step of the step S3 in, the value of offset distance m, n is set to tracing point average headway 5~10 times.

By such scheme, three the step of the step S3 in, the attitude sectional curve after unit orthogonalization has segmentation G²Or G³Continuity, i.e., the curve G between waypoint²Or G³Continuously, at least there is G at waypoint⁰Continuity.

By the contemplated above technical scheme of the present invention, compared with prior art, the present invention has the advantage that：

1st, the continuous trace generator method of industrial robot proposed by the present invention ensure that data point tolerance chord is high Error meets user's requirement：User can be instructed by MOVEB according to processing technology demand and specify data point tolerance chord mistake high Difference limen value, interpolation algorithm of the invention ensure that the data point tolerance chord error threshold high for meeting user's setting, and protect Demonstrate,prove the gentle and conformal of interpolation curve such that it is able to ensure the high precision of actual job；Can be used for industrial robot welding, Movement locus real-time continuous interpolation in the complex jobs such as spraying, laser cutting；

2nd, the continuous trace generator method of industrial robot proposed by the present invention ensure that tracing point continuity (G high² Continuous and G³Continuously), G²Continuous track has continuous speed and an acceleration, and G³Continuous track not only has continuous Speed and acceleration, also with continuous acceleration, in robot actual job, successional track high can be effective Operating efficiency and operation quality precision are improved, acceleration can continuously reduce machine vibration, so as to reduce wear and tear in machines；

3rd, the continuous trace generator method of industrial robot proposed by the present invention ensure that the segmentation of attitude is high continuous Property (G²Continuous and G³Continuously), segmentation attitude interpolation curve can guarantee that attitude (G between each two waypoint²Or G³) consecutive variations, The consecutive variations of robot pose can not only improve the homework precision of robot, additionally it is possible to reduce robot arm and significantly put It is dynamic, so as to effectively improve efficiency.

Brief description of the drawings

Fig. 1 is ABB robot motions instruction in the prior art；

The MOVEB movement instruction cases that Fig. 2 is designed for the embodiment of the present invention；

Fig. 3 is tracing point of the present invention continuity interpolation flow chart high；

Fig. 4 is embodiment of the present invention G²Bridge joint schematic diagram；

Fig. 5 is embodiment of the present invention G³Bridge joint schematic diagram；

Fig. 6 is embodiment of the present invention tracing point G²(G³) interpolation conformal constraint；

Fig. 7 is embodiment of the present invention tracing point G²(G³) interpolation iteration schematic diagram；

Fig. 8 is that embodiment of the present invention Z axis offset point interpolation schematic diagram.

Specific embodiment

In order to make the purpose , technical scheme and advantage of the present invention be clearer, below in conjunction with accompanying drawing and case study on implementation, The present invention will be described in further detail.It should be appreciated that specific implementation case described herein is only used to explain this hair It is bright, it is not intended to limit the present invention.

The industrial robot fairing trace generator method of Controllable Error of the present invention, is subdivided into following four contents：

<1>The generation of MOVEB movement instructions

A kind of new MOVEB movement instructions have been invented first, for describing the industrial robot that off-line programming technique is obtained Movement locus, and comprising tracing point error threshold and action error threshold information, user can set track according to technological requirement Point tolerance and action error.Data content behind MOVEB movement instructions includes X, Y, Z, I, J, K, U, V, W, D, E totally ten one Variable, wherein preceding nine data are used to describe the linear motion trajectory (tracing point and attitude) of industrial robot, latter two variable Respectively action error threshold and tracing point error threshold.As shown in Fig. 2 describing five tracing points and attitude, wherein user The action error threshold D=0.001mm for specifying, tracing point error threshold E=0.0001mm.

User can by D, E robot motion track interpolation is set required for error threshold, each tracing point can be with Identical error (track point tolerance chord error high) threshold value is set, it is also possible to set different threshold values.Error threshold is by user's root Determined according to technique, such as action error threshold may be configured as processing franchise in robot machining, such as in digital control processing, The processing request of parts is general in ± 0.05mm~± 0.1mm, can by tracing point action error be set to 0.005mm~ 0.03mm；Tracing point error threshold may be configured as numerical stability, generally 10^-4~10^-8mm。

MOVEB movement instructions are used for describing original motion trajectory and interpolation error, and robot control system can recognize that MOVEB Movement instruction simultaneously carries out high-order B-spline interpolation to original motion trajectory according to error threshold.The embodiment of the present invention supports there is G² Continuous cubic Bézier curves and with G³Continuous four B-spline interpolation, respectively obtain expiring for continual curvature and curvature fairing The fairing interpolation curve of sufficient error requirements.

<2>The interpolation of robot trajectory's point

MOVEB movement instructions are imported in robot control system, industrial robot motion track and error threshold is obtained Information, because user can set different error thresholds to continuous tracing point, therefore in robot control system parsing During MOVEB movement instructions, one section of continuous path of tracing point error threshold and action error threshold identical is constituted into one group is carried out Interpolation.

As shown in figure 3, the interpolation of tracing point refers to that the movement locus point of MOVEB movement instructions description is missed according to tracing point Difference and action error threshold enter row interpolation, and the interpolation method can be realized with G²Continuous cubic Bézier curves and with G³Continuously Four B-spline interpolation.The main technical schemes of the interpolation of tracing point are to import the motion rail that one group of MOVEB movement instruction is represented Mark and error threshold (action error threshold and tracing point error threshold), then calculate the bridge joint track of iteration tracing point, then Judge whether the error bridged between track and initial trace point meets tracing point error threshold, if meeting threshold value, interpolation terminates, Derive the track after bridge joint；Otherwise according to tracing point error update iteration tracing point and iteration action error threshold, bridge again.

In step S2, by constructing bridge joint B-spline curves, and constantly iteration renewal BridgeCurve makes track point tolerance chord Error high meets, and the final bridge joint B-spline curves of note are interpolation curve：Iteration tracing point and iteration action error threshold are set first Value, is then bridged at each tracing point with a B-spline curves, secondly calculates the track of BridgeCurve and initial trace Whether point tolerance meets user's requirement, if meet requiring, interpolation terminates, and derives interpolation curve；If being unsatisfactory for requiring, according to track Point tolerance updates iteration tracing point and iteration action error, bridges again, continuous iteration, is required until track point tolerance meets, Interpolation terminates；The interpolation of robot trajectory's point specifically includes following steps：

Step one：The decision of primary iteration parameter, obtains one group of robot initial trace point from MOVEB movement instructionsAction error threshold D and tracing point error threshold E；Iterations is designated as k=0, calculates initial changing For tracing pointWith primary iteration action error threshold

Step 2：First, each inner track point (not including head and the tail end points) P is set up_i, i=1 ... p bridge at N-1 B-spline curves are connect, in whole step 2, for convenience of describing, iterations k is omitted, i.e.,It is equal to P_i,It is equal to d_i：

The bridge joint B-spline curves have 2p+1 control point, respectively A_1,i, A_2,i…A_p,i,Q_i,B_p,i,…B_2,i,B_1,i, Knot vector is { 0,0.5,1 }, and the multiplicity of three nodes is respectively p+1, p, p+1；

Then, it is convenience of calculation, makes bridge joint B-spline curves G everywhere²Or G³Continuously, set up in Q_iSymmetrical B-spline curves, The bridge joint B-spline curves are converted into and are made up of two p times symmetrical Bezier curves, two control points of Bezier curve point Wei not A_1,i,A_2,i…A_p,i,Q_iAnd Q_i,B_p,i,…B_2,i,B_1,i, wherein preceding p control point is conllinear, rear p control point is conllinear, Q_iFor A_p,iB_p,iMidpoint, control point collinearly ensure that bridge joint B-spline curves head and the tail stitching portion G²(G³) continuity, but still need to structure Make Q_iG at point²(G³) continuity.

As shown in Fig. 4~Fig. 5, round dot is tracing point, and the point that five-pointed star is represented is the control point of BridgeCurve, in fig. 4, Cubic B-spline BridgeCurve is made up of seven control points, is equivalent to two symmetrical three bezier curves.In Figure 5, three times B-spline BridgeCurve is made up of nine control points, is equivalent to two symmetrical four time Bezier curves.

Then, object function and constraints are set up：Two G of Bezier curve composition²And G³BridgeCurve is in intersection point Q_i Locate maximum curvature, and maximum curvature is respectivelyWithDerive most Daqu Rate and h_iIt is inversely proportional, to obtain gentler BridgeCurve, it is desirable to k_g2maxAnd k_g3maxIt is smaller, therefore object function is established as to make All of h_iAnd maximum, so as to obtain gentler BridgeCurve.In G²In interpolation, β is can use_i=0.5；In G³In interpolation, it is known that α_iIt is directly proportional to curvature extremum value, α_iSpan be α_i>=0.7, therefore in actually calculating, can use α_i=0.7, it is known that 0<β_i ≤ 0.5, therefore desirable β_i=0.5.

Be defined as object function to solve bridge joint parameter h by the present invention_i, i=1,2 ... N-1's and greatest problem, i.e., as public Shown in formula (1).To realize conformal constraint, as shown in Figure 6, it is desirable to which the BridgeCurve between two tracing points does not exist coincidence part Point, make BridgeCurve conformal by setting up three below inequality (2), (3), (4)；To realize action error constraints, control is only needed P processed_iQ_iDistance meet be not more than iteration action error threshold d_i, set up inequality (5)；Four formulas of the above can turn Turn to h_iThe inequality of constraint.

Finally, solving optimization problem (1) makes BridgeCurve parameter meet constraint equation (2), (3), (4), (5) constraint Condition, solves BridgeCurve parameter, that is, obtain the bridge joint track of iteration tracing point.The optimization problem is linear programming problem, Linear search method can be used in solution procedure.h₁Initial value determined by action error constraints (5) and conformal constraint (3)；h_N's Initial value is determined by action error constraints (5) and conformal constraint (4)；The N-1 for i=2 ..., first according to action error in formula (5) Threshold value constraint, takes initial valueThen judge whether initial value meets the conformal condition of formula (2), if meeting, bridge successfully； If it is not satisfied, reducing h_iUntil formula (2) meets；

Step 3：The track point tolerance of bridge joint track and initial trace point is calculated, judges whether to meet tracing point error threshold Value demand, the computational methods of track point tolerance are calculating tracing point and two Bezier curve intersection pointsDistance：

Δ_kIt is the maximum of all track point tolerances, it is known that the BridgeCurve of present invention construction is symmetrical by two Bezier curve is obtained, as shown in figure 4,It is located atAngular bisector on, therefore can by calculateSize and angular bisector vector obtain.

If Δ_k>Tracing point error threshold E, illustrates that BridgeCurve does not meet tracing point error requirements, is updated into step 4 Iteration tracing point and iteration action error threshold；Otherwise, interpolation success, derives interpolation track；

Step 4：Iterative parameter is updated, Fig. 7 demonstrates the renewal process of iteration tracing point, made Iteration tracing point after adjustment is：Iteration action Error threshold is：Update iterations：K=k+1, turns step Rapid two.

It is high that the robot trajectory's point interpolation curve (interpolation track) obtained by the above method meets data point tolerance chord Error, conformal and curve are gentle, with continuity high, and due to the convergence of iterative simple and stable, can use in real time To in the Interpolation Process of robot control system.Ensure G everywhere using B-spline Curve interpolation²Continuously, i.e. speed and acceleration Degree is continuous；Ensure G everywhere using four B-spline curves interpolation³Continuously, i.e., speed, acceleration are continuous and acceleration is continuous.

<3>The interpolation of robot pose

The interpolation of robot motion track of the present invention not only includes the interpolation of tracing point, the also interpolation including attitude.This hair Bright to use the method similar with track point interpolation to attitude interpolation, the interpolation of attitude refers to the tool coordinates to each tracing point The interpolation of system, obtains being segmented G²(G³) continuous attitude interpolation curve.Known means coordinate system can determine that Y-axis can by Z axis and X-axis Calculated with according to Z axis and X-axis multiplication cross.

The interpolation of attitude is converted into the present invention Z axis and the interpolation of X-axis, i.e. interpolation of the tool coordinates system of each tracing point On Z axis and X-axis apart from tracing point fixed range offset point interpolation.Calculate in Z axis and X-axis apart from track solid point respectively first Two groups of offset points of measured length, are then most passed through afterwards by the interpolation method similar with tracing point respectively to two groups of skew point interpolations Unitization and orthogonalization process is crossed to obtain being segmented G²(G³) continuous interpolation attitude curve.In order to obtain the coordinate system after interpolation, need What is tracing point and offset point constituted is vectorial unitization.Z axis and X-axis simultaneously after interpolation are not necessarily orthogonal, therefore after interpolation Need to do orthogonalization process.

The interpolation of robot pose specifically includes following steps：

Step one：Offset point is calculated, tracing point is obtained into series of points along X-axis and Z axis skew fixed range m, n respectively, X-axis offset point and Z axis offset point are designated as respectively, as shown in figure 8, black round dotIt is six tracing points； It is six Z axis offset points, can similarly calculates X-axis offset point；The value of offset distance m in the step, n should not be too small Or it is excessive, may be configured as 5~10 times of tracing point average headway；

Step 2：To X-axis offset point and Z axis skew point interpolation, using the method for track point interpolation respectively to X-axis offset point Enter row interpolation with Z axis offset point, obtain piecewise interpolation curve O (t), R (t), as shown in figure 8, the sectional curve after interpolation is R (t)={ R_i(t) }, i=1 ... 13, wherein black five-pointed star represents waypoint, each R_iT (), i=1 ... 13 meets G²(G³) Continuously；

Step 3：Calculate the attitude of the track after interpolation.In order to calculate the attitude of every on interpolation track, it is necessary to calculate one The orthogonal X of group unit, Z axis vector, above-mentioned steps can obtain the X after interpolation, Z axis vector：X (t)=O (t)-P (t), Z (t)=R (t)-P (t), but the attitude of track need to be the orthogonal coordinate system of unit, therefore to X, Z axis vector units orthogonalization process, obtain The attitude curve of interpolation track.Due to the presence of action error, attitude curve after the unit orthogonalization appearance inside waypoint State G²(G³) continuous, the not necessarily G at waypoint²(G³) continuous.But at least there is G⁰Continuously.

<4>Movement locus after generation robot interpolation

By above-mentioned steps<2>The tracing point interpolation curve and step of acquisition<3>The robot pose curve of acquisition is common Obtain the movement locus after robot interpolation, movement locus fairing after interpolation, meet data point tolerance chord error requirements high, It is conformal and being capable of interpolation in real time.Both the fairing interpolation of tracing point had been realized, the segmentation fairing interpolation of attitude had been realized again so that work Robot people along fairing interpolation stability-of-path high-quality and high-efficiency operation.

The present invention is not restricted to listed utilization in specification and implementation method, comes for those skilled in the art Say, various corresponding changes and modification can be made according to the present invention, and all these corresponding changes and modification belong to this hair Bright scope of the claims.

Claims (7)

1. a kind of industrial robot fairing trace generator method of Controllable Error, it is characterised in that comprise the following steps：

S1, generation MOVEB movement instructions：MOVEB movement instructions are used for describing industrial robot motion track, including tracing point and Attitude, and user input tracing point error threshold and action error threshold；

The interpolation of S2, robot trajectory's point：The industrial robot motion track that MOVEB movement instructions are represented imports robot control System processed, using high-order B-spline interpolation algorithm, by high-order B-spline curves according to track point tolerance chord error threshold high to rail Mark clicks through row interpolation, and the G of robot trajectory's point is realized respectively²Interpolation and G³Interpolation, obtains with continuity high and ensures to meet The interpolation track of track point tolerance chord error requirements high；

The interpolation of S3, robot pose：Realize the segmentation G of robot pose respectively by B-spline interpolation algorithm²And G³Interpolation, and Attitude to the robot trajectory after interpolation carries out unit orthogonalization process, obtains being segmented G²And G³Continuous robot pose is bent Line；

Movement locus after S4, generation robot interpolation：The tracing point interpolation curve and step S3 obtained by above-mentioned steps S2 The robot pose curve of acquisition obtains the movement locus after robot interpolation jointly.

2. the industrial robot fairing trace generator method of Controllable Error according to claim 1, it is characterised in that The data content of MOVEB movement instructions includes X in the step S1, Y, Z, I, J, K, U, V, W, D, E totally ten one variables, wherein The coordinate of the tracing point under (X, Y, Z) representational tool coordinate system, (I, J, K) represents the tool coordinates system set up at above-mentioned tracing point Z axis unit vector, the X-axis unit vector of (U, V, W) representational tool coordinate system, D, E represent action error threshold and rail respectively Mark point tolerance threshold value.

3. the industrial robot fairing trace generator method of Controllable Error according to claim 1, it is characterised in that In the step S2 and step S3, during robot control system parsing MOVEB movement instructions, by tracing point error threshold and action One section of continuous path of error threshold identical constitutes one group and enters row interpolation.

4. the industrial robot fairing trace generator method of Controllable Error according to claim 2, it is characterised in that In the step S2, the interpolation of robot trajectory's point specifically includes following steps：

Step one：The decision of primary iteration parameter, obtains one group of robot initial trace point from MOVEB movement instructionsN >=2, action error threshold D and tracing point error threshold E；Iterations is designated as k=0, calculates primary iteration rail Mark pointWith primary iteration action error threshold

Step 2：Specifically include：

1. each inner track point is set upThe bridge joint B-spline curves at place, in whole step 2, omit Iterations k, i.e.,It is equal to P_i,It is equal to d_i：

$\begin{array}{c}{r}_i\left(u\right)=A_{1,i}{B}_{2p}^0\left(u\right)+A_{2,i}{B}_{2p}^1\left(u\right)+\mathrm{...}+A_{p,i}{B}_{2p}^{p-1}\left(u\right)+Q_i{B}_{2p}^p\left(u\right)+B_{p,i}{B}_{2p}^{p+1}\left(u\right)+\mathrm{...}\\ +B_{1,i}{B}_{2p}^{2p}\left(u\right)\end{array}$

The number of times for bridging B-spline curves is p, has 2p+1 control point, respectively A_1,i, A_2,i…A_p,i,Q_i,B_p,i,…B_2,i, B_1,i, knot vector is { 0,0.5,1 }, and the multiplicity of three nodes is respectively p+1, p, p+1；The bridge joint B-spline curves are equivalent to Two p times symmetrical Bezier curves；

2. set up in Q_iTwo symmetrical p Bezier curves, the control point of two Bezier curves is respectively A_1,i,A_2,i… A_p,i,Q_iAnd Q_i,B_p,i,…B_2,i,B_1,i, wherein A_1,i, A_2,i…A_p,iWith P_i-1P_iCollinearly, B_p,i,…B_2,i,B_1,iWith P_iP_i+1Collinearly, Q_iIt is A_p,iB_p,iMidpoint；

3. G is divided²And G³Two kinds of situations of interpolation provide guarantee bridge joint B-spline curves G everywhere²Or G³Continuous control point：

If

Wherein T_0,iIt is P_i-1P_iUnit vector；T_1,iFor A_p,iB_p,iUnit vector；h_iIt is line segment A_p,iP_iLength；θ_iIt is vector T_0,iAnd T_1,iAngle；

I) in G²During interpolation, G²The control vertex of BridgeCurve meets following condition：

Q_i=P_i-h_iT_0,i+h_icosθ_iT_1,i

A_3,i=Q_i-h_icosθ_iT_1,i

A_2,i=A_3,i-h_iT_0,i

A_1,i=A_2,i-β_ih_iT_0,i

Ii) in G³During interpolation, G³The control vertex of BridgeCurve meets following condition：

Q_i=P_i-h_iT_0,i+h_icosθ_iT_1,i

A_4,i=Q_i-h_icosθ_iT_1,i

A_3,i=A_4,i-α_ih_iT_0,i

$A_{2,i}=A_{3,i}-\frac{(9{\mathrm{\&alpha;}}_i-5){\mathrm{\&alpha;}}_i}{2}{h}_i{T}_{0,i}$

A_1,i=A_2,i-β_ih_iT_0,i

α_i≥0.7,0<β_i≤0.5

In G²During interpolation, h_i,β_iIt is two BridgeCurve parameters；In G³During interpolation, h_i,α_i,β_iIt is three BridgeCurve parameters, bridge Connect the position that parameter of curve determines BridgeCurve control point, following steps 5. in obtained by object function optimization Arrive, B_1,i,B_2,i,…,B_p,iObtained by the symmetry of bridge joint B-spline curves；

4. with BridgeCurve more gently for target sets up optimization object function, and conformal and action control errors constraint bars are provided Part：

Two G of Bezier curve construction²BridgeCurve and G³BridgeCurve is in intersection point Q_iThe maximum curvature at place, and maximum curvature RespectivelyWithTo obtain gentler BridgeCurve, it is desirable to k_g2max And k_g3maxIt is smaller, that is, require step 3. middle bridge joint parameter of curve h_iIt is bigger, therefore object function is defined as to solve BridgeCurve Parameter h_i, i=1,2 ... N-1's and greatest problem, i.e., as shown in following formula (1)：

${\max }_{h_i}{\mathrm{\&Sigma;}}_{i=1}^{N-1}{h}_i---\left(1\right)$

To realize conformal constraint, it is desirable to which the BridgeCurve between two tracing points does not exist intersection, wherein G²Continuous and G³Even Continuous bridge joint is conformal to be constrained to：

|P_i-1B_1,i-1|+|A_1,iP_i|≤|P_i-1P_i|, i=2 ... N-1 (2)

|A_1,1P₁|≤|P₀P₁| (3)

|P_N-1B_1,N-1|≤|P_N-1P_N| (4)

To realize action error constraints, P only need to be controlled_iQ_iDistance be not more than iteration action error threshold d_i, i.e.,：

|P_iQ_i|≤d_i (5)

5. solve optimization problem be formula (1), while require BridgeCurve parameter meet constraint equation (2), (3), (4), (5) BridgeCurve parameter, is solved, that is, obtains the bridge joint track of iteration tracing point；

Step 3：Calculate the tracing point max value of error Δ of bridge joint track and initial trace point_k, and judge Δ_kWhether track is more than Point tolerance threshold value E, the computational methods of track point tolerance are calculating tracing point and two Bezier curve intersection pointsDistance, this Add k subscripts parameter to be equal to k subscript parameters are not added in step 2 in step 3, be the error-detecting of description kth time iteration, do not save Slightly k：

${\mathrm{\&Delta;}}_k={\max }_{0<i<N}\left|Q_i^{\left(k\right)}{P}_i^{*}\right|$

WhereinIt is located atAngular bisector on, according to the symmetry of interpolation curve, by calculatingSize and angular bisector vector obtain, computational methods are：

$t_i^{\left(k\right)}=\frac{P_i^{\left(k\right)}{P}_{i-1}^{\left(k\right)}}{\left|P_i^{\left(k\right)}{P}_{i-1}^{\left(k\right)}\right|}+\frac{P_i^{\left(k\right)}{P}_{i+1}^{\left(k\right)}}{\left|P_i^{\left(k\right)}{P}_{i+1}^{\left(k\right)}\right|}$

${\mathrm{sin\&theta;}}_i^{\left(k\right)}=\sqrt{\frac{1}{2}+\frac{P_i^{\left(k\right)}{P}_{i-1}^{\left(k\right)}\&CenterDot;P_i^{\left(k\right)}{P}_{i+1}^{\left(k\right)}}{2\left|P_i^{\left(k\right)}{P}_{i-1}^{\left(k\right)}\right|\left|P_i^{\left(k\right)}{P}_{i+1}^{\left(k\right)}\right|}},i=1,...N-1$

If Δ_k>E, illustrates that BridgeCurve does not meet tracing point error requirements, and iteration tracing point and iteration string are updated into step 4 Error threshold high；Otherwise, interpolation success, exits iteration, derives interpolation track；

Step 4：Iterative parameter is updated, the adjustment vector for making iteration tracing point is： Iteration tracing point after adjustment is：Iteration action error threshold It is worth and is：Update iterations：K=k+1, goes to step two.

5. the industrial robot fairing trace generator method of Controllable Error according to claim 1, it is characterised in that In the step S3, the interpolation of robot pose specifically includes following steps：

Step one：Calculate offset point, noteThe X-axis unit vector of the tool coordinates system at place is Z axis unit vector isWillOne section of fixed range m is offset along X-axis, X-axis offset point is obtained and is designated asWillOne section of fixed range n is offset along Z axis, Z axis offset point is obtained and is designated as

Step 2：To X-axis offset point and Z axis skew point interpolation, using the method for track point interpolation respectively to X-axis offset point, Z axis Offset pointEnter row interpolation, obtain piecewise interpolation curve Q (t), R (t)；

Step 3：The attitude of the track after interpolation is calculated, the piecewise interpolation curve for remembering robot trajectory's point is P (t), after interpolation X-axis vector：X (t)=O (t)-P (t), Z axis vector：Z (t)=R (t)-P (t), at X-axis vector, Z axis vector units orthogonalization Reason, is further calculated unit after interpolation orthogonal X-axis unit vector, Z axis unit vector：

X-axis unit vector X after interpolation^newT () is：

$X^{new}\left(t\right)=\frac{O\left(t\right)-P\left(t\right)}{|O\left(t\right)-P\left(t\right)|}\left(\right|O\left(t\right)-P\left(t\right)|\&NotEqual;0)$

Z axis unit vector Z after interpolation^newT () is：

Z^new(t)=F (t) × X^new(t)×F(t)

Wherein

According to X derived above^new(t), Z^newT the attitude curve after () description interpolation, obtains each track interpolation curve upper rail The corresponding attitude information of mark point.

6. the industrial robot fairing trace generator method of Controllable Error according to claim 5, it is characterised in that In the step of step S3 one, the value of offset distance m, n is set to 5~10 times of tracing point average headway.

7. the industrial robot fairing trace generator method of Controllable Error according to claim 5, it is characterised in that In the step of step S3 three, the attitude sectional curve after unit orthogonalization has segmentation G²Or G³Continuity, i.e., in segmentation Curve G between point²Or G³Continuously, at least there is G at waypoint⁰Continuity.