CN105773622A - Industrial robot absolute accuracy calibrating method based on IEKF - Google Patents

Abstract

The invention discloses an industrial robot absolute accuracy calibrating method based on IEKF. The method is characterized by comprising the following steps: a parameter error model is built, and a laser tracking system is used for sampling data in a robot terminal position in a Cartesian space to obtain position errors under different coordinates; geometric parameter nominal values are used for building a kinetic model of a robot, and parameter vectors to be identified are built by combining with a vector product method to obtain a parameter Jacobian matrix; and parameter errors in the parameter error model are identified by using an IEKF algorithm, and parameter errors of the robot are obtained through iterative computation. Finally, the parameter errors are used for correcting the geometric parameter nominal values to finish kinematic calibration of the industrial robot so as to realize absolute accuracy calibration of the robot. The robot absolute accuracy calibrating method based on IEKF, provided by the invention, is suitable for any serial joint robot and any plane joint robot.

Description

A kind of industrial robot absolute precision calibration steps based on IEKF

Technical field

The present invention relates to Robot calibration technical field, particularly relate to a kind of industrial robot absolute precision calibration steps based on IEKF (iterative extended Kalman filter).

Background technology

The high-end high performance industrial robot of manufacturing industry urgent needs, this is also the important step improving whole manufacture system productivity, reduction production cost, raising product quality.The positioning precision of robot is an important indicator of reflection robot combination property, mainly includes repetitive positioning accuracy and absolute fix precision.At present, the repetitive positioning accuracy of industrial robot is higher, and its absolute fix precision is relatively low, it is difficult to meet the Production requirement of high accuracy industry spot.

The position error of robot is caused by many factors combined effect, is broadly divided into geometric error and non-geometric error.Wherein, the 80% of total error is accounted for geometrical factors such as the errors of the manufacture of each connecting rod of robot, assembling and installation, reference frame and actual coordinates.Therefore, industrial robot needs to utilize calibration technique that it is carried out Kinematic Calibration before use, picks out the parameter error of robot, geometric parameter nominal value is modified, thus the absolute fix precision of robot is calibrated.

Traditional robot localization precision calibration can be divided into: based on neutral net penalty method, based on interpolation thought penalty method, differential error penalty method, joint space penalty method.Compensate can be divided into again according to modeling pattern and have modelling by mechanism and the big class of Experimental modeling two.Differential error penalty method and joint space penalty method are a kind of modes compensated according to the Kinematics Law of robot, belong to and have modelling by mechanism.And neutral net penalty method and interpolation thought penalty method are research Robot Object Models, and estimate its input and the modeling method of output, belong to Experimental modeling, also known as method of black box.

Experimental Modeling mainly carries out data sampling with gridding sampled point in robot localization precision, then neural network model is utilized, or set up similarity relation, or adopt interpolating method that other errors in space are modeled, certain compensation effect can be reached.But at present problems faced is that the step-length of space lattice is determined owing to the difference of robot needs to carry out the substantial amounts of time in this respect, there is no theory support, and the data sampled need to be solidificated in the control system of robot, and data volume is very big so that the method actual application value is little.There is mechanism modeling method to be based on robot kinematics's rule identified parameters error, then geometric parameter nominal value is modified by error compensation.Current Chinese scholars proposes linear least squares method method, LM-LS (Levenberg-Marquardt least square) discrimination method, EKF (EKF) discrimination method etc., least square method in use easily first enters singularity problem, and scholar proposes LM-LS algorithm for this situation.Some scholars proposes EKF method and adopts precision during linear least squares method low to make up, and the problem such as identification process is consuming time, but EKF is by Taylor series expansion and ignore higher order term nonlinear system model is carried out linearisation, this cannot can introduce truncated error with being avoided, and causes that the precision to robot parameter error identification is still poor.

Summary of the invention

The present invention seeks to for industrial robot absolute fix precision low, it is proposed to a kind of industrial robot absolute precision calibration steps based on IEKF.

To achieve these goals, the present invention is achieved through the following technical solutions:

The first step: set up robot kinematics's model；

Second step: set up robot parameter error model；

