JPH0719818A - ３次元運動予測装置 - Google Patents

Abstract

(57)【要約】 【構成】 単振動を行う被計測体１の位置データをレン
ジファインダ５で解析する。運動パラメータ推定部９は
被計測体１の運動パラメータを推定し、位置予測部１１
は推定された運動パラメータを基にして被計測体１の将
来の位置を予測する。ロボット１３は位置予測部１１か
ら得られる被計測体１の将来位置情報を基にして、マニ
ュピュレータ１５により被計測体１を把持する。 【効果】 単振動を行う被計測体の運動パラメータを推
定し、被計測体の将来の位置を予測することができる。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、単振動を行う被計測体
の３次元位置を予測し、予測された３次元位置情報を、
被計測体を把持するロボットに供与する３次元運動予測
装置に関するものである。

【０００２】

【従来の技術】従来、動画像から動物体の運動パラメー
タを推定する手法として２つの手法がある。第１の手法
は２段階推定法と呼ばれ、まず第１段階で動物体の運動
前後の画像間でオプティカルフロー、あるいはフレーム
間対応を求め、第２段階でオプティカルフローから画像
中の動物体の運動パラメータを推定するものである。第
２の手法は直接推定法と呼ばれる手法で、オプティカル
フローを求めずに画像の濃度変化だけから直接に運動パ
ラメータを推定する手法である。

【０００３】

【発明が解決しようとする課題】しかしながら、従来の
手法では、２次元情報である画像情報から３次元運動の
解析を行うが、距離情報が知られていない場合、３次元
運動の解析を行うことは困難であった。また元来、画像
のごくわずかな変動を仮定しているので３次元運動を高
い精度で推定することは困難であった。

【０００４】「山本正信“動画像と距離情報の併用によ
る３次元運動パラメータの直接的推定法”電子情報通信
学会論文集 Ｊ６８−Ｄ、４、ｐｐ．５６２−５６９
（１９８５／４）」には距離情報を併用して運動パラメ
ータを動画像から直接推定する方法が開示されている。
しかしながらこの方法では、物体の運動を等速運動と仮
定しているので、このような運動を行わない対象には適
応できない。

【０００５】本発明は、このような問題に鑑みてなされ
たもので、その目的とするところは、振り子のようなそ
の運動を単振動と仮定できる運動を行う物体を対象と
し、３次元計測により得られた被計測体の位置の計測情
報から被計測体の運動パラメータを推定し、被計測体の
将来の位置を予測する３次元運動予測装置を提供するこ
とにある。

【０００６】

【課題を解決するための手段】前述した目的を達成する
ために本発明は、単振動を行う被計測体の３次元位置を
解析する解析手段と、前記解析手段によって解析された
前記被計測体の３次元位置から前記被計測体の運動パラ
メータを推定する手段と、推定された前記運動パラメー
タを基にして前記被計測体の将来の位置を予測する手段
と、を具備する３次元運動予測装置である。

【０００７】

【作用】本発明では、単振動を行う被計測体の３次元位
置を解析し、解析された被計測体の３次元位置から被計
測体の運動パラメータを推定し、推定された運動パラメ
ータを基にして被計測体の将来の位置を予測する。

【０００８】

【実施例】以下、図面に基づいて本発明の実施例を詳細
に説明する。図１は、本発明の１実施例に係る３次元運
動予測装置を用いたシステムの概略構成を示す図であ
る。図１において１は単振動（振り子運動）を行う被計
測体であり、３は３次元運動予測装置である。３次元運
動予測装置３はレンジファインダ５、運動パラメータ推
定部９、位置予測部１１からなる。レンジファインダ５
は被計測体１を撮像し、撮像された画像を基にし被計測
体１の３次元位置を求める。

【０００９】図２はレンジファインダ５と被計測体１の
関係を示すものである。図２においてＸ、Ｙは絶対座標
系、Ｘ_r 、Ｙ_r はレンジファインダ５の座標系、Ｘ_p 、
Ｙ_pは被計測体１の座標系を表わす。ただしいずれの座
標系においてもＺ軸は紙面に垂直方向であり、図示を省
略している。

