CN111158238B - Force feedback equipment dynamics parameter estimation algorithm based on particle swarm optimization - Google Patents

Force feedback equipment dynamics parameter estimation algorithm based on particle swarm optimization Download PDF

Info

Publication number
CN111158238B
CN111158238B CN202010019658.2A CN202010019658A CN111158238B CN 111158238 B CN111158238 B CN 111158238B CN 202010019658 A CN202010019658 A CN 202010019658A CN 111158238 B CN111158238 B CN 111158238B
Authority
CN
China
Prior art keywords
particle
joint
algorithm
force feedback
sampling
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010019658.2A
Other languages
Chinese (zh)
Other versions
CN111158238A (en
Inventor
李春泉
何永华
杨峰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanchang University
Original Assignee
Nanchang University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nanchang University filed Critical Nanchang University
Priority to CN202010019658.2A priority Critical patent/CN111158238B/en
Publication of CN111158238A publication Critical patent/CN111158238A/en
Application granted granted Critical
Publication of CN111158238B publication Critical patent/CN111158238B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Feedback Control In General (AREA)

Abstract

The invention provides a force feedback equipment dynamics parameter estimation algorithm based on a particle swarm algorithm. In particular to a dynamic parameter of a force feedback device better estimated by an improved comprehensive learning particle swarm optimization. The method mainly comprises the following steps: step 1, setting an ideal motion track of a joint angle of force feedback equipment; step 2, carrying out position tracking on the ideal motion track of the joint angle; step 3, sampling the joint angular motion track and the input torque; and 4, estimating the parameters of the force feedback equipment by using an ICLPSO algorithm. The present invention utilizes four strategies: the PSO algorithm comprises a hierarchical updating strategy, a position learning strategy, a guiding particle learning strategy and a global optimal learning strategy, and effectively solves the problems of 'two steps forward and one step backward' and the problem of precocity existing in the traditional PSO algorithm. Compared with the traditional PSO algorithm, the ICLPSO algorithm has the advantages that the convergence rate is higher, the obtained model parameters are more accurate, accurate moment estimation values can be provided for all joints, and the performance of the control algorithm is improved.

Description

Force feedback equipment dynamics parameter estimation algorithm based on particle swarm optimization
Technical Field
The invention relates to estimation of force feedback equipment parameters, in particular to a force feedback equipment dynamics parameter estimation algorithm based on a particle swarm algorithm.
Background
Since force touch has a significant position in human sense, researchers try to introduce force touch characteristics into the research fields of teleoperation robots, virtual reality and the like, and provide force touch feedback information for operators in the process of human-computer interaction. For example, when an accident occurs in a nuclear power station, tasks such as detection and maintenance need to be completed through a teleoperation robot, at the moment, video monitoring faces serious interference, and an operator can effectively improve the accuracy and reliability of operation by means of force touch feedback information. In the virtual operation, a doctor carries out real-time operation simulation training on a virtual human body, force touch feedback information is added, the training efficiency can be greatly improved, the training period is shortened, and a series of cost expenses are reduced. In addition, force haptics are also commonly used in deep sea operations, medical rehabilitation robots, minimally invasive surgery, and games and education to provide the operator with a realistic force and touch. The implementation of force haptics is dependent on the force feedback device, and therefore the performance of the force feedback device is particularly important. Generally speaking, the performance of a force feedback device is related to two factors. On one hand, the control algorithm is related to the selection of hardware such as a structure, a speed reducer, a sensor and the like, and on the other hand, the control algorithm is also a key factor influencing the performance of the control algorithm. The influence of factors such as self inertia, friction force, gravity and the like in the force feedback equipment can be eliminated through a control algorithm, and the transparency of the system and the reality of force feedback can be improved. Therefore, it is very meaningful to research the force feedback device and its related control algorithm, and it is a hot spot in research currently or in some future period.
The basic premise for studying force feedback device control algorithms is to analyze the kinematics and dynamics of the force feedback device. The accurate establishment of the dynamic model plays an important role in maintaining high-speed and high-precision operation of equipment, improving the performance of a controller and the like, and meanwhile, the design cost can be reduced, and the research time can be shortened. A joint dynamic model of the force feedback equipment is established, and dynamic parameters and friction parameters of the equipment need to be obtained, namely parameters such as joint mass, inertia tensor, mass center, coulomb friction force and viscous friction force of each joint are determined. However, in general, the force feedback device parameters are unknown or inaccurate, and the device is affected by factors such as abrasion, deformation and environment during long-term use, so that the parameters are deviated. Therefore, estimation of kinetic parameters is required. If the parameter estimation is more accurate, the accuracy of the dynamic model is higher, so that the estimation value of each joint moment is more accurate, and more accurate force feedback is provided for an operator.
The method for acquiring the force feedback equipment parameters comprises the following steps: the disintegration measurement method refers to a method for obtaining kinetic parameters of equipment by disintegrating the equipment, measuring geometrical parameters and material parameters of the equipment. The method can accurately acquire the mass, inertia tensor and mass center of the joint of the equipment. However, the operation is complex, the performability is low, the result of the parameter value is easily influenced by factors such as the shape and the material of the equipment, and the equipment can be damaged or cannot be restored in the process of disintegration; the CAD modeling method is based on computer graphics technology, and according to the structure diagram of the equipment, the parameters of the equipment are solved by using a corresponding dynamic model. Independent parameter values can be conveniently obtained, the parameter values of the force feedback equipment can be obtained in the design stage of the equipment, and then the dynamic characteristics of the robot are calculated according to the parameter values. However, the method is easily influenced by equipment manufacturing processes such as uneven material of the connecting rod and the like, so that certain errors exist in dynamic parameters; in addition to the above problems, the estimation method, whether it is a disassembly measurement method or a CAD modeling method, does not consider that the model parameters are biased due to the influence of factors such as abrasion, deformation and environment during the long-term use of the device. Honegger et al propose a method for online identification of kinetic parameters using a nonlinear Adaptive control algorithm (see: Adaptive control of the hexaglide, a 6dof parallel controllers [ C ] Proceedings of International Conference on Robotics and analysis. IEEE 1997,1: 543-. Because the particle swarm algorithm has the advantages of simplicity, feasibility and rapid convergence, the kinetic parameters are estimated by the Hossein Jahandideth et al by the particle swarm algorithm (see, for details, Use of PSO in Parameter Estimation of Robot Dynamics; Part One: No. New for Parameter Estimation [ C ]. International Conference on System theory. IEEE,2012.) in the method, each dimension of the particle represents the actual Parameter or the combination Parameter of the kinetic model, and the Parameter is estimated by taking the moment error as the target function according to the assumed kinetic model. But the estimated moment error is larger, the parameter estimation effect is poorer, and the local optimization is easier to fall into.
Disclosure of Invention
Based on the background, the invention provides a force feedback device dynamics parameter estimation algorithm based on a particle swarm algorithm. In particular to a method for better estimating dynamic parameters of a force feedback device by using an Improved Comprehensive Learning Particle Swarm Optimization (ICLPSO) algorithm. The invention is realized by the following technical scheme.
The invention relates to a force feedback equipment dynamics parameter estimation algorithm based on a particle swarm algorithm, which comprises the following steps:
the general force feedback device's equations of dynamics are detailed below:
Figure GDA0002770871000000031
Figure GDA0002770871000000032
wherein θ is a joint angle;
Figure GDA0002770871000000033
is the joint angular velocity;
Figure GDA0002770871000000034
is the angular acceleration of the joint; d is an inertia matrix of the force feedback equipment; c is a centrifugal force and Coriolis force matrix; g is a gravity matrix; fcIs a coulomb friction coefficient matrix; fvIs a viscous friction coefficient matrix; tau is joint moment; Δ X is a compensation term introduced to account for modeling uncertainty.
The parameters to be estimated are, in addition to the parameter X, a compensation parameter Δ X, as follows:
X＝[l1，l2,l3,ma，Iaxx，Iayy,Iazz，mbe，Ibexx,Ibeyy,Ibezz,mc,Icxx，Icyy,Iczz,l5,mdf，Idfxx,Idfyy,Idfzz,l6,Idf,Fc1,Fc2,Fc3,Fv1,Fv2,Fv3]
ΔX＝[a1,b1,a2,b2,a3,b3]
wherein: the model of the force feedback equipment is divided into seven sections A to G, and X is a kinetic parameter of the force feedback equipment; l1，l2Length of the B and A segments, respectively, l3Is the distance from the intersection of the two segments AB to the segment C, l5Is the distance from the centroid of the BE segment to the origin, l6Is the distance from the center of mass of the DF segment to the origin; m isa，mbe，mc,mdfThe quality of the A section, the BE section, the C section and the DF section respectively; i isaxx，Iayy,Iazz，Ibexx，Ibeyy,Ibezz,Icxx，Icyy,Iczz,Idfxx，Idfyy,IdfzzThe components of inertia tensor matrixes of the section A, the section BE, the section C and the section DF in three axes respectively; i isdfIs the inertia tensor matrix of the DF section; fc1,Fc2,Fc3Is the coulomb friction coefficient of the three joints; fv1,Fv2,Fv3Is the viscous friction coefficient of three joints, and Δ X is a compensation term introduced in consideration of modeling uncertainty; a is1,b1,a2,b2,a3,b3The coefficients of the three joint compensation equations.
Step 1: setting an ideal motion track of a joint angle of the force feedback equipment;
the force feedback device has N rotatable joints J1、J2…JNCorresponding N joint angles are theta1、θ2…θN. Setting ideal motion tracks of N joint angles as follows: angle theta1(t)、θ2(t)…θN(t), angular velocityDegree of rotation
Figure GDA0002770871000000035
Figure GDA0002770871000000036
Angular acceleration
Figure GDA0002770871000000037
Step 2: carrying out position tracking on the ideal motion track of the joint angle;
using a PID control algorithm to control N joints of the force feedback equipment to track the position of the ideal track to obtain the input torque tau of the N joints1、τ2…τNAnd angle theta1(t)、θ2(t)…θN(t) of (d). Angular velocity of each joint is obtained by utilizing a nonlinear tracking-differentiator
Figure GDA0002770871000000038
And angular acceleration
Figure GDA0002770871000000039
And step 3: sampling the joint angular motion track and the input torque;
for the motion track: angle theta of articulation1(t)、θ2(t)…θN(t), angular velocity of joint
Figure GDA0002770871000000041
Angular acceleration of joint
Figure GDA0002770871000000042
And the input joint moment tau1、τ2…τNTo carry out
Figure GDA0002770871000000043
And sampling, wherein T is sampling time, and P is the number of sampling points. Obtaining a sampling track: joint angle
Figure GDA0002770871000000044
Angular velocity of joint
Figure GDA0002770871000000045
And angular acceleration of joint
Figure GDA0002770871000000046
And moment of force
Figure GDA0002770871000000047
Figure GDA0002770871000000048
And 4, step 4: estimating force feedback equipment parameters by using an improved comprehensive learning particle swarm ICLPSO algorithm;
(a) the method comprises the following steps Parameters to be estimated
Figure GDA0002770871000000049
Substituting the angular motion track of the joint into a kinetic equation, and calculating to obtain the estimated joint moment, the moment of the ith joint and the moment of the p-th sampling
Figure GDA00027708710000000410
The calculation formula is as follows:
Figure GDA00027708710000000411
in the above formula, the first and second carbon atoms are,
Figure GDA00027708710000000412
the angle of the joint obtained by sampling for the ith joint and the p th time is obtained;
Figure GDA00027708710000000413
the joint angular velocity obtained by sampling for the ith joint and the p th time;
Figure GDA00027708710000000414
the joint angular acceleration obtained by sampling for the ith joint and the p th time; fcipCoulomb friction system obtained for ith joint and p th samplingCounting; fvipThe viscous friction coefficient obtained by sampling the ith joint and the p th time;
(b) the method comprises the following steps Defining an error matrix as calculating the measured joint moments tau and the estimated joint moments
Figure GDA00027708710000000415
Error between
Figure GDA00027708710000000416
Figure GDA00027708710000000417
The resulting error matrix E is:
Figure GDA00027708710000000418
defining an objective function:
f＝||E||2
(c) the method comprises the following steps Parameters of the ICLPSO algorithm are set. A particle dimension D; the number of particles Q; maximum number of iterations max _ iteration; maximum evaluation times max _ EFS; the maximum number of stalls max _ PST; particle search range of [ Hmin Hmax]；
Firstly, initializing parameters of ICLPSO algorithm, particle number Q, particle dimension D and search boundary [ Hmin Hmax]Iteration times iteration, evaluation times EFS, stagnation times PST of each particle, and random initialization of particle position H and velocity V; evaluating the positions of the particles to obtain the fitness value of each particle, updating the individual historical optimal position pb of each particle and the optimal position gb (global optimal position) found by the population in the whole searching process, and updating iteration and EFS;
updating the particle speed V and the particle position H by using a layered updating strategy, and setting a parameter M to be Q/2 and a parameter K to be Q/4; evaluating each particle to obtain a fitness value, updating pb and gb of each particle, and updating iteration and EFS;
if f (pb) of the particlei)＜f(S_pbM) The velocity of the particle is updated using the following formula
Figure GDA0002770871000000051
Otherwise, the velocity of the particle is updated using the following formula
Figure GDA0002770871000000052
Figure GDA0002770871000000053
Wherein,
Figure GDA0002770871000000054
is the d-dimensional velocity of the ith particle; w is the inertial weight, regulating the search capability on the solution space;
Figure GDA0002770871000000055
is the position of the guide particle of the ith particle in d dimension;
Figure GDA0002770871000000056
is the position of the ith particle in d dimension; gbdThe optimal position of the particle population found in d dimension; pbiIs the historical optimal location of the individual; s _ pb is the position of pb sorted from small to large according to the fitness value; g _ pb is the guide particle produced by pb; cn is the center of the first K positions in S _ pb; c. C1And c2Is an acceleration factor; r is1And r2Is a random number uniformly distributed between 0 and 1; cn (c)dThe first K values in S _ pb are at the center of the d-dimension position;
and fourthly, updating the particle position H by using a position learning strategy. Each particle has the ability to learn other particles, the probability of learning other particles is PLX is 0.25, if the random number r is 0-13Less than PLX, then in this dimension, the particle learns another particle;
using the guiding particle learning strategy to update the guiding particles. When the stagnation number PST of the particle reaches the set maximum stagnation number max _ PST, pb of one particle is randomly selected instead of the individual guide particle of the stagnant particle. In addition, the diversity of the population is lost in order to prevent premature aggregation of the particles together. When the pb of a particle is updated, the guide particle g _ pb will equal pb;
sixthly, using the global optimal learning strategy to optimize the gb. At each iteration, a dimension r is randomly selected4Let gb directly learn a random particle in pb
Figure GDA0002770871000000057
Seventhly, judging whether the algorithm meets the end condition, if the algorithm meets the end condition, finding the parameter which enables the target function f to be minimum, and determining the parameter at the moment
Figure GDA00027708710000000612
The algorithm is ended for the estimated optimal model parameters; if not, continuously jumping to the step 2 to continuously operate.
Compared with the prior art, the invention has the beneficial effects that:
the present invention utilizes four strategies: the PSO algorithm comprises a hierarchical updating strategy, a position learning strategy, a guiding particle learning strategy and a global optimal learning strategy, and effectively solves the problems of 'two steps forward and one step backward' and the problem of precocity existing in the traditional PSO algorithm. Compared with the traditional PSO algorithm, the algorithm has the advantages that the convergence speed is higher, the obtained model parameters are more accurate, accurate moment estimation values can be provided for all joints, and the performance of the control algorithm is improved.
Drawings
FIG. 1 is a flow chart of an improved ICLPSO algorithm for comprehensive learning particle swarm.
Detailed Description
The invention will be further illustrated by the following examples.
Step 1: setting an ideal motion track of a joint angle of the force feedback equipment;
the force feedback device has 3 rotatable joints J1、J2、J3Corresponding 3 joint anglesIs theta1、θ2、θ3. The ideal motion trajectories for setting 3 joint angles are respectively: angle theta1(t)、θ2(t)、θ3(t), angular velocity
Figure GDA0002770871000000061
Figure GDA0002770871000000062
Angular acceleration
Figure GDA0002770871000000063
The method comprises the following specific steps:
angle: theta1＝0.5sint θ2＝0.5sint θ3＝0.2sint
Angular velocity:
Figure GDA0002770871000000064
Figure GDA0002770871000000065
Figure GDA0002770871000000066
angular acceleration:
Figure GDA0002770871000000067
Figure GDA0002770871000000068
Figure GDA0002770871000000069
step 2: carrying out position tracking on the ideal motion track of the joint angle;
using a PID control algorithm to control 3 joints of the force feedback equipment to track the position of the ideal track to obtain the input torque tau of the 3 joints1、τ2、τ3And angle theta1(t)、θ2(t)、θ3(t) of (d). Angular velocity of each joint is obtained by utilizing a nonlinear tracking-differentiator
Figure GDA00027708710000000610
And angular acceleration
Figure GDA00027708710000000611
And step 3: sampling the joint angular motion track and the input torque;
for the motion track: angle theta of articulation1(t)、θ2(t)、θ3(t), angular velocity of joint
Figure GDA0002770871000000071
Angular acceleration of joint
Figure GDA0002770871000000072
And the input joint moment tau1、τ2、τ3To carry out
Figure GDA0002770871000000073
And sampling, wherein T is 30s, and P is 200. Obtaining a sampling track: joint angle
Figure GDA0002770871000000074
Angular velocity of joint
Figure GDA0002770871000000075
Figure GDA0002770871000000076
And angular acceleration of joint
Figure GDA0002770871000000077
And moment of force
Figure GDA0002770871000000078
And 4, step 4: the force feedback device parameters were estimated using an improved ensemble learning particle swarm (ICLPSO) algorithm.
(a) The method comprises the following steps Parameters to be estimated
Figure GDA00027708710000000717
Substituting the angular motion track of the joint into a kinetic equation, and calculating to obtain the estimated joint moment, the moment of the ith joint and the moment of the p-th sampling
Figure GDA00027708710000000718
The calculation formula is as follows:
Figure GDA0002770871000000079
in the above formula, the first and second carbon atoms are,
Figure GDA00027708710000000710
the angle of the joint obtained by sampling for the ith joint and the p th time is obtained;
Figure GDA00027708710000000711
the joint angular velocity obtained by sampling for the ith joint and the p th time;
Figure GDA00027708710000000712
for the ith joint, the p-th joint angular acceleration
(b) The method comprises the following steps Defining an error matrix as calculating the measured joint moments tau and the estimated joint moments
Figure GDA00027708710000000713
Error between
Figure GDA00027708710000000714
Figure GDA00027708710000000715
The resulting error matrix E is:
Figure GDA00027708710000000716
defining an objective function:
f＝||E||2
(c) the method comprises the following steps Parameters of the ICLPSO algorithm are set. ParticlesDimension D ═ 34; the number Q of particles is 34; the maximum number of iterations max _ iteration is 10000; the maximum evaluation number max _ EFS is 340000; the maximum number of times of stagnation max _ PST is 7; particle search range of Hmin＝0.5H、Hmax2H (when H is negative, the upper and lower boundaries are interchanged);
firstly, initializing parameters of ICLPSO algorithm, particle number Q, particle dimension D and search boundary [ Hmin Hmax]Iteration times iteration, evaluation times EFS, stagnation times PST of each particle, random initialization of particle position H and speed V, evaluation of the positions of the particles to obtain the fitness value of each particle, updating individual historical optimal positions pb of each particle and optimal positions gb (global optimal positions) found in the whole searching process by the population, and updating iteration and EFS;
and thirdly, updating the particle speed V by using a layered learning mode, and setting parameters M to Q/2 and K to Q/4. Updating the particle position H, evaluating each particle to obtain a fitness value, updating pb and gb of each particle, and updating iteration and EFS;
if f (pb) of the particlei)＜f(S_pbM) The velocity of the particle is updated using the following formula
Figure GDA0002770871000000081
Otherwise, the velocity of the particle is updated using the following formula
Figure GDA0002770871000000082
Figure GDA0002770871000000083
Wherein,
Figure GDA0002770871000000084
is the d-dimensional velocity of the ith particle; w is the inertial weight, regulating the search capability on the solution space;
Figure GDA0002770871000000085
is the position of the guide particle of the ith particle in d dimension;
Figure GDA0002770871000000086
is the position of the ith particle in d dimension; gbdThe optimal position of the particle population found in d dimension; pbiIs the historical optimal location of the individual; s _ pb is the position of pb sorted from small to large according to the fitness value; g _ pb is the guide particle produced by pb; cn is the center of the first K positions in S _ pb; c. C1And c2Is an acceleration factor; r is1And r2Is a random number uniformly distributed between 0 and 1; cn (c)dThe first K values in S _ pb are centered in the d-dimension position.
And fourthly, updating the particle position H by using a position learning strategy. Each particle has the ability to learn other particles, the probability of learning other particles is PLX is 0.25, if the random number r is 0-13Less than PLX, then in this dimension, the particle learns another particle;
using the guiding particle learning strategy to update the guiding particles. When the stagnation number PST of the particle reaches the set maximum stagnation number max _ PST, pb of one particle is randomly selected instead of the individual guide particle of the stagnant particle. In addition, the diversity of the population is lost in order to prevent premature aggregation of the particles together. When the pb of a particle is updated, the guide particle g _ pb will equal pb;
sixthly, using the global optimal learning strategy to optimize the gb. At each iteration, a dimension r is randomly selected4Let gb directly learn a random particle in pb
Figure GDA0002770871000000088
Seventhly, judging whether the algorithm meets the end condition, if the algorithm meets the end condition, finding the parameter which enables the target function f to be minimum, and determining the parameter at the moment
Figure GDA0002770871000000087
The algorithm is ended for the estimated optimal model parameters; if not, continuously jumping to the stepAnd step 2, continuing to operate.
The foregoing merely represents preferred embodiments of the invention, which are described in some detail and detail, and therefore should not be construed as limiting the scope of the invention. It should be noted that, for those skilled in the art, various changes, modifications and substitutions can be made without departing from the spirit of the present invention, and these are all within the scope of the present invention. Therefore, the protection scope of the present patent shall be subject to the appended claims.

