CN106272428A

CN106272428A - A kind of apple picking robot end effector grasp force Active Compliance Control method

Info

Publication number: CN106272428A
Application number: CN201610821173.9A
Authority: CN
Inventors: 姬伟; 唐伟; 许波; 钱志杰; 孟祥利; 赵德安
Original assignee: Jiangsu University
Current assignee: Jiangsu University
Priority date: 2016-09-13
Filing date: 2016-09-13
Publication date: 2017-01-04
Anticipated expiration: 2036-09-13
Also published as: CN106272428B

Abstract

The invention discloses an active compliance control method for the gripping force of an apple picking robot end effector, which belongs to the field of picking robot control technology. The gripping force control method is characterized in that a force sensor and an encoder are arranged on the apple picking robot end effector. The force acting on the grasping object and the amount of position change are collected, and the collected displacement and force are used as the input of the recursive least squares method identifier with variable forgetting factor, and the stiffness coefficient of the impedance controller is identified online, and according to The output result of the second-order impedance controller automatically adjusts the stiffness parameters of the impedance controller to meet different environmental requirements in real time. This control method can effectively reduce the damage rate of the picking robot to apples, and improve the anti-interference ability of the robot in the field.

Description

An active compliance control method for the grasping force of an apple picking robot end effector

技术领域technical field

本发明涉及苹果采摘机器人控制领域，尤其是关于一种基于变刚度系数阻抗控制的苹果采摘机器人末端执行器抓取力主动柔顺控制方法，属于农业信息化领域。The invention relates to the field of control of apple picking robots, in particular to an active compliance control method for grasping force of an end effector of an apple picking robot based on variable stiffness coefficient impedance control, and belongs to the field of agricultural informatization.

背景技术Background technique

我国是一个农业大国，实现农业生产的现代化、机械化和自动化是社会发展的必然趋势。果实采摘是农业生产中最耗时耗力的一个环节，具有成本高、季节性强、需要大量劳动力等特点。但由于工业生产的迅速发展，大量农业劳动力缺失以及人口老龄化加剧等原因，使得能够从事农业生产的劳动力越来越少，单靠人工劳动已经不能满足现有的需求。为了解决所存在的问题，农业机器人应运而生，末端执行器作为与果蔬直接接触的部件，在农业机器人研究中占据了重要的地位，现有末端执行器研究中，存在着两个难点：1.末端执行器的设计不具有通用性，现有的末端执行器都是针对一种或一类果蔬进行设计的，并且所采用的被动柔顺方法难于实现对果蔬的无损采摘，阻碍了农业机器人的推广；2.现有的末端执行器抓取控制技术无法实现对果蔬的主动柔顺抓取，末端执行器抓取力过大会损伤果蔬，较小的抓取力会导致在抓取过程中出现果蔬跌落，从而也导致果蔬的损伤，这也阻碍了农业机器人的商业推广。所以发明一种主动柔顺抓取控制机制，使其能对抓取作用力进行感知，适应和控制，对实现末端执行器对果蔬的柔顺抓取采摘尤为重要。my country is a large agricultural country, and realizing the modernization, mechanization and automation of agricultural production is an inevitable trend of social development. Fruit picking is the most time-consuming and labor-intensive link in agricultural production. It has the characteristics of high cost, strong seasonality, and a large amount of labor. However, due to the rapid development of industrial production, the lack of a large number of agricultural labor force and the aging of the population, the labor force capable of engaging in agricultural production has become less and less, and manual labor alone can no longer meet the existing demand. In order to solve the existing problems, agricultural robots emerged as the times require. As a component in direct contact with fruits and vegetables, the end effector occupies an important position in the research of agricultural robots. There are two difficulties in the existing research on end effectors: 1. .The design of the end effector is not universal. The existing end effectors are all designed for one or one type of fruits and vegetables, and the passive compliance method adopted is difficult to achieve non-destructive picking of fruits and vegetables, which hinders the development of agricultural robots. Promotion; 2. The existing end-effector grasping control technology cannot achieve active and smooth grasping of fruits and vegetables. Excessive grasping force of the end-effector will damage the fruits and vegetables. Falling, thereby also causing damage to fruits and vegetables, which also hinders the commercial promotion of agricultural robots. Therefore, inventing an active and compliant grasping control mechanism, which can sense, adapt and control the grasping force, is particularly important to realize the compliant grasping and picking of fruits and vegetables by the end effector.

