JP2017019057A - ロボット制御装置、ロボットおよびロボットシステム - Google Patents

Abstract

【課題】容易に衝突を検出できる技術の提供。
【解決手段】本発明のロボット制御装置は、ロボットの可動部の速度が速度指令に近くなるように、前記可動部を制御するロボット制御装置であって、センサーによって検出された前記可動部の速度である検出速度が、前記速度指令から乖離している場合に前記可動部にて衝突が生じたと判定する。
【選択図】図４

Description

本発明は、ロボット制御装置、ロボットおよびロボットシステムに関する。

外部からロボットに加わる外乱と関連する衝突評価量に基づいてロボットの衝突を検出する技術が知られている（特許文献１、参照）。特許文献１において、外乱は運動方程式モデルから導出される。また、モーターのトルクからロボットが受ける外乱を導出し、当該外乱に基づいて衝突を検出する技術が知られている（特許文献２、参照）。特許文献２において、モーターのトルクから外乱を導出するために、摩擦トルクや動摩擦トルクが考慮されている。

特開２００５−１０２４２７号公報 特開２００９−７２９１３号公報

しかしながら、運動方程式モデルに基づいて外乱を正確に導出することは困難であるという課題があった。外乱を正確に導出するためには、エンドエフェクターやワークも含めた質量や重心やイナーシャーが正しく設定されていなければならず、これらのパラメーターを調査して設定することは煩雑であった。さらに、ロボットの摩擦特性は、駆動部に充填されたグリスの状態に大きく依存するため、温度や使用年数等にも応じて大きくばらつくこととなる。したがって、特許文献２のように摩擦を考慮する場合、摩擦特性を適正に設定することは困難であった。

上記課題の少なくとも一つを解決するためのロボット制御装置は、ロボットの可動部が速度指令で移動するように、可動部を制御するロボット制御装置であって、センサーによって検出された可動部の速度である検出速度が、速度指令から乖離している場合に可動部にて衝突が生じたと判定する。

前記の構成において、検出速度と速度指令とが乖離しているか否かに基づいて、容易に可動部における衝突を検出でき、衝突を検出した場合に可動部にて衝突が生じたと判定することができる。すなわち、検出速度と速度指令とを取得できれば衝突を検出でき、可動部に作用している力から衝突によって可動部が受けた外乱を抽出しなくてもよい。

可動部とは、ロボットが自己の動力によって移動可能な部分であればよく、本体に対して移動可能に連結されたマニュピレーター等であってもよいし、全体が移動可能なロボットの全体を指してもよい。また、可動部は、マニュピレーター等の関節ごとに定義されてもよい。すなわち、マニュピレーター等を構成する複数の関節のそれぞれについて、検出速度が速度指令から乖離しているか否かが判定されてもよい。速度指令とは、可動部を移動させようとする速度であり、予定どおりの時刻に予定どおりの目標位置に可動部を移動させるための速度である。むろん、速度には角速度も含まれる。速度指令を得る際に可動部の重量や重心やイナーシャーや摩擦特性が未知であっても考慮しなくてもよく、容易に速度指令を得ることができる。

センサーとは、可動部の速度を導出可能な計測値を計測するセンサーであればよく、可動部の時系列の位置を検出するセンサーであってもよいし、可動部の速度そのものを検出する速度センサーであってもよいし、可動部の加速度を検出するセンサーであってもよい。

検出速度が速度指令から乖離しているとは、検出速度と速度指令との乖離度が閾値よりも大きいことであってもよい。閾値は、予め決められた固定値であってもよいし、ロボットの動作や使用者の設定に応じた可変値であってもよい。また、検出速度と速度指令との乖離度は、速度指令と検出速度の双方から導出される値であり、速度指令と検出速度の減算値であってもよいし、速度指令と検出速度との除算値であってもよいし、これらの値にフィルターを掛けた値であってもよい。フィルターの例としては、ハイパスフィルターや、ローパスフィルター、バンドパスフィルターなどがある。ロボット制御装置は、可動部にて衝突が生じたと判定した場合に、可動部を停止させてもよいし、衝突の発生を報知してもよい。可動部を停止させるとは、可動部を駆動力の発生を停止させることであってもよいし、可動部を電気的または機械的に制動することであってもよい。

なお請求項に記載された各手段の機能は、構成自体で機能が特定されるハードウェア資源、プログラムにより機能が特定されるハードウェア資源、又はそれらの組み合わせにより実現される。また、これら各手段の機能は、各々が物理的に互いに独立したハードウェア資源で実現されるものに限定されない。

ロボットシステムの模式図である。 ロボットシステムのブロック図である。 （３Ａ）（３Ｄ）は角速度のグラフ、（３Ｂ）（３Ｅ）は角速度偏差のグラフ、（３Ｃ）（３Ｆ）は診断値のグラフである。 動作の一覧表である。 第１閾値のグラフである。 他の実施形態のロボットシステムの模式図である。

以下、本発明の実施の形態を以下の順序にしたがって添付図面を参照しながら説明する。なお、各図において対応する構成要素には同一の符号が付され、重複する説明は省略される。
（１）ロボットシステムの構成：
（２）他の実施形態：

（１）ロボットシステムの構成：
図１は、本発明の第一実施例にかかるロボットシステムの模式図である。図１に示すように、ロボットシステムは、ロボット１と制御装置３とを備えている。制御装置３は、本発明のロボット制御装置を構成する。制御装置３は、専用のコンピューターであってもよいし、ロボット１のためのプログラムがインストールされた汎用のコンピューターであってもよい。

ロボット１は、１つのアームＡを備える単腕ロボットである。アームＡは基台Ｔと６つの関節Ｊ１〜Ｊ６を備える。基台Ｔは作業台に固定されている。関節Ｊ１〜Ｊ６によって基台Ｔと６個のアーム部材Ａ１〜Ａ６が連結される。アーム部材Ａ１〜Ａ６とエンドエフェクター２は本発明の可動部である。関節Ｊ２、Ｊ３、Ｊ５は曲げ関節であり、関節Ｊ１、Ｊ４、Ｊ６はねじり関節である。アームＡのうち最も先端側のアーム部材Ａ６には、力覚センサーＦＳとエンドエフェクター２とが装着される。エンドエフェクター２はグリッパーによってワークＷを把持する。エンドエフェクター２の所定位置をツールセンターポイント（ＴＣＰ）と表す。ＴＣＰの位置はエンドエフェクター２の基準の位置となる。力覚センサーＦＳは、６軸の力検出器である。力覚センサーＦＳは、互いに直交する３個の検出軸方向の力の大きさと、当該３個の検出軸まわりのトルクの大きさとを検出する。

