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

Joint torque limiting method based on robot dynamic model

Technical Field

The invention relates to a method for limiting joint torque of a series robot, in particular to a method for limiting joint torque based on a robot dynamic model.

Background

The series robot is more and more widely applied to the industries of manufacturing industry, agriculture, entertainment industry and the like. When the robot is in some extremely severe working conditions, output torque required by a joint servo motor may occur and exceed the maximum torque provided by the servo motor, so that a servo motor driver alarms, stops working and even damages the servo motor. Therefore, joint torque limitation is required to prevent accidents from occurring.

Judging whether the moment of the joints exceeds the limit in the whole robot motion process is not difficult, and only after the track is planned, the planned joint position, the angular velocity and the angular acceleration are substituted into a robot inverse dynamics model calculation formula shown in the formula (5-1) to calculate the output moment of each joint in the whole motion process, and the maximum value is selected from the output moments and compared with the product of the maximum moment of a servo motor manual multiplied by the reduction ratio, so that whether the moment of the joints exceeds the limit can be known. The difficulty is how to change the motion speed of the robot to keep the joint moment within the limit range on the premise of not changing the motion path after judging that the joint moment has the overrun.

Disclosure of Invention

The invention aims to provide a joint moment limiting method based on a robot dynamic model, and aims to solve the problem of changing the movement speed of a robot to keep the joint moment within a limiting range on the premise of not changing a movement path after judging that the joint moment exceeds the limit.

The purpose of the invention is realized by the following technical scheme:

a joint moment limiting method based on a robot dynamic model comprises the following steps:

s1, designing a dynamic scale related to time;

s2, establishing a functional relation between the expected joint moment and the limit joint moment;

s3, calculating the limiting torque and the expected torque in real time in the running process of the robot;

s4, if the expected torque is less than or equal to the limit torque, returning to the previous step S3; if the desired torque is greater than the limit torque, the next step S5 is performed;

and S5, calculating the function of the expected joint moment and the limited joint moment, obtaining the corresponding time dynamic scale as the joint interpolation period, and reducing the joint moment in a mode of increasing the joint motion time.

Further, the step S1 includes the following steps:

s1.1, after the trajectory planning, the motion trajectories of all joints of the robot are theta (t), t ∈ [0, t [_f]Assuming that the joint moment at a certain position of the joint motion track exceeds the limit, a new joint motion track is designedIt is assumed that it can ensure the joint torque to be within the safety limit range, and the formula (1) is satisfied

Where r is r (t), a strictly monotonically increasing function with respect to time t, and having r (0) 0, r (t)_f)＝t_f；

S1.2, first order and second order differential are respectively obtained for the formula (1) to obtain

For the trajectory theta (t), the inverse dynamics model of the robot is shown as the formula (4)

Rewriting the items of Coriolis force and centripetal force intoThe gravity term and the friction term are separated to obtain the formula (4)

Wherein,

for the same reason, for the trackCan obtain the product

Wherein,

s1.3, substituting formula (2) and formula (3)Is finished to obtain

Let t be r (t), willSubstitution of formula (7) to obtain

To sum up, the overrun joint moment tau (t) and the overrun joint moment can be obtainedThe relation between the function r (t) and the function r (t), wherein r (t) is called a dynamic scaling function, can be regarded as a mapping relation of the time t, is equivalent to the flow rate of the readjustment time, and maps the constant elapsed time into a functional relation which can be changed;

s1.4, to further simplify the formula (8), the dynamic scaling function r (t) may be the simplest linear scaling function as shown in the formula (9)

Wherein, in the formula of r (t) ═ ct (9), c is constant, then

Thus, the formula (5-9) can be simplified to

Further, the step S3 includes the following steps:

at the moment S3.1 and t, the inertial force, the Coriolis force and the centripetal force of the unlimited joint moment can be obtained

In the formula, τ_maxIs the joint limit moment, generally a fixed constant; g (theta (t)) is a gravity term of the robot inverse dynamics model;the friction force item of the robot inverse dynamics model is obtained;

at the time of S3.2 and t, the inertial force, the Coriolis force and the centripetal force items of the joint moment of the trajectory planning can be obtained by a robot inverse dynamics model

In the formula,an inertia force term of the robot inverse dynamics model is obtained;the coriolis force and centripetal force terms of the inverse kinematics model of the robot.

Compared with the prior art, the invention has the following advantages and effects:

the invention uses the inverse dynamics model of the robot, and can establish the functional relation between the expected joint moment and the limited joint moment by designing a dynamic scale related to time. When the joint moment exceeds the limit, the function of the expected joint moment and the limit joint moment is calculated to obtain the corresponding time dynamic scale as the joint interpolation period. The moving speed of the robot can be changed to keep the joint moment within the limit range on the premise of not changing the moving path.

Drawings

Fig. 1 is a schematic flow chart of a joint moment limiting method based on a robot dynamic model according to an embodiment of the invention.

Detailed Description

The present invention will be described in further detail with reference to examples and drawings, but the present invention is not limited thereto.

As shown in fig. 1, a robot with a six-degree-of-freedom vertical joint in series is now a joint torque limiting object, and joint torque limiting control based on a robot dynamic model is performed according to the following steps:

a joint moment limiting method based on a robot dynamic model comprises the following steps:

s1, designing a dynamic scale related to time;

s2, establishing a functional relation between the expected joint moment and the limit joint moment;

s3, calculating the limiting torque and the expected torque in real time in the running process of the robot;

s4, if the expected torque is less than or equal to the limit torque, returning to the previous step S3; if the desired torque is greater than the limit torque, the next step S5 is performed;

