A calibration method for inertial measurement unit and robot posture and its application

Abstract

The invention provides a calibration method for an inertial measurement unit and a robot posture, wherein the inertial measurement unit is fixed at the end of the robot; the robot drives the inertial measurement unit to move respectively along the three coordinate axis directions of the robot tool coordinate system, integrates the acceleration data to obtain the displacement data, and obtains the direction vector of the displacement data relative to the origin; the direction vectors respectively corresponding to the three coordinate axes constitute a rotation matrix; the robot TCP is kept at the origin, and drives the inertial measurement unit to perform circular motion; the spherical center coordinates are fitted according to the displacement data recorded at different times; the vector from the spherical center coordinates to the initial coordinates is calculated and recorded as a translation matrix; the method can obtain the rotation and translation relationship between the inertial measurement unit coordinate system and the robot tool coordinate system, convert the inertial measurement unit output data to the robot tool coordinate system, assist the robot trajectory planning, and avoid sudden acceleration / deceleration during the operation of the robot.

Description

Technical Field

[0001] The present invention relates to the application field of inertial measurement units, and in particular to a method for calibrating an inertial measurement unit and a robot posture and an application thereof. Background Art

[0002] In modern industrial production and processing scenarios, robots have been widely used and received attention. Before the robot is put into use, it is necessary to teach the robot trajectory and plan the moving path. Different trajectories often need to be planned for different production and processing scenarios. When planning the robot trajectory, the path points in the trajectory are given, and the robot passes through each path point in turn. Due to unreasonable setting of the path point position or too few path points, the robot end may experience rapid acceleration / deceleration, which is not conducive to the stable operation of the robot. For this reason, a method is needed to monitor the motion state of the robot end to assist in robot trajectory planning.

[0003] Among them, the inertial measurement unit (IMU) can measure the angular velocity and acceleration of an object in three-dimensional space. It includes a three-axis accelerometer: used to measure the acceleration relative to the earth's gravity field, more specifically, the three-axis accelerometer is used to measure the acceleration of each axis of the x, y and z axes; it also includes a three-axis gyroscope: the gyroscope is used to measure the rotation rate and provide the angular velocity of each axis of the x, y and z axes. When the inertial measurement unit is carried by a mobile device to move, it can output acceleration data and angular velocity data in real time, obtain displacement data by integration, and detect the movement, acceleration and direction change of the object.

[0004] In view of the above characteristics, an inertial measurement unit (IMU) can be installed at the end of the robot during path planning. However, the acceleration and angular velocity output by the inertial measurement unit (IMU) are only data in its own coordinate system, and it is impossible to directly map the abnormal position information to the robot tool coordinate system to assist in checking the specific location of sudden acceleration / deceleration when the robot is running. Summary of the invention

[0005] In order to solve the above technical problems, the present invention provides a calibration method for an inertial measurement unit (IMU) and a robot posture, which aims to obtain the rotational and translational relationship between the inertial measurement unit coordinate system and the robot tool coordinate system, establish a connection between the inertial measurement unit output data and the robot motion posture, and convert the inertial measurement unit output data to the robot tool coordinate system to assist the robot trajectory planning and avoid sudden acceleration / deceleration during the robot operation.

[0006] The technical solution is as follows:

[0007] A method for calibrating an inertial measurement unit and a robot posture, wherein the robot is a robot with six axes or more, and the inertial measurement unit is fixed at the end of the robot and moves synchronously with the robot;

[0008] The calibration method comprises the following steps:

[0009] Step 1: Define a certain position of the robot TCP in space as the origin;

[0010] Step 2: Obtain the rotation matrix and translation matrix between the inertial measurement unit coordinate system and the robot tool coordinate system;

[0011] The steps to obtain the rotation matrix are as follows:

[0012] ① Starting from the origin, the robot drives the inertial measurement unit to continuously move along any coordinate axis direction of the robot tool coordinate system for a preset time and then stops; during the movement, the inertial measurement unit continuously outputs acceleration data;