ロボット１が設置された空間を規定する座標系をロボット座標系と表す。本実施形態において、ロボット座標系は、水平面上において互いに直交するＸ軸とＹ軸と、鉛直上向きを正方向とするＺ軸とによって規定される３次元の直交座標系である。またＸ軸周りの回転角をＲＸで表し、Ｙ軸周りの回転角をＲＹで表し、Ｚ軸周りの回転角をＲＺで表す。Ｘ，Ｙ，Ｚ方向の位置により３次元空間における任意の位置を表現でき、ＲＸ，ＲＹ，ＲＺ方向の回転角により３次元空間における任意の姿勢（回転方向）を表現できる。以下、特に示さない限り、位置と表記した場合、姿勢も意味し得ることとする。また、特に示さない限り、力と表記した場合、ＲＸ，ＲＹ，ＲＺ方向に作用するトルクも意味し得ることとする。制御装置３は、アームＡを駆動することによって、ロボット座標系においてＴＣＰの位置を制御する。エンドエフェクター２は力覚センサーＦＳに対して相対的に移動不能に固定されており、力覚センサーＦＳが検出する力の大きさと方向は、ＴＣＰに作用する力の大きさと方向と一致すると見なす。

図２は、ロボットシステムのブロック図である。制御装置３にはロボット１の制御を行うための制御プログラムがインストールされている。制御装置３は、プロセッサーやＲＡＭやＲＯＭを含むコンピューターを備える。コンピューターがＲＯＭ等の記録媒体に記録された制御プログラムを実行することにより、制御装置３は、駆動制御部３１と停止制御部３２と閾値設定部３３とを機能構成として備えることとなる。駆動制御部３１と停止制御部３２と閾値設定部３３とは、制御装置３のハードウェア資源とソフトウェア資源とが協働して実現する。なお、駆動制御部３１と停止制御部３２と閾値設定部３３とは、ＡＳＩＣ（Application Specific Integrated Circuit）等によって実現されてもよい。

駆動制御部３１は、目標位置と目標力とがＴＣＰにて実現されるようにアームＡを制御する。駆動制御部３１は、制御周期ごとに目標位置と目標力とを設定し、アームＡを制御する。本実施形態において、制御周期の長さＰは、４ｍ秒であることとする。ＴＣＰの軌道上において、制御周期ごとの目標位置を設定することにより、ＴＣＰを軌道上にて移動させることができる。

目標力とは、力覚センサーＦＳが検出すべき力であり、ＴＣＰに作用すべき力である。Ｓの文字は、ロボット座標系を規定する軸の方向（Ｘ，Ｙ，Ｚ，ＲＸ，ＲＹ，ＲＺ）のなかのいずれか１個の方向を表すこととする。例えば、Ｓ＝Ｘの場合、ロボット座標系にて設定された目標位置のＸ方向成分がＳ_t＝Ｘ_tと表記され、目標力のＸ方向成分がｆ_St＝ｆ_Xtと表記される。また、Ｓは、Ｓ方向の位置（回転角）も表すこととする。

ロボット１は、図１に図示した構成のほかに、駆動部としてのモーターＭ１〜Ｍ６と、エンコーダーＥ１〜Ｅ６と、制動部Ｂとを備える。モーターＭ１〜Ｍ６とエンコーダーＥ１〜Ｅ６とは、関節Ｊ１〜Ｊ６のそれぞれに対応して備えられており、エンコーダーＥ１〜Ｅ６はモーターＭ１〜Ｍ６の駆動位置を検出する。アームＡを制御することは、駆動部としてのモーターＭ１〜Ｍ６を制御することを意味する。駆動制御部３１は、ロボット１と通信可能なっている。駆動制御部３１は、モーターＭ１〜Ｍ６の駆動位置の組み合わせと、ロボット座標系におけるＴＣＰの位置との対応関係Ｕを記憶している。

駆動制御部３１は、モーターＭ１〜Ｍ６の駆動位置Ｄ_aを取得すると、対応関係Ｕに基づいて、当該駆動位置Ｄ_aをロボット座標系におけるＴＣＰの位置Ｓ（Ｘ，Ｙ，Ｚ，ＲＸ，ＲＹ，ＲＺ）に変換する。駆動制御部３１は、ＴＣＰの位置Ｓと、力覚センサーＦＳの検出値とに基づいて、力覚センサーＦＳに現実に作用している作用力ｆ_Sをロボット座標系において特定する。なお、力覚センサーＦＳは、独自の座標系において検出値を検出するが、力覚センサーＦＳとＴＣＰとの相対位置・方向とが既知のデータとして記憶されているため、駆動制御部３１はロボット座標系における作用力ｆ_Sを特定できる。駆動制御部３１は、作用力ｆ_Sに対して重力補償を行う。重力補償とは、作用力ｆ_Sから重力成分を除去することである。重力補償を行った作用力ｆ_Sは、ＴＣＰに作用している重力以外の力と見なすことができる。ＴＣＰの姿勢ごとに嵌合部材に作用する作用力ｆ_Sの重力成分が予め調査されており、駆動制御部３１は、作用力ｆ_SからＴＣＰの姿勢に対応する重力成分を減算することにより重力補償が実現する。

駆動制御部３１は、目標力ｆ_Stと作用力ｆ_Sとをインピーダンス制御の運動方程式に代入することにより、力由来補正量ΔＳを特定する。（１）式は、インピーダンス制御の運動方程式である。

