CN107363832A - 一种工业机器人前馈力矩实时计算方法 - Google Patents

Abstract

本发明公开了一种工业机器人前馈力矩实时计算方法，包括步骤：(1)建立机器人各关节转角q^*的实时插补点队列Q；(2)在实时插补点队列Q中每次取五个点建立动态的插补缓存队列Q_c；(3)然后根据五次中心差分法，在已知五个点的关节转角q^*的前提下，实时计算插补缓存队列Q_c的中间点的关节角速度和关节角加速度(4)计算实时插补点队列Q中其他点的关节角速度和关节角加速度(5)将计算所得的各插补点的关节转角q^*、关节角速度和关节角加速度代入机器人的逆动力学模型，即可在插补时实时计算各关节的前馈驱动力矩。本发明能够实时而快速的计算机器人的前馈补偿力矩，保证机器人各关节伺服电机有足够大的力和力矩来驱动机器人的连杆和关节，保证精度。

Description

一种工业机器人前馈力矩实时计算方法

技术领域

本发明涉及工业机器人的动力学控制应用领域，尤其涉及一种工业机器人前馈力矩实时计算方法。

背景技术

因为机器人的复杂非线性、时变不确定性、强耦合性(特别是在高速运动时)，要使机器人能以期望的速度和加速度运动，机器人各关节伺服电机必须有足够大的力和力矩来驱动机器人的连杆和关节，否则，连杆将因运动迟缓而影响机器人的定位和轨迹跟踪精度，为此必须建立基于动力学模型的前馈力矩控制。而实时快速地计算前馈补偿力矩，则是本发明的主要内容。

发明内容

本发明的目的在于克服现有技术的不足，提供一种计算简单方便，满足工业机器人实时插补要求的前馈力矩计算方法。

上述目的是通过以下技术方案实现的：

一种工业机器人前馈力矩实时计算方法，包括步骤：

(1)建立机器人各关节转角q^*的实时插补点队列Q；

(2)在实时插补点队列Q中每次取五个点建立动态的插补缓存队列Q_c；

(3)然后根据五次中心差分法，在已知五个点的关节转角q^*的前提下，实时计算插补缓存队列Q_c的中间点的关节角速度和关节角加速度

(4)计算实时插补点队列Q中第一个点、第二个点、倒数第二个点、倒数第一个点的关节角速度和关节角加速度

(5)将计算所得的各插补点的关节转角q^*、关节角速度和关节角加速度代入机器人的逆动力学模型，即可在插补时实时计算各关节的前馈驱动力矩，实现前馈力矩控制。

进一步地，所述步骤(1)具体包括：

在工业机器人的实时插补中，实时地每次计算出一个插补周期的各关节转角q^*，放入实时插补点队列Q中。

进一步地，所述步骤(2)具体包括：

(21)从第一个点开始，从实时插补点队列Q中取五个点，放入插补缓存队列Q_c，使用五次中心差分法计算第三个点的关节角速度和关节角加速度；

(22)计算完之后，去掉插补缓存队列Q_c中位于队尾的第一个点，再从队列Q中取一个点加入插补缓存队列Q_c的队头，组成新的插补缓存队列Q_c；

(23)然后计算新的插补缓存队列Q_c第三个点的关节角速度和关节角加速度；

(24)重复上述步骤，直至将队列Q最后一个点加入插补缓存队列Q_c，计算完成。

进一步地，所述步骤(3)具体包括：

(31)设定五个插补点的时间间隔为机器人的理论插补周期T，设第三个点的关节转角时间序列为q(x)，则其他几个点的时间序列分别为q(x-2T)、q(x-T)、q(x+T)、q(x+2T)；

(32)分别对q(x-2T)、q(x-T)、q(x+T)、q(x+2T)进行泰勒四阶展开，并相减可以得到：

式中，c₁和c₂是常数；

(33)对上两式进行数学转化可以得到：

(34)找到一个常数c∈[x-2T,x+2T]，使得下式成立：

16q⁽⁵⁾(c₁)-64q⁽⁵⁾(c₂)＝-48q⁽⁵⁾(c)；

整理可得：

(35)具有o(T⁴)精度的一阶中心差分算法计算q'(x)：

同理，用二阶中心差分算法计算q”(x)：

进一步地，所述步骤(4)具体包括：

