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:
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:
wherein,
representing the force estimate in the contact point reference frame a,
representing a rotation transformation matrix transformed from the contact point reference frame a into the force measurement reference frame b,
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 b
3×3A zero matrix of 3 rows and 3 columns is shown,
representing the current force measurement converted from the contact point reference frame a into the force measurement reference frame b,
representing the coordinates of the center of gravity o of the tool articulated to the robot in the contact point reference coordinate system a,
the antisymmetrical operator corresponding to the coordinate representing the center of gravity o in the force measurement reference coordinate system b,
representing the coordinates in the force measurement reference coordinate system b with the center of gravity o,
indicating that the center of gravity o changes the current force measurement in the force measurement reference frame b
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
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](https://patentimages.storage.***apis.com/4b/f9/e1/1a897ea664bcfa/BDA00020562167900000712.png)
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:
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:
wherein, ω is
refRepresenting 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:
wherein q (t)
m) Representing an intermediate time t
mCorresponding desired servo control quantity, Δ q (t)
m) Representing an intermediate time t
mCorresponding deviation value of servo control parameter, Δ q (t)
m)/dt
mRepresenting the servo control parameter deviation Δ q (t)
m) To intermediate time t
mAnd (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:
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:
wherein,
representing the force estimate in the contact point reference frame a,
representing a rotation transformation matrix transformed from the contact point reference frame a into the force measurement reference frame b,
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 b
3×3A zero matrix of 3 rows and 3 columns is shown,
representing the current force measurement converted from the contact point reference frame a into the force measurement reference frame b,
representing the coordinates of the center of gravity o of the tool articulated to the robot in the contact point reference coordinate system a,
the antisymmetrical operator corresponding to the coordinate representing the center of gravity o in the force measurement reference coordinate system b,
representing the coordinates in the force measurement reference coordinate system b with the center of gravity o,
indicating that the center of gravity o changes the current force measurement in the force measurement reference frame b
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:
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:
wherein, ω is
refRepresenting 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:
wherein q (t)
m) Representing an intermediate time t
mCorresponding desired servo control quantity, Δ q (t)
m) Representing an intermediate time t
mCorresponding deviation value of servo control parameter, Δ q (t)
m)/dt
mRepresenting the servo control parameter deviation Δ q (t)
m) To intermediate time t
mAnd (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.