（１）式の左辺は、ＴＣＰの位置Ｓの２階微分値に仮想慣性係数ｍを乗算した第１項と、ＴＣＰの位置Ｓの微分値に仮想粘性係数ｄを乗算した第２項と、ＴＣＰの位置Ｓに仮想弾性係数ｋを乗算した第３項とによって構成される。（１）式の右辺は、目標力ｆ_Stから現実の作用力ｆ_Sを減算した力偏差Δｆ_S（ｔ）によって構成される。（１）式における微分とは、時間による微分を意味する。

インピーダンス制御とは、仮想の機械的インピーダンスをモーターＭ１〜Ｍ６によって実現する制御である。仮想慣性係数ｍはＴＣＰが仮想的に有する質量を意味し、仮想粘性係数ｄはＴＣＰが仮想的に受ける粘性抵抗を意味し、仮想弾性係数ｋはＴＣＰが仮想的に受ける弾性力のバネ定数を意味する。各パラメーターｍ，ｄ，ｋは方向ごとに異なる値に設定されてもよいし、方向に拘わらず共通の値に設定されてもよい。力由来補正量ΔＳとは、ＴＣＰが機械的インピーダンスを受けた場合に、目標力ｆ_Stとの力偏差Δｆ_S（ｔ）を解消するために、ＴＣＰが移動すべき位置Ｓの大きさを意味する。駆動制御部３１は、目標位置Ｓ_tに、力由来補正量ΔＳを加算することにより、インピーダンス制御を考慮した補正目標位置（Ｓ_t＋ΔＳ）を特定する。

そして、駆動制御部３１は、対応関係Ｕに基づいて、ロボット座標系を規定する各軸の方向の補正目標位置（Ｓ_t＋ΔＳ）を、各モーターＭ１〜Ｍ６の目標の駆動位置である目標駆動位置Ｄ_tに変換する。そして、駆動制御部３１は、目標駆動位置Ｄ_tからモーターＭ１〜Ｍ６の現実の駆動位置Ｄ_aを減算することにより、駆動位置偏差Ｄ_e（＝Ｄ_t−Ｄ_a）を算出する。駆動制御部３１は、駆動位置偏差Ｄ_eに位置制御ゲインＫ_pを乗算した値である角速度指令Ｖ_c（＝Ｋ_p・Ｄ_e）と、現実の駆動位置Ｄ_aの時間微分値である駆動速度との差である駆動速度偏差に、速度制御ゲインＫ_vを乗算した値とを加算することにより、制御量Ｄ_cを特定する。制御量Ｄ_cは、モーターＭ１〜Ｍ６のそれぞれについて特定される。

また、駆動制御部３１は、現在、アームＡに行わせている動作Ｑ１〜Ｑ６に対応する角加速度の上限値Ａ_maxを取得し、現実の角加速度Ａ_aの絶対値が上限値Ａ_maxよりも大きい場合に制御量Ｄ_cを下方修正する。例えば、制御量Ｄ_cを０に修正してもよい。以下、角加速度Ａ_aと表記した場合、特に示さない限り角加速度Ａ_aの絶対値を意味することとする。これにより、アーム部材Ａ１〜Ａ６やエンドエフェクター２が関節Ｊ１〜Ｊ６まわりに回転する角加速度Ａ_aを制限することができる。また、角加速度Ａ_aの上限値Ａ_maxが大きいほど、角加速度Ａ_aの制限が緩和されていると言うことができる。現実の角加速度Ａ_aは、現在の制御周期における現実の駆動位置Ｄ_a（ｎ）を直前の制御周期における現実の駆動位置Ｄ_a（ｎ−１）で減算した値を制御周期の長さＰの二乗で除算することにより得られてもよいし、ジャイロセンサー等の計測値に基づいて得られてもよい。なお、動作Ｑ１〜Ｑ６の詳細については後述する。ｎは制御周期の番号を示す自然数である。

以上説明した駆動制御部３１の構成により、目標位置Ｓ_tと目標力ｆ_Stとに基づいてアームＡを制御することができる。ここで、現実の作用力ｆ_Sを目標力ｆ_Stとするための制御が力制御であり、ＴＣＰの現実の位置Ｓを目標位置Ｓ_tとするための制御が位置制御である。
本実施形態において、駆動制御部３１は、ロボット１に行わせる作業内容に応じて、位置制御と力制御の双方を行うことと、位置制御のみを行うことができる。例えば、現実の作用力ｆ_Sに拘わらず、図２の力由来補正量ΔＳが常時０であると見なすことにより、実質的に位置制御のみを行うことができる。

停止制御部３２は、センサーによって検出された可動部の速度である検出速度が、速度指令から乖離している場合に可動部を停止させる。可動部とは、移動可能なアーム部材Ａ１〜Ａ６とエンドエフェクター２である。検出速度とは、関節Ｊ１〜Ｊ６の回転軸まわりにアーム部材Ａ１〜Ａ６とエンドエフェクター２とが回転する角速度であり、センサーとしてのエンコーダーＥ１〜Ｅ６によって検出された現実の駆動位置Ｄ_aから導出した検出角速度Ｖ_aである。例えば、アーム部材Ａ１の検出角速度Ｖ_aは、関節Ｊ１の回転軸まわりにアーム部材Ａ１が回転する角速度であり、当該アーム部材Ａ１を基台Ｔと連結する関節Ｊ１のエンコーダーＥ１の出力信号から導出される。

検出角速度Ｖ_aは、現在の制御周期における現実の駆動位置Ｄ_a（ｎ）を直前の制御周期における現実の駆動位置Ｄ_a（ｎ−１）で減算した値を制御周期の長さＰで除算することにより得られてもよいし、ジャイロセンサー等の計測値に基づいて得られてもよい。

速度指令とは、関節Ｊ１〜Ｊ６の回転軸まわりにアーム部材Ａ１〜Ａ６とエンドエフェクター２が回転すべき角速度であり、上述した角速度指令Ｖ_cである。具体的に、角速度指令Ｖ_cは、目標駆動位置Ｄ_tからモーターＭ１〜Ｍ６の現実の駆動位置Ｄ_aを減算した駆動位置偏差Ｄ_e（＝Ｄ_t−Ｄ_a）に位置制御ゲインＫ_pを乗算した値である。

停止制御部３２は、可動部の状態を示す診断値が第１閾値よりも大きくなった場合に、可動部を停止させる。可動部の状態を示す診断値とは、速度指令と検出速度との差にハイパスフィルター（ＨＰＦ）を掛けた値であり、角速度指令Ｖ_cと検出角速度Ｖ_aとの差である角速度偏差（Ｖ_c−Ｖ_a）の絶対値にハイパスフィルターを掛けた値（以下、第１診断値ＨＶ１と表記する）である。

図３Ａは、可動部としてのアーム部材Ａ１の角速度Ｖのグラフである。図３Ａの縦軸は角速度Ｖを示し、横軸は時刻ｔを示す。図３Ａでは、アーム部材Ａ１が加速し、その後一定の速度で回転し、最後に減速して停止する角速度Ｖを想定している。図３Ｂは、図３Ａの場合（図３Ａに基づいて目標位置Ｓ_tを順次設定した場合）の角速度偏差（Ｖ_c−Ｖ_a）の例を示す。図３Ｂの縦軸は角速度偏差（Ｖ_c−Ｖ_a）を示し、横軸は時刻ｔを示す。図３Ｂに示すように、目標位置Ｓ_tに対して現実の駆動位置Ｄ_aがわずかに遅れて追従するため、加速中においては角速度偏差（Ｖ_c−Ｖ_a）が正に振れ、減速中においては角速度偏差（Ｖ_c−Ｖ_a）が負に振れることとなる。従って、角速度偏差（Ｖ_c−Ｖ_a）は加速と減速の周期に対応した低周波成分を含むこととなる。

図３Ｃは、図３Ａの場合の第１診断値ＨＶ１のグラフである。角速度偏差（Ｖ_c−Ｖ_a）の絶対値にハイパスフィルターを掛けることにより、加速と減速の周期に対応した低周波成分を低減することができる。ハイパスフィルターの通過周波数帯は、加速と減速が繰り返される周波数よりも高い周波数帯とすることが望ましい。

図３Ｂに示すように、×で示す時刻ｔにて衝突（アーム部材Ａ１の移動を妨げる衝突）が生じると、角速度偏差（Ｖ_c−Ｖ_a）が急峻に減少することとなる。衝突に応じた角速度偏差（Ｖ_c−Ｖ_a）の変化の成分は急峻であるため、図３Ｃに示すようにハイパスフィルターを掛けても大きく減衰しない。

停止制御部３２は、第１診断値ＨＶ１が第１閾値ＴＨ１よりも大きい場合に、アームＡを停止させる。第１閾値ＴＨ１は、可動部としてのアーム部材Ａ１〜Ａ６とエンドエフェクター２のそれぞれについて設定されており、停止制御部３２は、アーム部材Ａ１〜Ａ６とエンドエフェクター２のそれぞれについて第１閾値ＴＨ１の判定を行う。

すなわち、停止制御部３２は、アーム部材Ａ１についてはエンコーダーＥ１の信号に基づく第１診断値ＨＶ１と第１閾値ＴＨ１の判定を行い、アーム部材Ａ２についてはエンコーダーＥ２の信号に基づく第１診断値ＨＶ１と第１閾値ＴＨ１の判定を行い、アーム部材Ａ３についてはエンコーダーＥ３の信号に基づく第１診断値ＨＶ１と第１閾値ＴＨ１の判定を行い、アーム部材Ａ４についてはエンコーダーＥ４の信号に基づく第１診断値ＨＶ１と第１閾値ＴＨ１の判定を行い、アーム部材Ａ５についてはエンコーダーＥ５の信号に基づく第１診断値ＨＶ１と第１閾値ＴＨ１の判定を行い、アーム部材Ａ６（エンドエフェクター２）についてはエンコーダーＥ６の信号に基づく第１診断値ＨＶ１と第１閾値ＴＨ１の判定を行う。

可動部としてのアーム部材Ａ１〜Ａ６とエンドエフェクター２のいずれかにおいて、第１診断値ＨＶ１が第１閾値ＴＨ１よりも大きいと判定した場合、停止制御部３２は、可動部にて衝突が生じたとして、モーターＭ１〜Ｍ６を停止させ、アームＡの全体を停止させる。なお、停止制御部３２は、アームＡ全体ではなく、第１診断値ＨＶ１が第１閾値ＴＨ１となったアーム部材Ａ１〜Ａ６とエンドエフェクター２を直接駆動する１個のモーターＭ１〜Ｍ６のみを停止させてもよい。

以上説明した本実施形態の構成では、検出速度と速度指令とが乖離しているか否かに基づいて、容易に可動部における衝突を検出でき、衝突を検出した場合に可動部にて衝突が生じたと判定することができる。すなわち、検出速度と速度指令とを取得できれば衝突を検出でき、可動部に作用している力から衝突によって可動部が受けた外乱を抽出しなくてもよい。従って、衝突によって可動部が受けた外乱を抽出するために運動方程式を解いたり、当該運動方程式に適用するパラメーターを予め調査しておいたりする必要がなく、容易に衝突を検出できる。

また、速度指令と検出速度との差にハイパスフィルターを掛けた値、すなわち角速度偏差（Ｖ_c−Ｖ_a）の絶対値にハイパスフィルターを掛けた第１診断値ＨＶ１が第１閾値ＴＨ１よりも大きい場合に、可動部としてのアーム部材Ａ１〜Ａ６とエンドエフェクター２を停止させる。第１診断値ＨＶ１は、サーボ制御で使用する値から導出できるため、第１診断値ＨＶ１を導出するための演算負荷を抑制できるとともに、第１診断値ＨＶ１を早期に導出でき、衝突判定を早期に完了させることができる。また、ハイパスフィルターを適用することにより、Ｓ／Ｎ比の良好な第１診断値ＨＶ１を得ることができ、誤判定の可能性を低減できる。すなわち、加速と減速の周期で表れる角速度偏差（Ｖ_c−Ｖ_a）の振れは衝突の判定の際のノイズとなるが、このノイズをハイパスフィルターにより抑制できる。さらに、Ｓ／Ｎ比の良好な第１診断値ＨＶ１を使用することにより第１閾値ＴＨ１を小さい値に設定することができ、衝突判定をより高速に行うことができる。

