CN102699733B - Method and device for measuring movement locus of automatic tool changing mechanical arm - Google Patents

Abstract

The invention relates to a method and a device for measuring the movement locus of an automatic knife-changing mechanical arm and belongs to the field of space movement locus tracking and measurement of an automatic knife-changing mechanical arm at a processing center. The method is a non-contact type three-dimensional online measurement method based on two-dimensional PSDs (Phase Sensory Detectors); light sources as measured targets are installed on a measured ATC (Air Traffic Control) mechanical arm and are projected onto two-dimensional induction screens of two PSD cameras composed of two position sensitive detectors respectively through a lens to form light spots; the two PSD cameras respectively output two-dimensional coordinate values under the excitation of the light spots; after passing through a signal processor, a voltage value corresponding to a coordinate position is outputted; and a computer collects and saves a corresponding signal through a voltage acquisition card. The method is quick in response speed and satisfies the real-time online measurement requirements. Through the movement locus of the mechanical arm, the positioning accuracy, the movement stability and other movement states in the knife changing process can be judged to provide a quantitative method for the reliability detection of a knife library.

Description

Measuring method and the device of automatic tool changer machinery arm motion track

Technical field

The invention belongs to the space motion path tracking measurement field of machining center automatic tool changer mechanical arm, is the three-dimensional measuring apparatus that utilizes two-dimensional position-sensitive detector to form, and realizes the non-contact type on-line measurement of mechanical arm movement locus.

Background technology

In recent years, along with the fast development of equipment manufacture, the demand of the high-grade, digitally controlled machine tools take machining center as representative grows with each passing day, be subject to the great attention of domestic and international government, enterprise as the research and development of the automatic tool changer (Automatic Tool Changer is called for short ATC) of the key feature-large-scale tool magazine of machining center.ATC, as the important component part of Tool Magazine in Machining Centers, is the middle bridge of tool magazine and machine tool chief axis, and major function is to use cutter automated exchanged cutter to main shaft processing, and the cutter unloading not re-using on main shaft is put back in tool magazine.The application of ATC has improved the working (machining) efficiency of lathe greatly, make clamped one time complete multiple working procedure or all operation become possibility, become the requisite functional part of Modern Machining Center, invention has formed the polytype finished product as leading take rotary type tower and tool magazine formula so far certainly.Large-scale tool magazine formula automatic tool changer storage cutter ability is strong, is applicable to complicated multistage manufacturing processes occasion, becomes current manufacturing study hotspot.Through innovation, research and development and manufacture in a few years, domestic and international well-known lathe factory commercial city has formed the large-scale tool magazine automatic tool changer of advanced person of certain scale, that have independent intellectual property right, but the reliability for automatic tool changer detects, to company standard, be all some general requirements from national standard, machinery industry, there is no the detection method of standard and the equipment that can quantize.ATC has and starts the features such as frequent, operation acts is many, transmission mechanism is complicated, kinematic accuracy requirement is high and measurement space is narrow, causes the tracking measurement of its working motion track very difficult.For now widely used globoid cam formula ATC, the measurement result of movement locus can effectively reflect foozle and the influence degree of rigging error to ATC positioning precision of cam, and the reliability of tool magazine automatic tool changer is detected and is significant.At present, the research of the tracking measurement to mechanism kinematic track is both at home and abroad also few, mainly concentrate on the measurement aspect of the plane motion track of simple mechanism, as Qin Wei " based on the Study on Motion of Planar Mechanism of CCD technology ", measurement target is in two-dimensional space, certainty of measurement requires lower, and the movement locus of ATC mechanical arm is the complex curve in three dimensions.

Summary of the invention

The technical barrier that the present invention will solve is the shortcoming that overcomes prior art, due to machining center ATC mechanism complexity, driving cam machining accuracy is difficult to detect, assembly precision is difficult to evaluate on the impact of terminal kinematic error, integrally-built closure has increased the detection difficulty of motion state, therefore form effective reliability checking method difficulty.Binding site sensing detector of the present invention (Position Sensory Device is called for short PSD) and binocular vision 3 D measurement principle have been invented a kind of non-contact 3-D measuring method and device based on two Two-dimensional PSD cameras.Study the malfunction of automatic tool changer according to the actual motion track of measured mechanical arm, and form a kind of effectively tool magazine cutter-exchange mechanism reliability checking method.The present invention adopts PSD to extract the mode of aiming pip coordinate, and fast response time, meets real-time online measuring requirement; Can judge the motion state such as positioning precision, robust motion in exchanging knives process by the movement locus of mechanical arm, provide quantization method for tool magazine reliability detects.

The technical scheme that the present invention will solve is a kind of measuring method of automatic tool changer machinery arm motion track, and the method is a kind of non-contact 3-D On-line Measuring Method based on Two-dimensional PSD.Be arranged on measured ATC mechanical arm as the light source of measured target, measuring principle as shown in Figure 1, light source scioptics are incident upon the upper hot spot that forms of two dimension induction screen of the PSD camera being made up of two Position-Sensitive Detectors, two PSD cameras are exported separately two-dimensional coordinate value under the exciting of hot spot, the output magnitude of voltage corresponding with coordinate position after signal processor, computer is preserved corresponding signal collection by voltage acquisition card, is gone out the D coordinates value of measured light by two groups of two-dimensional coordinate value inverses that collect.Finally, calculate the movement locus of series of discrete point formation mechanical arm according to the data of high frequency collection.

1) before three-dimensional measurement, will obtain the high-precision inside and outside parameter of PSD camera, measuring method is as follows:

If the coordinate at measured target light source place is world coordinate system W, projected spot place coordinate is PSD coordinate system S, and camera lens coordinate system L, image coordinate system I, PSD coordinate system S be with reference to shown in Fig. 2, establishes and under world coordinate system W, puts P (X _w, Y _w, Z _w), the coordinate of its corresponding incident point P ' under PSD coordinate system S is (u, v), the transformational relation between the two is:

$Z_L\left[\begin{array}{c}\mathrm{\μ}\\ v\\ 1\end{array}\right]=\left[\begin{array}{cccc}{f}_x& 0& {\mathrm{\μ}}_0& 0\\ 0& f_y& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\·\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]\·\left[\begin{array}{c}{X}_W\\ Y_W\\ Z_W\\ 1\end{array}\right]=\left[\begin{array}{cc}A& 0\end{array}\right]\·\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]\·\left[\begin{array}{c}{X}_W\\ Y_W\\ Z_W\\ 1\end{array}\right]---\left(1\right)$

Wherein, A is Intrinsic Matrix, and it has comprised focal distance f and side-play amount (u ₀, v ₀), R, T are respectively world coordinates and are tied to spin matrix and the translation matrix of camera lens coordinate system, and [R T] is the outer parameter matrix of PSD camera, Z _lfor the Z axis coordinate figure of measurement target under camera lens coordinate system;

Measure the inside and outside parameter of PSD camera with the plane template device shown in Fig. 3, establish the initial point of world coordinate system W at the center of template upper left corner light source, horizontal direction is X-axis, and vertical direction is Y-axis, is Z axis, Z inwards perpendicular to plane _w=0:

$Z_L\left[\begin{array}{c}\mathrm{\μ}\\ v\\ 1\end{array}\right]=\left[\begin{array}{cccc}{f}_x& 0& {\mathrm{\μ}}_0& 0\\ 0& f_y& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\·\left[\begin{array}{ccc}{R}_1& R_2& T\\ 0& 0& 1\end{array}\right]\·\left[\begin{array}{c}{X}_W\\ Y_W\\ 1\end{array}\right]=H\·\left[\begin{array}{c}{X}_W\\ Y_W\\ 1\end{array}\right]---\left(2\right)$

Solve inside and outside parameter according to Zhang Shi standardization, after fixing PSD camera and template, start control circuit 38, from array of source 36 upper left corners, scan-type is put bright light source successively, gather the output coordinate value of corresponding PSD simultaneously, to the last a light source measurement is complete, the following equation of coordinate figure substitution under world coordinate system by the data that collect and corresponding light source:

$\left[\begin{array}{ccccccccc}{X}_W& Y_W& 1& 0& 0& 0& -\mathrm{\&mu;}\&CenterDot;X_W& -\mathrm{\&mu;}\&CenterDot;Y_W& -\mathrm{\&mu;}\\ 0& 0& 0& X_W& Y_W& 1& -v\&CenterDot;X_W& -v\&CenterDot;Y_W& -v\end{array}\right]\&CenterDot;\left[\begin{array}{c}{h}_{11}\\ h_{12}\\ h_{13}\\ h_{21}\\ h_{22}\\ h_{21}\\ h_{31}\\ h_{32}\\ {\mathrm{<figure-callout\; id="33"\; label="h"\; filenames="HDA00001754555000041.png"\; state="\{\{state\}\}">h</figure-callout>}}_{33}\end{array}\right]=\left[\begin{array}{c}0\\ 0\end{array}\right]---\left(3\right)$

When quantity of light source in template is abundant, solve homography matrix H by least square method, change the orientation of camera or template, just can obtain different homography matrixes, will organize the following equation of H substitution more:

$\left[\begin{array}{c}{V_{12}}^T\\ {(V_{11}-V_{22})}^T\end{array}\right]\&CenterDot;b=0---\left(4\right)$

Wherein

$V_{\mathrm{ij}}={\left[\begin{array}{cccccc}{h}_{i1}\&CenterDot;h_{j1}& h_{i1}\&CenterDot;{\mathrm{<figure-callout\; id="2"\; label="h\; j"\; filenames="HDA00001754555000022.png"\; state="\{\{state\}\}">h</figure-callout>}}_j+h_{i2}\&CenterDot;h_{j1}& h_{i1}\&CenterDot;{\mathrm{<figure-callout\; id="3"\; label="h\; j"\; filenames="HDA00001754555000022.png,HDA00001754555000031.png"\; state="\{\{state\}\}">h</figure-callout>}}_j+h_{i3}\&CenterDot;h_{j1}& h_{i2}\&CenterDot;h_{j2}& h_{i2}\&CenterDot;{\mathrm{<figure-callout\; id="3"\; label="h\; j"\; filenames="HDA00001754555000022.png,HDA00001754555000031.png"\; state="\{\{state\}\}">h</figure-callout>}}_j+h_{i3}\&CenterDot;h_{j2}& h_{i3}\&CenterDot;h_{j3}\end{array}\right]}^T,$ B is the vector that the independent element of six of symmetrical matrix B forms.

$B={\left(A^{-1}\right)}^T{A}^{-1}=\left[\begin{array}{ccc}\frac{1}{f_x^2}& 0& -\frac{{\mathrm{\&mu;}}_0}{f_x^2}\\ 0& \frac{1}{f_y^2}& -\frac{v_0}{f_y^2}\\ -\frac{{\mathrm{\&mu;}}_0}{f_x^2}& -\frac{v_0}{f_y^2}& \frac{{\mathrm{\&mu;}}_0^2}{f_y^2}+\frac{v_0^2}{f_y^2}+1\end{array}\right]$

When the orientation number of times changing is when abundant, obtain b by least square method, and inverse goes out camera Intrinsic Matrix A, by A substitution following formula:

R ₁=A ^-1H ₁，R ₂=A ^-1H ₂，T=A ^-1H ₃ (5)

Then pass through R ₁and R ₂go out R by quadrature spread ₃, obtain Camera extrinsic matrix number [R T].

2) 3-D positioning method of two PSD cameras is as follows:

It is upper that the light beam that Scattering Targets light source sends is projected in PSD camera induction screen after camera lens converges, target light source centre coordinate (X under world coordinate system _w, Y _w, Z _w) with PSD coordinate system under projection hot spot centre coordinate (μ, ν) relation as shown in the formula:

$\left[\begin{array}{ccc}{m}_1-{\mathrm{<figure-callout\; id="9"\; label="m"\; filenames="HDA00001754555000022.png"\; state="\{\{state\}\}">m</figure-callout>}}_9\mathrm{\&mu;}& m_2-{\mathrm{<figure-callout\; id="10"\; label="m"\; filenames="HDA00001754555000022.png"\; state="\{\{state\}\}">m</figure-callout>}}_{10}\mathrm{\&mu;}& m_3-{\mathrm{<figure-callout\; id="11"\; label="m"\; filenames="HDA00001754555000022.png"\; state="\{\{state\}\}">m</figure-callout>}}_{11}\mathrm{\&mu;}\\ m_5-m_{9}v& m_6-m_{10}v& m_7-m_{11}v\end{array}\right]\&CenterDot;\left[\begin{array}{c}{X}_W\\ Y_W\\ Z_W\end{array}\right]=\left[\begin{array}{c}{\mathrm{<figure-callout\; id="12"\; label="m"\; filenames="HDA00001754555000022.png"\; state="\{\{state\}\}">m</figure-callout>}}_{12}\mathrm{\&mu;}-m_4\\ m_{12}v-m_8\end{array}\right]---\left(6\right)$

Wherein,

$M=\left[\begin{array}{cccc}{m}_1& m_2& m_3& m_4\\ m_5& m_6& m_7& m_8\\ m_9& m_{10}& m_{11}& {\mathrm{<figure-callout\; id="12"\; label="m"\; filenames="HDA00001754555000022.png"\; state="\{\{state\}\}">m</figure-callout>}}_{12}\end{array}\right]=\left[\begin{array}{cc}A& 0\end{array}\right]\&CenterDot;\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]$

Utilize the two-dimensional coordinate of single PSD camera output can list as two of above formula equations, two PSD cameras are responded to a target light source simultaneously, can list four equations and solve the coordinate figure of target light source center under world coordinate system by least square method;

3) measuring method of mechanical arm movement locus is as follows:

Target light source is arranged on the mechanical arm of machining center ATC, it forms the movement locus of mechanical arm in space with tool changing action, and movement locus is measured and adopted the D coordinates value of target light source to calculate and discrete point curve-fitting method.The sensitive surface of PSD camera is a two dimensional surface, the two-dimensional coordinate value of projection hot spot and the D coordinates value of aiming pip represent by the inside and outside parameter matrix of PSD camera, the coordinate figure of two Two-dimensional PSD camera outputs can instead solve the three-dimensional coordinate of impact point, and series of discrete impact point simulates the movement locus of mechanical arm in process.

4) invention measurement mechanism be take two PSD cameras as core has multivariant measurement mechanism.Measurement mechanism itself has the adjustment of vertical displacement, horizontal displacement, the anglec of rotation and four frees degree of luffing angle, the structure of device as shown in Figure 6:

On the riser of base 23, be processed with bolt hole, the automatically controlled platform 28 of Z axis with power-off protection is fixed by bolts on riser.Motor 17 drives carrying workbench 24 to move at vertical direction by ball-screw 18, on slip carrying workbench 24, two slide rails 21 that difference in height is adjustable are installed by one group of bolt 22, the horizontal dovetail guide at the top of slide rail coordinates with little supporting plate 20, and position is adjustable in the horizontal direction for the PSD camera on little supporting plate 20.Assembled successively and manually adjust rotation platform 25 and angular displacement platform 26 on little supporting plate 20 by series system, left PSD camera 27 and right PSD camera 19 are arranged on the workbench of angular displacement platform.

5) assembly structure of the core PSD camera of measurement mechanism is as shown in Figure 7:

A measuring method for automatic tool changer machinery arm motion track, the device that the method adopts is that PSD camera overall structure is made up of focusing block, detector, fine setting part and connecting frame four parts, what focusing block used is industrial lens 29, aperture in adjustable lens changes size and the luminosity of the upper projection hot spot of PSD induction screen, industrial lens 29 is connected by standard C mouth with L-type connecting plate 30, Position-Sensitive Detector 31 adopts 10 pin communication cables to be connected with signal processor, Position-Sensitive Detector 31 is arranged on the workbench of one dimension manual translation platform 35, by adjust one dimension manual translation platform 35 can adjusting position sensing detector 31 and industrial lens 29 between distance, L-type connecting plate 30 and positioning transfer plate 34 have formed connecting frame part, positioning transfer plate 34 is located and be clamped in to Position-Sensitive Detector 31 by the screw thread of alignment pin 32 and Position-Sensitive Detector 31 bottom surfaces, positioning transfer plate 34 is fixed on the workbench of one dimension manual translation platform 35 by soket head cap screw 33.

The invention has the beneficial effects as follows as machining center ATC reliability detects a kind of quantization method and device are provided.Based on ATC transmission mechanism, feature that closure is strong, the online movement locus measuring method of a kind of optical non-contact and device are invented, ATC mechanical arm in process is carried out to movement locus real-time tracking record, and its processing action is decomposed, detect motion positioning precision, movement accuracy and job stability, from but ATC reliability detect form quantitative criteria.

Accompanying drawing explanation

Fig. 1 is two PSD camera trajectory measurement principle schematic.

Fig. 2 is measuring system Coordinate system definition figure.

Fig. 3 is single PSD camera inside and outside parameter instrumentation plan.Wherein, 36-array of source, 37-plane reference template, 38-control circuit.

Fig. 4 is tool magazine automatic tool changer structure chart.Wherein: 1-fuselage, 2-mechanical arm, 3-axle sleeve, 4-cam top shell, 5-globoid cam, 6-end face slide block, 7-side face switch block, 8-transmitter shaft, 9-fork, 10-transmission lever, 11-master bevel gear, 12-bevel gear, 13-gyroaxis.

