JP2017124448A - 相対データ較正装置と方法 - Google Patents

Abstract

【課題】外部センサの観測データに基づいてロボットアームを制御する場合に、ロボットアームの作業中に、外部センサとロボットアームとの相対位置姿勢をキャリブレーションできるようにする。【解決手段】相対データ較正装置は、ロボットアーム３に設けられたアームマーカ１３と、外部センサ５の観測により取得されたアームマーカ１３の観測データに基づいて相対位置姿勢データを較正する較正処理部とを備える。【選択図】図１

Description

本発明は、外部センサの観測データに基づいてロボットアームを制御する場合におけるキャリブレーション技術に関する。より詳しくは、外部センサとロボットアームとの相対位置と相対姿勢の一方または両方を表す相対データをロボットアームの動作制御に用いる場合に、相対データを較正（キャリブレーション）する技術に関する。

ロボットアームは、様々な作業を行うために用いられている。ロボットアームが行う作業としては、例えば、次の（１）〜（３）がある。
（１）部品の組み付け
ロボットアームの先端部には、部品を把持する把持装置が取り付けられている。ロボットアームは、把持装置により部品を把持して、この部品を、対象物（例えば、自動車の車体）に組み付けるように動作する。
（２）対象物の搬送
ロボットアームは、上記と同様の把持装置により、ある場所に置かれている対象物を、把持して搬送し別の場所に置く。
（３）溶接
ロボットアームの先端部には、溶接トーチが取り付けられている。ロボットアームは、溶接トーチにより金属部材同士を溶接するように動作する。

ロボットアームに作業を行わせる制御において、ロボットアームの外部に設置した外部センサ（外部カメラ）が用いられる場合がある。この場合に、外部カメラとロボットアームとの間における相対位置と相対姿勢を表す相対データが予め求められる。

その上で、ロボットアームは、次のように制御されて作業を行う。外部カメラは、ロボットアームの動作領域を撮像することにより、この領域の画像データを得る。得られた画像データに基づいて、この領域内の計測対象（例えば、ロボットアームが作業する上述の対象物、障害物、目印など）の位置や姿勢を求める。求めた計測対象の位置や姿勢と、上述の相対データと、ロボットアームの現在位置および現在姿勢の計測値とに基づいて、ロボットアームが制御される。例えば、計測対象が上記対象物である場合には、求めた対象物の位置および姿勢に基づいて、ロボットアームが対象物に作業を行うように制御される。また、計測対象が障害物である場合には、求めた障害物の位置に基づいて、ロボットアームが障害物を回避するように制御される。計測対象が目印である場合には、求めた目印の位置や姿勢を基準にロボットアームの位置や姿勢が制御される。

上述の相対データを予め求める方法が、例えば下記の特許文献１に記載されている。特許文献１では、次のように相対データを求めている。ロボットアームにおける先端部の把持装置に指し棒を把持させる。指し棒の先端が長方形を描くようにロボットアームを動作させる。この時に、指し棒の動作を、外部カメラで撮像することにより画像データを取得する。この画像データに基づいて、指し棒の先端が移動した長方形軌跡の各頂点の位置を求める。これらの位置から、外部カメラとロボットアームとの相対位置姿勢関係（相対データ）を求める。

特許第３４０２０２１号公報

しかし、予め求めた相対データが、実際の値からずれてしまう場合がある。この原因としては、ロボットアームや外部カメラの配置変更、外部カメラへの人の接触、他の機械からの振動、地震、ロボットアームの経年劣化などがある。

このように予め求めた相対データが実際の値からずれた場合には、相対データを較正する必要がある。上述の原因がロボットアームの作業中に発生した場合には、較正を行うために、ロボットアームの作業を中断する。そのため、ロボットアームの作業能率が低下してしまう。

そこで、本発明の目的は、ロボットアームの作業中に上述の相対データが実際の値からずれた場合に、ロボットアームの作業を中断させることなく、相対データを較正できる手段を提供することにある。

上述の目的を達成するため、本発明によると、ロボットアームの動作領域を観測することにより観測データを取得する外部センサと、
前記ロボットアームと外部センサとの相対位置および相対姿勢の一方または両方を表す相対データを記憶する記憶部と、
前記観測データと前記相対データとに基づいて、前記ロボットアームが作業を行うように前記ロボットアームを制御するアーム制御部とを備える制御装置のために設けられる相対データ較正装置であって、
前記ロボットアームに設けられたアームマーカと、
前記外部センサの観測により取得された前記アームマーカの観測データに基づいて、前記相対データを較正する較正処理部と、を備える、相対データ較正装置が提供される。

また、上述の目的を達成するため、本発明によると、外部センサの観測データと、前記ロボットアームと前記外部センサとの相対位置および相対姿勢の一方または両方を表す相対データとに基づいて前記ロボットアームを制御する場合に、前記相対データを較正する方法であって、
（Ａ）前記ロボットアームにアームマーカを設け、
（Ｂ）前記外部センサにより、前記アームマーカを観測して前記アームマーカの観測データを取得し、
（Ｃ）前記アームマーカの前記観測データに基づいて、前記相対データを較正し、
前記ロボットアームが作業をするための動作をしている作業中に、前記（Ｂ）と（Ｃ）を行う、相対データ較正方法が提供される。