次に、第１閾値ＴＨ１の詳細について説明する。上述したように、第１閾値ＴＨ１は、可動部としてのアーム部材Ａ１〜Ａ６のそれぞれについて設定されるが、さらに第１閾値ＴＨ１は動作Ｑ１〜Ｑ６ごとに設定される。

図４は、動作Ｑ１〜Ｑ６と第１閾値ＴＨ１と角加速度Ａ_aの上限値Ａ_maxとの関係を示す表である。図４に示すように、動作Ｑ１は通常動作であり、動作Ｑ２はトルク飽和動作であり、動作Ｑ３はジョグ動作であり、動作Ｑ４は停止動作であり、動作Ｑ５はトラッキング動作であり、動作Ｑ６は特異点回避動作である。動作Ｑ１〜Ｑ６にはそれぞれ角加速度Ａ_aの上限値Ａ_maxが設定されており、動作Ｑ２〜Ｍ６には通常動作（Ｑ１）における角加速度Ａ_aの上限値Ａ_max1よりも大きい角加速度の上限値Ａ_max2〜Ａ_max6が設定されている。特異点回避動作（Ｑ６）には動作Ｑ１〜Ｑ５の角加速度Ａ_aの上限値Ａ_max1〜Ａ_max6よりも大きい角加速度Ａ_aの上限値Ａ_max6が設定されている。また、動作Ｑ１〜Ｑ６は、いずれも力制御を行うことなく位置制御のみを行う動作である。

トルク飽和動作（Ｑ２）は、モーターＭ１〜Ｍ６のトルクが飽和すると予測される場合に設定される動作であり、通常動作（Ｑ１）よりも角加速度Ａ_aの上限値Ａ_max2を緩和することにより、モーターＭ１〜Ｍ６を保護するために設定される。ジョグ動作（Ｑ３）は、図示しない操作部が操作されている操作期間だけモーターＭ１〜Ｍ６を駆動させる動作であり、通常動作（Ｑ１）よりも角加速度Ａ_aの上限値Ａ_max3を緩和することにより、操作応答性を向上させるために設定される。停止動作（Ｑ４）は、異常時等においてモーターＭ１〜Ｍ６の駆動を停止させる動作であり、通常動作（Ｑ１）よりも角加速度Ａ_aの上限値Ａ_max4を緩和することにより、急速にアームＡを停止させることができるように設定される。停止動作（Ｑ４）もモーターＭ１〜Ｍ６の駆動を停止させる動作であるが、停止制御部３２が衝突時にモーターＭ１〜Ｍ６の駆動を停止させる場合よりもゆるやかにモーターＭ１〜Ｍ６の駆動を停止させる動作である。

トラッキング動作（Ｑ５）は、ベルトコンベア等の搬送装置にエンドエフェクター２を追従させる動作であり、通常動作（Ｑ１）よりも角加速度Ａ_aの上限値Ａ_max5を緩和することにより、搬送装置への追従性を向上させるために設定される。特異点回避動作（Ｑ６）は、特異点（特異姿勢）となることを回避するためにいずれかの関節Ｊ１〜Ｊ６を大きく駆動（例えば約１８０度回転）させる動作であり、通常動作（Ｑ１）よりも角加速度Ａ_aの上限値Ａ_max6を緩和することにより、特異点の回避に要する期間を短縮するために設定される。

以上説明したように、角加速度の上限値Ａ_maxが緩和される動作においては、通常動作における角加速度Ａ_aの上限値Ａ_max1よりも大きい角加速度Ａ_aが許容され得る。なお、角加速度Ａ_aの上限値Ａ_maxも関節Ｊ１〜Ｊ６ごとに設定される値である。

図３Ａは通常動作（Ｑ１）におけるアーム部材Ａ１の角速度Ｖのグラフであり、図３Ｄはジョグ動作（Ｑ３）におけるアーム部材Ａ１の角速度Ｖのグラフである。ジョグ動作（Ｑ３）においては、通常動作（Ｑ１）よりも急な加減速（グラフにおける急な傾き）が許容される。図３Ｂは通常動作（Ｑ１）における角速度偏差（Ｖ_c−Ｖ_a）を示し、図３Ｅはジョグ動作（Ｑ３）における角速度偏差（Ｖ_c−Ｖ_a）を示している。アームＡの応答性には一定の限界があるため、急な加減速が行われるジョグ動作（Ｑ３）においては、速度指令Ｖ_cの急激な変化に対する検出角速度Ｖ_aの追従遅れが顕著となり、通常動作（Ｑ１）よりも角速度偏差（Ｖ_c−Ｖ_a）が大きく振れることとなる。図３Ｃは通常動作（Ｑ１）における第１診断値ＨＶ１を示し、図３Ｆはジョグ動作（Ｑ３）における第１診断値ＨＶ１を示している。ハイパスフィルターを掛けることによって加速や減速の周期で表れる第１診断値ＨＶ１の振れは軽減されるものの、通常動作（Ｑ１）よりもジョグ動作（Ｑ３）における第１診断値ＨＶ１の振れが大きくなっている。すなわち、角加速度Ａ_aの上限値Ａ_maxが通常動作よりも緩和される動作Ｑ２〜Ｑ６においては、衝突の判定における第１診断値ＨＶ１のノイズが大きくなり得る。

そこで、閾値設定部３３は、第１動作よりも可動部の加速度の制限が緩和される第２動作において、第１動作よりも第１閾値ＴＨ１を大きい値に設定する。すなわち、閾値設定部３３は、第１動作としての通常動作（Ｑ１）よりも角加速度Ａ_aの上限値Ａ_maxが大きい値に緩和される第２動作としてのトルク飽和動作（Ｑ２）とジョグ動作（Ｑ３）と停止動作（Ｑ４）とトラッキング動作（Ｑ５）と特異点回避動作（Ｑ６）とにおいて、通常動作（Ｑ１）よりも大きい第１閾値ＴＨ１を設定する。