Fig. 5 is ATC mechanical arm movement locus instrumentation plan.Wherein: 14-1 LED light source, 15-2 LED light source, the two PSD measurement mechanisms of 16-.

Fig. 6 is two PSD measurement mechanism detailed construction.Wherein: 17-motor, 18-leading screw, the right PSD camera of 19-, the little supporting plate of 20-, 21-slide rail, 22-hexagon-headed bolt, 23-base, 24-plummer, 25-rotation platform, 26-angular displacement platform, the left PSD camera of 27-, 28-Z axial displacement platform.

Fig. 7 is PSD camera structure figure.Wherein: 29-industrial lens, 30-L type type connecting plate, 31-Position-Sensitive Detector, 32-alignment pin, 33-hexagon socket head cap screw, 35-one dimension manual translation platform.

Fig. 8 is three-dimensional measurement flow chart.

The specific embodiment

The present invention is the on-line measurement for ATC course of work movement locus, two LED light sources are installed on ATC mechanical arm as tracking target, utilize two two-dimensional position-sensitive detectors to detect projection hot spot coordinate, then calculate the D coordinates value of impact point.This measurement mechanism is measured the impact point on globoid cam formula ATC mechanical arm, obtain the actual motion state of automatic tool changer, and contrast with ideal movements situation, thereby the anomaly such as " falling cutter ", " shake ", " moving not in place " is made prediction, assessed, formed a kind of ATC reliability detection means.

Describe specific embodiment of the invention in detail below in conjunction with accompanying drawing and technical scheme:

(1) Two-dimensional PSD camera overall structure

The effect of Two-dimensional PSD camera is that measurement target light source projects is formed to hot spot to PSD induction screen, and output facula centre coordinate, and its principle is similar to digital camera, therefore name with this.The structure of the Two-dimensional PSD camera of the present invention's design as shown in Figure 7, on the wall of L-type connecting plate 30, process a logical screwed hole, industrial lens connecter type as required of dimensions and determining, is generally C type, F type or CS type, adopts that focal length 25mm's focus C mouth industrial lens 29 in the design.The C10443-03 type Position-Sensitive Detector 31 of Bin Song company of Japan is placed on the rear of L-type connecting plate 30, guarantees that induction screen is positioned near rear lens focus point.Position-Sensitive Detector is positioned on keyset 34 by alignment pin 32 and bottom surface, then clamps by the M4 screw of bottom.Keyset is fixed on the workbench of one dimension manual translation platform 35 with soket head cap screw 33, the base of translation stage is fixed on L-type connecting plate 30, by regulating the adjusting lever of translation stage can adjust the displacement of sensing detector in camera lens optical axis direction, guarantee to be incident upon hot spot on PSD induction screen limpid in sight in measured zone.Camera adjustment is to carry out before measuring, and remains unchanged once inside and outside parameter measurement or three-dimensional measurement start each structure of camera.

(2) Two-dimensional PSD camera inside and outside parameter is measured

The operation principle of the method for three-dimensional measurement of Two-dimensional PSD camera is pinhole imaging system, and only along straightline propagation, that the light that target light source is sent is irradiated to PSD induction screen after lens is upper, and induction point and light source center on induction screen are in alignment.PSD induction screen is a two dimensional surface, and coordinate origin is in its geometric center.As shown in Figure 2, the coordinate of establishing measurement target place is world coordinate system W, and camera lens coordinate system L, image coordinate system I, PSD coordinate system S are with reference to shown in Fig. 2.If it is P (X that P point in space obtains coordinate under world coordinate system W _w, Y _w, Z _w), it is P ' (u, v) that its corresponding subpoint P ' obtains coordinate under PSD coordinate system S, the transformational relation between the two is:

$Z_L\left[\begin{array}{c}\mathrm{\&mu;}\\ v\\ 1\end{array}\right]=\left[\begin{array}{cccc}{f}_x& 0& {\mathrm{\&mu;}}_0& 0\\ 0& f_y& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\&CenterDot;\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]\&CenterDot;\left[\begin{array}{c}{X}_W\\ Y_W\\ Z_W\\ 1\end{array}\right]=\left[\begin{array}{cc}A& 0\end{array}\right]\&CenterDot;\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]\&CenterDot;\left[\begin{array}{c}{X}_W\\ Y_W\\ Z_W\\ 1\end{array}\right]---\left(1\right)$

The witness mark Zhang Shi plane reference method of inside and outside parameter, establishes the initial point of world coordinate system W at the center of template upper left corner light source, and horizontal direction is X-axis, and vertical direction is Y-axis, is Z axis, Z inwards perpendicular to plane _w=0:

$Z_L\left[\begin{array}{c}\mathrm{\&mu;}\\ v\\ 1\end{array}\right]=\left[\begin{array}{cccc}{f}_x& 0& {\mathrm{\&mu;}}_0& 0\\ 0& f_y& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\&CenterDot;\left[\begin{array}{ccc}{R}_1& R_2& T\\ 0& 0& 1\end{array}\right]\&CenterDot;\left[\begin{array}{c}{X}_W\\ Y_W\\ 1\end{array}\right]=H\&CenterDot;\left[\begin{array}{c}{X}_W\\ Y_W\\ 1\end{array}\right]---\left(2\right)$

In above formula, R ₁, R ₂be respectively the first and second row of spin matrix R.At timing signal, extraterrestrial target point is the light source center point on plane template, and the subpoint coordinate in PSD coordinate system goes PSD output valve.The solution procedure of inside and outside parameter adopts Zhang Shi standardization, and concrete steps are as follows:

1. the measurement of camera intrinsic parameter as shown in Figure 3, is made a sheet of planar template 37, is evenly arranged scattering point array of source 36 in the above, and its horizontal and vertical interval all equates, and precision is higher, as 30mm.It is many that the quantity of spot light is wanted as far as possible, as more than 9 × 9, makes the quantity of horizontal direction and vertical direction equate as far as possible, to guarantee that the certainty of measurement on both direction equates.Placing PSD camera apart from template a distance, make the camera visual field cover all light sources.

2. fix behind the position of camera and template, start array of source control circuit scan-type from the upper left corner and put successively bright light source, record the output coordinate value of the corresponding PSD of each spot light simultaneously, in this step is carried out, will guarantee PSD camera and template unchanged direction;

3. after in template, last light source point is measured, in the following equation of coordinate substitution that light source point is shielded in PSD induction at coordinate and the homolographic projection point of world coordinate system:

$\left[\begin{array}{ccccccccc}{X}_W& Y_W& 1& 0& 0& 0& -\mathrm{\&mu;}\&CenterDot;X_W& -\mathrm{\&mu;}\&CenterDot;Y_W& -\mathrm{\&mu;}\\ 0& 0& 0& X_W& Y_W& 1& -v\&CenterDot;X_W& -v\&CenterDot;Y_W& -v\end{array}\right]\&CenterDot;\left[\begin{array}{c}{h}_{11}\\ h_{12}\\ h_{13}\\ h_{21}\\ h_{22}\\ h_{21}\\ h_{31}\\ h_{32}\\ {\mathrm{<figure-callout\; id="33"\; label="h"\; filenames="HDA00001754555000041.png"\; state="\{\{state\}\}">h</figure-callout>}}_{33}\end{array}\right]=\left[\begin{array}{c}0\\ 0\end{array}\right]---\left(3\right)$

Two equations of the corresponding above formula of each light source point, 5 above measurement points just can solve homography matrix H by least square method.

4. change the orientation of camera or template, change amplitude is large as far as possible, comprehensive, repeats step 2., at least changes orientation 4 times, because spin matrix is orthonormal matrix, can obtain following equation according to its character:

$\left[\begin{array}{c}{V_{12}}^T\\ {(V_{11}-V_{22})}^T\end{array}\right]\&CenterDot;b=0---\left(4\right)$

Wherein

$V_{\mathrm{ij}}={\left[\begin{array}{cccccc}{h}_{i1}\&CenterDot;h_{j1}& h_{i1}\&CenterDot;{\mathrm{<figure-callout\; id="2"\; label="h\; j"\; filenames="HDA00001754555000022.png"\; state="\{\{state\}\}">h</figure-callout>}}_j+{\mathrm{<figure-callout\; id="2"\; label="h\; i"\; filenames="HDA00001754555000022.png"\; state="\{\{state\}\}">h</figure-callout>}}_i\&CenterDot;h_{j1}& h_{i1}\&CenterDot;{\mathrm{<figure-callout\; id="3"\; label="h\; j"\; filenames="HDA00001754555000022.png,HDA00001754555000031.png"\; state="\{\{state\}\}">h</figure-callout>}}_j+{\mathrm{<figure-callout\; id="3"\; label="h\; i"\; filenames="HDA00001754555000022.png,HDA00001754555000031.png"\; state="\{\{state\}\}">h</figure-callout>}}_i\&CenterDot;h_{j1}& {\mathrm{<figure-callout\; id="2"\; label="h\; i"\; filenames="HDA00001754555000022.png"\; state="\{\{state\}\}">h</figure-callout>}}_i\&CenterDot;h_{j2}& {\mathrm{<figure-callout\; id="2"\; label="h\; i"\; filenames="HDA00001754555000022.png"\; state="\{\{state\}\}">h</figure-callout>}}_i\&CenterDot;{\mathrm{<figure-callout\; id="3"\; label="h\; j"\; filenames="HDA00001754555000022.png,HDA00001754555000031.png"\; state="\{\{state\}\}">h</figure-callout>}}_j+{\mathrm{<figure-callout\; id="3"\; label="h\; i"\; filenames="HDA00001754555000022.png,HDA00001754555000031.png"\; state="\{\{state\}\}">h</figure-callout>}}_i\&CenterDot;h_{j2}& {\mathrm{<figure-callout\; id="3"\; label="h\; i"\; filenames="HDA00001754555000022.png,HDA00001754555000031.png"\; state="\{\{state\}\}">h</figure-callout>}}_i\&CenterDot;h_{j3}\end{array}\right]}^T,$ In b, contain 6 elements, respectively six units of corresponding symmetrical matrix B.

$B={\left(A^{-1}\right)}^T{A}^{-1}=\left[\begin{array}{ccc}\frac{1}{f_x^2}& 0& -\frac{{\mathrm{\&mu;}}_0}{f_x^2}\\ 0& \frac{1}{f_y^2}& -\frac{v_0}{f_y^2}\\ -\frac{{\mathrm{\&mu;}}_0}{f_x^2}& -\frac{v_0}{{\mathrm{<figure-callout\; id="2"\; label="f\; y"\; filenames="HDA00001754555000022.png"\; state="\{\{state\}\}">f</figure-callout>}}_y^{}}& \frac{{\mathrm{\&mu;}}_0^2}{{\mathrm{<figure-callout\; id="2"\; label="f\; y"\; filenames="HDA00001754555000022.png"\; state="\{\{state\}\}">f</figure-callout>}}_y^{}}+\frac{v_0^2}{{\mathrm{<figure-callout\; id="2"\; label="f\; y"\; filenames="HDA00001754555000022.png"\; state="\{\{state\}\}">f</figure-callout>}}_y^{}}+1\end{array}\right]$

Each orientation of camera can obtain two equations, changes 4 and just can obtain b by least square method with upper position, has obtained camera Intrinsic Matrix A.

5. solve first two columns and the translation matrix of spin matrix according to following relation:

R ₁=A ^-1H ₁，R ₂=A ^-1H ₂，T=A ^-1H ₃ (5)

Then pass through R ₁and R ₂go out R by quadrature spread ₃, obtain Camera extrinsic matrix number [R T].

(3) target light source three-dimensional coordinate measurement method

World coordinate system W and PSD coordinate S can be simplified shown as:

$\left[\begin{array}{ccc}{m}_1-{\mathrm{<figure-callout\; id="9"\; label="m"\; filenames="HDA00001754555000022.png"\; state="\{\{state\}\}">m</figure-callout>}}_9\mathrm{\&mu;}& m_2-{\mathrm{<figure-callout\; id="10"\; label="m"\; filenames="HDA00001754555000022.png"\; state="\{\{state\}\}">m</figure-callout>}}_{10}\mathrm{\&mu;}& m_3-{\mathrm{<figure-callout\; id="11"\; label="m"\; filenames="HDA00001754555000022.png"\; state="\{\{state\}\}">m</figure-callout>}}_{11}\mathrm{\&mu;}\\ m_5-m_{9}v& m_6-m_{10}v& m_7-m_{11}v\end{array}\right]\&CenterDot;\left[\begin{array}{c}{X}_W\\ Y_W\\ Z_W\end{array}\right]=\left[\begin{array}{c}{\mathrm{<figure-callout\; id="12"\; label="m"\; filenames="HDA00001754555000022.png"\; state="\{\{state\}\}">m</figure-callout>}}_{12}\mathrm{\&mu;}-m_4\\ m_{12}v-m_8\end{array}\right]---\left(6\right)$

In above formula, M is called transformed matrix, and k is proportionality coefficient, and itself and camera claim direct ratio to the distance between scaling board.For the ease of solving, k cancellation is obtained:

$\left[\begin{array}{ccc}{m}_1-{\mathrm{<figure-callout\; id="9"\; label="m"\; filenames="HDA00001754555000022.png"\; state="\{\{state\}\}">m</figure-callout>}}_9\mathrm{\&mu;}& m_2-{\mathrm{<figure-callout\; id="10"\; label="m"\; filenames="HDA00001754555000022.png"\; state="\{\{state\}\}">m</figure-callout>}}_{10}\mathrm{\&mu;}& m_3-{\mathrm{<figure-callout\; id="11"\; label="m"\; filenames="HDA00001754555000022.png"\; state="\{\{state\}\}">m</figure-callout>}}_{11}\mathrm{\&mu;}\\ m_5-m_{9}v& m_6-m_{10}v& m_7-m_{11}v\end{array}\right]*\left[\begin{array}{c}{X}_W\\ Y_W\\ Z_W\end{array}\right]=\left[\begin{array}{c}{\mathrm{<figure-callout\; id="12"\; label="m"\; filenames="HDA00001754555000022.png"\; state="\{\{state\}\}">m</figure-callout>}}_{12}\mathrm{\&mu;}-m_4\\ m_{12}v-m_8\end{array}\right]---\left(7\right)$

A bit in single PSD camera projection process, can list one as above formula equation when space, three dimensions point coordinates has three unknown quantitys, and aforesaid equation only has two equations, therefore use two PSD cameras just can solve the D coordinates value of spatial point.

(4) two PSD camera measurement mechanisms

Two PSD camera measurement mechanisms are that two PSD cameras are arranged on a multivariant platform, form a set of multiple measurement environment, three-dimension measuring system that measurement category is adjustable of being applicable to.Multiple degrees of freedom platform is for adjusting the spatial pose of PSD camera, not only can realize camera vertical, move horizontally, but also can realize by two rotation platforms the adjustment of horizontal view angle and the angle of pitch, widen greatly the visual field of camera, be applicable to being applied to the three-dimensional measurement under complex working condition more.

As shown in Figure 6, this design can make the free degree adjustment of 4 of PSD camera implementation spaces to the overall structure figure of two PSD camera measurement mechanisms.Thereby base 23 for support adjust platform, with the effect of measured equipment connection fixation measuring device.The automatically controlled motion platform 28 of Z axis with power-off protection is bolted on the riser at the end, adjusts the camera vertical displacement being fixed on plummer 24 by motor 17 and leading screw 18.Due to the particularity of the operating mode of measuring, may require the position of two PSD camera in the vertical directions also incomplete same, Given this, two slide rails 21 that adjustable height is poor are installed on plummer 24, it is made up of two parts, a part is for being connected with Z axis platform 28, and another part is used for the dove-tail form guide rail that camera moves horizontally.There are two grooves coupling part, passing groove with hexagon-headed bolt 22 screws in the screw on plummer, thereby slide rail 21 is fastened on plummer, adjusts the position of slide rail 21 on loading floor by the relative position that changes groove and bolt, thereby formed the difference in height of two PSD cameras.Rail sections is made up of the dovetail groove of a level, and it matches with little supporting plate 20, makes single PSD camera can adjust in the horizontal direction position.The level angle of camera and the change of the angle of pitch realize by manual adjustment rotation platform 25 and angular displacement platform 26.The public visual field of left PSD camera 27 and right PSD camera 19 is exactly the effective measuring area of two PSD camera measurement mechanisms.

(5) tracking measurement of tool-changing mechanical arm movement locus

Automatic tool changer is the important component part of Tool Magazine in Machining Centers, as shown in Figure 4.Its function will be processed and be used cutter automated exchanged cutter to main shaft, and the cutter unloading not re-using on main shaft is put back in tool magazine.Fuselage 1 is the support frame of ATC, the main movement of mechanical arm 2 is made up of the rotation of the slip along gyroaxis 13 and wraparound rotating shaft, these two componental movements are controlled by globoid cam 5, end face slide block 6 on fork 9 slides with the forming tank of cam face, thereby slides by transmission lever 10 driving mechanical arms.The axle sleeve 3 being connected with mechanical arm coordinates with gyroaxis 13 by spline, and side face switch block 11 rotates and passes to master bevel gear 11 by transmitter shaft 8 with the forming tank of cam side face, after speed change, drives gyroaxis 13 and mechanical arm 2 to rotate by inferior bevel gear 12.For the movement locus of measurement mechanical arm accurately, as shown in Figure 5, two PSD measurement mechanisms 16 are placed in cam top shell 4, be arranged symmetrically with two high-power scattering point light sources 14 and 15 at the two ends of mechanical arm as measurement light source.Must there is a measurement light source to move below mechanical arm in each moment of measuring process, lighted and another corresponding light source is closed, the three-dimensional coordinate of the real-time measurement point bright light source of two PSD camera measurement mechanism, and simulate the movement locus of measuring target point by software.

1) concrete operation step that two PSD camera measurement mechanism installation positions are adjusted is as follows:

1. measurement mechanism be placed on suitable measuring position and be fixed, as shown in Figure 5.Measurement mechanism mounting plane is selected the large surface of smooth equipment as far as possible, so that fixing, the present invention is selected in cam shell 4 upper surfaces of the MDH-80 horizontal Machining centers of Dalian high golden number control group production.The position of adjusting measurement mechanism, makes beyond the minimum image-forming range of all adjustment measurement target in camera lens, and the scope of activities of guaranteeing again measurement target in the depth of field of camera lens simultaneously.Adjust the setting angle of measurement mechanism, in the adjusting range of the overall track that makes measurement target in measurement mechanism.After adjustment, use magnetic support by fixing device pedestal 13;

2. vertical direction is adjusted two camera positions.First adjust Z axis motion platform 28, the present invention adopts the PSA150-11-Z of the Chinese light of standing upright, the optical axis of left and right PSD camera is moved near the center of measurement target mass motion track, then unclamp the bolt of fixed rack, the difference in height of adjusting two cameras, makes the movement locus that the visual field of camera comprises whole mechanical arm lower half in vertical direction;

3. adjust little supporting plate 20 and rotation platform 25 according to the distance of measurement target range of movement and measurement mechanism, change the level orientation of two PSD cameras, make the measurement category of each camera large as far as possible, after having adjusted, by little supporting plate and rotation platform locking, the rotation platform that the present invention adopts is the SX120-73 of connection English essence machine;

