JP2017124455A - ロボット装置、ロボット制御方法、プログラム及び記録媒体 - Google Patents

Abstract

【課題】ロボット本体が振動する設置面に設置された場合においても、ロボット本体の先端部の振動を抑制する。
【解決手段】振動演算部３７４は、検知部２４０による検知結果から、ロボット本体のベース部の振動加速度を取得し、ロボット本体のベース部の振動加速度から、ロボット本体のベース部の振動量、つまり補正量ＰＣを算出する。補正部３７１は、ロボット本体の先端部の位置指令値Ｐを、補正量ＰＣで補正する。逆キネマティクス演算部３７２は、補正したロボット本体の先端部の位置指令値（Ｐ−ＰＣ）から、ロボット本体の各関節の位置制御に用いる関節指令値θを求める。
【選択図】図３

Description

本発明は、ロボット本体の先端部の振動を抑制するものに関する。

一般的に、ロボットは、各関節に搭載されている減速器等のバネ要素により、アームリンクに対し各関節の剛性が低くなっている。各関節の剛性が低いことにより、ロボットの動作時にロボットの先端部が振動的になるという問題があった。

ロボットの先端部の振動を抑制する手法として特許文献１が知られている。特許文献１には、ロボットの先端部に加速度センサを設け、加速度センサにより検出される加速度量に基づいて、ロボットの構造に応じた所定の変換を行うことで、ロボットの先端部に生じる振動量を算出する。算出した振動量を抑制するように各関節への補償成分を算出し、各関節の位置指令値を補償成分で補正することで、ロボットの先端部の振動を抑制しようとするものである。

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

しかしながら、特許文献１の方法では、ロボットが設置される設置面の振動モデルを考慮していない。つまり、特許文献１の方法では、ロボットが剛性の低い架台や移動体上など、振動しやすい設置面上に設置された場合を想定したものではない。したがって、ロボットが振動しやすい設置面上に設置された場合、特許文献１の方法では、ロボットの先端部の振動を高精度に抑制することができなかった。

そこで、本発明は、ロボット本体が振動する設置面に設置された場合においても、ロボット本体の先端部の振動を抑制することを目的とする。

本発明のロボット装置は、複数の関節を有するロボット本体と、前記ロボット本体の動作を制御する制御部と、前記ロボット本体に発生する加速度を検知する検知部と、を備え、前記制御部は、前記ロボット本体の先端部の位置指令値から、前記ロボット本体の各関節の位置制御に用いる関節指令値を求める指令値算出処理と、前記検知部による検知結果から、前記ロボット本体のベース部の振動加速度を取得する取得処理と、前記ロボット本体のベース部の振動加速度から、前記ロボット本体のベース部の振動量を算出する振動算出処理と、前記指令値算出処理にて前記各関節指令値を求めるのに使用する前記位置指令値を、前記ロボット本体のベース部の振動量で補正する補正処理と、を実行する。

本発明によれば、ロボット本体の先端部の位置指令値が、ロボット本体のベース部の振動量で補正されるので、ロボット本体が振動する設置面に設置された場合においても、ロボット本体の先端部の振動を抑制することができる。

実施形態に係るロボット装置を示す斜視図である。 実施形態に係るロボット装置の制御系の構成を示すブロック図である。 実施形態に係るロボット装置の制御系の機能ブロック図である。 振動演算部の詳細を説明するブロック図である。

以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。図１は、実施形態に係るロボット装置を示す斜視図である。ロボット装置１００は、ロボット２００と、ロボット２００の動作を制御する制御装置３００と、ユーザの操作によりロボット２００の動作を教示する教示部としてのティーチングペンダント４００と、を備えている。

ロボット２００は、垂直多関節のロボットアーム２０１と、ロボットアーム２０１の先端に取り付けられた、先端加速度検知部としての力覚センサ２０３とを有する。また、ロボット２００は、力覚センサ２０３を介してロボットアーム２０１の先端に取り付けられた、エンドエフェクタとしてのロボットハンド２０２と、を有している。

ロボットアーム２０１は、架台１５０に固定されるベース部（基端のリンク）２１０と、変位や力を伝達する複数のリンク２１１〜２１６とを有する。複数のリンク２１０〜２１６が関節Ｊ１〜Ｊ６で屈曲（旋回）又は回転可能に連結されている。本実施形態では、ロボットアーム２０１は、屈曲する３軸と回転する３軸の６軸の関節Ｊ１〜Ｊ６で構成されている。ここで、屈曲とは２つのリンクの結合部のある点で折れ曲がること、回転とは２つのリンクの長手方向の回転軸でリンクが相対的に回ることをいい、それぞれを屈曲部、回転部と呼ぶ。ロボットアーム２０１は、６つの関節Ｊ１〜Ｊ６から構成され、関節Ｊ１，Ｊ４，Ｊ６が回転部、関節Ｊ２，Ｊ３，Ｊ５が屈曲部である。

ロボットハンド２０２は、複数のフィンガー２２０を有し、リンク（先端のリンク）２１６に力覚センサ２０３を介して取り付けられている。

