Working method of industrial robot control system based on MEMS sensor

Abstract

The present invention belongs to the technical field of industrial robot control, and specifically relates to a working method of an industrial robot control system based on a MEMS sensor. The present invention obtains acceleration data and angular velocity data of human hand movements through sensors; obtains posture angle data of human hand movements through acceleration data and angular velocity data; controls robot movements according to the obtained posture angle data of human hand movements; collects human hand movements, and maps hand movements with robot movements, thereby realizing robot movement control according to hand postures, and calibrates and reduces noise of the acquired data, thereby improving data accuracy and control precision.

Description

Technical Field

[0001] The present invention belongs to the technical field of industrial robot control, and in particular relates to a working method of an industrial robot control system based on a MEMS sensor. Background Art

[0002] As an important part of "machine replacement", industrial robots require operators to observe the position of the robot and its supporting tools relative to the work object before use. Through repeated operation of the teach pendant, the robot's working position, motion parameters and process parameters at the teaching point are adjusted, and then the data that meets the work requirements are saved and transferred to the next teaching point. This method requires the operator to be a professional robot technician and has rich experience.

[0003] Therefore, there is an urgent need to provide a method that can map somatosensory movements with robot movements to achieve control of robot movements. Summary of the invention

[0004] The purpose of the present invention is to provide a working method of an industrial robot control system based on a MEMS sensor to solve the technical problem of high difficulty in adapting a traditional process robot.

[0005] In order to solve the above technical problems, the present invention provides a working method of an industrial robot control system based on MEMS sensors, including: obtaining acceleration data and angular velocity data of human hand movements through sensors; obtaining posture angle data of human hand movements through acceleration data and angular velocity data; and controlling robot movements according to the obtained posture angle data of human hand movements.

[0006] Furthermore, the method for obtaining acceleration data and angular velocity data of human hand movements includes:

[0007] Establish a geographic coordinate system, that is

[0008] The OXYZ coordinate system is used, where the origin O of the coordinate is located at the center of gravity of the carrier, the Z axis is opposite to the direction of gravity, X points to the north, and Y points to the east;

[0009] Establish the carrier coordinate system, that is

[0010] The carrier coordinate system is fixedly connected to the sensor. The origin of the coordinate system is located at the center of gravity of the sensor. The angle between the Y' axis and the Y axis is the pitch angle θ; the angle between the X' axis and the X axis is the roll angle. as well as

[0011] Obtain component data of acceleration data and angular velocity data on each axis in the carrier coordinate system;

[0012] Calibrate the acquired component data;

[0013] The acceleration and angular velocity of the human hand movement and the angle data with each axis in the three-dimensional coordinate system are calculated based on the calibrated component data.

[0014] Further, the acquisition of component data of acceleration data and angular velocity data on each axis in the carrier coordinate system includes:

[0015] Read the component value data of acceleration and rotational angular velocity in the sensor;

[0016] The X-axis component of the accelerometer is ACC_X, the Y-axis component of the accelerometer is ACC_Y, and the Z-axis component of the accelerometer is ACC_Z;

[0017] The angular velocity of rotation around the X axis is GYR_X, the angular velocity of rotation around the Y axis is GYR_Y, and the angular velocity of rotation around the Z axis is GYR_Z;

[0018] The component value data of the acceleration and rotational angular velocity on each axis are expressed in matrix form;

[0019] The acceleration rate is selected as 4g, and the angular velocity range is selected as 2000 degrees / second, that is,

[0020] [Ax0, Ay0, Az0] T =[(4g*ACC_x) / 32768, (4g*ACC_Y) / 32768, (4g*ACC_Z) / 32768] T ;

[0021] [Gx0, Gy0, Gz0] T =[2000*GYR_X / 32768, 2000*GYR_Y / 32768, 2000*GYR_Z / 32768] T ; Where Ax0 is the measured value of x-axis acceleration, Ay0 is the measured value of y-axis acceleration, and Az0 is the measured value of z-axis acceleration; Gx0 is the measured value of x-axis angular velocity, Gy0 is the measured value of y-axis angular velocity, and Gz0 is the measured value of z-axis angular velocity.

[0022] Further, the method for calibrating the acquired component data includes:

[0023] When the sensor is stationary, the acceleration and angular velocity values ​​in each axis direction are obtained, and the offset is obtained by averaging, that is,

[0024] The acceleration offset on each axis is:

[0025] in

