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

Abstract

Translated fromChinese

本发明公开了一种基于机器人动力学模型的关节力矩限制方法。使用机器人逆动力学模型，通过设计一个关于时间的动态标度，可以建立期望关节力矩和限制关节力矩之间的函数关系。当出现关节力矩超出限制的时候，通过计算期望关节力矩和限制关节力矩的函数，求得相应的时间动态标度，作为关节插补周期，以增加关节运动时间的方式，减小关节力矩，起到关节力矩限制器的作用，防止运动过程中的力矩超限。本发明解决机器人运动过程中可能出现的关机力矩超出关节伺服电机所能提供最大力矩的问题。

The invention discloses a joint torque limiting method based on a robot dynamics model. Using the robot inverse dynamics model, by designing a dynamic scale with respect to time, the functional relationship between the desired joint torque and the limited joint torque can be established. When the joint torque exceeds the limit, the corresponding time dynamic scale is obtained by calculating the function of the expected joint torque and the limited joint torque, which is used as the joint interpolation cycle, and the joint torque is reduced by increasing the joint movement time. To the role of the joint torque limiter to prevent the torque from exceeding the limit during the movement. The invention solves the problem that the shut-down torque that may occur during the movement of the robot exceeds the maximum torque that the joint servo motor can provide.

Description

Translated fromChinese

一种基于机器人动力学模型的关节力矩限制方法A Joint Torque Limiting Method Based on Robot Dynamics Model

技术领域technical field

本发明涉及串联机器人关节力矩限制方法，特别涉及一种基于机器人动力学模型的关节力矩限制方法。The invention relates to a joint torque limiting method of a series robot, in particular to a joint torque limiting method based on a robot dynamic model.

背景技术Background technique

串联机器人在制造业、农业、娱乐业等行业得到越来越广泛的应用。当机器人处于某些极度恶劣的工作状况下，有可能出现关节伺服电机所需的输出力矩，超出伺服电机所能提供的最大转矩，导致伺服电机驱动器报警，停止工作，甚至造成伺服电机损坏。因此，需要进行关节力矩限制，以防止意外事故的发生。Tandem robots are increasingly used in manufacturing, agriculture, entertainment and other industries. When the robot is in some extremely bad working conditions, the output torque required by the joint servo motor may exceed the maximum torque that the servo motor can provide, causing the servo motor driver to alarm, stop working, and even cause damage to the servo motor. Therefore, it is necessary to limit the joint torque to prevent accidents.

判断整个机器人运动过程中是否存在关节力矩超限并不困难，只需要在轨迹规划之后，将规划的关节位置、角速度和角加速度代入式(5-1)所示的机器人逆动力学模型计算公式中，算得整个运动过程各个关节输出力矩，从中挑选出最大值与伺服电机手册最大力矩乘上减速比后的积进行比较，即可知道关节力矩是否存在超限的情况。困难的是判断关节力矩存在超限之后，如何在不改变运动路径的前提下，更改机器人运动速度以使关节力矩保持在限制范围之内。It is not difficult to judge whether there is joint torque exceeding the limit during the entire robot movement process. It is only necessary to substitute the planned joint position, angular velocity and angular acceleration into the calculation formula of the robot inverse dynamics model shown in formula (5-1) after trajectory planning , calculate the output torque of each joint in the whole motion process, select the maximum value from it and compare it with the product of the maximum torque in the servo motor manual multiplied by the reduction ratio, and then you can know whether the joint torque exceeds the limit. The difficulty is how to change the motion speed of the robot to keep the joint torque within the limit range without changing the motion path after judging that the joint torque exceeds the limit.

发明内容Contents of the invention

本发明的目的是提供一种基于机器人动力学模型的关节力矩限制方法，旨在解决判断关节力矩存在超限之后，如何在不改变运动路径的前提下，更改机器人运动速度以使关节力矩保持在限制范围之内的问题。The purpose of the present invention is to provide a joint torque limitation method based on the robot dynamics model, aiming to solve the problem of how to change the motion speed of the robot to keep the joint torque at problems within the limits.

本发明的目的通过下述技术方案来实现：The purpose of the present invention is achieved through the following technical solutions:

一种基于机器人动力学模型的关节力矩限制方法，包括如下步骤：A joint torque limitation method based on a robot dynamics model, comprising the steps of:

S1、设计一个关于时间的动态标度；S1. Design a dynamic scale about time;

S2、建立期望关节力矩和限制关节力矩之间的函数关系；S2. Establishing a functional relationship between the desired joint torque and the limited joint torque;

S3、在机器人运行过程中，实时计算限制力矩和期望力矩；S3. During the operation of the robot, calculate the limit torque and expected torque in real time;

S4、如果期望力矩小于或者等于限制力矩，则返回上一步S3；如果期望力矩大于限制力矩，则进行下一步S5；S4. If the expected torque is less than or equal to the limiting torque, return to the previous step S3; if the expected torque is greater than the limiting torque, proceed to the next step S5;

S5、计算期望关节力矩和限制关节力矩的函数，求得相应的时间动态标度，作为关节插补周期，以增加关节运动时间的方式，减小关节力矩。S5. Calculate the function of the expected joint torque and the limited joint torque, obtain the corresponding time dynamic scale, use it as the joint interpolation cycle, and reduce the joint torque by increasing the joint movement time.

进一步地，所述步骤S1包含如下步骤：Further, the step S1 includes the following steps:

S1.1、经过轨迹规划之后，机器人所有关节的运动轨迹为θ(t),t∈[0,t_f]，假定该关节运动轨迹某处存在关节力矩超限的情况，于是设计一条新的关节运动轨迹假定其能保证关节力矩保证安全限制范围之内，且满足式(1)S1.1. After trajectory planning, the trajectory of all joints of the robot is θ(t), t∈[0,t_f ], assuming that there is a situation where the joint torque exceeds the limit somewhere in the trajectory of the joint, a new one is designed Joint motion trajectory It is assumed that it can ensure that the joint torque is within the safe limit range, and satisfy the formula (1)

其中r＝r(t)，是关于时间t的严格单调递增函数，且有r(0)＝0,r(t_f)＝t_f；Where r=r(t), is a strict monotonically increasing function about time t, and r(0)=0, r(t_f )=t_f ;

S1.2、对式(1)分别求一阶和二阶微分，可得S1.2. Calculate the first-order and second-order differentials of formula (1), respectively, and get

对于轨迹θ(t)，机器人的逆动力学模型如式(4)所示For the trajectory θ(t), the inverse dynamics model of the robot is shown in formula (4)

将科氏力和向心力项重写为将重力项和摩擦力项独立出来，可得式(4)Rewrite the Coriolis and centripetal force terms as The gravity term and the friction term are separated, and the formula (4) can be obtained

其中，in,

同理，对于轨迹可得Similarly, for the trajectory Available

其中，in,

S1.3、将式(2)和式(3)代入整理得S1.3. Substitute formula (2) and formula (3) into Tidy up

令t＝r(t)，将代入式(7)，得Let t=r(t), will Substituting into formula (7), we get

综上可得超限关节力矩τ(t)和不超限关节力矩与函数r(t)之间的关系，r(t)被称作动态标度函数，可以视作时间t的一种映射关系，相当于重新调整时间的流速，将恒定不变流逝的时间，映射为可以发生变动的函数关系；In summary, the overrun joint torque τ(t) and the non-overrun joint torque can be obtained The relationship with the function r(t), r(t) is called a dynamic scaling function, which can be regarded as a mapping relationship of time t, which is equivalent to readjusting the flow rate of time, and the elapsed time will be constant, Mapped as a functional relationship that can change;

S1.4、为了进一步简化式(8)，动态标度函数r(t)可以选取如式(9)所示的最简单的线性标度函数S1.4. In order to further simplify formula (8), the dynamic scaling function r(t) can choose the simplest linear scaling function as shown in formula (9)

r(t)＝ct(9)式中，c为常数，则有r(t)=ct(9) In the formula, c is a constant, then there is

于是，式(5-9)可以简化为Then, formula (5-9) can be simplified as

进一步地，所述步骤S3包含如下步骤：Further, the step S3 includes the following steps:

S3.1、t时刻，不超限关节力矩的惯性力、科氏力和向心力项可以求得S3.1, at time t, the inertial force, Coriolis force and centripetal force terms of the joint moment that does not exceed the limit can be obtained

