CN111650882A - An online error compensation system and method for hybrid robots based on coarse interpolation - Google Patents

Abstract

The invention discloses an online error compensation system and method for a hybrid robot based on rough interpolation, wherein the system includes a hybrid robot arm, a first and a second rotating support, a detection system and a control system; a moving platform in the hybrid robot arm , its peripheral side is hinged with the first to third active arms, and its rear end is fixedly connected with a driven support arm; its front end is connected with an A/C axis double swing angle head; the second and third active arms and the driven support arm pass through The third to fifth rotating shafts are connected with the second rotating support; the second rotating support is connected with the bearing seat through the sixth rotating shaft; the detection system includes: first and second angle sensors for detecting the rotation angles of the sixth and fifth rotating shafts; detecting the C and A axes The third and fourth angle sensors of the rotation angle; the first displacement sensor to detect the axial displacement of the driven support arm; the multi-axis motion controller in the control system, which receives the feedback signal of each sensor, and outputs the signal to control the work of the corresponding servo motor. The compensation method of the invention is simple, and the positioning accuracy of the hybrid robot can be improved.

Description

An online error compensation system and method for hybrid robots based on coarse interpolation

technical field

The invention relates to the field of robot processing, in particular to an online error compensation system and method for a hybrid robot based on rough interpolation.

Background technique

In order to adapt to the large-scale movement of large-scale structural parts and the service environment of local high-speed precision machining, the large-scale structural parts have the characteristics of large external dimensions, complex geometric shapes and high precision requirements, and the single-machine manufacturing with hybrid configuration equipment as the core functional component Cell or multi-machine manufacturing systems are increasingly becoming an indispensable tool. However, during use, the hybrid robot will deform to a certain extent due to the load of the end effector and the gravity of the mechanical arm, which will cause the position of the end of the robot to be offset, and the actual size of the robot parts in the process of manufacturing and the theoretical size are not complete. Consistent, in the assembly process, the length of the drive chain will be inaccurate, the transmission error of each drive joint due to the reducer, and the joint error of the swing head due to the gap, friction, and zero deviation of the reducer will cause the motion controller. The theoretical model parameters used in the calculation are inconsistent with the actual parameters of the robot, which directly affects the positioning accuracy of the end.

The methods to improve the accuracy of robots mainly include error prevention technology and error compensation technology. At present, the error prevention technology has been constrained by the parts manufacturing technology and economic benefits, and the error compensation technology can effectively improve the robot accuracy with lower hardware investment. There are two main types of error compensation techniques: one is offline compensation, which improves accuracy by calibrating or establishing an error compensation mapping model before the robot is used. These methods are complex and cumbersome and cannot be changed during field use; Errors are detected during use, and errors are compensated in real time. The current online compensation method has a lot of monitoring data and a large amount of calculation, which occupies the storage capacity and running speed of the computer, and the running speed of the equipment is affected accordingly.

SUMMARY OF THE INVENTION

In order to solve the technical problems existing in the known technology, the present invention provides an efficient and accurate online error compensation system and method for a hybrid robot based on rough interpolation.

The technical solution adopted by the present invention to solve the technical problems existing in the known technology is: an online error compensation system for hybrid robots based on rough interpolation, comprising a hybrid robot arm, a first rotating support, a second rotating support, a detection systems and control systems; of which:

The hybrid manipulator includes a moving platform; three active arms driven and retracted by a servo motor are hinged on the peripheral side of the moving platform, which are a first active arm, a second active arm, and a third active arm in sequence; the rear end of the moving platform is fixed with a driven arm A support arm; the front end of the moving platform is connected with an A/C axis double swing angle head driven by a servo motor, wherein the C axis of the double swing angle head is rotatably connected with the moving platform; the first active arm rotates with the first rotating bracket through the first rotating shaft connection; the first rotating bracket is rotatably connected to the fixed bearing seat through the second rotating shaft; the second driving arm, the third driving arm and the driven support arm are correspondingly connected to the second rotating shaft through the third rotating shaft, the fourth rotating shaft and the fifth rotating shaft The support is rotatably connected; the second rotating support is rotatably connected with the fixed bearing seat through the sixth rotating shaft; the axis of the first rotating shaft is perpendicular to the axis of the second rotating shaft; the axes of the third rotating shaft, the fourth rotating shaft and the fifth rotating shaft are parallel to each other and the sixth rotating shaft The axis of the rotating shaft is vertical; the center of the second rotating support is located at the intersection of the axis of the fifth rotating shaft and the axis of the sixth rotating shaft;

The detection system includes: a first angle sensor for detecting the rotation angle of the sixth shaft; a second angle sensor for detecting the rotation angle of the fifth shaft; a third angle sensor for detecting the rotation angle of the C-axis; a fourth angle sensor for the rotation angle; a first displacement sensor for detecting the axial displacement of the driven support arm;