上述した本発明では、ロボットアームの作業中に、外部センサは、アームマーカを観測してその観測データを取得できる。この観測データに基づいて、較正処理装置は相対データを較正する。したがって、ロボットアームの作業中に、相対データを較正できる。

本発明が適用可能なロボットアームとその周辺を示す。 ロボットアームの制御装置と、本発明の実施形態による相対データ較正装置とを示すブロック図である。 外部センサとアーム側センサの正面図である。 本発明の実施形態による相対データ較正方法と、ロボットアームの制御処理とを示すフローチャートである。

本発明の実施形態を図面に基づいて説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。

図１は、本発明が適用可能なロボットアーム３とその周辺を示す。図２は、ロボットアーム３の制御装置２０と、本発明の実施形態による相対データ較正装置１０とを示すブロック図である。

（ロボットアームの作業）
ロボットアーム３は、対象物１に作業を行う。図１の例では、ロボットアーム３は、搬送装置により吊り下げられ且つ搬送されて来る対象物１を把持し、指定の場所へ置く作業を行う。ただし、ロボットアーム３は他の作業を行ってもよい。この作業は、例えば、上述した（１）〜（３）のいずれかの作業であってもよい。

（制御装置の構成）
ロボットアーム３は、制御装置２０に制御されることにより上述の作業を行う。制御装置２０は、図２のように、外部センサ５（図１も参照）と記憶部７とアーム計測装置９とアーム制御部１１を備える。

外部センサ５は、ロボットアーム３の外部に設けられる。外部センサ５は、ロボットアーム３の動作領域を観測することにより観測データを取得する。この観測データは、対象物１の観測データであってよい。本実施形態では、外部センサ５は、カメラであり、上記観測データとして、対象物１を含む上記動作領域の画像データを取得する。カメラ５は、例えば、路面または地面に置かれたカメラ支持体（例えば三脚）に取り付けられている。この場合、好ましくは、カメラ支持体は、路面または地面に固定される。外部カメラ５の数は、１つであっても複数であってもよい。以下において、外部センサ５が取得した観測データが、ロボットアーム３の制御に用いられる場合には、観測データをＤ１で示す。

記憶部７は、ロボットアーム３と外部センサ５との相対位置および相対姿勢の一方または両方を表す相対データを記憶する。なお、以下において、相対位置および相対姿勢の一方または両方を、単に相対位置姿勢といい、位置および姿勢の一方または両方を、単に位置姿勢という。

アーム計測装置９は、ロボットアーム３の位置姿勢を計測により求める。本実施形態では、ロボットアーム３は、複数のリンク３ａ〜３ｅが互いに対して回転可能にまたは伸縮可能に直列に連結されることにより構成される。アーム計測装置９は、これらのリンク３ａ〜３ｅの互いに対する回転量または伸縮量を計測し、これらの計測値に基づいてロボットアーム３の先端部３ｆの位置姿勢を求める。この場合、アーム計測装置９は、例えば、各リンク３ａ〜３ｅを駆動するモータの回転角を検出するロータリーエンコーダを用いるものであってよい。

アーム制御部１１は、外部センサ５が取得した観測データＤ１と、アーム計測装置９が求めたロボットアーム３（本実施形態では、先端部３ｆ）の位置姿勢と、記憶部７の相対データとに基づいて、ロボットアーム３が作業を行うようにロボットアーム３を制御する。この制御は、次の手順で行われてよい。アーム制御部１１は、観測データＤ１に基づいて、外部センサ５に固定された座標系（以下、単に外部センサ座標系Ｓａともいう）で表された対象物１の位置姿勢を求める。次いで、上記相対データに基づいて、アーム制御部１１は、求めた対象物１の位置姿勢を、ロボットアーム３（本実施形態では、先端部３ｆ）に固定された座標系（以下、単にアーム座標系Ｓｂともいう）で表された位置姿勢に変換する。その後、アーム制御部１１は、変換した対象物１の位置姿勢と、アーム計測装置９が計測したロボットアーム３の先端部３ｆの位置姿勢とに基づいて、ロボットアーム３を制御する。これにより、ロボットアーム３の先端部３ｆ（例えば、先端部３ｆのハンド）は対象物１に作業を行うように動作する。

（相対データ較正装置の構成）
相対データ較正装置１０は、記憶部７に記憶されている上述の相対データを較正する。記憶部７は、相対データ較正装置１０に較正された最新の相対データを記憶する。制御装置２０は、記憶部７に記憶されている最新の相対データに基づいて、上述のようにロボットアーム３を制御する。

