CN110142762B - Robot joint position control method and device and robot - Google Patents

Robot joint position control method and device and robot Download PDF

Info

Publication number
CN110142762B
CN110142762B CN201910390134.1A CN201910390134A CN110142762B CN 110142762 B CN110142762 B CN 110142762B CN 201910390134 A CN201910390134 A CN 201910390134A CN 110142762 B CN110142762 B CN 110142762B
Authority
CN
China
Prior art keywords
current
force
servo control
pose
representing
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
CN201910390134.1A
Other languages
Chinese (zh)
Other versions
CN110142762A (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.)
Wuhan Cobot Technology Co ltd
Original Assignee
Wuhan Cobot Technology Co ltd
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 Wuhan Cobot Technology Co ltd filed Critical Wuhan Cobot Technology Co ltd
Priority to CN201910390134.1A priority Critical patent/CN110142762B/en
Publication of CN110142762A publication Critical patent/CN110142762A/en
Application granted granted Critical
Publication of CN110142762B publication Critical patent/CN110142762B/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
    • B25J13/00—Controls for manipulators
    • B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
    • B25J13/085—Force or torque sensors
    • 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/1612—Programme controls characterised by the hand, wrist, grip control
    • 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/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • 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/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Human Computer Interaction (AREA)
  • Health & Medical Sciences (AREA)
  • General Health & Medical Sciences (AREA)
  • Orthopedic Medicine & Surgery (AREA)
  • Manipulator (AREA)

Abstract

The invention relates to the technical field of robot joint control, and provides a robot joint position control method, a device and a robot, wherein the method comprises the following steps: when the robot joint moves to a current pose point along a preset reference track, respectively acquiring a current force measurement value, a current force reference value, a current interpolation pose matrix and a current servo control parameter; calculating expected servo control quantity according to the current force measurement value, the current force reference value, the current interpolation pose matrix and the current servo control parameter; the robot joint is controlled to move from the current pose point to the expected pose point according to the expected servo control quantity, the current force measurement value and the current force reference are used as calculation parameters of the expected servo control quantity, the accuracy of the expected servo control quantity can be improved, the accuracy of the expected servo control quantity for controlling the robot joint to move is improved, the current pose point is corrected according to the expected pose point, and the defect that the robot joint is prone to deviation in the process of changing the position according to the movement track is overcome.

Description