发明内容Contents of the invention

本方案为解决现有苹果采摘机器人末端执行器抓取力控制技术的不足，无法实现柔顺抓取控制的缺陷，提出一种双闭环的机器人末端执行器抓取力主动柔顺控制方法，其中位置内环使用增量式PID控制，力外环使用变刚度系数的阻抗控制，这种控制方法能够兼顾抓取力与末端执行器的位移变换，并且对于农业机器人的野外工作环境也具有一定的适应性。In order to solve the deficiency of the existing grasping force control technology of the end effector of the apple picking robot and the defect that the compliant grasping control cannot be realized, this scheme proposes a double closed-loop active compliant control method for the grasping force of the robot end effector, in which the position within The ring uses incremental PID control, and the force outer ring uses impedance control with variable stiffness coefficient. This control method can take into account the grasping force and the displacement transformation of the end effector, and it also has certain adaptability to the field working environment of agricultural robots. .

本发明解决其技术问题所采用的技术方案包括以下步骤：The technical solution adopted by the present invention to solve its technical problems comprises the following steps:

步骤1采用力传感器得到末端执行器作用在苹果上的抓取力f，采用位置编码器和机器人运动学方程得到位移量△x；利用力传感器和位置编码器对f、△x进行采样，采样次数为N，且N>3；Step 1 Use the force sensor to obtain the grasping force f of the end effector acting on the apple, and use the position encoder and robot kinematics equation to obtain the displacement Δx; use the force sensor and position encoder to sample f and Δx, and sample The number of times is N, and N>3;

步骤2根据检测得到的抓取力和位移量数据，利用辨识器对等效刚度系数k_eq进行在线辨识，其中，在辨识器中引入变遗忘因子的递归最小二乘法对等效刚度系数进行辨识，将变遗忘因子设为关于测量值与计算值误差的函数，使遗忘因子能随着抓取力误差的变换而自动调整；Step 2 According to the grasping force and displacement data obtained by detection, use the identifier to identify the equivalent stiffness coefficient k _eq online, where the recursive least squares method with variable forgetting factor is introduced in the identifier to identify the equivalent stiffness coefficient , set the variable forgetting factor as a function of the error between the measured value and the calculated value, so that the forgetting factor can be automatically adjusted with the change of the grasping force error;

将末端执行器与苹果发生碰撞时的动力学模型简化为一阶导纳模型：y＝k_eq·△x；其中，y为力传感器检测的抓取力，△x为位置编码器测量的位移变换量，k_eq为所求取的等效刚度系数；The dynamic model when the end effector collides with the apple is simplified to a first-order admittance model: y=k _eq △x; where y is the grasping force detected by the force sensor, and △x is the displacement measured by the position encoder Transformation quantity, k _eq is the obtained equivalent stiffness coefficient;

步骤3通过等效刚度公式得到阻抗控制刚度参数，利用辨识得到的等效刚度系数k_eq对阻抗控制器刚度系数进行调整，所述的阻抗控制器为：Step 3 Obtain the impedance control stiffness parameters through the equivalent stiffness formula, and adjust the stiffness coefficient of the impedance controller by using the equivalent stiffness coefficient k _eq obtained from the identification. The impedance controller is:

${f f}_{e e} - - {f f}_{r r} = = {m m}_{t t} ((\overset{\cdot\cdot \cdot\cdot}{x x} - - {\overset{\cdot\cdot \cdot\cdot}{x x}}_{r r})) + + {b b}_{t t} ((\overset{\cdot &Center Dot;}{x x} - - {\overset{\cdot \cdot}{x x}}_{r r})) + + {k k}_{t t} ((x x - - {x x}_{r r}));;$

其中，m_t,b_t,k_t分别是阻抗控制器的惯性系数，阻尼系数和刚度系数，x_r是设定的加速度，速度和位移，x是实际的加速度，速度和位移；f_e,f_r是实际的抓取力和给定的抓取力；Among them, m _t , b _t , k _t are the inertia coefficient, damping coefficient and stiffness coefficient of the impedance controller respectively, x _r is the set acceleration, velocity and displacement, x is the actual acceleration, velocity and displacement; f _e , f _r are the actual grasping force and the given grasping force;