相対データ較正装置１０は、図２のように、アームマーカ１３、外部マーカ１５、アーム側センサ１７、センサ駆動装置１９、センサ向き制御部２１、および、較正処理部２３を備える。

アームマーカ１３は、ロボットアーム３に設けられている。本実施形態では、アームマーカ１３は、好ましくは、アームマーカ１３の姿勢が特定可能な形状を有する。アームマーカ１３は、後述のアーム側センサ１７に一体的に固定されている。

アームマーカ１３と外部センサ５は、ロボットアーム３が作業を行うための動作を行っている時に（すなわち、ロボットアーム３の作業中に）、外部センサ５によりアームマーカ１３を観測できるように配置されている。例えば、ロボットアーム３が作業を行うために移動する作業用軌道上の少なくともいずれかの位置にロボットアーム３が来た時に、外部センサ５はアームマーカ１３を観測できる。この作業用軌道は、予め設定されたものであってもよい。

したがって、外部センサ５は、ロボットアーム３の制御だけでなく相対データの較正にも用いられる。外部センサ５は、アームマーカ１３を観測することにより、アームマーカ１３の観測データを取得する。ここで、アームマーカ１３がカメラである場合には、カメラ５は、アームマーカ１３を含む領域を撮像することにより、アームマーカ１３の観測データとして、この領域の画像データを取得する。以下において、外部センサ５が取得した観測データが、相対データの較正に用いられる場合には、この観測データをＤ２で表す。なお、外部センサ５が取得した１つの観測データが、ロボットアーム３の制御にも相対データの較正にも用いられる場合がある。

外部センサ５の位置と向きは固定されていてよい。好ましくは、外部センサ５は、自身の観測範囲にアームマーカ１３が入っている期間において、アームマーカ１３の観測データＤ２を繰り返し取得する。

外部マーカ１５は、図１のように、外部センサ５に設けられている。外部マーカ１５は、好ましくは、外部マーカ１５の姿勢が特定可能な形状を有する。外部マーカ１５は、外部センサ５に一体的に固定されている。

アーム側センサ１７は、ロボットアーム３（本実施形態では、先端部３ｆ）に設けられている。アーム側センサ１７は、外部マーカ１５を観測することにより、外部マーカ１５の観測データＤ３を取得する。

アーム側センサ１７と外部マーカ１５は、ロボットアーム３の作業中にアーム側センサ１７により外部マーカ１５を観測できるように配置されている。例えば、ロボットアーム３が上述の作業用軌道上の少なくともいずれかの位置にロボットアーム３が来た時に、アーム側センサ１７は外部マーカ１５を観測できる。好ましくは、ロボットアーム３の作業中に、外部センサ５とアームマーカ１３とが向かい合い、かつ、アーム側センサ１７と外部マーカ１５とが向かい合う時点が存在する。

アーム側センサ１７がカメラである場合には、カメラ１７は、外部マーカ１５を含む領域を撮像することにより、外部マーカ１５の観測データＤ３として、この領域の画像データを取得する。

アーム側センサ１７は、先端部３ｆと一体で移動し姿勢を変える。好ましくは、アーム側センサ１７は、自身の観測範囲に外部マーカ１５が入っている期間において、外部マーカ１５の観測データＤ３を繰り返し取得する。

図３（Ａ）は、アーム側センサ１７を、その正面から見た図である。図３（Ｂ）は、外部センサ５を、その正面から見た図である。図３のように、アームマーカ１３と外部マーカ１５の各々は、互いに間隔をおいて配置された複数のマーカ部１３ａ，１５ａを有する。好ましくは、図３（Ａ）に示すように、アームマーカ１３は、アーム側センサ１７の観測部１７ａ（例えば、カメラ１７の対物レンズ）を挟むように配置された２つのマーカ部１３ａを含む。観測部１７ａが向いている側が、アーム側センサ１７の観測範囲になる。同様に、外部マーカ１５は、外部センサ５の観測部５ａ（例えば、カメラ５の対物レンズ）を挟むように配置された２つのマーカ部１５ａを含む。観測部５ａが向いている側が、外部マーカ１５の観測範囲になる。好ましい一例では、各マーカ部１３ａ，１５ａは、２次元バーコードであり、外部センサ５とアーム側センサ１７は、それぞれカメラ５，１７である。

センサ駆動装置１９（図１参照）は、アーム側センサ１７を回転軸まわりに回転駆動する。これにより、アーム側センサ１７の向きは、ロボットアーム３（先端部３ｆ）に対して変わる。

センサ向き制御部２１（図２参照）は、ロボットアーム３の動作に応じて、アーム側センサ１７が外部マーカ１５を向くようにセンサ駆動装置１９を制御する。センサ向き制御部２１は、予め定められた参照データと、アーム計測装置９により得られたロボットアーム３の位置姿勢の計測値とに基づいてセンサ駆動装置１９を制御する。

