CN106903692A - A kind of joint moment method for limiting based on Dynamic Models of Robot Manipulators - Google Patents

Abstract

The invention discloses a kind of joint moment method for limiting based on Dynamic Models of Robot Manipulators.Using inverse dynamics of robot model, by designing a Dynamic Scaling on the time, the functional relation expected between joint moment and limitation joint moment can be set up.When there is joint moment beyond limitation, the function of joint moment and limitation joint moment is expected by calculating, try to achieve corresponding time Dynamic Scaling, as joint interpolation cycle, in the way of increasing the joint motions time, reduce joint moment, play a part of joint moment limiter, prevent the torque in motion process from transfiniting.The present invention solves the problems, such as that the shutdown torque being likely to occur in robot kinematics can be provided maximum moment beyond joint servo motor.

Description

A kind of joint moment method for limiting based on Dynamic Models of Robot Manipulators

Technical field

It is more particularly to a kind of to be based on Dynamic Models of Robot Manipulators the present invention relates to serial manipulator joint moment method for limiting Joint moment method for limiting.

Background technology

Serial manipulator is increasingly widely applied in industries such as manufacturing industry, agricultural, show businesses.When robot is in Under the working condition of some extremes, it is possible to the output torque needed for there is joint servo motor, beyond servomotor institute The torque capacity that can be provided, causes motor servo driver to be alarmed, and is stopped, or even cause servomotor to damage.Therefore, need Joint moment limitation is carried out, to prevent the generation of contingency.

Judge to be transfinited with the presence or absence of joint moment in whole robot kinematics not difficult, it is only necessary in trajectory planning Afterwards, the inverse dynamics of robot model that joint position, angular speed and the angular acceleration that will be planned substitute into shown in formula (5-1) is calculated In formula, whole each joint output torque of motion process is calculated to obtain, select maximum and servomotor handbook maximum, force The product that square is multiplied by after speed reducing ratio is compared, you can know that joint moment whether there is situation about transfiniting.It is difficult to judging to close , in the presence of after transfiniting, how on the premise of motion path is not changed, change robot movement velocity is so that joint power for section torque Square is maintained within the scope of limitation.

The content of the invention

It is an object of the invention to provide a kind of joint moment method for limiting based on Dynamic Models of Robot Manipulators, it is intended to solve Judge joint moment exist transfinite after, how on the premise of motion path is not changed, change robot movement velocity so that Joint moment is maintained at the problem within the scope of limitation.

The purpose of the present invention is realized by following technical proposals：

A kind of joint moment method for limiting based on Dynamic Models of Robot Manipulators, comprises the following steps：

S1, one Dynamic Scaling on the time of design；

S2, the functional relation set up between expecting joint moment and limiting joint moment；

S3, in robot running, in real time calculate restraining moment and expect torque；

S4, if it is desire to torque is less than or equal to restraining moment, then return to previous step S3；If it is desire to torque is more than limit Torque processed, then carry out next step S5；

S5, the function for calculating expectation joint moment and limitation joint moment, try to achieve corresponding time Dynamic Scaling, as pass Section interpolation cycle, in the way of increasing the joint motions time, reduces joint moment.

Further, the step S1 is comprised the following steps：

S1.1, by trajectory planning after, the articulate movement locus of institute of robot is θ (t), t ∈ [0, t_f], it is assumed that should There is the situation that joint moment transfinites in oint motion trajectory somewhere, then design a new oint motion trajectoryIt is assumed that It can guarantee that joint moment ensures within safety limit range, and meet formula (1)

Wherein r=r (t), is the strictly monotone increasing function on time t, and has r (0)=0, r (t_f)=t_f；

S1.2, single order and second-order differential are asked respectively to formula (1), can obtain

For track θ (t), shown in the inverse dynamics model such as formula (4) of robot

It is by coriolis force and centripetal force term rewritingGravity and frictional force is independent, can obtain Formula (4)

Wherein,

Similarly, for trackCan obtain

Wherein,

S1.3, formula (2) and formula (3) are substituted intoArrange

T=r (t) is made, willSubstitution formula (7), obtains