The control system includes a multi-axis motion controller; the multi-axis motion controller receives detection signals from the first to fourth angle sensors and the first displacement sensor, and converts the detection values into corresponding first to third active arms. The telescopic displacement value and the rotation angle values of the A and C axes are subtracted from the corresponding given values to obtain the deviation. Based on the deviation, a control signal is given to control the servo drive of the first to third active arms and the A and C axes. work of the motor.

Further, the axis of the fifth rotating shaft is at the same distance from the axis of the third rotating shaft and the axis of the fourth rotating shaft.

Further, the first to fourth angle sensors are circular gratings; the first displacement sensor is a linear grating.

Further, the multi-axis motion controller adopts Omron CK3M multi-axis motion controller.

Further, the first rotating bracket and the second rotating bracket are arranged up and down.

Further, the distances from the hinge center of the first driving arm and the movable platform to the hinge centers of the second driving arm, the third driving arm and the movable platform are equal.

Further, the first active arm, the second active arm and the third active arm are connected with the moving platform in a spherical hinge type.

The present invention also provides a method for online error compensation of a hybrid robot based on coarse interpolation using the above-mentioned online error compensation system for a hybrid robot based on coarse interpolation. The method includes the following steps:

Step A, in the multi-axis controller, set up five global compensation variable memories for correspondingly storing the axial displacement compensation variables of the first to third active arms and the rotation angle compensation variables of the A and C axes, and initialize the variable memory ;

In step B, the multi-axis motion controller reads the G code, converts the tool nose machining trajectory into several continuous tiny line segments, and obtains the tool nose attitude data at both ends of the tiny line segments;

Step C, the multi-axis motion controller outputs the servo drive commands corresponding to the given values of the axial displacements of the first to third active arms according to the inverse kinematics algorithm of the hybrid robot combined with the compensation values in the compensation variable memory, and the corresponding The servo drive command for the given value of the rotation angle of the A and C axes;

Step D, the multi-axis motion controller receives the feedback signals of the current first to fourth angle sensors and the first displacement sensor, and obtains the axial displacement given values corresponding to the first to third active arms and A according to the spatial geometry algorithm. The deviation of the given value of the rotation angle of the C axis, the deviation is regarded as the new compensation value, and the variable memory is updated to the new compensation value accordingly;

Step E, repeat steps C to D until the interpolation ends.

Further, the tool nose attitude data includes: the x-axis, y-axis, and z-axis coordinates of the tool nose point, and the rotation angle of the tool nose point around the x-axis and the y-axis.

Further, the spatial geometry algorithm is as follows:

Set the detection value of the first angle sensor as θ ₁ , the detection value of the second angle sensor as θ ₂ , the detection value of the first displacement sensor as L ₁ , and the center point around which the first to third active arms and the movable platform rotate relative to each other. A ₁ , A ₂ , and A ₃ in sequence, and the rotation sub-centers of the first driving arm and the first rotating bracket are set as B ₁ ; the rotation sub-centers of the second and third driving arms and the second rotating bracket are respectively set as B ₂ , B ₃ ; set the midpoint of the line connecting the two points A ₂ and A ₃ as point A, set the center of the second rotating support as point B, set the base frame as B-xyz; set x _A , y _A , z _A corresponds to the x-axis, y-axis, and z-axis coordinates of point A under the base frame; the first to third active arms are rotated by the servo motor to drive the ball screw to perform axial displacement;

From the spatial geometric relationship, the coordinate values of point A under the base frame are obtained as follows:

x _A =L ₁ sinθ ₂

y _A =L ₁ cosθ ₂ sinθ ₁

z _A =L ₁ cosθ ₂ cosθ ₁

According to the inverse kinematics of the parallel mechanism, the actual rotation angles of the servo motors of the first to third active arms are obtained by the following equations:

r _A = [x _A y _A z _A ] ^T ;

q ₄ =||r _A ||;

w ₄ =r _A /q ₄ =(s _3x s _3y s _3z ) ^T ;

a _i0 =(a _i cosγ _i a _i sinγ _i 0) ^T ;

C _i =Ra _i0 ;

D _i =( _bi cosγ _i b _i sinγ _i 0) ^T ;

where:

r _A is the position vector of point A in the coordinate system B-xyz;

q ₄ is the distance from point A to point B;

w ₄ is the unit vector from point A to point B;

s _3x is the coordinate value of the point corresponding to the unit vector on the Z axis in the coordinate system Ax _A y _A z _A projected on the x axis in the coordinate system B-xyz;

s _3y is the coordinate value of the point corresponding to the unit vector on the Z axis in the coordinate system Ax _A y _A z _A projected on the y axis in the coordinate system B-xyz;