図５は、角加速度Ａ_aの上限値Ａ_maxと第１閾値ＴＨ１との関係を示すグラフである。図５の縦軸は第１閾値ＴＨ１を示し、横軸は角加速度Ａ_aの上限値Ａ_maxを示す。図５における黒丸はいずれの物体とも衝突しない状態において、角加速度Ａ_aの上限値Ａ_maxごとにアームＡの各関節Ｊ１〜Ｊ６を駆動させる実験を行った際の第１診断値ＨＶ１を示している。あらゆる動作状況を想定して、各種制御（ＰＴＰ(Point to Point)制御とＣＰ(Continuous Path)制御等）を、各種の荷重（ワークＷの質量）や偏心位置やイナーシャーの条件の下で実験を行って第１診断値ＨＶ１（連続した動作中における最大値）を得ることが望ましい。図５において一点鎖線で示すように、実験によって得られた多数の第１診断値ＨＶ１を近似する近似関数Ｒ（Ａ_max）を例えば最小二乗法等によって算出し、閾値設定部３３が読み出し可能に記録しておく。近似関数Ｒ（Ａ_max）は、関節Ｊ１〜Ｊ６ごとに算出され、記録される。

閾値設定部３３は、現在のアームＡに行わせている動作Ｑ１〜Ｑ６を取得し、当該動作Ｑ１〜Ｑ６に対応する角加速度Ａ_aの上限値Ａ_maxを近似関数Ｒ（Ａ_max）に代入する。閾値設定部３３は、現在の動作が動作Ｑ１〜Ｑ５のいずれかであれば、近似関数Ｒ（Ａ_max）によって得られた値をｎ倍（ｎは２以上の整数）した値を第１閾値ＴＨ１として設定する。閾値設定部３３は、現在の動作が特異点回避動作（Ｑ６）であれば、近似関数Ｒ（Ａ_max）によって得られた値をｍ倍（ｍはｎよりも大きい整数）した値を第１閾値ＴＨ１として設定する。図５では、ｎ＝３，ｍ＝４となっている。図５においては、角加速度Ａ_aの上限値Ａ_maxが連続的に変化した場合（実際には動作Ｑ１〜Ｑ６ごとの不連続値）における第１閾値ＴＨ１の推移が実線によって示されている。閾値設定部３３は、以上のようにして算出した第１閾値ＴＨ１を停止制御部３２に設定する。

以上説明した本実施形態の構成では、可動部の状態を示す第１診断値ＨＶ１が第１閾値ＴＨ１よりも大きくなった場合に、可動部の状態が衝突によって乱れたと判断することができ、衝突を検出できる。ここで、第１動作としての通常動作よりも可動部の加速度の上限値Ａ_maxが緩和される第２動作としての動作Ｑ２〜Ｑ６において、第１動作よりも第１閾値ＴＨ１が大きい値に設定される。これにより、急な加速が許容される動作Ｑ２〜Ｑ６において、ある程度、第１診断値ＨＶ１が乱れることを許容でき、衝突を誤検出する可能性を低減することができる。

また、第２動作は、可動部の特異点を回避する特異点回避動作を含むため、第１診断値ＨＶ１が乱れやすい特異点回避動作においても、衝突を誤検出する可能性を低減することができる。ここで、動作Ｑ１〜Ｑ５を第１動作と捉えれば、特異点回避動作（Ｑ６）のみを第２動作として捉えることができる。すなわち、本実施形態において、第１動作は、通常動作（Ｑ１）だけでなく、トルク飽和動作（Ｑ２）とジョグ動作（Ｑ３）と停止動作（Ｑ４）とトラッキング動作（Ｑ５）も含む、と捉えることができる。

本実施形態において、可動部が第３動作を行っている場合に、停止制御部３２は、第１診断値ＨＶ１の大きさに拘わらず可動部を停止させない。図４に示すように、第３動作は、可動部の機械的な制動を解除する制動解除動作（Ｑ９）と、可動部に作用する作用力ｆ_Sが目標力ｆ_Stとなるように可動部を制御する力制御動作（Ｑ７）と、のうちの少なくとも一つを含む。さらに、第３動作は、目標のトルクを生じさせるようにモーターＭ１〜Ｍ６を制御するトルク制御動作（Ｑ８）を含む。力制御動作（Ｑ７）とトルク制御動作（Ｑ８）とにおいては、目標力ｆ_Stと目標のトルクとが実現されるようにモーターＭ１〜Ｍ６が制御することが優先されるため、角加速度Ａ_aの上限値Ａ_maxは設定されていない。制動解除動作（Ｑ９）は、制動部ＢによるアームＡの機械的な制動を解除する動作であり、重力に抗して制動部ＢがアームＡを支持する力が失われる際の動作であり、重力加速度に起因する角加速度Ａ_aで関節Ｊ１〜Ｊ６が回転する際の動作である。重力加速度に起因する角加速度Ａ_aで関節Ｊ１〜Ｊ６が回転することは不可避であるため、制動解除動作（Ｑ９）についても角加速度の上限値Ａ_maxは設定されていない。

以上のように、第３動作としての動作Ｑ７〜Ｑ９においては、いずれも角加速度Ａ_aの上限値Ａ_maxが設定されておらず、第１診断値ＨＶ１のＳ／Ｎ比を確保することができないおそれがある。そこで、可動部が第３動作を行っている場合に、停止制御部３２は、第１診断値ＨＶ１の大きさに拘わらず可動部を停止させないようにすることにより、衝突を誤検出する可能性を低減することができる。

（２）他の実施形態：
可動部の状態を示す値にハイパスフィルターを掛けることにより、衝突に応じて急激に変動する可動部の状態を判定可能な診断値を得ることができる。ハイパスフィルターを掛けることにより、運動方程式モデルを解かなくても可動部の状態を示す値から外乱に起因する急激な変動成分を抽出できる。診断値は、可動部の状態を示す値であれば特に限定されず、例えば以下のような値であってもよい。