Can to sum up transfinite joint moment τ (t) and the joint moment that do not transfiniteRelation between and function r (t), r (t) Referred to as Dynamic Scaling function, a kind of mapping relations of time t can be regarded as, equivalent to the flow velocity of time of readjusting, by perseverance The time of fixed constant passage, it is mapped as the functional relation that can be changed；

S1.4, for further simplified style (8), Dynamic Scaling function r (t) can be chosen most simple as shown in formula (9) Lineal scale function

In r (t)=ct (9) formula, c is constant, then have

Then, formula (5-9) can be reduced to

Further, the step S3 is comprised the following steps：

S3.1, t, the inertia force of the joint moment that do not transfinite, coriolis force and centripetal force can be in the hope of

In formula, τ_maxIt is joint restraining moment, generally fixed constant；G (θ (t)) is the weight of inverse dynamics of robot model Power；It is the frictional force of inverse dynamics of robot model；

S3.2, t, the inertia force of the joint moment of trajectory planning, coriolis force and centripetal force can be by robot inverses Kinetic model is tried to achieve

In formula,It is the inertia item of inverse dynamics of robot model；For The coriolis force and centripetal force of inverse dynamics of robot model.

The present invention has the following advantages and effect relative to prior art：

The present invention uses inverse dynamics of robot model, by designing a Dynamic Scaling on the time, can set up Expect the functional relation between joint moment and limitation joint moment.When there is joint moment beyond limitation, by meter The function for expecting joint moment and limitation joint moment is calculated, corresponding time Dynamic Scaling is tried to achieve, as joint interpolation cycle.Can To ensure on the premise of motion path is not changed, change robot movement velocity so that joint moment be maintained at limitation scope it Interior problem.

Brief description of the drawings

Fig. 1 is the joint moment method for limiting schematic flow sheet based on Dynamic Models of Robot Manipulators of the embodiment of the present invention.

Specific embodiment

With reference to embodiment and accompanying drawing, the present invention is described in further detail, but embodiments of the present invention are not limited In this.

As shown in figure 1, it is existing with the artificial joint moment limitation object of certain six degree of freedom drag articulation serial machine, according to following Step carries out the joint moment limitation control based on Dynamic Models of Robot Manipulators：

A kind of joint moment method for limiting based on Dynamic Models of Robot Manipulators, comprises the following steps：

S1, one Dynamic Scaling on the time of design；

S2, the functional relation set up between expecting joint moment and limiting joint moment；

S3, in robot running, in real time calculate restraining moment and expect torque；

S4, if it is desire to torque is less than or equal to restraining moment, then return to previous step S3；If it is desire to torque is more than limit Torque processed, then carry out next step S5；

S5, the function for calculating expectation joint moment and limitation joint moment, try to achieve corresponding time Dynamic Scaling, as pass Section interpolation cycle, in the way of increasing the joint motions time, reduces joint moment.

Specifically, the step S1 is comprised the following steps：

S1.1, by trajectory planning after, the articulate movement locus of institute of robot is θ (t), t ∈ [0, t_f], it is assumed that should There is the situation that joint moment transfinites in oint motion trajectory somewhere, then design a new oint motion trajectoryIt is assumed that It can guarantee that joint moment ensures within safety limit range, and meet formula (1)

Wherein r=r (t), is the strictly monotone increasing function on time t, and has r (0)=0, r (t_f)=t_f。

S1.2, single order and second-order differential are asked respectively to formula (1), can obtain

For track θ (t), shown in the inverse dynamics model such as formula (4) of robot

It is by coriolis force and centripetal force term rewritingGravity and frictional force is independent, can obtain Formula (4)

Wherein,

Similarly, for trackCan obtain

Wherein,

S1.3, formula (2) and formula (3) are substituted intoArrange

T=r (t) is made, willSubstitution formula (7), obtains

Can to sum up transfinite joint moment τ (t) and the joint moment that do not transfiniteRelation between and function r (t), r (t) Referred to as Dynamic Scaling function, a kind of mapping relations of time t can be regarded as, equivalent to the flow velocity of time of readjusting, by perseverance The time of fixed constant passage, it is mapped as the functional relation that can be changed.

