CN103170979B - Online robot parameter identification method based on inertia measurement instrument - Google Patents

Abstract

The invention provides the online robot parameter identification method based on inertia measurement instrument, including step: (1) robot kinematics models, the joint of robot and attitude are linked together；(2) use the attitude of inertia measurement instrument robot measurement, and in conjunction with Quaternion Algorithm (FQA) and Kalman filtering (KFs), attitude data is estimated, thus obtaining stable robot pose；(3) the attitude identification machine Radix Ginseng number of robot is utilized.The present invention, by attitude data being estimated in conjunction with Quaternion Algorithm (FQA) and Kalman filtering (KFs), thus obtaining stable robot pose, and utilizes the parameter of the Attitude estimation robot of robot.

Description

Online robot parameter identification method based on inertia measurement instrument

Technical field

The present invention relates to robot calibration method, particularly to a kind of method of online robot parameter identification based on inertia measurement instrument.

Background technology

The industrial robot that great majority use at present remains employing teaching programming, particularly in automobile industry.But, the off-line programing industry of teaching programming as an alternative, its importance steadily improves.The main cause of this trend is that the demand shortening downtime and improving robot utilization rate is greatly increased.

In successful off-line programing example, robot not only has repeatability, and has accuracy.The repeatability of robot is not by the impact of programmed method, but is subject to the impact of random error (limited resolution such as combined coding device).By contrast, in the accuracy of robot, the systematic error of absolute fix is almost entirely and to be caused by robot off-line programming.Lack the one of the main reasons of accuracy, be not mating between the prediction of motion model and real system.Robot perseverance appearance error is owing to several sources, including the deviation that geometric parameter error (such as linkage length and combine skew) and shift in position are predicted.

One of problem hindering off-line programing development is the static immobilization of the low precision of robot system and dynamically positions.Robot calibration improves the positioning precision of robot, can be used to make diagnostic tool simultaneously, is widely used in the production and maintenance of robot.Robot static calibration is a process that numerical value is identified and New Mathematical Model realizes combining modeling, measurement, robot real physical characteristics.The measuring process of calibration process be determine real machine people " be best suitable for " parameter arrange institute requisite.The types entail measured combines tool location and corresponding co-location.Owing to the quality of combination is less than what obtained by initial data concentration, obtain one group of good data very important.Use one of video camera trend of video measuring system seemingly current robot calibration system's development carrying out calibration machine people, but being still limited by by the research group of big budget company sponsors in the knowledge involved by this field, the interest of its distribution technology details is little.

Summary of the invention

The present invention proposes the online robot parameter identification method based on inertia measurement instrument.Inertia measurement instrument is fixed on the distal portion of robot, by attitude data being estimated in conjunction with Quaternion Algorithm (FQA) and Kalman filtering (KFs), thus obtaining stable robot pose, and utilize the parameter of the Attitude estimation robot of robot.

The online robot parameter identification method based on inertia measurement instrument of the present invention, comprises the steps:

S1, robot kinematics's modeling, link together the joint of robot and attitude；

S2, using the attitude of inertia measurement instrument robot measurement, and in conjunction with Quaternion Algorithm (FQA) and Kalman filtering (KFs), attitude data being estimated, thus obtaining stable robot pose；

S3, utilize the attitude identification machine Radix Ginseng number of robot.

Model the robot kinematics's model obtained described in step S1 and meet the rule that following kinematics parameters is identified:

1) integrity: robot kinematics's model should have enough parameters to define any possible deviation standard value；

2) seriality: the minor variations of the necessary corresponding kinematics parameters of any minor variations of robot architecture；

3) minimality: kinematics model must only comprise the parameter of minimal amount.

Step S1 includes:

Position and the attitude of robot end is defined according to DH model；By the conversion of continuous uniform, the kinematical equation obtained is defined as:

$T_N^0=T_N^0\left(v\right)=T_1^0{T}_2^1...T_N^{N-1}=\underset{i=1}{\overset{N}{\mathrm{\Π}}}{T}_i^{i-1}---\left(1\right)$

Here definition robot body coordinate system is 0 coordinate system, and robot end's coordinate system is N coordinate system, and wherein N is the total number of link rod coordinate system or connecting rod；It is from 0 ordinate transform to the transition matrix of N coordinate system, namely from robot body ordinate transform to the transition matrix of robot end's coordinate system；Being the translation matrix being tied to i coordinate system from i-1 coordinate, i is connecting rod numbering, and the value of i is 1 ~ N；It is the parameter vector of robot, wherein contains connecting rod error；

v_i=v′_i+Δv_i(2)

Here v_iIt is the parameter vector of connecting rod i, v '_iIt is the scalar of connecting rod i,It it is the parameter error vector of connecting rod i；It is defined as according to the kinematical equation that DH model obtains:

$K_N^0=K_N^0\left(v\right)=K_1^0{K}_2^1...K_N^{N-1}=\underset{i=1}{\overset{N}{\mathrm{\Π}}}{K}_i^{i-1}---\left(3\right)$

With

