JPH03296604A - Distance and attitude measuring apparatus for moving body - Google Patents

Abstract

PURPOSE:To measure with high accuracy the position and attitude angle of an object to a moving body by obtaining the position and attitude angle on the basis of the detecting values of the position of a target of the object and the angle of the object when a still image of the object is photographed. CONSTITUTION:The image data of an object 9 to be measured including a target member 10 which is photographed by a TV camera 2 is processed in a signal processing part 3 and displayed at a display part 4. An operating part 5 is manipulated then to output a still image instructing signal to the processing part 3. As a result, a still image of the image data is displayed at the display part 4. A cursor 4a is sequentially adjusted to three targets of the member 10 via a cursor generating part 6 and the cursor positional data is output to an operating part 7. A joint angle of a manipulator 1 is detected by a joint angle detecting sensor 8 at the same time when the still image instructing signal is output. The joint angle data is output to the operating part 7. In the operating part 7, the position and attitude angle are operated from the cursor positional data, and the operated values are synthesized with the joint angle data. Accordingly, the position and attitude angle of the object 9 to the manipulator 1 are obtained.

Description

[Detailed Description of the Invention] [Objective of the Invention] (Industrial Application Field) The present invention is directed to installing a television camera (TV camera) on a manipulator built in, for example, outer space, and performing operations based on images taken by the manipulator. The present invention relates to a distance and orientation measuring device for a moving body that measures the position and orientation of a measurement object such as an object.

(Prior Art) Recently, in the field of space development, manned space programs such as space stations have been promoted, and along with this, there are plans to construct large space structures in outer space. In this large-scale space structure construction plan, the use of a manipulator system is being considered for the installation and assembly of the structure. What is required of such a manipulator system is, in particular, safety and a structure that allows accurate work to be performed easily. Therefore, in order to satisfy this requirement, it is necessary to accurately grasp the position and attitude angle of the object to be measured, such as the object to be worked on, with respect to the manipulator.

As such a position and orientation measuring means, a method has been considered in which a laser beam is irradiated onto the object to be measured and the reflected light is detected by a detection sensor such as a CCD.

However, with the above-mentioned position and attitude measuring means, it is difficult to make accurate measurements unless reflected light of sufficient and stable brightness is received from the object to be measured. It is necessary to include a reflecting mirror, which has the problem of complicating the structure and increasing the weight.

This situation is not limited to space manipulator systems, but also applies to manipulator systems used in gravitational environments.

(Problems to be Solved by the Invention) As described above, the conventional measuring means for detecting the position and orientation of the manipulator and the object to be measured has a problem that the structure is complicated and it is heavy and bulky. Ta.

This invention has been made in view of the above circumstances, and an object thereof is to provide a posture measuring device.

