CN111650882A - Hybrid robot error online compensation system and method based on coarse interpolation - Google Patents
Hybrid robot error online compensation system and method based on coarse interpolation Download PDFInfo
- Publication number
- CN111650882A CN111650882A CN202010479811.XA CN202010479811A CN111650882A CN 111650882 A CN111650882 A CN 111650882A CN 202010479811 A CN202010479811 A CN 202010479811A CN 111650882 A CN111650882 A CN 111650882A
- Authority
- CN
- China
- Prior art keywords
- axis
- point
- arm
- rotating shaft
- driving
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B19/00—Programme-control systems
- G05B19/02—Programme-control systems electric
- G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
- G05B19/404—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by control arrangements for compensation, e.g. for backlash, overshoot, tool offset, tool wear, temperature, machine construction errors, load, inertia
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/34—Director, elements to supervisory
- G05B2219/34117—Machine workpiece along, parallel to largest dimension
Abstract
The invention discloses a rough interpolation-based online error compensation system and method for a hybrid robot, wherein the system comprises a hybrid mechanical arm, a first rotating bracket, a second rotating bracket, a detection system and a control system; the periphery of a movable platform in the hybrid mechanical arm is hinged with first to third driving arms, and the rear end of the movable platform is fixedly connected with a driven supporting arm; the front end of the front end is connected with an A/C shaft double-swing-angle head; the second driving arm, the third driving arm and the driven supporting arm are connected with the second rotating support through third rotating shafts, the third rotating shafts and the fifth rotating shafts; the second rotating bracket is connected with the bearing seat through a sixth rotating shaft; the detection system comprises: the first angle sensor and the second angle sensor are used for detecting the rotating angles of the sixth rotating shaft and the fifth rotating shaft; a third and a fourth angular sensor for detecting C, A the rotation angle of the shaft; a first displacement sensor for detecting axial displacement of the driven support arm; and a multi-axis motion controller in the control system receives feedback signals of the sensors and outputs signals to control the work of the corresponding servo motor. The compensation method is simple, and the positioning accuracy of the hybrid robot can be improved.
Description
Technical Field
The invention relates to the field of robot machining, in particular to a hybrid robot error online compensation system and method based on coarse interpolation.
Background
In order to adapt to the service environment of large-scale movement and local high-speed precision machining for machining large structural parts, the characteristics of large overall dimension, complex geometric shape, high precision requirement and the like of the large structural parts are met, and a single-machine manufacturing unit or a multi-machine manufacturing system which takes series-parallel configuration equipment as a core functional part is gradually becoming an indispensable important tool. However, in the use process, the hybrid robot may deform to a certain extent due to the load of the end effector and the self gravity of the mechanical arm, so that the position of the end of the robot may deviate, and the actual size of the robot part may not completely conform to the theoretical size in the machining and manufacturing processes, which may cause the inaccuracy of the length of the driving branched chain in the assembly process, the transmission error of each driving joint due to the reducer, and the joint error of the swing head due to the clearance, friction and zero deviation of the reducer, which may cause the inconsistency of the theoretical model parameters used for calculation in the motion controller with the actual parameters of the robot, and directly affect the positioning accuracy of the end.
The method for improving the precision of the robot mainly comprises an error prevention technology and an error compensation technology. At present, the error prevention technology is always limited by the part manufacturing technology and economic benefit, and the error compensation technology can effectively improve the precision of the robot under the condition of lower hardware investment. The error compensation techniques are mainly of two types: one is off-line compensation, and the accuracy is improved by calibrating or establishing an error compensation mapping model before the robot is used, and the method is complex and cannot be changed when used on site; the other type is online compensation, errors are detected in the using process of the robot, and the errors are compensated in real time. The existing online compensation method has the disadvantages of more monitoring data, large calculation amount, occupation of the storage capacity and the running speed of a computer, and influence on the running speed of equipment.
Disclosure of Invention
The invention provides a high-efficiency and accurate hybrid robot error online compensation system and method based on coarse interpolation for solving the technical problems in the prior art.
The technical scheme adopted by the invention for solving the technical problems in the prior art is as follows: an error online compensation system of a hybrid robot based on coarse interpolation comprises 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.
Furthermore, the distance between the axis of the fifth rotating shaft and the axis of the third rotating shaft is equal to that between 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 an ohm dragon CK3M type 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 and the third driving arm and the movable platform are equal.
Furthermore, the first driving arm, the second driving arm and the third driving arm are connected with the movable platform in a spherical hinge mode.
The invention also provides a rough interpolation-based online error compensation method for the hybrid robot, which adopts the rough interpolation-based online error compensation system for the hybrid robot, and comprises the following steps:
step A, setting five global compensation variable memories in a multi-axis 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.
Further, the tool tip attitude data includes: 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.
Further, the spatial geometry algorithm is as follows:
let the first angle sensor detect value be theta1Let the second angle sensor detect a value of θ2Let the first displacement sensor detect a value L1A is the central point of the relative rotation and the surrounding of the first to the third driving arms and the movable platform in turn1、A2、A3Setting the center of the revolute pair of the first driving arm and the first revolute support as B1(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 bracket2、B3(ii) a Let A2、A3The 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 xA、yA、zACorresponding 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:
xA＝L1sinθ2
yA＝L1cosθ2sinθ1
zA＝L1cosθ2cosθ1
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:
rA＝[xAyAzA]T；
q4＝||rA||；
w4＝rA/q4＝(s3xs3ys3z)T；
ai0＝(aicosγiaisinγi0)T；
Ci＝Rai0；
Di＝(bicosγibisinγi0)T；
in the formula:
rAis the position vector of the point A under the coordinate system B-xyz;
q4is the distance from point a to point B;
w4is a unit vector from point a to point B;
s3xas a coordinate system A-xAyAzAThe 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;
s3yas a coordinate system A-xAyAzAThe 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;
s3zas a coordinate system A-xAyAzAThe 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-xAyAzAThe 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-xAyAzAAfter rotating the coordinate system B-xyz by the angle psi around the x-axis, the coordinate system A-xAyAzAIs wound around y with respect to the coordinate system B-xyzAThe rotation angle of the shaft;
r is a coordinate system A-xAyAzAA pose matrix relative to the coordinate system B-xyz;
γiis an intermediate variable;
Ciis point AiA position vector under the coordinate system B-xyz; i is 1,2, 3;
aiis point AiDistance to point a; i is 1,2, 3;
Diis point BiA position vector under the coordinate system B-xyz; i is 1,2, 3;
biis point BiDistance to point B; i is 1,2, 3;
ai0is point AiIn a coordinate system A-xAyAzAA lower position vector; i is 1,2, 3;
pithe 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.
The invention has the advantages and positive effects that: the method can compensate the tail end position deviation of the hybrid robot caused by the self weight of a mechanical arm and the tail end load in the machining process, the size error of parts generated in the machining and assembling process, the transmission error of each driving joint and the joint error of a swing angle head caused by the gap, friction and zero offset of a speed reducer in the machining and assembling process in real time on line, can improve the positioning precision of the hybrid robot, is simpler compared with the traditional kinematics calibration method and an off-line compensation method, completes the error compensation of adjacent interpolation points in the coarse interpolation process, has simple compensation value algorithm and easy on-line compensation control, and has the compensation effect meeting the requirement of high-speed and high-precision motion control of the robot.
Drawings
Fig. 1 is a schematic structural diagram of an error online compensation system of a hybrid robot based on coarse interpolation according to the present invention.
FIG. 2 is a flow chart of the online error compensation method for the hybrid robot based on the coarse interpolation.
Fig. 3 is a geometric schematic of the spatial geometry algorithm of the present invention.
In the figure: 1. a third servo motor; 2. a driven support arm; 3. a second servo motor; 4. a second angle sensor; 5. a fifth rotating shaft; 6. a second rotating bracket; 7. a movable platform; 8. a third angle sensor; 9. A/C shaft double-swing-angle head; 10. an A axis; 11. a fourth angular sensor; 12. a first displacement sensor; 13. a sixth rotating shaft; 14. a first angle sensor; 15. a first servo motor; 16. a first rotating bracket.
Detailed Description
For further understanding of the contents, features and effects of the present invention, the following embodiments are enumerated in conjunction with the accompanying drawings, and the following detailed description is given:
referring to fig. 1 to 3, an online error compensation system for a hybrid robot based on coarse interpolation includes a hybrid mechanical arm, a first rotating bracket 16, a second rotating bracket 6, a detection system and a control system; wherein:
the hybrid mechanical arm comprises a movable platform 7; three driving arms which are driven by a servo motor to stretch and retract are hinged to the periphery of the movable platform 7 and sequentially comprise a first driving arm, a second driving arm and a third driving arm; the rear end of the movable platform 7 is fixedly connected with a driven supporting arm 2; the front end of the movable platform 7 is connected with an A/C shaft double-swing-angle head 9 driven by a servo motor, wherein the C shaft of the A/C shaft double-swing-angle head 9 is rotatably connected with the movable platform 7; the first driving arm is rotatably connected with the first rotating bracket 16 through a first rotating shaft; the first rotating bracket 16 is rotatably connected with the fixed bearing seat through a second rotating shaft; the second driving arm, the third driving arm and the driven supporting arm 2 are correspondingly and rotatably connected with a second rotating bracket 6 through a third rotating shaft, a fourth rotating shaft and a fifth rotating shaft 5; the second rotating bracket 6 is rotationally connected with the fixed bearing seat through a sixth rotating shaft 13; 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 5 are parallel to each other and are vertical to the axis of the sixth rotating shaft 13; the center of the second rotating bracket 6 is positioned on the intersection point of the axis of the fifth rotating shaft 5 and the axis of the sixth rotating shaft 13;
the first driving arm, the second driving arm and the third driving arm form a parallel mechanism of the hybrid robot; the A shaft 10 of the A/C shaft double-swing-angle head and the C shaft of the A/C shaft double-swing-angle head 9 form a series-parallel robot series mechanism. The driven supporting arm 2 is used for fixedly supporting the movable platform 7 and restraining the movement of the movable platform 7.
The driving servo motor of the first main arm is called a first servo motor 15, the driving servo motor of the second main arm is called a second servo motor 3, the driving servo motor of the third main arm is called a third servo motor 1, the C-axis driving servo motor of the A/C-axis double-swing-angle head 9 is called a fourth servo motor, and the A-axis driving servo motor of the A/C-axis double-swing-angle head is called a fifth servo motor. The servo motors of the first to third driving arms can adopt servo motors for outputting torque, the output shafts of the servo motors can be connected with a transmission mechanism for converting rotary motion into linear motion, preferably a rolling screw transmission mechanism, the rotary drive of the servo motors is converted into linear drive, and the servo motors of the first to third driving arms can also adopt linear servo motors to realize the linear extension of the first driving arm, the second driving arm and the third driving arm.
The detection system comprises: a first angle sensor 14 for detecting a rotation angle of the sixth rotating shaft 13; a second angle sensor 4 for detecting a rotation angle of the fifth rotating shaft 5; a third angle sensor 8 for detecting a C-axis rotation angle; a fourth angle sensor 11 for detecting a 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 comprises a multi-axis motion controller; the multi-axis motion controller receives detection signals from a first angle sensor 14, a second angle sensor 4, a third angle sensor 8, a fourth angle sensor 11 and a first displacement sensor 12, converts detection values into telescopic displacement values corresponding to a first active arm, a second active arm and a third active arm and rotation angle values of an A shaft 10 and a C shaft, subtracts the telescopic displacement values from corresponding given values to obtain a deviation, gives a control signal based on the deviation, and controls the operation of servo motors driving the first active arm, the second active arm, the third active arm, the A shaft 10 and the C shaft.
The actual value of the telescopic displacement of the first active arm, the second active arm and the third active arm and the given value of the rotation angle of the A shaft 10 and the C shaft obtained by converting the detection values are used as feedback actual values, and are correspondingly subtracted from the given values of the telescopic displacement of the first active arm, the second active arm and the third active arm and the given values of the rotation angle of the A shaft 10 and the C shaft to obtain deviations, namely the actual value of the telescopic displacement of the first active arm is subtracted from the given value of the telescopic displacement thereof, the actual value of the telescopic displacement of the second active arm is subtracted from the given value of the telescopic displacement thereof, the actual value of the telescopic displacement of the third active arm is subtracted from the given value of the telescopic displacement thereof, the actual value of the rotation angle of the C shaft of the A/C shaft double-angle head is subtracted from the given value of the rotation angle thereof, and the actual values of the rotation angle of the A shaft 10 of the A/C shaft double-angle, and taking the corresponding deviation as a compensation value, giving a control signal by the multi-axis motion controller based on the deviation, and controlling the work of the servo motors driving the first to third driving arms and the A, C shaft so that the processing tool arranged on the A/C shaft double-swing-angle head 9 moves along the processing path.
If the servo motor for driving the first driving arm, the second driving arm and the third driving arm to move in a telescopic way is connected with the transmission mechanism for changing the rotary motion into the linear motion through an output shaft of the servo motor, and the transmission mechanism for changing the rotary motion into the linear motion can be a belt transmission mechanism, a chain wheel transmission mechanism, a gear rack transmission mechanism, a worm gear transmission mechanism, a lead screw transmission mechanism and the like, the given values and the actual values of the telescopic displacement of the first driving arm, the second driving arm and the third driving arm are converted into the given values and the actual values of the rotation angle of the corresponding servo motor.
Preferably, the axis of the fifth rotating shaft 5 may be equidistant 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 adopt angle sensors used for measuring rotation angles in the prior art, such as circular gratings; the first displacement sensor 12 may be a displacement sensor used in the prior art for measuring linear displacement, such as a linear grating.
The motion controller is used for realizing precise position control, speed control, acceleration control and torque or force control of mechanical motion, the multi-axis motion controller can simultaneously control a plurality of servo drivers, the multi-axis motion controller sends motion control instructions to the servo drivers, the servo drivers drive the servo motors to operate, actual rotation angles of the servo motors are detected through angle sensors, such as encoders and the like, on the servo motors, detection signals are fed back to the multi-axis motion controller as feedback signals, and closed-loop control of the system is realized. The multi-axis motion controller may be a prior art motion controller suitable for controlling a 5-axis or more servo motor, such as a CK3M programmable multi-axis motion controller manufactured by OMRON corporation.
Preferably, the first rotating bracket 16 and the second rotating bracket 6 may be disposed 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 and the third active arm and the movable platform 7 may be equal.
Preferably, the first driving arm, the second driving arm and the third driving arm may be ball-hinged to the movable platform 7.
The invention also provides a rough interpolation-based online error compensation method for the hybrid robot, which adopts the rough interpolation-based online error compensation system for the hybrid robot, and comprises the following steps:
and step A, setting five global compensation variable memories in the multi-axis controller, and initializing the variable memories, wherein the five global compensation variable memories are used for correspondingly storing the axial displacement compensation variables of the first driving arm, the second driving arm, the third driving arm and the A, C shaft rotation angle compensation variables.
And step 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.
And step C, outputting a servo driving command corresponding to the given axial displacement values of the first driving arm, the third driving arm and the A, C shaft rotation angle given value by the multi-axis motion controller according to an inverse kinematics algorithm of the hybrid robot and by combining the compensation value in the compensation variable storage.
And D, receiving feedback signals of the current first angle sensor 14, the current second angle sensor 4, the current third angle sensor 8, the current fourth angle sensor 11 and the current first displacement sensor 12 by the multi-axis motion controller, obtaining deviations corresponding to axial displacement given values of the first driving arm, the second driving arm, the third driving arm and a rotation angle given value of the A, C shaft according to a space geometric algorithm, taking the deviations as new compensation values, and correspondingly updating the variable memory into the new compensation values.
And E, repeating the step C to the step D until the interpolation is finished.
Wherein the tip attitude data may include: the x-axis, y-axis and z-axis coordinates of the tool point, the rotation angle of the tool point around the x-axis and the y-axis, and the like.
Preferably, the spatial geometry algorithm described above may be as follows:
the detection value of the first angle sensor 14 may be set to θ1The value detected by the second angle sensor 4 may be set to θ2The detection value of the first displacement sensor 12 can be set to L1The central points of the first to third driving arms and the movable platform which rotate relatively and surround can be set as A in sequence1、A2、A3I.e. the first active arm and the movable platform surround the point A1Relatively rotates, and the second driving arm and the movable platform surround the point A2Relatively rotates, and the third driving arm and the movable platform surround the point A3Relative rotation is carried out; can be provided with a first driving arm and a first rotating bracket16 has a revolute pair center of B1(ii) a The centers of the revolute pairs of the second and third driving arms and the second rotating bracket 6 can be respectively set as B2、B3(ii) a Can be provided with A2、A3The midpoint of the connecting line of the two points is a point A, the center of the second rotating bracket 6 is a point B, and the base mark can be set as B-xyz; can be provided with xA、yA、zACorresponding 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 can be arranged and driven by the servo motor to rotate so as to drive the ball screw to carry out axial displacement;
the coordinate values of the point A under the base coordinate system can be obtained by the space geometric relationship as follows:
xA＝L1sinθ2
yA＝L1cosθ2sinθ1
zA＝L1cosθ2cosθ1
the actual rotation angles of the servo motors of the first to third driving arms can be obtained by solving the following formulas according to the inverse kinematics of the parallel mechanism:
rA＝[xAyAzA]T；
q4＝||rA||；
w4＝rA/q4＝(s3xs3ys3z)T；
ai0＝(aicosγiaisinγi0)T；
Ci＝Rai0；
Di＝(bicosγibisinγi0)T；
in the formula:
rAis the position vector of the point A under the coordinate system B-xyz;
q4is the distance from point a to point B;
w4is a unit vector from point a to point B;
s3xas a coordinate system A-xAyAzAThe 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;
s3yas a coordinate system A-xAyAzAThe 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;
s3zas a coordinate system A-xAyAzAThe 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-xAyAzAThe 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-xAyAzAAfter rotating the coordinate system B-xyz by the angle psi around the x-axis, the coordinate system A-xAyAzAIs wound around y with respect to the coordinate system B-xyzAThe rotation angle of the shaft;
r is a coordinate system A-xAyAzAA pose matrix relative to the coordinate system B-xyz;
γiis an intermediate variable;
Ciis point AiA position vector under the coordinate system B-xyz; i is 1,2, 3;
aiis point AiDistance to point a; i is 1,2, 3;
Diis point BiA position vector under the coordinate system B-xyz; i is 1,2, 3;
biis point BiDistance to point B; i is 1,2, 3;
ai0is point AiIn a coordinate system A-xAyAzAA lower position vector; i is 1,2, 3;
pithe 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.
The working principle of the present invention is further explained by the following preferred embodiment of the hybrid robot error online compensation method based on the coarse interpolation of the present invention:
a hybrid robot error online compensation method based on coarse interpolation adopts the hybrid robot error online compensation system, wherein a driving servo motor of a first active arm is a first servo motor 15, a driving servo motor of a second active arm is a second servo motor 3, a driving servo motor of a third active arm is a third servo motor 1, a C-axis driving servo motor of an A/C-axis double-swing-angle head 9 is a fourth servo motor, and an A-axis 10 driving servo motor of an A/C-axis double-swing-angle head is a fifth servo motor. And setting the multi-axis motion controller to correspond to the first to fifth servo motors in the Nth coarse interpolation period, wherein the given output corresponds to: theta1d(n)、θ2d(n)、θ3d(n)、θ4d(n)、θ5d(n)。
The method comprises the following specific steps:
and 4, detecting the position error of the reference point of the movable platform 7 and the rotation angle error of the swing angle head in the motion process of the robot in real time, calculating compensation quantity, and realizing the online compensation control of the adjacent point error based on the coarse interpolation, wherein the method comprises the following steps:
(a) the robot motion controller reads in the G code, and performs coarse interpolation according to corresponding motion instructions (a straight line instruction, a circular arc instruction, a point-to-point instruction and the like) to obtain posture information of two end points of the tiny track processed by the tool nose;
(b) five virtual axis global variable memories are preset and initialized in the multi-axis motion controller, and the variable memories are used for storing compensation quantity data of each axis;
(c) aiming at the instruction output value of a real-axis servo motor in the multi-axis motion controller, the attitude information r of the micro-track endpoint in the course of coarse interpolationcCalling a hybrid robot inverse kinematics algorithm written in a project file of 'Language \ real routes \ usrcode.c' at the position in the motion controller system, and solving a real-axis servo motor instruction value;
(d) when the robot runs, the actual rotation angle theta of the sixth rotating shaft 13 is detected in real time by using the first angle sensor 14 in the parallel mechanism1The second angle sensor 4 detects the actual rotation angle theta of the fifth rotating shaft 5 in real time2The first displacement sensor 12 detects the displacement L of the driven supporting arm 2 along the axial direction in real time1Combining the space geometric relationship to obtain the position information x of the reference point A of the movable platform 7A、yA、zAThen calling an error algorithm program written in a usrcode.c in the multi-axis motion controller to calculate the actual rotation angle theta of the servo motors of the first to third driving arms in the N-1 th coarse interpolation period in the coarse interpolation1a(n-1)、θ2a(n-1)、θ3a(n-1) and comparing the actual value with the rotation angle given value theta1d(n-1)、θ2d(n-1)、θ3d(n-1) are subtracted to obtain a compensation value delta theta1(n-1)、Δθ2(n-1)、Δθ3(n-1); simultaneously utilizes a third angle sensor 8 of a series A/C shaft double-swing-angle head 9 to realize real-timeDetecting the actual rotation angle theta of the C-axis3And a fourth angle sensor 11 for detecting the actual rotation angle theta of the A axis 10 in real time4Obtaining the actual rotation angle theta of the serial A/C shaft double-swing-angle head 9 servo motor of the N-1 coarse interpolation period in the coarse interpolation through joint inversion4a(n-1)、θ5a(n-1) and the command rotation angle theta4d(n-1)、θ5d(n-1) are subtracted to obtain a compensation value delta theta4(n-1)、Δθ5(n-1), respectively putting the obtained compensation values into five preset virtual axis global variables, and storing and updating the five virtual axis global variables into new compensation values;
(e) adding the compensation value to the servo motor command output value theta of the first to third master arms and the A/C shaft double-swing-angle head 9 of the Nth coarse interpolation period in the coarse interpolation1d(n)、θ2d(n)、θ3d(n)、θ4d(n)、θ5dAnd (n) completing the online compensation of the errors of the adjacent points in the coarse interpolation process.
The above-mentioned embodiments are only for illustrating the technical ideas and features of the present invention, and the purpose thereof is to enable those skilled in the art to understand the contents of the present invention and to carry out the same, and the present invention shall not be limited to the embodiments, i.e. the equivalent changes or modifications made within the spirit of the present invention shall fall within the 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 theta1Let the second angle sensor detect a value of θ2Let the first displacement sensor detect a value L1A is the central point of the relative rotation and the surrounding of the first to the third driving arms and the movable platform in turn1、A2、A3Setting the center of the revolute pair of the first driving arm and the first revolute support as B1(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 bracket2、B3(ii) a Let A2、A3The 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 xA、yA、zACorresponding 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:
xA＝L1sinθ2
yA＝L1cosθ2sinθ1
zA＝L1cosθ2cosθ1
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:
rA＝[xAyAzA]T；
q4＝||rA||；
w4＝rA/q4＝(s3xs3ys3z)T；
ai0＝(aicosγiaisinγi0)T；
Ci＝Rai0；
Di＝(bicosγibisinγi0)T；
in the formula:
rAis the position vector of the point A under the coordinate system B-xyz;
q4is the distance from point a to point B;
w4is a unit vector from point a to point B;
s3xas a coordinate system A-xAyAzAThe 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;
s3yas a coordinateIs A-xAyAzAThe 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;
s3zas a coordinate system A-xAyAzAThe 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-xAyAzAThe 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-xAyAzAAfter rotating the coordinate system B-xyz by the angle psi around the x-axis, the coordinate system A-xAyAzAIs wound around y with respect to the coordinate system B-xyzAThe rotation angle of the shaft;
r is a coordinate system A-xAyAzAA pose matrix relative to the coordinate system B-xyz;
γiis an intermediate variable;
Ciis point AiA position vector under the coordinate system B-xyz; i is 1,2, 3;
aiis point AiDistance to point a; i is 1,2, 3;
Diis point BiA position vector under the coordinate system B-xyz; i is 1,2, 3;
biis point BiDistance to point B; i is 1,2, 3;
ai0is point AiIn a coordinate system A-xAyAzAA lower position vector; i is 1,2, 3;
pithe 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.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010479811.XA CN111650882A (en) | 2020-05-29 | 2020-05-29 | Hybrid robot error online compensation system and method based on coarse interpolation |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010479811.XA CN111650882A (en) | 2020-05-29 | 2020-05-29 | Hybrid robot error online compensation system and method based on coarse interpolation |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN111650882A true CN111650882A (en) | 2020-09-11 |
Family
ID=72350913
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010479811.XA Pending CN111650882A (en) | 2020-05-29 | 2020-05-29 | Hybrid robot error online compensation system and method based on coarse interpolation |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111650882A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113334357A (en) * | 2021-06-17 | 2021-09-03 | 北京航空航天大学 | Series-parallel robot system and virtual RCM motion control method |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6301525B1 (en) * | 1998-06-25 | 2001-10-09 | Neos Robotics Ab | System and method for controlling a robot |
| CN104985596A (en) * | 2015-07-09 | 2015-10-21 | 天津大学 | Five-freedom hybrid robot with multi-axis rotation brackets |
| CN105945954A (en) * | 2016-06-21 | 2016-09-21 | 天津大学 | Double-closed-loop control method for five-freedom-degree hybrid robot |
| CN107421442A (en) * | 2017-05-22 | 2017-12-01 | 天津大学 | A kind of robot localization error online compensation method of externally measured auxiliary |
-
2020
- 2020-05-29 CN CN202010479811.XA patent/CN111650882A/en active Pending
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6301525B1 (en) * | 1998-06-25 | 2001-10-09 | Neos Robotics Ab | System and method for controlling a robot |
| CN104985596A (en) * | 2015-07-09 | 2015-10-21 | 天津大学 | Five-freedom hybrid robot with multi-axis rotation brackets |
| CN105945954A (en) * | 2016-06-21 | 2016-09-21 | 天津大学 | Double-closed-loop control method for five-freedom-degree hybrid robot |
| CN107421442A (en) * | 2017-05-22 | 2017-12-01 | 天津大学 | A kind of robot localization error online compensation method of externally measured auxiliary |
Non-Patent Citations (1)
| Title |
|---|
| 黎广喜等: ""铣削加工用混联机器人参数曲线插补方法"", 《航空制造技术》 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113334357A (en) * | 2021-06-17 | 2021-09-03 | 北京航空航天大学 | Series-parallel robot system and virtual RCM motion control method |
| CN113334357B (en) * | 2021-06-17 | 2023-10-03 | 北京航空航天大学 | Series-parallel robot system and virtual RCM motion control method |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107443382B (en) | Industrial robot structure parameter error identification and compensation method | |
| CN108748159B (en) | Self-calibration method for tool coordinate system of mechanical arm | |
| KR101498835B1 (en) | Control method for seven-shaft multi-joint robot, control program, and robot control device | |
| JPH079606B2 (en) | Robot controller | |
| CN111037542B (en) | Track error compensation method for linear machining of inverse dynamics control robot | |
| CN111745623B (en) | Five-degree-of-freedom hybrid robot tail end pose error detection and compensation system and method | |
| CN102581849A (en) | Method for planning trajectories of industrial robot based on NC (numerical control) codes | |
| JPH1110575A (en) | Parallel link mechanism | |
| CN111775145A (en) | Control system of series-parallel robot | |
| CN112405519A (en) | Motion trajectory planning method for loading and unloading robot | |
| CN112373042A (en) | Method and system for monitoring pose of five-axis 3D printer | |
| EP1886771B1 (en) | Rotation center point calculating method, rotation axis calculating method, program creating method, operation method, and robot apparatus | |
| CN111650882A (en) | Hybrid robot error online compensation system and method based on coarse interpolation | |
| JP4396553B2 (en) | Robot controller, computer program | |
| JP4825614B2 (en) | Trajectory control device for articulated link mechanism | |
| CN110231010B (en) | Three-coordinate measuring machine and measuring method based on Delta parallel mechanism | |
| JP4531297B2 (en) | 6-axis control NC program generation method and generation apparatus, 6-axis control NC program generation program, and computer-readable recording medium storing the program | |
| WO2021241512A1 (en) | Control device, robot system, and control method for causing robot to execute work on workpiece | |
| JPH0260475B2 (en) | ||
| JPH0146276B2 (en) | ||
| CN114559416A (en) | Manipulator control method, manipulator control device, electronic device and storage medium | |
| CN114273681B (en) | Ring surface worm helicoid processing system and method based on serial mechanical arm | |
| JP6990120B2 (en) | Robot control device | |
| CN116021528B (en) | Robot turntable device correction method | |
| CN111844075B (en) | Coordinate measuring self-driven joint arm |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| WD01 | Invention patent application deemed withdrawn after publication | ||
| WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20200911 |