s _3z is the coordinate value of the point corresponding to the unit vector on the Z axis in the coordinate system Ax _A y _A z _A projected on the z axis in the coordinate system B-xyz;

ψ is the rotation angle of the attitude of the coordinate system Ax _A y _A z _A relative to the coordinate system B-xyz around the x-axis;

θ is the rotation angle of the posture of the coordinate system Ax _A y _A z _A relative to the coordinate system B-xyz around the y _A axis after the coordinate system Ax _A y _A z _A rotates the angle ψ about the x-axis relative to the coordinate system B-xyz;

R is the attitude matrix of the coordinate system Ax _A y _A z _A relative to the coordinate system B-xyz;

γ _i is an intermediate variable;

C _i is the position vector of point A _i in the coordinate system B-xyz; i=1,2,3;

a _i is the distance from point A _i to point A; i=1,2,3;

D _i is the position vector of point B _i in the coordinate system B-xyz; i=1,2,3;

b _i is the distance from point B _i to point B; i=1,2,3;

a _i0 is the position vector of point A _i in the coordinate system Ax _A y _A z _A ; i=1,2,3;

pi is the lead of the ball screw of the _i -th active arm; i=1,2,3;

θ _ia is the actual rotation angle of the servo motor of the i-th active arm; i=1, 2, 3.

The advantages and positive effects of the present invention are: online and real-time compensation for the end position deviation of the hybrid robot due to the self-weight and end load of the mechanical arm during the machining process, the dimensional error of the parts and components during the machining and assembly process, and the driving joints. It can improve the positioning accuracy of the hybrid robot, which is simpler than the traditional kinematics calibration method and offline compensation method. During the interpolation process, the error compensation of adjacent interpolation points is completed, the algorithm for calculating the compensation value is simple, the online compensation control is easy, and the compensation effect meets the needs of high-speed and high-precision motion control of the robot.

Description of drawings

FIG. 1 is a schematic structural diagram of a hybrid robot error online compensation system based on coarse interpolation according to the present invention.

FIG. 2 is a working flow chart of a method for online error compensation of a hybrid robot based on coarse interpolation according to the present invention.

FIG. 3 is a geometrical schematic diagram of the spatial geometry algorithm of the present invention.

In the figure: 1, the third servo motor; 2, the driven support arm; 3, the second servo motor; 4, the second angle sensor; 5, the fifth shaft; 6, the second rotating bracket; 7, the moving platform; 8 , the third angle sensor; 9, the A/C axis double swing angle head; 10, the A axis; 11, the fourth angle sensor; 12, the first displacement sensor; 13, the sixth axis; 14, the first angle sensor; 15 , the first servo motor; 16, the first rotating bracket.

Detailed ways

In order to further understand the content of the invention, features and effects of the present invention, the following embodiments are listed herewith, and are described in detail as follows in conjunction with the accompanying drawings:

Please refer to FIGS. 1 to 3 , an online error compensation system for hybrid robots based on rough interpolation, including a hybrid robot arm, a first rotating support 16 , a second rotating support 6 , a detection system and a control system; wherein:

The hybrid manipulator includes a moving platform 7; the moving platform 7 is hinged with three active arms driven and retracted by a servo motor, which are sequentially a first active arm, a second active arm, and a third active arm; the rear end of the moving platform 7 is fixedly connected There is a driven support arm 2; the front end of the moving platform 7 is connected with an A/C axis double swing angle head 9 driven by a servo motor, wherein the C axis of the A/C axis double swing angle head 9 is rotatably connected with the moving platform 7; the first The active arm is rotatably connected to the first rotating bracket 16 through the first rotating shaft; the first rotating bracket 16 is rotatably connected to the fixed bearing seat through the second rotating shaft; the second active arm, the third active arm, and the driven support arm 2 correspond to the The third rotating shaft, the fourth rotating shaft and the fifth rotating shaft 5 are rotatably connected with the second rotating support 6; the second rotating support 6 is rotatably connected with the fixed bearing seat through the sixth rotating shaft 13; the axes of the first rotating shaft and the second rotating shaft are perpendicular; The axes of the third rotating shaft, the fourth rotating shaft and the fifth rotating shaft 5 are parallel to each other and perpendicular to the axis of the sixth rotating shaft 13; the center of the second rotating support 6 is located at the intersection of the axis of the fifth rotating shaft 5 and the axis of the sixth rotating shaft 13;

The first active arm, the second active arm, and the third active arm constitute the parallel mechanism of the hybrid robot; the A-axis 10 of the A/C-axis double-swing angle head and the C-axis of the A/C-axis double-swing angle head 9 constitute the hybrid robot Robotic tandem mechanism. The driven support arm 2 is used to fix and support the movable platform 7 and restrain the movement of the movable platform 7 .

