Method and device for estimating contact force of tail end of robot without force sensor

Abstract

The invention belongs to the technical field of contact force detection, and particularly relates to a force sensor-free robot tail end contact force estimation method and device. A machine learning model capable of predicting the current of each joint of the robot in a non-contact state is trained in advance, and the recorded operation data of each joint of the robot in the contact state and the pre-trained machine learning model are utilized to obtain the current value of each joint of the robot in the non-contact state during actual use. Calculating the external force torque of each joint; designing a Kalman filter for smoothing the external force moment of each joint of the robot, and designing process noise of the Kalman filter based on the working interval of the joint speed of the mechanical arm and the error of the machine learning model between the adaptive joint speed range and the non-adaptive joint speed range; and then the robot tail end contact force is calculated according to the filtered external force torque of each joint of the robot, and the estimation error of the robot tail end contact force is reduced.

Description

Technical Field

[0001] The present invention belongs to the technical field of contact force detection, and in particular relates to a method and device for estimating the contact force of a robot end without a force sensor. Background Art

[0002] Robots are important production and service equipment in the industrial and non-industrial sectors, and are also indispensable automation equipment in the field of advanced manufacturing technology. In order to accurately control the robot, it is necessary to detect the contact force between the robot and the external environment. The traditional method is to establish a robot dynamics model and estimate the end contact force based on the dynamics model. However, the robot is a very complex multi-input and multi-output nonlinear system with time-varying and strongly coupled nonlinear dynamic characteristics. It is difficult to accurately model and identify parameters including gravity, friction, inertia and Coriolis force. In practice, it is difficult to establish a complete and accurate robot dynamics model, especially for robots without force sensors. It is extremely difficult to establish an accurate perception of the robot end contact force dynamics model only through the limited motion state information of the body, which leads to a large error in the calculated contact force. At present, there is also a method for estimating the contact force of the robot end based on a convolutional neural network. This method does not require the establishment of a complex dynamic model. However, since it does not consider any physical characteristic model of the robot itself, the robot contact force estimated based only on the convolutional neural network model may have a large error, and the reliability of the contact force estimation result is poor. For example, when the low-speed commutation part of the robot joint is prone to motor torque disturbance and the process noise of the motor current is large, the estimated robot end contact force error is large. In addition, this method only performs data probability calculations based on the robot's operating state and end contact force at a certain moment, and does not consider the relationship between the robot's current motion state and the previous motion state in time. The contact force estimation result is prone to sudden changes, which is not conducive to the robot's motion control. Summary of the invention

[0003] The purpose of the present invention is to provide a method and device for estimating the end contact force of a force-free sensor robot, so as to solve the problem that there is a large error in the estimated robot contact force when the actual operating conditions of the robot are significantly different from the data used to train the convolutional neural network model.

[0004] In order to solve the above technical problems, the present invention provides a method for estimating the contact force of a robot end without a force sensor, comprising:

[0005] Obtain the operating data of each joint of the robot in contact state, including displacement, speed and current;

[0006] The displacement and velocity of each joint of the robot in the contact state are input into a pre-trained machine learning model to obtain the current of each joint in the non-contact state; the external force current of each joint is determined based on the difference between the current of each joint in the contact state and the current of each joint in the non-contact state, and the external force torque of each joint is calculated using the external force current of each joint;

[0007] The external force torque of each joint is input as an initial value into the Kalman filter to obtain the filtered external force torque of each joint; the process noise variance of the Kalman filter is determined based on the working range of the joint speed and the error between the machine learning model in the adaptive joint speed range and the inadaptive joint speed range;

[0008] The robot end contact force is calculated based on the filtered external force torque of each joint.

[0009] Furthermore, the process noise variance expression of the Kalman filter is:

[0010] Q i =Q i-min +(Q i-max -Q i-min )exp(-β|q i | γ )

[0011] Among them, Q i is the process noise variance of joint i, Q i-min is the noise variance of the joint torque process corresponding to the joint i velocity being within the normal working range, Q i-max is the noise variance of the joint torque process corresponding to the joint i velocity being in the abnormal working range, β is the adaptation coefficient of the joint velocity range, γ is the error change order of the machine learning model between the adapted joint velocity range and the unadapted joint velocity range, q i is the displacement of joint i.