[0026] A x off is the acceleration offset of the x-axis, Ay off is the acceleration offset of the y-axis, A z off is the z-axis acceleration offset, A x [n] is the measured value of x-axis acceleration at rest, A y [n] is the measured value of the y-axis acceleration at rest, A z [n] is the measured value of z-axis acceleration at rest, and n is the number of groups measured;

[0027] The offset of angular velocity on each axis is:

[0028] in

[0029] G x off is the x-axis angular velocity offset, G y off is the y-axis angular velocity offset, G z off is the z-axis angular velocity offset, G x [n] is the measured value of the x-axis angular velocity at rest, G y [n] is the measured value of the y-axis angular velocity at rest, G z [n] is the measured value of the z-axis angular velocity at rest;

[0030] The component value data matrix of acceleration and rotational angular velocity on each axis is calibrated by offset, that is,

[0031] The component value data matrix of the calibrated acceleration on each axis is:

[0032]

[0033] The component value data matrix of the angular velocity on each axis after calibration is:

[0034] in

[0035] A x is the x-axis acceleration calibration value, A y is the y-axis acceleration calibration value, A z is the z-axis acceleration calibration value;

[0036] G x is the x-axis angular velocity calibration value, G y is the y-axis angular velocity calibration value, G z is the z-axis angular velocity calibration value.

[0037] Further, the method of calculating the angle data of the acceleration and angular velocity of the human hand movement with each axis in the three-dimensional coordinate system according to the calibrated component data includes:

[0038] The Euler angle of acceleration is obtained through trigonometric functions, that is,

[0039] The acceleration angle and roll angle are

[0040] Acceleration angle Pitch angle θ j =arctan(A x / A z );as well as

[0041] The Euler angle of angular velocity is obtained by integration operation, that is,

[0042] in

[0043] θ′ g It is the integral value calculated in the previous step, and the initial value is 0.

[0044] Further, the method for obtaining the posture angle of the human hand movement in each axis direction according to the angle data of the acceleration and angular velocity of the human hand movement in each axis direction includes:

[0045] The acceleration Euler angle data and angular velocity Euler angle data are fused by complementary filtering method, that is,

[0046] The fused roll angle is

[0047] The fusion pitch angle is θ t = k * θ j +(1-k)*(θ t-1 +θ g );in

[0048] The value of k is 0.6. Calculated at the last sampling θ t-1 is the θ calculated at the last sampling t .

[0049] Further, the method for controlling the robot motion according to the acquired human hand motion posture angle data comprises:

[0050] The left hand and the right hand wear the first carrier and the second carrier respectively;

[0051] The upper computer receives the posture angle data sent by the first carrier and the second carrier, and selects the corresponding motion mode to send an action command signal;

[0052] The robot receives the action command signal sent by the host computer and takes action.

[0053] Furthermore, the method of receiving the posture angle data sent by the first carrier and the second carrier through the host computer and selecting the corresponding motion mode to send the action command signal includes:

[0054] Establish serial port connection with the first carrier and the second carrier, and communicate with the robot through Socket;

[0055] Select the robot movement mode;

[0056] After analyzing the received serial port signal, the robot is sent action instructions;

[0057] The robot motion modes include linear motion and joint motion.

[0058] Furthermore, the method of performing data analysis on the received serial port signal and then sending an action instruction to the robot comprises:

[0059] Receiving the motion posture angles acquired by the first carrier and the second carrier;

[0060] By receiving the posture angle data and according to the selected robot motion mode, the robot is given action instructions.

[0061] The beneficial effect of the present invention is that the working method of the industrial robot control system based on MEMS sensors of the present invention realizes the robot controlling movement according to the hand posture by collecting human hand movements and mapping the hand movements with the robot movements, and improves the accuracy of the data and the precision of the control by calibrating and reducing the noise of the acquired data.

[0062] Other features and advantages of the present invention will be described in the following description, and partly become apparent from the description, or understood by practicing the present invention. The purpose and other advantages of the present invention are realized and obtained by the structures particularly pointed out in the description and the drawings.

[0063] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, preferred embodiments are given below and described in detail with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] In order to more clearly illustrate the specific implementation methods of the present invention or the technical solutions in the prior art, the drawings required for use in the specific implementation methods or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some implementation methods of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0065] Figure 1 It is a flow chart of the working method of the industrial robot control system based on MEMS sensor of the present invention;

[0066] Figure 2 It is a program flow chart of the host computer in the working method of the control system of the present invention;

