JPH0719818A - Three-dimensional movement predicting device - Google Patents

Three-dimensional movement predicting device

Info

Publication number
JPH0719818A
JPH0719818A JP18677893A JP18677893A JPH0719818A JP H0719818 A JPH0719818 A JP H0719818A JP 18677893 A JP18677893 A JP 18677893A JP 18677893 A JP18677893 A JP 18677893A JP H0719818 A JPH0719818 A JP H0719818A
Authority
JP
Japan
Prior art keywords
measured
equation
dimensional
motion
measured object
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
JP18677893A
Other languages
Japanese (ja)
Inventor
Yutaka Uchimura
裕 内村
Satoru Miura
悟 三浦
Motohisa Hirose
素久 廣瀬
Kazuo Nakazawa
和夫 中沢
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Kajima Corp
Original Assignee
Kajima Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Kajima Corp filed Critical Kajima Corp
Priority to JP18677893A priority Critical patent/JPH0719818A/en
Publication of JPH0719818A publication Critical patent/JPH0719818A/en
Pending legal-status Critical Current

Links

Abstract

PURPOSE:To estimate a movement parameter of an object to be measured that vibrates singly and predict a future position of the object. CONSTITUTION:The positional data of an object 1 to be measured that vibrates singly are analyzed by a range finder 5. A movement parameter estimating part 9 estimates the movement parameter of the object 1 and a position predicting part 11 predicts a future position of the object on the basis of the estimated movement parameter. A robot 13 holds the object 1 with a manipulator 15 on the basis of the future positional information for the object 1 that is obtained from the part 11.

Description

【発明の詳細な説明】Detailed Description of the Invention

【0001】[0001]

【産業上の利用分野】本発明は、単振動を行う被計測体
の3次元位置を予測し、予測された3次元位置情報を、
被計測体を把持するロボットに供与する3次元運動予測
装置に関するものである。
BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention predicts the three-dimensional position of an object to be subjected to simple vibration and outputs the predicted three-dimensional position information.
The present invention relates to a three-dimensional motion prediction device that is provided to a robot that holds a measured object.

【0002】[0002]

【従来の技術】従来、動画像から動物体の運動パラメー
タを推定する手法として2つの手法がある。第1の手法
は2段階推定法と呼ばれ、まず第1段階で動物体の運動
前後の画像間でオプティカルフロー、あるいはフレーム
間対応を求め、第2段階でオプティカルフローから画像
中の動物体の運動パラメータを推定するものである。第
2の手法は直接推定法と呼ばれる手法で、オプティカル
フローを求めずに画像の濃度変化だけから直接に運動パ
ラメータを推定する手法である。
2. Description of the Related Art Conventionally, there are two methods for estimating motion parameters of a moving object from moving images. The first method is called the two-step estimation method. First, in the first step, the optical flow between frames before and after the motion of the moving object or the correspondence between frames is obtained, and in the second step, the optical flow of the moving object in the image is calculated. The motion parameters are estimated. The second method is a method called a direct estimation method, which is a method of directly estimating a motion parameter only from a change in image density without obtaining an optical flow.

【0003】[0003]

【発明が解決しようとする課題】しかしながら、従来の
手法では、2次元情報である画像情報から3次元運動の
解析を行うが、距離情報が知られていない場合、3次元
運動の解析を行うことは困難であった。また元来、画像
のごくわずかな変動を仮定しているので3次元運動を高
い精度で推定することは困難であった。
However, in the conventional method, the three-dimensional motion is analyzed from the image information which is the two-dimensional information, but when the distance information is not known, the three-dimensional motion is analyzed. Was difficult. Further, since it is assumed that the image slightly changes, it is difficult to estimate the three-dimensional motion with high accuracy.

【0004】「山本正信“動画像と距離情報の併用によ
る3次元運動パラメータの直接的推定法”電子情報通信
学会論文集 J68−D、4、pp.562−569
(1985/4)」には距離情報を併用して運動パラメ
ータを動画像から直接推定する方法が開示されている。
しかしながらこの方法では、物体の運動を等速運動と仮
定しているので、このような運動を行わない対象には適
応できない。
"Masanobu Yamamoto," Direct estimation method of three-dimensional motion parameter by using moving image and distance information ", IEICE Transactions J68-D, 4, pp. 562-569.
(1985/4) ”discloses a method of directly estimating a motion parameter from a moving image together with distance information.
However, in this method, the motion of the object is assumed to be a constant velocity motion, and therefore it cannot be applied to an object that does not perform such motion.

【0005】本発明は、このような問題に鑑みてなされ
たもので、その目的とするところは、振り子のようなそ
の運動を単振動と仮定できる運動を行う物体を対象と
し、3次元計測により得られた被計測体の位置の計測情
報から被計測体の運動パラメータを推定し、被計測体の
将来の位置を予測する3次元運動予測装置を提供するこ
とにある。
The present invention has been made in view of the above problems, and an object thereof is to measure an object, such as a pendulum, which can be assumed to be a simple vibration by a three-dimensional measurement. It is an object of the present invention to provide a three-dimensional motion prediction device that estimates a motion parameter of a measured object from the obtained measurement information of the position of the measured object and predicts a future position of the measured object.

【0006】[0006]

【課題を解決するための手段】前述した目的を達成する
ために本発明は、単振動を行う被計測体の3次元位置を
解析する解析手段と、前記解析手段によって解析された
前記被計測体の3次元位置から前記被計測体の運動パラ
メータを推定する手段と、推定された前記運動パラメー
タを基にして前記被計測体の将来の位置を予測する手段
と、を具備する3次元運動予測装置である。
In order to achieve the above-mentioned object, the present invention provides an analyzing means for analyzing the three-dimensional position of an object to be subjected to simple vibration, and the object to be analyzed analyzed by the analyzing means. A three-dimensional motion prediction apparatus comprising: means for estimating a motion parameter of the measured object from the three-dimensional position; and means for predicting a future position of the measured object based on the estimated motion parameter. Is.

【0007】[0007]

【作用】本発明では、単振動を行う被計測体の3次元位
置を解析し、解析された被計測体の3次元位置から被計
測体の運動パラメータを推定し、推定された運動パラメ
ータを基にして被計測体の将来の位置を予測する。
According to the present invention, the three-dimensional position of the object to be subjected to simple vibration is analyzed, the motion parameter of the object to be measured is estimated from the analyzed three-dimensional position of the object to be measured, and the estimated motion parameter is used as a basis. Then, the future position of the measured object is predicted.

【0008】[0008]

【実施例】以下、図面に基づいて本発明の実施例を詳細
に説明する。図1は、本発明の1実施例に係る3次元運
動予測装置を用いたシステムの概略構成を示す図であ
る。図1において1は単振動(振り子運動)を行う被計
測体であり、3は3次元運動予測装置である。3次元運
動予測装置3はレンジファインダ5、運動パラメータ推
定部9、位置予測部11からなる。レンジファインダ5
は被計測体1を撮像し、撮像された画像を基にし被計測
体1の3次元位置を求める。
Embodiments of the present invention will now be described in detail with reference to the drawings. FIG. 1 is a diagram showing a schematic configuration of a system using a three-dimensional motion prediction device according to one embodiment of the present invention. In FIG. 1, 1 is a measured object that performs a simple vibration (pendulum motion), and 3 is a three-dimensional motion prediction device. The three-dimensional motion prediction device 3 includes a range finder 5, a motion parameter estimation unit 9, and a position prediction unit 11. Range finder 5
Takes an image of the object to be measured 1 and obtains the three-dimensional position of the object to be measured 1 based on the taken image.

【0009】図2はレンジファインダ5と被計測体1の
関係を示すものである。図2においてX、Yは絶対座標
系、Xr 、Yr はレンジファインダ5の座標系、Xp
pは被計測体1の座標系を表わす。ただしいずれの座
標系においてもZ軸は紙面に垂直方向であり、図示を省
略している。
FIG. 2 shows the relationship between the range finder 5 and the object to be measured 1. In FIG. 2, X and Y are absolute coordinate systems, X r and Y r are coordinate systems of the range finder 5, X p ,
Y p represents the coordinate system of the measured object 1. However, in any coordinate system, the Z axis is in the direction perpendicular to the paper surface and is not shown.

【0010】被計測体1が単振動を行っている場合、被
計測体1の重心PのXp 軸座標成分の時刻歴は図3に示
すようなものとなる。しかしながら、3次元計測の計測
処理に要する時間を考慮すると、得られる計測点は図3
中の黒丸点のような離散的なデータとなる。
When the measured object 1 is simply vibrating, the time history of the X p axis coordinate component of the center of gravity P of the measured object 1 is as shown in FIG. However, considering the time required for the measurement processing of the three-dimensional measurement, the obtained measurement points are shown in FIG.
It becomes discrete data like a black dot inside.

【0011】運動パラメータ推定部9は、このような時
間的に離散した重心Pの座標データから重心Pの運動パ
ラメータを推定する。位置予測部11は運動パラメータ
推定部9で得られた運動パラメータを基にして未来の時
刻Tf における重心Pの座標値(Xf ,Yf ,Zf )を
予測する。
The motion parameter estimation unit 9 estimates the motion parameter of the center of gravity P from the coordinate data of the center of gravity P discrete in time. The position prediction unit 11 predicts the coordinate value (X f , Y f , Z f ) of the center of gravity P at the future time T f based on the motion parameter obtained by the motion parameter estimation unit 9.