Claims (1)

1. A force feedback device dynamics parameter estimation algorithm based on a particle swarm algorithm is characterized by comprising the following steps:
the general force feedback device's equations of dynamics are detailed below:
Figure FDA0002770870990000011
Figure FDA0002770870990000012
wherein θ is a joint angle;
Figure FDA0002770870990000013
is the joint angular velocity;
Figure FDA0002770870990000014
is the angular acceleration of the joint; d is an inertia matrix of the force feedback equipment; c is a centrifugal force and Coriolis force matrix; g is a gravity matrix; fcIs a coulomb friction coefficient matrix; fvIs a viscous friction coefficient matrix; tau is joint moment; Δ X is a compensation term introduced in view of modeling uncertainty;
the parameters to be estimated are, in addition to the parameter X, a compensation parameter Δ X, as follows:
X＝[l1，l2，l3，ma，Iaxx，Iayy，Iazz，mbe，Ibexx，Ibeyy，Ibezz，mc，Icxx，Icyy，Iczz，l5，mdf，
Idfxx，Idfyy，Idfzz，l6，Idf，Fc1，Fc2，Fc3，Fv1，Fv2，Fv3]
ΔX＝[a1，b1，a2，b2，a3，b3]
wherein: the model of the force feedback equipment is divided into seven sections A to G, and X is a kinetic parameter of the force feedback equipment; l1，l2Length of the B and A segments, respectively, l3Is the distance from the intersection of the two segments AB to the segment C, l5Is the distance from the centroid of the BE segment to the origin, l6Is the distance from the center of mass of the DF segment to the origin; m isa，mbe，mc，mdfThe quality of the A section, the BE section, the C section and the DF section respectively; i isaxx，Iayy，Iazz，Ibexx，Ibeyy，Ibezz，Icxx，Icyy，Iczz，Idfxx，Idfyy，IdfzzThe components of inertia tensor matrixes of the section A, the section BE, the section C and the section DF in three axes respectively; i isdfIs the inertia tensor matrix of the DF section; fd，Fc2，Fc3Is the coulomb friction coefficient of the three joints; fv1，Fv2，Fv3Is the viscous friction coefficient of three joints, and Δ X is a compensation term introduced in consideration of modeling uncertainty; a is1，b1，a2，b2，a3，b3Coefficients for three joint compensation equations;
step 1: setting an ideal motion track of a joint angle of the force feedback equipment;
the force feedback device has N rotatable joints J1、J2…JNCorresponding N joint angles are theta1、θ2…θN(ii) a Set N number of switchesThe ideal motion trajectories of the pitch angles are respectively: angle theta1(t)、θ2(t)…θN(t), angular velocity
Figure FDA0002770870990000015
Figure FDA0002770870990000016
Angular acceleration
Figure FDA0002770870990000017
Step 2: carrying out position tracking on the ideal motion track of the joint angle;
using a PID control algorithm to control N joints of the force feedback equipment to track the position of the ideal track to obtain the input torque tau of the N joints1、τ2…τNAnd angle theta1(t)、θ2(t)…θN(t) obtaining angular velocity of each joint by using a nonlinear tracking-differentiator
Figure FDA0002770870990000021
And angular acceleration
Figure FDA0002770870990000022
And step 3: sampling the joint angular motion track and the input torque;
for the motion track: angle theta of articulation1(t)、θ2(t)…θN(t), angular velocity of joint
Figure FDA0002770870990000023
Angular acceleration of joint
Figure FDA0002770870990000024
And the input joint moment tau1、τ2…τNTo carry out
Figure FDA0002770870990000025
Sampling, wherein T is sampling time, and P is the number of sampling points; obtaining a sampling track: joint angle
Figure FDA0002770870990000026
Angular velocity of joint
Figure FDA0002770870990000027
And angular acceleration of joint
Figure FDA0002770870990000028
And moment of force
Figure FDA0002770870990000029
Figure FDA00027708709900000210
And 4, step 4: estimating force feedback equipment parameters by using an improved comprehensive learning particle swarm ICLPSO algorithm;
(a) the method comprises the following steps Parameters to be estimated
Figure FDA00027708709900000211
Substituting the angular motion track of the joint into a kinetic equation, and calculating to obtain the estimated joint moment, the moment of the ith joint and the moment of the p-th sampling
Figure FDA00027708709900000212
The calculation formula is as follows:
Figure FDA00027708709900000213
in the above formula, the first and second carbon atoms are,
Figure FDA00027708709900000214
the angle of the joint obtained by sampling for the ith joint and the p th time is obtained;
Figure FDA00027708709900000215
the joint angular velocity obtained by sampling for the ith joint and the p th time;
Figure FDA00027708709900000216
the joint angular acceleration obtained by sampling for the ith joint and the p th time; fcipThe Coulomb friction coefficient is obtained for the ith joint and the p th sampling; fvipThe viscous friction coefficient obtained by sampling the ith joint and the p th time;
(b) the method comprises the following steps Defining an error matrix as calculating the measured joint moments tau and the estimated joint moments
Figure FDA00027708709900000220
Error between
Figure FDA00027708709900000217
Figure FDA00027708709900000218
The resulting error matrix E is:
Figure FDA00027708709900000219
defining an objective function:
f＝||E||2
(c) the method comprises the following steps Setting parameters of an ICLPSO algorithm: a particle dimension D; the number of particles Q; maximum number of iterations max _ iteration; maximum evaluation times max _ EFS; the maximum number of stalls max _ PST; particle search range of [ Hmin Hmax]；
Firstly, initializing parameters of ICLPSO algorithm, particle number Q, particle dimension D and search boundary [ Hmin Hmax]Iteration times iteration, evaluation times EFS, stagnation times PST of each particle, and random initialization of particle position H and velocity V;
evaluating the positions of the particles to obtain the fitness value of each particle, updating the individual historical optimal position pb of each particle and the optimal position gb found by the population in the whole searching process, and updating iteration and EFS;
updating the particle speed V and the particle position H by using a layered updating strategy, and setting a parameter M to be Q/2 and a parameter K to be Q/4; evaluating each particle to obtain a fitness value, updating pb and gb of each particle, and updating iteration and EFS;
if f (pb) of the particlei)＜f(S_pbM) The velocity of the particle is updated using the following formula
Figure FDA0002770870990000031
Otherwise, the velocity of the particle is updated using the following formula
Figure FDA0002770870990000032
Figure FDA0002770870990000033
Wherein,
Figure FDA0002770870990000034
is the d-dimensional velocity of the ith particle; w is the inertial weight, regulating the search capability on the solution space;
Figure FDA0002770870990000035
is the position of the guide particle of the ith particle in d dimension;
Figure FDA0002770870990000036
is the position of the ith particle in d dimension; gbdThe optimal position of the particle population found in d dimension; pbiIs the historical optimal position of an individual, S _ pb is the position of pb sorted from small to large according to fitness value, g _ pb is the guide particle generated by pb, cn is the center of the first K positions in S _ pb, c1And c2Is an acceleration factor, r1And r2Is 0 to1, random numbers uniformly distributed among the random numbers; cn (c)dThe first K values in S _ pb are at the center of the d-dimension position;
fourthly, updating the particle position H by using a position learning strategy; each particle has the ability to learn other particles, the probability of learning other particles is PLX is 0.25, if the random number r is 0-13Less than PLX, then in this dimension, the particle learns another particle;
updating the guide particles by using a guide particle learning strategy; randomly selecting pb of one particle to replace an individual guide particle of a stagnant particle when the stagnation number PST of the particle reaches a set maximum stagnation number max _ PST, and in addition, in order to prevent the particles from gathering together too early and losing the diversity of the population, when the pb of the particles is updated, the guide particle g _ pb is equal to pb;
sixthly, using a global optimal learning strategy to optimize the gb; at each iteration, a dimension r is randomly selected4Let gb directly learn a random particle in pb
Figure FDA0002770870990000041
Seventhly, judging whether the algorithm meets the end condition, if the algorithm meets the end condition, finding the parameter which enables the target function f to be minimum, and determining the parameter at the moment
Figure FDA0002770870990000042
The algorithm is ended for the estimated optimal model parameters; if not, continuously jumping to the step 2 to continuously operate.
CN202010019658.2A 2020-01-08 2020-01-08 Force feedback equipment dynamics parameter estimation algorithm based on particle swarm optimization Active CN111158238B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010019658.2A CN111158238B (en) 2020-01-08 2020-01-08 Force feedback equipment dynamics parameter estimation algorithm based on particle swarm optimization

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010019658.2A CN111158238B (en) 2020-01-08 2020-01-08 Force feedback equipment dynamics parameter estimation algorithm based on particle swarm optimization

