CN104400783B - Tension force and joint position feedback flexible driving unit for robot joint control method - Google Patents

Abstract

Tension force and joint position feedback flexible driving unit for robot joint control method, belong to robot application control field.The present invention is in order to realize the flexible driving means tension feedback of joint of robot and joint full closed loop control, to reduce flexibility driving joint control error, to improve system frequency response.The present invention proposes control strategy based on viscoelastic Earth model model compensation and devises flexible driving unit controller, feedforward controller is designed according to steel wire rope elastic deformation formula, by motor angle estimate formula design of feedback controller, this method for designing is applicable to the controller design of flexible driving unit for robot joint；Build flexible driving unit control system hardware, carry out joint position feedback by joint encoders, utilize motor position estimate to carry out motor position feedback, it is achieved driver element control system position closed-loop.Compared with classical PID control method, it is remarkably improved SERVO CONTROL precision, reduces tracking error, improve system frequency response.

Description

Tension force and joint position feedback flexible driving unit for robot joint control method

Technical field

The present invention relates to a kind of flexible driving unit for robot joint control method, be specifically related to one and be applicable to rope driving, move Pulley reinforcement, there is the control method of the flexible driving unit for robot joint of tension force and joint position feedback, belong to robot Application controls field.

Background technology

Phase specific rigidity drives, and steel cable flexible drives can absorb vibrations, slows down impact, but due to its cause joint return difference relatively big, And have the most delayed, use classical PID control method less effective.Find that Chinese patent notification number is CN by literature search 101863034B, Patent No. 201010210888.3, the entitled joint of robot driven by assembly pulley and rope traction is used Flexible driving unit is that a kind of rope drives joint flexible driving unit, but does not give discharge control method.Application No. A kind of driving of restricting of 201410257786.5, movable pulley reinforcement, joint of robot with tension pick-up, joint encoders are used Flexible driving unit does not provide effective control method, it is impossible to realize the flexible driving means tension feedback of joint of robot and pass Joint full closed loop control, exists and controls the problem that error is big.

Summary of the invention

It is an object of the invention to for a kind of driving of restricting, movable pulley reinforcement, with tension pick-up, joint encoders robot close Save flexible driving unit (application number: 201410257786.5) and effective control method is provided, it is achieved joint of robot is with scratching Property driving means tension feedback and joint full closed loop control, drive joint control error, improve system frequency response reducing flexibility.

The present invention solves that above-mentioned technical problem adopts the technical scheme that:

A kind of flexible driving unit for robot joint control method with tension force and joint position feedback, described control method is used In control a kind of drivings of restricting, movable pulley reinforcement, with tension pick-up, joint encoders the flexible driving of joint of robot singly Unit, to realize the flexible driving means tension feedback of joint of robot and joint full closed loop control；The realization of described control method Cheng Wei:

Step one, derivation flexible driving unit viscoelastic Earth model model, to obtain driver element viscoelastic Earth model model and steel Cord elastic deformation formula；

Step 2, joint velocity is feedovered with acceleration: control strategy based on viscoelastic Earth model model compensation also designs Flexible driving unit controller, designs feedforward controller according to steel wire rope elastic deformation formula, by motor angle estimate formula Design of feedback controller；

Step 3, carry out joint closed-loop feedback control；Carry out joint position feedback by joint encoders, utilize motor position Estimate carries out motor position feedback, thus realizes driver element control system position closed-loop.

The process that implements of described control method is:

Step one, derivation flexible driving unit viscoelastic Earth model model, to obtain driver element viscoelastic Earth model model and steel Cord elastic deformation formula, its process is:

Reducer output shaft lay winding wire ropes steel wire rope tension is expressed as follows:

$F_i=-k_i(x_i-r_1{\mathrm{\θ}}_1)-c_i({\stackrel{\·}{x}}_i-r_1{\stackrel{\·}{\mathrm{\θ}}}_1)+F_{i,initial}---\left(7\right)$

Wherein, subscript i=1,2；

The implication of other parameter is as follows:

k_iRepresent steel wire rope L_iEquivalent stiffness, wherein i=1,2, k_i=EA/L_i

E represents steel wire rope Young's modulus, and A represents steel wire rope cross-sectional area, x_iRepresent steel wire rope L_iDisplacement, r₁Represent decelerator Output shaft radius, θ₁Represent reducer output shaft angle；c_iRepresent steel wire rope L_iEquivalent damping, c_i=f₁A/L_i, f₁Represent steel wire Rope viscosity；F_i,initialRepresent steel wire rope L_iPretightning force；

Solving above formula, take just displacement and be zero with initial velocity, obtaining reducer output shaft lay winding wire ropes displacement is:

$x_i=e^{-\frac{k_i}{c_i}t}\[(r_1{\mathrm{\θ}}_1+\frac{F_{i,initial}}{k_i})(e^{\frac{k_i}{c_i}t}-1)-\frac{1}{c_i}\underset{t_0}{\overset{t}{\∫}}{F}_i{e}^{\frac{k_i}{c_i}t}dt\]---\left(8\right)$

Wherein, subscript i=1,2；Had by running block reinforcement deceleration principle:

$\begin{array}{cccc}{F}_3^{\′}=4F_1^{\′}& F_4^{\′}=4F_2^{\′}& x_3=\frac{1}{4}{x}_1& x_4=\frac{1}{4}{x}_2\end{array}---\left(9\right)$