步骤4将阻抗控制器输出的位置量与给定的位置信号进行叠加得到位置控制器的输入信号，通过位置控制器输出驱动电机的电压信号；位置控制器为增量式PID控制器：Step 4. Superimpose the position quantity output by the impedance controller and the given position signal to obtain the input signal of the position controller, and output the voltage signal to drive the motor through the position controller; the position controller is an incremental PID controller:

$u u ((t t)) = = u u ((t t - - 11)) + + {k k}_{p p} * * [[((e e ((t t)) - - e e ((t t - - 11)))) + + \frac{T T}{{T T}_{11}} e e ((k k)) + + \frac{{T T}_{D D.}}{T T} ((e e ((t t)) - - 22 e e ((t t - - 11)) + + e e ((t t - - 22))))]]$

其中，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控制器采样时间，微分时间，积分时间；Among them, u(t), u(t-1) represent the output of the position controller at the sampling time t and t-1 respectively, e(t), e(t-1), e(t-2) are respectively The deviation value at the t, t-1, and t-2 sampling times, k _p is the proportional control gain, T, T _D , T ₁ are the sampling time, differential time, and integral time of the PID controller respectively;

步骤5将位置控制器输出的电压信号转变为力矩电机的驱动信号，用于控制电机的工作，执行抓取任务。Step 5 converts the voltage signal output by the position controller into the drive signal of the torque motor, which is used to control the work of the motor and perform the grabbing task.

进一步，所述步骤1中，力传感器为FSR 402力敏电阻型力传感器；位置编码器型号为TRD-NA1024NW。Further, in the step 1, the force sensor is an FSR 402 force-sensitive resistance type force sensor; the model of the position encoder is TRD-NA1024NW.

进一步，所述步骤2中，在迭代辨识过程中，当测量值与给定抓取力之间的误差小于10％时，系统进入稳定状态，迭代结束。Further, in the step 2, during the iterative identification process, when the error between the measured value and the given grasping force is less than 10%, the system enters a stable state and the iteration ends.

进一步，所述步骤2～3中辨识器的具体辨识过程为：Further, the specific identification process of the identifier in steps 2 to 3 is as follows:

测得一组(x_t,y_t)时，由下式得到等效刚度系数k_eq的初值，式中x_t,y_t分别代表第t次末端执行器的位移以及抓取力的大小；When a set of (x _t , y _t ) is measured, the initial value of the equivalent stiffness coefficient k _eq can be obtained from the following formula, where x _t and y _t represent the displacement and grasping force of the t-th end effector respectively ;

${k k}_{e e q q,, t t} = = {(({x x}_{t t}^{T T} {x x}_{t t}))}^{- - 11} {x x}_{t t}^{T T} {y the y}_{t t},, ((t t = = 11,, 2... 2...))$

然后由递归最小二乘法求取等效刚度系数k_eq的迭代公式如下：Then the iterative formula for obtaining the equivalent stiffness coefficient k _eq by the recursive least square method is as follows:

P_t+1＝P_t/(C_t+x_t+1P_tx_t+1)；θ_t+1＝p_t+1x_t+1；P _t+1 = P _t /(C _t +x _t+1 P _t x _t+1 ); θ _t+1 = p _t+1 x _t+1 ;

k_eq,t+1＝k_eq,t+θ_t+1(y_t+1-x_t+1θ_t)(t＝0,1,2…)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通常被认为是一个非常合理有效的方式；和普通的递归最小二乘法结构一样，将遗忘因子设置成为一个关于误差的函数；Where: P _t is the covariance matrix, k _eq,t is the equivalent stiffness, when t=0, P ₀ and k _eq,0 represent the initial covariance matrix and the initial equivalent stiffness respectively, take P ₀ =0,k _eq,0 =1. C _t is the forgetting factor, the selection of C _t is very important, because an inappropriate value will cause the covariance matrix P _t to become inaccurate; in a time-varying system, a function about the error e _t is established to obtain C _t is generally considered to be a very reasonable and effective method; like the ordinary recursive least squares structure, the forgetting factor is set as a function of the error;

