JPH06331380A - Method and apparatus for identifying motion of object - Google Patents

Abstract

PURPOSE:To achieve an identification of motion with higher reliability concerning a method and an apparatus for identifying motion which achieves a measurement from information on motion of an object moving in a space. CONSTITUTION:In this method of identifying motion of an object making a uniform linear motion and an isometrical motion, a processing in which the object is photographed to measure a position attitude of the object from an image taken thereof is performed at least at more than three time-series points and a combination of three time points from the more than four time points is selected to determine a motion parameter of the object based on a position attitude data at the three time points. The optional combination of three time points is changed sequentially to obtain a plurality of motion parameters thereby suppressing errors of the motion parameters based on the plurality of motion parameters.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a motion identifying method and a motion identifying apparatus for measuring motion information of an object moving in space.

For example, in order to collect an artificial satellite floating in outer space, it is conceivable to have a space robot or the like perform a work of capturing it. Therefore, in order for an unmanned space robot to approach an artificial satellite autonomously and capture it, it is necessary to measure motion information such as the position and orientation of the artificial satellite and identify the motion.

[0003]

2. Description of the Related Art For example, the present applicant has previously proposed the following method for identifying the motion of an object moving in space. That is, the position and orientation of a target mark attached to a moving object such as an artificial satellite is measured, the data of the position and orientation are obtained for each of three times, and the movement information of the object is calculated based on the data. Hereinafter, this method will be described in detail.

As shown in FIG. 6, an object moving in a space at a constant linear velocity and at a constant angular velocity has three time points i,
At (i + 1) and (i + 2), the target mark attached to the object is photographed, and the position and orientation of the target mark at each time is measured from the photographed data at each time. The position and orientation and velocity of the object are identified by obtaining the translational velocity v of the object, the direction k of the rotation axis of the object and the rotation angle ω from the position and orientation data at these three times by the coordinate transformation matrix.

FIG. 7 shows a device configuration. As shown in the figure, a target mark 11 for recognizing the position and orientation of the artificial satellite 10 as a moving object is attached, and a gripped rod 13 for capturing is provided below the target mark 11. An arm 31 for capturing and a camera 20 for capturing the outside world are attached to a space robot 30 for capturing the artificial satellite 10, image data captured by the camera 20 is processed by an image processing device 40, and an arithmetic processing device is based on the processed data. The motion information is calculated by 50, and the robot controller 60 controls the arm 30 based on the calculated motion information.

The artificial satellite 10 is rotating about its central axis 12 at a constant angular velocity, and is translated at a constant velocity v in the direction indicated by an arrow 14.

On the other hand, a robot 3 for capturing the artificial satellite 10
A holding device 32 for holding the holding rod 13 is attached to the tip of the arm 31 of No. 0. The camera 20 fixed to the arm 31 captures an image of the target mark 11 at regular time intervals and sends image data of each scene to the image processing device 40. The image processing device 40 processes the image data to recognize the position and orientation of the target mark 11. This recognition processing is performed for each of the three scenes at time i, (i + 1), and (i + 2). Based on the position and orientation data of the target mark obtained at these three times, the arithmetic processing unit 50 determines the position and orientation of the artificial satellite 10 and movement information such as the moving speed v and the rotation speed ω to identify the movement of the object, Send to the robot controller 60.

The robot controller 60 controls the space robot 30 so that the camera 20 can track the target mark 11 in the direction of the arm 31 based on the movement information, and further the gripping device 31 can grip the gripping rod 13. Control.

Next, an algorithm for identifying the motion of the artificial satellite from the position and orientation of the target mark 11 will be described. FIG. 8 shows a coordinate system for identifying the motion of the satellite. The reference coordinate system is Co (O, x, y, z). This reference coordinate system is generally located at the position of the camera 20. Also the coordinate system of the target mark 11 with respect to the reference coordinate system ^{_{Co C R (O R, x}} R, y R, z R) and. This target mark coordinate system C _R is represented by the following equation (1) with homogeneous coordinates. In the following explanation, The symbol of represents a matrix.

[0010]

This T represents the position and orientation of the target mark 11. A matrix T ′ of the following equation (2), which is an element in the matrix T, represents the attitude of the coordinate system C _R , that is, the attitude of the target mark 11.