(41)对于实时插补点队列Q中的第二个点和倒数第二个点，通过其前后两点，根据三点中心差分原理求解，即：

(42)对于实时插补点队列Q中的第一个点和倒数第一个点，其理论关节角速度理论关节角加速度不能通过中心差分得到，因此令第一个点和倒数第一个点的关节角速度和关节角加速度为零，即相当于这两个插补点只补偿前馈力矩的重力项。

因为插补周期很短,同时机器人启动和停止阶段第一个周期内速度和加速度较小，所以不会有太大的计算误差。

进一步地，所述步骤(5)具体包括：

将计算所得的各插补点的关节转角q^*、关节角速度和关节角加速度代入机器人的逆动力学模型，即可在插补时根据公式：

实时计算各关节的前馈驱动力矩，实现前馈力矩控制，其中：H_b为机器人逆动力学模型中的回归矩阵，β为机器人的基础动力学参数。

相比现有技术，本发明能够实时而快速的计算机器人的前馈补偿力矩，保证机器人各关节伺服电机有足够大的力和力矩来驱动机器人的连杆和关节，防止连杆因运动迟缓而影响机器人的定位和轨迹跟踪精度。

附图说明

图1是本发明实施例的前馈力矩控制示意图。

图2是本发明实施例的建立插补动态缓存队列的示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步的说明。

一种工业机器人前馈力矩实时计算方法，包括步骤：

(1)建立机器人各关节转角q^*的实时插补点队列Q；

(2)在实时插补点队列Q中每次取五个点建立动态的插补缓存队列Q_c；

(3)然后根据五次中心差分法，在已知五个点的关节转角q^*的前提下，实时计算插补缓存队列Q_c的中间点的关节角速度和关节角加速度

(4)计算实时插补点队列Q中第一个点、第二个点、倒数第二个点、倒数第一个点的关节角速度和关节角加速度

(5)将计算所得的各插补点的关节转角q^*、关节角速度和关节角加速度代入机器人的逆动力学模型，即可在插补时实时计算各关节的前馈驱动力矩，实现前馈力矩控制(见图1)。

具体而言，所述步骤(1)具体包括：

在工业机器人的实时插补中，实时地每次计算出一个插补周期的各关节转角q^*，放入实时插补点队列Q中，从而根据q^*实时计算和

工业机器人动力学控制中常用的前馈力矩补偿控制中，前馈力矩的计算表达式为：要想实现实时前馈力矩控制，必须已知各关节的关节转角q^*、角速度和角加速度

具体而言，如图2所示，所述步骤(2)具体包括：

(21)从第一个点开始，从实时插补点队列Q中取五个点，放入插补缓存队列Q_c，使用五次中心差分法计算第三个点的关节角速度和关节角加速度；

(22)计算完之后，去掉插补缓存队列Q_c中位于队尾的第一个点，再从队列Q中取一个点加入插补缓存队列Q_c的队头，组成新的插补缓存队列Q_c；

(23)然后计算新的插补缓存队列Q_c第三个点的关节角速度和关节角加速度；

(24)重复上述步骤，直至将队列Q最后一个点加入插补缓存队列Q_c，计算完成。

具体而言，所述步骤(3)具体包括：

(31)设定五个插补点的时间间隔为机器人的理论插补周期T，设第三个点的关节转角时间序列为q(x)，则其他几个点的时间序列分别为q(x-2T)、q(x-T)、q(x+T)、q(x+2T)；

(32)分别对q(x-2T)、q(x-T)、q(x+T)、q(x+2T)进行泰勒四阶展开，并相减可以得到：

式中，c₁和c₂是常数；

(33)对上两式进行数学转化可以得到：

(34)找到一个常数c∈[x-2T,x+2T]，使得下式成立：

16q⁽⁵⁾(c₁)-64q⁽⁵⁾(c₂)＝-48q⁽⁵⁾(c)；

整理可得：

(35)具有o(T⁴)精度的一阶中心差分算法计算q'(x)：

同理，用二阶中心差分算法计算q”(x)：

进一步地，所述步骤(4)具体包括：

(41)对于实时插补点队列Q中的第二个点和倒数第二个点，通过其前后两点，根据三点中心差分原理求解，即：