[0067] Figure 3 It is a flow chart of the robot program in the working method of the control system of the present invention. DETAILED DESCRIPTION

[0068] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the accompanying drawings. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0069] like Figures 1 to 3 As shown, this embodiment provides a working method of an industrial robot control system based on a MEMS sensor, including: obtaining acceleration data and angular velocity data of human hand movements through sensors; obtaining posture angle data of human hand movements through acceleration data and angular velocity data; and controlling robot movements according to the obtained posture angle data of human hand movements.

[0070] In this embodiment,

[0071] In this embodiment, the method for obtaining acceleration data and angular velocity data of human hand movements includes: establishing a geographic coordinate system, that is, using the North-East coordinate system OXYZ, the origin O of the coordinate is located at the center of gravity of the carrier, the Z axis is opposite to the direction of gravity, the X axis points to the north, and the Y axis points to the east; establishing a carrier coordinate system, that is, the carrier coordinate system is fixedly connected to the sensor, the origin O′ of the coordinate is located at the center of gravity of the sensor, the angle between the Y′ axis and the Y axis is the pitch angle θ; the angle between the X′ axis and the X axis is the roll angle And obtain the component data of acceleration data and angular velocity data on each axis in the carrier coordinate system; calibrate the obtained component data; and calculate the angle data of the acceleration and angular velocity of the human hand movement and each axis in the three-dimensional coordinate system according to the calibrated component data.

[0072] In this embodiment, the component data of the acceleration data and the angular velocity data on each axis in the carrier coordinate system are obtained, including: reading the component value data of the acceleration and the rotational angular velocity in the sensor; wherein the X-axis component of the accelerometer is ACC_X, the Y-axis component of the accelerometer is ACC_Y, and the Z-axis component of the accelerometer is ACC_Z; the angular velocity of rotation around the X-axis is GYR_X, the angular velocity of rotation around the Y-axis is GYR_Y, and the angular velocity of rotation around the Z-axis is GYR_Z; the component value data of the acceleration and the rotational angular velocity on each axis are expressed in a matrix form; that is,

[0073] [Ax0, Ay0, Az0] T=[(4g*ACC_x) / 32768, (4g*ACC_Y) / 32768, (4g*ACC_Z) / 32768] T ;

[0074] [Gx0, Gy0, Gz0] T =[2000*GYR_X / 32768, 2000*GYR_Y / 32768, 2000*GYR_Z / 32768] T .

[0075] In this embodiment, Ax0 is the measured value of x-axis acceleration, Ay0 is the measured value of y-axis acceleration, and Az0 is the measured value of z-axis acceleration; Gx0 is the measured value of x-axis angular velocity, Gy0 is the measured value of y-axis angular velocity, and Gz0 is the measured value of z-axis angular velocity; and the acceleration multiplier is selected as 4g, the angular velocity range is selected as 2000 degrees / second, and the calculation constant of the sensor is selected as 32768.

[0076] In this embodiment, the method for calibrating the acquired component data includes: acquiring the acceleration value and angular velocity value of the sensor in each axis direction when stationary, and acquiring the offset by averaging, that is,

[0077] The acceleration offset on each axis is:

[0078] in

[0079] A x off is the acceleration offset of the x-axis, A y off is the acceleration offset of the y-axis, A z off is the z-axis acceleration offset; A x [n] is the measured value of x-axis acceleration at rest, A y [n] is the measured value of the y-axis acceleration at rest, A z [n] is the measured value of z-axis acceleration at rest, and n is the number of groups measured;

[0080] The offset of angular velocity on each axis is:

[0081] in

[0082] G x off is the x-axis angular velocity offset, G y off is the y-axis angular velocity offset, G z off is the z-axis angular velocity offset; G x [n] is the measured value of the x-axis angular velocity at rest, G y [n] is the measured value of the y-axis angular velocity at rest, G z [n] is the measured value of the z-axis angular velocity at rest;

[0083] The component value data matrix of acceleration and rotational angular velocity on each axis is calibrated by offset, that is,

[0084] The component value data matrix of the calibrated acceleration on each axis is:

[0085]

[0086] The component value data matrix of the angular velocity on each axis after calibration is:

[0087] in

[0088] A x is the x-axis acceleration calibration value, A y is the y-axis acceleration calibration value, A z is the z-axis acceleration calibration value;

[0089] G x is the x-axis angular velocity calibration value, G y is the y-axis angular velocity calibration value, G z is the z-axis angular velocity calibration value.