Owing to decelerator end steel wire rope tension is unknown, and joint end steel wire rope tension is recorded by tension pick-up, with joint end tension force Replace decelerator end steel wire rope tension, it is known that F₁,F₂,F₃,F₄With F₁', F₂', F₃', F₄' difference counter-force, then decelerator the most each other Output shaft lay winding wire ropes displacement is following form:

$x_i=e^{-\frac{k_i}{c_i}t}\[(r_1{\mathrm{\θ}}_1+\frac{F_{j,initial}}{4k_i})(e^{\frac{k_i}{c_i}t}-1)-\frac{1}{4c_i}\underset{t_0}{\overset{t}{\∫}}{F}_j{e}^{\frac{k_i}{c_i}t}dt\]---\left(10\right)$

Wherein, subscript i=1,2, j=3,4, simultaneous formula (9) and (10), obtaining joint output shaft lay winding wire ropes displacement is:

$x_j=\frac{1}{4}{e}^{-\frac{k_i}{c_i}t}\[(r_1{\mathrm{\θ}}_1+\frac{F_{j,initial}}{4k_i})(e^{\frac{k_i}{c_i}t}-1)-\frac{1}{4c_i}\underset{t_0}{\overset{t}{\∫}}{F}_j{e}^{\frac{k_i}{c_i}t}dt\]---\left(11\right)$

Wherein, subscript i, j value is ibid；Assuming that the stretching of both sides, flexible driving unit joint steel wire rope is identical with quantity of margin, closed Joint both sides steel wire rope deformation constraints:

x₃+x₄=2r₂θ₂ (13)

r₂Represent joint output shaft radius, θ₂Represent joint output shaft angle；

Simultaneous formula (11) and (13), obtain flexible driving unit joint angle formula based on viscoelastic Earth model:

${\mathrm{\θ}}_2({\mathrm{\θ}}_1,F_3,F_4,t)=\frac{1}{8r_2}\left\{\begin{array}{c}{e}^{-\frac{k_1}{c_1}t}\[(r_1{\mathrm{\θ}}_1+\frac{F_{3,initial}}{4k_1})(e^{\frac{k_1}{c_1}t}-1)-\frac{1}{4c_1}\underset{t_0}{\overset{t}{\∫}}{F}_3{e}^{\frac{k_1}{c_1}t}dt\]+\\ e^{-\frac{k_2}{c_2}t}\[(r_1{\mathrm{\θ}}_1+\frac{F_{4,initia1}}{4k_2})(e^{\frac{k_2}{c_2}t}-1)-\frac{1}{4c_2}\underset{t_0}{\overset{t}{\∫}}{F}_4{e}^{\frac{k_2}{c_2}t}dt\]\end{array}\right\}---\left(14\right)$

When ignoring rope stretch amount with unit speed relation, damped coefficient is zero, formula (7) try to achieve joint output shaft and be wound around steel Cord displacement is:

$x_j=\frac{1}{4}(r_1{\mathrm{\θ}}_1+\frac{F_{j,initial}-F_j}{4k_i})---\left(15\right)$

Wherein, subscript i=1,2, j=3,4；Deriving steel wire rope elastic deformation is:

△x_j=x_j-r₂θ₂ (16)

Wherein, subscript j value is ibid；Simultaneous formula (13) and (15), obtain flexible driving unit joint angle formula as follows:

${\mathrm{\θ}}_2({\mathrm{\θ}}_1,F_3,F_4)=\frac{1}{8r_2}(2r_1{\mathrm{\θ}}_1+\frac{F_{3,initial}-F_3}{4k_1}+\frac{F_{4,initial}-F_4}{4k_2})---\left(17\right)$

Step 2, joint velocity is feedovered with acceleration；

Can conveniently obtain the steel wire rope tension F of any time according to steel wire rope tension formula (7), this value comprise steel wire rope pretightning force, Restoring force after frictional force and rope stretch, simultaneous formula (15) and (16) can obtain both sides, joint steel wire rope elastic deformation:

${\mathrm{\Δx}}_j=\frac{1}{4}(r_1{\mathrm{\θ}}_1+\frac{F_{j,initial}-F_j}{4k_i})-r_2{\mathrm{\θ}}_2---\left(18\right)$

Wherein, subscript i, the same formula of j value (15)；As it is assumed that steel wire rope is in tension, therefore, △ x all the time₃With △ x₄All More than zero, feedforward calculates the steel wire rope elastic elongation correspondence articulation direction needed, and this stretch value is more than articulation opposite direction Stretch value, then, calculate in feedforward and first have to judge feedforward steel wire rope elastic elongation value to be used according to joint direction of rotation, Judge as follows: if θ₂> 0, i.e. joint rotates counterclockwise, then subscript j=3；Otherwise, j=4；Calculate feed forward velocity and acceleration Degree formula is as follows:

$\begin{array}{c}\mathrm{\Δ}{\stackrel{\·}{\mathrm{\θ}}}_0=i_m{i}_r{\mathrm{\Δx}}_j/r_1\mathrm{\Δ}T\\ \mathrm{\Δ}{\stackrel{\·\·}{\mathrm{\θ}}}_0=\mathrm{\Δ}{\stackrel{\·}{\mathrm{\θ}}}_0/\mathrm{\Δ}T\end{array}---\left(19\right)$

In formula:Represent motor feedforward angular speed,Representing motor feedforward angular acceleration, △ T represents servo period,

According to above formula can real time modifying velocity and acceleration feed-forward coefficients, this coefficient is by current reference position and reference velocity and feedforward Velocity and acceleration try to achieve:

$\begin{array}{c}{K}_{vff}=\mathrm{\Δ}{\stackrel{\·}{\mathrm{\θ}}}_0/{\stackrel{\·}{\mathrm{\θ}}}_{0,e}\\ k_{eff}=\mathrm{\Δ}{\stackrel{\·\·}{\mathrm{\θ}}}_0/{\stackrel{\·\·}{\mathrm{\θ}}}_{0,e}\end{array}---\left(20\right)$

Represent motor desired speed,Represent motor expectation acceleration, K_vffRepresent joint feed forward velocity coefficient, K_affRepresent Joint feedforward acceleration factor；

Step 3, carry out joint closed-loop feedback control；

Carrying out joint full closed loop control, design of feedback controller, the feedback physical quantity of needs is joint encoders angle value and tension force Sensor values, can be obtained by θ by formula (17)₂、r、F₃And F₄The motor angle estimate represented is as follows:

${\widehat{\mathrm{\θ}}}_0(F_3,F_4,{\mathrm{\θ}}_{2,r})=\frac{i_r}{2r_1}(8r_2{\mathrm{\θ}}_{2,r}-\frac{F_{3,initial}-F_3}{4k_1}-\frac{F_{4,initial}-F_4}{4k_2})---\left(21\right)$

In formula, θ_2,rRepresent joint actual angle；Represent motor angle estimate；

By viscoelastic Earth model model (formula 14), carry out related operation according to both sides, joint tension value and joint angle angle value and ask Go out motor actual angle, do not directly read motor encoder angle value at this and feed back, viscoelastic Earth model model is introduced To feedback control loop；

Motor angle estimate compares with motor reference value, both (" motor angle estimate " and " motor expectation angle value ") Error feedback compensation formula as follows:

$e_1=K_{P2}({\widehat{\mathrm{\θ}}}_0-{\mathrm{\θ}}_{0,e})---\left(22\right)$

e₁Represent angular error feedback, K_P2Represent motor position error feedback proportional coefficient, θ_0,eRepresent motor expected angle value, Represent motor angle estimate；

By in positional error compensation to command motor expected angle value, make motor reference locus according to motor actual angle variation tendency Carry out real time modifying, it is achieved FEEDBACK CONTROL, K_P2Affected by motor actual speed and steel wire rope tension factor, manually regulated K_P2Error to be made is in allowed band；

Motor angle estimate is carried out Difference Calculation, obtains motor speed estimate based on viscoelastic Earth model model, should Estimate and motor actual speed do difference, then both error feedback compensation formula are as follows:

$e_2=K_{D2}({\stackrel{\·}{\widehat{\mathrm{\θ}}}}_0-{\stackrel{\·}{\mathrm{\θ}}}_{0,r})---\left(23\right)$

In formula, e₂Represent velocity error feedback, K_D2Represent speed error of motor feedback proportional coefficient,Represent that motor speed is estimated Evaluation,Represent motor actual speed；

K_D2Affected by steel wire rope tension and motor actual speed, K_P2、K_D2According to steel wire rope tension, motor actual speed Carrying out real time modifying, concrete control method is: obtain Proportional coefficient K_P2, differential coefficient K_D2(speed error of motor feedback proportional Coefficient) it is stored in motion control card internal memory with steel wire rope tension, the corresponding relation of motor actual speed as tables of data, drive in flexibility Call this tables of data according to the steel wire rope tension gathered, motor actual speed when moving cell system is run thus carry out the real-time of parameter Amendment.

Joint angle is generated by host computer track creator according to the controller (referring to controller shown in Fig. 4) that described method designs And angular speed sequence, send to motion control card and carry out servo interpolation calculation, before servo interpolation calculation, first carry out joint angle and The feedback of angular speed, this feedback carries out arithmetic and micro-according to the joint angle formula that flexible driving unit viscoelastic Earth model is derived Point, the joint angles obtained and Attitude rate estimator value are for feeding back with angular speed with reference to angle joint；At servo interpolation meter After calculation, carry out the feedforward of speed and acceleration, end both sides, the joint steel that feed-forward coefficients obtains according to joint angle and steel wire rope tension Cord deflection is calculated.

Flexible driving unit in said method is FDU-II type flexible driving unit, described FDU-II type flexible driving unit Frequency sound test process is: incoming frequency sequence f is for from f₀Start, with f_dIt is incremented by for equal difference item, until f_n, total S frequency Sequence.Use Variable Amplitude Frequency cosine function as input function, make A₀For maximum amplitude, A₁For change amplitude vector, It is expressed as follows:

$A_1=A_0\×{\left[\begin{array}{cccc}1& \frac{S-1}{S}& \mathrm{...}& \frac{1}{S}\end{array}\right]}^{1.5}$

Reference input function is as follows:

$P_i=P_{i-1}+A_{1i}\[cos\left(\frac{2\mathrm{\π}}{T_i}{t}_i\right)-1\]$

Wherein, subscript i=12...S；Above-mentioned function is the input function of FDU-II type flexible driving unit frequency sound test.

The invention has the beneficial effects as follows:

The present invention is to provide one can realize tension feedback, joint full closed loop control for a kind of flexible driving unit for robot joint Control method, the present invention proposes control strategy based on viscoelastic Earth model model compensation and also devises flexible driving unit control Device, devises feedforward controller according to steel wire rope elastic deformation formula, devises FEEDBACK CONTROL by motor angle estimate formula Device, this method for designing is applicable to the controller design of flexible driving unit for robot joint, and is remarkably improved SERVO CONTROL essence Degree, reduces tracking error, and improves system frequency response；Build flexible driving unit control system hardware, pass through joint encoders Carry out joint position feedback, utilize motor position estimate to carry out motor position feedback, it is achieved thereby that driver element control system Position closed-loop.This control method advantage is that algorithm is simply effective, according to steel wire rope tension, unit can be carried out velocity and acceleration Feedforward, and realize the full closed loop control of joint of robot, compared with classical PID control method, it is remarkably improved SERVO CONTROL essence Degree, reduces tracking error, and improves system frequency response.

The inventive method realizes the flexible driving means tension feedback of joint of robot and joint full closed loop control, and reduces and scratch Property drive joint control error, improve system frequency response.The present invention proposes control strategy based on viscoelastic Earth model model compensation also Devise flexible driving unit controller, devise feedforward controller according to steel wire rope elastic deformation formula, estimated by motor angle Evaluation formula devises feedback controller, and this method for designing is applicable to the controller design of flexible driving unit for robot joint； Build flexible driving unit control system hardware, carried out joint position feedback by joint encoders, utilize motor position to estimate Value carries out motor position feedback, it is achieved thereby that driver element control system position closed-loop.

The inventive method is used for realizing the flexible driving means tension feedback of joint of robot and joint full closed loop control.The present invention carries Go out control strategy based on viscoelastic Earth model model compensation and design flexible driving unit controller, according to steel wire rope elastic deformation Formula devises feedforward controller, devises feedback controller by motor angle estimate formula；Build flexible driving unit control System hardware processed, carries out joint position feedback by joint encoders, utilizes motor position estimate to carry out motor position feedback, Thus realize driver element control system position closed-loop.

Accompanying drawing explanation

Fig. 1 be the present invention controlled device virtual prototype figure (i.e. a kind of rope of Application No. 201410257786.5 drive, dynamic sliding Take turns reinforcement, with tension pick-up, the flexible driving unit for robot joint of joint encoders), Fig. 2 is the controlled of the present invention Object mechanism principle figure (flexible driving unit mechanism principle), Fig. 3 is that the controlled device kinetic model schematic diagram of the present invention (is scratched Property driver element kinetic model), Fig. 4 is the controller block diagram that designs of the present invention and schematic diagram thereof, wherein, a) is controller Block diagram, b) is controller principle figure；Fig. 5 is application example control system hardware system photo of the present invention, wherein, a) is this Bright application example 1 control system hardware system photo (in figure: 1-host computer, 2-counterweight, 3-driver element, 4-charger, 5-motion control card and A/D, 6-driver, 7-direct-current switch power supply), b) it is application example 2 of the present invention control system hardware system System photo；Fig. 6 is control system block diagram of the present invention；Fig. 7-Figure 10 is frequency sound test result figure of the present invention, and it is to close joint respectively Opinion and actual rotational angle, joint tracking error, joint rotation angle magnitude peak dot frequency, amplitude versus frequency characte, Fig. 7 is that joint is theoretical and real Border corner comparison diagram, Fig. 8 is joint tracking error figure, and Fig. 9 is joint rotation angle magnitude peak dot frequency figure, and Figure 10 is that amplitude-frequency is special Property comparison diagram；Figure 11 is joint phase diagram.

Detailed description of the invention:

Detailed description of the invention one: as shown in Fig. 1～11, the realized tension feedback described in present embodiment, joint position feedback Flexible driving unit for robot joint control method by flexible driving unit viscoelastic Earth model model carry out joint position and Joint velocity is fed back, and carries out joint velocity and the feedforward of joint acceleration according to steel wire rope elastic deformation.Joint position is anti-with speed Feedback, i.e. introduces position ring and the speed ring in joint, it is achieved flexible driving unit on the basis of motor internal position ring with speed ring Joint full closed loop control.Feedovering the velocity and acceleration of elastic deformation, its basic thought is can be tried to achieve by elastic deformation formula Rope stretch amount, is tried to achieve by elongation and needs the velocity and acceleration of feedforward, by real time modifying velocity feed forward coefficient and acceleration Degree feed-forward coefficients reaches to compensate the purpose of rope stretch amount.Control object is flexible driving unit for robot joint, and it is empty Intend model machine as it is shown in figure 1, its mechanism principle as shown in Figure 2.

Can realize tension feedback, the flexible driving unit for robot joint control method of joint position feedback realizes as follows:

Step one, derivation flexible driving unit viscoelastic Earth model model (its schematic diagram is as shown in Figure 3), control system needs to use To flexible driving unit joint angle formula (i.e. driver element viscoelastic Earth model model) and steel wire rope elastic deformation formula；

Reducer output shaft lay winding wire ropes steel wire rope tension is expressed as follows:

$F_i=-k_i(x_i-r_1{\mathrm{\θ}}_1)-c_i({\stackrel{\·}{x}}_i-r_1{\stackrel{\·}{\mathrm{\θ}}}_1)+F_{i,initial}---\left(7\right)$

Wherein, subscript i=1,2, other amount is shown in Table one, lower with.Solve above formula, take just displacement and be zero with initial velocity, obtain Reducer output shaft lay winding wire ropes displacement is:

$x_i=e^{-\frac{k_i}{c_i}t}\[(r_1{\mathrm{\θ}}_1+\frac{F_{i,initial}}{k_i})(e^{\frac{k_i}{c_i}t}-1)-\frac{1}{c_i}\underset{t_0}{\overset{t}{\∫}}{F}_i{e}^{\frac{k_i}{c_i}t}dt\]---\left(8\right)$

