JP5450242B2 - マニピュレータのキャリブレーション方法及びロボット制御システム - Google Patents

Description

本発明は、マニピュレータのキャリブレーション方法及びロボット制御システムに関するものである。

近年、手先カメラと呼ばれるセンサ装置をマニピュレータの先端に取り付け、センサから得られる情報により、マニピュレータの動作を制御する制御手法が開発されてきている。しかし、マニピュレータのモータ組み付け誤差やアーム加工誤差に加え、センサとマニピュレータの取り付け位置姿勢情報が正確に把握されていなければ、マニピュレータの正確な制御はできない。このため、マニピュレータの各リンク部の位置関係やマニピュレータの先端の座標系を基準としたセンサの位置姿勢情報を正確に導出する、つまりキャリブレーションを行うことが必要になる。

ここで、手先カメラを持つマニピュレータのキャリブレーション方法としては、特許文献１がある。特許文献１では、マニピュレータの手先にＣＣＤカメラを取り付け、所定位置に被写点を固定し、マニピュレータ及びカメラを様々に関節動作させ、撮像する。その後、実際のカメラの２次元画像上における被写点の位置と論理的に算出される被写点の位置のずれを複数個求め、マニピュレータのリンク要素のパラメータの補正値を変数として、ずれが最小になるように最小二乗法をすることにより補正値を演算する手法である。特許文献１では、各リンク長さを実際に測定することや、マニピュレータを実際にある一点に移動させる手間を省くことができ、作業工程が大幅に短縮できる利点がある。

特許文献２では、手先カメラのキャリブレーションではないが、マニピュレータの先端に、特徴点を有する治具を取付けるとともに、マニピュレータを支持するロボット本体に固定した撮像用、照準用のセンサにより前記特徴点を持つ治具を観測して、三次元位置センサのキャリブレーションを行う方法が提案されている。特許文献２では、基準座標系における前記特徴点の三次元位置と、撮像用，照準用であるセンサ部の２つのカメラの撮像面における前記特徴点の位置との組み合せを複数求め、これらから三角測量の原理で２つのカメラ（センサ）の三次元位置をキャリブレーションするようにしている。この方法は、人手でセンサを動かさなくてもよいところから、作業者の目にレーザが当たる危険性を回避することができ、手間も少なくてすむ利点がある。

又、特許文献３では、先端に視覚センサが取り付けられたロボットアームを姿勢変化させ、変化の前後それぞれにおいて、視覚センサにより取得されるワーク画像の変化と、カメラの取り付け位置姿勢の仮の値からワークの位置変化を求める。この際、仮の値を複数用意し、ワークの位置変化が最小となるものを選択することで、カメラの取り付け位置を決定することができる。

特許文献３では、ワークの置き場所を自由に設定でき、実際の作業に近い状態でキャリブレーションができる利点がある。

特許第２６８２７６３号公報 特開平１１−３３９６２号公報 特開２００７−６１９７９号公報

ところで、特許文献１、特許文献３にみられるように、現在、手先カメラのキャリブレーション手法はいくつか提案されている。しかし、キャリブレーションの実行の際に特別な冶具が必要となっている。すなわち、特許文献１、特許文献３では専用のキャリブレーション用の装置、治具が必要となり、キャリブレーションのために多くの時間とコストが必要となる問題がある。特許文献２は、手先カメラではないが、マニピュレータ先端の位置を観測するためのカメラのキャリブレーションであるとともに、マニピュレータに取付する専用の治具が必要となる。

本発明の目的は、直線部を有するだけの簡単な構成の治具により、センサのキャリブレーションのみならずマニピュレータのキャリブレーションをどこでも簡単に行うことができるとともに、前記治具の準備などの作業工数を大幅に削減できるばかりか、特別な治具が準備できない環境でも即座にキャリブレーションを行うことが可能であり、マニピュレータに求められる本来の作業を行うための不必要な待ち時間や工数が削減できるマニピュレータのキャリブレーション方法を提供することにある。

又、本発明の他の目的は、上記のキャリブレーション方法を実行するロボット制御システムを提供することにある。

上記問題点を解決するために、請求項１に記載の発明は、マニピュレータの周囲に配置された治具の直線部上の複数の点を、互いに異なる複数の観測位置姿勢をとらせた状態で、前記マニピュレータ先端に設けた視覚センサによりそれぞれ検出する第１ステップと、基準座標系を基準としたマニピュレータ先端座標系の原点位置姿勢と、前記マニピュレータ先端を基準とする前記視覚センサのセンサ座標系の原点位置姿勢を未知変数とした関数であるとともに、前記直線部上の点（以下、検出点という）の基準座標系座標を、前記観測位置姿勢毎にそれぞれ求める第２ステップと、前記観測位置姿勢毎に求めた前記直線部上の検出点の２つを一組としたとき、複数の組から前記関数を要素とするとともに、前記基準座標系を基準とした前記直線部の単位方向ベクトルを複数算出し、算出した前記直線部の単位方向ベクトルと、基準座標系のベクトル変数で表される前記直線部の単位方向ベクトルが等しいことを利用して連立方程式を作り、最小二乗法により、前記マニピュレータに関する第１較正値を求めるとともに、前記視覚センサのセンサ座標系の原点位置姿勢であって、前記マニピュレータ先端を基準とするセンサ座標系の原点位置姿勢を第２較正値として求める第３ステップを含むことを特徴とするマニピュレータのキャリブレーション方法を要旨としている。

