CN102152307A - Inclination-angle-constraint-based kinematic calibration method for Stewart parallel robot - Google Patents

Abstract

The invention discloses an inclination-angle-constraint-based kinematic calibration method for a Stewart parallel robot. The method comprises the following steps of: firstly, theoretically establishing a novel kinematic constraint, which is an inclination angle constant constraint, namely keeping two inclination angles of a motion platform of the Stewart parallel robot relative to a horizontal plane constant group by group; secondly, using a servo regulating way to physically realize the established kinematic constraint in a high-precision way; thirdly, establishing a calibration model on the basis of a principle of least square according to the kinematic constraint; and finally, identifying a model parameter by solving a nonlinear least square optimizing problem and compensating for the model parameter in a robot control software. By making full use of a characteristic that a repeating precision of a measuring instrument is superior to a position measuring precision of the measuring instrument, the method has the advantages of good calibration effect, simple measurement, high automatism in the calibrating process and the like.

Description

A kind of Stewart Kinematics of Parallel Robot scaling method based on the inclination angle constraint

Technical field:

The invention belongs to automatic field, relate to a kind of Stewart Kinematics of Parallel Robot scaling method, particularly a kind of scaling method based on the inclination angle constraint.

Background technology:

Advantage such as the Stewart parallel robot has the structural rigidity height, bearing capacity is strong, dynamic property good and accumulated error is little has obtained extensive use in fields such as industrial production, scientific research and services for life.Kinematic accuracy is to weigh an important indicator of parallel robot service behaviour, and it is subjected to robot parts manufacturing and all multifactor combined influences such as alignment error, departure, joint and hinge gap, thermal deformation and vibration.Wherein, the manufacturing of parts and alignment error are to influence the move main factor of terminal pose (position and attitude) precision of parallel robot, can carry out identification and compensation by the method that kinematics is demarcated.

The basic principle that kinematics is demarcated is: according to the difference instrument error functional between actual detected information and the ideal model output, utilize the actual geometric parameter of certain mathematical method identification parallel robot, and by revising the purpose that the model nominal value of controlling in the software reaches accuracy compensation.Complete kinematics calibration process comprises measurement, modeling, identification and four steps of compensation usually.Characteristics according to measuring process can be divided into existing scaling method outer demarcation method and self-calibrating method two big classes.Outer demarcation method need be used all or part of posture information of the direct or indirect measurement parallel robot motion platform of external observation equipment, based on the structure of the residual error between measured value and model calculated value object function, set up peg model then by minimizing object function.This scaling method has advantages such as principle is simple, feasibility strong, demarcation is effective.Yet, obtaining very difficulty of high-precision space rigid body posture information, required measurement device costs dearly and measurement process complexity usually.Self-calibrating method has avoided detecting the move difficulty of terminal posture information of parallel robot, usually based on the robot interior movable information or utilize mechanical locking to apply kinematic constraint and set up peg model.With respect to traditional outer demarcation method, these class methods have that measuring process is simple, good economy performance and be convenient to realize advantage such as precision online compensation.Yet, redundant sensor is installed on parallel robot passive joint hinge is obtained internal motion information not a duck soup, need consider in advance in the design phase of robot, can't be applicable to the robot device who has built moulding, and still need and consider the influence of the installation of redundant sensor the passive joint mechanical strength.Utilize mechanical locking to apply kinematic constraint and will reduce parallel manipulator robot end freedom of motion, require robot that the ability that the joint has passive operation and provides amount of exercise to measure is provided, thereby can't be applicable to numerous parallel robots that drive in " motor+ball-screw " mode.Simultaneously, use mechanical locking will reduce the originally narrow and small working space of parallel robot greatly, the selection limited space that makes calibration measurements position shape is unfavorable for fully encouraging the architectural characteristic of robot.In addition, rely on mechanical locking to realize kinematic constraint in calibration process, produce leverage easily between member and cause deformation, the lighter influences the accuracy of calibration result, also may destroy the passive joint hinge of " fragility " relatively when serious.

Summary of the invention:

The application's invention has proposed a kind of Stewart Kinematics of Parallel Robot scaling method that utilizes servo regulative mode to realize the constant constraint in inclination angle on the basis of the relatively more existing kinematics scaling method pluses and minuses of analysis-by-synthesis.It has avoided the move difficulty of terminal posture information of accurate measurement parallel robot, overcome and relied on mechanical locking to realize many defectives of kinematic constraint, the characteristics that general measure instrument repeatable accuracy is better than positional precision have been made full use of, the demarcation effect does not rely on the positional precision of measuring instrument, demarcates and measuring process automaticity height.

