JP4969279B2 - 位置検出方法および位置検出装置 - Google Patents

Description

本発明は、物体の画像から物体の位置又は姿勢を検出する位置検出方法および位置検出装置に関する。

従来、物体の画像から物体の位置又は姿勢を検出する位置検出方法および位置検出装置においては、物体の画像を撮影するために複数台のカメラが使用されていた（たとえば、特許文献１）。このため、装置の構成が複雑となり、大型化するという欠点があった。

他方、１台のカメラによって撮影した物体の画像から物体の位置又は姿勢を検出する位置検出方法および位置検出装置も提案されているが、十分な精度で位置又は姿勢を検出することはできなかった。
特開２００４−３０９３１８号公報

したがって、１台のカメラによって撮影した物体の画像から、十分な精度で物体の位置又は姿勢を検出する位置検出方法および位置検出装置に対するニーズがある。

本発明による位置検出方法は、１台のカメラによって撮影した、既知の位置関係にある物体上の複数の点の画像における測定画素位置から、前記カメラの非ピンホール性を考慮しながら前記物体の位置又は姿勢を検出する。本発明による位置検出方法は、前記カメラの非ピンホール性を考慮せずに、前記複数の点の位置関係および前記測定画素位置から前記物体の回転および平行移動を表す位置行列を求めるステップと、前記カメラの非ピンホール性を考慮して、前記測定画素位置に対応する補正画素位置を求めるステップと、を含む。本発明による位置検出方法は、補正画素位置と前記測定画素位置との距離が許容範囲内となるように、前記位置行列を調整するステップと、補正画素位置と前記測定画素位置との距離が許容範囲内となるように調整された、前記位置行列に対応する位置を、前記カメラの非ピンホール性を考慮した、前記物体の推定位置とするステップと、をさらに含む。

本発明による位置検出方法によれば、カメラの非ピンホール性を考慮せずに求めた位置行列を、カメラの非ピンホール性を考慮した補正画素位置と測定画素位置との距離が許容範囲内となるように調整するので、１台のカメラによって撮影した物体の画像から、十分な精度で物体の位置又は姿勢を検出することができる。

本発明の一実施形態による位置検出方法は、前記位置行列の、前記物体の回転を表す要素が、直交性を有するように調整されたことを特徴とする。

本実施形態によれば、位置行列の直交性が保障されるので、物体の位置検出精度が向上する。

本発明の他の実施形態による位置検出方法は、前記カメラの非ピンホール性に対する補正を、画素位置ごとに定めたキャリブレーションデータを使用しておこなうことを特徴とする。

本実施形態によれば、カメラの非ピンホール性に対する補正を、画素位置ごとに定めたキャリブレーションデータを使用しておこなうので、複雑な手順によらずに物体の位置検出精度が向上する。

本発明による物体の位置又は姿勢を検出する位置検出装置は、１台のカメラによって撮影した、既知の位置関係にある前記物体上の複数の点の画像を取得する画像取得手段と、前記画像における前記複数の点の測定画素位置を検出する画素位置検出手段と、前記複数の点の位置関係および前記複数の点の前記測定画素位置から、前記物体の位置を検出する物体位置推定手段と、を備える。前記物体位置推定手段は、前記カメラの非ピンホール性を考慮せずに、前記複数の点の位置関係および前記測定画素位置から前記物体の回転および平行移動を表す位置行列を求め、前記カメラの非ピンホール性を考慮して、前記測定画素位置に対応する補正画素位置を求める。前記物体位置推定手段は、補正画素位置と前記測定画素位置との距離が許容範囲内となるように、前記位置行列を調整し、補正画素位置と前記測定画素位置との距離が許容範囲内となるように調整された、前記位置行列に対応する位置を、前記カメラの非ピンホール性を考慮した、前記物体の推定位置とする。

本発明による位置検出装置によれば、物体位置推定手段が、カメラの非ピンホール性を考慮せずに求めた位置行列を、カメラの非ピンホール性を考慮した補正画素位置と測定画素位置との距離が許容範囲内となるように調整するので、１台のカメラによって撮影した物体の画像から、十分な精度で物体の位置又は姿勢を検出することができる。

本発明の一実施形態による位置検出装置は、画素位置ごとに定めたキャリブレーションデータを記憶したキャリブレーションデータ記憶手段をさらに備え、前記物体位置推定手段が、前記カメラの非ピンホール性に対する補正を、該キャリブレーションデータを使用しておこなうことを特徴とする。

本実施形態によれば、カメラの非ピンホール性に対する補正を、キャリブレーションデータ記憶手段に記憶されたキャリブレーションデータを使用しておこなうので、複雑な手順によらずに物体の位置検出精度が向上する。

図１は、本発明の一実施形態による位置検出装置１００の構成を示す図である。位置検出装置１００は、画像取得手段１０１、画素位置検出手段１０３、物体位置推定手段１０５およびキャリブレーションデータ記憶手段１０７を備える。

画像取得手段１０１は、１台のカメラＣで撮影した、物体上における複数の点の位置の画像を取得し記憶する。ここで該複数の点は、既知の位置関係にある。複数の点にマーカＭを付してマーカの画像を取得してもよい。

画素位置検出手段１０３は、画像取得手段１０１が取得した画像において、該複数の点の画素の位置を検出する。

キャリブレーションデータ記憶手段１０７は、画素の位置に対応するキャリブレーションデータを記憶する。

物体位置推定手段１０５は、該複数の点の画素の位置および該画素の位置に対応するキャリブレーションデータを使用して、物体の位置を推定する。

図２は、物体上の位置（Ｘ，Ｙ，Ｚ）と画像上の位置すなわち、画素位置（ｕ、ｖ、１）との関係を示す図である。

物体上の位置（Ｘ，Ｙ，Ｚ）から画像上の位置（ｕ、ｖ、１）への変換は、物体の回転と平行移動を表す位置行列Ｐと射影変換行列ｗに分けて以下のように表せる。

物体の初期位置姿勢をカメラの位置姿勢と一致させると、位置行列Ｐは物体の位置姿勢を示している。したがって、位置行列Ｐを求めることによって物体の位置を推定することができる。

物体位置推定手段１０５は、位置行列Ｐを求めることによって物体の位置を推定する。以下において、物体位置推定手段１０５の機能について説明する。

位置行列の求め方
最初に位置行列Ｐの求め方について説明する。

とすると

と表せる。これを書き下すと

Ｗをｕ，ｖに代入すると

と表せる。

ここで、

は測定により既知だから
未知数は

の１１個である。ただし、

は直交している。

aについての線形式にすると

と表せる。

ここで、点（Ｘ，Ｙ，Ｚ）がＺ＝０となる平面上にあるとすると

となるので未知数は

の８個になり４点についての（Ｘ，Ｙ）および（ｕ，ｖ）の数値から決定できる。

４点の３次元座標と画面上の２次元座標がそれぞれ

に射影されているものとすると、

を解くことで

が得られる。

は直交性より

から求める。

は

を利用して

から求める。

上記の手順によって位置姿勢を示す位置行列Ｐが決定される。

ところが、上記の手順から分かるように

にはそれぞれの間に関係が無いので、直交性が保障されない。

直交性を有する位置行列の求め方
以下に直交性を有する位置行列の求め方について説明する。

まず、得られた位置行列Ｐから回転角θを求める。角度は一部の値から求めたものであるので最適な角度ではない。そこで、求めた角度と平行移動成分を初期値として非線形最小化手法により最適な角度と平行移動成分を求める。

対象をθz→θx→θy (-180<θz≦180, -90<θx≦90, -180<θy≦180) の順に回転させると

となるのでＰと回転角の関係は
θx≠90
の場合は

である。

θx=90
の場合は
θy=0
とすると

となる。 以上の式で得られた回転角を元に位置行列Ｐｏを作ればＰｏは、直交性を有する。

この場合に、角度は、
P₂₁, P₂₂, P₂₃, P₃₁, P₃₃
または
P₁₁, P₁₂, P₂₃
だけから求めた角度である。したがって、位置行列Ｐｏを使用して画面上に射影した結果は、位置行列Ｐを使用して画面上に射影した結果とは異なる。そこで，これらの角度を初期値として、射影されたた点の位置と画像上の画素位置の誤差を最小になるように角度θを微調整することで直交性を保ったまま誤差を最小化する。

位置行列Ｐｏにより４点が

と射影されているとする。この時に誤差Eを射影されたた点の位置と画像上の画素位置との距離の自乗和

として，このEが最小になるように
θx, θy, θzとP₃₄
を勾配法などで調整する。

P₁₄, P₂₄
は、後述の理由により冗長であるため変えない。このようにして、位置行列Ｐを、直交性を有する行列に変換することができる。

位置行列Ｐは、以下のように、微小回転角度
Δθ
の要素を有する行列Ｒを分離した形で表すこともできる。

Δθ≪1ならcosθ≒1, sinθ≒θから

となる。以下においては、このように変換した位置行列を位置行列Ｐと呼称する。

上記の行列において、
Δθx, Δθy, ΔθzとP₁₄, P₂₄, P₃₄
を調整して上記のＥを最小化してもよい。

ここで、Ｒからわかるように微小回転時に
Δθx, Δθy
は平行移動成分にしかならないため、平行移動の要素
P₁₄, P₂₄
を変えることは冗長であり収束の障害になる。そこで、これらは変えない。つまり、
Δθx, Δθy, Δθz, P₃₄
を調整して上記のＥを最小化する。

非ピンホール性の補正方法
以下において、物体位置推定手段１０５による非ピンホール性の補正方法について説明する。

ピンホール性とは、カメラにおいてすべての入射光線が１点のピンホールを通って画像が形成される性質をいう。上記の位置行列を求めることによって物体の位置を推定する方法は、カメラのピンホール性を前提としている。しかし、実際のカメラに入射する光は、レンズを通過するのでピンホール性を有さず、非ピンホール性を有する。

図４は、カメラの非ピンホール性を説明するための図である。なお、ここでは説明を簡単にするため、レンズの代わりにガラス板２０１を使用している。図４において、ピンホールＣの物体側に厚さｔのガラス板２０１が設置されている。物体上の点Ｍからの光は、ガラス２０１で屈折し、ピンホールＣを通過した後に、スクリーン２０３上の点ｍに投影される。点ｍの位置は、測定画素位置に対応する。

上記の位置行列を求めることによって物体の位置を推定する方法は、カメラのピンホール性を前提としているので、点Ｍは、点ｍとピンホールＣを結んだ直線上の点Ｍ’の位置にあると判断される。したがって、カメラの非ピンホール性を考慮しないで、物体の位置を推定すると、点Ｍと点Ｍ’の位置の距離に起因する誤差が生じる。

以下に、カメラの非ピンホール性を考慮して物体の推定位置を求める方法について説明する。

図３は、物体位置推定手段１０５が、カメラの非ピンホール性を考慮して物体の推定位置を求める方法を示す流れ図である。

図３のステップＳ３０１０において、物体位置推定手段１０５は、既知の位置関係にある物体上の複数の点について、該位置関係および該複数の点の画像における測定画素位置から、カメラの非ピンホール性を考慮せずに、該物体の回転および平行移動を表す位置行列Ｐを求める。位置行列Ｐの求め方は、「直交性を有する位置行列の求め方」において説明した。上記のように、カメラの非ピンホール性を考慮せずに求めた位置行列は、図４における点Ｍ’の位置に対応する。

図３のステップＳ３０２０において、物体位置推定手段１０５は、キャリブレーション記憶手段１０７に記憶された、測定画素位置（ｍの位置）に対応するキャリブレーションデータを求める。つぎに、物体位置推定手段１０５は、位置行列Ｐおよびキャリブレーションデータを使用して補正画素位置を求める。

以下に、非ピンホールカメラの特性を数値化したキャリブレーションデータについて説明する。

図１０は、キャリブレーションデータの内容を説明するための説明図である。図１０に示すように、レンズｌに入射する入射光線Ｒは２点で特定することができる。ここでは、第１の光源位置Ｐ１と、第２の光源位置Ｐ２とから発光される光が同一の撮像画素（図示せず）に撮像されたときに、入射光線Ｒがその撮像画素に対応する入射光線であると特定する。

ここで、すべての入射光線との距離の自乗和が最小となる点を光学中心Ｏと定義し、各撮像画素に対応する入射光線Ｒと光学中心Ｏとの距離が最小となる点を、その入射光線Ｒの入射光基点Ｂと定義する。

すなわち、光学中心Ｏ（ｘ_０，ｙ_０，ｚ_０）は、すべての入射光線において、光源位置Ｐ１（ｘ_１，ｙ_１，ｚ_１）と、光源位置Ｐ２（ｘ_２，ｙ_２，ｚ_２）とで特定される入射光線Ｒからの距離ｄの自乗（（１）式）和が最小になる位置を、最小自乗法によって求めた位置となる。
ｄ^２＝−（Ａ^２／Ｂ）＋Ｃ …（１）
ただし、
Ａ＝（ｘ_２−ｘ_１）（ｘ_１−ｘ_０）＋（ｙ_２−ｙ_１）（ｙ_１−ｙ_０）＋（ｚ_２−ｚ_１）（ｚ_１−ｚ_０）
Ｂ＝（ｘ_２−ｘ_１）^２＋（ｙ_２−ｙ_１）^２＋（ｚ_２−ｚ_１）^２
Ｃ＝（ｘ_１−ｘ_０）^２＋（ｙ_１−ｙ_０）^２＋（ｚ_１−ｚ_０）^２
とする。

これによって、画素位置毎に、光源位置Ｐ１及びＰ２で特定される方向と、光学中心Ｏから入射光基点Ｂへの変位量（３次元ベクトルＶ_Ｄ（ｄｘ，ｄｙ，ｄｚ）で表現）とを関連付けたデータをキャリブレーションデータとすることで、非ピンホールカメラの特性を数値化することができる。

なお、キャリブレーションデータは、これに限定されるものではない。例えば、前記した例では、光学中心Ｏを基準位置とし、光学中心Ｏから入射光線へ降ろした垂線の足までのベクトルを変位量Ｖ_Ｄとしているが、基準位置は、光学中心に限らず、カメラと一定関係にある固定点であれば、そのような点でも構わない。そして、変位量Ｖ_Ｄは、基準位置から入射光線上の任意の一点へ向かうベクトルであればよく、基準位置から入射光線へ降ろした垂線の足へ向かうベクトルには限られない。

本実施形態においては、ピンホールを基準位置とし、基点をＢで表す。

このように、キャリブレーションデータは、入射光線Ｒの方向と、基準位置から基点への変異量を画素位置に関連付けたデータであり、測定によって求めることができる。キャリブレーションデータテーブル１０７は、測定によって求めたキャリブレーションデータを画素位置ごとに記憶する。

図５は、１回目の補正による補正画素位置（以下、補正画素位置１と呼称する、ｍ’の位置）の求め方を説明するための図である。測定画素位置（ｍの位置）に対応するキャリブレーションデータから、基点位置（Ｂの位置）を求める。つぎに、基点位置（Ｂの位置）と点Ｍ’を結ぶ直線を求める。点Ｍ’は、位置行列Ｐから求めたものである。さらに、この直線と平行で、ピンホール位置（Ｃの位置）を通る直線を求め、スクリーン２０３との交点を補正画素位置（ｍ’の位置）とする。

図３のステップＳ３０３０において、物体位置推定手段１０５は、補正画素位置（ｍ’の位置）と測定画素位置（ｍの位置）との距離が許容範囲内であるか判断する。許容範囲は、たとえば±０．０５画素とする。許容範囲内でなければ、ステップＳ３０４０に進む。許容範囲内であれば、ステップＳ３０５０に進む。

図３のステップＳ３０４０において、物体位置推定手段１０５は、補正画素位置を測定画素位置に近づけるように位置行列を調整する。

図６は、２回目の補正による補正画素位置（以下、補正画素位置２と呼称する、ｍ’’の位置）の求め方を説明するための図である。補正画素位置１（ｍ’の位置）は、測定画素位置（ｍの位置）よりも下にあるので、補正画素位置を上方向（Ｘ座標を増加させる方向）に移動させる必要がある。このためには、たとえば、点Ｍ’を右方向（Ｚ座標を増加させる方向）に移動させればよい。具体的には位置行列Ｐの
Δθx, Δθy, Δθz, P₃₄
を調整して、点Ｍ’を右方向（Ｚ座標を増加させる方向）に移動させて点Ｍ’’求める。

物体位置推定手段１０５は、ステップＳ３０４０の処理の終了後、ステップＳ３０２０の処理を行う。

ステップＳ３０２０の処理を、図６にしたがって説明する。補正画素位置１（ｍ’の位置）に対応するキャリブレーションデータから、基点位置（Ｂ’の位置）を求める。つぎに、基点位置（Ｂ’の位置）と点Ｍ’’を結ぶ直線を求める。点Ｍ’’は、上述のように調整された位置行列Ｐから求めたものである。さらに、この直線と平行で、ピンホール位置（Ｃの位置）を通る直線を求め、スクリーン２０３との交点を補正画素位置２（ｍ’’の位置）とする。

物体位置推定手段１０５は、以下、同様にして補正画素位置と測定画素位置（ｍの位置）との距離が許容範囲内になるまで、ステップＳ３０４０の処理とステップＳ３０２０の処理を繰り返す。

繰り返し計算において、位置行列Ｐの調整量に対する補正画素位置の移動量を考慮して、位置行列Ｐの調整量を求める。

図７は、３回目の補正による補正画素位置（以下、補正画素位置３と呼称する、ｍ’’’の位置）の求め方を説明するための図である。補正画素位置３（ｍ’’’の位置）は、測定画素位置（ｍの位置）との距離が許容範囲内となっている。

図３のステップＳ３０５０において、物体位置推定手段１０５は、補正画素位置（ｍ’’’の位置）と測定画素位置（ｍの位置）との距離が許容範囲内となるように調整された、位置行列Ｐに対応する位置を、カメラの非ピンホール性を考慮した、物体の推定位置とする。

図８は、ピンホールの前に板ガラスを置いて、カメラの非ピンホール性を考慮せずに位置を検出した場合に発生する誤差を示す図である。物体上のマーカ（点）の位置Ｍは、たとえば、図４においてＺ軸に垂直な面内にあり、対角線の交点がＺ軸上にある一辺が０．１メータの正方形の４頂点の位置である。横軸は、たとえば、図４において、ピンホール位置（Ｃの位置）から物体上の点ＭまでのＺ軸方向の距離（単位メータ）である。縦軸は、たとえば、図４において、物体上の点Ｍの位置と推定された位置（Ｍ’の位置）との距離を、Ｚ軸方向の距離の自乗で割って正規化した値（単位：１／メートル）、すなわち正規化された誤差である。ここで、標準偏差は、正規化された誤差のばらつきに対するものである。データは、ピンホール位置（Ｃの位置）から物体上の点ＭまでのＺ軸方向の距離が１メータでキャリブレーションしたものである。

ここで、カメラの水平画角は９０度、水平画素は６４０画素、アスペクト比は１：１とする。

図８は、ガラス板の厚さｔが、３ミリメータ、６ミリメータ、９ミリメータおよび１２ミリメータの場合についての誤差を示す。ピンホール位置（Ｃの位置）から物体上の点ＭまでのＺ軸方向の距離が１メータより小さくなると正規化された誤差は急激に増加する。

図９は、図８と同じ条件で本発明の位置検出方法によって位置を検出した場合に発生する誤差を示す図である。ピンホール位置（Ｃの位置）から物体上の点ＭまでのＺ軸方向の距離が１メータより小さくなっても誤差は増加しない。

このように、本発明の位置検出方法によれば、物体が近くに位置する場合であっても物体の位置を高精度で検出することができる。

本発明の一実施形態による位置検出装置の構成を示す図である。 物体上の位置（Ｘ，Ｙ，Ｚ）と画像上の位置すなわち、画素位置（ｕ、ｖ、１）との関係を示す図である。 物体位置推定手段が、カメラの非ピンホール性を考慮して物体の推定位置を求める方法を示す流れ図である。 カメラの非ピンホール性を説明するための図である。 １回目の補正による補正画素位置の求め方を説明するための図である。 ２回目の補正による補正画素位置の求め方を説明するための図である。 ３回目の補正による補正画素位置の求め方を説明するための図である。 ピンホールの前に板ガラスを置いて、カメラの非ピンホール性を考慮せずに位置を検出した場合に発生する誤差を示す図である。 図８と同じ条件で本発明の位置検出方法によって位置を検出した場合に発生する誤差を示す図である。 キャリブレーションデータの内容を説明するための説明図である。

符号の説明

１００…位置検出装置、１０１…画像取得手段、１０３…画素位置検出手段、１０５…物体位置推定手段、１０７…キャリブレーション記憶手段

Claims (4)

1. １台のカメラによって撮影した、既知の位置関係にある物体上の複数の点の画像における測定画素位置から、前記物体の位置又は姿勢を検出する位置検出方法であって、
   すべての入射光線が１点のピンホールを通るピンホール性を前提として、前記複数の点の位置関係および前記測定画素位置から前記物体の回転および平行移動を表す位置行列を求めるステップと、
   前記カメラが前記ピンホール性を有しないことに基づき、前記位置行列、および画素位置ごとに入射光の方向と前記レンズの光学中心から入射光基点への変位量とを関連付けたキャリブレーションデータを使用して前記測定画素位置に対応する補正画素位置を求めるステップと、
   補正画素位置と前記測定画素位置との距離が許容範囲内となるように、前記位置行列を調整するステップと、
   補正画素位置と前記測定画素位置との距離が許容範囲内となるように調整された、前記位置行列に対応する位置を、前記カメラがピンホール性を有しないことに基づいて、前記物体の推定位置とするステップと、を含む位置検出方法。
2. 前記位置行列の、前記物体の回転を表す要素が、直交性を有するように調整されたことを特徴とする請求項１に記載の位置検出方法。
3. 物体の位置又は姿勢を検出する位置検出装置であって、
   １台のカメラによって撮影した、既知の位置関係にある前記物体上の複数の点の画像を取得する画像取得手段と、
   前記画像における前記複数の点の測定画素位置を検出する画素位置検出手段と、
   前記複数の点の位置関係および前記複数の点の前記測定画素位置から、前記物体の位置を検出する物体位置推定手段と、を備え、
   前記物体位置推定手段は、すべての入射光線が１点のピンホールを通るピンホール性を前提として、前記複数の点の位置関係および前記測定画素位置から前記物体の回転および平行移動を表す位置行列を求め、前記カメラが前記ピンホール性を有しないことに基づき、前記位置行列、および画素位置ごとに入射光の方向と前記レンズの光学中心から入射光基点への変位量とを関連付けたキャリブレーションデータを使用して前記測定画素位置に対応する補正画素位置を求め、補正画素位置と前記測定画素位置との距離が許容範囲内となるように、前記位置行列を調整し、補正画素位置と前記測定画素位置との距離が許容範囲内となるように調整された、前記位置行列に対応する位置を、前記カメラがピンホール性を有しないことに基づいて、前記物体の推定位置とする位置検出装置。
4. 画素位置ごとに定めたキャリブレーションデータを記憶したキャリブレーションデータ記憶手段をさらに備えることを特徴とする請求項３に記載の位置検出装置。

Images