CN108608462A - A kind of quadruple precision compensation method for large-scale heavy duty mechanical arm - Google Patents

A kind of quadruple precision compensation method for large-scale heavy duty mechanical arm Download PDF

Info

Publication number
CN108608462A
CN108608462A CN201810423948.6A CN201810423948A CN108608462A CN 108608462 A CN108608462 A CN 108608462A CN 201810423948 A CN201810423948 A CN 201810423948A CN 108608462 A CN108608462 A CN 108608462A
Authority
CN
China
Prior art keywords
compensation
mechanical arm
error
model
tool
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.)
Granted
Application number
CN201810423948.6A
Other languages
Chinese (zh)
Other versions
CN108608462B (en
Inventor
陈鹏
韩强
李洪谊
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenyang Shangxian Medical System Co ltd
Original Assignee
Suzhou Shang Xian Medical Robot Technology Ltd By Share Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Suzhou Shang Xian Medical Robot Technology Ltd By Share Ltd filed Critical Suzhou Shang Xian Medical Robot Technology Ltd By Share Ltd
Priority to CN201810423948.6A priority Critical patent/CN108608462B/en
Publication of CN108608462A publication Critical patent/CN108608462A/en
Application granted granted Critical
Publication of CN108608462B publication Critical patent/CN108608462B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/10—Programme-controlled manipulators characterised by positioning means for manipulator elements
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/10—Programme-controlled manipulators characterised by positioning means for manipulator elements
    • B25J9/1005—Programme-controlled manipulators characterised by positioning means for manipulator elements comprising adjusting means
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

The invention belongs to robot control method fields, particularly relate to a kind of quadruple precision compensation method for large-scale heavy duty mechanical arm.The present invention mainly realizes the quadruple accuracy compensation of large-scale heavy duty mechanical arm by return difference compensation control, model compensation calibration, model compensation control, gravity compensation calibration, gravity compensation control, inclined square compensation calibration and square compensation control partially；The positioning accuracy that large-scale heavy duty mechanical arm can be increased substantially overcomes the contradiction between large-scale heavy duty mechanical arm and high accuracy positioning；It is less than 0.2mm using the error after quadruple accuracy compensation method, more than 2 orders of magnitude of absolute fix precision improvement.

Description

A kind of quadruple precision compensation method for large-scale heavy duty mechanical arm
Technical field
The invention belongs to robot control method field, particularly relate to it is a kind of for large-scale heavy duty mechanical arm four Weight precision compensation method.
Background technology
In mechanical arm field, large-scale heavy duty is always a pair of irreconcilable contradiction with high-precision.Large-scale heavy duty machinery Arm, which is difficult to reach high-precision reason, the following：Its assembly is difficult first, inevitably there is rigging error；Second its Armed lever is very long, and prodigious amplification is generated to joint error；Its dead weight of third and payload are all very big, and there are prodigious Vertical settlement effects.Under the collective effect of above a variety of error effects, the initial absolute positioning error of large-scale heavy duty mechanical arm Usually centimetre rank.At present to the adjustment in accuracy generally use gravitational settling correction factor or model of large-scale heavy duty mechanical arm The method of calibration, such method are absolute after adjustment in accuracy usually within an order of magnitude to the promotion limitation of precision Position error still reaches several millimeters of degree, it is difficult to meet the needs of high accuracy positioning.
Invention content
Goal of the invention：
The purpose of the present invention is by quadruple accuracy compensation, increasing substantially the positioning accuracy of large-scale heavy duty mechanical arm, gram Take the contradiction between large-scale heavy duty mechanical arm and high accuracy positioning.Quadruple precision compensation method through the invention can make big The positioning accuracy of type heavy-duty machinery arm improves two orders of magnitude or more, reaches within 0.5mm, while payload can be overcome inclined Terminal angle error caused by setting.
Technical solution：
A kind of quadruple precision compensation method for large-scale heavy duty mechanical arm, the quadruple precision compensation method specific steps are such as Under：
Step 1：Realize return difference compensating control method；
Step 2：On the basis of return difference is compensated and controlled, model compensation calibration is carried out, determines new kinematics model；
Step 3：Model compensation is controlled and is added in Mechanical arm control method, in model compensation control → return difference compensation control On the basis of system, gravity compensation calibration is carried out；
Step 4：Gravity compensation is controlled and is added in Mechanical arm control method, in gravity compensation control → model compensation control On the basis of system → return difference compensation control, inclined square compensation calibration is carried out；
Step 5：Inclined square is compensated control to be added in Mechanical arm control method, forms inclined square compensation control → gravity compensation The quadruple accuracy compensation control method of control → model compensation control → return difference compensation control, makes manipulator motion precision substantially carry It is high.
Return difference compensation control comprises the concrete steps that：
Reading position sensor reading, judge the error of joint output position and target location whether reach allowable range it It is interior；If not reaching allowable range, to target location direction with very slow speed kinematic error × proportionality coefficient away from From；Then position sensor reading is read again, judges whether the error of joint output position and target location reaches permission model Within enclosing；As do not reached allowable range, it is necessary to repeat above-mentioned compensation campaign process, until the mistake of joint output position Until difference reaches allowable range；If reaching allowable range, indicate that return difference compensation controls successfully, exits the compensation control of this return difference Cyclic process.
Step 2 comprises the concrete steps that：
Processing, installation error indicated with error matrix, processing, installation error place may to three translation, three Error is generated on the total six direction of rotation, uses dx respectivelyi、dyi、dzi、αi、βi、γiIt indicates, wherein i indicates the sequence of error matrix Number；The form that error matrix is expressed as：
Ideally, each error component is zero, then it is quadravalence unit matrix, i.e. this error matrix that error matrix, which is degenerated, It disappears in the kinematics model of mechanical arm；Conversely, mechanical arm ideal movements model any two transformation matrix it Between be inserted into an error matrix, indicate the two movement transmission links between there may be processing, installation error；
Gravitational settling indicates in the kinematics model of mechanical arm, and the translation transformation matrix of characterization connecting rod will be made to become as follows Form：
Wherein, i indicates that the serial number of translation transformation matrix, I indicate three rank unit matrixs；PiFor three dimensional vectors, the company of expression The translational movement of bar in the ideal situation；LiIt is a column vector, expression and the relevant length of connecting rod of gravitational settling herein, i-th Gravitational settling at connecting rod is to this connecting rod and this connecting rod before until all connecting rods of mechanical arm pedestal are all related, so LiIt is I dimensions；
In containing balanced new kinematics model, by the translation transformation matrix of all connecting rods with vertical settlement effects All replace with TRANgiForm；It is as follows to obtain form that is more accurate and meeting actual new kinematics model：
T=...i-1Ti·TRANgi·Tei·iTi+1·TRANgi+1·… (3)
Wherein,i-1TiIndicate that the transformation matrix of i-th of joint, T indicate that mechanical arm pedestal becomes to the totality of end flange Change matrix；
Model compensation calibration is carried out, after establishing the form of precise motion model, mechanical arm tail end flange center is obtained and exists The expression-form of position coordinates P under base coordinate system, is denoted as：
P=F (α, β, γ, dx, dy, dz, p)；Wherein, α, β, γ, dx, dy, dz, p indicate three kinds of rotation errors, three respectively The column vector that kind translation error, gravitational settling coefficient are constituted；If above-mentioned error is all sufficiently small, following total differential form is just obtained Error difference equation：
Wherein, Δ P is the difference of absolute fix and theoretical position, and δ is the column vector that all margins of error of mechanical arm are constituted, L δ are the matrix representation forms of Δ P；In above formula, δ is that model compensation calibration needs obtained result；L is each freedom of mechanical arm The function of position is spent, each margin of error being directed to when calculating L is all denoted as 0；The theoretical position of mechanical arm is each by mechanical arm Degree of freedom position is calculated by ideal movements model；So if manipulator motion is measured end to a certain position The absolute fix at end flanges center, re-records down the position of each degree of freedom of mechanical arm at this time, and it is mechanical arm just to obtain Δ P=L δ The equation of the margin of error；Measure enough position P1,…,Pm, just obtain the big matrix equation about δ：
Δ Q=LL δ (5)
Wherein,
The value of m should at least make the line number of matrix L L be more than columns；
Under normal conditions, LL is not sequency spectrum；It needs to find out linear relevant Column vector groups in LL at this time, by it In a row remove, and remove error component corresponding with this row in δ simultaneously；This operation is constantly repeated, until LL becomes row completely Until order matrix, this sequency spectrum matrix is denoted as LL', error column vector corresponding with LL' is denoted as δ '；Finally obtained square Battle array equation be：
Δ Q=LL' δ ' (6)
The error component removed is mathematically relevant with remaining error component, that is, the error component removed does not have Independent error effects, does not demarcate it；The above-mentioned matrix equation about the margin of error solves following least squqre approximation Solution：
δ '=(LL'TLL')-1LL'TΔQ (7)
In the case of mechanical arm not loading, the target of laser tracker is placed in mechanical arm tail end flange center position Ball；It allows manipulator motion to be combined to a series of joint position, the position coordinate value x, y, z of target ball is recorded with laser tracker, And corresponding joint position combination is recorded, need the practical joint position for recording joint output position sensor feedback；Profit The concrete numerical value of the various margins of error in compound movement model is fitted with the method described above；Selected joint position group The working space of mechanical arm will fully be covered by closing, while in order to reduce workload, not choose the position outside working space；Because Model compensation calibration is not the final demarcating steps of quadruple accuracy compensation method, so model compensation allows have remainder error；In order to Further decreasing workload should be regarded as managing to acceptable range for the unconspicuous error link of some error effects Think, the output position in certain joints is made to become unrelated with the calibration of model error amount in this way, dramatically reduces model and mend Repay the workload of calibration.
Model compensation control comprises the concrete steps that：
After model compensation calibration, obtains the more accurate of mechanical arm and meet actual compound movement model, be denoted as：
T=Kinaccu(q) (8)
Wherein q indicates the vector of each joint position composition of mechanical arm；It is applied in model compensation control, needs to solve Go out the Inverse Kinematics Solution of the model：
In general, after model error factor is added in Mechanical transmission test, kinematics model can become very complicated and be difficult to obtain The Inverse Kinematics Solution of parsing needs the numerical solution for using Inverse Kinematics Solution at this time；
There is the inverse solution of Mechanical transmission test, with regard to carrying out model compensation control；When an end target for specifying mechanical arm When pose, end object pose is converted to mechanical arm pedestal to the transformation matrix T of end flange, then pass through Inverse Kinematics Solution Solve joint output position q；Q is the joint of mechanical arm target output position with model compensation, while being also return difference compensation It is input in return difference compensating control method by the input of control, just continues return difference compensation after model compensation.
Gravity compensation calibration comprises the concrete steps that：
X, y, z and payload w four dimensions to the effective working space of mechanical arm carry out mesh generation；It is four-dimensional Each node (x, y, z, w) of grid indicates that mechanical arm moves to (x, y, z) in working space when payload is w Point；Each node of four-dimensional grid is demarcated, is measured between mechanical arm tail end actual measurement pose and theory target pose Difference establishes gravity compensation database, completes gravity compensation calibration process；Wherein, for payload w for mechanical arm tail end Flange generates great torque, and in gravity compensation calibration, it is 0 to make the inclined square that payload w is generated, and is not 0 by inclined square Situation, which is separated, gives inclined square compensation solution；
It is which type of terminal angle (x, y, z) point is moved to for mechanical arm, in gravity compensation calibration, mechanical arm Calibration point is moved to some common special terminal angle, obtains the nominal data under this special posture；Gravity compensation number According to the nominal data contained only in library under special posture, because mesh generation can not also be carried out to terminal angle, otherwise database In data volume will be increased to the degree that cannot achieve；The case where task posture of mechanical arm is other postures utilizes an algorithm to solve Certainly, the error of mechanical arm tail end tool and involved in this algorithm, needs first to demarcate end-of-arm tooling.
End-of-arm tooling calibration comprises the concrete steps that：
End-of-arm tooling all be can be regarded as in calibration for end-of-arm tooling, the part after mechanical arm tail end flange, including for reality The work that existing quadruple accuracy compensation method and the six-dimension force sensor and mechanical arm that install additional must install additional to realize job task Tool；The model of tool indicates that is, the displacement in tri- directions X, Y, Z is plus the corner around Y, around X with X, Y, Z, B, C；B, the name of C Adopted value is 0, and X, Y, Z also have respective nominal value；
First, allow manipulator motion to a specific position, the command value of object attitude angle is 0；Measure mechanical arm end The position of ending tool and posture, obtain at this time mechanical arm base coordinate system to the homogeneous transformation T of tool coordinates systemXYZABC；Then will The end flange rotary joint of mechanical arm rotates specific angle z, measures position and the posture of mechanical arm end-of-arm tooling again, then It is secondary to obtain a TXYZABC；Enough times are measured, a series of z-T are obtainedXYZABC, there is following relational expression：
TXYZABC(z)=TBase→FlangeTRot(z)Ttool,BCTtool,XYZ (10)
Wherein, TBase→FlangeIndicate mechanical arm pedestal to the transformation matrix of end flange, TRot(z) rotational angle z is indicated The rotational transformation matrix of generation, Ttool,BCAnd Ttool,XYZIndicate true rotation transformation and the translation transformation square of mechanical arm tail end tool Battle array；
The posture part in relational expression (10) is only investigated, following relationship is obtained：
RABC(z)=RBase→FlangeRot(z)Rtool,BC (11)
RABC(z)、RBase→Flange、Rot(z)、Rtool,BCT is indicated respectivelyXYZABC(z)、TBase→Flange、TRot(z)、Ttool,BC Spin matrix part；Lateral attitude angle B, C that tool is found out from posture relational expression (11), are asked with the method for intelligent search Solution；The phase of the absolute value of the difference for the attitude angle that the target function of intelligent search is characterized by above-mentioned posture relationship "=" both sides It hopes, smaller selected tool tilt attitude angle B, C of explanation of target function value is better；For selected one group of specific tool Lateral attitude angle B, C, are handled as follows：First R is found out in z=0Base→Flange, then utilize posture relationship when z ≠ 0 Formula and RBase→FlangeFind out target function value；
In TXYZABCRelational expression in, specify following symbol：
Then obtain the relational expression as follows about position：
Further transformation obtains：
According to the different values of the rotational angle z of end flange rotary joint, a series of relationship shaped like (13) is obtained Formula combines them：
X, Y, Z are acquired using least square method；To obtained whole feature X, Y in mechanical arm tail end tool model, Z, B, C complete end-of-arm tooling calibration.
For one group of specific value of terminal position theoretical value, by the theoretical value of three attitude angles A, B, C of terminal angle It is taken as A0, 0,0, wherein A0For the specific angles an A value；Have at this time：
Wherein,For the measured value of mechanical arm tail end pose,For the theory target of mechanical arm tail end pose Value,For the gravity compensation matrix of mechanical arm；
By gravity compensation matrixPose form, 3 position compensation amounts and 3 attitude angle compensation rates are converted into, this 6 numerical value are exactly the calibration result of this calibration point；All nodes in four-dimensional mesh generation are measured, and calculate the mark of each node Calmly as a result, ultimately forming gravity compensation database, gravity compensation calibration process is completed.
Gravity compensation control comprises the concrete steps that：
What gravity compensation to be done is exactly the database established when being demarcated by inquiring, and finds corresponding Tcomp, pass through relationship Formula：
TcompTreal=Tthe (16)
Calculate Tthe, by TtheIt is transmitted to mechanical arm as the pose after gravity compensation, after so that mechanical arm is moved by this pose Just reach Treal；
It is A for there was only attitude angle in database0, 0,0 when compensation rate, and face is posture when gravity compensation controls The problem of the case where angle is arbitrary value A, B, C, needs to use the end-of-arm tooling carried out before and demarcates to solve；If it is known that Tthe,ABC, pass through following formula：
It is calculatedWherein, Ttool,XYZ,theIndicate ideal end-of-arm tooling transformation, TZtIndicate mechanical arm wrist Center is to the transformation at end flange center, TRot(A-A0, B, C) and it indicates from attitude angle A0, 0,0 to attitude angle A, B, C rotation become Change matrix；Then by following formula, byCalculate to obtain Tcomp,ABC：
Wherein, Ttool,XYZAnd Ttool,BCRespectively indicate end-of-arm tooling actual end-of-arm tooling translation transformation obtained by calibrating and End-of-arm tooling rotation transformation；
All it is and T for the compensation rate in gravity compensation databasetheIt is corresponding, and currently known is Treal, Wo Menwu Method direct basis TrealUsing the problem of obtaining required compensation rate with Multi-dimension interpolation method of tabling look-up in gravity compensation database, have Gravity compensation relational expression：
Tcomp,ABCTreal,ABC=Tthe,ABC (19)
I.e. as known Treal,ABCWhen, as long as providing Tcomp,ABC, just obtain Tthe,ABC；
It then passes through cycle and tables look-up approximatioss solve can not direct basis TrealIt is obtained with Multi-dimension interpolation method using tabling look-up Obtain required compensation rate：First assume Tthe,ABCEqual to Treal,ABC, then T is obtained in gravity compensation database lookupcomp,ABC；According to Formula (19) updates Tthe,ABC, then by new Tthe,ABCTable look-up update Tcomp,ABC；T is updated further according to formula (19)the,ABC, constantly repeat This process, until reaching required precision.
Inclined square compensation calibration comprises the concrete steps that：
First, the inclined square range that the payload of mechanical arm may generate in two directions is determined；Within the scope of inclined square Mesh generation is carried out to the inclined square in both direction, generates a two-dimensional calibration grid；In the grid must include point (0, 0) benchmark that, (0,0) is compensated as inclined square, i.e., inclined square compensation rate is all 0 when not loading inclined square to mechanical arm；To two-dimensional grid In each node, it is real-time using six-dimension force sensor according to inclined square numerical value to the inclined square of mechanical arm tail end tool loads two dimension Inclined square value is measured, ensures that load is accurate；
The influence for excluding load gravity and mechanical arm dead weight is deformed is had to when square compensation is demarcated partially, so, first Pose on the basis of the most frequently used pose of one mechanical arm of definition, all inclined squares compensation calibration are carried out in this pose；Then right Control machinery arm is needed once to be moved using benchmark pose as target, at this after load is accurate in each two dimension inclined square Application in order is needed to demarcate all accuracy compensation control methods of completion in movement, to eliminate its other than inclined square The influence of his error source, i.e.,：
Gravity compensation control → model compensation control → return difference compensation control；
After the completion of movement, the pose of mechanical arm tail end tool at this time is measured, thus calculates end-of-arm tooling around both direction Deflection angleThe square combination partially of all two dimensions is measured, corresponding deflection angle is calculated, forms inclined square compensation database；By In the benchmark for compensating (0,0) as inclined square, so for the square combination partially of all two dimensions, it is real caused by inclined square Border deflection angle is：
Inclined square compensation control comprises the concrete steps that：
First, the numerical value of inclined square is read using six-dimension force sensor, is then tabled look-up and is utilized in inclined square compensation database Two-dimensional interpolation obtains deflection angle β, γ, carries out inclined square compensation control using following formula later：
Tout=TinTtool,XYZ,the -1TRot(-β,-γ)Ttool,XYZ,the (20)
Wherein, TinIndicate the object pose before inclined square compensation, TRotThe rotation transformation square that (- β ,-γ) expression-β ,-γ are generated Battle array, ToutIndicate the pose of inclined square compensation output.
Advantage and effect：
Quadruple accuracy compensation method is started with from four kinds of different main error sources of large-scale heavy duty mechanical arm, is moved to it continuously Four serial accuracy compensations are carried out, kinematic error can be made to decline 2 orders of magnitude, absolute precision is made to reach and repeatable accuracy phase Same magnitude, reaches the absolute fix precision of 0.5mm.Using quadruple accuracy compensation method, need end flange in mechanical arm with It installs six-dimension force sensor between tool additional, and the output end (end of joint transmission chain) in each joint of mechanical arm is needed to add Holding position sensor.
Quadruple accuracy compensation method makes the exhausted of the mechanical arm when being applied to large-scale heavy duty mechanical arm-proton therapeutic bed 40mm is more than in the case of only carrying out zero position calibration to positioning accuracy, is improved to using small after quadruple accuracy compensation method In 0.2mm, more than 2 orders of magnitude of absolute fix precision improvement.
Description of the drawings
Fig. 1 is joint of mechanical arm double-closed-loop control structure；
Fig. 2 is return difference compensating control method flow；
Fig. 3 is gravity compensation control flow；
Fig. 4 is the control main-process stream of quadruple accuracy compensation method.
Specific implementation mode
Quadruple accuracy compensation method is started with from four kinds of different main error sources of large-scale heavy duty mechanical arm, is moved to it continuously Four serial accuracy compensations are carried out, kinematic error can be made to decline 2 orders of magnitude, absolute precision is made to reach and repeatable accuracy phase Same magnitude, reaches the absolute fix precision of 0.5mm.Using quadruple accuracy compensation method, need end flange in mechanical arm with It installs six-dimension force sensor between tool additional, and the output end (end of joint transmission chain) in each joint of mechanical arm is needed to add Holding position sensor.
A kind of quadruple precision compensation method for large-scale heavy duty mechanical arm, the quadruple precision compensation method specific steps are such as Under：
Step 1：Realize return difference compensating control method；
Step 2：On the basis of return difference is compensated and controlled, model compensation calibration is carried out, determines new kinematics model；
Step 3：Model compensation is controlled and is added in Mechanical arm control method, in model compensation control → return difference compensation control On the basis of system, gravity compensation calibration is carried out；
Step 4：Gravity compensation is controlled and is added in Mechanical arm control method, in gravity compensation control → model compensation control On the basis of system → return difference compensation control, inclined square compensation calibration is carried out；
Step 5：Inclined square is compensated control to be added in Mechanical arm control method, forms inclined square compensation control → gravity compensation The quadruple accuracy compensation control method of control → model compensation control → return difference compensation control, makes manipulator motion precision substantially carry It is high.
Wherein, Mechanical arm control method is the prior art.
Quadruple accuracy compensation is respectively return difference compensation, model compensation, gravity compensation, the compensation of inclined square.Quadruple accuracy compensation method Essence be that one virtual target pose is gone out by compensation calculation by the object pose of mechanical arm, mechanical arm presses uncompensated mode (except return difference compensation) moves to virtual target pose, and virtual condition just has arrived at object pose.Quadruple accuracy compensation method Mechanical arm control in compensation flow be：
Inclined square compensation → gravity compensation → model compensation → return difference compensation
Quadruple accuracy compensation method before application, needs to demarcate each compensation process, and calibration is needed according to following Sequence carries out：
Return difference compensates calibration → model compensation calibration → gravity compensation calibration → square compensation calibration partially
The calibration in a stage is often carried out, it is necessary to which the backoff algorithm in corresponding stage is applied to the control of mechanical arm In, this is to continue with the premise for carrying out follow-up phase calibration.
Return difference compensates：
The target compensation of return difference compensation is that the backhaul of joint of mechanical arm transmission link is poor.Its compensation effect is to improve mechanical arm Joint position output accuracy makes the error of joint output position and target location within the scope of precision allowable error.Return difference is mended It is one most special in four-stage to repay, it is actually a kind of control method, and other stages are actually backoff algorithm. Return difference compensation need not demarcate, but directly apply its control method in mechanical arm control system.
Return difference compensating control method：
After each joint output end loading position sensor of mechanical arm, the joint control mode of mechanical arm becomes such as Fig. 1 Shown in double circle structure.Due to the presence of joint transmission return difference, after joint of robot motor completes positioning, joint carry-out bit Set random a small amount of there are still one, this random quantity will generate appreciable error through armed lever amplification in robot end.So After joint motor completes positioning, need to enter return difference compensation process.
Return difference compensation needs reading position sensor reading first, judges that the error of joint output position and target location is It is no to reach within allowable range；As do not reached allowable range, it is necessary to (have with very slow speed to the direction of target location Body speed is measured by testing, and speed is too low to cause compensation efficiency low, and speed is excessively high to will produce inertia mistake at transmission back clearance Rush and make return difference compensation that can not succeed) distance of kinematic error × proportionality coefficient；Then position sensor reading is read again, is sentenced Whether the error of disconnected joint output position and target location reaches within allowable range；As do not reached allowable range, it is necessary to Repeat above-mentioned compensation campaign process, until the error of joint output position reaches allowable range.Return difference compensation control Method flow is as shown in Figure 2.
Model compensation：
Model compensation is processing, installation error to be considered, caused by gravity on the basis of robot ideal movements model These factors are added to kinematics model by sedimentation and deformation, are obtained more accurate and are met actual new kinematics model.
Processing, installation error are indicated with error matrix.It may be to three translations, three in the presence of the place of processing, installation error Error is generated on a total six direction of rotation, is used respectively
dxi、dyi、dzi、αi、βi、γi
It indicates, wherein i indicates the serial number of error matrix.The form that then error matrix is expressed as：
Ideally, each error component is zero, then it is quadravalence unit matrix, i.e. this error matrix that error matrix, which is degenerated, It disappears in the kinematics model of mechanical arm.Conversely, can also be converted in any two of the ideal movements model of mechanical arm It is inserted into an error matrix between matrix, indicates that there may be processing, installation errors between the two movement transmission links.
Gravitational settling error is to cause itself link tilt due to the dead weight of large-scale heavy duty mechanical arm, caused by connecting rod End changes the relative position of connecting rod initiating terminal.Since mechanical arm is complicated multi-body system, gravitational settling is extremely complex, So being simplified shown as gravitational settling coefficient here.Gravitational settling indicates in the kinematics model of mechanical arm, will make table The translation transformation matrix of sign connecting rod becomes following form：
Wherein, i indicates that the serial number of translation transformation matrix, I indicate three rank unit matrixs；PiFor three dimensional vectors, the company of expression The translational movement of bar in the ideal situation；LiIt is a column vector, expression and the relevant length of connecting rod of gravitational settling herein, i-th Gravitational settling at connecting rod is to this connecting rod and this connecting rod before until all connecting rods of mechanical arm pedestal are all related, so LiIt is I dimensions；In some cases connecting rod is after the rotation of rotary joint, it can be all applied to by being not the dead weight of this connecting rod Gravitational settling is generated on connecting rod before, such case can be equivalent to LiIt is multiplied by the dimension of respective link before middle correspondence The trigonometric function of one joint rotation angles；
piIt is the gravitational settling coefficient matrix of 3 × i, this matrix is a sparse matrix under normal conditions, specific in matrix Which element is 0, which element is the same gravitational settling coefficient, needs to be built according to the concrete structure and coordinate system of mechanical arm Cube formula is made a concrete analysis of, and there is no universal methods.
Ideally, gravitational settling coefficient matrix piFor null matrix, TRANgiIt is perfect translational transformation matrix to degenerate.And In containing balanced new kinematics model, the translation transformation matrix of all connecting rods with vertical settlement effects should all be replaced For TRANgiForm.
Finally, it is as follows to obtain form that is more accurate and meeting actual new kinematics model：
T=...i-1Ti·TRANgi·Tei·iTi+1·TRANgi+1·… (3)
Wherein,i-1TiIndicate that the transformation matrix of i-th of joint, T indicate that mechanical arm pedestal becomes to the totality of end flange Change matrix.
Model compensation calibration process：
After the form for establishing precise motion model, mechanical arm tail end flange center can be obtained under base coordinate system Position coordinates P expression-form, can be denoted as：
P=F (α, β, γ, dx, dy, dz, p)；Wherein, α, β, γ, dx, dy, dz, p indicate three kinds of rotation errors, three respectively The column vector that kind translation error, gravitational settling coefficient are constituted.If above-mentioned error is all sufficiently small, so that it may to obtain following total differential The error difference equation of form：
Wherein, Δ P is the difference of absolute fix and theoretical position, and δ is the column vector that all margins of error of mechanical arm are constituted, L δ are the matrix representation forms of Δ P.
In above formula, δ is that model compensation calibration needs obtained result；L is the function of each degree of freedom position of mechanical arm, is being counted It calculates each margin of error being directed to when L and is all denoted as 0；The theoretical position of mechanical arm can be led to by each degree of freedom position of mechanical arm Ideal movements model is crossed to be calculated.So if by manipulator motion to a certain position, the reality at end flange center is measured Location is set, and the position of each degree of freedom of mechanical arm at this time is re-recorded down, so that it may to obtain Δ P=L δ as the side of the mechanical arm margin of error Journey.Measure enough position P1,…,Pm, so that it may to obtain the big matrix equation about δ：
Δ Q=LL δ (5)
Wherein,
The value of m should at least make the line number of matrix L L be more than columns.
Under normal conditions, LL is not sequency spectrum.It needs to find out linear relevant Column vector groups in LL at this time, by it In a row remove, and remove error component corresponding with this row in δ simultaneously.This operation is constantly repeated, until LL becomes row completely Until order matrix, this sequency spectrum matrix is denoted as LL', error column vector corresponding with LL' is denoted as δ '.Finally obtained square Battle array equation be：
Δ Q=LL' δ ' (6)
The error component removed is mathematically relevant with remaining error component, that is, the error component removed does not have Independent error effects can not demarcate it.The above-mentioned matrix equation about the margin of error can solve following minimum Two multiply approximate solution：
δ '=(LL'TLL')-1LL'TΔQ (7)
In the case of mechanical arm not loading, the target of laser tracker is placed in mechanical arm tail end flange center position Ball.It allows manipulator motion to be combined to a series of joint position, the position coordinate value x, y, z of target ball is recorded with laser tracker, And record corresponding joint position combination (needing to record the practical joint position of joint output position sensor feedback).This Sample can fit the concrete numerical value of the various margins of error in compound movement model using the method described above.Selected Joint position combines the working space that fully cover mechanical arm, while in order to reduce workload, not choose outside working space Position.Because model compensation calibration is not the final demarcating steps of quadruple accuracy compensation method, model compensation allows to have surplus Remaining error.The unconspicuous error link of some error effects is answered in order to further decrease workload to acceptable range It is regarded as preferably, the output position in certain joints being made to become unrelated with the calibration of model error amount in this way, Ke Yiji Reduce to big degree the workload of model compensation calibration.
Model compensation control method：
After model compensation calibration, the more accurate of mechanical arm can be obtained and meet actual compound movement model, It is denoted as：
T=Kinaccu(q) (8)
Wherein q indicates the vector of each joint position composition of mechanical arm.It is applied in model compensation control, needs to solve Go out the Inverse Kinematics Solution of the model：
In general, after model error factor is added in Mechanical transmission test, kinematics model can become very complicated and be difficult to obtain The Inverse Kinematics Solution of parsing needs the numerical solution for using Inverse Kinematics Solution at this time.
There is the inverse solution of Mechanical transmission test, so that it may to carry out model compensation control.When an end for specifying mechanical arm When object pose, end object pose can be converted to mechanical arm pedestal to the transformation matrix T of end flange, then pass through fortune Dynamic learn against solution solves joint output position q.Q is the joint of mechanical arm target output position with model compensation, is also simultaneously The input of return difference compensation control, it is input in return difference compensating control method, so that it may to continue back after model compensation Difference compensation.
Gravity compensation：
After model compensation calibration, a part of remainder error is still suffered from.On the other hand, large-scale heavy duty mechanical arm is effective Load can also introduce very important position error, and there is no compensated the error that payload introduces before this.So connecing Get off and needs to continue with gravity compensation.X, y, z and payload w of the gravity compensation to the effective working space of mechanical arm Four dimensions carry out mesh generation.Each node (x, y, z, w) expression mechanical arm of four-dimensional grid is w lucks in payload Move (x, y, z) point in working space.Each node of four-dimensional grid is demarcated, mechanical arm tail end actual measurement position is measured Difference between appearance and theory target pose, so that it may to establish gravity compensation database.It is mechanical whenever giving again after establishing database When the task of one (x, y, z, w) form of arm, so that it may to inquire the data in database, and Multi-dimension interpolation method is combined, to this Task carries out effective gravity compensation.
Here there are two problems：First, great torque generated to mechanical arm tail end flange without reference to payload w, two Be without reference to mechanical arm it is which type of terminal angle (x, y, z) point is moved to.For first problem, payload w Load deflection torque, referred to as inclined square are called to the torque that end flange generates；In gravity compensation calibration, payload w is made to produce Raw inclined square is 0, and the case where not being 0 by inclined square separates and gives the compensation of inclined square and solve.For Second Problem, in gravity In compensation calibration, we allow mechanical arm to move to calibration with some common special terminal angle (such as 3 attitude angles are all for 0) Point can obtain the nominal data under this special posture in this way；The mark under special posture is contained only in gravity compensation database Fixed number evidence, because we can not also carry out mesh generation to terminal angle, otherwise will be increased to can not for the data volume in database The degree of realization；The case where task posture of mechanical arm is other postures utilizes an algorithm to solve, and involved in this algorithm The error of mechanical arm tail end tool, so needing first to demarcate end-of-arm tooling.
End-of-arm tooling is demarcated：
In accurately compensating for strategy, need accurately to demarcate the end-of-arm tooling of mechanical arm.Mechanical arm tail end method End-of-arm tooling all be can be regarded as in part after orchid, include the six-dimension force sensor installed additional to realize quadruple accuracy compensation method, with And the tool that mechanical arm must install additional to realize job task.The model of tool indicates with X, Y, Z, B, C, i.e. X, Y, Z The displacement in three directions is plus the corner around Y, around X.B, the nominal value of C is 0, and X, Y, Z also have respective nominal value.
Allow manipulator motion to a specific position first, the command value of object attitude angle is 0.Measure mechanical arm tail end The position of tool and posture, can obtain at this time mechanical arm base coordinate system to the homogeneous transformation T of tool coordinates systemXYZABC.Then The end flange rotary joint of mechanical arm is rotated into specific angle z, measures position and the posture of mechanical arm end-of-arm tooling again, A T can be obtained againXYZABC.Enough times are measured, a series of z-T can be obtainedXYZABC, there is following relational expression：
TXYZABC(z)=TBase→FlangeTRot(z)Ttool,BCTtool,XYZ (10)
Wherein, TBase→FlangeIndicate mechanical arm pedestal to the transformation matrix of end flange, TRot(z) rotational angle z is indicated The rotational transformation matrix of generation, Ttool,BCAnd Ttool,XYZIndicate true rotation transformation and the translation transformation square of mechanical arm tail end tool Battle array.
The posture part in relational expression above is only investigated, following relationship can be obtained：
RABC(z)=RBase→FlangeRot(z)Rtool,BC (11)
Our target is that lateral attitude angle B, C of tool are found out from above-mentioned posture relationship, can use intelligent search Method solves.The absolute value of the difference for the attitude angle that the target function of intelligent search is characterized by above-mentioned posture relationship "=" both sides Expectation, selected tool tilt attitude angle B, C of the smaller explanation of target function value is better.It is specific for selected one group Tool tilt attitude angle B, C, we can be handled as follows：First R is found out in z=0Base→Flange, then utilize z ≠ 0 When posture relational expression and RBase→FlangeFind out target function value.
In TXYZABCRelational expression in, specify following symbol：
The relational expression about position as follows can then be obtained：
Further transformation can obtain：
According to the different values of the rotational angle z of end flange rotary joint, can obtain a series of shaped like (13) Relational expression combines them：
It can be in the hope of X, Y, Z using least square method.
Whole feature X, Y, Z, B, C in mechanical arm tail end tool model have thus been obtained, end-of-arm tooling mark is completed It is fixed.
Gravity compensation calibration process：
For each node in the mesh generation of payload w, mechanical arm tail end tool is added according to load requirement It carries.The six-dimension force sensor installed additional using mechanical arm tail end flange weighs to load, to ensure the accuracy of load, simultaneously also Inclined square is not generated after utilizing six-dimension force sensor to ensure load.After the completion of load, control machinery arm is transported with particular end posture It moves in effective working space in each nodes of locations of mesh generation, i.e. mesh generation nodes of locations and particular end posture For the object pose of mechanical arm.After the completion of movement, the attained pose of mechanical arm is measured, it is bound to wrong between object pose Difference, this error are exactly the compensation rate of gravity compensation.
Under normal conditions, for one group of specific value of terminal position theoretical value, we are by three postures of terminal angle The theoretical value of angle A, B, C are taken as A0, 0,0, wherein A0For the specific angles an A value.Have at this time：
Wherein,For the measured value of mechanical arm tail end pose,For the theory target of mechanical arm tail end pose Value,For the gravity compensation matrix of mechanical arm.
By gravity compensation matrixIt is converted into pose form (3 position compensation amounts, 3 attitude angle compensation rates), this 6 numerical value are exactly the calibration result of this calibration point.All nodes in four-dimensional mesh generation are measured, and calculate the mark of each node Calmly as a result, ultimately forming gravity compensation database, gravity compensation calibration process is completed.
Gravity compensation control method：
Gravity compensation is embodied in control method, is exactly by calculating, changing the object pose of mechanical arm.To mechanical arm Object pose is actually the pose T that mechanical arm needs actual motion to arrivereal.If without gravity compensation directly by TrealLife Order is transmitted to mechanical arm, and mechanical arm can not finally reach this pose because of gravitational settling.What gravity compensation to be done is exactly to pass through The database established when inquiry calibration, finds corresponding Tcomp, pass through relational expression：
TcompTreal=Tthe (16)
Calculate Tthe, by TtheIt is transmitted to mechanical arm as the pose after gravity compensation, after so that mechanical arm is moved by this pose Just reach Treal。
For problem one, needs to use the end-of-arm tooling carried out before and demarcate to solve.If it is known that Tthe,ABC, Ke Yitong Cross following formula：
It is calculatedWherein, Ttool,XYZ,theIndicate ideal end-of-arm tooling transformation, TZtIndicate mechanical arm wrist Center is to the transformation at end flange center, TRot(A-A0, B, C) and it indicates from attitude angle A0, 0,0 to attitude angle A, B, C rotation become Change matrix.Following formula is may then pass through, byCalculate to obtain Tcomp,ABC：
Wherein, Ttool,XYZAnd Ttool,BCRespectively indicate end-of-arm tooling actual end-of-arm tooling translation transformation obtained by calibrating and End-of-arm tooling rotation transformation.
For problem two, there is gravity compensation relational expression：
Tcomp,ABCTreal,ABC=Tthe,ABC (19)
I.e. as known Treal,ABCWhen, as long as providing Tcomp,ABC, so that it may to obtain Tthe,ABC.Then it can pass through such as Fig. 3 institutes The gravity compensation control flow that shows solves the problems, such as two.
Inclined square compensation：
Inclined square refers to the payload loaded on mechanical arm tail end tool, since load center of gravity is not or not end flange center And the loading moment relative to end flange center generated.The main effects of inclined square is to make six-dimension force sensor generate deflection to become Shape, to generate very important influence to mechanical arm tail end positioning accuracy.Inclined square compensation is on the amendment hand of this influence Section.
The load of mechanical arm, there may be the inclined square of both direction, is passed around six-dimensional force respectively relative to end flange center The x-axis direction of sensor, the y-axis direction around six-dimension force sensor.Correspondingly, the anamorphic effect that inclined square generates be exactly end-of-arm tooling around The deflection in the two directions.
Inclined square compensates calibration process：
The inclined square range that the payload of mechanical arm may generate in two directions is determined first.It is right within the scope of inclined square Inclined square in both direction carries out mesh generation, generates a two-dimensional calibration grid.Must include point (0,0) in the grid, The benchmark that (0,0) is compensated as inclined square, i.e., inclined square compensation rate is all 0 when not loading inclined square to mechanical arm.To in two-dimensional grid Each node surveyed in real time using six-dimension force sensor according to inclined square numerical value to the inclined square of mechanical arm tail end tool loads two dimension Inclined square value is measured, ensures that load is accurate.
The influence for excluding load gravity and mechanical arm dead weight is deformed is had to when square compensation is demarcated partially.To reach this mesh , pose on the basis of the most frequently used pose of a mechanical arm is defined first, and all inclined squares compensation calibration is carried out in this pose. Then control machinery arm is needed once to be moved using benchmark pose as target after load is accurate each inclined square of two dimension, Need application in order to demarcate all accuracy compensation control methods of completion in this movement, with eliminate in addition to inclined square it The influence of other outer error sources, i.e.,：
Gravity compensation control → model compensation control → return difference compensation control；
After the completion of movement, the pose of mechanical arm tail end tool at this time is measured, thus calculates end-of-arm tooling around both direction Deflection angleThe square combination partially of all two dimensions is measured, corresponding deflection angle is calculated, forms inclined square compensation database.By In the benchmark for compensating (0,0) as inclined square, so for the square combination partially of all two dimensions, it is real caused by inclined square Border deflection angle is：
Inclined square compensating control method：
Inclined square compensation is embodied in control method, is exactly by calculating, changing the object pose of mechanical arm so that mechanical arm The deflection that inclined square generates can be just offset according to the pose movement after compensation, just as not loading inclined square also without using inclined square Compensation control is the same.
The numerical value of inclined square is read first with six-dimension force sensor, then tables look-up in inclined square compensation database and utilizes two Dimension interpolation obtains deflection angle β, γ, carries out inclined square compensation control using following formula later：
Tout=TinTtool,XYZ,the -1TRot(-β,-γ)Ttool,XYZ,the (20)
Wherein, TinIndicate the object pose before inclined square compensation, TRotThe rotation transformation square that (- β ,-γ) expression-β ,-γ are generated Battle array, ToutIndicate the pose of inclined square compensation output.
Control main-process stream：
If before no any accuracy compensation of progress, the object pose of mechanical arm is denoted as Tar, then quadruple accuracy compensation The control main-process stream of method is as shown in Figure 4.As seen in Figure 4, the control of quadruple accuracy compensation method carried out inclined square compensation before this, then carried out Gravity compensation is model compensation later, is finally return difference compensation.
Quadruple accuracy compensation method is when being applied to large-scale heavy duty mechanical arm-proton therapeutic bed, existing proton therapeutic Bed weight is at 2.5 tons or so, and mechanical arm nominal load is 150kg, and arm exhibition is 3 meters, and quadruple accuracy compensation method makes the mechanical arm Absolute fix precision is more than 40mm in the case of only carrying out zero position calibration, is improved to using after quadruple accuracy compensation method It is better than 0.2mm, more than 2 orders of magnitude of absolute fix precision improvement.

Claims (10)

1. a kind of quadruple precision compensation method for large-scale heavy duty mechanical arm, it is characterised in that：The quadruple precision compensation method It is as follows：
Step 1：Realize return difference compensating control method；
Step 2：On the basis of return difference is compensated and controlled, model compensation calibration is carried out, determines new kinematics model；
Step 3：Model compensation is controlled and is added in Mechanical arm control method, in model compensation control → return difference compensation control On the basis of, carry out gravity compensation calibration；
Step 4：Gravity compensation is controlled and is added in Mechanical arm control method, gravity compensation control → model compensation control → On the basis of return difference compensation control, inclined square compensation calibration is carried out；
Step 5：Inclined square is compensated control to be added in Mechanical arm control method, forms inclined square compensation control → gravity compensation control The quadruple accuracy compensation control method of → model compensation control → return difference compensation control, makes manipulator motion precision greatly improve.
2. the quadruple precision compensation method according to claim 1 for large-scale heavy duty mechanical arm, it is characterised in that：Return difference Compensation control comprises the concrete steps that：
Reading position sensor reading, judges whether the error of joint output position and target location reaches within allowable range； If not reaching allowable range, to target location direction with the distance of very slow speed kinematic error × proportionality coefficient； Then read again position sensor reading, judge the error of joint output position and target location whether reach allowable range it It is interior；As do not reached allowable range, it is necessary to repeat above-mentioned compensation campaign process, until the error of joint output position reaches Until allowable range；If reaching allowable range, indicate that return difference compensation controls successfully, exits this return difference compensation control loop Process.
3. the quadruple precision compensation method according to claim 1 for large-scale heavy duty mechanical arm, it is characterised in that：Step Two comprise the concrete steps that：
Processing, installation error are indicated with error matrix, may be rotated to three translations, three in the place of processing, installation error Error is generated on six direction altogether, uses dx respectivelyi、dyi、dzi、αi、βi、γiIt indicates, wherein i indicates the serial number of error matrix；Accidentally The form that poor matrix is expressed as：
Ideally, each error component is zero, then it is quadravalence unit matrix that error matrix, which is degenerated, i.e., this error matrix is in machine It disappears in the kinematics model of tool arm；Conversely, any two transformation matrix in the ideal movements model of mechanical arm interleaves Enter an error matrix, indicates that there may be processing, installation errors between the two movement transmission links；
Gravitational settling indicates in the kinematics model of mechanical arm, and the translation transformation matrix of characterization connecting rod will be made to become following shape Formula：
Wherein, i indicates that the serial number of translation transformation matrix, I indicate three rank unit matrixs；PiFor three dimensional vectors, indicate that connecting rod is resonable Think the translational movement under state；LiA column vector, indicate with the relevant length of connecting rod of gravitational settling herein, at i-th of connecting rod Gravitational settling and this connecting rod and this connecting rod before until all connecting rods of mechanical arm pedestal are all related, so LiIt is that i is tieed up；
In containing balanced new kinematics model, the translation transformation matrix of all connecting rods with vertical settlement effects is all replaced It is changed to TRANgiForm；It is as follows to obtain form that is more accurate and meeting actual new kinematics model：
T=i-1Ti·TRANgi·Tei·iTi+1·TRANgi+1···· (3)
Wherein,i-1TiIndicate that the transformation matrix of i-th of joint, T indicate mechanical arm pedestal to the global transformation square of end flange Battle array；
Model compensation calibration is carried out, after establishing the form of precise motion model, obtains mechanical arm tail end flange center in pedestal The expression-form of position coordinates P under coordinate system, is denoted as：
P=F (α, β, γ, dx, dy, dz, p)；Wherein, α, β, γ, dx, dy, dz, p indicate respectively three kinds of rotation errors, three kinds it is flat The column vector that shift error, gravitational settling coefficient are constituted；If above-mentioned error is all sufficiently small, the mistake of following total differential form is just obtained Poor difference equation：
Wherein, Δ P is the difference of absolute fix and theoretical position, and δ is the column vector that all margins of error of mechanical arm are constituted, and L δ are The matrix representation forms of Δ P；In above formula, δ is that model compensation calibration needs obtained result；L is each degree of freedom position of mechanical arm Function, each margin of error being directed to when calculating L is all denoted as 0；The theoretical position of mechanical arm is by each degree of freedom of mechanical arm Position is calculated by ideal movements model；So if manipulator motion is measured end flange to a certain position The absolute fix at center, re-records down the position of each degree of freedom of mechanical arm at this time, and it is the mechanical arm margin of error just to obtain Δ P=L δ Equation；Measure enough position P1,…,Pm, just obtain the big matrix equation about δ：
Δ Q=LL δ (5)
Wherein,
The value of m should at least make the line number of matrix L L be more than columns；
Under normal conditions, LL is not sequency spectrum；It needs to find out linear relevant Column vector groups in LL at this time, it will wherein one Row remove, and remove error component corresponding with this row in δ simultaneously；This operation is constantly repeated, until LL becomes sequency spectrum square Until battle array, this sequency spectrum matrix is denoted as LL', error column vector corresponding with LL' is denoted as δ '；Finally obtained matrix side Cheng Wei：
Δ Q=LL' δ ' (6)
The error component removed is mathematically relevant with remaining error component, that is, the error component removed does not have independent Error effects, it is not demarcated；The above-mentioned matrix equation about the margin of error solves following least squqre approximation solution：
δ '=(LL'TLL')-1LL'TΔQ (7)
In the case of mechanical arm not loading, the target ball of laser tracker is placed in mechanical arm tail end flange center position；It allows Manipulator motion is combined to a series of joint position, and the position coordinate value x, y, z of target ball is recorded with laser tracker, and is recorded Under the combination of corresponding joint position, need the practical joint position for recording joint output position sensor feedback；Using above The method of narration fits the concrete numerical value of the various margins of error in compound movement model；Selected joint position combination will fill Divide the working space of covering mechanical arm, while in order to reduce workload, not choose the position outside working space；Because model is mended The final demarcating steps that calibration is not quadruple accuracy compensation method are repaid, so model compensation allows have remainder error；In order to further Reduce workload to acceptable range, for the unconspicuous error link of some error effects, should be regarded as preferably, this Sample makes the output position in certain joints become unrelated with the calibration of model error amount, dramatically reduces model compensation calibration Workload.
4. the quadruple precision compensation method according to claim 1 for large-scale heavy duty mechanical arm, it is characterised in that：Model Compensation control comprises the concrete steps that：
After model compensation calibration, obtains the more accurate of mechanical arm and meet actual compound movement model, be denoted as：
T=Kinaccu(q) (8)
Wherein q indicates the vector of each joint position composition of mechanical arm；It is applied in model compensation control, needs to solve this The Inverse Kinematics Solution of model：
In general, after model error factor is added in Mechanical transmission test, kinematics model can become very complicated and be difficult to be parsed Inverse Kinematics Solution, need the numerical solution for using Inverse Kinematics Solution at this time；
There is the inverse solution of Mechanical transmission test, with regard to carrying out model compensation control；When an end object pose for specifying mechanical arm When, end object pose is converted to mechanical arm pedestal to the transformation matrix T of end flange, then solved by Inverse Kinematics Solution Joint output position q；Q is the joint of mechanical arm target output position with model compensation, while being also return difference compensation control Input, it is input in return difference compensating control method, just continue after model compensation return difference compensation.
5. the quadruple precision compensation method according to claim 1 for large-scale heavy duty mechanical arm, it is characterised in that：Gravity Compensation calibration comprises the concrete steps that：
X, y, z and payload w four dimensions to the effective working space of mechanical arm carry out mesh generation；Four-dimensional grid Each node (x, y, z, w) indicate mechanical arm payload be w when move to (x, y, z) point in working space；It is right Each node of four-dimensional grid is demarcated, and the difference between mechanical arm tail end actual measurement pose and theory target pose is measured, Gravity compensation database is established, gravity compensation calibration process is completed；Wherein, for payload w for mechanical arm tail end flange Generate great torque, gravity compensation calibration in, make payload w generate inclined square be 0, and by inclined square be 0 the case where It separates and gives inclined square compensation solution；
It is which type of terminal angle (x, y, z) point is moved to for mechanical arm, in gravity compensation calibration, mechanical arm is with certain A common special terminal angle moves to calibration point, obtains the nominal data under this special posture；Gravity compensation database In contain only nominal data under special posture because mesh generation can not also be carried out to terminal angle, otherwise in database Data volume will be increased to the degree that cannot achieve；The case where task posture of mechanical arm is other postures utilizes an algorithm to solve, And the error of mechanical arm tail end tool involved in this algorithm, it needs first to demarcate end-of-arm tooling.
6. the quadruple precision compensation method according to claim 5 for large-scale heavy duty mechanical arm, it is characterised in that：End Tool calibration comprises the concrete steps that：
End-of-arm tooling all be can be regarded as in calibration for end-of-arm tooling, the part after mechanical arm tail end flange, including in order to realize four The tool that weight accuracy compensation method and the six-dimension force sensor and mechanical arm that install additional must install additional to realize job task； The model of tool indicates that is, the displacement in tri- directions X, Y, Z is plus the corner around Y, around X with X, Y, Z, B, C；B, the name of C Value is 0, and X, Y, Z also have respective nominal value；
First, allow manipulator motion to a specific position, the command value of object attitude angle is 0；Measure mechanical arm tail end work The position of tool and posture, obtain at this time mechanical arm base coordinate system to the homogeneous transformation T of tool coordinates systemXYZABC；It then will be mechanical The end flange rotary joint of arm rotates specific angle z, measures position and the posture of mechanical arm end-of-arm tooling again, again To a TXYZABC；Enough times are measured, a series of z-T are obtainedXYZABC, there is following relational expression：
TXYZABC(z)=TBase→FlangeTRot(z))Ttool,BCTtool,XYZ (10)
Wherein, TBase→FlangeIndicate mechanical arm pedestal to the transformation matrix of end flange, TRot(z) indicate what rotational angle z was generated Rotational transformation matrix, Ttool,BCAnd Ttool,XYZIndicate true rotation transformation and the translation transformation matrix of mechanical arm tail end tool；
The posture part in relational expression (10) is only investigated, following relationship is obtained：
RABC(z)=RBase→FlangeRot(z))Rtool,BC (11)
RABC(z)、RBase→Flange、Rot(z)、Rtool,BCT is indicated respectivelyXYZABC(z)、TBase→Flange、TRot(z)、Ttool,BCRotation Turn matrix part；Lateral attitude angle B, C that tool is found out from posture relational expression (11) are solved with the method for intelligent search；Intelligence The expectation of the absolute value of the difference for the attitude angle that the target function that can be searched for is characterized by above-mentioned posture relationship "=" both sides, index Smaller selected tool tilt attitude angle B, C of explanation of functional value is better；For selected one group specific tool tilt appearance State angle B, C, are handled as follows：First R is found out in z=0Base→Flange, then utilize z ≠ 0 when posture relational expression and RBase→FlangeFind out target function value；
In TXYZABCRelational expression in, specify following symbol：
Then obtain the relational expression as follows about position：
Further transformation obtains：
According to the different values of the rotational angle z of end flange rotary joint, a series of relational expression shaped like (13) is obtained, it will They combine：
X, Y, Z are acquired using least square method；To obtained whole feature X, Y in mechanical arm tail end tool model, Z, B, C completes end-of-arm tooling calibration.
7. the quadruple precision compensation method according to claim 5 for large-scale heavy duty mechanical arm, it is characterised in that：
For one group of specific value of terminal position theoretical value, the theoretical value of three attitude angles A, B, C of terminal angle are taken as A0, 0,0, wherein A0For the specific angles an A value；Have at this time：
Wherein,For the measured value of mechanical arm tail end pose,For the theoretical target values of mechanical arm tail end pose,For the gravity compensation matrix of mechanical arm；
By gravity compensation matrixPose form, 3 position compensation amounts and 3 attitude angle compensation rates are converted into, this 6 numbers Value is exactly the calibration result of this calibration point；All nodes in four-dimensional mesh generation are measured, and calculate the calibration knot of each node Fruit ultimately forms gravity compensation database, completes gravity compensation calibration process.
8. the quadruple precision compensation method according to claim 1 for large-scale heavy duty mechanical arm, it is characterised in that：Gravity Compensation control comprises the concrete steps that：
What gravity compensation to be done is exactly the database established when being demarcated by inquiring, and finds corresponding Tcomp, pass through relational expression：
TcompTreal=Tthe (16)
Calculate Tthe, by TtheIt is transmitted to mechanical arm as the pose after gravity compensation, after so that mechanical arm is moved by this pose just Reach Treal；
It is A for there was only attitude angle in database0, 0,0 when compensation rate, and face is that attitude angle is when gravity compensation controls The problem of the case where arbitrary value A, B, C, needs to use the end-of-arm tooling carried out before and demarcates to solve；If it is known that Tthe,ABC, Pass through following formula：
It is calculatedWherein, Ttool,XYZ,theIndicate ideal end-of-arm tooling transformation, TZtIndicate that mechanical arm wrist center is arrived The transformation at end flange center, TRot(A-A0, B, C) and it indicates from attitude angle A0, 0,0 arrive attitude angle A, B, C rotational transformation matrix； Then by following formula, byCalculate to obtain Tcomp,ABC：
Wherein, Ttool,XYZAnd Ttool,BCEnd-of-arm tooling actual end-of-arm tooling translation transformation obtained by calibrating and end are indicated respectively Tool rotation transformation；
All it is and T for the compensation rate in gravity compensation databasetheIt is corresponding, and currently known is Treal, we can not be straight It connects according to TrealUsing the problem of obtaining required compensation rate with Multi-dimension interpolation method of tabling look-up in gravity compensation database, there is gravity Compensation relationship formula：
Tcomp,ABCTreal,ABC=Tthe,ABC (19)
I.e. as known Treal,ABCWhen, as long as providing Tcomp,ABC, just obtain Tthe,ABC；
It then passes through cycle and tables look-up approximatioss solve can not direct basis TrealUsing tabling look-up with Multi-dimension interpolation method come needed for obtaining Compensation rate：First assume Tthe,ABCEqual to Treal,ABC, then T is obtained in gravity compensation database lookupcomp,ABC；According to formula (19) Update Tthe,ABC, then by new Tthe,ABCTable look-up update Tcomp,ABC；T is updated further according to formula (19)the,ABC, constantly repeat this mistake Journey, until reaching required precision.
9. the quadruple precision compensation method according to claim 1 for large-scale heavy duty mechanical arm, it is characterised in that：Inclined square Compensation calibration comprises the concrete steps that：
First, the inclined square range that the payload of mechanical arm may generate in two directions is determined；To two within the scope of inclined square Inclined square on a direction carries out mesh generation, generates a two-dimensional calibration grid；Must include point (0,0) in the grid, it will The benchmark that (0,0) is compensated as inclined square, i.e., inclined square compensation rate is all 0 when not loading inclined square to mechanical arm；To in two-dimensional grid Each node is measured according to inclined square numerical value to the inclined square of mechanical arm tail end tool loads two dimension using six-dimension force sensor in real time Inclined square value ensures that load is accurate；
The influence for excluding load gravity and mechanical arm dead weight is deformed is had to when square compensation is demarcated partially, so, it defines first Pose on the basis of the most frequently used pose of one mechanical arm, all inclined squares compensation calibration are carried out in this pose；Then for every A kind of two dimension inclined square needs control machinery arm once to be moved using benchmark pose as target after load is accurate, in this movement The middle all accuracy compensation control methods for needing application in order to demarcate completion, to eliminate other mistakes other than inclined square The influence in poor source, i.e.,：
Gravity compensation control → model compensation control → return difference compensation control；
After the completion of movement, the pose of mechanical arm tail end tool at this time is measured, thus calculates deflection of the end-of-arm tooling around both direction AngleThe square combination partially of all two dimensions is measured, corresponding deflection angle is calculated, forms inclined square compensation database；Due to The benchmark that (0,0) is compensated as inclined square, so for the square combination partially of all two dimensions, the actual deflection caused by inclined square Angle is：
10. the quadruple precision compensation method according to claim 1 for large-scale heavy duty mechanical arm, it is characterised in that：Partially Square compensation control comprises the concrete steps that：
First, the numerical value of inclined square is read using six-dimension force sensor, then tables look-up in inclined square compensation database and utilizes two dimension Interpolation obtains deflection angle β, γ, carries out inclined square compensation control using following formula later：
Tout=TinTtool,XYZ,the -1TRot(-β,-γ)Ttool,XYZ,the (20)
Wherein, TinIndicate the object pose before inclined square compensation, TRotThe rotational transformation matrix that (- β ,-γ) expression-β ,-γ are generated, ToutIndicate the pose of inclined square compensation output.
CN201810423948.6A 2018-05-07 2018-05-07 Quadruple precision compensation method for large heavy-duty mechanical arm Active CN108608462B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810423948.6A CN108608462B (en) 2018-05-07 2018-05-07 Quadruple precision compensation method for large heavy-duty mechanical arm

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810423948.6A CN108608462B (en) 2018-05-07 2018-05-07 Quadruple precision compensation method for large heavy-duty mechanical arm

