[发明专利]一种苹果采摘机器人末端执行器抓取力主动柔顺控制方法

权利要求书

1.一种苹果采摘机器人末端执行器抓取力主动柔顺控制方法，其特征在于，包括以下步骤：

步骤1 采用力传感器得到末端执行器作用在苹果上的抓取力f，采用位置编码器得到位移变换量Δx；利用力传感器和位置编码器对f、Δx进行采样，采样次数为N，且N＞3；

步骤2 根据检测得到的抓取力和位移量数据，利用辨识器对等效刚度系数k_eq进行在线辨识，其中，在辨识器中引入变遗忘因子的递归最小二乘法对等效刚度系数进行辨识，将变遗忘因子设为关于测量值与计算值误差的函数，使遗忘因子能随着抓取力误差的变换而自动调整；

将末端执行器与苹果发生碰撞时的动力学模型简化为一阶导纳模型：f＝k_eq·Δx；其中，f为力传感器检测的抓取力，Δx为位置编码器测量的位移变换量，k_eq为所求取的等效刚度系数；

步骤3 通过等效刚度公式得到阻抗控制刚度参数，利用辨识得到的等效刚度系数k_eq对阻抗控制器刚度系数进行调整，所述的阻抗控制器为：

其中，m_t，b_t，k_t分别是阻抗控制器的惯性系数，阻尼系数和刚度系数，x_r是设定的加速度，速度和位移，x是实际的加速度，速度和位移；f_e，f_r是实际的抓取力和给定的抓取力；

步骤4 将阻抗控制器输出的位置量与给定的位置信号进行叠加得到位置控制器的输入信号，通过位置控制器输出驱动电机的电压信号；位置控制器为增量式PID控制器：

其中，u(t)，u(t-1)分别表示第t，t-1次采样时刻位置控制器的输出，e(t)，e(t-1)，e(t-2)分别是第t，t-1，t-2次采样时刻的偏差值，k_p为比例控制增益，T、T_D、T₁分别是PID控制器采样时间，微分时间，积分时间；

步骤5 将位置控制器输出的电压信号转变为力矩电机的驱动信号，用于控制电机的工作，执行抓取任务。

2.根据权利要求1所述的一种苹果采摘机器人末端执行器抓取力主动柔顺控制方法，其特征在于，所述步骤1中，力传感器为FSR 402力敏电阻型力传感器；位置编码器型号为TRD-NA 1024NW。

3.根据权利要求1所述的一种苹果采摘机器人末端执行器抓取力主动柔顺控制方法，其特征在于，所述步骤2中，在迭代辨识过程中，当测量值与给定抓取力之间的误差小于10％时，系统进入稳定状态，迭代结束。

4.根据权利要求1所述的一种苹果采摘机器人末端执行器抓取力主动柔顺控制方法，其特征在于，所述步骤2～3中辨识器的具体辨识过程为：

首先测得一组(x_t，y_t)时，由下式得到等效刚度系数k_eq的初值，式中x_t，y_t分别代表第t次末端执行器的位移以及抓取力的大小；

k_eq，t＝(x_t^Tx_t)^-1x_t^Ty_t(t＝1，2…)

然后由递归最小二乘法求取等效刚度系数k_eq的迭代公式如下：

P_t+1＝P_t/(C_t+x_t+1P_tx_t+1)；θ_t+1＝P_t+1x_t+1；

k_eq，t+1＝k_eq，t+θ_t+1(y_t+1-x_t+1θ_t)(t＝0，1，2…)

其中：P_t是协方差矩阵，k_eq，t是等效刚度，当t＝0时，P₀和k_eq，0分别代表初始协方差矩阵和初始等效刚度，取P₀＝0，k_eq，0＝1；C_t为遗忘因子，C_t的选取是非常重要的，因为不恰当的值会导致协方差矩阵P_t变得不精确；在时变系统中，建立一个关于误差e_t的函数用来求取C_t通常被认为是一个非常合理有效的方式；和普通的递归最小二乘法结构一样，将遗忘因子设置成为一个关于误差的函数；

C_t＝1-a₁[arctan(a₂(|e_t|-a₃))/π+1/2]

其中，e_t＝y_t-K_eq，tx_t；a₁，a₂，a₃经过多次试验分别设为0.4、0.5、1；

根据等效刚度系数定义：k_tk_e分别代表阻抗控制器刚度系数和环境刚度系数；

则可以得到阻抗控制器刚度系数：

最后的控制器可以设计成：