[0013] Integrating the acceleration data to obtain displacement data, and obtaining a direction vector of the displacement data relative to the origin;

[0014] The robot drives the inertial measurement unit back to the origin, and the inertial measurement unit clears the acceleration data;

[0015] ②Perform step ① along the other two coordinate axes of the robot tool coordinate system;

[0016] The direction vector obtained when moving along the X axis is recorded as

[0017] The direction vector obtained when moving along the Y axis is recorded as

[0018] The direction vector obtained when moving along the Z axis is recorded as

[0019] Then the rotation matrix

[0020] The steps to obtain the translation matrix are as follows:

[0021] S1. Keep the robot TCP at the origin and rotate the other joint arms of the robot to drive the inertial measurement unit to perform at least two circular motions in different directions, and the circular motions are all located on the same spatial sphere;

[0022] During the circular motion, the inertial measurement unit continuously outputs acceleration data;

[0023] Integrating the acceleration data respectively to obtain displacement data recorded at different times;

[0024] Fit the sphere according to the displacement data recorded at different times to obtain the coordinates of the sphere center;

[0025] S2. Calculate the vector from the sphere center coordinate to the initial coordinate, recorded as the translation matrix T, where the initial coordinate is the displacement data obtained by integrating the first acceleration data output by the inertial measurement unit during the circular motion.

[0026] Furthermore, the acceleration data is integrated to obtain the displacement data as follows:

[0027] During the movement, the inertial measurement unit continuously outputs acceleration data. After stopping the movement, a total of t groups of acceleration data output at different times are recorded and stored as

[0028] Integrate t groups of acceleration data to obtain t groups of velocity data Then integrate the t groups of velocity data to obtain the t groups of displacement data

[0029] in, Represents t groups of acceleration data in the X-axis direction, Represents t groups of acceleration data in the Y-axis direction, Represents t groups of acceleration data in the Z-axis direction; i=1, 2...t, Δt represents the time interval for the inertial measurement unit to output acceleration data.

[0030] Further, in step ①, the formula for obtaining the direction vector of the displacement data relative to the origin is:

[0031] Direction Vector in, Indicates that the displacement data is obtained by integrating the acceleration data.

[0032] Furthermore, the translation matrix

[0033] Among them, (x′, y′, z′) represents the coordinates of the fitted sphere center, (x1, y1, z1) represents the initial coordinates, This is the first set of acceleration data output by the inertial measurement unit.

[0034] Preferably, the preset time value is 1 to 5 seconds.

[0035] Preferably, for the rotation matrix Perform orthogonalization.

[0036] Further, the robot TCP is the center of the robot end flange; or, a gun tip is fixed at the end of the robot, the gun tip is used to extend the center of the robot end flange to the outside of the robot body axis, and the tip position of the gun tip is recorded as the robot TCP.

[0037] The present invention also relates to a method for adjusting the trajectory of a robot using a rotation matrix and a translation matrix obtained by calibration, comprising the following steps:

[0038] I. After completing the robot trajectory planning, the robot drives the inertial measurement unit to run the planned trajectory. During the operation, the inertial measurement unit continuously outputs acceleration data and angular velocity data;

[0039] According to the rotation matrix and translation matrix obtained by calibration, each output acceleration data and each angular velocity data are converted into the robot tool coordinate system;

[0040] II. Compare each converted acceleration data and each angular velocity data with their corresponding thresholds, and mark the acceleration data and angular velocity data greater than the threshold as abnormal data;

[0041] The displacement data corresponding to the abnormal data is obtained by integration and recorded as the abnormal position;

[0042] III. Replan the robot trajectory for the abnormal position, and use the planned trajectory to perform steps I and II again until there is no abnormal data.

