JPH0680403B2 - Moving body position and orientation measurement method - Google Patents

Description

Description: TECHNICAL FIELD The present invention relates to a measuring method for detecting the position and orientation of a moving body by a camera in the position traveling direction control of the moving body such as a moving carriage.

[Prior Art] As a method for detecting the position and orientation of a moving carriage, (A) a method of observing a mark on the floor surface with a television camera, a light reflector or a white line on the floor surface, and an optical proximity sensor Therefore, a method of measuring the difference in light reflectance, a method of placing a magnetic plate on the floor and confirming the presence of the magnetic plate with a proximity magnetic sensor.

(B) A method in which a large mark, for example, a vertical line mark is attached to a wall, and this mark is observed with a television camera to measure the camera direction.

Etc. are used.

[Problems to be solved by the invention]

The conventional method of (A) can roughly know the position of the moving carriage, but the accuracy is very poor. Further, if the moving carriage deviates from the planned course and once the reference mark is no longer observable by the TV camera or the sensor, the position and orientation of the moving carriage cannot be known. Furthermore, since the mark and the white line are attached to the floor surface, it is easy to get dirty and requires constant cleaning, which is a maintenance problem.

In the case of (B), it can be used to determine whether or not the camera is approximately facing the mark direction, but it is impossible to accurately know the camera position and orientation in the absolute coordinate system.

As described above, conventionally, there is a problem that the position and orientation of the moving body cannot be accurately obtained by the camera mounted on the moving body.

The present invention has been made to solve the above problems, and an object of the present invention is to provide a moving body position / orientation measuring method with a simple configuration and capable of highly accurate measurement.

[Means for solving problems]

According to the moving body position / orientation measuring method of the present invention, the camera position / orientation parameter is calculated by observing a plurality of marks three-dimensionally distributed by the camera and reducing an error parameter representing a calculation error of the internal parameter of the camera.
The position and orientation of the moving body is measured.

[Action]

In the present invention, the position and orientation of the camera are calculated by observing a plurality of three-dimensionally distributed marks on either the stationary body or the moving body with the camera provided on the other side,
The position and orientation of the moving body is measured based on this.

[Embodiment 1] FIG. 1 is a diagram for explaining a first embodiment of the present invention, in which 1 is a moving carriage, 2 is a television camera, 3 is an image processing device, 4 is a data processing device, and 5 is a plurality. It is a mark plate with a mark. 6 is a wheel control device, and 7 is a wheel.
The mark plate 5 is observed by the television camera 2, and the mark image is binarized and labeled by the image processing device 3 to identify the mark position. The mark number is identified by the data processing device 4,
After calculating the camera position / orientation parameter and converting it to the position / orientation data of the moving carriage 1, the positional relationship between the destination and the moving carriage 1 is obtained, and the direction of the wheel 7 along the route to the destination is corrected. The value and the required number of rotations of the wheel 7 are calculated.
These values are sent to the wheel control device 6, the direction of the wheel 7 is corrected, and a predetermined rotation amount is given to the wheel 7.

FIG. 2 shows a specific example of the mark arrangement on the mark plate 5, and FIG. 3 is a view of the mark plate as seen obliquely. Further, FIG. 4 shows an example of arrangement of mark numbers.

In FIGS. 2 to 4, the circular portions are white circle marks, and the marks in the odd rows are indicated by 5A and the marks in the even rows are indicated by 5B. The other parts are black. The size of the marks 5A in the odd rows is 4 mmφ, and the size of the marks 5B in the even rows is 6 mmφ.
However, mark number 45 is 10 mmφ and 55 is 8 mmφ. The marks 5B in even rows are attached to the upper end of a black cylinder standing on the mark plate surface. The height of this column was 50 mm in the experimental example. The mark spacing was 50 mm.

FIG. 5 shows a processing procedure of mark identification in the image processing apparatus 3 and mark number identification in the data processing apparatus 4. The mark plate 5 shown in FIGS. 2 to 4 will be described as an object. In addition, (1) to (12) indicate each step.

