JP3056764B2 - Position and attitude measurement device for moving objects - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION [Purpose of the Invention] (Industrial application field) The present invention relates to a manipulator built in, for example, outer space or the like, in which a television camera (TV camera) is mounted, and an image taken by the television camera is taken. More specifically, the present invention relates to a position and posture measuring device for a moving body that measures a position and a posture between the work and a measurement object.

(Prior Art) Recently, in the field of space development, a manned space program such as a space station has been promoted, and accordingly, there is a plan to construct a large space structure in outer space. In this large space structure construction plan, use of a manipulator system is considered for mounting and assembling structures. What is demanded of such a manipulator system is, in particular, to be configured so that accurate work can be easily performed together with its safety. Therefore, in order to satisfy this requirement, it is necessary to accurately grasp the position and the posture angle of the measurement object, for example, the work object with respect to the manipulator.

As such a position and orientation measuring means, a method of irradiating a measurement object with laser light and detecting the reflected light with a detection sensor such as a CCD has been considered.

However, the above-mentioned position and orientation measuring means cannot accurately measure unless the reflected light of sufficient and stable luminance is received from the object to be measured. A device and a reflecting mirror must be provided, and the configuration is complicated, and the weight is increased.

Such a situation is not limited to the manipulator system for space, but is the same in a manipulator system used in a gravitational environment.