$K_i^{i-1}=T_i^{i-1}(v_i^{\′}+{\mathrm{\Δv}}_i)=T_i^i+{\mathrm{\ΔT}}_i---\left(4\right)$

Consider connecting rod variable, have

$K_N^0=T_N^0+\mathrm{\ΔT},\mathrm{\ΔT}=\mathrm{\ΔT}(u,\mathrm{\Δv})---\left(5\right)$

HereWithDefinition is consistent, is all from 0 ordinate transform to the transition matrix of N coordinate system, is distinctive in thatBe actual value andIt it is ideal value；It it is the actual translation matrix being tied to i coordinate system from i-1 coordinate；Δ T is error translation matrix；Being joint angle vector, θ is joint angle；Therefore span is'sIt it is a function relevant to joint angle vector u and link parameters error vector Δ v.

Described step S2 includes:

One inertia measurement instrument is installed in the distal portion of measured robot and determines the rolling of robot end, pitching and deflection attitude；Inertia measurement instrument is by a triaxial accelerometer, two two axis gyroscope instrument and three axle magnetometer compositions；

Define three coordinate system: body coordinate system x_by_bz_b, inertia measurement instrument coordinate system x_sy_sz_sWith terrestrial coordinate system x_ey_ez_e；Inertia measurement instrument coordinate system x_sy_sz_sThe accelerometer of corresponding three orthogonal installations and the axle of magnetometer；Owing to inertia measurement instrument is rigidly mounted on robot end, it is assumed that body coordinate system x_by_bz_bWith inertia measurement instrument coordinate system x_sy_sz_sIn conjunction with；Earth fixed coordinate system x_ey_ez_eFollow north-Dong-under (NED) pact, wherein x_ePoint to north, y_ePoint to east and z_eUnder sensing；Inertia measurement instrument can measure the rolling of himself, pitching and deflection attitude；Definition is around x_eRotation φ represent rolling, around y_eThe rotation θ of axle represents pitching and around z_eThe rotation ψ of axle represents deflection；According to Euler's rotation theorem, from Eulerian angles φ, θ and ψ being converted to quaternary number q:

$q=\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\left[\begin{array}{c}\cos (\mathrm{\φ}/2)\cos (\mathrm{\θ}/2)\cos (\mathrm{\ψ}/2)+\sin (\mathrm{\φ}/2)\sin (\mathrm{\θ}/2)\sin (\mathrm{\ψ}/2)\\ \sin (\mathrm{\φ}/2)\cos (\mathrm{\θ}/2)\cos (\mathrm{\ψ}/2)-\cos (\mathrm{\φ}/2)\sin (\mathrm{\θ}/2)\sin (\mathrm{\ψ}/2)\\ \cos (\mathrm{\φ}/2)\sin (\mathrm{\θ}/2)\cos (\mathrm{\ψ}/2)+\sin (\mathrm{\φ}/2)\cos (\mathrm{\θ}/2)\sin (\mathrm{\ψ}/2)\\ \cos (\mathrm{\φ}/2)\cos (\mathrm{\θ}/2)\sin (\mathrm{\ψ}/2)-\sin (\mathrm{\φ}/2)\sin (\mathrm{\θ}/2)\cos (\mathrm{\ψ}/2)\end{array}\right]---\left(6\right)$

And being constrained to of four Euler Parameter:

$q_0^2+q_1^2+q_2^2+q_3^2=1---\left(7\right)$

Here q₀It is scalar component and (q₁,q₂,q₃) it is vector section；So be tied to the attitude cosine matrix of earth fixed coordinate system from inertia measurement instrument coordinateCan be represented as:

$M_s^e=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_0{q}_2+q_1{q}_3)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_0{q}_1+q_2{q}_3)& q_0^2-q_1^2-q_2^2-q_3^2\end{array}\right]---\left(8\right);$

There are white noise and random walk due to gyroscope and magnetometer, in order to obtain stable data, utilize Kalman filtering to estimate the state of inertia measurement instrument from one group of noisy and incomplete measurement；From the inertia measurement instrument state x of moment k-1_k-1Estimate the inertia measurement instrument state x of moment k_kTransfer equation be:

x_k=A_k·x_k-1+B·u_k-1+w_k-1(9)

z_k=H·x_k+v_k

Here x_kIt is the state vector of n × 1, represents the inertia measurement instrument state of moment k；A is the state transition matrix of n × n；B is the system input matrix of n × p；u_k-1Being the k-1 moment definitiveness input vector of p × 1, p is the number of definitiveness input；w_k-1It it is the n × 1 process noise vector in k-1 moment；z_kIt it is the m × 1 measurement vector in k moment；H is the observation matrix of m × n and v_kIt it is the measurement noise vector of m × 1；N=7, m=6 herein；With the quaternion differential equation of time correlation it is:

$\left[\begin{array}{c}{\∂q}_0/\∂t\\ {\∂q}_1/\∂t\\ {\∂q}_2/\∂t\\ {\∂q}_3/\∂t\end{array}\right]=\left[\begin{array}{cccc}{q}_0& {-q}_1& {-q}_2& {-q}_3\\ q_1& q_0& {-q}_3& q_2\\ q_2& q_3& q_0& {-q}_1\\ q_3& {-q}_2& q_1& q_0\end{array}\right]\·\left[\begin{array}{c}0\\ v_x/2\\ v_y/2\\ v_z/2\end{array}\right]---\left(10\right)$

Wherein v_x,v_y,v_zIt is that inertia measurement instrument is at axle x_s,y_s,z_sAngular velocity；Due to state x_kIncluding quaternary number state and angular velocity, so x_kThere is following form:

x_k=[q_0,kq_1,kq_2,kq_3,kv_x,kv_y,kv_z,k](11)

Here q_{0, k}, q_{1, k}, q_{2, k}, q_{3, k}, v_{X, k}, v_{Y, k},v_z,kBeing quaternary number state and the angular velocity in k moment, can obtain from equation (10), State Transferring equation is:

$A_k=\left[\begin{array}{ccccccc}1& 0& 0& 0& {-q}_{1,k}\·\mathrm{\Δt}/2& {-q}_{2,k}\·\mathrm{\Δt}/2& {-q}_{3,k}\·\mathrm{\Δt}/2\\ 0& 1& 0& 0& q_{0,k}\·\mathrm{\Δt}/2& q_{3,k}\·\mathrm{\Δt}/2& q_{2,k}\·\mathrm{\Δt}/2\\ 0& 0& 1& 0& q_{3,k}\·\mathrm{\Δt}/2& q_{0,k}\·\mathrm{\Δt}/2& {-q}_{1,k}\·\mathrm{\Δt}/2\\ 0& 0& 0& 1& {-q}_{2,k}\·\mathrm{\Δt}/2& q_{1,k}\·\mathrm{\Δt}/2& q_{0,k}\·\mathrm{\Δt}/2\\ 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]---\left(12\right)$

Here Δ t is sample time, owing to not controlling input, allows B=0^n×p, then B u_k-1=0^n×1；Using angular velocity to estimate quaternary number state, obtaining process noise vector is

w_k=[0000w_xw_yw_z]^T(13)

Here w_x,w_y,w_zIt it is the process noise of angular velocity；Owing to using gyroscope to measure angular velocity, observation matrix H is

H=[0^3×4I^3×3](14)

In order to meet (7), definitionThe definitiveness quaternary number q in k moment_kCan be standardized as:

q_k=[q_0,k/Dq_1,k/Dq_2,k/Dq_3,k/D](15)

In conjunction with formula (9) to formula (15), can from the inertia measurement instrument state x of moment k-1_k-1Estimate the inertia measurement instrument state x of moment k_k。

Described step S3 includes:

Assuming that measuring attitude number is s, then can again be expressed as from robot body ordinate transform to the kinesiology formula of robot end's coordinate system

$\hat{K}={\hat{K}}_N^0={(\hat{K}(u_1,v),\hat{K}(u_2,v),...,\hat{K}(u_s,v))}^T---\left(16\right)$

And

$\mathrm{\Δ}\hat{T}=\mathrm{\Δ}{\hat{T}}_N^0={(\mathrm{\Δ}\hat{T}(u_1,v),\mathrm{\Δ}\hat{T}(u_2,v),...,\mathrm{\Δ}\hat{T}(u_s,v))}^T---\left(17\right)$

Here u_i(i=1,2 ..., s) be ith measurement attitude joint angle vector；

Reference formula (3), (4), (5), hereDefinition withDefinition consistent,Definition with Δ T is consistent；

The target of kinesiology identification is by calculating parameter vector v=v '+Δ v, it is desirable to minimize that calculate and between the attitude measured difference；

$\hat{K}(u,v)=Z\left(u\right)---\left(18\right)$

Be aboutThe function of attitude and Z (u)=(Z (u₁),Z(u₂) ... Z (u_s))^TIt it is the measurement functions of joint angle vector；

For each measurement attitude Z (u_i), there is attitude measurementAnd position measurementAnd

If measurement system can provide attitude measurement and position measurement, each attitude can formulate 12 and measure equation.If measurement system provides only attitude measurement, each attitude measurement may only be formulated three and measure equation, from equation (18), has

$\hat{K}(u,v)=Z\left(u\right)=\hat{K}(u,v)+C(u,\mathrm{\Δv})---\left(20\right)$

Here C isThe difference function of attitude assembly.The relation of Jacobian matrix and C is as follows:

C(u,Δv)=J·Δv(21)

And

$C(u,\mathrm{\Δv})=Z\left(u\right)-\hat{K}(u,v)---\left(22\right)$

Work as use

With

Equation (21) is rewritable is

J·h=b(25)

In order to solve nonlinear equation (25), Levenberg-Marquardt(LM algorithm) it is used to the data of fit non-linear model；When s is far longer than N, the most special and the most conventional method is nonlinear least square method.

The present invention has such advantages as relative to prior art and effect:

The present invention is fixed on the distal portion of robot inertia measurement instrument, by attitude data being estimated in conjunction with Quaternion Algorithm (FQA) and Kalman filtering (KFs), thus obtaining stable robot pose, and utilize the parameter of the Attitude estimation robot of robot.

Accompanying drawing explanation

Fig. 1 is the flow chart of the online robot parameter identification based on inertia measurement instrument.

Detailed description of the invention

Below in conjunction with embodiment, the present invention is described in further detail, but embodiments of the present invention are not limited to this.

Embodiment:

The present invention comprises the steps:

S1, robot kinematics's modeling, link together the joint of robot and attitude；

S2, using the attitude of inertia measurement instrument robot measurement, and in conjunction with Quaternion Algorithm (FQA) and Kalman filtering (KFs), attitude data being estimated, thus obtaining stable robot pose；

S3, utilize the attitude identification machine Radix Ginseng number of robot.

Model the robot kinematics's model obtained described in step S1 and meet the rule that following kinematics parameters is identified:

1) integrity: robot kinematics's model should have enough parameters to define any possible deviation standard value；

2) seriality: the minor variations of the necessary corresponding kinematics parameters of any minor variations of robot architecture；

3) minimality: kinematics model must only comprise the parameter of minimal amount.

Step S1 includes:

Position and the attitude of robot end is defined according to DH model；By the conversion of continuous uniform, the kinematical equation obtained is defined as:

$T_N^0=T_N^0\left(v\right)=T_1^0{T}_2^1...T_N^{N-1}=\underset{i=1}{\overset{N}{\mathrm{\Π}}}{T}_i^{i-1}---\left(1\right)$

Here definition robot body coordinate system is 0 coordinate system, and robot end's coordinate system is N coordinate system, and wherein N is the total number of link rod coordinate system or connecting rod；It is from 0 ordinate transform to the transition matrix of N coordinate system, namely from robot body ordinate transform to the transition matrix of robot end's coordinate system；Being the translation matrix being tied to i coordinate system from i-1 coordinate, i is connecting rod numbering, and the value of i is 1 ~ N；It is the parameter vector of robot, wherein contains connecting rod error；

v_i=v′_i+Δv_i(2)

Here v_iIt is the parameter vector of connecting rod i, v '_iIt is the scalar of connecting rod i,It it is the parameter error vector of connecting rod i；It is defined as according to the kinematical equation that DH model obtains:

$K_N^0=K_N^0\left(v\right)=K_1^0{K}_2^1...K_N^{N-1}=\underset{i=1}{\overset{N}{\mathrm{\Π}}}{K}_i^{i-1}---\left(3\right)$

With

$K_i^{i-1}=T_i^{i-1}(V_i^{\′}+\mathrm{\Δ}{v}_i)=T_i^i+\mathrm{\Δ}{T}_i---\left(4\right)$

Consider connecting rod variable, have

$K_N^0=T_N^0+\mathrm{\ΔT},\mathrm{\ΔT}=\mathrm{\ΔT}(u,\mathrm{\Δv})---\left(5\right)$

HereWithDefinition is consistent, is all from 0 ordinate transform to the transition matrix of N coordinate system, is distinctive in thatBe actual value andIt it is ideal value；It it is the actual translation matrix being tied to i coordinate system from i-1 coordinate；Δ T is error translation matrix；Being joint angle vector, θ is joint angle；Therefore span is'sIt it is a function relevant to joint angle vector u and link parameters error vector Δ v.

Described step S2 includes:

One inertia measurement instrument is installed in the distal portion of measured robot and determines the rolling of robot end, pitching and deflection attitude；Inertia measurement instrument is by a triaxial accelerometer, two two axis gyroscope instrument and three axle magnetometer compositions；

Define three coordinate system: body coordinate system x_by_bz_b, inertia measurement instrument coordinate system x_sy_sz_sWith terrestrial coordinate system x_ey_ez_e；Inertia measurement instrument coordinate system x_sy_sz_sThe accelerometer of corresponding three orthogonal installations and the axle of magnetometer；Owing to inertia measurement instrument is rigidly mounted on robot end, it is assumed that body coordinate system x_by_bz_bWith inertia measurement instrument coordinate system x_sy_sz_sIn conjunction with；Earth fixed coordinate system x_ey_ez_eFollow north-Dong-under (NED) pact, wherein x_ePoint to north, y_ePoint to east and z_eUnder sensing；Inertia measurement instrument can measure the rolling of himself, pitching and deflection attitude；Definition is around x_eRotation φ represent rolling, around y_eThe rotation θ of axle represents pitching and around z_eThe rotation ψ of axle represents deflection；According to Euler's rotation theorem, from Eulerian angles φ, θ and ψ being converted to quaternary number q:

$q=\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\left[\begin{array}{c}\cos (\mathrm{\φ}/2)\cos (\mathrm{\θ}/2)\cos (\mathrm{\ψ}/2)+\sin (\mathrm{\φ}/2)\sin (\mathrm{\θ}/2)\sin (\mathrm{\ψ}/2)\\ \sin (\mathrm{\φ}/2)\cos (\mathrm{\θ}/2)\cos (\mathrm{\ψ}/2)-\cos (\mathrm{\φ}/2)\sin (\mathrm{\θ}/2)\sin (\mathrm{\ψ}/2)\\ \cos (\mathrm{\φ}/2)\sin (\mathrm{\θ}/2)\cos (\mathrm{\ψ}/2)+\sin (\mathrm{\φ}/2)\cos (\mathrm{\θ}/2)\sin (\mathrm{\ψ}/2)\\ \cos (\mathrm{\φ}/2)\cos (\mathrm{\θ}/2)\sin (\mathrm{\ψ}/2)-\sin (\mathrm{\φ}/2)\sin (\mathrm{\θ}/2)\cos (\mathrm{\ψ}/2)\end{array}\right]---\left(6\right)$

And being constrained to of four Euler Parameter:

$q_0^2+q_1^2+q_2^2{+q}_3^2=1---\left(7\right)$

Here q₀It is scalar component and (q₁,q₂,q₃) it is vector section；So be tied to the attitude cosine matrix of earth fixed coordinate system from inertia measurement instrument coordinateCan be represented as:

$M_s^e=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_0{q}_2+q_1{q}_3)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_0{q}_1+q_2{q}_3)& q_0^2-q_1^2-q_2^2-q_3^2\end{array}\right]---\left(8\right);$

There are white noise and random walk due to gyroscope and magnetometer, in order to obtain stable data, utilize Kalman filtering to estimate the state of inertia measurement instrument from one group of noisy and incomplete measurement；From the inertia measurement instrument state x of moment k-1_k-1Estimate the inertia measurement instrument state x of moment k_kTransfer equation be:

x_k=A_k·x_k-1+B·u_k-1+w_k-1

z_k=H·x_k+v_k(9)

Here x_kIt is the state vector of n × 1, represents the inertia measurement instrument state of moment k；A is the state transition matrix of n × n；B is the system input matrix of n × p；u_k-1Being the k-1 moment definitiveness input vector of p × 1, p is the number of definitiveness input；w_k-1It it is the n × 1 process noise vector in k-1 moment；z_kIt it is the m × 1 measurement vector in k moment；H is the observation matrix of m × n and v_kIt it is the measurement noise vector of m × 1；N=7, m=6 herein；With the quaternion differential equation of time correlation it is:

$\left[\begin{array}{c}{\∂q}_0/\∂t\\ {\∂q}_1/\∂t\\ {\∂q}_2/\∂t\\ {\∂q}_3/\∂t\end{array}\right]=\left[\begin{array}{cccc}{q}_0& {-q}_1& {-q}_2& {-q}_3\\ q_1& q_0& {-q}_3& q_2\\ q_2& q_3& q_0& {-q}_1\\ q_3& {-q}_2& q_1& q_0\end{array}\right]\·\left[\begin{array}{c}0\\ v_x/2\\ v_y/2\\ v_z/2\end{array}\right]---\left(10\right)$

Wherein v_x,v_y,v_zIt is that inertia measurement instrument is at axle x_s,y_s,z_sAngular velocity；Due to state x_kIncluding quaternary number state and angular velocity, so x_kThere is following form:

x_k=[q_0,kq_1,kq_2,kq_3,kv_x,kv_y,kv_z,k](11)

Here q_{0, k}, q_{1, k}, q_{2, k}, q_{3, k}, v_{X, k}, v_{Y, k},v_z,kBeing quaternary number state and the angular velocity in k moment, can obtain from equation (10), State Transferring equation is:

$A_k=\left[\begin{array}{ccccccc}1& 0& 0& 0& {-q}_{1,k}\·\mathrm{\Δt}/2& {-q}_{2,k}\·\mathrm{\Δt}/2& {-q}_{3,k}\·\mathrm{\Δt}/2\\ 0& 1& 0& 0& q_{0,k}\·\mathrm{\Δt}/2& q_{3,k}\·\mathrm{\Δt}/2& q_{2,k}\·\mathrm{\Δt}/2\\ 0& 0& 1& 0& q_{3,k}\·\mathrm{\Δt}/2& q_{0,k}\·\mathrm{\Δt}/2& {-q}_{1,k}\·\mathrm{\Δt}/2\\ 0& 0& 0& 1& {-q}_{2,k}\·\mathrm{\Δt}/2& q_{1,k}\·\mathrm{\Δt}/2& q_{0,k}\·\mathrm{\Δt}/2\\ 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]---\left(12\right)$

Here Δ t is sample time, owing to not controlling input, allows B=0^n×p, then B u_k-1=0^n×1；Using angular velocity to estimate quaternary number state, obtaining process noise vector is

w_k=[0000w_xw_yw_z]^T(13)

Here w_x,w_y,w_zIt it is the process noise of angular velocity；Owing to using gyroscope to measure angular velocity, observation matrix H is

H=[0^3×4I^3×3](14)

In order to meet (7), definitionThe definitiveness quaternary number q in k moment_kCan be standardized as:

q_k=[q_0,k/Dq_1,k/Dq_2,k/Dq_3,k/D](15)

In conjunction with formula (9) to formula (15), can from the inertia measurement instrument state x of moment k-1_k-1Estimate the inertia measurement instrument state x of moment k_k。

Described step S3 includes:

Assuming that measuring attitude number is s, then can again be expressed as from robot body ordinate transform to the kinesiology formula of robot end's coordinate system

$\hat{K}={\hat{K}}_N^0={(\hat{K}(u_1,v),\hat{K}(u_2,v),...,\hat{K}(u_s,v))}^T---\left(16\right)$

And

$\mathrm{\Δ}\hat{T}=\mathrm{\Δ}{\hat{T}}_N^0={(\mathrm{\Δ}\hat{T}(u_1,v),\mathrm{\Δ}\hat{T}(u_2,v),...,\mathrm{\Δ}\hat{T}(u_s,v))}^T---\left(17\right)$

Here u_i(i=1,2 ..., s) be ith measurement attitude joint angle vector；

Reference formula (3), (4), (5), hereDefinition withDefinition consistent,Definition with Δ T is consistent；

The target of kinesiology identification is by calculating parameter vector v=v '+Δ v, it is desirable to minimize that calculate and between the attitude measured difference；

$\hat{K}(u,v)=Z\left(u\right)---\left(18\right)$

Be aboutThe function of attitude and Z (u)=(Z (u₁),Z(u₂) ... Z (u_s))^TIt it is the measurement functions of joint angle vector；

For each measurement attitude Z (u_i), there is attitude measurementAnd position measurementAnd

If measurement system can provide attitude measurement and position measurement, each attitude can formulate 12 and measure equation.If measurement system provides only attitude measurement, each attitude measurement may only be formulated three and measure equation, from equation (18), has

$\hat{K}(u,v)=Z\left(u\right)=\hat{K}(u,v)+C(u,\mathrm{\Δv})---\left(20\right)$

Here C isThe difference function of attitude assembly.The relation of Jacobian matrix and C is as follows:

C(u,Δv)=J·Δv(21)

And

$C(u,\mathrm{\Δv})=Z\left(u\right)-\hat{K}(u,v)---\left(22\right)$

Work as use

With

Equation (21) is rewritable is

J·h=b(25)

In order to solve nonlinear equation (25), Levenberg-Marquardt(LM algorithm) it is used to the data of fit non-linear model；When s is far longer than N, the most special and the most conventional method is nonlinear least square method.

Above-described embodiment is the present invention preferably embodiment; but embodiments of the present invention are also not restricted to the described embodiments; the change made under other any spirit without departing from the present invention and principle, modification, replacement, combination, simplification; all should be the substitute mode of equivalence, be included within protection scope of the present invention.

Claims (4)

1. based on the online robot parameter identification method of inertia measurement instrument, it is characterised in that comprise the following steps:

S1, robot kinematics's modeling, link together the joint of robot and attitude, specifically include:

Position and the attitude of robot end is defined according to DH model；By the conversion of continuous uniform, the kinematical equation obtained is defined as:

$T_N^0=T_N^0\left(v\right)=T_1^0{T}_2^1...T_N^{N-1}=\underset{i=1}{\overset{N}{\Π}}{T}_i^{i-1}---\left(1\right)$

If robot body coordinate system is 0 coordinate system, robot end's coordinate system is N coordinate system, and wherein N is the total number of link rod coordinate system or connecting rod；It is from 0 ordinate transform to the transition matrix of N coordinate system, namely from robot body ordinate transform to the transition matrix of robot end's coordinate system；Being the translation matrix being tied to i coordinate system from i-1 coordinate, i is connecting rod numbering, and the value of i is 1～N；It is the parameter vector of robot, wherein contains connecting rod error；

v_i=v '_i+Δv_i(2)

Wherein v_iIt is the parameter vector of connecting rod i, v '_iIt is the scalar of connecting rod i,It is connecting rod i

Parameter error vector；It is defined as according to the kinematical equation that DH model obtains:

$K_N^0=K_N^0\left(v\right)=K_1^0{K}_2^1...K_N^{N-1}=\underset{i=1}{\overset{N}{\Π}}{K}_i^{i-1}---\left(3\right)$

With

$K_i^{i-1}=T_i^{i-1}(v_i^{\′}+{\mathrm{\Δv}}_i)=T_i^i+{\mathrm{\ΔT}}_i---\left(4\right)$

Consider connecting rod variable, have

$K_N^0=T_N^0+\mathrm{\Δ}T,\mathrm{\Δ}T=\mathrm{\Δ}T(u,\mathrm{\Δ}v)---\left(5\right)$

WhereinWithDefinition is consistent, is all from 0 ordinate transform to the transition matrix of N coordinate system, is distinctive in thatBe actual value andIt it is ideal value；It it is the actual translation matrix being tied to i coordinate system from i-1 coordinate；Δ T is error translation matrix；Being joint angle vector, θ is joint angle；Therefore span is'sIt it is a function relevant to joint angle vector u and link parameters error vector Δ v；