参照データは、センサ向き制御部２１に記憶されていてよい。参照データは、予め定められたロボットアーム３の作業用軌道（位置の軌道と姿勢の軌道を含む）における各位置および姿勢と、該位置および姿勢に対して設定された回転駆動量（以下、設定駆動量という）とを互いに関連づけたデータである。設定駆動量は、アーム側センサ１７の基準回転位置からの回転駆動量である。

センサ向き制御部２１は、ロボットアーム３の位置姿勢の計測値を参照データに適用する。これにより、センサ向き制御部２１は、この計測値に対応する設定駆動量を特定する。センサ向き制御部２１は、特定された設定駆動量だけ基準回転位置から回転させた位置にアーム側センサ１７を回転軸まわりに回転駆動する。このような回転軸は１つ又は複数存在していてよい。ここで、回転軸が複数ある場合には、設定駆動量は回転軸毎に定められている。

較正処理部２３（図２を参照）は、外部センサ５の観測により取得されたアームマーカ１３の観測データＤ２と、アーム側センサ１７の観測により取得された外部マーカ１５の観測データＤ３とに基づいて、相対データを較正する。この時、較正処理部２３は、次の（ａ）〜（ｃ）の処理を行う。

（ａ）アームマーカ１３の観測データＤ２に基づいて、外部センサ座標系Ｓａで表したアームマーカ１３の位置姿勢を求める。

（ｂ）外部マーカ１５の観測データＤ３に基づいて、アーム座標系Ｓｂで表した外部マーカ１５の位置姿勢を求める。

（ｃ）上記（ａ）で求めたアームマーカ１３の位置姿勢に基づいて相対データを較正する。例えば、上記（ａ）で求めたアームマーカ１３の位置姿勢に基づいて、新たに相対データを求め、この相対データを較正後の相対データとする。上記（ｂ）で求めた外部マーカ１５の位置姿勢に基づいて相対データを較正する。例えば、上記（ｂ）で求めた外部マーカ１５の位置姿勢に基づいて、新たに相対データを求め、この相対データを較正後の相対データとする。

上記（ｃ）で較正された相対データは、外部センサ座標系Ｓａの各位置をアーム座標系Ｓｂの各位置に変換するための座標変換データ（例えば座標変換行列）であってよい。

上記（ａ）は、アームマーカ１３の観測データＤ２が取得される度に行われてよい。上記（ｂ）は、外部マーカ１５の観測データＤ３が取得される度に行われてよい。上記（ｃ）は、上記（ａ）が行われる度に行われ、上記（ｂ）が行われる度に行われてよい。

上記（ｃ）では、幾何学的に相対データを算出し、この相対データを較正後のデータとしてもよい。この場合、上記（ａ）（ｂ）が合計で複数回行われることにより、複数回の上記（ｃ）により相対データが複数回算出され、これらの相対データの平均値を較正後の相対データとしてもよい。
あるいは、上記（ｃ）では、後述するようにカルマンフィルタを用いて相対データを較正してもよい。または、上記（ｃ）では、別の方法で、観測データＤ２、Ｄ３に基づいて相対データを較正してもよい。

なお、特許請求の範囲における「アームマーカの観測データと外部マーカの観測データに基づいて、相対データを較正する」とは、相対データの１回の較正に１つの観測データＤ２またはＤ３を用いる場合だけでなく、相対データの１回の較正に複数の観測データ（例えば観測データＤ２と観測データＤ３の両方）を用いる場合も含む内容である。前者の場合の一例では、観測データＤ２に基づいて相対データを更新した後、観測データＤ３に基づいて相対データを更新する。後者の場合の一例では、上述したように複数回算出された相対データの平均値を較正後の相対データとする。

図４は、本発明の実施形態による相対データ較正方法と、ロボットアーム３の制御処理とを示すフローチャートである。この相対データ較正方法は、上述の相対データ較正装置１０を用いて行われる。
相対データ較正方法は、ステップＳ１〜Ｓ３を有する。

ステップＳ１において、外部センサ５により、アームマーカ１３を観測してアームマーカ１３の観測データＤ２を取得する。

ステップＳ２において、アーム側センサ１７により、外部マーカ１５を観測して外部マーカ１５の観測データＤ３を取得する。

ステップＳ１またはステップＳ２が行われたら、処理はステップＳ３へ移行する。
外部センサ５の数が複数である場合には、いずれかの外部センサ５が、ステップＳ１アームマーカ１３の観測データＤ２を取得したら（すなわち、ステップＳ１を行ったら）、処理はステップＳ３へ移行する。また、アーム側センサ１７が、いずれかの外部マーカ１５の観測データＤ３を取得したら、処理はステップＳ３へ移行する。外部センサ５の数が複数である場合には、外部センサ５毎に、相対データが存在し、各外部センサ５が取得したアームマーカ１３の観測データＤ２に基づいて、ステップＳ３で対応する相対データが較正される。この場合、アーム側センサ１７がいずれかの外部マーカ１５の観測データＤ３を取得したら、該外部マーカ１５に対応する相対データがステップＳ３で較正される。