【００１０】被計測体１が単振動を行っている場合、被
計測体１の重心ＰのＸ_p 軸座標成分の時刻歴は図３に示
すようなものとなる。しかしながら、３次元計測の計測
処理に要する時間を考慮すると、得られる計測点は図３
中の黒丸点のような離散的なデータとなる。

【００１１】運動パラメータ推定部９は、このような時
間的に離散した重心Ｐの座標データから重心Ｐの運動パ
ラメータを推定する。位置予測部１１は運動パラメータ
推定部９で得られた運動パラメータを基にして未来の時
刻Ｔ_f における重心Ｐの座標値（Ｘ_f ，Ｙ_f ，Ｚ_f ）を
予測する。

【００１２】被計測体１の将来の位置座標は、ロボット
１３に入力され、ロボット１３のマニュピュレータ１５
により被計測体１が把持される。

【００１３】次に運動パラメータ推定部９による運動パ
ラメータの推定の原理を説明する。被計測体１の座標系
における重心Ｐの運動モデルを次式のように設定する。

【００１４】 ｘ^b （ｔ_i ）＝ａ_x sin （ω_x ｔ_i ＋φ_x ）＋ｂ_x ……（１） ｙ^b （ｔ_i ）＝ａ_y sin （ω_y ｔ_i ＋φ_y ）＋ｂ_y ……（２） ｚ^b （ｔ_i ）＝ａ_z sin （ω_z ｔ_i ＋φ_z ）＋ｂ_z ……（３） ここでｘ^b （ｔ_i ）、ｙ^b （ｔ_i ）、ｚ^b （ｔ_i ）は被
計測体１の座標系における時刻ｔ_i におけるＸ、Ｙ、Ｚ
軸成分であり、ａ_x 、ａ_y 、ａ_z は振幅を、ω_x 、
ω_y 、ω_z は周波数を、φ_x 、φ_y 、φ_z は位相を、ｂ
_x 、ｂ_y 、ｂ_z は原点からのオフセットを表わす。

【００１５】なおｚ^b （ｔ_i ）はｘ^b （ｔ_i ）、ｙ
^b （ｔ_i ）と同様の手順で解くことができるので、以下
の説明はＸ、Ｙ座標成分についてのみ行う。

【００１６】図２に示すように被計測体１の座標系とレ
ンジファインダ５の座標系はＺ軸回りにθ回転した関係
にあるので、式（１）、式（２）はレンジファインダ５
の座標系において次式のように表わされる。

【００１７】

【数１】

【００１８】いま、ｘ^r （ｔ_i ）、ｙ^r （ｔ_i ）はレン
ジファインダ５によって計測された被計測体１の位置デ
ータであり、ｔ_i は計測時刻で既知であるので式（４）
は９個の未知パラメータを有する非線形方程式である。
ここで９個の未知パラメータとはθ、ａ_x 、ａ_y 、
ω_x 、ω_y 、φ_x 、φ_y 、ｂ_x 、ｂ_y である。よって、
９点の計測データから数値解法によりパラメータの近似
解を求めることは可能である。しかしながら式（４）に
はsin （）のような多価関数が含まれており、初期値の
設定によっては近似解が真値に収束する保証はない。こ
のため式（４）のω_x 、ω_y 以外のパラメータを以下の
ような手順で解析的に消去し、ω_x 、ω_y に関する式を
導出し、この式の数値解を検討することにより運動パラ
メータを推定する。

【００１９】まず、式（４）からｘ（ｔ₀ ）、ｙ
（ｔ₀ ）によってｂ_x 、ｂ_y を消去する。

【００２０】

【数２】

【００２１】ここでＰ_i 、Ａ、Ｇ_i を次のように定義す
る。

【００２２】

【数３】

【００２３】式（４）は次に示す式（８）のように変形
される。

【００２４】Ｐ_i ＝ＡＧ_i ……（８） さらに加法定理によりＧ_i を次に示す式（９）のように
変形する。