The driving servo motor of the first driving arm is called the first servo motor 15, the driving servo motor of the second driving arm is called the second servo motor 3, and the driving servo motor of the third driving arm is called the third servo motor 1, A The C-axis drive servo motor of the /C-axis double swing angle head 9 is called the fourth servo motor, and the A-axis 10 drive servo motor of the A/C-axis double swing angle head is called the fifth servo motor. The servo motors of the first to third active arms can use servo motors that output torque. The output shafts of such servo motors can be connected to a transmission mechanism that converts rotary motion into linear motion, preferably a rolling screw transmission mechanism to connect The rotational drive of the servo motor is transformed into a linear drive, and the servo motors of the first to third active arms can also use linear servo motors to realize the linear expansion and contraction of the first active arm, the second active arm and the third active arm.

The detection system includes: a first angle sensor 14 for detecting the rotation angle of the sixth shaft 13; a second angle sensor 4 for detecting the rotation angle of the fifth shaft 5; a third angle sensor 8 for detecting the rotation angle of the C-axis; a fourth angle sensor 11 for detecting the rotation angle of the A-axis 10; a first displacement sensor 12 for detecting the axial displacement of the driven support arm 2;

The control system includes a multi-axis motion controller; the multi-axis motion controller receives detections from the first angle sensor 14 , the second angle sensor 4 , the third angle sensor 8 , the fourth angle sensor 11 and the first displacement sensor 12 signal, which converts the detected value into the telescopic displacement value corresponding to the first active arm, the second active arm and the third active arm and the rotation angle value of the A-axis 10 and the C-axis, and subtracts the corresponding given value to obtain The deviation, which gives a control signal based on the deviation, controls the work of the servo motors that drive the first active arm, the second active arm, the third active arm, and the A-axis 10 and the C-axis.

The telescopic displacement values of the first active arm, the second active arm and the third active arm and the rotation angle values of the A-axis 10 and C-axis obtained by converting the detected values are used as the actual values of the feedback, corresponding to the first active arm, the second active arm and the second active arm. The given value of the telescopic displacement of the main arm and the third active arm and the given values of the rotation angles of the A-axis 10 and C-axis are subtracted to obtain the deviation, that is, the actual value of the telescopic displacement of the first active arm is subtracted from the given value of the telescopic displacement , the actual value of the telescopic displacement of the second active arm is subtracted from the given value of the telescopic displacement, the actual value of the telescopic displacement of the third active arm is subtracted from the given value of the telescopic displacement, and the rotation angle of the C-axis of the A/C-axis double swing angle head The actual value is subtracted from the given value of the rotation angle, and the actual value of the rotation angle of the A-axis 10 of the A/C-axis double swing angle head is subtracted from the given value of the rotation angle to obtain the corresponding deviation respectively, and the corresponding deviation is used as the compensation value. The multi-axis motion controller gives control signals based on the deviation to control the work of the servo motors that drive the first to third active arms and the A and C axes, so that the machining tools installed on the A/C axis double swing angle head 9 follow the machining path. move.

If the servo motor that drives the telescopic displacement of the first active arm, the second active arm and the third active arm is connected to the transmission mechanism that converts the rotary motion into linear motion through its output shaft, the transmission mechanism that converts the rotary motion into linear motion can be For belt transmission mechanism, sprocket transmission mechanism, rack and pinion transmission mechanism, worm gear transmission mechanism, screw transmission mechanism, etc., the given value of the telescopic displacement of the first driving arm, the second driving arm and the third driving arm and the actual The value is converted into the given value and actual value of the corresponding servo motor rotation angle.

Preferably, the axis of the fifth rotating shaft 5 may be at the same distance from the axis of the third rotating shaft and the axis of the fourth rotating shaft.

The first angle sensor 14 , the second angle sensor 4 , the third angle sensor 8 and the fourth angle sensor 11 can be angle sensors used in the prior art for measuring the rotation angle, such as a circular grating; the first displacement sensor 12 can be A displacement sensor used for measuring linear displacement in the prior art is used, for example, a linear grating can be selected.

The motion controller is used to realize precise position control, speed control, acceleration control, torque or force control of mechanical motion. The multi-axis motion controller can control multiple servo drives at the same time. The multi-axis motion controller sends motion control commands to the servo drives. , the servo motor is driven by the servo driver to run, and then the actual rotation angle of the servo motor is detected by the angle sensor on the servo motor such as an encoder, and the detection signal is fed back to the multi-axis motion controller as a feedback signal to realize the closed-loop control of the system. The multi-axis motion controller can be a motion controller suitable for controlling more than 5 axes of servo motors in the prior art, such as the CK3M programmable multi-axis motion controller produced by OMRON.

Preferably, the first rotating bracket 16 and the second rotating bracket 6 can be arranged up and down.

Preferably, the distances from the hinge center of the first active arm and the movable platform 7 to the hinge centers of the second active arm, the third active arm and the movable platform 7 may be equal.