A mark image is input by a camera (1), the mark image is binarized (2), unnecessary images smaller than the mark image are removed as isolated points (3), and then labeling processing is performed (4). The circularity of each labeling area is examined, and the labeling area having a low circularity is removed as something other than the mark (5). Also, the labeling area on the edge of the image is removed (6). Next, the area of each labeling area is obtained, and the labeling area having the largest area and its area are obtained (7). Based on the maximum area S obtained in step (7), four area sections are set according to the size of the mark, and each labeling area is classified into four classes (8). In this embodiment, a) maximum area S, b) (0.64 ±
0.1) × S, C) (0.36 ± 0.1) × S, D) (0.16 ± 0.1) ×
It was classified into four classes of S. The mark corresponding to (a) is mark number 45, and the mark corresponding to (b) is mark number 55.
The mark corresponding to (c) is classified as a mark having an even line number, and the mark corresponding to (d) is classified as an odd line number.

In step (9), for each mark 5A having an odd line number, the line number is identified using the vertical coordinate of the mark image and the fixed mark, that is, the coordinate of the mark 45.
In step (10), the marks of numbers 15, 25, 35, 65, 75, 85, and 95 are selected from the marks of each row with reference to the numbers 45 and 55. Next, using this mark, that is, the mark whose first digit (one's digit) is 5, the other marks are numbered.
In step (11), the center coordinates of each mark are obtained using the circumscribed rectangle or the center of gravity of each mark. Finally, in step (12), a mark table in which the mark number, the coordinate of the mark on the image memory, and the real space coordinate of the mark are recorded is created.

Although the mark arrangement shown in FIG. 4 has been described as an example here, the marks may be optical fiber ends or light emitting bodies such as LEDs, lasers, and lamps. In this case, it is also possible to selectively emit light in each row or each mark to simplify the row number identification or the mark number identification.

The mark plate 5 only needs to have a structure capable of observing the three-dimensional distribution of the mark array from a wide range, and various structures similar to the mark structure shown in FIGS. 2 to 4 are possible. Therefore, instead of a single mark plate, one mark table may be composed of a mark plate group composed of a plurality of mark plates.

Next, the camera position and orientation parameters are obtained using the mark table (mark list). This process is roughly divided into two steps. In the first step, the approximate value of the camera parameter is analytically obtained together with the error parameter. The second step is a correction routine for increasing the accuracy, and analytically obtains a camera parameter that reduces the error parameter obtained in the first step. Hereinafter, the method of processing for calculating the camera parameters will be described in detail by dividing it into the first step and the second step.

(1st step) The center of the camera lens is (c ₁ , c ₂ , c of the object coordinate system (x, y, z).
₃ ). Camera coordinate system (x _c ,
The relationship between y _c , z _c ) and the object coordinate system (x, y, z) is generally expressed by the following equation. The coordinate system is a right-handed system as shown in FIG. 6, and the camera optical axis direction is the y _c axis. Further, 5 is a mark plate, x, y, z are spatial coordinates, α, β are azimuth angles,
2 is a television camera, and x _c , y _c , and z _c are its axes.

However, M is a transformation matrix from the object coordinate system to the camera coordinate system. The matrix element of M is shown by the following equation.

M = (a _kl ) (k, l = 1,2,3) (2) On the other hand, the internal parameter of the camera is the distance (optical axis length) when a perpendicular is drawn from the center of the camera to the camera screen. L
And the coordinates of the intersection (optical axis point) of the perpendicular and the screen are (i ₀ , j ₀ ), and the scale factor (reduction ratio) between the image on the screen and the image on the image memory is Η _i in the i direction (horizontal direction) of the image, η in the j direction (vertical direction)
_j . The coordinates of the reference mark in the camera coordinate system and the image (i, j) have the following relationship.

In subsequent calculations, the transformation matrices are orthogonal, that is, Equation (3) and Equation (4) are rewritten as follows in order to guarantee that.

δ and (ε _i, ε _j) is the error parameter representing the error of an internal parameter optical axis length L and the optical axis point coordinates (i _o, j _o). By substituting the expressions (1) and (2) into the expression (7), the following expression is obtained.

a ′ ₂₁ ix + a ′ ₂₂ iy + a ′ ₂₃ iz + B ″ ₁ x + B ″ ₂ y +
B ″ ₃ z−B ″ ₁ c ₁ −B ″ ₂ c ₂ −B ″ ₃ c ₃ = i (8) where B _l ≡−La ₁ −δa ₁ −η _i i ₀ a _2l −η _i ε _i a _2l (l =
1,2,3) A ₂ ≡a ₂₁ c ₁ ＋ a ₂₂ c ₂ ＋ a ₂₃ c ₃ …… (9) B ″ _l ≡B _l / A ₂ η _i (l = 1,2,3) a ′ _2l ≡ a _2l / A ₂ (l = 1,2,3) Similarly, a ′ ₂₁ jx + a ′ ₂₂ jy + a ′ ₂₃ jz + D ″ ₁ x + D ″ ₂ y +
D ″ ₃ z−D ″ ₁ c ₁ −D ″ ₂ c ₂ −D ″ ₃ c ₃ = j (10) By using the equations (8) and (10), a plurality (n) of reference mark coordinates in the object coordinate system are (x ₁ , y ₁ , z ₁ ), ...,
(X _n , y _n , z _n ) and the corresponding images are (i ₁ ,
j ₁ ),…, (i _n , j _n ), GN = W ……………………………… (12) However, G is a multidimensional parameter vector and G ≡ (a ′ ₂₁ , a ′ ₂₂ , a ′ ₂₃ , B ″ ₁ , B ″ ₂ , B ″ ₃ , D ″ ₁ , D ″ ₂ ,
D ″ ₃ , u ′ ₁ , u ′ ₂ )… (13) Image point vector _{W = (i 1, i 2} , ... i n, j 1, j 2, ..., j n) ... (15) reference mark matrix However, Further, O _mn is an O matrix of m rows and n columns, and I and I ′ are unit matrices of 3 rows and 3 columns and n rows and n columns, respectively.

Next, G is obtained by the least square method using the equation (12). That is, the error E The minimum G is calculated. The result is given by

^{G = W · N T (NN} T) -1 ........................ (18) However, N ^T is the transposed matrix of reference marks matrix N.

Now, the following relationship is established by the equation (9).

a ′ ₂₁ c ₁ + a ′ ₂₂ c ₂ + a ′ ₂₃ c ₃ = 1 (19) From this fact and Equation (14), the camera lens center coordinates (c ₁ , c ₂ , c ₃ ) Is given by

Further, if the transformation matrix M is orthogonal, that is, if the transformation is performed using the conditions of the equations (5) and (6) and the equation (9), the error parameters ε _i and ε _j can be calculated. .

ε _i =-▲ A ² ₂ ▼ (a ′ ₂₁ B ″ ₁ + a ′ ₂₂ B ″ ₂ + a ′
₂₃ B ″ ₃ ) −i ₀ …… (21) ε _j = − ▲ A ² ₂ ▼ (a ′ ₂₁ D ″ ₁ + a ′ ₂₂ D ″ ₂ + a ′
₂₃ D ″ ₃ ) −j ₀ (22) Here, considering the expressions (5) and (6), ▲ A ² ₂
The value calculated by the following formula can be used as ▼.

_{^{▲ A 2 2 ▼ = (a}} '21 2 + a' 22 2 + a '23 2) -1 ...............
(23) Next, the error parameter δ is obtained. If (L + δ) ² a ² _il (l = 1,2,3) is obtained from the equation (9) and transformed using the equations (5) and (6),
The following equation is obtained.

(L + δ) ² / η ² _i = ▲ A ² ₂ ▼ (B ″ ₁ ² + B ″ ₂ ² +
B ″ ₃ ² ) + (i ₀ + ε _i ) ² + 2 (i ₀ + ε _i ) (B ″ ₁ a ′ ₂₁
+ B ″ ₂ a ′ ₂₂ + B ″ ₃ a ′ ₂₃ ) ▲ A ² ₂ ▼ …… (24) Similarly, (L + δ ² ) / η ² _j = ▲ A ² ₂ ▼ (D ″ ₁ ² + D ″ ₂ ² +
D ″ ₃ ² ) + (j ₀ + ε _j ) ² + 2 (j ₀ + ε _j ) (D ″ ₁ a ′ ₂₁
+ D ″ ₂ a ′ ₂₂ + D ″ ₃ a ′ ₂₃ ) ▲ A ² ₂ ▼ ……… (25) Next, from equation (9), a ′ ₁ = {− B ″ _l η _i −η _i (i ₀ + ε _i ) _a'2l } /
Since (L + δ) and (l = 1,2,3), a ′ ₁ = {− B ″ _l − (i ₀ + ε _i ) a ′ _2l } / {(L +
δ) / η _i } ... (26) Similarly, a ′ _3l = {− D ″ _l − (j ₀ + ε _j ) a ′ _2l } / {(L +
δ) / η _j } ... (27) Here, it is guaranteed that the transformation matrix M (a _kl ) obtained above is orthogonal. The apparent optical axis length is set to L + δ obtained by the equation (24) or the equation (25), and the apparent optical axis point is calculated by the equations (21) and (22) (i ₀ + ε _i , J ₀ + ε _j ), the rotation angles α, β of the camera around the x, y, z axes are α = sin ⁻¹ (−a ′ ₁₂ tan / a ′ ₃₂ ) β = tan ⁻¹ (a ′ ₃₁ / a ′ ₃₃ ) = tan ⁻¹ (−a ′ ₃₂ sin β / a ′ ₃₁ ).

If there is no tilt between the camera and the mark, the mark configuration or lighting conditions are adjusted optimally, and if the depth distribution of the mark extends over a wide range, the error parameter is a small value that can be ignored. Is likely to be. At this time, the obtained values of i ₀ + ε _i , j ₀ + ε _j , L + δ can be used as the internal parameters i _o , j _o , L, respectively. However, usually, the positional relationship between the camera and the mark and the illumination conditions change. Further, a reading error of the center coordinates of the mark also occurs in the actual measurement. Therefore, the calculated apparent optical axis point (i ₀ + ε _i , j ₀ + ε _j ) and the apparent optical axis length (L
+ Δ) differs from the true value, (i ₀ , j ₀ ), L, so that
An error occurs in the camera position / orientation parameter. In practice, this error can often be quite large. In order to calibrate the camera position and orientation parameters with high accuracy, the apparent optical axis point and the apparent optical axis length must match the true values, that is, the second step in which the error parameters ε _i , ε _j , and δ are set to 0. is necessary.

(Second Step) Generally, ε _i and ε _j cannot be separated from the apparent optical axis point by the above method, but the optical axis point (i ₀ , J ₀ ) can be obtained. With respect to the optical axis length L, when the reference mark array surface is substantially perpendicular to the optical axis of the camera and the number of reference marks is large, the parameter calibration of the first step according to the present invention can accurately obtain the reference mark arrangement surface. This allows
The relationship between the lens extension position and L is calibrated in advance.
In this way, the camera-specific parameters (i ₀ , j ₀ )
By obtaining L and L in advance, the values of ε _i , ε _j , and δ can be separated. Next, parameter correction is performed so that ε _i , ε _j , δ = 0 and the transformation matrices are orthogonal.

First, the angle θ _M formed by the line of intersection between the image plane and the horizontal plane (xy plane) of the object coordinate system and the i-axis of the image plane is obtained.

The following equation can be derived as θ _M.

θ _M = cos ⁻¹ (b ₁ , a ′ ₁₁ + b ₂ a ′ ₁₂ ) where The angle θ formed by the line of intersection and the line connecting (i ₀ , j ₀ ) and (i ₀ + ε _i , j ₀ + ε _j ) is given by the following equation including θ _M in equation (29). .

However, ▲ l ² ₀ ▼ = ε ² _i + ε ² _j Given in.

Δ is used for c ₁ , c ₂ , and c _3. Given in.

Next, using the azimuth corrected by the equation (31), a ₂₁ , a
₂₂ and a ₂₃ are calculated. Using this value and c ₁ , c ₂ and c ₃ corrected by the equation (32), a ′ ₂₁ , a ′ ₂₂ , a ′ _{23 is obtained} by the following equation.
Ask for.

_{a '21 = a 21 / A} 2, a' 22 = a 22 / A 2, a '23 = a 23 / A 2 ... (3
3) However, A ₂ = a ₂₁ c ₁ ＋ a ₂₂ c ₂ ＋ a ₂₃ c ₃ ………… (34) Up to this point is the preliminary correction.

Now, when the _{a '21, a' 22,} a '23 and known number, the (8)
The equation can be rewritten as the following equation in which the unknown coefficient is included only on the left side.

_{B "1 x + B" 2} y + B "3 z-B" 1 c 1 -B "2 c 2 -B" 3
c ₃ = i−a ′ ₂₁ ix−a ′ ₂₂ iy−a ′ ₂₃ iz (35) Similarly, instead of the equation (10), the following equation is obtained.

_{D "1 x + D" 2} y + D "3 z-D" 1 c 1 -D "2 c 2 -D" 3
c ₃ = j−a ′ ₂₁ jx−a ′ ₂₂ jy−a ′ ₂₃ jz (36) The plural (n) reference mark coordinates in the object coordinate system are (x ₁ , y ₁ , z ₁ ), ..., (x _n , y _n , z _n ) and the images corresponding to them are (i ₁ , j ₁ ), ..., (i _n , j _n ), Equation (35), Using the formula 36), the following relational formula is obtained.

P ・ Q = W ……………………………… (37) However, Is. Similarly to the case of deriving the equation (18), P is obtained by the following equation P = V · Q ^T (Q · Q ^T ) ^-1 …………………… (41), and P is B ″ _{1, B "2, B"} 3, D "1, D" 2, D "3, u '1, u' 2 is obtained. Using this, the (26), (27), (28)
The corrected angles α and β are obtained from the equation. The obtained transformation matrix is guaranteed to be orthogonal. These values are used to calculate c ₁ , c ₂ and c ₃ by the equation (20), and the equation (21),
From equations (22), (24), and (25), we obtain ε _i , ε _j , and δ.

In the processing up to this point, ε for normal reference mark conditions
_i , ε _j , and δ are sufficiently small. However, when there are few reference marks and the condition of the reference marks is bad such that the reference marks are gathered at one place on the image, ε _i , ε _j , and δ may not be negligible. In that case, the processes of the expressions (29) to (41) are re-executed. In this way, the camera position / orientation parameters can be known with high accuracy.

FIG. 7 shows a flow chart of the processing. Note that (2
1) to (34) show each step.

After inputting the coordinates of the reference mark at step (21), at step (22), N, W are calculated by the equations (15) and (16).
Was excisional, in step (23), G by the equation (18), i.e. _{a '21, a' 22,} a '23, B "1, B" 2, B "3, D" 1, D "2 , D ″ ₃ ,
Obtain u ′ ₁ and u ′ ₂ and, in step (24), formula (20) to formula (2
From equation (5), c ₁ , c ₂ , c ₃ , ε _i , ε _j , δ ₁ , and δ _j are obtained. Next, in step (25), equation (26),
The (27) seeking _{a '11, a' 12,} a '13, a' 31, a '32, a' 33 by equation by step (26), the azimuth angle alpha, beta, seek. The correction routine starts from step (27).
If ε _i , ε _j , and δ cannot be ignored, proceed to step (28). In step (28), after correcting the azimuth angle and c ₁ , c ₂ , and c ₃ by the equations (31) and (32), the equation (33) is calculated in step (29). , Equation (34) gives a ′ ₂₁ ,
a ′ ₂₂ and a ′ ₂₃ are obtained, and in step (30), the third (3
9 and Eq. (40) are used to set Q and V, and in Step (31), Eq. (37) is set to P, that is, B ″ ₁ , B ″ ₂ , B ″ ₃ , D ″ ₁ , D ″. Calculate ₂ , D ″ ₃ , u ′ ₁ , u ′ ₂ . In step (32), the (20), (21),
Eqs. (22), (24) and (25) are used to find ε _i , ε _j and δ, and the process returns to step (27). YES at step (27)
In the case of, that is, when ε _i , ε _j , and δ are so small that they can be ignored, the process proceeds to step (23) of the confirmation routine. In step (23), the camera azimuth α,
The reference mark image is calculated using β and the camera positions c ₁ , c ₂ , and c ₃ , and in step (34), the deviation between the coordinates of the actual mark image and the calculated mark image coordinates is calculated.

Next, the measurement accuracy and processing time of the position and orientation of the moving carriage according to the present invention will be described.

A CCD camera was used as the camera, and the focal length of the lens was 55 mm. The error of the position of the moving carriage measured when the moving carriage passes through a position where the optical axis of the camera has an angle of 0.4 radian with the normal of the mark board and the camera and the mark board are 900 mm apart. Was 0.99 mm (~ 0.1%), and the maximum orientation error was 0.001 radian. The total time required from the input of the mark image to the output of the position / orientation parameters of the moving carriage was about 6 seconds using a commercially available general-purpose image processing device and a minicomputer.

[Second Embodiment] FIG. 8 shows a second embodiment. In this figure,
Reference numeral 8 is a radio transmitter / receiver on the mobile carriage 1, 9 is an antenna, 10 is a fixed radio transmitter / receiver, and 11 is an antenna connected to the radio transmitter / receiver 10.

The mark plate 5 is observed by the TV camera 2, and the image processing device 3
The position of the mark is identified from the mark image, and the data processing device 4 identifies the mark number and calculates the camera position / orientation, which is converted into the position / orientation of the carriage. Further, the direction correction amount of the truck is calculated. The calculated correction amount is
It is transmitted to the mobile trolley 1 via 10 and the antenna 11. The antenna 9 detects a radio signal, the radio transmitter / receiver 8 converts the transmitted data into a digital electric signal, and the wheel control device 6
The direction of the wheel 7 is corrected with. The configuration of the mark plate 5, the identification of the mark plate 5, and the process of calculating the camera position / orientation are the same as those in the first embodiment. In addition, a plurality of mark plates can be mounted on the tower so that at least one mark plate 5 is always visible from the front, rear, left and right.

[Embodiment 3] FIG. 9 shows a third embodiment. In this figure,
12 is a wall in the room, 13 is another wall, and the direction is different from the wall 12 in the room. 51 to 54 are mark plates, and each mark plate 51 to 54 is provided with a special mark corresponding to the mark plate number. 1 is a mobile trolley, which is equipped with a television camera and a broadband wireless transmitter. Reference numeral 14 denotes an image data processing device that receives an image wirelessly transmitted from the mobile vehicle 1 and determines a position traveling direction of the mobile vehicle 1. Points A, B, C, D are planned passage points, points a, b, c, d are points actually passed, and 15 is a locus of the movable carriage 1. Since the unevenness of the floor is large and the traveling direction is not constant, the mark plates placed at a plurality of points are observed by the TV camera to measure the position and orientation of the moving carriage 1, and the traveling direction is corrected. The configuration of the mark plate and the processing procedure for obtaining the camera position / orientation parameters are described in the first embodiment.
As described in.

[Fourth Embodiment] FIG. 10 is a diagram for explaining a fourth embodiment. 30 is the water surface of the boat stadium, 31 is a boat with a TV camera and a broadband wireless transceiver. 51 and 52 are mark plates. The television camera can rotate because the mark plates 51 and 52 are placed in the field of view, and the rotation angle is transmitted to the image data processing device 14 together with the image. The image data processing device 14 calculates the trajectory and speed of the boat 31, records the same, and notifies the boat 31 of the position and orientation by wireless transmission.

As described above, the present invention can be applied to a place where it is difficult to measure the locus and the velocity in the past, such as on water.

〔The invention's effect〕

As described above, the present invention arranges a mark plate on which a plurality of marks are arranged on a stationary body and observes these marks with a camera mounted on a moving body, or arranges the camera on the stationary body. The method has the following advantages because it is a method of measuring the position and orientation of the moving body by observing the mark attached to the moving body to obtain the relative position and orientation of the camera and the mark.

(1) The position and orientation of the moving body can be accurately measured in a short time. Therefore, it is possible to accurately guide the traveling direction of the moving body.

(2) It is not necessary to place the mark very close to the moving body. Therefore, it can be attached to the ceiling, the side wall, or the billboard. Therefore, the mark can be arbitrarily arranged according to the environment.

(3) The moving body does not necessarily have to be on the floor surface. For example, it may be on the water or in the air.

[Brief description of drawings]

FIG. 1 is a diagram for explaining a first embodiment of the present invention,
FIG. 2 is a front view of a mark plate used in the present invention, FIG. 3 is a perspective view of the same, FIG. 4 is a diagram showing an arrangement example of mark numbers on the mark plate, and FIG. 5 is a process of mark identification and mark number identification. FIG. 6 is a flow chart showing the procedure, FIG. 6 is a view showing how to take the camera coordinate system and the object coordinate system, FIG. 7 is a flow chart of the process for calculating the camera position / orientation parameter, and FIG.
FIG. 9 is a view for explaining the third embodiment, FIG. 9 is a view for explaining the third embodiment, and FIG. 10 is a view for explaining the fourth embodiment. In the figure, 1 is a moving carriage, 2 is a television camera, 3 is an image processing device, 4 is a data processing device, 5 is a mark plate, 6 is a wheel control device, and 7 is a wheel.

Claims (1)

[Claims] 1. A plurality of marks, which are fixed on a stationary body or a moving body and are three-dimensionally distributed, are arranged at known positions, and these marks are observed by a camera provided on the moving body or the stationary body. Then, the position / orientation of the moving body is measured by reducing the error parameter representing the calculation error of the internal parameter of the camera to calculate the camera position / orientation parameter.