A kind of industrial robot external force method of estimation based on shake controlTechnical field
The present invention relates to Industrial Robot Technology field, more particularly to a kind of industrial robot external force based on shake controlMethod of estimation.
Background technology
It is generally basede on what position control was completed in conventional industrial robot's application task, such as carries, welds, sprays.WithThe extension in robot application field, increasing task not only needs Accurate Position Control to also need to accurate control machine peopleWith external contact force, such as assemble, polish, draw teaching.Real-time perception applied external force is needed in Robot Force control application, outsideForce information can be obtained by multiple dimension force/moment sensor measurement, can also pass through joint of robot torgue measurement and kinetic simulationEstimation is obtained type in real time.Current multiple dimension force/moment sensor is to small support industrial robot application from quality, volume or costAspect cost is all higher.
The external force method of estimation of non-moment sensor is a kind of more economical alternative solution.
The external force method of estimation of non-moment sensor is driven by the estimate to joint moment of kinetic model with motorIt is dynamic to compare, only rely on robot joint motions information in itself and driving current information can obtain external force in the effect point of each jointMeasure, but the difficult point that this method is present is 1) to set up complete Dynamic Models of Robot Manipulators and obtains accurate model data, 2)Frictional force estimates that especially under static or low-speed motion state Coulomb friction has very big uncertainty, how it is estimatedMeter and another difficult point that compensation is non-moment sensor.
Shake control (Dithering Control) is a kind of effective ways of Friction Compensation, passes through certain frequencyActive dither control signal, improves response characteristic in frictional force dead band, can effectively drop frictional force non-linear to being in dead bandThe influence for control accuracy of uniting, has been applied in high-precision servo tracing control.
The content of the invention
The purpose of the present invention is intended at least solve one of described technological deficiency.
Therefore, it is an object of the invention to propose a kind of industrial robot external force method of estimation based on shake control.
To achieve these goals, embodiments of the invention provide a kind of industrial robot external force based on shake control and estimatedMeter method, comprises the following steps：
Step S1, sets up external force estimation problem model；
Step S2, the model set up according to step S1 generates joint dither control signal, wherein, the joint shake controlThe function of signal processed is：
Wherein：T is engaging friction dead time；Amp is the amplitude of dither signal, is determined by the amplitude of Coulomb friction power,The parameter can be obtained by dynamic parameters identification；
For cycle square wave function, TditherFor dither control signal cycle, the frequency of dither signalDetermined by the dynamic response characteristic of Coulomb friction；TrampFor dither control signal ramp up time；
Step S3, while using response characteristic of the control raising system in frictional force dead band is actively shaken, passes throughExternal force interference transmission is joint control torque by joint control；
Step S4, extracts external power signal, including：By the intermediate value of extract real-time active dither control signal, and and motorControl moment is compared, and obtains the applied external force of robot end.
Further, in the step S1,
If robot Rigid Body Dynamics Model is：
Wherein：τ ' is control moment；G (q) is respectively the inertia force at joint end, coriolis force, againPower；
For joint-friction power；τeIt is the sextuple external force in end in the respective component at joint end, itself and the sextuple external force in endThere are following mapping relations：τe=JT(q)he, wherein, J (q) is robot speed's Jacobian matrix.
Further, the joint-friction powerRepresented with linear model, including dynamic friction and Coulomb friction two：
Wherein, Fv、FcThe respectively coefficient of kinetic friction and Coulomb friction coefficient.
Further, in the step S1,
Vibrate joint existence position by introducing periodic perturbation control signal in low regime, then Coulomb friction may be assumed that forWith the uniformly distributed random variable of velocity correlation, the Maximum-likelihood estimation problem of further frictional force and external force be converted to it is following mostExcellent estimation problem：
subject to τfmin≤τf≤τfmax
Wherein：ReAnd RefFor two estimation error variances battle array,Estimate average for external force, can be set by priori dataPut；
For motor control torque and inertia force, coriolis force, gravity etc. can Accurate Model torque difference, i.e.,：
The globally optimal solution of further optimal estimation problem is：
Moment of face estimation problem is converted to the observation of joint control power and frictional force estimation problem.
Further, in the step S2,
Shaken the ascent stage in frictional dead, the initial median of dither control signal and amplitude are zero, median target value for gramTake the active control torque of inertia force, coriolis force, gravityAmplitude desired value is Coulomb friction amplitude α Fc, wherein, α is adjustmentCoefficient；
It is being the uniform of value stabilization during active dither control signal is kept without External force interference in frictional dead shake saturation sectionVibration, until being changed in outside interference effect hypozygal motion state to sliding friction area, in sliding friction area, Coulomb friction powerDirection can accurately be judged by the gathered data of motor encoder, can be more accurate according to Frictional model and dynamics identified parametersEstimation frictional force, now cancel actively shake control, until joint motions are again introduced into frictional dead.
Further, in the step S3, using controller canceling position ring control action, while by improving speed ringProportional gain, to coordinate active dither control signal to improve the system response characteristic in static friction area.
Further, the step S4, comprises the following steps：
(1) motor control moment values are solved, are estimated with actual control moment sliding window average value
(2) estimation joint moment of face and frictional force sum
(3) joint-friction power is estimated
(4) robot end's external force is estimated
Industrial robot external force method of estimation based on shake control according to embodiments of the present invention, solves industrial machineThe external force estimation problem of people's non-moment sensor, it is uncertain for frictional force especially under static or low-speed motion stateEstimation problem.The present invention can gather information by industrial robot itself under non-moment sensor condition and more accurately estimateThe applied external force used in robot end is counted as, is controlled especially by active shake, is overcome in static friction dead band and rubPower uncertain problem so that the external force estimation of robot body can also obtain higher estimated accuracy in low regime.The present inventionCan apply in the applications such as the dragging teaching of no sensor, collision detection and power control assembling, with relatively low cost obtain compared withGood power sensitizing effect.
The additional aspect of the present invention and advantage will be set forth in part in the description, and will partly become from the following descriptionObtain substantially, or recognized by the practice of the present invention.
Brief description of the drawings
The above-mentioned and/or additional aspect and advantage of the present invention will become from description of the accompanying drawings below to embodiment is combinedSubstantially and be readily appreciated that, wherein：
Fig. 1 is the flow chart of the industrial robot external force method of estimation based on shake control according to the embodiment of the present invention；
Fig. 2 is the schematic diagram of the active dither control signal generating process according to the embodiment of the present invention；
Fig. 3 is the frame diagram of the control configuration according to the embodiment of the present invention.
Embodiment
Embodiments of the invention are described below in detail, the example of embodiment is shown in the drawings, wherein identical from beginning to endOr similar label represents same or similar element or the element with same or like function.Retouched below with reference to accompanying drawingThe embodiment stated is exemplary, it is intended to for explaining the present invention, and be not considered as limiting the invention.
As shown in figure 1, the industrial robot external force method of estimation based on shake control of the embodiment of the present invention, including it is as followsStep：
Step S1, sets up external force estimation problem model.
Specifically, robot Rigid Body Dynamics Model is as follows：
Wherein：τ ' control moments；G (q) is respectively the inertia force, coriolis force, gravity at joint end；
For joint-friction power, represented herein with linear model, including dynamic friction and Coulomb friction two：
Wherein Fv、FcThe respectively coefficient of kinetic friction and Coulomb friction coefficient.
τeIt is respective component of the sextuple external force in end at joint end, itself and end 6 DOF external force have following mapping relations：
τe=JT(q)he (0.3)
Wherein J (q) is robot speed's Jacobian matrix.
First four are this body dynamics on the right of equation (1.1), under the premise of modeling comparison is accurate, can pass through jointThe difference estimation applied external force of control moment and this body dynamics, modeling parameter used can pass through body kinetic parameterIdentification obtains more accurately data.But by formula (1.2) it can be seen that Coulomb friction is related to joint motions velocity attitude,Due to the presence of motor encoder signal acquisition noise under low regime or inactive state, it is difficult to pass through the reading of motor encoderNumber provides accurate joint motions direction, and is mainly Coulomb friction in low regime frictional force, so frictional force is in low regimeUncertainty be the estimation of real-time joint moment difficult point.Certain ratio is occupied in joint moment additionally, due to Coulomb friction powerWeight, if this estimation will definitely not cause larger external force estimated bias, can cause to control unstability when serious.
In high-precision servo control field, to eliminate uncertainty of the Coulomb friction in dead band, fixed frequency is introducedActive vibration signal can improve response characteristic of the system in frictional force dead band.The thought of actively shake control is used for reference, this is speciallyProfit makes joint there is a certain degree of position oscillation by introducing fixed cycle disturbance control signal in low regime, then Coulomb friction canIt is assumed to be the uniformly distributed random variable with velocity correlation, the Maximum-likelihood estimation problem of further frictional force and external force is converted toFollowing optimal estimation problem：
Wherein：
ReAnd RefFor two estimation error variances battle array,Estimate average for external force, can be set by priori data.
For motor control torque and inertia force, coriolis force, gravity etc. can Accurate Model torque difference, i.e.,：
The globally optimal solution of further optimal estimation problem (1.4) is：
Moment of face estimation problem is converted to the observation of joint control power and frictional force estimation problem.
Step S2, the model set up according to step S1 generates joint dither control signal.
Specifically, as shown in Fig. 2 active dither control signal is applied directly to Torque Control ring, using fixed cycle square waveSignal, to ensure that control process is steady, dither control signal uses the piecewise function form shown in formula (1.6), including：A. rubDead band is shaken the ascent stage, b. frictional deads dither signal saturation section, c. sliding frictions section.
Wherein：
T is engaging friction dead time；Amp is the amplitude of dither signal, is determined by the amplitude of Coulomb friction power, the parameterIt can be obtained by dynamic parameters identification；For cycle square wave function, TditherFor dither control signal cycle, dither signalFrequencyDetermined by the dynamic response characteristic of Coulomb friction.
TrampFor dither control signal ramp up time, the intermediate value and amplitude of dither signal are all according to figure within the timeTemporal regularity shown in 1 is ramped up.
Shaken the ascent stage in frictional dead, the initial median of dither control signal and amplitude are zero, median target value for gramTake the active control torque of inertia force, coriolis force, gravityAmplitude desired value is Coulomb friction amplitude α Fc(α is regulation coefficient).It is being the uniform vibration of value stabilization during active dither control signal is kept without External force interference, directly in frictional dead shake saturation sectionTo being changed in outside interference effect hypozygal motion state to sliding friction area, in sliding friction area, the direction of Coulomb friction powerIt can accurately be judged by the gathered data of motor encoder, accurate can be estimated according to Frictional model and dynamics identified parametersFrictional force, so now cancelling actively shake control, until joint motions are again introduced into frictional dead.
Step S3, while using response characteristic of the control raising system in frictional force dead band is actively shaken, passes throughExternal force interference transmission is joint control torque by joint control.
Specifically, for moment loading outside sensitivity, raising system is being controlled in frictional force dead band using actively shakeResponse characteristic while, it is necessary to by joint control by External force interference transmission be joint control torque, using the control shown in Fig. 3Configuration processed, relatively conventional three close-loop control, the controller canceling position ring control action, while being increased by improving velocity loop proportionalBenefit, to coordinate active dither control signal to improve the system response characteristic in static friction area.
Step S4, extracts external power signal, including：By the intermediate value of extract real-time active dither control signal, and and motorControl moment is compared, and obtains the applied external force of robot end.
In the case where actively shaking control action, control moment signal is to eliminate External force interference active force and actively shake control letterNumber superposition, by the intermediate value of extract real-time active dither control signal, and with motor control torque ratio to i.e. can obtain machineThe applied external force of people end (instrument).
(1) motor control moment values are solved, are estimated with actual control moment sliding window average value
(2) estimation joint moment of face and frictional force sum
(3) joint-friction power is estimated
(4) robot end's external force is estimated
The industrial solar term people external force method of estimation for being used to shake control of the embodiment of the present invention, it is possible to achieve set up joint fortuneThe model of dynamic low regime external force estimation problem, dynamic response of the control system in frictional force dead band is improved by shaking control,Realize application process of the shake control in all-purpose robot controller, and the robot external force estimative figure based on shake controlAccording to processing method.
Industrial robot external force method of estimation based on shake control according to embodiments of the present invention, solves industrial machineThe external force estimation problem of people's non-moment sensor, it is uncertain for frictional force especially under static or low-speed motion stateEstimation problem.The present invention can gather information by industrial robot itself under non-moment sensor condition and more accurately estimateThe applied external force used in robot end is counted as, is controlled especially by active shake, is overcome in static friction dead band and rubPower uncertain problem so that the external force estimation of robot body can also obtain higher estimated accuracy in low regime.The present inventionCan apply in the applications such as the dragging teaching of no sensor, collision detection and power control assembling, with relatively low cost obtain compared withGood power sensitizing effect.
In the description of this specification, reference term " one embodiment ", " some embodiments ", " example ", " specifically showThe description of example " or " some examples " etc. means to combine specific features, structure, material or the spy that the embodiment or example are describedPoint is contained at least one embodiment of the present invention or example.In this manual, to the schematic representation of above-mentioned term notNecessarily refer to identical embodiment or example.Moreover, specific features, structure, material or the feature of description can be anyOne or more embodiments or example in combine in an appropriate manner.
Although embodiments of the invention have been shown and described above, it is to be understood that above-described embodiment is exampleProperty, it is impossible to limitation of the present invention is interpreted as, one of ordinary skill in the art is not departing from the principle and objective of the present inventionIn the case of above-described embodiment can be changed within the scope of the invention, change, replace and modification.The scope of the present inventionExtremely equally limited by appended claims.