環境認識システム、環境認識方法およびロボット
【課題】ロボット等の装置の周辺に存在する物体の位置、姿勢および形状等、当該物体を対象とする当該装置の動作のために必要な情報を高精度で認識することができるシステム等を提供する。
【解決手段】本発明の環境認識システムによれば、３次元画像センサ１１および２次元画像センサ１２のそれぞれのカメラパラメータを用いて、被写体の３次元情報および物理情報（色情報など）が対応付けられる。その結果、ロボットＲの周囲にある被写体に関する位置、姿勢および形状、ならびに、被写体の物理情報が取得される。
【解決手段】本発明の環境認識システムによれば、３次元画像センサ１１および２次元画像センサ１２のそれぞれのカメラパラメータを用いて、被写体の３次元情報および物理情報（色情報など）が対応付けられる。その結果、ロボットＲの周囲にある被写体に関する位置、姿勢および形状、ならびに、被写体の物理情報が取得される。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、装置の環境を認識するシステム等に関する。
【背景技術】
【０００２】
ロボットに搭載されているカメラおよびレーザレンジファインダのそれぞれから取得される画像データおよびレンジデータに基づき、当該ロボットの位置を推定する手法が提案されている（特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００７−３２２１３８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかし、ロボットがそのハンドによる任意の物体の把持等、周辺に存在するさまざまな種類の物体を対象として臨機応変に動作するためには、その物体の位置、形状および姿勢等が正確に認識される必要がある。
【０００５】
そこで、本発明は、ロボット等の装置の周辺に存在する物体の位置、姿勢および形状等、当該物体を対象とする当該装置の動作のために必要な情報を高精度で認識することができるシステム等を提供することを解決課題とする。
【課題を解決するための手段】
【０００６】
前記課題を解決するための本発明の環境認識システムは、装置の環境を認識するシステムであって、前記装置の周辺の様子を撮像することにより、被写体の３次元情報を取得する３次元画像センサと、前記３次元画像センサに撮像範囲と重なる範囲を撮像することにより、前記被写体の物理情報を取得する２次元画像センサと、前記３次元画像センサにより取得された前記被写体の３次元情報および前記２次元画像センサにより取得された前記被写体の物理情報を対応付けることにより、前記被写体の位置、姿勢および形状、ならびに、物理情報を取得するように構成されている画像処理要素とを備えていることを特徴とする（第１発明）。
【０００７】
第１発明の環境認識システムによれば、３次元画像センサにより取得された被写体の３次元情報および２次元画像センサにより取得された物理情報が対応付けられる。その結果、装置の周辺に存在する被写体（物体）を対象とする当該装置の動作のために必要な情報、すなわち、被写体の位置、姿勢および形状ならびに物理情報が高精度で認識されうる。また、３次元画像センサおよび２次元画像センサのそれぞれの撮像範囲の重なり部分を大きくするために光軸を近づけることが好ましいことから、両センサを相互に近くに配置してシステムのコンパクト化が図られる。
【０００８】
第１発明の環境認識システムにおいて、前記画像処理要素が、前記３次元画像センサの撮像面により定義される第１画像座標系における複数の第１ピクセルのそれぞれの位置が、前記２次元画像センサの撮像面により定義される第２画像座標系に座標変換された結果としての複数の変換位置のそれぞれを算定し、前記第２画像座標系における少なくとも３つの変換位置のそれぞれに対応する、前記第１ピクセルが有する前記被写体の３次元情報に基づき、前記第２画像座標系において前記少なくとも３つの変換位置の近傍にある第２ピクセルが有する前記被写体の物理情報に対応付けられる前記被写体の３次元情報を算定するように構成されていてもよい（第２発明）。
【０００９】
第２発明の環境認識システムにおいて、前記画像処理要素が、前記第２画像座標系における３つの変換位置のそれぞれに対応する、前記第１ピクセルが有する前記被写体の３次元情報としての世界座標系における３つの位置を通る平面を算定し、前記２次元画像センサの主点と、前記第２画像座標系において前記３つの変換位置の近傍にある前記第２ピクセルとを通る直線を算定し、前記世界座標系における前記平面と前記直線との交点位置を、前記第２ピクセルが有する前記被写体の物理情報に対応付けられる前記被写体の３次元情報として算定するように構成されていてもよい（第３発明）。
【００１０】
第２または第３発明の環境認識システムにおいて、前記画像処理要素が、前記第２画像座標系における変換位置の周辺に存在する一または複数の前記第２ピクセルが有する前記被写体の物理情報に基づき、前記変換位置に対応する、前記第１ピクセルが有する前記被写体の３次元情報に対応付けられる前記被写体の物理情報を算定するように構成されていてもよい（第４発明）。
【００１１】
第１発明の環境認識システムにおいて、前記画像処理要素が、前記３次元画像センサの撮像面により定義される第１画像座標系における複数の第１ピクセルのそれぞれの位置が、前記２次元画像センサの撮像面により定義される第２画像座標系に座標変換された結果としての複数の変換位置のそれぞれを算定し、かつ、前記第２画像座標系における変換位置の周辺に存在する一または複数の前記第２ピクセルが有する前記被写体の物理情報に基づき、前記変換位置に対応する、前記第１ピクセルが有する前記被写体の３次元情報に対応付けられる前記被写体の物理情報を算定するように構成されていてもよい（第５発明）。
【００１２】
第２〜第５発明のそれぞれの環境認識システムによれば、第１画像座標系における第１ピクセルが第２画像座標系に座標変換された結果としての変換位置が、通常、第２画像座標系における第２ピクセルの位置に合致しないことが考慮された形で、３次元情報および物理情報が対応付けられる。これにより、被写体を対象とする装置の動作のために必要な情報が高精度で認識されうる。
【００１３】
第１〜第５発明のうちいずれか１つの環境認識システムにおいて、前記２次元画像センサが、前記被写体の物理情報として色情報または温度情報を取得するように構成され、前記画像処理要素が、前記３次元画像センサにより取得された前記被写体の３次元情報および前記２次元画像センサにより取得された前記被写体の色情報および温度情報のうち一方または両方を対応付けることにより、前記装被写体の位置、姿勢および形状、ならびに、色情報および温度情報のうち一方または両方を取得するように構成されていてもよい（第６発明）。
【００１４】
第６発明の環境認識システムによれば、被写体を対象とする装置の動作のために必要な情報、すなわち、当該被写体の位置、姿勢および形状に加えて、被写体の色情報および温度情報のうち一方または両方が高精度で認識されうる。
【００１５】
第１〜第６発明のうちいずれか１つの環境認識システムにおいて、前記３次元画像センサが前記被写体の３次元情報に加えて前記被写体の輝度情報を取得するように構成され、前記画像処理要素が、前記３次元画像センサにより取得されたキャリブレーション用物体の輝度情報および距離情報、ならびに、前記２次元画像センサにより取得された前記キャリブレーション用物体の物理情報を用いて、前記３次元画像の座標系、前記２次元画像の座標系および世界座標系の関係を表わすパラメータを求めるキャリブレーションを実行した上で、前記パラメータを用いて前記３次元画像センサにより取得された前記被写体の３次元情報および前記２次元画像センサにより取得された前記被写体の物理情報を対応付けるように構成されていてもよい（第７発明）。
【００１６】
第７発明の環境認識システムによれば、３次元画像センサにより取得されたキャリブレーション用物体の輝度情報および３次元情報、ならびに、２次元画像センサにより取得された当該物体の物理情報を用いて、第１画像座標系および第２画像座標系の関係を表わすパラメータを求めるキャリブレーションが実行される。そして、当該パラメータを用いて３次元画像センサにより取得された被写体の３次元情報および２次元画像センサにより取得された被写体の物理情報が対応付けられる。
【００１７】
前記課題を解決するための本発明の環境認識方法は、装置の環境を認識する方法であって、３次元画像センサにより前記装置の周辺の様子を撮像することにより、被写体の３次元情報を取得し、前記３次元画像センサに撮像範囲と重なる範囲を２次元画像センサにより撮像することにより、前記被写体の物理情報を取得し、前記被写体の３次元情報および前記被写体の物理情報を対応付けることにより、前記被写体の位置、姿勢および形状、ならびに、物理情報を取得することを特徴とする（第８発明）。
【００１８】
第８発明の環境認識方法によれば、装置の周辺に存在する被写体を対象とする当該装置の動作のために必要な情報、すなわち、当該被写体の位置、姿勢および形状ならびに物理情報が高精度で認識されうる。
【００１９】
前記課題を解決するための前記装置としての本発明のロボットは、第１〜第７発明のうちいずれか１つにの環境認識システムを備え、前記環境システムにより認識された被写体の位置、姿勢および形状ならびに物理情報を用いて、前記被写体を対象として動作することを特徴とする（第９発明）。
【００２０】
第９発明のロボットによれば、その周辺に存在する被写体の位置、姿勢および形状ならびに物理情報が高精度で認識されるので、当該被写体を対象とする所望の動作を正確に実行することができる。また、システムのコンパクト化が図られることから、ロボット全体としてのコンパクト化またはスペースの有効利用が可能となる。
【図面の簡単な説明】
【００２１】
【図１】本発明の一実施形態としての環境認識システムが搭載されているロボットの構成説明図。
【図２】環境認識システムの構成説明図。
【図３】本発明の第１実施形態としての環境認識方法の手順説明図。
【図４】３次元座標系および２次元座標系の対応付け方法に関する説明図。
【図５】３次元情報および物理情報の対応付け方法に関する説明図。
【図６】本発明の第２実施形態としての環境認識方法の手順説明図（その１）。
【図７】本発明の第２実施形態としての環境認識方法の手順説明図（その２）。
【図８】キャリブレーションボードの例示図。
【発明を実施するための形態】
【００２２】
本発明の一実施形態としての環境認識システムについて図面を用いて説明する。
【００２３】
まず、環境認識システムおよびこれが搭載されているロボット（装置）の構成について説明する。なお、環境認識システムは物体の加工作業または運搬作業などを実行するロボット、物体（前方車両など）との接触を回避するための動作制御を実行する車両等のさまざまな装置に搭載されてもよい。
【００２４】
図１に示されているロボットＲは脚式移動ロボットであり、人間と同様に、基体Ｂ０と、基体Ｂ０の上方に配置された頭部Ｂ１と、基体Ｂ０の上部に上部両側から延設された左右の腕体Ｂ２と、左右の腕体Ｂ２のそれぞれの先端に設けられているハンドＨと、基体Ｂ０の下部から下方に延設された左右の脚体Ｂ４とを備えている。なお、ロボットＲは脚式移動ロボットのみならず、ハンドＨの位置および姿勢を変化させるための腕体Ｂ２に相当する機構を備えているあらゆる種類のロボットであってもよい。
【００２５】
ロボットＲはその動作を制御する制御装置２（演算処理要素）を備えている。制御装置２はロボットＲの内部ネットワークを通じて接続された主制御ユニットおよび一または複数の副制御ユニットより構成される分散制御装置であってもよい。
【００２６】
制御装置２はコンピュータ（ＣＰＵ、ＲＯＭおよびＲＡＭ等のメモリ、ならびに、Ａ／Ｄ回路およびＩ／Ｏ回路等の回路により構成されている。）により構成されている。制御装置２によれば、ＣＰＵによりメモリから制御プログラムが適宜読み出され、読み出されたプログラムにしたがってハンドＨの動作が制御される。
【００２７】
制御装置２はロボットＲに搭載されている複数のアクチュエータ４のそれぞれの動作を制御することにより、ハンドＨの指機構Ｆ１〜Ｆ５の動き、腕体Ｂ２の各関節機構における動きおよび脚体Ｂ４の各関節における動き等を制御する。
【００２８】
基体Ｂ０はヨー軸回りに相対的に回動しうるように上下に連結された上部および下部により構成されている。頭部Ｂ１は基体Ｂ０に対してヨー軸回りに回動する等、動くことができる。
【００２９】
腕体Ｂ２は第１腕体リンクＢ２２と、第２腕体リンクＢ２４とを備えている。基体Ｂ０と第１腕体リンクＢ２１とは肩関節機構（第１腕関節機構）Ｂ２１を介して連結され、第１腕体リンクＢ２２と第２腕体リンクＢ２４とは肘関節機構（第２腕関節機構）Ｂ２３を介して連結され、第２腕体リンクＢ２４とハンドＨとは手根関節機構（第３腕関節機構）Ｂ２５を介して連結されている。肩関節機構Ｂ２１はロール、ピッチおよびヨー軸回りの回動自由度を有し、肘関節機構Ｂ２３はピッチ軸回りの回動自由度を有し、手根関節機構Ｂ２５はロール、ピッチ、ヨー軸回りの回動自由度を有している。
【００３０】
脚体Ｂ４は第１脚体リンクＢ４２と、第２脚体リンクＢ４４と、足部Ｂ５とを備えている。基体Ｂ０と第１脚体リンクＢ４２とは股関節機構（第１脚関節機構）Ｂ４１を介して連結され、第１脚体リンクＢ４２と第２脚体リンクＢ４４とは膝関節機構（第２脚関節機構）Ｂ４３を介して連結され、第２脚体リンクＢ４４と足部Ｂ５とは足関節機構（第３脚関節機構）Ｂ４５を介して連結されている。
【００３１】
股関節機構Ｂ４１はロール、ピッチおよびロール軸回りの回動自由度を有し、膝関節機構Ｂ４３はピッチ軸回りの回動自由度を有し、足関節機構Ｂ４５はロールおよびピッチ軸回りの回動自由度を有している。股関節機構Ｂ４１、膝関節機構Ｂ４３および足関節機構Ｂ４５は「脚関節機構群」を構成する。なお、脚関節機構群に含まれる各関節機構の並進および回転自由度は適宜変更されてもよい。また、股関節機構Ｂ４１、膝関節機構Ｂ４３および足関節機構Ｂ４５のうち任意の１つの関節機構が省略された上で、残りの２つの関節機構の組み合わせにより脚関節機構群が構成されていてもよい。さらに、脚体Ｂ４が膝関節とは別の第２脚関節機構を有する場合、当該第２脚関節機構が含まれるように脚関節機構群が構成されてもよい。足部Ｂ５の底には着床時の衝撃緩和のため、特開２００１−１２９７７４号公報に開示されているような弾性素材Ｂ５２が設けられている。
【００３２】
頭部Ｂ１には３次元画像センサ１１および２次元画像センサ１２が搭載されている。
【００３３】
３次元画像センサ１１は、本実施形態ではＴＯＦ（Time Of Flight）３次元センサである。３次元画像センサ１１は、ロボットＲの前方（頭部Ｂ０の正面が向けられている範囲）の様子を撮像することにより、被写体の３次元情報を取得する。
【００３４】
２次元画像センサ１２は、本実施形態ではカラーセンサである。２次元画像センサ１２は、３次元画像センサ１１による撮像範囲と重なる範囲を撮像することにより、被写体の色情報（物理情報）を取得する。
【００３５】
３次元画像センサ１１の解像度（たとえば１７６×２４４）と、２次元画像センサ１２の解像度（たとえば１０２４×７６８）とは異なっている。
【００３６】
３次元画像センサ１１の撮像面により定義される第１画像座標系および２次元画像センサ１２の撮像面により定義される第２画像座標系のロボット座標系に対する位置および姿勢はメモリに保存されており、後述するように世界座標系において認識された物体の位置および姿勢が、ロボット座標系に座標変換される際に用いられる。ロボット座標系は、たとえばロボットＲの質量中心（たとえば基体Ｂ０に含まれている。）が原点として定義され、ロボットＲの上方が＋ｘ方向として定義され、右方向が＋ｙ方向として定義され、前方が＋ｚ方向として定義されている。）
制御装置２は後述する画像処理を実行する画像処理要素２０を備えている（図２参照）。画像処理要素２０は、制御装置２とは別個の一または複数のモジュールにより構成されうる。
【００３７】
前記構成の環境認識システムの機能について説明する。なお、演算処理結果は適宜メモリに保存され、かつ、適宜メモリから読み出される。
【００３８】
３次元画像センサ１１のカメラパラメータおよびひずみパラメータがメモリから読み取られる（図３／ＳＴＥＰ１０２）。２次元画像センサ１２のカメラパラメータおよびひずみパラメータがメモリから読み取られる（図３／ＳＴＥＰ１０４）。
【００３９】
３次元画像センサ１１によりロボットＲの周辺の様子が撮像されることにより、被写体の３次元情報が取得される（図３／ＳＴＥＰ１０６）。「３次元情報」は、第１画像座標系における複数の第１ピクセルのそれぞれの位置における、センサ１１の主点から被写体までの距離Dを意味する。
【００４０】
２次元画像センサ１２によりロボットＲの周辺が撮像されることにより、被写体の色情報が取得される（図３／ＳＴＥＰ１０８）。「色情報」は、第２画像座標系における複数の第２ピクセルのそれぞれの位置における、被写体の色彩（たとえばRGB値により特定される。）を意味する。
【００４１】
続いて、３次元画像センサ１１のカメラパラメータおよびひずみパラメータを用いて、第１画像座標系における第１ピクセル位置が３次元の世界座標系の点に座標変換される（図３／ＳＴＥＰ１１０）。
【００４２】
具体的には、３次元画像センサ１１のカメラパラメータとしての回転行列Rtおよび並進ベクトルTtを用い、関係式（１）にしたがって第１画像座標系の第１ピクセル位置P1が世界座標系の点Pwに変換される。これにより、図４に示されているように３次元画像座標系において量子化された点（第１ピクセル位置）P1=(X1, Y1)が世界座標系の点Pw=(xw, yw, zw)に座標変換される。
【００４３】
Pw=Rt-1(P1-Tt) ..(1)
【００４４】
第１ピクセル位置P1=(X1, Y1)は、３次元画像センサ１１の主点から当該点P1までの距離D、焦点距離f、レンズひずみパラメータκ1およびκ2ならびにスケール係数sx（通常は「１」に設定される。）等に基づいて次の関係式（２ａ）〜（２ｃ）にしたがって算定されうる。
【００４５】
(d’xx/sx)(1+κ1r2+κ2r4)=f(X1/Z1) ..(2a)
dyy(1+κ1r2+κ2r4)=f(Y1/Z1) ..(2b)
X12+Y12+Z12=D2 ..(2c)
【００４６】
ここで「r」は画像原点（Cx, Cy）等を用いて関係式（３）により表現される。
【００４７】
r=((d’xx/sx)2+(d’yy)2)1/2, x≡X1-Cx, y≡Y1-Cy ..(3)
【００４８】
「d’x」はｘ方向の素子数Ncxおよび走査線上のサンプル数Nfxを用い、関係式（４）にしたがって補正されたｘ方向の素子間隔dxを表している。
【００４９】
d’x=(Ncx/Nfx) dx ..(4)
【００５０】
続いて、第２画像座標系において、世界座標系の点Pwに対応する変換位置が算定される（図３／ＳＴＥＰ１１２）。
【００５１】
具体的には、まず、２次元画像センサ１２のカメラパラメータである回転行列R＝(Rij)および並進ベクトルT=(tx, ty, tz)ならびに関係式（５）および（６）が用いられることにより非線型方程式（７）が得られる。
【００５２】
x (1+κ1r2+κ2r4)=(sx/d’x)f(R11xw+R12yw+R13zw+tx)/(R31xw+R32yw+R33zw+tz) ..(5)
y (1+κ1r2+κ2r4)= (1/dy)fRr21xw+R22yw+R23zw+ty)/(R31xw+R32yw+R33zw+tz) ..(6)
r+κ1r3+κ2r5=c=(c12+c22)1/2,
c1≡f(R11xw+R12yw+R13zw+tx)/(R31xw+R32yw+R33zw+tz),
c2≡f(R21xw+R22yw+R23zw+ty)/(R31xw+R32yw+R33zw+tz) ..(7)
【００５３】
線型方程式（７）がNewton-Rraphson法にしたがって近似されることにより関係式（８）が得られる。
【００５４】
f(rn)=rn+κ1rn3+κ2rn5-c, f’(rn)=1+3κ1rn2+5κ2rn4, rn+1=rnf(rn)/f’(rn) ..(8)
【００５５】
rnが関係式（８）にしたがって反復計算され、その収束率|1-(rn+1/rn)|が閾値ε（たとえばexp(-10)）以下になった際の収束結果がrの近似解として算定される。
【００５６】
当該近似解rを用いて、関係式（３）（５）および（６）にしたがって第２座標系における変換位置Pc=(xc, yc)が算定される。これにより、図４に示されているように、世界座標系の点Pw=(xw, yw, zw)に対応する第２画像座標系における変換位置Pcが求められる。
【００５７】
前記のように３次元画像センサ１１の解像度と、２次元画像センサ１２の解像度とは異なっているが、解像度の相違の有無にかかわらず補間が必要である。これは、図４に示されているように、第１画像座標系における第１ピクセル位置（X1, Y1）に対応する第２画像座標系における変換位置Pcは、通常、第２画像座標系における量子化された点（第２ピクセル位置）には一致しないからである。図４には、第２画像座標系における変換位置Pcが、第２ピクセル位置Ａ〜Ｄを頂点とする矩形領域に含まれている様子が示されている。
【００５８】
そこで、たとえば、最近傍点補間法、双線形補間法またはバイキュービックスプライン法にしたがって、３次元情報に色情報（物理情報）が対応付けられる。
【００５９】
「最近傍点補間法」によれば、第２画像座標系において変換位置Pcに最も近い第２ピクセル位置Ａの色情報が、当該変換位置Pcにおける色情報として適用される。「双線形補間法」によれば、第２画像座標系において変換位置Pcを囲う４つの第２ピクセル位置Ａ〜Ｄのそれぞれまでの距離の比と、第２ピクセル位置Ａ〜Ｄの色情報とに基づいて当該変換位置Pcの色情報が決定される。「バイキュービックスプライン補間法」によれば、第２画像座標系において変換位置Pcを囲う４つの第２ピクセル位置Ａ〜Ｄに加えて、さらに外側の１２の第２ピクセル位置の合計１６の第２ピクセル位置と、当該第２幾セル位置のそれぞれの色情報とに基づき、当該変換位置Pcの色情報が決定される。
【００６０】
第１画像座標系における対象となる第１ピクセル位置（すべての第１ピクセル位置でなくてもよい。）について、前記のような世界座標系への座標変換、第２画像座標系において対応する変換位置の決定、最近傍点補間法等による色情報の算定、ならびに、３次元情報に対する物理情報（色情報）の対応付けが完了したか否かが判定される（図３／ＳＴＥＰ１１６）。
【００６１】
当該判定結果が否定的である場合（図３／ＳＴＥＰ１１６‥ＮＯ）、対象となる残りの第１ピクセル位置について世界座標系への座標変換、第２画像座標系において対応する変換位置の決定、最近傍点補間法等による物理情報（色情報）の算定、ならびに、３次元情報に対する物理情報の対応付けが繰り返される（図３／ＳＴＥＰ１１０〜ＳＴＥＰ１１４）。
【００６２】
一方、対象となるすべての第１ピクセル位置に対して前記処理が実行されたと判定された場合（図３／ＳＴＥＰ１１６‥ＹＥＳ）、２次元画像に３次元情報を対応付けることの要否が判定される（図３／ＳＴＥＰ１１８）。たとえば、３次元画像センサ１１の解像度が、２次元画像センサ１２の解像度よりも高いか否かに応じて、当該要否が判定されうる。なお、当該判定処理は省略されてもよい。
【００６３】
当該判定結果が肯定的である場合（図３／ＳＴＥＰ１１８‥ＹＥＳ）、図５に示されているように第２画像座標系において対象となる第２ピクセル位置P2=(X2, Y2)を囲む最も近い３つの変換位置Pc1、Pc2およびPc3のそれぞれに対応する世界座標系の点の座標値Pw1=(xw1, yw1, zw1)、Pw2=(xw2, yw2, zw2)およびPw3=(xw3, yw3, zw3)がメモリから読み出される（図３／ＳＴＥＰ１２０）。世界座標系における当該３つの点Pw1、Pw2およびPw3が同一直線上には無いことが確認される。
【００６４】
そして、図５に示されているように世界座標系において３つの点Pw1、Pw2およびPw3を通る平面が算定されれる（図３／ＳＴＥＰ１２２）。
【００６５】
世界座標系において３つの点Pw1、Pw2およびPw3を通る平面は関係式（９）によって表される。
【００６６】
u1xw+u2yw+u3zw=1,
t(u1,u2,u3)=Q -1・t(1,1,1),
Q≡t(tPw1, tPw2, tPw3) ..(9)
【００６７】
さらに、２次元画像センサ１２の主点と、第２画像座標系における変換位置Pcとを通る直線が算定された上で、当該平面と当該直線との交点が、当該変換位置Pcに対応する世界座標系における点位置Pwとして算定される（図３／ＳＴＥＰ１２４）。
【００６８】
第２画像座標系における変換位置Pc=(Xc, Yc)は関係式（１０）（１１）により表現される。
【００６９】
Xc (1+κ1r2+κ2r4)=(sx/d’x)f(r11Xw+r12Xw+r13Xw+tx)/(r31Xw+r32Yw+r33Zw+tz)
(r=((Xc)2+(Yc)2)1/2) ..(10)
Yc(1+κ1r2+κ2r4)= (1/dy)f(r21Xw+r22Yw+r23Zw+ty)/(r31Xw+r32Yw+r33Zw+tz) ..(11)
【００７０】
関係式（９）〜（１１）から得られる関係式（１２）にしたがって変換位置Pcに対応する世界座標系の座標値Pw=(Xw,Yw,Zw)が算定される。
【００７１】
t(Xw,Yw,Zw)=B -1・t(1,1,1), B≡(Bij),
B1j= {Xc (1+κ1r2+κ2r4)r3j-(sx/d’x)f×r1j}/{(sx/d’x)f×tx- Xc (1+κ1r2+κ2r4) ×tz },
B2j= {Yc (1+κ1r2+κ2r4)r3j-(1/dy)f×r2j}/{(1/dy)f×ty- Yc (1+κ1r2+κ2r4) ×tz },
B3j=uj ..(12)
【００７２】
第２画像座標系における対象となる第２ピクセル位置P2（すべての第２ピクセル位置でなくてもよい。）について、前記のような２次元座標系における３つの変換位置の選択、世界座標系における平面の算定、２次元画像センサ１２の主点と当該第２ピクセル位置とを通る直線の算定、および、当該平面と当該直線との交点の算定が完了したか否かが判定される（図３／ＳＴＥＰ１２６）。
【００７３】
当該判定結果が否定的である場合（図３／ＳＴＥＰ１２６‥ＮＯ）、対象となる残りの第２ピクセル位置について２次元座標系における３つの変換位置の選択、世界座標系における平面の算定、２次元画像センサ１２の主点と当該第２ピクセル位置とを通る直線の算定、および、当該平面と当該直線との交点の算定が繰り返される（図３／ＳＴＥＰ１２０〜ＳＴＥＰ１２４）。
【００７４】
一方、対象となるすべての第２ピクセル位置に対して前記処理が実行されたと判定された場合（図３／ＳＴＥＰ１２６‥ＹＥＳ）、３次元画像センサ１１による３次元情報の取得（図３／ＳＴＥＰ１０６参照）および２次元画像センサ１２による物理情報の取得（図３／ＳＴＥＰ１０８参照）ならびにこれらに続く処理の繰り返し要否が判定される（図３／ＳＴＥＰ１２８）。なお、２次元画像に３次元情報を対応付ける必要がないと判定された場合（図３／ＳＴＥＰ１１８‥ＮＯ）、当該繰り返し要否が判定される（図３／ＳＴＥＰ１２８）。
【００７５】
繰り返し処理が必要であると判定された場合（図３／ＳＴＥＰ１２８‥ＮＯ）、３次元画像センサ１１による３次元情報の取得および２次元画像センサ１２による物理情報の取得ならびにこれらに続く処理が繰り返し実行される（図３／ＳＴＥＰ１０６〜ＳＴＥＰ１２６参照）。
【００７６】
繰り返し処理が不要であると判定された場合（図３／ＳＴＥＰ１２８‥ＹＥＳ）、前記一連の画像処理が終了する。
【００７７】
これにより、世界座標系における物体の位置、姿勢および形状ならびに世界座標系における各点における色、すなわち、ロボットＲによる物体の把持動作に必要な当該物体に関する情報が取得される。必要に応じてこれらの形状等に基づくデータベース検索またはパターンマッチング等により得られる物体の種類、質量中心など物体の付加的情報が取得されてもよい。
【００７８】
前記機能を発揮する本発明の環境認識システムによれば、ロボットＲの周辺に存在する物体に関する情報、すなわち、世界座標系における当該物体の位置、姿勢および形状ならびに世界座標系における各点の色を高精度で認識することができる。そして、ロボットＲは、腕部Ｂ２、ハンドＨおよび指機構Ｆ１〜Ｆ５のそれぞれを動かして物体を把持する等、被写体となった物体を対象として作業を確実に実行することができる。なお、ロボットＲは、必要に応じて、作業に先立って脚部Ｂ４等を動かして基体Ｂ０の位置（基体Ｂ０の質量中心位置）および姿勢（世界座標系の各軸に対する基体座標系の各軸の角度）を調節してもよい。
【００７９】
また、３次元画像センサ１１および２次元画像センサ１２のそれぞれの撮像範囲の重なり部分を大きくするために光軸を近づけることが好ましいことから、両センサ１１および１２を相互に近くに配置してシステムのコンパクト化が図られる。システムのコンパクト化が図られることから、ロボットＲ全体としてのコンパクト化またはスペースの有効利用が可能となる。
【００８０】
なお、３次元情報を取得するための３次元画像センサ１１としては、ＴＯＦ３次元画像センサのほか、スキャン型ＴＯＦ３次元画像センサおよびステレオカメラのうち一部または全部が採用されてもよい。物理情報を取得するための２次元画像センサ１２としては、被写体の色情報を取得するための可視光カラーカメラのほか、被写体の濃淡情報を取得するための可視光モノクロカメラ、被写体の暗視情報を取得するための近赤外カメラ、被写体の温度情報を取得するための遠赤外カメラ、ミリ波源情報を取得するためのミリ波カメラ、および、面の法線方向の情報を取得するための偏光カメラのうち一部または全部が採用されてもよい。
【００８１】
これらの３次元画像センサ１１および２次元画像センサ１２の組み合わせはさまざまに変更されうる。たとえば、環境認識システムの構成要素として、（１）一の３次元画像センサ１１および一の２次元画像センサ１２の組み合わせのほか、（２）一の３次元画像センサ１１および複数の同種または異種の２次元画像センサ１２、（３）複数の同種または異種の３次元画像センサ１１および一の２次元画像センサ１２の組み合わせ、または、（４）複数の同種または異種の３次元画像センサ１１および複数の同種または異種の２次元画像センサ１２の組み合わせが採用されてもよい。
【００８２】
本発明の他の実施形態として、２つの３次元画像センサ１１（ＴＯＦ３次元画像センサ）と、２つの異種の２次元画像センサ１２（可視光カラーカメラおよび遠赤外カメラ）が環境認識システムに採用されている場合の画像処理の手順について説明する。
【００８３】
まず、２つの３次元画像センサ１１のそれぞれのカメラパラメータおよびひずみパラメータがメモリから読み取られる（図６／ＳＴＥＰ２０２）。
【００８４】
２つの２次元画像センサ１２のそれぞれのカメラパラメータおよびひずみパラメータがメモリから読み取られる（図６／ＳＴＥＰ２０４）。
【００８５】
２つの３次元画像センサ１１のそれぞれによりロボットＲの周辺の様子が撮像されることにより、被写体の３次元情報が取得される（図６／ＳＴＥＰ２０６）。
【００８６】
２次元画像センサ１２としてのカラーカメラおよび遠赤外カメラのそれぞれによりロボットＲの周辺が撮像されることにより、被写体の物理情報として色情報および温度情報が取得される（図６／ＳＴＥＰ２０８，ＳＴＥＰ２１０）。
【００８７】
続いて、第１の３次元画像センサ１１のカメラパラメータおよびひずみパラメータを用いて、当該第１の３次元画像センサ１１の第１画像座標系（２次元座標系）における第１ピクセル位置が世界座標系（３次元座標系）における点に座標変換される（図６／ＳＴＥＰ２１２）。
【００８８】
さらに、世界座標系における当該点Pw1=(xw1, yw1, zw1)に対応する、カラーカメラの第２画像座標系における変換位置Pc11が算定される（図６／ＳＴＥＰ２１４）。
【００８９】
また、最近傍点補間法等にしたがって当該変換位置Pc11における色情報（R,G,B）が算定され、第１の３次元画像センサ１１の第１画像座標系において当該変換位置Pc11に対応する第１ピクセル位置P111の３次元情報に、この色情報（R,G,B）が対応付けられる（図６／ＳＴＥＰ２１６（図４参照））。
【００９０】
さらに、世界座標系における当該点Pw1に対応する、遠赤外カメラの第２画像座標系における変換位置Pc12が算定される（図６／ＳＴＥＰ２１８）。
【００９１】
また、最近傍点補間法等にしたがって当該変換位置Pc12における温度情報Temp（または温度に応じた輝度情報Lum）が算定され、第１の３次元画像センサ１１の第１画像座標系において当該変換位置Pc12に対応する第１ピクセル位置P112の３次元情報に、この温度情報Tempが対応付けられる（図６／ＳＴＥＰ２２０（図４参照））。
【００９２】
第１の３次元画像センサの第１画像座標系における対象となる第１ピクセル位置（すべての第１ピクセル位置でなくてもよい。）について、前記のような世界座標系への座標変換、第２画像座標系において対応する変換位置の決定、最近傍点補間法等による物理情報（色情報および温度情報）の算定、ならびに、３次元情報に対する物理情報の対応付けが完了したか否かが判定される（図６／ＳＴＥＰ２２２）。
【００９３】
当該判定結果が否定的である場合（図６／ＳＴＥＰ２２２‥ＮＯ）、対象となる残りの第１ピクセル位置について世界座標系への座標変換、第２画像座標系において対応する変換位置の決定、最近傍点補間法等による物理情報（色情報および温度情報）の算定、ならびに、３次元情報に対する物理情報の対応付けが繰り返される（図６／ＳＴＥＰ２１２〜ＳＴＥＰ２２０）。
【００９４】
一方、第１の３次元画像センサ１１の第１画像座標系における対象となるすべての第１ピクセル位置に対して前記処理が実行されたと判定された場合（図６／ＳＴＥＰ２２２‥ＹＥＳ）、第２の３次元画像センサ１１のカメラパラメータおよびひずみパラメータを用いて、第２の３次元画像センサ１１の第１画像座標系（２次元座標系）の第１ピクセル位置が世界座標系（３次元座標系）の点に座標変換される（図６／ＳＴＥＰ２２４）。
【００９５】
さらに、世界座標系における当該点Pw2=(xw2 ,yw2, zw2)に対応する、カラーカメラの第２画像座標系における変換位置Pc21が算定される（図６／ＳＴＥＰ２２６）。
【００９６】
また、最近傍点補間法等にしたがって当該変換位置Pc21における色情報（R,G,B）が算定され、第２の３次元画像センサ１１の第１画像座標系において当該変換位置Pc21に対応する第１ピクセル位置P121の３次元情報に、この色情報（R,G,B）が対応付けられる（図６／ＳＴＥＰ２２８（図４参照））。
【００９７】
さらに、世界座標系における当該点Pw2に対応する、遠赤外カメラの第２画像座標系における変換位置Pc22が算定される（図６／ＳＴＥＰ２３０）。
【００９８】
また、最近傍点補間法等にしたがって当該変換位置Pc22における温度情報Temp（または温度に応じた輝度情報Lum）が算定され、第２の３次元画像センサ１１の第１画像座標系において当該変換位置Pc22に対応する第１ピクセル位置P122の３次元情報に、この温度情報Tempが対応付けられる（図６／ＳＴＥＰ２３２（図４参照））。
【００９９】
第２の３次元画像センサの第１画像座標系における対象となる第１ピクセル位置（すべての第１ピクセル位置でなくてもよい。）について、前記のような世界座標系への座標変換、第２画像座標系において対応する変換位置の決定、最近傍点補間法等による物理情報（色情報および温度情報）の算定、ならびに、３次元情報に対する物理情報の対応付けが完了したか否かが判定される（図６／ＳＴＥＰ２３４）。
【０１００】
当該判定結果が否定的である場合（図６／ＳＴＥＰ２３４‥ＮＯ）、対象となる残りの第１ピクセル位置について世界座標系への座標変換、第２画像座標系において対応する変換位置の算定、最近傍点補間法等による物理情報（色情報および温度情報）の算定、ならびに、３次元情報に対する物理情報の対応付けが繰り返される（図６／ＳＴＥＰ２２４〜ＳＴＥＰ２３２）。
【０１０１】
一方、第２の３次元画像センサ１１の第１画像座標系における対象となるすべての第１ピクセル位置に対して前記処理が実行されたと判定された場合（図６／ＳＴＥＰ２３４‥ＹＥＳ）、２次元画像に３次元情報を対応付けることの要否が判定される（図７／ＳＴＥＰ２３６）。
【０１０２】
当該判定結果が肯定的である場合（図７／ＳＴＥＰ２３６‥ＹＥＳ）、カラーカメラの第２画像座標系における第２ピクセル位置P21を囲む最も近い３つの変換位置のそれぞれに対応する世界座標系の点位置がメモリから読み出され（図７／ＳＴＥＰ２４０）、世界座標系において当該３つの点を通る平面が算定される（図７／ＳＴＥＰ２４２（図５、関係式（９）参照））。
【０１０３】
さらに、カラーカメラの主点と、カラーカメラの第２画像座標系における第２ピクセル位置P21とを通る直線が算定された上で、当該平面と当該直線との交点が、当該変換位置Pcに対応する世界座標系における点位置Pwとして算定される（図７／ＳＴＥＰ２４４（図５、関係式（１２）参照）。
【０１０４】
カラーカメラの第２画像座標系における対象となる第２ピクセル位置（すべての第２ピクセル位置でなくてもよい。）について、前記のような２次元座標系における３つの変換位置の選択、世界座標系における平面の算定、カラーカメラの主点と当該第２ピクセル位置とを通る直線の算定、および、当該平面と当該直線との交点の算定が完了したか否かが判定される（図７／ＳＴＥＰ２４６）。
【０１０５】
当該判定結果が否定的である場合（図３／ＳＴＥＰ２４６‥ＮＯ）、カラーカメラの第２画像座標系における対象となる残りの第２ピクセル位置について２次元座標系における３つの変換位置の選択、世界座標系における平面の算定、カラーカメラの主点と当該第２ピクセル位置とを通る直線の算定、および、当該平面と当該直線との交点の算定が繰り返される（図７／ＳＴＥＰ２４０〜ＳＴＥＰ２４４）。
【０１０６】
一方、カラーカメラの第２画像座標系における対象となるすべての第２ピクセル位置に対して前記処理が実行されたと判定された場合（図７／ＳＴＥＰ２４６‥ＹＥＳ）、遠赤外カメラの第２画像座標系における対象となる第２ピクセル位置P22を囲む最も近い変換位置のそれぞれに対応する世界座標系の点位置がメモリから読み出され（図７／ＳＴＥＰ２４８）、世界座標系において当該３つの点を通る平面が算定される（図７／ＳＴＥＰ２５０（図５、関係式（９）参照））。
【０１０７】
さらに、遠赤外カメラの主点と、遠赤外カメラの第２画像座標系における第２ピクセル位置P22とを通る直線が算定された上で、当該平面と当該直線との交点が、当該変換位置Pcに対応する世界座標系における点位置Pwとして算定される（図７／ＳＴＥＰ２５２（図５、関係式（１２）参照）。
【０１０８】
遠赤外カメラの第２画像座標系における対象となる第２ピクセル位置（すべての第２ピクセル位置でなくてもよい。）について、前記のような２次元座標系における３つの変換位置の選択、世界座標系における平面の算定、遠赤外カメラの主点と当該第２ピクセル位置とを通る直線の算定、および、当該平面と当該直線との交点の算定が完了したか否かが判定される（図７／ＳＴＥＰ２５４）。
【０１０９】
当該判定結果が否定的である場合（図３／ＳＴＥＰ２５４‥ＮＯ）、２次元座標系における３つの変換位置の選択、世界座標系における平面の算定、遠赤外カメラの主点と当該第２ピクセル位置とを通る直線の算定、および、当該平面と当該直線との交点の算定が繰り返される（図７／ＳＴＥＰ２４８〜ＳＴＥＰ２５２）。
【０１１０】
一方、遠赤外カメラの第２画像座標系における対象となるすべての第２ピクセル位置に対して前記処理が実行されたと判定された場合（図７／ＳＴＥＰ２５４‥ＹＥＳ）、２つの３次元画像センサ１１のそれぞれによる３次元情報の取得（図６／ＳＴＥＰ２０６参照）および２つの２次元画像センサのそれぞれによる物理情報の取得（図６／ＳＴＥＰ２０８，ＳＴＥＰ２１０参照）ならびにこれらに続く処理の繰り返し要否が判定される（図７／ＳＴＥＰ２５６）。なお、２次元画像に３次元情報を対応付ける必要がないと判定された場合（図７／ＳＴＥＰ２３６‥ＮＯ）、当該繰り返し要否が判定される（図７／ＳＴＥＰ２５６）。
【０１１１】
繰り返し処理が必要であると判定された場合（図７／ＳＴＥＰ２５６‥ＮＯ）、２つの３次元画像センサ１１のそれぞれによる３次元情報の取得および２つの２次元画像センサのそれぞれによる物理情報の取得ならびにこれらに続く処理が繰り返し実行される（図６／ＳＴＥＰ２０６〜ＳＴＥＰ２３４、図７／ＳＴＥＰ２３６〜ＺＴＥＰ２５４参照）。
【０１１２】
繰り返し処理が不要であると判定された場合（図７／ＳＴＥＰ２５６‥ＹＥＳ）、前記一連の画像処理が終了する。
【０１１３】
これにより、物体の把持動作に必要な当該物体に関する情報、すなわち、世界座標系における物体の位置、姿勢および形状、ならびに、世界座標系における当該物体の各点の物理情報（色および温度）が取得される。
【０１１４】
なお、カメラパラメータを取得するため、次に説明するような手法でキャリブレーションが実行されてもよい。
【０１１５】
すなわち、まず、３次元画像センサ１１によりロボットＲの周辺にあるキャリブレーションボード（キャリブレーション用物体）を撮像することにより、輝度情報および距離情報を有する画素により構成されている３次元画像を取得する。
【０１１６】
また、２次元画像センサ１２により３次元画像センサ１１に撮像範囲と重なる範囲においてキャリブレーションボードを撮像することにより、色情報を有する画素により構成されている２次元画像を取得する。
【０１１７】
さらに、キャリブレーションが実行されることによりカメラパラメータが決定される。キャリブレーション方法としては公知の手法が採用される（R.Y.Tsai: “An Efficient and accurate calibration technique for 3D machine vision” (CVPR, pp.364-374 (1986))およびR.Y.Tsai: “A versatile camera calibration technique for high-accuracy 3D machine vision metrology using off-the-shelf TV cameras and lenses” (J-RA, vol.3, pp.323-344 (1987))参照）。
【０１１８】
この際、図８に示されているように黒色および白色の正方形が前後左右に交互に配置されているチェックまたは市松模様のパターンを有するキャリブレーションボードが用いられる。ボードの中心近くにある白色の正方形の隅角部分に三角形状のマークが付されている。一方のカメラで撮像されたボードの複数の正方形のそれぞれの頂点が、他方のカメラで撮像されたボードの複数の正方形の頂点のうちいずれに相当するかがこのマークを用いて確認されうる。
【０１１９】
なお、このパターンの形成方法は、キャリブレーションの対象となる画像センサの種類に応じて変更されてもよい。たとえば、２次元画像センサ１２がアクティブ型の近赤外カメラである場合、近赤外を反射する塗料の有無によって前期のようなパターンが形成されてもよい。
【０１２０】
赤外光を反射する塗料またはシールがボードに付されることによりマークが構成されている。これにより、３次元画像センサによるマークの認識精度の向上が図られている。ボードの表面は白色の正方形において乱反射が起こりにくいように仕上げ加工されている一方、黒色の正方形において乱反射が起こりやすいように仕上げ加工されている。これにより、３次元画像センサによる白黒パターンの認識精度の向上が図られている。
【０１２１】
キャリブレーションにより、世界座標値およびこれに対応するカメラの画像座標値に基づき、カメラの外部パラメータおよび内部パラメータが関係式（３）（５）および（６）にしたがって算定される。「外部パラメータ」として回転行列R（未知数は３つの３次正方行列）および並進ベクトルT（未知数３つ）が算定される。「内部パラメータ」として焦点距離f、レンズひすみパラメータκ1およびκ2、スケール係数sx（通常は「１」に設定される。）ならびに画像原点（Cx, Cy）が算定される。キャリブレーションにより得られたカメラパラメータはメモリに保存される。
【符号の説明】
【０１２２】
２‥制御装置、１１‥３次元画像センサ、１２‥２次元画像センサ、２０‥画像処理要素、Ｒ‥ロボット（装置）。
【技術分野】
【０００１】
本発明は、装置の環境を認識するシステム等に関する。
【背景技術】
【０００２】
ロボットに搭載されているカメラおよびレーザレンジファインダのそれぞれから取得される画像データおよびレンジデータに基づき、当該ロボットの位置を推定する手法が提案されている（特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００７−３２２１３８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかし、ロボットがそのハンドによる任意の物体の把持等、周辺に存在するさまざまな種類の物体を対象として臨機応変に動作するためには、その物体の位置、形状および姿勢等が正確に認識される必要がある。
【０００５】
そこで、本発明は、ロボット等の装置の周辺に存在する物体の位置、姿勢および形状等、当該物体を対象とする当該装置の動作のために必要な情報を高精度で認識することができるシステム等を提供することを解決課題とする。
【課題を解決するための手段】
【０００６】
前記課題を解決するための本発明の環境認識システムは、装置の環境を認識するシステムであって、前記装置の周辺の様子を撮像することにより、被写体の３次元情報を取得する３次元画像センサと、前記３次元画像センサに撮像範囲と重なる範囲を撮像することにより、前記被写体の物理情報を取得する２次元画像センサと、前記３次元画像センサにより取得された前記被写体の３次元情報および前記２次元画像センサにより取得された前記被写体の物理情報を対応付けることにより、前記被写体の位置、姿勢および形状、ならびに、物理情報を取得するように構成されている画像処理要素とを備えていることを特徴とする（第１発明）。
【０００７】
第１発明の環境認識システムによれば、３次元画像センサにより取得された被写体の３次元情報および２次元画像センサにより取得された物理情報が対応付けられる。その結果、装置の周辺に存在する被写体（物体）を対象とする当該装置の動作のために必要な情報、すなわち、被写体の位置、姿勢および形状ならびに物理情報が高精度で認識されうる。また、３次元画像センサおよび２次元画像センサのそれぞれの撮像範囲の重なり部分を大きくするために光軸を近づけることが好ましいことから、両センサを相互に近くに配置してシステムのコンパクト化が図られる。
【０００８】
第１発明の環境認識システムにおいて、前記画像処理要素が、前記３次元画像センサの撮像面により定義される第１画像座標系における複数の第１ピクセルのそれぞれの位置が、前記２次元画像センサの撮像面により定義される第２画像座標系に座標変換された結果としての複数の変換位置のそれぞれを算定し、前記第２画像座標系における少なくとも３つの変換位置のそれぞれに対応する、前記第１ピクセルが有する前記被写体の３次元情報に基づき、前記第２画像座標系において前記少なくとも３つの変換位置の近傍にある第２ピクセルが有する前記被写体の物理情報に対応付けられる前記被写体の３次元情報を算定するように構成されていてもよい（第２発明）。
【０００９】
第２発明の環境認識システムにおいて、前記画像処理要素が、前記第２画像座標系における３つの変換位置のそれぞれに対応する、前記第１ピクセルが有する前記被写体の３次元情報としての世界座標系における３つの位置を通る平面を算定し、前記２次元画像センサの主点と、前記第２画像座標系において前記３つの変換位置の近傍にある前記第２ピクセルとを通る直線を算定し、前記世界座標系における前記平面と前記直線との交点位置を、前記第２ピクセルが有する前記被写体の物理情報に対応付けられる前記被写体の３次元情報として算定するように構成されていてもよい（第３発明）。
【００１０】
第２または第３発明の環境認識システムにおいて、前記画像処理要素が、前記第２画像座標系における変換位置の周辺に存在する一または複数の前記第２ピクセルが有する前記被写体の物理情報に基づき、前記変換位置に対応する、前記第１ピクセルが有する前記被写体の３次元情報に対応付けられる前記被写体の物理情報を算定するように構成されていてもよい（第４発明）。
【００１１】
第１発明の環境認識システムにおいて、前記画像処理要素が、前記３次元画像センサの撮像面により定義される第１画像座標系における複数の第１ピクセルのそれぞれの位置が、前記２次元画像センサの撮像面により定義される第２画像座標系に座標変換された結果としての複数の変換位置のそれぞれを算定し、かつ、前記第２画像座標系における変換位置の周辺に存在する一または複数の前記第２ピクセルが有する前記被写体の物理情報に基づき、前記変換位置に対応する、前記第１ピクセルが有する前記被写体の３次元情報に対応付けられる前記被写体の物理情報を算定するように構成されていてもよい（第５発明）。
【００１２】
第２〜第５発明のそれぞれの環境認識システムによれば、第１画像座標系における第１ピクセルが第２画像座標系に座標変換された結果としての変換位置が、通常、第２画像座標系における第２ピクセルの位置に合致しないことが考慮された形で、３次元情報および物理情報が対応付けられる。これにより、被写体を対象とする装置の動作のために必要な情報が高精度で認識されうる。
【００１３】
第１〜第５発明のうちいずれか１つの環境認識システムにおいて、前記２次元画像センサが、前記被写体の物理情報として色情報または温度情報を取得するように構成され、前記画像処理要素が、前記３次元画像センサにより取得された前記被写体の３次元情報および前記２次元画像センサにより取得された前記被写体の色情報および温度情報のうち一方または両方を対応付けることにより、前記装被写体の位置、姿勢および形状、ならびに、色情報および温度情報のうち一方または両方を取得するように構成されていてもよい（第６発明）。
【００１４】
第６発明の環境認識システムによれば、被写体を対象とする装置の動作のために必要な情報、すなわち、当該被写体の位置、姿勢および形状に加えて、被写体の色情報および温度情報のうち一方または両方が高精度で認識されうる。
【００１５】
第１〜第６発明のうちいずれか１つの環境認識システムにおいて、前記３次元画像センサが前記被写体の３次元情報に加えて前記被写体の輝度情報を取得するように構成され、前記画像処理要素が、前記３次元画像センサにより取得されたキャリブレーション用物体の輝度情報および距離情報、ならびに、前記２次元画像センサにより取得された前記キャリブレーション用物体の物理情報を用いて、前記３次元画像の座標系、前記２次元画像の座標系および世界座標系の関係を表わすパラメータを求めるキャリブレーションを実行した上で、前記パラメータを用いて前記３次元画像センサにより取得された前記被写体の３次元情報および前記２次元画像センサにより取得された前記被写体の物理情報を対応付けるように構成されていてもよい（第７発明）。
【００１６】
第７発明の環境認識システムによれば、３次元画像センサにより取得されたキャリブレーション用物体の輝度情報および３次元情報、ならびに、２次元画像センサにより取得された当該物体の物理情報を用いて、第１画像座標系および第２画像座標系の関係を表わすパラメータを求めるキャリブレーションが実行される。そして、当該パラメータを用いて３次元画像センサにより取得された被写体の３次元情報および２次元画像センサにより取得された被写体の物理情報が対応付けられる。
【００１７】
前記課題を解決するための本発明の環境認識方法は、装置の環境を認識する方法であって、３次元画像センサにより前記装置の周辺の様子を撮像することにより、被写体の３次元情報を取得し、前記３次元画像センサに撮像範囲と重なる範囲を２次元画像センサにより撮像することにより、前記被写体の物理情報を取得し、前記被写体の３次元情報および前記被写体の物理情報を対応付けることにより、前記被写体の位置、姿勢および形状、ならびに、物理情報を取得することを特徴とする（第８発明）。
【００１８】
第８発明の環境認識方法によれば、装置の周辺に存在する被写体を対象とする当該装置の動作のために必要な情報、すなわち、当該被写体の位置、姿勢および形状ならびに物理情報が高精度で認識されうる。
【００１９】
前記課題を解決するための前記装置としての本発明のロボットは、第１〜第７発明のうちいずれか１つにの環境認識システムを備え、前記環境システムにより認識された被写体の位置、姿勢および形状ならびに物理情報を用いて、前記被写体を対象として動作することを特徴とする（第９発明）。
【００２０】
第９発明のロボットによれば、その周辺に存在する被写体の位置、姿勢および形状ならびに物理情報が高精度で認識されるので、当該被写体を対象とする所望の動作を正確に実行することができる。また、システムのコンパクト化が図られることから、ロボット全体としてのコンパクト化またはスペースの有効利用が可能となる。
【図面の簡単な説明】
【００２１】
【図１】本発明の一実施形態としての環境認識システムが搭載されているロボットの構成説明図。
【図２】環境認識システムの構成説明図。
【図３】本発明の第１実施形態としての環境認識方法の手順説明図。
【図４】３次元座標系および２次元座標系の対応付け方法に関する説明図。
【図５】３次元情報および物理情報の対応付け方法に関する説明図。
【図６】本発明の第２実施形態としての環境認識方法の手順説明図（その１）。
【図７】本発明の第２実施形態としての環境認識方法の手順説明図（その２）。
【図８】キャリブレーションボードの例示図。
【発明を実施するための形態】
【００２２】
本発明の一実施形態としての環境認識システムについて図面を用いて説明する。
【００２３】
まず、環境認識システムおよびこれが搭載されているロボット（装置）の構成について説明する。なお、環境認識システムは物体の加工作業または運搬作業などを実行するロボット、物体（前方車両など）との接触を回避するための動作制御を実行する車両等のさまざまな装置に搭載されてもよい。
【００２４】
図１に示されているロボットＲは脚式移動ロボットであり、人間と同様に、基体Ｂ０と、基体Ｂ０の上方に配置された頭部Ｂ１と、基体Ｂ０の上部に上部両側から延設された左右の腕体Ｂ２と、左右の腕体Ｂ２のそれぞれの先端に設けられているハンドＨと、基体Ｂ０の下部から下方に延設された左右の脚体Ｂ４とを備えている。なお、ロボットＲは脚式移動ロボットのみならず、ハンドＨの位置および姿勢を変化させるための腕体Ｂ２に相当する機構を備えているあらゆる種類のロボットであってもよい。
【００２５】
ロボットＲはその動作を制御する制御装置２（演算処理要素）を備えている。制御装置２はロボットＲの内部ネットワークを通じて接続された主制御ユニットおよび一または複数の副制御ユニットより構成される分散制御装置であってもよい。
【００２６】
制御装置２はコンピュータ（ＣＰＵ、ＲＯＭおよびＲＡＭ等のメモリ、ならびに、Ａ／Ｄ回路およびＩ／Ｏ回路等の回路により構成されている。）により構成されている。制御装置２によれば、ＣＰＵによりメモリから制御プログラムが適宜読み出され、読み出されたプログラムにしたがってハンドＨの動作が制御される。
【００２７】
制御装置２はロボットＲに搭載されている複数のアクチュエータ４のそれぞれの動作を制御することにより、ハンドＨの指機構Ｆ１〜Ｆ５の動き、腕体Ｂ２の各関節機構における動きおよび脚体Ｂ４の各関節における動き等を制御する。
【００２８】
基体Ｂ０はヨー軸回りに相対的に回動しうるように上下に連結された上部および下部により構成されている。頭部Ｂ１は基体Ｂ０に対してヨー軸回りに回動する等、動くことができる。
【００２９】
腕体Ｂ２は第１腕体リンクＢ２２と、第２腕体リンクＢ２４とを備えている。基体Ｂ０と第１腕体リンクＢ２１とは肩関節機構（第１腕関節機構）Ｂ２１を介して連結され、第１腕体リンクＢ２２と第２腕体リンクＢ２４とは肘関節機構（第２腕関節機構）Ｂ２３を介して連結され、第２腕体リンクＢ２４とハンドＨとは手根関節機構（第３腕関節機構）Ｂ２５を介して連結されている。肩関節機構Ｂ２１はロール、ピッチおよびヨー軸回りの回動自由度を有し、肘関節機構Ｂ２３はピッチ軸回りの回動自由度を有し、手根関節機構Ｂ２５はロール、ピッチ、ヨー軸回りの回動自由度を有している。
【００３０】
脚体Ｂ４は第１脚体リンクＢ４２と、第２脚体リンクＢ４４と、足部Ｂ５とを備えている。基体Ｂ０と第１脚体リンクＢ４２とは股関節機構（第１脚関節機構）Ｂ４１を介して連結され、第１脚体リンクＢ４２と第２脚体リンクＢ４４とは膝関節機構（第２脚関節機構）Ｂ４３を介して連結され、第２脚体リンクＢ４４と足部Ｂ５とは足関節機構（第３脚関節機構）Ｂ４５を介して連結されている。
【００３１】
股関節機構Ｂ４１はロール、ピッチおよびロール軸回りの回動自由度を有し、膝関節機構Ｂ４３はピッチ軸回りの回動自由度を有し、足関節機構Ｂ４５はロールおよびピッチ軸回りの回動自由度を有している。股関節機構Ｂ４１、膝関節機構Ｂ４３および足関節機構Ｂ４５は「脚関節機構群」を構成する。なお、脚関節機構群に含まれる各関節機構の並進および回転自由度は適宜変更されてもよい。また、股関節機構Ｂ４１、膝関節機構Ｂ４３および足関節機構Ｂ４５のうち任意の１つの関節機構が省略された上で、残りの２つの関節機構の組み合わせにより脚関節機構群が構成されていてもよい。さらに、脚体Ｂ４が膝関節とは別の第２脚関節機構を有する場合、当該第２脚関節機構が含まれるように脚関節機構群が構成されてもよい。足部Ｂ５の底には着床時の衝撃緩和のため、特開２００１−１２９７７４号公報に開示されているような弾性素材Ｂ５２が設けられている。
【００３２】
頭部Ｂ１には３次元画像センサ１１および２次元画像センサ１２が搭載されている。
【００３３】
３次元画像センサ１１は、本実施形態ではＴＯＦ（Time Of Flight）３次元センサである。３次元画像センサ１１は、ロボットＲの前方（頭部Ｂ０の正面が向けられている範囲）の様子を撮像することにより、被写体の３次元情報を取得する。
【００３４】
２次元画像センサ１２は、本実施形態ではカラーセンサである。２次元画像センサ１２は、３次元画像センサ１１による撮像範囲と重なる範囲を撮像することにより、被写体の色情報（物理情報）を取得する。
【００３５】
３次元画像センサ１１の解像度（たとえば１７６×２４４）と、２次元画像センサ１２の解像度（たとえば１０２４×７６８）とは異なっている。
【００３６】
３次元画像センサ１１の撮像面により定義される第１画像座標系および２次元画像センサ１２の撮像面により定義される第２画像座標系のロボット座標系に対する位置および姿勢はメモリに保存されており、後述するように世界座標系において認識された物体の位置および姿勢が、ロボット座標系に座標変換される際に用いられる。ロボット座標系は、たとえばロボットＲの質量中心（たとえば基体Ｂ０に含まれている。）が原点として定義され、ロボットＲの上方が＋ｘ方向として定義され、右方向が＋ｙ方向として定義され、前方が＋ｚ方向として定義されている。）
制御装置２は後述する画像処理を実行する画像処理要素２０を備えている（図２参照）。画像処理要素２０は、制御装置２とは別個の一または複数のモジュールにより構成されうる。
【００３７】
前記構成の環境認識システムの機能について説明する。なお、演算処理結果は適宜メモリに保存され、かつ、適宜メモリから読み出される。
【００３８】
３次元画像センサ１１のカメラパラメータおよびひずみパラメータがメモリから読み取られる（図３／ＳＴＥＰ１０２）。２次元画像センサ１２のカメラパラメータおよびひずみパラメータがメモリから読み取られる（図３／ＳＴＥＰ１０４）。
【００３９】
３次元画像センサ１１によりロボットＲの周辺の様子が撮像されることにより、被写体の３次元情報が取得される（図３／ＳＴＥＰ１０６）。「３次元情報」は、第１画像座標系における複数の第１ピクセルのそれぞれの位置における、センサ１１の主点から被写体までの距離Dを意味する。
【００４０】
２次元画像センサ１２によりロボットＲの周辺が撮像されることにより、被写体の色情報が取得される（図３／ＳＴＥＰ１０８）。「色情報」は、第２画像座標系における複数の第２ピクセルのそれぞれの位置における、被写体の色彩（たとえばRGB値により特定される。）を意味する。
【００４１】
続いて、３次元画像センサ１１のカメラパラメータおよびひずみパラメータを用いて、第１画像座標系における第１ピクセル位置が３次元の世界座標系の点に座標変換される（図３／ＳＴＥＰ１１０）。
【００４２】
具体的には、３次元画像センサ１１のカメラパラメータとしての回転行列Rtおよび並進ベクトルTtを用い、関係式（１）にしたがって第１画像座標系の第１ピクセル位置P1が世界座標系の点Pwに変換される。これにより、図４に示されているように３次元画像座標系において量子化された点（第１ピクセル位置）P1=(X1, Y1)が世界座標系の点Pw=(xw, yw, zw)に座標変換される。
【００４３】
Pw=Rt-1(P1-Tt) ..(1)
【００４４】
第１ピクセル位置P1=(X1, Y1)は、３次元画像センサ１１の主点から当該点P1までの距離D、焦点距離f、レンズひずみパラメータκ1およびκ2ならびにスケール係数sx（通常は「１」に設定される。）等に基づいて次の関係式（２ａ）〜（２ｃ）にしたがって算定されうる。
【００４５】
(d’xx/sx)(1+κ1r2+κ2r4)=f(X1/Z1) ..(2a)
dyy(1+κ1r2+κ2r4)=f(Y1/Z1) ..(2b)
X12+Y12+Z12=D2 ..(2c)
【００４６】
ここで「r」は画像原点（Cx, Cy）等を用いて関係式（３）により表現される。
【００４７】
r=((d’xx/sx)2+(d’yy)2)1/2, x≡X1-Cx, y≡Y1-Cy ..(3)
【００４８】
「d’x」はｘ方向の素子数Ncxおよび走査線上のサンプル数Nfxを用い、関係式（４）にしたがって補正されたｘ方向の素子間隔dxを表している。
【００４９】
d’x=(Ncx/Nfx) dx ..(4)
【００５０】
続いて、第２画像座標系において、世界座標系の点Pwに対応する変換位置が算定される（図３／ＳＴＥＰ１１２）。
【００５１】
具体的には、まず、２次元画像センサ１２のカメラパラメータである回転行列R＝(Rij)および並進ベクトルT=(tx, ty, tz)ならびに関係式（５）および（６）が用いられることにより非線型方程式（７）が得られる。
【００５２】
x (1+κ1r2+κ2r4)=(sx/d’x)f(R11xw+R12yw+R13zw+tx)/(R31xw+R32yw+R33zw+tz) ..(5)
y (1+κ1r2+κ2r4)= (1/dy)fRr21xw+R22yw+R23zw+ty)/(R31xw+R32yw+R33zw+tz) ..(6)
r+κ1r3+κ2r5=c=(c12+c22)1/2,
c1≡f(R11xw+R12yw+R13zw+tx)/(R31xw+R32yw+R33zw+tz),
c2≡f(R21xw+R22yw+R23zw+ty)/(R31xw+R32yw+R33zw+tz) ..(7)
【００５３】
線型方程式（７）がNewton-Rraphson法にしたがって近似されることにより関係式（８）が得られる。
【００５４】
f(rn)=rn+κ1rn3+κ2rn5-c, f’(rn)=1+3κ1rn2+5κ2rn4, rn+1=rnf(rn)/f’(rn) ..(8)
【００５５】
rnが関係式（８）にしたがって反復計算され、その収束率|1-(rn+1/rn)|が閾値ε（たとえばexp(-10)）以下になった際の収束結果がrの近似解として算定される。
【００５６】
当該近似解rを用いて、関係式（３）（５）および（６）にしたがって第２座標系における変換位置Pc=(xc, yc)が算定される。これにより、図４に示されているように、世界座標系の点Pw=(xw, yw, zw)に対応する第２画像座標系における変換位置Pcが求められる。
【００５７】
前記のように３次元画像センサ１１の解像度と、２次元画像センサ１２の解像度とは異なっているが、解像度の相違の有無にかかわらず補間が必要である。これは、図４に示されているように、第１画像座標系における第１ピクセル位置（X1, Y1）に対応する第２画像座標系における変換位置Pcは、通常、第２画像座標系における量子化された点（第２ピクセル位置）には一致しないからである。図４には、第２画像座標系における変換位置Pcが、第２ピクセル位置Ａ〜Ｄを頂点とする矩形領域に含まれている様子が示されている。
【００５８】
そこで、たとえば、最近傍点補間法、双線形補間法またはバイキュービックスプライン法にしたがって、３次元情報に色情報（物理情報）が対応付けられる。
【００５９】
「最近傍点補間法」によれば、第２画像座標系において変換位置Pcに最も近い第２ピクセル位置Ａの色情報が、当該変換位置Pcにおける色情報として適用される。「双線形補間法」によれば、第２画像座標系において変換位置Pcを囲う４つの第２ピクセル位置Ａ〜Ｄのそれぞれまでの距離の比と、第２ピクセル位置Ａ〜Ｄの色情報とに基づいて当該変換位置Pcの色情報が決定される。「バイキュービックスプライン補間法」によれば、第２画像座標系において変換位置Pcを囲う４つの第２ピクセル位置Ａ〜Ｄに加えて、さらに外側の１２の第２ピクセル位置の合計１６の第２ピクセル位置と、当該第２幾セル位置のそれぞれの色情報とに基づき、当該変換位置Pcの色情報が決定される。
【００６０】
第１画像座標系における対象となる第１ピクセル位置（すべての第１ピクセル位置でなくてもよい。）について、前記のような世界座標系への座標変換、第２画像座標系において対応する変換位置の決定、最近傍点補間法等による色情報の算定、ならびに、３次元情報に対する物理情報（色情報）の対応付けが完了したか否かが判定される（図３／ＳＴＥＰ１１６）。
【００６１】
当該判定結果が否定的である場合（図３／ＳＴＥＰ１１６‥ＮＯ）、対象となる残りの第１ピクセル位置について世界座標系への座標変換、第２画像座標系において対応する変換位置の決定、最近傍点補間法等による物理情報（色情報）の算定、ならびに、３次元情報に対する物理情報の対応付けが繰り返される（図３／ＳＴＥＰ１１０〜ＳＴＥＰ１１４）。
【００６２】
一方、対象となるすべての第１ピクセル位置に対して前記処理が実行されたと判定された場合（図３／ＳＴＥＰ１１６‥ＹＥＳ）、２次元画像に３次元情報を対応付けることの要否が判定される（図３／ＳＴＥＰ１１８）。たとえば、３次元画像センサ１１の解像度が、２次元画像センサ１２の解像度よりも高いか否かに応じて、当該要否が判定されうる。なお、当該判定処理は省略されてもよい。
【００６３】
当該判定結果が肯定的である場合（図３／ＳＴＥＰ１１８‥ＹＥＳ）、図５に示されているように第２画像座標系において対象となる第２ピクセル位置P2=(X2, Y2)を囲む最も近い３つの変換位置Pc1、Pc2およびPc3のそれぞれに対応する世界座標系の点の座標値Pw1=(xw1, yw1, zw1)、Pw2=(xw2, yw2, zw2)およびPw3=(xw3, yw3, zw3)がメモリから読み出される（図３／ＳＴＥＰ１２０）。世界座標系における当該３つの点Pw1、Pw2およびPw3が同一直線上には無いことが確認される。
【００６４】
そして、図５に示されているように世界座標系において３つの点Pw1、Pw2およびPw3を通る平面が算定されれる（図３／ＳＴＥＰ１２２）。
【００６５】
世界座標系において３つの点Pw1、Pw2およびPw3を通る平面は関係式（９）によって表される。
【００６６】
u1xw+u2yw+u3zw=1,
t(u1,u2,u3)=Q -1・t(1,1,1),
Q≡t(tPw1, tPw2, tPw3) ..(9)
【００６７】
さらに、２次元画像センサ１２の主点と、第２画像座標系における変換位置Pcとを通る直線が算定された上で、当該平面と当該直線との交点が、当該変換位置Pcに対応する世界座標系における点位置Pwとして算定される（図３／ＳＴＥＰ１２４）。
【００６８】
第２画像座標系における変換位置Pc=(Xc, Yc)は関係式（１０）（１１）により表現される。
【００６９】
Xc (1+κ1r2+κ2r4)=(sx/d’x)f(r11Xw+r12Xw+r13Xw+tx)/(r31Xw+r32Yw+r33Zw+tz)
(r=((Xc)2+(Yc)2)1/2) ..(10)
Yc(1+κ1r2+κ2r4)= (1/dy)f(r21Xw+r22Yw+r23Zw+ty)/(r31Xw+r32Yw+r33Zw+tz) ..(11)
【００７０】
関係式（９）〜（１１）から得られる関係式（１２）にしたがって変換位置Pcに対応する世界座標系の座標値Pw=(Xw,Yw,Zw)が算定される。
【００７１】
t(Xw,Yw,Zw)=B -1・t(1,1,1), B≡(Bij),
B1j= {Xc (1+κ1r2+κ2r4)r3j-(sx/d’x)f×r1j}/{(sx/d’x)f×tx- Xc (1+κ1r2+κ2r4) ×tz },
B2j= {Yc (1+κ1r2+κ2r4)r3j-(1/dy)f×r2j}/{(1/dy)f×ty- Yc (1+κ1r2+κ2r4) ×tz },
B3j=uj ..(12)
【００７２】
第２画像座標系における対象となる第２ピクセル位置P2（すべての第２ピクセル位置でなくてもよい。）について、前記のような２次元座標系における３つの変換位置の選択、世界座標系における平面の算定、２次元画像センサ１２の主点と当該第２ピクセル位置とを通る直線の算定、および、当該平面と当該直線との交点の算定が完了したか否かが判定される（図３／ＳＴＥＰ１２６）。
【００７３】
当該判定結果が否定的である場合（図３／ＳＴＥＰ１２６‥ＮＯ）、対象となる残りの第２ピクセル位置について２次元座標系における３つの変換位置の選択、世界座標系における平面の算定、２次元画像センサ１２の主点と当該第２ピクセル位置とを通る直線の算定、および、当該平面と当該直線との交点の算定が繰り返される（図３／ＳＴＥＰ１２０〜ＳＴＥＰ１２４）。
【００７４】
一方、対象となるすべての第２ピクセル位置に対して前記処理が実行されたと判定された場合（図３／ＳＴＥＰ１２６‥ＹＥＳ）、３次元画像センサ１１による３次元情報の取得（図３／ＳＴＥＰ１０６参照）および２次元画像センサ１２による物理情報の取得（図３／ＳＴＥＰ１０８参照）ならびにこれらに続く処理の繰り返し要否が判定される（図３／ＳＴＥＰ１２８）。なお、２次元画像に３次元情報を対応付ける必要がないと判定された場合（図３／ＳＴＥＰ１１８‥ＮＯ）、当該繰り返し要否が判定される（図３／ＳＴＥＰ１２８）。
【００７５】
繰り返し処理が必要であると判定された場合（図３／ＳＴＥＰ１２８‥ＮＯ）、３次元画像センサ１１による３次元情報の取得および２次元画像センサ１２による物理情報の取得ならびにこれらに続く処理が繰り返し実行される（図３／ＳＴＥＰ１０６〜ＳＴＥＰ１２６参照）。
【００７６】
繰り返し処理が不要であると判定された場合（図３／ＳＴＥＰ１２８‥ＹＥＳ）、前記一連の画像処理が終了する。
【００７７】
これにより、世界座標系における物体の位置、姿勢および形状ならびに世界座標系における各点における色、すなわち、ロボットＲによる物体の把持動作に必要な当該物体に関する情報が取得される。必要に応じてこれらの形状等に基づくデータベース検索またはパターンマッチング等により得られる物体の種類、質量中心など物体の付加的情報が取得されてもよい。
【００７８】
前記機能を発揮する本発明の環境認識システムによれば、ロボットＲの周辺に存在する物体に関する情報、すなわち、世界座標系における当該物体の位置、姿勢および形状ならびに世界座標系における各点の色を高精度で認識することができる。そして、ロボットＲは、腕部Ｂ２、ハンドＨおよび指機構Ｆ１〜Ｆ５のそれぞれを動かして物体を把持する等、被写体となった物体を対象として作業を確実に実行することができる。なお、ロボットＲは、必要に応じて、作業に先立って脚部Ｂ４等を動かして基体Ｂ０の位置（基体Ｂ０の質量中心位置）および姿勢（世界座標系の各軸に対する基体座標系の各軸の角度）を調節してもよい。
【００７９】
また、３次元画像センサ１１および２次元画像センサ１２のそれぞれの撮像範囲の重なり部分を大きくするために光軸を近づけることが好ましいことから、両センサ１１および１２を相互に近くに配置してシステムのコンパクト化が図られる。システムのコンパクト化が図られることから、ロボットＲ全体としてのコンパクト化またはスペースの有効利用が可能となる。
【００８０】
なお、３次元情報を取得するための３次元画像センサ１１としては、ＴＯＦ３次元画像センサのほか、スキャン型ＴＯＦ３次元画像センサおよびステレオカメラのうち一部または全部が採用されてもよい。物理情報を取得するための２次元画像センサ１２としては、被写体の色情報を取得するための可視光カラーカメラのほか、被写体の濃淡情報を取得するための可視光モノクロカメラ、被写体の暗視情報を取得するための近赤外カメラ、被写体の温度情報を取得するための遠赤外カメラ、ミリ波源情報を取得するためのミリ波カメラ、および、面の法線方向の情報を取得するための偏光カメラのうち一部または全部が採用されてもよい。
【００８１】
これらの３次元画像センサ１１および２次元画像センサ１２の組み合わせはさまざまに変更されうる。たとえば、環境認識システムの構成要素として、（１）一の３次元画像センサ１１および一の２次元画像センサ１２の組み合わせのほか、（２）一の３次元画像センサ１１および複数の同種または異種の２次元画像センサ１２、（３）複数の同種または異種の３次元画像センサ１１および一の２次元画像センサ１２の組み合わせ、または、（４）複数の同種または異種の３次元画像センサ１１および複数の同種または異種の２次元画像センサ１２の組み合わせが採用されてもよい。
【００８２】
本発明の他の実施形態として、２つの３次元画像センサ１１（ＴＯＦ３次元画像センサ）と、２つの異種の２次元画像センサ１２（可視光カラーカメラおよび遠赤外カメラ）が環境認識システムに採用されている場合の画像処理の手順について説明する。
【００８３】
まず、２つの３次元画像センサ１１のそれぞれのカメラパラメータおよびひずみパラメータがメモリから読み取られる（図６／ＳＴＥＰ２０２）。
【００８４】
２つの２次元画像センサ１２のそれぞれのカメラパラメータおよびひずみパラメータがメモリから読み取られる（図６／ＳＴＥＰ２０４）。
【００８５】
２つの３次元画像センサ１１のそれぞれによりロボットＲの周辺の様子が撮像されることにより、被写体の３次元情報が取得される（図６／ＳＴＥＰ２０６）。
【００８６】
２次元画像センサ１２としてのカラーカメラおよび遠赤外カメラのそれぞれによりロボットＲの周辺が撮像されることにより、被写体の物理情報として色情報および温度情報が取得される（図６／ＳＴＥＰ２０８，ＳＴＥＰ２１０）。
【００８７】
続いて、第１の３次元画像センサ１１のカメラパラメータおよびひずみパラメータを用いて、当該第１の３次元画像センサ１１の第１画像座標系（２次元座標系）における第１ピクセル位置が世界座標系（３次元座標系）における点に座標変換される（図６／ＳＴＥＰ２１２）。
【００８８】
さらに、世界座標系における当該点Pw1=(xw1, yw1, zw1)に対応する、カラーカメラの第２画像座標系における変換位置Pc11が算定される（図６／ＳＴＥＰ２１４）。
【００８９】
また、最近傍点補間法等にしたがって当該変換位置Pc11における色情報（R,G,B）が算定され、第１の３次元画像センサ１１の第１画像座標系において当該変換位置Pc11に対応する第１ピクセル位置P111の３次元情報に、この色情報（R,G,B）が対応付けられる（図６／ＳＴＥＰ２１６（図４参照））。
【００９０】
さらに、世界座標系における当該点Pw1に対応する、遠赤外カメラの第２画像座標系における変換位置Pc12が算定される（図６／ＳＴＥＰ２１８）。
【００９１】
また、最近傍点補間法等にしたがって当該変換位置Pc12における温度情報Temp（または温度に応じた輝度情報Lum）が算定され、第１の３次元画像センサ１１の第１画像座標系において当該変換位置Pc12に対応する第１ピクセル位置P112の３次元情報に、この温度情報Tempが対応付けられる（図６／ＳＴＥＰ２２０（図４参照））。
【００９２】
第１の３次元画像センサの第１画像座標系における対象となる第１ピクセル位置（すべての第１ピクセル位置でなくてもよい。）について、前記のような世界座標系への座標変換、第２画像座標系において対応する変換位置の決定、最近傍点補間法等による物理情報（色情報および温度情報）の算定、ならびに、３次元情報に対する物理情報の対応付けが完了したか否かが判定される（図６／ＳＴＥＰ２２２）。
【００９３】
当該判定結果が否定的である場合（図６／ＳＴＥＰ２２２‥ＮＯ）、対象となる残りの第１ピクセル位置について世界座標系への座標変換、第２画像座標系において対応する変換位置の決定、最近傍点補間法等による物理情報（色情報および温度情報）の算定、ならびに、３次元情報に対する物理情報の対応付けが繰り返される（図６／ＳＴＥＰ２１２〜ＳＴＥＰ２２０）。
【００９４】
一方、第１の３次元画像センサ１１の第１画像座標系における対象となるすべての第１ピクセル位置に対して前記処理が実行されたと判定された場合（図６／ＳＴＥＰ２２２‥ＹＥＳ）、第２の３次元画像センサ１１のカメラパラメータおよびひずみパラメータを用いて、第２の３次元画像センサ１１の第１画像座標系（２次元座標系）の第１ピクセル位置が世界座標系（３次元座標系）の点に座標変換される（図６／ＳＴＥＰ２２４）。
【００９５】
さらに、世界座標系における当該点Pw2=(xw2 ,yw2, zw2)に対応する、カラーカメラの第２画像座標系における変換位置Pc21が算定される（図６／ＳＴＥＰ２２６）。
【００９６】
また、最近傍点補間法等にしたがって当該変換位置Pc21における色情報（R,G,B）が算定され、第２の３次元画像センサ１１の第１画像座標系において当該変換位置Pc21に対応する第１ピクセル位置P121の３次元情報に、この色情報（R,G,B）が対応付けられる（図６／ＳＴＥＰ２２８（図４参照））。
【００９７】
さらに、世界座標系における当該点Pw2に対応する、遠赤外カメラの第２画像座標系における変換位置Pc22が算定される（図６／ＳＴＥＰ２３０）。
【００９８】
また、最近傍点補間法等にしたがって当該変換位置Pc22における温度情報Temp（または温度に応じた輝度情報Lum）が算定され、第２の３次元画像センサ１１の第１画像座標系において当該変換位置Pc22に対応する第１ピクセル位置P122の３次元情報に、この温度情報Tempが対応付けられる（図６／ＳＴＥＰ２３２（図４参照））。
【００９９】
第２の３次元画像センサの第１画像座標系における対象となる第１ピクセル位置（すべての第１ピクセル位置でなくてもよい。）について、前記のような世界座標系への座標変換、第２画像座標系において対応する変換位置の決定、最近傍点補間法等による物理情報（色情報および温度情報）の算定、ならびに、３次元情報に対する物理情報の対応付けが完了したか否かが判定される（図６／ＳＴＥＰ２３４）。
【０１００】
当該判定結果が否定的である場合（図６／ＳＴＥＰ２３４‥ＮＯ）、対象となる残りの第１ピクセル位置について世界座標系への座標変換、第２画像座標系において対応する変換位置の算定、最近傍点補間法等による物理情報（色情報および温度情報）の算定、ならびに、３次元情報に対する物理情報の対応付けが繰り返される（図６／ＳＴＥＰ２２４〜ＳＴＥＰ２３２）。
【０１０１】
一方、第２の３次元画像センサ１１の第１画像座標系における対象となるすべての第１ピクセル位置に対して前記処理が実行されたと判定された場合（図６／ＳＴＥＰ２３４‥ＹＥＳ）、２次元画像に３次元情報を対応付けることの要否が判定される（図７／ＳＴＥＰ２３６）。
【０１０２】
当該判定結果が肯定的である場合（図７／ＳＴＥＰ２３６‥ＹＥＳ）、カラーカメラの第２画像座標系における第２ピクセル位置P21を囲む最も近い３つの変換位置のそれぞれに対応する世界座標系の点位置がメモリから読み出され（図７／ＳＴＥＰ２４０）、世界座標系において当該３つの点を通る平面が算定される（図７／ＳＴＥＰ２４２（図５、関係式（９）参照））。
【０１０３】
さらに、カラーカメラの主点と、カラーカメラの第２画像座標系における第２ピクセル位置P21とを通る直線が算定された上で、当該平面と当該直線との交点が、当該変換位置Pcに対応する世界座標系における点位置Pwとして算定される（図７／ＳＴＥＰ２４４（図５、関係式（１２）参照）。
【０１０４】
カラーカメラの第２画像座標系における対象となる第２ピクセル位置（すべての第２ピクセル位置でなくてもよい。）について、前記のような２次元座標系における３つの変換位置の選択、世界座標系における平面の算定、カラーカメラの主点と当該第２ピクセル位置とを通る直線の算定、および、当該平面と当該直線との交点の算定が完了したか否かが判定される（図７／ＳＴＥＰ２４６）。
【０１０５】
当該判定結果が否定的である場合（図３／ＳＴＥＰ２４６‥ＮＯ）、カラーカメラの第２画像座標系における対象となる残りの第２ピクセル位置について２次元座標系における３つの変換位置の選択、世界座標系における平面の算定、カラーカメラの主点と当該第２ピクセル位置とを通る直線の算定、および、当該平面と当該直線との交点の算定が繰り返される（図７／ＳＴＥＰ２４０〜ＳＴＥＰ２４４）。
【０１０６】
一方、カラーカメラの第２画像座標系における対象となるすべての第２ピクセル位置に対して前記処理が実行されたと判定された場合（図７／ＳＴＥＰ２４６‥ＹＥＳ）、遠赤外カメラの第２画像座標系における対象となる第２ピクセル位置P22を囲む最も近い変換位置のそれぞれに対応する世界座標系の点位置がメモリから読み出され（図７／ＳＴＥＰ２４８）、世界座標系において当該３つの点を通る平面が算定される（図７／ＳＴＥＰ２５０（図５、関係式（９）参照））。
【０１０７】
さらに、遠赤外カメラの主点と、遠赤外カメラの第２画像座標系における第２ピクセル位置P22とを通る直線が算定された上で、当該平面と当該直線との交点が、当該変換位置Pcに対応する世界座標系における点位置Pwとして算定される（図７／ＳＴＥＰ２５２（図５、関係式（１２）参照）。
【０１０８】
遠赤外カメラの第２画像座標系における対象となる第２ピクセル位置（すべての第２ピクセル位置でなくてもよい。）について、前記のような２次元座標系における３つの変換位置の選択、世界座標系における平面の算定、遠赤外カメラの主点と当該第２ピクセル位置とを通る直線の算定、および、当該平面と当該直線との交点の算定が完了したか否かが判定される（図７／ＳＴＥＰ２５４）。
【０１０９】
当該判定結果が否定的である場合（図３／ＳＴＥＰ２５４‥ＮＯ）、２次元座標系における３つの変換位置の選択、世界座標系における平面の算定、遠赤外カメラの主点と当該第２ピクセル位置とを通る直線の算定、および、当該平面と当該直線との交点の算定が繰り返される（図７／ＳＴＥＰ２４８〜ＳＴＥＰ２５２）。
【０１１０】
一方、遠赤外カメラの第２画像座標系における対象となるすべての第２ピクセル位置に対して前記処理が実行されたと判定された場合（図７／ＳＴＥＰ２５４‥ＹＥＳ）、２つの３次元画像センサ１１のそれぞれによる３次元情報の取得（図６／ＳＴＥＰ２０６参照）および２つの２次元画像センサのそれぞれによる物理情報の取得（図６／ＳＴＥＰ２０８，ＳＴＥＰ２１０参照）ならびにこれらに続く処理の繰り返し要否が判定される（図７／ＳＴＥＰ２５６）。なお、２次元画像に３次元情報を対応付ける必要がないと判定された場合（図７／ＳＴＥＰ２３６‥ＮＯ）、当該繰り返し要否が判定される（図７／ＳＴＥＰ２５６）。
【０１１１】
繰り返し処理が必要であると判定された場合（図７／ＳＴＥＰ２５６‥ＮＯ）、２つの３次元画像センサ１１のそれぞれによる３次元情報の取得および２つの２次元画像センサのそれぞれによる物理情報の取得ならびにこれらに続く処理が繰り返し実行される（図６／ＳＴＥＰ２０６〜ＳＴＥＰ２３４、図７／ＳＴＥＰ２３６〜ＺＴＥＰ２５４参照）。
【０１１２】
繰り返し処理が不要であると判定された場合（図７／ＳＴＥＰ２５６‥ＹＥＳ）、前記一連の画像処理が終了する。
【０１１３】
これにより、物体の把持動作に必要な当該物体に関する情報、すなわち、世界座標系における物体の位置、姿勢および形状、ならびに、世界座標系における当該物体の各点の物理情報（色および温度）が取得される。
【０１１４】
なお、カメラパラメータを取得するため、次に説明するような手法でキャリブレーションが実行されてもよい。
【０１１５】
すなわち、まず、３次元画像センサ１１によりロボットＲの周辺にあるキャリブレーションボード（キャリブレーション用物体）を撮像することにより、輝度情報および距離情報を有する画素により構成されている３次元画像を取得する。
【０１１６】
また、２次元画像センサ１２により３次元画像センサ１１に撮像範囲と重なる範囲においてキャリブレーションボードを撮像することにより、色情報を有する画素により構成されている２次元画像を取得する。
【０１１７】
さらに、キャリブレーションが実行されることによりカメラパラメータが決定される。キャリブレーション方法としては公知の手法が採用される（R.Y.Tsai: “An Efficient and accurate calibration technique for 3D machine vision” (CVPR, pp.364-374 (1986))およびR.Y.Tsai: “A versatile camera calibration technique for high-accuracy 3D machine vision metrology using off-the-shelf TV cameras and lenses” (J-RA, vol.3, pp.323-344 (1987))参照）。
【０１１８】
この際、図８に示されているように黒色および白色の正方形が前後左右に交互に配置されているチェックまたは市松模様のパターンを有するキャリブレーションボードが用いられる。ボードの中心近くにある白色の正方形の隅角部分に三角形状のマークが付されている。一方のカメラで撮像されたボードの複数の正方形のそれぞれの頂点が、他方のカメラで撮像されたボードの複数の正方形の頂点のうちいずれに相当するかがこのマークを用いて確認されうる。
【０１１９】
なお、このパターンの形成方法は、キャリブレーションの対象となる画像センサの種類に応じて変更されてもよい。たとえば、２次元画像センサ１２がアクティブ型の近赤外カメラである場合、近赤外を反射する塗料の有無によって前期のようなパターンが形成されてもよい。
【０１２０】
赤外光を反射する塗料またはシールがボードに付されることによりマークが構成されている。これにより、３次元画像センサによるマークの認識精度の向上が図られている。ボードの表面は白色の正方形において乱反射が起こりにくいように仕上げ加工されている一方、黒色の正方形において乱反射が起こりやすいように仕上げ加工されている。これにより、３次元画像センサによる白黒パターンの認識精度の向上が図られている。
【０１２１】
キャリブレーションにより、世界座標値およびこれに対応するカメラの画像座標値に基づき、カメラの外部パラメータおよび内部パラメータが関係式（３）（５）および（６）にしたがって算定される。「外部パラメータ」として回転行列R（未知数は３つの３次正方行列）および並進ベクトルT（未知数３つ）が算定される。「内部パラメータ」として焦点距離f、レンズひすみパラメータκ1およびκ2、スケール係数sx（通常は「１」に設定される。）ならびに画像原点（Cx, Cy）が算定される。キャリブレーションにより得られたカメラパラメータはメモリに保存される。
【符号の説明】
【０１２２】
２‥制御装置、１１‥３次元画像センサ、１２‥２次元画像センサ、２０‥画像処理要素、Ｒ‥ロボット（装置）。
【特許請求の範囲】
【請求項１】
装置の環境を認識するシステムであって、
前記装置の周辺の様子を撮像することにより、被写体の３次元情報を取得する３次元画像センサと、
前記３次元画像センサに撮像範囲と重なる範囲を撮像することにより、前記被写体の物理情報を取得する２次元画像センサと、
前記３次元画像センサにより取得された前記被写体の３次元情報および前記２次元画像センサにより取得された前記被写体の物理情報を対応付けることにより、前記被写体の位置、姿勢および形状、ならびに、物理情報を取得するように構成されている画像処理要素とを備えていることを特徴とする環境認識システム。
【請求項２】
請求項１記載の環境認識システムにおいて、
前記画像処理要素が、前記３次元画像センサの撮像面により定義される第１画像座標系における複数の第１ピクセルのそれぞれの位置が、前記２次元画像センサの撮像面により定義される第２画像座標系に座標変換された結果としての複数の変換位置のそれぞれを算定し、
前記第２画像座標系における少なくとも３つの変換位置のそれぞれに対応する、前記第１ピクセルが有する前記被写体の３次元情報に基づき、前記第２画像座標系において前記少なくとも３つの変換位置の近傍にある第２ピクセルが有する前記被写体の物理情報に対応付けられる前記被写体の３次元情報を算定するように構成されていることを特徴とする環境認識システム。
【請求項３】
請求項２記載の環境認識システムにおいて、
前記画像処理要素が、前記第２画像座標系における３つの変換位置のそれぞれに対応する、前記第１ピクセルが有する前記被写体の３次元情報としての世界座標系における３つの位置を通る平面を算定し、前記２次元画像センサの主点と、前記第２画像座標系において前記３つの変換位置の近傍にある前記第２ピクセルとを通る直線を算定し、前記世界座標系における前記平面と前記直線との交点位置を、前記第２ピクセルが有する前記被写体の物理情報に対応付けられる前記被写体の３次元情報として算定するように構成されていることを特徴とする環境認識システム。
【請求項４】
請求項２または３記載の環境認識システムにおいて、
前記画像処理要素が、前記第２画像座標系における変換位置の周辺に存在する一または複数の前記第２ピクセルが有する前記被写体の物理情報に基づき、前記変換位置に対応する、前記第１ピクセルが有する前記被写体の３次元情報に対応付けられる前記被写体の物理情報を算定するように構成されていることを特徴とする環境認識システム。
【請求項５】
請求項１記載の環境認識システムにおいて、
前記画像処理要素が、前記３次元画像センサの撮像面により定義される第１画像座標系における複数の第１ピクセルのそれぞれの位置が、前記２次元画像センサの撮像面により定義される第２画像座標系に座標変換された結果としての複数の変換位置のそれぞれを算定し、かつ、
前記第２画像座標系における変換位置の周辺に存在する一または複数の前記第２ピクセルが有する前記被写体の物理情報に基づき、前記変換位置に対応する、前記第１ピクセルが有する前記被写体の３次元情報に対応付けられる前記被写体の物理情報を算定するように構成されていることを特徴とする環境認識システム。
【請求項６】
請求項１〜５のうちいずれか１つに記載の環境認識システムにおいて、
前記２次元画像センサが、前記被写体の物理情報として色情報または温度情報を取得するように構成され、
前記画像処理要素が、前記３次元画像センサにより取得された前記被写体の３次元情報および前記２次元画像センサにより取得された前記被写体の色情報および温度情報のうち一方または両方を対応付けることにより、前記装被写体の位置、姿勢および形状、ならびに、色情報および温度情報のうち一方または両方を取得するように構成されていることを特徴とする環境認識システム。
【請求項７】
請求項１〜６のうちいずれか１つに記載の環境認識システムにおいて、
前記３次元画像センサが前記被写体の３次元情報に加えて前記被写体の輝度情報を取得するように構成され、
前記画像処理要素が、前記３次元画像センサにより取得されたキャリブレーション用物体の輝度情報および距離情報、ならびに、前記２次元画像センサにより取得された前記キャリブレーション用物体の物理情報を用いて、前記３次元画像の座標系、前記２次元画像の座標系および世界座標系の関係を表わすパラメータを求めるキャリブレーションを実行した上で、前記パラメータを用いて前記３次元画像センサにより取得された前記被写体の３次元情報および前記２次元画像センサにより取得された前記被写体の物理情報を対応付けるように構成されていることを特徴とする環境認識システム。
【請求項８】
装置の環境を認識する方法であって、
３次元画像センサにより前記装置の周辺の様子を撮像することにより、被写体の３次元情報を取得し、
前記３次元画像センサに撮像範囲と重なる範囲を２次元画像センサにより撮像することにより、前記被写体の物理情報を取得し、
前記被写体の３次元情報および前記被写体の物理情報を対応付けることにより、前記被写体の位置、姿勢および形状、ならびに、物理情報を取得することを特徴とする環境認識方法。
【請求項９】
請求項１〜７のうちいずれか１つに記載の環境認識システムを備え、
前記環境システムにより認識された被写体の位置、姿勢および形状ならびに物理情報を用いて、前記被写体を対象として動作することを特徴とする前記装置としてのロボット。
【請求項１】
装置の環境を認識するシステムであって、
前記装置の周辺の様子を撮像することにより、被写体の３次元情報を取得する３次元画像センサと、
前記３次元画像センサに撮像範囲と重なる範囲を撮像することにより、前記被写体の物理情報を取得する２次元画像センサと、
前記３次元画像センサにより取得された前記被写体の３次元情報および前記２次元画像センサにより取得された前記被写体の物理情報を対応付けることにより、前記被写体の位置、姿勢および形状、ならびに、物理情報を取得するように構成されている画像処理要素とを備えていることを特徴とする環境認識システム。
【請求項２】
請求項１記載の環境認識システムにおいて、
前記画像処理要素が、前記３次元画像センサの撮像面により定義される第１画像座標系における複数の第１ピクセルのそれぞれの位置が、前記２次元画像センサの撮像面により定義される第２画像座標系に座標変換された結果としての複数の変換位置のそれぞれを算定し、
前記第２画像座標系における少なくとも３つの変換位置のそれぞれに対応する、前記第１ピクセルが有する前記被写体の３次元情報に基づき、前記第２画像座標系において前記少なくとも３つの変換位置の近傍にある第２ピクセルが有する前記被写体の物理情報に対応付けられる前記被写体の３次元情報を算定するように構成されていることを特徴とする環境認識システム。
【請求項３】
請求項２記載の環境認識システムにおいて、
前記画像処理要素が、前記第２画像座標系における３つの変換位置のそれぞれに対応する、前記第１ピクセルが有する前記被写体の３次元情報としての世界座標系における３つの位置を通る平面を算定し、前記２次元画像センサの主点と、前記第２画像座標系において前記３つの変換位置の近傍にある前記第２ピクセルとを通る直線を算定し、前記世界座標系における前記平面と前記直線との交点位置を、前記第２ピクセルが有する前記被写体の物理情報に対応付けられる前記被写体の３次元情報として算定するように構成されていることを特徴とする環境認識システム。
【請求項４】
請求項２または３記載の環境認識システムにおいて、
前記画像処理要素が、前記第２画像座標系における変換位置の周辺に存在する一または複数の前記第２ピクセルが有する前記被写体の物理情報に基づき、前記変換位置に対応する、前記第１ピクセルが有する前記被写体の３次元情報に対応付けられる前記被写体の物理情報を算定するように構成されていることを特徴とする環境認識システム。
【請求項５】
請求項１記載の環境認識システムにおいて、
前記画像処理要素が、前記３次元画像センサの撮像面により定義される第１画像座標系における複数の第１ピクセルのそれぞれの位置が、前記２次元画像センサの撮像面により定義される第２画像座標系に座標変換された結果としての複数の変換位置のそれぞれを算定し、かつ、
前記第２画像座標系における変換位置の周辺に存在する一または複数の前記第２ピクセルが有する前記被写体の物理情報に基づき、前記変換位置に対応する、前記第１ピクセルが有する前記被写体の３次元情報に対応付けられる前記被写体の物理情報を算定するように構成されていることを特徴とする環境認識システム。
【請求項６】
請求項１〜５のうちいずれか１つに記載の環境認識システムにおいて、
前記２次元画像センサが、前記被写体の物理情報として色情報または温度情報を取得するように構成され、
前記画像処理要素が、前記３次元画像センサにより取得された前記被写体の３次元情報および前記２次元画像センサにより取得された前記被写体の色情報および温度情報のうち一方または両方を対応付けることにより、前記装被写体の位置、姿勢および形状、ならびに、色情報および温度情報のうち一方または両方を取得するように構成されていることを特徴とする環境認識システム。
【請求項７】
請求項１〜６のうちいずれか１つに記載の環境認識システムにおいて、
前記３次元画像センサが前記被写体の３次元情報に加えて前記被写体の輝度情報を取得するように構成され、
前記画像処理要素が、前記３次元画像センサにより取得されたキャリブレーション用物体の輝度情報および距離情報、ならびに、前記２次元画像センサにより取得された前記キャリブレーション用物体の物理情報を用いて、前記３次元画像の座標系、前記２次元画像の座標系および世界座標系の関係を表わすパラメータを求めるキャリブレーションを実行した上で、前記パラメータを用いて前記３次元画像センサにより取得された前記被写体の３次元情報および前記２次元画像センサにより取得された前記被写体の物理情報を対応付けるように構成されていることを特徴とする環境認識システム。
【請求項８】
装置の環境を認識する方法であって、
３次元画像センサにより前記装置の周辺の様子を撮像することにより、被写体の３次元情報を取得し、
前記３次元画像センサに撮像範囲と重なる範囲を２次元画像センサにより撮像することにより、前記被写体の物理情報を取得し、
前記被写体の３次元情報および前記被写体の物理情報を対応付けることにより、前記被写体の位置、姿勢および形状、ならびに、物理情報を取得することを特徴とする環境認識方法。
【請求項９】
請求項１〜７のうちいずれか１つに記載の環境認識システムを備え、
前記環境システムにより認識された被写体の位置、姿勢および形状ならびに物理情報を用いて、前記被写体を対象として動作することを特徴とする前記装置としてのロボット。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【公開番号】特開２０１１−３３４９７（Ｐ２０１１−３３４９７Ａ）
【公開日】平成２３年２月１７日（２０１１．２．１７）
【国際特許分類】
【出願番号】特願２００９−１８０５５９（Ｐ２００９−１８０５５９）
【出願日】平成２１年８月３日（２００９．８．３）
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
【Ｆターム（参考）】
【公開日】平成２３年２月１７日（２０１１．２．１７）
【国際特許分類】
【出願日】平成２１年８月３日（２００９．８．３）
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
【Ｆターム（参考）】
[ Back to top ]