S2, using the attitude of inertia measurement instrument robot measurement, and in conjunction with Quaternion Algorithm and Kalman filtering, attitude data being estimated, thus obtaining stable robot pose；

S3, utilize the attitude identification machine Radix Ginseng number of robot.

2. the online robot parameter identification method based on inertia measurement instrument according to claim 1, it is characterised in that model the robot kinematics's model obtained described in step S1 and meet the character that following kinematics parameters is identified:

1) integrity: robot kinematics's model should have enough parameters to define any possible deviation standard value；

2) seriality: the minor variations of the necessary corresponding kinematics parameters of any minor variations of robot architecture；

3) minimality: kinematics model must only comprise the parameter of minimal amount.

3. the online robot parameter identification method based on inertia measurement instrument according to claim 1, it is characterised in that described step S2 comprises the following steps:

One inertia measurement instrument is installed in the distal portion of measured robot and determines the rolling of robot end, pitching and deflection attitude；Inertia measurement instrument is by a triaxial accelerometer, two two axis gyroscope instrument and three axle magnetometer compositions；

Define three coordinate system: body coordinate system x_by_bz_b, inertia measurement instrument coordinate system x_sy_sz_sWith terrestrial coordinate system x_ey_ez_e；Inertia measurement instrument coordinate system x_sy_sz_sThe accelerometer of corresponding three orthogonal installations and the axle of magnetometer；Owing to inertia measurement instrument is rigidly mounted on robot end, it is assumed that body coordinate system x_by_bz_bWith inertia measurement instrument coordinate system x_sy_sz_sIn conjunction with；Earth fixed coordinate system x_ey_ez_eFollow north-Dong-lower pact, wherein x_ePoint to north, y_ePoint to east and z_eUnder sensing；Inertia measurement instrument can measure the rolling of himself, pitching and deflection attitude；Definition is around x_eRotation φ represent rolling, around y_eThe rotation θ of axle represents pitching and around z_eThe rotation ψ of axle represents deflection；According to Euler's rotation theorem, from Eulerian angles φ, θ and ψ being converted to quaternary number q:

$q=\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\left[\begin{array}{c}\cos (\mathrm{\φ}/2)\cos (\mathrm{\θ}/2)\cos (\mathrm{\ψ}/2)+\sin (\mathrm{\φ}/2)\sin (\mathrm{\θ}/2)\sin (\mathrm{\ψ}/2)\\ \sin (\mathrm{\φ}/2)\cos (\mathrm{\θ}/2)\cos (\mathrm{\ψ}/2)-\cos (\mathrm{\φ}/2)\sin (\mathrm{\θ}/2)\sin (\mathrm{\ψ}/2)\\ \cos (\mathrm{\φ}/2)\sin (\mathrm{\θ}/2)\cos (\mathrm{\ψ}/2)+\sin (\mathrm{\φ}/2)\cos (\mathrm{\θ}/2)\sin (\mathrm{\ψ}/2)\\ \cos (\mathrm{\φ}/2)\cos (\mathrm{\θ}/2)\sin (\mathrm{\ψ}/2)-\sin (\mathrm{\φ}/2)\sin (\mathrm{\θ}/2)\cos (\mathrm{\ψ}/2)\end{array}\right]---\left(6\right)$

And being constrained to of four Euler Parameter:

$q_0^2+q_1^2+q_2^2+q_3^2=1---\left(7\right)$

Wherein q₀It is scalar component and (q₁,q₂,q₃) it is vector section；So be tied to the attitude cosine matrix of earth fixed coordinate system from inertia measurement instrument coordinateCan be represented as:

$M_s^e=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_0{q}_2+q_1{q}_3)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_0{q}_1+q_2{q}_3)& q_0^2-q_1^2-q_2^2-q_3^2\end{array}\right]---\left(8\right);$

There are white noise and random walk due to gyroscope and magnetometer, in order to obtain stable data, utilize Kalman filtering to estimate the state of inertia measurement instrument from one group of noisy and incomplete measurement；From the inertia measurement instrument state x of moment k-1_k-1Estimate the inertia measurement instrument state x of moment k_kTransfer equation be:

$\begin{array}{c}{x}_k=A_k\·x_{k-1}+B\·u_{k-1}+w_{k-1}\\ z_k=H\·x_k+v_k\end{array}---\left(9\right)$

Wherein x_kIt is the state vector of n × 1, represents the inertia measurement instrument state of moment k；A is the state transition matrix of n × n；B is the system input matrix of n × p；u_k-1Being the k-1 moment definitiveness input vector of p × 1, p is the number of definitiveness input；w_k-1It it is the n × 1 process noise vector in k-1 moment；z_kIt it is the m × 1 measurement vector in k moment；H is the observation matrix of m × n and v_kIt it is the measurement noise vector of m × 1；N=7, m=6 herein；With the quaternion differential equation of time correlation it is:

$\left[\begin{array}{c}\∂q_0/\∂t\\ \∂q_1/\∂t\\ \∂q_2/\∂t\\ \∂q_3/\∂t\end{array}\right]=\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& -q_3& q_2\\ q_2& q_3& q_0& -q_1\\ q_3& -q_2& q_1& q_0\end{array}\right]\·\left[\begin{array}{c}0\\ v_x/2\\ v_y/2\\ v_z/2\end{array}\right]---\left(10\right)$

Wherein v_x,v_y,v_zIt is that inertia measurement instrument is at axle x_s,y_s,z_sAngular velocity；Due to state x_kIncluding quaternary number state and angular velocity, so x_kThere is following form:

x_k=[q_0,kq_1,kq_2,kq_3,kv_x,kv_y,kv_z,k](11)

Here q_0,k,q_1,k,q_2,k,q_3,k,v_x,k,v_y,k,v_z,kBeing quaternary number state and the angular velocity in k moment, can obtain from equation (10), State Transferring equation is:

$A_k=\left[\begin{array}{ccccccc}1& 0& 0& 0& -q_{1,k}\·\mathrm{\Δ}t/2& -q_{2,k}\·\mathrm{\Δ}t/2& -q_{3,k}\·\mathrm{\Δ}t/2\\ 0& 1& 0& 0& q_{0,k}\·\mathrm{\Δ}t/2& q_{3,k}\·\mathrm{\Δ}t/2& q_{2,k}\·\mathrm{\Δ}t/2\\ 0& 0& 1& 0& q_{3,k}\·\mathrm{\Δ}t/2& q_{0,k}\·\mathrm{\Δ}t/2& -q_{1,k}\·\mathrm{\Δ}t/2\\ 0& 0& 0& 1& -q_{2,k}\·\mathrm{\Δ}t/2& q_{1,k}\·\mathrm{\Δ}t/2& q_{0,k}\·\mathrm{\Δ}t/2\\ 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]---\left(12\right)$

Wherein Δ t is sample time, owing to not controlling input, allows B=0^n×p, then B u_k-1=0^n×1；Using angular velocity to estimate quaternary number state, obtaining process noise vector is

w_k=[0000w_xw_yw_z]^T(13)

Here w_x,w_y,w_zIt it is the process noise of angular velocity；Owing to using gyroscope to measure angular velocity, observation matrix H is

H=[0^3×4I^3×3](14)

In order to meet (7), definitionThe definitiveness quaternary number q in k moment_kCan be standardized as:

q_k=[q_0,k/Dq_1,k/Dq_2,k/Dq_3,k/D](15)

In conjunction with formula (9) to formula (15), can from the inertia measurement instrument state x of moment k-1_k-1Estimate the inertia measurement instrument state x of moment k_k。

4. the online robot parameter identification method based on inertia measurement instrument according to claim 1, it is characterised in that described step S3 comprises the following steps:

Assuming that measuring attitude number is s, then can again be expressed as from robot body ordinate transform to the kinesiology formula of robot end's coordinate system

$\hat{K}={\hat{K}}_N^0{\left(\hat{K}\right(u_1,v),\hat{K}(u_2,v),\mathrm{...},\hat{K}(u_s,v\left)\right)}^T---\left(16\right)$

And

$\mathrm{\Δ}\hat{T}=\mathrm{\Δ}{\hat{T}}_N^0={\left(\mathrm{\Δ}\hat{T}\right(u_1,v),\mathrm{\Δ}\hat{T}(u_2,v),\mathrm{...},\mathrm{\Δ}\hat{T}(u_s,v\left)\right)}^T---\left(17\right)$

Here u_i(i=1,2 ..., s) be ith measurement attitude joint angle vector；

Reference formula (3), (4), (5), hereDefinition withDefinition consistent,Definition with Δ T is consistent；

The target of kinesiology identification is by calculating parameter vector v=v'+ Δ v, it is desirable to minimize that calculate and between the attitude measured difference；

$\hat{K}(u,v)=Z\left(u\right)---\left(18\right)$

Be aboutThe function of attitude and Z (u)=(Z (u₁),Z(u₂),...Z(u_s))^TIt it is the measurement functions of joint angle vector；

For each measurement attitude Z (u_i), there is attitude measurementAnd position measurementAnd

If measurement system can provide attitude measurement and position measurement, each attitude can formulate 12 and measure equation；If measurement system provides only attitude measurement, each attitude measurement may only be formulated three and measure equation, from equation (18), has

$\hat{K}(u,v)=Z\left(u\right)=\hat{K}(u,v)+C(u,\mathrm{\Δ}v)---\left(20\right)$

Here C isThe difference function of attitude assembly；The relation of Jacobian matrix and C is as follows:

C (u, Δ v)=J Δ v (21)

And

$C(u,\mathrm{\Δ}v)=Z\left(u\right)-\hat{K}(u,v)---\left(22\right)$

Work as use

With

Equation (21) is rewritable is

J h=b (25)

Adopting nonlinear least square method, solve nonlinear equation (25), Levenberg-Marquardt and LM algorithm is used to the data of fit non-linear model.