S1.4, for further simplified style (8), Dynamic Scaling function r (t) can be chosen most simple as shown in formula (9) Lineal scale function

In r (t)=ct (9) formula, c is constant, then have

Then, formula (5-9) can be reduced to

Specifically, the step S3 is comprised the following steps：

S3.1, t, the inertia force of the joint moment that do not transfinite, coriolis force and centripetal force can be in the hope of

In formula, τ_maxIt is joint restraining moment, generally fixed constant；G (θ (t)) is the weight of inverse dynamics of robot model Power；It is the frictional force of inverse dynamics of robot model.

S3.2, t, the inertia force of the joint moment of trajectory planning, coriolis force and centripetal force can be by robot inverses Kinetic model is tried to achieve

In formula,It is the inertia item of inverse dynamics of robot model；For The coriolis force and centripetal force of inverse dynamics of robot model.

Above-described embodiment is present aspect preferably implementation method, but the bright implementation method of we is not by above-described embodiment Limitation, it is other it is any away from Spirit Essence of the invention and the change, modification, replacement made under principle, combine, simplification, Equivalent substitute mode is should be, is included within protection scope of the present invention.

Claims (3)

1. a kind of joint moment method for limiting based on Dynamic Models of Robot Manipulators, it is characterised in that comprise the following steps：

S1, one Dynamic Scaling on the time of design；

S2, the functional relation set up between expecting joint moment and limiting joint moment；

S3, in robot running, in real time calculate restraining moment and expect torque；

S4, if it is desire to torque is less than or equal to restraining moment, then return to previous step S3；If it is desire to torque is more than restraint Square, then carry out next step S5；

S5, the function for calculating expectation joint moment and limitation joint moment, try to achieve corresponding time Dynamic Scaling, are inserted as joint In the benefit cycle, in the way of increasing the joint motions time, reduce joint moment.

2. a kind of joint moment method for limiting based on Dynamic Models of Robot Manipulators according to claim 1, its feature exists In the step S1 is comprised the following steps：

S1.1, by trajectory planning after, the articulate movement locus of institute of robot is θ (t), t ∈ [0, t_f], it is assumed that the joint There is the situation that joint moment transfinites in movement locus somewhere, then design a new oint motion trajectoryIt is assumed that its energy Ensure that joint moment ensures within safety limit range, and meet formula (1)

$\widetilde{\mathrm{\θ}}\left(t\right)=\mathrm{\θ}\left(r\right)---\left(1\right)$

Wherein r=r (t), is the strictly monotone increasing function on time t, and has r (0)=0, r (t_f)=t_f；

S1.2, single order and second-order differential are asked respectively to formula (1), can obtain

$\stackrel{\·}{\widetilde{\mathrm{\θ}}}\left(t\right)=\stackrel{\·}{\mathrm{\θ}}\left(r\right)\stackrel{\·}{r}\left(t\right)---\left(2\right)$

$\stackrel{\·\·}{\widetilde{\mathrm{\θ}}}\left(t\right)=\stackrel{\·\·}{\mathrm{\θ}}\left(r\right){\stackrel{\·}{r}}^2\left(t\right)+\stackrel{\·}{\mathrm{\θ}}\left(r\right)\stackrel{\·\·}{r}\left(t\right)---\left(3\right)$

For track θ (t), shown in the inverse dynamics model such as formula (4) of robot

$\mathrm{\τ}\left(t\right)=H\left(\mathrm{\θ}\right(t\left)\right)\stackrel{\·\·}{\mathrm{\θ}}\left(t\right)+C\left(\mathrm{\θ}\right(t),\stackrel{\·}{\mathrm{\θ}}(t\left)\right)\stackrel{\·}{\mathrm{\θ}}\left(t\right)+g\left(\mathrm{\θ}\right(t\left)\right)+f\left(\stackrel{\·}{\mathrm{\θ}}\right(t\left)\right)---\left(4\right)$

It is by coriolis force and centripetal force term rewritingGravity and frictional force is independent, formula (4) can be obtained

$\mathrm{\τ}\left(t\right)={\mathrm{\τ}}_a\left(t\right)+g\left(\mathrm{\θ}\right(t\left)\right)+f\left(\stackrel{\·}{\mathrm{\θ}}\right(t\left)\right)---\left(5\right)$

Wherein,

Similarly, for trackCan obtain

$\widetilde{\mathrm{\τ}}\left(t\right)={\widetilde{\mathrm{\τ}}}_a\left(t\right)+g\left(\widetilde{\mathrm{\θ}}\right(t\left)\right)+f\left(\stackrel{\·}{\widetilde{\mathrm{\θ}}}\right(t\left)\right)---\left(6\right)$

Wherein,

S1.3, formula (2) and formula (3) are substituted intoArrange

${\widetilde{\mathrm{\τ}}}_a\left(t\right)=\[H\left(\mathrm{\θ}\right(r\left)\right)\stackrel{\·\·}{\mathrm{\θ}}\left(r\right)+\stackrel{\·}{\mathrm{\θ}}\left(r\right)C\left(\mathrm{\θ}\right(r\left)\right)\stackrel{\·}{\mathrm{\θ}}\left(r\right)\]{\stackrel{\·}{r}}^2\left(t\right)+H\left(\mathrm{\θ}\right(r\left)\right)\stackrel{\·}{\mathrm{\θ}}\left(r\right)\stackrel{\·\·}{r}\left(t\right)---\left(7\right)$

T=r (t) is made, willSubstitution formula (7), obtains

${\widetilde{\mathrm{\τ}}}_a\left(t\right)={\stackrel{\·}{r}}^2\left(t\right){\mathrm{\τ}}_a\left(r\right)+\stackrel{\·\·}{r}\left(t\right)H\left(\mathrm{\θ}\right(r\left)\right)\stackrel{\·}{\mathrm{\θ}}\left(r\right)---\left(8\right)$

Can to sum up transfinite joint moment τ (t) and the joint moment that do not transfiniteRelation between and function r (t), r (t) is claimed Make dynamic scaling function, a kind of mapping relations of time t can be regarded as, equivalent to the flow velocity of time of readjusting, by it is constant not Become the time of passage, be mapped as the functional relation that can be changed；

S1.4, for further simplified style (8), Dynamic Scaling function r (t) can choose the simplest line as shown in formula (9) Property scaling function

R (t)=ct (9)

In formula, c is constant, then have

Then, formula (5-9) can be reduced to

${\widetilde{\mathrm{\τ}}}_a\left(t\right)=c^2{\mathrm{\τ}}_a\left(r\right).---\left(10\right)$

3. a kind of joint moment method for limiting based on Dynamic Models of Robot Manipulators according to claim 1, its feature exists In the step S3 is comprised the following steps：

S3.1, t, the inertia force of the joint moment that do not transfinite, coriolis force and centripetal force can be in the hope of

${\widetilde{\mathrm{\τ}}}_a\left(t\right)={\mathrm{\τ}}_{max}-g\left(\mathrm{\θ}\right(t\left)\right)-f\left(\stackrel{\·}{\mathrm{\θ}}\right(t\left)\right)---\left(11\right)$

In formula, τ_maxIt is joint restraining moment, generally fixed constant；G (θ (t)) is the gravity of inverse dynamics of robot model ；It is the frictional force of inverse dynamics of robot model；

S3.2, t, the inertia force of the joint moment of trajectory planning, coriolis force and centripetal force can be by robot inverse power Model is learned to try to achieve

${\mathrm{\τ}}_a\left(t\right)=H\left(\mathrm{\θ}\right(t\left)\right)\stackrel{\·\·}{\mathrm{\θ}}\left(t\right)+\stackrel{\·}{\mathrm{\θ}}\left(t\right)C\left(\mathrm{\θ}\right(t\left)\right)\stackrel{\·}{\mathrm{\θ}}\left(t\right)---\left(12\right)$

In formula, H (θ (t))It is the inertia item of inverse dynamics of robot model；It is robot The coriolis force and centripetal force of inverse dynamics model.