CN103279037A - Method for performing force follow-up motion control on robot on the basis of six-dimensional force/torque transducer - Google Patents

Abstract

The invention discloses a method for performing force follow-up motion control on a robot on the basis of a six-dimensional force/torque transducer. When the goals, such as teaching and the like, are achieved by using the robot, a very good effect can be obtained by the method. The method for performing the force follow-up motion control on the robot on the basis of the six-dimensional force/torque transducer disclosed by the invention mainly comprises four important steps: firstly, calibrating a zero value of the force/torque transducer by utilizing a simpler method, secondly, performing compensation on influences caused to the zero value of the force/torque transducer due to the gravity of the force/torque transducer and the gravity of tools installed on the force/torque transducer under different postures of the force/torque transducer by utilizing a specific method, thirdly, setting a stable interval of the force/torque transducer under the corresponding posture, and fourthly, performing the force follow-up motion control on the robot by utilizing the force/torque value of the force/torque transducer.

Description

Robot Force accompany movement control method based on sextuple power/torque sensor

Technical field

The present invention relates to robot control field, relate in particular to the Robot Force accompany movement control method based on sextuple power/torque sensor.

Background technology

Regular meeting uses power/torque sensor in industrial circle; power/torque sensor commonly used attitude is in use fixed; but if power/torque sensor operates in the occasion that attitude can change; because sensor and the instrument of installing in the above are subjected to gravity effect; the zero position of sensor (output valve of sensor when sensor does not apply external force to it under this attitude) can change, and therefore the effect to its compensation directly has influence on actual performance.For this change is compensated, need use special method.Aspect the demarcation of sextuple power/torque sensor, those skilled in the art have proposed a kind of method that the measure linear degree of sextuple power/torque sensor under a certain attitude demarcated, but they are in the application facet more complicated.

Summary of the invention

The objective of the invention is to overcome the shortcoming and defect of above-mentioned prior art, the Robot Force accompany movement control method based on sextuple power/torque sensor is provided, realize the power accompany movement of robot is controlled by force compensating.

The present invention is achieved through the following technical solutions:

Robot Force accompany movement control method based on sextuple power/torque sensor comprises the steps:

Step (1): also utilize following formula that power and the moment numerical value of power/torque sensor are compensated by the current attitude of feedback force/torque sensor:

$\left\{\begin{array}{c}{F}_{\mathrm{xc}}=r_{31}{k}_{\mathrm{xz}}G+F_{x0}\\ F_{\mathrm{yc}}=r_{32}{k}_{\mathrm{yz}}G+F_{y0}\\ F_{\mathrm{zc}}=r_{33}G+F_{z0}\\ M_{\mathrm{xc}}=M_{x0}\\ M_{\mathrm{yc}}=M_{y0}\\ M_{\mathrm{zc}}=M_{z0}\end{array}\right.$

In the formula, r ₃₁, r ₃₂, r ₃₃Respectively from the current attitude of power sensor $T=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$ The middle acquisition;

Step (2): up as initial position 1, the data that record current power sensor are F with power sensor x axle in elder generation _X1, F _Y1, F _Z1, M _X1, M _Y1, M _Z1Be rotated counterclockwise 90 ° around the z axle and obtain position 2 on initial position 1 basis, the variable that records current power sensor is F _X2, F _Y2, F _Z2, M _X2, M _Y2, M _Z2Be rotated counterclockwise 90 ° around the z axle and obtain position 3 on 2 bases, position, the variable that records current power sensor is F _X3, F _Y3, F _Z3, M _X3, M _Y3, M _Z3Be rotated counterclockwise 90 ° around the z axle and obtain position 4 on 3 bases, position, the variable that records current power sensor is F _X4, F _Y4, F _Z4, M _X4, M _Y4, M _Z4Be rotated counterclockwise 90 ° around the y axle and obtain position 5 on initial position 1 basis, the variable that records current power sensor is F _X5, F _Y5, F _Z5, M _X5, M _Y5, M _Z5Turn clockwise 90 ° around the y axle on initial position 1 basis and obtain position 6, the variable that records current power sensor is F _X6, F _Y6, F _Z6, M _X6, M _Y6, M _Z6

Obtain corresponding initial value by following formula:

$\left\{\begin{array}{c}{F}_{x0}=(F_{x1}+F_{x3})/2\\ F_{y0}=(F_{y2}+F_{y4})/2\\ F_{z0}=(F_{z5}+F_{z6})/2\end{array}\right.$

$\left\{\begin{array}{c}{M}_{x0}=(M_{x2}+M_{x4}+M_{x5}+M_{x6})/4\\ M_{y0}=(M_{y1}+M_{y3}+M_{y5}+M_{y6})/4\\ M_{z0}=(M_{z1}+M_{z2}+M_{z3}+M_{z4})/4\end{array}\right.$

After calculating initial value, calculate the gravity value that x-axis, y-axis and z-axis record respectively:

$\left\{\begin{array}{c}{G}_x=|F_{x3}-F_{x1}|/2\\ G_y=|F_{y2}-F_{y4}|/2\\ G_z=|F_{z6}-F_{z5}|/2\end{array}\right.$

The deviation that calculates between each axis linear degree of sensor according to this gravity value concerns

$\left\{\begin{array}{c}{k}_{\mathrm{xz}}=G_x/G_z\\ k_{\mathrm{yz}}=G_y/G_z\end{array}\right.$

With G _zBe gravity datum, i.e. G=G _z

According to the attitude of current power/torque sensor, the zero value F that utilizes the formula in the step (1) to calculate the current attitude of power/torque sensor _Xc, F _Yc, F _Zc, M _Xc, M _Yc, M _Zc, by above formula, the basic parameter of power/torque sensor is demarcated;

Step (3): utilize formula in the step (1) to obtain the zero value of 6 measured in the step (2) positions respectively, and compare with measured value, calculate the error maximum value of each measuring amount this moment by following formula:

${\mathrm{\η}}_f=\underset{i=1}{\overset{6}{\max }}|F_{\mathrm{ni}}-F_{\mathrm{nc}}|,(n=x,y,z)$

${\mathrm{\η}}_m=\underset{i=1}{\overset{6}{\max }}|M_{\mathrm{ni}}-M_{\mathrm{nc}}|,(n=x,y,z)$

F in the formula _Ni, M _NiThe amount that the expression sensor measurement is returned, F _Nc, M _NcThe compensation rate that the expression respective channel is calculated; The maximum value of each channel error is designated as η respectively _Fx, η _Fy, η _Fz, η _Mx, η _My, η _Mz

Get stability factor ε _Fx, ε _Fy, ε _Fz(all greater than 1, suggestion selects 1.5～2.0) makes the current zero-bit stable region of power/torque sensor be:

$\left\{\begin{array}{c}{\mathrm{\δ}}_{\mathrm{fx}}={\mathrm{\η}}_{\mathrm{fx}}{\mathrm{\ϵ}}_{\mathrm{fx}}\\ {\mathrm{\δ}}_{\mathrm{fy}}={\mathrm{\η}}_{\mathrm{fy}}{\mathrm{\ϵ}}_{\mathrm{fy}}\\ {\mathrm{\δ}}_{\mathrm{fz}}={\mathrm{\η}}_{\mathrm{fz}}{\mathrm{\ϵ}}_{\mathrm{fz}}\\ {\mathrm{\δ}}_{\mathrm{mx}}={\mathrm{\η}}_{\mathrm{mx}}{\mathrm{\ϵ}}_{\mathrm{mx}}\\ {\mathrm{\δ}}_{\mathrm{my}}={\mathrm{\η}}_{\mathrm{my}}{\mathrm{\ϵ}}_{\mathrm{my}}\\ {\mathrm{\δ}}_{\mathrm{mz}}={\mathrm{\η}}_{\mathrm{mz}}{\mathrm{\ϵ}}_{\mathrm{mz}}\end{array}\right.$

Then the power end zero-bit stable region of power/torque sensor is respectively

The zero-bit stable region of moment end is respectively [M _Xc-δ _Mx, M _Xc+ δ _Mx],

[M _Zc-δ _Mz, M _Zc+ δ _Mz].

Obtain the power under the current attitude of power/torque sensor and moment numerical stability interval are arranged by said method;

Step (4): obtaining the controlling party supernatural power end that power follows according to step (1), (2) and (3) design is:

$u_f=\left\{\begin{array}{cc}{k}_{\mathrm{fc}}(F_m-F_c+\mathrm{\&delta;})& F_m<F_c-\mathrm{\&delta;}\\ 0& F_c-\mathrm{\&delta;}<F_m<F_c+\mathrm{\&delta;}\\ k_{\mathrm{fc}}(F_m-F_c-\mathrm{\&delta;})& F_m>F_c+\mathrm{\&delta;}\end{array}\right.$

F in the formula _mBe illustrated in the measured value after the power that applies on certain force direction, F _cBe illustrated on this direction the compensation rate of power, δ represents the stable region on this direction;

In like manner, the control method of moment end is:

$u_m=\left\{\begin{array}{cc}{k}_{\mathrm{mc}}(M_m-M_c+\mathrm{\&delta;})& M_m<M_c-\mathrm{\&delta;}\\ 0& M_c-\mathrm{\&delta;}<M_m<M_c+\mathrm{\&delta;}\\ k_{\mathrm{mc}}(M_m-M_c-\mathrm{\&delta;})& M_m>M_c+\mathrm{\&delta;}\end{array}\right.$

M in the formula _mBe illustrated in the measured value after the power that applies on certain moment direction, M _cBe illustrated on this direction the compensation rate of power, δ represents the stable region on this direction;

Controlling value will feed back to carries out power and follows control in the robot, and is undertaken by following formula:

$\stackrel{\&CenterDot;}{q}=J^{-1}u$

Wherein J is the current Jacobi matrix of robot, and u is the control input of power and moment, is expressed as

$u=\left[\begin{array}{c}{u}_f\\ u_m\end{array}\right]=\left[\begin{array}{c}{u}_{\mathrm{fx}}\\ u_{\mathrm{fy}}\\ u_{\mathrm{fz}}\\ u_{\mathrm{mx}}\\ u_{\mathrm{my}}\\ u_{\mathrm{mz}}\end{array}\right]$

Speed for each joint of robot, after calculating the speed in each joint, utilize this speed that each joint of robot is controlled, make it do the power accompany movement, whenever robot pose changes, all need to use the method in the step (2) that the zero value of sensor is demarcated again.

The present invention can draw the higher compensation effect of precision for power/torque sensor by simple several steps; Provide because various measuring error and data are disturbed the solution of the zero-bit change that causes.

Description of drawings

Fig. 1 represents intention for power control function of the present invention.

Fig. 2 is Torque Control function representation intention of the present invention.

The state of initial 6 positions that Fig. 3 will measure for power/torque sensor of the present invention (gravity is all down) changes block diagram, wherein:

Table 1(initial position 1) position shown in be x axle with the power sensor up as initial position, the variable that records current power sensor is F _X1, F _Y1, F _Z1, M _X1, M _Y1, M _Z1

Table 2(position 2) position shown in is that the described position of table 1 is rotated counterclockwise 90 ° of resulting positions around the z axle, and the variable that records current power sensor is F _X2, F _Y2, F _Z2, M _X2, M _Y2, M _Z2

Table 3(position 3) position shown in is that the described position of table 2 is rotated counterclockwise 90 ° of resulting positions around the z axle, and the variable that records current power sensor is F _X3, F _Y3, F _Z3, M _X3, M _Y3, M _Z3

Table 4(position 4) position shown in is that the described position of table 3 is rotated counterclockwise 90 ° of resulting positions around the z axle, and the variable that records current power sensor is F _X4, F _Y4, F _Z4, M _X4, M _Y4, M _Z4

Table 5(position 5) position shown in is that the described position of table 1 is rotated counterclockwise 90 ° of resulting positions around the y axle, and the variable that records current power sensor is F _X5, F _Y5, F _Z5, M _X5, M _Y5, M _Z5

Table 6(position 6) position shown in is the described position of table 1 around the y axle 90 ° of resulting positions that turn clockwise, and the variable that records current power sensor is F _X6, F _Y6, F _Z6, M _X6, M _Y6, M _Z6

Embodiment

Below in conjunction with specific embodiment the present invention is done further concrete detailed description the in detail.

Embodiment

As Fig. 1, Fig. 2, shown in Figure 3.The present invention is based on the Robot Force accompany movement control method of sextuple power/torque sensor, it is characterized in that comprising the steps:

Step (1): also utilize following formula that power and the moment numerical value of power/torque sensor are compensated by the current attitude of feedback force/torque sensor:

$\left\{\begin{array}{c}{F}_{\mathrm{xc}}=r_{31}{k}_{\mathrm{xz}}G+F_{x0}\\ F_{\mathrm{yc}}=r_{32}{k}_{\mathrm{yz}}G+F_{y0}\\ F_{\mathrm{zc}}=r_{33}G+F_{z0}\\ M_{\mathrm{xc}}=M_{x0}\\ M_{\mathrm{yz}}=M_{y0}\\ M_{\mathrm{zc}}=M_{z0}\end{array}\right.$

In the formula, r ₃₁, r ₃₂, r ₃₃Respectively from the current attitude of power sensor $T=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$ The middle acquisition;

Step (2): up as initial position 1, the data that record current power sensor are F with power sensor x axle in elder generation _X1, F _Y1, F _Z1, M _X1, M _Y1, M _Z1Be rotated counterclockwise 90 ° around the z axle and obtain position 2 on initial position 1 basis, the variable that records current power sensor is F _X2, F _Y2, F _Z2, M _X2, M _Y2, M _Z2Be rotated counterclockwise 90 ° around the z axle and obtain position 3 on 2 bases, position, the variable that records current power sensor is F _X3, F _Y3, F _Z3, M _X3, M _Y3, M _Z3Be rotated counterclockwise 90 ° around the z axle and obtain position 4 on 3 bases, position, the variable that records current power sensor is F _X4, F _Y4, F _Z4, M _X4, M _Y4, M _Z4Be rotated counterclockwise 90 ° around the y axle and obtain position 5 on initial position 1 basis, the variable that records current power sensor is F _X5, F _Y5, F _Z5, M _X5, M _Y5, M _Z5Turn clockwise 90 ° around the y axle on initial position 1 basis and obtain position 6, the variable that records current power sensor is F _X6, F _Y6, F _Z6, M _X6, M _Y6, M _Z6

Obtain corresponding initial value by following formula:

$\left\{\begin{array}{c}{F}_{x0}=(F_{x1}+F_{x3})/2\\ F_{y0}=(F_{y2}+F_{y4})/2\\ F_{z0}=(F_{z5}+F_{z6})/2\end{array}\right.$

$\left\{\begin{array}{c}{M}_{x0}=(M_{x2}+M_{x4}+M_{x5}+M_{x6})/4\\ M_{y0}=(M_{y1}+M_{y3}+M_{y5}+M_{y6})/4\\ M_{z0}=(M_{z1}+M_{z2}+M_{z3}+M_{z4})/4\end{array}\right.$

After calculating initial value, calculate the gravity value that x-axis, y-axis and z-axis record respectively:

$\left\{\begin{array}{c}{G}_x=|F_{x3}-F_{x1}|/2\\ G_y=|F_{y2}-F_{y4}|/2\\ G_z=|F_{z6}-F_{z5}|/2\end{array}\right.$

The deviation that calculates between each axis linear degree of sensor according to this gravity value concerns

$\left\{\begin{array}{c}{k}_{\mathrm{xz}}=G_x/G_z\\ k_{\mathrm{yz}}=G_y/G_z\end{array}\right.$

With G _zBe gravity datum, i.e. G=G _z

According to the attitude of current power/torque sensor, the zero value F that utilizes the formula in the step (1) to calculate the current attitude of power/torque sensor _Xc, F _Yc, F _Zc, M _Xc, M _Yc, M _Zc, by above formula, the basic parameter of power/torque sensor is demarcated;

Step (3): utilize formula in the step (1) to obtain the zero value of 6 measured in the step (2) positions respectively, and compare with measured value, calculate the error maximum value of each measuring amount this moment by following formula:

${\mathrm{\&eta;}}_f=\underset{i=1}{\overset{6}{\max }}|F_{\mathrm{ni}}-F_{\mathrm{nc}}|,(n=x,y,z)$

${\mathrm{\&eta;}}_m=\underset{i=1}{\overset{6}{\max }}|M_{\mathrm{ni}}-M_{\mathrm{nc}}|,(n=x,y,z)$

F in the formula _Ni, M _NiThe amount that the expression sensor measurement is returned, F _Nc, M _NcThe compensation rate that the expression respective channel is calculated; The maximum value of each channel error is designated as η respectively _Fx, η _Fy, η _Fz, η _Mx, η _My, η _Mz

Get stability factor ε _Fx, ε _Fy, ε _Fz(all greater than 1, suggestion selects 1.5～2.0) makes the current zero-bit stable region of power/torque sensor be:

$\left\{\begin{array}{c}{\mathrm{\&delta;}}_{\mathrm{fx}}={\mathrm{\&eta;}}_{\mathrm{fx}}{\mathrm{\&epsiv;}}_{\mathrm{fx}}\\ {\mathrm{\&delta;}}_{\mathrm{fy}}={\mathrm{\&eta;}}_{\mathrm{fy}}{\mathrm{\&epsiv;}}_{\mathrm{fy}}\\ {\mathrm{\&delta;}}_{\mathrm{fz}}={\mathrm{\&eta;}}_{\mathrm{fz}}{\mathrm{\&epsiv;}}_{\mathrm{fz}}\\ {\mathrm{\&delta;}}_{\mathrm{mx}}={\mathrm{\&eta;}}_{\mathrm{mx}}{\mathrm{\&epsiv;}}_{\mathrm{mx}}\\ {\mathrm{\&delta;}}_{\mathrm{my}}={\mathrm{\&eta;}}_{\mathrm{my}}{\mathrm{\&epsiv;}}_{\mathrm{my}}\\ {\mathrm{\&delta;}}_{\mathrm{mz}}={\mathrm{\&eta;}}_{\mathrm{mz}}{\mathrm{\&epsiv;}}_{\mathrm{mz}}\end{array}\right.$

Then the power end zero-bit stable region of power/torque sensor is respectively

The zero-bit stable region of moment end is respectively [M _Xc-δ _Mx, M _Xc+ δ _Mx],

[M _Zc-δ _Mz, M _Zc+ δ _Mz].

Obtain the power under the current attitude of power/torque sensor and moment numerical stability interval are arranged by said method;

Step (4): obtaining the controlling party supernatural power end that power follows according to step (1), (2) and (3) design is:

$u_f=\left\{\begin{array}{cc}{k}_{\mathrm{fc}}(F_m-F_c+\mathrm{\&delta;})& F_m<F_c-\mathrm{\&delta;}\\ 0& F_c-\mathrm{\&delta;}<F_m<F_c+\mathrm{\&delta;}\\ k_{\mathrm{fc}}(F_m-F_c-\mathrm{\&delta;})& F_m>F_c+\mathrm{\&delta;}\end{array}\right.$

F in the formula _mBe illustrated in the measured value after the power that applies on certain force direction, F _cBe illustrated on this direction the compensation rate of power, δ represents the stable region on this direction;

In like manner, the control method of moment end is:

$u_m=\left\{\begin{array}{cc}{k}_{\mathrm{mc}}(M_m-M_c+\mathrm{\&delta;})& M_m<M_c-\mathrm{\&delta;}\\ 0& M_c-\mathrm{\&delta;}<M_m<M_c+\mathrm{\&delta;}\\ k_{\mathrm{mc}}(M_m-M_c-\mathrm{\&delta;})& M_m>M_c+\mathrm{\&delta;}\end{array}\right.$

M in the formula _mBe illustrated in the measured value after the power that applies on certain moment direction, M _cBe illustrated on this direction the compensation rate of power, δ represents the stable region on this direction;

Controlling value will feed back to carries out power and follows control in the robot, and is undertaken by following formula:

$\stackrel{\&CenterDot;}{q}=J^{-1}u$

Wherein J is the current Jacobi matrix of robot, and u is the control input of power and moment, is expressed as

$u=\left[\begin{array}{c}{u}_f\\ u_m\end{array}\right]=\left[\begin{array}{c}{u}_{\mathrm{fx}}\\ u_{\mathrm{fy}}\\ u_{\mathrm{fz}}\\ u_{\mathrm{mx}}\\ u_{\mathrm{my}}\\ u_{\mathrm{mz}}\end{array}\right]$

Speed for each joint of robot, after calculating the speed in each joint, utilize this speed that each joint of robot is controlled, make it do the power accompany movement, whenever robot pose changes, all need to use the method in the step (2) that the zero value of sensor is demarcated again.

As mentioned above, just can realize the present invention preferably.

Embodiments of the present invention are not restricted to the described embodiments; other are any not to deviate from change, the modification done under spiritual essence of the present invention and the principle, substitute, combination, simplify; all should be the substitute mode of equivalence, be included within protection scope of the present invention.

Claims (1)

1. based on the Robot Force accompany movement control method of sextuple power/torque sensor, it is characterized in that comprising the steps:

(1) also utilize following formula that power and the moment numerical value of power/torque sensor are compensated by the current attitude of feedback force/torque sensor:

$\left\{\begin{array}{c}{F}_{\mathrm{xc}}=r_{31}{k}_{\mathrm{xz}}G+F_{x0}\\ F_{\mathrm{yc}}=r_{32}{k}_{\mathrm{yz}}G+F_{y0}\\ F_{\mathrm{zc}}=r_{33}G+F_{z0}\\ M_{\mathrm{xc}}=M_{x0}\\ M_{\mathrm{yz}}=M_{y0}\\ M_{\mathrm{zc}}=M_{z0}\end{array}\right.$

In the formula, r ₃₁, r ₃₂, r ₃₃Respectively from the current attitude of power sensor $T=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$ The middle acquisition;

(2) earlier with power sensor x axle up as initial position 1, the data that record current power sensor are F _X1, F _Y1, F _Z1, M _X1, M _Y1, M _Z1Be rotated counterclockwise 90 ° around the z axle and obtain position 2 on initial position 1 basis, the variable that records current power sensor is F _X2, F _Y2, F _Z2, M _X2, M _Y2, M _Z2Be rotated counterclockwise 90 ° around the z axle and obtain position 3 on initial position 2 bases, the variable that records current power sensor is F _X3, F _Y3, F _Z3, M _X3, M _Y3, M _Z3Be rotated counterclockwise 90 ° around the z axle and obtain position 4 on initial position 3 bases, the variable that records current power sensor is F _X4, F _Y4, F _Z4, M _X4, M _Y4, M _Z4Be rotated counterclockwise 90 ° around the y axle and obtain position 5 on initial position 1 basis, the variable that records current power sensor is F _X5, F _Y5, F _Z5, M _X5, M _Y5, M _Z5Turn clockwise 90 ° around the y axle on initial position 1 basis and obtain position 6, the variable that records current power sensor is F _X6, F _Y6, F _Z6, M _X6, M _Y6, M _Z6

Obtain corresponding initial value by following formula:

$\left\{\begin{array}{c}{F}_{x0}=(F_{x1}+F_{x3})/2\\ F_{y0}=(F_{y2}+F_{y4})/2\\ F_{z0}=(F_{z5}+F_{z6})/2\end{array}\right.$

$\left\{\begin{array}{c}{M}_{x0}=(M_{x2}+M_{x4}+M_{x5}+M_{x6})/4\\ M_{y0}=(M_{y1}+M_{y3}+M_{y5}+M_{y6})/4\\ M_{z0}=(M_{z1}+M_{z2}+M_{z3}+M_{z4})/4\end{array}\right.$

After calculating initial value, calculate the gravity value that x-axis, y-axis and z-axis record respectively:

$\left\{\begin{array}{c}{G}_x=|F_{x3}-F_{x1}|/2\\ G_y=|F_{y2}-F_{y4}|/2\\ G_z=|F_{z6}-F_{z5}|/2\end{array}\right.$

The deviation that calculates between each axis linear degree of sensor according to this gravity value concerns

$\left\{\begin{array}{c}{k}_{\mathrm{xz}}=G_x/G_z\\ k_{\mathrm{yz}}=G_y/G_z\end{array}\right.$

With G _zBe gravity datum, i.e. G=G _z

According to the attitude of current power/torque sensor, the zero value F that utilizes the formula in the step (1) to calculate the current attitude of power/torque sensor _Xc, F _Yc, F _Zc, M _Xc, M _Yc, M _Zc, by above formula, the basic parameter of power/torque sensor is demarcated;

(3) utilize formula in the step (1) to obtain the zero value of 6 measured in the step (2) positions respectively, and compare with measured value, calculate the error maximum value of each measuring amount this moment by following formula:

${\mathrm{\&eta;}}_f=\underset{i=1}{\overset{6}{\max }}|F_{\mathrm{ni}}-F_{\mathrm{nc}}|,(n=x,y,z)$

${\mathrm{\&eta;}}_m=\underset{i=1}{\overset{6}{\max }}|M_{\mathrm{ni}}-M_{\mathrm{nc}}|,(n=x,y,z)$

F in the formula _Ni, M _NiThe amount that the expression sensor measurement is returned, F _Nc, M _NcThe compensation rate that the expression respective channel is calculated; The maximum value of each channel error is designated as η respectively _Fx, η _Fy, η _Fz, η _Mx, η _My, η _Mz

Get stability factor ε _Fx, ε _Fy, ε _Fz(all greater than 1, suggestion selects 1.5～2.0) makes the current zero-bit stable region of power/torque sensor be:

$\left\{\begin{array}{c}{\mathrm{\&delta;}}_{\mathrm{fx}}={\mathrm{\&eta;}}_{\mathrm{fx}}{\mathrm{\&epsiv;}}_{\mathrm{fx}}\\ {\mathrm{\&delta;}}_{\mathrm{fy}}={\mathrm{\&eta;}}_{\mathrm{fy}}{\mathrm{\&epsiv;}}_{\mathrm{fy}}\\ {\mathrm{\&delta;}}_{\mathrm{fz}}={\mathrm{\&eta;}}_{\mathrm{fz}}{\mathrm{\&epsiv;}}_{\mathrm{fz}}\\ {\mathrm{\&delta;}}_{\mathrm{mx}}={\mathrm{\&eta;}}_{\mathrm{mx}}{\mathrm{\&epsiv;}}_{\mathrm{mx}}\\ {\mathrm{\&delta;}}_{\mathrm{my}}={\mathrm{\&eta;}}_{\mathrm{my}}{\mathrm{\&epsiv;}}_{\mathrm{my}}\\ {\mathrm{\&delta;}}_{\mathrm{mz}}={\mathrm{\&eta;}}_{\mathrm{mz}}{\mathrm{\&epsiv;}}_{\mathrm{mz}}\end{array}\right.$

Then the power end zero-bit stable region of power/torque sensor is respectively

The zero-bit stable region of moment end is respectively [M _Xc-δ _Mx, M _Xc+ δ _Mx],

[M _Zc-δ _Mz, M _Zc+ δ _Mz]

Obtain the power under the current attitude of power/torque sensor and moment numerical stability interval are arranged by said method;

(4) obtaining the controlling party supernatural power end that power follows according to step (1), (2) and (3) design is:

$u_f=\left\{\begin{array}{cc}{k}_{\mathrm{fc}}(F_m-F_c+\mathrm{\&delta;})& F_m<F_c-\mathrm{\&delta;}\\ 0& F_c-\mathrm{\&delta;}<F_m<F_c+\mathrm{\&delta;}\\ k_{\mathrm{fc}}(F_m-F_c-\mathrm{\&delta;})& F_m>F_c+\mathrm{\&delta;}\end{array}\right.$

F in the formula _mBe illustrated in the measured value after the power that applies on certain force direction, F _cBe illustrated on this direction the compensation rate of power, δ represents the stable region on this direction;

In like manner, the control method of moment end is:

$u_m=\left\{\begin{array}{cc}{k}_{\mathrm{mc}}(M_m-M_c+\mathrm{\&delta;})& M_m<M_c-\mathrm{\&delta;}\\ 0& M_c-\mathrm{\&delta;}<M_m<M_c+\mathrm{\&delta;}\\ k_{\mathrm{mc}}(M_m-M_c-\mathrm{\&delta;})& M_m>M_c+\mathrm{\&delta;}\end{array}\right.$

M in the formula _mBe illustrated in the measured value after the power that applies on certain moment direction, M _cBe illustrated on this direction the compensation rate of power, δ represents the stable region on this direction;

Controlling value will feed back to carries out power and follows control in the robot, and is undertaken by following formula:

$\stackrel{\&CenterDot;}{q}=J^{-1}u$

Wherein J is the current Jacobi matrix of robot, and u is the control input of power and moment, is expressed as

$u=\left[\begin{array}{c}{u}_f\\ u_m\end{array}\right]=\left[\begin{array}{c}{u}_{\mathrm{fx}}\\ u_{\mathrm{fy}}\\ u_{\mathrm{fz}}\\ u_{\mathrm{mx}}\\ u_{\mathrm{my}}\\ u_{\mathrm{mz}}\end{array}\right]$

Speed for each joint of robot, after calculating the speed in each joint, utilize this speed that each joint of robot is controlled, make it do the power accompany movement, whenever robot pose changes, all need to use the method in the step (2) that the zero value of sensor is demarcated again.

Images