JPS58208606A - Three dimensional position measuring method - Google Patents

Abstract

PURPOSE:To set two pieces of two dimensional optical position detectors at arbitrary positions, by imparting an automatic calibrating function of coordinate conversion data. CONSTITUTION:At the time of a measuring mode, light from an LED12, which emits light based on a signal from an LED driving circuit part 13, is received by cameras 1 and 2. Two dimensional position measuring parts 5 and 6 receive the outputs of the camera and compute coordinate value. A central processing unit 7 computes three dimensional coordinate values. The result is displayed on a three dimensional coordinate display part 9. An LED lighting command signal is sent to the LED driving circuit part 13 from the unit 7, and the LED12 is lighted again. At the time of a calibration mode, LEDs at three points PA-PC on a reference coordinate device 11 are sequentially lighted. The light beams are measured by the cameras 1 and 2. Coordinate conversion data are calibrated based on the three dimensional coordinate value computed in the central processing unit 7. The values are stored in a memory element 14.

Description

DETAILED DESCRIPTION OF THE INVENTION The present invention relates to a three-dimensional position measurement method that measures the spatial position of a light spot by combining two sets of two-dimensional optical position detectors, and particularly relates to a three-dimensional optical position measurement method with an automatic calibration function. It is related to the method.

In the conventional three-dimensional optical position measurement method, two sets of two-dimensional optical position detectors such as television cameras are used to measure the two-dimensional position of a light spot attached to the surface of the object to be measured. A method is adopted in which three-dimensional position measurement is performed based on values and coordinate transformation data. here,
The light spot may be from a light emitting diode (LED) or may be from a laser beam.

Since the conversion data is determined by the positional relationship in which the two sets of cameras are installed, it is necessary to precisely measure the positions of the two sets of cameras and the central axis direction of the optical lens prior to measuring the light spot. Since it is extremely difficult to perform this each time a measurement is made, conventionally two sets of cameras have been mounted on a pedestal and their positional relationship has been fixed.

However, in such a measurement method, the pedestal that supports the camera is large and long, and also requires a large space even when measurements are not being performed. In addition, if the positional relationship of the cameras changes due to some unknown factor, it is difficult to capture the change, and from now on, 3D position measurement will be performed based on inaccurate coordinate transformation data. There was a problem. Furthermore, if the object to be measured moves in a complex manner, such as the tip of a robot's arm, the tip of the arm may be hidden behind the arm. This has the disadvantage that it may not be able to adequately adapt to the movements of people.

In view of the above points, the present invention provides a three-dimensional position measurement method designed to solve such problems and eliminate such drawbacks, and provides a function of automatic calibration of zazen conversion data. A set of two-dimensional optical position detectors can be installed at any position.

Hereinafter, embodiments of the present invention will be described in detail based on the drawings.

FIG. 1 is a block diagram showing an embodiment of the three-dimensional position measuring method according to the present invention, and only the parts necessary for explanation are shown.

In the figure, 1 and 2 are first and second cameras that integrate a semiconductor device detector and an optical lens; 3 and 4;
are the camera pedestals of the first and second cameras 1.2, respectively, and 5 and 6 are the camera pedestals of the first and second cameras 1.2, respectively.
7 is a main calculation unit that receives the outputs of the first and second two-dimensional position detectors 5.6, 8 is a mode selection switch, 9 is a three-dimensional coordinate display section, 10 is a multiplication circuit section, and 11 is a three-point Pa
, Pb, and Pc to form a light emitting diode (L
ED) and its supporting part; 12 is an LED to be attached to the object to be measured; 13 is an LED drive circuit; 14 is a storage element for storing coordinate transformation data and calculated values at intermediate stages of calculation. be. In addition, P in LED12
indicates a three-dimensional coordinate value.

Then, 2 of the three light points whose coordinate positions in the base coordinate system are known
The coordinate transformation data is calibrated from the dimensional position measurement value, and the spatial position of the light door is measured using the calibrated coordinate transformation data.

Next, the operation of the embodiment shown in FIG. 1 will be explained. The three-dimensional measurement system to which the present invention is applied has a measurement mode and a calibration mode, which are selected by a mode selection switch 8.

