JPH08101038A

JPH08101038A - 運動同定装置と運動同定方法および形状推定装置と形状測定方法

Info

Publication number: JPH08101038A
Application number: JP6237414A
Authority: JP
Inventors: Masayoshi Hashima; 正芳橋間
Original assignee: Fujitsu Ltd
Current assignee: Fujitsu Ltd
Priority date: 1994-09-30
Filing date: 1994-09-30
Publication date: 1996-04-16

Abstract

(57)【要約】（修正有）【目的】形状に関して全く情報がない場合にも，物体
の位置，姿勢，運動パラメータ等を計測する。【構成】運動している物体の画像データを入力する画
像入力装置２，画像データにより物体を抽出する画像処
理部３，画像面に対する特徴点までの位置を計測する位
置計算部４，並びに特徴点の時系列の位置のデータに基
づいて物体の姿勢及び物体の運動パラメータを求める運
動同定部５を設け，求めた位置，姿勢，及び運動パラメ
ータを出力する。また，運動に併せて，物体の形状を推
定する。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は，等速運動，等角速度運
動をしている物体の姿勢，位置，運動パラメータ（並進
速度，回転軸，角速度等）を求める運動同定装置と運動
同定方法およびその物体の形状を求める形状推定装置お
よび形状測定方法に関する。

【０００２】宇宙開発等において，宇宙空間を浮遊して
いる人工衛星などの捕獲作業を必要とする場合がある。
このような作業を自動的に行うためには，まず，捕獲す
る人工衛星に接近する必要があるので，その位置，姿
勢，運動パラメータ（並進速度，角速度）を計測する必
要がある。また，人工衛星に限らず，宇宙空間をただよ
うゴミを捕獲する必要もあり，そのような場合にもその
位置と運動の計測が必要になる。また，人工衛星や宇宙
ゴミを捕獲する場合にはその三次元的な形状（表面形
状，立体形状等）の計測も必要である。

【０００３】

【従来の技術】従来開発されている技術は，人工衛星の
モデルに基づく計測方法であり，人工衛星の大きさ，形
状が既知であったり，人工衛星にあらかじめ距離計測用
マークを付け，それらをもとに人工衛星の位置，姿勢お
よび運動パラメータを計測するようにしていた。また，
物体の形状推定も運動パラメータ（並進速度，角速度
等）が既知のものについて画像データをもとに形状を推
定する程度であった。

【０００４】

【発明が解決しようとする課題】従来の技術では，人工
衛星の形状のモデルがない場合，または実際には変形し
ていてモデルが使用できないような場合，さらには宇宙
空間のゴミのように形状に関して全く情報がない場合に
は対象物の位置，姿勢，運動パラメータ等を計測するこ
とができなかった。

【０００５】本発明は，形状に関して全く情報がない場
合にも対象物の位置，姿勢，運動パラメータ等を計測す
ることができる運動同定装置と運動同定方法および形状
推定装置と形状測定方法を提供することを目的とする。

【０００６】

【課題を解決するための手段】物体の角や模様などを特
徴点とし，その特徴点の距離を計測することにより時系
列の画像データと特徴点の距離とにより物体の位置，姿
勢，運動パラメータ（並進速度，回転軸，角速度等）を
計測することができる。例えば，物体を撮像装置等の画
像入力装置で画像データを求め，特徴点を抽出する。ま
た，その際に複数の視点（画像入力装置の位置）により
物体の特徴点を抽出できれば三角測量の原理により画像
入力装置から物体の特徴点までの距離および方向を計算
できる。あるいは，レーザなどを物体に照射して物体の
距離画像を生成し，その距離画像から物体の角（特徴
点）を抽出して特徴点の距離および方向を測定すること
もできる。

【０００７】また，物体の運動パラメータが求まれば，
物体の形状は画像面における輪郭（シルエット）の時系
列データと姿勢，位置，運動パラメータにより表面形状
を測定することができる。

【０００８】図１は本発明の基本構成を示す。図１にお
いて，１は測定対象の物体であって，等速運動，等角速
度運動をしているものである。

【０００９】Ｆは運動同定装置である。２は画像入力装
置であって，複数の視点を持つ撮像装置もしくは視点一
つの撮像装置と物体の距離を測定するレーザ装置等であ
る。また，画像入力装置は撮影した画像データを保存す
る磁気ディスク装置等の画像データ保存装置１０からの
保存データを入力するインタフェースも含む。

【００１０】Ｋは運動同定手段である。Ｋ’は形状推定
手段である。３は画像処理部であって，物体の左右の画
像データを基に物体の特徴点を抽出するものである。

【００１１】４は位置計算部であって，複数（左右）の
視点により求められた特徴点の左右の対応付けを行い，
特徴点と画像面の間の距離および方向を計算し，特徴点
の位置を求めるものである。あるいは，視点ひとつの撮
像装置とレーザ装置等の距離測定装置により特徴点の距
離および方向を計測し，位置を求めても良い。

【００１２】５は運動同定部であって，特徴点の時系列
の位置のデータに基づいて物体の位置，姿勢，運動パラ
メータを求めるものである。６は形状推定部であって，
画像入力装置２により得られた画像の輪郭の時系列デー
タと位置，姿勢，運動パラメータ等に基づいて物体の形
状を推定するものである。

【００１３】７は出力部であって，求めた位置，姿勢，
運動パラメータ，形状等を出力するものである。１０は
画像データ保存装置であって，撮像装置等で得られた画
像データを保存するものである。

【００１４】

【作用】図１の本発明の基本構成の動作を説明する。画
像入力装置が複数の視点（例えば，左右２台の撮像装置
がある）の場合を例として説明する。

【００１５】画像入力装置２は物体１の左右の画像デー
タを入力する。画像処理部３は左右の画像データの特徴
点（画像の角の点等）を抽出する。位置計算部４は左右
の画像データの特徴点同士を対応付け，各特徴点と撮像
装置との距離および方向を計算し，特徴点の位置を求め
る。

【００１６】運動同定部５は，特徴点の位置の時系列デ
ータに基づいて，物体の姿勢および運動パラメータを求
める。例えば，３点の特徴点Ａ，Ｂ，Ｃに対して距離を
求める。そして，点Ａを原点としてベクトルＡＢをｘ
軸，ベクトルＡＢとベクトルＡＣの外積をｚ軸，ｘ軸と
ｚ軸の外積をｙ軸とすれば物体の直交座標系（重心座標
系）が決まる。重心座標の原点は，例えば特徴点の座標
の平均にとる。その重心座標系の座標軸の直交座標のカ
メラ座標に対する方向ベクトルとその原点の位置を各特
徴点の座標により計算し，その時系列データから物体の
姿勢，運動パラメータが求まり，運動を同定することが
できる。

【００１７】出力部７は運動同定部５の求めた位置，姿
勢，運動パラメータを物体の姿勢制御装置等に出力す
る。次に図２により本発明の形状推定装置の形状測定方
法について説明する。

【００１８】図２ (a)は回転軸と垂直な平面の断面図で
あり，図２ (b)は回転軸に平行な画像面を示す。図２は
物体２０の周囲を視点Ａ，視点Ｂ，視点Ｃが時間ととも
に回転する状態を示す（実際の装置では視点が固定さ
れ，物体２０が回転するのであるが，図２のように視点
が固定されていて物体２０が回転する場合と相対的に同
じである）。

【００１９】図２ (a)， (b)において，２０は物体であ
る。２１は物体の回転軸である。

【００２０】２２は撮影範囲であって，視点Ａ(24)に撮
影される物体２０の範囲である。２３は画像面であっ
て，時刻ｔ₁における画像面である。２４は視点Ａであ
って，時刻ｔ₁の視点である。

【００２１】２５は視点Ｂであって，時刻ｔ₂の視点で
ある。２６は視点Ｃであって，時刻ｔ₃の視点である
（時刻はｔ₁，ｔ₂，ｔ₃の順番に経過するものとす
る）。

【００２２】３０は画像面であって，視点Ａ(24)の方向
から見た回転軸に平行な面である。３１は画像であっ
て，画像面３０に映された物体２０の画像である。３２
は輪郭であって，画像３１の輪郭である。

【００２３】時刻ｔ₁の画像３１の輪郭，時刻ｔ₂の画
像の輪郭，時刻ｔ₃の画像の輪郭等の画像の輪郭の時系
列データと物体２０の姿勢，位置，運動パラメータによ
り物体２０の形状を推定できる。

【００２４】形状推定部６は，画像データを入力し，各
時刻における輪郭データを抽出し，運動同定部の求めた
物体の姿勢，位置，運動パラメータとにより物体の形状
推定をする。

【００２５】なお，上記において，視点が一つの撮像装
置と物体の位置を測定するレーザ装置を備え，画像デー
タから特徴点を抽出し，その特徴点に対してレーザ光を
照射して反射光を受光し，位置を測定するようにしても
良い。

【００２６】なお，撮像装置により撮影された画像デー
タを磁気ディスク装置等の画像データ保存装置１０に保
存し，画像データ保存装置１０の画像データを取り出し
て入力するようにしても良い。

【００２７】本発明によれば，等速運動もしくは等角速
度運動している形状の既知でない物体の位置，姿勢，並
進速度，回転軸，角速度等の運動パラメータを求めるこ
とができる。また，形状，姿勢，位置，並進速度，角速
度の運動パラメータの既知でない物体に対して形状推定
も行うことができる。

【００２８】

【実施例】図３は本発明の運動同定装置の実施例であ
る。図３において，５０は人工衛星等の浮遊物体であっ
て，並進速度（ｄｖ），回転軸（ｋｖ）の角速度（ω）
である。ｄｖは速度ベクトル，ｋｖは回転ベクトルであ
る。

【００２９】５１は運動同定装置である。５２，５３は
画像入力装置であって左右の撮像装置である。５４，５
５は左右の画像処理部であって，特徴点を求めるもので
ある。

【００３０】５６は位置計算部であって，左右の特徴点
を対応付け，特徴点と画像入力装置の間の距離および方
向を計算し，位置を求めるものである。５７は運動同定
部であって，浮遊物体５０の位置，姿勢，運動パラメー
タ（並進速度（ｄｖ），回転軸（ｋｖ），角速度
（ω））を求めるものである。

【００３１】５８は出力部であって，運動同定部５７の
求めた結果を出力するインタフェースである。５９は磁
気ディスク装置であって，運動同定部５７の求めた結
果，画像入力装置５２の画像データ等を保存するもので
ある。

【００３２】６０はディスプレイである。６１’はプリ
ンタである。出力結果は浮遊物体５０の姿勢制御装置等
の外部装置にも出力される。

【００３３】図３の構成の動作を説明する。左右の画像
入力装置５２，５３は浮遊物体５０を撮影する。左右の
撮像データはそれぞれ画像処理部５４，画像処理部５５
に入力され，特徴点が抽出される。

【００３４】位置計算部５６は左右の画像データの特徴
点の対応付けを行い，特徴点と画像入力装置５２，５３
との距離および方向を計算し，位置を求める。運動同定
部５７は特徴点の位置の時系列データに基づいて，浮遊
物体５０の姿勢，運動パラメータを計算する。運動同定
部５７の計算結果は必要に応じてディスプレイ６０，プ
リンタ６１’等に出力される。また，磁気ディスク装置
５９に格納されて保存される。

【００３５】図４は本発明の運動同定装置のシステム構
成実施例である。図４において図３と共通部分は共通の
番号で示す。図４において，５１は運動同定装置であ
る。

【００３６】５２は画像入力装置であって，撮像装置等
である。５８は出力部である。５９は磁気ディスク装置
である。

【００３７】６０はディスプレイである。６１’はプリ
ンタである。６５はＣＰＵである。

【００３８】６６はメモリである。メモリ６６において
Ｋは運動同定手段である。メモリ６６において，５４は
画像処理部であって，画像データから特徴抽出するプロ
グラムである。

【００３９】５６は位置計算部であって，左右の画像デ
ータの特徴点の対応付けと，画像入力装置と物体との距
離および方向を計算し，位置を求めるプログラムであ
る。５７は運動同定部であって，座標系変換，姿勢演
算，運動同定演算等を行うプログラムである。

【００４０】図４の動作の概略は図３と同様である。運
動同定手段の動作の詳細は後述する。図５は本発明の運
動同定の計算方法の説明図である。

【００４１】図５ (a)は実施例１の座標系であり，図５
(b)は実施例２の座標系である。〔実施例１の運動同定の計算方法〕図５ (a)において，
３０は画像入力装置の画像面であり，ｘ軸，ｙ軸，ｚ軸
はカメラ座標系の直交座標であり，ｘ軸，ｙ軸は画像面
３０にあり，ｚ軸は画像面３０に垂直（光軸方向）であ
る。

【００４２】５０は物体である。物体５０において，
Ａ，Ｂ，Ｃは特徴点であり，Ｇはその重心（位置座標の
平均）である。物体５０は，例えば人工衛星等の浮遊物
体である。

【００４３】時刻ｆの特徴点Ａの位置ベクトルをＡｖ_f
とし，Ａｖ_f＝（Ａ_x，Ａ_y，Ａ_z）とする。同様に特
徴点Ｂの時刻ｆの位置ベクトルをＢｖ_f，特徴点Ｃの時
刻ｆの位置ベクトルをＣｖ_fとする。

【００４４】ｔｖ_fは重心Ｇの位置を表すベクトルであ
り，ｘ，ｙ，ｚ座標系での成分が（ｔ_x，ｔ_y，ｔ_z）
である。ｎｖは重心座標系のｘ軸方向の方向ベクトルで
ある。ｍｖは重心座標系のｙ軸方向の方向ベクトルであ
る。ｏｖは重心座標系のｚ軸方向の方向ベクトルであ
る。ｎｖはＡＢベクトルの方向にとる。回転軸のベクト
ルｋｖを表すベクトルの原点となる位置もあらかじめ定
めておく。

【００４５】ｎｖ＝（ｎ_x＝ｉ_x，ｎ_y＝ｉ_y，ｎ_z＝ｉ_z）ａｖ＝（ａ_x＝ｊ_x，ａ_y＝ｊ_y，ａ_z＝ｊ_z）ｏｖ＝（ｏ_x，ｏ_y，ｏ_z）とする。

【００４６】時刻ｆのｎｖ，ａｖ，ｏｖをそれぞれｎｖ
_f，ａｖ_f，ｏｖ_fとすると，ｎｖ_f＝Ｂｖ_f−Ａｖ_f ａｖ_f＝（Ｂｖ_f−Ａｖ_f）×（Ｃｖ_f−Ａｖ_f）ｏｖ_f＝ｎｖ_f×ａｖ_f である。（“×”は外積）Ｒｖ_fは物体５０の姿勢を表し，ｎｖ，ａｖ，ｏｖを成
分とする次のマトリックスで表されるものである。

【００４７】

【数１】

【００４８】である。ｋｖは角速度ωの回転ベクトル，
ｄｖは物体５０の並進ベクトルである。ｍｖは回転中心
座標系に対する重心Ｇの位置を表すベクトルである。

【００４９】時刻ｆのｔｖ，Ｒｖをそれぞれｔｖ_f，Ｒ
ｖ_fとし，ベクトルｋｖ（角速度ω）の回転変換行列を
Ｒωとすると_, Ｒｖ_f+1＝ＲωＲｖ_f 従って，Ｒω＝Ｒｖ_f+1Ｒｖ_f ^-1 である。

【００５０】時刻ｆ（ｆフレーム），Ｐ番目の特徴点の
カメラ座標系の座標（Ｐ_fx，Ｐ_fy，Ｐ_fz）に対して，Ｐ
_fx＝ｘ_fp，Ｐ_fy＝ｙ_fp，Ｐ_fz＝ｚ_fpとして表すと，時刻
ｆの各特徴の重心座標は次のようになる。

【００５１】

【数２】

【００５２】重心から見た特徴点の位置（カメラ座標
系）は，重心座標系をＯｇ−ＸｇＹｇＺｇとすると，

【００５３】

【数３】

【００５４】である。次に重心座標系における各時刻の
座標により，

【００５５】

【数４】

【００５６】という行列を作ったとき，Ｗ’を特異値分
解すると，

【００５７】

【数５】

【００５８】となる。ここで，（ｎ_xf，ｎ_yf，ｎ_zf）は
時刻ｆのＸｇ軸のカメラ座標系に対する方向ベクトルで
ある。

【００５９】（ｏ_xf，ｏ_yf，ｏ_zf）は時刻ｆのＹｇ軸の
カメラ座標系に対する方向ベクトルである。（Ｓ_xp，Ｓ
_yp，Ｓ_zp）は重心座標系の特徴点の位置である。物体の
姿勢は，

【００６０】

【数６】

【００６１】であり，時刻ｆ，ｆ＋１において，Ｒｖ_f+1＝Ｒω・Ｒｖ_f であるので，回転変換行列ＲωはＲω＝Ｒｖ_f+1Ｒｖ_f ^-1 により計算する。

【００６２】時刻ｆの重心の位置をｔｖ_fとすると，

【００６３】

【数７】

【００６４】である。以上の関係から，カメラ座標に対
する回転軸の方向（ｋｖ），位置（ｇｖ），並進速度
（ｄｖ），角速度ωを求める。ｋｖとωは，Ｒω＝Ｒｖ
_f+1Ｒｖ_f ^-1から求めることができる。

【００６５】また，ｇｖ_f+1＝ｇｖ_f＋ｄｖｔｖ_f＝ｇｖ_f＋Ｒｖ_f・ｍｖであるので，ｔｖ_f＝ｇｖ₀＋ｄｖ・ｆ＋Ｒｖ_f・ｍｖである。

【００６６】従って，ｔ_x＝ｇｖ_0x＋ｆ・ｄｖ_x＋ｎ_x・ｍｖ_x＋ｏ_x・ｍｖ
_y＋ａ_x・ｍｖ_z ｔ_y＝ｇｖ_0y＋ｆ・ｄｖ_y＋ｎ_y・ｍｖ_x＋ｏ_y・ｍｖ
_y＋ａ_y・ｍｖ_z ｔ_z＝ｇｖ_0z＋ｆ・ｄｖ_z＋ｎ_z・ｍｖ_x＋ｏ_z・ｍｖ
_y＋ａ_z・ｍｖ_z 以上の関係により，３時刻のｔｖ_f，Ｒｖ_fを測定し，
その測定値から得られる連立方程式を解いて，ｇｖ₀，
ｄｖ，ｍｖを求めることができる。

【００６７】図６は本発明のシステム構成実施例のフロ
ーチャート(1) である。Ｓ１，Ｓ１’ 画像入力装置から左右の画像データを入
力する。Ｓ２，Ｓ２’ 画像処理部５４は左右の画像データから
それぞれの特徴抽出をする。

【００６８】Ｓ３位置計算部５６は左右の特徴点の対
応付けを行う。Ｓ４位置計算部５６は特徴点の距離および方向を計算
し，位置を求める。Ｓ５運動同定部５７は，各特徴点の位置の座標（カメ
ラ座標系）を重心座標系に変換する。

【００６９】Ｓ６各特徴点の座標のマトリックを特異
値分解する。Ｓ７運動同定部５７は姿勢，回転成分を求める。Ｓ８運動同定部５７は並進成分を計算する。

【００７０】Ｓ９運動同定結果を出力する。Ｓ１０次の時刻の画像データ入力を要求する。〔実施例２の運動同定方法の計算方法〕以上の説明で
は，特異値分解をして運動パラメータを求めたが，特異
値分解しないでも運動同定をすることができる。

【００７１】図５ (b)を参照して，実施例２の物体の姿
勢，運動パラメータを計算する方法について説明する。
図５ (b)において，３０は画像入力装置の画像面であ
り，ｘ軸，ｙ軸，ｚ軸はカメラ座標系の直交座標（原点
Ｏであり，ｘ軸，ｙ軸は画像面３０にあり，画像面の各
辺に平行である。ｚ軸はｘｙ平面に垂直（光軸方向）で
ある。

【００７２】５０は物体である。物体５０において，
Ａ，Ｂ，Ｃは特徴点である。時刻ｆの特徴点Ａの位置ベ
クトルをＡｖ_fとし，カメラ座標系での座標をＡｖ_f＝
（Ａ_x，Ａ_y，Ａ_z）とする。

【００７３】同様に特徴点Ｂの時刻ｆの位置ベクトルを
Ｂｖ_f，特徴点Ｃの時刻ｆの位置ベクトルをＣｖ_fとす
る。対象物の座標系を重心座標系とし，その原点（重
心）Ｇのカメラ座標系の原点Ｏに対するベクトルをｔｖ
_fとする。重心座標系の原点を特徴点Ａにとる。

【００７４】ｔｖｆ＝Ａｖｆである。ｔｖｆの成分を
（ｔ_x，ｔ_y，ｔ_z）とする。ｎｖは重心座標系のｘ軸
の単位方向ベクトルである。ｍｖは重心座標系のｙ軸の
単位方向ベクトルである。ｏｖは重心座標系のｚ軸の単
位方向ベクトルである。ｎｖ，ａｖ，ｏｖをカメラ座標
系で表し，ｎｖ＝（ｎ_x，ｎ_y，ｎ_z）ａｖ＝（ａ_x，ａ_y，ａ_z）ｏｖ＝（ｏ_x，ｏ_y，ｏ_z）とする。

【００７５】時刻ｆのｎｖ，ａｖ，ｏｖをそれぞれｎｖ
_f，ａｖ_f，ｏｖ_fとすると，ａｖ_f＝（Ｂｖ_f−Ａｖ_f）×（Ｃｖ_f−Ａｖ_f）ｏｖ_f＝ｎｖ_f×ａｖ_f ｎｖ_f＝Ｂｖ_f−Ａｖ_f である（ａｖ_f，ｏｖ_f，ｎｖ_fは単位ベクトルであっ
て，実際には右辺の計算結果を規格化したものである
が，便宜的に上式で表す）。

【００７６】Ｒｖ_fは物体５０の姿勢を表し，ｎｖ，ａ
ｖ，ｏｖを成分とする次のマトリックスで表されるもの
である。

【００７７】

【数８】

【００７８】ものである。ｋｖは角速度ωの回転ベクト
ル，ｄｖは物体５０の並進ベクトルである。ｍｖは回転
中心座標系に対する重心Ｇの位置を表すベクトルであ
る。

【００７９】時刻ｆのｔｖ，Ｒｖをそれぞれｔｖ_f，Ｒ
ｖ_fとし，ベクトルｋｖ（角速度ω）の回転変換行列を
Ｒωとすると_, Ｒｖ_f+1＝ＲωＲｖ_f 従って，Ｒω＝Ｒｖ_f+1Ｒｖ_f ^-1である。

【００８０】以上の関係から，カメラ座標に対する回転
軸の方向（ｋｖ），位置（ｇｖ），並進速度（ｄｖ），
角速度（ω）を求める。ｋｖとωは，Ｒω＝Ｒｖ_f+1Ｒ
ｖ_f ^-1から求めることができる。

【００８１】また，ｇｖ_f+1＝ｇｖ_f＋ｄｖｔｖ_f＝ｇｖ_f＋Ｒｖ_f・ｍｖであるので，ｔｖ_f＝ｇｖ₀＋ｄｖ・ｆ＋Ｒｖ_f・ｍｖである。

【００８２】従って，ｔ_x＝ｇｖ_0x＋ｆ・ｄｖ_x＋ｎ_x・ｍｖ_x＋ｏ_x・ｍｖ
_y＋ａ_x・ｍｖ_z ｔ_y＝ｇｖ_0y＋ｆ・ｄｖ_y＋ｎ_y・ｍｖ_x＋ｏ_y・ｍｖ
_y＋ａ_y・ｍｖ_z ｔ_z＝ｇｖ_0z＋ｆ・ｄｖ_z＋ｎ_z・ｍｖ_x＋ｏ_z・ｍｖ
_y＋ａ_z・ｍｖ_z 以上の関係により，３時刻のｔｖ_f，Ｒｖ_fを測定し，
その測定値から得られる連立方程式を解いて，ｇｖ₀，
ｄｖ，ｍｖが求まる。

【００８３】なお，運動同定の計算方法のは，特願平５
−１４５６３３号に詳述されている。図７は本発明のシ
ステム構成の実施例のフローチャート(2) であって，上
記の運動同定方法(2) により運動同定する場合のフロー
チャートである。

【００８４】Ｓ１，Ｓ１’ 画像データ入力部６１は各
時刻毎に左右の画像入力装置から画像データを入力す
る。Ｓ２，Ｓ２’ 画像処理部５４は各時刻毎に左右の画像
の特徴点Ａ，Ｂ，Ｃをそれぞれ抽出する。

【００８５】Ｓ３位置計算部５６は各時刻毎の左右の
特徴点の対応付けを行う。Ｓ４位置計算部５６は各時刻毎に特徴点の距離および
方向を計算し，位置を求める。即ち，時刻ｆの特徴点
Ａ，Ｂ，Ｃの位置ベクトルＡｖ_f＝（Ａ_x，Ａ_y，
Ａ_z），Ｂｖ_f＝（Ｂ_x，Ｂ_y，Ｂ_z），Ｃｖ_f＝（Ｃ
_x，Ｃ_y，Ｃ_z）を求める。

【００８６】Ｓ５運動同定部５７は重心座標系に座標
変換し，姿勢演算をする。即ち，時刻ｆの重心座標の直
交軸の方向ベクトルｎｖ，ａｖ，ｏｖをそれぞれｎ
ｖ_f，ａｖ_f，ｏｖ_fとすると，ａｖ_f＝（Ｂｖ_f−Ａｖ_f）×（Ｃｖ_f−Ａｖ_f）ｏｖ_f＝ｎｖ_f×ａｖ_f ｎｖ_f＝Ｂｖ_f−Ａｖ_f を求める。

【００８７】Ｓ６運動同定部５７は特徴点の距離をも
とに連続する３時刻のｔｖ_f，Ｒｖ _f（ｆ＝０，１，
２）を求める。そして回転行列Ｒω＝Ｒｖ_f+1Ｒｖ_f ^-1
を求める。

【００８８】さらに，ｇｖ_f+1＝ｇｖ_f＋ｄｖ（ｄｖ
は並進速度ベクトル）よりｔｖ_f＝ｇｖ₀＋ｄｖ・ｆ＋Ｒｖ_f・ｍｖ（ｆ＝０，
１，２）であるので，３時刻のｔｖ_f，Ｒｖ_fの測定値より得ら
れる，連立方程式を解いて，ｇｖ₀，ｄｖ，ｍｖを求め
る。

【００８９】Ｓ７出力部は運動同定結果を出力する。Ｓ８次の連続する３時刻の画像データを要求する。図８は本発明の形状推定装置の実施例である。

【００９０】図８ (a)は装置構成を示す。図８ (b)は図
８ (a)の装置のフローチャートである。図８ (a)におい
て，５０は浮遊物体であり，並進速度（ｋｖ），角速度
（ω），回転軸（ｋｖ）である。

【００９１】５１’は形状推定装置であって，浮遊物体
５０の形状を推定するものである。５２，５３は左右の
画像入力装置である。５４，５５は左右の画像処理部で
あって，左右の画像データの特徴点を抽出するものであ
る。

【００９２】５６は位置計算部であって，左右の特徴点
の対応付けをし，特徴点の位置を算出するものである。
５８は出力部であって，形状推定した結果を出力するイ
ンタフェースである。

【００９３】７０は形状推定部であって，浮遊物体５０
の形状推定を行うものである。形状推定部７０におい
て，５７は運動同定部であって，浮遊物体５０の姿勢，
運動パラメータを求めるものである。

【００９４】６５は輪郭点座標保持部であって，画像の
輪郭データを抽出して保持するものである。６６’は形
状推定演算部であって，画像の輪郭データと運動パラメ
ータにより形状推定を行うものである。

【００９５】６０はディスプレイであって，推定形状を
画面に表示するものである。５９は磁気ディスク装置で
あって，推定した形状データを保存するものである。

【００９６】６１’はプリンタであって，形状推定結果
のデータを印刷出力するものである。なお，出力部は物
体の位置，姿勢，運動パラメータ，形状を姿勢制御装置
等の外部装置にも出力する。

【００９７】図８ (b)の説明は後述する。図９は本発明
の形状推定装置のシステム構成の実施例である。図９に
おいて，５１’は形状推定装置である。

【００９８】５２は左右の画像入力装置である。６１は
画像データ入力部であって，画像データを入力するイン
タフェースである。

【００９９】６５はＣＰＵである。６６はメモリであ
る。Ｋ’は形状推定手段である。５８は出力部であっ
て，形状推定データを外部装置に出力するインタフェー
スである。

【０１００】５９は磁気ディスク装置であって，画像デ
ータおよび形状推定データ等を保存するものである。６
０はディスプレイであって，推定した形状を画面に表示
するものである。

【０１０１】６１’はプリンタである。メモリ６６にお
いて，５４は左右の画像処理部であって，画像データに
基づいて特徴点を抽出する等の処理を行うプログラムで
ある。

【０１０２】５６は位置計算部であって，左右の特徴点
を対応付ける処理および各特徴点の位置を求めるプログ
ラムである。５７は運動同定部であって，運動同定する
プログラムである。

【０１０３】７０は形状推定部である。形状推定部７０
において，６４は輪郭抽出部であって，画像データに基
づいて画像の輪郭を抽出するプログラムである。

【０１０４】６５は輪郭点座標保持部であって，輪郭の
座標を保持する領域である。６６’は形状推定演算部で
あって，形状推定の演算を行うプログラムである。５９
は磁気ディスク装置である。

【０１０５】６０はディスプレイである。６１’はプリ
ンタである。図８ (b)により本発明の形状推定装置の動
作を説明する。

【０１０６】本発明の形状推定装置の画像入力から運動
同定までの処理は前述の運動同定装置のものと同じであ
る。Ｓ１左右の画像入力装置５２，５３から画像入力をす
る。

【０１０７】Ｓ２左右の画像処理部５４，５５は画像
データから特徴点を抽出する。Ｓ３位置計算部５６は左右の特徴点の対応付けを行
う。Ｓ４位置計算部５６は特徴点の距離を計算する。

【０１０８】Ｓ５運動同定部５７は浮遊物体５０の特
徴点の位置の時系列データから姿勢，運動パラメータを
求める。Ｓ６輪郭抽出部６４は画像データから輪郭を抽出す
る。

【０１０９】Ｓ７形状推定部７０は連続する複数時点
の輪郭データと姿勢，位置，運動パラメータにより形状
を推定する。Ｓ８位置，姿勢，運動パラメータ，推定した形状を出
力する。

【０１１０】Ｓ９次の画像データの入力要求をする。

【０１１１】

【発明の効果】本発明の運動同定装置によれば，物体の
モデルや距離計測用のマークがなくても物体の位置，姿
勢，運動パラメータを求めることができる。また，本発
明の形状測定装置によれば，物体のモデルや距離計測用
のマークがなくても物体の位置，姿勢，運動パラメータ
を求め，その測定結果を用いて物体の形状を求めること
もできる。

【図面の簡単な説明】

【図１】本発明の基本構成を示す図である。

【図２】本発明の形状推定装置の形状測定方法の説明図
である。

【図３】本発明の運動同定装置の実施例を示す図であ
る。

【図４】本発明の運動同定装置のシステム構成実施例を
示す図である。

【図５】本発明の運動同定の計算方法(1) ，(2) の説明
図である。

【図６】本発明のシステム構成実施例のフローチャート
(1) を示す図である。

【図７】本発明のシステム構成実施例のフローチャート
(2) を示す図である。

【図８】本発明の形状推定装置の実施例を示す図であ
る。

【図９】本発明の形状推定装置のシステム構成実施例を
示す図である。

【符号の説明】

Ｆ：運動同定装置１：測定対象の物体２：画像入力装置（形状推定装置）Ｋ：運動同定手段Ｋ’：形状推定手段３：画像処理部４：位置計算部５：運動同定部６：形状推定部７：出力部１０：画像データ保存装置

フロントページの続き (51)Int.Cl.⁶ 識別記号庁内整理番号ＦＩ技術表示箇所Ｇ０１Ｐ 3/36 ＣＧ０６Ｔ 7/00 7/60

Claims

【特許請求の範囲】

【請求項１】運動している物体の画像データを入力す
る画像入力装置と，該画像データにより物体の特徴点を
抽出する画像処理部と，画像面に対する特徴点までの位
置を計測する位置計算部と，該特徴点の時系列の位置の
データに基づいて該物体の姿勢および物体の運動パラメ
ータを求める運動同定部と，求めた位置，姿勢，運動パ
ラメータを出力する出力部とを備えたことを特徴とする
運動同定装置。
【請求項２】画像入力装置と運動している物体の位置と
姿勢および運動パラメータを求める運動同定手段と出力
部とを備え，運動同定手段は，画像入力装置から入力さ
れる画像データにより物体の特徴点を抽出し，該特徴点
の画像面に対する位置を算出し，該特徴点の時系列の位
置のデータに基づいて物体の姿勢および運動パラメータ
を求め，求めた位置，姿勢，運動パラメータを出力する
ことを特徴とする運動同定方法。
【請求項３】運動している物体の画像データを入力す
る画像入力装置と，該画像データにより物体の特徴点を
抽出する画像処理部と，画像面に対する該特徴点の位置
を計算する位置計算部と，該特徴点の時系列の位置のデ
ータに基づいて物体の姿勢および運動パラメータを求め
る運動同定部と，該物体の画像面上の輪郭と位置と姿勢
および運動パラメータとに基づいて該物体の形状を推定
する形状推定部と，求めた位置，姿勢，運動パラメータ
および形状を出力する出力部とを備えたことを特徴とす
る形状推定装置。
【請求項４】画像入力装置と運動している物体の位置と
姿勢および運動パラメータを求める形状推定手段と出力
部とを備え，形状推定手段は，画像入力装置から入力さ
れる画像データにより物体の特徴点を抽出し，画像面に
対する特徴点の位置を計算し，該位置の時系列データに
基づいて物体の姿勢および運動パラメータを求め，時系
列の画像の輪郭データを求め，該時系列の輪郭データと
位置と姿勢および運動パラメータとに基づいて物体の形
状を推定し，求めた位置，姿勢，運動パラメータおよび
形状を出力することを特徴とする形状測定方法。