JP6692854B2

JP6692854B2 - ロボットアームのための負荷推定及び重力補償方法、並びにそのシステム

Info

Publication number: JP6692854B2
Application number: JP2018088744A
Authority: JP
Inventors: 嘉輝簡; 正欽陳; 榮富侯
Original assignee: Hiwin Technologies Corp
Current assignee: Hiwin Technologies Corp
Priority date: 2017-09-21
Filing date: 2018-05-02
Publication date: 2020-05-13
Anticipated expiration: 2038-05-02
Also published as: US20190084154A1; DE102018209594A1; TWI642523B; US10702988B2; TW201914788A; DE102018209594B4; JP2019193965A

Description

本発明は負荷推定及び重力補償方法、並びにそのシステムに関し、特に、ロボットアームのための負荷推定及び重力補償方法、並びにそのシステムに関する。

ロボットアームは、高耐久性、自動制御、高精度などの特徴を備えるので、製造産業に幅広く利用されている。ロボットアームのコンプライアンス制御では、ロボットアームは、動作中に（単独操作又はマン−マシン操作のオペレータによる操作）外部負荷を受ける際に、ロボットアームの出力のパラメータは、その外部負荷に基づいて、対応する重力補償を与えることにより、該外部負荷を受けながら正常に動作できることが保証される。

ロボットアームの負荷が変わる際に、その負荷に対応する重力補償を与えるために、予めロボットアームにかかる負荷の変動量を得る必要がある。そのために、一般的に、負荷がロボットアームにかかる前に、該負荷の重量を測る。そして、その重量をロボットアームに入力することにより、新しい重力補償を生成する。しかし、ロボットアームの負荷が変わる度に、上記の方法を繰り返さないといけないので、操作の手順が繁雑になる欠点がある。

重力補償を生成する他の方法として、例えば特許文献１には、ロボットアームの各関節にトルクセンサを設置し、ロボットアームに負荷がかかった後、ロボットアームの機械偏差値を測って、ロボットアームの特定の回転角に対応する重力係数を算出し、そして、校正処理により、トルクセンサのフィードバック及び出力軸の回転角に基づいて重力補償を算出する方法が記載されている。

米国特許第９５３３４１４Ｂ２号明細書

しかし、特許文献１の方法は、高価なトルクセンサを設置する必要があり、そして、ロボットアームの負荷が変わる度に、較正処理を行う必要もある。
このような問題に鑑みて、本発明は、高価なトルクセンサを使用せず、より簡単な手順で重力補償を得られる負荷推定及び重力補償方法、並びにそのシステムを提供することを目的とする

上記目的を達成すべく、本発明は、負荷推定及び重力補償システムによって実行されるロボットアームのための負荷推定及び重力補償方法であって、前記ロボットアームは、少なくとも１つの関節と、前記少なくとも１つの関節に設置された駆動モジュールと、が備わっており、前記負荷推定及び重力補償システムは、前記駆動モジュールに連結された信号処理装置と、前記少なくとも１つの関節に対応すると共に、前記信号処理装置に連結された負荷推定モジュールと、前記負荷推定モジュールに連結された重力補償モジュールと、が備わっており、前記負荷推定モジュールは、重力モジュール補正ユニットと、仮想ロボットアームユニットと、演算部と、が備わっており、前記ロボットアームのための負荷推定及び重力補償方法は、（Ａ）前記駆動モジュールを操作モードで操作し、前記ロボットアームを第１の位置に移動して、現時点の負荷である第１の負荷を該第１の位置における前記ロボットアームに受けさせてから、前記駆動モジュールを位置制御モードに切り替えるステップと、（Ｂ）前記信号処理装置により、前記位置制御モードにおける前記駆動モジュールから発信された、前記第１の負荷に対応して生ずる第１のトルク信号を受信し、前記仮想ロボットアームユニットにより、前記位置制御モードにおける前記駆動モジュールから発信された、前記第１の位置に位置する前記関節の姿勢に対応する第１の関節角度を受信するステップと、（Ｃ）前記信号処理装置が、前記第１のトルク信号を受信した後、更に前記第１のトルク信号を第１のトルク値に変換させるステップと、（Ｄ）前記重力モジュール補正ユニットにより、前記第１のトルク値を受信し、そして、１組の補正パラメータを生成して、前記仮想ロボットアームユニットに送信するステップと、（Ｅ）前記仮想ロボットアームユニットが、前記１組の補正パラメータ及び前記第１の関節角度を受信した後、更に無負荷トルク値及び最大負荷トルク値を生成するステップと、（Ｆ）前記ロボットアームが受ける負荷を未知の負荷に変更するステップと、（Ｇ）前記信号処理装置により、前記駆動モジュールから発信された前記未知の負荷に対応して生ずる第２のトルク信号を受信するステップと、（Ｈ）前記信号処理装置が、前記第２のトルク信号を受信した後、更に前記第２のトルク信号を第２のトルク値に変換させるステップと、（Ｉ）前記演算部により、前記第１のトルク値、前記無負荷トルク値、前記最大負荷トルク値及び前記第２のトルク値に基づいて、前記未知の負荷の推定負荷値を推定するステップと、（Ｊ）前記駆動モジュールをトルク制御モードに切り替えてから、前記ロボットアームを第２の位置に移動するステップと、（Ｋ）前記重力補償モジュールにより、前記駆動モジュールから発信された前記第２の位置に位置する前記関節の姿勢に対応する第２の関節角度を受信するステップと、（Ｌ）前記重力補償モジュールにより、前記推定負荷値及び前記第２の関節角度に基づいて、補償トルク値を生成して、該補償トルク値を前記駆動モジュールに発信するステップと、を含むことを特徴とするロボットアームのための負荷推定及び重力補償方法を提供する。

また、少なくとも１つの関節と、前記少なくとも１つの関節に設置された駆動モジュールと、が備わっているロボットアームに使用する負荷推定及び重力補償システムであって、前記負荷推定及び重力補償システムは、信号処理装置と、負荷推定モジュールと、重力補償モジュールと、が備わっており、前記信号処理装置は、前記ロボットアームが現時点の負荷である第１の負荷を受ける際に、前記駆動モジュールから発信された第１のトルク信号を受信し、前記第１のトルク信号を第１のトルク値に変換するために、且つ、前記ロボットアームが未知の負荷を受ける際に、前記駆動モジュールから発信された第２のトルク信号を受信し、前記第２のトルク信号を第２のトルク値に変換するために、前記駆動モジュールに連接されており、前記負荷推定モジュールは、前記少なくとも１つの関節に対応すると共に、前記第１のトルク値及び前記第２のトルク値を受信するために、前記信号処理装置に連接されていると共に、前記第1のトルク値に基づいて１組の補正パラメータを生成し、前記１組の補正パラメータ及び前記ロボットアームが前記第１の負荷を受ける際に前記駆動モジュールから発信された第１の関節角度に基づいて無負荷トルク値及び最大負荷トルク値を生成し、且つ、前記第１のトルク値、前記無負荷トルク値、前記最大負荷トルク値及び前記第２のトルク値に基づいて、未知の負荷の推定負荷値を推定するようにアレンジされており、前記重力補償モジュールは、前記推定負荷値及び前記ロボットアームが前記未知の負荷を受ける際に前記駆動モジュールから発信された第２の関節角度に基づいて、補償トルク値を生成し、且つ、該補償トルク値を前記駆動モジュールに発信するために、前記負荷推定モジュールに連接されていることを特徴とする負荷推定及び重力補償システムを提供する。

本発明における負荷推定及び重力補償方法は、ロボットアーム２が第１の位置に位置する時点において関節の第１の関節角度及びその時点の負荷である第１の負荷に対応する第１のトルク信号による第１のトルク値と、未知の負荷に対応する関節の第２のトルク信号による第２のトルク値と、第１のトルク値により生成する１組の補正パラメータと、１組の補正パラメータ及び第１の関節角度により生成する無負荷トルク値及び最大負荷トルク値と、に基づいて、該未知の負荷の推定負荷値を推定する。そして、推定負荷値及びロボットアーム２が第２の位置に位置する第２の関節角度に基づいて、補償トルク値を生成して、該補償トルク値を駆動モジュールに発信して、重力補償を行う。従って、従来のような未知の荷重の重量を事前に測定することや、負荷の変化に対する較正プロセスや、高価なトルクセンサの設置などの必要がなく、未知の荷重による重力影響を算出して、重力補償を行うことができる。

本発明における負荷推定及び重力補償システムとロボットアームとの連接関係を説明する図である。本発明における負荷推定及び重力補償方法のプロセスを説明する図である。負荷の作用方向と関節の出力軸の軸方向とが交差又は平行する状況を説明する図である。本発明における実施例において、ロボットアームが第１の位置に位置することを示す側視図である。ロボットアームが第１の位置に位置する場合における、関節の負荷と出力トルクとの関係図である。上記の実施例において、ロボットアームが第２の位置に位置することを示す側視図である。ロボットアームが第２の位置に位置する場合における、関節の負荷と出力トルクとの関係図である。ロボットアームが第２の位置に位置する場合における、関節の負荷と出力トルクとの関係図である。

以下、各図面を参照しながら、本発明における各実施形態について詳しく説明する。

本発明をより詳細に説明する前に、適切と考えられる場合において、符号は、同様の特性を有し得る対応する要素を示すために各図面間で繰り返し用いられることに留意されたい。

本発明におけるロボットアームのための負荷推定及び重力補償方法は、本発明における負荷推定及び重力補償システムによって実行されるロボットアームのための負荷推定及び重力補償方法である。

本発明に係る方法の実施対象であるロボットアーム２は、例えば、図１に示されるように、少なくとも１つの関節２１と、関節２１に設置された駆動モジュール２２と、関節２１を介して互いに接続された少なくとも２つの接続棒と、が備わっている。

以下、負荷推定及び重力補償システムの概要を説明する。

前記負荷推定及び重力補償システムは、駆動モジュール２２に連結された信号処理装置３と、関節２１に対応すると共に、信号処理装置３に連結された負荷推定モジュール４と、負荷推定モジュール４に連結された重力補償モジュール５と、が備わっている。

負荷推定モジュール４は、重力モジュール補正ユニット４１と、重力モジュール補正ユニット４１に連結された仮想ロボットアームユニット４２と、仮想ロボットアームユニット４２に連結された演算部４３と、が備わっている。

以下、本発明における負荷推定及び重力補償システムの構成を詳しく説明する。

本発明における負荷推定及び重力補償システムは、上記のロボットアーム２に使用するシステムであって、図１に示されるように、信号処理装置３と、負荷推定モジュール４と、重力補償モジュール５と、が備わっている。

信号処理装置３は、ロボットアーム２が現時点の負荷である第１の負荷を受ける際に、駆動モジュール２２から発信された第１のトルク信号及び第１の関節角度を受信し、前記第１のトルク信号を第１のトルク値に変換するために、且つ、ロボットアーム２が未知の負荷を受ける際に、駆動モジュール２２から発信された第２のトルク信号及び第２の関節角度を受信し、前記第２のトルク信号を第２のトルク値に変換するために、駆動モジュール２２に連接されている。

負荷推定モジュール４は、関節２１に対応すると共に、前記第１のトルク値及び前記第２のトルク値を受信するために、信号処理装置３に連接されていると共に、前記第1のトルク値に基づいて１組の補正パラメータを生成し、前記１組の補正パラメータ及び第１の関節角度に基づいて無負荷トルク値及び最大負荷トルク値を生成し、且つ、前記第１のトルク値、前記無負荷トルク値、前記最大負荷トルク値及び前記第２のトルク値に基づいて、未知の負荷の推定負荷値を推定するようにアレンジ（プログラム）されている。

重力補償モジュール５は、前記推定負荷値及び前記第２の関節角度に基づいて、補償トルク値を生成し、且つ、該補償トルク値を駆動モジュール２２に発信するために、負荷推定モジュール４に連接されている。

この実施形態において、ロボットアーム２は、図１に示されるように、複数の関節２１と、複数の関節２１を介して互いに接続された複数の接続棒と、複数の関節２１それぞれに設置された複数の駆動モジュール２２と、が備わっている。複数の駆動モジュール２２それぞれは、作動装置２２１と、作動装置２２１に接続されている機械構成２２２（例えば、接続棒と減速装置の組み合わせなど）と、機械構成２２２に接続されているエンコーダ２２３と、が更に備わっている。本実施形態での負荷推定及び重力補償システムにおいて、信号処理装置３は、各作動装置２２１に連接されている。負荷推定モジュール４は、複数あり、複数の関節２１それぞれに対応して設置されていると共に、信号処理装置３に連結されている。重力補償モジュール５は、負荷推定モジュール４に連結されている。

信号処理装置３は、レジスタ及び/又はメモリからの電子データを処理して、その電子データを他の電子データに変換する任意のデバイス又はデバイスの部分を指す。例えば、信号処理装置３として、シングルコアプロセッサ、マルチコアプロセッサ、デュアルコアモバイルプロセッサ、マイクロプロセッサ、マイクロコントローラ、デジタルシグナルプロセッサ（ＤＳＰ）、フィールドプログラマブルゲートアレイ（ＦＰＧＡ）、特定用途向け集積回路（ＡＳＩＣ）、無線周波数集積回路（ＲＦＩＣ）などが挙げられる。

負荷推定モジュール４及び重力補償モジュール５の実施形態としては、ハードウェア、ファームウェア又はソフトウェアの単独又はその組み合わせで実施されてもよい。また、命令が格納されたコンピュータ可読記憶装置として実施されてもよい。この場合、本明細書に記載されている動作を実行するために、少なくとも１つのプロセッサによって、該命令を読み取り且つ実行する。コンピュータ可読記憶装置は、機械（例えば、コンピューター）によって読み取り可能な形式でデータを格納するための任意の非一時的なメモリ機構を含む。例えば、コンピュータ可読記憶装置として、読み出し専用メモリ（ＲＯＭ）、ランダムアクセスメモリ（ＲＡＭ）、磁気ディスク記憶媒体、光学記憶媒体、フラッシュメモリ装置及び他の記憶装置又は媒体などが挙げられる。

複数の関節２１が存在する実施形態において、負荷推定及び重力補償システムは、複数の関節２１それぞれに対応する複数の負荷推定モジュール４が備えてもよい。

操作中、ロボットアーム２が現時点の負荷である第１の負荷を受ける際に、各作動装置２２１は、それぞれ対応する関節２１の第１のトルク信号を発信するように構成され、エンコーダ２２３は、それぞれ対応する関節２１の第１の関節角度を発信するように構成されている。ロボットアーム２が未知の負荷（Ｗ）を受ける際に、駆動モジュール２２の作動装置２２１が機械構成２２２に制御信号を発信して、ロボットアーム２を駆動する。そして、各作動装置２２１は、それぞれ対応する関節２１及び前記未知の負荷（Ｗ）に応じる第２のトルク信号を更に発信するように構成され、エンコーダ２２３は、それぞれ対応する関節２１の別の角度位置に応じる第２の関節角度を発信するように構成されている。

そして、信号処理装置３は、作動装置２２１から発信された前記第１のトルク信号及び前記第２のトルク信号を受信し、関節２１に対応する第１のトルク値及び第２のトルク値に変換する。

負荷推定モジュール４は、前記未知の負荷と前記第１の負荷との負荷差を得て、前記第１の負荷に前記負荷差を足すことにより前記未知の負荷の推定負荷値を推定するようにアレンジされており、そして、重力モジュール補正ユニット４１と、仮想ロボットアームユニット４２と、演算部４３と、が備わっている。

重力モジュール補正ユニット４１は、前記第１のトルク値を受信し、そして、関節２１に対応する１組の補正パラメータを生成する。仮想ロボットアームユニット４２は、該１組の補正パラメータ及び前記第１の関節角度を受信し、そして、無負荷トルク値及び最大負荷トルク値を生成する。具体的には、無負荷トルク値とは、ロボットアーム２が無負荷状態の時にロボットアーム２の関節２１が出力するトルク値であり、最大負荷トルク値は、ロボットアーム２が運搬可能な最大負荷値（即ち、ロボットアーム２のペイロード（最大積載量））に等しい重量を有する負荷をロボットアーム２が受けた時にロボットアーム２の関節２１が出力するトルク値である。

演算部４３は、換算変換ユニット４３１と、換算変換ユニット４３１に接続され、前記第１の負荷に前記負荷差を足すことにより前記未知の負荷の推定負荷値を計算するようにアレンジされている目標負荷計算ユニット４３２と、が備わっている。

換算変換ユニット４３１は、前記負荷差の計算において、ロボットアーム２に受けさせることができる前記最大負荷値、前記無負荷トルク値及び前記最大負荷トルク値に基づいてパラメトリック方程式を確立し、且つ、前記第１のトルク値及び前記第２のトルク値それぞれを前記パラメトリック方程式に適用して前記負荷差を得るようにアレンジされている。

即ち、無負荷トルク値と最大負荷トルク値が得られると、換算変換ユニット４３１は、２つの異なる負荷の下で検出された２つの異なるトルク値（即ち、第１のトルク値及び第２のトルク値）に基づいて負荷差を計算することができる。目標負荷計算ユニット４３２は、前記第１の負荷及び前記負荷差により推定負荷値を計算する（即ち、２つの負荷のうちの１つが既知である場合に、２つの負荷の他の１つの重量を計算する）

重力補償モジュール５は、前記推定負荷値及び前記第２の関節角度に基づいて、補償トルク値を生成して、該補償トルク値を駆動モジュール２２に発信することにより、ロボットアーム２が重力補償の作用下で動作することができる。

ロボットアーム２に複数の関節２１が存在する実施形態において、複数の関節２１は、それぞれの第１のトルク信号、第２のトルク信号、第１のトルク値、第２のトルク値、第１の関節角度、第２の関節角度、１組の補正パラメータ、無負荷トルク値、最大負荷トルク値及び補償トルク値を有する。

以下、本発明におけるロボットアームのための負荷推定及び重力補償方法の概略を説明する。図２に示されるように、当該方法は、以下のステップを含む：
（Ａ）駆動モジュール２２を操作モードで操作し、ロボットアーム２を第１の位置に移動して、現時点の負荷である第１の負荷を該第１の位置におけるロボットアーム２に受けさせてから、駆動モジュール２２を位置制御モードに切り替えるステップ；
（Ｂ）信号処理装置３により、前記位置制御モードにおける駆動モジュール２２から発信された、前記第１の負荷に対応して生ずる第１のトルク信号を受信し、仮想ロボットアームユニット４２により、前記位置制御モードにおける駆動モジュール２２から発信された、前記第１の位置に位置する関節２１の姿勢に対応する第１の関節角度を受信するステップ；
（Ｃ）信号処理装置３が、前記第１のトルク信号を受信した後、更に前記第１のトルク信号を第１のトルク値に変換させるステップ；
（Ｄ）重力モジュール補正ユニット４１により、前記第１のトルク値を受信し、そして、１組の補正パラメータを生成して、仮想ロボットアームユニット４２に送信するステップ；
（Ｅ）仮想ロボットアームユニット４２が、前記１組の補正パラメータ及び前記第１の関節角度を受信した後、更に無負荷トルク値及び最大負荷トルク値を生成するステップ；
（Ｆ）ロボットアーム２が受ける負荷を未知の負荷に変更するステップ；
（Ｇ）信号処理装置３により、駆動モジュール２２から発信された前記未知の負荷に対応して生ずる第２のトルク信号を受信するステップ；
（Ｈ）信号処理装置３が、前記第２のトルク信号を受信した後、更に前記第２のトルク信号を第２のトルク値に変換させるステップ；
（Ｉ）演算部４３により、前記第１のトルク値、前記無負荷トルク値、前記最大負荷トルク値及び前記第２のトルク値に基づいて、前記未知の負荷の推定負荷値を推定するステップ；
（Ｊ）駆動モジュール２２をトルク制御モードに切り替えてから、ロボットアーム２を第２の位置に移動するステップ；
（Ｋ）重力補償モジュール５により、駆動モジュール２２から発信された前記第２の位置に位置する前記関節２１の姿勢に対応する第２の関節角度を受信するステップ；及び
（Ｌ）重力補償モジュール５により、前記推定負荷値及び前記第２の関節角度に基づいて、補償トルク値を生成して、該補償トルク値を駆動モジュール２２に発信するステップ。

まとめると、ステップ（Ａ）〜ステップ（Ｉ）は、負荷推定のステージであり、ステップ（Ｊ）〜ステップ（Ｋ）は、重力補償機能の実行である。

以下、再び図２を参照しながら、本発明におけるロボットアームのための負荷推定及び重力補償方法をより具体的に説明する。

ステップ（Ａ）において、前記操作モードで例えば操作者がロボットアーム２を第１の位置に移動するように操作することにより、作動装置２２１が機械構成２２２に制御信号を発信して、ロボットアーム２が第１の位置に移動されると、ロボットアーム２が第１の位置にいる時点の負荷である第１の負荷Ｗ_ｃを受けさせてから、駆動モジュール２２の作動装置２２１を位置制御モードに切り替える。また、第１の負荷Ｗ_ｃの力の方向は、関節２１の出力軸の軸方向と交差せず且つ平行しないものとする。

図３に示されるように、負荷Ｗの作用方向が関節２１の出力軸の軸方向Ｌと交差（図３の上方の図面に示す）又は平行する（図３の下方の図面に示す）と、モーメントアームが０になるため、トルクが生じなく、未知の負荷を推定できなくなる。従って、全ての負荷Ｗの作用方向は、その負荷がかかる関節２１の出力軸の軸方向Ｌと、交差せず且つ平行しない。

また、この実施形態において、ロボットアーム２に受けさせる負荷Ｗは、単一の機構又は複数の機構の組み合わせであり、特に制限されていない。

また、第１の位置において、駆動モジュール２２は位置制御モードに切り替えられる。

ステップ（Ｂ１）において、各駆動モジュール２２の作動装置２２１が第１の負荷Ｗ_ｃに対応する第１のトルク信号を信号処理装置３に発信する。

ステップ（Ｂ２）において、各駆動モジュール２２のエンコーダ２２３が前記第１の位置に対応する各関節２１の第１の関節角度θを仮想ロボットアームユニット４２に発信する。なお、上記ステップ（Ｂ１）と（Ｂ２）は、同時に進行することも可能である。

ステップ（Ｃ）において、信号処理装置３は作動装置２２１から前記第１のトルク信号を受信した後、前記第１のトルク信号を第１のトルク値Ｃ_ｃ（θ）に変換する。ちなみに、作動装置２２１として使用するモーターが違うと、そのモーターに対応して異なるトルク信号が発信される。トルク信号は、例えば、電圧信号、電流信号などの信号であり、その信号がトルク値に変換される。この実施形態において、前記第１のトルク信号は、電流値で表される電流信号であり、第１のトルク値Ｃ_ｃ（θ）は、信号処理装置３により、作動装置２２１から受信された、前記第１のトルク信号の電流値及び作動装置２２１に対応するトルクパラメータに基づいて、算出したものである。

ステップ（Ｄ）において、重力モジュール補正ユニット４１は、第１のトルク値Ｃ_ｃ（θ）を受信し、そして、補正パラメータα、βを生成して、仮想ロボットアームユニット４２に送信する。

ステップ（Ｅ）において、仮想ロボットアームユニット４２は、補正パラメータα、β及び第１の関節角度θを受信し、そして、無負荷トルク値Ｇ_０（θ）及び最大負荷トルク値Ｇ_ｍａｘ（θ）を生成する。

なお、第１の関節角度θは、この実施形態のように、エンコーダ２２３から直接に負荷推定モジュール４の仮想ロボットアームユニット４２に発信されることができ、又は、エンコーダ２２３から信号処理装置３を経由して仮想ロボットアームユニット４２に発信されることもできる。

具体的には、５軸のロボットアーム２において、１組の補正パラメータ（α、β）、無負荷トルク値Ｇ_０（θ）及び最大負荷トルク値Ｇ_ｍａｘ（θ）は、順運動学に基づいて導出され、且つ、関節角度及び関節２１により出力されるトルクに関連する一組の方程式を使用して、負荷推定モジュール４により算出された。

上記の計算は、ロボットアーム２に及ぼされる重力の影響のみを考慮することに基づいて行われる。ヒステリシスなどの他の効果も考慮すれば、より正確な結果を得ることができる。従って、補正モデルは、以下のように表す。

式中、ＧｒａｖｉｔｙＭｏｄｅｌは、補正モデルを関数として表し、Ｇ_ｍａｘ（θ）は最大負荷トルク値であり、Ｇ_０（θ）は無負荷トルク値であり、Ｗ_ｍａｘは最大負荷値である。

上記補正モデルを使用することにより、ステップ（Ｄ）において、重力モジュール補正ユニット４１が第1のトルク値Ｃ_ｃ（θ）を受信した後、１組の補正パラメータα、βを生成することができる。

ステップ（Ｆ）において、ロボットアーム２が受ける負荷を未知の負荷Ｗ_ｕに変更する。

ステップ（Ｇ）において、信号処理装置３により、駆動モジュール２２の作動装置２２１から発信された未知の負荷Ｗ_ｕに対応して生ずる第２のトルク信号を受信する。この実施形態において、前記第２のトルク信号は、電流値で表される電流信号である。

ステップ（Ｈ）において、ステップ（Ｃ）と同じように、信号処理装置３は、前記第２のトルク信号を受信した後、前記第２のトルク信号及び作動装置２２１に対応するトルクパラメータに基づいて、前記第２のトルク信号を第２のトルク値Ｃ（θ）に変換する。

ステップ（Ｉ）は、前記未知の負荷と第１の負荷Ｗ_ｃとの負荷差を得て、第１の負荷Ｗ_ｃに前記負荷差を足すことにより前記未知の負荷Ｗ_ｕの推定負荷値Ｗ_ｅを推定するステップであって、サブステップ（Ｉ１）と、サブステップ（Ｉ２）とを含む。

サブステップ（Ｉ１）において、第１のトルク値Ｃ_ｃ（θ）、無負荷トルク値Ｇ_０（θ）、最大負荷トルク値Ｇ_ｍａｘ（θ）及び第２のトルク値Ｃ（θ）に基づいて、換算変換ユニット４３１により、前記負荷差を計算する。

具体的に、サブステップ（Ｉ１）において、換算変換ユニット４３１は、最大負荷値Ｗ_ｍａｘ、無負荷トルク値Ｇ_０（θ）及び最大負荷トルク値Ｇ_ｍａｘ（θ）に基づいてパラメトリック方程式を確立するように構成されている。該パラメトリック方程式は、ロボットアーム２に加えられる負荷（例えば重量）と関節２１によって出力された対応トルクとの間の関係により確立された。続いて、換算変換ユニット４３１は、第１のトルク値Ｃ_ｃ（θ）及び第２のトルク値Ｃ（θ）それぞれを前記パラメトリック方程式に適用して、第１の負荷Ｗ_ｃと未知の負荷Ｗ_ｕとの負荷差を得る。

サブステップ（Ｉ２）において、目標負荷計算ユニット４３２により、第１の負荷Ｗ_ｃに前記負荷差を足すことで未知の負荷Ｗ_ｕの推定負荷値Ｗ_ｅを推定する。

ステップ（Ｊ）において、駆動モジュール２２をトルク制御モードに切り替えてから、例えば操作者によりロボットアーム２を第１の位置から第２の位置に移動する。

ステップ（Ｋ）において、重力補償モジュール５により、駆動モジュール２２のエンコーダ２２３から発信された前記第２の位置に位置する各関節２１の姿勢に対応する各第２の関節角度θ′を受信する。

なお、第２の関節角度θ′は、この実施形態のように、エンコーダ２２３から直接に重力補償モジュール５に発信されることができ、又は、エンコーダ２２３から信号処理装置３及び負荷推定モジュール４を経由して重力補償モジュール５に発信されることもできる。

ステップ（Ｌ）において、重力補償モジュール５により、推定負荷値Ｗ_ｅ及び第２の関節角度θ′に基づいて、補償トルク値を生成して、該補償トルク値を駆動モジュール２２に発信する。

具体的に、ステップ（Ｌ）において、重力補償モジュール５は、推定負荷値Ｗ_ｅ、第２の関節角度θ′、関節２１の出力軸の軸方向、各前記接続棒それぞれの重量及び各前記接続棒それぞれの質量中心位置に基づいて前記補償トルク値を計算するように構成されている。ロボットアーム２内に複数の関節２１が存在する場合には、前記補償トルク値は、前述の推定負荷値Ｗｅ、第２の関節角度θ′、関節２１の出力軸の軸方向、各前記接続棒それぞれの重量及び各前記接続棒それぞれの質量中心位置に加えて、更に、関節２１の数、各関節２１の出力軸の軸方向及び各関節２１の第２の関節角度に基づいて計算する。そして、該補償トルク値を駆動モジュール２２に発信して、ロボットアーム２の重力補償を行う。

なお、補償トルク値の生成に関する技術は公知の技術であり、ここでは詳しい説明を省略する。

推定負荷値Ｗ_ｅは、以下の式により計算された。

式中、第１の負荷Ｗ_ｃに前記負荷差を足すことにより未知の負荷Ｗ_ｕの推定負荷値Ｗ_ｅを推定する。

前記負荷差は、以下の式により計算された。

第１のトルク値Ｃ_ｃ（θ）は、以下の式により計算された。

式中、Ａ_Ｃは第１のトルク信号の電流値であり、Ｔ_Ｃは作動装置２２１に対応して生ずるトルクパラメータであり、また、Ｔ_Ｃは定数とすることもできる。

同じく、第２のトルク値Ｃ（θ）は、以下の式により計算された。

式中、Ａは第２のトルク信号の電流値である。

即ち、上述した方法は、未知の荷重Ｗ_ｕの重量（即ち、推定荷重値Ｗ_ｅ）を推定した後、ロボットアーム２に加えられる負荷の変化に応じて駆動モジュール２２に対して出力する補償トルク値を計算することができるので、未知の荷重Ｗ_ｕの重量を事前に測定する必要がなく、負荷の変化に対する較正プロセスも必要がない。

また、関節２１が出力したトルクを電流信号で測定することにより、関節２１にトルクセンサを設置する必要がない。

この方法に適用するロボットアーム２は、例えば外科手術又はリハビリテーションロボット、及び、工業工程などの様々な用途に使用するロボットアームが挙げられる。

以下、図面に参照して、実施例で本発明を更に説明する。

図４はこの実施例において、ロボットアーム２が第１の位置に位置することを示す側視図である。図５はロボットアーム２が第１の位置に位置する場合において、関節の負荷と出力トルクとの関係図である。

ロボットアーム２は、図４に示されるように、２つの関節２１ａ、２１ｂが備わっており、且つ、第１の位置に位置している。該第１の位置では、関節２１ａの第１の角度値はπ／２であり、関節２１ｂの第１の角度値は０である。そして、ロボットアーム２の最大負荷値（即ち、ペイロード）を１．６ｋｇとし、作動装置２２１のトルクパラメータを２０．８ｍＮｍ／Ａとした上で、重量が異なる実際の負荷（２００ｇ〜１０００ｇ）それぞれをロボットアーム２に加え、各実際の負荷に対応して関節２１ｂが出力したトルク信号で各負荷の推定値を計算する。その結果は、表１及び図５に示す。

図６はこの実施例において、ロボットアーム２が第２の位置に位置することを示す側視図である。図７はロボットアーム２が第２の位置に位置する場合において、関節の負荷と出力トルクとの関係図である。図８はロボットアーム２が第２の位置に位置する場合において、もう１つの関節の負荷と出力トルクとの関係図である。

ロボットアーム２が図６に示されるように第２の位置に位置している場合、関節２１ａの第２の角度値はπ／８であり、関節２１ｂの第２の角度値は９π／４０である。そして、ロボットアーム２の最大負荷値（即ち、ペイロード）を１．６ｋｇとし、作動装置２２１のトルクパラメータを２０．８ｍＮｍ／Ａとした上で、重量が異なる実際の負荷（２００ｇ〜１０００ｇ）それぞれをロボットアーム２に加え、各実際の負荷に対応して関節２１ａ、２１ｂが出力したトルク信号で各負荷の推定値を計算する。関節２１ａの結果は、表２及び図７に示す。関節２１ｂの結果は、表３及び図８に示す。

表１〜表３、図５及び図７〜図８に記載されている結果によれば、本発明におけるロボットアームのための負荷推定及び重力補償方法を用いて推定された推定負荷は、実際の負荷が２００ｇの場合を除いて、実際の負荷との誤差が許容範囲内にあるので、未知の負荷の推定に実用できる。

実際の負荷が２００ｇの場合に推定負荷と実際の負荷との誤差が大きい原因は、ロボットアームを移動する際に、各関節が最大静止摩擦力を超える力を出す必要があるので、負荷によるトルクが最大静止摩擦力より小さい時に、負荷を推定する精確性が大きく影響されるからである。しかし、負荷によるトルクが最大静止摩擦力より小さい場合においては、ロボットアーム２は負荷の重力影響により予期しない移動が起こらないので、その場合に生じた大きい誤差は許容できる。

実際の負荷が４００ｇ〜１０００ｇの場合には、負荷によるトルクが最大静止摩擦力より大きく、ロボットアーム２は負荷の重力影響により予期しない移動が起こり得るが、上記の重力補償方法により、ロボットアーム２は所定の位置に位置することができる。

なお、本発明に適用できるロボットアームは、図４及び図６に示されているロボットアームに限らず、１つの関節だけを有するロボットアームや、３つ以上の関節を有するロボットアームなどにも適用できる。

上記の方法及びシステムによれば、本発明は、異なる負荷の下で作動装置２２１が出力するトルク信号の電流値と、ロボットアーム２の機械構成のパラメータにより確立された重力モジュール補正ユニット４１と、関節２１の位置と、に基づいて未知の負荷Ｗ_ｕを推定する。即ち、第１のトルク値Ｃ_ｃ（θ）と、第２のトルク値Ｃ（θ）と、１組の補正パラメータα、βと、仮想ロボットアームユニット４２が生成した無負荷トルク値Ｇ_０（θ）及び最大負荷トルク値Ｇ_ｍａｘ（θ）と、に基づいて、演算部４３により推定負荷値Ｗ_ｅを推定する。そして、重力補償モジュール５により、推定負荷値Ｗ_ｅ及び第２の関節角度θ′に基づいて、補償トルク値を生成して、該補償トルク値を駆動モジュール２２に発信して、重力補償を行う。従って、未知の荷重Ｗ_ｕの重量を事前に測定することや、負荷の変化に対する較正プロセスや、高価なトルクセンサの設置などが必要なく、未知の荷重Ｗ_ｕによる重力影響を算出して、重力補償を行うことができる。

本発明は、ロボットアームに適用でき、特に負荷が頻繁に変更する用途に使用するロボットアームに好適である。

２ロボットアーム
２１、２１ａ、２１ｂ関節
２２駆動モジュール
２２１作動装置
２２２機械構成
２２３エンコーダ
３信号処理装置
４負荷推定モジュール
４１重力モジュール補正ユニット
４２仮想ロボットアームユニット
４３演算部
４３１換算変換ユニット
４３２目標負荷計算ユニット
５重力補償モジュール
Ｌ出力軸の軸方向
Ｗ負荷

Claims