[0012] Furthermore, the machine learning model includes a neural network model for extracting spatial structure features and a neural network model for processing time series data.

[0013] Furthermore, the machine learning model is a CNN-GRU model.

[0014] Furthermore, the training data used in the training of the CNN-GRU model are the displacement, velocity and current of each joint of the robot in a non-contact state. During training, the displacement and velocity of each joint in a non-contact state are used as input, and the current of each joint in a non-contact state is used as output.

[0015] Furthermore, the external force torque of each joint of the robot is calculated according to the transmission ratio between the motor shaft of each joint of the robot and the joint output shaft and the external force current of each joint.

[0016] The beneficial effects of the above technical solution are as follows: the present invention is an improved invention creation, which pre-trains a machine learning model that can predict the current of each joint of the robot in a non-contact state. In actual use, the recorded operation data of each joint of the robot in a contact state and the pre-trained machine learning model are used to obtain the current value of each joint of the robot in a non-contact state, and then calculate the external force torque of each joint; a Kalman filter is designed for smoothing the external force torque of each joint of the robot, and considering that the low-speed reversing part of the robot joint is prone to cause motor torque disturbance, resulting in large process noise of the motor current, thereby affecting the accuracy of the external force torque of each joint, the process noise of the Kalman filter is designed based on the working range of the robot arm joint speed and the error between the machine learning model in the adaptive joint speed range and the inadaptable joint speed range, and then the robot terminal contact force is calculated according to the filtered external force torque of each joint of the robot, which can infer the hidden optimal joint external force torque in real time from an uncertain environment and noise pollution, reduce the estimation error of the robot terminal contact force, and solve the problem that when the actual operating condition is greatly different from the data used to train the convolutional neural network model, the robot contact force estimated by the convolutional neural network model has a large error.

[0017] To solve the above technical problems, the present invention also provides a device for estimating the end contact force of a force-free sensorless robot, comprising a processor, wherein the processor is used to execute computer program instructions to implement the steps in the above-mentioned force-free sensorless robot end contact force estimation method.

[0018] The beneficial effects of the above technical solution are as follows: the present invention is an improved invention creation, which pre-trains a machine learning model that can predict the current of each joint of the robot in a non-contact state. In actual use, the recorded operation data of each joint of the robot in a contact state and the pre-trained machine learning model are used to obtain the current value of each joint of the robot in a non-contact state, and then calculate the external force torque of each joint; a Kalman filter is designed for smoothing the external force torque of each joint of the robot, and considering that the low-speed reversing part of the robot joint is prone to cause motor torque disturbance, resulting in large process noise of the motor current, thereby affecting the accuracy of the external force torque of each joint, the process noise of the Kalman filter is designed based on the working range of the robot arm joint speed and the error between the machine learning model in the adaptive joint speed range and the inadaptable joint speed range, and then the robot terminal contact force is calculated according to the filtered external force torque of each joint of the robot, which can infer the hidden optimal joint external force torque in real time from an uncertain environment and noise pollution, reduce the estimation error of the robot terminal contact force, and solve the problem that when the actual operating condition is greatly different from the data used to train the convolutional neural network model, the robot contact force estimated by the convolutional neural network model has a large error. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 Schematic diagram of the contact force estimation principle of the end contact force of the forceless sensor robot according to the method embodiment of the present invention;

[0020] Figure 2 It is a schematic diagram of a CNN-GRU neural network model of an embodiment of the method of the present invention. DETAILED DESCRIPTION

[0021] In order to make the purpose, technical solutions and advantages of the present invention more clear, the specific implementation methods of the present invention are further described below in conjunction with the accompanying drawings.

[0022] The present invention pre-trains a machine learning model that can predict the current of each joint of the robot in a non-contact state, designs a Kalman filter for smoothing the external force torque of each joint of the robot, and considers that the low-speed commutation part of the robot joint is prone to cause motor torque disturbance, resulting in large process noise of the motor current, thereby affecting the accuracy of the external force torque of each joint. The process noise of the Kalman filter is designed based on the working range of the robot arm joint speed and the error between the adaptive joint speed range and the unadaptable joint speed range of the machine learning model. Then, the robot terminal contact force is calculated according to the filtered external force torque of each joint of the robot. The hidden optimal joint external force torque can be inferred in real time from an uncertain environment and noise pollution, thereby reducing the estimation error of the robot terminal contact force.

