Methods for estimating the contact force at the end of a tendon-driven manipulator

Abstract

This invention discloses a method for estimating the end-effector contact force of a tendon-driven manipulator. Based on the dynamics and kinematics model of a dexterous hand, the invention first transforms the dynamic model of the dexterous hand into state equations, then discretizes the state equations, and finally estimates the contact force at the end of the dexterous hand using a particle filtering method, thereby obtaining the corresponding end-effector torque to complete the grasping of the target object. This invention provides a new approach for estimating the end-effector contact force of robotic arms and dexterous hands without the need for force sensors. Moreover, the algorithm is easy to understand, has strong anti-interference ability, high accuracy, and strong adaptability, enabling high-precision estimation and control of the end-effector contact force of tendon-driven manipulators. At the same time, it can save space for installing a six-dimensional force sensor at the end of the dexterous hand, reducing the cost of the dexterous hand.

Description

Technical Field

[0001] This invention relates to the estimation and control of contact force at the end of a robotic arm, belonging to the technical fields of robotic arm control, contact force estimation and control, etc. Background Technology

[0002] To avoid collisions at the end effector of a dexterous hand or mitigate their adverse consequences, force control is necessary. Force control first requires acquiring force information about the dexterous hand. This is typically done by installing six-dimensional torque sensors at the end effector or joints. However, this method has several drawbacks: firstly, the sensors are expensive, impacting the overall cost; secondly, the sensors reduce the stiffness of the overall dexterous hand system, affecting its dynamic modeling; and thirdly, the sensors introduce electrical noise, necessitating the addition of observers to mitigate this noise, which increases system complexity and response speed. Therefore, sensorless end-effector force estimation has gradually become a key research focus.

[0003] Estimating the external forces at the end of a dexterous hand is equivalent to estimating the external moments at the joints. Researchers have proposed many observation methods to estimate the external forces of multi-joint rigid bodies. Hacksel et al. first used an observer to estimate the external forces. Their basic idea was to treat the observer error dynamics as a mass-damped spring system driven by environmental forces, and to estimate the external forces through the actual position error of the rigid body. Luca et al. proposed a method for estimating external forces using generalized momentum and applied it to the automatic collision detection technology of DLR flexible multi-joint rigid bodies. Chen et al. proposed a nonlinear perturbation observer, and ensured the performance of the perturbation observer by selecting design parameters. Jung et al. proposed an external force estimation method based on a Kalman filter for multi-degree-of-freedom dexterous hands. This method does not require calculating the inverse matrix of the inertia matrix, and constructs an expression based on generalized momentum and discretizes it into a system equation suitable for standard Kalman filtering. Subsequently, Capurso and Wahrburg improved upon Jung's work and proposed an external force estimation method based on extended Kalman filtering. However, the above methods have poor resistance to model uncertainties and observation noise, resulting in poor external force estimation performance. Summary of the Invention

[0004] The purpose of this invention is to design an algorithm for estimating the contact force between the end effector and the environment in a tendon-driven dexterous hand. This method estimates external contact force based on a model, which can reduce the cost of the dexterous hand. The contact force estimation has strong anti-interference capabilities and improves the accuracy of contact force estimation, thereby improving the force control accuracy of the system's end effector operation, reducing the impact from free space to constrained space, and reducing tendon wear and extending its lifespan by constraining tendon tension. Ultimately, it can effectively realize the operation control of the tendon-driven manipulator, improving the manipulator's dexterity and lifespan.

[0005] The solution adopted in this invention is as follows:

[0006] A method for estimating the contact force at the end effector of a tendon-driven manipulator includes the following steps:

[0007] Step 1: Obtain the system's state equations based on the system's dynamic equations;

[0008] Step 2: Discretize the state equations of the system obtained in Step 1;

[0009] Step 3: Based on the state equation and measurement equation obtained after discretization in Step 2, and based on the state-space model of the system, the initial parameter samples of the state variables can be obtained using the Monte Carlo sampling method, which means the initial particle set can be obtained.

[0010] Step 4: Set the termination condition for particle filtering, perform particle filtering, and obtain the optimal estimate of the system's state variables;

[0011] Step 5: Calculate the optimal estimate of the contact force at the end of the dexterous hand based on the optimal estimate obtained in Step 4.

[0012] Furthermore, step 1, obtaining the system's state equations based on the system's dynamic equations, specifically includes the following steps:

[0013] Step 1.1: When the joint is subjected to an external torque, the dynamic equation of the dexterous finger is:

[0014]

[0015] Where M(q) represents the inertia matrix, Let G(q) represent the Coriolis force matrix, and G(q) represent the gravity term. q represents the acceleration, velocity, and displacement of the joint, respectively, and τ represents the displacement. e This represents the external torque acting on the joint, and u represents the driving torque.

[0016] Step 1.2: Determine the system's state variables, namely the system's generalized momentum p and τ. e p is specifically represented by the following formula:

[0017] p=M(q)q

[0018] Step 1.3: Taking the derivative of the generalized momentum of the system, we can obtain:

[0019]

[0020] Step 1.4: Record the observed torque value. Specifically, it is expressed as follows:

[0021]

[0022] Step 1.5: Substitute the result obtained in Step 1.4 into Step 1.3:

[0023]

[0024] Step 1.6: Since each model has its own uncertainty, we denote the incorrect qualitative assessment of the model as w. p In fact, it is just Gaussian-distributed white noise;

[0025] Step 1.7: Based on the classical observer concept, the external torque τ acting on the joint. e This can be represented as the following model:

[0026]

[0027] General A τ It is 0, while w τ It is Gaussian white noise;

[0028] Step 1.8: From this, we can obtain the system's state equations:

[0029]

[0030] Where N is the number of degrees of freedom of the dexterous fingers, N e It is the dimension of the external torque, typically 6;

[0031] Step 1.9: Let the states of the system be p and τ respectively. e denoted as x=[pτ e ] T Therefore, the state equation of the system can be expressed as:

[0032]

[0033] In the formula,

[0034] Step 1.10: Record A τ If the value is 0, then the observation denoted as y can be expressed as:

[0035]

[0036] In the formula, v = (0, R) c () indicates the measurement noise term;

[0037] Step 1.11: From this, we can obtain the system's measurement equation as follows:

[0038] y = C c x+v

[0039] in,

[0040] The specific steps for discretizing the system state equations in step 2 are as follows:

[0041] Step 2.1: The discretized state equation and measurement equation can be expressed as:

[0042] With y k+1 =C k x k +v k

[0043] Where the subscripts k and k+1 represent the current time and the next time, respectively, A k B k C k Table the coefficient matrix of the discretized system; Step 2.2: The specific solution method for the parameters of the discretized state equation and measurement equation is as follows:

[0044] Step 2.2.1: The parameters of the system's state equations are as follows:

[0045]

[0046] exp represents the power series of e, and Δt represents the sampling interval;

[0047] Step 2.2.2: The parameters of the system's measurement equations are as follows:

[0048]

[0049] Where R k The covariance matrix representing the measurement noise;

[0050] Step 2.3: Define the parameters of the particle filter, specifically:

[0051] Q c =diag{0.0001,0.0001,0.0001,10,10,10}, R c =diag{0.01,0.01,0.01};

[0052] Furthermore, the specific steps for particle initialization in step 3 are as follows:

[0053] Step 3.1: Bayesian prediction and update equations:

[0054] Step 3.1.1: The Bayesian prediction equation is:

[0055] P(X k |Y 1:k-1 )=∫P(X k |Xk-1 )P(X k-1 |Y 1:k-1 )dX k-1

[0056] Step 3.1.2: The Bayesian update equation is:

[0057]

[0058] In the formula, the posterior probability P(X) at the previous time step k-1 |Y k-1 Given that the posterior probability at the current time is P(X) k |Y k ), P(Y k |X k ) represents the likelihood probability;

[0059] Step 3.2: Monte Carlo sampling, the sampling equation is:

[0060]

[0061] In the formula, δ(·) represents the Dirac function. Represents the estimated posterior probability;

[0062] Furthermore: The specific steps of particle filtering in step 4 are as follows:

[0063] Step 4.1: Importance Sampling

[0064] Step 4.1.1: The expected value of the particle was obtained through the Monte Carlo method. Therefore, in principle, the state value to be estimated can be obtained by averaging the posterior probabilities through sampling. However, the posterior probabilities are unknown, so importance sampling is needed to solve this problem. Specifically:

[0065]

[0066] in, This represents the weight of the i-th particle;

[0067] Step 4.1.2: The importance sampling method specifically involves sampling from a known, sampleable distribution, such as q(X|Y). The expectation of the state to be estimated can then be expressed as:

[0068]

[0069] f(·) represents the state equation;

[0070] Step 4.1.3: w(X) k ) represents the particle weight, specifically:

[0071]

[0072] Step 4.1.4: Normalize the particle weights. The normalized weights are specifically as follows:

[0073]

[0074] Step 4.2: Calculate particle weights recursively:

[0075] Step 4.2.1: Importance sampling solves the sampling problem, but the weight of each particle is calculated separately during the sampling process, which is inefficient because the weight must be recalculated every time a sample is added. Therefore, to solve this problem, a recursive method for calculating particle weights is introduced, specifically:

[0076]

[0077] In the formula, Let represent the posterior probability of the i-th particle at the current moment. Let P(X) represent the importance sampling density function of the i-th particle at the current moment. k |Y 1:k Let P(X) represent the posterior probability at the current time step. k-1 |Y 1:k-1 () represents the posterior probability at the previous time step. Represents the likelihood probability. This represents the probability of transitioning from the previous state to the current state. This represents the sampling density function indicating the importance of the i-th particle transitioning from its previous state to its current state. This represents the importance sampling density function of the i-th particle at the previous time step. The weight of the previous state of the i-th particle is represented by .

[0078] Step 4.2.2: Normalize the particle weights using the same method as in Step 4.1.4;

[0079] Step 4.2.3: Calculate the state to be estimated, X:

[0080]

[0081] In the formula, f(·) represents the state equation;

[0082] Step 4.3: Prevent particle degradation settings:

[0083] In the filtering process using step 2, a degradation problem exists. After several iterations, the weights of many particles become very small and negligible, with only a few particles having relatively large weights. Furthermore, the variance of the particle weights increases over time, resulting in a smaller number of effective particles in the state space. As the number of invalid sampled particles increases, a significant amount of computation is wasted on particles that have almost no effect on estimating the posterior filtering probability distribution, leading to a decline in estimation performance. Therefore, it is necessary to redefine the particle weights, i.e., resampling.

[0084] Step 4.3.1: Define the effective number of particles, i.e. Specifically:

[0085]

[0086] Step 4.3.2: When the number of effective particles is less than When this happens, resampling is required. During resampling, particles with high weights are copied, while particles with low weights are removed, ensuring the total number of particles is N. After resampling, all posterior particles have the same weight, which is 1. N The specific number of times a particle with a high weight is replicated is determined by the following formula:

[0087]

[0088] Step 4.4: Obtain the new weight update formula:

[0089]

[0090] Step 4.5: Set the simulation time and repeat steps 2-4 to obtain the optimal estimate of the state variables, i.e. At this time, the estimated value of the external torque on the joint is:

[0091]

[0092] Step 5: Based on the Jacobian matrix J at time k+1 k+1 This allows us to obtain the external force on the joint at that moment. The optimal estimate:

[0093]

[0094] Step 5.1 Here for The inverse matrix is ​​also solved using the LU decomposition method:

[0095] Step 5.1.1: Decompose the matrix into an upper triangular matrix and a lower triangular matrix, as shown below: Let... Step 5.1.2: Calculate the inverse of the matrix based on the LU decomposition result:

[0096]

[0097] The beneficial effects of this invention are as follows: The method proposed in this invention uses an improved particle swarm optimization algorithm to predict unknown parameters of complex models, giving full play to the advantage of particle swarm optimization algorithm in quickly approximating the optimal solution, and can estimate the contact force between the dexterity end and the environment more quickly and accurately.

[0098] Compared with existing end-contact force estimation methods, the present invention has the following advantages:

[0099] 1. A new method and approach for estimating end-contact force are proposed;

[0100] 2. The method is easy to understand, and the algorithm's parameters are simple to adjust;

[0101] 3. The method is easy to implement, has strong applicability, and can achieve high-precision and robust end contact force estimation;

[0102] 4. The method estimates the external contact force based on the model, replacing the traditional sensor solution, which improves the stiffness of the dexterous hand and reduces the manufacturing cost of the dexterous hand. Attached Figure Description

[0103] Figure 1 A flowchart of the method for estimating the end-effector contact force of a tendon-driven manipulator provided by the present invention;

[0104] Figure 2 A flowchart for determining the state equations of a system provided by the present invention;

[0105] Figure 3 A flowchart of LU decomposition provided by the present invention. Detailed Implementation

[0106] This embodiment provides a method for estimating the contact force at the end effector of a tendon-driven manipulator. The main feature of this method is the use of an improved particle swarm optimization algorithm to achieve optimal estimation of the contact force while meeting accuracy requirements. This algorithm can be used for small- to medium-scale data regression problems, and in the context of dexterous hands, it can be used to estimate parameters such as end effector contact force.

[0107] Reference Figure 1-3 The method for estimating the contact force at the end of a tendon-driven manipulator provided in this embodiment includes the following steps:

[0108] Determine the system's state value based on the system's dynamic equations and the value to be estimated;

[0109] For the system's state values, construct the system's state equations;

[0110] Discretize the state equations of the system.

[0111] Particle filtering is applied to the discretized state equations to obtain the optimal estimates of the system state variables.

[0112] The specific steps are as follows:

[0113] 1) Based on the discretized state equations of the system, the Bayesian prediction and update equations are obtained:

[0114] 2) Based on the system's state-space model, the initial parameter samples of the state variables can be obtained using the Monte Carlo sampling method, which means the initial particle set can be obtained.

[0115] 3) Importance sampling. Particle updates are performed at each time step, updating the particle state using the state equation;

[0116] 4) Calculate the particle weights based on the observation Z at time k. k The weight of each particle can be updated using the likelihood probability density function;

[0117] 5) Normalize the weight of each particle;

[0118] 6) The weights of the particles are calculated recursively;

[0119] 7) Calculate the state variables at time k+1;

[0120] 8) Calculate the number of effective particles. If the number of effective particles does not meet the requirements, resample the particles.

[0121] 9) Through continuous iterative updates of steps 2) to 8) above, the optimal predicted value of the external torque of the system is obtained;

[0122] 10) Obtain the optimal estimate of the contact force through the Jacobian matrix.

[0123] Step 2) of the prediction mainly involves estimating the information at the current moment based on prior known information from the previous moment. The update is performed recursively based on the actual observed values ​​at the current moment to obtain the posterior probability of the value to be estimated at the current moment. The state variable contains all uncertainties; therefore, assuming the posterior probability P(X) at the previous moment... k-1 |Y 1:k-1 It is known that, based on Bayesian estimation theory, the posterior probability P(X) at the current time can be obtained. k |Y 1:k The prediction and update equations are as follows:

[0124] P(X k |Y 1:k-1 )=∫P(X k |Xk-1 )P(X k-1 |Y 1:k-1 )dX k-1

[0125]

[0126] P(X k |X k-1 P(Y) represents the probability of a state transition. 1:k |X k ) represents the likelihood probability;

[0127] In Bayesian computation, directly calculating the posterior probability density function involves complex integral calculations, which are difficult to solve for state estimation problems of nonlinear dynamic systems. Therefore, the Monte Carlo method based on random sampling is introduced to calculate the posterior probability.

[0128] Step 3) of the Monte Carlo method estimates the probability of the random problem by using the frequency of occurrence in a large number of trials. When the statistical data is large, the frequency of the random event can be considered to be infinitely close to the expected value, thus allowing the integral calculation in the Bayesian posterior probability to be approximated by the average return of a large number of sample points. The Monte Carlo method allows for large-scale sampling of the posterior probability density, thereby avoiding complex integral calculations. The specific sampling is represented as follows:

[0129]

[0130] Where δ(·) represents the Dirac function, Represents the estimated posterior probability;

[0131] Therefore, by estimating the posterior probability, the expected value of the state to be estimated can be obtained:

[0132]

[0133] Where f(·) represents the state equation. Let i represent the i-th particle in the particle set sampled using the Monte Carlo method.

[0134] Step 4) mainly involves calculating the particle's expected value to obtain the particle's weight. The specific steps for solving the particle weight are as follows:

[0135] The expected value of the particle was obtained through the Monte Carlo method. Therefore, in principle, the state value to be estimated can be obtained by averaging the posterior probabilities through sampling. However, the posterior probabilities are unknown, so importance sampling is needed to solve this problem, as shown below:

[0136] This represents the weight value of the i-th particle;

[0137] b) The importance sampling method specifically involves sampling from a known, sampleable distribution, such as q(X|Y). The expectation of the state variable to be estimated can then be expressed as:

[0138]

[0139] c)P(Y 1:k w(X) represents the probability of the observed value. k ) represents the particle weight, specifically:

[0140]

[0141] P(X k ) represents state X k The probability of step 5) is to normalize the particle weights obtained in step 4), specifically as follows:

[0142]

[0143] Combining steps c) and d) of step 4), we can obtain the expected value of the state equation:

[0144]

[0145] Step 6) solves the sampling problem through importance sampling in step 3). However, the weight of each particle is calculated separately during the sampling process, which is inefficient because the weight must be recalculated every time a sample is added. Therefore, to solve this problem, a recursive method for calculating particle weights is introduced.

[0146]

[0147] in, Let represent the posterior probability of the i-th particle at the current moment. Let P(X) represent the importance sampling density function of the i-th particle at the current moment. k |Y 1:k Let P(X) represent the posterior probability at the current time step. k-1 |Y 1:k-1 () represents the posterior probability at the previous time step. Represents the likelihood probability. This represents the probability of transitioning from the previous state to the current state. This represents the sampling density function indicating the importance of the i-th particle transitioning from its previous state to its current state. This represents the importance sampling density function of the i-th particle at the previous time step. The weight of the previous state of the i-th particle is represented by .

[0148] Step 7) involves normalizing the particle weights and then calculating the state variable value at time k:

[0149]

[0150] Step 8) involves a degradation problem during the filtering process described above. After several iterations, the weights of many particles become very small and negligible, with only a few particles having relatively large weights. Furthermore, the variance of the particle weights increases over time, resulting in a smaller number of effective particles in the state space. As the number of invalid sampled particles increases, a significant amount of computation is wasted on particles that have almost no effect on estimating the posterior filtering probability distribution, leading to a decline in estimation performance. Therefore, it is necessary to redefine the particle weights, i.e., resampling. The specific steps are as follows:

[0151] a) Define the effective number of particles, specifically as follows:

[0152]

[0153] b) When the number of effective particles is less than When this happens, resampling is required. During resampling, particles with higher weights are copied, while particles with lower weights are removed, ensuring the total number of particles is N. The specific number of times higher-weight particles are copied is determined by the importance probability density function. Decide:

[0154]

[0155] c) Substituting b) into step 6) yields the new weight update formula:

[0156]

[0157] Step 9) Repeat the above steps 3)-8) iteratively until the simulation of the algorithm is completed, and obtain the optimal estimate of the state variables;

[0158] Step 10) Obtain the optimal predicted value of the external torque of the system through step 9), and finally obtain the optimal estimated value of the contact force through the Jacobian matrix.

[0159] In this embodiment of the invention, a tendon-driven manipulator end-effector contact force estimation algorithm is used in the finger contact force control of the five-finger dexterous hand. This algorithm exhibits strong anti-interference capability, high accuracy, and excellent finger contact force control performance. Furthermore, it can reduce the impact from free space to constrained space, reduce tendon wear by constraining tendon tension, and improve tendon lifespan. Ultimately, it can effectively realize the operational control of the tendon-driven manipulator, improving the manipulator's dexterity and lifespan.

[0160] This invention has many specific applications. The above description is only a preferred embodiment of this invention. It should be noted that for those skilled in the art, any improvements made without departing from the principle of this invention should also be considered within the scope of protection of this invention.

Claims

1. A method for estimating the contact force at the end of a tendon-driven manipulator, characterized in that, Includes the following steps: Step 1: Based on the system's dynamic equations and the value of the end contact force to be estimated, determine the system's state value, and derive the system's state equations and measurement equations; Step 2: Discretize the state equation and measurement equation; Step 3: Based on the discretized state equation and measurement equation, and using the system's state-space model, the initial parameter samples of the state variables can be obtained using the Monte Carlo sampling method, which is the initial particle set of the system. Step 4: Set the termination condition for particle filtering, perform particle filtering, and obtain the optimal estimate of the system's state variables; the specific steps of particle filtering are as follows: Step 4.1: Importance sampling, the specific steps are as follows: Step 4.1.1: Sample from a known, sampleable distribution to obtain the expected value of the state variable to be estimated; Step 4.1.2: Calculate the weight value for each particle. ; Step 4.1.3: Normalize the weights of the particles; Step 4.2: Calculate particle weights recursively. The specific steps are as follows: Step 4.2.1: Solve for the particle weights using the recursive method. Specifically, it is expressed as: ； In the formula, Let represent the posterior probability of the i-th particle at the current moment. This represents the importance sampling density function of the i-th particle at the current moment. Let represent the posterior probability of the i-th particle at the previous time step. Represents the likelihood probability. This represents the probability of transitioning from the previous state to the current state. This represents the sampling density function indicating the importance of the i-th particle transitioning from its previous state to its current state. This represents the importance sampling density function of the i-th particle at the previous time step. The weight of the previous state of the i-th particle is represented by . Step 4.2.2: Normalize the particle weights calculated by the recursive formula; Step 4.2.3: Calculate the state to be estimated : ； In the formula Indicates the first The weight of each particle, Represents the state equation function; Step 4.3: Prevent particle degradation settings, the specific steps are as follows: Step 4.3.1: Define the effective number of particles, i.e. Specifically: ； Step 4.3.2: When the number of effective particles is less than When this happens, resampling is required. During resampling, particles with high weights are copied, while particles with low weights are removed, ensuring the total number of particles is [value missing]. After resampling, all posterior particles have the same weight, which is 1. The number of replications for particles with high specific weights is determined by the importance sampling density function. The decision is as follows: ； Step 4.4: Update the weight of each particle according to the new weight calculation formula; Step 4.5: Set the simulation time and repeat steps 2-4 to obtain the optimal estimate of the state variables, i.e. At this point, the optimal estimated value of the joint external torque is : ； In the formula express The 0 matrix, express The identity matrix, It is the number of degrees of freedom of the dexterous fingers. It is the dimension of the external torque, typically 6; Step 5: Calculate the optimal estimate of the contact force at the end of the dexterous hand based on the optimal estimate.

2. The method for estimating the contact force at the end of a tendon-driven manipulator according to claim 1, characterized in that, Step 1 determines the system's state values ​​as follows: system generalized momentum External torque on the joint Then, the system's state equation is calculated based on the system's dynamic equation.

3. The method for estimating the contact force at the end of a tendon-driven manipulator according to claim 1, characterized in that, The specific steps of step 2 are as follows: Step 2.1: Based on the system's state equation, calculate the parameter values ​​of the discretized state equation to obtain the discretized state equation; Step 2.2: Based on the system's measurement equations, calculate the parameter values ​​of the discretized measurement equations to obtain the discretized measurement equations; Step 2.3: Define the parameter values ​​in the system based on the discretized state equation and measurement equation.

4. The method for estimating the contact force at the end of a tendon-driven manipulator according to claim 1, characterized in that, The specific steps for obtaining the initial parameter samples of the state variables in step 3, based on the Monte Carlo sampling method, are as follows: Step 3.1: Based on Bayesian theory, obtain the Bayesian prediction equation and the Bayesian update equation; Step 3.2: Obtain the initial particles of the system based on the Monte Carlo method.

5. The method for estimating the contact force at the end of a tendon-driven manipulator according to claim 1, characterized in that, The specific steps for calculating the optimal estimate of the end contact force in step 5 are as follows: Step 5.1: According to Jacobian matrix at time This allows us to obtain the external force on the joint at that moment. The optimal estimate: ； In the formula This is the optimal estimate of the external torque on the joint; Step 5.2: Solve The inverse matrix is ​​obtained by the following steps: Step 5.2.1: [The text appears to be incomplete and contains several grammatical errors. A more accurate translation would require the full context.] Matrix operation The breakdown is as follows: ； In the formula It is an upper triangular matrix. It is a lower triangular matrix; Step 5.2.1: According to Solving the decomposition results The inverse matrix is ​​as follows: 。

Images