【0012】被計測体1の将来の位置座標は、ロボット
13に入力され、ロボット13のマニュピュレータ15
により被計測体1が把持される。
The future position coordinates of the object to be measured 1 are input to the robot 13, and the manipulator 15 of the robot 13 is used.
The object to be measured 1 is gripped by.

【0013】次に運動パラメータ推定部9による運動パ
ラメータの推定の原理を説明する。被計測体1の座標系
における重心Pの運動モデルを次式のように設定する。
Next, the principle of motion parameter estimation by the motion parameter estimation unit 9 will be described. The motion model of the center of gravity P in the coordinate system of the measured object 1 is set as in the following equation.

【0014】 xb (ti )=ax sin (ωx i +φx )+bx ……(1) yb (ti )=ay sin (ωy i +φy )+by ……(2) zb (ti )=az sin (ωz i +φz )+bz ……(3) ここでxb (ti )、yb (ti )、zb (ti )は被
計測体1の座標系における時刻ti におけるX、Y、Z
軸成分であり、ax 、ay 、az は振幅を、ωx
ωy 、ωz は周波数を、φx 、φy 、φz は位相を、b
x 、by 、bz は原点からのオフセットを表わす。
X b (t i ) = a x sin (ω x t i + φ x ) + b x (1) y b (t i ) = a y sin (ω y t i + φ y ) + b y (2) z b (t i ) = a z sin (ω z t i + φ z ) + b z (3) where x b (t i ), y b (t i ), z b (t i ). Is X, Y, Z at time t i in the coordinate system of the measured object 1.
Is an axial component, and a x , a y , and a z are amplitudes, ω x ,
ω y and ω z are frequencies, φ x , φ y and φ z are phases, and b
x, b y, b z represents an offset from the origin.

【0015】なおzb (ti )はxb (ti )、y
b (ti )と同様の手順で解くことができるので、以下
の説明はX、Y座標成分についてのみ行う。
Note that z b (t i ) is x b (t i ), y
Since it can be solved by the same procedure as b (t i ), the following description will be made only for the X and Y coordinate components.

【0016】図2に示すように被計測体1の座標系とレ
ンジファインダ5の座標系はZ軸回りにθ回転した関係
にあるので、式(1)、式(2)はレンジファインダ5
の座標系において次式のように表わされる。
As shown in FIG. 2, since the coordinate system of the object to be measured 1 and the coordinate system of the range finder 5 have a relationship of θ rotation about the Z axis, the equations (1) and (2) are expressed by the range finder 5
It is expressed by the following equation in the coordinate system of.

【0017】[0017]

【数1】 [Equation 1]

【0018】いま、xr (ti )、yr (ti )はレン
ジファインダ5によって計測された被計測体1の位置デ
ータであり、ti は計測時刻で既知であるので式(4)
は9個の未知パラメータを有する非線形方程式である。
ここで9個の未知パラメータとはθ、ax 、ay
ωx 、ωy 、φx 、φy 、bx 、by である。よって、
9点の計測データから数値解法によりパラメータの近似
解を求めることは可能である。しかしながら式(4)に
はsin ()のような多価関数が含まれており、初期値の
設定によっては近似解が真値に収束する保証はない。こ
のため式(4)のωx 、ωy 以外のパラメータを以下の
ような手順で解析的に消去し、ωx 、ωy に関する式を
導出し、この式の数値解を検討することにより運動パラ
メータを推定する。
Now, x r (t i ) and y r (t i ) are position data of the object to be measured 1 measured by the range finder 5, and t i is known at the measurement time.
Is a nonlinear equation with 9 unknown parameters.
Here, the nine unknown parameters are θ, a x , a y ,
ω x, ω y, φ x , φ y, b x, is a b y. Therefore,
It is possible to obtain the approximate solution of the parameter from the measurement data of 9 points by the numerical solution method. However, equation (4) includes a polyvalent function such as sin (), and there is no guarantee that the approximate solution will converge to the true value depending on the setting of the initial value. Therefore, the parameters other than ω x and ω y in Eq. (4) are analytically deleted by the following procedure, the formulas relating to ω x and ω y are derived, and the numerical solution of this formula is examined to determine the motion. Estimate the parameters.

【0019】まず、式(4)からx(t0 )、y
(t0 )によってbx 、by を消去する。
First, from equation (4), x (t 0 ), y
Erase b x and b y by (t 0 ).

【0020】[0020]

【数2】 [Equation 2]

【0021】ここでPi 、A、Gi を次のように定義す
る。
Here, P i , A, and G i are defined as follows.

【0022】[0022]

【数3】 [Equation 3]

【0023】式(4)は次に示す式(8)のように変形
される。
The equation (4) is transformed into the following equation (8).