Wherein, i value is ibid.Had by running block reinforcement deceleration principle:

$\begin{array}{cccc}{F}_3^{\′}=4F_1^{\′}& F_4^{\′}=4F_2^{\′}& x_3=\frac{1}{4}{x}_1& x_4=\frac{1}{4}{x}_2\end{array}---\left(9\right)$

Owing to decelerator end steel wire rope tension is unknown, and joint end steel wire rope tension can be recorded by tension pick-up, so with closing Joint end tension force replaces decelerator end steel wire rope tension, it is known that F₁,F₂,F₃,F₄With F '₁,F’₂,F’₃,F’₄Counter-force the most each other, then Reducer output shaft lay winding wire ropes displacement is following form:

$x_i=e^{-\frac{k_i}{c_i}t}\[(r_1{\mathrm{\θ}}_1+\frac{F_{j,initial}}{4k_i})(e^{\frac{k_i}{c_i}t}-1)-\frac{1}{4c_i}\underset{t_0}{\overset{t}{\∫}}{F}_j{e}^{\frac{k_i}{c_i}t}dt\]---\left(10\right)$

Wherein, subscript i=1,2, j=3,4.Simultaneous formula (9) and (10), obtaining joint output shaft lay winding wire ropes displacement is:

$x_j=\frac{1}{4}{e}^{-\frac{k_i}{c_i}t}\[(r_1{\mathrm{\θ}}_1+\frac{F_{j,initial}}{4k_i})(e^{\frac{k_i}{c_i}t}-1)-\frac{1}{4c_i}\underset{t_0}{\overset{t}{\∫}}{F}_j{e}^{\frac{k_i}{c_i}t}dt\]---\left(11\right)$

Wherein, subscript i, j value is ibid.Assuming that the stretching of both sides, flexible driving unit joint steel wire rope is identical with quantity of margin, closed Joint both sides steel wire rope deformation constraints:

x₃+x₄=2r₂θ₂ (13)

Simultaneous formula (11) and (13), obtain flexible driving unit joint angle formula based on viscoelastic Earth model:

${\mathrm{\θ}}_2({\mathrm{\θ}}_1,F_3,F_4,t)=\frac{1}{8r_2}\left\{\begin{array}{c}{e}^{-\frac{k_1}{c_1}t}\[(r_1{\mathrm{\θ}}_1+\frac{F_{3,inital}}{4k_1})(e^{\frac{k_1}{c_1}t}-1)-\frac{1}{4c_1}\underset{t_0}{\overset{t}{\∫}}{F}_3{e}^{\frac{k_1}{c_1}t}dt\]+\\ e^{-\frac{k_2}{c_2}t}\[(r_1{\mathrm{\θ}}_1+\frac{F_{4,initial}}{4k_2})(e^{\frac{k_2}{c_2}t}-1)-\frac{1}{4c_2}\underset{t_0}{\overset{t}{\∫}}{F}_4{e}^{\frac{k_2}{c_2}t}dt\]\end{array}\right\}---\left(14\right)$

When ignoring rope stretch amount with unit speed relation, damped coefficient is zero, formula (7) try to achieve joint output shaft and be wound around steel Cord displacement is:

$x_j=\frac{1}{4}(r_1{\mathrm{\θ}}_1+\frac{F_{j,initial}-F_j}{4k_i})---\left(15\right)$

Wherein, subscript i=1,2, j=3,4.Can derive steel wire rope elastic deformation is:

△x_j=x_j-r₂θ₂ (16)

Wherein, subscript j value is ibid.Simultaneous formula (13) and (15), obtain flexible driving unit joint angle formula as follows:

${\mathrm{\θ}}_2({\mathrm{\θ}}_1,F_3,F_4)=\frac{1}{8r_2}(2r_1{\mathrm{\θ}}_1+\frac{F_{3,initial}-F_3}{4k_1}+\frac{F_{4,initial}-F_4}{4k_2})---\left(17\right)$

Step 2, joint velocity is feedovered with acceleration；

Joint velocity is feedovered with acceleration, needs to design feedforward controller, mainly extend feelings according to steel wire rope elastic deformation Condition calculates, and elastic elongation is calculated by the tension force of tension pick-up and unit joint angles, according to steel wire rope tension Formula (7) can conveniently obtain the steel wire rope tension F of any time, and this value contains steel wire rope pretightning force, frictional force and steel wire rope and stretches Restoring force after length, simultaneous formula (15) and (16) can obtain both sides, joint steel wire rope elastic deformation:

$\begin{array}{cc}{\mathrm{\Δx}}_j=\frac{1}{4}(r_1{\mathrm{\θ}}_1+\frac{F_{j,initial}-F_j}{4k_i})-r_2{\mathrm{\θ}}_2& \left(18\right)\end{array}---\left(18\right)$

Wherein, subscript i, the same formula of j value (15).As it is assumed that steel wire rope is in tension, therefore, △ x all the time₃With △ x₄All More than zero, feedforward calculates the steel wire rope elastic elongation correspondence articulation direction needed, and this stretch value is more than articulation opposite direction Stretch value, then, calculate in feedforward and first have to judge feedforward steel wire rope elastic elongation value to be used according to joint direction of rotation, Judge as follows: if θ₂> 0 (i.e. joint rotates counterclockwise), then subscript j=3；Otherwise, j=4.Calculate feed forward velocity and acceleration Degree formula is as follows:

$\begin{array}{cc}\begin{array}{c}\mathrm{\Δ}{\stackrel{\·}{\mathrm{\θ}}}_0=i_m{i}_r{\mathrm{\Δx}}_j/r_1\mathrm{\Δ}T\\ \mathrm{\Δ}{\stackrel{\·\·}{\mathrm{\θ}}}_0=\mathrm{\Δ}{\stackrel{\·}{\mathrm{\θ}}}_0/\mathrm{\Δ}T\end{array}& \left(19\right)\end{array}---\left(19\right)$

According to above formula can real time modifying velocity and acceleration feed-forward coefficients, this coefficient is by current reference position and reference velocity and feedforward Velocity and acceleration try to achieve:

$\begin{array}{c}{K}_{vff}=\mathrm{\Δ}{\stackrel{\·}{\mathrm{\θ}}}_0/{\stackrel{\·}{\mathrm{\θ}}}_{0,e}\\ K_{aff}=\mathrm{\Δ}{\stackrel{\·\·}{\mathrm{\θ}}}_0/{\stackrel{\·\·}{\mathrm{\θ}}}_{0,e}\end{array}---\left(20\right)$

Step 3, carry out joint closed-loop feedback control.

Carrying out joint full closed loop control, design of feedback controller, the feedback physical quantity of needs is joint encoders angle value and tension force Sensor values, can be obtained by θ by formula (17)₂、r、F₃And F₄The motor angle estimate represented is as follows:

${\widehat{\mathrm{\θ}}}_0(F_3,F_4,{\mathrm{\θ}}_{2,r})=\frac{i_r}{2r_1}(8r_2{\mathrm{\θ}}_{2,r}-\frac{F_{3,initial}-F_3}{4k_1}-\frac{F_{4,initial}-F_4}{4k_2})---\left(21\right)$

By viscoelastic Earth model model, carry out related operation according to both sides, joint tension value and joint angle angle value and obtain motor reality Angle, does not directly read motor encoder angle value and feeds back, it is therefore an objective to be incorporated into instead by viscoelastic Earth model model at this Feedback controls loop, in experiment below, also demonstrates the feasibility of the method.

After obtaining motor angle estimate, comparing with motor reference value, both error feedback compensation formula are as follows:

$e_1=K_{P2}(\widehat{\mathrm{\θ}}-{\mathrm{\θ}}_{0,e})---\left(22\right)$

By in positional error compensation to command motor expected angle value, be equivalent to motor reference locus and change according to motor actual angle Trend carries out real time modifying, serves the effect of FEEDBACK CONTROL, simultaneously K_P2Affected by two factors again, i.e. motor is actual Speed and steel wire rope tension, according to the impact of the two factor, K_P2Need to take different value corresponding thereto.

Motor angle estimate is carried out Difference Calculation, obtains motor speed estimate based on viscoelastic Earth model model, should Estimate and motor actual speed do difference, then both error feedback compensation formula are as follows:

$e_2=K_{D2}({\stackrel{\·}{\widehat{\mathrm{\θ}}}}_0-{\stackrel{\·}{\mathrm{\θ}}}_{0,r})---\left(23\right)$

K_D2Also affected by steel wire rope tension and motor actual speed.Proportional coefficient K_P2, differential coefficient K_D2According to steel wire rope Tension force, motor actual speed carry out real time modifying, and concrete control method is: obtain Proportional coefficient K_P2, differential coefficient K_D2With steel Cord tension force, the corresponding relation of motor actual speed are stored in motion control card internal memory as tables of data, in flexible driving unit system Call this tables of data according to the steel wire rope tension gathered, motor actual speed during operation thus carry out the real time modifying of parameter.

Detailed description of the invention two: the controller block diagram designed according to this control method and schematic diagram thereof are as shown in Figure 4.Bold portion For classical PID SERVO CONTROL link, the controlling unit that according to dotted portion, this control method is supplemented.This controller is by upper Machine track creator generates joint angle and angular speed sequence, and transmission to motion control card carries out servo interpolation calculation, servo interpolation meter Before calculation, first carry out the feedback of joint angle and angular speed, the joint that this feedback is derived according to flexible driving unit viscoelastic Earth model Angle formula carries out arithmetic and differential, and the joint angles obtained and Attitude rate estimator value are for joint reference angle and angular speed Feed back；After servo interpolation calculation, carrying out the feedforward of speed and acceleration, feed-forward coefficients is according to joint angle and steel wire rope End both sides, the joint steel wire rope deflection that tension force obtains is calculated.

Detailed description of the invention three: this patent application example 1, the flexible driving unit control system i.e. built according to this control method Hardware photo such as Fig. 5 a) shown in, schematic diagram is as shown in Figure 6.Host computer carries out trajectory planning, the instruction planned is sent to PMAC motion control card, motor program, by Ethernet and host computer communication, is sent to PMAC by PMAC motion control card, PMAC can upload program or system mode to host computer；PMAC controls driver by PFM mode, and driver will letter Number amplify and be sent to motor；Motor encoder A phase, B phase, I (index) signal feed back to driver and PMAC simultaneously Motion control card；A, B, I signal are fed back to PMAC card by joint encoders, constitute full closed loop control；According to limit switch Signal value and tension feedback value carry out the enable of motor driver and control, thus system is carried out safeguard protection.

