JP2019084645A - 位置情報取得装置およびそれを備えたロボット制御装置 - Google Patents

Abstract

【課題】視覚センサを有するロボットの操作対象物にマーカーを付与することなく、保管部における操作対象物の位置情報を高速かつ精度よく取得可能にする。【解決手段】位置情報取得装置は、視覚センサにより取得された画像から保管部に付与された複数のマーカーの平面座標系における２次元座標を取得し、当該２次元座標から定まる平面領域の重心のセンサ座標系における３次元座標および回転姿勢を取得し、平面領域の画像と、上記重心の３次元座標等に対応した第１テンプレート画像とのテンプレートマッチングにより操作対象物の存否を判別し、存在する操作対象物ごとに、平面領域の画像と、平面視した保管部および操作対象物を示す第２テンプレート画像とのテンプレートマッチングにより得られる相関度が最大になる位置の２次元座標を取得し、当該２次元座標から操作対象物の３次元座標および回転姿勢を取得する。【選択図】図２

Description

本開示は、視覚センサを有するロボットの操作対象物を複数保管可能な保管部における当該操作対象物の位置情報を取得する位置情報取得装置およびそれを備えたロボット制御装置に関する。

従来、対象物体を撮像して画像データを生成する撮像部と、撮像部を可動に支持するロボット本体と、テンプレート情報記憶部と、位置姿勢データ生成部とを含むロボット装置が知られている（例えば、特許文献１参照）。このロボット装置のテンプレート情報記憶部は、リファレンスに対する撮像部からの視線方向ごとに、視線方向に対する面のテンプレート画像データと、ロボット座標系における面の位置および姿勢を表す情報と、ロボット座標系における撮像部および姿勢を表す情報とを含むテンプレート情報を記憶する。そして、位置姿勢データ生成部は、撮像部が生成した画像データに含まれる対象物体の画像から可視である面を検出すると共に、テンプレート情報記憶部から可視である面に対応するテンプレート情報を読み込み、テンプレート情報に基づいてロボット座標系における対象物体の位置および姿勢を表す位置姿勢データを生成する。

また、従来、患者サンプルを効率的に処理するためのシステムとして、サンプルコンテナホルダのサンプルコンテナの１つ以上の画像を得るように構成された画像取得デバイスと、画像取得デバイスに接続された画像分析デバイスと、サンプルコンテナホルダから画像分析デバイスの分析結果に応じたサンプルコンテナを選択可能なロボットアーム（グリッパ）とを含むものも知られている（例えば、特許文献２参照）。このシステムの画像分析デバイスは、プロセッサによって、サンプルコンテナホルダおよびサンプルコンテナホルダの中のサンプルコンテナの画像を分析し、サンプルコンテナの管キャップインジケータが遠心分離インジケータおよび緊急サンプルインジケータの何れであるかを判別する。

特開２０１２−１８５７５２号公報 特表２０１４−５３２８８０号公報

上記特許文献１に記載されたようなテンプレートマッチングを用いることで、ロボット装置の撮像部により取得された画像からロボット座標系における対象物体（操作対象物）の位置および姿勢を求めることができる。更に、ロボットの操作対象物に対して特許文献２に記載された管キャップインジケータのようなマーカーを付与しておくことで、画像取得デバイスにより取得された画像から複数の操作対象物の位置および姿勢をより精度よく求めることができるであろう。しかしながら、操作対象物の数や形状、材質といった特性によっては、上述のようなマーカーを当該操作対象物のすべてに付与することが困難となる。また、複数の操作対象物のそれぞれにマーカーを付与し得たとしても、画像中のマーカーの認識処理における演算負荷や処理速度が増大化してしまい、操作対象物の位置情報を高速かつ精度よく取得することが困難となる。

そこで、本開示は、視覚センサを有するロボットの操作対象物にマーカーを付与することなく、保管部における操作対象物の位置情報を高速かつ精度よく取得可能にすることを主目的とする。

本開示の位置情報取得装置は、視覚センサを有するロボットの操作対象物を複数保管可能な保管部における前記操作対象物の位置情報を取得する位置情報取得装置であって、前記視覚センサにより取得された前記保管部の画像から前記保管部に付与された複数のマーカーの前記画像の平面座標系における２次元座標を取得すると共に、前記複数のマーカーの前記２次元座標から定まる平面領域の重心のセンサ座標系における３次元座標および回転姿勢を取得する第１座標姿勢取得部と、前記視覚センサにより取得された前記画像と、前記複数のマーカーの前記２次元座標とに基づいて、前記平面領域の画像を生成する平面領域画像生成部と、前記操作対象物を全数保管している前記保管部の複数の角度から撮影された複数の画像に基づいて、前記平面領域の前記重心の前記３次元座標および回転姿勢に対応した第１テンプレート画像を生成するテンプレート生成部と、前記平面領域の前記画像と前記第１テンプレート画像とのテンプレートマッチングにより前記操作対象物の存否を判別すると共に、前記保管部に存在する前記操作対象物ごとに、前記平面領域の前記画像と、平面視した前記保管部および前記操作対象物を示す第２テンプレート画像とのテンプレートマッチングにより得られる相関度が最大になる位置の前記平面座標系における２次元座標を取得し、取得した前記２次元座標に基づいて前記操作対象物の前記センサ座標系における３次元座標および回転姿勢を前記位置情報として取得する第２座標姿勢取得部とを含むものである。

本開示の位置情報取得装置の第１座標姿勢取得部は、視覚センサにより取得された保管部の画像から当該保管部に付与された複数のマーカーの当該画像の平面座標系における２次元座標を取得すると共に、当該複数のマーカーの２次元座標から定まる平面領域の重心のセンサ座標系における３次元座標および回転姿勢を取得する。また、平面領域画像生成部は、視覚センサにより取得された画像と、複数のマーカーの２次元座標とに基づいて、当該平面領域の画像を生成する。更に、テンプレート生成部は、操作対象物を全数保管している保管部の複数の角度から撮影された複数の画像に基づいて、平面領域の重心の３次元座標および回転姿勢に対応した第１テンプレート画像を生成する。また、第２座標姿勢取得部は、上記平面領域の画像と第１テンプレート画像とのテンプレートマッチングにより操作対象物の存否を判別する。更に、第２座標姿勢取得部は、保管部に存在する操作対象物ごとに、平面領域の画像と、平面視した保管部および操作対象物を示す第２テンプレート画像とのテンプレートマッチングにより得られる相関度が最大になる位置の平面座標系における２次元座標を取得する。そして、第２座標姿勢取得部は、取得した２次元座標に基づいて操作対象物のセンサ座標系における３次元座標および回転姿勢を位置情報として取得する。

このように、本開示の位置情報取得装置は、保管部に付与された比較的少数のマーカーに基づいて生成された平面領域の画像と、第１および第２テンプレート画像とのテンプレートマッチングを実行することで、操作対象物のセンサ座標系における３次元座標および回転姿勢を取得する。これにより、操作対象物にマーカーを付与する必要がなくなり、視覚センサにより取得された画像中のマーカーの認識処理における演算負荷や処理速度を大幅に低減化することが可能となる。更に、第１および第２テンプレート画像を用いたテンプレートマッチングを実行することで、操作対象物のセンサ座標系における３次元座標および回転姿勢を高精度に取得することができる。この結果、視覚センサを有するロボットの操作対象物にマーカーを付与することなく、保管部における操作対象物の位置情報を高速かつ精度よく取得することが可能となる。

また、前記平面領域画像生成部は、前記視覚センサにより取得された画像を前記複数のマーカーの前記２次元座標に基づくホモグラフィ変換により正規化画像に変換すると共に、前記正規化画像を所定の色空間画像に変換することにより前記平面領域の前記画像を生成するものであってもよい。これにより、第１および第２テンプレート画像を用いたテンプレートマッチングの精度およびロバスト性をより向上させることが可能となる。

更に、前記第２座標姿勢取得部は、前記保管部に存在する前記操作対象物ごとに、前記平面領域の前記画像と、前記相関度が最大になる位置の前記２次元座標とに基づいて、同一円周上に位置するピクセルの輝度値の和を複数算出すると共に、前記輝度値の和が最大になる円周の中心の前記平面座標系における２次元座標を取得し、取得した前記２次元座標から前記操作対象物の前記センサ座標系における３次元座標および回転姿勢を取得するものであってもよい。これにより、上記平面領域における操作対象物の２次元座標をより精度よく取得することができるので、操作対象物のセンサ座標系における３次元座標および回転姿勢をより一層精度よく取得することが可能となる。

また、前記第２座標姿勢取得部は、前記保管部に存在する前記操作対象物ごとに、前記相関度に基づいて該相関度が最大になる位置からの距離が減少するにつれて値が小さくなる第１指標を複数算出し、算出した前記第１指標を視覚的に把握可能な態様で前記保管部を示す画像と共に表示装置に表示させるものであってもよい。これにより、ロボットを用いた操作対象物への処理を管理する作業者に対して、有用な操作対象物の位置情報をわかりやすく提供することが可能となり、ロボットを用いた操作対象物への処理の作業効率をより向上させることができる。

更に、前記第２座標姿勢取得部は、前記保管部に存在する前記操作対象物ごとに、前記ロボットのロボットハンドの現在位置からの移動距離が長いほど小さくなると共に前記第１指標が大きいほど大きくなる第２指標を算出し、算出した前記第２指標に基づいて前記保管部に存在する前記操作対象物への前記ロボットハンドのアクセス順序を設定するものであってもよい。これにより、ロボットを用いた操作対象物への処理の作業効率をより向上させることが可能となる。

また、前記第２座標姿勢取得部は、前記第２指標を視覚的に把握可能な態様で前記保管部を示す画像と共に前記表示装置に表示させるものであってもよい。これにより、ロボットを用いた操作対象物への処理を管理する作業者に対して、ロボットハンドのアクセス順序に関する有用な情報をわかりやすく提供することができるので、ロボットを用いた操作対象物への処理の作業効率をより一層向上させることが可能となる。

更に、前記操作対象物は、開口が上方に位置するように前記保管部により保持される有底筒状の容器であってもよい。すなわち、本開示の位置情報取得装置は、マーカーを付与し難い有底筒状の容器の保管部における位置情報を取得するのに極めて有用である。ただし、本開示における操作対象物は、このような容器に限られるものではない。

本開示のロボット制御装置は、上記何れかの位置情報取得装置を含み、前記第２座標姿勢取得装置により取得された前記操作対象物の前記センサ座標系における３次元座標および回転姿勢に基づいて前記ロボットを制御するものであってもよい。これにより、高速かつ高精度なロボットによる作業を実現することが可能となる。

本開示のロボット制御装置を含むロボット装置を示す概略構成図である。 図１に示すロボット装置の制御ブロック図である。 本開示のロボット制御装置により実行されるルーチンを示すフローチャートである。 本開示のロボット制御装置により実行される他のルーチンを示すフローチャートである。 本開示のロボット制御装置に接続された表示装置に表示される画像を例示する説明図である。

次に、図面を参照しながら、本開示の発明を実施するための形態について説明する。

図１は、ロボット１および本開示のロボット制御装置１０を含むロボット装置ＲＤを示す概略構成図であり、図２は、ロボット装置ＲＤの制御ブロック図である。図示するように、ロボット１は、保管部としての試験管ラック１００に保管されている多数の試験管ＴＴに対して例えば薬品等の投入といった予め定められた処理を自動的に実行するように本開示のロボット制御装置１０によって制御されるものである。ロボット１の操作対象物である試験管ＴＴは、円形の開口部を有する有底円筒状の容器であり、試験管ラック１００は、複数の保持孔１０１を有する。各保持孔１０１は、試験管ラック１００の上面で開口しており、上方から挿入された試験管ＴＴを保持する。また、本実施形態において、試験管ラック１００の上面は、四角形状を呈しており、当該上面には、少なくとも３つ以上の画像マーカーＭが設けられている。本実施形態では、合計４つの画像マーカーＭが試験管ラック１００の上面の四隅に１つずつ設けられており、各画像マーカーＭは、同一または固有の特徴的なテクスチャパターンを有する。

ロボット１は、先端部（手先部）に図示しないロボットハンドが装着される多関節アーム機構２と、当該多関節アーム機構２を駆動するアクチュエータ３（図２参照）と、多関節アーム機構２の先端部に取り付けられた視覚センサ４とを含む。多関節アーム機構２に装着されるロボットアームは、例えば試験管ＴＴ内に薬品等を投入するように構成されたものである。ただし、ロボットアームは、これに限られるものではなく、例えば試験管ＴＴを把持して試験管ラック１００の保持孔１０１から挿脱するように構成されたもの等であってもよい。また、本実施形態において、視覚センサ４は、光学式デジタルカメラであり、ロボット制御装置１０からの撮影指令信号に応じて試験管ラック１００を撮影し、当該試験管ラック１００を示す画像データをロボット制御装置１０に与える。ただし、視覚センサ４は、光学式デジタルカメラ以外の撮像装置であってもよい。

ロボット制御装置１０は、何れも図示しないＣＰＵ，ＲＯＭ，ＲＡＭ、入出力ポート等を有するコンピュータである。図２に示すように、ロボット制御装置１０には、ＣＰＵやＲＯＭ，ＲＡＭといったハードウエアと、ＲＯＭにインストールされた制御プログラムといったソフトウェアとの協働により、平面認識処理部（第１座標姿勢取得部）２０、テンプレート生成部３０、位置情報処理部（第２座標姿勢取得部）４０および動作制御部５０が機能ブロック（モジュール）として構築され、平面認識処理部２０、テンプレート生成部３０および位置情報処理部４０は、本開示の位置情報取得装置を構成する。また、ロボット制御装置１０には、ロボット１による作業に際して各種情報を表示させる液晶モニタ等の表示装置６０が接続されており、ロボット制御装置１０は、当該表示装置６０を制御する図示しない表示制御部を更に含む。

平面認識処理部２０は、それぞれＣＰＵ等のハードウエアとソフトウェアとの協働により構築されるマーカー認識部２１および平面領域推定部２２を含む。マーカー認識部２１は、視覚センサ４からの試験管ラック１００を示す画像データを解析し、当該画像データから各画像マーカーＭの重心のセンサ座標系における３次元座標および回転姿勢を示すベクトルＭ_G ⁱ（以下、単に「３次元座標および回転姿勢」という。ただし、“ｉ”は、画像マーカーＭに付与された識別子であり、本実施形態では、ｉ＝１，…，４である。）を取得する。ここで、センサ座標系は、視覚センサ４の光学系の光軸を座標軸の１つとして含む右手座標系であり、回転姿勢は、当該光軸に対する傾きを示す。更に、マーカー認識部２１は、取得した画像マーカーＭの３次元座標および回転姿勢Ｍ_G ⁱに基づいて当該画像マーカーＭの重心の画像の平面座標系（ＸＹ座標系）における２次元座標を示すベクトル（以下、単に「２次元座標」という。）Ｍ_g ⁱを取得する。また、平面領域推定部２２は、マーカー認識部２１により取得された画像マーカーＭの重心の２次元座標Ｍ_g ⁱに基づいて、画像マーカーＭの２次元座標Ｍ_g ⁱから定まる四角形状の平面領域Ｌ_Mi＾の重心のセンサ座標系における３次元座標および回転姿勢を取得する。

テンプレート生成部３０は、平面認識処理部２０の平面領域推定部２２から上記平面領域Ｌ_Mi＾の重心のセンサ座標系における３次元座標および回転姿勢を入力し、試験管ラック１００における試験管ＴＴの存否判定に用いられるテンプレート画像を学習済みニューラルネットワークにより生成するものである。このため、テンプレート生成部３０は、すべての保持孔１０１に試験管ＴＴが挿入された試験管ラック１００を様々な角度から撮影した多数の画像を格納した画像データベース３１を含む。そして、テンプレート生成部３０は、画像データベース３１に格納された画像に基づいて、平面領域推定部２２により取得された平面領域Ｌ_Mi＾の重心のセンサ座標系における３次元座標および回転姿勢に対応した第１テンプレート画像を生成する。本実施形態において、画像データベース３１に格納される画像は、例えばＨＳＶ色空間で一定の色領域のみを抽出・強調した色空間画像（以下、適宜「ＨＳＶ画像」という。）に変換されている。また、画像データベース３１に格納される画像には、試験管ＴＴが挿入された単一の保持孔１０１の周辺を真上から撮影して得られた画像をＨＳＶ色空間で一定の色領域のみを抽出・強調した第２テンプレート画像（ＨＳＶ画像）が含まれている。

位置情報処理部４０は、それぞれＣＰＵ等のハードウエアとソフトウェアとの協働により構築される平面領域画像生成部４１、存否判定部４２、位置姿勢推定部４３および位置情報演算部４４を含む。平面領域画像生成部４１は、視覚センサ４からの画像データと、平面認識処理部２０により取得された画像マーカーＭの重心の２次元座標Ｍ_g ⁱとに基づいて、上記平面領域Ｌ_Mi＾の画像（平面画像）を生成する。存否判定部４２は、平面領域画像生成部４１により生成された画像とテンプレート生成部３０により生成された第１テンプレート画像とのテンプレートマッチングを実行し、試験管ラック１００における試験管ＴＴの存否を判定する。

位置情報処理部４０の位置姿勢推定部４３は、平面領域画像生成部４１により生成された画像と、テンプレート生成部３０の画像データベース３１に格納された第２テンプレート画像とのテンプレートマッチングを実行し、試験管ラック１００に存在する試験管ＴＴごとに、開口部の中心（重心）の上記画像の平面座標系における２次元座標を取得する。位置情報演算部４４は、位置姿勢推定部４３により取得された２次元座標等に基づいて、試験管ラック１００に存在する試験管ＴＴの開口部の中心のセンサ座標系における３次元座標および回転姿勢といった位置情報を算出する。

動作制御部５０は、それぞれＣＰＵ等のハードウエアとソフトウェアとの協働により構築される位置姿勢補正部５１および関節角度演算部５２を含む。位置姿勢補正部５１は、位置情報処理部４０（位置情報演算部４４）により取得された試験管ＴＴの開口部の中心のセンサ座標系における３次元座標および回転姿勢をロボット１の手先座標系における座標および回転姿勢に変換する。関節角度演算部５２は、位置姿勢補正部５１により取得された座標および回転姿勢に基づいて、多関節アーム機構２の各関節の角度を算出すると共に、算出した各関節の角度に基づいて当該多関節アーム機構２を駆動するアクチュエータ３を制御する。

次に、図３から図５を参照しながら、ロボット制御装置１０における演算処理の手順について説明する。

図３は、ロボット制御装置１０の平面認識処理部２０により実行されるルーチンの一例を示すフローチャートである。平面認識処理部２０は、作業者によるロボット１の作業開始指令に応じて視覚センサ４に撮影指令信号を送信し、当該視覚センサ４から試験管ラック１００を示す画像データを入力する（ステップＳ１００）。以下、ステップＳ１００にて入力される画像データを適宜「初期画像データ」という。次いで、平面認識処理部２０（マーカー認識部２１）は、入力した画像データの解析を行い、試験管ラック１００に付与された複数の画像マーカーＭの少なくとも何れか１つが検出されたか否かを判定する（ステップＳ１１０）。ステップＳ１１０にて試験管ラック１００に付与された画像マーカーＭが検出されなかったと判定した場合（ステップＳ１１０：ＮＯ）、平面認識処理部２０は、視覚センサ４に撮影指令信号を再度送信し、当該視覚センサ４から画像データを再度入力する（ステップＳ１００）。なお、画像データからの画像マーカーＭの検出には、「拡張現実感システム構築ツールARToolKitの開発」（加藤 博一、Technical report of IEICE. PRMU 101(652), 79-86, 2002-02-14）に記載された手法を用いることができる。

ステップＳ１１０にて試験管ラック１００に付与された少なくとも何れか１つの画像マーカーＭが検出されたと判定した場合（ステップＳ１１０：ＹＥＳ）、平面認識処理部２０（マーカー認識部２１）は、検出された画像マーカーＭの重心のセンサ座標系における３次元座標および回転姿勢Ｍ_G ⁱを取得する（ステップＳ１２０）。また、ステップＳ１２０において、平面認識処理部２０（マーカー認識部２１）は、画像マーカーＭの３次元座標および回転姿勢Ｍ_G ⁱに基づいて当該画像マーカーＭの重心の画像の平面座標系（ＸＹ座標系）における２次元座標Ｍ_g ⁱを取得する。２次元座標Ｍ_g ⁱは、画像マーカーＭの３次元座標および回転姿勢Ｍ_G ⁱに基づく画像解析により取得された画像マーカーＭの４つのコーナー部の画像上における２次元座標から求められる。

更に、ステップＳ１２０において、平面認識処理部２０（平面領域推定部２２）は、画像マーカーＭの重心の２次元座標Ｍ_g ⁱに基づいて、画像マーカーＭの２次元座標Ｍ_g ⁱから定まる平面領域Ｌ_Mi＾の重心のセンサ座標系における３次元座標および回転姿勢を取得する。マーカー認識部２１によって試験管ラック１００の全画像マーカーＭ（本実施形態では、４つ）の重心の２次元座標Ｍ_g ⁱが取得されている場合、平面領域Ｌ_Mi＾の重心のセンサ座標系における３次元座標および回転姿勢は、すべての画像マーカーＭの重心の２次元座標Ｍ_g ⁱから定まる四角形状の平面領域Ｌ_Mi＾の重心をマーカー認識部２１により取得された各画像マーカーＭの３次元座標および回転姿勢Ｍ_G ⁱに基づいてセンサ座標系に射影することにより求められる。また、マーカー認識部２１によって試験管ラック１００の全画像マーカーＭの重心の２次元座標Ｍ_g ⁱが取得されていない場合、平面領域Ｌ_Mi＾の重心のセンサ座標系における３次元座標および回転姿勢は、予め判明している試験管ラック１００の上面の平面形状と、マーカー認識部２１により取得された画像マーカーＭの重心の３次元座標および回転姿勢Ｍ_G ⁱ並びに２次元座標Ｍ_g ⁱとに基づいて推定される。

ステップＳ１２０の処理の後、平面認識処理部２０は、視覚センサ４に撮影指令信号を再度送信し、当該視覚センサ４から画像データを入力する（ステップＳ１３０）。以下、ステップＳ１３０にて入力される画像データを適宜「次画像データ」という。更に、平面認識処理部２０（マーカー認識部２１）は、ステップＳ１３０にて入力した次画像データと、ステップＳ１２０にて取得した画像マーカーＭの重心の３次元座標および回転姿勢Ｍ_G ⁱとに基づくパーティクルフィルタ処理により、次画像データにおける画像マーカーＭの重心の３次元座標および回転姿勢Ｍ_G ⁱを推定する（ステップＳ１４０）。ステップＳ１３０にて用いられるパーティクルフィルタは、「複合現実感のための２次元矩形マーカーを用いたカメラ追跡の精度向上」（植松裕子ら、「画像の認識・理解シンポジウム（MIRU2007）」２００７年７月）に記載されたものを並進運動に関する推定が可能となるように改変したものである。また、ステップＳ１４０において、平面認識処理部２０は、ステップＳ１２０と同様にして、推定した次画像データにおける画像マーカーＭの重心の３次元座標および回転姿勢Ｍ_G ⁱに基づいて次画像データにおける画像マーカーＭの重心の２次元座標Ｍ_g ⁱと、当該２次元座標Ｍ_g ⁱから定まる平面領域Ｌ_Mi＾の重心のセンサ座標系における３次元座標および回転姿勢とを取得する。

ステップＳ１４０にて上記次画像データにおける画像マーカーＭの重心の三次元的位置および回転姿勢Ｍ_G ⁱを推定した後、平面認識処理部２０（マーカー認識部２１）は、ステップＳ１３０にて取得した次画像データにおける画像マーカーＭの重心の３次元座標および回転姿勢Ｍ_G ⁱから尤度を算出し（ステップＳ１５０）、算出した尤度が閾値以上であるか否かを判定する（ステップＳ１６０）。ステップＳ１５０において、尤度は、画像マーカーＭの輪郭情報に加えて、画像マーカーＭと外部環境との関係や、画像マーカーＭのテクスチャパターンと入力画像の類似度を考慮した画像マーカーＭの輪郭に対して垂直な方向のテクスチャパターン検出評価関数を用いて算出される。また、ステップＳ１６０にて用いられる閾値は、次画像データにおける画像マーカーＭの重心の３次元座標および回転姿勢Ｍ_G ⁱの推定に要する時間に応じて変更されるとよい。

ステップＳ１６０にて尤度が閾値以上であると判定した場合（ステップＳ１６０：ＹＥＳ）、平面認識処理部２０は、ステップＳ１４０にて推定された次画像データにおける画像マーカーＭの重心の３次元座標および回転姿勢Ｍ_G ⁱ、画像マーカーＭの重心の２次元座標Ｍ_g ⁱ、並びに当該２次元座標Ｍ_g ⁱから定まる平面領域Ｌ_Mi＾の重心のセンサ座標系における３次元座標および回転姿勢を図示しないＲＡＭに格納する。また、ステップＳ１６０にて尤度が閾値未満であると判定した場合（ステップＳ１６０：ＮＯ）、平面認識処理部２０は、ステップＳ１２０にて取得した初期画像データにおける画像マーカーＭの重心の３次元座標および回転姿勢Ｍ_G ⁱ、画像マーカーＭの重心の２次元座標Ｍ_g ⁱ、並びに当該２次元座標Ｍ_g ⁱから定まる平面領域Ｌ_Mi＾の重心のセンサ座標系における３次元座標および回転姿勢を図示しないＲＡＭに格納する。

ステップＳ１７０またはＳ１８０の処理の後、平面認識処理部２０は、作業者によりロボット１の動作開始指令がなされたか否かを判定し（ステップＳ１９０）、当該動作開始指令がなされていないと判定した場合、（ステップＳ１９０：ＮＯ）上述のステップＳ１００以降の処理を再度実行する。これに対して、ステップＳ１９０にて作業者によりロボット１の動作開始指令がなされたと判定した場合（ステップＳ１９０：ＹＥＳ）、その時点で図３のルーチンを終了させる。

図４は、作業者によるロボット１の動作開始指令に応じて、ロボット制御装置１０の位置情報処理部４０により実行されるルーチンの一例を示すフローチャートである。図４のルーチンの開始に際し、位置情報処理部４０（平面領域画像生成部４１）は、平面認識処理部２０により図示しないＲＡＭに格納された画像マーカーＭの重心の３次元座標および回転姿勢Ｍ_G ⁱ、画像マーカーＭの重心の２次元座標Ｍ_g ⁱ、並びに当該２次元座標Ｍ_g ⁱから定まる平面領域Ｌ_Mi＾の重心のセンサ座標系における３次元座標および回転姿勢を入力する（ステップＳ２００）。次いで、処理に必要なデータを取得し得たか否かを判定し（ステップＳ２１０）、処理に必要なデータが取得されていない場合（ステップＳ２１０：ＮＯ）、その時点で図４のルーチンを終了させ、図３のルーチンの再実行を指示する。

ステップＳ２１０にて処理に必要なデータを取得し得たと判定した場合（ステップＳ２１０：ＹＥＳ）、位置情報処理部４０（平面領域画像生成部４１）は、ステップＳ２００にて読み出した画像マーカーＭの重心の２次元座標Ｍ_g ⁱに対応した初期画像データまたは次画像データを当該２次元座標Ｍ_g ⁱに基づくホモグラフィ変換により正規化画像に変換する（ステップＳ２２０）。ステップＳ２２０にて用いられるホモグラフィ変換としては、「単眼動画像からの可展面物体の３次元変形とその展開テクスチャの復元」（柴山 裕樹ら、「画像の認識・理解シンポジウム（MIRU2011）」２０１１年７月）に記載されたものを用いることができる。更に、位置情報処理部４０（平面領域画像生成部４１）は、ステップＳ２２０にて得られた正規化画像をＨＳＶ画像に変換することにより平面領域Ｌ_Mi＾の画像を生成する（ステップＳ２３０）。

続いて、位置情報処理部４０（存否判定部４２）は、ステップＳ２００にて読み出した平面領域Ｌ_Mi＾の重心のセンサ座標系における３次元座標および回転姿勢に対応した第１テンプレート画像の生成をテンプレート生成部３０に要求し、当該テンプレート生成部３０により生成された第１テンプレート画像を取得する（ステップＳ２４０）。第１テンプレート画像を取得した後、位置情報処理部４０（存否判定部４２）は、平面領域Ｌ_Mi＾のＨＳＶ画像と第１テンプレート画像とのテンプレートマッチングを実行する（ステップＳ２５０）。ステップＳ２５０では、平面領域Ｌ_Mi＾のＨＳＶ画像を第１テンプレート画像により走査しながら、両画像の輝度値の平均値を考慮した明るさの変動に強い正規化相互相関値を求めると共に、当該正規化相互相関値と閾値を比較することにより試験管ＴＴの存否が判別される。かかるテンプレートマッチングに際して、平面領域Ｌ_Mi＾の画像と第１テンプレート画像とをＨＳＶ色空間で一定の色領域のみを抽出・強調したＨＳＶ画像とすることで、ノイズの影響を低減化することができる。

ステップＳ２５０におけるテンプレートマッチングの実行後、位置情報処理部４０（存否判定部４２）は、試験管ラック１００に試験管ＴＴが存在しているか否かを判定する（ステップＳ２６０）。ステップＳ２６０にて試験管ラック１００に試験管ＴＴが存在していないと判定した場合（ステップＳ２６０：ＮＯ）、位置情報処理部４０（存否判定部４２）は、その時点で図４のルーチンを終了させる。この場合、位置情報処理部４０は、図４のルーチンの終了後、表示装置６０に所定のエラー表示を表示させる等の予め定められた処理を実行する。

一方、ステップＳ２６０にて試験管ラック１００に試験管ＴＴが存在していると判定した場合（ステップＳ２６０：ＹＥＳ）、位置情報処理部４０（位置姿勢推定部４３）は、テンプレート生成部３０から、平面視した試験管ＴＴおよび試験管ラック１００の一部を示す上述の第２テンプレート画像を入力し（ステップＳ２７０）、平面領域Ｌ_Mi＾のＨＳＶ画像と第２テンプレート画像とのテンプレートマッチングを実行する（ステップＳ２８０）。ステップＳ２８０では、存在が確認された試験管ＴＴごとに、平面領域Ｌ_Mi＾のＨＳＶ画像を第２テンプレート画像により走査しながら、両画像の相関度ｐが求められる。

ステップＳ２８０のテンプレートマッチングにより上記相関度ｐを得た後、位置情報処理部４０（位置姿勢推定部４３）は、試験管ラック１００に存在する試験管ＴＴごとに、平面領域Ｌ_Mi＾のＨＳＶ画像と第２テンプレート画像との相関度ｐが最大になる位置の上記平面座標系における２次元座標を取得する（ステップＳ２９０）。更に、位置情報処理部４０（位置姿勢推定部４３）は、試験管ラック１００に存在する試験管ＴＴごとに、ステップＳ２９０にて取得した２次元座標を含む所定の２次元領域内で円の中心位置と半径とを変化させながら同一円周上に位置するピクセルの輝度値の和を複数算出すると共に、輝度値の和が最大になる円周の中心の上記平面座標系における２次元座標および半径を取得し、取得した２次元座標を当該試験管ＴＴの開口部中心の２次元座標とする（ステップＳ３００）。これにより、平面領域Ｌ_Mi＾のＨＳＶ画像と第２テンプレート画像とのテンプレートマッチングにより得られた相関度ｐに基づく２次元座標が画像の輝度値により補正された上で試験管ＴＴの開口部中心の２次元座標として取得されることになる。

そして、位置情報処理部４０（位置情報演算部４４）は、ステップＳ３００にて取得した試験管ＴＴの開口部中心の２次元座標をマーカー認識部２１により取得された各画像マーカーＭの３次元座標および回転姿勢Ｍ_G ⁱに基づいてセンサ座標系に射影することにより、当該開口部中心の３次元座標および回転姿勢を算出する（ステップＳ３１０）。更に、位置情報処理部４０（位置情報演算部４４）は、作業者の便に供するための第１指標Ｐ（ｒ）を算出する（ステップＳ３２０）。ステップＳ３２０において、第１指標Ｐは、試験管ＴＴの開口部中心でステップＳ２８０のテンプレートマッチングにより得られた相関度ｐが最大になるとの仮定のもと、当該開口部中心からの距離ｒが減少するにつれて値が小さくなるように、次式（１）に従って試験管ラック１００に存在する試験管ＴＴごとに複数算出される。本実施形態において、第１指標Ｐの算出に際し、距離ｒは、ステップＳ３００にて取得された輝度値の和が最大になる円周の半径を最大として、所定間隔で変化させられる。

Ｐ＝ｐ−α×ｒ …（１）

また、位置情報処理部４０（位置情報演算部４４）は、ロボット１のロボットハンドの現在位置からの移動距離Ｌと上記第１指標Ｐとに基づいてロボット１による処理の作業効率を向上させるための第２指標Ｑを算出する（ステップＳ３３０）。ステップＳ３３０において、第２指標Ｑは、試験管ラック１００に存在する試験管ＴＴの開口部中心ごとに、ロボット１のロボットハンドの現在位置からの移動距離Ｌが長いほど小さくなると共に上記第１指標Ｐが大きいほど大きくなるように次式（２）に従って算出される。式（２）において、係数ａは、予め定められた負の一定値であり、係数ｂは、予め定められた正の一定値である。ステップＳ３３０では、試験管ラック１００に存在する試験管ＴＴごとに、ロボットハンドの停止位置（待機位置）を基準として開口部中心における第２指標Ｑが算出される。更に、ステップＳ３３０では、算出された第２指標Ｑを最大にする開口部中心の２次元座標に対応した試験管ＴＴがロボットハンドの最初の移動先として定められ、以後、第２指標Ｑの大きい順に、試験管ラック１００に存在する試験管ＴＴへのロボットハンドのアクセス順序が定められる。

Ｑ＝ａ×Ｌ＋ｂ×Ｐ …（２）

ステップＳ３３０の処理の完了後、位置情報処理部４０（位置情報演算部４４）は、算出した試験管ラック１００に存在する試験管ＴＴ（開口部中心）の３次元座標および回転姿勢やロボットハンドのアクセス順序といった位置情報を動作制御部５０に送信すると共に、試験管ラック１００に存在する試験管ＴＴ（開口部中心）の２次元座標や第１指標Ｐおよび第２指標Ｑを表示装置６０の表示制御部（図示省略）に送信し（ステップＳ３４０）、図４のルーチンを終了させる。動作制御部５０は、位置情報処理部４０から受け取った３次元座標および回転姿勢に基づいて多関節アーム機構２の各関節の角度を算出し、上述のアクセス順序に従って多関節アーム機構２を駆動するアクチュエータ３を制御する。

また、位置情報処理部４０から位置情報としての第１および第２指標Ｐ，Ｑ等を受け取った図示しない表示制御部は、第１および第２指標Ｐ，Ｑを視覚的に把握可能な態様で試験管ラック１００および試験管ＴＴを示す画像（例えば、上述の平面領域Ｌ_Mi＾の画像）と共に表示装置６０に表示させる。本実施形態において、第１指標Ｐの値は、図５に示すように、値が大きいほど赤みが増し、小さいほど青みが増すように色温度を用いて試験管ラック１００を示す画像上の試験管ＴＴが存在する箇所に表示される。同様に、第２指標Ｑも色温度に対応づけられ、試験管ラック１００を示す画像上の試験管ＴＴが存在する箇所の表示色は、第２指標Ｑの値が大きいほど赤みが増し、小さいほど青みが増すように設定される。

以上説明したように、ロボット制御装置１０の平面認識処理部２０（第１座標姿勢取得部）は、視覚センサ４により取得された試験管ラック１００の画像から当該試験管ラック１００に付与された複数の画像マーカーＭの当該画像の平面座標系における２次元座標Ｍ_g ⁱを取得すると共に、当該複数のマーカーの２次元座標から定まる平面領域Ｌ_Mi＾の重心のセンサ座標系における３次元座標および回転姿勢を取得する（図３のステップＳ１００〜Ｓ１９０）。また、位置情報処理部４０（第２座標姿勢取得部）の平面領域画像生成部４１は、視覚センサ４により取得された画像と、複数の画像マーカーＭの２次元座標Ｍ_g ⁱとに基づいて、平面領域Ｌ_Mi＾の画像（ＨＳＶ画像）を生成する（図４のステップＳ２２０，Ｓ２３０）。

更に、テンプレート生成部３０は、位置情報処理部４０の存否判定部４２からの要求に応じて、すべての保持孔１０１に試験管ＴＴが挿入された試験管ラック１００を様々な角度から撮影した多数の画像に基づいて、平面領域Ｌ_Mi＾の重心のセンサ座標系における３次元座標および回転姿勢に対応した第１テンプレート画像を生成する（図４のステップＳ２４０）。また、位置情報処理部４０の存否判定部４２は、平面領域Ｌ_Mi＾のＨＳＶ画像と第１テンプレート画像とのテンプレートマッチングにより試験管ラック１００における試験管ＴＴの存否を判別する（図４のステップＳ２５０）。更に、位置情報処理部４０の位置姿勢推定部４３は、存在が確認された試験管ＴＴごとに、平面領域Ｌ_Mi＾のＨＳＶ画像と、平面視した試験管ＴＴおよび試験管ラック１００の一部を示す第２テンプレート画像とのテンプレートマッチングを実行し（図４のステップＳ２７０，Ｓ２８０）、当該テンプレートマッチングにより得られる相関度ｐが最大になる位置の上記平面座標系における２次元座標を取得する（図４のステップＳ２９０，Ｓ３００）。そして、位置情報処理部４０の位置情報演算部４４は、取得した２次元座標に基づいて試験管ＴＴのセンサ座標系における３次元座標および回転姿勢を位置情報として取得する（図４のステップＳ３１０）。

このように、ロボット制御装置１０は、試験管ラック１００に付与された比較的少数の画像マーカーＭに基づいて生成された平面領域Ｌ_Mi＾の画像と、第１および第２テンプレート画像とのテンプレートマッチングを実行することで、試験管ＴＴのセンサ座標系における３次元座標および回転姿勢を取得する。これにより、各試験管ＴＴに画像マーカーＭを付与する必要がなくなり、視覚センサ４により取得された画像中の画像マーカーＭの認識処理における演算負荷や処理速度を大幅に低減化することが可能となる。更に、上述のような第１および第２テンプレート画像を用いたテンプレートマッチングを実行することで、試験管ＴＴのセンサ座標系における３次元座標および回転姿勢を高精度に取得することができる。この結果、視覚センサ４を有するロボット１の操作対象物である試験管ＴＴに画像マーカーＭを付与することなく、試験管ラック１００における試験管ＴＴの位置情報を高速かつ精度よく取得することが可能となり、高速かつ高精度なロボットによる作業を実現することができる。

また、位置情報処理部４０の平面領域画像生成部４１は、視覚センサ４により取得された画像データを複数の画像マーカーＭの２次元座標Ｍ_g ⁱに基づくホモグラフィ変換により正規化画像に変換すると共に、正規化画像をＨＳＶ画像に変換することにより平面領域Ｌ_Mi＾の画像を生成する（図４のステップＳ２２０，Ｓ２３０）。これにより、第１および第２テンプレート画像を用いたテンプレートマッチングの精度およびロバスト性をより向上させることが可能となる。

更に、位置情報処理部４０の位置姿勢推定部４３は、試験管ラック１００に存在する試験管ＴＴごとに、平面領域Ｌ_Mi＾のＨＳＶ画像と、上記相関度ｐが最大になる位置の２次元座標とに基づいて、同一円周上に位置するピクセルの輝度値の和を複数算出すると共に、輝度値の和が最大になる円周の中心の平面座標系における２次元座標を取得する（図４のステップＳ３００）。そして、位置情報処理部４０の位置情報演算部４４は、位置姿勢推定部４３により取得された２次元座標から試験管ＴＴのセンサ座標系における３次元座標および回転姿勢を取得する（図４のステップＳ３１０）。これにより、平面領域Ｌ_Mi＾における試験管ＴＴの２次元座標をより精度よく取得することができるので、当該試験管ＴＴのセンサ座標系における３次元座標および回転姿勢をより一層精度よく取得することが可能となる。なお、画像データベース３１に格納される画像や、平面領域Ｌ_Mi＾の画像、第１および第２テンプレート画像は、例えばＲＧＢ画像やグレースケール画像といったＨＳＶ画像以外の色空間画像であってもよい。

また、ロボット制御装置１０の位置情報処理部４０は、試験管ラック１００に存在する試験管ＴＴごとに、相関度ｐに基づいて当該相関度ｐが最大になる位置からの距離が減少するにつれて値が小さくなる第１指標Ｐを複数算出し（図４のステップＳ３２０）、算出した第１指標Ｐを視覚的に把握可能な色温度を用いて試験管ラック１００を示す画像と共に表示装置６０に表示させる（図４のステップＳ３４０）。これにより、ロボット１を用いた試験管ＴＴへの処理を管理する作業者に対して、有用な当該試験管ＴＴの位置情報をわかりやすく提供することが可能となり、ロボット１を用いた処理の作業効率をより向上させることが可能となる。

更に、ロボット制御装置１０の位置情報処理部４０は、試験管ラック１００に存在する操試験管ＴＴごとに、ロボット１のロボットハンドの現在位置からの移動距離Ｌが長いほど小さくなると共に第１指標Ｐが大きいほど大きくなる第２指標Ｑを算出し、算出した第２指標Ｑに基づいて試験管ラック１００に存在する試験管ＴＴへのロボットハンドのアクセス順序を設定する（図４のステップＳ３３０）。これにより、ロボット１を用いた試験管ＴＴへの処理の作業効率をより向上させることが可能となる。

