JP3086550B2 - Method and apparatus for identifying motion of moving body - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a method and an apparatus for identifying the motion of a moving object for identifying the motion of an object moving in space, and more particularly to the capture of an artificial satellite floating in space. TECHNICAL FIELD The present invention relates to a method for identifying a motion of a moving body and a device for identifying the motion of a moving body used in the method.

[0002] Artificial satellites need to be temporarily recovered for repair or the like. For this purpose, it is necessary to capture satellites floating in outer space.

[0003]

2. Description of the Related Art At present, the work of recovering suspended matters such as artificial satellites in outer space is performed by human extravehicular activities. But this is very dangerous. For this purpose, a remote operation from the ground using a space robot has been considered.

[0004]

However, since there is a long communication delay between the ground and the satellite, it is extremely difficult to capture a moving satellite. For this reason, an autonomous capture operation by a space robot is desired. In order to perform an autonomous capture operation, it is necessary to identify its movement in order to approach the satellite. However, in the measurement in outer space, resources are limited, and it is necessary to identify the motion of the artificial satellite with few resources.

[0005] The present invention has been made in view of the above points, and it is an object of the present invention to provide a method for identifying a movement of a moving body that can reliably identify the movement of the moving body from limited resources.

It is another object of the present invention to provide an apparatus for identifying the movement of a moving body which can identify the movement of the moving body in a short time.

[0007]

The motion of the moving body of the present invention
In the fixed method, as shown in FIG.
Three times T of a moving body moving and moving at a constant angular velocity
_i, T_{i + 1}, T_{i + 2}Target mark 11 of moving body
And the position and orientation of this target mark 11
Recognize. And from these positions and postures,
The velocity d of the body, the direction k of the rotating shaft 12 of the moving body, and the
The rotational position θ is calculated by a coordinate transformation matrix,
Identify body position, posture and speed.

[0008] Further, a motion identification device for a moving body according to the present invention comprises:
As shown in FIG. 2, a target mark 11 provided on the moving body 10, a camera 20 for imaging the target mark 11, and image information of the imaging information from the camera 20 are processed to output position information of the target mark 11. From the position information, the position, posture and speed of the target mark 11 at a plurality of times,
An arithmetic processing unit 50 for calculating the velocity d of the moving body, the direction k of the rotating shaft 12 of the moving body, and the rotational position θ of the moving body from a position and a posture by using a coordinate transformation matrix.

[0009]

The target mark 11 of the moving object at three times T _i , T _{i + 1} , and T _{i + 2} is imaged, and the target marks 11 at three positions are obtained from the three image data.
Recognize the position and orientation of. Then, from the data of these three positions and postures, the speed d of the moving body, the direction k of the rotating shaft 12 of the moving body, and the rotational position θ of the moving body are calculated and obtained by the coordinate conversion matrix using a coordinate conversion matrix. , The position, posture and speed of the moving body are identified.

[0010] The camera 20 captures an image of the target mark 11 fixed to the moving body 10 and sends the captured data to the image processing device 40. The image processing device 40 performs image processing on the captured data to convert the image data into position data.
Send to The arithmetic processing unit 50 arithmetically processes the position data to obtain the velocity d of the moving body, the direction k of the rotating shaft 12, and the rotational position θ. Thereby, the position, posture, and speed of the moving body are identified.

[0011]

An embodiment of the present invention will be described below with reference to the drawings. FIG. 2 is a diagram showing the entire configuration of the motion identification apparatus for a moving body according to the present invention. These are composed of an artificial satellite 10, a robot 30 for capturing the artificial satellite 10, a camera 20 for recognizing the artificial satellite 10, an image processing device 40, an arithmetic processing device 50, and a robot control device 60 for controlling the robot. I have.

The artificial satellite 10 has a position of the artificial satellite 10,
A target mark 11 for recognizing the posture is fixed on the front surface. Further, below the target mark 11, a graspable rod 13 for capturing an artificial satellite is provided.

The artificial satellite 10 is moving at a constant speed in a direction indicated by an arrow 14 while rotating at a constant angular speed around its central axis 12. On the other hand, at the tip of the arm 31 of the robot 30 that captures the artificial satellite 10,
3 is fixed. Also,
The camera 20 is fixed on the arm 31. The camera 20 captures an image of the target mark 11 at regular time intervals and sends the captured data to the image processing device 40. The image processing device 40 performs image processing on the captured data to recognize the position and orientation of the target mark 11. Further, the position and posture of the plurality of target marks 11 are calculated, and the movement information such as the position, posture and moving speed and rotation speed of the artificial satellite 10 is sent to the robot controller 60. Robot controller 6
0 controls the robot 30 from the movement information so that the direction of the arm 31, that is, the camera 20 can track the target mark 11. Further, the robot is controlled by the grip device 31 so that the grip bar 13 can be gripped.

Next, an algorithm for identifying the motion of the artificial satellite 10 from the position and attitude of the target mark 11 will be described. FIG. 3 shows a coordinate system for identifying the motion of the satellite 10. Set the reference coordinate system to CO (O, X, Y, Z)
However, the reference coordinate system Tc is generally based on the camera 20. Furthermore, the coordinate system of the target mark 11 with respect to the reference coordinate system ^{Tc CR (O R, X R} , Y R, Z R) and,
The coordinate system CR is a homogeneous coordinate and is represented by equation (1).

[0015]

(Equation 1)

A matrix T ^N represented by the equation (2) consisting of the elements in the matrix represents the orientation of the coordinate system T.

[0017]

(Equation 2)

Coordinate axis X of coordinate system CR^R, Y^R, Z^RSimply
The position direction vectors are respectively defined with respect to the reference coordinate system CO.
[N_x, N_y, N_z], [O_x, O_y, O_z],
[A_x, A _y, A_z] Is shown. Thus, the matrix T
^NIs a unit direction vector [1, 0,
0], [0, 1, 0], and [0, 0, 1]
_x, N _y, N_z], [O_x, O_y, O_z], [A_x, A
_y, A_z] Shows the posture after rotation conversion. Also,
Sometimes this rotation transformation is also represented. This rotation transformation is
It can be represented by the following equation (3).

[0019]

(Equation 3)

[0020] The fourth column of p = matrix T [px, py, pz] ^T is the origin of the coordinate system CR in the reference coordinate system CO position O ^R
Indicates the position of. (In the homogeneous coordinates, the three-dimensional position vector is represented by four elements, and the last element is a scale factor.) This p is from the origin O of the reference coordinate system CO to the coordinate system CR of the target mark 11. It represents the displacement vector of the translational transform to the origin O ^R, the parallel conversion is expressed by the following equation (4) in homogeneous coordinates.

[0021]

(Equation 4)

Further, the above coordinate transformation matrix, _T = U T R T becomes, T is seen that represent the coordinate transformation. In such homogeneous coordinates, the position and orientation, the coordinate system, and the coordinate transformation are expressed in the same form.

Next, the coordinate system C of the rotation axis 12 of the artificial satellite 10
Set Q. This coordinate system CQ can be set to any position on the rotary shaft 12, in this embodiment, a point where a perpendicular line drawn to the rotary shaft 12 from the coordinate origin O ^R of the coordinate system CR target marks rotary shaft 12 intersect Origin Q and rotation axis 12
Is set to be the same as the position of the target mark 11 in the coordinate system CQ. That is, the position vector of the origin Q as viewed from the coordinate system CR target marks 11, m = [m _x, m _y, m _z] When ^{T, the} coordinate of the rotary shaft 12 as viewed from the coordinate system CR target marks 11 The system CQ can be expressed by the following equation (5).

[0024]

(Equation 5)

It should be noted that the rotation axis 12 is the target mark 11
Since it is constant as viewed from the coordinate system _{CR, m x, m y,} m
_z is a constant. Also, the origin Q viewed from the reference coordinate system CO
Let g = [g _x , g _y , g _z ] ^T be the position vector of the rotation axis coordinate system CQ viewed from the reference coordinate system CO.
Can be expressed by the following equation (6).

[0026]

(Equation 6)

Here, since M is also a coordinate transformation of the coordinate system CR of the target mark 11 to the coordinate system CQ of the rotation axis 12, the three coordinate systems CO, CR, and CQ have:

[0028]

The following relationship holds: G = TM─── (7) Also, looking only at the position components of the equation (1), g and m are as follows.

[0029]

The relational expression g = Tmｍ (8) holds.

Next, the motion of the artificial satellite 10 will be described. This will be described with reference to FIG. The position and orientation of the target mark 11 of the artificial satellite 10 are measured from scenes at fixed time intervals. Then, the scene number is set to i, and the coordinate systems of the target mark 11 and the rotation axis 12 of the i-th scene are set to Ti and Gi, respectively. Similarly, the origin Q and the coordinate g are set to Qi and gi in each scene. Therefore, equations (7) and (8) are

[0031]

Gi = TiM─── (9)

[0032]

Gi = Tim─── (10)

Here, the motion of the artificial satellite 10 is assumed to be a constant velocity linear motion and a constant angular velocity motion.
Rotates about the rotation axis 12 by a certain angle θ and translates by a certain amount d during a certain time. Therefore, the motion of the object can be described by a rotation transformation and a translation transformation. Go through the origin,
A rotation transformation R that rotates by an angle θ about a straight line of a unit direction vector k = [k _x , k _y , k _z ] is represented by the following equation (11).
BR>.

[0034]

[Equation 11]

The displacement vector d = [d _x , d _y , d
_z ] can be expressed by the following equation (12).

[0036]

(Equation 12)

Now, the satellite 10 with respect to the reference coordinate system CO
Of the satellite 10
The unit direction vector k of the axis 12 and the displacement vector d are reference
Set for the coordinate system CO, rotation and translational conversion are also standard
This is performed for the coordinate system CO. And the movement of the artificial satellite 10
Assuming the rotation axis 12 as _i
And G after a certain time_{i + 1}The relationship is

[0038]

G _{i + 1} = UD _i RD _i ^-1 G _i ─── (13) Here D _i is the origin O of the reference coordinate system CO represents the translational transform into coordinate origin Q of the coordinate system CQ of the rotary shaft 12, is expressed by the following equation (14).

[0039]

[Equation 14]

That is, in order to rotationally transform the coordinate system CQ of the rotational axis 12, the rotational axis 12 is translated so as to pass through the origin O of the reference coordinate system CO. Then, the rotation is converted, the position is returned to the original position, and the translation is performed. When a certain coordinate system is converted with respect to the reference coordinate system CO, the conversion matrix only has to be left from the coordinate matrix. (9), (13)
From the equation, the relationship between T _i and T _{i + 1} is

[0041]

T _{i + 1} = UD _i R D _i ⁻¹ T _i ─── (15)

Next, identification of movement will be described. The positions and orientations of the target mark 11 in three consecutive scenes are defined as T ₁ , T ₂ , and T ₃ , and the following (16), (1)
7) and (18).

[0043]

(Equation 16)

[0044]

[Equation 17]

[0045]

(Equation 18)

First, a rotation component is obtained. The rotation component R ^R is obtained from the posture components of the two scenes.

[0047]

In Equation 19] _{^{T R i + 1 = R R}} T i ─── (19) which, by substituting T _1, T _2, obtains the R ^R.

[0048]

R ^R = T ^R ₂ (T ^R ₁ ) ^-1 (20) This R ^R is represented by the following equation (21):

[0049]

(Equation 21)

The components k _x , k _y , and k _z of the unit direction vector k of the rotation axis 12 and the rotation angle θ are obtained as follows in comparison with the right side of the equation (11).

[0051]

(Equation 22)

[0052]

(Equation 23)

[0053]

(Equation 24)

[0054]

(Equation 25)

[0055] That is, the rotation angle of the rotary shaft 12 by the equation (24) theta is, each component k _x direction vector k of the rotary shaft 12 by equation (25), k _y, k _z is given. This determines the rotational component of the motion.

Next, the translation component will be described. Artificial satellite 1
For the translation component of 0, the displacement at the position of the coordinate origin Q of the rotating shaft 12 may be obtained. Since the origin Q moves linearly at a constant speed,

[0057]

The following relationship holds: g _i −g _{i + 1} = g _{i + 1} −g _{i + 2} (26) Substituting equation (10) into this,

[0058]

(T _i −2T _{i + 1} + T _{i + 2} ) m = 0─── (27) By substituting T ₁ , T ₂ , and T ₃ into this, the following equation (28) is obtained.

[0059]

[Equation 28]

[0060] Now, m _x, m _y, but it is three equations for m _z, 1 single is redundant. This is given by equation (2)
6) holds for all points on the rotating shaft 12. m _x, m _y, to determine the m _z, foot perpendicular drawn to the rotary shaft 12 from the coordinate origin O ^R of the target mark 11 gave a condition that it is Q. With this condition,

[0061]

The following equation holds: p _i −g _i ⊥k ─── (29)

[0062]

[Number 30] the _{(p i -T i m) k} = 0 ─── (30). By substituting T ₁ into this, the following equation (31) is obtained.

[0063]

(Equation 31)

From this, m is obtained from equations (28) and (31). Since g is obtained from m and equation (10), the translation component d is obtained from the following equation (32).

[0065]

D = g _{i + 1} −g _i ─── (32) From the above, the rotation component and the translation component are obtained, and the motion of the artificial satellite 10 can be identified.

Next, a new method for estimating the scene of the artificial satellite 10 will be described. The position and orientation of the target mark 11 in the fourth scene in FIG. 1 can be obtained from Expression (15). Expression (15) is summarized as the following expression (33).

[0067]

[Equation 33]

That is, if i = 3, the fourth sea
Position T of the target mark 11 _{i + 1}Can be estimated
Moreover, the position and attitude of the satellite 10 can be estimated.
You. Next, the processing flow of the arithmetic processing unit 50 of FIG.
State. FIG. 4 shows the flow of the arithmetic processing of the arithmetic processing unit 50.
FIG. In the figure, following S
Numerical values indicate step numbers. [S1] n = 1 is set. [S2] Measure the position and orientation of the target mark 11
You. [S3] Replace n with n + 1. [S4] Check whether n is larger than 3, and if it is larger, proceed to S5,
If not, return to S2.

That is, the positions and orientations of the target marks 11 for three scenes are measured in steps S1 to S4. [S5] As described above, the rotational position θ of the rotary shaft 12, the components of the direction vector k, and the velocity vector d are obtained from the data on the position and orientation of the target mark 11 for three scenes. As a result, the position, attitude and speed, that is, the motion of the artificial satellite 10 are identified. [S6] New target mark 11 for the next scene
Is measured, and the motion of the artificial satellite 10 is identified from data of two past scenes and one new scene.

In the above description, an artificial satellite having a target mark has been described as an example. However, the present invention can be applied to floating objects other than the artificial satellite if their positions and attitudes can be measured directly or indirectly. it can.

[0071]

As described above, according to the present invention, the position, posture and speed of the moving object are identified from the position and posture of the target mark, so that the robot or the like can approach the artificial satellite accurately. It becomes possible to capture artificial satellites and the like.

Further, since the target mark is imaged by the camera, the image is processed, the position information is obtained, and the motion of the moving object is identified from the position information. , And the capture operation can be performed.

[Brief description of the drawings]

FIG. 1 is a principle diagram of the present invention.

FIG. 2 is a diagram showing an entire configuration of a moving object motion identification device of the present invention.

FIG. 3 is a diagram showing a coordinate system for identifying a motion of an artificial satellite.

FIG. 4 is a flowchart illustrating a flow of a calculation process of the calculation processing device.

[Explanation of symbols]

 Reference Signs List 10 artificial satellite 11 target mark 12 rotation axis 20 camera 30 robot 40 image processing device 50 arithmetic processing device 60 robot control device CR coordinate system of target mark CO reference coordinate system CQ coordinate system of rotation axis

──────────────────────────────────────────────────続 き Continued on the front page (58) Fields surveyed (Int. Cl. ⁷ , DB name) G01B 11/00-11/30 B25J 19/00-19/04 G01S 7/48 G06T 7/20

Claims (6)

(57) [Claims] 1. A method for identifying a position, a posture, and a velocity of a moving body that linearly moves in a space at a constant velocity and moves at a constant angular velocity, wherein a target of the moving body at three times is specified. Mark (1
1) is imaged, and the position and orientation of the target mark (11) are recognized. From the position and orientation, the speed (d) of the moving body and the direction (k) of the rotation axis (12) of the moving body are determined. And a rotational position (θ) of the moving body is calculated by a coordinate transformation matrix, and a position, a posture, and a velocity of the moving body are identified. 2. The method according to claim 1, further comprising adding a position and a posture of the target mark at a new time, and continuously identifying a position, a posture and a velocity of the moving body. A method for identifying the motion of a moving body. 3. The motion identification of a moving body according to claim 1, wherein the position, the posture, and the speed of the moving body after a certain time are estimated from the position and the posture of the target mark at the three times. Method. 4. A motion identification device for a moving body that linearly moves in a space at a constant speed and identifies a position, a posture, and a speed of the moving body that is moving at a constant angular velocity, wherein the moving body is provided on the moving body (10). Target mark (1
1) and a camera (2) for imaging the target mark (11).
0), an image processing device (40) that performs image processing of image pickup information from the camera and outputs position information of the target mark (11), and, based on the position information, the position of the target mark at a plurality of times. Recognizing the position, posture and speed, the speed (d) of the moving body and the rotation axis (1
An arithmetic processing unit (5) for calculating the direction (k) and the rotational position (θ) of the moving body by using a coordinate transformation matrix.
0) and a motion identification device for a moving body, comprising: 5. The moving body according to claim 4, further comprising: a robot controller that receives the position, posture, and speed information of the moving body and controls the robot. Exercise identification device. 6. A device to be gripped (1) is provided on said moving body (10).
3) The apparatus for identifying a motion of a moving body according to claim 1, wherein the robot (10) has a gripping device (32) for gripping the moving body at a tip of an arm (31). .