Robot joint position control method and device and robot
Technical Field
The invention relates to the technical field of robot joint control, in particular to a robot joint position control method and device and a robot.
Background
With the development of robot technology, robots are widely applied to machining of a plurality of workpieces such as grinding, polishing and cleaning, in order to improve workpiece machining quality and reduce the using difficulty of the robots, the robots are adaptive to flexibly control the pose of robot joints, and the robot joints drive machining tools to change positions, so that the machining tools flexibly machine the workpieces, and the robots are very important for improving the workpiece machining quality and reducing the using difficulty of the robots.
In the related technology, the robot joint pose self-adaptive control method comprises the following steps: the robot plans a motion track based on the workpiece three-dimensional point cloud image, solves servo control quantity based on path points in the motion track and controls robot joint motion servo control quantity, and the robot joint changes the position according to the motion track.
However, in the process that the robot joint changes position according to the movement track, the robot joint is subjected to the action of pressure, gravity and the like applied by a processing tool fixedly connected with the robot joint, so that the robot joint has position deviation, the processing quality of workpieces is reduced, and the use difficulty of the robot is improved.
Disclosure of Invention
The invention aims to solve the technical problem that in the prior art, position deviation is easy to occur in the process that a robot joint changes position according to a motion track, and provides a robot joint position control method and device and a robot.
The technical scheme for solving the technical problems is as follows:
according to a first aspect of the present invention, there is provided a robot joint position control method including:
when a robot joint moves to a current pose point along a preset reference track, respectively acquiring a current force measurement value, a current force reference value, a current interpolation pose matrix and a current servo control parameter of the robot joint at the current pose point;
calculating expected servo control quantity according to the current force measurement value, the current force reference value, the current interpolation pose matrix and the current servo control parameter;
and controlling the robot joint to move from the current pose point to an expected pose point according to the expected servo control amount.
According to a second aspect of the present invention, there is provided a robot joint position control device including:
the system comprises an acquisition module, a motion estimation module and a motion estimation module, wherein the acquisition module is used for respectively acquiring a current force measurement value, a current force reference value, a current interpolation pose matrix and a current servo control parameter of a robot joint at a current pose point when the robot joint moves to the current pose point along a preset reference track;
the calculation module is used for calculating expected servo control quantity according to the current force measurement value, the current force reference value, the current interpolation pose matrix and the current servo control parameter;
and the control module is used for controlling the robot joint to move from the current pose point to an expected pose point according to the expected servo control amount.
According to a third aspect of the present invention, there is provided a robot comprising:
the force processor is used for respectively acquiring a current force measurement value, a current force reference value, a current interpolation pose matrix and a current servo control parameter of the robot joint at the current pose point when the robot joint moves to the current pose point along a preset reference track; calculating expected servo control quantity according to the current force measurement value, the current force reference value, the current interpolation pose matrix and the current servo control parameter;
and the robot joint servo controller is used for controlling the robot joint to move from the current pose point to the expected pose point according to the expected servo control amount.
The robot joint position control method and device and the robot provided by the invention have the beneficial effects that: the current force measurement value, the current force reference value, the current interpolation pose matrix and the current servo control parameter are used as calculation parameters to calculate the expected servo control quantity, the accuracy of the expected servo control quantity is guaranteed, the robot joint is controlled to move the expected servo control quantity from the current pose point accurately, the robot joint is driven to be corrected from the current pose point to the expected pose point, in the process that the robot joint changes the position according to the movement track, the position deviation of the robot joint caused by force action can be reduced, the accuracy of the robot joint changing the position according to the movement track can be improved, the workpiece machining quality is improved, the use difficulty of the robot is reduced, and the robot joint is suitable for machining different types of workpieces.
Drawings
Fig. 1 is a schematic flowchart of a robot joint position control method according to an embodiment of the present invention;
fig. 2a is a schematic diagram of force fluctuation in a process that a robot joint moves to a current pose point along a preset reference track according to an embodiment of the present invention;
FIG. 2b is a schematic diagram of force fluctuation after a robot joint is corrected from a current pose point to a desired pose point according to an embodiment of the present invention;
fig. 3a is a schematic structural diagram of a robot joint position control device according to an embodiment of the present invention;
FIG. 3b is a schematic structural diagram of another robot joint position control apparatus according to an embodiment of the present invention;
fig. 3c is a schematic structural diagram of another robot joint position control device according to an embodiment of the present invention;
fig. 4a is a schematic structural diagram of a robot according to an embodiment of the present invention;
fig. 4b is a schematic circuit diagram corresponding to the robot in fig. 3 a.
Detailed Description
The principles and features of this invention are described below in conjunction with the following drawings, which are set forth by way of illustration only and are not intended to limit the scope of the invention.
Example one
As shown in fig. 1, a flowchart of a method for controlling a position of a robot joint according to an embodiment of the present invention includes: when the robot joint moves to a current pose point along a preset reference track, respectively acquiring a current force measurement value, a current force reference value, a current interpolation pose matrix and a current servo control parameter of the robot joint at the current pose point; calculating expected servo control quantity according to the current force measurement value, the current force reference value, the current interpolation pose matrix and the current servo control parameter; and controlling the robot joint to move from the current pose point to the expected pose point according to the expected servo control amount.
In some embodiments, the current force reference value is pre-stored in the force processor, and the force sensor collects a current force measurement value when the robot joint moves to the current pose point; the robot joint encoder collects the angular displacement of the current pose point, and the angular displacement is solved based on a positive kinematic equation to obtain current pose information; the force processor respectively obtains a current force measurement value input by the force sensor and current pose information input by the robot joint encoder, stores the current pose information into a pose matrix in a matrix form, and reads current servo control parameters in the robot joint servo controller.
The force processor calculates the expected servo control quantity by taking the current force measurement value, the current force reference value, the current interpolation pose matrix and the current servo control parameter as calculation parameters, ensures the accuracy of the expected servo control quantity, inputs the expected servo control quantity into the robot joint servo controller, can accurately control the robot joint to move the expected servo control quantity from the current pose point through the robot joint servo controller, realizes the correction of the robot joint from the current pose point to the expected pose point, reduces the position deviation of the robot joint caused by force action in the process that the robot joint changes the position according to the motion track, not only can improve the accuracy of the robot joint changing the position according to the motion track, but also is beneficial to improving the workpiece processing quality and reducing the use difficulty of the robot.
As an optional implementation, the obtaining of the current interpolation pose matrix specifically includes: determining the moment when the robot joint moves to the current pose point as the current movement moment; searching a current reference pose matrix corresponding to the current motion moment and a next reference pose matrix adjacent to the current reference pose matrix from a preset reference track; determining a next movement time after the current movement time from the set period, and determining an intermediate time between the current movement time and the next movement time; calculating the time difference between the intermediate time and the current movement time; and performing linear calculation on the current reference pose matrix, the next reference pose matrix and the set time interval duration and time difference based on a linear interpolation model to obtain a current interpolation pose matrix.
The set time interval is represented by taking the current motion moment and the next motion moment as parameters and is used for indicating the time interval that the robot joint moves from the current pose point to the next pose point represented by the next reference pose matrix.
In some embodiments, before the robot joint drives the machining tool to machine the tool, detecting, by a robot joint encoder, a length of time for the robot joint to move from a current pose point to a next pose point; in the process that the robot joint moves from the current pose point to the expected pose point, the force processor reversely solves the later movement moment according to the time length and the current movement moment, the current movement moment is used as the starting moment, the later movement moment is used as the ending moment to represent the set time period, and the middle moment approaching the current movement moment can be selected from the set time period, for example: the current movement time is '2018-04-2918: 31:38: 41', the intermediate time is '2018-04-2918: 31:39: 100' and the later movement time is '2018-04-2918: 31: 40'.
As an optional implementation, the preset reference trajectory is represented by a pose point sequence, and the pose point sequence specifically includes:
T＝{T(t1) T(t2) T(t3) ..... T(ti+1)}
wherein, T (T)i+1) And representing a reference pose matrix corresponding to the (i + 1) th motion moment in the pose point sequence T.
The linear interpolation model is specifically as follows:
Figure BDA0002056216790000051
wherein, tnRepresenting the current moment of motion, tn+1Indicating the current moment of motion tnThe next moment of movement, t, being the starting pointmIndicating the current moment of motion tnIntermediate time, T (T), being the starting pointm) Representation and intermediate time tmThe corresponding current interpolation pose matrix, T (T)n) Representing the time t of the current movementnCorresponding current reference pose matrix, T (T)n+1) Representing the time t of the following movementn+1And the corresponding latter reference pose matrix is 1 < n < i.
The current interpolation pose matrix, the next reference pose matrix, the time length of the set time period and the time difference can be quickly calculated through a linear interpolation model, the current interpolation pose matrix can be calculated in a simple mode, and the calculation efficiency and accuracy of the current interpolation pose matrix can be considered.
As an optional implementation manner, calculating an expected servo control amount according to the current force measurement value, the current force reference value, the current interpolation pose matrix, and the current servo control parameter specifically includes: estimating the current force measurement value based on the force estimation model to obtain a force estimation value; calculating a force compensation amount based on the force estimation value and the current force reference value; calculating an expected pose matrix based on the force compensation quantity and the current interpolation pose matrix; solving the expected pose matrix by using an inverse dynamics model to obtain expected servo control parameters; a desired servo control amount is calculated based on the desired servo control parameter and the current servo control parameter.
As shown in fig. 2a and 2b, the current movement time t is based oni+1And the force F to establish a two-dimensional coordinate system, as shown in FIG. 2a, when the robot moves to the current position along the preset reference trackDuring the attitude point process, the force measurement FMbCompared to the force reference value FrefThe fluctuation of (2) is large; as shown in FIG. 2b, after the robot joint is corrected from the current pose point to the desired pose point, the force side magnitude FMaCompared to the force reference value FrefIs small, i.e.: the force fluctuation range of the robot joint corrected to the expected pose point is well reduced, the stability of the robot joint at the expected pose point is improved, and therefore the problem that the robot joint has position deviation due to force action is solved.
As an alternative embodiment, the force estimation model is specifically:
Figure BDA0002056216790000071
wherein,
Figure BDA0002056216790000072
representing the force estimate in the contact point reference frame a,
Figure BDA0002056216790000073
representing a rotation transformation matrix transformed from the contact point reference frame a into the force measurement reference frame b,
Figure BDA0002056216790000074
denotes the antisymmetric operator, 0, corresponding to the coordinates of the origin of the contact point reference frame a in the force measurement reference frame b3×3A zero matrix of 3 rows and 3 columns is shown,
Figure BDA0002056216790000075
representing the current force measurement converted from the contact point reference frame a into the force measurement reference frame b,
Figure BDA0002056216790000076
representing the coordinates of the center of gravity o of the tool articulated to the robot in the contact point reference coordinate system a,
Figure BDA0002056216790000077
the antisymmetrical operator corresponding to the coordinate representing the center of gravity o in the force measurement reference coordinate system b,
Figure BDA0002056216790000078
representing the coordinates in the force measurement reference coordinate system b with the center of gravity o,
Figure BDA0002056216790000079
indicating that the center of gravity o changes the current force measurement in the force measurement reference frame b
Figure BDA00020562167900000710
The change value of (c).
In the process that a robot joint drives a machining tool to machine a tool, any point where the machining tool is contacted with a workpiece is taken as a contact point, the contact point is taken as a first origin point in a first three-dimensional coordinate system, the first three-dimensional coordinate system is taken as a contact point reference coordinate system a, a force measurement reference coordinate system b is constructed based on a six-dimensional force sensor, and a matrix is rotationally transformed
Figure BDA00020562167900000711
A rotational transformation relationship between the axes of the contact point reference frame a and the force measurement reference frame b may be provided, for example: and constructing a second three-dimensional coordinate system by taking the gravity center of the six-dimensional force sensor as a second origin, and taking the second three-dimensional coordinate system as a force measurement reference coordinate system b.
In some embodiments, the current force measurement includes a six-dimensional force value, such as: the six-dimensional force values are expressed in a matrix form as
Figure BDA00020562167900000712
2N denotes a pressure induced in the force measurement reference frame b by the six-dimensional force sensor in a positive direction along the X-axis, 5N denotes a pressure induced in the force measurement reference frame b by the six-dimensional force sensor in a positive direction along the Y-axis, 18N denotes a pressure induced in the force measurement reference frame b by the six-dimensional force sensor in a positive direction along the Z-axis, 1.2Nm denotes a pressure induced in the force measurement reference frame b by the six-dimensional force sensor in a positive direction along the X-axisThe torque in the direction, 2.3Nm indicates that the six-dimensional force sensor induces a torque in the force measurement reference frame b in the positive direction along the Y-axis, and 0.6Nm indicates that the six-dimensional force sensor induces a torque in the force measurement reference frame b in the positive direction along the Z-axis.
The force estimation model is used to quickly and accurately estimate the current force measurement to improve the efficiency and accuracy of the estimation of the desired servo control quantity.
As an alternative embodiment, the calculating the force compensation amount based on the force estimation value and the current force reference value specifically includes: calculating a force deviation value between the force estimation value and the current force reference value; and calculating the force deviation value by using an incremental PID control model to obtain a force compensation value.
The incremental PID control model specifically comprises:
Figure BDA0002056216790000081
wherein u (t)m) Representing an intermediate time tmCorresponding force compensation quantity, kpDenotes the proportionality coefficient, kiRepresenting the integral coefficient, kdDenotes a differential coefficient, e (t)m) Representing an intermediate time tmCorresponding force deviation value, dtmRepresenting an intermediate time tmIncrement of (d), de (t)m)/dtmIndicates the force deviation value e (t)m) To intermediate time tmAnd (5) deriving the differential quotient.
Force deviation value e (t)m) The method specifically comprises the following steps:
Figure BDA0002056216790000082
wherein, ω isrefRepresenting the current force reference value, for example: the current force reference value is 20N.
The incremental PID control model compensates the current force measurement value by using the current force reference value, so that the force compensation amount is accurately calculated, the performance of well correcting the robot joint from the current pose point to the expected pose point in a closed-loop control mode is facilitated, the false action influence is small, and the stability of controlling the movement of the robot joint is facilitated to be improved.
As an optional implementation, calculating the expected pose matrix based on the force compensation amount and the current interpolation pose matrix specifically includes: determining a translation transformation matrix based on the force compensation amount; and multiplying the translation transformation matrix and the current interpolation pose matrix by using the pose transformation model to obtain an expected pose matrix.
The pose conversion model specifically comprises the following steps: t (T)m)×H(u(tm) Wherein, H (u (t))m) Represents a translation of the force compensation u (t) along one dimension in the force measurement reference frame bm) The resulting translation transformation matrix, for example: the translation transformation matrix translates the force compensation u (t) along the Z-axis in the force measurement reference frame bm)。
The translation of the current interpolation pose matrix can be quickly converted into the expected pose matrix through the pose conversion model, and the calculation efficiency of the expected pose matrix is improved.
As an optional implementation, calculating the desired servo control amount based on the desired servo control parameter and the current servo control parameter specifically includes: calculating a servo control parameter deviation value between the expected servo control parameter and the current servo control parameter; and calculating the deviation value of the servo control parameter by using a PI control model to obtain the expected servo control quantity.
The PI control model specifically comprises:
Figure BDA0002056216790000091
wherein q (t)m) Representing an intermediate time tmCorresponding desired servo control quantity, Δ q (t)m) Representing an intermediate time tmCorresponding deviation value of servo control parameter, Δ q (t)m)/dtmRepresenting the servo control parameter deviation Δ q (t)m) To intermediate time tmAnd (5) deriving the differential quotient.
Servo control parameter deviation Δ q (t)m) The method specifically comprises the following steps: e (t)m)＝qref-qactualWherein q isrefRepresenting the current servo control parameter, qactualRepresenting desired servo control parameters, the desired pose parameters including angular displacement related parameters.
Compared with a PID control model, the PI control model is simpler, after the servo control parameter deviation value is calculated, the calculation mode of the expected servo control quantity is facilitated to be simplified through the mode that the PI control model calculates the servo control parameter deviation value, and the control efficiency of the robot correcting the current pose point to the expected pose point is improved.
Example two
As shown in fig. 3a, a schematic structural diagram of a robot joint position control apparatus according to an embodiment of the present invention includes: the device comprises an acquisition module, a calculation module and a control module.
And the acquisition module is used for respectively acquiring a current force measurement value, a current force reference value, a current interpolation pose matrix and a current servo control parameter of the robot joint at the current pose point when the robot joint moves to the current pose point along a preset reference track.
And the calculation module is used for calculating the expected servo control quantity according to the current force measurement value, the current force reference value, the current interpolation pose matrix and the current servo control parameter.
And the control module is used for controlling the robot joint to move from the current pose point to the expected pose point according to the expected servo control amount.
As an alternative implementation, as shown in fig. 3b, the obtaining module includes: the system comprises a force acquisition submodule, a pose matrix acquisition submodule and a servo parameter acquisition submodule.
And the force acquisition submodule is used for respectively reading the current force reference value and the current force measured value detected by the force sensor.
The pose matrix acquisition submodule is used for determining the moment when the robot joint moves to the current pose point as the current movement moment; searching a current reference pose matrix corresponding to the current motion moment and a next reference pose matrix adjacent to the current reference pose matrix from a preset reference track; determining a next movement time after the current movement time from the set period, and determining an intermediate time between the current movement time and the next movement time; calculating the time difference between the intermediate time and the current movement time; and performing linear calculation on the current reference pose matrix, the next reference pose matrix and the set time interval duration and time difference based on a linear interpolation model to obtain a current interpolation pose matrix.
And the servo parameter acquisition submodule is used for reading the current servo control parameters input by the robot joint servo controller.
The preset reference track is represented by a pose point sequence, and the pose point sequence specifically comprises the following steps:
T＝{T(t1) T(t2) T(t3) ..... T(ti+1)}
wherein, T (T)i+1) And representing a reference pose matrix corresponding to the (i + 1) th motion moment in the pose point sequence T.
The linear interpolation model is specifically as follows:
Figure BDA0002056216790000101
wherein, tnRepresenting the current moment of motion, tn+1Indicating the current moment of motion tnThe next moment of movement, t, being the starting pointmIndicating the current moment of motion tnIntermediate time, T (T), being the starting pointm) Representation and intermediate time tmThe corresponding current interpolation pose matrix, T (T)n) Representing the time t of the current movementnCorresponding current reference pose matrix, T (T)n+1) Representing the time t of the following movementn+1And the corresponding latter reference pose matrix is 1 < n < i.
As an alternative implementation, as shown in fig. 3c, the calculation module includes: the force estimation submodule is used for estimating a current force measurement value based on a force estimation model to obtain a force estimation value; the force compensation submodule is used for calculating a force compensation quantity based on the force estimation value and the current force reference value; the first matrix calculation submodule is used for calculating an expected pose matrix based on the force compensation quantity and the current interpolation pose matrix; the second matrix calculation submodule is used for solving the expected pose matrix by applying the inverse dynamics model to obtain expected servo control parameters; and the servo parameter calculation submodule is used for calculating the expected servo control quantity based on the expected servo control parameter and the current servo control parameter.
The force estimation model specifically comprises:
Figure BDA0002056216790000111
wherein,
Figure BDA0002056216790000112
representing the force estimate in the contact point reference frame a,
Figure BDA0002056216790000113
representing a rotation transformation matrix transformed from the contact point reference frame a into the force measurement reference frame b,
Figure BDA0002056216790000114
denotes the antisymmetric operator, 0, corresponding to the coordinates of the origin of the contact point reference frame a in the force measurement reference frame b3×3A zero matrix of 3 rows and 3 columns is shown,
Figure BDA0002056216790000115
representing the current force measurement converted from the contact point reference frame a into the force measurement reference frame b,
Figure BDA0002056216790000116
representing the coordinates of the center of gravity o of the tool articulated to the robot in the contact point reference coordinate system a,
Figure BDA0002056216790000117
the antisymmetrical operator corresponding to the coordinate representing the center of gravity o in the force measurement reference coordinate system b,
Figure BDA0002056216790000118
representing the coordinates in the force measurement reference coordinate system b with the center of gravity o,
Figure BDA0002056216790000119
indicating that the center of gravity o changes the current force measurement in the force measurement reference frame b
Figure BDA00020562167900001110
The change value of (c).
As an alternative embodiment, the force compensation submodule is specifically configured to: calculating a force deviation value between the force estimation value and the current force reference value; and calculating the force deviation value by using an incremental PID control model to obtain a force compensation value.
The incremental PID control model specifically comprises:
Figure BDA0002056216790000121
wherein u (t)m) Representing an intermediate time tmCorresponding force compensation quantity, kpDenotes the proportionality coefficient, kiRepresenting the integral coefficient, kdDenotes a differential coefficient, e (t)m) Representing an intermediate time tmCorresponding force deviation value, dtmRepresenting an intermediate time tmIncrement of (d), de (t)m)/dtmIndicates the force deviation value e (t)m) To intermediate time tmAnd (5) deriving the differential quotient.
Force deviation value e (t)m) The method specifically comprises the following steps:
Figure BDA0002056216790000122
wherein, ω isrefRepresenting the current force reference value.
As an optional implementation, the first matrix calculation submodule is specifically configured to: determining a translation transformation matrix based on the force compensation amount; and multiplying the translation transformation matrix and the current interpolation pose matrix by using the pose transformation model to obtain an expected pose matrix.
The pose conversion model specifically comprises the following steps: t (T)m)×H(u(tm) Wherein, H (u (t))m) Represents a translation of the force compensation u (t) along one dimension in the force measurement reference frame bm) And obtaining a translation transformation matrix.
As an optional implementation manner, the servo parameter calculation submodule is specifically configured to: calculating a servo control parameter deviation value between the expected servo control parameter and the current servo control parameter; and calculating the deviation value of the servo control parameter by using a PI control model to obtain the expected servo control quantity.
The PI control model specifically comprises:
Figure BDA0002056216790000123
wherein q (t)m) Representing an intermediate time tmCorresponding desired servo control quantity, Δ q (t)m) Representing an intermediate time tmCorresponding deviation value of servo control parameter, Δ q (t)m)/dtmRepresenting the servo control parameter deviation Δ q (t)m) To intermediate time tmAnd (5) deriving the differential quotient.
Servo control parameter deviation Δ q (t)m) The method specifically comprises the following steps: e (t)m)＝qref-qactualWherein q isrefRepresenting the current servo control parameter, qactualIndicating the desired servo control parameters.
EXAMPLE III
The embodiment of the invention provides a robot, as shown in fig. 4a, the robot comprises an AGV trolley 1, a robot joint 2, a six-dimensional force sensor 3 and a polishing tool 4, wherein the robot joint 2 is positioned on the AGV trolley 1, the six-dimensional force sensor 3 is installed on the robot joint 2, and the polishing tool 4 is installed on the six-dimensional force sensor 3.
As shown in fig. 4b, the AGV cart 1 includes a force processor, a robot joint encoder, and a robot joint servo controller, and the force processor is respectively connected to the robot joint encoder, the robot joint servo controller, and the six-dimensional force sensor in a communication manner.
The force processor is used for respectively acquiring a current force measurement value, a current force reference value, a current interpolation pose matrix and a current servo control parameter of the robot joint at the current pose point when the robot joint moves to the current pose point along a preset reference track; and calculating expected servo control quantity according to the current force measurement value, the current force reference value, the current interpolation pose matrix and the current servo control parameter.
And the robot joint servo controller is used for controlling the robot joint to move the expected servo control quantity so as to enable the robot joint to move from the current pose point to the expected pose point.
A force processor, specifically to: determining the moment when the robot joint moves to the current pose point as the current movement moment; searching a current reference pose matrix corresponding to the current motion moment and a next reference pose matrix adjacent to the current reference pose matrix from a preset reference track; determining a next movement time after the current movement time from the set period, and determining an intermediate time between the current movement time and the next movement time; calculating the time difference between the intermediate time and the current movement time; and performing linear calculation on the current reference pose matrix, the next reference pose matrix and the set time interval duration and time difference based on a linear interpolation model to obtain a current interpolation pose matrix.
The force processor is further specifically configured to: estimating the current force measurement value based on the force estimation model to obtain a force estimation value; calculating a force compensation amount based on the force estimation value and the current force reference value; calculating an expected pose matrix based on the force compensation quantity and the current interpolation pose matrix; solving the expected pose matrix by using an inverse dynamics model to obtain expected servo control parameters; a desired servo control amount is calculated based on the desired servo control parameter and the current servo control parameter.
The force processor is further specifically configured to: calculating a force deviation value between the force estimation value and the current force reference value; calculating the force deviation value by using an incremental PID control model to obtain a force compensation value; determining a translation transformation matrix based on the force compensation amount; multiplying the translation transformation matrix and the current interpolation pose matrix by using a pose transformation model to obtain an expected pose matrix; solving the expected pose matrix by using an inverse dynamics model to obtain expected servo control parameters; calculating a servo control parameter deviation value between the expected servo control parameter and the current servo control parameter; and calculating the deviation value of the servo control parameter by using a PI control model to obtain the expected servo control quantity.
It should be noted that, in the third embodiment, the expression modes of the force estimation model, the incremental PID control model, the pose conversion model, the inverse dynamics model, the PI control model, and the like are the same as those in the first embodiment, and for brevity, the description thereof is omitted here.
The reader should understand that in the description of this specification, reference to the description of the terms "aspect," "alternative embodiments," or "some specific examples," etc., means that a particular feature, step, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the invention, and the terms "first" and "second," etc., are used for descriptive purposes only and are not to be construed as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second", etc., may explicitly or implicitly include at least one of the feature.
The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, and any modifications, equivalents, improvements and the like that fall within the spirit and principle of the present invention are intended to be included therein.

Claims (5)

1. A robot joint position control method, comprising:
when a robot joint moves to a current pose point along a preset reference track, respectively acquiring a current force measurement value, a current force reference value, a current interpolation pose matrix and a current servo control parameter of the robot joint at the current pose point;
calculating expected servo control quantity according to the current force measurement value, the current force reference value, the current interpolation pose matrix and the current servo control parameter;
controlling the robot joint to move from the current pose point to an expected pose point according to the expected servo control amount;
calculating an expected servo control quantity according to the current force measurement value, the current force reference value, the current interpolation pose matrix and the current servo control parameter, and specifically comprises the following steps:
estimating the current force measurement value based on a force estimation model to obtain a force estimation value;
calculating a force compensation amount based on the force estimate and the current force reference;
calculating an expected pose matrix based on the force compensation quantity and the current interpolation pose matrix;
solving the expected pose matrix by using an inverse dynamics model to obtain expected servo control parameters;
calculating the desired servo control amount based on the desired servo control parameter and the current servo control parameter;
the force estimation model specifically comprises:
Figure FDA0003414823020000011
wherein,
Figure FDA0003414823020000012
representing said force estimate in the contact point reference frame a,
Figure FDA0003414823020000013
representing a rotation transformation matrix transformed from the contact point reference frame a into a force measurement reference frame b,
Figure FDA0003414823020000014
representing an antisymmetric operator, 0, corresponding to the coordinates of the origin of the contact point reference frame a in the force measurement reference frame b3×3A zero matrix of 3 rows and 3 columns is shown,
Figure FDA0003414823020000015
representing the current force measurement converted from the contact point reference frame a into the force measurement reference frame b,
Figure FDA0003414823020000021
representing the coordinates of the center of gravity o of a tool coupled to the robot joint in the contact point reference coordinate system a,
Figure FDA0003414823020000022
an antisymmetric operator corresponding to the coordinates representing the center of gravity o in the force measurement reference coordinate system b,
Figure FDA0003414823020000023
representing the coordinates in the force measurement reference coordinate system b with the center of gravity o,
Figure FDA0003414823020000024
indicating that the center of gravity o changes the current force measurement value in the force measurement reference coordinate system b
Figure FDA0003414823020000025
A change value of (d);
acquiring the current interpolation pose matrix, specifically including:
determining the moment when the robot joint moves to the current pose point as the current movement moment;
searching a current reference pose matrix corresponding to the current motion moment and a next reference pose matrix adjacent to the current reference pose matrix from the preset reference track;
determining a subsequent movement time after the current movement time from a set period of time, and determining an intermediate time between the current movement time and the subsequent movement time, wherein the set period of time is represented by the current movement time and the subsequent movement time as parameters and is used for indicating the period of time for the robot joint to move from the current pose point to a subsequent pose point represented by the subsequent reference pose matrix;
calculating the time difference between the intermediate time and the current movement time;
and performing linear calculation on the current reference pose matrix, the next reference pose matrix, the set time interval duration and the time difference based on a linear interpolation model to obtain the current interpolation pose matrix.
2. The method of claim 1, wherein calculating a force compensation amount based on the force estimate and the current force reference comprises:
calculating a force deviation value between the force estimate and the current force reference;
calculating the force deviation value by applying an incremental PID control model to obtain the force compensation quantity;
the incremental PID control model specifically comprises:
Figure FDA0003414823020000026
wherein u (t)m) Representing said intermediate time tmCorresponding compensation quantity of force, kpDenotes the proportionality coefficient, kiRepresenting the integral coefficient, kdDenotes a differential coefficient, e (t)m) Representing said intermediate time tmCorresponding said force deviation value, dtmRepresenting said intermediate time tmIncrement of (d), de (t)m)/dtmRepresenting said force deviation value e (t)m) For the intermediate time tmDerivative the differential quotient;
said force deviation value e (t)m) The method specifically comprises the following steps:
Figure FDA0003414823020000031
wherein, ω isrefRepresenting the current force reference value.
3. The method of claim 1, wherein calculating the desired servo control amount based on the desired servo control parameter and the current servo control parameter comprises:
calculating a servo control parameter deviation value between the expected servo control parameter and the current servo control parameter;
calculating the deviation value of the servo control parameter by applying a PI control model to obtain the expected servo control quantity;
the PI control model specifically comprises the following steps:
Figure FDA0003414823020000032
wherein q (t)m) Representing said intermediate time tmCorresponding desired servo control amount, kpDenotes the proportionality coefficient, kiDenotes the integral coefficient, Δ q (t)m) Representing said intermediate time tmCorresponding deviation value of the servo control parameter, Δ q (t)m)/dtmRepresenting said servo control parameter deviation value Δ q (t)m) For the intermediate time tmDerivative the differential quotient;
the servo control parameter deviation value Deltaq (t)m) The method specifically comprises the following steps:
Δq(tm)＝qref-qactual
wherein q isrefRepresenting said current servo control parameter, qactualRepresenting the desired servo control parameter.
4. A robot joint position control device characterized by comprising:
the system comprises an acquisition module, a motion estimation module and a motion estimation module, wherein the acquisition module is used for respectively acquiring a current force measurement value, a current force reference value, a current interpolation pose matrix and a current servo control parameter of a robot joint at a current pose point when the robot joint moves to the current pose point along a preset reference track;
the calculation module is used for calculating expected servo control quantity according to the current force measurement value, the current force reference value, the current interpolation pose matrix and the current servo control parameter;
a control module for controlling the robot joint to move from the current pose point to an expected pose point according to the expected servo control amount;
the calculation module is specifically configured to calculate an expected servo control amount according to the current force measurement value, the current force reference value, the current interpolation pose matrix, and the current servo control parameter, and specifically includes:
estimating the current force measurement value based on a force estimation model to obtain a force estimation value;
calculating a force compensation amount based on the force estimate and the current force reference;
calculating an expected pose matrix based on the force compensation quantity and the current interpolation pose matrix;
solving the expected pose matrix by using an inverse dynamics model to obtain expected servo control parameters;
calculating the desired servo control amount based on the desired servo control parameter and the current servo control parameter;
the force estimation model specifically comprises:
Figure FDA0003414823020000041
wherein,
Figure FDA0003414823020000042
representing said force estimate in the contact point reference frame a,
Figure FDA0003414823020000043
representing a rotation transformation matrix transformed from the contact point reference frame a into a force measurement reference frame b,
Figure FDA0003414823020000044
representing the origin of the reference coordinate system a with the contact point at the positionInverse symmetry operator corresponding to the coordinates of the force measurement reference coordinate system b, 03×3A zero matrix of 3 rows and 3 columns is shown,
Figure FDA0003414823020000045
representing the current force measurement converted from the contact point reference frame a into the force measurement reference frame b,
Figure FDA0003414823020000046
representing the coordinates of the center of gravity o of a tool coupled to the robot joint in the contact point reference coordinate system a,
Figure FDA0003414823020000047
an antisymmetric operator corresponding to the coordinates representing the center of gravity o in the force measurement reference coordinate system b,
Figure FDA0003414823020000048
representing the coordinates in the force measurement reference coordinate system b with the center of gravity o,
Figure FDA0003414823020000049
indicating that the center of gravity o changes the current force measurement value in the force measurement reference coordinate system b
Figure FDA0003414823020000051
A change value of (d);
the acquisition module is specifically configured to: determining the moment when the robot joint moves to the current pose point as the current movement moment;
searching a current reference pose matrix corresponding to the current motion moment and a next reference pose matrix adjacent to the current reference pose matrix from the preset reference track;
determining a subsequent movement time after the current movement time from a set period of time, and determining an intermediate time between the current movement time and the subsequent movement time, wherein the set period of time is represented by the current movement time and the subsequent movement time as parameters and is used for indicating the period of time for the robot joint to move from the current pose point to a subsequent pose point represented by the subsequent reference pose matrix;
calculating the time difference between the intermediate time and the current movement time;
and performing linear calculation on the current reference pose matrix, the next reference pose matrix, the set time interval duration and the time difference based on a linear interpolation model to obtain the current interpolation pose matrix.
5. A robot, comprising:
the force processor is used for respectively acquiring a current force measurement value, a current force reference value, a current interpolation pose matrix and a current servo control parameter of the robot joint at the current pose point when the robot joint moves to the current pose point along a preset reference track; calculating expected servo control quantity according to the current force measurement value, the current force reference value, the current interpolation pose matrix and the current servo control parameter;
a robot joint servo controller for controlling the robot joint to move from the current pose point to an expected pose point according to the expected servo control amount;
the force processor is specifically configured to calculate an expected servo control amount according to the current force measurement value, the current force reference value, the current interpolation pose matrix, and the current servo control parameter, and specifically includes:
estimating the current force measurement value based on a force estimation model to obtain a force estimation value;
calculating a force compensation amount based on the force estimate and the current force reference;
calculating an expected pose matrix based on the force compensation quantity and the current interpolation pose matrix;
solving the expected pose matrix by using an inverse dynamics model to obtain expected servo control parameters;
calculating the desired servo control amount based on the desired servo control parameter and the current servo control parameter;
the force estimation model specifically comprises:
Figure FDA0003414823020000061
wherein,
Figure FDA0003414823020000062
representing said force estimate in the contact point reference frame a,
Figure FDA0003414823020000063
representing a rotation transformation matrix transformed from the contact point reference frame a into a force measurement reference frame b,
Figure FDA0003414823020000064
representing an antisymmetric operator, 0, corresponding to the coordinates of the origin of the contact point reference frame a in the force measurement reference frame b3×3A zero matrix of 3 rows and 3 columns is shown,
Figure FDA0003414823020000065
representing the current force measurement converted from the contact point reference frame a into the force measurement reference frame b,
Figure FDA0003414823020000066
representing the coordinates of the center of gravity o of a tool coupled to the robot joint in the contact point reference coordinate system a,
Figure FDA0003414823020000067
an antisymmetric operator corresponding to the coordinates representing the center of gravity o in the force measurement reference coordinate system b,
Figure FDA0003414823020000068
representing the coordinates in the force measurement reference coordinate system b with the center of gravity o,
Figure FDA0003414823020000069
indicating that the center of gravity o changes the current force measurement value in the force measurement reference coordinate system b
Figure FDA00034148230200000610
A change value of (d);
the force processor is specifically configured to: determining the moment when the robot joint moves to the current pose point as the current movement moment;
searching a current reference pose matrix corresponding to the current motion moment and a next reference pose matrix adjacent to the current reference pose matrix from the preset reference track;
determining a subsequent movement time after the current movement time from a set period of time, and determining an intermediate time between the current movement time and the subsequent movement time, wherein the set period of time is represented by the current movement time and the subsequent movement time as parameters and is used for indicating the period of time for the robot joint to move from the current pose point to a subsequent pose point represented by the subsequent reference pose matrix;
calculating the time difference between the intermediate time and the current movement time;
and performing linear calculation on the current reference pose matrix, the next reference pose matrix, the set time interval duration and the time difference based on a linear interpolation model to obtain the current interpolation pose matrix.
CN201910390134.1A 2019-05-10 2019-05-10 Robot joint position control method and device and robot Active CN110142762B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910390134.1A CN110142762B (en) 2019-05-10 2019-05-10 Robot joint position control method and device and robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910390134.1A CN110142762B (en) 2019-05-10 2019-05-10 Robot joint position control method and device and robot

Publications (2)

Publication Number Publication Date
CN110142762A CN110142762A (en) 2019-08-20
CN110142762B true CN110142762B (en) 2022-02-01

Family

ID=67595287

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910390134.1A Active CN110142762B (en) 2019-05-10 2019-05-10 Robot joint position control method and device and robot

Country Status (1)

Country Link
CN (1) CN110142762B (en)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111409076B (en) * 2020-04-28 2021-11-05 珠海格力智能装备有限公司 Method and device for determining motion state of manipulator
CN113561170B (en) * 2020-04-29 2023-01-24 北京配天技术有限公司 Robot torque compensation method, robot, device and storage medium
CN115211970A (en) * 2021-06-11 2022-10-21 诺创智能医疗科技(杭州)有限公司 Pose transformation method and system of mechanical arm and mechanical arm assembly
CN113547522B (en) * 2021-07-30 2022-09-23 山东大学 Robot path precision compensation method and system
CN116512237B (en) * 2022-11-28 2023-09-19 广东建石科技有限公司 Industrial robot vision servo method, device, electronic equipment and storage medium

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2676793B2 (en) * 1988-06-30 1997-11-17 トヨタ自動車株式会社 Copy control robot
CN1200692A (en) * 1995-09-11 1998-12-02 株式会社安川电机 Robert controller
KR20080114197A (en) * 2007-06-27 2008-12-31 한양대학교 산학협력단 Position computation apparatus of wearable robot arm using human force input and wearable robot arm having the same and position moving method of robot arm using the same
CN102363301A (en) * 2011-10-19 2012-02-29 浙江工业大学 Method for adaptively tracking and controlling fingertip force of robot anthropomorphic finger
CN206254004U (en) * 2016-12-12 2017-06-16 武汉库柏特科技有限公司 A kind of robot flexibility polishing system
CN109514571A (en) * 2018-12-05 2019-03-26 上海发那科机器人有限公司 It is a kind of to utilize robot vision and force snesor to the pinpoint method of car body polishing point

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2676793B2 (en) * 1988-06-30 1997-11-17 トヨタ自動車株式会社 Copy control robot
CN1200692A (en) * 1995-09-11 1998-12-02 株式会社安川电机 Robert controller
KR20080114197A (en) * 2007-06-27 2008-12-31 한양대학교 산학협력단 Position computation apparatus of wearable robot arm using human force input and wearable robot arm having the same and position moving method of robot arm using the same
CN102363301A (en) * 2011-10-19 2012-02-29 浙江工业大学 Method for adaptively tracking and controlling fingertip force of robot anthropomorphic finger
CN206254004U (en) * 2016-12-12 2017-06-16 武汉库柏特科技有限公司 A kind of robot flexibility polishing system
CN109514571A (en) * 2018-12-05 2019-03-26 上海发那科机器人有限公司 It is a kind of to utilize robot vision and force snesor to the pinpoint method of car body polishing point

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于打磨机器人的力/位混合控制策略研究;***等;《化工自动化及仪表》;20120731;884-887 *
***等.基于打磨机器人的力/位混合控制策略研究.《化工自动化及仪表》.2012, *

Also Published As

Publication number Publication date
CN110142762A (en) 2019-08-20

Similar Documents

Publication Publication Date Title
CN110142762B (en) Robot joint position control method and device and robot
JP7314475B2 (en) ROBOT CONTROL DEVICE AND ROBOT CONTROL METHOD
CN110948504B (en) Normal constant force tracking method and device for robot machining operation
CN106994686B (en) The computational methods and device, robot of joint external force torque
CN108214454B (en) Robot system, robot control device, and robot control method
JP4202365B2 (en) Force control device
JP5327722B2 (en) Robot load estimation apparatus and load estimation method
TWI540402B (en) Motor controlling device
US20140229005A1 (en) Robot system and method for controlling the same
JP2512099B2 (en) Robot motion teaching method and control device
CN109849025B (en) Vibration suppressing device
JP2017170599A (en) Positioning device using robot
CN110253574B (en) Multi-task mechanical arm pose detection and error compensation method
CN110340909B (en) Robot system for learning control using motor encoder and sensor
US11230005B2 (en) Following robot and work robot system
CN110394802B (en) Polishing robot and position compensation method
US20240009836A1 (en) 3d printer using robot and control apparatus for robot
JP2013013950A (en) Robot, robot control device, robot control method and robot control program
CN114536346B (en) Mechanical arm accurate path planning method based on man-machine cooperation and visual detection
CN113143465B (en) Method for dragging, guiding and positioning mechanical arm based on joint torque
CN110682287B (en) Robot and control device
EP3416007B1 (en) Control device, position control system, position control method, and position control program
Santos et al. Decoupled model predictive control for path following on complex surfaces
JP5631787B2 (en) Servo motor control device and servo motor control method
JPH0146276B2 (en)

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
PE01 Entry into force of the registration of the contract for pledge of patent right

Denomination of invention: A robot joint position control method, device and robot

Effective date of registration: 20220329

Granted publication date: 20220201

Pledgee: Guanggu Branch of Wuhan Rural Commercial Bank Co.,Ltd.

Pledgor: WUHAN COBOT TECHNOLOGY Co.,Ltd.

Registration number: Y2022420000085

PE01 Entry into force of the registration of the contract for pledge of patent right
PC01 Cancellation of the registration of the contract for pledge of patent right

Date of cancellation: 20230323

Granted publication date: 20220201

Pledgee: Guanggu Branch of Wuhan Rural Commercial Bank Co.,Ltd.

Pledgor: WUHAN COBOT TECHNOLOGY Co.,Ltd.

Registration number: Y2022420000085

PC01 Cancellation of the registration of the contract for pledge of patent right