Publications (2)

Publication Number Publication Date
CN108608462A true CN108608462A (en) 2018-10-02
CN108608462B CN108608462B (en) 2021-04-27

Family

ID=63661988

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810423948.6A Active CN108608462B (en) 2018-05-07 2018-05-07 Quadruple precision compensation method for large heavy-duty mechanical arm

Country Status (1)

Country Link
CN (1) CN108608462B (en)

Cited By (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109514602A (en) * 2018-11-08 2019-03-26 东南大学 A kind of industrial robot torque compensation control method based on loaded self-adaptive identification
CN110561500A (en) * 2019-09-30 2019-12-13 宜宾职业技术学院 space positioning error measuring device and measuring method thereof
CN110802585A (en) * 2019-11-20 2020-02-18 山东大学 Mechanical arm tail end sensor compensation method and contact force/moment measurement method
CN110977978A (en) * 2019-12-13 2020-04-10 易思维（杭州）科技有限公司 Demonstration method for settlement compensation in vehicle door assembly
WO2020073929A1 (en) * 2018-10-09 2020-04-16 中国铁建重工集团股份有限公司 Operation trolley, and robotic arm optical target positioning device and positioning method thereof
CN111251290A (en) * 2018-11-30 2020-06-09 汉翔航空工业股份有限公司 System and method for compensating selectable path of mechanical arm
CN111775153A (en) * 2020-07-17 2020-10-16 中国科学院宁波材料技术与工程研究所 Heavy-load robot calibration method
CN113927594A (en) * 2021-09-26 2022-01-14 珠海格力智能装备有限公司 Method and device for compensating joint rotation angle of robot
CN114098960A (en) * 2021-11-29 2022-03-01 杭州柳叶刀机器人有限公司 Automatic positioning method and device for medical instrument, mechanical arm and readable storage medium
CN114098986A (en) * 2021-11-30 2022-03-01 杭州柳叶刀机器人有限公司 Gravity error evaluation method and device, plane compensation method and robot
CN114833866A (en) * 2021-02-01 2022-08-02 精工爱普生株式会社 Angle transmission error correction method of speed reducer and robot system
WO2023024277A1 (en) * 2021-08-25 2023-03-02 深圳市优必选科技股份有限公司 Method and apparatus for controlling dual-arm robot, and dual-arm robot and readable storage medium
CN116587283A (en) * 2023-06-16 2023-08-15 南方电网电力科技股份有限公司 Mechanical arm control zero calibration method and related device
CN116945195A (en) * 2023-09-19 2023-10-27 成都飞机工业（集团）有限责任公司 Omnidirectional measurement device system arrangement, registration method, electronic device and storage medium

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS62196710A (en) * 1986-02-24 1987-08-31 Agency Of Ind Science & Technol Position control system adding frictional compensation
CN1852790A (en) * 2003-09-22 2006-10-25 松下电器产业株式会社 Apparatus and method for controlling elastic actuator
JP2009061519A (en) * 2007-09-05 2009-03-26 Panasonic Corp Method of controlling articulated robot
CN101806335A (en) * 2010-04-17 2010-08-18 吴声震 Three-chip type single-stage cycloidal speed reducer for industrial robot
CN103144109A (en) * 2013-02-25 2013-06-12 南京航空航天大学 Substation type precision compensation for robot system with additional external shaft
CN106737855A (en) * 2016-08-22 2017-05-31 南京理工大学 A kind of robot precision compensation method of comprehensive position and attitude error model and rigidity compensation
CN107421442A (en) * 2017-05-22 2017-12-01 天津大学 A kind of robot localization error online compensation method of externally measured auxiliary
CN106406277B (en) * 2016-09-23 2019-01-25 贵州珞石三盛科技有限公司 Robot kinematics' parameter error Optimization Compensation method and device

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS62196710A (en) * 1986-02-24 1987-08-31 Agency Of Ind Science & Technol Position control system adding frictional compensation
CN1852790A (en) * 2003-09-22 2006-10-25 松下电器产业株式会社 Apparatus and method for controlling elastic actuator
JP2009061519A (en) * 2007-09-05 2009-03-26 Panasonic Corp Method of controlling articulated robot
CN101806335A (en) * 2010-04-17 2010-08-18 吴声震 Three-chip type single-stage cycloidal speed reducer for industrial robot
CN103144109A (en) * 2013-02-25 2013-06-12 南京航空航天大学 Substation type precision compensation for robot system with additional external shaft
CN106737855A (en) * 2016-08-22 2017-05-31 南京理工大学 A kind of robot precision compensation method of comprehensive position and attitude error model and rigidity compensation
CN106406277B (en) * 2016-09-23 2019-01-25 贵州珞石三盛科技有限公司 Robot kinematics' parameter error Optimization Compensation method and device
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)