C_t＝1-a₁[arctan(a₂(|e_t|-a₃))/π+1/2]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；Among them, e _t =y _t -K _eq,t x _t ; a ₁ , a ₂ , a ₃ are respectively set to 0.4, 0.5, 1 after many tests;

根据等效刚度系数定义：k_t k_e分别代表阻抗控制器刚度系数和环境刚度系数；According to the definition of equivalent stiffness coefficient: k _t k _e respectively represent the impedance controller stiffness coefficient and the environment stiffness coefficient;

则可以得到阻抗控制器刚度系数： Then the stiffness coefficient of the impedance controller can be obtained:

最后的控制器可以设计成： The final controller can be designed as:

本发明由于采用以上技术方案，具有以下优点：The present invention has the following advantages due to the adoption of the above technical scheme:

1.本发明通过阻抗控制，能够实现抓取力与抓取位置之间的动态平衡，从而实现苹果采摘机器人抓取的主动柔顺控制。1. The present invention can realize the dynamic balance between the grasping force and the grasping position through impedance control, thereby realizing the active and compliant control of the grasping of the apple picking robot.

2.考虑到操作环境的特殊性，利用递归最小二乘法对阻抗控制参数进行实时调整，能够更好地实现对抓取力的跟随，有效地降低苹果抓取受损率。2. Considering the particularity of the operating environment, the real-time adjustment of the impedance control parameters by using the recursive least square method can better realize the following of the grasping force and effectively reduce the damage rate of apple grasping.

附图说明Description of drawings

图(1)是本系统控制方法框图；Figure (1) is a block diagram of the system control method;

图(2)是本发明所依据苹果采摘机器人末端执行器的示意图。Figure (2) is a schematic diagram of the end effector of the apple picking robot on which the present invention is based.

具体实施方式detailed description

一种基于阻抗控制的苹果采摘机器人抓取力主动柔顺控制方法，包括如下步骤：An active compliance control method for the gripping force of an apple picking robot based on impedance control, comprising the following steps:

1)利用FSR 402力敏电阻型力传感器200测量得到末端执行器抓取力f，利用TRD-NA1024NW位置编码器190的信息得到末端执行器的位移变换量△x，将得到抓取力和位移信号输入到在线辨识器识别环境与控制器的等效刚度系数k_eq，并通过所建立的二阶阻抗一阶导纳模型y＝k_eq·△x，求出阻抗控制器中的刚度系数。1) Use the FSR 402 force-sensitive resistive force sensor 200 to measure the grasping force f of the end effector, and use the information of the TRD-NA1024NW position encoder 190 to obtain the displacement transformation amount Δx of the end effector, and then obtain the grasping force and displacement The signal is input to the online identifier to identify the equivalent stiffness coefficient k _eq of the environment and the controller, and the stiffness coefficient in the impedance controller is obtained through the established second-order impedance and first-order admittance model y=k _eq ·△x.

2)利用力传感器200和位置编码器210对末端执行器的抓取力f_r和位移变量△x，在一个采样周期内，进行N次采样，且N>3，取N次数据测得的平均值作为一个采样周期得到最终数据输入到在线辨识器中，再根据等效抓取模型，采用变遗忘因子的递归最小二乘法计算得到等效刚度系数的在线估计值。当实际抓取力与给定抓取力之间的误差小于10％时，迭代结束，系统进入稳定工作状态。2) Using the force sensor 200 and the position encoder 210 to the grasping force f _r and the displacement variable Δx of the end effector, within a sampling period, perform N times of sampling, and N>3, take N times of data and measure The average value is used as a sampling period to obtain the final data and input to the online identifier, and then according to the equivalent grasping model, the online estimated value of the equivalent stiffness coefficient is calculated by using the recursive least square method with variable forgetting factor. When the error between the actual grasping force and the given grasping force is less than 10%, the iteration ends and the system enters a stable working state.

3)利用等效刚度系数k_eq对阻抗控制器120中的刚度系数123进行调节。所述的阻抗控制器120为：3) Using the equivalent stiffness coefficient k _eq to adjust the stiffness coefficient 123 in the impedance controller 120 . Described impedance controller 120 is:

${f f}_{e e} - - {f f}_{r r} = = {m m}_{t t} ((\overset{\cdot\cdot \cdot\cdot}{x x} - - {\overset{\cdot\cdot \cdot\cdot}{x x}}_{r r})) + + {b b}_{t t} ((\overset{\cdot \cdot}{x x} - - {\overset{\cdot &Center Dot;}{x x}}_{r r})) + + {k k}_{t t} ((x x - - {x x}_{r r}))$

其中，m_t,b_t,k_t分别是阻抗控制器的惯性系数，阻尼系数和刚度系数，x_r是设定的加速度，速度和位移，x是实际的加速度，速度和位移。f_e,f_r是实际的抓取力和给定的抓取力。Among them, m _t , b _t , k _t are the inertia coefficient, damping coefficient and stiffness coefficient of the impedance controller respectively, x _r is the set acceleration, velocity and displacement, x is the actual acceleration, velocity and displacement. f _e , _fr are the actual grasping force and the given grasping force.

仅考虑一维环境阻抗，也就是将多维变量矩阵转换为一维变量，对阻抗控制器刚度系数修正的方法是 Considering only one-dimensional environmental impedance, that is, transforming the multidimensional variable matrix into one-dimensional variable, the method for correcting the stiffness coefficient of the impedance controller is

4)将阻抗控制器输出的位置量与给定的位置信号进行叠加得到位置控制器的输入信号，通过位置控制器输出驱动电机的电压信号。位置控制器为增量式PID控制器：4) Superimpose the position quantity output by the impedance controller and the given position signal to obtain the input signal of the position controller, and output the voltage signal to drive the motor through the position controller. The position controller is an incremental PID controller:

其中，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控制器采样时间，微分时间，积分时间。Among them, u(t), u(t-1) represent the output of the position controller at the sampling time t and t-1 respectively, e(t), e(t-1), e(t-2) are respectively The deviation values at the t, t-1, and t-2 sampling times, k _p is the proportional control gain, T, T _D , and T ₁ are the sampling time, differential time, and integral time of the PID controller, respectively.

5)将阻抗控制器120的输出的位置变换量输入到位置控制器中，位置控制器输出驱动力矩电机运动的电压，从而实现末端执行器的抓取。5) The position change output output by the impedance controller 120 is input into the position controller, and the position controller outputs the voltage for driving the torque motor to realize the grasping of the end effector.

下面结合附图和实施例对本发明进行详细的描述Below in conjunction with accompanying drawing and embodiment the present invention is described in detail

一种基于变刚度系数阻抗控制的苹果采摘机器人末端执行器抓取力主动柔顺控制方法，如图1所示，其结构包括：比较器110、130、180，阻抗控制器120、位置PID控制器150、驱动电机160、被控对象末端执行器170、位置编码器190、力传感器200和在线辨识器210。其中阻抗控制器120包括惯性系数121、阻尼系数122、和刚度系数123。比较器110接收给定抓取力和实际抓取力，比较器的输出端接阻抗控制器的输入端，阻抗控制器的输出端与给定位置信息相加作为比较器130的输入，比较器130的输出接位置控制器150端。位置控制器150输出端接力矩电机160的输入端，力矩电机160的输出端接末端执行器170的输入端，末端执行器170输出端接位置编码器190和力传感器200的输入端，力传感器200的输出端接比较器110的输入端，同时，位置编码器190和力传感器200的输出端还连接到在线辨识器210的输入端。An active compliance control method for the grasping force of the end effector of an apple picking robot based on variable stiffness coefficient impedance control, as shown in Figure 1, its structure includes: comparators 110, 130, 180, impedance controller 120, and position PID controller 150 , drive motor 160 , controlled object end effector 170 , position encoder 190 , force sensor 200 and online identifier 210 . The impedance controller 120 includes an inertia coefficient 121 , a damping coefficient 122 , and a stiffness coefficient 123 . The comparator 110 receives the given grasping force and the actual grasping force, the output terminal of the comparator is connected to the input terminal of the impedance controller, and the output terminal of the impedance controller is added with the given position information as the input of the comparator 130, and the comparator The output of 130 is connected to the position controller 150 end. The output end of the position controller 150 is connected to the input end of the torque motor 160, the output end of the torque motor 160 is connected to the input end of the end effector 170, the output end of the end effector 170 is connected to the input end of the position encoder 190 and the force sensor 200, and the force sensor The output terminal of the comparator 200 is connected to the input terminal of the comparator 110 , meanwhile, the output terminals of the position encoder 190 and the force sensor 200 are also connected to the input terminal of the online identifier 210 .