ステップＳ３において、較正処理部２３により、ステップＳ１またはステップＳ２により得られた観測データＤ２，Ｄ３に基づいて、相対データを較正する。すなわち、較正処理部２３は、上述の（ａ）〜（ｃ）の処理を行う。

上述のステップＳ１〜Ｓ３は、ロボットアーム３の作業中に行われる。本実施形態では、ロボットアーム３の作業中に、上述のステップＳ１〜Ｓ３が、繰り返し行われる。この繰り返しにおいて、ステップＳ３で、較正処理部２３は、ベイズフィルタ（好ましくは後述のカルマンフィルタ）を用いて相対データを較正（更新）する。ステップＳ３で相対データが較正される度に、較正後の相対データが最新の相対データとして記憶部７に記憶される。
なお、相対データの初期値は、予め求められていてもよいし、相対データ較正装置１０により求められてもよい。

上述のステップＳ１〜Ｓ３を繰り返し行っている間に、ロボットアーム３の制御処理が行われる。ロボットアーム３の制御処理は、ステップＳ１１〜Ｓ１５を有する。

ステップＳ１１において、外部センサ５は、ロボットアーム３の動作領域を観測することにより、この領域内に位置する対象物１の観測データＤ１を取得する。

ステップＳ１２において、アーム制御部１１は、ステップＳ１１で得た観測データＤ１に基づいて、外部センサ座標系Ｓａでの対象物１の位置姿勢を求める。

ステップＳ１３において、記憶部７内の最新の相対データに基づいて、アーム制御部１１は、ステップＳ１２で求めた、外部センサ座標系Ｓａでの対象物１の位置姿勢を、アーム座標系Ｓｂでの位置姿勢に座標変換する。

ステップＳ１４において、アーム計測装置９は、ロボットアーム３の先端部３ｆの位置姿勢を計測により求める。この位置姿勢は、アーム座標系Ｓｂで表されたものである。

ステップＳ１５において、アーム制御部１１は、ステップＳ１３で変換した対象物１の位置姿勢と、ステップＳ１４で求めたロボットアーム３の先端部３ｆの位置姿勢とに基づいて、ロボットアーム３を制御する。これにより、ロボットアーム３の先端部３ｆ（例えばハンド）は対象物１に作業を行うように動作する。

（較正処理）
ステップＳ３における相対データの較正処理の一例を詳しく説明する。この例では、カルマンフィルタを用いる。

最初に、次の式（１）〜（３）で示すように、上述の相対データ（内部状態）Ｘ_ｔと、相対データＸ_ｔの初期値Ｘ_ｔ０と、相対データＸ_ｔの初期分散共分散行列ＣｏｖＸ_ｔ０を定義しておく。

式（１）のｘ，ｙ，ｚは、外部センサ５（外部センサ座標系Ｓａ）に対するロボットアーム３の先端部３ｆ（アーム座標系Ｓｂ）の位置を示す。外部センサ座標系Ｓａは、互いに直交するｘ軸とｙ軸とｚ軸を有する。式（１）のｘ，ｙ，ｚは、それぞれ、外部センサ座標系Ｓａのｘ軸とｙ軸とｚ軸の座標値である。式（１）のｘ，ｙ，ｚは、外部センサ座標系Ｓａの原点に対するアーム座標系Ｓｂの原点の座標値であってよい。

式（１）のｒ_ｘ，ｒ_ｙ，ｒ_ｚは、外部センサ５（外部センサ座標系Ｓａ）に対するロボットアーム３の先端部３ｆ（アーム座標系Ｓｂ）の姿勢を示す角度（ラジアン）である。すなわち、ｒ_ｘ，ｒ_ｙ，ｒ_ｚは、それぞれ外部センサ座標系Ｓａのｘ軸とｙ軸とｚ軸と平行な各軸（例えばアーム座標系Ｓｂの原点を通る軸）まわりの回転角である。
なお、式（１）（２）では省略しているが、好ましくは、Ｘ_ｔには、ｘ，ｙ，ｚの１回時間微分（速度）と２回時間微分（加速度）と、ｒ_ｘ，ｒ_ｙ，ｒ_ｚの１回時間微分（角速度）も含まれている。

（アームマーカの観測に基づく較正）
ステップＳ１で取得されたアームマーカ１３の観測データＤ２に基づいて、外部センサ座標系Ｓａで表したアームマーカ１３の位置姿勢Ｙ_１を求める。較正処理部２３は、この位置姿勢Ｙ_１を次の式（４）（５）に適用することにより、相対データＸ_ｔとＣｏｖＸ_ｔを較正（更新）する。式（４）で較正された相対データＸ_ｔは、最新のデータとして記憶部７に記憶される。ＣｏｖＸ_ｔは、Ｘ_ｔの分散共分散行列を示す。

Ｘ_ｔ＝Ｘ_ｔ ^＊−Ｋ_１（Ｙ_１Ｐ−Ｙ_１） ・・・（４）