3rd step: carry out data sampling in robot cartesian space；

4th step: utilize Vector product to build parameter vector, calculates the parameter Jacobian matrix J under different pieces of information sampling_j, j=1,2 ... k, k are data sampling number；

5th step: utilize IEKF algorithm that the parameter error in parameter error model is carried out identification, by iterative computation, when certain conditions are met, obtains the parameter error Δ s picked out；

6th step: geometric parameter nominal value is modified by the parameter error picked out, it is achieved the absolute precision calibration of robot.

According to above technical scheme, it is possible to achieve following beneficial effect:

(1) the industrial robot absolute precision calibration steps based on IEKF of the present invention is applicable to any series connection revolute robot and any selective compliance assembly robot arm, method universal strong；

(2) parameter error model of the present invention considers all geometric parameters of robot body, is compensated by the parameter error after identification in robot geometric parameter nominal value, closer to realistic model, it is possible to robot precision realizes calibration effectively；

(3) carrying out identification owing to being directed to parameter error, the valid data after identification are little, will not bring data storage problem；

(4) present invention adopts IEKF to constitute identification algorithm, and compared to method of least square and modified model method of least square, its identification speed is fast, and precision is high；Compared to EKF algorithm, decrease the linearized stability of nonlinear filtering, approach the time of day of robot parameter better, improve identification precision.

Accompanying drawing explanation

The robot cartesian space data sampling schematic diagram of Fig. 1 present invention；

The concrete operations flow process of Fig. 2 present invention.

Detailed description of the invention

For making the object, technical solutions and advantages of the present invention etc. clearly understand, below in conjunction with embodiment, and with reference to accompanying drawing, the present invention is described in more detail.

A kind of industrial robot absolute precision calibration steps based on IEKF, operational flowchart is as in figure 2 it is shown, said method comprising the steps of:

The first step: set up robot kinematics's model, comprise the following steps:

Step (1), uses D-H rule to build robot kinematics's model.

In D-H rule, the kinematic relation between adjacent two bars is:

$T_{i-1}^i=\left[\begin{array}{cccc}{\mathrm{cos\θ}}_i& -{\mathrm{sin\θ}}_i{\mathrm{cos\α}}_i& {\mathrm{sin\θ}}_i{\mathrm{sin\α}}_i& a_i{\mathrm{cos\θ}}_i\\ {\mathrm{sin\θ}}_i& {\mathrm{cos\θ}}_i{\mathrm{cos\α}}_i& -{\mathrm{cos\θ}}_i{\mathrm{sin\α}}_i& a_i{\mathrm{sin\θ}}_i\\ 0& {\mathrm{sin\α}}_i& {\mathrm{cos\α}}_i& d_i\\ 0& 0& 0& 1\end{array}\right]---\left(1\right)$

In formula, for the motion of connecting rod i Yu connecting rod i-1Relation, a_iFor length of connecting rod, d_iFor connecting rod offset distance, α_iFor joint torsional angle, θ_iFor joint rotation angle.

Step (2), is defined as successively by each homogeneous transform matrixBeing always transformed to then from the pedestal of robot to robot end tool coordinates system:

$T_n^0=T_1^{0}T_2^1\mathrm{...}T_n^{n-1}=\left[\begin{array}{cccc}{n}_n& o_n& a_n& g_n\\ 0& 0& 0& 1\end{array}\right]---\left(2\right)$

Wherein n_n、o_n、a_nFor the direction vector of robot end, g_nFor the position of robot end, n is joint of robot number, for the robot for a six degree of freedom, correspond to 6 homogeneous transform matrix.

Step (3), according to homogeneous transform matrix, it is possible to obtain the theoretical coordinate P of robot end_t。

P_t=F (a, d, α, θ) (3)

Second step: set up robot parameter error model, comprise the following steps:

Step (1), due to the existence of parameter error, the actual coordinate P of robot end_m, P_mFor measuring gained.

P_m=F (a, d, α, θ, Δ s)=F (a+ Δ a, d+ Δ d, α+Δ α, θ+Δ θ) (4)