图2为本发明的实施实例：苹果采摘机器人在工作时，电机正转可以带动末端执行器闭合进行抓取苹果，电机反转可以使末端执行器打开释放苹果，力矩电机下安放位置编码器190用来测量相对位移，末端执行器上安装力传感器200测量抓取苹果的力。Figure 2 is an implementation example of the present invention: when the apple picking robot is working, the forward rotation of the motor can drive the end effector to close to grab apples, the reverse rotation of the motor can make the end effector open and release the apples, and a position encoder 190 is placed under the torque motor To measure relative displacement, a force sensor 200 is installed on the end effector to measure the force of grasping the apple.

系统的工作流程为：比较器110将输入的预定抓取力与从力传感器200处得到的实际力进行比较，产生误差信号。阻抗控制器120对误差信号进行控制，得到位置偏移信号，并与给定的位置信号相加得到位置控制器150的输入信号，经过位置控制器150的调节，得到驱动电机的控制信号，驱动末端执行器以一定的速度对目标果实进行抓取。力传感器200将力信号输入在线辨识器210，同时位置编码器将位置信号输入到在线辨识器210中，在线辨识器210通过辨识算法得到果蔬的等效刚度系数k_eq在通过等效刚度模型得到阻抗控制刚度系数，并将其输入到阻抗控制器120中，更好地修正阻抗控制器120的刚度系数k_t。The working process of the system is as follows: the comparator 110 compares the input predetermined grasping force with the actual force obtained from the force sensor 200 to generate an error signal. The impedance controller 120 controls the error signal to obtain a position offset signal, and adds it to a given position signal to obtain the input signal of the position controller 150, and through the adjustment of the position controller 150, obtains the control signal of the driving motor, and drives The end effector grabs the target fruit at a certain speed. The force sensor 200 inputs the force signal to the online identifier 210, and the position encoder inputs the position signal to the online identifier 210. The online identifier 210 obtains the equivalent stiffness coefficient k _eq of fruits and vegetables through the identification algorithm, and obtains it through the equivalent stiffness model The impedance controls the stiffness coefficient and inputs it into the impedance controller 120 to better modify the stiffness coefficient k _t of the impedance controller 120 .

系统的工作原理为：将力传感器和位置编码器的信息输入到在线辨识器在线得到等效刚度系数k_eq。其中所述的在线辨识器的算法，详细分为以下步骤：The working principle of the system is: input the information of the force sensor and the position encoder to the online identifier to obtain the equivalent stiffness coefficient k _eq online. The algorithm of the online identifier described therein is divided into the following steps in detail:

当测得一组(x_t,y_t)时，由下式得到等效刚度系数k_eq的初值。When a set of (x _t , y _t ) is measured, the initial value of the equivalent stiffness coefficient k _eq can be obtained from the following formula.

然后由递归最小二乘法求取等效刚度系数k_eq的迭代公式如下Then the iterative formula for obtaining the equivalent stiffness coefficient k _eq by the recursive least square method is as follows

P_t+1＝P_t/(C_t+x_t+1P_tx_t+1)P _t+1 ＝P _t /(C _t +x _t+1 P _t x _t+1 )

θ_t+1＝p_t+1x_t+1 θ _t+1 = p _t+1 x _t+1

k_eq,_t+1＝k_eq,_t+θ_t+1(y_t+1-x_t+1θ_t)k _eq , _t+1 ＝k _eq , _t +θ _t+1 (y _t+1 -x _t+1 θ _t )