複数のフィンガー２２０を閉動作させることにより、第１ワークであるワークＷ１を把持することができ、複数のフィンガー２２０を開動作させることにより、ワークＷ１を把持解放することができる。ロボットハンド２０２は、複数のフィンガー２２０を用いてワークＷ１を把持することにより、第１ワークであるワーク（嵌合部品）Ｗ１を第２ワークであるワーク（被嵌合部品）Ｗ２に嵌合する嵌合作業（組立作業）を行うことができる。この組立作業により、組立部品Ｗ０が製造される。

ロボットアーム２０１は、各関節Ｊ１〜Ｊ６に対して設けられ、各関節Ｊ１〜Ｊ６をそれぞれ駆動するための複数（６つ）の関節駆動装置２３０_Ｎを有している。ここで、Ｎは１〜６の通し番号（整数）であり、ロボットアーム２０１の基端から先端に向かって順に割り振られている。なお、図１では、関節駆動装置２３０_{Ｎ（＝２）}は、便宜上、関節Ｊ２にのみ図示しているが、他の関節Ｊ１，Ｊ３〜Ｊ６にも、同様の構成の関節駆動装置２３０_Ｎが配置されている。

図２は、実施形態に係るロボット装置の制御系の構成を示すブロック図である。制御装置３００は、メイン制御部３３０と、複数（関節の数に対応した数：本実施形態では６つ）の位置制御部３４０_Ｎと、を有する（Ｎ＝１〜６）。

各位置制御部３４０_Ｎには、関節駆動装置２３０_Ｎが接続されている。関節駆動装置２３０_Ｎは、電動モータである回転モータ（以下、「モータ」という）２３１_Ｎと、モータ２３１の回転軸の回転を減速する減速機２３３_Ｎと、を有している。

モータ２３１_Ｎは、サーボモータであり、例えばブラシレスＤＣサーボモータやＡＣサーボモータである。減速機２３３_Ｎは、例えば小型軽量で減速比の大きい波動歯車減速機である。モータ２３１_Ｎの回転軸の回転は、減速機２３３_Ｎで１／Ｎ_０の減速比で減速されて、一方のリンクに対して他方のリンクを相対的に回転運動させ、関節を屈曲又は回転させる。

ロボットアーム２０１は、各関節Ｊ１〜Ｊ６に設けられ、関節の角度（位置）、即ちモータ２３１_Ｎの回転軸の回転角度を検出する位置検出部であるロータリエンコーダ２３６_Ｎを有している。また、ロボットアーム２０１は、各関節Ｊ１〜Ｊ６に設けられ、関節に発生するトルクを検知するトルクセンサ２３５_Ｎを有している。

トルクセンサ２３５_Ｎ及びエンコーダ２３６_Ｎは、位置制御部３４０_Ｎに接続されている。エンコーダ２３６_Ｎは、アブソリュート型のロータリエンコーダが望ましく、１回転の絶対角度エンコーダ、絶対角度エンコーダの回転総数のカウンタ、及びカウンタに電力を供給するバックアップ電池を有して構成される。ロボットアーム２０１への電源の供給がオフになっても、このバックアップ電池が有効であれば、ロボットアーム２０１への電源供給のオン／オフに関係なく、カウンタにおいて回転総数が保持される。したがって、ロボットアーム２０１の姿勢が制御可能となる。

本実施形態では、ロボットアーム２０１（トルクセンサ２３５_Ｎ及びエンコーダ２３６_Ｎを除く）とロボットハンド２０２とでロボット本体２５０が構成されている。また、力覚センサ２０３及び複数（６つ）のトルクセンサ２３５_１〜２３５_６で、ロボット本体２５０に発生する加速度を検知する検知部２４０が構成されている。

また、力覚センサ２０３は、ロボット本体２５０の先端部（つまりロボットハンド２０２）に発生する加速度を検知する先端加速度検知部として機能する。力覚センサ２０３は、６軸力覚センサであり、ロボット２００（ロボット本体２５０）の手首部に搭載されており、直接的にはロボットハンド２０２にかかる力を検知する。力覚センサにより検知される力は、Ｘ軸、Ｙ軸、Ｚ軸の力成分と、各軸まわりのモーメント成分からなる。

トルクセンサ２３５_Ｎは、ロボット本体２５０の関節に発生する関節の駆動方向の加速度を検知する関節加速度検知部として機能する。トルクセンサ２３５_Ｎは、直接的には減速機２３３_Ｎの出力軸にかかるトルクを検知する。

メイン制御部３３０は、コンピュータで構成されており、制御部（演算部）としてのＣＰＵ（Central Processing Unit）３０１を備えている。また、メイン制御部３３０は、記憶部として、ＲＯＭ（Read Only Memory）３０２、ＲＡＭ（Random Access Memory）３０３、ＨＤＤ（Hard Disk Drive）３０４を備えている。また、メイン制御部３３０は、記録ディスクドライブ３０５、検出回路３１２及びインタフェース３１１，３１３_Ｎを備えている。