[0023] Method Embodiment

[0024] The present invention provides a method for estimating the contact force of a robot end without a force sensor. The implementation principle of the method is as follows: Figure 1 As shown, the detailed description is given below.

[0025] 1. Train the machine learning model.

[0026] The machine learning model can use CNN, RNN, LSTM, etc. In order to avoid the sudden change of the estimated contact force caused by the data probability calculation based only on the operation state of the robot at a certain moment and the end contact force, the machine learning model of the present invention adopts a neural network model (such as CNN, GAN, RBF, etc.) that can extract spatial structure features and a neural network model (such as RNN, GRU, LSTM, etc.) that can process time series data, so that the relationship between the current motion state and the previous motion state in time is also considered when calculating the contact force. As a preferred embodiment, since convolutional neural networks (CNN) are good at processing data with spatial structure features, and gated recurrent networks (GRU) are good at processing time series data, the machine learning model of the present invention adopts a CNN-GRU model combining convolutional neural networks and gated recurrent networks. The model can combine the advantages of both, and can perform efficient feature extraction on the operation data of the robot body, and more accurately predict the joint current in the non-contact state.

[0027] The training process of a machine learning model consists of the following steps:

[0028] 1) Collect data in advance.

[0029] The robot is allowed to perform multiple reciprocating motions over a large range in the working space in a non-contact state, and the operating data of each joint of the robotic arm in a non-contact state during the motion are recorded. The operating data includes robot motion state information such as displacement, speed and current.

[0030] 2) Establish a CNN-GRU model.

[0031] like Figure 2 As shown in the figure, the CNN-GRU model includes 1 preprocessing layer, 2 convolutional layers (Conv), 2 maximum pooling layers (Pooling), 2 CNN Relu activation layers, 2 GRU layers, 1 GRU Relu activation layer and 1 fully connected layer (FC). The convolutional layers, maximum pooling layers and CNN Relu activation layers in CNN, as well as the GRU layers and GRU Relu activation layers in CRU are set according to specific needs and are not limited here.

[0032] 3) Train the CNN-GRU model.

[0033] The CNN-GRU model is trained using the pre-collected displacement, velocity and current of each joint of the robot in a non-contact state. During training, the displacement and velocity of each joint in a non-contact state are used as input, and the current of each joint in a non-contact state is used as output. The predicted value (i.e., the current of each joint in a non-contact state) is made as close to the true value as possible through back propagation. The RMSE root mean square error is used as the loss function of the CNN-GRU model. The Adam optimizer algorithm is used to optimize the parameters of the CNN-GRU model. When the loss function converges, the training ends and the trained CNN-GRU model is obtained.

[0034] The loss function expression is:

[0035]

[0036] Among them, RMSE is the loss function, N is the prediction duration, i is the total number of input data collected during one training, O i r is the current value of joint r in the i-th group of data, p i r is the predicted value corresponding to the corresponding model. The smaller the RMSE value, the better the prediction effect of the prediction model.

[0037] The optimization algorithm formula of Adam optimizer is:

[0038]

[0039] Among them, θ t is the parameter value of the model at the tth optimization, a random value when t=0, α is the learning rate, ε is the parameter to prevent the denominator from being 0, and m t is the weighted average of the gradients during the tth optimization, v t It is the weighted average of the squared gradients during the t-th optimization.

[0040] m t It can be obtained by the following formula:

[0041] m t =β1m t-1 +(1-β1)g t-1

[0042] v t It can be obtained by the following formula:

[0043] v t =β2v t-1 +(1-β2)g 2 t-1

[0044] Among them, β1 and β2 are exponential decay rates, g t-1is the gradient value at time t-1, g 2 t-1 is the square of the gradient value at t-1, and the gradient value is the loss function RMSE on the model parameter θ t β1 controls the weight distribution, and β2 controls the influence of the previous gradient square.

[0045] The speed of the joint is calculated based on the motor speed feedback from the driver and the transmission ratio between the motor shaft and the joint output shaft, and the joint current is the motor current feedback from the driver. Therefore, during training, the displacement of each joint in a non-contact state and the motor speed feedback from the driver can be used as input, and the motor current can be used as output to train the model.