Publications (2)

Publication Number Publication Date
CN111158238A CN111158238A (en) 2020-05-15
CN111158238B true CN111158238B (en) 2021-01-19

Family

ID=70562360

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010019658.2A Active CN111158238B (en) 2020-01-08 2020-01-08 Force feedback equipment dynamics parameter estimation algorithm based on particle swarm optimization

Country Status (1)

Country Link
CN (1) CN111158238B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112506054B (en) * 2020-11-27 2022-06-03 沈阳工业大学 Rehabilitation robot random finite time stable control based on SCN observation active thrust
CN115220342B (en) * 2022-07-06 2024-09-20 复旦大学 Motion control method for foot-type robot based on dynamic search

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5155423A (en) * 1986-02-18 1992-10-13 Robotics Research Corporation Industrial robot with servo
CN104570736A (en) * 2014-02-28 2015-04-29 中国科学院力学研究所 Kinetic parameter on-orbit identification method and device of satellite-arm coupling system
CN107498562A (en) * 2017-04-21 2017-12-22 浙江工业大学 Six-degree-of-freedom mechanical arm dynamics model identification method
CN107685343A (en) * 2017-08-28 2018-02-13 北京邮电大学 A kind of Mechanical transmission test parameter calibration configuration optimization method
CN110471274A (en) * 2019-08-12 2019-11-19 余姚市浙江大学机器人研究中心 Based on the machine components process line dispatching method for improving unified particle swarm algorithm

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5155423A (en) * 1986-02-18 1992-10-13 Robotics Research Corporation Industrial robot with servo
CN104570736A (en) * 2014-02-28 2015-04-29 中国科学院力学研究所 Kinetic parameter on-orbit identification method and device of satellite-arm coupling system
CN107498562A (en) * 2017-04-21 2017-12-22 浙江工业大学 Six-degree-of-freedom mechanical arm dynamics model identification method
CN107685343A (en) * 2017-08-28 2018-02-13 北京邮电大学 A kind of Mechanical transmission test parameter calibration configuration optimization method
CN110471274A (en) * 2019-08-12 2019-11-19 余姚市浙江大学机器人研究中心 Based on the machine components process line dispatching method for improving unified particle swarm algorithm

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
Comprehensive Learning Particle Swarm Optimisation with Limited Local Search for UAV Path Planning;Jing Liu，等;《2019 IEEE Symposium Series on Computational Intelligence (SSCI)》;20191209;第2287-2294页 *
Comprehensive Learning Particle Swarm Optimization Algorithm With Local Search for Multimodal Functions;Yulian Gao，等;《IEEE TRANSACTIONS ON EVOLUTIONARY COMPUTATION》;20190831;第23卷(第4期);第718-731页 *
力反馈设备运动学分析及轨迹规划;代逍遥，等;《机床与液压》;20170228;第45卷(第3期);第10-13页 *
基于粒子群算法的阻抗控制在机械臂柔顺控制中的应用;周晓东，等;《空间控制技术与应用》;20160630;第42卷(第3期);第15-20页 *
多目标全面学习粒子群算法在分布式电源选址定容中的应用与改进;赵娜，等;《陕西电力》;20171231;第45卷(第6期);第1-6页 *