例えば、停止制御部３２は、可動部を駆動させる駆動部にて生じたトルクにハイパスフィルターを掛けた値が、第２閾値よりも大きい場合に、可動部を停止させてもよい。具体的に、停止制御部３２は、モーターＭ１〜Ｍ６に流れる電流を微小なサンプリング周期ごとにサンプリングし、当該電流にハイパスフィルターを掛けた値を第２診断値として取得し、当該第２診断値が第２閾値よりも大きい場合に、衝突が生じたと判定してもよい。衝突が生じた場合、衝突によってアーム部材Ａ１〜Ａ６やエンドエフェクター２が受けた力によってモーターＭ１〜Ｍ６が急激なトルクを受けることとなり、モーターＭ１〜Ｍ６の電流にも急激な変化が生じることとなる。モーターＭ１〜Ｍ６のトルクにハイパスフィルターを掛けることにより、モーターＭ１〜Ｍ６が本来生じさせるべきトルクの振幅（低周波成分）を軽減することができ、急激なトルクの変化の有無を判定できる。さらに、停止制御部３２は、可動部を駆動させる駆動部にて生じたトルクを微分した値にハイパスフィルターを掛けた値が、閾値よりも大きい場合に、可動部を停止させてもよい。

さらに、可動部には角速度検出器が設けられてもよい。例えば、図６に示すようにアーム部材Ａ１〜Ａ５のそれぞれにジャイロセンサーＧＹ１〜ＧＹ５が備えられ、当該ジャイロセンサーＧＹ１〜ＧＹ５の出力信号を停止制御部３２が取得してもよい。なお、停止制御部３２は、力覚センサーＦＳからエンドエフェクター２の角速度を取得すればよい。停止制御部３２は、角速度検出器から出力される可動部の角速度を微分した値にハイパスフィルターを掛けた値が、第３閾値よりも大きい場合に、可動部を停止させてもよい。具体的に、停止制御部３２は、ジャイロセンサーＧＹ１〜ＧＹ５と力覚センサーＦＳから角速度を取得し、当該角速度を微分したトルク比例値にハイパスフィルターを掛けた値を第３診断値として取得し、当該第３診断値が第３閾値よりも大きい場合に、衝突が生じたと判定してもよい。ジャイロセンサーＧＹ１〜ＧＹ５と力覚センサーＦＳによって角速度を検出することにより、モーターＭ１〜Ｍ６に流れる電流に基づいてトルクを検出するよりも、応答性のよい診断値を得ることができ、早期に衝突を判定できる。他の実施形態においても、ハイパスフィルターの通過周波数帯を、加速と減速が繰り返される周波数よりも高い周波数帯とすることが望ましい。なお、ジャイロセンサーＧＹ１〜ＧＹ５は、必ずしもアーム部材Ａ２〜Ａ６のそれぞれに備えられなくてもよく、アーム部材Ａ１〜Ａ６のうちの１個以上に備えられればよい。

さらに、停止制御部３２は、可動部を駆動させる駆動部にて生じたトルクを２階微分した値にハイパスフィルターを掛けた値が、閾値よりも大きい場合に、可動部を停止させてもよい。なお、アーム部材Ａ１〜Ａ６やエンドエフェクター２に加速度センサーが備えられてもよく、停止制御部３２は、加速度センサーから出力される加速度に基づいて衝突の有無を判定してもよい。また、第１閾値〜第３閾値による衝突の有無の判定を複数並行して行ってもよいし、第１閾値〜第３閾値のうちのいずれか１個による衝突の有無の判定を単独で行ってもよい。また、第２閾値と第３閾値も、動作Ｑ１〜Ｑ６に応じて設定されてもよい。

前記実施形態においては、力制御動作（Ｑ７）において、第１診断値ＨＶ１に基づく衝突の判定を行わないようにしたが、停止制御部３２は、力制御動作（Ｑ７）においても第１診断値ＨＶ１に基づく衝突の判定を行ってもよい。この場合、閾値設定部３３は、他の物体に接触させた状態で可動部を制御する力制御動作（Ｑ７）において、他の物体に接触させない状態で可動部を制御する動作Ｑ１〜Ｑ６よりも第１閾値ＴＨ１を大きい値に設定してもよい。なお、エンドエフェクター２をワークＷ等の接触予定物に接触させて作業を行う際に、力制御動作（Ｑ７）が行われる。

インピーダンス制御を行うことにより、エンドエフェクター２と接触予定物との間に作用する作用力ｆ_Sを制御して接触物予定物に力を加える作業を行うことができる。また、仮想的な機械インピーダンスを実現することにより、エンドエフェクター２や接触予定物の破損を防止できる。このように、エンドエフェクター２が接触予定物に接触することが予定されている力制御動作（Ｑ７）においては、動作Ｑ１〜Ｑ６よりも第１診断値ＨＶ１が大きくなり得るため、第１閾値ＴＨ１を大きい値に設定することにより、衝突の誤判定を防止すればよい。また、目標力ｆ_Sが大きいほど角加速度Ａ_aが大きくなり得るため、第１診断値ＨＶ１も大きくなり得ることとなる。そのため、停止制御部３２は、目標力ｆ_Sが大きいほど第１閾値ＴＨ１を大きい値に設定してもよい。また、インピーダンス制御における仮想粘性係数ｄと仮想弾性係数ｋと仮想慣性係数ｍが、エンドエフェクター２を接触予定物にやわらかく接触させるような設定となっている場合には、第１閾値ＴＨ１を小さい値に設定してもよい。

また、停止制御部３２は、ロボット１に行わせている動作Ｑ１〜Ｑ９を必ずしも駆動制御部３１から取得しなくてもよい。例えば、停止制御部３２は、エンドエフェクター２が移動している速度が予め決められた閾値以下に転じた場合に、エンドエフェクター２が接触予定物に接触し得る状態にあると判定し、第１閾値ＴＨ１を現在の値よりも大きい値に設定してもよい。さらに、停止制御部３２は、エンドエフェクター２の速度が継続的に減少している減速中において、エンドエフェクター２の速度が閾値以下に転じた場合に、エンドエフェクター２が接触予定物に接触し得る状態にあると判定してもよい。すなわち、停止制御部３２は、エンドエフェクター２の速度が閾値以下に転じた場合であっても、エンドエフェクター２が減速中でなければ、第１閾値ＴＨ１を現在の値のまま維持してもよい。これにより、エンドエフェクター２の速度が０に近い状態から加速しようとする場合における衝突の検知漏れを抑制できる。さらに、通常動作（Ｑ１）等において予定されたＴＣＰの移動経路の経路長もしくは所要時間のうち、予め決められた比率（例えば９０〜９５％）の距離もしくは時間だけＴＣＰの移動が完了した場合に、エンドエフェクター２が接触予定物に接触し得る状態にあると判定し、第１閾値ＴＨ１を現在の値よりも大きい値に設定してもよい。むろん、以上説明した手法によってエンドエフェクター２が接触予定物に接触し得る状態にあると判定した場合に、停止制御部３２は、第１診断値ＨＶ１に基づく衝突の検知を無効にしてもよい。

