CN107053257A - 用于四自由度圆柱坐标型码垛机器人的末端残余振动抑制的方法 - Google Patents

Abstract

本发明提出一种用于四自由度圆柱坐标型码垛机器人的末端残余振动抑制的方法，采集安装在四自由度圆柱坐标型码垛机器人大臂和末端的两个加速度传感器信号a_B(t)和a_T(t)，将a_B(t)和a_T(t)进行离散傅立叶变换得到a_B(jω)和a_T(jω)，将a_T(jω)进行截止频率计算并得到截止频率ω_c，通过a_B(jω)、a_T(jω)和ω_c计算求得速度因子s和位置因子p，然后，通过s、p和ω_c计算求得补偿力矩的参数，并得到补偿力矩F，将F作为码垛机器人腰部关节伺服电机的前馈补偿力矩对末端残余振动进行抑制。本发明的有益效果：针对四自由度圆柱坐标型码垛机器人高速定位时末端容易产生残余振动的问题，基于加速度传感器信号建立机器人腰部伺服电机补偿力矩进行振动抑制，该补偿力矩的参数通过在线整定方式进行快速优化。

Description

用于四自由度圆柱坐标型码垛机器人的末端残余振动抑制的 方法

技术领域

本发明涉及机器人控制技术领域，特别是涉及一种用于四自由度圆柱坐标型码垛机器人的末端残余振动抑制的方法。

背景技术

四自由度圆柱坐标型码垛机器人是目前码垛机器人普遍采用的一种结构形式，该结构包括一个腰部旋转运动关节，一个末端旋转运动关节，一个水平直线运动关节和一个垂直直线运动关节，每个关节为一个自由度，机器人的大臂采用平行四边形的连杆机构实现机器人末端的水平和垂直直线运动。该结构的四个关节运动各自独立，不存在耦合关系，相比于其他结构形式，控制更简单，运动更高效和高速。但是，由于机器人的运动关节中采用了减速机、齿形带等器件，同时加工装配过程中存在误差，导致机器人的机械本体存在不同程度的柔性特性，会对机器人的运动性能产生一定的影响，速度越快影响越大，特别是机器人在高速定位时，其末端极易产生残余振动，残余振动会极大降低机器人的重复定位精度，延长机器人的定位时间并降低工作效率。由于运动速度是码垛机器人最关键的技术指标之一，因此，在机械本体已经无法改动的情况下，如果通过控制手段抑制机器人的末端残余振动是提高机器人性能的重要途径。

发明内容

本发明的目的在于提供一种用于四自由度圆柱坐标型码垛机器人的末端残余振动抑制的方法，通过施加在腰部关节伺服电机上的补偿力矩进行残余振动的抑制，并通过参数在线自整定的方式确定补偿力矩中的比例系数和时间常数。

本发明提供一种用于四自由度圆柱坐标型码垛机器人的末端残余振动抑制的方法，在四自由度圆柱坐标型码垛机器人的大臂和末端分别安装加速度传感器，包括如下步骤：

步骤一、四自由度圆柱坐标型码垛机器人的末端到达某一定位点时，开始对四自由度圆柱坐标型码垛机器人的腰部旋转关节的伺服电机施加补偿力矩，连续施加第1次至第n次补偿力矩，在施加第1次至第n次补偿力矩的每一次的过程中四自由度圆柱坐标型码垛机器人的末端处于不同的定位点，补偿力矩表达式为F＝ga_T(t-τ)，其中，F为补偿力矩,g为补偿力矩的比例系数，τ为补偿力矩的时间常数，a_T(t)为安装在机器人末端的加速度传感器的测量值；

对四自由度圆柱坐标型码垛机器人的腰部旋转关节的伺服电机施加第n次补偿力矩，实现第n次抑制过程如下，

四自由度圆柱坐标型码垛机器人的控制器以T为采样周期分别采集安装在机器人大臂的加速度传感器的测量值a_B(t)和安装在机器人末端的加速度传感器的测量值a_T(t)，采集点数为N，将a_B(t)和a_T(t)分别进行离散傅立叶变换，得到了第n次抑制过程中两个传感器信号的频域序列a_B ^(n-1)(jω_k)和a_T ^(n-1)(jω_k)，如下式所示，