Preferably, the first active arm, the second active arm and the third active arm can be connected to the moving platform 7 in a spherical hinged connection.

The present invention also provides a method for online error compensation of a hybrid robot based on coarse interpolation using the above-mentioned online error compensation system for a hybrid robot based on coarse interpolation. The method includes the following steps:

Step A, in the multi-axis controller, set up five global compensation variable memories for correspondingly storing the axial displacement compensation variables of the first to third active arms and the rotation angle compensation variables of the A and C axes, and initialize the variable memory .

In step B, the multi-axis motion controller reads the G code, converts the tool nose machining trajectory into several continuous tiny line segments, and obtains the tool nose attitude data at both ends of the tiny line segments.

Step C: According to the inverse kinematics algorithm of the hybrid robot and the compensation value in the compensation variable memory, the multi-axis motion controller outputs the servo drive commands corresponding to the given values of the axial displacements of the first to third active arms, and the corresponding The servo drive command for the given value of the rotation angle of the A and C axes.

In step D, the multi-axis motion controller receives the current feedback signals of the first angle sensor 14, the second angle sensor 4, the third angle sensor 8, the fourth angle sensor 11 and the first displacement sensor 12, which are obtained according to the spatial geometry algorithm. Corresponding to the deviation of the given values of the axial displacement of the first to third active arms and the given values of the rotation angles of the A and C axes, the deviation is taken as the new compensation value, and the variable memory is updated correspondingly to the new compensation value.

Step E, repeat steps C to D until the interpolation ends.

The tool nose attitude data may include: the x-axis, y-axis, and z-axis coordinates of the tool nose point, and the rotation angle of the tool nose point around the x-axis and the y-axis.

Preferably, the above-mentioned spatial geometry algorithm can be as follows:

The detection value of the first angle sensor 14 can be set to be θ ₁ , the detection value of the second angle sensor 4 can be set to be θ ₂ , the detection value of the first displacement sensor 12 can be set to be L ₁ , the first to third active arms can be set to be connected to the moving arm. The center points around which the platform rotates relative to each other are A ₁ , A ₂ , and A ₃ in sequence, that is, the first active arm and the moving platform rotate relative to each other around point A ₁ , the second active arm and the moving platform rotate relative to each other around point A ₂ , and the third active arm rotates relative to the moving platform around point A 2 . The arm and the moving platform rotate relative to the point A3; the rotation sub-center of the first driving arm and the first rotating bracket 16 can be set as B _{1 ;} the rotation sub-center of the second and third driving arms and the second rotating bracket 6 can be set They are B ₂ and B ₃ respectively; the midpoint of the line connecting the two points A ₂ and A ₃ can be set as point A, the center of the second rotating bracket 6 can be set as point B, and the base label can be set as B-xyz; Let x _A , y _A , and z _A correspond to the x-axis, y-axis, and z-axis coordinates of point A under the base frame; it can be set that the first to third active arms are rotated by the servo motor to drive the ball screw to perform axial displacement ;

From the spatial geometric relationship, the coordinate values of point A under the base frame can be obtained as follows:

x _A =L ₁ sinθ ₂

y _A =L ₁ cosθ ₂ sinθ ₁

z _A =L ₁ cosθ ₂ cosθ ₁

According to the inverse kinematics of the parallel mechanism, the actual rotation angles of the servo motors of the first to third active arms can be obtained by the following equations:

r _A = [x _A y _A z _A ] ^T ;

q ₄ =||r _A ||;

w ₄ =r _A /q ₄ =(s _3x s _3y s _3z ) ^T ;

a _i0 =(a _i cosγ _i a _i sinγ _i 0) ^T ;

C _i =Ra _i0 ;

D _i =( _bi cosγ _i b _i sinγ _i 0) ^T ;

where:

r _A is the position vector of point A in the coordinate system B-xyz;

q ₄ is the distance from point A to point B;

w ₄ is the unit vector from point A to point B;

s _3x is the coordinate value of the point corresponding to the unit vector on the Z axis in the coordinate system Ax _A y _A z _A projected on the x axis in the coordinate system B-xyz;

s _3y is the coordinate value of the point corresponding to the unit vector on the Z axis in the coordinate system Ax _A y _A z _A projected on the y axis in the coordinate system B-xyz;

s _3z is the coordinate value of the point corresponding to the unit vector on the Z axis in the coordinate system Ax _A y _A z _A projected on the z axis in the coordinate system B-xyz;

ψ is the rotation angle of the attitude of the coordinate system Ax _A y _A z _A relative to the coordinate system B-xyz around the x-axis;

θ is the rotation angle of the posture of the coordinate system Ax _A y _A z _A relative to the coordinate system B-xyz around the y _A axis after the coordinate system Ax _A y _A z _A rotates the angle ψ about the x-axis relative to the coordinate system B-xyz;