請求項２の発明は、請求項１において、前記連立方程式は、線形化して一次近似式にしたものであることを特徴とする。
請求項３の発明は、請求項２おいて、前記第１較正値及び第２較正値が、それぞれ仮の実数と誤差変数の和で成り立っているものとしたとき、前記第３ステップでは、第１較正値及び第２較正値がそれぞれ得られたとき、前記仮の実数を、該得られた第１較正値及び第２較正値に更新した後、前記誤差変数が予め定められた閾値以下になるまで前記第３ステップの処理を繰り返すことを特徴とする。

請求項４の発明は、請求項１乃至請求項３のうちいずれか１項において、前記治具は、複数の面と面間に設けられた複数の稜線を有し、前記直線部は、複数の稜線のうち、１つの稜線であることを特徴とする。

請求項５の発明は、マニピュレータの周囲に配置された治具の直線部上の複数の点を検出するために前記マニピュレータの互いに異なる複数の観測位置姿勢をとらせる制御手段と、前記マニピュレータの互いに異なる複数の観測位置姿勢毎に、マニピュレータ先端に設けられた視覚センサが前記治具の直線部上の異なる複数の点についてそれぞれ検出した値を記憶する記憶手段と、基準座標系を基準としたマニピュレータ先端座標系の原点位置姿勢と、前記マニピュレータ先端を基準とする前記視覚センサのセンサ座標系の原点位置姿勢を未知変数とした関数であるとともに、前記直線部上の点（以下、検出点という）の基準座標系座標を、前記観測位置姿勢毎にそれぞれ求める検出点座標算出手段と、前記観測位置姿勢毎に求めた前記直線部上の検出点の２つを一組としたとき、複数の組から前記関数を要素とするとともに、前記基準座標系を基準とした前記直線部の単位方向ベクトルを複数算出し、算出した前記直線部の単位方向ベクトルと、基準座標系のベクトル変数で表される前記直線部の単位方向ベクトルが等しいことを利用して連立方程式を作り、最小二乗法により、前記マニピュレータに関する第１較正値を求めるとともに、前記マニピュレータ先端を基準とするセンサ座標系の原点位置姿勢を第２較正値として求める処理を行う較正値算出手段を含むことを特徴とするロボット制御システムを要旨としている。

請求項６の発明は、請求項５において、前記較正値算出手段は、前記連立方程式を、線形化して一次近似式にした上で前記較正値を算出するものであることを特徴とする。
請求項７の発明は、請求項６において、前記較正値算出手段は、前記第１較正値及び第２較正値が、それぞれ仮の実数と誤差変数の和で成り立っているものとし、第１較正値及び第２較正値がそれぞれ得られたとき、前記仮の実数を、該得られた第１較正値及び第２較正値に更新した後、前記誤差変数が予め定められた閾値以下になるまで、前記処理を繰り返すことを特徴とする。

請求項１の発明によれば、直線部を有するだけの簡単な構成の治具により、センサのキャリブレーションのみならずマニピュレータのキャリブレーションをどこでも簡単に行うことができる。又、治具の準備などの作業工数を大幅に削減できるばかりか、特別な治具が準備できない環境でも即座にキャリブレーションを行うことが可能であり、マニピュレータに求められる本来の作業を行うための不必要な待ち時間や工数が削減できる。

請求項２の発明によれば、連立方程式が線形化して一次近似式にされているため、第３ステップの処理で行う計算を簡便化でき、線形化して一次近似式にしない場合に比して、演算に係る時間を短くできる効果がある。

請求項３の発明によれば、前記連立方程式を線形化すると誤差が生じるが、この誤差を閾値以下にすることができ、較正値の精度を保障することができる。
請求項４の発明によれば、治具の稜線を直線部としているため、特別な治具が必要でなくなり、直方体、立方体等の直線部を面間に有する既存の治具を使用することができ、或いは単純な形状の治具で良くなり、キャリブレーションのための特別な形状の治具を必要としない。

請求項５の発明によれば、上記の請求項１のキャリブレーション方法を実行するロボット制御システムを提供できる。
請求項６の発明によれば、較正値算出手段は連立方程式が線形化して一次近似式にするため、第３ステップの処理で行う計算を簡便化でき、線形化して一次近似式にしない場合に比して、演算に係る時間を短くできるキャリブレーション方法を実行するロボット制御システムを提供できる。

請求項７の発明によれば、前記連立方程式を線形化すると誤差が生じるが、この誤差を閾値以下にすることができ、較正値の精度を保障することができるキャリブレーション方法を実行するロボット制御システムを提供できる。

一実施形態のロボット制御システムの構成を示すブロック図。 レーザ変位センサＬＳが治具に対して観測位置姿勢を変化させた状態を示す説明図。 実施形態で使用する座標系の説明図。

以下、本発明を具体化した視覚センサを有するマニピュレータのキャリブレーション方法及びロボット制御システムを具体化した一実施形態を図１〜３を参照して説明する。
図１はロボット制御システム１０の構成を示すブロック図である。ロボット制御システム１０は、ワーク（作業対象物）に対して、特定の作業を行うように制御するものである。ロボット制御システム１０は、前記作業を行うマニピュレータＭと、マニピュレータＭを制御するロボット制御装置ＲＣと、ワークの形状を検出する視覚センサ及びレーザセンサとしてのレーザ変位センサＬＳとを備える。

マニピュレータＭは、フロア等に固定されるベース部材１２と、複数の回転軸を介して連結された複数のアーム１３とを備える。本実施形態のロボットは、６軸のロボットである。最も先端側に位置するアーム１３先端（すなわち、マニピュレータＭ先端）には、前記特定の作業を行うためのツール（図示しない）、例えば、溶接トーチ、塗装ガン、ロボットハンド等が設けられる。

ロボット制御装置ＲＣは、コンピュータからなる。すなわち、ロボット制御装置ＲＣはＣＰＵ（中央処理装置）２０、マニピュレータＭを制御するための各種プログラムを記憶する書き換え可能なＲＯＭ２１や、作業メモリとなるＲＡＭ２２、各種データを記憶する書換可能な不揮発性メモリからなる記憶部２３を備える。ＣＰＵ２０は、制御手段、検出点座標算出手段、及び較正値算出手段に相当する。又、記憶部２３は記憶手段に相当する。

前記ＲＯＭ２１には、マニピュレータのキャリブレーション、及び、前記ツールの特定の作業を行わせるための各種のロボット制御プログラムが記憶されている。そして、前記プログラムに従って前記ＣＰＵ２０が、各アーム１３間に設けられた図示しないモータを制御駆動することにより、前記ツールの位置姿勢及びレーザ変位センサＬＳの観測位置姿勢が変化する。

レーザ変位センサＬＳは、前記ツールと併設されるようにアーム１３先端（すなわち、マニピュレータ先端）にブラケットＢｒを介して取付けされている（図３参照）。そして、本実施形態では、このマニピュレータＭ先端において、任意の一点を基準とするセンサ座標系の原点位置姿勢を後述する第２較正値Ｃとして求めるようにしている。なお、本実施形態では、マニピュレータＭ先端において、前記ブラケットの取付部位を前記任意の一点としているが、限定するものではなく、マニピュレータＭ先端の特定の位置を任意の一点とすればよい。

レーザ変位センサＬＳは、二次元レーザセンサであって、レーザの発光及び受光によりワークＷまでの距離を測定する走査型のレーザ変位センサであり、図２に示すようにレーザ照射部Ｌｓａから扇形の検出範囲を有する。前記レーザ変位センサＬＳは、レーザを照射するレーザ照射部Ｌｓａと、ワーク等の作業対象物で反射したレーザを受光する受光部（図示しない）等を備える。レーザ変位センサＬＳが検出した値は、図１に示すセンサインターフェイスユニット２４を介してロボット制御装置ＲＣに出力され、記憶部２３に格納される。

上記のように構成されたロボット制御システム１０を使用して、マニピュレータＭ及びレーザ変位センサＬＳのキャリブレーションの方法を説明する。
（作業手順）
まず、本実施形態のキャリブレーションの作業手順について説明する。

マニピュレータＭ先端に対して、レーザ変位センサＬＳをブラケットＢｒを介して取り付ける。このとき、マニピュレータＭ先端からレーザ照射部Ｌｓａまでの、位置姿勢を測定し、あるいは、ブラケットＢｒ及びレーザ変位センサＬＳのカタログ値に基づいて得ておき、この位置姿勢を第２較正値Ｃの仮の実数（すなわち、近似値）として、予め図示しないティーチペンダント、或いはパーソナルコンピュータ等の入力装置を介して、ロボット制御装置ＲＣの記憶部２３に格納しておく。

又、テーブル３０上に治具Ｇを配置する。なお、治具Ｇの配置はテーブル３０上に限定するものではなく、床面に直接配置して、動かないようにしてもよい。治具Ｇは、本実施形態では、複数の面、及び互いに隣接する面間には直線を有する稜線を有する。又、本実施形態では、治具Ｇは図２に示すように平板、或いは直方体に形成され、上面の長手方向に延びる上面の稜線が直線部３２とされている。すなわち、治具Ｇは複数の平面を有しているとともに複数の稜線を有しており、この複数の稜線のうち、上面の縁部に設けられた１つの稜線を直線部３２としている。

次に、前記ＲＯＭ２１に格納したキャリブレーションの算出プログラムをＣＰＵ２０が実行することにより、ＣＰＵ２０は図示しないマニピュレータＭの各回転軸のモータを駆動制御する。このＣＰＵ２０のマニピュレータＭの制御により、検出点の検出のための観測位置姿勢、すなわち、マニピュレータＭ先端の観測位置姿勢を異ならしめて、すなわち、これらの複数の観測位置姿勢（図２では、姿勢１、姿勢２、姿勢３で示す。）のもとで、レーザ変位センサＬＳにより治具Ｇに設けられた直線部３２上の異なる複数の点を測定する。観測位置姿勢の数は、少なくとも６点以上が必要である。この処理は第１ステップに相当する。

なお、観測位置姿勢の数（すなわち、検出点の数）は、ロボットの軸数に応じて、決定すればよい。
例えば、ロボットの軸数の増減に応じて、較正するマニピュレータのパラメータは増減するが、較正するパラメータの数をｐ個とすると、検出点の数ｑは、下式を満たす必要がある。

ｐ＜_ｑＣ_２ ×３ （なお、_ｑＣ_２は組み合わせを表す。）、
これを解くと、３ｑ^２−３ｑ−２ｐ＞０ となる。
従って、ｑ＞√（９＋２４ｐ）／６
となる。このため、本実施形態の６軸ロボットの場合は、
ｐ＝４５（＝６×６＋６＋３）となるため、
ｑ＞５．５となり、検出点の数ｑは正の整数であるから、６以上となる。なお、「６×６＋６＋３」中、「６×６」はマニピュレータのパラメータ数であり、「６」は、センサのパラメータ数であり、「３」は直線部３２のパラメータ数である。

次に、ロボット制御装置ＲＣは、レーザ変位センサＬＳでそれぞれ測定した点（以下、検出点という）における検出値をセンサインターフェイスユニット２４を介して取り込み、記憶部２３に記憶する。図２では、前記検出点は、姿勢１〜姿勢３において、それぞれ測定した検出点をＫ１〜Ｋ３で示している。

この後、ロボット制御装置ＲＣのＣＰＵ２０は、前記検出値と、予め記憶部２３に記憶されている、マニピュレータＭのリンクパラメータの初期値を使用して、マニピュレータに関する第１較正値と、レーザ変位センサＬＳの取付誤差を求める。なお、リンクパラメータは、例えば、デナビット・ハーテンバーグ法(Denavit-Hartenberg method )により、マニピュレータＭを構成している各アーム寸法と各回転軸の配置を定義したものであり、公知であるため、説明を省略する。前記リンクパラメータの初期値は記憶部２３に格納されている。

（較正値の算出）
次に、ＣＰＵ２０が実行する較正値の算出方法について説明する。
まず、本実施形態において、基本要素となる座標系、座標変換行列（同次変換行列）及びベクトルの定義を図３を参照して説明する。なお、本実施形態で説明する各座標系は右手直交系である。

図３中、Ｔｉは、姿勢ｉ（ｉ＝１，２，３……）におけるロボット座標系を基準としたマニピュレータＭ先端のＪ６基準座標系（すなわち、マニピュレータ先端座標系）の原点位置姿勢であって観測により求めることができる。Ｔｉは、観測により求まるエンコーダ値と、求める第１較正値となる未知パラメータより成り立つ。なお、Ｊ６は、マニピュレータＭの回転軸をベース部材１２側から順に数えて第６番目を表している。従って、Ｊ６基準座標系とは、第６番目の回転軸を基準とした座標系のことである。

又、ロボット座標系は、図３に示すようにベース部材１２の特定点を原点とする座標系であり、基準座標系に相当する。前記特定点は、ベース部材１２上の任意の点でよい。
Ｃは、マニピュレータＭ先端のＪ６基準座標系を基準としたレーザ照射部Ｌｓａの座標系の原点位置姿勢であって未知数であり、本実施形態において算出する較正値（すなわち、第２較正値）である。ここで、レーザ照射部Ｌｓａの座標系は、センサ座標系に相当する。センサ座標系は、レーザの照射方向をｘ軸（＋）方向とした右手直交系である。

Ｆ_ｉは、姿勢ｉにおけるレーザ照射部Ｌｓａの座標系（センサ座標系）を基準とした検出点の位置であり、レーザ変位センサＬＳの出力（すなわち、検出値）により求めることができる。

Ｐ_ｉは、姿勢ｉにおけるロボット座標系（基準座標系）を基準とした検出点の位置であり、Ｔ_ｉ、Ｃ、Ｆ_ｉから求めることができる。
なお、Ｐ_ｉ，Ｐ_ｊは、リンクパラメータと、視覚センサが検出した値とから、第１較正値となる未知パラメータを含むＴｉと、第２較正値Ｃとを未知変数とした関数に相当する。

前記リンクパラメータの初期値は、後述する繰り返し演算のときのＴ_ｉに関する第１較正値ｘ_ａｊ，ｙ_ａｊ，ｚ_ａｊ，α_ａｊ，β_ａｊ，μ_ａｉを求める際の仮の実数A' の初期値となる。ここで、ｘ，ｙ，ｚ，α，β，μのサフィックスａは、これらの値が仮の実数であることを意味している。

Ｃについての初期値は、レーザ変位センサＬＳを取り付けるブラケットＢｒとレーザ変位センサＬＳ自身の形から、較正値Ｃの併進とロール・ピッチ・ヨーのパラメータの近似値として得られる。これら初期値は、予め記憶部２３に格納されている。

Ｌは、ロボット座標系を基準とした直線部３２の単位方向ベクトルであって、未知の値である。なお、Ｔ_ｉ、Ｃは４×４の同次変換行列であり、Ｆ_ｉ、Ｐ_ｉ及びＬは４次元ベクトルである。

次に、前記各基本要素についてＣＰＵ２０が行う算出について説明する。
マニピュレータＭにおいて、姿勢ｉにおけるｊ−１軸先端の座標系を基準としたｊ軸先端の座標系の原点位置姿勢の同次変換行列をＡ_ｉｊとする。Ａ_ｉｊを併進とロール・ピッチ・ヨーのパラメータ（ｘ，ｙ，ｚ，ｙａｗ，ｐｉｔｃｈ，ｒｏｌｌ）＝（ｘ_ａｊ，ｙ_ａｊ，ｚ_ａｊ，α_ａｊ，β_ａｊ，γ_ａｉｊ）で表すと式（１）となる。

ここで、ｘ_ａｊ，ｙ_ａｊ，ｚ_ａｊ，α_ａｊ，β_ａｊはロボット毎に決定される値である。又、γ_ａｉｊは、マニピュレータＭの各回転軸を駆動するモータに設けられたロータリーエンコーダのエンコーダ値ε_ｉｊとロボット毎に決定されるμ_ａｉの和で表される値である。ｘ_ａｊ，ｙ_ａｊ，ｚ_ａｊ，α_ａｊ，β_ａｊ，μ_ａｉは本実施形態で求める前記Ｔｉに関する第１較正値である。

又、本実施形態では、Ｔｉはベース部材１２から６つ目のアームであるＡ_ｉ６先端の座標なので式（３）で表される。

レーザ照射部Ｌｓａの座標系（センサ座標系）を基準とした姿勢ｉにおける検出点の位置Ｆｉは式（４）で表される。ここでｙ_fｉ，ｚ_fｉ はレーザ変位センサＬＳにより取得される値である。なお、式（４）式及び式（５）中の「Ｔ」は転置行列を表している。

ロボット座標系（基準座標系）を基準とした直線部３２の単位方向ベクトルＬは式（５）で表される。ここでｘ_l，ｙ_l，ｚ_lは未知の値であって、ベクトル変数である。

そして、ＣＰＵ２０は、ロボット座標系（基準座標系）を基準とした姿勢ｉにおける検出点の位置Ｐ_ｉを式（６）で算出する。

この式（６）の計算処理は、第２ステップに相当する。

（拘束条件、及び較正値のＣＰＵ２０による算出）
上記のように互いに異なる検出点は全て直線部３２上であるため、ｉ番目の検出点からｊ番目の検出点に向かうベクトルは直線部３２の方向ベクトルとなる。この方向ベクトルは正規化すると単位方向ベクトルとなり、ロボット座標系を基準とした直線部３２の単位方向ベクトルＬに等しくなる。式（７）中、左辺において、分子は方向ベクトルを示し、分母は正規化していることを示している。

なお、前記式（７）は四次元のベクトル式であるが、四次元目が全て０であるため、実質はＣとＬに含まれる未知数（すなわち、前記較正値）を変数とした関数を要素とする３次元ベクトルであり、要素ごとの３つの方程式が成り立つ。又、検出点の検出、すなわち、観測位置姿勢の組の総数を考慮すると、ＣとＬに含まれる未知数を変数とした方程式は、取得した観測位置姿勢の数をnとすると、３×（ｎ（ｎ−１））／２個取得できる。例えば、観測位置姿勢ｎが６の場合、４５個の方程式を取得できる。

ＣＰＵ２０は、これらの方程式を生成して、すなわち連立方程式を生成して、最小二乗法を使用することにより、未知数を求める。すなわち、較正値ｘ_ａｊ，ｙ_ａｊ，ｚ_ａｊ，α_ａｊ，β_ａｊ，μ_ａｉ、及び較正値Ｃを求める。なお、ＣＰＵ２０のここでの処理は第３ステップに相当する。

（ＣＰＵ２０による線形化と繰り返し演算）
ここで、線形化について述べる。本実施形態では、簡単化のために変数の線形化を行う。又、未知数が多い理由から、説明を簡略化するとともに、誤差変数を微少値として扱うために、変数（すなわち、未知数）をＡという一般形で表して説明する。ここでは、変数Aを仮の実数A' と、AとA' との差を表す誤差変数ΔAとに分割し、Aをそれらの和（A' +ΔA）で表す。すなわち、A＝A' +ΔAで表す。ここで、未知数Ａを、前記較正値（すなわち、第１較正値及び第２較正値を含む）として考えるものとする。

そして、前項までで説明した前記較正値（すなわち、未知数Ａ）に関する計算を行うものとする。式（７）において作成された方程式を解く前に誤差変数ΔAが微少値ということを利用し、前記方程式の解を得るために、ＣＰＵ２０により、各式を１次のマクローリン展開による線形化を行う。

ここでマクローリン展開とは元関数をｎ次微分し、原点近傍の超平面の傾きを算出することで、原点周辺のｎ次の近似式を求める方法である。本実施形態では１次近似を行っているため、非常に簡単な式で記述できる。すなわち、式（７）を式（８）のように表すと、マクローリン展開の１次近似の式は式（９）にすることができる。

又、本実施形態では、ＣＰＵ２０は、様々な検出点ｉ，ｊの組により、式（９）式から、最小二乗法により全ての未知数Ａを求める、すなわち、初期値を除いた第１較正値ｘ_ａｊ，ｙ_ａｊ，ｚ_ａｊ，α_ａｊ，β_ａｊ，μ_ａｉ、及び第２較正値Ｃを算出する。なお、算出した結果である第１較正値ｘ_ａｊ，ｙ_ａｊ，ｚ_ａｊ，α_ａｊ，β_ａｊ，μ_ａｉ、及び第２較正値Ｃには線形化により誤差が生じるため、ＣＰＵ２０はＡ（＝A' ＋ΔＡ）を新たな、A' として更新した後、始めから計算を行う。

そして、誤差ΔＡを構成している各変数、例えば、第２較正値Ｃの場合は、Δｘｃ，Δｙ_ｃ，Δｚ_ｃ，Δα_ｃ，Δβ_ｃ，Δγ_ｃの大きさが下記の式（１２）、式（１３）を満足する迄、この処理を繰り返す。