The present invention is achieved by the following technical solutions: a kind of Stewart Kinematics of Parallel Robot scaling method based on the inclination angle constraint may further comprise the steps:

Step (1) tectonic movement constraint: can come tectonic movement to retrain from parallel robot internal motion variable (comprise passive joint corner that fixed platform and motion platform are installed etc.) and terminal movement locus two aspects.The present invention is based on the latter considers, a kind of novel kinematic constraint has been proposed---the constant constraint in inclination angle, its basic thought can be expressed as: choose many groups and measure position shape in calibration process, measure shape place, position at each that belongs to same group, two inclination angles of horizontal plane are constant relative to the earth to keep motion platform; And, make that corresponding inclination angle value is inequality not between on the same group the measurement position shape.Make α _{X '}And α _{Y '}Two inclination angles of the relative horizontal plane of expression parallel robot motion platform, then the mathematical description of the constant constraint in inclination angle is:

$\left\{\begin{array}{c}{\mathrm{\α}}_{x^{\′}(i_1,i_1)}={\mathrm{\α}}_{x^{\′}(i_2,j_2)},i_1=i_2\\ {\mathrm{\α}}_{y^{\′}(i_1,j_1)}={\mathrm{\α}}_{y^{\′}(i_2,j_2)},i_1=i_2\\ {\mathrm{\α}}_{x^{\′}(i_1,j_1)}\≠{\mathrm{\α}}_{x^{\′}(i_2,j_2)},i_1\≠i_2\\ {\mathrm{\α}}_{y^{\′}(i_1,j_1)}\≠{\mathrm{\α}}_{y^{\′}\left(i_{2,}{j}_2\right)},i_1\≠i_2\end{array}\right.,i_1,i_2=\mathrm{1,2},...,m,j_1,j_2=\mathrm{1,2},...,n$ Formula (one);

In the formula:

M---measure position shape packet count, m ∈ N, the arbitrary value of the desirable m that satisfies condition 〉=2;

N---the position shape number in every group, n ∈ N, the arbitrary value of the desirable n that satisfies condition 〉=2 and mn 〉=16;

Step (2) is utilized servo regulative mode physics realization kinematic constraint: basic principle can be expressed as: use the checkout equipment of externally measured instrument as parallel robot output information, make up the metrical information feedback channel, design servo adjustment control, form measured closed loop servo regulating system.According to the deviation between measured and the set-point, produce the initiatively regulated quantity in joint of parallel robot by servo adjustment control, import the pose output that correspondingly changes motion platform by the continuous active joint of adjusting, make it to satisfy the kinematic constraint form of being constructed.

Specifically, the method that realizes kinematic constraint by servo regulative mode is described with the constant example that is constrained in inclination angle.A double-shaft tilt angle instrument (mounting means as shown in Figure 1) is installed on the parallel robot motion platform, is made up inclination angle closed-loop regulating system as shown in Figure 2.Wherein, Guan Jian tilt adjustment process can be divided into three phases:

At first, data acquisition and pretreatment stage.Inclinator sends to the parallel robot upper control computer by serial communication (perhaps other communication modes) with measurement data, carries out the data preliminary treatment by host computer then.The mode of taking is: remove the maximum and the minimum of a value of many group measurement data, ask for the measurement result of average as this then.

Secondly, the long update stage of drive rod command bar.Whether host computer procedure at first judges the deviation of measurement of dip angle value and set-point less than the given limits of error, if satisfy, then the tilt adjustment process finishes.Otherwise, the set-point of robot pose vector when calculating next step adjusting, and further calculate bar long vector set-point against separating model by kinematics.Host computer sends to slave computer with the long set-point of bar, begins the long measured value of bar of waiting for that reception is uploaded by slave computer then.If both deviations are returned collection, processing and the judgement of carrying out inclination data less than the long regulating error limit of given bar, and begin next step long renewal of command bar; Otherwise, continue to send the current long set-point of bar, limit less than the long regulating error of the bar of setting until the deviation of long measured value of bar and set-point.

At last, the long adjusting stage of bar.Slave computer constitutes a position closed loop control loop by servomotor controller, motor, encoder, data acquisition channel.Behind the long set-point of bar that receives the host computer transmission, slave computer compares set-point and the long measured value of bar that is obtained by the motor encoder feedback, produce the long deviation of bar, regulate strategy according to PID and produce speed command, flexible by servo controller control driven by motor ball-screw, make bar reach set-point.

Step (3) is set up peg model.To should keep constant amount as the kinematic constraint variable in the kinematic constraint of being constructed, foundation can reflect the residual vector of measuring kinematic constraint variable difference between the shape of position on the same group.Quadratic sum minimum with the control residual vector is a purpose structure object function, sets up the peg model that satisfies the principle of least square, and kinematics demarcation problem is converted into the non-linear least square optimization problem.

Step (4) identification model parameter and compensation.By finding the solution the parameter identification of non-linear least square problem realization peg model, realize the kinematic accuracy compensation of robot by design (name) value of revising kinematics model parameter in the parallel robot control software.

The present invention has tangible creativeness and beneficial effect.The constant constraint in the parallel robot of Stewart described in invention inclination angle is a kind of novel kinematic constraint, can be used as to set up rationally the theoretical foundation of peg model efficiently.Described method high-precision kinematic constraint of being constructed of having realized on the basis of the repeatable accuracy that makes full use of measuring instrument of realizing kinematic constraint based on servo regulative mode, avoided the accurately dependence of measurement of parallel robot posture information, thoroughly restrained many drawbacks that clothes use mechanical locking to bring, solved the key issue of restriction based on kinematic constraint scaling method practicality.The described method of setting up the method for peg model and carrying out identification of Model Parameters and compensation can effectively obtain the parallel robot mechanism parameter error, significantly improves the pose accuracy of robot.

Description of drawings:

The mounting means signal of Fig. 1 double-shaft tilt angle instrument;

The theory diagram of Fig. 2 inclination angle servo adjustment system;

Fig. 3 demarcates the coordinate system signal;

Inclinator X-axis measurement data contrast before and after Fig. 4 demarcates;

Inclinator Y-axis measurement data contrast before and after Fig. 5 demarcates.

The specific embodiment:

Below in conjunction with accompanying drawing the present invention is done and to describe in further detail:

Referring to Fig. 1-5, a kind of Stewart Kinematics of Parallel Robot scaling method based on the inclination angle constraint may further comprise the steps:

At first, clearly demarcate the mode of setting up of coordinate system, as shown in Figure 3, wherein:

Kinetic coordinate system M ₁-x ₁' y ₁' z ₁'---with the joint hinge M on the motion platform ₁As coordinate origin; x ₁' beam warp is crossed M ₂Point, and edge

Direction; x ₁' y ₁' plane is by M ₁, M ₂And M ₆Determine, set up kinetic coordinate system for 3 according to the right-handed system principle;

Central motion coordinate system O _m-x ₂' y ₂' z ₂'---will be M ₁-x ₁' y ₁' z ₁' around self z ₁' axle 5 π/6 that turn clockwise move to the nominal geometric center O of motion platform then _mObtaining is O _m-x ₂' y ₂' z ₂';

Fixed coordinate system B ₁-x ₁y ₁z ₁---with the joint hinge B on the pedestal ₁As coordinate origin, z ₁Axle is defined as gravity in the other direction, by z ₁Axle and B ₂Point is determined x ₁z ₁Fixed coordinate system is set up according to the right-handed system principle in the plane;

Center fixation coordinate system O _b-x ₂y ₂z ₂---will be B ₁-x ₁y ₁z ₁Around self z ₁Axle 5 π/6 that turn clockwise move to the nominal geometric center O of pedestal then _bObtaining is O _b-x ₂y ₂z ₂

Inclinator coordinate system O _I-x _I' y _I' z _I'---coordinate origin is an inclinator diaxon intersection point, x _I' axle is that of inclinator measures axle, x _I' y _IThe plane of determining is intersected on ' plane for the inclinator diaxon.The installation site of inclinator and towards being is arbitrarily regulated for the ease of the pose of realizing motion platform, can make inclinator coordinate origin O _INominal geometric center O with motion platform _mOverlap x _I' axle is approximately perpendicular to straight line M ₁M ₆, x _I' y _I' plane approximation is parallel to plane M ₁M ₂M ₆

The position coordinates of joint hinge on the Stewart parallel robot motion platform in the central motion coordinate system, the joint hinge on the pedestal position coordinates and the long deviation of fixed bar of each drive rod of robot in the center fixation coordinate system constituted and needed the mechanism parameter of identification vector, is designated as q _α

Secondly, introduce the servo control method in inclination angle in detail:

The Step1 inclination data is gathered and preliminary treatment: gathering the measurement of dip angle data is the prerequisites of carrying out the servo adjusting in inclination angle, also is link the most consuming time in the whole calibrating procedure.Because inclinator measuring principle difference, its data acquiring frequency is also inequality.Under the prerequisite that guarantees the inclinator repeatable accuracy, should improve data acquiring frequency as much as possible.Simultaneously,,, after collecting 3 groups of measurement data, host computer promptly compares, with the median of gained inclination deviation result as this measurement with inclination angle setting value in the starting stage of tilt adjustment in order to accelerate tilt adjustment speed; After entering the inclination angle accurate adjustment stage, in order to guarantee the accuracy of measurement result, by host computer gather after 7 groups of data again with set-point relatively, reject maximum and minimum of a value in the inclination deviation, carry out arithmetic average then, as the result of this measurement.

Step2 parallel robot drive rod command bar is long to be upgraded: order

With

Deviation between actual output of expression inclinator and the set-point is at first judged

Whether this condition satisfies, wherein ε _αFor the tilt adjustment limits of error of setting, identical with the repeatable accuracy of inclinator.If condition satisfies, finish the tilt adjustment process, and the current long measured value of bar of record parallel robot; Otherwise, utilize the long measured value of bar to calculate the current pose vector p of motion platform by direct kinematics _Now=[p _xp _yp _zφ θ ψ] ^T, p wherein _x, p _y, p _zBe the position coordinates of central motion coordinate origin under the center fixation coordinate system, φ, θ, ψ are respectively toppling direction angle, flip angle and the anglec of rotation of motion platform.According to the mounting means of inclinator as can be known, the model calculated value of inclinator diaxon output

With

Be respectively:

$\left\{\begin{array}{c}{\mathrm{\α}}_{x_I^{\′}}\stackrel{\·}{=}\arcsin (\mathrm{c\φs\θs\ψ}+\mathrm{s\φc\ψ})\\ {\mathrm{\α}}_{y_I^{\′}}\stackrel{\·}{=}\arcsin (\mathrm{c\φs\θc\ψ}-\mathrm{s\φs\ψ})\end{array}\right.$ Formula (two);

In the formula:

S---SIN function sin writes a Chinese character in simplified form, below identical;

C---cosine function cos writes a Chinese character in simplified form, below identical;

Thereby can get:

$\left\{\begin{array}{c}\mathrm{\φ}=\arcsin (\mathrm{c\ψs}{\mathrm{\α}}_{x_I^{\′}}-\mathrm{s\ψs}{\mathrm{\α}}_{y_I^{\′}})\\ \mathrm{\θ}=\arcsin \left(\frac{\mathrm{s\ψs}{\mathrm{\α}}_{x_I^{\′}}+\mathrm{c\ψs}{\mathrm{\α}}_{y_I^{\′}}}{\mathrm{c\φ}}\right)\end{array}\right.$ Formula (three);

With inclination deviation

Respectively with

Stack back substitution following formula, the set-point of φ and θ in the time of can obtaining next step adjusting:

$\left\{\begin{array}{c}{\mathrm{\φ}}_{\mathrm{next}}=\arcsin (\mathrm{c\ψs}({\mathrm{\α}}_{x_I^{\′}}+{\mathrm{\Δ\α}}_{x_I^{\′}})-\mathrm{s\ψs}({\mathrm{\α}}_{y_I^{\′}}+{\mathrm{\Δ\α}}_{y_I^{\′}}))\\ {\mathrm{\θ}}_{\mathrm{next}}=\arcsin \left(\frac{\mathrm{s\ψs}({\mathrm{\α}}_{x_I^{\′}}+{\mathrm{\Δ\α}}_{x_I^{\′}})+\mathrm{c\ψs}({\mathrm{\α}}_{y_I^{\′}}+{\mathrm{\Δ\α}}_{y_I^{\′}})}{{\mathrm{c\φ}}_{\mathrm{next}}}\right)\end{array}\right.$ Formula (four);

Make remaining pose component remain unchanged, the set-point of motion platform pose vector was when then next step was regulated:

p _Next=[p _xp _yp _zφ _Nextθ _Nextψ] ^TFormula (five);

Further separate model, can calculate the set-point l of rod long vector according to Kinematics of Parallel Robot is contrary _Next

When host computer with the long set-point l of bar _NextAfter sending to slave computer, just begin the long measured value of bar of waiting for that reception is uploaded by slave computer.If the deviation that host computer procedure is judged both during less than 1 μ m, is returned collection, processing and the judgement of carrying out inclination data, and is begun next step long renewal of command bar; Otherwise, continue to send l _NextDeviation up to long measured value of bar and set-point satisfies the long regulating error limit of the bar of setting.Measuring average of shape place, position at one in the calibration process needs 4～5 times the long renewal of command bar can finish whole tilt adjustment process.

The Step3 bar is long to be regulated: in the long adjustment process of bar, slave computer constitutes a position closed loop control loop by servomotor controller, motor, encoder, data acquisition channel.Behind the long set-point of bar that receives the host computer transmission, slave computer compares set-point and the long measured value of bar that is obtained by the motor encoder feedback, produce the long deviation of bar, regulate strategy according to PID and produce speed command, flexible by servo controller control driven by motor ball-screw, make bar reach set-point.By selecting the precision ball screw of high-precision motor encoder and fine pitch for use, can effectively guarantee the long precision of regulating of bar.

Once more, the concrete method of setting up peg model of introducing:

According to the constant constraint in the described inclination angle of formula (), choose the inclination angle of the relative horizontal plane of central motion coordinate system

With As the kinematic constraint variable, they can be calculated by direct kinematics by the bar long vector of actual measurement; Set up the residual vector of kinematic constraint variable according to following formula:

Formula (six);

In the formula:

${\stackrel{\‾}{\mathrm{\α}}}_{x_2^{\′}\left(i\right)}-{\stackrel{\‾}{\mathrm{\α}}}_{x_2^{\′}\left(i\right)}=\frac{1}{n}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\α}}_{x_2^{\′}(i,j)};$

${\stackrel{\‾}{\mathrm{\α}}}_{y_2^{\′}\left(i\right)}-{\stackrel{\‾}{\mathrm{\α}}}_{y_2^{\′}\left(i\right)}=\frac{1}{n}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\α}}_{y_2^{\′}(i,j)};$