and S5, calculating the function of the expected joint moment and the limited joint moment, obtaining the corresponding time dynamic scale as the joint interpolation period, and reducing the joint moment in a mode of increasing the joint motion time.

Specifically, the step S1 includes the steps of:

s1.1, after the trajectory planning, the motion trajectories of all joints of the robot are theta (t), t ∈ [0, t [_f]Assuming that the joint moment at a certain position of the joint motion track exceeds the limit, a new joint motion track is designedIt is assumed that it can ensure the joint torque to be within the safety limit range, and the formula (1) is satisfied

Where r is r (t), a strictly monotonically increasing function with respect to time t, and having r (0) 0, r (t)_f)＝t_f。

S1.2, first order and second order differential are respectively obtained for the formula (1) to obtain

For the trajectory theta (t), the inverse dynamics model of the robot is shown as the formula (4)

Rewriting the items of Coriolis force and centripetal force intoThe gravity term and the friction term are separated to obtain the formula (4)

Wherein,

for the same reason, for the trackCan obtain the product

Wherein,

s1.3, substituting formula (2) and formula (3)Is finished to obtain

Let t be r (t), willSubstitution of formula (7) to obtain

To sum up, the overrun joint moment tau (t) and the overrun joint moment can be obtainedThe relation with the function r (t), which is called dynamic scaling function, can be regarded as a mapping relation of the time t, which is equivalent to the flow rate of the readjustment time, and the time which is constantly elapsed is mapped into a functional relation which can be changed.

S1.4, to further simplify the formula (8), the dynamic scaling function r (t) may be the simplest linear scaling function as shown in the formula (9)

Wherein, in the formula of r (t) ═ ct (9), c is constant, then

Thus, the formula (5-9) can be simplified to

Specifically, the step S3 includes the steps of:

at the moment S3.1 and t, the inertial force, the Coriolis force and the centripetal force of the unlimited joint moment can be obtained

In the formula, τ_maxIs the joint limiting moment, generally a fixed constant(ii) a g (theta (t)) is a gravity term of the robot inverse dynamics model;is the friction force item of the robot inverse dynamics model.

At the time of S3.2 and t, the inertial force, the Coriolis force and the centripetal force items of the joint moment of the trajectory planning can be obtained by a robot inverse dynamics model

In the formula,an inertia force term of the robot inverse dynamics model is obtained;the coriolis force and centripetal force terms of the inverse kinematics model of the robot.

The above embodiments are preferred embodiments of the present invention, but the present invention is not limited to the above embodiments, and any other changes, modifications, substitutions, combinations, and simplifications which do not depart from the spirit and principle of the present invention should be construed as equivalents and all such changes, modifications, substitutions, combinations, and simplifications are intended to be included in the scope of the present invention.

Claims (3)

1. A joint moment limiting method based on a robot dynamic model is characterized by comprising the following steps:

s1, designing a dynamic scale related to time;

s2, establishing a functional relation between the expected joint moment and the limit joint moment;

s3, calculating the limiting torque and the expected torque in real time in the running process of the robot;

s4, if the expected torque is less than or equal to the limit torque, returning to the previous step S3; if the desired torque is greater than the limit torque, the next step S5 is performed;

and S5, calculating the function of the expected joint moment and the limited joint moment, obtaining the corresponding time dynamic scale as the joint interpolation period, and reducing the joint moment in a mode of increasing the joint motion time.

2. The method for limiting joint moment based on robot dynamic model according to claim 1, wherein the step S1 comprises the steps of:

s1.1, after the trajectory planning, the motion trajectories of all joints of the robot are theta (t), t ∈ [0, t [_f]Assuming that the joint moment at a certain position of the joint motion track exceeds the limit, a new joint motion track is designedIt is assumed that it can ensure the joint torque to be within the safety limit range, and the formula (1) is satisfied

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

Where r is r (t), a strictly monotonically increasing function with respect to time t, and having r (0) 0, r (t)_f)＝t_f；

S1.2, first order and second order differential are respectively obtained for the formula (1) to 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 the trajectory theta (t), the inverse dynamics model of the robot is shown as the formula (4)

$\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)$

Rewriting the items of Coriolis force and centripetal force intoThe gravity term and the friction term are separated to obtain the formula (4)

$\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,

for the same reason, for the trackCan obtain the product

$\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, substituting formula (2) and formula (3)Is finished to obtain

${\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)$

Let t be r (t), willSubstitution of formula (7) to obtain

${\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)$

To sum up, the overrun joint moment tau (t) and the overrun joint moment can be obtainedThe relation between the function r (t) and the function r (t), wherein r (t) is called a dynamic scaling function, can be regarded as a mapping relation of the time t, is equivalent to the flow rate of the readjustment time, and maps the constant elapsed time into a functional relation which can be changed;

s1.4, to further simplify the formula (8), the dynamic scaling function r (t) may be the simplest linear scaling function as shown in the formula (9)

r(t)＝ct (9)

Wherein c is a constant, then

Thus, the formula (5-9) can be simplified to

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

3. The method for limiting joint moment based on robot dynamic model according to claim 1, wherein the step S3 comprises the steps of:

at the moment S3.1 and t, the inertial force, the Coriolis force and the centripetal force of the unlimited joint moment can be obtained

${\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 the formula, τ_maxIs the joint limit moment, generally a fixed constant; g (theta (t)) is a gravity term of the robot inverse dynamics model;the friction force item of the robot inverse dynamics model is obtained;

at the time of S3.2 and t, the inertial force, the Coriolis force and the centripetal force items of the joint moment of the trajectory planning can be obtained by a robot inverse dynamics model

${\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 the formula, H (θ (t))An inertia force term of the robot inverse dynamics model is obtained;the coriolis force and centripetal force terms of the inverse kinematics model of the robot.