[Structure of the Invention (Means for Solving the Problems) The present invention includes a target member provided on an object to be measured and having at least three targets that are not located in a straight line, and a manipulator provided with the target member. an imaging means for taking an image, a display means for displaying an image taken by the imaging means, an image stilling means for making the display means freeze an image of the target member taken by the imaging means, and an image displayed on the display means. target position detection means for detecting the position of the target of the target member in the still image by superimposing a cursor on the target in the still image of the target member; angle detection means for detecting the angle of the moving body; The distance and attitude angle of the object to be measured with respect to the moving object are determined based on the detected position of the target member and the detection value of the angle detecting means at the time of capturing a still image of the target member displayed on the display means. A moving body position and orientation measuring device is provided with calculating means for calculating the position and orientation of a moving body.

(Operation) According to the above configuration, the position and attitude angle of the object to be measured with respect to the moving object are determined by the position and attitude angle of the target member relative to the imaging means detected from a still image of the target member, and the angle of the moving object at the time of detection. By combining the position and attitude angle of the imaging means of the tip of the moving body with respect to the base of the moving body (the attachment is the part) determined by required in a relationship. Therefore, accurate measurements can be made regardless of the influence of vibrations caused by backlash etc. applied to the moving object, and accurate measurements can be made based on the accurate position and attitude angle of the moving object with respect to the object to be measured. This enables safe operation control.

Further, according to this, since it becomes possible to use the imaging means as a monitoring television camera or the like, it is possible to simplify the configuration as much as possible, and contribute to promoting miniaturization and weight reduction.

(Example) Hereinafter, an example of the present invention will be described in detail with reference to the drawings.

A monitoring television (TV) camera 2 is mounted on the generator 1 as an imaging means. This TV left camera is connected to the display section 4 via the signal processing section 3. The signal processing section 3 selectively displays a still image of the image taken by the TV left camera on the display section 4 via the operation section 5 . In addition, the display section 4
A cursor 4a is displayed in response to an operation on the operation unit 5.

The cursor 4a is set to move via the cursor generating section 6 in conjunction with the operation of the operating section 5, and outputs cursor position data to the calculating section 7 in accordance with the set position. The calculation unit 7 is connected to the output end of a joint angle detection sensor 8 provided on the manipulator 1, and receives a detection signal from the joint angle detection sensor 8 as input.

On the other hand, a measurement target member 10 is attached to a predetermined position on the measurement object 9 imaged by the TV right camera. This target member 10, as shown in FIG.
~103.

In the above configuration, when measuring the position and attitude angle of the measurement object 9 with respect to the manipulator 1, first, the image data of the measurement object 9 including the target member 10 captured by the TV right camera is processed by the signal processing unit 3. displayed on the display section 4. Next, after confirming that the target member 10 is displayed on the display section 4, the operator operates the operation section 5 to output a still image command signal to the signal processing section 3, and outputs a still image command signal to the signal processing section 3, thereby converting the image data. A still image is displayed on the display unit 4. Here, the operator:
The cursor generating section 6 is activated by operating the cursor operating section of the operating section 5.
to move the cursor 4a to the targets 101-1 of the target member 10.
03 in order. When the cursor 4a corresponds to the targets 101 to 103 of the target member 10, the operation section 5 outputs the cursor position data to the calculation section 7. Also, at the same time as the above still image command signal, the joint angle detection sensor 8
detects the joint angle of the manipulator 1 and outputs the joint angle data to the calculation unit 7. The calculation unit 7 first calculates the position and attitude angle from the cursor position data, and synthesizes the joint angle data of the manipulator 1 with the calculated values to calculate the position and attitude angle of the object 2 to be measured with respect to the manipulator 1, as will be described later. to ask.

That is, from the cursor position data, the single-mera reference coordinate X in the target direction of the target member 10 at the still image command sending time is determined.
M, YM, and ZM (see Figure 2). For example, as shown in FIG. 3, the target reference point P+ (
i-1, 2, 3) on the image corresponding to x
,,y, and the unit vector e + T in the P direction
The horizontal angle of view of V camera 2 is 2λ. (In Figure 3, 2
If the vertical angle of view is 2μ0 (corresponding to the angle with respect to 2Yo in Figure 3), then point P1
On the other hand, assuming that the target member 10 as shown in FIGS. 2 and 4 is attached to the measurement object 9, the azimuth angle λ and the vertical angle μ can be determined from the following relational expression. Therefore, the unit vectors e and ha hold true.

becomes. Also, since the three points of targets 101 to 103 are well positioned, from these and el+62+e,
Target origin 10. relative to camera reference coordinates, as described below. (see FIG. 4) and the attitude angle of the object to be measured 9 are determined.

Figure 5 shows a mark 2a consisting of coordinate axes Xt, YT, and ZT.
and the unit vector connecting targets 101 to 103, Dl, I1
2. I3 is each distance from the camera reference point 2a to the targets 101 to 103.

In addition, the mutual relationship between the targets 101 to 103 is defined as r,・r
i - constant value 0, j -1, 2, 3) ... (3) In the illustrations shown in Figs. 2 and 4, the reference axis xT
, y, , zT.

With these conditions added, the above equation (2) becomes: Here, rl, r3 and e I+ e I (scalar product) are known values, and 11. I12. Is is an unknown value to be determined, and e l 1 e I is
Angle φ1 between vectors 7 below and 7 below. equal to the cosine of 1
In other words, it can be expressed as elajl-coSφ1(i-J-1°2°3) (6).

On the other hand, solutions R1, I2 of the above equation (5). In order to find I13, we introduce the following function and set it as is obtained. Here, f + * gi + hl
each means a derivative with respect to xl. Above (9
) is rewritten in matrix form. By using 6g derived from the equation (11) to find δgl again as X, -δIt X+, Xl asymptotically approaches 11, and g is found.

Next, by substituting the obtained value of gl into equation (2), we get the following solution to equation (5): 11. Setting values near Iz and 1g and finding the coefficients of equations (7) and (9),
From equation (10), deviation amount δ expressed by equation (8)
11 will be found. That is, rl+r3 means that these components have been found for the camera/coordinate axis. As a result, the target coordinate axis XT shown in FIG.
, YT, and Zt, and the transformation matrix (MT is...(1
2) Determined by.

The elements of the matrix in equation (15) are related to the Euler angle, so that the Euler angle that meets the definition of the rotation order can be derived by ordinary calculation.

As an example, the Euler angle is expressed as the azimuth angle ψ with respect to the camera reference coordinates.
(Yy axis) Pitch angle θ (Xt axis), roll angle φ
If defined in terms of the rotation order of (ZM axis), if the ij acid component of CMT is C1, then the Euler angle is Sθ2 sin θ,
Cθ: cos θSψ: sinψ, Cφ:
It is determined by the formula cosψSφ: sinφl Cφ:cosφ.

Here, the relationship between the coordinate transformation matrix (MT and Euler's angle) can be found as follows.

Further, the position vector L from the camera reference point 2a to the target origin 10a is given by L=I3e3 r3 (18).

As described above, the positions of the targets 101 to 103 in the still image are detected from the still image of the target member 10 displayed on the display unit 4, and based on the positions of the targets 101 to 103, the calculation unit 7 calculates Perform calculations to find the direction (unit vector) of each target reference point, and use this and the positional relationship (known) between each target reference point to determine the direction of the TV left camera at the time when the target member 10 is made into a still image. The position and attitude angle of the target member 10 with respect to the reference point 2a are determined.

At the same time, the joint angle of the manipulator 1 at the time when the target member 10 is made into a still image is input to the calculation unit 7 from the joint angle detection sensor 8, and based on that value, the manipulator 1
For installation, find the position and attitude angle of the camera reference point 2a of the arm tip with respect to the part (base) la, and use the found values and
The position and attitude angle of the target member 10 relative to the reference point 2a of the TV left camera obtained as described above are combined to determine the position and attitude angle of the target member 10 relative to the base 1a of the manipulator 1.

For example, in the case of a manipulator 1 with two degrees of freedom (two joints) as shown in FIG. 6, the camera reference point 2a of the TV left camera and the camera reference coordinate axis (XM.

YM, ZM), the distance from the reference point 2a to the target origin 10a of the target member 10 and the attitude of the target coordinate axes (Xt, Yr, Zt) with respect to the camera reference coordinate axis are expressed as follows. However, L is shown as a vector component with respect to the camera coordinate axis, and ψ, θ, and φ are Euler angles shown in equation (16) above. Furthermore, the joint angle of the manipulator m L at the time when the still image is made is rl+r'2 as shown in FIG.

At this time, the position L's+m (Lx,
LY +LZ ) is the coordinate axis (
For XB, YB, ZB)...(20
) is obtained as. In addition, the base 1a is fixed and the coordinate axis (
The Euler angle of the target coordinate axes (Xt, YT, Zt) of the target member 10 with respect to Xs, Ya, Za) is defined in the same manner as the above equation (16) as follows: ψ-r, +r2+α+β, θ-0. φ-0...(21
) is required. As a result, the position and attitude angle of the marker member 10 (object to be measured 9) with respect to the base 1a of the manipulator 1 are determined.

In this manner, the manipulator distance and orientation measuring device displays a still image of the target member 10 on the display unit 4, determines the cursor position in the still image for the targets 101 to 103 from this still image, and then displays the position of the cursor in the still image with respect to the targets 101 to 103. The position and attitude angle of the target member 10 relative to the target member 10 are calculated, and the position and attitude angle of the arm tip with respect to the TV left camera with respect to the base 1a of the manipulator 1 are determined from the joint angle of the manipulator 1 at the time of detection, and these values are calculated. The configuration is such that the position and attitude angle of the target member 10 with respect to the manipulator 1 are calculated by combining them. According to this, the position and posture angle of the target member 10 with respect to the base 1a of the manipulator 1 can be determined, so that even when vibrations due to so-called backlash etc. are applied to the joints of the manipulator 1, the vibrations can be suppressed. Since accurate measurements can be made regardless of the situation, accurate and highly accurate operation control of the manipulator 1 is possible, and safety can be improved. Further, according to this, the TV left camera can also be used as a television camera for monitoring operation, thereby contributing to the simplification of the configuration as much as possible, and the promotion of miniaturization and weight reduction.

In addition, in the above embodiment, for convenience of explanation, a manipulator with two degrees of freedom was used, but the application is not limited to manipulators with this number of degrees of freedom, and when the number of degrees of freedom increases. is similarly determined by the known forward kinematics analysis of the manipulator.

Further, in the above embodiment, the target 10 is used as the target member 10.
1 to 103 are arranged at the vertices of a substantially triangular shape. However, the present invention is not limited to this, and it is also possible to use a plurality of targets in which at least three targets are arranged not in a straight line. It is possible.

Further, in the above embodiment, the case where the device is configured for a manipulator has been described, but the configuration is not limited to this, and it is also possible to configure the device to measure the position and attitude of the spacecraft with respect to the object to be measured.

Moreover, as a moving body, it is also possible to be configured to measure the position and orientation of a measurement target such as a car on the ground.

Therefore, it goes without saying that the present invention is not limited to the above embodiments, and that various modifications can be made without departing from the spirit of the invention.

[Effects of the Invention] As detailed above, according to the present invention, a structure measuring device can be provided.

[Brief explanation of drawings]

FIG. 1 is a block diagram showing a distance and attitude measuring device for a manipulator according to an embodiment of the present invention, FIG. 2 is a diagram showing the target member in FIG. 1
FIG. 3 is a diagram shown to explain the operation of the diagram. 1... Manipulator, 1a... Base, 2... T
V left camera 2a...Camera reference point, 3...Signal processing section, 4...Display section, 5...Operation section, 6...Cursor generation section, 7...Calculation section, 8. ...Joint angle detection sensor, 9...Measurement object, 10...Marking member, 101
~103...Target, 10a...Target origin.

Claims (1)

[Claims] A target member provided on an object to be measured and having at least three targets that are not located in a straight line, and provided on a moving body,
an imaging means for taking an image of the target member; a display means for displaying an image taken by the imaging means; an image stilling means for making the display means freeze the image of the target member taken by the imaging means; target position detection means for detecting the position of the target of the target member in the still image by superimposing a cursor on the target in the still image of the target member displayed on the target member; angle detection means for detecting the angle of the moving body; The position of the target member relative to the object to be measured with respect to the moving object based on the position of the target detected by the position detecting means and the detected value of the angle detecting means when capturing a still image of the target member displayed on the display means. and a calculation means for determining an attitude angle.