[0043] This method enables the robot end to be equipped with an inertial measurement unit, and by moving a specific trajectory, obtains the rotation and translation relationship between the inertial measurement unit coordinate system and the robot tool coordinate system, and realizes data conversion between the two; it is applied to the robot trajectory planning process, by analyzing the acceleration and angular velocity data output during the robot movement, the displacement data corresponding to the abnormal data is located, recorded as the abnormal position, and the robot trajectory is replanned for the abnormal position to avoid sudden acceleration / deceleration during the robot operation. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 Schematic diagram of the simulation route of the multi-segment circular motion in step S1. DETAILED DESCRIPTION

[0045] The technical solution of the present invention is described in detail below in conjunction with the accompanying drawings and specific implementation methods.

[0046] A calibration method for an inertial measurement unit and a robot, wherein the robot is a six-axis or more robot, and the inertial measurement unit is fixed at the end of the robot and moves synchronously with the robot;

[0047] The calibration method comprises the following steps:

[0048] Step 1: Define a certain position of the robot TCP in space as the origin;

[0049] Step 2: Obtain the rotation matrix and translation matrix between the inertial measurement unit coordinate system and the robot tool coordinate system;

[0050] The steps to obtain the rotation matrix are as follows:

[0051] ① Starting from the origin, the robot drives the inertial measurement unit to move continuously along any coordinate axis of the robot tool coordinate system for a preset time and then stops; during the movement, the inertial measurement unit continuously outputs acceleration data;

[0052] Integrate the acceleration data to obtain the displacement data, and find the direction vector of the displacement data relative to the origin;

[0053] The robot drives the inertial measurement unit back to the origin, and the inertial measurement unit clears the acceleration data;

[0054] ②Perform step ① along the other two coordinate axes of the robot tool coordinate system;

[0055] The direction vector obtained when moving along the X axis is recorded as

[0056] The direction vector obtained when moving along the Y axis is recorded as

[0057] The direction vector obtained when moving along the Z axis is recorded as

[0058] Then the rotation matrix

[0059] The steps to obtain the translation matrix are as follows:

[0060] S1, such as Figure 1 As shown, the robot TCP is kept at the origin position unchanged, and the other joint arms of the robot are rotated to drive the inertial measurement unit 3 to perform at least two circular motions in different directions, and the circular motions are all located on the same spatial sphere;

[0061] During the circular motion, the inertial measurement unit continuously outputs acceleration data;

[0062] The acceleration data are integrated to obtain the displacement data recorded at different times;

[0063] Fit the sphere according to the displacement data recorded at different times to obtain the coordinates of the sphere center;

[0064] S2. Calculate the vector from the sphere center coordinate to the initial coordinate, recorded as the translation matrix T, where the initial coordinate is the displacement data obtained by integrating the first acceleration data output by the inertial measurement unit during the circular motion.

[0065] Specifically, the method of integrating the acceleration data to obtain the displacement data is as follows:

[0066] During the movement, the inertial measurement unit continuously outputs acceleration data. After stopping the movement, a total of t groups of acceleration data output at different times are recorded and stored as The acceleration data is the data that has been corrected using the pre-calibrated internal parameters of the inertial measurement unit.

[0067] Integrate t groups of acceleration data to obtain t groups of velocity data Then integrate the t groups of velocity data to obtain the t groups of displacement data

[0068] in, Represents t groups of acceleration data in the X-axis direction, Represents t groups of acceleration data in the Y-axis direction, Represents t groups of acceleration data in the Z-axis direction;

[0069]

[0070]

[0071] i=1, 2...t, Δt represents the time interval for the inertial measurement unit to output acceleration data.

[0072] That is to say,

[0073] In step ①, the formula for obtaining the direction vector of the displacement data relative to the origin is:

[0074] Direction Vector in, Indicates that the displacement data is obtained by integrating the acceleration data.

[0075] More specifically, starting from the origin, the robot drives the inertial measurement unit to move continuously along the X-axis direction of the robot tool coordinate system for 2 seconds and then stops; during the movement, the inertial measurement unit continuously outputs acceleration data; the acceleration data is integrated to obtain the displacement data Obtaining displacement data Direction vector relative to the origin