Detailed description of the invention four: carry out the frequency sound test of FDU-II type flexible driving unit.Incoming frequency sequence f is from f₀Start, With f_dIt is incremented by for equal difference item, until f_n, total S frequency sequence.Use Variable Amplitude Frequency cosine function as input function, Make A₀For maximum amplitude, A₁For the amplitude vector of change, it is expressed as follows:

$A_1=A_0\×{\left[\begin{array}{cccc}1& \frac{S-1}{S}& \mathrm{...}& \frac{1}{S}\end{array}\right]}^{1.5}$

Reference input function is as follows:

$P_i=P_{i-1}+A_{1i}\[cos\left(\frac{2\mathrm{\π}}{T_i}{t}_i\right)-1\]$

Wherein, subscript i=12...S.Reference input is as shown in Fig. 7 dotted line.Above-mentioned function is driven as FDU-II type flexibility The input function of moving cell frequency sound test, test result is as is seen in figs 7-10.The joint reference input amplitude of FDU-II is-5 °～0 ° , tracking error is about ± 0.2 °, and joint actual speed is about 80 °/s near magnitude peak point, is 60 °/s near remaining point. FDU-II joint rotation angle magnitude peak dot frequency as it is shown in figure 9, amplitude versus frequency characte as shown in Figure 10, after the 9th Frequency point, Its amplitude versus frequency characte starts to raise up, and i.e. the posterior joint output actual angle of this point is more than point of theory, shows that now FDU-II joint is returned Difference has been above theoretical amplitude, and FDU-II fan-out capability reaches capacity, is therefore clicked on as amplitude versus frequency characte by front nine Frequency points Line linearity matching, interpolation obtains double ten cut-off frequencies of FDU-II and is respectively 4.5Hz, 6.1Hz with classical cut-off frequency.

Detailed description of the invention five: this patent application example 2 is the biped robot's system with flexible driving unit, it controls system System hardware photo such as Fig. 5 b) shown in, flexible driving unit uses the control method described in claim one, carries out biped robot Walk test, single leg takes a step cycle 5s, and single pin supports phase 3s, double support phase 2s, and multiple step-length 260mm, robot is four Individual complete walking period (not including that walking starting stage half step and walking complete stage half step) interior walking distance 1040mm, step Row time 40s, reaches the leg speed of about 0.1km/h, and the joint limit cycle (macrocyclic in Figure 11) of flexible driving unit presents periodically Recurrent state, shows that robot is in stable state in the gait processes stepping multiple step.

The existing table one of implication ginseng of all parameters or variable in the inventive method:

The required argument table of table one control method of the present invention explanation

Claims (4)

1. a tension force and joint position feedback flexible driving unit for robot joint control method, described control method for control a kind of drivings of restricting, movable pulley reinforcement, with tension pick-up, the flexible driving unit for robot joint of joint encoders, to realize joint of robot flexibility driving means tension feedback and joint full closed loop control；It is characterized in that: the process that realizes of described control method is:

Step one, derivation flexible driving unit viscoelastic Earth model model, to obtain driver element viscoelastic Earth model model and steel wire rope elastic deformation formula；

Step 2, joint velocity is feedovered with acceleration: control strategy based on viscoelastic Earth model model compensation also designs flexible driving unit controller, feedforward controller is designed, by motor angle estimate formula design of feedback controller according to steel wire rope elastic deformation formula；

Step 3, carry out joint closed-loop feedback control；Carry out joint position feedback by joint encoders, utilize motor position estimate to carry out motor position feedback, thus realize driver element control system position closed-loop.

Tension force the most according to claim 1 and joint position feedback flexible driving unit for robot joint control method, it is characterised in that: the process that implements of described control method is:

Step one, derivation flexible driving unit viscoelastic Earth model model, to obtain driver element viscoelastic Earth model model and steel wire rope elastic deformation formula, its process is:

Reducer output shaft lay winding wire ropes steel wire rope tension is expressed as follows:

Wherein, subscript i=1,2；

The implication of other parameter is as follows:

k_iRepresent steel wire rope L_iEquivalent stiffness, wherein i=1,2, k_i=EA/L_i

E represents steel wire rope Young's modulus, and A represents steel wire rope cross-sectional area, x_iRepresent steel wire rope L_iDisplacement, r₁Represent reducer output shaft radius, θ₁Represent reducer output shaft angle；c_iRepresent steel wire rope L_iEquivalent damping, c_i=f₁A/L_i, f₁Represent steel wire rope viscosity；F_i,initialRepresent steel wire rope L_iPretightning force；

Solving above formula, take just displacement and be zero with initial velocity, obtaining reducer output shaft lay winding wire ropes displacement is:

Wherein, subscript i=1,2；Had by running block reinforcement deceleration principle:

F′₃=4F '₁F′₄=4F '₂

Owing to decelerator end steel wire rope tension is unknown, and joint end steel wire rope tension is recorded by tension pick-up, replaces decelerator end steel wire rope tension with joint end tension force, it is known that F₁,F₂,F₃,F₄With F '₁, F '₂, F '₃, F '₄Counter-force the most each other, then the displacement of reducer output shaft lay winding wire ropes is following form:

Wherein, subscript i=1,2, j=3,4, simultaneous formula (9) and (10), obtaining joint output shaft lay winding wire ropes displacement is:

Wherein, subscript i, j value is ibid；Assuming that the stretching of both sides, flexible driving unit joint steel wire rope is identical with quantity of margin, obtain both sides, joint steel wire ropes deformation constraints:

x₃+x₄=2r₂θ₂ (13)