加えて、ロボット制御装置１０の位置情報処理部４０は、第２指標Ｑを視覚的に把握可能な色温度を用いて試験管ラック１００を示す画像と共に表示装置６０に表示させる（図４のステップＳ３４０）。これにより、ロボット１を用いた試験管ＴＴへの処理を管理する作業者に対して、ロボットハンドのアクセス順序に関する有用な情報をわかりやすく提供することができるので、ロボット１を用いた処理の作業効率をより一層向上させることが可能となる。

そして、ロボット制御装置１０によれば、画像マーカーＭを付与し難い有底筒状の試験管ＴＴの試験管ラック１００における位置情報を高速かつ精度よく取得することが可能となる。ただし、ロボット制御装置１０により制御されるロボット１の操作対象物は、有底筒状の容器に限られるものではない。

なお、本開示の発明は上記実施形態に何ら限定されるものではなく、本開示の外延の範囲内において様々な変更をなし得ることはいうまでもない。更に、上記実施形態は、あくまで発明の概要の欄に記載された発明の具体的な一形態に過ぎず、発明の概要の欄に記載された発明の要素を限定するものではない。

本開示の発明は、ロボットを利用する産業等において利用可能である。

１ ロボット、２ 多関節アーム機構、３ アクチュエータ、４ 視覚センサ、１０ ロボット制御装置、２０ 平面認識処理部、２１ マーカー認識部、２２ 平面領域推定部、３０ テンプレート生成部、３１ 画像データベース、４０ 位置情報処理部、４１ 平面領域画像生成部、４２ 存否判定部、４３ 位置姿勢推定部、４４ 位置情報演算部、５０ 動作制御部、５１ 位置姿勢補正部、５２ 関節角度演算部、６０ 表示装置、１００ 試験管ラック、１０１ 保持孔、Ｍ 画像マーカー、ＲＤ ロボット装置、ＴＴ 試験管。

Claims (8)

1. 視覚センサを有するロボットの操作対象物を複数保管可能な保管部における前記操作対象物の位置情報を取得する位置情報取得装置であって、
   前記視覚センサにより取得された前記保管部の画像から前記保管部に付与された複数のマーカーの前記画像の平面座標系における２次元座標を取得すると共に、前記複数のマーカーの前記２次元座標から定まる平面領域の重心のセンサ座標系における３次元座標および回転姿勢を取得する第１座標姿勢取得部と、
   前記視覚センサにより取得された前記画像と、前記複数のマーカーの前記２次元座標とに基づいて、前記平面領域の画像を生成する平面領域画像生成部と、
   前記操作対象物を全数保管している前記保管部の複数の角度から撮影された複数の画像に基づいて、前記平面領域の前記重心の前記３次元座標および回転姿勢に対応した第１テンプレート画像を生成するテンプレート生成部と、
   前記平面領域の前記画像と前記第１テンプレート画像とのテンプレートマッチングにより前記操作対象物の存否を判別すると共に、前記保管部に存在する前記操作対象物ごとに、前記平面領域の前記画像と、平面視した前記保管部および前記操作対象物を示す第２テンプレート画像とのテンプレートマッチングにより得られる相関度が最大になる位置の前記平面座標系における２次元座標を取得し、取得した前記２次元座標に基づいて前記操作対象物の前記センサ座標系における３次元座標および回転姿勢を前記位置情報として取得する第２座標姿勢取得部と、
   を備える位置情報取得装置。
2. 請求項１に記載の位置情報取得装置において、
   前記平面領域画像生成部は、前記視覚センサにより取得された画像を前記複数のマーカーの前記２次元座標に基づくホモグラフィ変換により正規化画像に変換すると共に、前記正規化画像を所定の色空間画像に変換することにより前記平面領域の前記画像を生成する位置情報取得装置。
3. 請求項１または２に記載の位置情報取得装置において、
   前記第２座標姿勢取得部は、前記保管部に存在する前記操作対象物ごとに、前記平面領域の前記画像と、前記相関度が最大になる位置の前記２次元座標とに基づいて、同一円周上に位置するピクセルの輝度値の和を複数算出すると共に、前記輝度値の和が最大になる円周の中心の前記平面座標系における２次元座標を取得し、取得した前記２次元座標から前記操作対象物の前記センサ座標系における３次元座標および回転姿勢を取得する位置情報取得装置。
4. 請求項１から３の何れか一項に記載の位置情報取得装置において、
   前記第２座標姿勢取得部は、前記保管部に存在する前記操作対象物ごとに、前記相関度に基づいて該相関度が最大になる位置からの距離が減少するにつれて値が小さくなる第１指標を複数算出し、算出した前記第１指標を視覚的に把握可能な態様で前記保管部を示す画像と共に表示装置に表示させる位置情報取得装置。
5. 請求項４に記載の位置情報取得装置において、
   前記第２座標姿勢取得部は、前記保管部に存在する前記操作対象物ごとに、前記ロボットのロボットハンドの現在位置からの移動距離が長いほど小さくなると共に前記第１指標が大きいほど大きくなる第２指標を算出し、算出した前記第２指標に基づいて前記保管部に存在する前記操作対象物への前記ロボットハンドのアクセス順序を設定する位置情報取得装置。
6. 請求項５に記載の位置情報取得装置において、
   前記第２座標姿勢取得部は、前記第２指標を視覚的に把握可能な態様で前記保管部を示す画像と共に前記表示装置に表示させる位置情報取得装置。
7. 請求項１から６の何れか一項に記載の位置情報取得装置において、
   前記操作対象物は、開口が上方に位置するように前記保管部により保持される有底筒状の容器である位置情報取得装置。
8. 請求項１から７の何れか一項に記載の位置情報取得装置を含むロボット制御装置であって、
   前記第２座標姿勢取得装置により取得された前記操作対象物の前記センサ座標系における３次元座標および回転姿勢に基づいて前記ロボットを制御するロボット制御装置。