[0076] The robot drives the inertial measurement unit back to the origin, and the inertial measurement unit clears the acceleration data;

[0077] Starting from the origin, the robot drives the inertial measurement unit to move continuously along the Y-axis direction of the robot tool coordinate system for 2 seconds and then stops; during the movement, the inertial measurement unit continuously outputs acceleration data; the acceleration data is integrated to obtain the displacement data Obtaining displacement data Direction vector relative to the origin

[0078] The robot drives the inertial measurement unit back to the origin, and the inertial measurement unit clears the acceleration data;

[0079] Starting from the origin, the robot drives the inertial measurement unit to move continuously along the Z coordinate axis direction of the robot tool coordinate system for 2 seconds and then stops; during the movement, the inertial measurement unit continuously outputs acceleration data; the acceleration data is integrated to obtain the displacement data Obtaining displacement data Direction vector relative to the origin

[0080] Among them, the translation matrix

[0081] Among them, (x′, y′, z′) represents the coordinates of the fitted sphere center, (x1, y1, z1) represents the initial coordinates, This is the first set of acceleration data output by the inertial measurement unit.

[0082] As a preferred implementation, the preset time value is 1 to 5 seconds.

[0083] Rotation Matrix Perform orthogonalization processing (such as Schmidt orthogonalization processing).

[0084] In specific implementation, the robot TCP is the center of the robot end flange; alternatively, a gun tip 2 is fixed at the robot end, and the gun tip is used to extend the center of the robot end flange 1 to the outside of the robot body axis, and the tip position of the gun tip 2 is recorded as the robot TCP.

[0085] As a specific application:

[0086] In this embodiment, a method for adjusting the trajectory of a robot using a rotation matrix and a translation matrix obtained by calibration is disclosed, comprising:

[0087] I. After completing the robot trajectory planning, the robot drives the inertial measurement unit to run the planned trajectory. During the operation, the inertial measurement unit continuously outputs acceleration data and angular velocity data;

[0088] According to the rotation matrix and translation matrix obtained by calibration, each output acceleration data and each angular velocity data are converted into the robot tool coordinate system;

[0089] II. Compare each converted acceleration data and each angular velocity data with their corresponding thresholds, and mark the acceleration data and angular velocity data greater than the threshold as abnormal data;

[0090] The displacement data corresponding to the abnormal data is obtained by integration and recorded as the abnormal position;

[0091] III. Replan the robot trajectory for abnormal positions (such as adding waypoints, modifying the distance between waypoints), and use the planned trajectory to perform steps I and II again until there is no abnormal data.

[0092] In this solution, by analyzing the acceleration and angular velocity data output during the robot's movement, the displacement data corresponding to the abnormal data is located and recorded as the abnormal position. The robot trajectory is replanned for the abnormal position to avoid sudden acceleration / deceleration during the robot's operation.

[0093] The foregoing description of the specific exemplary embodiments of the present invention is presented for the purpose of illustration and description. The foregoing description is not intended to be exhaustive, nor is it intended to limit the present invention to the precise form disclosed, and it is apparent that many changes and variations are possible in light of the above teachings. The exemplary embodiments are selected and described to explain the specific principles of the present invention and its practical application, so that other technicians in the field can realize and utilize the various exemplary embodiments of the present invention and its different selected forms and modified forms. The scope of the present invention is intended to be limited by the appended claims and their equivalents.

Claims

