CN106313044A - 一种工业机器人前馈力矩补偿方法 - Google Patents

Abstract

本发明涉及一种工业机器人前馈力矩补偿方法，包括步骤：(1)根据各电机进行插补运算后的插补点队列，建立一个缓存队列，采用中心差分的方法求每个插补点各电机对应的理论关节角速度和关节角加速度；(2)使用拉格朗日方程，根据各插补周期各关节期望转角、角速度、角加速度和机器人的动力学模型求各插补周期各关节前馈补偿力矩；(3)将前馈补偿力矩τ_feedforwward加入到电机底层电流环输入处作为PD的反馈力矩τ_feedback的补充。本发明所提供的前馈力矩补偿方法，具有计算简单方便，且进行力矩补偿后，可以减少机器人的轨迹跟踪误差，提高机器人的定位精度。

Description

一种工业机器人前馈力矩补偿方法

技术领域

本发明涉及六轴工业机器人应用领域，尤其涉及一种用于工业机器人的前馈力矩补偿方法。

背景技术

在工业机器人的控制领域，一般采用电机底层自带的位置环、速度环和电流环调节机器人各关节的轨迹跟踪误差。最常用的方法是用位置环的PD反馈，将反馈力矩输入到电流环，对机器人各电机进行力控制。

在高速高负载的场合，单独的PD反馈已经很难满足机器人高速高精度的控制要求，常见的问题主要有：机器人响应速度慢、轨迹跟踪误差大、定位精度不够。针对以上问题，根据机器人的理论轨迹，求出各插补点各关节的关节转角、关节角速度、关节角加速度等机器人运动学参数，带入到机器人的动力学模型中，用拉格朗日法求出各插补点各关节对应的关节前馈补偿力矩。将该前馈补偿力矩加入到电机电流环输入中，与PD反馈力矩共同作用，可以提高工业机器人的控制响应速度，减少机器人轨迹跟踪误差。

发明内容

本发明的目的在于克服现有技术的不足，提供一种计算简单方便，提高工业机器人控制响应速度，减少机器人轨迹跟踪误差的方法。

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

一种工业机器人前馈力矩补偿方法，包括步骤：

(1)根据各电机插补运算后的插补点队列，建立一个缓存队列，采用中心差分的方法求每个插补点各电机对应的理论关节角速度和关节角加速度；

(2)使用拉格朗日方程，根据各插补周期各关节期望转角、角速度、角加速度和机器人的动力学模型求各插补周期各关节的前馈补偿力矩τ_feedforwward，同时通过各关节各插补周期实际转角和理论转角反馈计算得到反馈力矩τ_feedback；

(3)在每个插补周期内，将前馈补偿力矩τ_feedforwward加入到电机底层电流环输入处作为PD的反馈力矩τ_feedback的补充。

进一步地，所述采用中心差分的方法求每个插补点各电机对应的理论关节角速度和关节角加速度的步骤具体包括：

(11)设关节i的插补点队列Q1为根据插补计算得到的理论值，插补点的总数为N个；

(12)从插补点队列中，依次取三个插补点，分别为q_i1、q_i2和q_i3，加入到插补缓存队列Q2；

(13)根据中心差分法，求插补缓存队列Q2第二个点的关节角速度和关节角加速度；

(14)每计算完一次关节角速度和关节角加速度，把缓存队列Q2的第一个点的数据清除，再从插补点队列Q1中取后一个点，放入缓存队列Q2，再求新的缓存队列Q2第二个点的关节角速度和关节角加速度；

(15)重复步骤(11)～(14)，依次求出第2个插补点到第N-1个插补点的关节角速度和关节角加速度，第一个插补点和最后一个插补点不能计算角速度和角加速度，令两者都为零，相当于这两个点只进行重力补偿。

进一步地，设插补周期为T，所述根据中心差分法，求插补缓存队列Q2第二个点的关节角速度和关节角加速度的计算方法包括：

第二个点的关节角速度为：

第二个点的关节角加速度为：

即可根据中心差分法求每个缓存队列第二个点的关节角速度和关节角加速度。

进一步地，各插补周期各关节的前馈补偿力矩τ_feedforwward的具体计算过程为：

${\mathrm{\τ}}_{feedforward}=M\left(q^{*}\right){\stackrel{\·\·}{q}}^{*}+C(q^{*},{\stackrel{\·}{q}}^{*}){\stackrel{\·}{q}}^{*}+F\left({\stackrel{\·}{q}}^{*}\right)+G\left(q^{*}\right),$

式中，τ_feedforwward为前馈补偿力矩，代表惯性力项，代表哥氏力项，代表摩擦力项，G(q^*)代表重力项；q^*为机器人期望轨迹各关节转角，即直接从插补点队列Q1里取得，为机器人期望轨迹各关节角速度，为机器人期望轨迹各关节角加速度。

进一步地，所述反馈力矩τ_feedback的计算方法是：

${\mathrm{\τ}}_{feedback}=K_v(q^{*}-q)+K_p({\stackrel{\·}{q}}^{*}-\stackrel{\·}{q}),$

q为机器人实际运行时的各关节实际转角，为机器人实际运行时的各关节实际角速度，K_ν为电机位置环比例PID系数，K_p为电机位置环微分PID系数。

同时，电机内部也有电流环、位置环和速度环共同作用，位置环有PD反馈，通过各关节各插补周期实际转角和理论转角反馈得到反馈力矩，用于减少机器人各关节的轨迹跟踪误差。

进一步地，所述在每个插补周期内，将前馈补偿力矩τ_feedforwward加入到电机底层电流环输入处作为PD的反馈力矩τ_feedback的补充时，电机电流环的总输入为：

τ＝τ_feedforward+τ_feedback。

相比现有技术，本发明的前馈力矩可以作为PD反馈力矩的极大补充，在相同的电机三环PID参数的基础上，大大减少系统的跟随误差，提高系统响应速度，从而提高系统的控制精度，具有计算简单方便，且进行力矩补偿后，可以减少机器人的轨迹跟踪误差，提高机器人的定位精度。

附图说明

图1是根据关节插补点队列求关节缓存队列示意图。

图2是前馈补偿力矩和PD反馈力矩示意图。

图3是机器人三环控制示意图。

具体实施方式

下面通过具体实施例对本发明的目的作进一步详细地描述，实施例不能在此一一赘述，但本发明的实施方式并不因此限定于以下实施例。

一种工业机器人前馈力矩补偿方法，包括步骤：

(1)根据各电机插补运算后的插补点队列，建立一个缓存队列，采用中心差分的方法求每个插补点各电机对应的理论关节角速度和关节角加速度；

(2)使用拉格朗日方程，根据各插补周期各关节期望转角、角速度、角加速度和机器人的动力学模型求各插补周期各关节的前馈补偿力矩τ_feedforwward，同时通过各关节各插补周期实际转角和理论转角反馈计算得到反馈力矩τ_feedback；

(3)在每个插补周期内，将前馈补偿力矩τ_feedforwward加入到电机底层电流环输入处作为PD的反馈力矩τ_feedback的补充。

如图1所示，所述采用中心差分的方法求每个插补点各电机对应的理论关节角速度和关节角加速度的步骤具体包括：

(11)设关节i的插补点队列Q1为根据插补计算得到的理论值，插补点的总数为N个；

(12)从插补点队列中，依次取三个插补点，分别为q_i1、q_i2和q_i3，加入到插补缓存队列Q2；

(13)根据中心差分法，求插补缓存队列Q2第二个点的关节角速度和关节角加速度；

(14)每计算完一次关节角速度和关节角加速度，把缓存队列Q2的第一个点的数据清除，再从插补点队列Q1中取后一个点，放入缓存队列Q2，再求新的缓存队列Q2第二个点的关节角速度和关节角加速度；

(15)重复步骤(11)～(14)，依次求出第2个插补点到第N-1个插补点的关节角速度和关节角加速度，第一个插补点和最后一个插补点不能计算角速度和角加速度，令两者都为零，相当于这两个点只进行重力补偿。

具体而言，设插补周期为T，所述根据中心差分法，求插补缓存队列Q2第二个点的关节角速度和关节角加速度的计算方法包括：

第二个点的关节角速度为：

第二个点的关节角加速度为：

即可根据中心差分法求每个缓存队列第二个点的关节角速度和关节角加速度。

具体而言，各插补周期各关节的前馈补偿力矩τ_feedforwward的具体计算过程为：

${\mathrm{\τ}}_{feedforward}=M\left(q^{*}\right){\stackrel{\·\·}{q}}^{*}+C(q^{*},{\stackrel{\·}{q}}^{*}){\stackrel{\·}{q}}^{*}+F\left({\stackrel{\·}{q}}^{*}\right)+G\left(q^{*}\right),$

式中，τ_feedforwward为前馈补偿力矩，代表惯性力项，代表哥氏力项，代表摩擦力项，G(q^*)代表重力项；q^*为机器人期望轨迹各关节转角，即直接从插补点队列Q1里取得，为机器人期望轨迹各关节角速度，为机器人期望轨迹各关节角加速度。

如图2所示，所述反馈力矩τ_feedback的计算方法是：

${\mathrm{\τ}}_{feedback}=K_v(q^{*}-q)+K_p({\stackrel{\·}{q}}^{*}-\stackrel{\·}{q}),$

q为机器人实际运行时的各关节实际转角，为机器人实际运行时的各关节实际角速度，K_ν为电机位置环比例PID系数，K_p为电机位置环微分PID系数。

同时，电机内部也有电流环、位置环和速度环共同作用。位置环有PD反馈，通过各关节各插补周期实际转角和理论转角反馈得到反馈力矩，用于减少机器人各关节的轨迹跟踪误差。

如图3所示，所述在每个插补周期内，将前馈补偿力矩τ_feedforwward加入到电机底层电流环输入处作为PD的反馈力矩τ_feedback的补充时，电机电流环的总输入为：

τ＝τ_feedforward+τ_feedback。

这样，前馈补偿力矩τ_feedforwward可以作为PD反馈力矩的极大补充，在相同的电机三环PID参数的基础上，大大减少系统的跟随误差，提高系统响应速度，从而提高系统的控制精度。

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

Claims (6)

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

(1)根据各电机插补运算后的插补点队列，建立一个缓存队列，采用中心差分的方法求每个插补点各电机对应的理论关节角速度和关节角加速度；

(2)使用拉格朗日方程，根据各插补周期各关节期望转角、角速度、角加速度和机器人的动力学模型求各插补周期各关节的前馈补偿力矩τ_feedforwward，同时通过各关节各插补周期实际转角和理论转角反馈计算得到反馈力矩τ_feedback；

(3)在每个插补周期内，将前馈补偿力矩τ_feedforwward加入到电机底层电流环输入处作为PD的反馈力矩τ_feedback的补充。

2.根据权利要求1所述的工业机器人前馈力矩补偿方法，其特征在于，所述采用中心差分的方法求每个插补点各电机对应的理论关节角速度和关节角加速度的步骤具体包括：

(11)设关节i的插补点队列Q1为根据插补计算得到的理论值，插补点的总数为N个；

(12)从插补点队列中，依次取三个插补点，分别为q_i1、q_i2和q_i3，加入到插补缓存队列Q2；

(13)根据中心差分法，求插补缓存队列Q2第二个点的关节角速度和关节角加速度；

(14)每计算完一次关节角速度和关节角加速度，把缓存队列Q2的第一个点的数据清除，再从插补点队列Q1中取后一个点，放入缓存队列Q2，再求新的缓存队列Q2第二个点的关节角速度和关节角加速度；

(15)重复步骤(11)～(14)，依次求出第2个插补点到第N-1个插补点的关节角速度和关节角加速度，第一个插补点和最后一个插补点不能计算角速度和角加速度，令两者都为零，相当于这两个点只进行重力补偿。

3.根据权利要求2所述的工业机器人前馈力矩补偿方法，其特征在于，设插补周期为T，所述根据中心差分法，求插补缓存队列Q2第二个点的关节角速度和关节角加速度的计算方法包括：

第二个点的关节角速度为：

第二个点的关节角加速度为：

即可根据中心差分法求每个缓存队列第二个点的关节角速度和关节角加速度。

4.根据权利要求3所述的工业机器人前馈力矩补偿方法，其特征在于，各插补周期各关节的前馈补偿力矩τ_feedforwward的具体计算过程为：

${\mathrm{\τ}}_{feedforward}=M\left(q^{*}\right){\stackrel{\·\·}{q}}^{*}+C(q^{*},{\stackrel{\·}{q}}^{*}){\stackrel{\·}{q}}^{*}+F\left({\stackrel{\·}{q}}^{*}\right)+G\left(q^{*}\right),$

式中，τ_feedforwward为前馈补偿力矩，代表惯性力项，代表哥氏力项，代表摩擦力项，G(q^*)代表重力项；q^*为机器人期望轨迹各关节转角，即直接从插补点队列Q1里取得，为机器人期望轨迹各关节角速度，为机器人期望轨迹各关节角加速度。

5.根据权利要求4所述的工业机器人前馈力矩补偿方法，其特征在于，所述反馈力矩τ_feedback的计算方法是：

${\mathrm{\τ}}_{feedback}=K_v(q^{*}-q)+K_p({\stackrel{\·}{q}}^{*}-\stackrel{\·}{q}),$

q为机器人实际运行时的各关节实际转角，为机器人实际运行时的各关节实际角速度，K_ν为电机位置环比例PID系数，K_p为电机位置环微分PID系数。

6.根据权利要求5所述的工业机器人前馈力矩补偿方法，其特征在于，所述在每个插补周期内，将前馈补偿力矩τ_feedforwward加入到电机底层电流环输入处作为PD的反馈力矩τ_feedback的补充时，电机电流环的总输入为：τ＝τ_feedforward+τ_feedback。