CN113043283A - Robot tail end external force estimation method - Google Patents

Robot tail end external force estimation method Download PDF

Info

Publication number
CN113043283A
CN113043283A CN202110443328.0A CN202110443328A CN113043283A CN 113043283 A CN113043283 A CN 113043283A CN 202110443328 A CN202110443328 A CN 202110443328A CN 113043283 A CN113043283 A CN 113043283A
Authority
CN
China
Prior art keywords
robot
external force
ext
force
model
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.)
Granted
Application number
CN202110443328.0A
Other languages
Chinese (zh)
Other versions
CN113043283B (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.)
Jiangsu University of Technology
Original Assignee
Jiangsu University of Technology
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 Jiangsu University of Technology filed Critical Jiangsu University of Technology
Priority to CN202110443328.0A priority Critical patent/CN113043283B/en
Publication of CN113043283A publication Critical patent/CN113043283A/en
Application granted granted Critical
Publication of CN113043283B publication Critical patent/CN113043283B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture

Landscapes

  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Feedback Control In General (AREA)
  • Manipulator (AREA)

Abstract

The invention discloses a robot tail end external force estimation method, which adopts the technical scheme that: the method comprises the following steps: step1, establishing a terminal dynamic model of the interaction between the robot and the external force; step2, establishing a robot dynamic compensation error model; step3, establishing a robot terminal external force estimation model; step4, calibrating the external force estimation model parameters by adopting a particle swarm algorithm; according to the method for estimating the external force of the terminal powerless sensor of the robot, the optimal robot dynamics compensation error variance matrix, the optimal force variance matrix and the optimal robot body sensor information are optimized in an off-line mode based on the particle swarm optimization, the interaction force between the terminal of the robot and the environment is estimated in real time, and the force sensor is prevented from being introduced into a robot control system. The method has weak dependence on the robot dynamics precision model, is superior to the generalized momentum method of the external force estimation method depending on the model precision, does not need extra complicated robot dynamics model parameter calibration test, and improves the working efficiency.

Description

Robot tail end external force estimation method
Technical Field
The invention relates to the technical field of industrial robots, in particular to a robot tail end external force estimation method.
Background
The new generation of robot should have better environment self-adaptive ability, and can adopt a compliance control strategy to self-adaptively adjust the running state of the robot according to the received force information in the process of interacting with the environment so as to meet the environmental constraint. At present, in order to realize the safety of interaction between a robot and the environment, most robots realize compliant control by means of force sensors, so that the dynamic characteristics of the robots comply with the interactive force characteristics. However, the introduction of force sensors undoubtedly increases the structural difficulty of the robot control system and the manufacturing cost of the robot, and reduces the robustness of the system to the environment. Therefore, the research of estimating the interaction force between the robot and the environment based on the robot body sensor has great application value.
Numerous scholars have conducted extensive research into the accurate estimation of the interaction force between a robot and the environment. Cirillo et al cover sensitive material on the robot body as the skin of the robot, to directly measure the interaction force and the location of the contact. However, this method has not been widely adopted because the covering of sensitive materials on the robot body has not been completely accepted.
In addition, based on robot joint body sensor information, such as joint position, joint moment, and the like, Haddadin et al and Smith et al estimate the impact force between the robot and the environment using an observer. Luca and Mattone propose a generalized momentum method based on robot inertia and robot joint angular velocity to estimate robot collision moment. Tian et al and Lee et al estimate the friction torque between the joints of the robot by using a generalized momentum observer, but the generalized momentum method depends on an accurate model of the robot, and the accurate model of the robot is not suitable for practical acquisition. Therefore, a method for estimating the external force at the end of the robot with low cost and good accuracy is needed in the industry.
Disclosure of Invention
In view of the problems mentioned in the background art, the present invention is to provide a method for estimating an external force at a robot end, so as to solve the problems mentioned in the background art.
The technical purpose of the invention is realized by the following technical scheme:
a robot terminal external force estimation method comprises the following steps:
step1, establishing a terminal dynamic model of the interaction between the robot and the external force;
step2, establishing a robot dynamic compensation error model;
step3, establishing a robot terminal external force estimation model;
and Step4, calibrating the external force estimation model parameters by adopting a particle swarm algorithm.
Preferably, in Step3, the robot end receives an external force FextIncluding the interaction forces of the robot tip with the environment, including forces and moments, namely:
Fext＝[fext_x,fext_y,fext_z,τext_x,τext_y,τext_z]T；
wherein, FextFor expressing external force F borne by the tail end of the robot based on the robot base coordinate systemextAnd (4) matrix.
Preferably, in Step3, the robot end dynamic model is formed by a robot joint space dynamic model through an end Jacobian matrix Jc(q) converting to a robot joint space dynamic model:
Figure BDA0003035951950000021
wherein M iss(q) is a robot link inertia matrix;
Figure BDA0003035951950000022
is the centrifugal coriolis force vector; g (q) is the robot gravitational moment;
Figure BDA0003035951950000023
is the joint friction torque; tau iscIs a robot drive torque;
Figure BDA0003035951950000024
is FextEquivalent joint moments in the robot joint space; q, q,
Figure BDA0003035951950000025
Respectively the angular position, velocity and acceleration of the robot link.
Preferably, in Step1, the end dynamics of the robot and the external force are as follows:
Figure BDA0003035951950000026
wherein x is the terminal pose of the robot;
Figure BDA0003035951950000027
Figure BDA0003035951950000031
Figure BDA0003035951950000032
wherein, Λs(q) is an operating space inertia matrix;
Figure BDA0003035951950000033
is the friction in the operating space;
Figure BDA0003035951950000034
is the coriolis force in the operating space; fg(q) is the gravitational force in the operating space; fcIs a joint driving force in the operation space;
Figure BDA0003035951950000035
a weighted generalized inverse matrix of a robot terminal Jacobian matrix;
when the robot jacobian matrix is in a singular state or close to a singular state,
Figure BDA0003035951950000036
the damping least square method can be adopted to avoid singular states and ensure the continuity of the angular velocity of the joint of the robot.
Preferably, in Step2, the robot dynamics compensation error model is used for analyzing the influence of the robot end dynamics compensation error factor on external force estimation;
order to
Figure BDA0003035951950000037
Are respectively as
Figure BDA0003035951950000038
Fg(q) a kinetic feedforward compensation term, the robot drive torque F being under an external forcecCan be expressed as:
Figure BDA0003035951950000039
wherein, FresTo comprise FextThe residual effective force of (c).
Preferably, in Step2, because the robot dynamics model parameters cannot be accurately grasped in reality, the feedforward compensation term has an error relative to the actual condition, and the robot dynamics compensation error e isdynComprises the following steps:
edyn＝eΛU+ef+eg；
wherein the content of the first and second substances,
Figure BDA00030359519500000310
Figure BDA00030359519500000311
wherein eΛUCompensating errors for operating space inertial forces and coriolis forces; e.g. of the typefCompensating for errors in friction for operating space
；egCompensating errors for operating space gravity;
based on the terminal dynamics of the robot and the dynamics compensation error of the robot, obtaining an expression of the prediction influence of the robot dynamics compensation error on the robot external force, namely:
Fext＝edyn-Fres；
wherein, FextThe external force needs to be estimated; e.g. of the typedynCompensating for errors for dynamics; fresTo participate in efficacy;
the robot terminal external force prediction model is used for predicting the force of the robot terminal contacting the environment in real time based on the robot body sensor signals including the motor angle position, the motor angular speed and the motor torque under the condition of no force sensor.
Preferably, the robot dynamics compensate for the error edynAnd said external force FextThe method comprises the steps of enabling a robot to not keep a constant value constantly in the interaction process of the robot and the environment, and setting a dynamic compensation error e of the robotdynAnd said external force FextRandom variables obeying normal distribution and are independent of each other;
setting a robot dynamics compensation error expectation E [ E ]dyn]Force expectation E [ F ] 0ext]When the difference is 0, the robot dynamics compensates the error variance Var [ e ]dyn]＝ΩedynForce variance Var [ F ]ext]＝ΩFextAnd e is adynAnd FextHas a covariance of zero, i.e.
Figure BDA0003035951950000041
Based on the force variance ΩFextRobot dynamics compensation error variance omegaedynAnd residual effective force FresEstablishing a robot terminal external force estimation model, which comprises the following steps:
Figure BDA0003035951950000042
wherein the content of the first and second substances,
Figure BDA0003035951950000043
is said external force FextAn estimate of (2).
Preferably, if the robot dynamic model has deviation dfeObey a normal distribution, which expects E [ d ]fe]＝ΝbiaVariance Var [ d ]fe]＝ΩedynAnd the external force estimated by the robot terminal external force estimation model has deviation FbiaNamely:
Fbia＝ΩFext(ΩFext+Ωedyn)-1Nbia。
preferably, the external force estimation model parameters include a robot dynamics compensation error variance ΩedynMatrix and force variance ΩFextMatrix, calibrating external force estimation model parameters by using particle swarm optimization, comprising the following steps:
A. establishing robot operation space zero force control model
Figure BDA0003035951950000051
B. The robot end effector is dragged at a low speed by a hand to acquire the angle theta of the robot motor in real timei(t) angular velocity of Motor
Figure BDA0003035951950000052
Motor torque taum,i(t) and sensor force information Fext(t) optimizing omega off-line by adopting a particle swarm optimization algorithmFextAnd ΩedynSo that the external force F can be accurately estimated by the robot terminal external force estimation model based on the information of the robot body sensorext；
The robot operating space zero-force control model is obtained by converting a joint space robot zero-force control model through a tail end Jacobian matrix, and compensates the gravity and the joint friction force of the robot;
the robot connecting rod angle qi(t) and angular velocity
Figure BDA0003035951950000053
And the robot driving torque tauc,i(t) is the angle θ of the robot motori(t) and angular velocity
Figure BDA0003035951950000054
And the motor torque τm,i(t) reduction of the factor N by the servo systemratio,iThe conversion calculation results in:
qi(t)＝θi(t)/Nratio,i；
Figure BDA0003035951950000055
τc,i(t)＝τm,i(t)Nratio,i，i＝1,…,n；
wherein n is the degree of freedom of the robot joint.
Preferably, the particle swarm algorithm can avoid premature algorithm and fall into a local optimal value, wherein a fitness function of the particle swarm algorithm is defined as:
Figure BDA0003035951950000061
wherein the content of the first and second substances,
Figure BDA0003035951950000062
the predicted error for the external force,
Figure BDA0003035951950000063
Estimating the maximum singular value, z, of the matrix factor for external forces1And z2Are respectively an optimized weight value, and z1+z2＝1，z1＞0，z2＞0。
In summary, the invention mainly has the following beneficial effects:
according to the method for estimating the external force of the terminal powerless sensor of the robot, the optimal robot dynamics compensation error variance matrix, the optimal force variance matrix and the optimal robot body sensor information are optimized in an off-line mode based on the particle swarm optimization, the interaction force between the terminal of the robot and the environment is estimated in real time, the introduction of a robot control system by a force sensor is avoided, and the related defects brought to the robot system are overcome. On the other hand, the method has weak dependence on the robot dynamics precision model, is superior to the generalized momentum method of the external force estimation method relying on the model precision, does not need extra complicated robot dynamics model parameter calibration tests, and improves the working efficiency.
Drawings
FIG. 1 is a flow chart of a method of the present invention;
FIG. 2 is a diagram of the variance relationship of the external force estimation model of the present invention;
fig. 3 is a zero-force control block diagram of the robot in the invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Example 1
Referring to fig. 1 to 3, a method for estimating an external force at a robot end includes the following steps:
the method comprises the following steps:
step1, establishing a terminal dynamic model of the interaction between the robot and the external force;
step2, establishing a robot dynamic compensation error model;
step3, establishing a robot terminal external force estimation model;
and Step4, calibrating the external force estimation model parameters by adopting a particle swarm algorithm.
Preferably, in Step3, the robot end receives an external force FextIncluding the interaction forces of the robot tip with the environment, including forces and moments, namely:
Fext＝[fext_x,fext_y,fext_z,τext_x,τext_y,τext_z]T；
wherein, FextFor expressing external force F borne by the tail end of the robot based on the robot base coordinate systemextAnd (4) matrix.
Preferably, in Step3, the robot end dynamic model is formed by a robot joint space dynamic model through an end Jacobian matrix Jc(q) converting to a robot joint space dynamic model:
Figure BDA0003035951950000071
wherein M iss(q) is a robot link inertia matrix;
Figure BDA0003035951950000072
is the centrifugal coriolis force vector; g (q) is the robot gravitational moment;
Figure BDA0003035951950000073
is the joint friction torque; tau iscIs a robot drive torque;
Figure BDA0003035951950000074
is FextEquivalent joint moments in the robot joint space; q, q,
Figure BDA0003035951950000075
Respectively the angular position, velocity and acceleration of the robot link.
Preferably, in Step1, the end dynamics of the robot and the external force are as follows:
Figure BDA0003035951950000076
wherein the content of the first and second substances,xis the terminal pose of the robot;
Figure BDA0003035951950000077
Figure BDA0003035951950000078
Figure BDA0003035951950000079
wherein, Λs(q) is an operating space inertia matrix;
Figure BDA0003035951950000081
is the friction in the operating space;
Figure BDA0003035951950000082
is the coriolis force in the operating space; fg(q) is the gravitational force in the operating space; fcIs a joint driving force in the operation space;
Figure BDA0003035951950000083
a weighted generalized inverse matrix of a robot terminal Jacobian matrix;
when the robot jacobian matrix is in a singular state or close to a singular state,
Figure BDA0003035951950000084
the damping least square method can be adopted to avoid singular states and ensure the continuity of the angular velocity of the joint of the robot.
Preferably, in Step2, the robot dynamics compensation error model is used for analyzing the influence of the robot end dynamics compensation error factor on external force estimation;
order to
Figure BDA0003035951950000085
Are respectively as
Figure BDA0003035951950000086
Fg(q) a kinetic feedforward compensation term, the robot drive torque F being under an external forcecCan be expressed as:
Figure BDA0003035951950000087
wherein, FresTo comprise FextThe residual effective force of (c).
Preferably, in Step2, because the robot dynamics model parameters cannot be accurately grasped in reality, the feedforward compensation term has an error relative to the actual condition, and the robot dynamics compensation error e isdynComprises the following steps:
edyn＝eΛU+ef+eg；
wherein the content of the first and second substances,
Figure BDA0003035951950000088
Figure BDA0003035951950000089
wherein eΛUCompensating errors for operating space inertial forces and coriolis forces; e.g. of the typefCompensating for errors in friction for operating space
；egCompensating errors for operating space gravity;
based on the terminal dynamics of the robot and the dynamics compensation error of the robot, obtaining an expression of the prediction influence of the robot dynamics compensation error on the robot external force, namely:
Fext＝edyn-Fres；
wherein, FextThe external force needs to be estimated; e.g. of the typedynCompensating for errors for dynamics; fresTo participate in efficacy;
the robot terminal external force prediction model is used for predicting the force of the robot terminal contacting the environment in real time based on the robot body sensor signals including the motor angle position, the motor angular speed and the motor torque under the condition of no force sensor.
Preferably, the robot dynamics compensate for the error edynAnd said external force FextThe method comprises the steps of enabling a robot to not keep a constant value constantly in the interaction process of the robot and the environment, and setting a dynamic compensation error e of the robotdynAnd said external force FextRandom variables obeying normal distribution and are independent of each other;
setting a robot dynamics compensation error expectation E [ E ]dyn]Force expectation E [ F ] 0ext]When the difference is 0, the robot dynamics compensates the error variance Var [ e ]dyn]＝ΩedynForce variance Var [ F ]ext]＝ΩFextAnd e is adynAnd FextHas a covariance of zero, i.e.
Figure BDA0003035951950000091
Based on the force variance ΩFextMachine for the production of a plastic materialHuman dynamics compensation error variance omegaedynAnd residual effective force FresEstablishing a robot terminal external force estimation model, which comprises the following steps:
Figure BDA0003035951950000092
wherein the content of the first and second substances,
Figure BDA0003035951950000093
is said external force FextAn estimate of (2).
Preferably, if the robot dynamic model has deviation dfeObey a normal distribution, which expects E [ d ]fe]＝ΝbiaVariance Var [ d ]fe]＝ΩedynAnd the external force estimated by the robot terminal external force estimation model has deviation FbiaNamely:
Fbia＝ΩFext(ΩFext+Ωedyn)-1Nbia。
preferably, the external force estimation model parameters include a robot dynamics compensation error variance ΩedynMatrix and force variance ΩFextMatrix, calibrating external force estimation model parameters by using particle swarm optimization, comprising the following steps:
A. establishing robot operation space zero force control model
Figure BDA0003035951950000101
B. The robot end effector is dragged at a low speed by a hand to acquire the angle theta of the robot motor in real timei(t) angular velocity of Motor
Figure BDA0003035951950000102
Motor torque taum,i(t) and sensor force information Fext(t) optimizing omega off-line by adopting a particle swarm optimization algorithmFextAnd ΩedynSo that the external force F can be accurately estimated by the robot terminal external force estimation model based on the information of the robot body sensorext；
The robot operating space zero-force control model is obtained by converting a joint space robot zero-force control model through a tail end Jacobian matrix, and compensates the gravity and the joint friction force of the robot;
the robot connecting rod angle qi(t) and angular velocity
Figure BDA0003035951950000103
And the robot driving torque tauc,i(t) is the angle θ of the robot motori(t) and angular velocity
Figure BDA0003035951950000104
And the motor torque τm,i(t) reduction of the factor N by the servo systemratio,iThe conversion calculation results in:
qi(t)＝θi(t)/Nratio,i；
Figure BDA0003035951950000105
τc,i(t)＝τm,i(t)Nratio,i，i＝1,…,n；
wherein n is the degree of freedom of the robot joint.
Preferably, the particle swarm algorithm can avoid premature algorithm and fall into a local optimal value, wherein a fitness function of the particle swarm algorithm is defined as:
Figure BDA0003035951950000106
wherein the content of the first and second substances,
Figure BDA0003035951950000111
the predicted error for the external force,
Figure BDA0003035951950000112
Estimating the maximum singular value, z, of the matrix factor for external forces1And z2Are respectively an optimized weight value, and z1+z2＝1，z1＞0，z2＞0。
Referring to fig. 1 to 3, the method for estimating the external force of the terminal weak sensor of the robot optimizes an optimal robot dynamics compensation error variance matrix, a force variance matrix and robot body sensor information in an off-line manner based on a particle swarm optimization, estimates the interaction force between the terminal of the robot and the environment in real time, avoids introducing a robot control system into a force sensor, and overcomes the related defects brought to the robot system by the force sensor. On the other hand, the method has weak dependence on the robot dynamics precision model, is superior to the generalized momentum method of the external force estimation method relying on the model precision, does not need extra complicated robot dynamics model parameter calibration tests, and improves the working efficiency.
Example 2
The robot comprises a robot body, a teaching handle and a six-dimensional force sensor; the present embodiment includes the following schemes:
firstly, establishing a terminal dynamic model of interaction between a robot and an external force:
the external force that the robot receives is mainly the interactive force of robot and environment, including power and moment, promptly:
Fext＝[fext_x,fext_y,fext_z,τext_x,τext_y,τext_z]T；
wherein, FextIs expressed in a robot-based coordinate system, in this embodiment, FextThe true value is detected by the force sensor and follows a normal distribution.
The robot joint space dynamics equation can be expressed as:
Figure BDA0003035951950000113
wherein M iss(q) is a robot link inertia matrix;
Figure BDA0003035951950000114
is a centrifugal force departmentA force vector; g (q) is gravity moment;
Figure BDA0003035951950000115
is the joint friction torque; tau iscIs a robot drive torque; j. the design is a squarec(q) is a robot terminal Jacobian matrix;
Figure BDA0003035951950000121
is FextEquivalent joint moments in the robot joint space; q, q,
Figure BDA0003035951950000122
Respectively the angular position, velocity and acceleration of the robot link.
The end dynamics of the robot interaction force with the environment is represented as:
Figure BDA0003035951950000123
Figure BDA0003035951950000124
Figure BDA0003035951950000125
Figure BDA0003035951950000126
Figure BDA0003035951950000127
Figure BDA0003035951950000128
Figure BDA0003035951950000129
and in addition, when the Jacobian matrix of the robot is in a singular state or is close to the singular state, the singular state can be avoided by adopting a damping least square method, and the continuity of the angular velocity of the joint of the robot is ensured.
Secondly, establishing a robot dynamic compensation error model:
order to
Figure BDA00030359519500001210
Are respectively as
Figure BDA00030359519500001211
Fg(q) a kinetic feedforward compensation term for the robot drive moment F under the action of an external forcecCan be expressed as:
Figure BDA00030359519500001212
wherein, FresTo comprise FextThe residual effective force of (c).
Because the parameters of the robot dynamic model cannot be completely and accurately mastered in reality, the feedforward compensation item has errors relative to the reality, and the robot dynamic compensation error edynComprises the following steps:
edyn＝eΛU+ef+eg；
Figure BDA0003035951950000131
Figure BDA0003035951950000132
Figure BDA0003035951950000133
based on the robot terminal dynamics and the robot dynamics compensation error, an expression of the robot dynamics compensation error on the robot external force estimation influence is obtained, namely:
Fext＝edyn-Fres；
it should be noted that e is ignoreddynUnder the conditions of (A) Fext＝-Fres。
Thirdly, establishing a robot terminal external force estimation model:
compensating for errors e due to robot dynamicsdynAnd an external force FextThe constant value can not be kept all the time in the interaction process of the robot and the environment, so the dynamic compensation error e of the robot is setdynAnd an external force FextAnd (4) following a normally distributed random variable, wherein the random variable and the random variable are independent. Setting a robot dynamics compensation error expectation E [ E ]dyn]Force expectation E [ F ] 0ext]When the difference is 0, the robot dynamics compensates the error variance Var [ e ]dyn]＝ΩedynForce variance Var [ F ]ext]＝ΩFextAnd e is adynAnd FextHas a covariance of zero, i.e.
Figure BDA0003035951950000134
Set true value FextAnd an estimate
Figure BDA0003035951950000135
An error of
Figure BDA0003035951950000136
Seeking a calibration matrix Ψ such that the variance matrix Var [ Δ F ]ext]The sum of the elements being minimal, i.e.
Figure BDA0003035951950000137
Then Δ FextRedefined as:
Figure BDA0003035951950000138
ΔFext＝Fext-ΨFres；
then Var [ Delta F ]ext]Expressed as:
Var[ΔFext]＝E[(Fext-ΨFres)(Fext-ΨFres)T]＝ΩFext+ΩFextΨT+ΨΩFext+ΨΩFextΨT+ΨΩedynΨT；
then
Figure BDA0003035951950000141
Order to
Figure BDA0003035951950000142
The calibration matrix is then:
Ψ＝-ΩFext(ΩFext+Ωedyn)-1。
based on the force variance omegaFextRobot dynamics compensation error variance omegaedynAnd residual effective force FresEstablishing a robot terminal external force estimation model, comprising the following steps:
Figure BDA0003035951950000143
as can be seen from fig. 2 and 3, the relative ΩFextIn terms of Ψ vs ΩedynIs relatively sensitive with omegaedynThe increase in the value of the element(s),
Figure BDA0003035951950000144
with gradual loss of residual effectiveness F of the calibration matrix psiresAnd (5) carrying out effectiveness of external force estimation. When omega is higher thanedynC ≠ 0, C is a constant value,
Figure BDA0003035951950000145
relative omegaFextSlowly changing and when omegaedynThe robot feed-forward control can accurately compensate the state variables, which is 0, psi-I,
Figure BDA0003035951950000146
if omegaFextWhen it is equal to I, FextFollowing a standard normal distribution.
If expectation of robot model error Edyn]Not equal to 0, this will result in the final
Figure BDA0003035951950000147
Not approximated by a true value FextThe expected values are:
E[edyn]＝E[Fres]；
set deviation dfeIs generated under the condition of inaccurate robot model, Edfe]＝Νbia，Var[dfe]＝Ωedyn. Therefore, the estimated external force error Δ F 'at this time'extRelative to E [ E ]dyn]When the case is 0, it becomes:
ΔF′ext＝Fext-Ψ(dfe-Fext)＝(I+Ψ)Fext-Ψdfe；
then the corresponding variance Var [ Delta F'ext]Comprises the following steps:
Var[ΔF′ext]＝(I+Ψ)ΩFext(I+Ψ)T-ΨΩedynΨT；
bringing into the calibration matrix Ψ, yields:
Figure BDA0003035951950000151
based on external force FextAnd a force error expectation E [ d ] associated with the model errorfe]＝Nbia，ΔF′extNew deviation FbiaCan be expressed as:
Fbia＝E[ΔF′ext]＝ΩFext(ΩFext+Ωedyn)-1Nbia；
if omegaFext＝I，ΩedynWhen the robot dynamics model and the model deviation can be accurately obtained, F, 0bia＝NbiaI.e. the expectation of the deviation of the external force estimate FbiaExpectation N equal to equivalent force corresponding to robot model biasbia。
Fourthly, calibrating external force estimation model parameters by adopting a particle swarm algorithm:
establishing a robot operation space zero-force control model, comprising the following steps:
Figure BDA0003035951950000152
the robot zero-force control block diagram is shown in fig. 3. The gravity and the joint friction of the robot are compensated, and the robot can be easily pushed by external force under the zero-force state.
Under the condition of hand dragging, the robot can traverse various configuration configurations as much as possible, and simultaneously acquire six-dimensional force sensor data feedback information F in real timeextInformation q of joint angle, information q of joint angular velocity
Figure BDA0003035951950000153
And joint torque information τc。
Wherein, the angle q of the connecting rod of the roboti(t) and angular velocity
Figure BDA0003035951950000154
And robot driving torque tauc,i(t) is the angle θ of the robot motori(t) and angular velocity
Figure BDA0003035951950000155
And motor torque τm,i(t) reduction of the factor N by the servo systemratio,iThe conversion calculation results in:
qi(t)＝θi(t)/Nratio,i；
Figure BDA0003035951950000161
τc,i(t)＝τm,i(t)Nratio,i，i＝1,…,7。
the particle swarm algorithm fitness function is established as follows:
seeking optimal omegaFextAnd ΩedynSo as to be at the same time
Figure BDA0003035951950000162
And FdefThe modulus of the difference of (a) is minimal, i.e.:
Figure BDA0003035951950000163
in addition, the robot model bias equivalent power expectation NbiaMay exist, but in practice, NbiaIs unknown, so that if the expectation F of the deviation of the external force estimate is directly optimizedbiaIs not feasible. However, since matrix singular value decomposition characterizes the degree of feature variation between a matrix and a vector, a is defined as ΩFext(ΩFext+Ωedyn)-1，Α＝UΑSΑVΑ，UΑ∈Rn×n，SΑ∈Rn×n，VΑ∈Rn×nA left singular vector matrix, a singular value matrix and a right singular vector matrix of A, respectively, wherein
Figure BDA0003035951950000164
Is the maximum singular value
Figure BDA0003035951950000165
The corresponding feature changes direction. If it is
Figure BDA0003035951950000166
The larger the amplitude is, the corresponding FbiaThe more easily the output cartesian direction is subjected to NbiaInfluence. Therefore, it is necessary to limit the maximum singular value of a
Figure BDA0003035951950000167
Thereby in NbiaIs optimized to a, maintaining its optimal isotropy, i.e.:
Figure BDA0003035951950000168
in summary, by combining the two optimization objectives, a comprehensive index is obtained as a fitness function of the particle swarm algorithm, that is:
Figure BDA0003035951950000169
wherein the content of the first and second substances,
Figure BDA00030359519500001610
z1and z2Respectively, is an optimized weight value, z1＝0.5，z2＝0.5。
Wherein the particle swarm algorithm is iteratively updated:
in the particle swarm optimization, the position of the ith particle
Figure BDA0003035951950000171
And velocity
Figure BDA0003035951950000172
According to the optimal position of the particle itself
Figure BDA0003035951950000173
And global optimal position
Figure BDA0003035951950000174
Under the comparison of the fitness function f, the automatic iterative updating is carried out, and the updating formula is as follows:
Figure BDA0003035951950000175
Figure BDA0003035951950000176
wherein, TpsoIs the maximum overlapGeneration times; dpsoIs the maximum dimension number; n is a radical ofpsoIs the maximum number of particles;
Figure BDA0003035951950000177
the position and the speed of the ith particle in the d dimension of the t iteration are respectively;
Figure BDA0003035951950000178
the d-dimension position of the ith particle and the d-dimension position of the global optimal particle are respectively the optimal position of the ith particle; c. C1And c2Respectively, an algorithm learning factor, frequently take c1＝c2＝2；r1And r2Is [0,1 ]]The normal distribution random variable of (1); omegapsoIs an algorithm inertia weight factor.
In addition, the chaos mapping is adopted to increase the diversity of population initialization, namely:
Figure BDA0003035951950000179
ci∈[-1,0)∪(0,1]；
wherein the content of the first and second substances,
Figure BDA00030359519500001710
is the d-dimension chaotic variable of the ith chaotic sequence.
After the chaos initializes the population, the chaos sequence needs to be converted into the particle position of the particle swarm at the initialization time, namely:
Figure BDA00030359519500001711
wherein the content of the first and second substances,
Figure BDA00030359519500001713
and
Figure BDA00030359519500001712
a lower boundary and an upper boundary respectively searched for the d-dimension particle position.
To effectively balance the algorithmThe local development and local search capability is realized, the particle swarm algorithm adopts an exponential inertia weight method, and the inertia weight omega is updated in an iterative mannerpsoNamely:
Figure BDA0003035951950000181
wherein the content of the first and second substances,
Figure BDA0003035951950000182
maximum and minimum inertia weight factors, respectively.
The particle swarm algorithm adopts the variance of the population fitness value to judge the algorithm earliness, and a disturbance mechanism is added, so that local minimum value points of the algorithm are skipped. The variance of the fitness value is expressed as:
Figure BDA0003035951950000183
in the formula (I), the compound is shown in the specification,
Figure BDA0003035951950000184
is the average value of the fitness of the current iteration population; f. ofn＝max(1,max(|fi-favg|)) is a population fitness normalization value.
When rho2≤[ρ2]Then the algorithm has already fallen into a local optimum and the global perturbation mechanism takes effect. Generating N based on chaotic mapping and chaotic mappingrParticles of NrAnd (5) substituting the N into the particle swarm which is already in the premature state, and iterating, updating and optimizing again.
For offline optimization to obtain optimal robot dynamics compensation error variance omegaedynMatrix and force variance ΩFextAnd the matrix can execute multiple external force estimation model parameter calibration tests. Robot dynamics compensation error variance omega based on calibrationedynMatrix and force variance ΩFextMatrix, the robot can pre-estimate the external force F interacting with the environment in real time based on the signal of the robot body sensor in the subsequent interaction process with the environmentext。
Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that changes, modifications, substitutions and alterations can be made in these embodiments without departing from the principles and spirit of the invention, the scope of which is defined in the appended claims and their equivalents.

Claims (10)

1. A robot terminal external force estimation method is characterized by comprising the following steps: the method comprises the following steps:
step1, establishing a terminal dynamic model of the interaction between the robot and the external force;
step2, establishing a robot dynamic compensation error model;
step3, establishing a robot terminal external force estimation model;
and Step4, calibrating the external force estimation model parameters by adopting a particle swarm algorithm.
2. The method for estimating the external force at the tail end of the robot as claimed in claim 1, wherein: in Step3, the robot end receives an external force FextIncluding the interaction forces of the robot tip with the environment, including forces and moments, namely:
Fext＝[fext_x,fext_y,fext_z,τext_x,τext_y,τext_z]T；
wherein, FextFor expressing external force F borne by the tail end of the robot based on the robot base coordinate systemextAnd (4) matrix.
3. The method for estimating the external force at the tail end of the robot as claimed in claim 1, wherein: in the Step3, the robot tail end dynamic model is formed by a robot joint space dynamic model through a tail end Jacobian matrix Jc(q) converting to a robot joint space dynamic model:
Figure FDA0003035951940000011
wherein M iss(q) is a robot link inertia matrix;
Figure FDA0003035951940000012
is the centrifugal coriolis force vector; g (q) is the robot gravitational moment;
Figure FDA0003035951940000013
is the joint friction torque; tau iscIs a robot drive torque;
Figure FDA0003035951940000014
is FextEquivalent joint moments in the robot joint space; q, q,
Figure FDA0003035951940000015
Respectively the angular position, velocity and acceleration of the robot link.
4. The method for estimating the external force at the tail end of the robot as claimed in claim 1, wherein: in Step1, the end dynamics of the robot and the external force are as follows:
Figure FDA0003035951940000016
wherein x is the terminal pose of the robot;
Figure FDA0003035951940000021
Figure FDA0003035951940000022
Figure FDA0003035951940000023
wherein, Λs(q) is an operating space inertia matrix;
Figure FDA0003035951940000024
is the friction in the operating space;
Figure FDA0003035951940000025
is the coriolis force in the operating space; fg(q) is the gravitational force in the operating space; fcIs a joint driving force in the operation space;
Figure FDA0003035951940000026
a weighted generalized inverse matrix of a robot terminal Jacobian matrix;
when the robot jacobian matrix is in a singular state or close to a singular state,
Figure FDA0003035951940000027
the damping least square method can be adopted to avoid singular states and ensure the continuity of the angular velocity of the joint of the robot.
5. The method for estimating the external force at the tail end of the robot as claimed in claim 1, wherein: in Step2, the robot dynamics compensation error model is used for analyzing the influence of the robot tail end dynamics compensation error factor on external force estimation;
order to
Figure FDA0003035951940000028
Are respectively as
Figure FDA0003035951940000029
Figure FDA00030359519400000210
Fg(q) a kinetic feedforward compensation term, the robot drive torque F being under an external forcecCan be expressed as:
Figure FDA00030359519400000211
wherein, FresTo comprise FextThe residual effective force of (c).
6. The method for estimating the external force at the tail end of the robot as claimed in claim 1, wherein: in Step2, because the robot dynamics model parameters cannot be accurately mastered in reality, the feedforward compensation term has an error relative to the reality, and the robot dynamics compensation error edynComprises the following steps:
edyn＝eΛU+ef+eg；
wherein the content of the first and second substances,
Figure FDA0003035951940000031
Figure FDA0003035951940000032
wherein eΛUCompensating errors for operating space inertial forces and coriolis forces; e.g. of the typefCompensating for errors in operating space friction; e.g. of the typegCompensating errors for operating space gravity;
based on the terminal dynamics of the robot and the dynamics compensation error of the robot, obtaining an expression of the prediction influence of the robot dynamics compensation error on the robot external force, namely:
Fext＝edyn-Fres；
wherein, FextThe external force needs to be estimated; e.g. of the typedynCompensating for errors for dynamics; fresTo participate in efficacy;
the robot terminal external force prediction model is used for predicting the force of the robot terminal contacting the environment in real time based on the robot body sensor signals including the motor angle position, the motor angular speed and the motor torque under the condition of no force sensor.
7. The method for estimating the external force at the tail end of the robot as claimed in claim 6, wherein: the robot dynamics compensate for error edynAnd saidExternal force FextThe method comprises the steps of enabling a robot to not keep a constant value constantly in the interaction process of the robot and the environment, and setting a dynamic compensation error e of the robotdynAnd said external force FextRandom variables obeying normal distribution and are independent of each other;
setting a robot dynamics compensation error expectation E [ E ]dyn]Force expectation E [ F ] 0ext]When the difference is 0, the robot dynamics compensates the error variance Var [ e ]dyn]＝ΩedynForce variance Var [ F ]ext]＝ΩFextAnd e is adynAnd FextHas a covariance of zero, i.e.
Figure FDA0003035951940000033
Based on the force variance ΩFextRobot dynamics compensation error variance omegaedynAnd residual effective force FresEstablishing a robot terminal external force estimation model, which comprises the following steps:
Figure FDA0003035951940000041
wherein the content of the first and second substances,
Figure FDA0003035951940000042
is said external force FextAn estimate of (2).
8. The method for estimating the external force at the tail end of the robot as claimed in claim 7, wherein: if the robot power model has deviation dfeObey a normal distribution, which expects E [ d ]fe]＝ΝbiaVariance Var [ d ]fe]＝ΩedynAnd the external force estimated by the robot terminal external force estimation model has deviation FbiaNamely:
Fbia＝ΩFext(ΩFext+Ωedyn)-1Nbia。
9. according to claim 1The robot terminal external force estimation method is characterized by comprising the following steps: the external force estimation model parameters comprise the robot dynamics compensation error variance omegaedynMatrix and force variance ΩFextMatrix, calibrating external force estimation model parameters by using particle swarm optimization, comprising the following steps:
A. establishing robot operation space zero force control model
Figure FDA0003035951940000043
B. The robot end effector is dragged at a low speed by a hand to acquire the angle theta of the robot motor in real timei(t) angular velocity of Motor
Figure FDA0003035951940000044
Motor torque taum,i(t) and sensor force information Fext(t) optimizing omega off-line by adopting a particle swarm optimization algorithmFextAnd ΩedynSo that the external force F can be accurately estimated by the robot terminal external force estimation model based on the information of the robot body sensorext；
The robot operating space zero-force control model is obtained by converting a joint space robot zero-force control model through a tail end Jacobian matrix, and compensates the gravity and the joint friction force of the robot;
the robot connecting rod angle qi(t) and angular velocity
Figure FDA0003035951940000045
And the robot driving torque tauc,i(t) is the angle θ of the robot motori(t) and angular velocity
Figure FDA0003035951940000046
And the motor torque τm,i(t) reduction of the factor N by the servo systemratio,iThe conversion calculation results in:
qi(t)＝θi(t)/Nratio,i；
Figure FDA0003035951940000051
τc,i(t)＝τm,i(t)Nratio,i，i＝1,…,n；
wherein n is the degree of freedom of the robot joint.
10. The method for estimating the external force at the tail end of the robot as claimed in claim 9, wherein: the particle swarm algorithm can avoid the algorithm from being premature and falling into a local optimal value, wherein the fitness function of the particle swarm algorithm is defined as:
Figure FDA0003035951940000052
wherein the content of the first and second substances,
Figure FDA0003035951940000053
the predicted error for the external force,
Figure FDA0003035951940000054
Estimating the maximum singular value, z, of the matrix factor for external forces1And z2Are respectively an optimized weight value, and z1+z2＝1，z1＞0，z2＞0。
CN202110443328.0A 2021-04-23 2021-04-23 Robot tail end external force estimation method Active CN113043283B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110443328.0A CN113043283B (en) 2021-04-23 2021-04-23 Robot tail end external force estimation method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110443328.0A CN113043283B (en) 2021-04-23 2021-04-23 Robot tail end external force estimation method

Publications (2)

Publication Number Publication Date
CN113043283A true CN113043283A (en) 2021-06-29
CN113043283B CN113043283B (en) 2022-07-08

Family

ID=76520179

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110443328.0A Active CN113043283B (en) 2021-04-23 2021-04-23 Robot tail end external force estimation method

Country Status (1)

Country Link
CN (1) CN113043283B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113910244A (en) * 2021-11-15 2022-01-11 武汉联影智融医疗科技有限公司 Mechanical arm dragging hybrid control method based on moment feedforward for neurosurgery

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110060460A1 (en) * 2008-10-31 2011-03-10 Kabushiki Kaisha Toshiba Robot control apparatus
CN104517297A (en) * 2013-09-28 2015-04-15 沈阳新松机器人自动化股份有限公司 Robot calibrate method based on particle swarm optimization
CN105258839A (en) * 2015-10-30 2016-01-20 南京信息工程大学 Array type air pressure measurement compensation device and method based on quantum particle swarm wavelet neural network
CN205978275U (en) * 2016-08-04 2017-02-22 唐山港集团股份有限公司 Bridge type ship unloaders damping cable device
CN107016208A (en) * 2017-04-17 2017-08-04 珞石（北京）科技有限公司 A kind of industrial robot external force method of estimation based on shake control
CN107590340A (en) * 2017-09-15 2018-01-16 浙江大学 A kind of mechanical arm external force method of estimation and device
CN108015774A (en) * 2017-12-15 2018-05-11 北京艾利特科技有限公司 A kind of sensorless mechanical arm collision checking method
US20200061835A1 (en) * 2017-05-29 2020-02-27 Franka Emika Gmbh Collision handling by a robot
WO2020101516A1 (en) * 2018-11-12 2020-05-22 Obshchestvo S Ogranichennoy Otvetstvennostyu "Tra Robotics" Sensor-free force/torque sensing in an articulated electromechanical actuator-driven robot
CN111272334A (en) * 2020-02-18 2020-06-12 金陵科技学院 Particle swarm optimization BP neural network-based multidimensional force sensor calibration decoupling method
CN112043384A (en) * 2020-07-29 2020-12-08 上海大学 External force prediction method of fracture reduction robot

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110060460A1 (en) * 2008-10-31 2011-03-10 Kabushiki Kaisha Toshiba Robot control apparatus
CN104517297A (en) * 2013-09-28 2015-04-15 沈阳新松机器人自动化股份有限公司 Robot calibrate method based on particle swarm optimization
CN105258839A (en) * 2015-10-30 2016-01-20 南京信息工程大学 Array type air pressure measurement compensation device and method based on quantum particle swarm wavelet neural network
CN205978275U (en) * 2016-08-04 2017-02-22 唐山港集团股份有限公司 Bridge type ship unloaders damping cable device
CN107016208A (en) * 2017-04-17 2017-08-04 珞石（北京）科技有限公司 A kind of industrial robot external force method of estimation based on shake control
US20200061835A1 (en) * 2017-05-29 2020-02-27 Franka Emika Gmbh Collision handling by a robot
CN107590340A (en) * 2017-09-15 2018-01-16 浙江大学 A kind of mechanical arm external force method of estimation and device
CN108015774A (en) * 2017-12-15 2018-05-11 北京艾利特科技有限公司 A kind of sensorless mechanical arm collision checking method
WO2020101516A1 (en) * 2018-11-12 2020-05-22 Obshchestvo S Ogranichennoy Otvetstvennostyu "Tra Robotics" Sensor-free force/torque sensing in an articulated electromechanical actuator-driven robot
CN111272334A (en) * 2020-02-18 2020-06-12 金陵科技学院 Particle swarm optimization BP neural network-based multidimensional force sensor calibration decoupling method
CN112043384A (en) * 2020-07-29 2020-12-08 上海大学 External force prediction method of fracture reduction robot

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113910244A (en) * 2021-11-15 2022-01-11 武汉联影智融医疗科技有限公司 Mechanical arm dragging hybrid control method based on moment feedforward for neurosurgery
CN113910244B (en) * 2021-11-15 2022-12-20 武汉联影智融医疗科技有限公司 Mechanical arm dragging hybrid control method based on moment feedforward for neurosurgery

Also Published As

Publication number Publication date
CN113043283B (en) 2022-07-08

Similar Documents

Publication Publication Date Title
Lippiello et al. Cartesian impedance control of a UAV with a robotic arm
Liang et al. Adaptive task-space tracking control of robots without task-space-and joint-space-velocity measurements
CN113748597B (en) Motor control device
CN108555914B (en) DNN neural network self-adaptive control method based on tendon-driven dexterous hand
CN112743541A (en) Soft floating control method for mechanical arm of powerless/torque sensor
Wilson et al. Dynamic task execution using active parameter identification with the Baxter research robot
Andaluz et al. Robust control with redundancy resolution and dynamic compensation for mobile manipulators
Xing et al. PDE model-based state-feedback control of constrained moving vehicle-mounted flexible manipulator with prescribed performance
Deutschmann et al. Position control of an underactuated continuum mechanism using a reduced nonlinear model
CN113043283B (en) Robot tail end external force estimation method
Wang et al. Dynamic hybrid position/force control for the quadrotor with a multi-degree-of-freedom manipulator
Mishra et al. A geometric controller for fully-actuated robotic capture of a tumbling target
CN115480583A (en) Visual servo tracking and impedance control method of flying operation robot
Zarebidoki et al. Dynamic modeling and adaptive control of a cable-suspended robot
Tong et al. Neural network based visual servo control under the condition of heavy loading
CN117270398A (en) Robot gait planning algorithm based on neural network and reinforcement learning
Secchi et al. An experience on stable control of mobile robots
Cheah et al. Adaptive Jacobian motion and force tracking control for constrained robots with uncertainties
Anuar et al. Sliding mode control with adaptive gain of quadrotor with rigid manipulator
CN115284276A (en) Robot joint torque control method based on long-term and short-term memory network
WO2022203933A1 (en) Dynamic torque saturation limits for robot actuator(s)
Hagane et al. Adaptive generalized predictive controller and cartesian force control for robot arm using dynamics and geometric identification
CN111203883B (en) Self-learning model prediction control method for robot electronic component assembly
CN116736748A (en) Method for constructing controller of robot and robot
CN116635802A (en) Numerical control device

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