【0024】Pi =AGi ……(8) さらに加法定理によりGi を次に示す式(9)のように
変形する。
P i = AG i (8) Further, by addition theorem, G i is transformed into the following formula (9).

【0025】[0025]

【数4】 [Equation 4]

【0026】ここでsxi、syi、cxi、cyiは次式で定
義される。
Here, s xi , s yi , c xi and c yi are defined by the following equations.

【0027】[0027]

【数5】 [Equation 5]

【0028】次にP1 、P3 、G1 、G3 からAを消去
して式(10)を得る。
Then, A is eliminated from P 1 , P 3 , G 1 , and G 3 to obtain the equation (10).

【0029】G1 1 -1=G3 3 -1……(10) P1 -1、P3 -1を式(11)のように置き換える。G 1 P 1 -1 = G 3 P 3 -1 (10) P 1 -1 , P 3 -1 are replaced as shown in equation (11).

【0030】[0030]

【数6】 [Equation 6]

【0031】次に式(10)からtan φx 、tan φy
関する式(12)〜式(15)を得る。
Next, from equation (10), equations (12) to (15) relating to tan φ x and tan φ y are obtained.

【0032】 tan φx =−{(qx1x1+qy1x2−qx3x3−qy3x4)/ (qx1x1+qy1x2−qx3x3−qy3x4)}……(12) tan φx =−{(qx2x1+qy2x2−qx4x3−qy4x4)/ (qx2x1+qy2x2−qx4x3−qy4x4)}……(13) tan φy =−{(qx1y1+qy1y2−qx3y3−qy3y4)/ (qx1y1+qy1y2−qx3y3−qy3y4)}……(14) tan φy =−{(qx2y1+qy2y2−qx4y3−qy4y4)/ (qx2y1+qy2y2−qx4y3−qy4y4)}……(15) 次に式(12)、式(13)および式(14)、式(1
5)を連立させてφx、φy を消去し、式(16)、式
(17)を得る。
Tan φ x = − {(q x1 s x1 + q y1 s x2 −q x3 s x3 −q y3 s x4 ) / (q x1 c x1 + q y1 c x2 −q x3 c x3 −q y3 c x4 ) } (12) tan φ x = − {(q x2 s x1 + q y2 s x2 −q x4 s x3 −q y4 s x4 ) / (q x2 c x1 + q y2 c x2 −q x4 c x3 −q y4 c x4 )} (13) tan φ y = − {(q x1 s y1 + q y1 s y2 −q x3 s y3 −q y3 s y4 ) / (q x1 cy1 + q y1 cy2 −q x3 cy3 −q y3 cy4 )} (14) tan φ y = − {(q x2 s y1 + q y2 s y2 −q x4 s y3 −q y4 s y4 ) / (q x2 cy1 + q y2 cy2 −q x4 cy3 −q y4 cy4 )} (15) Next, equation (12), equation (13), equation (14), and equation (1)
5) are made simultaneous, and φ x and φ y are eliminated, and equations (16) and (17) are obtained.

【0033】 (qx1x1+qy1x2−qx3x3−qy3x4)・(qx2x1+qy2x2 −qx4x3−qy4x4)= (qx2x1+qy2x2−qx4x3−qy4x4)・(qx1x1+qy1x2 −qx3x3−qy3x4) ……(16) (qx1y1+qy1y2−qx3y3−qy3y4)・(qx2y1+qy2y2 −qx4y3−qy4y4)= (qx2y1+qy2y2−qx4y3−qy4y4)・(qx1y1+qy1y2 −qx3y3−qy3y4) ……(17) 式(16)、式(17)を解く代わりに式(16)、式
(17)において左辺−右辺をそれぞれF(ωx )、F
(ωy )と置き、式(18)、式(19)を得て、F
(ωx )=0、F(ωy )=0となるωx 、ωy を求め
る。
(Q x1 s x1 + q y1 s x2 −q x3 s x3 −q y3 s x4 ) · (q x2 c x1 + q y2 c x2 −q x4 c x3 −q y4 c x4 ) = (q x2 s x1 + Q y2 s x2 −q x4 s x3 −q y4 s x4 ) ・ (q x1 c x1 + q y1 c x2 −q x3 c x3 −q y3 c x4 ) (16) (q x1 s y1 + q y1 s y2 -Q x3 sy3 -q y3 sy4 )-(q x2 cy1 + q y2 cy2 -q x4 cy3 -q y4 cy4 ) = (q x2 sy1 + q y2 sy2 -q x4 sy3 -q y4 s y4 ) · (q x1 cy1 + q y1 cy2 −q x3 cy3 −q y3 cy4 ) (17) Instead of solving equations (16) and (17), equations (16) and (17) ), The left side-the right side is F (ω x ), F
y ), Equation (18) and Equation (19) are obtained, and F
Ω x and ω y for which (ω x ) = 0 and F (ω y ) = 0 are obtained.

【0034】 F(ωx )=(qx1x1+qy1x2−qx3x3−qy3x4)・ (qx2x1+qy2x2−qx4x3−qy4x4) −(qx2x1+qy2x2−qx4x3−qy4x4)・ (qx1x1+qy1x2−qx3x3−qy3x4) ……(18) F(ωy )=(qx1y1+qy1y2−qx3y3−qy3y4)・ (qx2y1+qy2y2−qx4y3−qy4y4) −(qx2y1+qy2y2−qx4y3−qy4y4)・ (qx1y1+qy1y2−qx3y3−qy3y4) ……(19) 式(18)、式(19)はそれぞれωx 、ωy に関する
式であるが、ωx とωy を入れ替えると同じ式となる。
これは、式(5)においてはθを未知数としたが、θを
π/2−θとし、ax とay 、ωx とωy を入れ替えて
も式(5)は成立することに対応する。被計測体1の座
標系に対するレンジファインダ5の座標系の傾きθはπ
/2回転しても周波数とは無関係であるからである。
F (ω x ) = (q x1 s x1 + q y1 s x2 −q x3 s x3 −q y3 s x4 ) · (q x2 c x1 + q y2 c x2 −q x4 c x3 −q y4 c x4 ) − (Q x2 s x1 + q y2 s x2 −q x4 s x3 −q y4 s x4 ) · (q x1 c x1 + q y1 c x2 −q x3 c x3 −q y3 c x4 ) (18) F (ω y ) = (q x1 s y1 + q y1 s y2 −q x3 s y3 −q y3 s y4 ) · (q x2 cy1 + q y2 cy2 −q x4 cy3 −q y4 cy4 ) − (q x2 s y1 + Q y2 s y2 −q x4 s y3 −q y4 s y4 ) · (q x1 c y1 + q y1 c y2 −q x3 c y3 −q y3 c y4 ) (19) Formula (18), Formula (19) Are expressions relating to ω x and ω y , respectively, but if ω x and ω y are exchanged, the same equation is obtained.
This corresponds to the fact that θ is an unknown number in the equation (5), but θ is π / 2−θ, and the equation (5) holds even if a x and a y and ω x and ω y are exchanged. To do. The inclination θ of the coordinate system of the range finder 5 with respect to the coordinate system of the DUT 1 is π
This is because it is irrelevant to the frequency even if it is rotated by / 2.

【0035】したがって、式(18)、式(19)から
別々にωx 、ωy を求める代わりに式(18)において
F(ωx )=0をF(ω)=0と置き換えて、「0」に
近い2つの解を求め、そのうち「0」に近いものを
ωx 、ωy とする。ωx 、ωy が求まると、以上に述べ
たのと逆の手順により、他の運動パラメータを求める。
Therefore, instead of separately obtaining ω x and ω y from the equations (18) and (19), F (ω x ) = 0 is replaced with F (ω) = 0 in the equation (18), and Two solutions close to "0" are obtained, and the ones close to "0" are defined as ω x and ω y . When ω x and ω y are obtained, other motion parameters are obtained by the procedure reverse to that described above.

【0036】式(18)においてF(ω)=0を解き、
ωx 、ωy を求めるにはニュートンラプソン法を用いる
が、初期値の設定の仕方によってはωが真値に収束しな
いことがあるので、このような事態を避けるため、F
(ω)を微分して解が収束するような手順を踏む。
Solving F (ω) = 0 in the equation (18),
The Newton-Raphson method is used to obtain ω x and ω y , but ω may not converge to the true value depending on how the initial values are set.
Take steps to differentiate (ω) and converge the solution.

【0037】図4は、F(ω)を微分して解が収束する
ような手順を踏んで、F(ω)=0を解き、被計測体1
の運動パラメータを推定して将来の位置を予測する処理
を示すフローチャートである。
In FIG. 4, F (ω) = 0 is solved by following a procedure in which F (ω) is differentiated and the solution converges.
6 is a flowchart showing a process of estimating a motion parameter of a and predicting a future position.

【0038】図5はF(ω)=0に対して、図4に示す
フローチャートに従って解を求める場合の説明図であ
る。図5(a)はF(ω)を示し、図5(b)はF´
(ω)を示し、図5(c)はF″(ω)を示す。
FIG. 5 is an explanatory diagram for obtaining a solution in accordance with the flowchart shown in FIG. 4 for F (ω) = 0. FIG. 5A shows F (ω), and FIG. 5B shows F ′.
(Ω), and FIG. 5C shows F ″ (ω).

【0039】図4に示すように、まず時刻ti における
位置データxr (ti )、yr (ti )を計測し(ステ
ップ401)、周波数ωに関する式(18)にPxi、P
yi、ti (i=1〜4)を代入する(ステップ40
2)。適当な初期値をω0 とすると共に、パラメータ
n、jをクリアする(ステップ403)。ニュートンラ
プソン法によってFn (ω)上の第1近似解ω1 n を求
める(ステップ404)。ω1 n とω0 を比較し(ステ
ップ405)、ω1 n <ω0 が満たされない場合(図5
(a)、図5(b)の場合)は、Fn (ω)を微分する
(ステップ406)。
As shown in FIG. 4, the time t the position of the i data x r (t i), to measure the y r (t i) (step 401), P xi in equation (18) related to the frequency omega, P
yi, substituting t i (i = 1~4) (Step 40
2). An appropriate initial value is set to ω 0, and the parameters n and j are cleared (step 403). A first approximate solution ω 1 n on F n (ω) is obtained by the Newton-Raphson method (step 404). ω 1 n and ω 0 are compared (step 405), and when ω 1 n0 is not satisfied (FIG. 5).
In the case of (a) and FIG. 5 (b), F n (ω) is differentiated (step 406).

【0040】ステップ405においてω1 n <ω0 が満
たされる(図5(c)の場合)と、Fn (ω)における
収束解ωn*を求める(ステップ407)。そしてωn*
δ(δ>0)を初期値とし、nを1減らし(ステップ4
08)、n<0か否かを判定する(ステップ409)。
n<0が満たされるまでステップ407、ステップ40
8の処理を繰り返す。
When ω 1 n0 is satisfied in step 405 (in the case of FIG. 5C), a convergent solution ω n * in F n (ω) is obtained (step 407). And ω n *
The initial value is δ (δ> 0), and n is reduced by 1 (step 4
08), and it is determined whether n <0 (step 409).
Steps 407 and 40 until n <0 is satisfied
The process of 8 is repeated.

【0041】ステップ409でn<0となると、ω
0 (j)にF(ω)における収束解ω0*を格納し、jを
「1」増加させ(ステップ410)、ω0*=0か否かを
判定し(ステップ411)、ω0*=0でない場合、ω0*
−δを初期値とし、nに「0」を代入し(ステップ41
2)、ステップ404に戻る。
When n <0 in step 409, ω
The convergent solution ω 0 * in F (ω) is stored in 0 (j), j is increased by “1” (step 410), it is determined whether ω 0 * = 0 (step 411), and ω 0 * If = 0, then ω 0 *
With −δ as an initial value, “0” is substituted for n (step 41
2) and returns to step 404.

【0042】ステップ411においてω0*=0の場合、
ω0 (j−1)、ω0 (j−2)をそれぞれωx 、ωy
に代入する(ステップ413)。このようにしてωx
ωyが算出されると、式(18)までの手順を逆に辿
り、θ、ax 、ay 、φx 、φy 、bx 、by を求める
(ステップ414)。このようにしてパラメータωx
ωy 、θ、ax 、ay 、φx 、φy 、bx 、by が算出
されると式(1)、式(2)により所望の時刻tf にお
ける被計測体1の重心Pの座標xf 、yf を求める(ス
テップ415)。
If ω 0 * = 0 in step 411,
Let ω 0 (j-1) and ω 0 (j-2) be ω x and ω y , respectively.
(Step 413). Thus ω x ,
When ω y is calculated, the procedure up to Expression (18) is reversed, and θ, a x , a y , φ x , φ y , b x , and b y are obtained (step 414). Thus the parameter ω x ,
When ω y , θ, a x , a y , φ x , φ y , b x , and b y are calculated, the center of gravity P of the measured object 1 at the desired time t f is calculated by the formulas (1) and (2). The coordinates x f and y f of the are calculated (step 415).

【0043】図5について説明すると、ω0 を初期値と
してω0*が求められ、F(ω)=0の「0」に近い2つ
の解がωx 、ωy とされる。
[0043] When the FIG. 5 will be described, omega 0 omega 0 * as an initial value is determined for the two solutions close to "0" in the F (omega) = 0 is omega x, are omega y.

【0044】以上の手順と同様にして式(3)のz
b (ti )に関するパラメータを求め、得られたパラメ
ータにより時刻tf における被計測体1の重心Pの座標
f 、yf 、zf を求める。
In the same manner as the above procedure, z of equation (3)
Parameters relating to b (t i ) are obtained, and the coordinates x f , y f , and z f of the center of gravity P of the measured object 1 at time t f are obtained from the obtained parameters.

【0045】このように本実施例では、単振動を行う被
計測体1をレンジファインダ5で撮像して、この画像デ
ータを解析することにより運動パラメータを推定し、将
来の被計測体1の位置を予測することができる。
As described above, in this embodiment, the object 1 to be subjected to simple vibration is imaged by the range finder 5 and the image data is analyzed to estimate the motion parameter, and the future position of the object 1 to be measured. Can be predicted.

【0046】[0046]

【発明の効果】以上、詳細に説明したように本発明によ
れば、単振動を行う被計測体の運動パラメータを推定
し、被計測体の将来の位置を予測することができる。
As described above in detail, according to the present invention, it is possible to estimate the motion parameter of the object to be subjected to simple vibration and to predict the future position of the object to be measured.

【図面の簡単な説明】[Brief description of drawings]

【図1】 本発明の1実施例に係る3次元運動予測装置
を用いたシステムの概略構成を示す図
FIG. 1 is a diagram showing a schematic configuration of a system using a three-dimensional motion prediction device according to an embodiment of the present invention.

【図2】 絶対座標系とレンジファインダ5の座標系と
被計測体1の座標系の関係を示す図
FIG. 2 is a diagram showing the relationship between the absolute coordinate system, the coordinate system of the range finder 5 and the coordinate system of the DUT 1.

【図3】 被計測体1の重心PのX座標成分の時刻歴を
示す図
FIG. 3 is a diagram showing a time history of the X coordinate component of the center of gravity P of the measured object 1.

【図4】 本実施例の処理を示すフローチャートFIG. 4 is a flowchart showing the processing of this embodiment.

【図5】 F(ω)=0の解を求める場合の説明図FIG. 5 is an explanatory diagram for obtaining a solution of F (ω) = 0.

【符号の説明】[Explanation of symbols]

1……被計測体 3……3次元運動予測装置 5……レンジファインダ 9……運動パラメータ推定部 11……位置予測部 13……ロボット 15……マニュピュレータ 1 ... Object to be measured 3 ... Three-dimensional motion prediction device 5 ... Range finder 9 ... Motion parameter estimation unit 11 ... Position prediction unit 13 ... Robot 15 ... Manipulator

───────────────────────────────────────────────────── フロントページの続き (72)発明者 中沢 和夫 神奈川県横浜市港北区日吉3−11−16 伴 野コーポ 4−C ─────────────────────────────────────────────────── ─── Continued Front Page (72) Inventor Kazuo Nakazawa 3-11-16 Hiyoshi, Kohoku Ward, Yokohama City, Kanagawa Banno Corp. 4-C

Claims (2)

【特許請求の範囲】[Claims] 【請求項1】 単振動を行う被計測体の3次元位置を解
析する解析手段と、 前記解析手段によって解析された前記被計測体の3次元
位置から前記被計測体の運動パラメータを推定する手段
と、 推定された前記運動パラメータを基にして前記被計測体
の将来の位置を予測する手段と、 を具備する3次元運動予測装置。
1. Analyzing means for analyzing a three-dimensional position of a measured object that performs simple vibration, and means for estimating a motion parameter of the measured object from the three-dimensional position of the measured object analyzed by the analyzing means. And a means for predicting a future position of the measured object based on the estimated motion parameter.
【請求項2】 前記3次元運動予測装置によって得られ
た前記被計測体の将来の位置情報はロボットに入力さ
れ、ロボットが前記被計測体を把持することを特徴とす
る請求項1記載の3次元運動予測装置。
2. The robot according to claim 1, wherein future position information of the measured object obtained by the three-dimensional motion prediction apparatus is input to a robot, and the robot grips the measured object. Dimensional motion prediction device.
JP18677893A 1993-06-30 1993-06-30 Three-dimensional movement predicting device Pending JPH0719818A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP18677893A JPH0719818A (en) 1993-06-30 1993-06-30 Three-dimensional movement predicting device

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP18677893A JPH0719818A (en) 1993-06-30 1993-06-30 Three-dimensional movement predicting device

Publications (1)

Publication Number Publication Date
JPH0719818A true JPH0719818A (en) 1995-01-20

Family

ID=16194445

Family Applications (1)

Application Number Title Priority Date Filing Date
JP18677893A Pending JPH0719818A (en) 1993-06-30 1993-06-30 Three-dimensional movement predicting device

Country Status (1)

Country Link
JP (1) JPH0719818A (en)

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007240344A (en) * 2006-03-09 2007-09-20 Fujitsu Ltd Dynamic shape measuring method and dynamic shape measuring device
JP2008058221A (en) * 2006-09-01 2008-03-13 Kobe Univ Velocity of high-speed moving body, estimation method of position, estimation program, and estimation system
US8140188B2 (en) 2008-02-18 2012-03-20 Toyota Motor Engineering & Manufacturing North America, Inc. Robotic system and method for observing, learning, and supporting human activities
WO2012153629A1 (en) * 2011-05-12 2012-11-15 株式会社Ihi Device and method for controlling prediction of motion
JP2012236254A (en) * 2011-05-12 2012-12-06 Ihi Corp Device and method for holding moving body
JP2012247835A (en) * 2011-05-25 2012-12-13 Ihi Corp Robot movement prediction control method and device
JP2012245568A (en) * 2011-05-25 2012-12-13 Ihi Corp Device and method for controlling and predicting motion
JP2013240847A (en) * 2012-05-18 2013-12-05 Ihi Corp Robot hand device, and control method
CN108020855A (en) * 2017-11-29 2018-05-11 安徽省通信息科技有限公司 The pose and instantaneous center of rotation combined estimation method of a kind of glide steering robot
WO2020032211A1 (en) * 2018-08-10 2020-02-13 川崎重工業株式会社 Data generating device, data generating method, data generating program, and remote operation system

Cited By (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007240344A (en) * 2006-03-09 2007-09-20 Fujitsu Ltd Dynamic shape measuring method and dynamic shape measuring device
JP2008058221A (en) * 2006-09-01 2008-03-13 Kobe Univ Velocity of high-speed moving body, estimation method of position, estimation program, and estimation system
US8140188B2 (en) 2008-02-18 2012-03-20 Toyota Motor Engineering & Manufacturing North America, Inc. Robotic system and method for observing, learning, and supporting human activities
US9108321B2 (en) 2011-05-12 2015-08-18 Ihi Corporation Motion prediction control device and method
JP2012236254A (en) * 2011-05-12 2012-12-06 Ihi Corp Device and method for holding moving body
CN103517789A (en) * 2011-05-12 2014-01-15 株式会社Ihi Device and method for controlling prediction of motion
WO2012153629A1 (en) * 2011-05-12 2012-11-15 株式会社Ihi Device and method for controlling prediction of motion
JP2012247835A (en) * 2011-05-25 2012-12-13 Ihi Corp Robot movement prediction control method and device
JP2012245568A (en) * 2011-05-25 2012-12-13 Ihi Corp Device and method for controlling and predicting motion
JP2013240847A (en) * 2012-05-18 2013-12-05 Ihi Corp Robot hand device, and control method
CN108020855A (en) * 2017-11-29 2018-05-11 安徽省通信息科技有限公司 The pose and instantaneous center of rotation combined estimation method of a kind of glide steering robot
CN108020855B (en) * 2017-11-29 2020-01-31 安徽省一一通信息科技有限公司 posture and rotation instantaneous center joint estimation method for skid-steer robot
WO2020032211A1 (en) * 2018-08-10 2020-02-13 川崎重工業株式会社 Data generating device, data generating method, data generating program, and remote operation system

Similar Documents

Publication Publication Date Title
JP2021503134A (en) Unsupervised learning of image depth and egomotion prediction neural networks
US20140333741A1 (en) Constrained key frame localization and mapping for vision-aided inertial navigation
WO2018209862A1 (en) Pose error correction method and device, robot and storage medium
CN105931275A (en) Monocular and IMU fused stable motion tracking method and device based on mobile terminal
JPH0719818A (en) Three-dimensional movement predicting device
CN109643116A (en) System and method for positioning mobile object
US20170104932A1 (en) Correction method and electronic device
JPH10185612A (en) Method and device for compensating sculling in strap down type inertia navigation system, and memory for strap down type inertia navigation system
WO2022205663A1 (en) Neural network training method and apparatus, target object detecting method and apparatus, and driving control method and apparatus
JP2013097675A (en) Gradient estimation device, gradient estimation method, and gradient estimation program
Piga et al. ROFT: Real-time optical flow-aided 6D object pose and velocity tracking
Lippiello et al. Position and orientation estimation based on Kalman filtering of stereo images
JP6905390B2 (en) Own vehicle position estimation environment map generator, own vehicle position estimation device, own vehicle position estimation environment map generation program, and own vehicle position estimation program
JP2882136B2 (en) Video analysis method
JP5011904B2 (en) Tracking method and apparatus
JP2024012160A (en) Target state estimation method, device, electronic device and medium
CN113487676B (en) Method and apparatus for determining relative attitude angle between cameras mounted to acquisition entity
Wang et al. Time-to-Contact control for safety and reliability of self-driving cars
WO2023273467A1 (en) True value data determination method and apparatus, neural network training method and apparatus, and travel control method and apparatus
Huber et al. Application of time-delay estimation to mixed reality multisensor tracking
CN115556769A (en) Obstacle state quantity determination method and device, electronic device and medium
JP3985610B2 (en) Vehicle traveling path recognition device
JP2001357403A (en) Method and device for tracking vehicle
CN112525143B (en) Method for determining installation angle of equipment and vehicle-mounted terminal
JPH104358A (en) Signal processing method and equipment