First, in the measurement mode, the first and second cameras 1 and 2 capture the light emitted by the LED 12 in response to a signal from the LED drive circuit section 13, and the outputs of the first and second cameras 1 and 2 are captured. The Xa and y axis coordinate values (x', yl
) and (x2.y2). Here, the two axes of the camera companion system are made to coincide with the central axis of the optical lens. Therefore, the coordinate value of 2@ cannot be determined at all. Then, the main calculation unit 7 converts these two-dimensional position measurement values into M=(
Xl, y', x2ry") T (T means 1 snow)
and the coordinate transformation data V stored in the storage element 14,
1' (3x4 matrix), w2 (3xt vector) and force・
The three-dimensional coordinate value P==(x.

3’ * Z )” is calculated using the following formula (1).

P = W I M + W 2
- (1) However, multiplication is performed in the multiplication circuit section 10 in order to speed up the calculation. After calculating the three-dimensional coordinate value P, the main calculation unit 7 displays the result on the three-dimensional coordinate display unit 9.
An LED light emission command signal is sent to the LED 12 to cause the LED 12 to emit light again.

The above operation is repeated until the mode is changed by the mode selection switch 8.

Next, when in the 1 calibration mode, 3 on the reference coordinate instrument 11
LED t at point P ), * P B e P o
The -111st order light is emitted, measured by the first and second cameras 1 and 2, and the coordinate transformation data is automatically calibrated as shown below.

FIG. 2 is an explanatory diagram of the coordinate system. Base coordinate system OX +
3'tZ, the coordinate system fixed to the first camera 1 is 0□-X1+3'1+Z1, and the coordinate system fixed to the second camera 2 is 02X2+72+Z2. Here, the two axes of each camera coordinate device coincide with the central axis of the optical lens. And 1. Oi from the origin of j to OX*7+Z
X ij 3' i l Z The vector to the origin of l is expressed as 0Xy7*Z, and the light points at arbitrary intervals are OX
The vector expressed as ry+Z is P” (X, 7
+) T and the vectors 1 1, 1 1 expressed by 0-X, y,, z, are S' - (xj+ door + z i zo), then the relationship of the following equation holds true.

P=L'+E'S', (i=1.2)...
...(2) Here, Ei is a coordinate transformation matrix. Then, translate the origin of OX + 7 + Z to 0l-
xl.

Yi+7. Coincide with the origin of i, and set it to 0 around the X axis.
If we make it match 0l-xi, 74, zi after one rotation, ψi rotation around y@, and φ1 rotation around the second axis,
The coordinate transformation matrix E1 can be described by the following equation.