Also Published As

Publication number Publication date
CN111158238A (en) 2020-05-15

Similar Documents

Publication Publication Date Title
CN108115681B (en) Simulation learning method and device for robot, robot and storage medium
Li et al. Robot skill acquisition in assembly process using deep reinforcement learning
Ijspeert et al. Movement imitation with nonlinear dynamical systems in humanoid robots
WO2020207219A1 (en) Non-model robot control method for multi-shaft-hole assembly optimized by environmental prediction
Su et al. Neural network enhanced robot tool identification and calibration for bilateral teleoperation
CN108621159A (en) A kind of Dynamic Modeling in Robotics method based on deep learning
WO2020216155A1 (en) Three-dimensional static modeling method of cable-driven continuous robotic arm
CN116460860B (en) Model-based robot offline reinforcement learning control method
CN111158238B (en) Force feedback equipment dynamics parameter estimation algorithm based on particle swarm optimization
CN111673733B (en) Intelligent self-adaptive compliance control method of robot in unknown environment
CN114417738B (en) Sparse IMU real-time human body motion capture and joint stress prediction method and system
WO2020118730A1 (en) Compliance control method and apparatus for robot, device, and storage medium
CN112975987A (en) Orthopedic surgery robot control method based on dynamic model
Ji et al. Model-based trajectory prediction and hitting velocity control for a new table tennis robot
Khadivar et al. Adaptive fingers coordination for robust grasp and in-hand manipulation under disturbances and unknown dynamics
Yan et al. Hierarchical policy learning with demonstration learning for robotic multiple peg-in-hole assembly tasks
Ma et al. A Human-Robot Collaboration Controller Utilizing Confidence for Disagreement Adjustment
Wochner et al. Learning with muscles: Benefits for data-efficiency and robustness in anthropomorphic tasks
Ren et al. Knowledge database-based multiobjective trajectory planning of 7-DOF manipulator with rapid and continuous response to uncertain fast-flying objects
CN112140098B (en) Underwater snake-shaped robot high-speed gait generation method based on near-end strategy optimization
WO2023020036A1 (en) Redundant manipulator tracking control method based on echo state network
CN110900608A (en) Robot kinematics calibration method based on optimal measurement configuration selection
Lin et al. The arm planning with dynamic movement primitive for humanoid service robot
WO2019095108A1 (en) Robot imitation learning method and apparatus, robot and storage medium
Men et al. Policy Fusion Transfer: the Knowledge Transfer for Different Robot Peg-in-Hole Insertion Assemblies

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant