JP2011230249A - 視覚センサを有するマニピュレータのセンサキャリブレーション方法及びロボット制御システム - Google Patents

Abstract

【課題】
視覚センサを持つマニピュレータのキャリブレーションをどこでも簡単に行うことができ、作業工数を大幅に削減でき、特別な治具が準備できない環境でも即座にキャリブレーションを行うことを可能とする。
【解決手段】
マニピュレータＭの周囲に配置された治具Ｇの直線部上の点を複数の観測位置姿勢毎にレーザ変位センサＬＳによりそれぞれ検出する。次にリンクパラメータと、レーザ変位センサＬＳが検出した値から直線部上の検出点であるロボット座標系の座標を、観測位置姿勢毎にそれぞれ求める。検出点の２つを一組として複数の組からロボット座標系を基準とした直線部の単位方向ベクトルを複数算出する。算出した直線部の単位方向ベクトルと、ロボット座標系のベクトル変数で表される直線部の単位方向ベクトルから連立方程式を作り、最小二乗法により較正値を求める。
【選択図】図１

Description

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

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

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

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

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

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

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

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

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

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

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

請求項２の発明は、請求項１において、前記連立方程式は、線形化して一次近似式にしたものであることを特徴とする。
請求項３の発明は、請求項２において、前記較正値を、近似値と同次変換行列の誤差の積で表わすとともに、予め得られている前記マニピュレータ先端を基準とするセンサ座標系の原点位置姿勢を前記近似値の初期値とし、前記第３ステップにおいて、前記較正値が得られたとき、前記近似値を該得られた較正値に更新した後、前記誤差に含まれる変数の値が予め定められた閾値以下になるまで前記第３ステップの処理を繰り返すことを特徴とする。

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

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

請求項６の発明は、請求項において、前記較正値算出手段は、前記連立方程式を、線形化して一次近似式にした上で前記較正値を算出するものであることを特徴とする。
請求項７の発明は、請求項６において、前記較正値算出手段は、前記較正値を、近似値と同次変換行列の誤差の積で表わすとともに、予め得られている前記マニピュレータ先端を基準とするセンサ座標系の原点位置姿勢を前記近似値の初期値とし、前記較正値を得たとき、前記近似値を前記得た較正値に更新した後、前記誤差に含まれる変数の値が予め定められた閾値以下になるまで前記処理を繰り返すことを特徴とする。

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

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

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

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

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

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

以下、本発明を具体化した視覚センサを有するマニピュレータのセンサキャリブレーション方法及びロボット制御システムを具体化した一実施形態を図１〜３を参照して説明する。

図１はロボット制御システム１０の構成を示すブロック図である。ロボット制御システム１０は、ワーク（作業対象物）に対して、特定の作業を行うように制御するものである。ロボット制御システム１０は、前記作業を行うマニピュレータＭと、マニピュレータＭを制御するロボット制御装置ＲＣと、ワークの形状を検出する視覚センサ及びレーザセンサとしてのレーザ変位センサＬＳとを備える。

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

ロボット制御装置ＲＣは、コンピュータからなる。すなわち、ロボット制御装置ＲＣはＣＰＵ（中央処理装置）２０、マニピュレータＭを制御するための各種プログラムを記憶する書き換え可能なＲＯＭ２１や、作業メモリとなるＲＡＭ２２、各種データを記憶する書換可能な不揮発性メモリからなる記憶部２３を備える。ＣＰＵ２０は、制御手段、検出点座標算出手段、及び較正値算出手段に相当する。又、記憶部２３は記憶手段に相当する前記ＲＯＭ２１には、センサキャリブレーション、及び、前記ツールの特定の作業を行わせるための各種のロボット制御プログラムが記憶されている。そして、前記プログラムに従って前記ＣＰＵ２０が、各アーム１３間に設けられた図示しないモータを制御駆動することにより、前記ツールの位置姿勢及びレーザ変位センサＬＳの観測位置姿勢が変化する。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

次に、前記各基本要素についてＣＰＵ２０が行う算出について説明する。
前記Ｔ_ｉは、下記のようにして、ＣＰＵ２０により算出される。
マニピュレータＭにおいて、姿勢ｉにおけるｊ−１軸先端の座標系を基準としたｊ軸先端の座標系の原点位置姿勢の同次変換行列をＡ_ｉｊとする。Ａ_ｉｊを併進とロール・ピッチ・ヨーのパラメータ（ｘ，ｙ，ｚ，ｙａｗ，ｐｉｔｃｈ，ｒｏｌｌ）＝（ｘ_ａｊ，ｙ_ａｊ，ｚ_ａｊ，α_ａｊ，β_ａｊ，γ_ａｉｊ）で表すと式（１）となる。ここで、ｘ_ａｊ，ｙ_ａｊ，ｚ_ａｊ，α_ａｊ，β_ａｊはロボット毎に決定される定数であって、リンクパラメータが含まれている。又、γ_ａｉｊは、マニピュレータＭの各回転軸を駆動するモータに設けられたロータリーエンコーダのエンコーダ値より求められる。

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

又、レーザ変位センサＬＳを取り付けるブラケットＢｒとレーザ変位センサＬＳ自身の形から、較正値Ｃの併進とロール・ピッチ・ヨーのパラメータの近似値（ｘ，ｙ，ｚ，ｙａｗ，ｐｉｔｃｈ，ｒｏｌｌ）＝（ｘ_ｃ，ｙ_ｃ，ｚ_ｃ，α_ｃ，β_ｃ，γ_ｃ）が得られるとすると、マニピュレータＭ先端のＪ６基準座標系を基準としたレーザ照射部Ｌｓａの同次変換行列の近似値Ｃａは式（３）となる。近似値Ｃａは、前述したように、予め、記憶部２３に格納されている。

ここで、ΔＣａを較正値Ｃと近似値Ｃａ の誤差を示す同次変換行列とすると、較正値Ｃ は式（４）のようにΔＣａと近似値Ｃａの積で表される。

ここで、ΔＣａは誤差なので各要素は微少と仮定できる。従って、ΔＣａは併進とロール・ピッチ・ヨーの微少な未知パラメータ（ｘ，ｙ，ｚ，ｙａｗ，ｐｉｔｃｈ，ｒｏｌｌ）＝（Δｘｃ，Δｙｃ，Δｚｃ，Δαｃ，Δβｃ，Δγｃ）を使い式（５）で表すことができる。

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

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

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

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

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

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

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

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

又、本実施形態では、ＣＰＵ２０は、様々な検出点ｉ，ｊの組により、式（１１）式から、最小二乗法により全ての未知数を求める、すなわち、較正値Ｃを算出する。なお、算出した結果である較正値Ｃには線形化により誤差が生じるため、ＣＰＵ２０は、Ｃ（＝ΔＣａ Ｃａ）を新たな近似値Ｃａとして更新した後、始めから計算を行う。

そして、誤差ΔＣａを構成している各変数Δｘｃ，Δｙｃ，Δｚｃ，Δαｃ，Δβｃ，Δγｃの大きさが下記の式（１２）、式（１３）を満足する迄、繰り返す。このことにより、最終的に求められる較正値Ｃの精度を保障している。式（１２）、式（１３）は、ＣＰＵ２０が行う閾値Ｓ，及び閾値θとの比較処理である。閾値Ｓ、閾値θはそれぞれ、位置及び姿勢の閾値である。

さて、本実施形態によれば、以下のような特徴がある。

（１） 本実施形態のセンサキャリブレーション方法は、第１ステップとして、マニピュレータＭの周囲に配置された治具Ｇの直線部３２上の点を、マニピュレータＭの互いに異なる複数の観測位置姿勢毎に、レーザ変位センサＬＳによりそれぞれ検出する。また、第２ステップとして、マニピュレータＭのリンクパラメータと、レーザ変位センサＬＳが検出した値とから、較正値Ｃを未知変数とした関数Ｐ_ｉ，Ｐ_ｊであって、直線部３２上の検出点であるロボット座標系の座標を、観測位置姿勢毎にそれぞれ求める。又、第３ステップとして、前記観測位置姿勢毎に求めた直線部３２上の検出点の２つを一組とし、複数の組から前記関数を要素とするとともに、ロボット座標系を基準とした直線部３２の単位方向ベクトルを複数算出する。そして、算出した直線部３２の単位方向ベクトルと、ロボット座標系のベクトル変数（ｘ_l，ｙ_l，ｚ_l）で表される直線部３２の単位方向ベクトルが等しいことを利用して連立方程式を作り、最小二乗法により較正値Ｃを求めるようにした。

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

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

（３）本実施形態のセンサキャリブレーション方法は、較正値Ｃを、近似値Ｃａと同次変換行列の誤差ΔＣａの積で表わすとともに、予め得られているマニピュレータＭ先端を基準とするレーザ照射部Ｌｓａの座標系（センサ座標系）の原点位置姿勢を近似値Ｃａの初期値とする。そして、第３ステップにおいて、較正値が得られたとき、前記近似値を、該得られた較正値に更新した後、誤差ΔＣａに含まれる変数の値が予め定められた閾値以下になるまで第３ステップの処理を繰り返すようにした。この結果、本実施形態によれば、連立方程式を線形化すると誤差が生じるが、この誤差を閾値以下にすることができ、較正値の精度を保障することができる。

（４） 本実施形態のセンサキャリブレーション方法では、治具Ｇは、複数の面と面間に設けられた複数の稜線を有し、直線部３２を複数の稜線のうち、１つの稜線で構成している。この結果、治具Ｇの稜線を直線部３２としているため、特別な治具が必要でなくなり、直方体、平板等の直線部を面間に有する既存の治具を使用することができ、或いは単純な形状の治具で良くなり、キャリブレーションのための特別な形状の治具を必要としない。

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

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

（７） 本実施形態のロボット制御システム１０は、ＣＰＵ２０（較正値算出手段）は、較正値Ｃを得たとき、近似値を、前記得た較正値Ｃに更新した後、誤差ΔＣａに含まれる変数の値が予め定められた閾値以下になるまで第３ステップの処理を繰り返す。この結果、本実施形態のロボット制御システム１０は、連立方程式を線形化すると誤差が生じるが、この誤差を閾値以下にすることができ、較正値の精度を保障することができるセンサキャリブレーション方法を実行するロボット制御システムを提供できる。

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

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

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

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

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

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

この場合、前記実施形態の式（２）において、マニピュレータＭ先端の座標系を、変更後の軸数に応じて変更すればよい。例えば、７軸ロボットのときは、Ｔｉはベース部材１２から７つ目のアームであるＡ_ｉ７先端の座標となるように変更し、
Ｔ７＝Ａ_ｉ１Ａ_ｉ２Ａ_ｉ３Ａ_ｉ４Ａ_ｉ５Ａ_ｉ６Ａ_ｉ７
とすればよい。

・ 前記実施形態では、連立方程式をマクローリン展開により線形化を行い、計算を簡単にしたが、マクローリン展開せずに、非線形のままで前記連立方程式の解を求めても良い。

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

Claims (7)

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

Images