4. adjust the angle of pitch of camera.The swing arm of adjusting angle bit platform 26 changes the angle of pitch of camera, guarantees that mechanical arm is in the time of horizontal level, and the public visual field of two PSD cameras can comprise No. 1 and No. 2 LED light sources simultaneously, and the angle bit platform 26 that the present invention uses is the SJ110-15 of connection English essence machine.

5. the spatial pose of inching camera among a small circle, makes the range of movement of measured target be full of the more than 95% of effective measurement visual field.

2) concrete operation step that movement locus is measured is as follows:

1. when the position of mechanical arm started to measure in time shown in Fig. 5, first light LED light source No. 1, close LED light source No. 2, No. 1 all below in gyroaxis of LED light source in the ensuing half cycles cycle, in the measurement category in measurement mechanism.Meanwhile, the coordinate figure of exporting with certain frequency collection PSD;

2. after half work period finishes, No. 1 LED light source position in No. 2 LED light sources shown in Fig. 5, now closes light source No. 1, lights light source No. 2, and repeating step 1.;

3. by the coordinate of the output of left and right PSD camera in the following equation of substitution:

$\left[\begin{array}{ccc}{m}_{1l}{m}_{9l}{\mathrm{\&mu;}}_l& m_{2l}-m_{10l}{\mathrm{\&mu;}}_l& m_{3l}-m_{11l}{\mathrm{\&mu;}}_l\\ m_{5l}-m_{9l}{v}_l& m_{6l}-m_{10l}{v}_l& m_{7l}-m_{11l}{v}_l\\ m_{1r}-m_{9r}{\mathrm{\&mu;}}_r& m_{2r}-m_{10r}{\mathrm{\&mu;}}_r& m_{3r}-m_{11r}{\mathrm{\&mu;}}_r\\ m_{5r}-m_{9r}{v}_r& m_{6r}-m_{10r}{v}_r& m_{7r}-m_{11r}{v}_r\end{array}\right]\&CenterDot;\left[\begin{array}{c}{X}_W\\ Y_W\\ Z_W\end{array}\right]=\left[\begin{array}{c}{m}_{12l}{\mathrm{\&mu;}}_l-m_{4l}\\ m_{12l}{\mathrm{v\&mu;}}_l-m_{8l}\\ m_{12r}{\mathrm{\&mu;}}_r-m_{4r}\\ m_{12r}{v}_r-m_{8r}\end{array}\right]---\left(8\right)$

Every a pair of coordinate, to using least square method solving equation to obtain the D coordinates value of an impact point, so just can be converted into a series of discrete point by the movement locus of ATC.

4. use SPL function that discrete spatial point is fitted to a curve movement;

5. decompose processing action according to measured curve movement, by with the reliability of design curve comparative evaluation tool magazine ATC.

The realization of algorithm of the present invention relies on software programming, and the thinking of its programming is as shown in the program flow diagram of Fig. 8, and first calibration for cameras intrinsic parameter, then adjusts camera position and fixing calibrating template.Demarcate again left and right Camera extrinsic number and select unified coordinate points, on the basis of the unified origin of coordinates, carry out the calculating of spatial point coordinate.Finally, matching geometric locus.

One of the present invention is used for measuring ATC mechanical arm movement locus method and apparatus, motion state that can real-time online track record mechanical arm.The method is that optical non-contact is measured, the general more complicated of structure of ATC mechanism, and structure closure is strong, is not easy to adopt contact type measurement.Adopt optical non-contact metering system can not intervene the action of ATC, adopt the extract real-time of luminous point, measurement target is LED light source, installs simple and convenient, applied widely.Traditional non-contact vision measurement is to determine spatial attitude based on extracting workpiece shape characteristic, this method extraction rate and precision are two contradictory faces, often reduced extraction rate in order to improve precision, so can only be applied to off-line measurement, the present invention adopts PSD to extract the mode of aiming pip coordinate, fast response time, meets real-time online measuring requirement; Can judge the motion state such as positioning precision, robust motion in exchanging knives process by the movement locus of mechanical arm, provide quantization method for tool magazine reliability detects.

Claims (2)

1. the measuring method of an automatic tool changer machinery arm motion track, it is characterized in that: the method is a kind of non-contact 3-D On-line Measuring Method based on Two-dimensional PSD, be arranged on measured ATC mechanical arm as the light source of measured target, light source scioptics are incident upon the upper hot spot that forms of two dimension induction screen of the PSD camera being made up of two Position-Sensitive Detectors, two PSD cameras are exported separately two-dimensional coordinate value under the exciting of hot spot, the output magnitude of voltage corresponding with coordinate position after signal processor, computer is preserved corresponding signal collection by voltage acquisition card, gone out the D coordinates value of measured light by two groups of two-dimensional coordinate value inverses that collect, finally, calculate the movement locus of series of discrete point formation mechanical arm according to the data of high frequency collection, concrete steps are as follows:

1) measurement of PSD camera inside and outside parameter

If the coordinate at measured target light source place is world coordinate system W, projected spot place coordinate is PSD coordinate system S, establishes certain some P (X under world coordinate system W _w, Y _w, Z _w), the coordinate of its corresponding incident point P ' under PSD coordinate system S is P ' (u, v), the transformational relation between the two is:

Wherein, A is Intrinsic Matrix, and it has comprised focal distance f and side-play amount (u ₀, v ₀), R, T are respectively world coordinates and are tied to spin matrix and the translation matrix of camera lens coordinate system, and [R T] is the outer parameter matrix of PSD camera;

Use plane template (37) to measure the inside and outside parameter of PSD camera, establish Z _w=0;

Solve inside and outside parameter according to Zhang Shi standardization, after fixing PSD camera and template, start control circuit (38), from array of source (36) upper left corner, scan-type is put bright light source successively, gather the output coordinate value of corresponding PSD simultaneously, to the last a light source measurement is complete, the following equation of coordinate figure substitution under world coordinate system by the data that collect and corresponding light source:

When quantity of light source in plane template (37) is abundant, solve homography matrix H by least square method, change the orientation of camera or template, just can obtain different homography matrixes, will organize the following equation of H substitution more:

Wherein

B is the vector that the independent element of six of symmetrical matrix B forms;

When the orientation number of times changing is when abundant, obtain b by least square method, and inverse goes out camera Intrinsic Matrix A, by A substitution following formula:

R ₁=A ^-1H ₁，R ₂=A ^-1H ₂，T=A ^-1H ₃ (5)

Then pass through R ₁and R ₂go out R by quadrature spread ₃, obtain Camera extrinsic matrix number [R T];

2) calculating of aiming pip three-dimensional coordinate

It is upper that the light beam that Scattering Targets light source sends is projected in PSD camera induction screen after camera lens converges, target light source centre coordinate (X under world coordinate system _w, Y _w, Z _w) with PSD coordinate system under projection hot spot centre coordinate (μ, ν) relation as shown in the formula:

Wherein,

Utilize the two-dimensional coordinate of single PSD camera output can list as two of above formula equations, two PSD cameras are responded to a target light source simultaneously, can list four equations and solve the coordinate figure of target light source center under world coordinate system by least square method;

3) measurement of mechanical arm movement locus

Target light source is arranged on the mechanical arm of machining center ATC, it forms the movement locus of mechanical arm in space with tool changing action, and movement locus is measured and adopted the D coordinates value of target light source to calculate and discrete point curve-fitting method; The sensitive surface of PSD camera is a two dimensional surface, the two-dimensional coordinate value of projection hot spot and the D coordinates value of aiming pip represent by the inside and outside parameter matrix of PSD camera, the coordinate figure of two Two-dimensional PSD camera outputs can instead solve the three-dimensional coordinate of impact point, and series of discrete impact point simulates the movement locus of mechanical arm in process.

2. the measuring method of a kind of automatic tool changer machinery arm motion track as claimed in claim 1, is characterized in that: the measurement mechanism that the method adopts be take two PSD cameras as core has multivariant measurement mechanism; In measurement mechanism, have shift adjusting machine in vertical direction, horizontal direction, also have the guiding mechanism of the anglec of rotation and pitching, the concrete structure of device is as follows:

On the riser of base (23), be processed with bolt hole, the automatically controlled platform of Z axis (28) with power-off protection is fixed by bolts on riser, the shift adjusting machine of vertical direction is to drive plummer (24) to move at vertical direction by motor (17) by ball-screw (18), at plummer (24), two slide rails (21) that difference in height is adjustable are above installed by one group of bolt (22), the horizontal dovetail guide at the top of slide rail coordinates with little supporting plate (20), PSD camera on little supporting plate (20) position is in the horizontal direction adjusted by this horizontal direction shift adjusting machine, it is upper that the angular displacement platform (26) of the rotation platform (25) of the adjustment anglec of rotation and adjustment angle of pitch angle is assembled to little supporting plate (20) successively, and left PSD camera (27) and right PSD camera (19) are arranged on respectively on the workbench of angular displacement platform (26),

PSD camera overall structure is made up of focusing block, Position-Sensitive Detector, fine setting part and connecting frame four parts, what focusing block used is industrial lens (29), aperture in adjustable lens changes size and the luminosity of the upper projection hot spot of PSD induction screen, industrial lens (29) is connected by standard C mouth with L-type connecting plate (30), Position-Sensitive Detector (31) adopts 10 pin communication cables to be connected with signal processor, Position-Sensitive Detector (31) is arranged on the workbench of one dimension manual translation platform (35), by adjust one dimension manual translation platform (35) can adjusting position sensing detector (31) and industrial lens (29) between distance, L-type connecting plate (30) and positioning transfer plate (34) have formed connecting frame part, positioning transfer plate (34) is located and be clamped in to Position-Sensitive Detector (31) by the screw thread of alignment pin (32) and Position-Sensitive Detector (31) bottom surface, positioning transfer plate (34) is fixed on the workbench of one dimension manual translation platform (35) by soket head cap screw (33).

Images