The unit direction vector of the coordinate axes X ^R , Y ^R and Z ^R of the target mark coordinate system C _R is the reference coordinate system C
[n _X , n _y , n _z ], [O _X , O
_y , O _z ], [a _X , a _y , a _z ]. As described above, the matrix T ′ is defined by unit direction vectors [1,0,0], [0,1,0], and [0,0,1] of the reference coordinate system Co at [n _X , n _y , and n _z ], [O _X , O _y , O _z ],
The posture after rotation conversion is shown as [a _X , a _y , a _z ]. At the same time, this rotation conversion is also shown. This rotation conversion can be expressed by the following equation (3) with homogeneous coordinates.

[0013]

[0014] The fourth row of t = matrix T _{[t X, t y, t z} , 1 ] ^T is the position of the origin O ^R of the reference coordinate system Co seen from the coordinate system C _R (i.e. the target mark 11 Position). In the homogeneous coordinates, the three-dimensional position vector is represented by four elements, and the last element is the scale factor. Also, the position t represents the displacement vector of the translational transform from the origin O of the reference coordinate system Co to the origin O ^R target marks coordinate system C _R, the translational transform in homogeneous coordinates by the following formula (4) expressed.

[0015]

From the above coordinate transformation matrix, it can be seen that T = U _T · R _T , where T also represents coordinate transformation. With 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 _Q of the rotation axis 12 of the artificial satellite 10 is set. This coordinate system C _Q can be set to any position on the rotary shaft 12, in this example, the origin a point at which a perpendicular line drawn to the rotary shaft 12 from the origin O ^R target marks coordinate system C _R rotation axis 12 intersects Q is set so that the attitude of the coordinate system C _Q of the rotation axis 12 is the same as the attitude of the target mark 11. That is, assuming that the position vector of the origin Q viewed from the target mark coordinate system C _R is m = [m _X , m _y , m _z , 1] ^T , the coordinate system C of the rotation axis 12 viewed from the target mark coordinate system C _{R. Q} can be expressed by the following equation (5).

[0018]

Since the rotation axis 12 is constant when viewed from the target mark coordinate system C _R , m _X , m _y and m _z are constants. Further, if the position vector of the origin Q viewed from the reference coordinate system Co is g = [g _X , g _y , g _z , 1] ^T , the coordinate system C _{Q of} the rotation axis viewed from the reference coordinate system Co
Can be expressed by the following equation (6).

[0020]

Here, since M in the equation (5) is a coordinate conversion of the target mark coordinate system C _R into the coordinate system C _Q of the rotary shaft 12, the three coordinate systems Co, C _R and C _Q are The relation G = T · M (7) holds. Further, looking only at the position component of equation (1), g (position vector of the origin Q viewed from the reference coordinate system Co) and m (position vector of the origin Q viewed from the target mark coordinate system C _R ) are g = The relational expression Tm (8) holds.

Next, the movement of the artificial satellite 10 will be described with reference to FIG. It is assumed that the position and orientation of the target mark 11 of the artificial satellite 10 is measured for each scene at each time with a fixed time interval. Here, the scene number is i,
Target mark 11 and rotation axis 1 of the i-th scene
The two coordinate systems are represented as ⁱ T and ⁱ G, respectively. Similarly, the origin Q and the position vector g are ⁱ Q, ^{i in} each scene.
Expressed as g. Therefore, the formulas (7) and (8) are ⁱ G = ⁱ T · M (9) ⁱ g = ⁱ T · m (10), respectively.

Here, since the motion of the artificial satellite 10 is assumed to be a constant velocity linear motion and a constant angular velocity motion, the artificial satellite 1
0 rotates about the rotation axis 12 by an angle ω and translates by a constant amount v per sampling time. Therefore, the motion of the object can be described by rotation conversion and translational conversion. A unit direction vector k = [k _X , k passing through the origin
_y , k _z ] rotational transformation R that rotates around the line ω
Can be expressed by the following equation (11).

[0024]

[Equation 1]

Further, the displacement vector v = [v _X , v _y , v
The translational transformation U of _z ] can be expressed by the following equation (12).

[0026]