其中，a_B ^(n-1)(nT)和a_T ^(n-1)(nT)分别是两个传感器信号的时域离散序列，

序列a_T ^(n-1)(jω_k)绝对值的最大值对应的k值记为k_max，则得到第n次抑制的截止频率为

则a_B ^(n-1)(jω_c ^(n-1))和a_T ^(n-1)(jω_c ^(n-1))分别为序列a_T ^(n-1)(jω_k)和a_B ^(n-1)(jω_k)的最大值，即

a_B ^(n-1)(jω_c ^(n-1))＝max|a_B ^(n-1)(jω_k)|

a_T ^(n-1)(jω_c ^(n-1))＝max|a_T ^(n-1)(jω_k)|

求取速度因子s第n次抑制值s^(n-1)，位置因子p的第n次抑制值p^(n-1)，表达式如下所示，

求取第n次抑制过程中补偿力矩比例系数g^(n-1)和补偿力矩时间常数τ^(n-1)，如下式所示，

其中，m为机器人的大臂、小臂和末端的总质量,

由此可得对四自由度圆柱坐标型码垛机器人的腰部旋转关节的伺服电机施加第n次补偿力矩F^(n-1)的表达式如下所示，

F^(n-1)＝g^(n-1)a_T ^(n-1)(t-τ^(n-1))；

步骤二、在对四自由度圆柱坐标型码垛机器人的腰部旋转关节的伺服电机施加第1次至第n次补偿力矩的过程中，四自由度圆柱坐标型码垛机器人的控制器实时检测末端的加速度传感器的测量值a_T(t)；

步骤三、四自由度圆柱坐标型码垛机器人的控制器判断a_T(t)在确定时间内是否始终稳定在[a_Tmin,a_Tmax]的范围内，若在[a_Tmin,a_Tmax]的范围内则停止对四自由度圆柱坐标型码垛机器人的腰部旋转关节的伺服电机施加补偿力矩，若不在[a_Tmin,a_Tmax]的范围内则循环步骤一至步骤三的过程一直到a_T(t)在确定时间内始终稳定在[a_Tmin,a_Tmax]的范围内。

与现有技术相比，本发明的用于四自由度圆柱坐标型码垛机器人的末端残余振动抑制的方法具有以下特点和优点：

本发明的用于四自由度圆柱坐标型码垛机器人的末端残余振动抑制的方法，针对四自由度圆柱坐标型码垛机器人高速定位时末端容易产生残余振动的问题，基于加速度传感器信号建立机器人腰部伺服电机的补偿力矩进行振动抑制，该补偿力矩的参数通过在线整定方式进行快速优化。该方法简单可靠且便于实现，对于提高四自由度圆柱坐标型码垛机器人重复定位精度和工作效率有重要的意义。

结合附图阅读本发明的具体实施方式后，本发明的特点和优点将变得更加清楚。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例中用于四自由度圆柱坐标型码垛机器人的末端残余振动抑制的方法的结构框图；

图2为本发明实施例中适用于四自由度圆柱坐标型码垛机器人的的结构示意图。

具体实施方式

如图1、图2所示，本实施例提供一种用于四自由度圆柱坐标型码垛机器人的末端残余振动抑制的方法，在四自由度圆柱坐标型码垛机器人的大臂和末端分别安装加速度传感器，包括如下步骤：

步骤一、四自由度圆柱坐标型码垛机器人的末端到达某一定位点时，对四自由度圆柱坐标型码垛机器人的腰部旋转关节的伺服电机施加第1次至第n次补偿力矩，

补偿力矩表达式为F＝ga_T(t-τ) (1)

其中，F为补偿力矩,g为补偿力矩的比例系数，τ为补偿力矩的时间常数，a_T(t)为安装在机器人末端的加速度传感器的测量值；

