(57)【要約】      （修正有）【課題】高精度な３次元形状の計測が可能な３次元計測装置及びこれを備えるロボット装置を提供する。【解決手段】ワークを撮像して第１画像を取得する基準カメラ２２と、基準カメラ２２とは異なる視点でワークを撮像して第２画像を取得する参照カメラ２３と、第１画像及び第２画像のそれぞれからワークの複数のエッジ線を抽出し、第１画像の複数のエッジ線の中から任意に選択された第１エッジ線の端部と、第１エッジ線に対応するように第２画像の複数のエッジ線から抽出された第２エッジ線の端部と、の仮の３次元座標に対する視線誤差を演算し、第１エッジ線の端点及び第２エッジ線の端点のそれぞれの視線誤差とワークの３次元形状に基づいて設定された３次元拘束条件とから、評価関数Ｆを設定し、評価関数Ｆを最適化計算することでワークの３次元座標を計測するカメラ制御部５と、を備えた３次元計測装置。【選択図】図３
2014013147.tif 000002
【特許請求の範囲】
【請求項１】
ワークを撮像して第１画像を取得する第１カメラと、
前記第１カメラとは異なる視点でワークを撮像して第２画像を取得する第２カメラと、
前記第１画像及び前記第２画像のそれぞれからワークの複数のエッジ線を抽出し、前記第１画像の複数のエッジ線の中から任意に選択された第１エッジ線の端部と、前記第１エッジ線に対応するように前記第２画像の複数のエッジ線から抽出された第２エッジ線の端部と、の仮の３次元座標に対する視線誤差を演算し、前記第１エッジ線の端点及び第２エッジ線の端点のそれぞれの視線誤差とワークの３次元形状に基づいて設定された３次元拘束条件とから、評価関数を設定し、前記評価関数の値を最小又は最大にする最適化計算を行ってワークの３次元座標を決定する制御部と、を備えた、
ことを特徴とする３次元計測装置。
【請求項２】
ワークを撮像して第１画像を取得する第１カメラと、
前記第１カメラとは異なる視点でワークを撮像して第２画像を取得する第２カメラと、
前記第１画像及び前記第２画像のそれぞれからワークの複数のエッジ線を抽出し、前記第１画像の複数のエッジ線の中から任意に選択された第１エッジ線の端部の仮の３次元座標に対する視線誤差を演算すると共に、前記第１エッジ線に対応するように前記第２画像の複数のエッジ線の中から抽出された第２エッジ線の前記仮の３次元座標に対する視平面誤差を演算し、前記第１エッジ線の端点の視線誤差、第２エッジ線の視平面誤差及びワークの形状に基づいて設定された３次元拘束条件から評価関数を設定し、前記評価関数の値を最小又は最大にする最適化計算を行ってワークの３次元座標を決定する制御部と、を備えた、
ことを特徴とする３次元計測装置。
【請求項３】
前記３次元拘束条件は、前記第１画像又は前記第２画像の複数のエッジ線の中の２本のエッジ線が、同一平面、同一直線、垂直及び平行の中からいずれか１つ以上を設定したものである、
ことを特徴とする請求項１又は２に記載の３次元計測装置。
【請求項４】
ワークを３次元計測する請求項１から３のいずれか１項に記載の３次元計測装置と、
前記３次元計測装置により計測されたワークの３次元位置に基づいてワークを把持するロボットアームと、を備えた、
ことを特徴とするロボット装置。
【請求項５】
第１カメラでワークを撮像して取得した第１画像からワークの複数のエッジ線を抽出する第１画像エッジ線抽出工程と、
第２カメラでワークを撮像して取得した第２画像からワークの複数のエッジ線を抽出する第２画像エッジ線抽出工程と、
前記第１画像の前記複数のエッジ線の中から任意に選択した第１エッジ線に対応するように前記第２画像の前記複数のエッジ線の中から抽出された第２エッジ線の、前記第１エッジ線の端点に対応する端点を演算する対応点演算工程と、
仮の３次元座標に対する、前記第１エッジ線の端点と、前記第２エッジ線の端点と、のそれぞれの視線誤差を演算する視線誤差演算工程と、
前記第１エッジ線の端点と前記第２エッジ線の端点とのそれぞれに設定された前記視線誤差と、ワークの形状に基づいて設定された３次元拘束条件とから評価関数を設定する評価関数設定工程と、
前記評価関数を最小又は最大にする最適化計算を行ってワークの３次元座標を決定する３次元座標計測工程と、備えた、
ことを特徴とする３次元計測方法。
【請求項６】
第１カメラでワークを撮像して取得した第１画像からワークの複数のエッジ線を抽出する第１画像エッジ線抽出工程と、
第２カメラでワークを撮像して取得した第２画像からワークの複数のエッジ線を抽出する第２画像エッジ線抽出工程と、
前記第１画像の前記複数のエッジ線の中から任意に選択された第１エッジ線の端部の、仮の３次元座標に対する視線誤差と、前記第１エッジ線に対応するように前記第２画像の前記複数のエッジ線の中から抽出された第２エッジ線の、前記仮の３次元座標に対する視平面誤差と、を演算する誤差演算工程と、
前記第１エッジ線の端点の視線誤差、前記第２エッジ線の視平面誤差、及びワークの３次元形状に基づいて設定された３次元拘束条件と、から評価関数を設定する評価関数設定工程と、
前記評価関数の値を最小又は最大にする最適化計算を行ってワークの３次元座標を決定する３次元座標計算工程と、備えた、
ことを特徴とする３次元計測方法。
【請求項７】
請求項５又は６に記載の各工程をコンピュータに実行させるための３次元計測プログラム。
【請求項８】
請求項７に記載の３次元計測プログラムを記録したコンピュータ読み取り可能な記録媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、２つ以上の視点からワークの撮像を行い、３次元計測や立体視を行う３次元計測装置及びこれを備えるロボット装置に関する。
【背景技術】
【０００２】
従来、非接触にてワークの３次元形状を計測する方法として、複数視点に設置したカメラからワークを撮像し、撮像した複数の画像からワークの３次元形状を計測するステレオ法が知られている。また、ステレオ法においては、少ない情報量でワークの３次元形状を計測するために、撮像した画像上のワークのエッジ線を利用する場合がある。
【０００３】
しかしながら、エッジ線を利用して３次元形状を計測するステレオ法は、理想的な条件下においてはワークの正確な３次元形状を得ることができるが、実際にはカメラ校正時の誤差やエッジ線抽出時の誤差が存在する。そのため、計測した３次元形状において、本来繋がっているべきエッジ線がねじれの位置に復元されたり、エッジ線間の角度が正確でなかったりする場合があるという問題があった。
【０００４】
これに対しては、ワークの複数のエッジ線が同一平面上に存在するという条件（以下、「平面拘束条件」という）を用いて対応点を補正することで、３次元計測の精度を向上させる技術が提案されている（特許文献１参照）。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００３−２４８８１４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、特許文献１に開示の技術は、同一平面上に存在する点群が満たすべき平面拘束条件を用いて平面パラメータを求めて、画像上の対応点を補正する。そのため、同一平面上にないエッジ線同士の相対角度やエッジ線同士の立体的な位置関係等のような複合的な３次元拘束条件を加えることができなかった。
【０００７】
ところが、撮像した画像から実際に３次元データを生成する場合には、設計上から、「エッジ線同士が垂直になっている」、「エッジ線同士が水平になっている」、「エッジ線同士が同一直線上にある」等の複合的な３次元拘束条件が予め分かっていることが多い。そのため、これらの複合的な３次元拘束条件を適用して３次元計測の精度を向上させた３次元計測装置が望まれていた。
【０００８】
そこで、本発明は、高精度な３次元形状の計測が可能な３次元計測装置及びこれを備えるロボット装置を提供することを目的とする。
【課題を解決するための手段】
【０００９】
本発明は、３次元計測装置において、ワークを撮像して第１画像を取得する第１カメラと、前記第１カメラとは異なる視点でワークを撮像して第２画像を取得する第２カメラと、前記第１画像及び前記第２画像のそれぞれからワークの複数のエッジ線を抽出し、前記第１画像の複数のエッジ線の中から任意に選択された第１エッジ線の端部と、前記第１エッジ線に対応するように前記第２画像の複数のエッジ線から抽出された第２エッジ線の端部と、の仮の３次元座標に対する視線誤差を演算し、前記第１エッジ線の端点及び第２エッジ線の端点のそれぞれの視線誤差とワークの３次元形状に基づいて設定された３次元拘束条件とから、評価関数を設定し、前記評価関数の値を最小又は最大にする最適化計算を行ってワークの３次元座標を決定する制御部と、を備えたことを特徴とする。
【００１０】
また、本発明は、３次元計測装置において、ワークを撮像して第１画像を取得する第１カメラと、前記第１カメラとは異なる視点でワークを撮像して第２画像を取得する第２カメラと、前記第１画像及び前記第２画像のそれぞれからワークの複数のエッジ線を抽出し、前記第１画像の複数のエッジ線の中から任意に選択された第１エッジ線の端部の仮の３次元座標に対する視線誤差を演算すると共に、前記第１エッジ線に対応するように前記第２画像の複数のエッジ線の中から抽出された第２エッジ線の前記仮の３次元座標に対する視平面誤差を演算し、前記第１エッジ線の端点の視線誤差、第２エッジ線の視平面誤差及びワークの形状に基づいて設定された３次元拘束条件から評価関数を設定し、前記評価関数の値を最小又は最大にする最適化計算を行ってワークの３次元座標を決定する制御部と、を備えたことを特徴とする。
【００１１】
また、本発明は、３次元計測方法において、第１カメラでワークを撮像して取得した第１画像からワークの複数のエッジ線を抽出する第１画像エッジ線抽出工程と、第２カメラでワークを撮像して取得した第２画像からワークの複数のエッジ線を抽出する第２画像エッジ線抽出工程と、前記第１画像の前記複数のエッジ線の中から任意に選択した第１エッジ線に対応するように前記第２画像の前記複数のエッジ線の中から抽出された第２エッジ線の、前記第１エッジ線の端点に対応する端点を演算する対応点演算工程と、仮の３次元座標に対する、前記第１エッジ線の端点と、前記第２エッジ線の端点と、のそれぞれの視線誤差を演算する視線誤差演算工程と、前記第１エッジ線の端点と前記第２エッジ線の端点とのそれぞれに設定された前記視線誤差と、ワークの形状に基づいて設定された３次元拘束条件とから評価関数を設定する評価関数設定工程と、前記評価関数を最小又は最大にする最適化計算を行ってワークの３次元座標を決定する３次元座標計測工程と、備えたことを特徴とする。
【００１２】
また、本発明は、３次元計測方法において、第１カメラでワークを撮像して取得した第１画像からワークの複数のエッジ線を抽出する第１画像エッジ線抽出工程と、第２カメラでワークを撮像して取得した第２画像からワークの複数のエッジ線を抽出する第２画像エッジ線抽出工程と、前記第１画像の前記複数のエッジ線の中から任意に選択された第１エッジ線の端部の、仮の３次元座標に対する視線誤差と、前記第１エッジ線に対応するように前記第２画像の前記複数のエッジ線の中から抽出された第２エッジ線の、前記仮の３次元座標に対する視平面誤差と、を演算する誤差演算工程と、前記第１エッジ線の端点の視線誤差、前記第２エッジ線の視平面誤差、及びワークの３次元形状に基づいて設定された３次元拘束条件と、から評価関数を設定する評価関数設定工程と、前記評価関数の値を最小又は最大にする最適化計算を行ってワークの３次元座標を決定する３次元座標計算工程と、備えたことを特徴とする。
【発明の効果】
【００１３】
本発明によれば、同時に満たす複合的な３次元拘束条件を設定した評価関数を最適化計算することで、高精度な３次元形状の計測を行うことができる。
【図面の簡単な説明】
【００１４】
【図１】本発明の実施形態に係るロボット装置の概略構成を示す断面図である。
【図２】本実施形態に係るロボット装置の制御装置を示すブロック図である。
【図３】第１実施形態に係るカメラ制御部の構成を示すブロック図である。
【図４】第１実施形態に係る３次元座標の計測を示すフローチャートである。
【図５】第１実施形態に係る基準カメラ、参照カメラ及びワークを示す模式図である。
【図６】エピポーラ線を用いた対応点の求め方を説明するための図である。
【図７】第１エッジ線の第１端点及び第２エッジ線の第２端点の視線誤差を説明するための模式図である。
【図８】第１実施形態に係るエッジ線間の対応付けを行った際のデータ構造を示す図である。
【図９】第１実施形態に係るエッジ線間の３次元拘束条件を説明するための模式図である。
【図１０】第２実施形態に係るカメラ制御部の構成を示すブロック図である。
【図１１】第２実施形態に係る３次元座標の計測を示すフローチャートである。
【図１２】第２実施形態に係る視線誤差と視平面誤差とを説明するための模式図である。
【発明を実施するための形態】
【００１５】
＜第１実施形態＞
以下、本発明の第１実施形態に係るロボット装置１について、図１から図９を参照しながら説明する。まず、ロボット装置１の概略構成について、図１から図３を参照しながら説明する。図１は、本発明の実施形態に係るロボット装置１を示す断面図である。図２は、本実施形態に係るロボット装置１の制御装置４の構成を示すブロック図である。図３は、第１実施形態に係るカメラ制御部５の構成を示すブロック図である。
【００１６】
図１に示すように、第１実施形態に係るロボット装置１は、ワークの組立を行うロボット２と、ロボット２によるワークの組立が行われる架台３と、ロボット２を制御する制御装置４と、を備えている。
【００１７】
ロボット２は、６軸多関節の汎用ロボットであり、ロボットアーム２０と、ロボットアームの先端部に取り付けられたハンド２１と、第１カメラとしての基準カメラ２２と、第２カメラとしての参照カメラ２３と、を備えている。なお、本実施形態においては、基準カメラ２２と参照カメラ２３の２台のカメラを用いたが、例えば、ステレオカメラのような複眼カメラを用いてもよい。
【００１８】
ロボットアーム２０は、各関節を各関節軸まわりにそれぞれ回転駆動する６つのアクチュエータ（図示せず）を備えており、６つのアクチュエータのそれぞれを選択的に駆動することで先端部に取り付けられたハンド２１を任意の３次元位置に移動させる。ハンド２１は、ワーク等を把持する把持部２１ａと、把持部２１ａを駆動する不図示のアクチュエータと、を備えており、ロボットアーム２０の駆動により移動し、移動した位置でアクチュエータを駆動することでワーク等を把持する。なお、ハンド２１には、不図示の力覚センサが設けられており、ワークを把持する際には、力覚センサによりワークを把持する把持力が調整される。また、ハンド２１は、ロボットアーム２０の先端に着脱自在に取り付けられており、作業するワークの形状等により交換可能になっている。
【００１９】
基準カメラ２２は、ロボットアーム２０の先端に取り付けられており、ワークを撮像してワークの第１画像を取得する。参照カメラ２３は、ロボットアーム２０の先端で基準カメラ２２に隣接して取り付けられており、基準カメラ２２と異なる視点でワークを撮像してワークの第２画像を取得する。
【００２０】
架台３は、矩形箱状に形成されており、上面にワークが載置される平面状の載置部３０が設けられている。載置部３０の略中央部には、ワークの組立に用いられる治具３１が設けられており、載置部３０の隅部には、ロボットアーム２０の基端部を固定支持する支持部３２と、部品供給機３３と、工具置き台３４と、が設けられている。架台３の下部には、架台３を移動させるための４つのキャスター３５・・・と、架台３を床面に固定させるための４つの固定金具３６・・・と、が設けられており、任意の位置に移動した後、固定可能となっている。
【００２１】
図２に示すように、制御装置４は、演算装置４０と、記憶装置４１と、を有するコンピュータ本体に、ロボットアーム２０、ハンド２１、基準カメラ２２及び参照カメラ２３がバスを介して接続されて構成されている。また、コンピュータ本体には、入力装置４２、ティーチングペンダント４３、ディスプレイ４４、スピーカ４５、記録メディア読取装置４６及び通信装置４７などもバスを介して接続されている。なお、図２においては、これらを接続するためのインターフェイスは不図示としている。
【００２２】
演算装置４０は、ＣＰＵ４０ａと、画像処理装置４０ｂと、音声処理装置４０ｃと、を備えている。ＣＰＵ４０ａは、カメラ制御部５と、ロボット制御部６と、を備えている。カメラ制御部５は、記憶装置４１に記憶された各種プログラムや入力装置４２から入力される設定等に従って、ワークの３次元位置を計測する。
【００２３】
図３に示すように、カメラ制御部５は、第１画像エッジ線抽出部５１と、第２画像エッジ線抽出部５２と、対応点演算部５３と、視線誤差演算部５４と、評価関数設定部５５と、３次元座標計測部５６と、を備えている。
【００２４】
第１画像エッジ線抽出部５１は、基準カメラ２２でワークを撮像して取得した第１画像から、ワークの複数のエッジ線を抽出する。第２画像エッジ線抽出部５２は、参照カメラ２３でワークを撮像して取得した第２画像から、ワークの複数のエッジ線を抽出する。対応点演算部５３は、第１画像の複数のエッジ線の中から任意に選択した第１エッジ線に対応する第２エッジ線を第２画像の複数のエッジ線の中から抽出し、抽出した第２エッジ線の第１エッジ線の端点に対応する端点を演算する。視線誤差演算部５４は、仮の３次元座標を設定し、仮の３次元座標に対する、第１エッジ線の端点と、第２エッジ線の端点と、のそれぞれの視線誤差を演算する。評価関数設定部５５は、第１エッジ線の端点と第２エッジ線の端点とのそれぞれに設定された視線誤差と、ワークの形状に基づいて設定された３次元拘束条件と、から視線誤差と３次元拘束条件とを含む評価関数を設定する。３次元座標計測部５６は、評価関数が最小ないし最大となるように最適化計算を行ってワークの３次元座標を計測する。なお、カメラ制御部５によるワークの３次元位置の計測については、後に詳しく説明する。
【００２５】
ロボット制御部６は、カメラ制御部５により計測されたワークの３次元位置及び記憶装置４１に記憶された各種プログラムや入力装置４２から入力される設定等に基づいて、ロボットアーム２０やハンド２１を制御する。なお、ロボット制御部６によるロボットアーム２０やハンド２１の制御については、その説明を省略する。
【００２６】
画像処理装置４０ｂは、ＣＰＵ４０ａからの描画指示に応じてディスプレイ４４を制御して、画面上に所定の画像を表示させる。音声処理装置４０ｃはＣＰＵ４０ａからの発音指示に応じた音声信号生成してスピーカ４５に出力する。
【００２７】
記憶装置４１は、バスを介してＣＰＵ４０ａに接続されており、各種プログラムやデータ等が格納されたＲＯＭ４１ａと、ＣＰＵ４０ａの作業領域として確保されたＲＡＭ４１ｂと、を備えている。本実施形態においては、ＲＯＭ４１ａには、ロボットアーム２０やハンド２１の駆動プログラムに加え、ワークを３次元計測するための各種プログラム等が格納されている。例えば、後述する第１画像エッジ線抽出工程、第２画像エッジ線抽出工程、対応点演算工程、視線誤差演算工程、評価関数設定工程及び３次元座標計測工程を実行させるための３次元計測プログラム等が格納されている。また、ＲＯＭ４１ａは、作業者により、３次元拘束条件や基準カメラ２２及び参照カメラ２３のカメラパラメータ等のデータが入力装置４２から入力された際には、これらを格納する。
【００２８】
入力装置４２は、キーボード４２ａと、マウス４２ｂと、から構成されており、ワークの３次元位置の計測に必要な情報、或いはその他の指示の入力を可能としている。記録メディア読取装置４６は、例えば、３次元計測プログラム等を記録したコンピュータ読み取り可能な記録媒体４８を読み込み、ＲＯＭ４１ａに格納させるため等に用いられる。通信装置４７は、例えば、上述したような記録媒体４８を使用せずに、通信装置４７を介してインターネット等から配信される３次元計測プログラム等をダウンロードする際に用いられる。
【００２９】
次に、上述のように構成されたロボット装置１のカメラ制御部５によるワークの３次元座標の計測方法について、図４に示すフローチャートに沿って、図５から図９を参照しながら説明する。図４は、第１実施形態に係る３次元座標の計測を示すフローチャートである。図５は、第１実施形態に係る基準カメラ２２、参照カメラ２３及びワークを示す模式図である。図６は、エピポーラ線を用いた対応点の求め方を説明するための図である。図７は、第１エッジ線の第１端点及び第２エッジ線の第２端点の視線誤差を説明するための模式図である。図８は、第１実施形態に係るエッジ線間の対応付けを行った際のデータ構造を示す図である。図９は、第１実施形態に係るエッジ線間の３次元拘束条件を説明するための模式図である。
【００３０】
まず、図５に示すように、異なる視点から共通視野を持つように配置された、基準カメラ２２及び参照カメラ２３のそれぞれのレンズ主点ＯＬ、ＯＲを原点とし、各々のカメラ座標系のＺ軸が画像面と直交するように設定する。以下、基準カメラ２２を代表する座標系を基準カメラ座標系ＣＬ、参照カメラ２３を代表する座標系を参照カメラ座標系ＣＲで表し、基準カメラ２２の画像面を基準画像面２２ａ、参照カメラ２３の画像面を参照画像面２３ａと表す。
【００３１】
次に、基準カメラ２２の内部パラメータと、参照カメラ２３の内部パラメータと、カメラの外部パラメータと、を求め、ＲＯＭ４１ａに記憶させておく。内部パラメータとは、カメラ座標系で表された任意の３次元座標を画像面上の点へ投影する計算の際に必要となるパラメータであり、公知のカメラキャリブレーション技術によって求めることができる。本実施形態においては、内部パラメータを算出する詳細な説明は省略し、下記の表現を用いるものとする。
【００３２】
ｆをレンズの焦点距離、ｃｕを画像面とカメラ光軸の交点の列座標、ｃｖを画像面とカメラ光軸の交点の行座標、ｓｘを画像センサの１画素あたりの幅、ｓｙを画像センサの１画素あたりの高さ、とする。なお、列座標ｃｕ、行座標ｃｖは、画像ピクセルに対し、左からｃｕ列目、上からｃｖ行目が、カメラ光軸と画像面の交点であることを意味する。また、ｃｕ、ｃｖは整数である必要はなく、サブピクセルの精度で求まっていてもよい。また、焦点距離ｆ、１画素あたりの幅ｓｘ及び１画素あたりの高さｓｙは、一般的な長さの単位（例えば、ミリメートルやメートル等）で表現される。
【００３３】
すると、基準カメラ２２の内部パラメータは、（ｆＬ、ｃｕＬ、ｃｖＬ、ｓｘＬ、ｓｙＬ）と表すことができ、参照カメラ２３の内部パラメータは、（ｆＲ、ｃｕＲ、ｃｖＲ、ｓｘＲ、ｓｙＲ）と表すことができる。なお、添え字Ｌは、基準カメラ２２の内部パラメータを示し、添え字Ｒは、参照カメラ２３の内部パラメータを示している。
【００３４】
また、画像座標（ｕ、ｖ）の点は、内部パラメータを用いて下記のようにカメラ座標系の座標Ｐ＝Ｔ（ｘ、ｙ、ｚ）に変換することができる。
【００３５】
【数１】
2014013147.tif 000003
【００３６】
外部パラメータは、基準カメラ２２と参照カメラ２３の６自由度の相対姿勢を表すパラメータであり、公知のキャリブレーション技術によって求めることができる。本実施形態においては、基準カメラ２２と参照カメラ２３の間の外部パラメータを算出する詳細な説明は省略し、回転行列Ｒ及び並進ベクトルｔで表す。
【００３７】
ここで、回転行列Ｒと並進ベクトルｔは、基準カメラ座標系ＣＬから参照カメラ座標系ＣＲへの座標変換を表しており、参照カメラ座標系ＣＲで記述された任意の３次元座標（Ｒ）Ｘは、基準カメラ座標系ＣＬでは外部パラメータを用いて下のように表される。
【００３８】
【数２】
2014013147.tif 000004
【００３９】
なお、上記においては、基準カメラ座標系ＣＬ及び参照カメラ座標系ＣＲで表現された座標値であることを明確にするために、（Ｌ）及び（Ｒ）の添え字を用いた。以下の説明においては、基準カメラ座標系を全体の基準座標系とし、特に座標系を表す添え字の無い場合は基準カメラ座標系で記述されているものとする。このときＯＬ＝Ｔ（０、０、０）、ＯＲ＝ｔである。また、カメラの内部パラメータ、外部パラメータに関しては行列形式での表現など、様々な表現方法が存在するが、本実施形態においてはどの形式の表現を用いるかという点は本質的ではない。また、本実施形態においては、レンズ歪については考慮していないが、レンズ歪について考慮したパラメータを用いてもよい。
【００４０】
次に、評価関数の準備を行う。本実施形態においては、画像点に対する視線からの誤差と３次元拘束条件とからの誤差を評価するための評価関数が重要な役割を担う。本実施形態においては、評価関数をＦで表す。本実施形態に係る評価関数Ｆは、視線誤差項部分と３次元拘束条件項部分の和の形で表される。評価関数Ｆの視線誤差項部分をＦｓｉｇｈｔ（Ｎ）、評価関数Ｆの３次元拘束条件項部分をＦｃｏｎｓｔ（Ｍ）と表す。すなわち、評価関数Ｆは、
【００４１】
【数３】
2014013147.tif 000005
となる。なお、図４に示す処理フロー開始時点の初期値は、Ｆｓｉｇｈｔ（Ｎ）＝０、かつ、Ｆｃｏｎｓｔ（Ｍ）＝０であり、ループ処理により項を追加していく。添え字ＮはＮ番目までのエッジ対応付けに関する視線誤差項であることを表し、添え字ＭはＭ個の拘束条件を設定した時点での拘束条件項であることを表している。
【００４２】
次に、基準カメラ２２でワーク１０を撮像して第１画像としての基準画像２２ｂを取得し、取得した基準画像２２ｂに対して、ワーク１０の複数のエッジ線を抽出するエッジ線抽出処理を行う（ステップＳ１０１、第１画像エッジ線抽出工程）。具体的には、まず、基準画像２２ｂに対して、ソーベルフィルタやキャニーフィルタ等のエッジ抽出フィルタを用い、輝度変化が最大となる点を、エッジ点群として抽出する。次に、抽出したエッジ点群に対して、直線近似の処理を行うとともに線分に分割し、それぞれの端点の画像座標（画素の位置）を求める。画像座標に関しては、画素単位の値としても構わないが、高精度の計測を行うためにはサブピクセル精度の処理を用いることが望ましい。本実施形態においては、エッジ点群を線分に近似したものを、エッジ線と呼ぶ。同様に、参照カメラ２３でワーク１０を撮像した第２画像としての参照画像２３ｂに対して、ワーク１０の複数のエッジ線を抽出するエッジ線抽出処理を行う（ステップＳ１０１、第２画像エッジ線抽出工程）。
【００４３】
次に、基準画像２２ｂにおいて抽出された複数のエッジ線が、参照画像２３ｂでどこに撮影されているかを特定する対応付け処理を行い、ＲＯＭ４１ａに記憶させる。つまり、基準画像２２ｂから抽出された複数のエッジ線と、参照画像２３ｂから抽出された複数のエッジ線と、の対応付けを行い、これを記憶しておく。対応付け処理は、対応付け終了と判定されるまで続けて行われる（ステップＳ１０２〜Ｓ１０６）。なお、対応付け自体は、ユーザがＵＩ上で画像を見ながら同一と思われるエッジ線を指定してもよく、カメラ制御部５が画像相関を用いて類似の画像パターンを検出することで選択する公知の手法を用いてもよい。具体的には、基準画像２２ｂから抽出した複数のエッジ線から１つのエッジ線（第１エッジ線）を選択し（ステップＳ１０３）、参照画像２３ｂ内でそれと対応するエッジ線（第２エッジ線）を選択する（ステップＳ１０４）。関連づけた第１エッジ線と第２エッジ線とに対して、後述の処理を行うことにより、３次元空間での１本のエッジ線が求まる。
【００４４】
次に、上述により対応付けられたエッジ線（例えば、第１エッジ線と第２エッジ線）は、照明条件等の制約により、一般には端点が同一の点とは限らない。そのため、対応付けられたエッジ線に対してエピポーラ拘束を用いてエッジ線上の点同士の対応を求める（ステップＳ１０５、対応点演算工程）。
【００４５】
ステップＳ１０１において、例えば、選択した第１エッジ線の端点の画像座標が分かっているので、第１エッジ線の端点に対する参照画像２３ｂ上のエピポーラ線と参照画像２３ｂ上の第２エッジ線との交点を求めることで、対応点の座標を求めることができる。例えば、図６に示すように、エピポーラ拘束を用いて基準画像２２ｂと参照画像２３ｂで共通に撮影されている部分を判定し、それぞれの端点を求める。なお、図６においては、基準画像２２ｂと参照画像２３ｂとの間で、同種の破線で表されたエピポーラ線がそれぞれ対応している。この対応点計算処理を行うことにより、ｉ番目のエッジ線の対応に対して、各々の画像上での画像点ＰＬｉ１、ＰＬｉ２、ＰＲｉ１、ＰＲｉ２の画像座標が求まる。
【００４６】
次に、ステップＳ１０５で求めた対応点の画像座標を用いて、未知の３次元座標Ｘｉｊに対する視線誤差項を求める（ステップＳ１０６、視線誤差演算工程）。ここで、添え字ｉは、ｉ番目のエッジ線対応を表し、添え字ｊは、ｊ＝１、２がそれぞれエッジ線の両端点に対応している。視線誤差項としては、レンズ主点ＯＬ、ＯＲと画像点ＰＬｉｊ、ＰＲｉｊとを結ぶ基準画像視線２４、参照画像視線２５を考えた際に、基準画像視線及び参照画像視線から３次元座標Ｘｉｊまでの距離ｄＬｉｊ、ｄＲｉｊに応じて値が大きくなるように定める。ここで、「基準画像視線２４、２５から３次元座標Ｘｉｊまでの距離ｄＬｉｊ、ｄＲｉｊ」とは、図７に示すように、３次元座標Ｘｉｊから基準画像視線２４、参照画像視線２５のそれぞれに降ろした垂線の長さである。本実施形態においては、基準画像視線２４、参照画像視線２５から３次元座標Ｘｉｊまでの距離ｄＬｉｊ、ｄＲｉｊの自乗和を、視線誤差項として用いる。
【００４７】
以下、視線誤差項の計算について、数式を用いて説明する。まず、画像点ＰＬｉｊの画像座標を（ｕＬｉｊ、ｖＬｉｊ）、画像点ＰＲｉｊの画像座標を（ｕＲｉｊ、ｖＲｉｊ）と表すと、それぞれのカメラ座標系における画像点ＰＬｉｊ、ＰＲｉｊの座標値は、次の式で表すことができる。
【００４８】
【数４】
2014013147.tif 000006
【００４９】
次に、それぞれのカメラ座標系原点ＯＬ、ＯＲから画像点ＰＬｉｊ、ＰＲｉｊに向かう単位視線ベクトルを求める。単位視線ベクトルをｅＬｉｊ、ｅＲｉｊで表すと、以下のように表せる。
【００５０】
【数５】
2014013147.tif 000007
【００５１】
この時点で基準カメラ座標系における単位視線ベクトルｅＬｉｊ及び単位視線ベクトルｅＲｉｊは、画像から抽出した対応点とカメラの内部パラメータ及び外部パラメータを用いて一意に求めることが可能な定数である。
【００５２】
ここで、未知の３次元座標Ｘｉｊ＝（ｘｉｊ、ｙｉｊ、ｚｉｊ）を導入し、Ｘｉｊと視線との距離を求める。求める距離ｄＬｉｊ及び距離ｄＲｉｊはベクトルの外積を用いて表すと以下のようになる。
【００５３】
【数６】
2014013147.tif 000008
【００５４】
ステップＳ１０６の最後に、未知の３次元座標Ｘｉｊと視線との距離ｄＬｉｊ、ｄＲｉｊに応じた視線誤差項を評価関数に加える。本実施形態においては、評価関数Ｆに距離ｄＬｉｊ、ｄＲｉｊの自乗和を加える。ｉ番目の視線誤差項を追加した時点における評価関数Ｆの視線誤差項部分をＦｓｉｇｈｔ（ｉ）と書くことにすると、
【００５５】
【数７】
2014013147.tif 000009
となる。
【００５６】
視線誤差項を加えた後は、ステップＳ１０２に戻り、追加の対応付けを終了するか否かの判断を行い、終了と判定されるまでステップＳ１０３〜Ｓ１０６の処理を繰り返し、処理を繰り返すたびに視線誤差項が追加される。その結果、Ｎ回の繰り返しを行った場合の評価関数の視線誤差項部分Ｆｓｉｇｈｔ（Ｎ）は下記のようになる。
【００５７】
【数８】
2014013147.tif 000010
【００５８】
ここで、評価関数の視線誤差項部分Ｆｓｉｇｈｔ（Ｎ）は、（ｘ１１、ｙ１１、ｚ１１、ｘ１２、ｙ１２、ｚ１２、・・・、ｘＮ１、ｙＮ１、ｚＮ１、ｘＮ２、ｙＮ２、ｚＮ２）の６Ｎ個の変数をもつ関数である。理解を助けるため、ステップＳ１０２のループを抜けた時点での対応づけのデータ構造例を図８に示す。対応付けのｉ回目に基準画像２２ｂのエッジ線Ｌα（ｉ）と参照画像２３ｂのエッジ線Ｒθ（ｉ）を対応づけたとすると、その組合せに対して３次元エッジ線Ｅｉが定義される。基準画像２２ｂにおけるエッジ線Ｌα（ｉ）上の画像点ＰＬｉ１、ＰＬｉ２と、参照画像２３ｂにおけるエッジ線Ｒθ（ｉ）上の点ＰＲｉ１、ＰＲｉ２がそれぞれ対応点として求められる。さらに、それぞれの３次元エッジ線Ｅｉは、Ｘｉ１＝Ｔ（ｘｉ１、ｙｉ１、ｚｉ１）とＸｉ２＝Ｔ（ｘｉ２、ｙｉ２、ｚｉ２）との２つの端点を持っている。さらに、端点の３次元座標Ｘｉｊ（ｉ＝１、２、・・・Ｎ、ｊ＝１、２）のそれぞれについて、視線誤差項（ｄＬｉｊ２、ｄＲｉｊ２）が定義されている。
【００５９】
次に、２本のエッジ線間の拘束条件の設定を行うとともに拘束条件項を求める（ステップＳ１０７〜Ｓ１１１）。まず、ステップＳ１０７において、拘束条件の追加を行うか否かを判定する。この判定は、ユーザの判断によって追加の要否を入力するようにしてもよく、自動的に判定するようなアルゴリズムを用いてもよい。拘束条件の追加を終了するまで、次のステップＳ１０８〜Ｓ１１１の処理を繰り返す。
【００６０】
次に、拘束条件を設定する２本の３次元エッジ線を選択する。なお、１本の３次元エッジ線は、ステップＳ１０３及びＳ１０４で設定された一組の組合せに対応している。例えば、図８に示された３次元エッジ線のリストの中から、ステップＳ１０８においてＥ１を選択し、ステップＳ１０９においてＥ２を選択する。
【００６１】
さらに、ステップＳ１０８〜Ｓ１０９で選択した２本のエッジ線に対して、エッジ線間の幾何的な３次元拘束条件を選択する。つまり、ワーク１０について事前に分かっている、エッジ線間の位置関係を設定する。本実施形態においては、２エッジ線間の幾何関係のうち、「２エッジ線が同一平面上にある」、「２エッジ線が同一直線上にある」、「２エッジ線の方向が平行である」及び「２エッジ線の方向が垂直である」、の４種類を取り扱う。以上の４種類の３次元拘束条件のうち、１つを選択してステップＳ１１１に進む。
【００６２】
次に、ステップＳ１１０において設定した３次元拘束条件に応じて、３次元拘束条件項ｆｃｏｎｓｔ（ｋ）を評価関数に追加する（図４（ｂ）参照）。まず、ステップＳ２０１において、追加するべき３次元拘束条件項を選択する。以下、各々の３次元拘束条件に応じた３次元拘束条件項の内容について説明する。なお、以下のステップＳ２０２〜Ｓ２０５の説明において、ステップＳ１０８で選択されたエッジ線の両端点をＸａ１、Ｘａ２、ステップＳ１０９で選択されたエッジ線の両端点をＸｂ１、Ｘｂ２とする。
【００６３】
「２本のエッジ線が同一平面上にある（同一平面項追加、ステップＳ２０２）」という３次元拘束条件が選択された場合は、この処理を行う。同一平面項として、２本のエッジ線が同一平面上に存在するときに値が最小となる関数を用いる。例えば、４点が同一平面上に存在するという条件は、Ｘａ１、Ｘｂ１、Ｘｂ２の３点で張った平面上にＸａ２も存在することから、下記の式で表すことができる。
【００６４】
【数９】
2014013147.tif 000011
【００６５】
よって、ｋ番目の拘束条件項ｆｃｏｎｓｔ（ｋ）として、下記のような同一平面項ｆｐｌａｎｅを代入する。
【００６６】
【数１０】
2014013147.tif 000012
【００６７】
なお、ここでλｋは重み係数であり、異なる３次元拘束条件間で重みづけを調整するために使用する定数である。重み係数λｋは、ユーザが個々に設定するようにしてもよいし、エッジ線の長さに応じて重みづけを決定するアルゴリズムを用いてもよい。
【００６８】
次に、「２本のエッジ線が同一直線上にある」という３次元拘束条件が選択された場合は、この処理を行う。同一直線項として、２本のエッジ線が同一直線上に存在するときに値が最小となる関数を用いる。例えば、３点が同一直線上にあるという条件は、ベクトルの外積を用いて次の式で表すことができる。
【００６９】
【数１１】
2014013147.tif 000013
【００７０】
よって、ｋ番目の拘束条件項ｆｃｏｎｓｔ（ｋ）として、下記のような同一直線項ｆｌｉｎｅａｒを代入する。
【００７１】
【数１２】
2014013147.tif 000014
【００７２】
なお、ここでλｋ１、λｋ２は重み係数であり、異なる３次元拘束条件間で重みづけを調整するために使用する定数である。重み係数λｋ１、λｋ２は、同一平面項の場合と同様に、ユーザが個々に設定するようにしてもよいし、エッジ線の長さに応じて重みづけを決定するアルゴリズムを用いてもよい。
【００７３】
次に、「２本のエッジ線が平行である」という３次元拘束条件が選択された場合は、この処理を行う。平行項として、２本のエッジ線が平行であるときに値が最小となる関数を用いる。「２本のエッジ線が平行である」という条件は、ベクトルの外積を用いて下記の数式で表現することができる。
【００７４】
【数１３】
2014013147.tif 000015
【００７５】
よって、ｋ番目の拘束条件項ｆｃｏｎｓｔ（ｋ）として、下記のような同一直線項ｆｐａｒａｌｌｅｌを代入する。
【００７６】
【数１４】
2014013147.tif 000016
【００７７】
なお、ここでλｋは重み係数であり、異なる３次元拘束条件間で重みづけを調整するために使用する定数である。他の拘束条件項と同様に、重み係数λｋは任意の方法で設定してよい。
【００７８】
次に、「２本のエッジ線が垂直である」という３次元拘束条件が選択された場合は、この処理を行う。垂直項として、２本のエッジ線が垂直であるときに値が最小となる関数を用いる。２本のエッジ線が垂直である、という条件は、ベクトルの内積を用いて下記の数式で表現することができる。
【００７９】
【数１５】
2014013147.tif 000017
【００８０】
よって、ｋ番目の拘束条件項ｆｃｏｎｓｔ（ｋ）として、下記のような同一直線項ｆｖｅｒｔｉｃａｌを代入する。
【００８１】
【数１６】
2014013147.tif 000018
【００８２】
なお、ここでλｋは重み係数であり、異なる３次元拘束条件間で重みづけを調整するために使用する定数である。他の拘束条件項と同様に、重み係数λｋは任意の方法で設定してよい。本実施形態に場合、拘束条件項ｆｃｏｎｓｔ（ｋ）は、（ｘａ１、ｙａ１、ｚａ１、ｘａ２、ｙａ２、ｚａ２、ｘｂ１、ｙｂ１、ｚｂ１、ｘｂ２、ｙｂ２、ｚｂ２）の１２個の変数をもつ関数である。
【００８３】
ステップＳ１１１において、拘束条件項ｆｃｏｎｓｔ（ｋ）を追加した後はステップＳ１０７の処理に戻り、さらに、３次元拘束条件を追加するか否かを判定する。そして、３次元拘束条件の追加を終了するまでステップＳ１０８〜Ｓ１１１の処理を繰り返す。３次元拘束条件項ｆｃｏｎｓｔ（ｋ）は、矛盾しない範囲で任意の個数設定することができる。例えば、ある２本のエッジ線に対して「同一平面上」と「垂直」という２つの３次元拘束条件を設定することも可能である。ステップＳ１０７〜Ｓ１１１のループによってＭ個の３次元拘束条件を設定した場合、評価関数Ｆの拘束条件項部分Ｆｃｏｎｓｔ（Ｍ）は下記のようになる。
【００８４】
【数１７】
2014013147.tif 000019
【００８５】
Ｆｃｏｎｓｔ（Ｍ）は、（ｘ１１、ｙ１１、ｚ１１、ｘ１２、ｙ１２、ｚ１２、・・・、ｘＮ１、ｙＮ１、ｚＮ１、ｘＮ２、ｙＮ２、ｚＮ２）の最大６Ｎ個の変数をもつ関数である。ただし、実際にＦｃｏｎｓｔ（Ｍ）に含まれる変数は、ステップＳ１０８〜Ｓ１０９において選択されたエッジ線の数に対応する。
【００８６】
次に、ステップＳ１０２〜Ｓ１０６において追加された視線誤差項部分Ｆｓｉｇｈｔ（Ｎ）と、ステップＳ１０７〜Ｓ１１１の処理において追加された拘束条件項部分Ｆｃｏｎｓｔ（Ｍ）とから、評価関数Ｆを設定する（評価関数設定工程）。上述したように、評価関数Ｆは、Ｆｓｉｇｈｔ（Ｎ）と、拘束条件項部分Ｆｃｏｎｓｔ（Ｍ）と、の和の形で表される。即ち、
【００８７】
【数１８】
2014013147.tif 000020
で表される。
【００８８】
次に、評価関数Ｆを最少とすることで３次元座標Ｘｉｊ（ｉ＝１、２、・・・、ｊ＝１、２）を求める（３次元座標計測工程）。つまり、評価関数Ｆに対して、
【００８９】
【数１９】
2014013147.tif 000021
となるような変数の組（ｘ１１、ｙ１１、ｚ１１、ｘ１２、ｙ１２、ｚ１２、・・・、ｘＮ１、ｙＮ１、ｚＮ１、ｘＮ２、ｙＮ２、ｚＮ２）を求める。この計算は、公知の多変数最適化手法を用いて行えばよく、例えば、非線形探索手法のひとつであるＮｅｌｄｅｒ―Ｍｅａｄの滑降シンプレックス法を使用することができる。
【００９０】
以上の処理により求められた座標値（ｘ１１、ｙ１１、ｚ１１、ｘ１２、ｙ１２、ｚ１２、・・・、ｘＮ１、ｙＮ１、ｚＮ１、ｘＮ２、ｙＮ２、ｚＮ２）は、エッジ線間の３次元拘束条件を考慮して復元された形状となる。しかも、本実施形態によれば、立体的に配置された複数のエッジ線間に、任意の３次元拘束条件を適用することができる。そのため、従来手法に比べて、より高精度な形状を得ることができる。
【００９１】
＜第２実施形態＞
次に、本発明の第２実施形態に係るロボット装置１Ａについて、図１及び図２を援用すると共に、図１０から図１２を参照しながら説明する。第２実施形態においては、カメラ制御部５によるワークの３次元座標の計測方法が第１実施形態と相違する。そのため、第２実施形態においては、第１実施形態と相違する点、即ち、カメラ制御部による３次元位置の計測方法を中心に説明し、第１実施形態と同様の構成については同じ符号を付してその説明は省略する。
【００９２】
まず、ロボット装置１Ａの概略構成について、図１及び図２を援用すると共に、図１０を参照しながら説明する。図１０は、第２実施形態に係るカメラ制御部５Ａの構成を示すブロック図である。
【００９３】
図１及び図２に示すように、第２実施形態に係るロボット装置１Ａは、ロボット２と、架台３と、ロボット２を制御する制御装置４Ａと、を備えており、制御装置４Ａは、演算装置４０Ａと、記憶装置４１と、を備えている。演算装置４０Ａは、ＣＰＵ４０ａＡと、画像処理装置４０ｂと、音声処理装置４０ｃと、を備えており、ＣＰＵ４０ａＡは、カメラ制御部５Ａと、ロボット制御部６と、を備えている。
【００９４】
図１０に示すように、カメラ制御部５Ａは、第１画像エッジ線抽出部５１と、第２画像エッジ線抽出部５２と、対応点演算部５３と、誤差演算部５７と、評価関数設定部５５と、３次元座標計測部５６と、を備えている。誤差演算部５７は、仮の３次元座標を設定し、仮の３次元座標に対する第１エッジ線の端点の視線誤差を演算する。また、誤差演算部５７は、仮の３次元座標に対する第２エッジ線の端点の視平面誤差を演算する。
【００９５】
次に、上述のように構成されたロボット装置１Ａのハンド２１がワークを把持する際の、カメラ制御部５Ａによるワークの３次元位置の計測方法について、図１１に示すフローチャートに沿って、図１２を参照しながら説明する。図１１は、第２実施形態に係る３次元座標の計測を示すフローチャートである。図１２は、第２実施形態に係る視線誤差と視平面誤差とを説明するための模式図である。
【００９６】
基本的な流れとしては、まず、基準カメラ２２と参照カメラ２３とで、複数の異なる視点から撮影した基準画像２２ｂと、参照画像２３ｂと、に対してエッジ線の抽出を行う（ステップＳ３０１）。次に、対応づけの追加を繰り返すか否かを判定する（ステップＳ３０２）。そして、基準画像２２ｂの複数のエッジ線の中から第１エッジ線を選択して、参照画像２３ｂから対応する第２エッジ線を選択する（対応付けを行う）（Ｓ３０３〜Ｓ３０４）。そして、基準画像２２ｂの第１エッジ線に関して視線誤差項を計算する（Ｓ３０５）。同様に、参照画像２３ｂに関して視平面誤差項を計算する（Ｓ３０６）。さらに、対応付けを行った２本のエッジ線間に幾何的な３次元拘束条件を設定し、３次元拘束条件項を計算する（Ｓ３０７〜Ｓ３１１）。最後に、視線誤差項、視平面誤差項及び３次元拘束条件項を含んだ評価関数の値が最小となるように最適化問題を解き、３次元座標を求める（Ｓ３１２）。
【００９７】
第２実施形態においては、第１エッジ線を選択してから対応付けした第２エッジ線を選択するまでの処理ステップＳ３０１〜Ｓ３０４は、第１実施形態と同様である。また、３次元拘束条件を追加する処理ステップＳ３０７〜Ｓ３１１も、第１実施形態と同様である。そのため、これらの詳細な説明は省略する。以下においては、第１実施形態と異なる、第１エッジ線の端点の視線誤差項、第２エッジ線の視平面誤差項の追加処理（ステップＳ３０５〜Ｓ３０６）と３次元座標計算（Ｓ３１２）について説明する。
【００９８】
まず、ステップＳ３０３において選択された第１エッジ線の端点に対し、視線誤差項を求めて評価関数に追加する。視線誤差項に関する考え方は、第１実施形態と同様であるが、基準画像視線２４に対する視線誤差項のみを追加する。基準画像視線２４に対する視線誤差項ｄＬｉｊ２は、下記のようになる。
【００９９】
【数２０】
2014013147.tif 000022
【０１００】
次に、ステップＳ３０４において選択された第２エッジ線の両端点から、視平面誤差項を求めて評価関数に追加する。視平面誤差項は、未知の３次元座標Ｘと、選択した第２エッジ線上の点群が作る視平面との距離である。視平面を定義するための法線ベクトルｎＲｉは下記のように求められる。
【０１０１】
【数２１】
2014013147.tif 000023
【０１０２】
参照画像視平面と未知の点Ｘとの距離ｄは、原点０からＸに向かうベクトルの、法線方向成分の大きさである。よって、視平面誤差項ｄ´Ｒｉｊは、下記のように表すことができる。
【０１０３】
【数２２】
2014013147.tif 000024
【０１０４】
自乗和で評価することとして、以下の項を評価関数に加える。
【０１０５】
【数２３】
2014013147.tif 000025
【０１０６】
誤差項を加えたあとは、ステップＳ３０２に戻り、追加の対応付けを終了するか否かの判断を行い、終了と判定されるまでステップＳ３０２〜Ｓ３０６の処理を繰り返す。処理を繰り返すたびに視線誤差項ｄＬｉｊ２と視平面誤差項ｄ´Ｌｉｊ２が追加される。その結果、Ｎ回の繰り返しを行った場合の評価関数Ｆの誤差項部分Ｆｓｉｇｈｔ（Ｎ）は、下記のようになる。
【０１０７】
【数２４】
2014013147.tif 000026
【０１０８】
ステップＳ３０２において終了と判定した後は、ステップＳ３０７〜Ｓ３１１において拘束条件の追加処理を行う。なお、前述の通り、３次元拘束条件の追加処理は、第１実施形態におけるステップＳ１０７〜Ｓ１１１と同様の処理であるため説明は省略する。本実施形態のように、視線誤差項の代わりに視平面誤差項を用いた場合でも、３次元拘束条件項として同じ計算式が適用できる。
【０１０９】
次に、ステップＳ３０２〜Ｓ３０６において追加された視線誤差項及び視平面誤差項と、ステップＳ３０７〜Ｓ３１１の処理において追加された３次元拘束条件項と、を統合した評価関数Ｆを設定する（評価関数設定工程）。上述したように、評価関数Ｆは、Ｆｓｉｇｈｔ（Ｎ）と、拘束条件項部分Ｆｃｏｎｓｔ（Ｍ）と、の和の形で表される。即ち、
【０１１０】
【数２５】
2014013147.tif 000027
で表される。
【０１１１】
次に、評価関数Ｆを最少とすることで３次元座標Ｘｉｊ（ｉ＝１、２、・・・Ｎ、ｊ＝１、２）を求める（３次元座標計測工程）。つまり、評価関数Ｆに対して、
【０１１２】
【数２６】
2014013147.tif 000028
となるような変数の組（ｘ１１、ｙ１１、ｚ１１、ｘ１２、ｙ１２、ｚ１２、・・・、ｘＮ１、ｙＮ１、ｚＮ１、ｘＮ２、ｙＮ２、ｚＮ２）を求める。この計算は、第１実施形態と同様に、公知の多変数最適化手法を用いて、変数の組を求めることができる。
【０１１３】
以上説明したように、本実施形態に係る３次元位置の計測方法によれば、ワークのエッジ線を近似した線同士の位置関係について、既知の立体的な拘束条件を複数同時に反映した３次元形状計測を行うことができる。そのため、従来に比して高精度な３次元形状データを得ることができる。
【０１１４】
また、本実施形態に係る３次元位置の計測方法によれば、３次元座標を計算する際に「エピポーラ拘束を用いて対応点を一意に求める」という処理を行う必要がなくなる。そのため、基準画像２２ｂから抽出した第１エッジ線に対して先に参照画像２３ｂ上の対応点を求める必要がなくなる。これにより、カメラパラメータの誤差やエッジ線抽出の誤差等によってエピポーラ拘束自体が満たされていない場合は、対応点を求める方法を用いた場合は対応点が正確でなくなるが、本実施形態の方法を用いることで、この影響を軽減することができる。
【０１１５】
以上、本発明の実施形態について説明したが、本発明は上述した実施形態に限定されるものではない。また、本発明の実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、本発明の実施形態に記載されたものに限定されない。
【０１１６】
例えば、本実施形態においては、説明を簡単化するために基準カメラと参照カメラの２つのカメラを用いて説明したが、本発明においてはこれに限定されない。例えば、カメラが３つ以上の場合においても、参照カメラに対する視平面誤差項を追加することにより、本発明の方法を適用可能である。また、ワークに対する視点が異なれば、同一のカメラを動かしながら複数回撮影した画像を用いることもできる。
【０１１７】
また、例えば、本実施形態においては、評価関数を最少とすることで３次元座標を求めたが、本発明においてはこれに限定されない。例えば、評価関数を最大とすることで３次元座標を求めてもよい。
【符号の説明】
【０１１８】
１、１Ａ    ロボット装置
２    ロボット
５、５Ａ    カメラ制御部（制御部）
１０    ワーク
２２    基準カメラ（第１カメラ）
２２ｂ    基準画像（第１画像）
２３    参照カメラ（第２カメラ）
２３ｂ    参照画像（第２画像）
【図１】
2014013147.tif 000029
【図２】
2014013147.tif 000030
【図３】
2014013147.tif 000031
【図４】
2014013147.tif 000032
【図５】
2014013147.tif 000033
【図６】
2014013147.tif 000034
【図７】
2014013147.tif 000035
【図８】
2014013147.tif 000036
【図９】
2014013147.tif 000037
【図１０】
2014013147.tif 000038
【図１１】
2014013147.tif 000039
【図１２】
2014013147.tif 000040