ＣＰＵ３０１には、ＲＯＭ３０２、ＲＡＭ３０３、ＨＤＤ３０４、記録ディスクドライブ３０５、検出回路３１２及びインタフェース３１１，３１３_Ｎが、バスを介して接続されている。ＲＯＭ３０２には、ＢＩＯＳ等の基本プログラムが格納されている。ＲＡＭ３０３は、ＣＰＵ３０１の演算処理結果等、各種データを一時的に記憶する記憶装置である。

ＨＤＤ３０４は、ＣＰＵ３０１の演算処理結果や外部から取得した各種データ等を記憶する記憶装置であると共に、ＣＰＵ３０１に、後述する演算処理を実行させるためのプログラム３２０を記録するものである。ＣＰＵ３０１は、ＨＤＤ３０４に記録（格納）されたプログラム３２０に基づいてロボット制御方法の各工程を実行する。

記録ディスクドライブ３０５は、記録ディスク３２１に記録された各種データやプログラム等を読み出すことができる。なお、メイン制御部３３０には、書き換え可能な不揮発性メモリや外付けＨＤＤ等の不図示の外部記憶装置が接続されていてもよい。

教示部であるティーチングペンダント４００は、インタフェース３１１に接続されている。ティーチングペンダント４００は、ユーザの入力操作により、ロボット２００を教示する教示点、即ちロボット本体２５０の先端部の目標位置を指定するものである。教示点のデータは、インタフェース３１１及びバスを通じてＨＤＤ３０４に出力される。

ＨＤＤ３０４は、ティーチングペンダント４００により指定された教示点のデータを格納することができる。ＣＰＵ３０１は、ＨＤＤ３０４に設定（格納）された教示点のデータを読み出すことができる。

インタフェース３１３_Ｎには、位置制御部３４０_Ｎが接続されている。なお、本実施形態では、ロボットアーム２０１が６つの関節Ｊ１〜Ｊ６を有しているので、制御装置３００は、６つの位置制御部３４０_Ｎを有するが、図２では、位置制御部３４０_Ｎを１つだけ図示し、残りの５つは図示を省略している。各位置制御部３４０_Ｎは、制御装置３００の筐体内に配置されている。なお、各位置制御部３４０_Ｎの配置位置は、制御装置３００の筐体内に限定するものではなく、例えばロボットアーム２０１（リンク）の内部であってもよい。

ＣＰＵ３０１は、予め設定された教示点に基づき、ロボット本体２５０の軌道を計算し、各関節Ｊ１〜Ｊ６の位置（角度）、具体的にはモータ２３１_Ｎの回転軸の回転角度を示す関節指令値の信号を所定時間間隔で各位置制御部３４０_Ｎに出力する。

位置制御部３４０_Ｎは、ＣＰＵ３５１_Ｎ、記憶部としてのＥＥＰＲＯＭ３５２_Ｎ及びＲＡＭ３５３_Ｎ、インタフェース３６１_Ｎ、検出回路３６２_Ｎ，３６３_Ｎ並びにモータ駆動回路３６５_Ｎを備えており、これらがバスを介して接続されて構成されている。

ＣＰＵ３５１_Ｎは、プログラム３６８_Ｎに従って演算処理を実行する。ＥＥＰＲＯＭ３５２_Ｎは、プログラム３６８_Ｎを記憶する記憶装置である。ＲＡＭ３５３_Ｎは、ＣＰＵ３５１_Ｎの演算処理結果等、各種データを一時的に記憶する記憶装置である。

メイン制御部３３０は、複数（６つ）のインタフェース３１３_Ｎ（図２では１つのみ図示）を有している。各インタフェース３１３_Ｎと各位置制御部３４０_Ｎのインタフェース３６１_Ｎとがケーブル等で接続されており、メイン制御部３３０と各位置制御部３４０_Ｎとの間で信号の送受信を行うことができる。

トルクセンサ２３５_Ｎは、検出回路３６２_Ｎに接続され、エンコーダ２３６_Ｎは、検出回路３６３_Ｎに接続されている。トルクセンサ２３５_Ｎからは、検出したトルク値を示す信号が出力され、エンコーダ２３６_Ｎからは、検出した角度（位置）を示す信号が出力される。検出回路３６２_Ｎは、トルクセンサ２３５_Ｎから信号を取得し、ＣＰＵ３５１_Ｎにて取得可能な信号に変換してＣＰＵ３５１_Ｎに出力する。検出回路３６３_Ｎは、エンコーダ２３６_Ｎから信号を取得し、ＣＰＵ３５１_Ｎにて取得可能な信号に変換してＣＰＵ３５１_Ｎに出力する。ＣＰＵ３５１_Ｎは、これら検知結果をＣＰＵ３０１に出力する。このように、メイン制御部３３０のＣＰＵ３０１は、各位置制御部３４０_Ｎを介してトルク値及び角度値を取得するが、各位置制御部３４０_Ｎを介さずに直接取得してもよい。