ＣｏｖＸ_ｔ＝（Ｉ−Ｋ_１Ｂ_１）・ＣｏｖＸ_ｔ ^＊ ・・・（５）

式（４）において、左辺のＸ_ｔは、較正後のＸ_ｔであり、右辺のＸ_ｔ ^＊は、較正直前のＸ_ｔである。式（５）において、左辺のＣｏｖＸ_ｔは、較正後のＣｏｖＸ_ｔであり、右辺のＣｏｖＸ_ｔ ^＊は、較正直前のＣｏｖＸ_ｔである。また、式（５）において、Ｉは、単位行列を示す。

上式（４）のＹ_１Ｐは、次の式（６）で表される。

Ｙ_１Ｐ＝ｇ_１（Ｘ_ｔ） ・・・（６）

ｇ_１（Ｘ_ｔ）は、時刻ｔにおけるＹ_１の予測値を算出するための関数である。ｇ_１（Ｘ_ｔ）は、ロボットアーム３の運動モデルに基づいて定められる。較正処理部２３は、アーム計測装置９や他のセンサにより計測されたＸ_ｔをｇ_１（Ｘ_ｔ）に代入することにより、予測値Ｙ_１Ｐを算出する。

Ｋ_１は、カルマンゲインであり、次の式（７）で表される。

Ｋ_１＝ＣｏｖＸ_ｔ・Ｂ_１ ^Ｔ・Ｖ_１ ^−１ ・・・（７）

ここで、Ｂ_１は、次の式（８）で表される偏微分行列であり、その上付き文字「Ｔ」は転置行列を示す。

上式（７）のＶ_１は、次の式（９）で表される。なお、上式（７）においてＶ_１の上付き数字「−１」は逆行列を示す。

Ｖ_１＝Ｂ_１・ＣｏｖＸ_ｔ・Ｂ_１ ^Ｔ＋Ｒ_１ ・・・（９）

ここで、Ｒ_１は、ステップＳ１で得た観測データＤ２の計測誤差として想定される分散値である。

なお、上述では、１つの外部センサ５の観測に基づいて、カルマンフィルタで相対データＸ_ｔを較正する場合を説明した。外部センサ５が複数ある場合には、他の外部センサ５についても、上記と同様にカルマンフィルタでＸ_ｔを較正する。この場合、ただし、Ｋ_１やＢ_１は、外部センサ５毎に異なっていてよい。

（外部マーカの観測に基づく較正）
較正処理部２３は、ステップＳ２で取得された外部マーカ１５の観測データＤ３に基づいて、アーム座標系Ｓｂで表した外部マーカ１５の位置姿勢Ｙを求める。較正処理部２３は、この位置姿勢Ｙと、最新の相対データＸ_ｔに基づいて、外部センサ座標系Ｓａで表したアーム座標系Ｓｂの位置姿勢Ｙ_２を求める。

較正処理部２３は、この位置姿勢Ｙ_２を次の式（１０）（１１）に適用することにより、相対データＸ_ｔとＣｏｖＸ_ｔを較正（更新）する。式（１０）で較正された相対データＸ_ｔは、最新のデータとして記憶部７に記憶される。

Ｘ_ｔ＝Ｘ_ｔ ^＊−Ｋ_２（Ｙ_２Ｐ−Ｙ_２） ・・・（１０）

ＣｏｖＸ_ｔ＝（Ｉ−Ｋ_２Ｂ_２）・ＣｏｖＸ_ｔ ^＊ ・・・（１１）

式（１０）において、左辺のＸ_ｔは、較正後のＸ_ｔであり、右辺のＸ_ｔ ^＊は、較正直前のＸ_ｔである。式（１１）において、左辺のＣｏｖＸ_ｔは、較正後のＣｏｖＸ_ｔであり、右辺のＣｏｖＸ_ｔ ^＊は、較正直前のＣｏｖＸ_ｔである。

上式（１０）のＹ_２Ｐは、次の式（１２）で表される。

Ｙ_２Ｐ＝ｇ_２（Ｘ_ｔ） ・・・（１２）

ｇ_２（Ｘ_ｔ）は、時刻ｔにおけるＹ_２の予測値を算出するための関数である。ｇ_２（Ｘ_ｔ）は、ロボットアーム３の運動モデルに基づいて定められる。較正処理部２３は、アーム計測装置９や他のセンサにより計測されたＸ_ｔをｇ_２（Ｘ_ｔ）に代入することにより、予測値Ｙ_２Ｐを算出する。なお、ｇ_２（Ｘ_ｔ）は、上述のｇ_１（Ｘ_ｔ）と同じであってもよい。

Ｋ_２は、カルマンゲインであり、次の式（１３）で表される。

Ｋ_２＝ＣｏｖＸ_ｔ・Ｂ_２ ^Ｔ・Ｖ_２ ^−１ ・・・（１３）

ここで、Ｂ_２は、次の式（１４）で表される偏微分行列である。

上式（１３）のＶ_２は、次の式（１５）で表される。