R is the attitude matrix of the coordinate system Ax _A y _A z _A relative to the coordinate system B-xyz;

γ _i is an intermediate variable;

C _i is the position vector of point A _i in the coordinate system B-xyz; i=1,2,3;

a _i is the distance from point A _i to point A; i=1,2,3;

D _i is the position vector of point B _i in the coordinate system B-xyz; i=1,2,3;

b _i is the distance from point B _i to point B; i=1,2,3;

a _i0 is the position vector of point A _i in the coordinate system Ax _A y _A z _A ; i=1,2,3;

pi is the lead of the ball screw of the _i -th active arm; i=1,2,3;

θ _ia is the actual rotation angle of the servo motor of the i-th active arm; i=1, 2, 3.

The working principle of the present invention is further described below with a preferred embodiment of a rough interpolation-based hybrid robot error online compensation method of the present invention:

A method for online error compensation of hybrid robot based on rough interpolation, the method adopts the aforementioned online error compensation system of hybrid robot, wherein the driving servo motor of the first active arm is the driving of the first servo motor 15 and the driving of the second active arm The servo motor is the second servo motor 3, the driving servo motor of the third active arm is the third servo motor 1, the C-axis driving servo motor of the A/C-axis double swing angle head 9 is the fourth servo motor, and the A/C-axis dual The A-axis 10 of the swivel head drives the servo motor as the fifth servo motor. Assume that the multi-axis motion controller corresponds to the first to fifth servo motors in the Nth coarse interpolation cycle, and the given outputs correspond to: θ _1d (n), θ _2d (n), θ _3d (n), θ _4d (n), θ _5d (n).

The method includes the following specific steps:

Step 1, import the drawing of the part to be processed into the Unigraphics application system (UG for short), and generate the G code processed by the CNC machine after UG processing;

Step 2, the multi-axis motion controller adopts the CK3M programmable multi-axis motion controller produced by OMRON Company. In the multi-axis motion controller, the first angle sensor 14, the second angle sensor 4, the third angle sensor 8, the The four-angle sensor 11 and the first displacement sensor 12 are zeroed and initialized;

Step 3, write the G code of the part to be processed into the upper computer operating system of the hybrid robot, and download it to the lower computer multi-axis motion controller after being compiled by the upper computer;

Step 4: Detect the position error of the reference point of the moving platform 7 and the angle error of the swivel head in real time during the movement of the robot, calculate the compensation amount, and realize the online compensation control of the adjacent point error based on the rough interpolation. The steps are as follows:

(a) The robot motion controller reads the G code, performs rough interpolation according to the corresponding motion commands (line command, arc command, point-to-point command, etc.), and obtains the attitude information of the two ends of the micro-track of the tool nose machining;

(b) Five virtual axis global variable memories are preset and initialized in the multi-axis motion controller, and the variable memories are used to store the compensation amount data of each axis;

(c) For the command output value of the real-axis servo motor in the multi-axis motion controller, the position in the motion controller system is called in the "Language\Realtime Routines\ _usrcode.c " project from the attitude information rc of the endpoint of the micro-track in the rough interpolation process. The inverse kinematics algorithm of the hybrid robot written in the file solves the command value of the real axis servo motor;

(d) After the robot runs, the first angle sensor 14 in the parallel mechanism is used to detect the actual rotation angle θ ₁ of the sixth rotating shaft 13 in real time, the second angle sensor 4 detects the actual rotation angle θ ₂ of the fifth rotating shaft 5 in real time, and the first displacement The sensor 12 detects the displacement L ₁ of the driven support arm 2 along the axial direction in real time, obtains the position information x _A , y _A , z _A of the reference point A of the moving platform 7 in combination with the spatial geometric relationship, and then calls usrcode. The error algorithm program written in c calculates the actual rotation angle θ _1a (n-1), θ _2a (n-1 ), θ _3a (n-1), and subtract the actual value from the given value of the rotation angle θ _1d (n-1), θ _2d (n-1), and θ _3d (n-1) to obtain the compensation value Δθ ₁ (n-1), Δθ ₂ (n-1), Δθ ₃ (n-1); at the same time, the actual rotation angle θ of the C-axis is detected in real time by using the third angle sensor 8 of the A/C-axis double-swing angle head 9 in series ₃ and the fourth angle sensor 11 detect the actual rotation angle θ ₄ of the A-axis 10 in real time, and obtain the actual value of the A/C-axis double-swing angle head 9 servo motor of the series A/C-axis double-swing angle head 9 of the N-1th coarse interpolation cycle in the rough interpolation through the inversion of the joints. The rotation angles θ _4a (n-1) and θ _5a (n-1) are subtracted from the command rotation angles θ _4d (n-1) and θ _5d (n-1) to obtain the compensation value Δθ ₄ (n-1 ), Δθ ₅ (n-1), put the obtained compensation values into the five pre-set virtual axis global variables respectively, and the five virtual axis global variables are stored and updated to new compensation values;

