CN108972536B - System and method for determining kinetic parameters of mechanical arm and storage medium - Google Patents
System and method for determining kinetic parameters of mechanical arm and storage medium Download PDFInfo
- Publication number
- CN108972536B CN108972536B CN201710401262.2A CN201710401262A CN108972536B CN 108972536 B CN108972536 B CN 108972536B CN 201710401262 A CN201710401262 A CN 201710401262A CN 108972536 B CN108972536 B CN 108972536B
- Authority
- CN
- China
- Prior art keywords
- arm
- sub
- test
- joint
- kinetic
- 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
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/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
- B25J13/00—Controls for manipulators
- B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
- B25J13/088—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices with position, velocity or acceleration sensors
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/40—Robotics, robotics mapping to robotics vision
- G05B2219/40519—Motion, trajectory planning
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Human Computer Interaction (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a system and a method for determining kinetic parameters of a mechanical arm and a storage medium. The method comprises the following steps: the MEMS sensor with the same data as the sub-arm of the mechanical arm is used for measuring the sensor angular velocity and the sensor direction of each sub-arm, the joint velocity of the first sub-arm is obtained based on the sensor angular velocity of the first sub-arm connected with the base, the joint velocity of each sub-arm is sequentially calculated according to the relational expression between the joint velocities of the sub-arms, and then the joint acceleration of each sub-arm can be obtained by only conducting one-time derivation on the joint velocity of each sub-arm, so that the noise introduced in the secondary derivation process is reduced, the accuracy of parameter determination is improved, and the control performance of the mechanical arm is further guaranteed.
Description
Technical Field
The invention relates to the field of control, in particular to a system and a method for determining kinetic parameters of a mechanical arm and a storage medium.
Background
For a control mechanism based on a dynamic model, determining dynamic parameters in the dynamic model is a key step for constructing the dynamic model, and the control mechanism has good performance due to proper dynamic parameter setting. For a dynamic model of a mechanical arm, the dynamic parameters mainly include: mass, center of mass, and moment of inertia. At present, there are three main methods for determining kinetic parameters:
the first method comprises the following steps: and (4) directly measuring. The method uses a special device, such as a double-line pendulum or a three-line pendulum, to measure the mass center position and the rotational inertia of each section of arm of the mechanical arm. However, in the specific measurement, the arm sections are assembled first and the parameters of the arms are measured sequentially. This method requires high labor costs. Furthermore, it is difficult to measure the effect of non-rigid mechanics like friction.
And the second method comprises the following steps: and (4) CAD estimation method. This method is the most convenient method for determining kinetic parameters. But this method requires first establishing an accurate 3D model of all the components of the robot arm, otherwise it would be difficult to avoid errors. In fact, it is difficult to build accurate 3D models of robotic arms due to manufacturing and assembly errors. In addition, this method also does not allow an accurate estimation of the effect of friction.
And the third is that: a correction method based on a kinetic model. This method is widely used for parameter determination. First, the nonlinear dynamical equation is converted into a linear dynamical equation containing dynamical parameters. In addition, the linear kinetic equation also includes joint output torque, joint position, joint velocity and joint acceleration. The joint output torque can be measured by a torque sensor or calculated by motor current. In addition, the motor encoder may provide information on the joint position, and then derive the joint velocity by deriving the joint position, and the joint acceleration by deriving the joint velocity. And then substituting the obtained joint output torque, joint position, joint speed and joint acceleration into the linear dynamics equation to calculate the dynamics parameters.
However, the joint velocity and the joint acceleration are obtained by performing first derivation and second derivation on the joint position, and a large amount of noise is introduced in the derivation process. Those skilled in the art are also working to find other solutions for this purpose.
Disclosure of Invention
In view of this, the embodiments of the present invention provide a system for determining a kinetic parameter of a robot arm, and provide a method for determining a kinetic parameter of a robot arm and a computer-readable storage medium, so as to improve the accuracy of determining a parameter and further ensure the control performance of the robot arm.
According to the dynamics parameter determination system of the mechanical arm provided by the embodiment of the invention, the motion of each sub-arm of the mechanical arm is controlled by the corresponding motor driver and the corresponding motor, and a motor encoder for measuring the corresponding joint position of the mechanical arm is installed corresponding to each motor; the system comprises:
the MEMS sensors are the same as the sub-arms of the mechanical arm in number, one MEMS sensor is mounted on one section of the sub-arm, and each MEMS sensor comprises a gyroscope, an accelerometer and a magnetometer; the gyroscope is used for measuring the angular velocity of the sensor corresponding to the sub-arm, and the accelerometer and the magnetometer measure the direction of the sensor corresponding to the sub-arm together;
the first data receiving unit is used for periodically receiving first test data from each MEMS sensor according to a set test step length (delta t) in the process that the mechanical arm moves according to a set path; the first test data comprises sensor angular velocity and sensor direction from corresponding sub-arms of each MEMS sensor;
the second data receiving unit is used for periodically receiving second test data from each motor driver according to a set test step length (delta t) in the process that the mechanical arm moves according to a set path; the second test data includes: motor current of each sub-arm and joint position of each sub-arm;
the central processing unit is used for converting the motor current into joint output torque according to the test data received by each test step length; and according to the following formulas (1) and (2), the joint rotating speed of each sub-arm of the mechanical arm is obtained by sequentially calculating the angular speed and the direction of the sensor
For the calculated joint rotating speed
Obtaining the joint acceleration by once derivation; taking the joint output torque, the joint position, the joint rotating speed and the joint acceleration of each sub-arm obtained each time as a group of known parameter vectors; selecting M groups of known parameter vectors from all C groups of known parameter vectors obtained in the process of driving the mechanical arm to move according to a set path, substituting the selected M groups of known parameter vectors into a linear dynamics equation, and solving a target dynamics parameter; determining the target parameters as kinetic parameters of the mechanical arm kinetic model; wherein M x n is larger than the number of kinetic parameters to be solved in the linear kinetic equation;
wherein i is 0,1,2, …, n-1; n is the number of sub-arms of the mechanical arm;
the joint rotation speed of the (i + 1) th sub-arm,
is the z-axis direction of the coordinate system of the (i + 1) th sub-arm,iωiis the angular velocity of the ith sub-arm in the sub-arm coordinate system,
is a rotation matrix transformed from the coordinate system of the ith sub-arm to the coordinate system of the (i + 1) th sub-arm, ωi+1The sensor angular velocity measured for the sensor of the (i + 1) th sub-arm,i+1r is a rotation matrix transformed from a sensor coordinate system to an i +1 th sub-arm coordinate system; k is a radical ofx、ky、kzRepresenting the axis of rotation of the robot arm coordinate system,
is a scalar quantity calculated by the formula (3) based on the angular velocity measured by the sensor of the first sub-arm.
In one embodiment, before determining the target parameter as the kinetic parameter of the mechanical arm kinetic model, the central processing unit further selects N sets of known parameter vectors other than the M sets of known parameter vectors from the C sets of known parameter vectors as test parameter vectors or selects N sets of test data from the received first test data and second test data as test parameter vectors; selecting one value of each group of test parameter vectors in the N groups of test parameter vectors as a test value, substituting other related values and the solved target kinetic parameters into the linear kinetic equation, and solving a verification value corresponding to the test value; calculating the difference value between the verification value and the test value, and if the difference values between the verification value and the test value obtained by the N groups of test parameter vectors are smaller than a set error threshold value (epsilon), determining the target kinetic parameter as the kinetic parameter of the kinetic model; wherein N is greater than or equal to 1.
In one embodiment, the central processing unit uses the motor current or the joint output torque as a test value.
According to the method for determining the kinetic parameters of the mechanical arm, provided by the embodiment of the invention, the motion of each sub-arm of the mechanical arm is controlled by a corresponding motor driver and a corresponding motor; each section of sub-arm of the mechanical arm is provided with an MEMS sensor, and each MEMS sensor comprises a gyroscope, an accelerometer and a magnetometer; the method comprises the following steps:
periodically receiving test data according to a set test step length (delta t) in the process of driving the mechanical arm to move according to a set path; the test data includes: the sensor angular velocity corresponding to each sub-arm is measured by the gyroscope of each MEMS sensor, the sensor direction corresponding to each sub-arm is measured by the accelerometer and the magnetometer of each MEMS sensor together, and the motor current and the joint position of each corresponding sub-arm from each motor driver;
for the test data received by each test step, converting the motor current into joint output torque; and according to the following formulas (1) and (2), the joint rotating speed of each sub-arm of the mechanical arm is obtained by sequentially calculating the angular speed and the direction of the sensor
For the calculated joint rotating speed
Obtaining the joint acceleration by once derivation;
wherein i is 0,1,2, …, n-1; n is the number of sub-arms of the mechanical arm;
the joint rotation speed of the (i + 1) th sub-arm,
is the z-axis direction of the coordinate system of the (i + 1) th sub-arm,iωiis the angular velocity of the ith sub-arm in the sub-arm coordinate system,
is a rotation matrix transformed from the coordinate system of the ith sub-arm to the coordinate system of the (i + 1) th sub-arm, ωi+1The sensor angular velocity measured for the sensor of the (i + 1) th sub-arm,i+1r is a rotation matrix transformed from a sensor coordinate system to an i +1 th sub-arm coordinate system; k is a radical ofx、ky、kzRepresenting the axis of rotation of the robot arm coordinate system,
is a scalar quantity calculated by formula (3) according to the angular velocity measured by the sensor of the first sub-arm;
taking the joint output torque, the joint position, the joint rotating speed and the joint acceleration of each sub-arm obtained each time as a group of known parameter vectors;
selecting M groups of known parameter vectors from all C groups of known parameter vectors obtained in the process of driving the mechanical arm to move according to a set path, substituting the selected M groups of known parameter vectors into a linear dynamics equation, and solving a target dynamics parameter; wherein M x n is larger than the number of kinetic parameters to be solved in the linear kinetic equation;
and determining the target parameters as the dynamic parameters of the mechanical arm dynamic model.
In one embodiment, before determining the target parameter as the kinetic parameter of the mechanical arm kinetic model, the method further comprises:
selecting N groups of known parameter vectors except the M groups of known parameter vectors from the C groups of known parameter vectors as test parameter vectors or selecting N groups of test data from the received test data as test parameter vectors;
selecting one value of each group of test parameter vectors in the N groups of test parameter vectors as a test value, substituting other related values and the solved target kinetic parameters into the linear kinetic equation, and solving a verification value corresponding to the test value;
calculating the difference value between the verification value and the test value, and if the difference values between the verification value and the test value obtained by the N groups of test parameter vectors are smaller than a set error threshold, determining the target kinetic parameter as the kinetic parameter of the kinetic model;
wherein N is greater than or equal to 1.
In one embodiment, the selecting a value as the test value is: and selecting the current of the motor or the output torque of the joint as a test value.
The computer-readable storage medium provided in the embodiments of the present invention has a computer program stored thereon, where the computer program is executed by a processor to implement the steps of the method for determining the kinetic parameters of a mechanical arm kinetic model described above.
According to the scheme, the MEMS sensors with the same number as the sub-arms of the mechanical arm are only needed to be arranged to measure the relative angular velocity of each sub-arm of the mechanical arm, the joint velocity of each sub-arm is sequentially calculated based on the joint velocity of the first sub-arm connected with the base according to the relational expression between the joint velocities of each sub-arm, and then the joint acceleration of each sub-arm can be obtained by conducting derivation on the joint velocity of each sub-arm once, so that the noise introduced in the secondary derivation process is reduced, the accuracy of parameter determination is improved, and the control performance of the mechanical arm is further ensured.
Furthermore, after the target kinetic parameters are solved, N groups of test parameter vectors are used for verifying the correctness of the kinetic parameters, so that the accuracy of parameter determination can be further improved, and the control performance of the mechanical arm is further guaranteed.
In addition, only the MEMS sensor is added, so that the cost is low, the MEMS sensor can be mounted at any position of each sub-arm of the mechanical arm, and meanwhile, the dynamic parameter determination system of the mechanical arm dynamic model can be used as a plug-and-play plug-in, so that the integration is convenient. And the mechanical arm is only required to move once according to the preset track, all data can be acquired, and the kinetic parameters can be determined, so that the method is convenient and fast to realize.
Drawings
The foregoing and other features and advantages of the invention will become more apparent to those skilled in the art to which the invention relates upon consideration of the following detailed description of a preferred embodiment of the invention with reference to the accompanying drawings, in which:
fig. 1 is an exemplary configuration diagram of a kinetic parameter determination system of a robot arm in an embodiment of the present invention.
Fig. 2 is an exemplary flowchart of a method for determining a kinetic parameter of a robotic arm in an embodiment of the present invention.
Wherein the reference numbers are as follows:
Detailed Description
In the embodiment of the invention, in order to improve the accuracy of parameter determination and further ensure the control performance of the mechanical arm, the joint speed and the joint acceleration are not obtained by carrying out primary derivation and secondary derivation on the joint position, but the joint speed of each sub-arm is obtained by measuring the relative angular speed of each sub-arm of the mechanical arm and based on the relative angular speed of the first sub-arm connected with the base, the joint speed of each sub-arm is sequentially calculated according to the relational expression between the joint speeds of each sub-arm, and then the joint acceleration of each sub-arm can be obtained by carrying out primary derivation on the joint speed of each sub-arm, so that the noise introduced in the secondary derivation process is reduced. In order to measure the relative angular velocity of the various sub-arms of the robot arm, a Micro Electro Mechanical Systems (MEMS) sensor is introduced in this embodiment.
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is further described in detail by referring to the following examples.
Fig. 1 is an exemplary configuration diagram of a kinetic parameter determination system of a robot arm in an embodiment of the present invention. In this embodiment, the movement of each sub-arm of the robot arm 10 is controlled by a motor driver 20 and a motor (not shown) installed corresponding to the sub-arm, and a motor encoder (not shown) for measuring the joint position of the corresponding sub-arm is installed corresponding to each motor. Accordingly, the motor current and the joint position corresponding to each sub-arm can be directly obtained from the corresponding motor driver 20. Wherein, the motor can be servo motor, and the motor drive can be servo motor drive. As shown in fig. 1, the system for determining the kinetic parameters of the robot arm in the present embodiment may include: a plurality of MEMS sensors 110, a first data receiving unit 120, a second data receiving unit 130, and a central processing unit 140.
The number of the MEMS sensors 110 is the same as the number of the sub-arms of the robot arm, for example, the number of the MEMS sensors 110 is 3 corresponding to the robot arm with three sub-arms in fig. 1. For another example, for a six-segment sub-arm robot arm, the number of MEMS sensors 110 is 6. Wherein, a MEMS sensor 110 is mounted on a sub-arm of the mechanical arm, the mounting position can be any position of the sub-arm, and each MEMS sensor 110 can include a gyroscope, an accelerometer and a magnetometer; the gyroscope is used for measuring and obtaining the angular velocity of the sensor corresponding to the sub-arm, and the accelerometer and the magnetometer jointly measure and obtain the direction of the sensor corresponding to the sub-arm.
The first data receiving unit 120 is configured to periodically receive first test data from each MEMS sensor 110 according to a set test step Δ t during a process in which the robot arm 10 moves according to a set path; the first test data includes sensor angular velocity and sensor orientation from corresponding sub-arms of each MEMS sensor 110. In the embodiment of the present invention, the first data receiving unit 120 may be a hardware module such as an ARM processor, and may also be other integrated circuits.
The second data receiving unit 130 is configured to periodically receive second test data from each motor driver according to a set test step Δ t during the process that the robot arm 10 moves according to the set path; the second test data includes: motor current of each sub-arm and joint position of each sub-arm. In the embodiment of the present invention, the second data receiving unit 130 may be an interface module of an Industrial Personal Computer (IPC). In other embodiments, the first data receiving unit 120 and the second data receiving unit 130 may be the same data receiving unit.
The central processing unit 140 is configured to store the test data received by the first data receiving unit 120 and the second data receiving unit 130 at each test step Δ t; for the test data group received by each test step delta t, converting the motor current in the test data group into joint output torque; and according to the following formulas (1) and (2), the joint rotating speed of each sub-arm of the mechanical arm is obtained by sequentially calculating the angular speed and the direction of the sensor
For the calculated joint rotating speed
Obtaining the joint acceleration by once derivation; taking the joint output torque, the joint position, the joint rotating speed and the joint acceleration of each sub-arm obtained each time as a group of known parameter vectors; selecting M groups of known parameter vectors from all C groups of known parameter vectors obtained in the process of driving the mechanical arm 10 to move according to a set path, and substituting the selected M groups of known parameter vectors into the linear motionSolving target kinetic parameters in a kinetic equation; and determining the target parameters as the dynamic parameters of the mechanical arm dynamic model. Wherein M n is larger than the number of kinetic parameters to be solved in the linear kinetic equation.
Wherein i is 0,1,2, …, n-1; n is the number of sub-arms of the mechanical arm;
the joint rotation speed of the (i + 1) th sub-arm,
is the z-axis direction of the coordinate system of the (i + 1) th sub-arm,iωiis the angular velocity of the ith sub-arm in the sub-arm coordinate system,
is a rotation matrix transformed from the coordinate system of the ith sub-arm to the coordinate system of the (i + 1) th sub-arm, ωi+1The sensor angular velocity measured for the sensor of the (i + 1) th sub-arm,i+1r is a rotation matrix transformed from a sensor coordinate system to an i +1 th sub-arm coordinate system; k is a radical ofx、ky、kzRepresenting the axis of rotation of the robot arm coordinate system,
is a scalar quantity calculated by the formula (3) based on the angular velocity measured by the sensor of the first sub-arm. Wherein the rotation between the sensor coordinate system and the coordinate system of the corresponding sub-arm of the mechanical armThe relationship can be obtained from data such as joint position, angular velocity, and sensor direction of the sub-arm.
In the embodiment of the invention, the central processing unit can be an industrial personal computer IPC or other types of integrated modules.
Specifically, according to the equation (2), the angular velocity of the 1 st sub-arm in the sub-arm coordinate system can be obtained1ω1：
Further, the joint rotation speed of the second sub-arm can be obtained from equation (4):
and by analogy, after the angular speed and the sensor direction of the previous section of sub-arm are obtained, the joint rotating speed of each section of sub-arm can be obtained in sequence.
In the present embodiment, the expression of the linear kinetic equation may be represented by the following formula (6):
I＝Φγ+f (6)
wherein, I is a joint output torque represented by motor current, phi is a known parameter matrix obtained by calculation according to joint positions, joint rotating speeds and joint accelerations of all sub-arms of the mechanical arm, f is a friction force obtained from a predetermined friction force model, and gamma is a dynamic parameter vector comprising the mass, the mass center, the rotational inertia and the rotational inertia of all the sub-arms.
For each parameter in the kinetic parameter vector, the mass, the centroid, etc. of each sub-arm can be directly measured, or can be obtained by the above calculation. If the vector is obtained by direct measurement, it can be further used as a known quantity to solve other unknown quantities in the kinetic parameter vector.
In another embodiment, before the cpu 140 determines the target parameter as the kinetic parameter of the mechanical arm kinetic model, N groups of known parameter vectors except the M groups of known parameter vectors may be further selected from the C groups of known parameter vectors as test parameter vectors or N groups of test data may be selected from the stored C groups of test data as test parameter vectors before calculating the joint rotation speed of each sub-arm; selecting one value of each group of test parameter vectors in the N groups of test parameter vectors as a test value, substituting other related values and the solved target kinetic parameters into the linear kinetic equation, and solving a verification value corresponding to the test value; and calculating the difference value between the verification value and the test value, and if the difference values between the verification value and the test value obtained by the N groups of test parameter vectors are less than a set error threshold epsilon, determining that the target kinetic parameter is the kinetic parameter of the kinetic model. Wherein N is greater than or equal to 1; otherwise, the target kinetic parameters may be re-determined.
In one embodiment, the motor current or the joint output torque represented by the motor current can be used as the test value.
The system for determining the kinetic parameters of the mechanical arm kinetic model in the embodiment of the present invention is described in detail above, and the method for determining the kinetic parameters of the mechanical arm kinetic model in the embodiment of the present invention is described in detail below.
Fig. 2 is an exemplary flowchart of a method for determining a kinetic parameter of a robotic arm in an embodiment of the present invention. In this embodiment, the motion of each sub-arm of the robot arm is controlled by a motor driver (not shown) and a motor (not shown) installed corresponding to the sub-arm, and a motor encoder (not shown) for measuring the joint position of the corresponding sub-arm is installed corresponding to each motor. Accordingly, the motor current and the joint position corresponding to each sub-arm can be directly obtained from the corresponding motor driver. Wherein, the motor can be servo motor, and the motor drive can be servo motor drive. In addition, in the embodiment, each sub-arm of the mechanical arm is provided with one MEMS sensor, and each MEMS sensor includes a gyroscope, an accelerometer and a magnetometer. As shown in fig. 2, the method for determining the kinetic parameters of the mechanical arm kinetic model in the present embodiment may include the following steps:
in step 210, the central processing unit receives initialization data. Wherein initializing data may include: the motion path of the mechanical arm, the test step length delta t, the error threshold value, the group number N of the test parameter vectors and the like. Wherein N is greater than or equal to 1.
wherein i is 0,1,2, …, n-1; n is the number of sub-arms of the mechanical arm;
the joint rotation speed of the (i + 1) th sub-arm,
is the z-axis direction of the coordinate system of the (i + 1) th sub-arm,iωiis the angular velocity of the ith sub-arm in the sub-arm coordinate system,
is a rotation matrix transformed from the coordinate system of the ith sub-arm to the coordinate system of the (i + 1) th sub-arm, ωi+1The sensor angular velocity measured for the sensor of the (i + 1) th sub-arm,i+1r is a rotation matrix transformed from a sensor coordinate system to an i +1 th sub-arm coordinate system; k is a radical ofx、ky、kzRepresenting the axis of rotation of the robot arm coordinate system,
is a scalar quantity calculated by the formula (3) based on the angular velocity measured by the sensor of the first sub-arm.
And step 240, storing the joint output torque, the joint position, the joint rotating speed and the joint acceleration of each sub-arm obtained each time as a group of known parameter vectors.
And step 250, selecting M groups of known parameter vectors from all the C groups of known parameter vectors obtained in the process of driving the mechanical arm to move according to the set path, substituting the selected M groups of known parameter vectors into a linear dynamics equation, and solving the target dynamics parameter. And M x n is greater than the number of kinetic parameters to be solved in the linear kinetic equation, and n is the number of sub-arms of the mechanical arm.
And 270, selecting one value of each group of test parameter vectors in the N groups of test parameter vectors as a test value, substituting other values and the solved target kinetic parameters into the linear kinetic equation, and solving a verification value corresponding to the test value. In one embodiment, the motor current or the joint output torque represented by the motor current may be selected as the test value.
In some embodiments, step 290 may not be performed after step 260 to step 280, i.e., step 250 is performed.
The present invention also provides a computer-readable storage medium storing instructions for causing a computer to perform a method for determining kinetic parameters of a kinetic model of a mechanical arm as described herein. Specifically, a system or an apparatus equipped with a storage medium on which software program codes that realize the functions of any of the above-described embodiments are stored may be provided, and a computer (or a CPU or MPU) of the system or the apparatus is caused to read out and execute the program codes stored in the storage medium.
In this case, the program code itself read from the storage medium can realize the functions of any of the above-described embodiments, and thus the program code and the storage medium storing the program code constitute a part of the present invention.
Examples of the storage medium for supplying the program code include a flexible disk, a hard disk, a magneto-optical disk, an optical disk (e.g., CD-ROM, CD-R, CD-RW, DVD-ROM, DVD-RAM, DVD-RW, DVD + RW), a magnetic tape, a nonvolatile memory card, a ROM, a PC, a cellular phone, and various smart devices. Alternatively, the program code may be downloaded from a server computer via a communications network.
Further, it should be clear that the functions of any one of the above-described embodiments may be implemented not only by executing the program code read out by the computer, but also by causing an operating system or the like operating on the computer to perform a part or all of the actual operations based on instructions of the program code.
Further, it is to be understood that the program code read out from the storage medium is written to a memory provided in an expansion board inserted into the computer or to a memory provided in an expansion unit connected to the computer, and then causes a CPU or the like mounted on the expansion board or the expansion unit to perform part or all of the actual operations based on instructions of the program code, thereby realizing the functions of any of the above-described embodiments.
According to the scheme, the MEMS sensors with the same number as the sub-arms of the mechanical arm are only needed to be arranged to measure the relative angular velocity of each sub-arm of the mechanical arm, the joint velocity of each sub-arm is sequentially calculated based on the joint velocity of the first sub-arm connected with the base according to the relational expression between the joint velocities of each sub-arm, and then the joint acceleration of each sub-arm can be obtained by conducting derivation on the joint velocity of each sub-arm once, so that the noise introduced in the secondary derivation process is reduced, the accuracy of parameter determination is improved, and the control performance of the mechanical arm is further ensured.
Furthermore, after the target kinetic parameters are solved, N groups of test parameter vectors are used for verifying the correctness of the kinetic parameters, so that the accuracy of parameter determination can be further improved, and the control performance of the mechanical arm is further guaranteed.
In addition, only the MEMS sensor is added, so that the cost is low, the MEMS sensor can be mounted at any position of each sub-arm of the mechanical arm, and meanwhile, the dynamic parameter determination system of the mechanical arm dynamic model can be used as a plug-and-play plug-in, so that the integration is convenient. And the mechanical arm is only required to move once according to the preset track, all data can be acquired, and the kinetic parameters can be determined, so that the method is convenient and fast to realize.
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 (7)
1. The system comprises a dynamic parameter determining system of the mechanical arm, wherein the motion of each sub-arm of the mechanical arm is controlled by a corresponding motor driver and a corresponding motor, and a motor encoder for measuring the position of a corresponding joint of the mechanical arm is installed corresponding to each motor; characterized in that the system comprises:
a plurality of MEMS sensors in the same number as the sub-arms of the mechanical arm, one MEMS sensor being mounted on a section of the sub-arm, and each MEMS sensor comprising a gyroscope, an accelerometer and a magnetometer; the gyroscope is used for measuring the angular velocity of the sensor corresponding to the sub-arm, and the accelerometer and the magnetometer measure the direction of the sensor corresponding to the sub-arm together;
the first data receiving unit is used for periodically receiving first test data from each MEMS sensor according to a set test step length delta t in the process that the mechanical arm moves according to a set path; the first test data comprises sensor angular velocity and sensor direction from corresponding sub-arms of each MEMS sensor;
the second data receiving unit is used for periodically receiving second test data from each motor driver according to a set test step length delta t in the process that the mechanical arm moves according to a set path; the second test data includes: motor current of each sub-arm and joint position of each sub-arm;
the central processing unit is used for converting the motor current into joint output torque according to the test data received by each test step length; and according to the following formulas (1) and (2), the joint rotating speed of each sub-arm of the mechanical arm is obtained by sequentially calculating the angular speed and the direction of the sensor
For the calculated joint rotating speed
Obtaining the joint acceleration by once derivation; taking the joint output torque, the joint position, the joint rotating speed and the joint acceleration of each sub-arm obtained each time as a group of known parameter vectors; selecting M groups of known parameter vectors from all C groups of known parameter vectors obtained in the process of driving the mechanical arm to move according to a set path, substituting the selected M groups of known parameter vectors into a linear dynamics equation, and solving a target dynamics parameter; determining the target kinetic parameters as kinetic parameters of a mechanical arm kinetic model; wherein M x n is larger than the number of kinetic parameters to be solved in the linear kinetic equation;
wherein i is 0,1,2, …, n-1; n is the number of sub-arms of the mechanical arm;
the joint rotation speed of the (i + 1) th sub-arm,
is the z-axis direction of the coordinate system of the (i + 1) th sub-arm,iωiis the angular velocity of the ith sub-arm in the sub-arm coordinate system,
is a rotation matrix transformed from the coordinate system of the ith sub-arm to the coordinate system of the (i + 1) th sub-arm, ωi+1The sensor angular velocity measured for the sensor of the (i + 1) th sub-arm,i+1r is a rotation matrix transformed from a sensor coordinate system to an i +1 th sub-arm coordinate system; k is a radical ofx、ky、kzRepresenting the axis of rotation of the robot arm coordinate system,
is a scalar quantity calculated by the formula (3) based on the angular velocity measured by the sensor of the first sub-arm.
2. The system of claim 1, wherein before the cpu determines the target kinetic parameter as the kinetic parameter of the mechanical arm kinetic model, the cpu further selects N sets of known parameter vectors other than the M sets of known parameter vectors from the C sets of known parameter vectors as test parameter vectors or selects N sets of test data from the received first and second test data as test parameter vectors; selecting one value of each group of test parameter vectors in the N groups of test parameter vectors as a test value, substituting the solved target kinetic parameter into the linear kinetic equation, and solving a verification value corresponding to the test value; calculating the difference value between the verification value and the test value, and if the difference values between the verification value and the test value obtained by the N groups of test parameter vectors are smaller than a set error threshold epsilon, determining the target kinetic parameter as the kinetic parameter of the kinetic model; wherein N is greater than or equal to 1.
3. The system of claim 2, wherein the central processing unit takes the motor current or the joint output torque as a test value.
4. The method comprises the steps that the kinetic parameters of the mechanical arm are determined, and the motion of each sub-arm of the mechanical arm is controlled by a corresponding motor driver and a corresponding motor; the mechanical arm is characterized in that each sub-arm of the mechanical arm is provided with an MEMS sensor, and each MEMS sensor comprises a gyroscope, an accelerometer and a magnetometer; the method comprises the following steps:
periodically receiving test data according to a set test step length (delta t) in the process of driving the mechanical arm to move according to a set path; the test data includes: the sensor angular velocity corresponding to each sub-arm is measured by the gyroscope of each MEMS sensor, the sensor direction corresponding to each sub-arm is measured by the accelerometer and the magnetometer of each MEMS sensor together, and the motor current and the joint position of each corresponding sub-arm from each motor driver;
for the test data received by each test step, converting the motor current into joint output torque; and according to the following formulas (1) and (2), the joint rotating speed of each sub-arm of the mechanical arm is obtained by sequentially calculating the angular speed and the direction of the sensor
For the calculated joint rotating speed
Obtaining the joint acceleration by once derivation;
wherein, i is 0,1,2, …,n-1; n is the number of sub-arms of the mechanical arm;
the joint rotation speed of the (i + 1) th sub-arm,
is the z-axis direction of the coordinate system of the (i + 1) th sub-arm,iωiis the angular velocity of the ith sub-arm in the sub-arm coordinate system,
is a rotation matrix transformed from the coordinate system of the ith sub-arm to the coordinate system of the (i + 1) th sub-arm, ωi+1The sensor angular velocity measured for the sensor of the (i + 1) th sub-arm,i+1r is a rotation matrix transformed from a sensor coordinate system to an i +1 th sub-arm coordinate system; k is a radical ofx、ky、kzRepresenting the axis of rotation of the robot arm coordinate system,
is a scalar quantity calculated by formula (3) according to the angular velocity measured by the sensor of the first sub-arm;
taking the joint output torque, the joint position, the joint rotating speed and the joint acceleration of each sub-arm obtained each time as a group of known parameter vectors;
selecting M groups of known parameter vectors from all C groups of known parameter vectors obtained in the process of driving the mechanical arm to move according to a set path, substituting the selected M groups of known parameter vectors into a linear dynamics equation, and solving a target dynamics parameter; wherein M x n is larger than the number of kinetic parameters to be solved in the linear kinetic equation;
and determining the target kinetic parameters as kinetic parameters of a mechanical arm kinetic model.
5. The method of claim 4, wherein prior to determining the target kinetic parameter as the kinetic parameter of the mechanical arm kinetic model, the method further comprises:
selecting N groups of known parameter vectors except the M groups of known parameter vectors from the C groups of known parameter vectors as test parameter vectors or selecting N groups of test data from the received test data as test parameter vectors;
selecting one value of each group of test parameter vectors in the N groups of test parameter vectors as a test value, substituting the solved target kinetic parameter into the linear kinetic equation, and solving a verification value corresponding to the test value;
calculating the difference value between the verification value and the test value, and if the difference values between the verification value and the test value obtained by the N groups of test parameter vectors are smaller than a set error threshold value (epsilon), determining the target kinetic parameter as the kinetic parameter of the kinetic model;
wherein N is greater than or equal to 1.
6. A method of determining kinetic parameters of a robotic arm as claimed in claim 5, wherein the selected value as a test value is: and selecting the current of the motor or the output torque of the joint as a test value.
7. A computer-readable storage medium, on which a computer program is stored, which, when being executed by a processor, carries out the steps of the method for determining a kinetic parameter of a robot arm according to any of the claims 4 to 6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710401262.2A CN108972536B (en) | 2017-05-31 | 2017-05-31 | System and method for determining kinetic parameters of mechanical arm and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710401262.2A CN108972536B (en) | 2017-05-31 | 2017-05-31 | System and method for determining kinetic parameters of mechanical arm and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108972536A CN108972536A (en) | 2018-12-11 |
| CN108972536B true CN108972536B (en) | 2021-06-22 |
Family
ID=64500971
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710401262.2A Active CN108972536B (en) | 2017-05-31 | 2017-05-31 | System and method for determining kinetic parameters of mechanical arm and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108972536B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110834332B (en) * | 2019-11-13 | 2021-10-22 | 珠海格力智能装备有限公司 | Manipulator control method and device and manipulator |
| CN111551973B (en) * | 2020-04-16 | 2022-04-05 | 北京踏歌智行科技有限公司 | Fault detection and correction method for unmanned inertial navigation system of strip mine |
| CN111421551A (en) * | 2020-05-08 | 2020-07-17 | 哈尔滨理工大学 | Robot joint information acquisition method |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1649698A (en) * | 2002-03-18 | 2005-08-03 | 索尼株式会社 | Robot device, legged locomotion robot operation control device and operation control method, legged locomotion robot sensor system, and locomotion device |
| CN102328312A (en) * | 2010-07-12 | 2012-01-25 | 精工爱普生株式会社 | Manipulator and method for controlling the manipulator |
| CN102398262A (en) * | 2010-08-06 | 2012-04-04 | 精工爱普生株式会社 | Robotic device, method for controlling robotic device, and computer program |
| CN103433924A (en) * | 2013-07-26 | 2013-12-11 | 无锡信捷电气股份有限公司 | High-accuracy position control method for serial robot |
| CN104339351A (en) * | 2013-08-05 | 2015-02-11 | 株式会社东芝 | Robot control device |
| CN106054599A (en) * | 2016-05-25 | 2016-10-26 | 哈尔滨工程大学 | Master-slave underwater robotic arm delay control method |
| CN106064377A (en) * | 2016-06-02 | 2016-11-02 | 西北工业大学 | A kind of excitation track optimizing method of robot for space dynamic parameters identification |
| DE202016007293U1 (en) * | 2016-11-29 | 2016-12-23 | Kuka Roboter Gmbh | Passive manipulator |
-
2017
- 2017-05-31 CN CN201710401262.2A patent/CN108972536B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1649698A (en) * | 2002-03-18 | 2005-08-03 | 索尼株式会社 | Robot device, legged locomotion robot operation control device and operation control method, legged locomotion robot sensor system, and locomotion device |
| CN102328312A (en) * | 2010-07-12 | 2012-01-25 | 精工爱普生株式会社 | Manipulator and method for controlling the manipulator |
| CN102398262A (en) * | 2010-08-06 | 2012-04-04 | 精工爱普生株式会社 | Robotic device, method for controlling robotic device, and computer program |
| CN103433924A (en) * | 2013-07-26 | 2013-12-11 | 无锡信捷电气股份有限公司 | High-accuracy position control method for serial robot |
| CN104339351A (en) * | 2013-08-05 | 2015-02-11 | 株式会社东芝 | Robot control device |
| CN106054599A (en) * | 2016-05-25 | 2016-10-26 | 哈尔滨工程大学 | Master-slave underwater robotic arm delay control method |
| CN106064377A (en) * | 2016-06-02 | 2016-11-02 | 西北工业大学 | A kind of excitation track optimizing method of robot for space dynamic parameters identification |
| DE202016007293U1 (en) * | 2016-11-29 | 2016-12-23 | Kuka Roboter Gmbh | Passive manipulator |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108972536A (en) | 2018-12-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN104246433B (en) | For automatic calibrating method in the use of the sensor in mobile device | |
| CN108972536B (en) | System and method for determining kinetic parameters of mechanical arm and storage medium | |
| KR20210084622A (en) | Time synchronization processing methods, electronic devices and storage media | |
| KR100898169B1 (en) | Initial alignment method of inertial navigation system | |
| CN104756039A (en) | Estimating the gravity vector in a world coordinate system using an accelerometer in a mobile device | |
| CN113984051B (en) | Method, device, equipment and storage medium for fusion of IMU and rigid body posture | |
| CN109030867A (en) | The method and apparatus for calculating angular speed using acceleration transducer and geomagnetic sensor | |
| CN108318027B (en) | Method and device for determining attitude data of carrier | |
| CN113551690A (en) | Calibration parameter acquisition method and device, electronic equipment and storage medium | |
| CN111998870B (en) | Calibration method and device of camera inertial navigation system | |
| CN113110107B (en) | Unmanned aerial vehicle flight control simulation system, device and storage medium | |
| CN113465632B (en) | Sensor calibration method, device, equipment and medium | |
| CN113283152B (en) | Adjustment method and driving method of driving electric signal, driving circuit and electronic equipment | |
| CN114913242A (en) | Camera calibration method, device, system, electronic equipment and storage medium | |
| CN112363196B (en) | Vehicle attribute determining method, device, storage medium and electronic equipment | |
| CN107423515B (en) | Mechanical arm friction identification method, device, equipment and storage medium | |
| CN113340324A (en) | Visual inertia self-calibration method based on depth certainty strategy gradient | |
| CN115135962A (en) | Method and apparatus for zero g offset calibration of MEMS-based accelerometers | |
| CN113065572A (en) | Multi-sensor fusion data processing method, positioning device and virtual reality equipment | |
| CN110987018B (en) | Specific force differential position method DVL error calibration method and system | |
| CN114199278A (en) | Low-precision inertial measurement unit parameter estimation method, device, equipment and storage medium | |
| CN115855072B (en) | Gesture estimation method, device and equipment of driving simulation platform and storage medium | |
| CN115326106B (en) | Improved method for inertial sensor data simulation | |
| CN111568304A (en) | Sweeping robot positioning method and device and sweeping robot | |
| EP4375618A1 (en) | Dynamic gravity vector estimation for memory constrained devices |
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 |