JPH0798214A - 視覚に基く三次元位置および姿勢の認識方法ならびに視覚に基く三次元位置および姿勢の認識装置 - Google Patents

Abstract

(57)【要約】 【目的】 平面マーカを撮像手段により斜方向から撮影
し、簡単な画像処理を実施するだけで、高速，高精度且
つロバストに位置姿勢認識を行う。 【構成】 平面状のマーカ６には正方形部７，８が一つ
の頂点で接するようにして横方向に並べて表示されてい
る。ＣＣＤカメラ５によりこれを撮影して三次元位置認
識装置９により位置姿勢を認識する。画像データに基づ
いて、画像処理を行って二値化されたデジタル画像デー
タとし、マーカ６の線分を６本の直線として検出する。
複比不変の法則に基づいて消失点の座標を求めると共
に、二次形式最小化問題として消失点の座標を求めて単
位方向ベクトルの最適ベクトルを求める。これにより、
位置姿勢を求めて座標変換行列を計算する。直線を精度
良く検出でき、簡単な画像処理計算で精度良く検出でき
る。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、単眼視カメラにより撮
影した画像情報に基づいてその単眼視カメラの三次元位
置および姿勢を認識するようにした視覚に基く三次元位
置および姿勢の認識方法ならびに視覚に基く三次元位置
および姿勢の認識装置に関する。

【０００２】

【従来の技術】近年、例えば、自律移動ロボット等のよ
うに、形状および絶対座標位置が既知である対象物体に
対して接近するように移動し、ロボットハンドの先端に
設けたマニピュレータにより対象物体を把持するといっ
た能動的操作を行うようにしたものがある。このような
自律移動ロボット等においては、ＣＣＤカメラなどの撮
像手段を搭載してその二次元的な画像情報に基いて対象
物体に対する自己の三次元的な相対位置を認識すること
が必要になる。この場合、位置認識を行うための装置に
おいては、高精度且つ迅速に計算を行って三次元的な位
置のデータを求めることが要求され、しかも安価に実現
できる構成が望まれている。

【０００３】そこで、従来では、比較的簡単なものとし
て、例えば、特開平３−１６６０７２号公報あるいは特
開平３−１６６０７３号公報に示されるようなものが考
えられている。すなわち、これらにおいては、対象装置
に固定された形状および寸法が既知の特殊な幾何学形状
（マーカと呼ぶ）をカメラにより撮影し、そのマーカの
二次元的な画像情報に基いて、その重心位置を計算する
等の方法により対象装置に対するＣＣＤカメラの相対的
な三次元の位置関係を求め、これに基づいてロボットハ
ンドを移動制御するようにしたものである。

【０００４】

【発明が解決しようとする課題】しかしながら、上述の
ような従来構成のものでは、ＣＣＤカメラによるマーカ
の撮影を、ＣＣＤカメラの光軸に対する垂直面内にマー
カ表示面が位置するようにＣＣＤカメラを移動させて行
うようにしており、マーカの撮影画面がマーカ表示面の
二次元的な相対位置と１対１の対応関係となるようにす
る必要があるため、次のような実用上の不具合がある。

【０００５】すなわち、例えば、対象物を把持するため
のロボットハンドにカメラを取り付けていると、対象物
の近傍に配置されたマーカを垂直方向から撮影するため
にロボットハンドを適切な位置まで移動させる必要があ
るが、このとき、ロボットハンド先端のマニピュレータ
が物品の把持状態にある場合などにおいては、その移動
範囲に制約を受けることになる。

【０００６】また、ＣＣＤカメラによりマーカを視野内
に入るように移動させるために、マーカの設置領域が狭
いと、ロボットハンドがその作業環境と接触して互いに
損傷を与える不具合が生ずるため、マーカの設置領域を
ロボットハンドが十分に移動可能となるような作業環境
にする必要があり、作業環境に大きく制約を与えること
になるという不具合がある。

【０００７】ところが、従来、このような不具合を解消
すべく、マーカから斜方向に離れた位置から撮影するこ
とにより、カメラの位置姿勢を高速且つ高精度でロバス
ト（ロバストとは、一般的に、処理や認識の結果がノイ
ズや環境条件の変動によってあまり影響を受けないこと
を定性的に示すことばとして用いられている。）に認識
できるものがなかった。

【０００８】本発明は、上記事情に鑑みてなされたもの
で、その目的は、撮像手段により平面マーカを撮影して
三次元位置を認識するときに、平面マーカに対して撮像
手段を垂直方向に対向させることなく、斜方向から撮影
した画像情報に基づいて、簡単な二値化画像処理を実施
するだけで、高速且つ高精度でしかもロバストな位置姿
勢認識を行うことができるようにした視覚に基く三次元
位置および姿勢の認識方法およびその装置を提供するこ
とにある。

【０００９】

【課題を解決するための手段】本発明の視覚に基く三次
元位置および姿勢の認識方法は、所定寸法を有し互いに
直交する２本の線分とそれら２本の線分にそれぞれ平行
な線分とを含んで囲まれた図形を背景部分に対して濃淡
情報あるいは色情報の変化により平面上に表示されたマ
ーカを備え、前記撮像手段からの画像データに基づいて
前記マーカの画像領域を選択すると共に、その選択され
た画像領域内の画像データの値の変化点を検出して輪郭
点位置を抽出する画像処理ステップと、この画像処理ス
テップにて抽出された前記輪郭点位置のデータに基づい
て、撮影画面上における前記マーカの直交する線分およ
びそれらに平行な線分をそれぞれ含む直線のパラメータ
を最小二乗推定法により計算する直線あてはめ処理ステ
ップと、前記マーカに表示された平行な線分に対応して
前記直線あてはめ処理ステップにて計算された直線の交
点である消失点のデータに基づいて前記マーカの単位方
向ベクトルを計算する単位方向ベクトル計算処理ステッ
プと、前記マーカの直交する線分の交点を原点としてそ
れら各線分の方向およびマーカ面に垂直でその原点を通
る法線方向を基準方向としたマーカ座標系を設定し、前
記単位方向ベクトルに基づいて前記マーカの位置姿勢を
示すパラメータを計算する位置姿勢計算処理ステップ
と、この位置姿勢計算処理ステップにおける前記撮像手
段の位置姿勢を示すパラメータ計算結果に基づいて、前
記撮像手段の基準座標系から見た前記マーカの位置姿勢
を表す座標変換行列を計算する座標変換行列計算処理ス
テップとを設けたところに特徴を有する。

【００１０】また、前記マーカを、背景部分に対して濃
淡情報あるいは色情報の変化により表示された少なくと
も２個の長方形状あるいは正方形状をなす図形から構成
し、それらの図形を互いに一つの頂点で接すると共にそ
の頂点を含む隣接する辺同士が直角をなすように配置し
て構成すると良い。

【００１１】そして、前記直線あてはめ処理ステップに
おいては、前記画像処理ステップにて抽出された前記輪
郭点位置のデータに基づいて、前記マーカのひとつの線
分に対応して直線を複数本検出し、それらの直線の平均
化処理を行うことにより１本の直線を求めてその直線パ
ラメータ決定することが好ましい。

【００１２】さらに、前記単位方向ベクトル計算処理ス
テップにおいては、複比不変の法則を利用して前記消失
点の候補値を３つ以上計算し、それらの候補値の２次元
座標データに基づいて前記単位方向ベクトルを計算する
ことが好ましい。

【００１３】また、前記単位方向ベクトル計算処理ステ
ップにおいては、前記マーカの３本以上の平行な線分に
それぞれ対応して得られた直線について、それらの交点
に対する単位方向ベクトルをモーメント行列に対する二
次形式最小化問題として解くことにより前記単位方向ベ
クトルを計算すると良い。

【００１４】そして、本発明の視覚に基く三次元位置お
よび姿勢の認識装置は、所定寸法を有する少なくとも２
本の平行な線分からなる２組の平行線群が互いに直交す
るように配置された状態を背景部分に対して濃淡情報あ
るいは色情報の変化により平面上に表示されたマーカ
と、撮影画像の画像データを出力する撮像手段と、前記
マーカの形状寸法を示すマーカデータが記憶された記憶
手段と、前記撮像手段からの画像データに基づいて前記
マーカの画像領域を選択すると共に、その選択された画
像領域内の画像データの値の変化点を検出して輪郭点位
置を抽出する画像処理手段と、この画像処理手段により
抽出された前記輪郭点位置のデータに基づいて前記マー
カの直交する平行線群のそれぞれの線分を含む直線のパ
ラメータを最小二乗推定法により求めるようにした直線
あてはめ処理手段と、前記２組の平行線群の各線分を含
む直線の前記撮影画面上における交点である消失点のデ
ータに基づいて前記マーカの単位方向ベクトルを計算す
る単位方向ベクトル計算処理手段と、前記直線あてはめ
処理手段により求められた２本の直交する直線とその交
点を通る法線とを座標系の軸としたマーカ座標系を設定
し、このマーカ座標系と前記単位方向ベクトルに基づい
て前記マーカの位置姿勢を計算する位置姿勢計算処理手
段と、この位置姿勢計算処理手段による前記撮像手段の
位置姿勢を示すパラメータ計算結果に基づいて、前記撮
像手段の基準座標系から見た前記マーカの位置姿勢を表
す座標変換行列を計算する座標変換行列計算手段とを設
けて構成したところに特徴を有する。

【００１５】また、前記マーカを、背景部分に対して濃
淡情報あるいは色情報の変化により表示された少なくと
も２個の長方形状あるいは正方形状をなす図形から構成
し、それらの図形を互いに一つの頂点で接すると共にそ
の頂点を含む隣接する辺同士が直角をなすように配置す
ることが好ましい。

【００１６】そして、前記直線あてはめ処理手段を、前
記画像処理手段により抽出された前記輪郭点位置のデー
タに基づいて、前記マーカのひとつの線分に対応して直
線を複数本検出し、それらの直線の平均化処理を行うこ
とにより１本の直線を求めてその直線パラメータ決定す
るように構成すると良い。

【００１７】さらに、前記単位方向ベクトル計算処理手
段を、複比不変の法則を利用して前記消失点の候補値を
３つ以上計算し、それらの候補値の２次元座標データに
基づいて前記単位方向ベクトルを計算するように構成す
ることができる。

【００１８】また、前記単位方向ベクトル計算処理手段
を、前記マーカの３本以上の平行な線分にそれぞれ対応
して得られた直線について、それらの交点に対する単位
方向ベクトルをモーメント行列に対する二次形式最小化
問題として解くことにより前記単位方向ベクトルを計算
するように構成することもできる。

【００１９】

【作用】請求項１記載の視覚に基く三次元位置および姿
勢の認識方法によれば、マーカに対して三次元空間の斜
方向に位置する撮像手段によりそのマーカを撮影したと
きに、以下のように各処理ステップを実行することによ
り、マーカに対する撮像手段の三次元位置および姿勢を
認識することができるようになる。

【００２０】すなわち、まず、撮像手段により撮影され
たマーカの画像データは、画像処理ステップにおいて、
マーカの画像領域が選択されると共に、その選択された
画像領域内の画像データの値の変化点を検出して輪郭点
位置が抽出されるようになる。このとき、マーカの各線
分は濃淡情報あるいは色情報の変化で表示されてるの
で、線分の検出を線幅を持たない図形の輪郭線から求め
ることができるようになり、画像処理が簡単になると共
に、検出誤差を極力低減して輪郭点位置を精度良く抽出
することができる。

【００２１】次に、直線あてはめ処理ステップにおい
て、上述の画像処理ステップにて抽出された輪郭点位置
のデータに基づいてマーカの直交する線分およびそれら
に平行な線分をそれぞれ含む直線のパラメータが最小二
乗推定法により求められるようになる。

【００２２】そして、単位方向ベクトル計算処理ステッ
プにおいては、マーカを撮像手段により斜方向から撮影
していることにより、マーカに表示された平行な線分に
対応して直線あてはめ処理ステップにて計算された直線
が無限遠で交差するため、その交点である消失点の２次
元座標が計算され、これから単位方向ベクトルが計算さ
れるようになる。

【００２３】続いて、位置姿勢計算処理ステップにおい
て、マーカの直交する線分の交点を原点としてそれら各
線分の方向およびマーカ面に垂直でその原点を通る法線
方向を基準方向としたマーカ座標系が設定され、単位方
向ベクトル計算処理ステップにて求められた単位方向ベ
クトルに基づいてマーカの位置姿勢を示すパラメータが
計算されるようになる。

【００２４】そして、座標変換行列計算処理ステップに
おいて、撮像手段の位置姿勢を示すパラメータ計算結果
に基づき、撮像手段の基準座標系から見たマーカの位置
姿勢を表す座標変換行列が計算されるようになる。

【００２５】この場合、撮像手段によりマーカを斜方向
から撮影することにより三次元位置および姿勢が認識で
きると共に、マーカの図形に対する簡単な計算処理を実
行するだけで良いので、高速且つ高精度で認識できると
共に、ロバストな処理結果を得ることができるようにな
る。

【００２６】請求項２記載の視覚に基く三次元位置およ
び姿勢の認識方法によれば、マーカを、２個以上の長方
形状あるいは正方形状をなす簡単な図形で表示している
ので、マーカの位置姿勢認識の計算処理を簡単化するこ
とができるようになると共に、例えば、図形を横方向に
並べるように配置することにより、撮像手段の撮影画面
に対してマーカの傾き角度が大きくなる場合でも、線分
の奥行き方向の距離変化を少なくして焦点ずれによる誤
差を低減して精度良く検出することができるようにな
る。

【００２７】請求項３記載の視覚に基く三次元位置およ
び姿勢の認識方法によれば、直線あてはめ処理ステップ
において、画像処理ステップで抽出された前記輪郭点位
置のデータに基づいて、マーカのひとつの線分に対応し
て直線を複数本検出し、それらの直線の平均化処理を行
うことにより１本の直線を求めてその直線パラメータを
決定するので、例えば、撮像手段により撮影されたマー
カが、焦点ずれにより輪郭線が直交する線分として検出
できない場合でも、それらの輪郭点位置から得られる複
数本の直線から平均化することにより、より正確な直線
として検出することができるようになる。

【００２８】請求項４記載の視覚に基く三次元位置およ
び姿勢の認識方法によれば、単位方向ベクトル計算処理
ステップにおいて、複比不変の法則を利用して消失点の
候補値を３つ以上計算し、それらの候補値の２次元座標
データに基づいて単位方向ベクトルを計算するので、例
えば、平行な線分に対応して検出された直線に誤差が生
じていてそれらの交点から得られる消失点が複数存在す
る場合でも、より正確な単位方向ベクトルを検出するこ
とができるようになる。

【００２９】請求項５記載の視覚に基く三次元位置およ
び姿勢の認識方法によれば、単位方向ベクトル計算処理
ステップにおいて、マーカの３本以上の平行な線分にそ
れぞれ対応して得られた直線について、それらの交点に
対する単位方向ベクトルをモーメント行列に対する二次
形式最小化問題として解くことにより計算するので、上
述と同様にして検出された直線に誤差が生じている場合
でも、より正確な単位方向ベクトルを検出することがで
きるようになる。

【００３０】請求項６記載の視覚に基く三次元位置およ
び姿勢の認識装置によれば、マーカに対して三次元空間
の斜方向に位置する撮像手段によりそのマーカを撮影し
たときに、以下のようしてマーカに対する撮像手段の三
次元位置および姿勢を認識することができるようにな
る。

【００３１】すなわち、撮像手段によりマーカを三次元
空間の斜方向から撮影したときに得られるマーカの画像
データに対して、画像処理手段により、マーカの画像領
域が選択されると共に、その選択された画像領域内の画
像データの値の変化点を検出して輪郭点位置を抽出す
る。このとき、マーカの各線分は濃淡情報あるいは色情
報の変化で表示されてるので、線分の検出を線幅を持た
ない図形の輪郭線から求めることができるようになり、
画像処理が簡単になると共に、検出誤差を極力低減して
輪郭点位置を精度良く抽出することができる。

【００３２】次に、直線あてはめ処理手段により、抽出
された輪郭点位置のデータに基づいてマーカの直交する
線分およびそれらに平行な線分をそれぞれ含む直線のパ
ラメータを最小二乗推定法により求める。次に、マーカ
を撮像手段により斜方向から撮影していることにより、
マーカに表示された平行な線分に対応して直線あてはめ
処理手段により計算された直線が無限遠位置では交差す
ることを利用して、単位方向ベクトル計算処理手段によ
り、その交点である消失点の２次元座標を計算し、これ
から単位方向ベクトルを計算するようになる。

【００３３】続いて、位置姿勢計算処理手段により、マ
ーカの直交する線分の交点を原点としてそれら各線分の
方向およびマーカ面に垂直でその原点を通る法線方向を
基準方向としたマーカ座標系を設定し、単位方向ベクト
ル計算手段により求められた単位方向ベクトルに基づい
てマーカの位置姿勢を示すパラメータを計算する。そし
て、撮像手段の位置姿勢を示すパラメータ計算結果に基
づき、座標変換行列計算処理手段により、撮像手段の基
準座標系から見たマーカの位置姿勢を表す座標変換行列
を計算する。

【００３４】この場合、撮像手段によりマーカを斜方向
から撮影することにより三次元位置および姿勢が認識で
きると共に、マーカの図形に対する簡単な計算処理を実
行するだけで良いので、高速且つ高精度で認識できると
共に、ロバストな処理結果を得ることができるようにな
る。

【００３５】請求項７記載の視覚に基く三次元位置およ
び姿勢の認識装置によれば、マーカを、２個以上の長方
形状あるいは正方形状をなす簡単な図形で表示している
ので、マーカの位置姿勢認識の計算処理を簡単化するこ
とができるようになると共に、例えば、図形を横方向に
並べるように配置することにより、撮像手段の撮影画面
に対してマーカの傾き角度が大きくなる場合でも、線分
の奥行き方向の距離変化を少なくして焦点ずれによる誤
差を低減して精度良く検出することができるようにな
る。

【００３６】請求項８記載の視覚に基く三次元位置およ
び姿勢の認識装置によれば、直線あてはめ処理手段によ
り、画像処理ステップで抽出された前記輪郭点位置のデ
ータに基づいて、マーカのひとつの線分に対応して直線
を複数本検出し、それらの直線の平均化処理を行うこと
により１本の直線を求めてその直線パラメータ決定する
ので、例えば、撮像手段により撮影されたマーカが、焦
点ずれにより輪郭線が直交する線分として検出できない
場合でも、それらの輪郭点位置から得られる複数本の直
線から平均化することにより、より正確な直線として検
出することができるようになる。

【００３７】請求項９記載の視覚に基く三次元位置およ
び姿勢の認識装置によれば、単位方向ベクトル計算処理
手段により、複比不変の法則を利用して消失点の候補値
を３つ以上計算し、それらの候補値の２次元座標データ
に基づいて単位方向ベクトルを計算するので、例えば、
平行な線分に対応して検出された直線に誤差が生じてい
てそれらの交点から得られる消失点が複数存在する場合
でも、より正確な単位方向ベクトルを検出することがで
きるようになる。

【００３８】請求項１０記載の視覚に基く三次元位置お
よび姿勢の認識装置によれば、単位方向ベクトル計算処
理手段により、マーカの３本以上の平行な線分にそれぞ
れ対応して得られた直線について、それらの交点に対す
る単位方向ベクトルをモーメント行列に対する二次形式
最小化問題として解くことにより計算するので、上述と
同様にして検出された直線に誤差が生じている場合で
も、より正確な単位方向ベクトルを検出することができ
るようになる。

【００３９】

【実施例】以下、本発明を工場内などの軌道上を走行す
る自律移動ロボットに搭載する三次元位置認識装置に適
用した場合の第１の実施例について図１ないし図１４を
参照しながら説明する。全体構成の概略を示す図２にお
いて、自律走行ロボット（図示せず）は、工場内などの
軌道上を走行して所定の作業位置で停止して、本体上部
に取り付けられたロボットハンド１により対象物を把持
したり載置したりするなどの作業を行うものであり、所
定の位置に、センサからの検出信号や外部情報などに基
づいて停止されるようになっている。

【００４０】ロボットハンド１は、自律走行ロボットの
本体上部に回動可能に保持されたアーム部２とこのアー
ム部２の先端に回動可能に設けられたヘッド部３から構
成され、このヘッド部３に対象物を把持するためのマニ
ピュレータ４が設けられている。そして、ロボットハン
ド１は、図示しない制御装置からの制御信号に応じてア
ーム部２，ヘッド部３およびマニピュレータ４が駆動制
御されるようになっている。

【００４１】この場合、アーム部２は、基端部２ａにお
いて垂直方向の軸を回転軸として水平面内で回動可能と
され、中間部に設けられた関節２ｂにおいて水平方向の
軸を回転軸として垂直面内で回動可能とされている。ま
た、アーム部２の先端部に取り付けられたヘッド部３
は、水平方向の軸を回転軸として垂直面内で回動可能と
され、ヘッド部３の上下方向の軸を回転軸として回動可
能とされると共に、ヘッド部３の突出方向の軸を回転軸
として捩じる方向に回動可能とされている。

【００４２】また、このヘッド部３の上部には、マニピ
ュレータ４の動作に支障を来さない位置に撮像手段とし
ての小形のＣＣＤカメラ５が取り付けられており、その
ヘッド部３と共に移動制御されるようになっている。そ
して、このＣＣＤカメラ５により撮影された画像データ
に基づいて、後述するようにして、ＣＣＤカメラ５の位
置と姿勢が計算により求められると、その結果に応じて
制御装置によりロボットハンド１の動作が駆動制御され
るようになっている。

【００４３】マニピュレータ４により作業する位置に
は、対象物との位置関係があらかじめ決められた関係で
配置されたマーカ６が配設されている。このマーカ６
は、図３に示すように、例えば、白地の背景部分に黒色
で表示された２個の正方形部７および８を横に並べた状
態で配置された構成となっている。

【００４４】この場合、正方形部７および８は、ひとつ
の頂点で接する状態に配置され、正方形部７の４つの辺
ａ１，ａ２，ａ３，ａ４および正方形部８の４つの辺ｂ
１，ｂ２，ｂ３，ｂ４のうち、辺ａ１および辺ｂ２と、
辺ａ２および辺ｂ１とがそれぞれ一直線上に位置するよ
うに配置されている。そして、正方形部７および８が接
する点をマーカ座標系の原点Ｏとしている。なお、マー
カ６の大きさは、ＣＣＤカメラ５で撮影したときにその
撮影視野内に全体が十分な大きさで且つ歪みのない状態
で入るように設定されている。

【００４５】さて、三次元位置姿勢認識装置９は、具体
的には、例えば全体の計算制御を実行させるコンピュー
タ等から構成されるもので、その内部の機能的なブロッ
ク構成は図１に示すように構成されている。すなわち、
図１において、三次元位置姿勢認識装置９の全体は、概
略構成として、画像処理手段としての画像処理部１０，
直線あてはめ手段としての直線あてはめ部１１，単位方
向ベクトル計算手段としての消失点計算部１２，位置姿
勢計算手段としての位置姿勢計算部１３，座標変換行列
計算手段としての座標変換行列計算部１４および記憶手
段としてのメモリ１５から構成されており、それぞれ
は、後述するフローチャートの流れに従って計算処理が
実行されるようになっている。

【００４６】そして、画像処理部１０は、前処理計算部
１６，サンプル点抽出部１７から構成されている。前処
理計算部１６は、前処理計算としてＣＣＤカメラ５が撮
影したマーカ６の画像データを二値化したデジタル画像
データとし、マーカ６の存在する画像領域を切り出して
出力する。サンプル点抽出部１７は、切り出された画像
領域のマーカ画像が存在する位置の座標データから、正
方形部７および８の輪郭点を抽出してサンプル点座標デ
ータとして出力する。

【００４７】直線あてはめ部１２は、最小二乗推定部１
８および直線パラメータ出力部１９から構成されてお
り、最小二乗推定部１８により、後述するようにしてサ
ンプル点座標データから撮影画面上における直線を検出
すると、直線パラメータ出力部１９により、検出した直
線のパラメータが出力されるようになっている。

【００４８】消失点計算部１２は、モーメント行列計算
部２０，最小固有値計算部２１，単位固有ベクトル計算
部２２，最適ベクトル計算部２３および消失点座標推定
部２４から構成される。これらは、検出された直線のパ
ラメータから、後述するように、マーカ６の平行な線分
に対応する直線のパラメータに基づいて、それらの直線
が無限遠点で撮影画面上において交差する点である消失
点を計算するもので、ここでは二次形式最小化問題とし
てこれを解くようにしている。

【００４９】位置姿勢計算部１３は、焦点距離計算部２
５，姿勢計算部２６およびマーカ原点位置計算部２７か
ら構成される。これらは、求められた撮影画面上におけ
る消失点の二次元座標データとマーカ６の原点Ｏを基準
としたマーカ座標系とに基づいて、最適ベクトル，焦点
距離，姿勢ベクトルおよびマーカ原点位置を計算するよ
うになっている。

【００５０】なお、メモリ１５は、マーカ６の形状寸法
等を示すマーカデータが記憶されると共に、ＣＣＤカメ
ラ５の撮影画角やアスペクト比（画面の縦横の比率）な
どの、あらかじめわかっている情報が記憶されている。

【００５１】座標変換行列部１４は、上述のようにして
求められたマーカ６の位置姿勢を表すデータから、ＣＣ
Ｄカメラ５の座標系を基準としたマーカ６の位置姿勢を
表す座標変換行列を計算して求めるようになっており、
その計算結果を、データ出力部２８を介して自律移動ロ
ボットの動作を制御する制御装置等に与えるようになっ
ている。

【００５２】次に、本実施例の作用について、図４ない
し図１３をも参照し、以下、各処理ステップに対応して
順次説明する。まず、自律走行ロボットは、作業位置近
傍に近付くと、別途に設けられたセンサ等により所定の
停止領域内に停止され、その位置でＣＣＤカメラ５によ
りマーカ６の撮影を行なうように、あらかじめティーチ
ングされている。この場合、ＣＣＤカメラ５は、図５に
示すように、マーカ６を正方形部７，８が横に並んだ状
態で斜め上方向から撮影するようになっており、これに
より、ＣＣＤカメラ５の撮影視野内にマーカ６の全体像
が歪みなく取り込まれるようになっている。

【００５３】そして、三次元位置認識装置８は、図４に
示す認識プログラムの流れを示すフローチャートにした
がって三次元位置姿勢の認識を行うための処理ステップ
を実行するようになる。

【００５４】（ａ）画像処理ステップ（Ｓ１〜Ｓ４） さて、まず、プログラムがスタートされると、各種の初
期値設定を実行した（ステップＳ１）後、前処理計算部
１６により、ＣＣＤカメラ５で撮影されたマーカ６の画
像データを入力するようになる（ステップＳ２）。次
に、前処理計算部１６は、ステップＳ３にて、前処理計
算を実行するようになる。この前処理計算においては、
ＣＣＤカメラ５から入力した画像データを所定のしきい
値で二値化することにより、明暗を示すアナログデータ
を「１」，「０」などの二値化されたデジタル画像デー
タに変換すると共に、マーカ６が撮影された画像領域を
切り出して出力するようになる。

【００５５】これにより、マーカ６に黒色で描かれた正
方形部７および８に該当する位置のデジタル画像データ
と白地の背景部分に該当する位置のデジタル画像データ
とが、それぞれ「１」および「０」のいずれかのデジタ
ル画像データとして識別されるようになる。

【００５６】この後、サンプル抽出部１７は、ステップ
Ｓ４で、デジタル画像データに基づいて、マーカ６の正
方形部７および８の部分の辺ａ１〜ａ４，ｂ１〜ｂ４に
相当する輪郭線をサンプリングする。マーカ６の正方形
部７および８の各辺を通る直線は、図６に平面図で示す
ように、６本の直線Ｌｍ１〜Ｌｍ６がある。

【００５７】サンプル抽出においては、これらの直線Ｌ
ｍ１〜Ｌｍ６に対応する撮影画面上における直線Ｌ１〜
Ｌ６を検出するために、図７に示すように、撮影画面上
で横方向つまりｘ軸方向に走査してデジタル画像データ
値が変化する位置の画素の座標データを記憶していく。
これにより、撮影画面上における正方形部７，８の各辺
ａ１〜ａ４，ｂ１〜ｂ４上のサンプリング点を、撮影画
面上の二次元座標で示された例えばｓ個のサンプル抽出
点ＳＰｎ［ｘｎ，ｙｎ］として得ることができる（ただ
しｎ＝１，２，…，ｓ）。

【００５８】（ｂ）直線あてはめ処理ステップ（Ｓ５〜
Ｓ９） 次に、ステップＳ５に進むと、直線あてはめ処理部１１
の最小二乗推定部１８および直線パラメータ出力部１９
において、得られたｓ個のサンプル抽出点ＳＰｎの座標
データに基づいて、マーカ６の直線Ｌｍ１〜Ｌｍ６に対
応する撮影画面上における直線Ｌ１〜Ｌ６（図８参照）
を最小二乗推定法により計算し、得られた直線のパラメ
ータを出力するようになる。

【００５９】この場合、最小二乗推定法を適用して計算
を実施するにあたって、本実施例においては、直線Ｌ１
〜Ｌ６の一般式を式（１）のようなヘッセの標準形で表
現し、この方程式にサンプル抽出点ＳＰｎ［ｘｎ，ｙ
ｎ］の座標データを適用し、後述する（付録）に示した
ような導出過程により計算を実行して直線パラメータ
［θｉ，ｈｉ］を計算している。

【００６０】なお、（付録）に記載した導出過程及び以
下の探索過程における導出では、小野寺，金谷氏による
『計算射影幾何学によるカメラキャリブレーション』
（情報処理学会研究報告６８−ＣＶ−１，１９９０−０
９）を参考にした。

【００６１】

【数１】

【００６２】なお、上述のようにして直線の検出を行う
場合に、例えば、ＣＣＤカメラ５による撮影画面がフォ
ーカスずれを起こしていることにより、マーカ６の画像
にボケが生ずる場合があると、図９に示すように、輪郭
線があいまいになっていわゆるグレーゾーンＧＬが生ず
ることがある。そして、本実施例においては、このよう
なフォーカスずれに対応して、ステップＳ６において、
直線Ｌｉが検出されるべき部分のグレーゾーンＧＬに対
応して複数本の直線Ｌｉ１，Ｌｉ２，…を検出してそれ
らの直線パラメータの平均化処理を行うことにより一つ
の直線Ｌｉのパラメータを決定するようにしている。

【００６３】そして、以上のようにして、例えば直線Ｌ
１についての計算が終了すると、ステップＳ７にてその
直線のパラメータ［θ１，ｈ１］を求め、ステップＳ
８，Ｓ９を経て再びステップＳ５〜Ｓ７を実行して次の
直線Ｌ２について同様にして直線のパラメータ［θ２，
ｈ２］を求め、以下、同様にして直線Ｌ３〜Ｌ６につい
ても直線のパラメータを計算して求めるようになる。

【００６４】（ｃ）マーカ単位方向ベクトル計算処理ス
テップ（Ｓ１０〜Ｓ１７） 次に、ＣＣＤカメラ５により撮影しているマーカ６の姿
勢を示す単位方向ベクトルを上述のようにして得られた
６本の直線Ｌ１〜Ｌ６のパラメータ［θｉ，ｈｉ］（ｉ
＝１〜６）のデータに基づいて計算する。

【００６５】この場合、マーカ６の面内の平行な直線Ｌ
ｍ１〜Ｌｍ３の組および直線Ｌｍ４〜Ｌｍ６の組に対し
て、撮影画面上の対応する平行な直線Ｌ１〜Ｌ３の組お
よびＬ４〜Ｌ６の組においては、ＣＣＤカメラ５により
マーカ６を斜方向から撮影していることから、それぞれ
の平行な直線の組における無限遠に対応する点が撮像画
面上のある１点でそれぞれ交差するようになる。

【００６６】このとき、それらの交点をそれぞれ消失点
Ｖ１，Ｖ２とすると、マーカ６の座標系を設定すること
により、そのマーカ座標原点Ｏと撮影画面上のその消失
点Ｖ１，Ｖ２との二次元座標データに基づいて幾何学的
性質を利用すると、ＣＣＤカメラ５の位置姿勢を一意的
に決めることができる。

【００６７】ところが、上述の各直線の組においてその
消失点Ｖ１あるいはＶ２を計算すると、一般に、図１０
に示すように、複数個の消失点候補Ｖ１ａ，Ｖ１ｂ，…
およびＶ２ａ，Ｖ２ｂ，…が得られるので、これらの消
失点候補のデータから最適な消失点Ｖ１あるいはＶ２を
求めるべく、以下の２つの方法により別々に計算し、そ
れらの結果から最適な消失点Ｖ１，Ｖ２に対応するデー
タを計算するようにしている。

【００６８】すなわち、第１には（ア）複比不変の法則
を利用して計算する方法であり、第２には（イ）二次形
式最小化問題として計算する方法である。なお、これら
の計算方法を用いるにあたっては、あらかじめ実験等に
より得られたデータに基づいて、両者の計算方法の結果
をどのように合成して用いるかを指標αで指定されてお
り、ステップＳ１０およびＳ１３においてはその指標α
の値に応じて選択が分かれるようになっている。

【００６９】この場合、指標α値の設定は、あらかじめ
実験等により得られたデータに基づいて０≦α≦１の範
囲で決定されるようになっている。そして、指標αの値
は、後述する消失点Ｖ１，Ｖ２にそれぞれ対応する最適
ベクトルｍｏ１，ｍｏ２を求める際の指標となるもの
で、次式で定義されるように、（ア）複比不変の法則に
基づいて求めた単位方向ベクトルｍ１′，ｍ２′と、
（イ）二次形式最小化問題として求めた単位方向ベクト
ルｍ１，ｍ２とを合成して最適ベクトルｍｏ１，ｍｏ２
を求めるための算出式（２）の合成比率を設定している
ものである。

【００７０】そして、指標αの値が、α＝１である場合
には単位方向ベクトルｍ１，ｍ２のみを用い、α＝０で
ある場合には単位方向ベクトルｍ１′，ｍ２′のみを用
いて最適ベクトルｍｏ１，ｍｏ２を求めることに相当し
ている。また、指標αの値が、０＜α＜１である場合に
は、両者を用いて式（２）から最適ベクトルｍｏ１，ｍ
ｏ２を求めるようになっている。

【００７１】

【数２】

【００７２】（ア）複比不変の法則を用いた計算過程 ここでは、指標αの値が「１」でない場合に対応してお
り、ステップＳ１０にて「ＹＥＳ」と判断してステップ
Ｓ１１，Ｓ１２の計算ステップに進んだときに実行され
る。

【００７３】複比不変の法則というのは、「直線から直
線への射影変換は、その直線上の相異なる３点がどのよ
うに写像されるかを指定すると、それで一意に決定され
る」という幾何学的な法則であり、これを利用すると、
例えば、図１１に示すように、直線Ｌ上（マーカ６上の
直線Ｌｍ１〜Ｌｍ６に対応する）の点Ａ，Ｂ，Ｃ，Ｄが
直線Ｌ′上（撮影画面上の直線Ｌ１〜Ｌ６に対応する）
の点Ａ′，Ｂ′，Ｃ′，Ｄ′の各点に写像されるとする
と、これらの各点間で区切られる線分の長さの関係は、
式（３）のようになる。

【００７４】

【数３】

【００７５】そこで、直線Ｌ上の線分ＡＢおよび線分Ｂ
Ｃの長さ寸法を「１」とし、点Ｄｎが点Ａから距離ｎの
位置にあるとすると、式（３）の左辺は式（４）のよう
になり、このとき、直線Ｌ′上の線分Ａ′Ｄｎ′の長さ
寸法をｄｎとおくと、式（３）の右辺は式（５）のよう
になる。式（３）の関係から式（４）と式（５）の各右
辺の値は等しいから、式（６）が得られる。

【００７６】

【数４】

【００７７】すると、式（６）を直線Ｌ′上の長さ寸法
ｄｎについて解くと、式（７）が得られる。ここで、直
線Ｌの点Ｄｎが無限遠にあるときを考えると、これに対
応する直線Ｌ′上の点Ｄｎ′は式（７）中のｎが無限大
になるときの長さ寸法ｄｎの値であるから、式（８）が
得られる。つまり、この長さ寸法の値ｄｎが消失点に相
当する位置の距離になるのである。

【００７８】

【数５】

【００７９】そこで、本実施例における消失点Ｖ１，Ｖ
２を求めるために、図１２に示すように、撮影画面上の
直線Ｌ１〜Ｌ６の各交点を特徴点Ｐ１〜Ｐ９として抽出
し、これらの各特徴点と求めるべき消失点Ｖ１，Ｖ２の
消失点座標（ｘｖ１，ｙｖｉ），（ｘｖ２、ｙｖ２）と
の関係を式（８）に示した点Ａ〜Ｃに対応させると表１
のように表すことができるので、上述の式（８）の関係
を用いて各座標値を求めれば良い。

【００８０】

【表１】

【００８１】すなわち、式（８）の関係から、各直線Ｌ
１〜Ｌ６における各特徴点間の距離ｄｖ（ｔ）（ｔ＝１
〜６）は、式（９）のように表せるから、消失点Ｖ１，
Ｖ２の各座標値ｘｖ１，ｙｖ１，ｘｖ２，ｙｖ２の値は
式（１０）〜（１３）のように表すことができる。

【００８２】

【数６】

【００８３】そして、消失点Ｖ１，Ｖ２の撮影画面上で
の二次元座標は、上述のようにＶ１（ｘｖ１，ｙｖ
１），Ｖ２（ｘｖ２、ｙｖ２）としているので、ＣＣＤ
カメラ５からこの消失点Ｖ１，Ｖ２に向かうベクトル
は、三次元空間における実際のマーカ６の各直線Ｌｍ１
〜Ｌｍ３の方向およびＬｍ４〜Ｌｍ６の方向を示す単位
ベクトルとなる。

【００８４】したがって、図１３に示すように、マーカ
６の原点Ｏから消失点Ｖ１に向かう単位方向ベクトルを
ｍ１′（ｘ軸方向）、消失点Ｖ２に向かう単位方向ベク
トルをｍ２′（ｙ軸方向）とすると、それぞれの単位方
向ベクトルｍ１′およびｍ２′は、ＣＣＤカメラ５から
撮影画面の中心点までの距離をｆ（焦点距離）とすると
式（１４），（１５）のように表すことができる。つま
り、ステップＳ１２では、この式（１４），（１５）に
基づいて単位方向ベクトルｍ１′，ｍ２′を計算するよ
うになっているのである。

【００８５】

【数７】

【００８６】（イ）二次形式最小化問題としての計算過
程 さて、指標αの値が「０」でない場合には、ステップＳ
１３になると、ここで「ＹＥＳ」と判断してステップＳ
１４〜Ｓ１６においてこの計算過程が実行されるように
なる。

【００８７】まず、ステップＳ１４においては、モーメ
ント行列計算部２０により、マーカ６の各直線Ｌｍ１〜
Ｌｍ６に対して、単位方向ベクトルｍ１，ｍ２を求める
べく、ステップＳ７にて計算した直線Ｌ１〜Ｌ６の直線
パラメータ［θｉ，ｈｉ］（ｉ＝１〜６）に基づいて、
式（１６），（１７）で定義されるモーメント行列Ｍ
１，Ｍ２を計算する。

【００８８】この場合、三次元空間中の直線Ｌｍｉに対
して、ＣＣＤカメラ５の視点Ｐと直線Ｌｍｉを含む平面
の単位法線ベクトルを直線ＬｍｉのＮベクトルｎｉと定
義すると、直線Ｌｍｉが撮影画面上において直線Ｌｉと
して式（１）のように表現されているから、そのＮベク
トルｎｉは式（１８）のようになる。ただし、式（１
８）におけるｆ′はＣＣＤカメラ５の仮の焦点距離を示
すデータである。

【００８９】この仮の焦点距離ｆ′の値は、メモリ１５
内に記憶されているデータで、本実施例においては、計
算精度の向上を図るために、ＣＣＤカメラ５の仕様書の
公称値あるいは既に算出した計算結果を用いるようにし
ている。

【００９０】

【数８】

【００９１】そして、以下のステップＳ１５，１６にお
いては、３本の直線の組（Ｌｍ１〜Ｌｍ３，Ｌｍ４〜Ｌ
ｍ６）のそれぞれの組における直線の交点となる消失点
Ｖ１，Ｖ２のＮベクトルｍ１，ｍ２を、それぞれモーメ
ント行列Ｍ１，Ｍ２に対する二次形式最小化問題すなわ
ち、 ｍｉ＝｛［ｍ，Ｍｉｍ］］を最小にするｍ｝ として解き、ｍ１，ｍ２をＭ１，Ｍ２の最小固有値に対
する単位固有ベクトルとして計算するものである。よく
知られているように、二次形式［ｍ，Ｍｍ］（ｉ＝１，
２）を最小にする単位ベクトルｍ１，ｍ２は、それぞれ
式（１６），（１７）のモーメント行列の最小固有値に
対する固有ベクトルである。

【００９２】すなわち、ステップＳ１５においては、最
小固有値計算部２１により、上述の最小固有値を計算
し、続くステップＳ１６においては、単位固有ベクトル
計算部２２により上述の単位固有ベクトルｍ１，ｍ２を
計算する。

【００９３】この場合、Ｎベクトルがｍである点Ｐ（点
ＰのＮベクトルとは、視点から空間座標系の点Ｐに向う
単位ベクトルとして定義される）がすべての直線Ｌｉ上
にあれば、すべてのｉに対して、内積（ｍ，ｎｉ）＝０
となるから、次式（１９）を最小にする単位ベクトルを
考えると上述のように二次形式最小化問題に帰着するの
である。そして、以上の結果から、単位固有ベクトルｍ
１，ｍ２が式（２０），（２１）のように求まる。な
お、式（２０）、（２１）中のｆはＣＣＤカメラ５の真
の焦点距離である。

【００９４】

【数９】

【００９５】以上のようにして（ア）の複比不変の法則
に基づく単位方向ベクトルｍ１′，ｍ２′と（イ）の二
次形式最小化問題として求めた単位固有ベクトルｍ１，
ｍ２に基づいて、ステップＳ１７において、最適ベクト
ル計算部２３により、前述の式（２）にしたがって、指
標αの値に応じて最適ベクトルｍｏ１，ｍｏ２を計算
し、これを単位方向ベクトルｍ１，ｍ２として決定する
ようになる。

【００９６】（ｄ）位置姿勢計算処理ステップ（Ｓ１８
〜Ｓ２０） 次に、ステップＳ１８に進むと、焦点距離計算部２５に
より、ＣＣＤカメラ５の真の焦点距離ｆを上述の結果に
基づいて計算するようになる。すなわち、単位方向ベク
トルｍ１，ｍ２は、互いに直交しているから、その内積
をとると０になるから式（２２）が成り立ち、これから
ｆについて解くと真の焦点距離ｆの値が式（２３）のよ
うに得られる。

【００９７】

【数１０】

【００９８】続いて、ステップＳ１９に進むと、姿勢計
算部２６により、図１３に示したマーカ６の姿勢ベクト
ル［ｍｓ１，ｍｓ２，ｍｓ３］が計算される。この場
合、姿勢ベクトルｍｓ３は、姿勢ベクトルｍｓ１とｍｓ
２との外積として与えられるから、式（２４）のように
なる。そして、姿勢ベクトルｍｓ１，ｍｓ２は、前述の
最適ベクトルｍｏ１，ｍｏ２および真の焦点距離ｆの値
から式（２５）のように計算することができる。

【００９９】

【数１１】

【０１００】次に、ステップＳ２０になると、マーカ原
点位置計算部２７により、マーカ６の原点位置Ｏが計算
される。すなわち、まず、ステップＳ７にて求められた
６本の直線Ｌ１〜Ｌ６のパラメータと真の焦点距離ｆの
値に基づいて、各直線Ｌ１〜Ｌ６のＮベクトルｎ１〜ｎ
６を次式（２６）にしたがって計算し、これらから、各
特徴点Ｐｋ（ｋ＝１〜９）のＮベクトルｍＰｋを式（２
７）にしたがって計算する。なお、点ＰのＮベクトルｍ
Ｐは、ＣＤＤカメラ５の視点Ｖから点Ｐに向かう単位方
向ベクトルとして定義されるものである。

【０１０１】そして、これらから、各点ＰｋについてＣ
ＣＤカメラ５の視点とマーカ６の原点Ｏとの間の距離を
もとめてそれらの平均値を計算することによりマーカ原
点位置までの距離Ｒを式（２８）にしたがって計算す
る。

【０１０２】

【数１２】

【０１０３】（ｅ）座標変換行列計算処理ステップ（Ｓ
２１） このようにして、マーカ６の原点位置姿勢が求まると、
ステップＳ２１に進み、座標変換行列計算部１４にて、
上述の結果に基づいてＣＣＤカメラ５の空間座標系に相
対的な位置と向きを示す座標変換行列を、周知の計算方
法により回転変換行列および平行移動変換行列とに分け
た位置パラメータとして計算するようになる。

【０１０４】このような本実施例によれば、マーカ６と
して、２個の簡単な正方形部７，８を白黒の二値化され
た情報で表示した構成としてこれをＣＣＤカメラ５によ
り斜方向から撮影して三次元の位置姿勢を認識できるよ
うにしたので、マーカ６の線分を正方形部７，８のエッ
ジデータとして抽出することができるようになり、線幅
をもたない線分として精度良く検出することができ、し
かも、ＣＣＤカメラ５をマーカ６の正面まで移動させな
くとも検出することができるので、ＣＣＤカメラ５の撮
影位置の自由度を大きくしてロボットハンド１の移動範
囲の制約を低減すると共に、画像処理を簡単化しなが
ら、高速，高精度且つロバストにＣＣＤカメラ５の位置
姿勢の認識を行うことができる。

【０１０５】また、本実施例によれば、直線あてはめ処
理部１１において直線の検出を行う場合に、ステップＳ
６において、複数の直線を検出してそれらの平均をとる
ことにより１本の直線を決定するようにしたので、ＣＣ
Ｄカメラ５によるマーカ６の撮影画面が焦点ずれでグレ
ーゾーンＧＬが生じている場合においても、つねに、正
確な直線の検出処理を行うことができる。

【０１０６】そして、本実施例においては、単位方向ベ
クトルの計算において、指標αの設定により、複比不変
の法則を利用した方法によって消失点Ｖ１，Ｖ２を求め
ると共に、二次形式最小化問題として消失点Ｖ１，Ｖ２
のＮベクトルを求めることにより単位方向ベクトルの最
適ベクトルを計算するようにしているので、必要に応じ
て適切な単位方向ベクトルを計算することができる。

【０１０７】図１５ないし図１９は、本発明の第２ない
し第６の実施例を示すもので、いずれも、第１の実施例
におけるマーカ６の代わるマーカ２９〜３３を示してた
ものである。

【０１０８】すなわち、図１５に示す第２の実施例にお
いては、マーカ２９を、第１の実施例で用いたマーカ６
の背景部分に対する正方形部７，８の白黒を反転させた
状態の正方形部３４，３５を表示して形成したもので、
これによっても第１の実施例と同様の作用効果を得るこ
とができるものである。

【０１０９】また、図１６に示す第３の実施例において
は、マーカ３０を、正方形部７、８に代えて２個の長方
形３６，３７を直角をなすようにハ字状に配置した状態
に形成したもので、図の方向からＣＣＤカメラ５により
撮影するようにティーチングして位置姿勢の認識を行う
ようにするものである。

【０１１０】そして、この場合には、第１の実施例と同
様の効果が得られると共に、ＣＣＤカメラ５によりマー
カ２９を撮影するときの傾斜角度が大きい場合でも、遠
方側に位置する長方形部３６，３７の辺ａ３，ｂ３を撮
影画面上で比較的長い寸法のデータとして入力すること
ができるので、直線の検出精度を向上させることができ
るものである。

【０１１１】図１７に示す第４の実施例においては、マ
ーカ３１を、大きい正方形部３８の内部に小さい正方形
部３９を異なる濃淡情報で示すと共に、それらの正方形
部３８および３９の対角線が重なる位置に配置したもの
であり、このようなマーカ３１を用いても第１の実施例
と同様の作用効果を得ることができる。

【０１１２】図１８に示す第５の実施例においては、マ
ーカ３２を、マーカ６で使用した正方形部７，８に相当
する正方形部４０，４１を上下２段に配置して表示した
ものであり、図１９に示す第６の実施例においては、マ
ーカ３３を、３個の正方形部４２〜４４と直角三角形部
４５とを互いに等間隔だけ離間した状態で配置して表示
したもので、これらのマーカ３２，３３によっても第１
の実施例と同様の作用効果を得ることができるものであ
る。

【０１１３】なお、上記各実施例においては、マーカ
６，２９〜３３を白黒の濃淡情報により表示する構成の
場合について説明したが、これに限らず、色情報により
表示する構成とすることもできるものであり、この場合
には、カラー撮影可能なＣＣＤカメラを用いると共に、
前処理計算部にてカラー信号を二値化するように構成す
れば良い。

【０１１４】また、上記各実施例においては、本発明を
自律移動ロボットの三次元位置認識装置に適用した場合
について説明したが、ロボット自身は固定された状態と
され、作業対象がロボットの近傍に移動してくる構成の
三次元位置認識装置等に適用することもできるものであ
る。

【０１１５】（付録）最小二乗推定法による直線の検出 前述した直線あてはめ処理ステップにおける、最小二乗
推定法による直線の検出のための導出過程について説明
する。前述のように、直線の式をヘッセの標準形で示す
と、式（ａ）のように表され、Ｎ個の抽出サンプル点Ｐ
ｉ（ｘｉ，ｙｉ）との距離は式（ｂ）のようになる。し
たがって、式（ｃ）のように定義した関数の値を最小に
するときのθおよびｈの値を求めることにより式（ａ）
の直線の方程式が得られる。

【０１１６】

【数１３】

【０１１７】ここで、以下の導出過程で用いる種々の記
号を次式（ｄ）ないし（ｆ）のように定義する。

【０１１８】

【数１４】

【０１１９】さて、式（ｃ）をｈで微分して「０」とお
くと、ｈは式（ｇ）のように得られる。式（ｇ）の結果
を式（ｃ）に代入し、θで微分して「０」とおくと、θ
は式（ｈ）のように得られる。

【０１２０】

【数１５】

【０１２１】この場合、ｈおよびθの選択条件は、それ
ぞれ式（ｉ）および式（ｊ）とすれば良い。

【０１２２】

【数１６】

【０１２３】

【発明の効果】以上説明したように、本発明の視覚に基
く三次元位置および姿勢の認識方法ならびに視覚に基く
三次元位置および姿勢の認識装置によれば、以下のよう
な優れた効果を奏する。

【０１２４】すなわち、請求項１記載の視覚に基く三次
元位置および姿勢の認識方法によれば、平面上に所定寸
法を有し互いに直交する２本の線分とそれら２本の線分
にそれぞれ平行な線分とを含んで囲まれた図形を背景部
分に対して濃淡情報あるいは色情報の変化により表示し
たマーカを、三次元空間の斜方向に位置する撮像手段に
より撮影し、画像処理ステップにおいて輪郭点位置を抽
出し、直線あてはめ処理ステップにおいてマーカの線分
に対応する直線のパラメータが最小二乗推定法により求
め、単位方向ベクトル計算処理ステップにおいて単位方
向ベクトルを計算し、位置姿勢計算処理ステップにおい
て、マーカの位置姿勢を示すパラメータを計算して撮像
手段の基準座標系から見たマーカの位置姿勢を表す座標
変換行列を計算するようにしたので、マーカに表示され
た直交する線分を精度良く検出することができると共
に、単純な図形に対する画像処理を行うことから計算処
理が簡単になり、高速，高精度且つロバストに三次元位
置および姿勢が認識できるという優れた効果を奏する。

【０１２５】請求項２記載の視覚に基く三次元位置およ
び姿勢の認識方法によれば、マーカを、２個以上の長方
形状あるいは正方形状をなす簡単な図形で表示している
ので、マーカの位置姿勢認識の計算処理を簡単化するこ
とができると共に、例えば、図形を横方向に並べるよう
に配置することにより、撮像手段の撮影画面に対してマ
ーカの傾き角度が大きくなる場合でも、線分の奥行き方
向の距離変化を少なくして焦点ずれによる誤差を低減し
て精度良く検出することができるようになるという優れ
た効果を奏する。

【０１２６】請求項３記載の視覚に基く三次元位置およ
び姿勢の認識方法によれば、直線あてはめ処理ステップ
において、画像処理ステップで抽出された前記輪郭点位
置のデータに基づいて、マーカのひとつの線分に対応し
て直線を複数本検出し、それらの直線の平均化処理を行
うことにより１本の直線を求めてその直線パラメータ決
定するので、例えば、撮像手段により撮影されたマーカ
が、焦点ずれにより輪郭線が直交する線分として検出で
きない場合でも、より正確な直線として検出することが
できるという優れた効果を奏する。

【０１２７】請求項４記載の視覚に基く三次元位置およ
び姿勢の認識方法によれば、単位方向ベクトル計算処理
ステップにおいて、複比不変の法則を利用して消失点の
候補値を３つ以上計算し、それらの候補値の２次元座標
データに基づいて単位方向ベクトルを計算するので、例
えば、平行な線分に対応して検出された直線に誤差が生
じていてそれらの交点から得られる消失点が複数存在す
る場合でも、より正確な単位方向ベクトルを検出するこ
とができるという優れた効果を奏する。

【０１２８】請求項５記載の視覚に基く三次元位置およ
び姿勢の認識方法によれば、単位方向ベクトル計算処理
ステップにおいて、マーカの３本以上の平行な線分にそ
れぞれ対応して得られた直線について、それらの交点に
対する単位方向ベクトルをモーメント行列に対する二次
形式最小化問題として解くことにより計算するので、上
述と同様にして検出された直線に誤差が生じている場合
でも、より正確な単位方向ベクトルを検出することがで
きるようになるという優れた効果を奏する。

【０１２９】また、請求項６記載の視覚に基く三次元位
置および姿勢の認識装置によれば、前記請求項１記載の
マーカを利用すると共に各処理ステップを実行する各手
段を設けて構成したので、マーカに表示された直交する
線分を精度良く検出することができると共に、単純な図
形に対する画像処理を行うことから計算処理が簡単にな
り、高速，高精度且つロバストに三次元位置および姿勢
が認識できるという優れた効果を奏する。

【０１３０】請求項７記載の視覚に基く三次元位置およ
び姿勢の認識装置によれば、マーカを、２個以上の長方
形状あるいは正方形状をなす簡単な図形で表示している
ので、マーカの位置姿勢認識の計算処理を簡単化するこ
とができると共に、例えば、図形を横方向に並べるよう
に配置することにより、撮像手段の撮影画面に対してマ
ーカの傾き角度が大きくなる場合でも、線分の奥行き方
向の距離変化を少なくして焦点ずれによる誤差を低減し
て精度良く検出することができるようになるという優れ
た効果を奏する。

【０１３１】請求項８記載の視覚に基く三次元位置およ
び姿勢の認識装置によれば、直線あてはめ処理手段によ
り、画像処理ステップで抽出された前記輪郭点位置のデ
ータに基づいて、マーカのひとつの線分に対応して直線
を複数本検出し、それらの直線の平均化処理を行うこと
により１本の直線を求めてその直線パラメータ決定する
ので、例えば、撮像手段により撮影されたマーカが、焦
点ずれにより輪郭線が直交する線分として検出できない
場合でも、それらの輪郭点位置から得られる複数本の直
線から平均化することにより、より正確な直線として検
出することができるという優れた効果を奏する。

【０１３２】請求項９記載の視覚に基く三次元位置およ
び姿勢の認識装置によれば、単位方向ベクトル計算処理
手段により、複比不変の法則を利用して消失点の候補値
を３つ以上計算し、それらの候補値の２次元座標データ
に基づいて単位方向ベクトルを計算するので、例えば、
平行な線分に対応して検出された直線に誤差が生じてい
てそれらの交点から得られる消失点が複数存在する場合
でも、より正確な単位方向ベクトルを検出することがで
きるという優れた効果を奏する。

【０１３３】請求項１０記載の視覚に基く三次元位置お
よび姿勢の認識装置によれば、単位方向ベクトル計算処
理手段により、マーカの３本以上の平行な線分にそれぞ
れ対応して得られた直線について、それらの交点に対す
る単位方向ベクトルをモーメント行列に対する二次形式
最小化問題として解くことにより計算するので、上述と
同様にして検出された直線に誤差が生じている場合で
も、より正確な単位方向ベクトルを検出することができ
るという優れた効果を奏する。

【図面の簡単な説明】

【図１】本発明の第１の実施例を示す全体のブロック構
成図

【図２】ロボットハンドとマーカとの関係を示す図

【図３】マーカの平面図

【図４】認識プログラムの概略的なフローチャート

【図５】ＣＣＤカメラによるマーカの撮影位置と姿勢

【図６】マーカの各線分と検出すべき直線とを示す説明
図

【図７】撮影画面上のサンプル抽出点の走査方向を示す
説明図

【図８】マーカの正方形部と検出すべき直線との関係を
示す説明図

【図９】フォーカスずれを生じているときの補償を行う
場合の説明図

【図１０】３本以上の直線に対する消失点が複数発生す
る場合の説明図

【図１１】複比不変の法則の説明図

【図１２】撮影画面上の特徴点と直線との関係の説明図

【図１３】マーカの姿勢ベクトルと座標系の説明図

【図１４】最適な消失点方向ベクトルの説明図

【図１５】本発明の第２の実施例を示す図３相当図

【図１６】本発明の第３の実施例を示す図３相当図

【図１７】本発明の第４の実施例を示す図３相当図

【図１８】本発明の第５の実施例を示す図３相当図

【図１９】本発明の第６の実施例を示す図３相当図

【符号の説明】

１はロボットハンド、２はアーム部、３はヘッド部、４
はマニピュレータ、５はＣＣＤカメラ（撮像手段）、６
はマーカ、７，８は正方形部、９は三次元位置姿勢認識
装置、１０は画像処理部（画像処理手段）、１１は直線
あてはめ部（直線あてはめ手段）、１２は消失点計算部
（単位方向ベクトル計算手段）、１３は位置姿勢計算部
（位置姿勢計算手段）、１４は座標変換行列計算部（座
標変換行列計算手段）、１５はメモリ（記憶手段）、１
６は前処理計算部、１７はサンプル点抽出部、１８は最
小二乗推定部、１９は直線パラメータ出力部、２０はモ
ーメント行列計算部、２１は最小固有値計算部、２２は
単位固有ベクトル計算部、２３は最適ベクトル計算部、
２４は消失点座標推定部、２５は焦点距離計算部、２６
は位置姿勢計算部、２７はマーカ原点計算部、２８はデ
ータ出力部、２９〜３３はマーカである。

【手続補正書】

【提出日】平成５年１０月１４日

【手続補正１】

【補正対象書類名】明細書

【補正対象項目名】００８２

【補正方法】変更

【補正内容】

【００８２】

【数６】

【手続補正２】

【補正対象書類名】明細書

【補正対象項目名】０１１８

【補正方法】変更

【補正内容】

【０１１８】

【数１４】

【手続補正３】

【補正対象書類名】明細書

【補正対象項目名】０１２２

【補正方法】変更

【補正内容】

【０１２２】

【数１６】

Claims (10)

【特許請求の範囲】
1. 【請求項１】 所定寸法を有し互いに直交する２本の線
   分とそれら２本の線分にそれぞれ平行な線分とを含んで
   囲まれた図形を背景部分に対して濃淡情報あるいは色情
   報の変化により平面上に表示されたマーカを備え、この
   マーカを斜方向から撮像手段により撮影したときの画像
   データに基づいて、以下（ａ）〜（ｅ）の処理ステップ
   を実行することを特徴とした視覚に基く三次元位置およ
   び姿勢の認識方法。 （ａ）前記撮像手段からの画像データに基づいて前記マ
   ーカの画像領域を選択すると共に、その選択された画像
   領域内の画像データの値の変化点を検出して輪郭点位置
   を抽出する画像処理ステップ、 （ｂ）この画像処理ステップにて抽出された前記輪郭点
   位置のデータに基づいて、撮影画面上における前記マー
   カの直交する線分およびそれらに平行な線分をそれぞれ
   含む直線のパラメータを最小二乗推定法により計算する
   直線あてはめ処理ステップ、 （ｃ）前記マーカに表示された平行な線分に対応して前
   記直線あてはめ処理ステップにて計算された直線の交点
   である消失点のデータに基づいて前記マーカの単位方向
   ベクトルを計算する単位方向ベクトル計算処理ステッ
   プ、 （ｄ）前記マーカの直交する線分の交点を原点としてそ
   れら各線分の方向およびマーカ面に垂直でその原点を通
   る法線方向を基準方向としたマーカ座標系を設定し、前
   記単位方向ベクトルに基づいて前記マーカの位置姿勢を
   示すパラメータを計算する位置姿勢計算処理ステップ、 （ｅ）この位置姿勢計算処理ステップにおける前記撮像
   手段の位置姿勢を示すパラメータ計算結果に基づいて、
   前記撮像手段の基準座標系から見た前記マーカの位置姿
   勢を表す座標変換行列を計算する座標変換行列計算処理
   ステップ。
2. 【請求項２】 前記マーカは、背景部分に対して濃淡情
   報あるいは色情報の変化により表示された少なくとも２
   個の長方形状あるいは正方形状をなす図形から構成さ
   れ、それらの図形は互いに一つの頂点で接すると共にそ
   の頂点を含む隣接する辺同士が直角をなすように配置さ
   れていることを特徴とする請求項１記載の視覚に基く三
   次元位置および姿勢の認識方法。
3. 【請求項３】 前記直線あてはめ処理ステップにおいて
   は、前記画像処理ステップにて抽出された前記輪郭点位
   置のデータに基づいて、前記マーカのひとつの線分に対
   応して直線を複数本検出し、それらの直線の平均化処理
   を行うことにより１本の直線を求めてその直線パラメー
   タ決定することを特徴とする請求項１または請求項２記
   載の視覚に基く三次元位置および姿勢の認識方法。
4. 【請求項４】 前記単位方向ベクトル計算処理ステップ
   においては、複比不変の法則を利用して前記消失点の候
   補値を３つ以上計算し、それらの候補値の２次元座標デ
   ータに基づいて前記単位方向ベクトルを計算することを
   特徴とする請求項２または３記載の視覚に基く三次元位
   置および姿勢の認識方法。
5. 【請求項５】 前記単位方向ベクトル計算処理ステップ
   においては、前記マーカの３本以上の平行な線分にそれ
   ぞれ対応して得られた直線について、それらの交点に対
   する単位方向ベクトルをモーメント行列に対する二次形
   式最小化問題として解くことにより前記単位方向ベクト
   ルを計算することを特徴とする請求項２、３または４記
   載の視覚に基く三次元位置および姿勢の認識方法。
6. 【請求項６】 所定寸法を有する少なくとも２本の平行
   な線分からなる２組の平行線群が互いに直交するように
   配置された状態を背景部分に対して濃淡情報あるいは色
   情報の変化により平面上に表示されたマーカと、 撮影画像の画像データを出力する撮像手段と、 前記マーカの形状寸法を示すマーカデータが記憶された
   記憶手段と、 前記撮像手段からの画像データに基づいて前記マーカの
   画像領域を選択すると共に、その選択された画像領域内
   の画像データの値の変化点を検出して輪郭点位置を抽出
   する画像処理手段と、 この画像処理手段により抽出された前記輪郭点位置のデ
   ータに基づいて前記マーカの直交する平行線群のそれぞ
   れの線分を含む直線のパラメータを最小二乗推定法によ
   り求めるようにした直線あてはめ処理手段と、 前記２組の平行線群の各線分を含む直線の前記撮影画面
   上における交点である消失点のデータに基づいて単位方
   向ベクトルを計算する単位方向ベクトル計算処理手段
   と、 前記直線あてはめ処理手段により求められた２本の直交
   する直線とその交点を通る法線とを座標系の軸としたマ
   ーカ座標系を設定し、このマーカ座標系と前記単位方向
   ベクトルに基づいて前記マーカの位置姿勢を計算する位
   置姿勢計算処理手段と、 この位置姿勢計算処理手段による前記撮像手段の位置姿
   勢を示すパラメータ計算結果に基づいて、前記撮像手段
   の基準座標系から見た前記マーカの位置姿勢を表す座標
   変換行列を計算する座標変換行列計算手段とを備えたこ
   とを特徴とする視覚に基く三次元位置および姿勢の認識
   装置。
7. 【請求項７】 前記マーカは、背景部分に対して濃淡情
   報あるいは色情報の変化により表示された少なくとも２
   個の長方形状あるいは正方形状をなす図形から構成さ
   れ、それらの図形は互いに一つの頂点で接すると共にそ
   の頂点を含む隣接する辺同士が直角をなすように配置さ
   れていることを特徴とする請求項６記載の視覚に基く三
   次元位置および姿勢の認識装置。
8. 【請求項８】 前記直線あてはめ処理手段は、前記画像
   処理手段により抽出された前記輪郭点位置のデータに基
   づいて、前記マーカのひとつの線分に対応して直線を複
   数本検出し、それらの直線の平均化処理を行うことによ
   り１本の直線を求めてその直線パラメータ決定するよう
   に構成されていることを特徴とする請求項６または請求
   項７記載の視覚に基く三次元位置および姿勢の認識装
   置。
9. 【請求項９】 前記単位方向ベクトル計算処理手段は、
   複比不変の法則を利用して前記消失点の候補値を３つ以
   上計算し、それらの候補値の２次元座標データに基づい
   て前記単位方向ベクトルを計算するように構成されてい
   ることを特徴とする請求項７または８記載の視覚に基く
   三次元位置および姿勢の認識装置。
10. 【請求項１０】 前記単位方向ベクトル計算処理手段
    は、前記マーカの３本以上の平行な線分にそれぞれ対応
    して得られた直線について、それらの交点に対する単位
    方向ベクトルをモーメント行列に対する二次形式最小化
    問題として解くことにより前記単位方向ベクトルを計算
    するように構成されていることを特徴とする請求項７、
    ８または９記載の視覚に基く三次元位置および姿勢の認
    識装置。