[0046] 2. Obtain the operating data of each joint of the robot in the contact state, input the displacement and speed of each joint of the robot in the contact state into the pre-trained machine learning model, obtain the current of each joint in the non-contact state, determine the external force current of each joint based on the difference between the current of each joint of the robot in the contact state and the current in the non-contact state, and use the external force current of each joint to calculate the external force torque of each joint.

[0047] When the robot is in contact with the external environment, that is, when the robot is in contact operation, the operation data of each joint of the robot in the contact state is obtained, including displacement, speed and current. When the data used for model training is the motor speed, the displacement of each joint of the robot in the contact state, the motor speed and current feedback from the driver of each joint of the robot in the contact state and other information are collected in real time, and the displacement of each joint and the motor speed of each joint are input into the CNN-GRU model to obtain the current of each joint in the non-contact state. Then, the current of each joint of the robot in the contact state and the current difference in the non-contact state are calculated, and the external force torque of each joint of the robot is calculated according to the transmission ratio and difference between the motor shaft of each joint of the robot and the output shaft of the joint.

[0048] Since the collected current of each joint of the robot in the contact state includes the current in the non-contact state and the joint external force current in the contact state, the current of each joint of the robot in the contact state is subtracted from the current in the non-contact state predicted by the CNN-GRU model to obtain the external force current of each joint, and then the external force current of each joint and the transmission coefficient (motor torque coefficient, transmission ratio, etc.) are used to obtain the external force torque of each joint of the robot, as follows:

[0049]

[0050] Among them, I ci (t) is the current value of the robot joint i in the contact state at time t collected in real time, is the current value of robot joint i in non-contact state at time t, is the current value of robot joint i in non-contact state at time t predicted by CNN-GRU model, is the joint external force current value of robot joint i in contact state at time t, is the joint external force torque value of robot joint i at time t, K Ti is the transmission ratio of the motor shaft of robot joint i to the joint output shaft, q(t) is the displacement of robot joint i at time t, is the velocity of robot joint i at time t.

[0051] 3. Design a Kalman filter, input the external force torque of each joint of the robot as the initial value into the Kalman filter for optimal estimation, filter each joint external force separately, and obtain the filtered external force torque of each joint of the robot.

[0052] The Kalman filter uses the Kalman filter method to perform optimal smoothing estimation processing. The specific design is as follows:

[0053] The state prediction equation is:

[0054]

[0055] The variance prediction equation is:

[0056] P i - (t) = P i (t-1)+Q i

[0057] The Kalman gain equation is:

[0058]

[0059] The state estimation equation is:

[0060]

[0061] The state estimation variance equation is:

[0062] P i (t) = (1-K i (t))P i - (t)

[0063] The initial value of the state estimation variance is:

[0064]

[0065] in, is the torque value predicted for the joint i state at time t, is the optimal estimated torque value of joint i after filtering at time t, that is, the torque value of the external force of robot joint i after filtering, P i - (t) is the variance prediction value of joint i at time t, Pi(t) is the variance prediction value of the state estimation of joint i at time t, Q i is the process noise variance of joint i, K i (t) is the Kalman gain of joint i at time t, R i is the measurement noise variance of joint i, i is the joint number of the robot. The experiment found that Q i The value is less than 1, R i The filtering effect is better when the value is greater than 1. Therefore, preferably, the process noise variance value of the Kalman filter is less than 1 and the measurement noise variance value is greater than 1.

[0066] The process noise variance of the Kalman filter is determined based on the working range of the robot joint speed and the error between the machine learning model's adaptive joint speed range and the unadaptive joint speed range. Since the low-speed commutation part of the robot joint is prone to cause motor torque disturbance, which in turn leads to large process noise of the motor current, the process noise variance Q i Designed as a function related to the joint velocity of the robot arm, the process noise variance expression of the Kalman filter is:

[0067] Q i =Q i-min +(Q i-max -Q i-min )exp(-β|q i | γ )

[0068] Among them, Q i is the process noise variance of joint i, Q i-min is the noise variance of the joint torque process corresponding to the joint i velocity being within the normal working range, Q i-max is the noise variance of the joint torque process corresponding to the joint i speed being in the abnormal working range, β is the joint speed range adaptation coefficient, the larger the β value is, the larger the adapted joint speed range is, γ is the error change order of the machine learning model between the adapted joint speed range and the unadapted joint speed range, the smoother the error change of the machine learning model when the joint speed is in the normal working range and the abnormal working range, the smaller the γ value is, and q i is the displacement of joint i. The abnormal working range refers to the area around 0 speed. β and γ are determined experimentally. The machine learning model is adapted to the joint speed range when the input model data is the joint speed range data used in training, and is not adapted to the joint speed range when the input model data is the joint speed range data not used in training.