Wherein, Δ s is made up of Δ a, Δ d, Δ α, Δ θ, for the parameter error that robot exists.

Step (2), calculates the difference Δ P of physical location and theoretical position.

Δ P=P_m-P_t

(5)

=F (a+ Δ a, d+ Δ d, α+Δ α, θ+Δ θ)-F (a, d, α, θ)

Formula (5) is carried out linearization process

$\begin{array}{c}\mathrm{\Δ}P=\frac{\∂P}{\∂a}\mathrm{\Δ}a+\frac{\∂P}{\∂d}\mathrm{\Δ}d+\frac{\∂P}{\∂\mathrm{\α}}\mathrm{\Δ}\mathrm{\α}+\frac{\∂P}{\∂\mathrm{\θ}}\mathrm{\Δ}\mathrm{\θ}\\ =J\·\mathrm{\Δ}s\end{array}---\left(6\right)$

Wherein, Δ s is the parameter error of robot, Δ s=[Δ a₁Δd₁Δα₁Δθ₁…Δa_nΔd_nΔα_nΔθ_n]^T, J is a parameter Jacobian matrix relevant with Δ P and Δ s,N is joint of robot number.General J can pass through Vector product and calculate acquisition, and the column vector of J is calculated by below equation:

J_ai=[n_i], J_di=[a_i], J_αi=[n_i×g_i], J_θi=[a_i×g_i](7)

Wherein, n_i、o_i、a_iIt is the direction vector of i-th connecting rod, is the position relative to end effector about i-th connecting rod.

3rd step: carry out data sampling in robot cartesian space, comprise the following steps:

Initially set up the transformational relation between robot cartesian coordinate system and laser tracking system coordinate system before measurement, the present embodiment supposes had built up coordinate transformation relation.

The cartesian space of robot chooses k coordinate points as far as possible uniformly, makes robot arrive theoretical j point coordinates P from dead-center position with random attitude_{T, j}, and the joint rotation angle θ of corresponding record robot demonstrator_j, use laser tracking system to measure conversion and draw the actual j point coordinates P of robot_{M, j}, j=1,2 ... k.

4th step: calculate the parameter Jacobian matrix J under different coordinates_j, j=1,2 ... k, J_jBeing the matrix of 3 × m, m is number of parameters to be identified.

In real process, the direction difficulty in each joint of robot measurement is very big, and the mode being generally adopted Vector product calculating draws.

Corresponding calculating, when the theoretical cartesian coordinate of jth point, calculates in process successivelyNow can calculate the n of correspondence_{I, j}、o_{I, j}、q_{I, j}、g_{I, j}, calculate each joint at the formula provided according to step 2Finally build parameter Jacobian matrix J_j。

$J_j=\left[\begin{array}{ccccccccc}{J}_{a_l,j}& J_{d_1,j}& J_{{\mathrm{\α}}_1,j}& J_{{\mathrm{\θ}}_1,j}& \mathrm{...}& J_{a_n,j}& J_{d_n,j}& J_{{\mathrm{\α}}_n,j}& J_{{\mathrm{\θ}}_n,j}\end{array}\right]---\left(8\right)$

Wherein, n is joint of robot number, i=1,2 ... n, j=1,2 ... k.

5th step: utilize IEKF algorithm that the parameter error in parameter error model is carried out identification, the parameter error Δ s, Δ s that obtain picking out are the matrix of m × 1, and m is number of parameters to be identified.

IEKF algorithm is used to determine the parameter error Δ s of robot, is used for by IEKF algorithm in robot geometric parameter error identification, and step is as follows.

Step (1), calculates the observation vector Z of jth point_j:

Z_j=Δ P_j=P_{M, j}-P_{T, j}J=1,2 ... k (9)

Wherein, Z_jIt it is the matrix of 3 × 1.

Step (2), initiation parameter error

${\mathrm{\Δs}}_{j-1|j-1}^i=0---\left(10\right)$

Wherein,For the matrix of m × 1, m is number of parameters to be identified；I=1, j=1, i are iteration count, i=1,2 ... w, w are maximum iteration time；J is data acquisition sampling point, j=1,2 ... k；