このことにより、最終的に求められる第２較正値Ｃの精度を保障している。式（１２）、式（１３）は、ＣＰＵ２０が行う閾値Ｓ，及び閾値θとの比較処理の例である。閾値Ｓ、閾値θはそれぞれ、位置及び姿勢の閾値である。

なお、Ｔｉに関する第１較正値ｘ_ａｊ，ｙ_ａｊ，ｚ_ａｊ，α_ａｊ，β_ａｊ，μ_ａｉに関しても、Ｃと同様にそれぞれ設けられた閾値よりも小さな値となるまで、ＣＰＵ２０はＡ（＝A' ＋ΔＡ）を新たなA' として更新した後、前記処理を繰り返す。このことにより、最終的に求められるＴｉに関する第１較正値ｘ_ａｊ，ｙ_ａｊ，ｚ_ａｊ，α_ａｊ，β_ａｊ，μ_ａｉの精度を保障している。

さて、本実施形態によれば、以下のような特徴がある。
（１） 本実施形態のキャリブレーション方法は、第１ステップとして、マニピュレータＭの周囲に配置された治具Ｇの直線部３２上の複数の点を、マニピュレータＭの互いに異なる複数の観測位置姿勢とらせた状態で、レーザ変位センサＬＳによりそれぞれ検出する。また、第２ステップとして、ロボット座標系（基準座標系）を基準としたＪ６基準座標系（マニピュレータ先端座標系）の原点位置姿勢Ｔｉと、マニピュレータＭ先端を基準とするレーザ変位センサＬＳのレーザ照射部Ｌｓａの座標系（センサ座標系）の原点位置姿勢Ｃを未知変数とした関数Ｐｉ，Ｐｊであるとともに、直線部３２上の検出点の基準座標系座標（ロボット座標系座標）を、観測位置姿勢毎にそれぞれ求める。

また、第３ステップとして、観測位置姿勢毎に求めた直線部３２上の検出点の２つを一組としたとき、複数の組から関数Ｐｉ，Ｐｊを要素とするとともに、前記ロボット座標系（基準座標系）を基準とした直線部３２の単位方向ベクトルを複数算出する。さらに、算出した直線部３２の単位方向ベクトルと、ロボット座標系（基準座標系）のベクトル変数（ｘ_l，ｙ_l，ｚ_l）で表される直線部３２の単位方向ベクトルＬが等しいことを利用して連立方程式を作る。さらに、最小二乗法により、マニピュレータＭに関する第１較正値を求めるとともに、マニピュレータＭ先端を基準とするレーザ照射部Ｌｓａの座標系（センサ座標系）の原点位置姿勢Ｃを第２較正値として求める。

この結果、直線部３２を有するだけの簡単な構成の治具Ｇにより、センサのキャリブレーションのみならずマニピュレータＭのキャリブレーションをどこでも簡単に行うことができる。又、治具の準備などの作業工数を大幅に削減できるばかりか、特別な治具が準備できない環境でも即座にマニピュレータＭのキャリブレーションを行うことが可能であり、マニピュレータＭに求められる本来の作業を行うための不必要な待ち時間や工数が削減できる。

（２） 本実施形態のキャリブレーション方法では、連立方程式が線形化して一次近似式にされているため、第３ステップの処理で行う計算を簡便化でき、線形化して一次近似式にしない場合に比して、演算に係る時間を短くできる。

（３）本実施形態のキャリブレーション方法は、第１較正値及び第２較正値Ｃが、それぞれ仮の実数（近似値）と誤差変数の和で成り立っているものとし、第３ステップでは、第１較正値及び第２較正値Ｃがそれぞれ得られたとき、仮の実数を、該得られた第１較正値及び第２較正値Ｃに更新した後、誤差変数が予め定められた閾値以下になるまで第３ステップの処理を繰り返すようにした。

この結果、本実施形態によれば、連立方程式を線形化すると誤差が生じるが、この誤差を閾値以下にすることができ、較正値の精度を保障することができる。
（４） 本実施形態のキャリブレーション方法では、治具Ｇは、複数の面と面間に設けられた複数の稜線を有し、直線部３２を複数の稜線のうち、１つの稜線で構成している。この結果、治具Ｇの稜線を直線部３２としているため、特別な治具が必要でなくなり、直方体、平板等の直線部を面間に有する既存の治具を使用することができ、或いは単純な形状の治具で良くなり、キャリブレーションのための特別な形状の治具を必要としない。

（５） 本実施形態のロボット制御システム１０は、ロボット制御装置ＲＣのＣＰＵ２０が、制御手段として、マニピュレータＭの周囲に配置された治具Ｇの直線部３２上の複数の点を検出するためにマニピュレータＭの互いに異なる複数の観測位置姿勢をとらせる。