(42)对于实时插补点队列Q中的第一个点和倒数第一个点，其理论关节角速度理论关节角加速度不能通过中心差分得到，因此令第一个点和倒数第一个点的关节角速度和关节角加速度为零，即相当于这两个插补点只补偿前馈力矩的重力项。因为插补周期很短,同时机器人启动和停止阶段第一个周期内速度和加速度较小，所以不会有太大的计算误差。

具体而言，所述步骤(5)具体包括：

将实时计算所得的各插补点的关节转角q^*、关节角速度和关节角加速度代入机器人的逆动力学模型，即可在插补时根据公式：

实时计算各关节的前馈驱动力矩，实现前馈力矩控制，其中：H_b为机器人逆动力学模型中的回归矩阵，β为机器人的基础动力学参数。

本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (6)

1.一种工业机器人前馈力矩实时计算方法，其特征在于，包括步骤：

(1)建立机器人各关节转角q^*的实时插补点队列Q；

(2)在实时插补点队列Q中每次取五个点建立动态的插补缓存队列Q_c；

(3)然后根据五次中心差分法，在已知五个点的关节转角q^*的前提下，实时计算插补缓存队列Q_c的中间点的关节角速度和关节角加速度

(4)计算实时插补点队列Q中第一个点、第二个点、倒数第二个点、倒数第一个点的关节角速度和关节角加速度

(5)将计算所得的各插补点的关节转角q^*、关节角速度和关节角加速度代入机器人的逆动力学模型，即可在插补时实时计算各关节的前馈驱动力矩，实现前馈力矩控制。

2.根据权利要求1所述的一种工业机器人前馈力矩实时计算方法，其特征在于，所述步骤(1)具体包括：

在工业机器人的实时插补中，实时地每次计算出一个插补周期的各关节转角q^*，放入实时插补点队列Q中。

3.根据权利要求1所述的一种工业机器人前馈力矩实时计算方法，其特征在于，所述步骤(2)具体包括：

(21)从第一个点开始，从实时插补点队列Q中取五个点，放入插补缓存队列Q_c，使用五次中心差分法计算第三个点的关节角速度和关节角加速度；

(22)计算完之后，去掉插补缓存队列Q_c中位于队尾的第一个点，再从队列Q中取一个点加入插补缓存队列Q_c的队头，组成新的插补缓存队列Q_c；

(23)然后计算新的插补缓存队列Q_c第三个点的关节角速度和关节角加速度；

(24)重复上述步骤，直至将队列Q最后一个点加入插补缓存队列Q_c，计算完成。

4.根据权利要求1所述的一种工业机器人前馈力矩实时计算方法，其特征在于，所述步骤(3)具体包括：

(31)设定五个插补点的时间间隔为机器人的理论插补周期T，设第三个点的关节转角时间序列为q(x)，则其他几个点的时间序列分别为q(x-2T)、q(x-T)、q(x+T)、q(x+2T)；

(32)分别对q(x-2T)、q(x-T)、q(x+T)、q(x+2T)进行泰勒四阶展开，并相减可以得到：

式中，c₁和c₂是常数；

(33)对上两式进行数学转化可以得到：

(34)找到一个常数c∈[x-2T,x+2T]，使得下式成立：

16q⁽⁵⁾(c₁)-64q⁽⁵⁾(c₂)＝-48q⁽⁵⁾(c)；

整理可得：

(35)具有o(T⁴)精度的一阶中心差分算法计算q'(x)：

同理，用二阶中心差分算法计算q”(x)：

5.根据权利要求1所述的一种工业机器人前馈力矩实时计算方法，其特征在于，所述步骤(4)具体包括：

(41)对于实时插补点队列Q中的第二个点和倒数第二个点，通过其前后两点，根据三点中心差分原理求解，即：

(42)对于实时插补点队列Q中的第一个点和倒数第一个点，其理论关节角速度理论关节角加速度不能通过中心差分得到，因此令第一个点和倒数第一个点的关节角速度和关节角加速度为零，即相当于这两个插补点只补偿前馈力矩的重力项。

6.根据权利要求1所述的一种工业机器人前馈力矩实时计算方法，其特征在于，所述步骤(5)具体包括：

将计算所得的各插补点的关节转角q^*、关节角速度和关节角加速度代入机器人的逆动力学模型，即可在插补时根据公式：

实时计算各关节的前馈驱动力矩，实现前馈力矩控制，其中：H_b为机器人逆动力学模型中的回归矩阵，β为机器人的基础动力学参数。