=6θ】■raφ1 Exposure deviation 5t11Sinφi+amθi su+ψ1cxea
φ1”Sinθi 5illφj −CO3θi Si
Mψ1(ifflφi(...(3) Since this matrix is an orthogonal matrix, the inverse matrix is the inverted row (E'
)−””(Ei) 7e,'k] * * 11
@ 11 (4,), the following relational expression holds true for each element of the matrix.

Then, the three points P A I P B+ on the basic reed coordinate machine 11
Oi X i + 7 i + Z i
What is displayed is S.

When Si and Sl are used, the following relationship holds true in equation (2) above.

PA=Li+EiSAiIPB=Li+EiSB'
IP = Ll + EIS @1111
@11 (8) Here, Qi meeting PA-PB+ Q2Q
PB po IN, 1△S 1S ir N'仝s'
−s 1 and l==mu B
2B C[:(1-+Q-+11.'l]
", N,'=[:ni, nj, n; ]".

3x Engineering Y JZ
Coco X Coy
2 (j=1.2), the following equation holds true.

(Ej) Qj=\i (1=1,2.j=1,2
)...(9) Sara Ic.

bl ”'q1yq2z'llz”2y ” 2=q
IZq2X Q2zqlz'b3""Q1zq2y
Q1vq2y ”...0.00) (i=l,
2) ***・11℃ ii
iii 1c3=n1yq2z
-q1zn2y'e4:n1yq2y-qlyn2y(
i=l, 2) meeting＊＊＊＊(1
; Set it as 5. Then, the following equation can be derived from the above equation (9).

e' = (e'+b ei)/b re' = (-ten b
3ez, )/b212 1 211
1 13... (first floor) Substituting this equation (13) into the above equation (5), we get 2 of el.
The following 1 equation is derived, and its solution is ・Happy・Strong・(Yen.Similarly, e , e , and e are also given in (6) above.
From the equation and (9) 21 22 23, it can be written as the following equation.

・--Italy・j5) @22=(e'3+yb2e21)/b1r @, 3=
(e knee 1 (13 be,) / l), * @ (16 + here the above (141, (the sign of 151 is 7 above)
Select the one that satisfies the formula. Furthermore, the values of the three points PA, PB, and Po on the reference coordinate instrument 12 are known, and SA! ＋S
F.i.

Since each xy coordinate value of So+ is detected, bl, b2゜1iii - b3 and c, c aC+C are %Q fl ol
〜(i It can be calculated using the 2′ formula. Therefore, (Ei)″
The first and second rows of the T matrix are obtained by the algebraic calculations of equations 63) to (16) above.

Next, when Li is displayed with the camera seated, crt) TL
i, and when this is calculated using the above equation (8), (El
)7L1=(Ej)TPA-81仝('X+i)
' + engineering Z) T ・auto・07). here,(
Ei) Jx and Ay can be calculated since the first and second rows of T have a total of ~l-1~i. 1z is not needed here, and when the vector P in the base coordinate system is expressed in the camera coordinate system, it becomes (Ei)TP, and from the above equation, (Ei)T
P=Si+(Ei)TLi (i=1.2)...There will be 0 groups of fists and fights. Here, if , then the above 0 group formula is UP=M+V ---6-(1
It becomes 9. Each element of U and ■ is obtained by the calculation formula shown above, and M is a position calculation value.

This (Formula 19j) is redundant as it has 4 equations for 3 unknown parameters, but taking into consideration the measurement error of the two-dimensional position and the calculation error during the process, we tried to minimize II M+V-UP 11. A three-dimensional coordinate value P is determined, and the three-dimensional coordinate value P is calculated using the following equation.

P=(UTU)-10T(M+V) However, the above II*II means the square root of the sum of squares of the elements of vector *. Therefore, the coordinate transformation data W1 and the coordinate transformation data W2 are each described by the following equations.

W1=[UTU]-1U” W2”CU U) U V As a result, the three-dimensional measured value of the light point in space can be calculated from the two-dimensional position measured values by the first and second cameras 1 and 2 as described in (1) above. This means that it can be calculated using the formula. In the calibration mode, the calculation shown above is performed in the main calculation unit 7 to calibrate the coordinate transformation data, and the value is stored in the storage element 1.
It is stored in 4.

As mentioned above, calibration mode and measurement mode can be selected, and 3
Prior to dimensional optical position measurement, highly accurate three-dimensional position measurement can be realized by calibrating the conversion data.

As is clear from the above description, according to the present invention, by providing the function of automatic calibration of coordinate transformation data,
It becomes easy to change the installation location of the measuring device and the positional relationship between the two sets of two-dimensional position detectors to suit the measurement conditions.
Further, since a large pedestal for identifying two sets of two-dimensional position detectors is not required, the practical effect is extremely large. Further, by performing automatic calibration every time a measurement is performed, errors are not accumulated, which is extremely effective in that highly accurate three-dimensional position measurement can be performed.

[Brief explanation of the drawing]

FIG. 1 is a block diagram showing an embodiment of the three-dimensional position measuring method according to the present invention, and FIG. 2 is an explanatory diagram of a coordinate system used to explain the operation of the embodiment shown in FIG. 1.2・Camera, 5,6−・2D position measurement unit, 7・Fist・Main calculation unit, 8・e・Mode selection switch, 9・ψ・3D coordinate display unit , 10... Multiplication circuit unit, 11... Tatami base single coordinate unit, 12... Light emitting diode (LED), 13... LED drive circuit unit,
14@...Memory element. Figure 1 Figure 2 Figure V.

Claims (1)

[Claims] In three-dimensional position measurement that measures the spatial position of a light spot by combining two sets of two-dimensional optical position detectors, coordinate conversion data is obtained from the two-dimensional position measurement values of three light spots whose coordinate positions in the base coordinate system are known. A three-dimensional position measuring method characterized in that the spatial position of the light spot is measured using the calibrated coordinate transformation data.