(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 configuration is complicated and the weight increases. Was.

The present invention has been made in view of the above circumstances, and provides a position and orientation measuring apparatus for a moving body that has a simple configuration, can be reduced in weight, and can realize highly accurate measurement. With the goal.

[Constitution of the Invention] (Means for Solving the Problems) The present invention has a set of three targets, and at least one of the targets is not in a straight line connecting the two targets and is not in space. A target member provided on the measurement object to be arranged,
Imaging means provided on a moving body for imaging the target member, display means for displaying an image taken by the imaging means, and image stillness for stopping the image of the target member taken by the imaging means on the display means Means, target position detecting means for superimposing a cursor on the target in the still image of the target member displayed on the display means to detect the position of the target member in the still image, and detecting the angle of the moving body Angle detection means, and the movement based on the position of the target of the target member detected by the target position detection means and the detection value of the angle detection means at the time of capturing a still image of the target member displayed on the display means A position and posture measuring device for a moving body is provided, comprising a calculation means for calculating a position and a posture angle of the measurement object with respect to a body.

(Operation) According to the above configuration, the position and the posture angle of the measurement target object with respect to the moving body are the position and the posture angle of the target member with respect to the imaging unit detected from the still image of the target member, and the angle of the moving body at the time of the detection. The position and the attitude angle of the imaging means at the tip of the moving body with respect to the base (attaching part) of the moving body obtained above are combined and obtained to obtain the relationship between the base (mounting part) of the moving body and the target member. Can be Therefore, accurate measurement is performed irrespective of the influence of vibration due to backlash or the like applied to the moving body, and the moving body can be accurately determined based on the accurate position and attitude angle of the moving body with respect to the measurement object. As a result, safe operation control becomes possible. Further, according to this, the imaging means can be used also as a monitoring television camera or the like, which can contribute to the simplification of the configuration as much as possible and the promotion of miniaturization and weight reduction.

Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.

FIG. 1 shows a position and orientation measuring apparatus for a moving body according to an embodiment of the present invention. A manipulator 1 is equipped with a monitoring television (TV) camera 2 as an image pickup means. This TV camera 2 is connected to a display unit 4 via a signal processing unit 3. The signal processing unit 3 selectively displays a still image of an image captured by the TV camera 2 via the operation unit 5 on the display unit 4. A cursor 4a is displayed on the display unit 4 in accordance with the operation of the operation unit 5. The cursor 4a is moved and set via the cursor generating unit 6 in conjunction with the operation of the operation unit 5, and outputs cursor position data to the arithmetic unit 7 corresponding to the set position. The calculation unit 7 is connected to an output terminal of a joint angle detection sensor 8 provided on the manipulator 1, and receives a detection signal of the joint angle detection sensor 8.

On the other hand, the measurement target member 10 is attached to a predetermined position on the measurement target 9 imaged by the TV camera 2. For example, as shown in FIG. 2, the target member 10 includes three targets 103 which are not located on a straight line connecting the targets 101 and 102, and which are arranged at substantially triangular vertices located in space. 101
~ 103.

In the above configuration, when measuring the position and attitude angle of the measuring object 9 with respect to the manipulator 1, first, the TV
Image data of the measurement object 9 including the target member 10 captured by the camera 2 is processed by the signal processing unit 3 and displayed on the display unit 4. Next, while confirming that the target member 10 is displayed on the display unit 4, the operator operates the operation unit 5 to output a still image command signal to the signal processing unit 3 and output the image data. The display unit 4 displays a still image of the data. Here, the operator operates the cursor operation unit of the operation unit 5 to sequentially align the cursor 4 a with the targets 101 to 103 of the target member 10 via the cursor generation unit 6. When the cursor 4a corresponds to the targets 101 to 103 of the target member 10, the operation unit 5 outputs the cursor position data to the calculation unit 7. At the same time as the 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 arithmetic unit 7. The calculation unit 7 first calculates and obtains a position and an attitude angle from the cursor position data, and calculates the calculated value by using a manipulator.
By combining the joint angle data of the data 1, the position and the attitude angle of the measuring object 2 with respect to the manipulator 1 are obtained as described later.

That is, the unit vector in the target direction of the target member 10 at the still image command transmission time from the cursor position data
▼, ▲ ▼, ▲ ▼ (see FIG. 2) are obtained for the camera reference coordinates X _M , Y _M , Z _M (see FIG. 2). For example, as shown in FIG. 3, mosquitoes on the image corresponding to the target reference point P _{i (i} = 1,2,3) - Sol position and X _i, Y _i, unit vector P _i direction ▲ ▼, a horizontal angle of view of the TV camera 2 2λ _O
(In the third drawing, corresponds to an angle relative to 2X _{O), (in} the third figure, corresponds to an angle with respect to 2Y _O) Vertical angle 2.mu. _O When the azimuth lambda _i and the vertical angle of the point P _i mu _i Is Find more. Therefore, the unit vector ▲ ▼ is Becomes In addition, the positional relationship of the three points of the targets 101 to 103
▼, ▲ ▼, ▲ ▼ (refer to FIG. 4) are known and good.
As described below, the position of the target origin 10 _a (see FIG. 4) with respect to the camera reference coordinates and the measurement object 9 are described below.
Is required.

FIG. 5 shows a target coordinate system composed of coordinate axes X _T , Y _T , and Z _T , where ▲ ▼, ▲ ▼, ▲ ▼ indicate coordinate axes X _T , Y
_It shows a unit vector along _T and ZT.

On the other hand, assuming that a target member 10 as shown in FIGS. Holds.

Here, ▲ ▼, ▲ ▼, ▲ ▼ are unit vectors connecting the camera reference point 2a and the targets 101 to 103, and l ₁ , l ₂ , l ₃ are the distances from the camera reference point 2a to the targets 101 to 103, respectively. is there.

Also, a vector ▲ that defines the interrelationship of the targets 101 to 103
▼, ▲ ▼, ▲ ▼ In the illustrations shown in FIGS. 2 and 4, And with these conditions added, the above equation (2) becomes Becomes

Where r ₁ , r ₃ and ▲ ▼, ▲ ▼ (scalar product)
Are known values, l ₁ , l ₂ , l ₃ are unknown values to be determined, and ▲ ▼, ▲ ▼ are vector ▲
It is equal to the cosine of the angle φ _ij between ▼ and ▲ ▼. That is, Can be represented by

On the other hand, to find the solutions l ₁ , l ₂ , l ₃ of the above equation (5), By introducing the following function, X _i = l _i + δl _i (i = 1,2,3) (8), then from the tailor expansion around l _{i in} equation (7), Is obtained. Here, f _i , g _i , and h _i each mean a derivative with respect to X _i . When the above equation (9) is rewritten into a matrix form, In this way, by setting values in the vicinity of the solutions l ₁ , l ₂ , and l ₃ of the equation (5) and calculating the coefficients of the equations (7) and (9), the equation (8) is obtained from the equation (10). deviation amount .DELTA.l _i expressed so that is obtained. That is, (11) by again seeking .DELTA.l _i as X _i -δl _i → X _i with .DELTA.l _i derived by formula, X _i is asymptotic to l _i,
l _i will be obtained.

Next, by substituting the value of l _{i found in} equation (2), And ▲ ▼, ▲ ▼ with respect to the camera coordinate axis,
These components are required. Thus, the unit vectors ▲ ▼, ▲ ▼, ▲ ▼ along the target coordinate axes X _T , Y _T , Z _T shown in FIG. And the transformation matrix C _M ^T to the target coordinate axis is Determined by

The components of the matrix of the expression (15) are related to the Euler angles, so that the Euler angles that satisfy the definition of the rotation order may be derived by ordinary calculation.

As an example, the Euler angle is defined as the azimuth angle ψ (Y _M axis), pitch angle θ (X _M axis), and roll angle φ (Z _M axis) with respect to the camera reference coordinates.
If defined by a rotating sequence, if a ij component of C _M ^T and C _ij,
Euler angle is It is obtained by the following equation.

Here, the relationship between the coordinate transformation matrix C _M ^T and the Euler angle is More required.

Also, the position vector from the camera reference point 2a to the target origin 10a is Given by

As described above, the positions in the still images of the targets 101 to 103 are detected from the still images of the target member 10 displayed on the display unit 4, and the arithmetic unit 7 is described above based on the positions of the targets 101 to 103. An operation is performed to determine the direction (unit vector) of each target reference point, and the positional relationship between this and each target reference point (known)
At the time when the target member 10 is turned into a still image.
The position and the posture angle of the target member 10 with respect to the reference point 2a of the camera 2 are obtained.

At the same time, the joint angle of the manipulator 1 at the time when the target member 10 is turned into a still image is input to the arithmetic unit 7 by the joint angle detection sensor 8, and based on the value, the mounting unit (base) of the manipulator 1 is used. The position and posture angle of the camera reference point 2a at the arm tip with respect to 1a are obtained, and the obtained values, and the position and posture angle of the target member 10 with respect to the reference point 2a of the TV camera 2 obtained as described above are obtained. Is synthesized into a manipure
The position and the posture angle of the target member 10 with respect to the base 1a of the table 1 are obtained.

For example, in the case of a manipulator 1 having two degrees of freedom (two joints) as shown in FIG.
2a and the camera reference coordinate axes (X _M , Y _M , Z _M )
The distance L and the target coordinate axes with respect to the camera reference coordinate axis from 2a to the target origin 10a of the target member _{10 (X T, Y T,} Z T) is the attitude of the It is expressed as Here, L is represented by a vector component with respect to the camera coordinate axis, and ，, θ, and φ are Euler angles shown in the above equation (16). The arm length of the manipulator is |
▲ ▼ | = L and | ▲ ▼ | = L, and the joint angles at the time of making the still image are r ₁ and r ₂ as shown in FIG.

At this time, Manipyure - position L of the target origin 10a of the target member 10 with respect to the base 1a of the motor _{1 '= (L X',} L Y ', L Z')
The coordinate axes fixed to the base _{1a (X B, Y B,} Z B) against Is required. Further, the base 1a is fixed and the target coordinate axes (X _T , Y _T , Y _T ) of the target member 10 with respect to the coordinate axes (X _B , Y _B , Z _B )
If the Eulerian angle of Z _T ) is defined in the same manner as in the above equation (16), オ = r ₁ + r ₂ + α + β, θ = 0, φ = 0 (21) Thus, the position and the attitude angle of the marker member 10 (the object 9 to be measured) with respect to the base 1a of the manipulator 1 are obtained.

As described above, the position and orientation measuring device for a moving object includes:
A still image of the target member 10 is displayed on the display unit 4, and a cursor position in the still image for the target 101 to 103 is obtained from the still image to calculate a position and a posture angle of the target member 10 with respect to the TV camera 2. From the joint angle of the manipulator 1 at the time of the detection, the position and the attitude angle of the arm tip with respect to the base 1a of the manipulator 1 with respect to the TV camera 2 are obtained, and these values are combined to obtain the manipulator. The position and the posture angle of the target member 10 with respect to 1 are configured to be calculated.
According to this, since the position and the posture angle of the target member 10 with respect to the base 1a of the manipulator 1 can be obtained, for example, in a state where vibration due to so-called backlash or the like is applied to the joint of the manipulator 1 Since accurate measurement can be performed irrespective of the vibration, the manipulator 1 can be accurately controlled, and highly accurate operation control can be performed, and safety can be improved. According to this, the TV camera 2 can also be used as a television camera for surveillance operation, thereby contributing to simplification of the configuration as much as possible and promotion of miniaturization and weight reduction.

Although the above embodiment has been described using a manipulator having two degrees of freedom for convenience of explanation, the present invention is not limited to the manipulator having the number of degrees of freedom, but the present invention is applicable and the number of degrees of freedom is limited. In the case of an increase, similarly, it can be determined by a forward kinematic analysis of a known manipulator.