(e) The compensation value is added to the servo motor command output values θ _1d (n), θ of the first to third active arms and the A/C axis double swing angle head 9 in the Nth coarse interpolation cycle in the coarse interpolation _{In 2d} (n), θ _3d (n), θ _4d (n), and θ _5d (n), the on-line compensation of adjacent point errors in the rough interpolation process is completed.

The above-mentioned embodiments are only used to illustrate the technical idea and characteristics of the present invention, and the purpose is to enable those skilled in the art to understand the content of the present invention and implement them accordingly, and the present invention cannot be limited only by the present embodiment. The patent scope, that is, all equivalent changes or modifications made to the spirit disclosed in the present invention, still fall within the patent scope of the present invention.

Claims (10)

1. An error online compensation system of a hybrid robot based on coarse interpolation is characterized by comprising a hybrid mechanical arm, a first rotating bracket, a second rotating bracket, a detection system and a control system; wherein:

the hybrid mechanical arm comprises a movable platform; three driving arms which are driven by a servo motor to stretch and retract are hinged on the periphery of the movable platform and sequentially comprise a first driving arm, a second driving arm and a third driving arm; the rear end of the movable platform is fixedly connected with a driven supporting arm; the front end of the movable platform is connected with an A/C shaft double-swing-angle head driven by a servo motor, wherein a C shaft of the double-swing-angle head is rotatably connected with the movable platform; the first driving arm is rotationally connected with the first rotating bracket through a first rotating shaft; the first rotating bracket is rotationally connected with the fixed bearing seat through a second rotating shaft; the second driving arm, the third driving arm and the driven supporting arm are correspondingly and rotatably connected with the second rotating support through a third rotating shaft, a fourth rotating shaft and a fifth rotating shaft; the second rotating bracket is rotationally connected with the fixed bearing seat through a sixth rotating shaft; the axes of the first rotating shaft and the second rotating shaft are vertical; the axes of the third rotating shaft, the fourth rotating shaft and the fifth rotating shaft are parallel to each other and are vertical to the axis of the sixth rotating shaft; the center of the second rotating bracket is positioned on the intersection point of the axis of the fifth rotating shaft and the axis of the sixth rotating shaft;

the detection system comprises: the first angle sensor is used for detecting the rotation angle of the sixth rotating shaft; the second angle sensor is used for detecting the rotation angle of the fifth rotating shaft; a third angle sensor for detecting a rotation angle of the C-axis; a fourth angle sensor for detecting a rotation angle of the a axis; a first displacement sensor for detecting axial displacement of the driven support arm;

the control system comprises a multi-axis motion controller; the multi-axis motion controller receives detection signals from the first to fourth angle sensors and the first displacement sensor, converts the detection values into telescopic displacement values corresponding to the first to third main arms and a rotation angle value corresponding to the A, C axis, subtracts the values from corresponding given values to obtain a deviation, and gives a control signal based on the deviation to control the operation of servo motors driving the first to third main arms and the A, C axis.

2. The system for online error compensation of a hybrid robot based on coarse interpolation of claim 1, wherein the distance between the axis of the fifth rotating shaft and the axis of the third rotating shaft is equal to the distance between the axis of the fourth rotating shaft and the axis of the fifth rotating shaft.

3. The system for the online error compensation of the hybrid robot based on the coarse interpolation according to claim 1, wherein the first to fourth angular sensors are circular gratings; the first displacement sensor is a linear grating.

4. The system for the online error compensation of the hybrid robot based on the coarse interpolation as claimed in claim 1, wherein the multi-axis motion controller is an Onglong CK3M type multi-axis motion controller.

5. The system for the online error compensation of the hybrid robot based on the coarse interpolation according to claim 1, wherein the first rotating bracket and the second rotating bracket are arranged on top of each other.

6. The system for the online compensation of the error of the hybrid robot based on the coarse interpolation as recited in claim 1, wherein the distances from the hinge center of the first active arm and the movable platform to the hinge centers of the second active arm and the third active arm and the movable platform are equal.

7. The system of claim 1, wherein the first active arm, the second active arm and the third active arm are connected to the moving platform in a ball joint manner.

8. An online error compensation method for a hybrid robot based on coarse interpolation, which utilizes the online error compensation system for a hybrid robot based on coarse interpolation according to any one of claims 1 to 7, is characterized by comprising the following steps:

step A, setting five global compensation variable memories in a multi-axis motion controller, and initializing the variable memories, wherein the five global compensation variable memories are used for correspondingly storing axial displacement compensation variables of first to third driving arms and a rotation angle compensation variable of an A, C shaft;