又、ロボット制御装置ＲＣの記憶部２３は記憶手段として、マニピュレータＭの互いに異なる複数の観測位置姿勢毎に、レーザ変位センサＬＳが治具Ｇの直線部３２上の異なる複数の点についてそれぞれ検出した値を記憶するようにした。又、ＣＰＵ２０は、検出点座標算出手段として、ロボット座標系（基準座標系）を基準としたＪ６基準座標系（マニピュレータ先端座標系）の原点位置姿勢と、マニピュレータＭ先端を基準とするレーザ変位センサＬＳのレーザ照射部Ｌｓａの座標系（センサ座標系）の原点位置姿勢を未知変数とした関数であるとともに、直線部３２上の検出点のロボット座標系（基準座標系）座標を、観測位置姿勢毎にそれぞれ求めるようにした。さらに、ＣＰＵ２０は、較正値算出手段として、観測位置姿勢毎に求めた直線部３２上の検出点の２つを一組としたとき、複数の組から関数Ｐｉ，Ｐｊを要素とするとともに、前記ロボット座標系（基準座標系）を基準とした直線部３２の単位方向ベクトルを複数算出する。さらに、ＣＰＵ２０は、較正値算出手段として、算出した直線部３２の単位方向ベクトルと、ロボット座標系（基準座標系）のベクトル変数（ｘl，ｙl，ｚl）で表される直線部３２の単位方向ベクトルＬが等しいことを利用して連立方程式を作る。さらに、ＣＰＵ２０は、較正値算出手段として、最小二乗法により、マニピュレータＭに関する第１較正値を求めるとともに、マニピュレータＭ先端を基準とするレーザ照射部Ｌｓａの座標系（センサ座標系）の原点位置姿勢Ｃを第２較正値として求める。

この結果、本実施形態のロボット制御システム１０は、前記（１）のキャリブレーション方法を実行するロボット制御システムを提供することができる。
（６） 本実施形態のロボット制御システム１０は、ＣＰＵ２０（較正値算出手段）は、前記連立方程式を、線形化して一次近似式にした上で較正値Ｃを算出するようにした。この結果、ロボット制御システム１０は、第３ステップの処理で行う計算を簡便化でき、線形化して一次近似式にしない場合に比して、演算に係る時間を短くできるキャリブレーション方法を実行するロボット制御システムを提供することができる。

（７） 本実施形態のロボット制御システム１０では、ＣＰＵ２０（較正値算出手段）は、第１較正値及び第２較正値Ｃが、それぞれ仮の実数と誤差変数の和で成り立っているものとし、第１較正値及び第２較正値Ｃがそれぞれ得られたとき、仮の実数を、該得られた第１較正値及び第２較正値Ｃに更新した後、誤差変数が予め定められた閾値以下になるまで、処理を繰り返すようにした。

この結果、本実施形態によれば、連立方程式を線形化すると誤差が生じるが、この誤差を閾値以下にすることができ、較正値の精度を保障することができる。
（８） なお、従来技術は、２次元画像情報や三次元センサなどを対象にしたものが多く２次元レーザセンサを対象にしたセンサキャリブレーションは提案されていない。前記特許文献１〜３は、ＣＣＤカメラを利用したキャリブレーションの方法であり、これらの方法では、センサ原点に対しＹ軸とそれに対応するＺ軸の１点しか取得できないレーザセンサではキャリブレーションが実現できない。これに対して、本実施形態の方法及びロボット制御システム１０は二次元レーザセンサのセンサキャリブレーションができる。

なお、本発明は前記実施形態に限定されるものではなく、下記のように構成してもよい。
・ 前記実施形態では、視覚センサを二次元レーザセンサとしたが、二次元レーザセンサに限定されるものではなく、ＣＣＤカメラ等の撮像手段であってもよい。この場合、撮像手段が取得して得られた画像中において、任意の画素を基準として、例えば撮像した画像の中心位置を前記基準として、該中心位置に最も近位の直線部の画素の点を検出点とすれば、以下、前記実施形態と同様にキャリブレーションを行うことができる。

・ 前記実施形態では、治具Ｇを平板、或いは直方体の稜線を直線部としたが、治具Ｇは、平板、或いは直方体に限定されるものではなく、立方体でもよい。或いは、稜線が直線となるものであれば、治具Ｇの形状は限定されるものではない。前記直線となった稜線を直線部とすることもできる。

・ 又、直線部は、稜線に限定されるものではない、例えば、平面を有する一側面、例えば、上面、或いは上面に隣接する側面に対して凹設された直線上の溝を直線状に形成して直線部としてもよい。前記溝は、レーザ変位センサＬＳで検出できる深さを有している。さらに、直線部３２は、溝に限定されるものではなく、前記上面に突出されるとともに直線上に上面に沿って位置方向に延出された突条であってもよい。この場合、突条は、レーザ変位センサＬＳにより、上面からの高さが検出できる高さを有するものとする。

又、直線部３２が設けられる治具Ｇの面は、上面に限定されるものではなく、上面に隣接する側面であってもよい。
・ 又、治具Ｇの上面の一部に対して、上方に高さを有する段部を設け、前記上面に沿って前記段部が一方向に直線状に延びるように形成して、その直線状の部分を直線部としてもよい。

・ 前記実施形態では、６軸ロボットで説明したが、６軸ロボットに限定されるものではない。前記方法、及びロボット制御システムは、６軸よりも少ない５軸、４軸等の軸数が少ないロボット、或いは、７軸以上のロボットにも変更してもよい。

この場合、前記実施形態の式（２）において、マニピュレータＭ先端の座標系を、変更後の軸数に応じて変更すればよい。例えば、７軸ロボットのときは、Ｔｉはベース部材１２から７つ目のアームであるＡ_ｉ７先端の座標となるように変更し、
Ｔ７＝Ａ_ｉ１Ａ_ｉ２Ａ_ｉ３Ａ_ｉ４Ａ_ｉ５Ａ_ｉ６Ａ_ｉ７
とすればよい。又、この場合、ＣとＬに含まれる未知数となる変数が増加するため、その分、６軸ロボットの観測位置姿勢の数よりも観測位置姿勢ｎを増加すればよい。