In the above embodiment, the target member 10 is used as the target 101.
103103 has been described using an arrangement of approximately triangular vertices, but without being limited to this, at least one target is not located on a straight line connecting the other and the two targets, and It is possible to use a target member having three or more targets arranged to be located in space.

Further, in the above-described embodiment, the case where the spacecraft is used for a manipulator has been described. However, the present invention is not limited to this, and the spacecraft can be configured to measure the position and attitude of the spacecraft with respect to a measurement target. In addition, as a mobile object,
It is also possible to configure so as to measure the position and the attitude of a measurement object such as a car on the ground.

Therefore, it is needless to say that the present invention is not limited to the above-described embodiment, and that various modifications can be made without departing from the scope of the present invention.

[Effects of the Invention] As described above in detail, according to the present invention, there is provided a position and orientation measuring apparatus for a moving body which has a simple configuration, can be reduced in weight, and can realize a reliable measurement. Can be provided.

[Brief description of the drawings]

FIG. 1 is a block diagram showing a position and orientation measuring device for a moving object according to an embodiment of the present invention, FIG. 2 is a diagram showing a target member of FIG. 1 taken out, and FIGS. FIG. 2 is a diagram shown to explain the operation of FIG. 1; 1 ... manipulator, 1a ... base, 2 ... TV camera,
2a: Camera reference point, 3: Signal processing unit, 4: Display unit, 5: Operation unit, 6: Cursor generation unit, 7: Operation unit, 8: Joint angle detection sensor, 9 …… Measurement object, 10
... Marking member, 101 to 103... Target, 10a.

────────────────────────────────────────────────── ─── Continuation of the front page (56) References JP-A-61-129508 (JP, A) JP-A-61-277010 (JP, A) JP-A-61-256207 (JP, A) (58) Field (Int.Cl. ⁷ , DB name) G01B 11/00-11/30

Claims (1)

(57) [Claims] 1. A target member provided on an object to be measured, having a set of three targets, at least one of the targets being arranged not in a straight line connecting two targets but in space. An imaging unit provided on a moving body for imaging the target member; a display unit for displaying an image captured by the imaging unit; and an image for stopping the image of the target member captured by the imaging unit on the display unit. Stationary means; target position detecting means for superimposing a cursor on the target in the still image of the target member displayed on the display means to detect the position of the target member in the still image; Angle detection means for detecting, the position of the target of the target member detected by the target position detection means and the detected value of the angle detection means at the time of capturing a still image of the target member displayed on the display means, Mobile object position and orientation measuring apparatus, characterized by comprising a calculating means for calculating the position and attitude angle of the measurement object relative to the moving object.