ところで、衝突によって可動部の状態がゆるやかに変化する場合も有り得る。例えば、モーターＭ１〜Ｍ６のトルクや角速度が一定値以下に制限されている動作（ローパワー動作）において、静止している障害物に可動部が衝突する場合には、衝突による診断値の変動成分がハイパスフィルターを通過できない場合も考えられ得る。そのため、停止制御部３２は、角速度偏差（Ｖ_c−Ｖ_a）にハイパスフィルターを掛けた第１診断値ＨＶ１に基づく衝突の判定とともに、角速度偏差（Ｖ_c−Ｖ_a）にハイパスフィルターを掛けていない診断値に基づく衝突の判定を行ってもよい。これにより、ゆるやかな衝突の検出漏れが生じる可能性を低減できるとともに、急激な衝突も検出できる。

なお、停止制御部３２は、検出速度が速度指令から乖離しているか否かを、必ずしも検出速度と速度指令との差に基づいて判定しなくてもよい。例えば、停止制御部３２は、角速度指令Ｖ_cを検出角速度Ｖ_aで除算した値が１から基準値以上離れている場合に衝突があったと判定してもよい。また、停止制御部３２は、角速度偏差（Ｖ_c−Ｖ_a）に高周波のノイズが混入する場合等において、角速度偏差（Ｖ_c−Ｖ_a）にハイパスフィルターだけでなくローパスフィルターを掛けてもよい。また、上述した動作Ｑ１〜Ｑ９は一例に過ぎず、駆動制御部３１は、動作Ｑ１〜Ｑ９のうちのいずれかを行わないようにしてもよいし、動作Ｑ１〜Ｑ９以外の動作を行ってもよい。

さらに、第１閾値ＴＨ１は、必ずしも動作Ｑ１〜Ｑ６に応じて設定されなくてもよく、動作Ｑ１〜Ｑ６に拘わらず一定の値であってもよい。例えば、最も角加速度Ａ_aの制限が緩和される特異点回避動作（Ｑ６）においても衝突の誤検出が生じない第１閾値ＴＨ１を特定し、当該第１閾値ＴＨ１をすべての動作Ｑ１〜Ｑ６に対して適用してもよい。

ロボット１は、必ずしも６軸の単腕ロボットでなくてもよく、アームＡの関節数は特に限定されない。ロボット１は、双腕ロボットであってもよいし、スカラーロボットであってもよい。また、力検出器は、力覚センサーＦＳでなくてもよく、関節Ｊ１〜Ｊ６ごとに当該関節Ｊ１〜Ｊ６に作用するトルクを検出するトルクセンサーであってもよい。また、トルクセンサーの代わりにモーターＭ１〜Ｍ６の負荷に基づいてトルクが検出されてもよい。また、制御装置３は、ロボット１と独立した装置でなくてもよく、ロボット１に内蔵されてもよい。この場合、駆動制御部３１や停止制御部３２や閾値設定部３３は、ロボット１に備えられることとなる。

１…ロボット、２…エンドエフェクター、３…制御装置、３１…駆動制御部、３２…停止制御部、３３…閾値設定部、Ａ…アーム、Ａ１〜Ａ６…アーム部材、ｄ…仮想粘性係数、Ｄ_a…駆動位置、Ｄ_c…制御量、Ｄ_e…駆動位置偏差、Ｄ_t…目標駆動位置、Ｅ１〜Ｅ６…エンコーダー、ｆ…力、ｆ_S…作用力、ｆ_S…目標力、ＦＳ…力覚センサー、ｆ_St…目標力、Ｊ１〜Ｊ６…関節、ｋ…仮想弾性係数、Ｋ_p…位置制御ゲイン、Ｋ_v…速度制御ゲイン、ｍ…仮想慣性係数、Ｍ１〜Ｍ６…モーター、Ｗ…ワーク、Δｆ_S…力偏差、ΔＳ…力由来補正量

Claims (8)

1. ロボットの可動部の速度が速度指令に近くなるように、前記可動部を制御するロボット制御装置であって、
   センサーによって検出された前記可動部の速度である検出速度が、前記速度指令から乖離している場合に前記可動部にて衝突が生じたと判定する、
   ロボット制御装置。
2. 前記可動部にて衝突が生じたと判定した場合に前記可動部を停止させる、
   請求項１に記載のロボット制御装置。
3. 前記速度指令と前記検出速度との差が第１閾値よりも大きい場合に、前記可動部にて衝突が生じたと判定する、
   請求項１または請求項２のいずれかに記載のロボット制御装置。
4. 前記速度指令と前記検出速度との差にハイパスフィルターを掛けた値が第１閾値よりも大きい場合に、前記可動部にて衝突が生じたと判定する、
   請求項３に記載のロボット制御装置。
5. 前記可動部を駆動させる駆動部にて生じたトルクにハイパスフィルターを掛けた値が、第２閾値よりも大きい場合に、前記可動部にて衝突が生じたと判定する、
   請求項１から請求項４のいずれか一項に記載のロボット制御装置。
6. 前記可動部には角速度検出器が設けられており、
   前記角速度検出器から出力される前記可動部の角速度を微分した値にハイパスフィルターを掛けた値が、第３閾値よりも大きい場合に、前記可動部にて衝突が生じたと判定する、
   請求項１から請求項５のいずれか一項に記載のロボット制御装置。
7. 請求項１から請求項６のいずれか一項に記載のロボット制御装置によって前記可動部が制御されるロボット。
8. 請求項１から請求項６のいずれか一項に記載のロボット制御装置と、
   前記ロボット制御装置によって前記可動部が制御されるロボットと、を備える、
   ロボットシステム。

Images