Ｖ_２＝Ｂ_２・ＣｏｖＸ_ｔ・Ｂ_２ ^Ｔ＋Ｒ_２ ・・・（１５）

ここで、Ｒ_２は、ステップＳ２で得た観測データＤ３の計測誤差として想定される分散値である。

（実施形態の構成とその効果）
上述の実施形態は、以下のように特徴づけられる構成により以下の効果が得られる。

相対データ較正装置１０は、外部センサ５に設けられた外部マーカ１５と、ロボットアーム３に設けられ外部マーカ１５を観測することにより外部マーカ１５の観測データＤ３を取得するアーム側センサ１７と、を備える。較正処理部２３は、アームマーカ１３の観測データＤ２と外部マーカ１５の観測データＤ３に基づいて、相対データを較正する。

この構成では、外部センサ５にも外部マーカ１５を設ける。したがって、較正処理部２３は、アームマーカ１３の観測データＤ２だけでなく、外部マーカ１５の観測データＤ３にも基づいて、相対データを較正できる。よって、相対データを高精度に較正できる。

アームマーカ１３は、該アームマーカ１３の姿勢が特定可能な形状を有し、アーム側センサ１７に一体的に固定されている。外部マーカ１５は、該外部マーカ１５の姿勢が特定可能な形状を有し、外部センサ５に一体的に固定されている。

この構成では、アームマーカ１３と外部マーカ１５は、それぞれ、その姿勢が特定可能な形状を有する。したがって、較正処理部２３は、アームマーカ１３の観測データＤ２と外部マーカ１５の観測データＤ３とに基づいて、相対データとして、ロボットアーム３と外部センサ５との相対姿勢を較正できる。

相対データ較正装置１０は、アーム側センサ１７を回転軸まわりに回転駆動することによりアーム側センサ１７の向きを変えるセンサ駆動装置１９と、ロボットアーム３の動作に応じてアーム側センサ１７が外部マーカ１５を向くようにセンサ駆動装置１９を制御するセンサ向き制御部２１と、を備える。

この構成では、ロボットアーム３の作業中に、ロボットアーム３の動作に応じて、アーム側センサ１７が外部マーカ１５を向くようにセンサ駆動装置１９はアーム側センサ１７を駆動する。したがって、ロボットアーム３の作業において、より長い期間にわたって、アーム側センサ１７は外部マーカ１５の観測データＤ３を取得できる。すなわち、観測データＤ３の取得回数が増える。よって、これらの観測データＤ３に基づいて（例えばベイズフィルタを用いて）相対データを較正できるので、相対データの精度を高められる。

アームマーカ１３と外部マーカ１５の各々は、互いに間隔をおいて配置された複数のマーカ部１３ａ，１５ａを有する。外部センサ５とアーム側センサ１７の各々は、カメラである。アームマーカ１３の観測データＤ２と外部マーカ１５の観測データＤ３は、画像データである。

この構成では、アームマーカ１３は、互いに間隔をおいて配置された複数のマーカ部１３ａを有する。したがって、較正処理装置は、アームマーカ１３の画像データによりアームマーカ１３の姿勢を精度よく特定できる。外部マーカ１５も同様である。よって、簡易なカメラを用いた場合でも、姿勢に関して相対データを精度よく較正できる。

また、ロボットの作業中に、相対データの較正（すなわち、上述のステップＳ１〜Ｓ３）を繰り返し行う。したがって、１回目の較正の精度が低くても、（例えばベイズフィルタを用いた）複数回の較正により相対データの精度を高められる。

本発明は上述した実施の形態に限定されず、本発明の要旨を逸脱しない範囲で種々変更を加え得ることは勿論である。例えば、以下の変更例１〜４のいずれかを採用してもよいし、変更例１〜４の２つ以上を任意に組み合わせて採用してもよい。この場合、以下で説明しない点は、上述と同じである。

（変更例１）
外部マーカ１５とアーム側センサ１７を省略してもよい。この場合、較正処理部２３は、外部センサ５により観測されたアームマーカ１３の観測データＤ２に基づいて、相対データを較正する。

（変更例２）
アームマーカ１３と外部マーカ１５は、特定の形状を有する部分、特定の色を有する部分、特定の時間間隔をおいて点灯する点滅光源、または、他のものであってもよい。