1. A method for calibrating the position and posture of an inertial measurement unit and a robot, wherein the robot is a robot with six axes or more, and the inertial measurement unit is fixed at the end of the robot and moves synchronously with the robot; It is characterized in that The calibration method comprises the following steps: Step 1: Define a certain position of the robot TCP in space as the origin; Step 2: Obtain the rotation matrix and translation matrix between the inertial measurement unit coordinate system and the robot tool coordinate system; The steps to obtain the rotation matrix are as follows: ① Starting from the origin, the robot drives the inertial measurement unit to continuously move along any coordinate axis direction of the robot tool coordinate system for a preset time and then stops; during the movement, the inertial measurement unit continuously outputs acceleration data; Integrating the acceleration data to obtain displacement data, and obtaining a direction vector of the displacement data relative to the origin; The robot drives the inertial measurement unit back to the origin, and the inertial measurement unit clears the acceleration data; ②Perform step ① along the other two coordinate axes of the robot tool coordinate system; The direction vector obtained when moving along the X axis is recorded as The direction vector obtained when moving along the Y axis is recorded as The direction vector obtained when moving along the Z axis is recorded as Then the rotation matrix The steps to obtain the translation matrix are as follows: S1. Keep the robot TCP at the origin and rotate the other joint arms of the robot to drive the inertial measurement unit to perform at least two circular motions in different directions, and the circular motions are all located on the same spatial sphere; During the circular motion, the inertial measurement unit continuously outputs acceleration data; Integrating the acceleration data respectively to obtain displacement data recorded at different times; Fit the sphere according to the displacement data recorded at different times to obtain the coordinates of the sphere center; S2. Calculate the vector from the sphere center coordinate to the initial coordinate, recorded as the translation matrix T, where the initial coordinate is the displacement data obtained by integrating the first acceleration data output by the inertial measurement unit during the circular motion.

2. The calibration method according to claim 1, characterized in that: The way to integrate the acceleration data to get the displacement data is as follows: During the movement, the inertial measurement unit continuously outputs acceleration data. After stopping the movement, a total of t groups of acceleration data output at different times are recorded and stored as Integrate t groups of acceleration data to obtain t groups of velocity data Then integrate the t groups of velocity data to obtain the t groups of displacement data in, Represents t groups of acceleration data in the X-axis direction, Represents t groups of acceleration data in the Y-axis direction, Represents t groups of acceleration data in the Z-axis direction; i=1, 2...t, Δt represents the time interval for the inertial measurement unit to output acceleration data.

3. The calibration method according to claim 1, characterized in that: In step ①, the formula for obtaining the direction vector of the displacement data relative to the origin is: Direction Vector in, Indicates that the displacement data is obtained by integrating the acceleration data.

4. The calibration method according to claim 1, characterized in that: Translation Matrix Among them, (x′, y′, z′) represents the coordinates of the fitted sphere center, (x1, y1, z1) represents the initial coordinates, This is the first set of acceleration data output by the inertial measurement unit.

5. The calibration method according to claim 1, characterized in that: The preset time value is 1 to 5 seconds.

6. The calibration method according to claim 1, characterized in that: Rotation Matrix Perform orthogonalization.

7. The calibration method according to claim 1, characterized in that: The robot TCP is the center of the robot end flange; or, a gun tip is fixed at the end of the robot, and the gun tip is used to extend the center of the robot end flange to the outside of the robot body axis, and the tip position of the gun tip is recorded as the robot TCP.

8. A method for adjusting the trajectory of a robot using a rotation matrix and a translation matrix obtained by calibration, characterized in that: The following steps are involved: I. After completing the robot trajectory planning, the robot drives the inertial measurement unit to run the planned trajectory. During the operation, the inertial measurement unit continuously outputs acceleration data and angular velocity data; According to the rotation matrix and translation matrix obtained by calibration, each output acceleration data and each angular velocity data are converted into the robot tool coordinate system; II. Compare each converted acceleration data and each angular velocity data with their corresponding thresholds, and mark the acceleration data and angular velocity data greater than the threshold as abnormal data; The displacement data corresponding to the abnormal data is obtained by integration and recorded as the abnormal position; III. Replan the robot trajectory for the abnormal position, and use the planned trajectory to perform steps I and II again until there is no abnormal data.

Images