* Cited by examiner, † Cited by third party
Title
郑笑红等: "《工业机器人技术及应用》", 31 August 2004 *

Cited By (23)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2020073929A1 (en) * 2018-10-09 2020-04-16 中国铁建重工集团股份有限公司 Operation trolley, and robotic arm optical target positioning device and positioning method thereof
CN109514602A (en) * 2018-11-08 2019-03-26 东南大学 A kind of industrial robot torque compensation control method based on loaded self-adaptive identification
CN109514602B (en) * 2018-11-08 2020-06-16 东南大学 Industrial robot moment compensation control method based on load self-adaptive identification
CN111251290A (en) * 2018-11-30 2020-06-09 汉翔航空工业股份有限公司 System and method for compensating selectable path of mechanical arm
CN110561500B (en) * 2019-09-30 2021-04-13 宜宾职业技术学院 Space positioning error measuring device and measuring method thereof
CN110561500A (en) * 2019-09-30 2019-12-13 宜宾职业技术学院 space positioning error measuring device and measuring method thereof
CN110802585A (en) * 2019-11-20 2020-02-18 山东大学 Mechanical arm tail end sensor compensation method and contact force/moment measurement method
CN110802585B (en) * 2019-11-20 2021-04-16 山东大学 Mechanical arm tail end sensor compensation method and contact force/moment measurement method
CN110977978B (en) * 2019-12-13 2021-09-21 易思维（杭州）科技有限公司 Demonstration method for settlement compensation in vehicle door assembly
CN110977978A (en) * 2019-12-13 2020-04-10 易思维（杭州）科技有限公司 Demonstration method for settlement compensation in vehicle door assembly
CN111775153A (en) * 2020-07-17 2020-10-16 中国科学院宁波材料技术与工程研究所 Heavy-load robot calibration method
CN111775153B (en) * 2020-07-17 2022-08-26 中国科学院宁波材料技术与工程研究所 Heavy-load robot calibration method
CN114833866B (en) * 2021-02-01 2023-12-12 精工爱普生株式会社 Angle transfer error correction method of speed reducer and robot system
CN114833866A (en) * 2021-02-01 2022-08-02 精工爱普生株式会社 Angle transmission error correction method of speed reducer and robot system
WO2023024277A1 (en) * 2021-08-25 2023-03-02 深圳市优必选科技股份有限公司 Method and apparatus for controlling dual-arm robot, and dual-arm robot and readable storage medium
CN113927594A (en) * 2021-09-26 2022-01-14 珠海格力智能装备有限公司 Method and device for compensating joint rotation angle of robot
CN114098960A (en) * 2021-11-29 2022-03-01 杭州柳叶刀机器人有限公司 Automatic positioning method and device for medical instrument, mechanical arm and readable storage medium
CN114098960B (en) * 2021-11-29 2024-05-07 杭州柳叶刀机器人有限公司 Medical instrument automatic positioning device, mechanical arm and readable storage medium
CN114098986B (en) * 2021-11-30 2023-11-21 杭州柳叶刀机器人有限公司 Gravity error evaluation method and device, plane compensation method and robot
CN114098986A (en) * 2021-11-30 2022-03-01 杭州柳叶刀机器人有限公司 Gravity error evaluation method and device, plane compensation method and robot
CN116587283A (en) * 2023-06-16 2023-08-15 南方电网电力科技股份有限公司 Mechanical arm control zero calibration method and related device
CN116945195A (en) * 2023-09-19 2023-10-27 成都飞机工业（集团）有限责任公司 Omnidirectional measurement device system arrangement, registration method, electronic device and storage medium
CN116945195B (en) * 2023-09-19 2024-01-12 成都飞机工业（集团）有限责任公司 Omnidirectional measurement device system arrangement, registration method, electronic device and storage medium