Step (3), state one-step prediction:

${\mathrm{\Δs}}_{j|j-1}^i={\mathrm{\Δs}}_{j-1|j-1}^i---\left(11\right)$

Wherein,State estimation for the sampled point j-1 of ith iteration；Status predication for the sampled point j of ith iteration.

Step (4), it was predicted that error covariance matrix:

$S_{j|j-1}^i=S_{j-1|j-1}^i+Q---\left(12\right)$

Wherein,Error covariance matrix for the sampled point j-1 of ith iteration is estimated；For the error covariance matrix prediction of the sampled point j of ith iteration, for the matrix of m × m, m is number of parameters to be identified.Q is the variance intensity battle array of process noise W, draws by process signal is carried out mathematical statistics, in industrial robot, generally takes empirical value Q=10^-4I_m×m, m is number of parameters to be identified.

Step (5), updates filtering gain matrix:

$K_j^i=S_{j|j-1}^i{\left(J_j\right)}^T{\[J_j{S}_{j|j-1}^i{\left(J_j\right)}^T+R\]}^{-1}---\left(13\right)$

Wherein,For the filtering gain matrix of the sampled point j-1 of ith iteration, for the matrix of m × 3, m is number of parameters to be identified.R is the variance intensity battle array of observation noise V, obtains according to the certainty of measurement of laser tracking system, is the matrix of 3 × 3.

Step (6), updates state estimation:

${\mathrm{\Δs}}_{j|j}^i={\mathrm{\Δs}}_{j|j-1}^i+K_j^i\{{\tilde{Z}}_j-J_j\mathrm{\Δ}{\tilde{s}}_j\}---\left(14\right)$

Wherein,For observing new breath,Newly cease for state,State estimation for the sampled point j of ith iteration.

This method is in that from the different of EKF identification algorithm, the state-updating mode using IEKF adopts the new breath of observation and state newly to cease and be updated, and EKF identification algorithm adopts observation vector to be updated, the error that IEKF identification algorithm enables to parameter estimation is therefore used to reduce further.

Step (7), estimation error variance battle array:

$S_{j|j}^i=(I-K_j^i{J}_j)S_{j|j-1}^i---\left(15\right)$

Step (8), updates observation vector

$P_{i,j}^i=F(a,d,\mathrm{\α},\mathrm{\θ},{\mathrm{\Δs}}_{j|j}^i)---\left(16\right)$

$Z_j^i=P_{m,j}-P_{t,j}^i---\left(17\right)$

Wherein,For theoretical correction position,For observation vector.

Step (9), updates iterations i or j.If all sampled datas are used in both, then i is incremented by, and j is from k to 1；Otherwise, next sampled data is used.

Repeat step (3) to (9).OrderIf d_min> d, then d_min=d,Otherwise,Wherein d_minFor minimum average B configuration error.When continuous h iteration is all without being updated, iteration stopping, then it is assumed that parameter error value has restrained stable, now obtain the parameter error Δ s of the best, general h takes 4.

6th step: geometric parameter nominal value is modified by the parameter error picked out, it is achieved the absolute precision calibration of robot.The parameter error picked out is Δ s, s_g=s_n+ Δ s, s_nFor robot geometric parameter nominal value, s_gFor robot geometric parameter actual value.

Claims (3)

1., based on an industrial robot absolute precision calibration steps of IEKF, specifically include following step:

The first step: set up robot kinematics's model；

Second step: set up robot parameter error model；

3rd step: carry out data sampling in robot cartesian space；

4th step: utilize Vector product to build parameter vector, calculates the parameter Jacobian matrix J under different pieces of information sampling_j, j=1,2 ... k, k are data sampling number；

5th step: utilize IEKF algorithm that the parameter error in parameter error model is carried out identification, by iterative computation, when certain conditions are met, obtains the parameter error picked out；

6th step: geometric parameter nominal value is modified by the parameter error picked out, it is achieved the absolute precision calibration of robot.