Here, since it is desired to obtain the motion information of the artificial satellite 10 with respect to the reference coordinate system Co, the unit direction vector k and the displacement vector v of the rotation axis 12 of the artificial satellite 10 are set with respect to the reference coordinate system Co and rotated. Conversion and translational conversion are also performed on the reference coordinate system Co. Then, considering the rotating shaft 12 as the motion of the artificial satellite 10, the relationship between the coordinate system ⁱ G of the rotating shaft 12 and ^{i + 1} G after a certain time is ^{i + 1} G = U · ⁱ D · R · ⁱ D ⁻¹ · ⁱ G (13) Here, ⁱ D represents the translational conversion from the origin O of the reference coordinate system Co to the coordinate origin Q of the coordinate system C _Q of the rotary shaft 12, and is represented by the following equation (14).

[0028]

In other words, in rotating the coordinate system C _Q of the rotating shaft 12, the rotating shaft 12 is translated so as to pass through the origin O of the reference coordinate system Co. Then, it is converted by rotation, returned to the original position, and converted by translation. When converting a certain coordinate system with respect to the reference coordinate system Co, the conversion matrix may be written from the left side of the coordinate matrix. Also, equation (9) and equation (13)
Therefore, the relationship between ⁱ T and ^{i + 1} T is ^{i + 1} T = U · ⁱ D · R · ⁱ D ⁻¹ · ⁱ T (15)

Next, motion identification will be described. The position and orientation of the target mark 11 in three consecutive scenes are ¹ T, ² T, and ³ T, and the following equations (16) and (1)
7) and equation (18).

[0031]

[0032]

[0033]

First, the rotation component is obtained. The rotation component R ^R is obtained from the posture components of the two scenes. That is, ^{i + 1} T ^R = R ^R · ⁱ T (19) When determining the R ^R by substituting ¹ T, ² T ^{to, R R = 2 T R ·} (1 T R) is ^-1.. (20). This R ^R is represented by the following equation (21).

[0035]

The components k _X , k _y and k _z of the unit direction vector k of the rotation axis 12 and the rotation angle ω per sampling time.
Is calculated as follows by comparing with the right side of Expression (11).

[0037] sin .omega = ^{_{[(r 32 -r 23) 2 +}} (r 13 -r 31) 2 + (r 21 -r 12) 2 ] ^{1/2 / 2 ··· (22) cosω} = (r 11 + r ₂₂ + r ₃₃ -1) / 2 (23) ω = atan2 (sin ω, cos ω) (24)

[0038] _{k x = (r 23 -r 32} ) / 2 sinω k y = (r 13 -r 31) / 2 sinω k x = (r 21 -r 12) / 2 sinω ··· (25)

That is, the rotation angle ω of the rotary shaft 12 is given by the equation (24), and the respective components k _X , k _y , k _z of the direction vector k of the rotary shaft 12 are given by the equation (25). As a result, the rotational component of the motion is obtained.

Next, the translation component will be described. Artificial satellite 1
For the translational component of 0, the displacement of the position of the coordinate origin Q of the rotary shaft 12 may be obtained. Since a constant velocity linear motion is performed with the origin Q, the relationship of ⁱ g− ^{i + 1} g = ^{i + 1} g− ^{i + 2} g (26) holds. By substituting the equation (10) into this, ( ⁱ T−2 · ^{i + 1} T + ^{i + 2} T) m = 0 (27).

Substituting ¹ T, ² T, and ³ T into this equation (27), the following equation (28) is obtained. _{(A j1 -2b j1 + c j1} ) m X + (a j2 -2b j2 + c j2) m y + (a j3 -2b j3 + c j3) m z + (a j4 -2b j4 + c j4) = 0 where, j = 1, 2, 3 (28)

From this, three equations for m _x , m _y , m _z are made. One of these is redundant, because this holds for equation (26) for all points on the axis of rotation 12. In this example, m _X, m _y, to determine the m _z, rotation axis 1 from the coordinate origin O ^R of the target mark 11
The condition is that the foot of the perpendicular drawn in 2 is Q. Under this condition, ⁱ p = ⁱ g ⊥k (29) holds, and when the expression (10) is substituted into this, ( ⁱ p + ⁱ T · m) k = 0 (30).