Also Published As

Publication number Publication date
CN108608462B (en) 2021-04-27

Similar Documents

Publication Publication Date Title
CN108608462A (en) A kind of quadruple precision compensation method for large-scale heavy duty mechanical arm
Ma et al. Modeling and calibration of high-order joint-dependent kinematic errors for industrial robots
CN110193829B (en) Robot precision control method for coupling kinematics and rigidity parameter identification
CN109822574B (en) Industrial robot end six-dimensional force sensor calibration method
Conrad et al. Robotic calibration issues: Accuracy, repeatability and calibration
CN105583824B (en) Force control traction and swinging multi-degree-of-freedom mechanical arm control device and method
Wu A kinematic CAD tool for the design and control of a robot manipulator
WO2020134426A1 (en) Plane precision calibration method for industrial robot
Hayati Robot arm geometric link parameter estimation
Greenway Robot accuracy
Judd et al. A technique to calibrate industrial robots with experimental verification
CN108406771A (en) A kind of plane restriction error model and robot self-calibrating method
CN110815206A (en) Stewart type parallel robot kinematics calibration method
Saund et al. High accuracy articulated robots with CNC control systems
CN104833324A (en) Robot calibration method based on measuring head
CN112720483A (en) Method and device for acquiring combined mass center state, humanoid robot and readable storage medium
Huang et al. Identification of structure errors of 3-PRS-XY mechanism with regularization method
CN113427481A (en) Direct attitude feedback control method and direct attitude feedback control machine
CN117340897B (en) Dynamic response prediction-oriented robot digital twin model modeling method and system
Yang et al. Self-calibration of three-legged modular reconfigurable parallel robots based on leg-end distance errors
Li et al. A laser-guided solution to manipulate mobile robot arm terminals within a large workspace
CN106096099A (en) A kind of engineering goods error transfer's analysis method and device thereof
Schmitt et al. Cognition-enhanced, self-optimizing assembly systems
Mayer et al. Global kinematic calibration of a Stewart platform
Mei et al. Calibration of a 6-DOF industrial robot considering the actual mechanical structures and CNC system

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
CB02 Change of applicant information
CB02 Change of applicant information

Address after: 215123 A326, No. 150 Ren'ai Road, Dushu Lake Higher Education Area, Suzhou Industrial Park, Jiangsu Province

Applicant after: Suzhou Shang Xian medical robot technology Limited by Share Ltd

Address before: A326, No. 150 Ren'ai Road Second Teaching Building, Dushu Lake Higher Education Area, Suzhou Industrial Park, Shenyang City, Liaoning Province

Applicant before: Suzhou Shang Xian medical robot technology Limited by Share Ltd

GR01 Patent grant
GR01 Patent grant
TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20220620

Address after: 117000 China Pharmaceutical capital innovation park, Mulan Road, high tech Industrial Development Zone, Benxi City, Liaoning Province

Patentee after: SHENYANG SHANGXIAN MEDICAL SYSTEM Co.,Ltd.

Address before: 215123 A326, No. 150 Ren'ai Road, Dushu Lake Higher Education Area, Suzhou Industrial Park, Jiangsu Province

Patentee before: SUZHOU SHANGXIAN MEDICAL ROBOT TECHNOLOGY CO.,LTD.