CN106903692A - 一种基于机器人动力学模型的关节力矩限制方法 - Google Patents

Abstract

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

Description

一种基于机器人动力学模型的关节力矩限制方法

技术领域

本发明涉及串联机器人关节力矩限制方法，特别涉及一种基于机器人动力学模型的关节力矩限制方法。

背景技术

串联机器人在制造业、农业、娱乐业等行业得到越来越广泛的应用。当机器人处于某些极度恶劣的工作状况下，有可能出现关节伺服电机所需的输出力矩，超出伺服电机所能提供的最大转矩，导致伺服电机驱动器报警，停止工作，甚至造成伺服电机损坏。因此，需要进行关节力矩限制，以防止意外事故的发生。

判断整个机器人运动过程中是否存在关节力矩超限并不困难，只需要在轨迹规划之后，将规划的关节位置、角速度和角加速度代入式(5-1)所示的机器人逆动力学模型计算公式中，算得整个运动过程各个关节输出力矩，从中挑选出最大值与伺服电机手册最大力矩乘上减速比后的积进行比较，即可知道关节力矩是否存在超限的情况。困难的是判断关节力矩存在超限之后，如何在不改变运动路径的前提下，更改机器人运动速度以使关节力矩保持在限制范围之内。

发明内容

本发明的目的是提供一种基于机器人动力学模型的关节力矩限制方法，旨在解决判断关节力矩存在超限之后，如何在不改变运动路径的前提下，更改机器人运动速度以使关节力矩保持在限制范围之内的问题。

本发明的目的通过下述技术方案来实现：

一种基于机器人动力学模型的关节力矩限制方法，包括如下步骤：

S1、设计一个关于时间的动态标度；

S2、建立期望关节力矩和限制关节力矩之间的函数关系；

S3、在机器人运行过程中，实时计算限制力矩和期望力矩；

S4、如果期望力矩小于或者等于限制力矩，则返回上一步S3；如果期望力矩大于限制力矩，则进行下一步S5；

S5、计算期望关节力矩和限制关节力矩的函数，求得相应的时间动态标度，作为关节插补周期，以增加关节运动时间的方式，减小关节力矩。

进一步地，所述步骤S1包含如下步骤：

S1.1、经过轨迹规划之后，机器人所有关节的运动轨迹为θ(t),t∈[0,t_f]，假定该关节运动轨迹某处存在关节力矩超限的情况，于是设计一条新的关节运动轨迹假定其能保证关节力矩保证安全限制范围之内，且满足式(1)

其中r＝r(t)，是关于时间t的严格单调递增函数，且有r(0)＝0,r(t_f)＝t_f；

S1.2、对式(1)分别求一阶和二阶微分，可得

对于轨迹θ(t)，机器人的逆动力学模型如式(4)所示

将科氏力和向心力项重写为将重力项和摩擦力项独立出来，可得式(4)

其中，

同理，对于轨迹可得

其中，

S1.3、将式(2)和式(3)代入整理得

令t＝r(t)，将代入式(7)，得

综上可得超限关节力矩τ(t)和不超限关节力矩与函数r(t)之间的关系，r(t)被称作动态标度函数，可以视作时间t的一种映射关系，相当于重新调整时间的流速，将恒定不变流逝的时间，映射为可以发生变动的函数关系；

S1.4、为了进一步简化式(8)，动态标度函数r(t)可以选取如式(9)所示的最简单的线性标度函数

r(t)＝ct(9)式中，c为常数，则有

于是，式(5-9)可以简化为

进一步地，所述步骤S3包含如下步骤：

S3.1、t时刻，不超限关节力矩的惯性力、科氏力和向心力项可以求得

式中，τ_max是关节限制力矩，一般为固定常数；g(θ(t))为机器人逆动力学模型的重力项；为机器人逆动力学模型的摩擦力项；

S3.2、t时刻，轨迹规划的关节力矩的惯性力、科氏力和向心力项可以由机器人逆动力学模型求得

式中，为机器人逆动力学模型的惯性力项；为机器人逆动力学模型的哥氏力和向心力项。

本发明相对于现有技术具有如下的优点及效果：

本发明使用机器人逆动力学模型，通过设计一个关于时间的动态标度，可以建立期望关节力矩和限制关节力矩之间的函数关系。当出现关节力矩超出限制的时候，通过计算期望关节力矩和限制关节力矩的函数，求得相应的时间动态标度，作为关节插补周期。可以保证在不改变运动路径的前提下，更改机器人运动速度以使关节力矩保持在限制范围之内的问题。

附图说明

图1是本发明实施例的基于机器人动力学模型的关节力矩限制方法流程示意图。

具体实施方式

下面结合实施例及附图对本发明作进一步的详细描述，但本发明的实施方式不限于此。

如图1所示，现以某六自由度垂直关节串联机器人为关节力矩限制对象，按照以下步骤进行基于机器人动力学模型的关节力矩限制控制：

一种基于机器人动力学模型的关节力矩限制方法，包括如下步骤：

S1、设计一个关于时间的动态标度；

S2、建立期望关节力矩和限制关节力矩之间的函数关系；