負荷推定及び重力補償システムによって実行されるロボットアームのための負荷推定及び重力補償方法であって、
前記ロボットアームは、少なくとも１つの関節と、前記少なくとも１つの関節に設置された駆動モジュールと、が備わっており、
前記負荷推定及び重力補償システムは、前記駆動モジュールに連結された信号処理装置と、前記少なくとも１つの関節に対応すると共に、前記信号処理装置に連結された負荷推定モジュールと、前記負荷推定モジュールに連結された重力補償モジュールと、が備わっており、
前記負荷推定モジュールは、重力モジュール補正ユニットと、仮想ロボットアームユニットと、演算部と、が備わっており、
前記ロボットアームのための負荷推定及び重力補償方法は、
（Ａ）前記駆動モジュールを操作モードで操作し、前記ロボットアームを第１の位置に移動して、現時点の負荷である第１の負荷を該第１の位置における前記ロボットアームに受けさせてから、前記駆動モジュールを位置制御モードに切り替えるステップと、
（Ｂ）前記信号処理装置により、前記位置制御モードにおける前記駆動モジュールから発信された、前記第１の負荷に対応して生ずる第１のトルク信号を受信し、前記仮想ロボットアームユニットにより、前記位置制御モードにおける前記駆動モジュールから発信された、前記第１の位置に位置する前記関節の姿勢に対応する第１の関節角度を受信するステップと、
（Ｃ）前記信号処理装置が、前記第１のトルク信号を受信した後、更に前記第１のトルク信号を第１のトルク値に変換させるステップと、
（Ｄ）前記重力モジュール補正ユニットにより、前記第１のトルク値を受信し、そして、１組の補正パラメータを生成して、前記仮想ロボットアームユニットに送信するステップと、
（Ｅ）前記仮想ロボットアームユニットが、前記１組の補正パラメータ及び前記第１の関節角度を受信した後、更に無負荷トルク値及び最大負荷トルク値を生成するステップと、
（Ｆ）前記ロボットアームが受ける負荷を未知の負荷に変更するステップと、
（Ｇ）前記信号処理装置により、前記駆動モジュールから発信された前記未知の負荷に対応して生ずる第２のトルク信号を受信するステップと、
（Ｈ）前記信号処理装置が、前記第２のトルク信号を受信した後、更に前記第２のトルク信号を第２のトルク値に変換させるステップと、
（Ｉ）前記演算部により、前記第１のトルク値、前記無負荷トルク値、前記最大負荷トルク値及び前記第２のトルク値に基づいて、前記未知の負荷の推定負荷値を推定するステップと、
（Ｊ）前記駆動モジュールをトルク制御モードに切り替えてから、前記ロボットアームを第２の位置に移動するステップと、
（Ｋ）前記重力補償モジュールにより、前記駆動モジュールから発信された前記第２の位置に位置する前記関節の姿勢に対応する第２の関節角度を受信するステップと、
（Ｌ）前記重力補償モジュールにより、前記推定負荷値及び前記第２の関節角度に基づいて、補償トルク値を生成して、該補償トルク値を前記駆動モジュールに発信するステップと、を含むことを特徴とするロボットアームのための負荷推定及び重力補償方法。
前記ステップ（Ｉ）において、前記未知の負荷と前記第１の負荷との負荷差を得て、前記第１の負荷に前記負荷差を足すことにより前記未知の負荷の推定負荷値を推定することを特徴とする請求項１に記載のロボットアームのための負荷推定及び重力補償方法。
前記演算部は、トルク値を負荷値に換算して変換する換算変換ユニットと、目標負荷計算ユニットと、が更に備わっており、
前記ステップ（Ｉ）は、
（Ｉ１）前記第１のトルク値、前記無負荷トルク値、前記最大負荷トルク値及び前記第２のトルク値に基づいて、前記換算変換ユニットにより、前記負荷差を計算するサブステップと、
（Ｉ２）前記目標負荷計算ユニットにより、前記第１の負荷に前記負荷差を足すことで未知の負荷の推定負荷値を推定するサブステップと、を更に含むことを特徴とする請求項２に記載のロボットアームのための負荷推定及び重力補償方法。
前記サブステップ（Ｉ１）において、
前記ロボットアームに受けさせることができる最大負荷値、前記無負荷トルク値及び前記最大負荷トルク値に基づいてパラメトリック方程式を確立し、そして、
前記第１のトルク値及び前記第２のトルク値それぞれを前記パラメトリック方程式に適用して前記負荷差を得ることを特徴とする請求項３に記載のロボットアームのための負荷推定及び重力補償方法。
前記ロボットアームは、前記関節を介して互いに接続された少なくとも２つの接続棒が更に備わっており、
前記ステップ（Ｌ）において、更に、前記少なくとも１つの関節の出力軸の軸方向、前記接続棒それぞれの重量及び前記接続棒それぞれの質量中心位置に基づいて前記補償トルク値を計算することを特徴とする請求項１に記載のロボットアームのための負荷推定及び重力補償方法。
前記第１の負荷及び前記未知の負荷それぞれからの力の方向は、前記少なくとも１つの関節の出力軸の軸方向と交差せず且つ平行しないことを特徴とする請求項５に記載のロボットアームのための負荷推定及び重力補償方法。
前記ロボットアームは、複数の前記関節と、前記複数の関節を介して互いに接続された複数の前記接続棒とが備わっており、
前記ステップ（Ｌ）において、更に、前記関節の数、各前記関節の出力軸の軸方向及び各前記関節の前記第２の関節角度に基づいて、前記補償トルク値を計算することを特徴とする請求項５に記載のロボットアームのための負荷推定及び重力補償方法。
前記駆動モジュールは、作動装置と、前記作動装置に接続されている機械構成と、前記機械構成に接続されているエンコーダと、が更に備わっており、
前記操作モードにおいては、前記作動装置が前記機械構成に制御信号を発信して、前記ロボットアームを前記第１の位置に移動し、
前記ステップ（Ｂ）において、前記第１のトルク信号は、前記作動装置により発信され、且つ、前記第１の関節角度は、前記エンコーダにより発信され、
前記ステップ（Ｇ）において、前記第２のトルク信号は、前記作動装置により発信され、
前記ステップ（Ｋ）において、前記第２の関節角度は、前記エンコーダにより発信されることを特徴とする請求項１に記載のロボットアームのための負荷推定及び重力補償方法。
前記作動装置により発信された前記第１のトルク信号及び前記第２のトルク信号それぞれは、電流値で表される電流信号であり、
前記ステップ（Ｃ）においては、前記信号処理装置により、前記作動装置から受信された前記第１のトルク信号の電流値及び前記作動装置に対応して生ずるトルクパラメータに基づいて、前記第１のトルク値を計算し、
前記ステップ（Ｈ）においては、前記信号処理装置により、前記作動装置から受信された、前記第２のトルク信号の電流値及び前記トルクパラメータに基づいて前記第２のトルク値を計算することを特徴とする請求項８に記載のロボットアームのための負荷推定及び重力補償方法。
少なくとも１つの関節と、前記少なくとも１つの関節に設置された駆動モジュールと、が備わっているロボットアームに使用する負荷推定及び重力補償システムであって、
前記負荷推定及び重力補償システムは、信号処理装置と、負荷推定モジュールと、重力補償モジュールと、が備わっており、
前記信号処理装置は、前記ロボットアームが現時点の負荷である第１の負荷を受ける際に、前記駆動モジュールから発信された第１のトルク信号を受信し、前記第１のトルク信号を第１のトルク値に変換するために、且つ、前記ロボットアームが未知の負荷を受ける際に、前記駆動モジュールから発信された第２のトルク信号を受信し、前記第２のトルク信号を第２のトルク値に変換するために、前記駆動モジュールに連接されており、
前記負荷推定モジュールは、前記少なくとも１つの関節に対応すると共に、前記第１のトルク値及び前記第２のトルク値を受信するために、前記信号処理装置に連接されていると共に、前記第1のトルク値に基づいて１組の補正パラメータを生成し、前記１組の補正パラメータ及び前記ロボットアームが前記第１の負荷を受ける際に前記駆動モジュールから発信された第１の関節角度に基づいて、無負荷トルク値及び最大負荷トルク値を生成し、且つ、前記第１のトルク値、前記無負荷トルク値、前記最大負荷トルク値及び前記第２のトルク値に基づいて、前記未知の負荷の推定負荷値を推定するようにアレンジされており、
前記重力補償モジュールは、前記推定負荷値及び前記ロボットアームが前記未知の負荷を受ける際に前記駆動モジュールから発信された第２の関節角度に基づいて、補償トルク値を生成し、且つ、該補償トルク値を前記駆動モジュールに発信するために、前記負荷推定モジュールに連接されていることを特徴とする負荷推定及び重力補償システム。
前記負荷推定モジュールは、前記未知の負荷と前記第１の負荷との負荷差を得て、前記第１の負荷に前記負荷差を足すことにより前記未知の負荷の推定負荷値を推定するようにアレンジされていることを特徴とする請求項１０に記載の負荷推定及び重力補償システム。
前記負荷推定モジュールは、
トルク値を負荷値に換算して変換するために、前記第１のトルク値、前記無負荷トルク値、前記最大負荷トルク値及び前記第２のトルク値に基づいて、前記負荷差を計算するようにアレンジされている換算変換ユニットと、
前記第１の負荷に前記負荷差を足すことにより前記未知の負荷の推定負荷値を計算するようにアレンジされている目標負荷計算ユニットと、
を更に含むことを特徴とする請求項１１に記載の負荷推定及び重力補償システム。
前記換算変換ユニットは、前記負荷差の計算において、
前記ロボットアームに受けさせることができる最大負荷値、前記無負荷トルク値及び前記最大負荷トルク値に基づいてパラメトリック方程式を確立し、且つ、
前記第１のトルク値及び前記第２のトルク値それぞれを前記パラメトリック方程式に適用して前記負荷差を得るようにアレンジされていることを特徴とする請求項１２に記載の負荷推定及び重力補償システム。