JP2610625B2 - ３次元空間に存在する物体の姿勢推定方法 - Google Patents

Description

【発明の詳細な説明】 〔概要〕 対象物体の画像情報とモデルのパターンとのマッチン
グにより３次元空間に存在する物体の姿勢を推定する３
次元空間に存在する物体の姿勢推定方法に関し、 少ない情報を使って簡単かつ高速に３次元空間存在す
る物体の姿勢を認識することを目的とし、対象物体のモ
デルを構成する複数の面夫々について、各面の向き及び
大きさを表す面ベクトルと、該各面の重心位置を表す重
心ベクトルとを抽出し、カメラで撮像された３次元空間
に存在する物体の画像情報を領域分割し、各領域の面積
比を、該各領域に対応する該重心位置のｘ座標,y座標ま
たは該重心位置ベクトルの角度の大小関係に応じた順列
で抽出し、該モデルの各面の面ベクトルと重心位置ベク
トルとを３次元空間で回転させて、任意に選定した視線
軸に対する該各面の面ベクトルの視線軸成分比を、該各
面の重心位置ベクトルの該視線軸と直交する平面上に投
影した位置ベクトルのＸ軸,Y軸成分または角度の大小関
係に応じた順列で抽出し、該各面の面ベクトルの視線軸
成分比と各領域の面積比とを順列が同一となるものどう
しでマッチングを行い、該マッチングがとれたときに該
３次元空間に存在する物体の姿勢を推定するよう構成す
る。

〔産業上の利用分野〕

本発明は３次元空間に存在する物体（以下単に「３次
元物体」という）の姿勢推定方法に関し、３次元物体の
画像情報とモデルのパターンとのマッチングにより３次
元物体の姿勢を推定する３次元空間に存在する物体の姿
勢推定方法に関する。

視覚から得られる３次元物体の画像情報を処理してパ
ターン化し、その姿勢の推定を行なうプロセスは、幾つ
もの段階から構成されるが、物理レベルの処理と概念レ
ベルの処理とに大別することができる。物理レベルの処
理は入力画像データを処理する部分であり、前処理及び
特徴抽出等を含んでいる。概念レベルの処理は、物理レ
ベルの処理によって得られた画像の特徴などのシンボル
化された画像データの処理と認識の部分であり、モデル
パターンとのマッチングをとり、最良のマッチングを選
択するステップを含んでいる。数多くの姿勢推定方法が
提案されているが、それぞれ一長一短があり、より速く
より精度の良い姿勢推定方法が模索されている。

〔従来の技術〕

従来の３次元物体の推定は、これまで多くの場合、第
９図に示す視覚センサ（カメラ）から入力した濃淡画像
情報を雑音除去，濃度変換等の前処理（ブロック71）を
した後、境界を抽出し（ブロック72）、これからエッジ
として抽出し（ブロック73）、そこから幾つかの特徴を
選び（ブロック74）、数値化した特徴と同様の形で蓄え
られているモデル側の特徴とのマッチングをとる（ブロ
ック75）という方法が行なわれていた。

〔発明が解決しようとする問題点〕

しかるに、計算機による画像処理では入力濃淡情報の
量子化の際に情報が欠落し、例えば第10図（Ａ）になし
地で示す四辺形6a領域のように画素となる枠内を少しは
み出した形状領域の情報は同図（Ｂ）に示す矩形6bのみ
しか情報が残らない。同様に濃淡情報についても連続的
情報が離散的に扱われるため情報の欠落を生じる。

このことから、エッジの抽出時に同図（Ｃ）に示す如
く、エッジの一部が欠落してしまうことが多くあり、こ
れは特徴抽出時には同図（Ｄ）の如く認識されて、閉じ
たエッジによって初めて認識できる形状（この場合三角
形）を認識できないことになる。

しかし、このような曖昧さが残る境界の影響をできる
だけ小さくするために、密度の高い標本化によって高い
精度を追及していくという対処方法は、膨大な計算量を
引き起こすことになり、計算機の容量，速度の点から実
現的ではないという問題点があった。

また、本出願人は昭和62年10月20日付提出の特許願
（１）、発明の名称「３次元空間に存在する物体の姿勢
推定方法」において、物体を構成する各面の面積を抽出
すると共に、面の隣接面情報として各面の重心位置関係
を用いる方法を提案した。しかし、この方法ではモデル
側，画像情報側夫々の各面の規格化した重心位置座標と
面積とを個々に比較するため、必要以上のデータ比較を
行なわねばならず、処理が遅くなるという問題点があっ
た。

本発明はこのような点に鑑みなされたものであり、少
ない情報を使って簡単かつ高速に３次元物体の姿勢を推
定する３次元空間に存在する物体の姿勢推定方法を提供
することを目的とする。

〔問題点を解決するための手段〕

３次元空間に存在する物体を、視覚センサ（カメラ）
を通し画像情報としたものと、この対象物のモデル情報
（設計情報、既に観察してわかっている形状情報等）と
の間で、姿勢，距離により異なる特徴を使いマッチング
をとることで、空間に置かれた物体の距離，姿勢の推定
を行いえる。この際、物体の形を構成する面についての
記述特に構成という面から隣接面関係の記述が重要にな
ってくる。本発明は、この隣接面関係の簡単且つ有効な
記述として、物体を構成する面各々の面積重心関係（配
置）を用いている。

第１図は本発明方法の原理ブロック図を示す。

同図中、ブロック１では外部からの指示により対象物
体のモデルを選択する。

ブロック２では３次元物体のモデルを構成する複数の
面夫々について、各面の向き及び大きさを表わす面ベク
トルと、該各面の重心位置を表わす重心位置ベクトルと
を抽出する。

ブロック３では３次元物体を捕捉し、その画像情報を
得る。

ブロック４では画像情報を例えば濃度に応じて領域分
割する。

ブロック５では上記分割された各領域の面積比を各領
域に対応する重心位置のｘ座標,y座標または重心位置ベ
クトルの角度の大小関係に応じた順列で抽出する。

ブロック６ではモデルの各面の面ベクトルと重心位置
ベクトルとを３次元空間で回転させて、任意に選定した
視線軸に対する各面の面ベクトルの視線軸成分比を各面
の重心位置ベクトルの視線軸と直交する平面上に投影し
た位置ベクトルのＸ軸,Y軸成分または角度の大小関係に
応じた順列で抽出する。

ブロック７では各面の面ベクトルの視線軸成分比と各
領域の面積比とを順列が同一となるものどうしでマッチ
ングを行ない、マッチングがとれたとき３次元物体の姿
勢を推定する。

〔作用〕

本発明においては、モデルを構成する各面の視線軸方
向の面積比を各面の重心位置の関係に従った順列とし、
３次元物体の各領域の面積比を各領域の重心位置の関係
に従った順列とし、両者をマッチングさせることによ
り、ラフな姿勢推定を高速に達成している。ここでは形
状情報を捨てて面情報を面ベクトルとして単純化するこ
とによりモデル側の計算機処理を高速にし、入力情報を
領域分割法で処理することにより曖昧さに柔軟に対応し
ている。

〔実施例〕

第２図は本発明方法を適用した宇宙空間での宇宙飛翔
体同士のランデブー・ドッキングの概略フローチャート
を示す。ナビゲーションフィルタ10には、加速度計及び
ジャイロより構成されるINS（Inertial Navigation Sys
tem:慣性航法装置）11の加速度計及びジャイロの出力よ
り３次元空間のＸ軸方向,Y軸方向,Z軸方向及びこの３軸
夫々の回転方向の６自由度航法精度が、GPS（Global Po
sitioning Satellie:全地球的位置決め衛星）12から自
己（チェィサー）及びターゲットの位置情報が、サンセ
ンサ，アースセンサ，スターセンサ，等のセンサ13から
自己の姿勢情報が入力されている。処理シーケンスのス
テップ14において自己及びターゲットの初期情報を得
る。次いでステップ15において、ターゲットが遠いか近
いかを判断する。例えば、ターゲットが画像として判断
できる状態となったとき近距離と判断する。ターゲット
が未だ遠距離の場合には、ステップ16に進んだレーザレ
ーダ等の遠距離センサを使用した接近をはかる。

ステップ15において、ターゲットが近距離に近付いた
と判断された場合には、ステップ17に進んで近距離セン
サを使用した本発明によるラフマッチングとファインマ
ッチングとを組付せた階層的認識方法によりターゲット
の姿勢，距離を推定し、宇宙飛翔体同士のランデブー・
ドッキングを実現する。

第３図は近距離センサによる認識フローチャートを示
しており、まずステップ21においてフラグｍをセットし
て１回目はファインマッチングまで行なう。次いでステ
ップ22においてカウンタｎを１にセットしてから、ステ
ップ23に進んで視覚センサよりターゲットの画像入力を
行なう。ステップ24においては、本発明の面ベクトル及
び重心ベクトルを使用したラフマッチングを行ない、ス
テップ25においてマッチングを認識したら、ターゲット
の運動を把握しているフライトマネイジャー26を介して
アクチュエータ27を駆動し接近をはかる。

また一方では、ステップ25においてラフマッチングを
確認したらステップ28に進んでｎ＝ｋかあるいはｍ＝１
かを判断する。すなわちこのステップでは、最初のマッ
チングとｎ＝ｋ回に１回はファインマッチングまでを行
なう処理を示している。ステップ28において否定判定の
場合には、ステップ29に進んでターゲットの運動方程式
が既知であることから、次のターゲットの相対位置及び
姿勢を計算により予測する。この計算結果はステップ24
のマッチングに利用される。次いでステップ30におい
て、ｎを１つインクリメントしてステップ22〜25までの
処理を行なう。ステップ28において肯定判定の場合に
は、ステップ31に進んでファインマッチングを行ない、
ステップ32においてファインマッチングを確認したらフ
ライトマネイジャー26を介してアクチュエータ27を駆動
して接近をはかる。

これと共に、ドッキングの終了を判別し（ステップ3
3）、終了してない場合にはステップ34でｎ＝０として
ステップ29に進む。

このように本応用例においては、本発明によるラフマ
ッチング（ステップ24）とファインマッチング（ステッ
プ21）とを組合せて、宇宙飛翔体同士のランデブー・ド
ッキングを達成する。

第４図は上述したような宇宙飛翔体同士のランデブー
・ドッキングのラフマッチングに適用する本発明方法の
第１実施例のブロック図を示す。

まずモデル側では、第１図のブロック１に対応するブ
ロック40でミッション要求等の外部からの指示によりタ
ーゲットに対応するモデルを選択する。

ここで、モデルは凸多面体で、各面の頂点座標で表現
されている。例えば第５図に示す如き六面体の場合頂点
A,B,C,D,E,F,G,Hの夫々の原点０よりの座標データで構
成されている。

第１図のブロック２に対応するブロック41では、モデ
ルの各面毎に面ベクトルと重心位置ベクトルとを得る。
例えば第５図のモデルで、面ABCDについてはベクトルBC
とベクトルBAとの外積を求め、この外積のベクトルの大
きさを1/2とし、同様にベクトルDAとベクトルDCの外積
のベクトルの大きさを1/2としこれらを加える。次にベ
クトルの始点を物体内部の適当な回転中心０′に移動し
てその座標系（ｘ′,y′,z′）における面ABCDの面ベク
トルとする。また、回転中心０′を始点とし、かつ面AB
CDの重心を終点とするベクトルを面ABCの重心位置ベク
トルとする。このようにして他の面AEHD,AEFB,BCGF,CDH
G,EFGH夫々の面ベクトル及び重心位置ベクトルを得る。
面ベクトルはその面の面積と面の向きを表わし、重心位
置ベクトルはその面の位置即ち隣接面関係を表わしてい
る。

ブロック42では例えばＺ軸をカメラの視線軸として、
各面夫々の面ベクトルの視線軸成分および各面夫々の重
心位置ベクトルの視線軸を除く２軸つまりＸ軸,Y軸成分
を抽出する。

また、各面夫々の面ベクトルの視線軸成分の比、つま
り視線方向の面積比を求め、かつ各面夫々の重心位置ベ
クトルのＸ軸,Y軸成分から重心位置の順列を決める。

この順列の決定は例えば各面の重心位置ベクトルのＸ
軸成分を比較し、これが小さいものから順に順列の番号
を付す。上記Ｘ軸成分が同一の場合にはＹ軸成分が小さ
いものを先とする。

この後、重心位置ベクトルと１対１に対応する面ベク
トルの視線軸成分を上記順列の番号の順に並べかえ、か
つ視線方向の面積比を並べかえる。

一方カメラ側では、第１図のブロック３に対応するブ
ロック50でターゲットをカメラで補足し、ターゲットの
画像情報を領域分割する。例えば次のようにして行な
う。得られた画像情報をその濃淡に応じて画素毎に濃度
ラベルを振り、濃淡画像情報を生成し、これを記憶する
（ブロック51）。つまり、この情報の濃度ラベルが例え
ば「１」の画素と「２」の画素とは濃度が異なることを
表わす。

次に、ステップ52で濃淡画像情報の濃度ラベルが同一
である領域を見付けることにより領域分割を行なう。こ
のように画像の特徴を抽出してターゲット画像を構成す
る複数の領域を切り出す。上記のブロック51,52がブロ
ック４に対応する。

この後、ブロック５に対応するブロック53で切り出さ
れた各領域毎に画素数をカウントして面積を求め、かつ
重心位置を求める。ここで、カメラ側は濃淡画像情報の
各画素の画面上の位置に対応してアドレスを割り当てて
おり、切り出された領域を構成する画素のアドレス及び
画素数から一意的にこの領域の重心位置が求められる。

例えば、第６図（Ａ）に示す如く、ターゲット画像60
が領域60a,60b,60cから構成れている。この場合、同図
（Ｂ）に示す如く、領域60a,60b,60c夫々の面積S₁,S₂,S
₃及び重心位置（x₁,y₁），（x₂,y₂），（x₃,y₃）を求め
る。これによって面積S₁,S₂,S₃夫々は重心位置（x₁,
y₁），（x₂,y₂），（x₃,y₃）の関数S₁（x₁,y₁）,S
₂（x₂,y₂）,S₃（x₃,y₃）とで最終的に同図（Ｃ）に示す
如く表現される。

更に、重心位置（x₁,y₁），（x₂,y₂），（x₃,y₃）の
ｘ座標x₁,y₁,x₃を比較し、これが小さいものから順に順
列の番号を付す。上記ｘ座標が同一の場合にはｙ座標が
小さいものを先とする。

この後、面積S₁,S₂,S₃を上記順列の番号の順に並べか
えて面積S₁（１）,S₂（２）,S₃（３）とし、その面積比
を求める。

１次マッチング（第１図のブロック７に対応するブロ
ック43）ではモデルの各面の順列の番号とターゲットの
各面積の順列の番号とを一致させて、モデルの各面の面
積比とターゲットの面積比とをマッチングする。

マッチングがとれない場合にはブロック44において、
モデルの面ベクトル及び重心位置ベクトルを物体に固定
した座標系をオイラー角（ψ，θ，φ）で回転させる。
最初のｘ軸の回りの回転をψ、次のｙ軸の回りの回転を
θ、最後のｚ軸の回りの回転をφとすると、ψを０〜36
0゜、各ψに対しθを−90゜〜＋90゜、各ψとθに対し
φを−90゜〜＋90゜に変化させる。

上記のブロック42,44が第１図のブロック６に対応す
る。

そして各回転位置毎にブロック42で各面の面ベクトル
の視線軸成分及び重心位置ベクトルのＸ軸,Y軸成分を抽
出し、ブロック43で１次マッチングを行なう。これによ
ってターゲットの姿勢が推定される。

１次マッチングがとれると、ブロック45で２次マッチ
ングが行なわれる。ここでは、モデルの各面夫々の面ベ
クトルの視線軸成分そのものと、ターゲットの各領域の
面積S₁,S₂,S₃そのものとのマッチングを行ない、これま
で比のみでマッチングしていたものを大きさでマッチン
グする作業を行なう。

マッチングがとれない場合にはブロック46において、
モデルの各面の面ベクトルとの大きさを所定比率で順次
拡大・縮小し、ブロック42を経て再びブロック45の２次
マッチングを行なう。

この２次マッチングによってターゲットまでの距離の
推定が行なわれ、拡大・縮小の比率から距離を知ること
ができる。

上記第１実施例ではターゲットの各領域の面積をその
重心位置に応じた順の順列に並べ、モデルの各面の面積
を重心位置ベクトルに応じた順の順列に並べ１枚マッチ
ングを行なっている。このため両者の各面積の比較順が
絞られ、各面積の位置関係のもつ役割を果しており、１
次マッチングを高速に行なうことができる。

第７図は本発明方法の第２実施例のブロック図を示
す。同図中、第４図と同一部分には同一符号を付し、そ
の説明を省略する。

第７図においてブロック62では例えばＺ軸を視線軸と
して、各面夫々の面ベクトルを抽出し、視線方向の面積
比を求める。また、各面の重心位置ベクトルをXY平面に
投影し、XY平面におけるモデルの投影像の重心を原点に
とって、XY平面に投影された各面の重心位置ベクトルの
終点夫々を極座標表現する。このとき、上記原点と、面
ベクトルの視線軸成分が最大のものに対応する重心位置
ベクトルの終点とを通る直線を基準線とする。

この後、上記重心位置の極座標で角度θが小さいもの
から順に順列の番号を付し、角度θが同一の場合には距
離ｒが小さいものを先とする。

次に面ベクトルの視線軸成分を順列の番号の順に並べ
かえ、視線方向面積比を並べかえる。

ブロック63では、ブロック53と同様に第８図（Ａ）に
示すターゲット画像60から第８図（Ｂ）に示す如く領域
60a,60b,60c夫々の面積S₁,S₂,S₃を得、また、ターゲッ
ト画像60の重心Ｇを原点として領域60a〜60c夫々の重心
位置を極座標表現する。このとき原点と面積最大の領域
60bの重心位置とを通る直線を基準線とする。これによ
って各面積は重心位置の関数S₁（r₁,θ_１）,S₂（r₂,θ
_２）,S₃（r₃,θ_３）と表現される。

この後、極座標で角度θが小さいものから順に順列の
番号を付し、角度θが同一の場合は距離ｒを小さいもの
を先とする。

次に面積を順列の番号の順に並べかえて面積S
₁（１）,S₂（２）,S₃（３）とし、また面積比を並べか
える。

ブロック64においては、視線軸（Ｚ軸）回りの回転は
行なわれず、視線軸を除く２軸つまりＸ軸回りの回転θ
及びＹ軸回りの回転φとしてθを−90゜〜＋90゜、各θ
に対しφを−90゜〜＋90゜に変化させる。

このため、１次マッチングがとれてもターゲットをと
らえる視線軸回り（ロール方向）の自由度が残されてい
る。

ブロック65の３次のマッチングでは、ターゲット側に
おけるＸ軸に対する極座標の基準線の傾きと、モデル側
におけるＸ軸に対する極座標の基準線の傾きとの角度差
を求め、視線軸を中心とする回転角度を求める。これに
よって１次マッチングで残されていた視線軸を中心とす
る回転角度が認識され姿勢の推定が完了する。

ところで、ターゲット側における極座標の基準線を決
定するとき、面積S₁とS₂とが同一で最大である場合には
基準線の決定が困難である。

このような場合、ブロック63において第８図（Ｃ），
（Ｄ），（Ｅ）夫々に示す如く、ターゲット画像60の重
心Ｇである原点と領域60a,60b,60c夫々の重心との間直
線夫々を基準線とし、第８図（Ｃ）〜（Ｅ）夫々の極座
標表現を行なう。

この後、各極座標について面積S₁,S₂,S₃を並べかえ
る。これによって視線方向の面積比は並びが異なった３
種類ができる。

更にブロック43においては、モデルの各面の面積比を
まず第８図（Ｃ）に示す順のターゲットの面積比とマッ
チングし、ここでマッチングがとれなければ次に第８図
（Ｄ）に示す順のターゲットの面積比とマッチングし、
ここでマッチングがとれなければ次に第８図（Ｅ）に示
す順のターゲットの面積比とマッチングする。

このように１次マッチングのマッチング回数は多くな
るが、基準線の決定にこだわる必要がなくなる。

上記第２実施例では、ターゲット各領域の面積及びモ
デルの各面の面積夫々を順列に並べて１次マッチングを
行なう点で第１実施例と同様に１次マッチングを高速と
することができる。

更に１次マッチング時にブロック64で視線軸を中心と
する回転を行なう必要がないため、１次マッチングを高
速化できる。

このように、ターゲットの画像を領域として分割する
ため曖昧さから落とされてしまう情報も柔軟に取り扱う
ことができ、モデルを構成する各面の視線軸方向の面積
及比とターゲットの各領域の面積比とを順序づけてマッ
チングさせるという特徴的な情報のマッチングを行なう
ため姿勢推定を高速に行なうことができる。

〔発明の効果〕

上述の如く、本発明の３次元空間に存在する物体の姿
勢推定方法によれば、３次元物体を領域として分割する
ため曖昧さから落とされてしまう情報を柔軟に取り扱う
ことができ、モデルを構成する各面の視線軸方向の面積
比と３次元物体の各領域の面積比とを順序づけてマッチ
ングさせるという特徴的な情報のマッチングを行なうた
め、姿勢推定を短時間にかつ曖昧さに強いマッチングを
行なうことができ、必要以上のデータ比較を行なうこと
がないので処理が高速化され、実用上きわめて有用であ
る。

【図面の簡単な説明】

第１図は本発明方法の原理ブロック図、 第２図は本発明方法を適用したランデブー・ドッキング
の概略フローチャート、 第３図は第２図の近距離センサによる接近の詳細フロー
チャート、 第４図，第７図は夫々本発明方法の各実施例のブロック
図、 第５図，第６図，第８図は本発明方法を説明するための
図、 第９図は従来方法の一例のブロック図、 第10図は従来方法での情報の欠落を説明するための図で
ある。 図において、 10はナビゲーションフィルタ、 11はINS、 12はGPS、 13はセンサ、 14〜25,28〜34はステップ、 26はフライトマネイジャー、 27はアクチュエータ、 40〜53,63〜65はブロック、 60はターゲット画像、 60a〜60cは領域 を示す。

Claims (1)

(57)【特許請求の範囲】
1. 【請求項１】対象物体のモデルを構成する複数の面夫々
   について、各面の向き及び大きさを表す面ベクトルと、
   該各面の重心位置を表す重心ベクトルとを抽出し、 カメラで撮像された３次元空間に存在する物体の画像情
   報を領域分割し、各領域の面積比を、該各領域に対応す
   る該重心位置のｘ座標,y座標または該重心位置ベクトル
   の角度の大小関係に応じた順列で抽出し、 該モデルの各面の面ベクトルと重心位置ベクトルとを３
   次元空間で回転させて、任意に選定した視線軸に対する
   該各面の面ベクトルの視線軸成分比を、該各面の重心位
   置ベクトルの該視線軸と直交する平面上に投影した位置
   ベクトルのＸ軸,Y軸成分または角度の大小関係に応じた
   順列で抽出し、 該各面の面ベクトルの視線軸成分比と各領域の面積比と
   を順列が同一となるものどうしでマッチングを行い、 該マッチングがとれたときに該３次元空間に存在する物
   体の姿勢を推定することを特徴とする３次元空間に存在
   する物体の姿勢推定方法。