式中，τ_max是关节限制力矩，一般为固定常数；g(θ(t))为机器人逆动力学模型的重力项；为机器人逆动力学模型的摩擦力项；In the formula, τ_max is the joint limit torque, which is generally a fixed constant; g(θ(t)) is the gravity term of the inverse dynamics model of the robot; is the friction term of the inverse dynamics model of the robot;

S3.2、t时刻，轨迹规划的关节力矩的惯性力、科氏力和向心力项可以由机器人逆动力学模型求得S3.2, at time t, the inertial force, Coriolis force and centripetal force terms of the joint moment of trajectory planning can be obtained by the robot inverse dynamics model

式中，为机器人逆动力学模型的惯性力项；为机器人逆动力学模型的哥氏力和向心力项。In the formula, is the inertial force term of the inverse dynamics model of the robot; are the Coriolis force and centripetal force terms of the inverse dynamics model of the robot.

本发明相对于现有技术具有如下的优点及效果：Compared with the prior art, the present invention has the following advantages and effects:

本发明使用机器人逆动力学模型，通过设计一个关于时间的动态标度，可以建立期望关节力矩和限制关节力矩之间的函数关系。当出现关节力矩超出限制的时候，通过计算期望关节力矩和限制关节力矩的函数，求得相应的时间动态标度，作为关节插补周期。可以保证在不改变运动路径的前提下，更改机器人运动速度以使关节力矩保持在限制范围之内的问题。The invention uses the inverse dynamics model of the robot, and can establish the functional relationship between the expected joint torque and the limited joint torque by designing a dynamic scale about time. When the joint torque exceeds the limit, the corresponding time dynamic scale is obtained by calculating the function of the expected joint torque and the limited joint torque, which is used as the joint interpolation period. It is guaranteed to change the robot's motion speed to keep the joint torque within the limit range without changing the motion path.

附图说明Description of drawings

图1是本发明实施例的基于机器人动力学模型的关节力矩限制方法流程示意图。FIG. 1 is a schematic flowchart of a method for limiting joint torque based on a robot dynamics model according to an embodiment of the present invention.

具体实施方式detailed description

下面结合实施例及附图对本发明作进一步的详细描述，但本发明的实施方式不限于此。The present invention will be further described in detail below with reference to the embodiments and accompanying drawings, but the embodiments of the present invention are not limited thereto.

如图1所示，现以某六自由度垂直关节串联机器人为关节力矩限制对象，按照以下步骤进行基于机器人动力学模型的关节力矩限制控制：As shown in Figure 1, a six-degree-of-freedom vertical joint serial robot is used as the joint torque limiting object, and the joint torque limiting control based on the robot dynamic model is performed according to the following steps:

一种基于机器人动力学模型的关节力矩限制方法，包括如下步骤：A joint torque limitation method based on a robot dynamics model, comprising the steps of:

S1、设计一个关于时间的动态标度；S1. Design a dynamic scale about time;

S2、建立期望关节力矩和限制关节力矩之间的函数关系；S2. Establishing a functional relationship between the desired joint torque and the limited joint torque;

S3、在机器人运行过程中，实时计算限制力矩和期望力矩；S3. During the operation of the robot, calculate the limit torque and expected torque in real time;

S4、如果期望力矩小于或者等于限制力矩，则返回上一步S3；如果期望力矩大于限制力矩，则进行下一步S5；S4. If the expected torque is less than or equal to the limiting torque, return to the previous step S3; if the expected torque is greater than the limiting torque, proceed to the next step S5;

S5、计算期望关节力矩和限制关节力矩的函数，求得相应的时间动态标度，作为关节插补周期，以增加关节运动时间的方式，减小关节力矩。S5. Calculate the function of the expected joint torque and the limited joint torque, obtain the corresponding time dynamic scale, use it as the joint interpolation cycle, and reduce the joint torque by increasing the joint movement time.

具体而言，所述步骤S1包含如下步骤：Specifically, the step S1 includes the following steps:

S1.1、经过轨迹规划之后，机器人所有关节的运动轨迹为θ(t),t∈[0,t_f]，假定该关节运动轨迹某处存在关节力矩超限的情况，于是设计一条新的关节运动轨迹假定其能保证关节力矩保证安全限制范围之内，且满足式(1)S1.1. After trajectory planning, the trajectory of all joints of the robot is θ(t), t∈[0,t_f ], assuming that there is a situation where the joint torque exceeds the limit somewhere in the trajectory of the joint, a new one is designed Joint motion track It is assumed that it can ensure that the joint torque is within the safe limit range, and satisfy the formula (1)

其中r＝r(t)，是关于时间t的严格单调递增函数，且有r(0)＝0,r(t_f)＝t_f。Where r=r(t) is a strictly monotonically increasing function about time t, and r(0)=0, r(t_f )=t_f .

S1.2、对式(1)分别求一阶和二阶微分，可得S1.2. Calculate the first-order and second-order differentials of formula (1), respectively, and get

对于轨迹θ(t)，机器人的逆动力学模型如式(4)所示For the trajectory θ(t), the inverse dynamics model of the robot is shown in formula (4)

将科氏力和向心力项重写为将重力项和摩擦力项独立出来，可得式(4)Rewrite the Coriolis and centripetal force terms as The gravity term and the friction term are separated, and the formula (4) can be obtained

其中，in,

同理，对于轨迹可得Similarly, for the trajectory Available

其中，in,

S1.3、将式(2)和式(3)代入整理得S1.3. Substitute formula (2) and formula (3) into Tidy up

令t＝r(t)，将代入式(7)，得Let t=r(t), will Substituting into formula (7), we get

综上可得超限关节力矩τ(t)和不超限关节力矩与函数r(t)之间的关系，r(t)被称作动态标度函数，可以视作时间t的一种映射关系，相当于重新调整时间的流速，将恒定不变流逝的时间，映射为可以发生变动的函数关系。In summary, the overrun joint torque τ(t) and the non-overrun joint torque can be obtained The relationship with the function r(t), r(t) is called a dynamic scaling function, which can be regarded as a mapping relationship of time t, which is equivalent to readjusting the flow rate of time, and the elapsed time will be constant, Mapped as a functional relationship that can change.

S1.4、为了进一步简化式(8)，动态标度函数r(t)可以选取如式(9)所示的最简单的线性标度函数S1.4. In order to further simplify formula (8), the dynamic scaling function r(t) can choose the simplest linear scaling function as shown in formula (9)

r(t)＝ct(9)式中，c为常数，则有r(t)=ct(9) In the formula, c is a constant, then there is

于是，式(5-9)可以简化为Then, formula (5-9) can be simplified as

具体而言，所述步骤S3包含如下步骤：Specifically, the step S3 includes the following steps:

S3.1、t时刻，不超限关节力矩的惯性力、科氏力和向心力项可以求得S3.1, at time t, the inertial force, Coriolis force and centripetal force terms of the joint moment that does not exceed the limit can be obtained

式中，τ_max是关节限制力矩，一般为固定常数；g(θ(t))为机器人逆动力学模型的重力项；为机器人逆动力学模型的摩擦力项。In the formula, τ_max is the joint limit torque, which is generally a fixed constant; g(θ(t)) is the gravity term of the inverse dynamics model of the robot; is the friction term of the inverse dynamics model of the robot.

S3.2、t时刻，轨迹规划的关节力矩的惯性力、科氏力和向心力项可以由机器人逆动力学模型求得S3.2, at time t, the inertial force, Coriolis force and centripetal force terms of the joint moment of trajectory planning can be obtained by the robot inverse dynamics model

式中，为机器人逆动力学模型的惯性力项；为机器人逆动力学模型的哥氏力和向心力项。In the formula, is the inertial force term of the inverse dynamics model of the robot; are the Coriolis force and centripetal force terms of the inverse dynamics model of the robot.

上述实施例为本方面较佳的实施方式，但本方明的实施方式并不受上述实施例的限制，其他的任何背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。The above-mentioned embodiment is a preferred implementation mode of this aspect, but the implementation mode of this invention is not limited by the above-mentioned embodiment, and any other changes, modifications, substitutions, combinations, Simplifications should be equivalent replacement methods, and all are included in the protection 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.