r₂Represent joint output shaft radius, θ₂Represent joint output shaft angle；

Simultaneous formula (11) and (13), obtain flexible driving unit joint angle formula based on viscoelastic Earth model:

When ignoring rope stretch amount with unit speed relation, damped coefficient is zero, formula (7) trying to achieve joint output shaft lay winding wire ropes displacement is:

Wherein, subscript i=1,2, j=3,4；Deriving steel wire rope elastic deformation is:

Δx_j=x_j-r₂θ_{2 (16)}

Wherein, subscript j value is ibid；Simultaneous formula (13) and (15), obtain flexible driving unit joint angle formula as follows:

Step 2, joint velocity is feedovered with acceleration；

The steel wire rope tension F of any time can be conveniently obtained according to steel wire rope tension formula (7), this value comprises the restoring force after steel wire rope pretightning force, frictional force and rope stretch, and both sides, joint steel wire rope elastic deformation can be obtained in simultaneous formula (15) and (16):

Wherein, subscript i, the same formula of j value (15)；As it is assumed that steel wire rope is in tension, therefore, △ x all the time₃With △ x₄It is all higher than zero, feedforward calculates the steel wire rope elastic elongation correspondence articulation direction needed, and this stretch value is more than the reciprocal stretch value of articulation, then, calculate in feedforward and first have to judge, according to joint direction of rotation, the steel wire rope elastic elongation value that feedforward is to be used, it is judged that be as follows: if θ₂> 0, i.e. joint rotates counterclockwise, then subscript j=3；Otherwise, j=4；Calculate feed forward velocity as follows with Acceleration Formula:

In formula:Represent motor feedforward angular speed,Representing motor feedforward angular acceleration, △ T represents servo period,

According to above formula can real time modifying velocity and acceleration feed-forward coefficients, this coefficient tried to achieve by the velocity and acceleration of current reference position and reference velocity with feedforward:

Represent motor desired speed,Represent motor expectation acceleration, K_vffRepresent joint feed forward velocity coefficient, K_affRepresent joint feedforward acceleration factor；

Step 3, carry out joint closed-loop feedback control；

Carrying out joint full closed loop control, design of feedback controller, the feedback physical quantity of needs is joint encoders angle value and tension pick-up value, formula (17) can obtain by θ₂、r、F₃And F₄The motor angle estimate represented is as follows:

In formula, θ_2,rRepresent joint actual angle；Represent motor angle estimate；

By viscoelastic Earth model model, carry out related operation according to both sides, joint tension value and joint angle angle value and obtain motor actual angle, do not directly read motor encoder angle value at this and feed back, viscoelastic Earth model model is incorporated into feedback control loop；

Motor angle estimate compares with motor reference value, and both error feedback compensation formula are as follows:

e₁Represent angular error feedback, K_P2Represent motor position error feedback proportional coefficient, θ_0,eRepresent motor expected angle value,Represent motor angle estimate；

By in positional error compensation to command motor expected angle value, motor reference locus is made to carry out real time modifying according to motor actual angle variation tendency, it is achieved FEEDBACK CONTROL, K_P2Affected by motor actual speed and steel wire rope tension factor, manually regulated K_P2Error to be made is in allowed band；

Motor angle estimate is carried out Difference Calculation, obtains motor speed estimate based on viscoelastic Earth model model, this estimate and motor actual speed are done difference, then both error feedback compensation formula are as follows:

In formula, e₂Represent velocity error feedback, K_D2Represent speed error of motor feedback proportional coefficient,Represent motor speed estimate,Represent motor actual speed；

K_D2Affected by steel wire rope tension and motor actual speed, K_P2、K_D2Carrying out real time modifying according to steel wire rope tension, motor actual speed, concrete control method is: obtain Proportional coefficient K_P2, differential coefficient K_D2It is stored in motion control card internal memory with the corresponding relation of steel wire rope tension, motor actual speed as tables of data, calls this tables of data when flexible driving unit system is run according to the steel wire rope tension gathered, motor actual speed thus carry out the real time modifying of parameter.

A kind of tension force the most according to claim 2 and joint position feedback flexible driving unit for robot joint control method, it is characterized in that: generate joint angle and angular speed sequence according to the controller that described method designs by host computer track creator, transmission to motion control card carries out servo interpolation calculation, before servo interpolation calculation, first carry out the feedback of joint angle and angular speed, the joint angle formula that this feedback is derived according to flexible driving unit viscoelastic Earth model carries out arithmetic and differential, the joint angles obtained and Attitude rate estimator value are for feeding back with angular speed with reference to angle joint；After servo interpolation calculation, carrying out the feedforward of speed and acceleration, end both sides, the joint steel wire rope deflection that feed-forward coefficients obtains according to joint angle and steel wire rope tension is calculated.

4. according to a kind of tension force described in claim 1,2 or 3 and joint position feedback flexible driving unit for robot joint control method, it is characterized in that: the flexible driving unit in described method is FDU-II type flexible driving unit, the frequency sound test process of FDU-II type flexible driving unit is: incoming frequency sequence f is for from f₀Start, with f_dIt is incremented by for equal difference item, until f_n, total S frequency sequence；Use Variable Amplitude Frequency cosine function as input function, make A₀For maximum amplitude, A₁For the amplitude vector of change, it is expressed as follows:

Reference input function is as follows:

Wherein, subscript i=1 2 ... S；Above-mentioned function is the input function of FDU-II type flexible driving unit frequency sound test.