By substituting ⁱ T into this equation (30), the following equation (31) is obtained. _{(A 11 k X + a 21} k y + a 31 k z) m X + (a 12 k X + a 22 k y + a 32 k z) m y + (a 13 k X + a 23 k y + a 33 k z) m z = 0 ... (31)

From equation (28) and equation (31), m
Is required. Since g is obtained from this m and the equation (10), the translational component v is obtained from the following equation (32). v = ^{i + 1} g- ⁱ g (32)

From the above, the rotational component and the translational component are obtained, and the motion of the artificial satellite 10 can be identified.

[0046]

In the above-described motion identification method, the motion parameter of the object is calculated from the position / orientation data at 3 times, but the position / orientation data measured at each time includes an error. Therefore, there is no redundancy only with the data at 3 times, and the calculated motion parameter is affected by the error, resulting in motion identification with low reliability.

The present invention has been made in view of such problems, and an object thereof is to enable more reliable motion identification.

[0048]

1 and 2 are explanatory views of the principle of the present invention. The object motion identification method of the present invention is
As shown in FIG. 1 (A), as one form, there is a method of identifying the motion of an object that is moving at a constant linear velocity or at a constant angular velocity. The process of measuring the position and orientation of an object is performed at least at four or more time-series time points, and an arbitrary combination of three time points is selected from the backs of the four or more time points, and the position and orientation data at the three time points is selected. A motion parameter of the object based on the motion parameter, the combination of the arbitrary three time points is sequentially changed to obtain a plurality of motion parameters, and an error of the motion parameter is suppressed based on the plurality of motion parameters. To do.

The object motion identification method of the present invention is shown in FIG.
As another form, as shown in (B) of the above, there is provided a method of identifying the motion of an object that is moving at a uniform linear velocity or at a constant angular velocity, in which the object is photographed, and the object is photographed from the photographed image. It is characterized in that the processing for measuring the position and orientation is performed at at least four or more time-series time points, and the least squares method is applied to the position and orientation data at the four or more time points to obtain the motion parameter.

The method for identifying the motion of an object according to the present invention is shown in FIG.
(A) in another form, as another form, a method for identifying the motion of an object that is moving at a uniform linear velocity or at a constant angular velocity, which is an image of the object and the object is captured from the captured image. Is performed at at least four time points in time series, and the Kalman filter is applied to the position and orientation data at the four or more time points to obtain the motion parameter.

Further, the method for identifying the motion of an object according to the present invention is shown in FIG.
(B), a motion identification device for identifying the motion of an object that is moving at a uniform linear velocity or at a constant angular velocity, the object being photographed, and the position and orientation of the object is determined from the photographed image. A position / orientation measuring device 101 for performing measurement processing at least at four or more time-series time points, and a position / orientation measuring device 1
Based on the position / orientation data of 4 or more time points obtained in 01, motion identification by a combination of position / orientation data of 3 time points arbitrarily selected, motion by least-squares method for position / orientation data of 4 or more time points An arithmetic processing unit 102 that obtains a motion parameter while suppressing an error by using the identification or the motion identification by the Kalman filter for the position and orientation data at four or more time points.

[0052]

[Function] First, the first motion identification method will be explained.
Motion identification can be calculated from position / orientation data of three scenes. Now, when position / orientation data of four or more scenes can be measured, a plurality of combinations of three scenes for motion identification can be set. If motion identification is performed for each of these combinations, multiple motion parameters can be obtained.
By statistically processing such as averaging them, the error can be suppressed and more accurate motion identification can be performed.

In the second motion identification method, 4
When the position and orientation data of the scene or more is obtained, the square of the error of the motion parameters such as the translation speed and the rotation speed can be minimized under the condition that the object performs the uniform linear motion and the uniform angular velocity motion.

In the third motion identification method, 4
When position / orientation data for a scene or more is obtained, a Kalman filter can be used to sequentially estimate motion parameters from the time-series data and reduce errors.

In the motion identifying apparatus of the present invention which performs the above method, the position / orientation measuring apparatus 101 obtains the position / orientation of an object or a part thereof as time-series data, which is given to the arithmetic processing unit 102 via an interface. . Four
When the position and orientation data at the above time points are obtained, the arithmetic processing unit 102 identifies the motion of the object using a combination of three scenes, a least squares method, or a Kalman filter.

[0056]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of a model to which the motion identification method of the present invention can be applied is shown in FIG. 3 for explaining an embodiment of the present invention with reference to the drawings. In this embodiment, an artificial satellite is used as an object of which motion is to be identified. This object 10
Is moving at a constant linear velocity and at a constant angular velocity, and the object 10
A target mark 11 for position / orientation measurement is attached to the. Further, by measuring the target mark 11 with the camera 20, the position t and the posture T ′ from the camera 20 to the target mark 11 can be obtained. Further, the motion parameters of the object 10 (for example, the direction of the rotation axis of the object, the rotation speed, the translation speed, etc.) are obtained from the time series of the position and orientation data.

FIG. 4 shows the coordinate system of this model example. Based on the assumption that the object 10 is moving at a constant linear velocity and at a constant angular velocity, the target mark 11 is moving at a constant angular velocity around a certain rotary shaft 12, and the rotary shaft 12 is performing a constant linear motion. Then, from the position and orientation T of the target mark 11, the direction of the rotation axis, the position and the rotation speed, and the translation speed are obtained.

As described above, the position / orientation T of the target mark 11 with respect to the reference coordinate system is expressed by the homogeneous coordinate expression.

[0059]

It is expressed as In this matrix,

Represents a posture, and t = [t _X , t _Y , t _z , 1] ^T represents a position. The position / orientation T of the target mark 11 is measured at a constant sampling time, and the position / orientation of the i-th measured target mark 11 is ⁱ T. Similarly, with respect to the position t and the attitude T ′, ⁱ t, ⁱ T ′ Express.

The position of the rotary shaft 12 is at a fixed position when viewed from the target mark coordinate system, and the foot of the perpendicular line drawn from the target mark 11 to the rotary shaft 12 is calculated as the center of gravity Q (in the coordinate system of the rotary shaft). Origin). Target mark coordinate system and the position of the center of gravity Q with respect to the reference coordinate system, respectively m, and g, m = _{[m x, m y, m z} , 1 ] ^T g = _{[g x, g y, g z} , 1 ] ^{Let T.}

At this time, the position ⁱ g of the ⁱ -th center of gravity Q can be expressed as ⁱ g = ⁱ T · m. Also, the direction vector of the rotation axis with respect to the reference coordinate system, k = _{[k x, k y, k z} , 1 ] when is ^T, can be described with ⁱ t = ^{i g⊥k.}

The motion parameters of the object are estimated using the above relationships. Motion parameters of an object are divided into a rotational component and a translational component. Rotational component is determined from the change in the attitude of the target mark 11, the ^{i + 1 T '= R'} · i T '··· (43). Where R is a matrix that represents the rotation,

[0065]

If so, the report vector k of the rotation axis and the rotation angle ω per sampling time can be obtained as follows.

[0067] _{sin ω = [(r 32 -r} 23) 2 + (r 13 -r 31) 2 + (r 21 -r 12) 2] 1/2 / 2 cos ω = (r 11 + r 22 + r 33 - 1) / 2 ω = atan2 ( sinω, cosω) k x = (r 23 -r 32) / 2 sinω k y = (r 13 -r 31) / 2 sinω k x = (r 21 -r 12) / 2 sinω

The translational component is obtained from the change in the position of the rotation axis. Since the center of gravity Q on the axis of rotation makes a uniform linear motion, the relationship of ⁱ g− ^{i + 1} g = ^{i + 1} g− ^{i + 2} g = v (44) holds. Here, v represents the amount of movement of the center of gravity Q per sampling time. This is an expression (44) Equation (41) described above ^{to, (i T-2 i +} 1 T + i + 2 T) m = 0 ··· (45) _{becomes, m x, m y, m} z Equation (45) holds for all points on the rotation axis. Therefore, by using the relation of equation (42),
The position m of the center of gravity Q in the target mark coordinate system can be determined, and the movement amount v of the center of gravity Q can be obtained from the equation (44).