検出点の数は、ロボットの軸数に応じて前述した、
ｑ＞√（９＋２４ｐ）／６を満足する正の整数以上とすればよい。
・ 前記実施形態では、連立方程式をマクローリン展開により線形化を行い、計算を簡単にしたが、マクローリン展開せずに、非線形のままで前記連立方程式の解を求めても良い。

Ｍ…マニピュレータ、Ｇ…治具、ＲＣ…ロボット制御装置、
ＬＳ…レーザ変位センサ（視覚センサ）、
１０…ロボット制御システム、
２０…ＣＰＵ（制御手段、検出点座標算出手段、較正値算出手段）、
２１…ＲＯＭ、２３…記憶部（記憶手段）、３２…直線部。

Claims (7)

1. マニピュレータの周囲に配置された治具の直線部上の複数の点を、互いに異なる複数の観測位置姿勢をとらせた状態で、前記マニピュレータ先端に設けた視覚センサによりそれぞれ検出する第１ステップと、
   基準座標系を基準としたマニピュレータ先端座標系の原点位置姿勢と、前記マニピュレータ先端を基準とする前記視覚センサのセンサ座標系の原点位置姿勢を未知変数とした関数であるとともに、前記直線部上の点（以下、検出点という）の基準座標系座標を、前記観測位置姿勢毎にそれぞれ求める第２ステップと、
   前記観測位置姿勢毎に求めた前記直線部上の検出点の２つを一組としたとき、複数の組から前記関数を要素とするとともに、前記基準座標系を基準とした前記直線部の単位方向ベクトルを複数算出し、算出した前記直線部の単位方向ベクトルと、基準座標系のベクトル変数で表される前記直線部の単位方向ベクトルが等しいことを利用して連立方程式を作り、最小二乗法により、前記マニピュレータに関する第１較正値を求めるとともに、前記視覚センサのセンサ座標系の原点位置姿勢であって、前記マニピュレータ先端を基準とするセンサ座標系の原点位置姿勢を第２較正値として求める第３ステップを含む
   ことを特徴とするマニピュレータのキャリブレーション方法。
2. 前記連立方程式は、線形化して一次近似式にしたものであることを特徴とする請求項１に記載のマニピュレータのキャリブレーション方法。
3. 前記第１較正値及び第２較正値が、それぞれ仮の実数と誤差変数の和で成り立っているものとしたとき、
   前記第３ステップでは、第１較正値及び第２較正値がそれぞれ得られたとき、前記仮の実数を、該得られた第１較正値及び第２較正値に更新した後、前記誤差変数が予め定められた閾値以下になるまで前記第３ステップの処理を繰り返すことを特徴とする請求項２に記載のマニピュレータのキャリブレーション方法。
4. 前記治具は、複数の面と面間に設けられた複数の稜線を有し、
   前記直線部は、複数の稜線のうち、１つの稜線であることを特徴とする請求項１乃至請求項３のうちいずれか１項に記載のマニピュレータのキャリブレーション方法。
5. マニピュレータの周囲に配置された治具の直線部上の複数の点を検出するために前記マニピュレータの互いに異なる複数の観測位置姿勢をとらせる制御手段と、
   前記マニピュレータの互いに異なる複数の観測位置姿勢毎に、マニピュレータ先端に設けられた視覚センサが前記治具の直線部上の異なる複数の点についてそれぞれ検出した値を記憶する記憶手段と、基準座標系を基準としたマニピュレータ先端座標系の原点位置姿勢と、前記マニピュレータ先端を基準とする前記視覚センサのセンサ座標系の原点位置姿勢を未知変数とした関数であるとともに、前記直線部上の点（以下、検出点という）の基準座標系座標を、前記観測位置姿勢毎にそれぞれ求める検出点座標算出手段と、
   前記観測位置姿勢毎に求めた前記直線部上の検出点の２つを一組としたとき、複数の組から前記関数を要素とするとともに、前記基準座標系を基準とした前記直線部の単位方向ベクトルを複数算出し、算出した前記直線部の単位方向ベクトルと、基準座標系のベクトル変数で表される前記直線部の単位方向ベクトルが等しいことを利用して連立方程式を作り、最小二乗法により、前記マニピュレータに関する第１較正値を求めるとともに、前記マニピュレータ先端を基準とするセンサ座標系の原点位置姿勢を第２較正値として求める処理を行う較正値算出手段を含む
   ことを特徴とするロボット制御システム。
6. 前記較正値算出手段は、前記連立方程式を、線形化して一次近似式にした上で前記較正値を算出するものである請求項５に記載のロボット制御システム。
7. 前記較正値算出手段は、前記第１較正値及び第２較正値が、それぞれ仮の実数と誤差変数の和で成り立っているものとし、第１較正値及び第２較正値がそれぞれ得られたとき、前記仮の実数を、該得られた第１較正値及び第２較正値に更新した後、前記誤差変数が予め定められた閾値以下になるまで、前記処理を繰り返すことを特徴とする請求項６に記載のロボット制御システム。

Images