2. a kind of industrial robot absolute precision calibration steps based on IEKF according to claim 1, it is characterized in that: described 5th step utilizes IEKF algorithm that the parameter error in parameter error model is carried out identification, obtain the parameter error Δ s picked out, comprise the following steps:

Step (1), calculates the observation vector Z of jth point_j:

Z_j=Δ P_j=P_{M, j}-P_{I, j}J=1,2 ... k (1)

Wherein, Z_jIt it is the matrix of 3 × 1；

Step (2), initiation parameter error

${\mathrm{\Δs}}_{j-1|j-1}^i=0---\left(2\right)$

Wherein,For the matrix of m × 1, m is number of parameters to be identified；I=1, j=1, i are iteration count, i=1,2 ... w, w are maximum iteration time；J is data acquisition sampling point, j=1,2 ... k；

Step (3), state one-step prediction:

${\mathrm{\Δs}}_{j|j-1}^i={\mathrm{\Δs}}_{j-1|j-1}^i---\left(3\right)$

Wherein,State estimation for the sampled point j-1 of ith iteration；Status predication for the sampled point j of ith iteration；

Step (4), it was predicted that error covariance matrix:

$S_{j|j-1}^i=S_{j-1|j-1}^i+Q---\left(4\right)$

Wherein,Error covariance matrix for the sampled point j-1 of ith iteration is estimated；For the error covariance matrix prediction of the sampled point j of ith iteration, for the matrix of m × m, m is number of parameters to be identified；Q is the variance intensity battle array of process noise W, draws by process signal is carried out mathematical statistics, in industrial robot, generally takes empirical value Q=10^-4I_m×m, m is number of parameters to be identified；

Step (5), updates filtering gain matrix:

$K_j^i=S_{j|j-1}^i{\left(J_j\right)}^T{\[J_j{S}_{j|j-1}^i{\left(J_j\right)}^T+R\]}^{-1}---\left(5\right)$

Wherein,For the filtering gain matrix of the sampled point j-1 of ith iteration, for the matrix of m × 3, m is number of parameters to be identified.R is the variance intensity battle array of observation noise V, obtains according to the certainty of measurement of laser tracking system, is the matrix of 3 × 3；

Step (6), updates state estimation:

${\mathrm{\Δs}}_{j|j}^i={\mathrm{\Δs}}_{j|j-1}^i+K_j^i\{{\tilde{Z}}_j-J_j\mathrm{\Δ}{\tilde{s}}_j\}---\left(6\right)$

Wherein,For observing new breath, Newly cease for state, State estimation for the sampled point j of ith iteration；

This method is in that from the different of EKF identification algorithm, the state-updating mode using IEKF adopts the new breath of observation and state newly to cease and be updated, and EKF identification algorithm adopts observation vector to be updated, the error that IEKF identification algorithm enables to parameter estimation is therefore used to reduce further；

Step (7), estimation error variance battle array:

$S_{j|j}^i=(I-K_j^i{J}_j)S_{j|j-1}^i---\left(7\right)$

Step (8), updates observation vector

$P_{t,j}^i=F(a,d,a,\mathrm{\θ},{\mathrm{\Δs}}_{j|j}^i)---\left(8\right)$

$Z_j^i=P_{m,j}-P_{t,j}^i---\left(9\right)$

Wherein,For theoretical correction position,For observation vector.

Step (9), updates iterations i or j.If all sampled datas are used in both, then i is incremented by, and j is from k to 1；Otherwise, next sampled data is used；

Repeat step (3) to (9).OrderIf d_min> d, then d_min=d,Otherwise,Wherein d_minFor minimum average B configuration error.When continuous h iteration is all without being updated, iteration stopping, then it is assumed that parameter error value has restrained stable, now obtain the parameter error Δ s of the best, general h takes 4.

3. a kind of robot absolute precision calibration steps based on IEKF according to claim 1, it is characterized in that: geometric parameter nominal value is modified by described 6th step by the parameter error picked out, realize the absolute precision calibration of robot, comprise the following steps:

The parameter error picked out is Δ s, s_g=s_n+ Δ s, s_nFor robot geometric parameter nominal value, s_gFor robot geometric parameter actual value.