步骤二、在对四自由度圆柱坐标型码垛机器人的腰部旋转关节的伺服电机施加第1次至第n次补偿力矩的过程中，四自由度圆柱坐标型码垛机器人的控制器实时检测末端的加速度传感器的测量值a_T(t)；

步骤三、四自由度圆柱坐标型码垛机器人的控制器判断a_T(t)在50ms内是否始终稳定在[a_Tmin,a_Tmax]的范围内，若在[a_Tmin,a_Tmax]的范围内则停止对四自由度圆柱坐标型码垛机器人的腰部旋转关节的伺服电机施加补偿力矩，补偿力矩的比例系数g和时间常数τ在机器人每次定位时都进行1次参数自整定，不断循环逐渐逼近最优值。

该方法实施的具体过程如下：

A.第1次施加补偿力矩(初次抑制过程)

在机器人第1个连续运动轨迹末端，当机器人控制器发送完该轨迹的最后1个位置指令信号并等待至少5个位置闭环周期后，即开始以T为采样周期采集安装在机器人大臂和末端的加速度传感器时域信号a_B(t)和a_T(t)，采集点数为N。然后，将a_B(t)和a_T(t)离散信号分别进行离散傅立叶变换(DFT)，得到了初次抑制过程中两个传感器信号的频域序列a_B ⁽⁰⁾(jω_k)和a_T ⁽⁰⁾(jω_k)，如下式所示：

其中，T为采样周期，N为采样点数，a_B ⁽⁰⁾(nT)和a_T ⁽⁰⁾(nT)分别是传感器信号的时域离散序列，

序列a_T ⁽⁰⁾(jω_k)绝对值的最大值对应的k值记为k_max，则得到初次抑制的截止频率为：

则a_B ⁽⁰⁾(jω_c ⁽⁰⁾)和a_T ⁽⁰⁾(jω_c ⁽⁰⁾)分别为序列a_T ⁽⁰⁾(jω_k)和a_B ⁽⁰⁾(jω_k)的最大值，即

a_B ⁽⁰⁾(jω_c ⁽⁰⁾)＝max|a_B ⁽⁰⁾(jω_k)| (5)

a_T ⁽⁰⁾(jω_c ⁽⁰⁾)＝max|a_T ⁽⁰⁾(jω_k)| (6)

在(0,10]范围内任意选取某一实数作为补偿力矩比例系数的初始化值g_i。

在(0,100]范围内任意选取某一实数作为补偿力矩时间常数的初始化值τ_i。

求取速度因子s的初始抑制值s⁽⁰⁾，位置因子p的初始抑制p⁽⁰⁾，表达式如下所示：

其中，m为机器人的大臂、小臂和末端的总质量。

求取初次抑制过程中补偿力矩比例系数g⁽⁰⁾和补偿力矩时间常数τ⁽⁰⁾，如下式所示：

由此可得初次抑制过程中的补偿力矩F⁽⁰⁾的表达式如下所示：

F⁽⁰⁾＝g⁽⁰⁾a_T ⁽⁰⁾(t-τ⁽⁰⁾) (11)

将F⁽⁰⁾作为转矩前馈补偿量加入到码垛机器人腰部伺服电机控制系统中，然后检测末端加速度传感器的值a_T(t)是否连续在[a_Tmin,a_Tmax]区间内50ms以上，如果没有达到这一条件，则继续施加补偿力矩，如果达到了这一条件，则停止补偿。

B.第2次施加补偿力矩(第2次抑制过程)

在机器人第2个连续运动轨迹末端，当机器人控制器发送完该轨迹的最后1个位置指令信号并等待至少5个位置闭环周期后，即开始以T为采样周期采集安装在机器人大臂和末端的加速度传感器时域信号a_B(t)和a_T(t)，采集点数为N。然后，将a_B(t)和a_T(t)离散信号分别进行离散傅立叶变换(DFT)，得到了第2次抑制过程中两个传感器信号的频域序列a_B ⁽¹⁾(jω_k)和a_T ⁽¹⁾(jω_k)，如下式所示：