式中:P＝1，k_s,t＝0，C_t为遗忘因子。C_t的选取是非常重要的，因为不恰当的值会导致协方差矩阵P_t变得不精确。在时变系统中，建立一个关于误差e_t的函数用来求取C_t通常被认为是一个非常合理有效的方式。和普通的递归最小二乘法结构一样，将遗忘因子设置成为一个关于误差的函数。In the formula: P = 1, k _{s, t} = 0, C _t is the forgetting factor. The choice of C _t is very important, because inappropriate values will cause the covariance matrix P _t to become inaccurate. In a time-varying system, it is usually considered a very reasonable and effective way to establish a function about the error e _t to obtain C _t . Like the ordinary recursive least squares structure, the forgetting factor is set as a function of the error.

其中，e_t＝y_t-K_eq,tx_t。a₁,a₂,a₃经过多次试验分别设为0.4、0.5、1。Wherein, e _t =y _t -K _eq,t x _t . a ₁ , a ₂ , and a ₃ were set to 0.4, 0.5, and 1 respectively after several experiments.

根据等效刚度系数定义According to the definition of equivalent stiffness coefficient

${k k}_{e e q q} = = {(({k k}_{t t}^{- - 11} + + {k k}_{e e}^{- - 11}))}^{- - 11} = = \frac{{k k}_{t t} \cdot \cdot {k k}_{e e}}{{k k}_{t t} + + {k k}_{e e}}$

则可以得到阻抗控制器刚度系数：Then the stiffness coefficient of the impedance controller can be obtained:

${k k}_{t t + + 11} = = \frac{{k k}_{e e q q,, t t + + 11} \cdot \cdot {k k}_{e e}}{{k k}_{e e} - - {k k}_{e e q q,, t t + + 11}}$

最后的控制器可以设计成：The final controller can be designed as:

${m m}_{t t} ((\overset{\cdot\cdot \cdot\cdot}{x x} - - {\overset{\cdot\cdot \cdot\cdot}{x x}}_{r r})) + + {b b}_{t t} ((\overset{\cdot \cdot}{x x} - - {\overset{\cdot \cdot}{x x}}_{r r})) + + {k k}_{t t + + 11} ((x x - - {x x}_{r r})) = = {f f}_{e e} - - {f f}_{r r} . .$

综上，本发明的一种苹果采摘机器人末端执行器抓取力主动柔顺控制方法，该抓取力控制方法特征在于由苹果采摘机器人末端执行器上配置的力传感器以及编码器采集得到作用于抓取对象上的作用力和以及位置变换量，将采集得到的位移和力作为变遗忘因子的递归最小二乘法辨识器的输入，对阻抗控制器刚度系数进行在线辨识，并且根据二阶阻抗控制器的输出结果实时自动调整适应于不同环境要求的阻抗控制器的刚度参数。该控制方法可以有效降低采摘机器人对苹果的抓取损伤率，并且提高机器人在野外工作的抗干扰能力。To sum up, the present invention provides an active compliance control method for the grasping force of the end effector of an apple picking robot. Take the force and the position change on the object, and use the acquired displacement and force as the input of the recursive least squares method identifier with variable forgetting factor to identify the stiffness coefficient of the impedance controller online, and according to the second-order impedance controller The output results can automatically adjust the stiffness parameters of the impedance controller adapting to different environmental requirements in real time. This control method can effectively reduce the damage rate of the picking robot to apples, and improve the anti-interference ability of the robot working in the field.

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。The above descriptions are only preferred embodiments of the present invention, and are not intended to limit the present invention. Any modifications, equivalent replacements and improvements made within the spirit and principles of the present invention shall be included within the protection scope of the present invention.

Claims

1. An active and compliant control method for the grasping force of an apple picking robot end effector, characterized in that, comprising the following steps:

Step 1 Use the force sensor to obtain the grasping force f of the end effector acting on the apple, and use the position encoder and robot kinematics equation to obtain the displacement Δx; use the force sensor and position encoder to sample f and Δx, and sample The number of times is N, and N>3;

Step 2 According to the grasping force and displacement data obtained by detection, use the identifier to identify the equivalent stiffness coefficient k _eq online, where the recursive least squares method with variable forgetting factor is introduced in the identifier to identify the equivalent stiffness coefficient , set the variable forgetting factor as a function of the error between the measured value and the calculated value, so that the forgetting factor can be automatically adjusted with the change of the grasping force error;

The dynamic model when the end effector collides with the apple is simplified to a first-order admittance model: y=k _eq △x; where y is the grasping force detected by the force sensor, and △x is the displacement measured by the position encoder Transformation quantity, k _eq is the obtained equivalent stiffness coefficient;