[0069] 4. Calculate the robot end contact force based on the filtered external force torque of each robot joint.

[0070] Through the robot Jacobian matrix The end contact force is obtained by combining the external force torque of the robot joint output after Kalman filtering. The expression is as follows:

[0071]

[0072] For example, when a robot has 6 joints on one arm, the calculation formula for the contact force at the end of the arm is:

[0073]

[0074] in, n F ext is the contact force at the end of the robot arm with n joints, is the inverse matrix of the Jacobian matrix transposed of the robot arm, and n is the number of joints of the robot arm.

[0075] Device Embodiment

[0076] The present invention provides a sensorless robot end contact force estimation device, comprising a processor, the processor is used to execute computer program instructions to implement a sensorless robot end contact force estimation method. The specific implementation process of the method has been described in detail in the method embodiment, and will not be repeated here.

[0077] The present invention adopts the CNN-GRU model and the force transformation method based on Kalman filtering to optimally estimate the robot contact force, avoiding the sudden change of the estimated contact force caused by only performing data probability calculation based on the robot's operating state and terminal contact force at a certain moment, solving the problem that the robot contact force estimated by the convolutional neural network model has a large error when the actual operating condition is greatly different from the data used to train the convolutional neural network model, and avoiding the tedious manual modeling and parameter identification process, thereby realizing a convenient, efficient, low-cost and real-time estimation of the terminal contact force of a force-free sensor robot.

Claims

1. A method for estimating the end contact force of a forceless sensorless robot, characterized in that: include: Obtain the operating data of each joint of the robot in contact state, including displacement, speed and current; The displacement and velocity of each joint of the robot in the contact state are input into a pre-trained machine learning model to obtain the current of each joint in the non-contact state; the external force current of each joint is determined based on the difference between the current of each joint in the contact state and the current of each joint in the non-contact state, and the external force torque of each joint is calculated using the external force current of each joint; The external force torque of each joint is input as an initial value into the Kalman filter to obtain the filtered external force torque of each joint; the process noise variance of the Kalman filter is determined based on the working range of the joint speed and the error between the machine learning model in the adaptive joint speed range and the inadaptive joint speed range; The robot end contact force is calculated based on the filtered external force torque of each joint.

2. The method for estimating the end contact force of a forceless sensorless robot according to claim 1, characterized in that: The process noise variance expression of the Kalman filter is: Q i =Q i-min +(Q i-max -Q i-min )exp(-β|q i | γ ) Among them, Q i is the process noise variance of joint i, Q i-min is the noise variance of the joint torque process corresponding to the joint i velocity being within the normal working range, Q i-max is the noise variance of the joint torque process corresponding to the joint i velocity being in the abnormal working range, β is the adaptation coefficient of the joint velocity range, γ is the error change order of the machine learning model between the adapted joint velocity range and the unadapted joint velocity range, q i is the displacement of joint i.

3. The method for estimating contact force of a forceless sensorless robot end according to claim 1, characterized in that: The machine learning model includes a neural network model for extracting spatial structure features and a neural network model for processing time series data.

4. The method for estimating the end contact force of a forceless sensorless robot according to claim 3, characterized in that: The machine learning model is a CNN-GRU model.

5. The method for estimating the end contact force of a forceless sensorless robot according to claim 4, characterized in that: The training data used by the CNN-GRU model during training are the displacement, velocity and current of each joint of the robot in a non-contact state. During training, the displacement and velocity of each joint in a non-contact state are used as input, and the current of each joint in a non-contact state is used as output.

6. The method for estimating contact force of a forceless sensorless robot end according to claim 1, characterized in that: The external force torque of each joint of the robot is calculated according to the transmission ratio of the motor shaft of each joint of the robot and the joint output shaft and the external force current of each joint.

7. A force estimation device for the end contact force of a forceless sensor robot, comprising a processor, characterized in that: The processor is used to execute computer program instructions to implement the steps in the method for estimating contact force of a forceless sensorless robot end as described in any one of claims 1-6.

Images