[0090] In this embodiment, the MEMS sensor uses the MPU6050 chip, and since the raw data provided by the MPU6050 chip is mixed with serious noise, and there is a phenomenon of data offset in a static working state, it is necessary to calibrate the data first and then eliminate the noise through a filtering algorithm;

[0091] When the chip is in a stationary state, the chip reads acceleration in the X and Y directions, and the directional acceleration is the local gravity acceleration. The angular velocity should be 0 in theory. A simple method is used to estimate the offset. In the initialization phase, n (10 in this article) values ​​are read continuously, that is, n = 1, 2...10. The average of these 10 values ​​is the offset in that direction. After obtaining the offset, the offset is subtracted from each reading to get the calibrated reading.

[0092] In this embodiment, the method of calculating the angle data of the acceleration and angular velocity of the human hand motion with each axis in the three-dimensional coordinate system according to the calibrated component data includes:

[0093] The Euler angle of acceleration is obtained through trigonometric functions, that is,

[0094] The acceleration angle and roll angle are

[0095] Acceleration angle Pitch angle θ j =arctan(A x / A z );as well as

[0096] The Euler angle of angular velocity is obtained by integration operation, that is,

[0097] in

[0098] θ′ g It is the integral value calculated in the previous step, and the initial value is 0.

[0099] In this embodiment, the method for obtaining the posture angle of the human hand movement in each axis direction according to the angle data of the acceleration and angular velocity of the human hand movement in each axis direction includes: fusing the acceleration Euler angle data and the angular velocity Euler angle data by complementary filtering method, that is,

[0100] The fused roll angle is

[0101] The fusion pitch angle is θ t = k * θ j +(1-k)*(θ t-1 +θ g );in

[0102] The value of k is 0.6. Calculated at the last sampling θ t-1 is the θ calculated at the last sampling t .

[0103] In this embodiment, dt is taken as the sampling time, which is 0.01s. After the angular velocities of the three axes are multiplied by the time dt, another set of angle values ​​are obtained. The coefficient k is set for filtering. The value of k is obtained based on multiple experiments, and k is 0.6.

[0104] In this embodiment, the method for controlling the robot's motion based on the obtained human hand motion posture angle data includes: wearing a first carrier and a second carrier on the left hand and the right hand respectively; receiving the posture angle data sent by the first carrier and the second carrier through a host computer, and selecting a corresponding motion mode to send a motion command signal; receiving the motion command signal sent by the host computer through the robot and taking action.

[0105] In this embodiment, the host computer communicates with the control circuit via USB. After starting to run, it is necessary to determine the robot's motion mode, that is, linear motion or joint motion. Then, the host computer continuously analyzes the received serial port signal and converts the signal into a string. Then, it sends the data to the ABB IRB 120 robot through Socket communication. The serial port baud rate is set to 9600, and the time interval between each two Socket communication transmissions is 10ms. The host computer program is written in Python.

[0106] In this embodiment, the method of receiving the posture angle data sent by the first carrier and the second carrier through the upper computer, and selecting the corresponding motion mode to send an action command signal includes: establishing a serial port connection with the first carrier and the second carrier, and communicating with the robot through the Socket; selecting the robot motion mode; performing data analysis on the received serial port signal and sending an action command to the robot; wherein the robot motion mode includes: linear motion and joint motion.

[0107] In this embodiment, the ABB robot continuously receives the character string sent by the host computer and parses the character string. According to the parsed content, the robot's operating mode, direction and speed are determined, and finally the robot's linear motion or joint motion is controlled. In addition, the robot receives the character string and automatically corrects the error through the error handling program. When the Socket connection fails, the error handling program controls the re-establishment of the connection.

[0108] In this embodiment, the method of sending motion instructions to the robot after performing data analysis on the received serial port signal includes: receiving the motion posture angles acquired by the first carrier and the second carrier; and sending motion instructions to the robot based on the received posture angle data and the selected robot motion mode.

[0109] In this embodiment, the method for selecting the motion mode by receiving the posture angle data is selected as a table lookup method, and the mapping relationship between the hand posture and the linear motion of the robot is obtained through Table 1, and the mapping relationship between the hand posture and the joint motion of the robot is obtained through Table 2:

[0110]

[0111]

[0112] Table 1 Mapping relationship between hand posture and robot linear motion

[0113]

[0114] Table 2 Mapping relationship between hand posture and robot joint motion