Step 3 Obtain the impedance control stiffness parameters through the equivalent stiffness formula, and adjust the stiffness coefficient of the impedance controller by using the equivalent stiffness coefficient k _eq obtained from the identification. The impedance controller is:

Among them, m _t , b _t , k _t are the inertia coefficient, damping coefficient and stiffness coefficient of the impedance controller respectively, are the set acceleration, velocity and displacement, is the actual acceleration, velocity and displacement; f _e , f _r are the actual grasping force and the given grasping force;

Step 4 Superimpose the position quantity output by the impedance controller and the given position signal to obtain the input signal of the position controller, and output the voltage signal of the driving motor through the position controller; the position controller is an incremental PID controller:

u u ((t t)) = = u u ((t t - - 11)) + + {k k}_{p p} * * [[((e e ((t t)) - - e e ((t t - - 11)))) + + \frac{T T}{T T} e e ((k k)) + + \frac{{T T}_{D D.}}{T T} ((e e ((t t)) - - 22 e e ((t t - - 11)) + + e e ((t t - - 22))))]]

Among them, u(t), u(t-1) represent the output of the position controller at the sampling time t and t-1 respectively, e(t), e(t-1), e(t-2) are respectively The deviation value at the t, t-1, and t-2 sampling times, k _p is the proportional control gain, T, T _D , T ₁ are the sampling time, differential time, and integral time of the PID controller respectively;

Step 5 converts the voltage signal output by the position controller into the drive signal of the torque motor, which is used to control the work of the motor and perform the grabbing task.

2. The active and compliant control method for the grasping force of an apple picking robot end effector according to claim 1, wherein in said step 1, the force sensor is a FSR 402 force-sensitive resistance type force sensor; a position encoder The model number is TRD-NA1024NW.

3. The active and compliant control method for the gripping force of an apple picking robot end effector according to claim 1, characterized in that, in the step 2, in the iterative identification process, when the measured value and the given gripping force When the error between them is less than 10%, the system enters a stable state and the iteration ends.

4. The active and compliant control method for the gripping force of the end effector of an apple picking robot according to claim 1, wherein the specific identification process of the identifier in steps 2 to 3 is as follows:

First, when a set of (x _t , y _t ) is measured, the initial value of the equivalent stiffness coefficient k _eq is obtained from the following formula, where x _t and y _t represent the displacement of the end effector and the grasping force of the tth time respectively size;

{k k}_{e e q q,, t t} = = {(({x x}_{t t}^{T T} {x x}_{t t}))}^{- - 11} {x x}_{t t}^{T T} {y the y}_{t t} ((t t = = 11,, 22 ... ...))

Then the iterative formula for obtaining the equivalent stiffness coefficient k _eq by the recursive least square method is as follows:

P _t+1 = P _t /(C _t +x _t+1 P _t x _t+1 ); θ _t+1 = p _t+1 x _t+1 ;

k _eq , _t+1 ＝k _eq , _t +θ _t+1 (y _t+1 -x _t+1 θ _t )(t=0,1,2…)

Where: P _t is the covariance matrix, k _eq,t is the equivalent stiffness, when t=0, P ₀ and k _eq,0 represent the initial covariance matrix and the initial equivalent stiffness respectively, take P ₀ =0,k _eq,0 ＝1; C _t is the forgetting factor, the selection of C _t is very important, because inappropriate values will cause the covariance matrix P _t to become _inaccurate ; The function used to obtain C _t is generally considered to be a very reasonable and effective way; and the same as the ordinary recursive least squares method structure, the forgetting factor is set as a function about the error;

C _t ＝1-a ₁ [arctan(a ₂ (|e _t |-a ₃ ))/π+1/2]

Among them, e _t =y _t -K _eq,t x _t ; a ₁ , a ₂ , a ₃ are respectively set to 0.4, 0.5, 1 after many tests;

According to the definition of equivalent stiffness coefficient: k _t k _e respectively represent the impedance controller stiffness coefficient and the environment stiffness coefficient;

Then the stiffness coefficient of the impedance controller can be obtained:

The final controller can be designed as: