JPH1047924A

JPH1047924A - ３次元位置計測装置

Info

Publication number: JPH1047924A
Application number: JP8220491A
Authority: JP
Inventors: Ko Sano; 香佐野; Kohei Nishikawa; 晃平西川; Akira Okamoto; 陽岡本
Original assignee: Kobe Steel Ltd
Current assignee: Kobe Steel Ltd
Priority date: 1996-08-01
Filing date: 1996-08-01
Publication date: 1998-02-20

Abstract

(57)【要約】（修正有）【課題】調整機構を不要にして部品コストを低減する
ことができると共に、オペレータの負担および調整時間
を低減することができる３次元位置計測装置の提供。【解決手段】三次元計測装置を、所定の光パターン５
を位置計測対象物２に斜め上方から照射する光照射手段
３と、該光照射手段３による前記位置計測対象物２から
の反射光を上方で画像として結像する撮像装置９と、該
結像画像をフレームバッファ座標系の画像位置情報に変
換する演算を行う画像処理手段と、前記画像位置情報に
基づき前記計測対象物のセンサ座標系における三次元位
置を三角測量の原理により演算する位置演算手段と、前
記三次元位置をキャリブレーション座標系の三次元位置
に変換する変換演算手段と、前記光照射手段３および撮
像装置９の取付位置の微少なズレを較正する演算を行う
較正手段とを含んで構成する。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、貨物の積み上げや
荷降ろし等を行わせるハンドリングロボットに貨物の三
次元位置を認識させるための三次元位置計測装置であっ
て、該三次元位置計測装置を構成する撮像装置及び光照
射装置の取付位置の機械的な調整を不要にする三次元位
置計測装置に関するものである。

【０００２】

【従来の技術】積み上げ・荷降し作業をハンドリングロ
ボットに実施させようとすると、次の積み上げ位置とな
るプレース位置、或いは次の荷降し位置となるピック位
置を水平座標および鉛直（高さ）座標により指定してハ
ンドリングロボットに指示する必要がある。そこで、例
えば特開平３−２３４４９１号公報には、プレース位置
或いはピック位置をハンドリングロボットに指示するこ
とができるように、貨物の立体的な配置状況を認識する
３次元位置計測装置が提案されている。即ち、この３次
元位置計測装置は、図８に示すように、パレット５２上
に積み上げられた貨物５１に対して上方に設けられた光
照射装置５７から一定の光パターン５３を照射し、この
光パターン５３を斜め上方から第１撮像装置５５により
撮像して三角測量により貨物５１の高さ位置を求める。
この後、貨物５１の全体を第２撮像装置５６により上方
から撮像し、撮像画像中における貨物５１の固有マーク
５４の濃淡画像を基本マーク形状との突き合わせ処理に
より個々に検出して平面的な配置状況を求めることによ
って、貨物５１の立体的な配置状況を認識するようにな
っている。

【０００３】ところで、上記のように三角測量により貨
物５１の高さ位置を求める場合には、光パターン５３と
この光パターン５３を斜め上方から撮像する第１撮像装
置５５との幾何学的位置関係、および第１撮像装置５５
と第１撮像装置５５の２次元座標を３次元座標に展開し
て高さ位置を算出するキャリブレーション座標系との幾
何学的位置関係が高さ位置の測定精度に大きな影響を及
ぼすことになる。従って、従来は、上述の両位置関係を
所定の関係として貨物５１の高さ位置を高精度に測定す
ることができるように、３次元位置計測装置を導入する
際に、第１撮像装置５５の取り付け姿勢および光照射装
置５７の取付姿勢を所定の姿勢に微調整するようになっ
ている。

【０００４】

【発明が解決しようとする課題】しかしながら、上記従
来の構成では、第１撮像装置５５および光照射装置５７
を微調整する調整機構を３次元位置計測装置に設けるこ
とが必要となるため、この調整機構により部品コストが
増大するという問題がある。さらに、調整機構を用いた
機械的な調整であるため、調整機構を操作するオペレー
タに対する負担および調整時間が大きなものになり易
く、このような調整時における負担および調整時間は、
短期間で装置の立ち上げを完了させる必要のある現場に
おいては極めて大きな問題となる。

【０００５】従って、本発明は、調整機構を不要にして
部品コストを低減することができると共に、オペレータ
の負担および調整時間を低減することができる３次元位
置計測装置を提供しようとするものである。

【０００６】

【課題を解決するための手段】上記課題を解決するため
に、本発明にかかる三次元位置計測装置は、実際の空間
における位置を表すキャリブレーション座標系において
所定の位置に取り付けられ、光照射手段による位置計測
対象物からの反射光を上方で所定の焦点距離に画像とし
て結像する撮像装置と、前記撮像装置の位置を原点に取
り直したセンサ座標系において所定の位置に取り付けら
れ、反射ミラーの傾斜角度を変更することにより所定の
光パターンを任意の照射角度で位置計測対象物に斜め上
方から照射する前記光照射手段と、撮像装置からの連続
した画像信号を離散的な画像データとして記憶する記憶
装置と、前記結像画像を記憶装置上の位置を表すフレー
ムバッファ座標系の画像位置情報に変換する演算を行う
画像処理手段と、前記画像位置情報に基づき、前記セン
サ座標系における前記計測対象物の三次元位置を三角測
量の原理により演算する位置演算手段と、前記三次元位
置をキャリブレーション座標系の三次元位置に変換する
変換演算手段と、前記光照射手段および撮像装置の取付
位置の微少なズレを較正する演算を行う較正手段とを含
んでなり、該較正手段は、位置が既知である較正用ター
ゲットの位置情報と、前記反射ミラーの傾斜角度の情報
と、前記撮像装置の焦点距離の情報とから、前記フレー
ムバッファ座標系で表される前記画像位置情報を、セン
サ座標系の位置に変換する画像変換の第一係数と、三角
測量の原理から演算したセンサ座標系での三次元位置
を、前記キャリブレーション座標系の位置に変換する座
標変換の第二係数と、前記光照射手段が照射する光パタ
ーンの前記センサ座標系における照射中心位置および照
射角度からなる光パラメータとを演算するものであっ
て、前記位置計測対象物の前記キャリブレーション座標
系における三次元位置を、前記画像処理手段と、前記位
置演算手段と、前記変換演算手段と、前記較正手段とに
基づいて得るように構成したことを特徴とする３次元位
置計測装置である。これにより、三次元位置計測装置に
おける光照射手段および撮像装置の取付位置の微少なズ
レを以下のようにして較正でき、位置計測対象物の三次
元位置を正確に測定することができる。即ち、前記の位
置が既知である較正用ターゲットに前記光照射手段を作
用させると、前記記憶装置上に画像を得て、該画像から
画像処理手段により前記フレームバッファ座標系におけ
る画像位置情報および、その前記キャリブレーション座
標系および前記センサ座標系における位置情報を得るこ
とができる。前記較正手段は、既知である前記較正用タ
ーゲットのキャリブレーション座標系における位置情報
および三角測量の原理による演算によって求められたセ
ンサ座標系とキャリブレーション座標系における三次元
位置と、前記反射ミラーの傾斜角度の情報と、前記撮像
装置の焦点距離の情報とを加味して、前記の画像変換の
第一係数、前記の座標変換の第二係数、前記の光パラメ
ータとを演算することができる。この較正手段が得た前
記の係数やパラメータと、前記画像処理手段と、前記位
置演算手段と、前記変換演算手段とに基づいて、位置計
測対象物の前記キャリブレーション座標系における正確
な三次元位置を得ることができる。

【０００７】

【発明の実施の形態】本発明の実施の形態の例を図１な
いし図７に基づいて以下に説明する。本実施形態に係る
３次元位置計測装置は、図１に示すように、パレット１
上に積み上げられた袋状や箱状の貨物やキャリブレーシ
ョン用の位置計測対象物となるターゲット２の上方に設
置されたセンサヘッド機構３と、センサヘッド機構３に
接続された認識処理制御盤４とを有している。センサヘ
ッド機構３は、キャリブレーション座標系（ＸC ，ＹC
，ＺC ）において所定の位置に取り付けられており、
その位置をセンサ座標系における原点とする。この例に
おいては、光照射手段および撮像装置を備える構成とし
ている。また、この例においては、ターゲット２に照射
する光パターンとしてスリット光を照射することとして
いる。

【０００８】具体的には、図２にも示すように、帯状の
レーザ光からなるスリット光５を拡散させながら出射す
るスリット光投光器６と、スリット光投光器６からのス
リット光５をターゲット２方向に反射する反射ミラー７
と、反射ミラー７の傾斜角度を任意に変更可能なステッ
ピングモータ８とを備え、これらの部材が光照射手段を
構成している。かかる構成とすると、ステッピングモー
タ８を駆動することにより反射ミラー７の傾斜角度を変
更し、スリット光５を任意の照射角度でターゲット２に
照射することができる。

【０００９】また、センサ機構３に備わる撮像装置９は
ターゲット２に照射されたスリット光５を撮像する。こ
の撮像装置９は、レンズ１０と撮像部材１１とを有し、
電動ズーム機構により撮像部材１１に対するレンズ１０
の位置を自動で合わせることができる。レンズ１０は、
撮像方向がセンサ座標系（Ｘｓ，Ｙｓ，Ｚｓ）において
鉛直方向（Ｚｓ方向）となるように設定されている。ま
た、撮像部材１１は、レンズ１０を介してターゲット２
上に照射されて反射されたスリット光５が、所定の焦点
距離の位置に画像として結像される撮像面１１ａを有し
ており、撮像面１１ａは、鉛直方向（Ｚｓ方向）に対し
て垂直となるように設定されている。そして、撮像部材
１１の撮像面１１ａには、レンズ中心Ｏを中心として点
対称に画像が結像されるようになっており、例えばスリ
ット光５をターゲット２に照射して形成された照射線５
のＱ₁点、Ｑ₂点、Ｑ₃点は、それぞれ光線Ｌ1 、Ｌ2
、Ｌ3 を介して撮像面１１ａにＰ₁点、Ｐ₂点、Ｐ₃
点として結像されるようになっている。

【００１０】上記の撮像部材１１は、光量に応じた電荷
を発生させる受光素子部（画素）をＸ方向およびＹ方向
のマトリックス状に複数有しており、これらの受光素子
部の電荷を例えばラスタスキャン形式により電圧出力と
して読み出して画像信号として順に出力するようになっ
ている。この撮像部材１１は、図２に示すように、認識
処理制御盤４に接続されている。認識処理制御盤４は、
上述の撮像部材１１やスリット光投光器６等に接続され
たＩ／Ｏ部１２ａ・１２ｂと、これらのＩ／Ｏ部１２ａ
・１２ｂに信号バス１４を介して接続されたモータ駆動
部１３、演算部１６、ＲＡＭ１７、およびＲＯＭ１８と
を有している。

【００１１】上記のＲＡＭ１７には、撮像部材１１の受
光素子部の配列と一致したＸ方向およびＹ方向のマトリ
ックス状のデータテーブルを有するようにフレームバッ
ファ領域１７ａが形成されている。このフレームバッフ
ァ領域１７ａは、２次元のフレームバッファ座標系（Ｘ
ｐ，Ｙｐ）を形成する。そして、上述の撮像装置９から
のスリット光が結像された画像の画像信号がＩ／Ｏ部１
２ａを介して入力されるが、フレームバッファ領域１７
ａは、この連続した画像信号を離散的な画像データとし
て記憶する記憶装置を構成する。

【００１２】また、ＲＡＭ１７には、キャリブレーショ
ン領域１７ｂが形成されており、キャリブレーション領
域１７ｂは、３次元のキャリブレーション座標系（Ｘ
ｃ，Ｙｃ，Ｚｃ）を形成する。このキャリブレーション
座標系においては、前記のフレームバッファ座標系で表
されたターゲット２の画像位置情報が、これに対応する
三次元位置情報として与えられており、キャリブレーシ
ョン領域１７ｂには、この三次元位置情報が格納されて
いる。

【００１３】さらに、ＲＡＭ１７には、カメラパラメー
タを格納するカメラパラメータ領域１７ｃと、光パラメ
ータを格納する光パラメータ領域１７ｄが形成されてい
る。カメラパラメータ領域１７ｃには、フレームバッフ
ァ座標系（Ｘｐ，Ｙｐ）からセンサ座標系（Ｘｓ，Ｙ
ｓ，Ｚｓ）に変換する画像変換と、センサ座標系（Ｘ
ｓ，Ｙｓ，Ｚｓ）からキャリブレーション座標系（Ｘ
ｃ，Ｙｃ，Ｚｃ）に変換する平行移動行列および回転移
動行列からなる座標変換とに使用されるカメラパラメー
タが格納されるようになっている。この例では、画像変
換としてアフィン変換を用いることとするが、その場
合、カメラパラメータは、アフィン変換のスケールファ
クタ（ｓｘ，ｓｙ）、平行移動成分（ｄｘ，ｄｙ）、回
転移動行列のｘ軸の回転成分（ａ）、ｙ軸の回転成分
（ｂ）、ｚ軸の回転成分（ｃ）、平行移動行列のｘ軸方
向の移動成分（ｌｘ）、ｙ軸方向の移動成分（ｌｙ）、
ｚ軸方向の移動成分（ｌｚ）の合計１０個からなってい
る。

【００１４】そして、このカメラパラメータのうち前記
画像変換に伴う部分が画像変換の第一係数にあたり、前
記座標変換に伴う部分が座標変換の第二係数にあたる。
これらのパラメータはターゲット２に対する撮像装置９
の幾何学的位置関係を決めるパラメータとして重要であ
る。

【００１５】また、スリット光パラメータ領域１７ｄに
は、センサ座標系（Ｘｓ，Ｙｓ，Ｚｓ）におけるスリッ
ト光５の光パラメータとして、スリット光５の回転中
心、即ち、光照射手段の照射中心位置（ｘ０，０，ｚ
０）と、光照射手段の照射角度（スリット光５の法線ベ
クトルｎ）が格納される。これらの光パラメータは、タ
ーゲット２の三次元位置を正確に測定する上で、ターゲ
ット２に対する幾何学的位置関係を決めるパラメータと
して重要である。これらのパラメータは、ターゲット２
に対するスリット光５の前記関係を決めるスリット光５
の平面方程式を定める座標位置と、該平面の法線ベクト
ルに対応している。

【００１６】一方、ＲＯＭ１８には、キャリブレーショ
ンルーチン１８ａ、画像処理ルーチン１８ｂ、三次元位
置測定ルーチン１８ｃ、ターゲット２の３次元位置から
ターゲット２全体の配置パターンを求める配置パターン
検出ルーチン１８ｄ等の各種の処理ルーチンが格納され
ている。

【００１７】キャリブレーションルーチン１８ａは、前
記の較正手段を構成し、ターゲット２として位置が既知
である較正用ターゲットの位置情報ｚと、反射ミラー７
の傾斜角度θの情報と、撮像装置９に備わるレンズ１０
の焦点距離ｆの情報とから、前記の画像変換の第一係数
および座標変換の第二係数にあたるカメラパラメータ、
前記のスリット光５の照射中心位置と照射角度にあたる
光パラメータを求める演算を行う。その具体的な処理手
順については後述する。

【００１８】画像処理ルーチン１８ｂは、画像処理手段
を構成し、ターゲット２からの反射光を撮像装置９の撮
像部材１１に結像して得られる結像画像から、前記フレ
ームバッファ座標系（Ｘｐ、Ｙｐ）における画像位置情
報に変換する演算を行う。この演算には、例えば、ター
ゲット２からの反射光の濃淡分布として与えられる結像
画像について、その中に含まれる中心線に当たる部分を
抽出し、前記座標系（Ｘｐ、Ｙｐ）における二次元位置
に変換する等の演算処理がある。

【００１９】三次元位置測定ルーチン１８ｃは、位置演
算手段および変換演算手段を構成する。三次元位置測定
ルーチン１８ｃは、前記画像位置情報から前記のキャリ
ーブレーションルーチンの手順にも含まれる画像変換お
よび座標変換の演算と、上述のルーチン１８ａ、１８ｂ
で得た各種の位置情報やパラメータ等に三角測量の原理
を加味した前記のセンサ座標系におけるターゲット２の
三次元位置の演算と、センサ座標系における三次元位置
を前記座標変換を用いてキャリブレーション座標系での
三次元位置に変換する変換演算とを行うルーチンを含ん
でいる。

【００２０】上記の構成を有した認識処理制御盤４は、
Ｉ／Ｏ部１２ｂを介してロボット制御盤２０に接続され
ている。ロボット制御盤２０は、図２に示すように、パ
レット１の近傍に設置されたハンドリングロボット２１
に接続されている。また、ハンドリングロボット２１の
近傍には、ターゲット２を移送するローラコンベア２２
が配設されている。そして、ロボット制御盤２０は、認
識処理制御盤４から配置パターンを受信したときに、次
のターゲット２のプレース（ピック）位置を決定し、こ
のプレース（ピック）位置とローラコンベア２２との間
でターゲット２を移載するようにハンドリングロボット
２１に対して指令するようになっている。

【００２１】上記の構成において、３次元位置計測装置
の動作を説明する。先ず、図２に示すように、センサヘ
ッド機構３が現場の所定位置に所定の取り付け姿勢とな
るように取り付けられる。そして、このセンサヘッド機
構３が認識処理制御盤４に接続された後、センサヘッド
機構３の取り付け誤差を解消するように、認識処理制御
盤４においてキャリブレーションルーチンが実行される
ことになる。即ち、図４に示すように、高さ変数ｍおよ
びフォーカス変数ｉがそれぞれ“０”に初期設定される
（Ｓ１）。この後、較正用のターゲット２の上面がキャ
リブレーション座標系（Ｘｃ，Ｙｃ，Ｚｃ）において既
知の高さ位置ｚ_mを有したターゲット平面となるように
設定されると共に、この高さ位置ｚ_mが記憶される（Ｓ
２）。また、撮像装置９がフォーカス値ｆ_iの焦点距離
に設定されると共に、このフォーカス値ｆ_iが記憶され
る（Ｓ３）。

【００２２】次に、ステッピングモータ８が回転駆動さ
れ、反射ミラー７が所定の傾斜角度θとされると共に、
スリット光投光器６から帯状に拡散されたスリット光５
が反射ミラー７方向に出射される。そして、図５に示す
ように、スリット光５が反射ミラー７からターゲット２
の方向に反射し、ターゲット面の両端にかけて線状に照
射されると、照射された投光ライン５ａおよびターゲッ
ト２が撮像装置９で撮影されることによって、この映像
がレンズ１０を介して撮像部材１１の撮像面１１ａに結
像されることになる（Ｓ４）。

【００２３】上記の撮像面１１ａに結像された画像は、
図３に示すように、２次元のスリット光画像データとし
てＩ／Ｏ部１２ａを介して位置検出制御盤４に取り込ま
れ、ＲＡＭ１７のフレームバッファ領域１７ａに格納さ
れる。この後、図５に示すように、フレームバッファ領
域１７ａの２次元のフレームバッファ座標系（Ｘｐ，Ｙ
ｐ）におけるスリット光画像データ中からスリット光５
の投光ライン５ａが光切断線として特定され、図６に示
すように、投光ライン５ａ上の任意の点のデータ（Ｍ０
〜Ｍ６）と、両端点のデータ（Ｐ００〜Ｐ０６，Ｐ１０
〜Ｐ１６）とが読み出されて記憶されると共に、このス
リット光画像データを得た反射ミラー７の傾斜角度θが
記憶される（Ｓ５）。さらに、このようにして得られた
両端点群のデータ（Ｐ００〜Ｐ０６，Ｐ１０〜Ｐ１６）
からキャリブレーション用のターゲット２の４頂点（Ｃ
０〜Ｃ３）が算出されて記憶される（Ｓ６）。

【００２４】次に、フォーカス変数ｉに“１”が加算さ
れ（Ｓ７）、フォーカス変数ｉが２以下であるか否かが
判定される（Ｓ８）。フォーカス変数ｉが２以下である
場合には（Ｓ８，ＹＥＳ）、上述のＳ３から再実行され
ることによって、加算後のフォーカス値ｆ_iの焦点距離
となるように撮像装置９が設定された後、ターゲット２
の撮影、投光ライン５ａ上のデータ（Ｍ０〜Ｍ６，Ｐ０
０〜Ｐ０６，Ｐ１０〜Ｐ１６）および反射ミラー７の傾
斜角度θの記憶、図６に示されるターゲット２の４頂点
（Ｃ０〜Ｃ３）の記憶が行われることになる（Ｓ３〜Ｓ
６）。

【００２５】一方、フォーカス変数ｉが２以下でない場
合には（Ｓ８，ＮＯ）、高さ変数ｍに“１”が加算され
（Ｓ９）、高さ変数ｍが２以下であるか否かが判定され
る（Ｓ１０）。高さ変数ｍが２以下である場合には（Ｓ
１０，ＹＥＳ）、フォーカス変数ｉが“０”に初期設定
された後（Ｓ１１）、上述のＳ２から再実行される。こ
れにより、加算後の高さ位置ｚ_mを有したターゲット平
面となるようにターゲット２が設定された後、“０”お
よび“１”の２種類のフォーカス変数ｉからなるフォー
カス値ｆ_iでもって、撮像装置９がターゲット２を撮影
し、投光ライン５ａ上のデータ（Ｍ０〜Ｍ６，Ｐ００〜
Ｐ０６，Ｐ１０〜Ｐ１６）および反射ミラー７の傾斜角
度θの記憶、ターゲット２の４頂点（Ｃ０〜Ｃ３）の記
憶が行われることになる（Ｓ３〜Ｓ７）。

【００２６】この後、高さ変数ｍが３以上になると（Ｓ
１０，ＮＯ）、フレームバッファ座標系（Ｘｐ，Ｙｐ）
からセンサ座標系（Ｘｓ，Ｙｓ，Ｚｓ）へのアフィン変
換と、センサ座標系（Ｘｓ，Ｙｓ，Ｚｓ）からキャリブ
レーション座標系（Ｘｃ，Ｙｃ，Ｚｃ）への平行移動行
列および回転移動行列からなる座標変換とに使用される
カメラパラメータを求めるように、カメラキャリブレー
ションの収束計算処理が行われる。

【００２７】具体的には、アフィン変換の対角成分は、
回転移動行列と分離できないので省略すると、フレーム
バッファ座標系（Ｘｐ，Ｙｐ）からキャリブレーション
座標系（Ｘｃ，Ｙｃ，Ｚｃ）への変換パラメータである
カメラパラメータは、アフィン変換のスケールファクタ
（ｓｘ，ｓｙ）、平行移動成分（ｄｘ，ｄｙ）、回転移
動行列のｘ軸の回転成分（ａ）、ｙ軸の回転成分
（ｂ）、ｚ軸の回転成分（ｃ）、平行移動行列のｘ軸方
向の移動成分（ｌｘ）、ｙ軸方向の移動成分（ｌｙ）、
ｚ軸方向の移動成分（ｌｚ）の合計１０個となる。

【００２８】このカメラパラメータによる前記の二つの
座標系間の対応を示すのが図７である。フレームバッフ
ァ座標系とキャリブレーション座標系の間で、（ｘp0、
ｙp0）と（ｘc0、ｙc0）、（ｘp1、ｙp1）と（ｘc1、ｙ
c1）、（ｘp2、ｙp2）と（ｘc2、ｙc2）、（ｘp3、ｙp
3）と（ｘc3、ｙc3）が各々に対応する関係にある。

【００２９】ここで、フレームバッファ座標系（Ｘｐ，
Ｙｐ）からセンサ座標系（Ｘｓ，Ｙｓ，Ｚｓ）への変換
は、（１）式で表される。

【００３０】

【数１】

【００３１】キャリブレーション座標系（ｘｃ，ｙｃ，
ｚｃ）からセンサ座標系（Ｘｓ，Ｙｓ，Ｚｓ）への変換
は、（２）式で表される。

【００３２】

【数２】

【００３３】但し、Ｔｒａｎｓ（ｌ_x，ｌ_y，ｌ_z）：
平行移動行列Ｒｏｔ（ｚ，ｃ），Ｒｏｔ（ｙ，ｂ），Ｒｏｔ（ｘ，
ａ）：ｘ軸、ｙ軸、ｚ軸回りの回転行列センサ座標系（Ｘｓ，Ｙｓ，Ｚｓ）における透視変換
は、（３）式で表される。

【００３４】

【数３】

【００３５】（１）式、（２）式、（３）式により
（４）式が導出される。

【００３６】

【数４】

【００３７】カメラパラメータｐについて、各パラメー
タの微小変化を考えると、（５）式のように表すことが
できる。

【００３８】

【数５】

【００３９】そこで、フレームバッファ座標系（Ｘｐ，
Ｙｐ）におけるデータ群（ｘｐ_i，ｙｐ_i）と、このデ
ータ群（ｘｐ_i，ｙｐ_i）に対応するキャリブレーショ
ン座標系（Ｘｃ，Ｙｃ，Ｚｃ）におけるデータ群（ｘｃ
_i，ｙｃ_i，ｚｃ_i）が与えられると、フレームバッフ
ァ座標系（Ｘｐ，Ｙｐ）からキャリブレーション座標系
（Ｘｃ，Ｙｃ，Ｚｃ）へのカメラパラメータｐ＝（ｓ
ｘ，ｓｙ，ｄｘ，ｄｙ，ｌｘ，ｌｙ，ｌｚ，ａ，ｂ，
ｃ）を得ることができる。即ち、（５）式に示すヤコビ
行列Ｊを用いて、反復計算によって算出することができ
る。

【００４０】尚、本実施例においては、焦点距離と高さ
を２通りに変えているが、（５）式において、Δｐを求
める必要条件は、rankＪ＝１０（パラメータ数）であ
る。これは、１平面の４頂点を２種類の焦点距離（フォ
ーカス値ｆ_i）で測定することによって、あるいは、２
平面の２頂点を１種類の焦点距離で測定することによっ
て、カメラパラメータを算出できることを示している。
正確には、８点のデータのうちの５点分のデータを用い
て得られるため、同一の焦点距離でない、あるいは同一
の高さでない少なくとも５点のデータを用いることによ
って、カメラパラメータを算出できることを示してい
る。

【００４１】以上のようにしてカメラパラメータが求め
られると（Ｓ１２）、続いて、センサ座標系（Ｘｓ，Ｙ
ｓ，Ｚｓ）におけるスリット光５の平面方程式に関する
光パラメータを求めるように、スリット光キャリブレー
ションの収束計算処理が行われる。

【００４２】具体的には、スリット光５の平面がＹｓ軸
に平行であると仮定する。そして、先の図５に示すよう
に、スリット光５の回転中心を（ｘ０，０，ｚ０）とし
た任意の基準スリット光５Ａを設定すると、このときの
スリット光５の平面方程式のパラメータ（光パラメー
タ）は、ｘ０およびｚ０と、スリット光５の法線ベクト
ルｎとなる。従って、反射ミラー７を基準スリット光５
Ａからθ[rad] 回転させたときのスリット光５の平面方
程式は、（６）式で表すことができる。

【００４３】

【数６】

【００４４】ここで、

【００４５】

【数７】

【００４６】キャリブレーションによりスリット光５の
平面方程式を求めるためには、センサ座標系（Ｘｓ，Ｙ
ｓ，Ｚｓ）における計測点およびこの計測点を計測する
際の反射ミラー７の傾斜角度θが必要である。

【００４７】前記のカメラキャリブレーションにおいて
記憶したデータより、スリット光５のキャリブレーショ
ンに必要なキャリブレーションデータ（ｆi 、ｚci、ｘ
pj、ｙpj、θj ）を作成する。このキャリブレーション
データについて、ｆi は撮像装置９のフォーカス値、ｚ
ciはキャリーブレーションターゲットの高さ、（ｘpj、
ｙpj）はフレームバッファ座標系（Ｘｐ、Ｙｐ）におけ
る光切断線上の任意の点の計測位置、θj は基準平面か
らの反射ミラー７の回転角を表す。ここで、ｉは各平面
の高さに対応する添字であり、ｊはミラーの各回転角に
対応する添字である。

【００４８】次に、前記のカメラキャリブレーションに
おいて得たカメラパラメータを用いて、前記のキャリブ
レーションデータ（ｚci、ｘpj、ｙpj）をセンサ座標系
（Ｘｓ，Ｙｓ，Ｚｓ）のデータ（ｘsij 、ｙsij 、ｚsi
j ）に変換する。フレームバッファ座標系（Ｘｐ、Ｙp
）における計測点を、キャリーブレーション座標系
（Ｘc 、Ｙc 、Ｚc ）における撮像面１１ａの計測点に
変換する。

【００４９】

【数８】

【００５０】

【数９】

【００５１】但し、

【数１０】

【００５２】

【数１１】

【００５３】キャリブーレーション座標系におけるレン
ズ中心は次式で表される。（９）、（１１）式よりキャ
リブーレーション座標系における計測点は次式のように
なる。

【００５４】

【数１２】

【００５５】センサ座標系における計測点は次式で表さ
れる。

【００５６】

【数１３】

【００５７】

【数１４】スリット光５の平面方程式のパラメータｐ＝（ｎ、ｘ
０、ｚ０）は、（１４）式のヤコビ行列Ｊを用いて、反
復計算を行うことにより算出することができる。尚、
（１４）式において、△ｐを求める必要条件は、ｒａｎ
ｋＪ＝３である。これは、２平面を３回角度を変えて３
点計測するか、２平面を２回角度を変えて３点計測する
ことによって満たされる。

【００５８】なお、ここで示した例においては、ヤコビ
行列Ｊを用いた反復計算により、上記のカメラパラメー
タと、スリット光５の平面方程式のパラメータとを得る
手順を示したが、これ以外の計算法、例えばニュートン
法による反復計算によっても前記パラメータを得ること
ができる。さらに、データ点数を多くするほど、精度は
向上する。

【００５９】なお、ここに上げた較正手段の例としての
キャリブレーションルチーンは、カメラキャリブレーシ
ョンとスリット光キャリブレーションを行うために必要
な計測と、そのデータの記憶を同時に行い、そのデータ
群から各々のキャリブレーションに必要なデータを随時
に選択して演算する手順により構成している。

【００６０】このキャリブレーションルーチンを構成す
るにあたって、カメラキャリブレーションと、スリット
光キャリブレーションを別個に行う手順により構成する
こともできる。即ち、最初にカメラキャリブレーション
に必要なデータの計測と記憶を行い、カメラキャリブレ
ーションのための演算を行ってカメラパラメータを得た
後に、スリット光キャリブレーションを行うとする手順
の構成とすることもできる。いずれの手順により構成す
るにしても、本発明にかかる三次元位置計測装置におい
ては、上記の二つのキャリブレーションのいずれも行う
必要がある。

【００６１】

【発明の効果】以上説明したように、本発明にかかる三
次元位置計測装置は、該装置の構成要素として取付け位
置が確定されているべき光照射手段と撮像装置の取付け
位置の微少なズレを、演算手段によって較正することが
できるという効果を奏する。さらに、前記の効果によっ
て、前記光照射手段や撮像装置の取付け位置を確定する
ための機械的な調整機構を不要にできるので、部品コス
トを低減することができると共に、オペレータの負担お
よび調整時間を低減しつつ、高精度の三次元位置計測を
行うことができるという効果を奏する。

【図面の簡単な説明】

【図１】三次元位置計測装置による位置計測の概要を示
す図である。

【図２】三次元位置計測装置の構成を示す図である。

【図３】三次元位置計測装置のブロック構成を示す図で
ある。

【図４】三次元位置計測を行う手順を示すフローチャー
ト図である。

【図５】光パターンの照射と、撮像装置による画像位置
を記録する様子を示す図である。

【図６】光切断線上の任意の点および両端点を示す図で
ある。

【図７】フレームバッファ座標系とキャリブレーション
座標系の対応を示す図である。

【図８】三次元位置計測により貨物の立体配置の状況を
認識する様子を示す図である。

【符号の説明】

１パレット２ターゲット３センサ機構４認識処理制御盤５スリット光６スリット光投光器７反射ミラー８ステッピングモーター９撮像装置１０レンズ１１撮像部材１２ａ、１２ｂＩ／Ｏ部１６演算部１７ａフレームバッファ領域１７ｂキャリブレーション領域１７ｃカメラパラメータ領域１７ｄスリット光パラメータ領域１８ａキャリブレーションルーチン１８ｂ画像処理ルーチン１８ｃ三次元位置測定ルーチン１８ｄ配置パターン検出ルーチン２０ロボット制御盤２１ハンドリングロボット２２ローラコンベア５１貨物

Claims

【特許請求の範囲】

【請求項１】実際の空間における位置を表すキャリブ
レーション座標系において所定の位置に取り付けられ、
光照射手段による位置計測対象物からの反射光を上方で
所定の焦点距離に画像として結像する撮像装置と、前記撮像装置の位置を原点に取り直したセンサ座標系に
おいて所定の位置に取り付けられ、反射ミラーの傾斜角
度を変更することにより所定の光パターンを任意の照射
角度で位置計測対象物に斜め上方から照射する前記光照
射手段と、撮像装置からの連続した画像信号を離散的な画像データ
として記憶する記憶装置と、前記結像画像を記憶装置上の位置を表すフレームバッフ
ァ座標系の画像位置情報に変換する演算を行う画像処理
手段と、前記画像位置情報に基づき、前記センサ座標系における
前記計測対象物の三次元位置を三角測量の原理により演
算する位置演算手段と、前記三次元位置をキャリブレーション座標系の三次元位
置に変換する変換演算手段と、前記光照射手段および撮像装置の取付位置の微少なズレ
を較正する演算を行う較正手段とを含んでなり、該較正手段は、位置が既知である較正用ターゲットの位
置情報と、前記反射ミラーの傾斜角度の情報と、前記撮
像装置の焦点距離の情報とから、前記フレームバッファ座標系で表される前記画像位置情
報を、センサ座標系の位置に変換する画像変換の第一係
数と、三角測量の原理から演算したセンサ座標系での三次元位
置を、前記キャリブレーション座標系の位置に変換する
座標変換の第二係数と、前記光照射手段が照射する光パターンの前記センサ座標
系における照射中心位置および照射角度からなる光パラ
メータとを演算するものであって、前記位置計測対象物の前記キャリブレーション座標系に
おける三次元位置を、前記画像処理手段と、前記位置演
算手段と、前記変換演算手段と、前記較正手段とに基づ
いて得るように構成したことを特徴とする３次元位置計
測装置。