其中，a_B ⁽¹⁾(nT)和a_T ⁽¹⁾(nT)分别是传感器信号的时域离散序列。

序列a_T ⁽¹⁾(jω_k)绝对值的最大值对应的k值记为k_max，则得到第2次抑制的截止频率为：

则a_B ⁽¹⁾(jω_c ⁽¹⁾)和a_T ⁽¹⁾(jω_c ⁽¹⁾)分别为序列a_T ⁽¹⁾(jω_k)和a_B ⁽¹⁾(jω_k)的最大值，即

a_B ⁽¹⁾(jω_c ⁽¹⁾)＝max|a_B ⁽¹⁾(jω_k)| (15)

a_T ⁽¹⁾(jω_c ⁽¹⁾)＝max|a_T ⁽¹⁾(jω_k)| (16)

求取速度因子s第2次抑制值s⁽¹⁾，位置因子p的第2次抑制值p⁽¹⁾，表达式如下所示：

求取第2次抑制过程中补偿力矩比例系数g⁽¹⁾和补偿力矩时间常数τ⁽¹⁾，如下式所示：

由此可得第2次抑制过程中的补偿力矩F⁽¹⁾的表达式如下所示：

F⁽¹⁾＝g⁽¹⁾a_T ⁽¹⁾(t-τ⁽¹⁾) (21)

将F⁽¹⁾作为转矩前馈补偿量加入到码垛机器人腰部伺服电机控制系统中，然后检测末端加速度传感器的值a_T(t)是否连续在[a_Tmin,a_Tmax]区间内50ms以上，如果没有达到这一条件，则继续施加补偿力矩，如果达到了这一条件，则停止补偿。

C.第n次施加补偿力矩(第n次抑制过程)

在机器人第n个连续运动轨迹末端，当机器人控制器发送完该轨迹的最后1个位置指令信号并等待至少5个位置闭环周期后，即开始以T为采样周期采集安装在机器人大臂和末端的加速度传感器时域信号a_B(t)和a_T(t)，采集点数为N。然后，将a_B(t)和a_T(t)离散信号分别进行离散傅立叶变换(DFT)，得到了第n次抑制过程中两个传感器信号的频域序列a_B ^{(n -1)}(jω_k)和a_T ^(n-1)(jω_k)，如下式所示：

其中，a_B ^(n-1)(nT)和a_T ^(n-1)(nT)分别是传感器信号的时域离散序列。

序列a_T ^(n-1)(jω_k)绝对值的最大值对应的k值记为k_max，则得到第n次抑制的截止频率为：

则a_B ^(n-1)(jω_c ^(n-1))和a_T ^(n-1)(jω_c ^(n-1))分别为序列a_T ^(n-1)(jω_k)和a_B ^(n-1)(jω_k)的最大值，即

a_B ^(n-1)(jω_c ^(n-1))＝max|a_B ^(n-1)(jω_k)| (25)

a_T ^(n-1)(jω_c ^(n-1))＝max|a_T ^(n-1)(jω_k)| (26)

求取速度因子s第n次抑制值s^(n-1)，位置因子p的第n次抑制值p^(n-1)，表达式如下所示：

求取第n次抑制过程中补偿力矩比例系数g^(n-1)和补偿力矩时间常数τ^(n-1)，如下式所示：

由此可得第n次抑制过程中的前馈补偿力矩F^(n-1)的表达式如下所示：

F^(n-1)＝g^(n-1)a_T ^(n-1)(t-τ^(n-1)) (31)

将F^(n-1)作为转矩前馈补偿量加入到码垛机器人腰部伺服电机控制系统中，然后检测末端加速度传感器的值a_T(t)是否连续在[a_Tmin,a_Tmax]区间内超过至少50ms，如果没有达到这一条件，则继续施加补偿力矩，如果达到了这一条件，则停止补偿。

本实施例中的用于四自由度圆柱坐标型码垛机器人的末端残余振动抑制的方法，针对四自由度圆柱坐标型码垛机器人高速定位时末端容易产生残余振动的问题，基于加速度传感器信号建立机器人腰部伺服电机的补偿力矩进行振动抑制，该补偿力矩的参数通过在线整定方式进行快速优化。该方法简单可靠且便于实现，对于提高四自由度圆柱坐标型码垛机器人重复定位精度和工作效率有重要的意义。

当然，上述说明并非是对本发明的限制，本发明也并不仅限于上述举例，本技术领域的技术人员在本发明的实质范围内所做出的变化、改型、添加或替换，也应属于本发明的保护范围。

Claims (1)

1.一种用于四自由度圆柱坐标型码垛机器人的末端残余振动抑制的方法，在四自由度圆柱坐标型码垛机器人的大臂和末端分别安装加速度传感器，其特征在于，包括如下步骤：

步骤一、四自由度圆柱坐标型码垛机器人的末端到达某一定位点时，开始对四自由度圆柱坐标型码垛机器人的腰部旋转关节的伺服电机施加补偿力矩，连续施加第1次至第n次补偿力矩，在施加第1次至第n次补偿力矩的每一次的过程中四自由度圆柱坐标型码垛机器人的末端处于不同的定位点，补偿力矩表达式为F＝ga_T(t-τ)，其中，F为补偿力矩,g为补偿力矩的比例系数，τ为补偿力矩的时间常数，a_T(t)为安装在机器人末端的加速度传感器的测量值；

对四自由度圆柱坐标型码垛机器人的腰部旋转关节的伺服电机施加第n次补偿力矩，实现第n次抑制过程如下，

四自由度圆柱坐标型码垛机器人的控制器以T为采样周期分别采集安装在机器人大臂的加速度传感器的测量值a_B(t)和安装在机器人末端的加速度传感器的测量值a_T(t)，采集点数为N，将a_B(t)和a_T(t)分别进行离散傅立叶变换，得到了第n次抑制过程中两个传感器信号的频域序列a_B ^(n-1)(jω_k)和a_T ^(n-1)(jω_k)，如下式所示，

<mrow> <msup> <msub> <mi>a</mi> <mi>B</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mrow> <mo>(</mo> <msub> <mi>j&amp;omega;</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>n</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mi>N</mi> <mo>-</mo> <mn>1</mn> </mrow> </munderover> <msup> <msub> <mi>a</mi> <mi>B</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mrow> <mo>(</mo> <mi>n</mi> <mi>T</mi> <mo>)</mo> </mrow> <msup> <mi>e</mi> <mrow> <mo>-</mo> <msub> <mi>nTj&amp;omega;</mi> <mi>k</mi> </msub> </mrow> </msup> <mo>,</mo> <mi>k</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mo>...</mo> <mi>N</mi> </mrow>

<mrow> <msup> <msub> <mi>a</mi> <mi>T</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mrow> <mo>(</mo> <msub> <mi>j&amp;omega;</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>n</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mi>N</mi> <mo>-</mo> <mn>1</mn> </mrow> </munderover> <msup> <msub> <mi>a</mi> <mi>T</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mrow> <mo>(</mo> <mi>n</mi> <mi>T</mi> <mo>)</mo> </mrow> <msup> <mi>e</mi> <mrow> <mo>-</mo> <msub> <mi>nTj&amp;omega;</mi> <mi>k</mi> </msub> </mrow> </msup> <mo>,</mo> <mi>k</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mo>...</mo> <mi>N</mi> </mrow>

其中，a_B ^(n-1)(nT)和a_T ^(n-1)(nT)分别是两个传感器信号的时域离散序列，

序列a_T ^(n-1)(jω_k)绝对值的最大值对应的k值记为k_max，则得到第n次抑制的截止频率为

<mrow> <msup> <msub> <mi>&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>=</mo> <mfrac> <mi>&amp;pi;</mi> <mrow> <mi>T</mi> <mi>N</mi> </mrow> </mfrac> <msub> <mi>k</mi> <mrow> <mi>m</mi> <mi>a</mi> <mi>x</mi> </mrow> </msub> </mrow>

则a_B ^(n-1)(jω_c ^(n-1))和a_T ^(n-1)(jω_c ^(n-1))分别为序列a_T ^(n-1)(jω_k)和a_B ^(n-1)(jω_k)的最大值，即

a_B ^(n-1)(jω_c ^(n-1))＝max|a_B ^(n-1)(jω_k)|

a_T ^(n-1)(jω_c ^(n-1))＝max|a_T ^(n-1)(jω_k)|

求取速度因子s第n次抑制值s^(n-1)，位置因子p的第n次抑制值p^(n-1)，表达式如下所示，

<mrow> <msup> <mi>s</mi> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>=</mo> <mfrac> <mrow> <mo>&amp;lsqb;</mo> <mn>1</mn> <mo>-</mo> <mi>Re</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <msup> <msub> <mi>a</mi> <mi>B</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mrow> <mo>(</mo> <msup> <msub> <mi>j&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>)</mo> </mrow> </mrow> <mrow> <msup> <msub> <mi>a</mi> <mi>T</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mrow> <mo>(</mo> <msup> <msub> <mi>j&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> <msup> <mi>g</mi> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>2</mn> <mo>)</mo> </mrow> </msup> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mrow> <mo>(</mo> <msup> <mi>&amp;tau;</mi> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>2</mn> <mo>)</mo> </mrow> </msup> <msup> <msub> <mi>&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>)</mo> </mrow> <mo>+</mo> <mi>Im</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <msup> <msub> <mi>a</mi> <mi>B</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mrow> <mo>(</mo> <msup> <msub> <mi>j&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>)</mo> </mrow> </mrow> <mrow> <msup> <msub> <mi>a</mi> <mi>T</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mrow> <mo>(</mo> <msup> <msub> <mi>j&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>&amp;lsqb;</mo> <mi>m</mi> <mo>-</mo> <msup> <mi>g</mi> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>2</mn> <mo>)</mo> </mrow> </msup> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mrow> <mo>(</mo> <msup> <mi>&amp;tau;</mi> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>2</mn> <mo>)</mo> </mrow> </msup> <msup> <msub> <mi>&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mi>n</mi> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mrow> <msup> <mrow> <mo>&amp;lsqb;</mo> <mn>1</mn> <mo>-</mo> <mi>Re</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <msup> <msub> <mi>a</mi> <mi>B</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mrow> <mo>(</mo> <msup> <msub> <mi>j&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>)</mo> </mrow> </mrow> <mrow> <msup> <msub> <mi>a</mi> <mi>T</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mrow> <mo>(</mo> <msup> <msub> <mi>j&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>&amp;lsqb;</mo> <mi>Im</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <msup> <msub> <mi>a</mi> <mi>B</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mrow> <mo>(</mo> <msup> <msub> <mi>j&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>)</mo> </mrow> </mrow> <mrow> <msup> <msub> <mi>a</mi> <mi>T</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mrow> <mo>(</mo> <msup> <msub> <mi>j&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mn>2</mn> </msup> </mrow> </mfrac> <msup> <msub> <mi>&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> </mrow>

<mrow> <msup> <mi>p</mi> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>=</mo> <mfrac> <mrow> <mo>&amp;lsqb;</mo> <mn>1</mn> <mo>-</mo> <mi>Re</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <msup> <msub> <mi>a</mi> <mi>B</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mrow> <mo>(</mo> <msup> <msub> <mi>j&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>)</mo> </mrow> </mrow> <mrow> <msup> <msub> <mi>a</mi> <mi>T</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mrow> <mo>(</mo> <msup> <msub> <mi>j&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> <mo>&amp;lsqb;</mo> <mi>m</mi> <mo>-</mo> <msup> <mi>g</mi> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>2</mn> <mo>)</mo> </mrow> </msup> <mi>cos</mi> <mrow> <mo>(</mo> <msup> <mi>&amp;tau;</mi> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>2</mn> <mo>)</mo> </mrow> </msup> <msup> <msub> <mi>&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>)</mo> </mrow> <mo>-</mo> <mi>Im</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <msup> <msub> <mi>a</mi> <mi>B</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mrow> <mo>(</mo> <msup> <msub> <mi>j&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>)</mo> </mrow> </mrow> <mrow> <msup> <msub> <mi>a</mi> <mi>T</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mrow> <mo>(</mo> <msup> <msub> <mi>j&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>)</mo> </mrow> <msup> <mi>g</mi> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>2</mn> <mo>)</mo> </mrow> </msup> <mi>sin</mi> <mrow> <mo>(</mo> <msup> <mi>&amp;tau;</mi> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>2</mn> <mo>)</mo> </mrow> </msup> <msup> <msub> <mi>&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mi>n</mi> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mrow> <msup> <mrow> <mo>&amp;lsqb;</mo> <mn>1</mn> <mo>-</mo> <mi>Re</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <msup> <msub> <mi>a</mi> <mi>B</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mrow> <mo>(</mo> <msup> <msub> <mi>j&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>)</mo> </mrow> </mrow> <mrow> <msup> <msub> <mi>a</mi> <mi>T</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mrow> <mo>(</mo> <msup> <msub> <mi>j&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>&amp;lsqb;</mo> <mi>Im</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <msup> <msub> <mi>a</mi> <mi>B</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mrow> <mo>(</mo> <msup> <msub> <mi>j&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>)</mo> </mrow> </mrow> <mrow> <msup> <msub> <mi>a</mi> <mi>T</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mrow> <mo>(</mo> <msup> <msub> <mi>j&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mn>2</mn> </msup> </mrow> </mfrac> <msup> <msub> <mi>&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> <mn>2</mn> </mrow> </msup> </mrow>

求取第n次抑制过程中补偿力矩比例系数g^(n-1)和补偿力矩时间常数τ^(n-1)，如下式所示，

<mrow> <msup> <mi>g</mi> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <msup> <msub> <mi>&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> <mn>2</mn> </mrow> </msup> </mrow> </mfrac> <msqrt> <mrow> <msup> <mrow> <mo>(</mo> <msup> <mi>s</mi> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <msup> <msub> <mi>&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <msup> <msub> <mi>m&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> <mn>2</mn> </mrow> </msup> <mo>-</mo> <msup> <mi>p</mi> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </msqrt> </mrow> 1

<mrow> <msup> <mi>&amp;tau;</mi> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>=</mo> <mfrac> <mrow> <mi>a</mi> <mi> </mi> <mi>t</mi> <mi>a</mi> <mi>n</mi> <mn>2</mn> <mrow> <mo>(</mo> <msup> <mi>s</mi> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <msup> <msub> <mi>&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>,</mo> <msup> <msub> <mi>m&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> <mn>2</mn> </mrow> </msup> <mo>-</mo> <msup> <mi>p</mi> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> <mo>)</mo> </mrow> </mrow> <mrow> <msup> <msub> <mi>&amp;omega;</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </msup> </mrow> </mfrac> </mrow>

其中，m为机器人的大臂、小臂和末端的总质量,

由此可得对四自由度圆柱坐标型码垛机器人的腰部旋转关节的伺服电机施加第n次补偿力矩F^(n-1)的表达式如下所示，

F^(n-1)＝g^(n-1)a_T ^(n-1)(t-τ^(n-1))；

步骤二、在对四自由度圆柱坐标型码垛机器人的腰部旋转关节的伺服电机施加第1次至第n次补偿力矩的过程中，四自由度圆柱坐标型码垛机器人的控制器实时检测末端的加速度传感器的测量值a_T(t)；

步骤三、四自由度圆柱坐标型码垛机器人的控制器判断a_T(t)在确定时间内是否始终稳定在[a_Tmin,a_Tmax]的范围内，若在[a_Tmin,a_Tmax]的范围内则停止对四自由度圆柱坐标型码垛机器人的腰部旋转关节的伺服电机施加补偿力矩，若不在[a_Tmin,a_Tmax]的范围内则循环步骤一至步骤三的过程一直到a_T(t)在确定时间内始终稳定在[a_Tmin,a_Tmax]的范围内。