モータ駆動回路３６５_Ｎは、例えば半導体スイッチング素子を有するモータドライバであり、入力した電流指令値に応じて、パルス幅変調された３相交流のＰＷＭ波形の電圧をモータ２３１_Ｎに出力することで、モータ２３１_Ｎに電流を供給する。

各位置制御部３４０_ＮのＣＰＵ３５１_Ｎは、メイン制御部３３０のＣＰＵ３０１から入力を受けた関節指令値に関節の角度（位置）が近づくようにモータ２３１_Ｎへの電流の電流指令値を演算し、電流指令値をモータ駆動回路３６５_Ｎに出力する。

位置制御部３４０_Ｎは、一般にカスケード制御と呼ばれている制御方式をとっており、位置制御器、速度制御器、電流制御器を直列に接続した構成となっている。カスケード制御方式の特徴として、各制御器の制御周期を個別に設定できるという特徴がある。本実施形態において、各制御器の制御周期は、電流制御器＜速度制御器＜位置制御器となるように設定されている。

なお、本実施形態では、エンコーダ２３６_Ｎは、モータ２３１_Ｎの回転軸の回転位置を検知するため、位置制御部３４０_Ｎは、関節指令値を減速機２３３_Ｎの減速比Ｎ_０で補正して、モータ２３１_Ｎの回転位置を制御する。

モータ駆動回路３６５_Ｎは、入力を受けた電流指令値に対応する電流をモータ２３１_Ｎに供給する。そして、モータ２３１_Ｎは、モータ駆動回路２６５_Ｎから電力供給を受けて駆動トルクを発生し、減速機２３３_Ｎの入力軸にトルクを伝達する。減速機２３３_Ｎの出力軸は、入力軸の回転に対して１／Ｎ_０の回転数で回転する。これにより、一方のリンクが他方のリンクに対して相対的に回転（旋回）する。

なお、本実施形態では、コンピュータ読み取り可能な記録媒体がＨＤＤ３０４であり、ＨＤＤ３０４にプログラム３２０が格納される場合について説明するが、これに限定するものではない。プログラム３２０は、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラム３２０を供給するための記録媒体としては、図２に示す記録ディスク３２１、不図示の外部記憶装置等を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、磁気テープ、ＵＳＢメモリ等の不揮発性メモリ、ＲＯＭ等を用いることができる。

ここで、ロボット本体２５０の先端部、即ちロボットハンド２０２には、ツールセンターポイント（ＴＣＰ）が設定されている。ＴＣＰは、位置を表す３つのパラメータ（ｘ，ｙ，ｚ）と、姿勢を表す３つのパラメータ（Ｒｘ，Ｒｙ，Ｒｚ）、即ち６つのパラメータ（ｘ，ｙ，ｚ，Ｒｘ，Ｒｙ，Ｒｚ）で表され、タスク空間上では、１つの点としてみなすことができる。つまり、タスク空間は、これら６つの座標軸で規定された空間である。本実施形態では、教示点は、タスク空間で指定される。

ＣＰＵ３０１は、教示点間を補間するロボット本体２５０の経路（補間経路）を生成する。ロボット本体２５０の経路とは、タスク空間におけるロボット本体２５０のＴＣＰの軌跡である。そして、ＣＰＵ３０１は、ロボット本体２５０の軌道（補正前）を仮計算する。ロボット本体２５０の軌道とは、時間をパラメータとして経路を表したものであり、本実施形態では、時刻毎（例えば１ｍｓ毎）のロボット本体２５０の先端部（ＴＣＰ）の位置指令値の集合である。位置指令値は、６つのパラメータ（ｘ，ｙ，ｚ，Ｒｘ，Ｒｙ，Ｒｚ）で表される。

図３は、実施形態に係るロボット装置の制御系の機能ブロック図である。図４は、振動演算部の詳細を説明するブロック図である。本実施形態では、図２に示すＣＰＵ３０１がプログラム３２０を実行することにより、図３に示す各部３７１〜３７５として機能する。具体的には、ＣＰＵ３０１は、補正部３７１と、逆キネマティクス演算部３７２と、ロボットパラメータ演算部３７３と、振動演算部３７４と、補正部３７５として機能する。

振動演算部３７４は、図４に示すように、取得部３７６と、振動量算出部３７７と、関節振動量算出部３７８とを有する。

取得部３７６は、検知部２４０の検知結果から、ロボット本体２５０のベース部２１０の振動加速度αＣを演算することにより取得する（取得処理、取得工程）。また、振動量算出部３７７は、振動加速度αＣから、ベース部２１０の振動量である補正値ＰＣを算出する。

本実施形態では、検知部２４０は、直接ベース部２１０の振動加速度を検知するものではなく、力覚センサ２０３及びトルクセンサ２３５_１〜２３５_６を用いて間接的に振動加速度を検知する。したがって、取得部３７６は、力覚センサ２０３及びトルクセンサ２３５_１〜２３５_６の検知結果を用いて、ベース部２１０の振動加速度を演算により取得する。なお、検知部２４０が、直接、ベース部２１０の振動加速度を検知する加速度センサであった場合には、取得演算部２７６は、検知部２４０の検知結果をそのままベース部２１０の振動加速度として取得するだけでよい。