[0115] In summary, the working method of the industrial robot control system based on MEMS sensors of the present invention collects human hand movements and maps the hand movements with robot movements, thereby controlling the robot's movements according to the hand postures, and calibrates and reduces noise on the acquired data, thereby improving the accuracy of the data and the precision of the manipulation.

[0116] In the several embodiments provided in the present application, it should be understood that the disclosed systems, devices and methods can be implemented in other ways. The device embodiments described above are merely schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some communication interfaces, and the indirect coupling or communication connection of devices or units can be electrical, mechanical or other forms.

[0117] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0118] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit.

[0119] Based on the above ideal embodiments of the present invention, the relevant staff can make various changes and modifications without departing from the technical concept of the present invention through the above description. The technical scope of the present invention is not limited to the contents of the specification, and its technical scope must be determined according to the scope of the claims.

Claims

1. A method based on MEMS The working method of the sensor industrial robot control system is characterized in that include: Acquire acceleration data and angular velocity data of human hand movements through sensors; The posture angle data of the human hand movement is obtained through the acceleration data and the angular velocity data; Control the robot's motion according to the acquired human hand motion posture angle data; The method for obtaining acceleration data and angular velocity data of human hand movements comprises: Establish a geographic coordinate system, that is Using the North Celestial East coordinate system OXYZ , the origin of the coordinates O Located at the center of gravity of the carrier, Z The axis is opposite to the direction of gravity. X Pointing north, Y Pointing east; Establish the carrier coordinate system, that is The carrier coordinate system is fixed to the sensor, and the origin of the coordinate system Located at the center of gravity of the sensor, Axis and The angle between the axes is the pitch angle ; Axis and The angle between the axes is the roll angle ;as well as Obtain component data of acceleration data and angular velocity data on each axis in the carrier coordinate system; Calibrate the acquired component data; The acceleration and angular velocity of the human hand motion and the angle data of each axis in the three-dimensional coordinate system are calculated according to the calibrated component data; The acquisition of component data of acceleration data and angular velocity data on each axis in the carrier coordinate system includes: Read the component value data of acceleration and rotational angular velocity in the sensor; Accelerometer X The axis component is ACC _ X , the accelerometer Y The axis component is ACC _ Y , the accelerometer Z The axis component is ACC _ Z ; Around X The angular velocity of the axis rotation is G JZ _ X , around Y The angular velocity of the axis rotation is G JZ _ Y , around Z The angular velocity of the axis rotation is G JZ _ Z ; The component value data of the acceleration and rotational angular velocity on each axis are expressed in matrix form; The acceleration multiplier is selected as 4 g , the angular velocity range is selected as 2000 degrees / second, that is ； ;in for x Measured value of axis acceleration, for y Measured value of axis acceleration, for z Measured value of axis acceleration; for x Measured value of shaft angular velocity, for y Measured value of shaft angular velocity, for z Measured value of axis angular velocity; The method for calibrating the acquired component data comprises: When the sensor is stationary, the acceleration and angular velocity values ​​in each axis direction are obtained, and the offset is obtained by averaging, that is, The acceleration offset on each axis is: , , ;in for x Axis acceleration offset, for y Axis acceleration offset, for z Axis acceleration offset, When stationary x Measured value of axis acceleration, When stationary y Measured value of axis acceleration, When stationary z Measured value of axis acceleration, n is the number of groups measured; The offset of angular velocity on each axis is: , , ;in for x Axis angular velocity offset, for y Axis angular velocity offset, for z Axis angular velocity offset, When stationary x Measured value of shaft angular velocity, When stationary y Measured value of shaft angular velocity, When stationary z Measured value of axis angular velocity; The component value data matrix of acceleration and rotational angular velocity on each axis is calibrated by offset, that is, The component value data matrix of the calibrated acceleration on each axis is: ； The component value data matrix of the angular velocity on each axis after calibration is: ;in for x Axis acceleration calibration value, for y Axis acceleration calibration value, for z Axis acceleration calibration value; for x Axis angular velocity calibration value, for y Axis angular velocity calibration value, for z Axis angular velocity calibration value; The method for calculating the angle data of the acceleration and angular velocity of the human hand motion and each axis in the three-dimensional coordinate system according to the calibrated component data includes: The Euler angle of acceleration is obtained through trigonometric functions, that is, The acceleration angle and roll angle are ; Acceleration angle Pitch angle ;as well as The Euler angle of angular velocity is obtained by integration operation, that is, ;in , It is the integral value calculated in the previous step, and the initial value is 0.

Images