S3、在机器人运行过程中，实时计算限制力矩和期望力矩；

S4、如果期望力矩小于或者等于限制力矩，则返回上一步S3；如果期望力矩大于限制力矩，则进行下一步S5；

S5、计算期望关节力矩和限制关节力矩的函数，求得相应的时间动态标度，作为关节插补周期，以增加关节运动时间的方式，减小关节力矩。

具体而言，所述步骤S1包含如下步骤：

S1.1、经过轨迹规划之后，机器人所有关节的运动轨迹为θ(t),t∈[0,t_f]，假定该关节运动轨迹某处存在关节力矩超限的情况，于是设计一条新的关节运动轨迹假定其能保证关节力矩保证安全限制范围之内，且满足式(1)

其中r＝r(t)，是关于时间t的严格单调递增函数，且有r(0)＝0,r(t_f)＝t_f。

S1.2、对式(1)分别求一阶和二阶微分，可得

对于轨迹θ(t)，机器人的逆动力学模型如式(4)所示

将科氏力和向心力项重写为将重力项和摩擦力项独立出来，可得式(4)

其中，

同理，对于轨迹可得

其中，

S1.3、将式(2)和式(3)代入整理得

令t＝r(t)，将代入式(7)，得

综上可得超限关节力矩τ(t)和不超限关节力矩与函数r(t)之间的关系，r(t)被称作动态标度函数，可以视作时间t的一种映射关系，相当于重新调整时间的流速，将恒定不变流逝的时间，映射为可以发生变动的函数关系。

S1.4、为了进一步简化式(8)，动态标度函数r(t)可以选取如式(9)所示的最简单的线性标度函数

r(t)＝ct(9)式中，c为常数，则有

于是，式(5-9)可以简化为

具体而言，所述步骤S3包含如下步骤：

S3.1、t时刻，不超限关节力矩的惯性力、科氏力和向心力项可以求得

式中，τ_max是关节限制力矩，一般为固定常数；g(θ(t))为机器人逆动力学模型的重力项；为机器人逆动力学模型的摩擦力项。

S3.2、t时刻，轨迹规划的关节力矩的惯性力、科氏力和向心力项可以由机器人逆动力学模型求得

式中，为机器人逆动力学模型的惯性力项；为机器人逆动力学模型的哥氏力和向心力项。

上述实施例为本方面较佳的实施方式，但本方明的实施方式并不受上述实施例的限制，其他的任何背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (3)

1.一种基于机器人动力学模型的关节力矩限制方法，其特征在于，包括如下步骤：

S1、设计一个关于时间的动态标度；

S2、建立期望关节力矩和限制关节力矩之间的函数关系；

S3、在机器人运行过程中，实时计算限制力矩和期望力矩；

S4、如果期望力矩小于或者等于限制力矩，则返回上一步S3；如果期望力矩大于限制力矩，则进行下一步S5；

S5、计算期望关节力矩和限制关节力矩的函数，求得相应的时间动态标度，作为关节插补周期，以增加关节运动时间的方式，减小关节力矩。

2.根据权利要求1所述的一种基于机器人动力学模型的关节力矩限制方法，其特征在于，所述步骤S1包含如下步骤：

S1.1、经过轨迹规划之后，机器人所有关节的运动轨迹为θ(t),t∈[0,t_f]，假定该关节运动轨迹某处存在关节力矩超限的情况，于是设计一条新的关节运动轨迹假定其能保证关节力矩保证安全限制范围之内，且满足式(1)

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

其中r＝r(t)，是关于时间t的严格单调递增函数，且有r(0)＝0,r(t_f)＝t_f；

S1.2、对式(1)分别求一阶和二阶微分，可得

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

对于轨迹θ(t)，机器人的逆动力学模型如式(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)$

将科氏力和向心力项重写为将重力项和摩擦力项独立出来，可得式(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)$

其中，

同理，对于轨迹可得

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

其中，

S1.3、将式(2)和式(3)代入整理得

${\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)，将代入式(7)，得

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

综上可得超限关节力矩τ(t)和不超限关节力矩与函数r(t)之间的关系，r(t)被称作动态标度函数，可以视作时间t的一种映射关系，相当于重新调整时间的流速，将恒定不变流逝的时间，映射为可以发生变动的函数关系；

S1.4、为了进一步简化式(8)，动态标度函数r(t)可以选取如式(9)所示的最简单的线性标度函数

r(t)＝ct (9)

式中，c为常数，则有

于是，式(5-9)可以简化为

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

3.根据权利要求1所述的一种基于机器人动力学模型的关节力矩限制方法，其特征在于，所述步骤S3包含如下步骤：

S3.1、t时刻，不超限关节力矩的惯性力、科氏力和向心力项可以求得

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

式中，τ_max是关节限制力矩，一般为固定常数；g(θ(t))为机器人逆动力学模型的重力项；为机器人逆动力学模型的摩擦力项；

S3.2、t时刻，轨迹规划的关节力矩的惯性力、科氏力和向心力项可以由机器人逆动力学模型求得

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

式中，H(θ(t))为机器人逆动力学模型的惯性力项；为机器人逆动力学模型的哥氏力和向心力项。