i——i＝1，2，...，m；

j——j＝1，2，...，n；

According to the principle of least square, the structure object function:

${\stackrel{\‾}{E}}_{\mathrm{\αobj}}={\left|\right|c_{\stackrel{\‾}{\mathrm{\α}}}\left|\right|}_2^2=c_{\stackrel{\‾}{\mathrm{\α}}}^T{c}_{\stackrel{\‾}{\mathrm{\α}}}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}[{({\mathrm{\α}}_{x_2^{\′}(i,j)}-{\stackrel{\‾}{\mathrm{\α}}}_{x_2^{\′}\left(i\right)})}^2+{({\mathrm{\α}}_{y_2^{\′}(i,j)}-{\stackrel{\‾}{\mathrm{\α}}}_{y_2^{\′}\left(i\right)})}^2]$ Formula (seven);

Kinematics demarcation problem is converted into the non-linear least square optimization problem shown in the following formula:

$\underset{q_{\mathrm{\α}}}{\min }{\stackrel{\‾}{E}}_{\mathrm{\αobj}}$ Formula (eight);

In the formula:

q _α---Stewart parallel robot mechanism parameter vector to be identified;

Formula (eight) is the peg model of setting up based on the constant constraint in inclination angle.

At last, the identification of peg model parameter and compensation:

Object function shown in the formula (seven) is the quadratic function about residual vector, can effectively find the solution by Levenberg-Marquardt (L-M) algorithm for this type of non-linear least square optimization problem.The L-M algorithm is the correction on Gauss-Newton method basis, and what its direction of search was smooth changes between Newton method and these two kinds of extreme cases of steepest descent method, has good numerical stability.After relatively obtaining parameter error with the model parameter nominal value, in parallel robot control software, revise, can effectively compensate the pose accuracy of robot.

Fig. 4 and Fig. 5 have showed the inclinator diaxon actual measured results that is installed in before and after the demarcation on the Stewart parallel robot motion platform respectively.Utilize the mechanism parameter nominal value of parallel robot and identifier as the control model parameter before and after demarcating respectively, measure the actual inclination angle of motion platforms at a plurality of constant shapes places of inclination angle nominal value.As can be seen, because the mechanism parameter of parallel robot exists than mistake before demarcating, make that the pose position error of motion platform is bigger, thereby cause the fluctuating of fluctuating of the actual measured value of inclinator diaxon in a big way, the uniformity at inclination angle is relatively poor; And through after demarcating, the mechanism parameter error of parallel robot has obtained effective compensation, and the pose positioning accuracy of motion platform significantly improves, and makes the fluctuation range of inclinator measured value obviously reduce, and reaches unanimity.