また、関節振動量算出部３７８は、後述するロボット本体２５０の各関節Ｊ１〜Ｊ６の振動加速度から、ロボット本体２５０の各関節Ｊ１〜Ｊ６の振動量を示す補正値θＢ（θＢ１〜θＢ６）を求める（関節振動算出処理、関節振動算出工程）。

図３に示す補正部３７１は、ロボット本体２５０の先端部、つまりロボットハンド２０２（ＴＣＰ）の位置指令値Ｐを、補正値ＰＣで補正、具体的には位置指令値Ｐから補正値ＰＣを減算する（補正処理、補正工程）。補正値ＰＣは、位置指令値Ｐと同様、６つのパラメータからなり、演算により求めたロボット本体２５０のベース部２１０の振動量である。よって、補正値ＰＣは、ベース部２１０の振動を補償（抑制）するためのものである。

位置指令値Ｐをベース部２１０の振動量である補正値ＰＣで補正することで、ロボット本体２５０の先端部、つまりロボットハンド２０２は、架台１５０に対しては、架台１５０の振動と逆方向に相対的に振動することになる。よって、ロボット本体２５０の先端部、つまりロボットハンド２０２は、架台１５０が設置されている床面に対しては、架台１５０の振動分が相殺されることになる。

逆キネマティクス演算部３７２は、補正値ＰＣで補正された位置指令値Ｐから、ロボット本体２５０の各関節Ｊ１〜Ｊ６の位置制御に用いる関節指令値θ（θ１〜θ６）を求める（指令値算出処理、指令値算出工程）。具体的には、逆キネマティクス演算部３７２は、位置指令値Ｐと補正値（ベース部の振動量）ＰＣとの差分とから、逆運動学計算により、各関節Ｊ１〜Ｊ６に対する関節指令値（角度指令値）θを求める。

ロボットパラメータ演算部３７３は、ロボットモデルデータと関節指令値θに基づき、ロボット本体２５０の動作及び姿勢に応じたロボットパラメータＴＡ，Ｌ，Ｍを算出する。ロボットモデルデータ（ロボット本体２５０の設計値）は、予め記憶部（例えばＨＤＤ３０４）に記憶されている。

具体的に説明すると、ロボットパラメータ演算部３７３は、関節指令値θに従って動作するロボット本体２５０の各関節Ｊ１〜Ｊ６に発生する想定トルクＴＡ（ＴＡ１〜ＴＡ６）を、関節指令値θ及びロボットモデルデータを用いて求める。また、ロボットパラメータ演算部３７３は、ベース部２１０と各関節Ｊ１〜Ｊ６との距離Ｌ（Ｌ１〜Ｌ６）と、各関節Ｊ１〜Ｊ６に作用する重量Ｍ（Ｍ１〜Ｍ６）とをロボットモデルデータと各関節指令値θを用いて求める。関節指令値θは、時間と共に変化するので、ロボットパラメータ演算部３７３は、これらロボットパラメータＴＡ，Ｌ，Ｍを関節指令値θに応じて算出する。

このように、ロボットパラメータ演算部３７３は、逆キネマティクス演算部３７２で算出した各関節指令値θからロボット本体２５０の各関節Ｊ１〜Ｊ６に発生する想定トルクを求める（想定トルク算出処理、想定トルク算出工程）。

補正部３７５は、逆キネマティクス演算部３７２で算出した各関節指令値θ（θ１〜θ６）を、ロボット本体２５０の各関節Ｊ１〜Ｊ６の振動量を示す補正値θＢ（θＢ１〜θＢ６）で補正する（関節補正処理、関節補正工程）。具体的には、補正部３７５は、関節指令値θと補正値θＢとの差（θ−θＢ）を演算する。したがって、各位置制御部３４０_１〜３４０_６は、関節指令値θを補正値θＢで補正した値を用いて、ロボット本体２５０の各関節Ｊ１〜Ｊ６の位置制御を行うことになる。

以下、振動演算部３７４の演算処理について具体的に説明する。振動演算部３７４は、力覚センサ２０３により検知された検知結果Ｆ、トルクセンサ２３５_１〜２３５_６により検知された検知結果Ｔ（Ｔ１〜Ｔ６）、及びロボットパラメータＴＡ，Ｌ，Ｍから、補正値ＰＣと、補正値θＢとを算出する。

図４において、取得部３７６は、加速度演算部３８１と、関節加速度想定部３８２_１〜３８２_６と、先端加速度想定部３８３と、先端加速度算出部３８４と、基端加速度算出部３８５とを有する。

加速度演算部３８１は、力覚センサ２０３により検出された力Ｆを、ロボット本体２５０の先端部（ロボットハンド２０２）の加速度αに換算する。具体的には、加速度演算部３８１は、力覚センサ２０３により検出された力Ｆを、ロボットハンド２０２の重量Ｍ０で除算して、ロボットハンド２０２の加速度αを求める。ロボットハンド２０２の重量Ｍ０のデータは、予めＨＤＤ３０４等の記憶部に記憶されている。

なお、力覚センサ２０３は、ロボットハンド２０２に発生する力を検知しているので、ロボットハンド２０２の加速度αを間接的に検知していることになるが、直接、ロボットハンド２０２の加速度αを検知してもよい。即ち、先端加速度検知部が、加速度センサであってもよい。この場合、加速度演算部３８１は省略することができる。なお、本実施形態では、先端加速度検知部が、ロボット２００（ロボット本体２５０）の力制御に用いる力覚センサ２０３であるので、別途、加速度センサを設けなくてもよい。

関節加速度想定部３８２_１〜３８２_６は、位置指令値（Ｐ−ＰＣ）（つまり関節指令値θ）に基づくロボット本体２５０の各関節Ｊ１〜Ｊ６の仮想的な動作から、各関節Ｊ１〜Ｊ６に発生する各関節Ｊ１〜Ｊ６の駆動方向の想定加速度αＡ１〜αＡ６を求める。つまり、関節加速度想定部３８２_１〜３８２_６は、各関節指令値θ（θ１〜θ６）からロボット本体２５０の各関節Ｊ１〜Ｊ６に発生する各関節Ｊ１〜Ｊ６の駆動方向の想定加速度αＡ１〜αＡ６を算出する（関節加速度想定処理、関節加速度想定工程）。

具体的に説明すると、ロボットパラメータ演算部３７３において各関節指令値θ（θ１〜θ６）から想定トルクＴＡ（ＴＡ１〜ＴＡ６）が求められている。したがって、関節加速度想定部３８２_１〜３８２_６は、各想定トルクＴＡ（ＴＡ１〜ＴＡ６）からロボット本体２５０の各関節Ｊ１〜Ｊ６に発生する各関節Ｊ１〜Ｊ６の駆動方向の想定加速度αＡ１〜αＡ６を算出する。

関節Ｊ１を例に、想定加速度αＡ１の算出方法について、更に具体的に説明する。まず、関節加速度想定部３８２_１は、関節Ｊ１での想定トルクＴＡ１に、ベース部２１０と関節Ｊ１との間の距離Ｌ１を乗算することにより、関節Ｊ１に発生する力を算出する。そして、関節加速度想定部３８２_１は、関節Ｊ１に発生する力を関節Ｊ１に作用する重量Ｍ１で除算することで、関節Ｊ１に発生する関節Ｊ１の駆動方向の加速度αＡ１を算出する。関節Ｊ２〜Ｊ６についても同様の演算を行う。なお、関節加速度想定部３８２_１〜３８２_６は、各関節指令値θ（θ１〜θ６）から直接、ロボット本体２５０の各関節Ｊ１〜Ｊ６に発生する各関節Ｊ１〜Ｊ６の駆動方向の想定加速度αＡ１〜αＡ６を算出してもよい。

先端加速度想定部３８３は、位置指令値からロボット本体２５０の先端部（ロボットハンド２０２）に発生する想定加速度αＡを算出する（先端加速度想定処理、先端加速度想定工程）。つまり、位置指令値からは、関節指令値θが求められ、関節指令値θからは、想定トルクＴＡが求められ、想定トルクＴＡからは、各関節Ｊ１〜Ｊ６の想定加速度αＡ１〜αＡ６が求められている。本実施形態では、先端加速度想定部３８３は、位置指令値から求められた想定加速度αＡ１〜αＡ６から、ロボット本体の先端部に発生する想定加速度αＡを算出する。

具体的に説明すると、先端加速度想定部３８３は、ロボット本体２５０の各関節Ｊ１〜Ｊ６の想定加速度αＡ１〜αＡ６を合計することで、ロボット本体２５０の先端部に発生する想定加速度αＡを算出する。想定加速度αＡは、実際に先端部に発生している実加速度ではなく、シミュレーションにより求まる理想的な状態での加速度（シミュレーション値）である。

なお、本実施形態では、位置指令値から求めた各関節の想定加速度αＡ１〜αＡ６から、先端部の想定加速度αＡを求める場合について説明したが、位置指令値から直接、想定加速度αＡを求めてもよい。

先端加速度算出部３８４は、関節加速度算出部３８６_１〜３８６_６と、振動加速度算出部３８７とを有する。各関節加速度算出部３８６_１〜３８６_６は、ロボット本体２５０の各関節の実加速度と、ロボット本体２５０の各関節の想定加速度との偏差に応じて、ロボット本体２５０の各関節に発生する各関節の駆動方向の振動加速度αＢ１〜αＢ６を求める。そして、振動加速度算出部３８７は、ロボット本体２５０の先端部の振動加速度αＢを算出する（先端加速度算出処理、先端加速度算出工程）。