（変更例３）
外部センサ５とアーム側センサ１７の一方または両方は、その観測範囲内の各方向へレーザ光を射出することにより観測データを取得するレーザ距離計測装置であってもよい。この場合、観測データは、観測範囲内の物体上の各点までの距離と、これら各点へのレーザ光の射出方向とを含む。
（変更例４）
ロボットアーム３は、次のように制御されてもよい。ステップＳ１１において、外部センサ５は、その観測範囲内における、対象物１以外の計測対象（例えば、障害物または目印）の観測データＤ２を取得する。ステップＳ１２において、ステップＳ１１で得た観測データＤ２に基づいて、外部センサ座標系Ｓａでの計測対象の位置姿勢を求める。ステップＳ１３において、記憶部７内の最新の相対データに基づいて、アーム制御部１１は、ステップＳ１２で求めた、外部センサ座標系Ｓａでの計測対象の位置姿勢を、アーム座標系Ｓｂでの位置姿勢に座標変換する。ステップＳ１４において、アーム計測装置９は、ロボットアーム３の先端部３ｆの位置姿勢を計測により求める。ステップＳ１５において、アーム制御部１１は、ステップＳ１３で変換した計測対象の位置姿勢と、ステップＳ１４で求めたロボットアーム３の先端部３ｆの位置姿勢とに基づいて、ロボットアーム３を制御する。これにより、計測対象の位置姿勢に応じて、ロボットアーム３の先端部３ｆ（例えばハンド）が動作する。計測対象が障害物である場合には、ステップＳ１５において、アーム制御部１１は、障害物の位置に基づいて、ロボットアーム３が障害物を回避するようにロボットアーム３を制御する。計測対象が目印である場合には、ステップＳ１５において、アーム制御部１１は、目印の位置姿勢を基準にロボットアーム３の位置姿勢を制御する。

なお、変更例４によると、対象物１の位置姿勢は、外部センサ５または他の装置（例えば、ロボットアーム３の先端部３ｆに設けたカメラやレーザ距離計測装置）を用いて計測されてもよい。アーム制御部１１は、計測された対象物１の位置姿勢に基づいて、ロボットアーム３が対象物１に作業をするようにロボットアーム３を制御する。

１ 対象物、３ ロボットアーム、３ａ，３ｂ，３ｃ，３ｄ，３ｅ リンク、３ｆ 先端部、５ 外部センサ、５ａ 観測部、７ 記憶部、９ アーム計測装置、１０ 相対データ較正装置、１１ アーム制御部、１３ アームマーカ、１３ａ マーカ部、１５ 外部マーカ、１５ａ マーカ部、１７ アーム側センサ、１７ａ 観測部、１９ センサ駆動装置、２０ 制御装置、２１ センサ向き制御部、２３ 較正処理部、Ｓａ 外部センサ座標系、Ｓｂ アーム座標系

Claims (7)

1. ロボットアームの動作領域を観測することにより観測データを取得する外部センサと、
   前記ロボットアームと外部センサとの相対位置および相対姿勢の一方または両方を表す相対データを記憶する記憶部と、
   前記観測データと前記相対データとに基づいて、前記ロボットアームが作業を行うように前記ロボットアームを制御するアーム制御部とを備える制御装置のために設けられる相対データ較正装置であって、
   前記ロボットアームに設けられたアームマーカと、
   前記外部センサの観測により取得された前記アームマーカの観測データに基づいて、前記相対データを較正する較正処理部と、を備える、相対データ較正装置。
2. 前記外部センサに設けられた外部マーカと、
   前記ロボットアームに設けられ、前記外部マーカを観測することにより、前記外部マーカの観測データを取得するアーム側センサと、を備え、
   前記較正処理部は、前記アームマーカの前記観測データと前記外部マーカの前記観測データに基づいて、前記相対データを較正する、請求項１に記載の相対データ較正装置。
3. 前記アームマーカは、該アームマーカの姿勢が特定可能な形状を有し、前記アーム側センサに一体的に固定されており、
   前記外部マーカは、該外部マーカの姿勢が特定可能な形状を有し、前記外部センサに一体的に固定されている、請求項２に記載の相対データ較正装置。
4. 前記アーム側センサを回転軸まわりに回転駆動することにより、前記アーム側センサの向きを変えるセンサ駆動装置と、
   前記ロボットアームの動作に応じて、前記アーム側センサが前記外部マーカを向くように前記センサ駆動装置を制御するセンサ向き制御部と、を備える、請求項２に記載の相対データ較正装置。
5. 前記アームマーカと前記外部マーカの各々は、互いに間隔をおいて配置された複数のマーカ部を有し、
   前記外部センサと前記アーム側センサの各々は、カメラであり、
   前記アームマーカの前記観測データと前記外部マーカの前記観測データは、画像データである、請求項２〜４のいずれか一項に記載の相対データ較正装置。
6. 外部センサの観測データと、前記ロボットアームと前記外部センサとの相対位置および相対姿勢の一方または両方を表す相対データとに基づいて前記ロボットアームを制御する場合に、前記相対データを較正する方法であって、
   （Ａ）前記ロボットアームにアームマーカを設け、
   （Ｂ）前記外部センサにより、前記アームマーカを観測して前記アームマーカの観測データを取得し、
   （Ｃ）前記アームマーカの前記観測データに基づいて、前記相対データを較正し、
   前記ロボットアームが作業をするための動作をしている作業中に、前記（Ｂ）と（Ｃ）を行う、相対データ較正方法。
7. 前記作業中に、前記（Ｂ）と（Ｃ）を繰り返し行う、請求項６に記載の相対データ較正方法。
   

Images