Above content is to further describing that the present invention did in conjunction with concrete preferred implementation; can not assert that the specific embodiment of the present invention only limits to this; for the general technical staff of the technical field of the invention; without departing from the inventive concept of the premise; can also make some simple deduction or replace, all should be considered as belonging to the present invention and determine scope of patent protection by claims of being submitted to.

Claims (4)

1. the Stewart Kinematics of Parallel Robot scaling method based on the inclination angle constraint is characterized in that, may further comprise the steps:

(1) tectonic movement constraint: choose many groups and measure position shapes in calibration process, measure shape place, position at each that belongs to same group, two inclination angles of horizontal plane are constant relative to the earth to keep motion platform; And, make that corresponding inclination angle value is inequality not between on the same group the measurement position shape; Make α _{X '}And α _{Y '}Two inclination angles of the relative horizontal plane of expression parallel robot motion platform, then the mathematical description of the constant constraint in inclination angle is:

$\left\{\begin{array}{c}{\mathrm{\α}}_{x^{\′}(i_1,i_1)}={\mathrm{\α}}_{x^{\′}(i_2,j_2)},i_1=i_2\\ {\mathrm{\α}}_{y^{\′}(i_1,j_1)}={\mathrm{\α}}_{y^{\′}(i_2,j_2)},i_1=i_2\\ {\mathrm{\α}}_{x^{\′}(i_1,j_1)}\≠{\mathrm{\α}}_{x^{\′}(i_2,j_2)},i_1\≠i_2\\ {\mathrm{\α}}_{y^{\′}(i_1,j_1)}\≠{\mathrm{\α}}_{y^{\′}\left(i_{2,}{j}_2\right)},i_1\≠i_2\end{array}\right.,i_1,i_2=\mathrm{1,2},...,m,j_1,j_2=\mathrm{1,2},...,n$ Formula (one);

In the formula:

M---measure position shape packet count, m ∈ N, the arbitrary value of the desirable m that satisfies condition 〉=2;

N---the position shape number in every group, n ∈ N, the arbitrary value of the desirable n that satisfies condition 〉=2 and mn 〉=16;

(2) utilize servo regulative mode physics realization kinematic constraint: use the checkout equipment of externally measured instrument, make up the metrical information feedback channel, design servo adjustment control, form measured closed loop servo regulating system as parallel robot output information.According to the deviation between measured and the set-point, produce the initiatively regulated quantity in joint of parallel robot by servo adjustment control, import the pose output that correspondingly changes motion platform by the continuous active joint of adjusting, make it to satisfy the kinematic constraint form of being constructed; Satisfy measurement position shape place of kinematic constraint at each, respectively drive sensor measurement joint variable initiatively on the joint by being installed in parallel robot;

(3) set up peg model: will should keep constant amount as the kinematic constraint variable in the kinematic constraint of being constructed, foundation can reflect the residual vector of measuring kinematic constraint variable difference between the shape of position on the same group; Quadratic sum minimum with the control residual vector is a purpose structure object function, sets up peg model based on the principle of least square, and kinematics demarcation problem is converted into the non-linear least square optimization problem;

(4) identification model parameter and compensation:, finally realize the kinematic accuracy compensation of robot by the design load of revising kinematics model parameter in the parallel robot control software by finding the solution the parameter identification of non-linear least square problem realization peg model.

2. a kind of according to claim 1 Stewart Kinematics of Parallel Robot scaling method based on the inclination angle constraint is characterized in that described step (2) may further comprise the steps:

A double-shaft tilt angle instrument is installed on Stewart parallel robot motion platform, is made up the inclination angle closed-loop regulating system;

Set up the relevant coordinate system of demarcating at first, in the following manner:

Kinetic coordinate system M ₁-x ₁' y ₁' z ₁'---with the joint hinge M on the motion platform ₁As coordinate origin; x ₁' beam warp is crossed M ₂Point, and along M ₁

Direction; x ₁' y ₁' plane is by M ₁, M ₂And M ₆Determine, set up kinetic coordinate system for 3 according to the right-handed system principle;

Central motion coordinate system O _m-x ₂' y ₂' z ₂'---will be M ₁-x ₁' y ₁' z ₁' around self z ₁' axle 5 π/6 that turn clockwise move to the nominal geometric center O of motion platform then _mObtaining is O _m-x ₂' y ₂' z ₂';

Fixed coordinate system B ₁-x ₁y ₁z ₁---with the joint hinge B on the pedestal ₁As coordinate origin, z ₁Axle is defined as gravity in the other direction, by z ₁Axle and B ₂Point is determined x ₁z ₁Fixed coordinate system is set up according to the right-handed system principle in the plane;

Center fixation coordinate system O _b-x ₂y ₂z ₂---will be B ₁-x ₁y ₁z ₁Around self z ₁Axle 5 π/6 that turn clockwise move to the nominal geometric center O of pedestal then _bObtaining is O _b-x ₂y ₂z ₂

Inclinator coordinate system O _I-x _I' y _I' z _I'---coordinate origin is an inclinator diaxon intersection point, x _I' axle is that of inclinator measures axle, x _I' y _IThe plane of determining is intersected on ' plane for the inclinator diaxon; Inclinator on motion platform the installation site and towards being arbitrarily, regulate for the ease of the pose of realizing motion platform, can make inclinator coordinate origin O _INominal geometric center O with motion platform _mOverlap x _I' axle is approximately perpendicular to straight line M ₁M ₆, x _I' y _I' plane approximation is parallel to plane M ₁M ₂M ₆

The position coordinates of joint hinge on the Stewart parallel robot motion platform in the central motion coordinate system, the joint hinge on the pedestal position coordinates and the long deviation of fixed bar of each drive rod of robot in the center fixation coordinate system constituted and needed the mechanism parameter of identification vector, is designated as q _α

Secondly, crucial tilt adjustment method is divided into following three steps:

The Step1 inclination data is gathered and preliminary treatment: because the measuring principle of dissimilar inclinator foundations is different, its data acquiring frequency is also inequality; Under the prerequisite that guarantees the inclinator repeatable accuracy, should improve data acquiring frequency as far as possible; Simultaneously, in order to accelerate tilt adjustment speed, in the starting stage of tilt adjustment, after collecting 3 groups of measurement data, the parallel robot upper control computer promptly compares, the median of 3 groups of inclination deviations of gained result as this measurement with inclination angle setting value; And in the inclination angle accurate adjustment stage, in order to guarantee the accuracy of measurement result, by host computer gather after 7 groups of data again with set-point relatively, maximum and the minimum of a value rejected in the inclination deviation are carried out arithmetic average then, as the result of this measurement;

Step2 parallel robot drive rod command bar is long to be upgraded: order

With

Deviation between actual output of expression inclinator and the set-point is at first judged

Whether this condition satisfies, wherein ε _αFor the tilt adjustment limits of error of setting, identical with the repeatable accuracy of inclinator; If condition satisfies, finish the tilt adjustment process, and the current long measured value of bar of record parallel robot; Otherwise, utilize the long measured value of bar to calculate the current pose vector p of motion platform by direct kinematics _Now=[p _xp _yp _zφ θ ψ] ^T, p wherein _x, p _y, p _zBe the position coordinates of central motion coordinate origin under the center fixation coordinate system, φ, θ, ψ are respectively toppling direction angle, flip angle and the anglec of rotation of motion platform.According to the mounting means of inclinator as can be known, the model calculated value of inclinator diaxon output With

For:

$\left\{\begin{array}{c}{\mathrm{\α}}_{x_I^{\′}}\stackrel{\·}{=}\arcsin (\mathrm{c\φs\θs\ψ}+\mathrm{s\φc\ψ})\\ {\mathrm{\α}}_{y_I^{\′}}\stackrel{\·}{=}\arcsin (\mathrm{c\φs\θc\ψ}-\mathrm{s\φs\ψ})\end{array}\right.$ Formula (two);

In the formula:

S---SIN function sin writes a Chinese character in simplified form, below identical;

C---cosine function cos writes a Chinese character in simplified form, below identical;

Thereby can get:

$\left\{\begin{array}{c}\mathrm{\φ}=\arcsin (\mathrm{c\ψs}{\mathrm{\α}}_{x_I^{\′}}-\mathrm{s\ψs}{\mathrm{\α}}_{y_I^{\′}})\\ \mathrm{\θ}=\arcsin \left(\frac{\mathrm{s\ψs}{\mathrm{\α}}_{x_I^{\′}}+\mathrm{c\ψs}{\mathrm{\α}}_{y_I^{\′}}}{\mathrm{c\φ}}\right)\end{array}\right.$ Formula (three);

With inclination deviation

Respectively with

Stack back substitution following formula, the set-point of φ and θ in the time of can obtaining next step adjusting:

$\left\{\begin{array}{c}{\mathrm{\φ}}_{\mathrm{next}}=\arcsin (\mathrm{c\ψs}({\mathrm{\α}}_{x_I^{\′}}+{\mathrm{\Δ\α}}_{x_I^{\′}})-\mathrm{s\ψs}({\mathrm{\α}}_{y_I^{\′}}+{\mathrm{\Δ\α}}_{y_I^{\′}}))\\ {\mathrm{\θ}}_{\mathrm{next}}=\arcsin \left(\frac{\mathrm{s\ψs}({\mathrm{\α}}_{x_I^{\′}}+{\mathrm{\Δ\α}}_{x_I^{\′}})+\mathrm{c\ψs}({\mathrm{\α}}_{y_I^{\′}}+{\mathrm{\Δ\α}}_{y_I^{\′}})}{{\mathrm{c\φ}}_{\mathrm{next}}}\right)\end{array}\right.$ Formula (four);

Make remaining pose component remain unchanged, the motion platform pose with the set-point of amount was when then next step was regulated:

p _Next=[p _xp _yp _zφ _Nextθ _Nextψ] ^TFormula (five);

Further separate model, can calculate the set-point l of rod long vector according to Kinematics of Parallel Robot is contrary _Next

When host computer with the long set-point l of bar _NextAfter sending to the next control computer, just begin the long measured value of bar of waiting for that reception is uploaded by slave computer; If the deviation that host computer procedure is judged both during less than 1 μ m, is returned collection, processing and the judgement of carrying out inclination data, and is begun next step long renewal of command bar; Otherwise, continue to send l _NextDeviation up to long measured value of bar and set-point satisfies the long regulating error limit of the bar of setting;

The Step3 bar is long to be regulated: in the long adjustment process of bar, slave computer constitutes a position closed loop control loop by servomotor controller, motor, encoder and data acquisition channel; Behind the long set-point of bar that receives the host computer transmission, slave computer compares set-point and the long measured value of bar that is obtained by the motor encoder feedback, produce the long deviation of bar, regulate strategy according to PID and produce speed command, flexible by servo controller control driven by motor ball-screw, make bar reach set-point.

3. a kind of according to claim 1 Stewart Kinematics of Parallel Robot scaling method based on the inclination angle constraint is characterized in that described step (3) comprising:

According to the constant constraint in the described inclination angle of formula (), choose the inclination angle of the relative horizontal plane of central motion coordinate system

With

As the kinematic constraint variable, they can be calculated by direct kinematics by the bar long vector of actual measurement; Set up the residual vector of kinematic constraint variable according to following formula:

Formula (six);

In the formula:

${\stackrel{\‾}{\mathrm{\α}}}_{x_2^{\′}\left(i\right)}-{\stackrel{\‾}{\mathrm{\α}}}_{x_2^{\′}\left(i\right)}=\frac{1}{n}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\α}}_{x_2^{\′}(i,j)};$

${\stackrel{\‾}{\mathrm{\α}}}_{y_2^{\′}\left(i\right)}-{\stackrel{\‾}{\mathrm{\α}}}_{y_2^{\′}\left(i\right)}=\frac{1}{n}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\α}}_{y_2^{\′}(i,j)};$