ここで、本実施形態では、トルクセンサ２３５_１〜２３５_６は、加速度を直接検知しているのではなく、トルクを検知している。したがって、先端加速度算出部３８４は、実加速度に対応する実トルクＴ（Ｔ１〜Ｔ６）と、想定加速度に対応する想定トルクＴＡ（ＴＡ１〜ＴＡ６）との偏差（ＴＡ−Ｔ）を求め、この偏差から、各振動加速度αＢ１〜αＢ６を算出する。

ここで、関節Ｊ１を例に、振動加速度αＢ１の算出方法について、更に具体的に説明する。関節加速度算出部３８６_１は、想定トルクＴＡ１と実トルクＴ１とのトルク差（ＴＡ１−Ｔ１）を算出する。トルク差（ＴＡ１−Ｔ１）は、関節Ｊ１の振動により発生するトルクである。関節加速度算出部３８６_１は、トルク差（ＴＡ１−Ｔ１）に、ベース部２１０と関節Ｊ１との間の距離Ｌ１を乗算することにより、関節Ｊ１に発生する振動力を算出する。そして、関節加速度算出部３８６_１は、関節Ｊ１に発生する振動力を関節Ｊ１に作用する重量Ｍ１で除算することで、関節Ｊ１に発生する関節Ｊ１の駆動方向の振動加速度αＢ１を算出する。関節Ｊ２〜Ｊ６についても同様の演算を行う。

振動加速度算出部３８７は、ロボット本体２５０の各関節の振動加速度αＢ１〜αＢ６を合計することで、ロボット本体２５０の先端部（ロボットハンド２０２）の振動加速度αＢを算出する。

基端加速度算出部３８５は、加速度αから想定加速度αＡと振動加速度αＢとを減算して、ベース部２１０の振動加速度αＣを求める（基端加速度算出処理、基端加速度算出工程）。

振動量算出部３７７は、振動加速度αＣを時間で２階積分する演算を行うことで、ベース部２１０における振動量、つまり補正値ＰＣを算出する（振動算出処理、振動算出工程）。なお、図４中のｓはラプラス演算子である。

そして、補正部３７１は、逆キネマティクス演算部３７２にて各関節指令値θ１〜θ６を求めるのに使用する位置指令値を、ベース部２１０の振動量である補正値ＰＣで補正する。これにより、ロボット本体２５０の先端部における、ベース部２１０（つまり架台１５０）の振動に起因する振動を抑制することができる。

関節振動量算出部３７８は、各関節Ｊ１〜Ｊ６の振動加速度αＢ１〜αＢ６を時間で２階積分することで、ロボット本体２５０の各関節Ｊ１〜Ｊ６の振動量を示す補正値θＢ（θＢ１〜θＢ６）を求める（関節振動算出処理、関節振動算出工程）。

そして、補正部３７５は、各位置制御部３４０_１〜３４０_６にて位置制御に用いる指令値θ１〜θ６を、各補正値θＢ１〜θＢ６でそれぞれ補正する。これにより、各関節Ｊ１〜Ｊ６自体の振動を抑制することができる。

以上、本実施形態によれば、ロボット本体２５０の先端部の位置指令値Ｐが、ロボット本体２５０のベース部２１０の振動量である補正値ＰＣで補正される。したがって、ロボット本体２５０が振動する設置面、例えば架台１５０に設置された場合においても、ロボット本体２５０の先端部の振動を抑制することができる。つまり、本実施形態によれば、ベース部２１０の振動量を算出し、この振動量を補正値ＰＣとして、位置指令値Ｐに与えることで、ベース部２１０の振動によるロボット本体２５０の先端部の振動を高精度に抑制することが可能となる。

また、各関節Ｊ１〜Ｊ６で発生する振動量を各関節のトルクから算出し、各振動量を各補正値θＢ１〜θＢ６として、各関節指令値θ１〜θ６に与えることで、逆キネマティクス演算部６１０を介さずに各関節の振動を補償することができる。したがって、各関節Ｊ１〜Ｊ６の振動を高精度かつ高速に抑制することができる。

以上より、ベース部２１０に発生する振動と、各関節Ｊ１〜Ｊ６に発生する振動を高精度かつ高速に抑制することができるため、ロボット本体２５０の先端部に発生する振動を高精度かつ高速に抑制することが可能となる。また、ロボット２００が設置される架台１５０が低い剛性であったとしても、ロボット本体２５０の先端部の振動を高精度かつ高速に抑制することができる。

なお、本発明は、以上説明した実施形態に限定されるものではなく、本発明の技術的思想内で多くの変形が可能である。また、本発明の実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、本発明の実施形態に記載されたものに限定されない。

上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。

また、上述の実施形態では、ロボットアームが垂直多関節のロボットアームの場合について説明したが、これに限定するものではない。ロボットアームが、例えば、水平多関節のロボットアーム、パラレルリンクのロボットアーム、直交ロボット等、種々のロボットアームであってもよい

また、上述の実施形態では、ロボット２００が架台１５０上に設置される場合について説明したが、無人搬送車（ＡＧＶ）等の移動体上に設置されていてもよい。この場合であっても、移動体が動作することにより発生するベース部２１０の振動を求めることが可能なため、ロボット本体２５０の先端部の振動を高精度かつ高速に抑制することが可能となる。

