[发明专利]四自由度圆柱坐标型码垛机器人末端残余振动抑制方法

权利要求书

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)，如下式所示，

其中，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_B^(n-1)(jω_k)和 a_T^(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]的范围内。