As described above, the motion parameter can be obtained from the position and orientation data of the three scenes. And n
(> 3) If the position and orientation data of the scene is obtained,
First, motion identification is performed from the position / orientation data ⁰ T, ¹ T, ² T, and motion parameters ⁰ k, ⁰ ω, ⁰ m, ⁰ v are obtained. Next, motion identification is performed from the position / orientation data ¹ T, ² T, ³ T, and motion parameters ¹ k, ¹ ω, ¹ m, ¹ v are obtained. Similarly, motion parameters ^n-3 k, ^n-3 ω,
(n-2) parameters up to ^n-3 m and ^n-3 v are obtained. By averaging these, accurate parameters can be obtained.

FIG. 5 shows an example of the structure of a motion identification device for performing the above-described motion identification. In the figure, 20 is a camera for photographing a moving object, 21 is an image processing apparatus for extracting features of an image captured by the camera 20 by image processing, and 22 is a position and orientation for calculating the position and orientation of a target mark based on the extracted features. An arithmetic device, 23 is a motion identification arithmetic device that calculates motion parameters of an object based on a plurality of position and orientation data calculated by the position and orientation calculation device 22, and 24 is a host CP
U and 25 are timers for generating sampling times at regular intervals.

In this apparatus, the target mark 11 attached to the object whose movement is desired to be measured is photographed by the camera 20, and the photographed image is feature-extracted by the image processing device 21 to extract the feature-marked target mark. The position / orientation calculation device 22 calculates the position / orientation of the object based on the above data, and further, this position / orientation measurement is performed at every constant sampling time generated by the timer 25, and based on the plurality of position / orientation data obtained thereby. The motion identification calculation device 23 determines the motion parameter of the object.

Various modifications are possible in carrying out the present invention. For example, in the above-described embodiment, the motion parameter of the object is obtained by calculation from the position and orientation data of the object in a plurality of scenes, but the present invention is not limited to this. For example, the motion using the least square method or the Kalman filter is used. It is also possible to find the parameters. Less than,
These modifications will be described.

First, a modified method using the least squares method will be described. The method of this modification estimates the motion parameter of the object by the least square method when the position and orientation data of three or more scenes are obtained. In the method using the least squares method, the same coordinate system as that in the above-described embodiment is used, and the following evaluation function is created in order to accurately obtain the rotation speed and the translation speed. In the equation, θ represents the posture of the target mark, and i represents the scene number.

[0074] _{Jθ = Σ (θ i -θ '} ) 2 However, Σ is integral ... (46) from i = 0 to (n-2) Jg = Σ (g i -g') 2 However, Σ Is the integral from i = 0 to (n−2) (47) where θ ′ = ω _i + θ ₀ (48) g ′ = v _i + g ₀ (49) . In order to obtain the motion parameter using the least squares method, a known calculation algorithm of the least squares method is used, and ω, θ ₀ , which minimizes the above Jθ, Jg,
It suffices to determine v and g ₀ .

Next, a modified method using the Kalman filter will be described. The Kalman filter is a method for estimating the state variable of a system online from the observed value when the system structure is known. In the method using the Kalman filter, motion parameters are estimated from a large number of position and orientation data by the Kalman filter. That is, when the position and orientation data of four or more scenes are obtained, the Kalman filter is used to sequentially estimate the motion parameter from the time-series data, and the error is reduced.

The method of using this Kalman filter will be described in detail below. Now, assume that the state variable x and the observed value y are x = [g _X , g _y , g _z , v _X , v _y , v _z , m _X , m, respectively.
_{y, m z, k X,} k y, k z, θ, ω ] ^T y = _{[t X, t y, t z} , k X, k y, k z, and theta] ^T, the state equation, x _{i + 1} = Ax _i + Γw _i (50) y _i = f (x _i ) + v (51) Now, from the condition that the center of gravity moves at a constant linear velocity and a constant angular velocity,

[0077]

[0078]

It becomes Also, for f (x),

[0080]

It can be described as However, b () is a function representing the position from the center of gravity to the target mark.

Here, w _i and v are system noise and observation noise, respectively, and the average value and variance are ε [w _i ] = 0 ε [w _i w _j ^T ] = W (i = j) or 0 (I ≠ 0) ... (52) ε [v _i ] = 0 ε [v _i v _j ^T ] = V (i = j) or 0 (i ≠ 0) ... (53) Independently Gaussian white noise. The covariance matrices W and V are semidefinite and positive definite matrices, respectively. Here, ε [·] represents an expected value.

Further, these noises satisfy ε [x ₀ ] = x ₀ ^* ε [(x ₀ −x ₀ ^* ) (x ₀ −x ₀ ^* ) ^T ] = Σ ₀ (54) It is also assumed that the initial state x _{0 is} independent and Σ ₀ is a positive definite matrix.

At this time, the state x _{i at} time i and its estimated value x based on the observation data y ₀ , y ₁ , ... Y _i-1
The optimum estimated value that minimizes the expected norm of the difference with _i J _i = ε [| x _i −x _i | ² ] (55) is given by the following filter.

X _{i + 1} = A x _i + K _i (y _i −C _i x _i ) (56) where C _i = δf (x _i ) / δx (57) W = ΓWΓ ^T・ ・ ・ (58) K _i = AΣ _i C _i ^T (W + C _i Σ _i C _i ^T ) ⁻¹ C _i Σ _i A ^T・ ・ ・ (59) Σ _{i + 1} = V + A Σ _i A ^T −A Σ _i C _i ^T (W + C _i Σ _i C _i ^T ) ^-1 C _i Σ _i A ^T (60)

The gain K _i of the filter can be sequentially calculated by using Σ ₀ in the initial state and equations (59) and (60).

[0087]

As described above, according to the present invention, more reliable motion identification can be performed.

[Brief description of drawings]

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

FIG. 2 is a diagram illustrating the principle of the present invention.

FIG. 3 is a diagram showing an example of a model for performing a motion identification method according to an embodiment of the present invention.

FIG. 4 is a diagram illustrating a coordinate system applied to a model example.

FIG. 5 is a diagram showing a configuration example of an apparatus for performing the method of the embodiment.

FIG. 6 is a diagram illustrating a method of identifying the motion of an artificial satellite from three scenes.

FIG. 7 is a diagram illustrating a coordinate system for identifying the motion of an artificial satellite.

FIG. 8 is a diagram showing a configuration example of a motion identification device.

[Explanation of symbols]

 10 Artificial Satellite 11 Target Mark 12 Rotational Axis 13 Gripping Rod 20 Camera 21 Image Processing Device 22 Position and Attitude Calculator 23 Motion Identification Calculator 24 Host CPU 25 Timer 30 Robot 31 Arm 32 Grasping Device 40 Image Processing Device 50 Computing Processor 60 Robot controller

Claims (4)

[Claims] 1. A method for identifying the motion of an object that is moving at a uniform linear velocity or at a constant angular velocity, which comprises at least four or more processes of photographing an object and measuring the position and orientation of the object from the photographed image. At time points in time series, selecting an arbitrary combination of three time points from among the four or more time points, and obtaining the motion parameter of the object based on the position and orientation data at the three time points. A method for identifying a motion of an object, which comprises sequentially changing combinations of three time points to obtain a plurality of motion parameters, and suppressing errors of the motion parameters based on the plurality of motion parameters. 2. A method for identifying the motion of an object that is moving at a constant linear velocity or at a constant angular velocity, wherein at least four or more processes are performed for photographing the object and measuring the position and orientation of the object from the photographed image. The method for identifying a motion of an object, characterized in that the motion parameter is obtained by applying the least squares method to the position and orientation data at the four or more time points. 3. A method for identifying the motion of an object that is moving at a constant linear velocity or at a constant angular velocity, wherein at least four or more processes are performed for photographing the object and measuring the position and orientation of the object from the photographed image. The time-sequential time point of, and a motion identification method of an object, characterized in that a Kalman filter is applied to the position and orientation data at the four or more time points to obtain motion parameters. 4. A motion identification device for identifying the motion of an object that is moving at a uniform linear velocity or at a constant angular velocity, wherein at least a process of photographing the object and measuring the position and orientation of the object from the photographed image. A position / orientation measuring device (101) for performing time points of 4 or more time series, and position / orientation of 3 time points arbitrarily selected based on position / orientation data of 4 or more time points obtained by the position / orientation measuring device. Motion identification by combination of data, motion identification by least squares method for position / orientation data at 4 or more time points, or motion identification by Kalman filter for position / orientation data at 4 or more time points while suppressing errors An apparatus for identifying motion of an object, comprising an arithmetic processing unit (102) for obtaining a parameter.