１００…ロボット装置、２０２…ロボットハンド（先端部）、２１０…ベース部、２４０…検知部、２５０…ロボット本体、３０１…ＣＰＵ（制御部）

Claims (11)

1. 複数の関節を有するロボット本体と、
   前記ロボット本体の動作を制御する制御部と、
   前記ロボット本体に発生する加速度を検知する検知部と、を備え、
   前記制御部は、
   前記ロボット本体の先端部の位置指令値から、前記ロボット本体の各関節の位置制御に用いる関節指令値を求める指令値算出処理と、
   前記検知部による検知結果から、前記ロボット本体のベース部の振動加速度を取得する取得処理と、
   前記ロボット本体のベース部の振動加速度から、前記ロボット本体のベース部の振動量を算出する振動算出処理と、
   前記指令値算出処理にて前記各関節指令値を求めるのに使用する前記位置指令値を、前記ロボット本体のベース部の振動量で補正する補正処理と、を実行するロボット装置。
2. 前記検知部は、
   前記ロボット本体の先端部に発生する加速度を検知する先端加速度検知部と、
   前記ロボット本体の各関節に発生する各関節の駆動方向の加速度をそれぞれ検知する複数の関節加速度検知部と、を有し、
   前記制御部は、
   前記取得処理として、
   前記指令値算出処理で算出した各関節指令値から前記ロボット本体の各関節に発生する各関節の駆動方向の想定加速度を算出する関節加速度想定処理と、
   前記位置指令値から前記ロボット本体の先端部に発生する想定加速度を算出する先端加速度想定処理と、
   前記ロボット本体の各関節の実加速度と、前記ロボット本体の各関節の想定加速度との偏差に応じて、前記ロボット本体の各関節に発生する各関節の駆動方向の振動加速度を求め、前記ロボット本体の先端部の振動加速度を算出する先端加速度算出処理と、
   前記先端加速度検知部により検知された加速度から、前記先端加速度想定処理で算出した前記ロボット本体の先端部の想定加速度と、前記先端加速度算出処理で算出した前記ロボット本体の先端部の振動加速度とを減算して、前記ロボット本体のベース部の振動加速度を求める基端加速度算出処理と、を実行する請求項１に記載のロボット装置。
3. 前記制御部は、前記先端加速度算出処理では、前記ロボット本体の各関節の振動加速度の合計により、前記ロボット本体の先端部の振動加速度を算出する請求項２に記載のロボット装置。
4. 前記制御部は、前記先端加速度想定処理では、前記関節加速度想定処理で算出した前記ロボット本体の各関節の想定加速度の合計により、前記ロボット本体の先端部に発生する想定加速度を算出する請求項２又は３に記載のロボット装置。
5. 前記制御部は、
   前記ロボット本体の各関節の振動加速度から、前記ロボット本体の各関節の振動量を求める関節振動算出処理と、
   前記指令値算出処理で算出した各関節指令値を前記ロボット本体の各関節の振動量で補正する関節補正処理と、を更に実行する請求項２乃至４のいずれか１項に記載のロボット装置。
6. 前記先端加速度検知部が、前記ロボット本体の先端部に発生する力を検知する力覚センサである請求項２乃至５のいずれか１項に記載のロボット装置。
7. 前記関節加速度検知部が、前記ロボット本体の関節に発生するトルクを検知するトルクセンサである請求項２乃至６のいずれか１項に記載のロボット装置。
8. 前記制御部は、前記指令値算出処理で算出した各関節指令値から前記ロボット本体の各関節に発生する想定トルクを求める想定トルク算出処理を更に実行し、
   前記関節加速度想定処理では、前記ロボット本体の各関節に発生する想定トルクから前記ロボット本体の各関節の想定加速度を求め、
   前記先端加速度算出処理では、前記各トルクセンサにより検知された各関節の実トルクと、前記ロボット本体の各関節に発生する想定トルクとの偏差から、前記ロボット本体の各関節に発生する各関節の駆動方向の振動加速度を求める請求項７に記載のロボット装置。
9. 複数の関節を有するロボット本体の動作を制御するロボット制御方法であって、
   前記ロボット本体の先端部の位置指令値から、前記ロボット本体の各関節の位置制御に用いる関節指令値を求める指令値算出工程と、
   前記ロボット本体に発生する加速度を検知する検知部の検知結果から、前記ロボット本体のベース部の振動加速度を取得する取得工程と、
   前記ロボット本体のベース部の振動加速度から、前記ロボット本体のベース部の振動量を算出する振動算出工程と、
   前記指令値算出工程にて前記各関節指令値を求めるのに使用する前記位置指令値を、前記ロボット本体のベース部の振動量で補正する補正工程と、を備えたロボット制御方法。
10. コンピュータに、請求項９に記載のロボット制御方法の各工程を実行させるためのプログラム。
11. 請求項１０に記載のプログラムが記録された、コンピュータが読み取り可能な記録媒体。

Images