i——i＝1，2，...，m；

j——j＝1，2，...，n；

According to the principle of least square, the structure object function:

${\stackrel{\‾}{E}}_{\mathrm{\αobj}}={\left|\right|c_{\stackrel{\‾}{\mathrm{\α}}}\left|\right|}_2^2=c_{\stackrel{\‾}{\mathrm{\α}}}^T{c}_{\stackrel{\‾}{\mathrm{\α}}}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}[{({\mathrm{\α}}_{x_2^{\′}(i,j)}-{\stackrel{\‾}{\mathrm{\α}}}_{x_2^{\′}\left(i\right)})}^2+{({\mathrm{\α}}_{y_2^{\′}(i,j)}-{\stackrel{\‾}{\mathrm{\α}}}_{y_2^{\′}\left(i\right)})}^2]$ Formula (seven);

Kinematics demarcation problem is converted into the non-linear least square optimization problem shown in the following formula:

$\underset{q_{\mathrm{\α}}}{\min }{\stackrel{\‾}{E}}_{\mathrm{\αobj}}$ Formula (eight);

In the formula:

q _α---Stewart parallel robot mechanism parameter vector to be identified;

Formula (eight) is the peg model of setting up based on the constant constraint in inclination angle.

4. a kind of according to claim 1 Stewart Kinematics of Parallel Robot scaling method based on the inclination angle constraint is characterized in that described step (4) comprising:

Object function shown in the formula (seven) is the quadratic function about residual vector, can effectively find the solution by classical Levenberg-Marquardt (L-M) algorithm for this type of non-linear least square optimization problem; The L-M algorithm is the correction on Gauss-Newton method basis, and what its direction of search was smooth changes between Newton method and these two kinds of extreme cases of steepest descent method, has good numerical stability; Obtain the identifier of mechanism parameter by the solving-optimizing problem after, relatively can obtain the mechanism parameter error, revise the motion model parameters in the parallel robot control software then, effectively compensate the pose accuracy of robot with the mechanism parameter nominal value.

Images