【００２５】

【数４】

【００２６】ここでｓ_xi、ｓ_yi、ｃ_xi、ｃ_yiは次式で定
義される。

【００２７】

【数５】

【００２８】次にＰ₁ 、Ｐ₃ 、Ｇ₁ 、Ｇ₃ からＡを消去
して式（１０）を得る。

【００２９】Ｇ₁ Ｐ₁ ^-1＝Ｇ₃ Ｐ₃ ^-1……（１０） Ｐ₁ ^-1、Ｐ₃ ^-1を式（１１）のように置き換える。

【００３０】

【数６】

【００３１】次に式（１０）からtan φ_x 、tan φ_y に
関する式（１２）〜式（１５）を得る。

【００３２】 tan φ_x ＝−｛（ｑ_x1ｓ_x1＋ｑ_y1ｓ_x2−ｑ_x3ｓ_x3−ｑ_y3ｓ_x4）／ （ｑ_x1ｃ_x1＋ｑ_y1ｃ_x2−ｑ_x3ｃ_x3−ｑ_y3ｃ_x4）｝……（１２） tan φ_x ＝−｛（ｑ_x2ｓ_x1＋ｑ_y2ｓ_x2−ｑ_x4ｓ_x3−ｑ_y4ｓ_x4）／ （ｑ_x2ｃ_x1＋ｑ_y2ｃ_x2−ｑ_x4ｃ_x3−ｑ_y4ｃ_x4）｝……（１３） tan φ_y ＝−｛（ｑ_x1ｓ_y1＋ｑ_y1ｓ_y2−ｑ_x3ｓ_y3−ｑ_y3ｓ_y4）／ （ｑ_x1ｃ_y1＋ｑ_y1ｃ_y2−ｑ_x3ｃ_y3−ｑ_y3ｃ_y4）｝……（１４） tan φ_y ＝−｛（ｑ_x2ｓ_y1＋ｑ_y2ｓ_y2−ｑ_x4ｓ_y3−ｑ_y4ｓ_y4）／ （ｑ_x2ｃ_y1＋ｑ_y2ｃ_y2−ｑ_x4ｃ_y3−ｑ_y4ｃ_y4）｝……（１５） 次に式（１２）、式（１３）および式（１４）、式（１
５）を連立させてφ_x、φ_y を消去し、式（１６）、式
（１７）を得る。

【００３３】 （ｑ_x1ｓ_x1＋ｑ_y1ｓ_x2−ｑ_x3ｓ_x3−ｑ_y3ｓ_x4）・（ｑ_x2ｃ_x1＋ｑ_y2ｃ_x2 −ｑ_x4ｃ_x3−ｑ_y4ｃ_x4）＝ （ｑ_x2ｓ_x1＋ｑ_y2ｓ_x2−ｑ_x4ｓ_x3−ｑ_y4ｓ_x4）・（ｑ_x1ｃ_x1＋ｑ_y1ｃ_x2 −ｑ_x3ｃ_x3−ｑ_y3ｃ_x4） ……（１６） （ｑ_x1ｓ_y1＋ｑ_y1ｓ_y2−ｑ_x3ｓ_y3−ｑ_y3ｓ_y4）・（ｑ_x2ｃ_y1＋ｑ_y2ｃ_y2 −ｑ_x4ｃ_y3−ｑ_y4ｃ_y4）＝ （ｑ_x2ｓ_y1＋ｑ_y2ｓ_y2−ｑ_x4ｓ_y3−ｑ_y4ｓ_y4）・（ｑ_x1ｃ_y1＋ｑ_y1ｃ_y2 −ｑ_x3ｃ_y3−ｑ_y3ｃ_y4） ……（１７） 式（１６）、式（１７）を解く代わりに式（１６）、式
（１７）において左辺−右辺をそれぞれＦ（ω_x ）、Ｆ
（ω_y ）と置き、式（１８）、式（１９）を得て、Ｆ
（ω_x ）＝０、Ｆ（ω_y ）＝０となるω_x 、ω_y を求め
る。

【００３４】 Ｆ（ω_x ）＝（ｑ_x1ｓ_x1＋ｑ_y1ｓ_x2−ｑ_x3ｓ_x3−ｑ_y3ｓ_x4）・ （ｑ_x2ｃ_x1＋ｑ_y2ｃ_x2−ｑ_x4ｃ_x3−ｑ_y4ｃ_x4） −（ｑ_x2ｓ_x1＋ｑ_y2ｓ_x2−ｑ_x4ｓ_x3−ｑ_y4ｓ_x4）・ （ｑ_x1ｃ_x1＋ｑ_y1ｃ_x2−ｑ_x3ｃ_x3−ｑ_y3ｃ_x4） ……（１８） Ｆ（ω_y ）＝（ｑ_x1ｓ_y1＋ｑ_y1ｓ_y2−ｑ_x3ｓ_y3−ｑ_y3ｓ_y4）・ （ｑ_x2ｃ_y1＋ｑ_y2ｃ_y2−ｑ_x4ｃ_y3−ｑ_y4ｃ_y4） −（ｑ_x2ｓ_y1＋ｑ_y2ｓ_y2−ｑ_x4ｓ_y3−ｑ_y4ｓ_y4）・ （ｑ_x1ｃ_y1＋ｑ_y1ｃ_y2−ｑ_x3ｃ_y3−ｑ_y3ｃ_y4） ……（１９） 式（１８）、式（１９）はそれぞれω_x 、ω_y に関する
式であるが、ω_x とω_y を入れ替えると同じ式となる。
これは、式（５）においてはθを未知数としたが、θを
π／２−θとし、ａ_x とａ_y 、ω_x とω_y を入れ替えて
も式（５）は成立することに対応する。被計測体１の座
標系に対するレンジファインダ５の座標系の傾きθはπ
／２回転しても周波数とは無関係であるからである。

【００３５】したがって、式（１８）、式（１９）から
別々にω_x 、ω_y を求める代わりに式（１８）において
Ｆ（ω_x ）＝０をＦ（ω）＝０と置き換えて、「０」に
近い２つの解を求め、そのうち「０」に近いものを
ω_x 、ω_y とする。ω_x 、ω_y が求まると、以上に述べ
たのと逆の手順により、他の運動パラメータを求める。

【００３６】式（１８）においてＦ（ω）＝０を解き、
ω_x 、ω_y を求めるにはニュートンラプソン法を用いる
が、初期値の設定の仕方によってはωが真値に収束しな
いことがあるので、このような事態を避けるため、Ｆ
（ω）を微分して解が収束するような手順を踏む。

【００３７】図４は、Ｆ（ω）を微分して解が収束する
ような手順を踏んで、Ｆ（ω）＝０を解き、被計測体１
の運動パラメータを推定して将来の位置を予測する処理
を示すフローチャートである。

【００３８】図５はＦ（ω）＝０に対して、図４に示す
フローチャートに従って解を求める場合の説明図であ
る。図５（ａ）はＦ（ω）を示し、図５（ｂ）はＦ´
（ω）を示し、図５（ｃ）はＦ″（ω）を示す。

【００３９】図４に示すように、まず時刻ｔ_i における
位置データｘ^r （ｔ_i ）、ｙ^r （ｔ_i ）を計測し（ステ
ップ４０１）、周波数ωに関する式（１８）にＰ_xi、Ｐ
_yi、ｔ_i （ｉ＝１〜４）を代入する（ステップ４０
２）。適当な初期値をω₀ とすると共に、パラメータ
ｎ、ｊをクリアする（ステップ４０３）。ニュートンラ
プソン法によってＦⁿ （ω）上の第１近似解ω₁ ⁿ を求
める（ステップ４０４）。ω₁ ⁿ とω₀ を比較し（ステ
ップ４０５）、ω₁ ⁿ ＜ω₀ が満たされない場合（図５
（ａ）、図５（ｂ）の場合）は、Ｆⁿ （ω）を微分する
（ステップ４０６）。

【００４０】ステップ４０５においてω₁ ⁿ ＜ω₀ が満
たされる（図５（ｃ）の場合）と、Ｆⁿ （ω）における
収束解ω^n*を求める（ステップ４０７）。そしてω^n*−
δ（δ＞０）を初期値とし、ｎを１減らし（ステップ４
０８）、ｎ＜０か否かを判定する（ステップ４０９）。
ｎ＜０が満たされるまでステップ４０７、ステップ４０
８の処理を繰り返す。

【００４１】ステップ４０９でｎ＜０となると、ω
₀ （ｊ）にＦ（ω）における収束解ω^0*を格納し、ｊを
「１」増加させ（ステップ４１０）、ω^0*＝０か否かを
判定し（ステップ４１１）、ω^0*＝０でない場合、ω^0*
−δを初期値とし、ｎに「０」を代入し（ステップ４１
２）、ステップ４０４に戻る。

【００４２】ステップ４１１においてω^0*＝０の場合、
ω₀ （ｊ−１）、ω₀ （ｊ−２）をそれぞれω_x 、ω_y
に代入する（ステップ４１３）。このようにしてω_x 、
ω_yが算出されると、式（１８）までの手順を逆に辿
り、θ、ａ_x 、ａ_y 、φ_x 、φ_y 、ｂ_x 、ｂ_y を求める
（ステップ４１４）。このようにしてパラメータω_x、
ω_y 、θ、ａ_x 、ａ_y 、φ_x 、φ_y 、ｂ_x 、ｂ_y が算出
されると式（１）、式（２）により所望の時刻ｔ_f にお
ける被計測体１の重心Ｐの座標ｘ_f 、ｙ_f を求める（ス
テップ４１５）。

【００４３】図５について説明すると、ω₀ を初期値と
してω^0*が求められ、Ｆ（ω）＝０の「０」に近い２つ
の解がω_x 、ω_y とされる。

【００４４】以上の手順と同様にして式（３）のｚ
^b （ｔ_i ）に関するパラメータを求め、得られたパラメ
ータにより時刻ｔ_f における被計測体１の重心Ｐの座標
ｘ_f 、ｙ_f 、ｚ_f を求める。

【００４５】このように本実施例では、単振動を行う被
計測体１をレンジファインダ５で撮像して、この画像デ
ータを解析することにより運動パラメータを推定し、将
来の被計測体１の位置を予測することができる。

【００４６】

【発明の効果】以上、詳細に説明したように本発明によ
れば、単振動を行う被計測体の運動パラメータを推定
し、被計測体の将来の位置を予測することができる。

【図面の簡単な説明】

【図１】 本発明の１実施例に係る３次元運動予測装置
を用いたシステムの概略構成を示す図

【図２】 絶対座標系とレンジファインダ５の座標系と
被計測体１の座標系の関係を示す図

【図３】 被計測体１の重心ＰのＸ座標成分の時刻歴を
示す図

【図４】 本実施例の処理を示すフローチャート

【図５】 Ｆ（ω）＝０の解を求める場合の説明図

【符号の説明】

１……被計測体 ３……３次元運動予測装置 ５……レンジファインダ ９……運動パラメータ推定部 １１……位置予測部 １３……ロボット １５……マニュピュレータ

───────────────────────────────────────────────────── フロントページの続き (72)発明者 中沢 和夫 神奈川県横浜市港北区日吉３−11−16 伴 野コーポ ４−Ｃ

Claims (2)

【特許請求の範囲】
1. 【請求項１】 単振動を行う被計測体の３次元位置を解
   析する解析手段と、 前記解析手段によって解析された前記被計測体の３次元
   位置から前記被計測体の運動パラメータを推定する手段
   と、 推定された前記運動パラメータを基にして前記被計測体
   の将来の位置を予測する手段と、 を具備する３次元運動予測装置。
2. 【請求項２】 前記３次元運動予測装置によって得られ
   た前記被計測体の将来の位置情報はロボットに入力さ
   れ、ロボットが前記被計測体を把持することを特徴とす
   る請求項１記載の３次元運動予測装置。