b, reading the G code by the multi-axis motion controller, converting the tool nose processing track into a plurality of continuous micro line segments, and obtaining tool nose posture data of two end points of the micro line segments;

step C, outputting a servo driving instruction corresponding to the given axial displacement values of the first driving arm, the third driving arm and a servo driving instruction corresponding to the given rotation angle value of the A, C shaft by the multi-shaft motion controller according to an inverse kinematics algorithm of the hybrid robot and by combining a compensation value in the compensation variable storage;

d, the multi-axis motion controller receives feedback signals of the current first to fourth angle sensors and the current first displacement sensor, obtains deviations corresponding to given axial displacement values of the first to third driving arms and given rotation angle values of the A, C shaft according to a space geometry algorithm, takes the deviations as new compensation values, and correspondingly updates the variable memory into the new compensation values;

and E, repeating the step C to the step D until the interpolation is finished.

9. The hybrid robot error online compensation method based on the coarse interpolation as claimed in claim 8, wherein the tool tip attitude data comprises: the x-axis, y-axis and z-axis coordinates of the tool point, and the rotation angle of the tool point around the x-axis and the y-axis.

10. The hybrid robot error online compensation method based on the coarse interpolation as claimed in claim 8, wherein the spatial geometry algorithm is as follows:

let the first angle sensor detect value be theta₁Let the second angle sensor detect a value of θ₂Let the first displacement sensor detect a value L₁A is the central point of the relative rotation and the surrounding of the first to the third driving arms and the movable platform in turn₁、A₂、A₃Setting the center of the revolute pair of the first driving arm and the first revolute support as B₁(ii) a B is respectively set as the centers of the revolute pairs of the second driving arm, the third driving arm and the second rotating bracket₂、B₃(ii) a Let A₂、A₃The midpoint of the connecting line of the two points is a point A, the center of the second rotating bracket is a point B, and the base mark is B-xyz; let x_A、y_A、z_ACorresponding to the coordinates of the point A on the x axis, the y axis and the z axis under the base standard system; the first to third driving arms are arranged and driven by the servo motor to rotate so as to drive the ball screw to perform axial displacement;

and obtaining the coordinate values of the point A under the base coordinate system according to the space geometric relationship as follows:

x_A＝L₁sinθ₂

y_A＝L₁cosθ₂sinθ₁

z_A＝L₁cosθ₂cosθ₁

according to the inverse kinematics of the parallel mechanism, the actual rotation angles of the servo motors of the first to third driving arms are obtained by the following solving methods:

r_A＝[x_Ay_Az_A]^T；

q₄＝||r_A||；

w₄＝r_A/q₄＝(s_3xs_3ys_3z)^T；

a_i0＝(a_icosγ_ia_isinγ_i0)^T；

C_i＝Ra_i0；

D_i＝(b_icosγ_ib_isinγ_i0)^T；

in the formula:

r_Ais the position vector of the point A under the coordinate system B-xyz;

q₄is the distance from point a to point B;

w₄is a unit vector from point a to point B;

s_3xas a coordinate system A-x_Ay_Az_AThe point corresponding to the unit vector on the Z axis is projected on the coordinate value on the x axis in the coordinate system B-xyz;

s_3yas a coordinateIs A-x_Ay_Az_AThe point corresponding to the unit vector on the Z axis is projected on the coordinate value on the y axis in the coordinate system B-xyz;

s_3zas a coordinate system A-x_Ay_Az_AThe point corresponding to the unit vector on the Z axis is projected on the coordinate value on the Z axis in the coordinate system B-xyz;

psi as a coordinate system A-x_Ay_Az_AThe angle of rotation of the attitude of (a) relative to the coordinate system B-xyz about the x-axis;

theta is a coordinate system A-x_Ay_Az_AAfter rotating the coordinate system B-xyz by the angle psi around the x-axis, the coordinate system A-x_Ay_Az_AIs wound around y with respect to the coordinate system B-xyz_AThe rotation angle of the shaft;

r is a coordinate system A-x_Ay_Az_AA pose matrix relative to the coordinate system B-xyz;

γ_iis an intermediate variable;

C_iis point A_iA position vector under the coordinate system B-xyz; i is 1,2, 3;

a_iis point A_iDistance to point a; i is 1,2, 3;

D_iis point B_iA position vector under the coordinate system B-xyz; i is 1,2, 3;

b_iis point B_iDistance to point B; i is 1,2, 3;

a_i0is point A_iIn a coordinate system A-x_Ay_Az_AA lower position vector; i is 1,2, 3;

p_ithe lead of the ball screw of the ith driving arm; i is 1,2, 3;

θ_iathe actual rotation angle of the servo motor of the ith driving arm; i is 1,2, 3.

Images