JP2010247309A - ロボットアーム、及びその制御方法 - Google Patents

Abstract

【課題】制御性の高いロボットアーム、及びその制御方法を提供すること。
【解決手段】本発明にかかるロボットアームは、冗長性を有するロボットアームであって、肘部１６よりも本体部側に設けられた力センサ３１と、力センサ３１からの出力に応じて、ロボットアームに設けられた関節を制御する制御部１３と、を備え、制御部１３が、インピーダンスモデルを用いて、力センサ３１からの出力に応じた肘旋回角を算出し、逆運動学を用いて、肘部１６を肘旋回角に基づいて肘旋回角周りに旋回させるよう、関節を制御する、ものである。
【選択図】図１

Description

本発明は、ロボットアーム、及びその制御方法に関し、特に詳しくは、冗長性を有するロボットアーム、及びその制御方法に関する。

ロボットアームは、通常、リンクと、リンクを結びつける複数の関節とによって構成されている。このようなロボットアームでは、関節を駆動することによって、手先が目標位置、姿勢になるよう制御している。そして、目標位置で、所定の作業を行なう。

ロボットアームには、主に６自由度のものと、７自由度のものがある。手先を任意の位置、姿勢にするには６自由度で十分であるため、７自由度のものは、冗長性（冗長自由度）を有している。このような７自由度のロボットアームの制御方法が開示されている（特許文献１乃至３）。

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

特許文献１では、（アームの自由度−６自由度）を冗長自由度として扱い、手先位置、姿勢を手先位置・姿勢（６自由度分の関節）と、冗長自由度としての関節をそれぞれ分割して制御している。

特許文献２では、冗長ロボットアームをいくつかのサブシステムに分割し、各サブシステムにおいて、インピーダンス制御を実施している。特許文献３には、力センサを設けて、制御を行う方法が開示されている。この方法では、力センサを境として、２つのシステム（ロボット）に分割している。しかしながら、これらの方法では、冗長関節やサブシステムの設定を行うことが困難である。例えば、ロボットアームに実行させるタスクが異なると、別の設定を行う必要が生じてしまう。

なお、上記の制御において、逆運動学は、数値的手法（ヤコビ行列に基づく手法）を用いている。このため、計算量が多くなり、実時間制御が困難になってしまう。さらに、逆運動学の解が初期値に依存してしまう。このように、上記の技術では、制御性を向上することが困難であるという問題点がある。

本発明は、上記の問題点に鑑みてなされたものであり、制御性と安全性の高いロボットアーム、及びその制御方法を提供することを目的とする。

本発明の第１の態様にかかるロボットアームは、冗長性を有するロボットアームであって、肘部よりも本体部側に設けられた第１の力センサと、前記第１の力センサからの出力に応じて、ロボットアームに設けられた関節を制御する制御部と、を備え、前記制御部が、インピーダンスモデルを用いて、前記第１の力センサからの出力に応じた肘旋回角を算出し、逆運動学を用いて、前記肘部を前記肘旋回角に基づいて肘旋回角周りに旋回させるよう、前記関節を制御する、ものである。これにより、外力を逃がすようなインピーダンス制御のパラメータ設計が容易になり、制御性を向上することができる。

本発明の第２の態様にかかるロボットアームは、上記のロボットアームであって、前記制御部が、手先の目標軌道と前記肘旋回角とに基づく前記逆運動学によって前記関節を制御する、ものである。これにより、手先作業を行いながら肘部外力を逃がすような制御を簡便に行うことができる。

本発明の第３の態様にかかるロボットアームは、上記のロボットアームであって、前記制御部が、インピーダンスモデルにおいて、前記第１の力センサによって測定した外力を閾値と比較して、前記外力が閾値を越えていた場合に、前記インピーダンスモデルにおけるバネ項によって生じる反力を一定値にすることを特徴とするものである。これにより、安全性を向上することができる。

本発明の第４の態様にかかるロボットアームは、上記のロボットアームであって、前記肘部よりも先端側に設けられた第２の力センサをさらに備え、インピーダンスモデルを用いて、前記第２の力センサからの出力に応じた手先の位置・姿勢を算出し、逆運動学を用いて、前記手先の位置・姿勢に追従するよう前記関節を制御するものである。これにより、手先作業のコンプライアンスと手先以外に加わった外力へのコンプライアンスを同時に制御することができる。

本発明の第５の態様にかかるロボットアームは、上記のロボットアームであって、インピーダンスモデルにおいて、前記第２の力センサによって測定した外力を閾値と比較して、前記外力が閾値を越えていた場合に、前記インピーダンスモデルにおけるバネ項によって生じる反力を一定値にすることを特徴とするものである。これにより、安全性を向上することができる。

本発明の第６の態様にかかるロボットアームの制御方法は、冗長性を有するロボットアームの制御方法であって、インピーダンスモデルを用いて、肘部よりも本体部側に設けられた第１の力センサからの出力に応じた肘旋回角を算出し、逆運動学を用いて、前記肘部を前記肘旋回角に基づいて肘旋回角周りに旋回させるよう、前記関節を制御する、ものである。これにより、外力を逃がすようなインピーダンス制御のパラメータ設計が容易になり、制御性を向上することができる。

本発明の第７の態様にかかるロボットアームの制御方法は、上記の制御方法であって、手先の目標軌道と前記肘旋回角とに基づく前記逆運動学によって前記関節を制御する、ものである。これにより、手先作業を行いながら肘部外力を逃がすような制御を簡便に行うことができる。

本発明の第８の態様にかかるロボットアームの制御方法は、上記の制御方法であって、インピーダンスモデルにおいて、前記第１の力センサによって測定した外力を閾値と比較して、前記外力が閾値を越えていた場合に、前記インピーダンスモデルにおけるバネ項によって生じる反力を一定値にすることを特徴とするものである。これにより、安全性を向上することができる。

本発明の第９の態様にかかるロボットアームの制御方法は、上記の制御方法であって、インピーダンスモデルを用いて、前記肘部よりも先端側に設けられた第２の力センサからの出力に応じた手先の位置・姿勢を算出し、逆運動学を用いて、前記手先の位置・姿勢に追従するよう前記関節を制御するものである。これにより、手先作業のコンプライアンスと手先以外に加わった外力へのコンプライアンスを同時に制御することができる。

本発明の第１０の態様にかかるロボットアームの制御方法は、上記の制御方法であって、インピーダンスモデルにおいて、前記第２の力センサによって測定した外力を閾値と比較して、前記外力が閾値を越えていた場合に、前記インピーダンスモデルにおけるバネ項によって生じる反力を一定値にすることを特徴とするものである。これにより、安全性を向上することができる。

本発明によれば、制御性と安全性の高いロボットアーム、及びその制御方法を提供することができる。

本発明の実施の形態１にかかるロボットアームの構成を模式的に示す図である。 本発明の実施の形態１にかかるロボットアームの軸構成例を示す図である。 本発明の実施の形態１にかかるロボットアームの動作を説明するための図である。 本発明の実施の形態１にかかるロボットアームの制御系を示すブロック図である。 本発明の実施の形態１にかかるロボットアームの制御方法を示すフローチャートである。 本発明の実施の形態１にかかるロボットアームにおいて、姿勢を決定する方法を説明するための図である。 本発明の実施の形態１にかかるロボットアームにおいて、肘旋回関節周りの外力の算出方法を説明するための図である。 肘旋回関節と手先反力の関係を示すグラフである。 本発明の実施の形態２にかかるロボットアームの構成を模式的に示す図である。 本発明の実施の形態２にかかるロボットアームの軸構成例を示す図である。 本発明の実施の形態２にかかるロボットアームの制御系を示すブロック図である。 本発明の実施の形態２にかかるロボットアームの制御方法を示すフローチャートである。

以下、本発明に係るロボットアームの実施形態を、図面に基づいて詳細に説明する。但し、本発明が以下の実施形態に限定される訳ではない。また、説明を明確にするため、以下の記載及び図面は、適宜、簡略化されている。

発明の実施の形態１．
実施形態１に係るロボットアームについて、図１を用いて説明する。図１は、ロボットアームの構成を模式的に示す図である。ロボットアームは多関節型のマニピュレータである。図１に示すようにロボットアームは、胴体部１０とアーム部１１と制御部１３を有している。さらに、アーム部１１には、ハンド１２、リンク１４、肩部１５、肘部１６、力センサ３１が設けられている。

アーム部１１は、本体部となる胴体部１０に肩部１５を介して取り付けられている。肩部１５の先端側（ハンド側）には、上腕部となるリンク１４が取り付けられている。さらに、そのリンク１４の先端には、肘部１６が取り付けられている。肘部１６の先端側には、前腕部となるリンク１４が設けられている。すなわち、２つのリンク１４が肘部１６を介して連結されている。リンク１４の先端側（ハンド側）には、手首部１７が設けられている。リンク１４には、手首部１７を介してハンド１２が連結されている。エンドエフェクタであるハンド１２によって、例えば、対象物体が把持される。

肩部１５、肘部１６、手首部１７には、少なくとも１つ以上の関節が設けられている。すなわち、肩部１５には、アーム部１１において最も胴体部１０側にある関節が含まれている。手首部１７には、アーム部１１において最も先端側（ハンド１２側）にある関節が含まれている。そして、手首部１７と肩部１５の間にある関節が肘部１６になる。もちろん、肩部１５、肘部１６、手首部１７のそれぞれに、２以上の関節が設けられていてもよい。

さらに、肩部１５には、アーム部１１に加わる力を検出するための力センサ３１が取り付けられている。なお、肩部１５と肘部１６との間に、力センサ３１が設けられていてもよい。すなわち、力センサ３１は、肘部１６よりも胴体部１０側にあればよい。なお、力センサ３１を肘部１６よりも遠ざけて配置することが好ましい。このようにすることでアーム部１１に加わった力を検出することができる。従って、本実施の形態では、力センサ３１を肩部１５に配置している。力センサ３１の測定値は、制御部１３に出力される。

肩部１５、肘部１６、手首部１７には、それぞれ関節が設けられている。ここでは、ロボットアームが７自由度の冗長アーム機構であるとして説明する。従って、ロボットアームは、１自由度分の冗長性を有している。各関節には、関節モータやエンコーダなどが設けられている。さらには、各関節には、関節モータを駆動するためのアンプが設けられている。そして、エンコーダの出力は、制御部１３に入力される。制御部１３は、エンコーダの出力、及び力センサ３１の出力に応じて、各関節を駆動する。各関節を駆動することで、ハンド１２の位置・姿勢を所定の位置・姿勢にすることができる。

さらに、ロボットアームには、アーム部１１の動作を制御するための制御部１３が設けられている。制御部１３は、ＣＰＵ（Central Processing Unit）などを有する演算装置１８と、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、ＨＤＤ、光ディスク、光磁気ディスク等を有する記憶装置１９を有している。例えば、記憶装置１９は、各種プログラムや制御パラメータなどを記憶している。そして、そのプログラムやデータを必要に応じて演算装置１８に供給する。これにより、アーム部１１に設けられた各関節が所定の制御に従って、動作する。よって、ハンド１２を所望の位置・姿勢にすることができる。

次に、ロボットアームの各関節について図２を用いて説明する。図２は、ロボットアームの軸構成例を示す図である。図２に示すように、ロボットアームは、７自由度を有する冗長アームである。肩回転関節２１と肩旋回関節２２と肩回転関節２３が肩部１５を構成する。また、肘回転関節２４と肘回転関節２５が肘部１６を構成する。手首回転関節２７と手首回転関節２６が手首部１７を構成する。もちろん、関節の配置は図２で示した配置に限られるものではない。すなわち、ロボットアームが少なくとも１自由度分の冗長性を有していればよい。各関節には、関節を駆動する関節モータや、その角度を測定するためのエンコーダが設けられている。

本実施の形態では、肩部１５に搭載した力センサ３１でアーム部１１に加わる外力を測定している。力センサ３１の出力に応じて、アーム部１１を制御している。ここでは、力センサ３１からの出力に応じて、アーム部１１に加わった力を逃がしている。このアーム部１１の動作について図３を用いて説明する。

図３に示すように、アーム部１１に外力が加わったとする。力センサ３１はアーム部１１に加わる力を計測する。そして、制御部１３は、インピーダンスモデルにより肘旋回角を修正する。ここでは、外力を冗長自由度周りの肘旋回角によって逃がす。肘旋回角指令値θ_ｄを修正して肘旋回角θ_ｅを求める。そして、肘旋回角θ_ｅに追従するように関節を駆動する。なお、基準面４０は、肘旋回角度０度の時を示しており、肩部１５−手首部１７を含む鉛直平面になっている。すなわち、肩部１５と手首部１７を結ぶ直線に対して垂直な平面が基準面４０となる。肘旋回角が変化したとしても、手首部１７とハンド１２の位置姿勢は変化しない。よって、ハンド１２は、外力による影響を受けないため、手先での所望タスクを実現することができる。例えば、肘部１６を机上につきながらのタスクが可能になる。

本実施形態では、冗長自由度は、肘の旋回角と定義して、幾何学的な手法により逆運動学を解いている。外力を逃がすことのみに注視して、アーム部１１全体で外力を逃がす従来の外力追従制御と異なり、冗長自由度を活用して、手先動作とは独立に外力を逃している。これにより、外力を逃がしつつ、手先での所望の動作を実現することができる。このように、肘部１６が柔軟に動作するように制御することで、アーム部１１に加わる衝撃を低減することができる。

次に、ロボットアームの制御系について図４を用いて説明する。図４は、制御系全体を示すブロック図である。

まず、目標位置姿勢に基づいて手先の目標軌道を決定する。また、目標肘旋回角に基づいて肘旋回角指令値θ_ｄを求める。なお、肘旋回角指令値θ_ｄは、外力が加わらない場合、変化しない。アーム部１１に設けられた力センサ３１でアーム部１１に加わる力を検出する。そして、力センサ３１の出力に基づいて、肘旋回角の接線方向の力Ｆ_ｅを算出する。そして、肘旋回角周りのインピーダンスモデルによって、修正された肘旋回角θ_ｅを算出する。ここでは、肘旋回角周りのインピーダンスモデルで肘旋回角θ_ｅを算出する。肘旋回角指令値θ_ｄと肘旋回周りの外力Ｆ_ｅとを用いて、肘旋回角θ_ｅを算出する。この肘旋回角θ_ｅと、手先目標軌道による手先軌道指令値から、逆運動学によって、各関節角度を算出する。この各関節角度を関節モータのコントローラに出力して、各関節を駆動する。これにより、ロボットアームが動作する。

図５を用いてロボットアームの制御方法について説明する。図５は、ロボットアームの制御方法を示すフローチャートである。

まず、肩部１５の力センサ３１が測定した測定値を制御部１３に出力する（ステップＳ１０１）。これにより、アーム部１１に加えられた力の大きさと方向を検出することができる。そして、測定値が閾値を越えているか否かを制御部１３が判定する（ステップＳ１０２）。測定値が閾値を越えている場合、測定値を肘旋回角の接線方向へと変換する（ステップＳ１０３）。また、上記の処理と平行して、肘旋回角指令値θ_ｄを求めておく（ステップＳ１０４）。そして、インピーダンスモデルを用いて、目標肘旋回角と肘旋回角の接線方向の力Ｆ_ｅから、肘旋回角θ_ｅを算出する（ステップＳ１０５）。また、ステップＳ１０２において、測定値が閾値を越えていない場合、Ｆ_ｅによる修正が行われない。すなわち、肘旋回角指令値θ_ｄがそのまま肘旋回角θ_ｅとなる。

また、上記の処理と並行して、手先の位置・姿勢から、手先軌道指令値を求めておく（ステップＳ１０６）。そして、手先軌道指令値と、肘旋回角θ_ｅを用いて逆運動学によって、各関節の角度を決定する（ステップＳ１０７）。ここでは、幾何学的な手法を用いる。各関節角度に基づいて、ロボットアームが動作する（ステップＳ１０８）。これにより、肘部１６が肘旋回角に基づいて肘旋回角周りに旋回するとともに、手先の位置・姿勢に追従するように関節が駆動する。このように制御することで、手先軌道指令に追従させながら、リンク１４又は肘部１６に作用する外力を逃すことができる。さらに、幾何学的な逆運動学手法を用いるため、実時間制御が可能で、動作が直感的である。このように、制御性と安全性を向上することができ、簡便にロボットアームを制御することができる。

次に、冗長自由度を肘旋回角により定義して、アーム部１１の姿勢を決定する方法について、図６を用いて説明する。１自由度分の冗長性があるため、手先の目標位置・姿勢が決まっても、各関節角度が一意に決まらない。そこで、冗長自由度を肘旋回角として定義することで、アーム部１１の姿勢を一意に決めることができる。ここでは、例えば、「関節の可動範囲を考慮に入れた７自由度冗長マニピュレータ解析的逆運動学解法」、清水昌幸、角谷啓、尹祐根、北垣高成、小菅一弘、ロボット学会誌、Ｖｏｌ．２５、Ｎｏ．４、６０６−６１７に記載されている方法を用いることができる。

例えば、図４の逆運動学及び図５のステップＳ１０７において、上記の手法を用いる。すると、図６に示すように、肘部１６が基準面上で旋回する。このとき、ハンド１２の位置が変化しない。よって、動作が直感的になり、制御性を向上することができる。

このように、逆運動学手法を用いた場合、冗長自由度は、肘の旋回角として表現することができる。そのため、ステップＳ１０３で示したように、肩部１５の力センサ３１の情報を肘部１６の旋回角周りの接線方向へと変化する。すなわち、力センサ３１の測定値から、肘部１６の旋回角周りの接線方向における力を算出する。この算出方法について、図７を用いて説明する。

肩部１５から肘部１６への単位ベクトル^ｓｈｕ_ｅを算出する。また、肩部から手首部１７への単位ベクトル^ｓｈｕ_ｗｒを算出する。肩部１５、肘部１６、手首部１７により生成される平面に対する法線ベクトルｕ_ｎｏｒ（肘の旋回角周りの接線方向と一致）を算出する。法線ベクトルｕ_ｎｏｒは単位ベクトル^ｓｈｕ_ｅと単位ベクトル^ｓｈｕ_ｗｒの外積により算出される。すなわち、ｕ_ｎｏｒ＝^ｓｈｕ_ｅ×^ｓｈｕ_ｗｒとなる。そして、力センサ３１で測定した力Ｆ_ｓｈを肘の旋回角周りの接線方向の情報に変換する。すなわち、肘の旋回角周りの接線方向に加わった力Ｆ_ｅを求める。この力Ｆ_ｅは、Ｆ_ｓｈとｕ_ｎｏｒの内積により算出される。すなわち、Ｆ_ｅ＝Ｆ_ｓｈ・ｕ_ｎｏｒとなる。このように、力センサ３１で検出した力から、肘の旋回角周りの接線方向に加わった力Ｆ_ｅを算出することができる。

次に、肘部１６の旋回角周りのインピーダンス制御の一例について説明する。まず、肘旋回角周りに下記を用いてインピーダンス制御を設定する。このインピーダンス制御については、「ファジー補償器を用いた位置指令型インピーダンス制御法」、永田寅臣、渡辺圭吾、泉清高、第１回日本ファジィ学会九州支部学術講演会、ｐｐ２９−３３、１９９９に記載されている方法を用いることができる。

この例では、以下の式１に示すように、スイッチング行列を用いている点に特徴の一つがある。

なお、上記の式１において、θ_ｅは肘旋回角、θ_ｄは肘旋回角指令値、Ｍは実慣性行列、Ｂは仮想ダンパー行列、Ｋは仮想ばね行列、Ｆは肘旋回周りの力応答値、Ｆ_ｄは肘旋回周りの力指令値、Ｓはスイッチング行列、Ｋ_ｆは力ゲイン、Ｅは単位行列である。ここで、Ｓ、Ｍ、Ｂ、Ｋは６×６の行列である。

以下の式２に示すように、スイッチング行列を用いることにより制御系の切換を容易にすることができる。

このように、スイッチング行列Ｓによって制御を切換える。スイッチング行列を単位行列又はゼロ行列に切換える。この切換の特徴を活かし、以下の式３に示すように、バネ項によって生じる手先反力に閾値θ_ｔｈを設ける。

このインピーダンス制御では、θ_ｅとθ_ｄの偏差（絶対値）と閾値θ_ｔｈを比較して、その比較結果に応じて制御を切換えている。ここでは、図８に示すように、偏差が閾値θ_ｔｈよりも小さい場合、バネ項によって生じる手先反力が偏差に比例している。一方、偏差が閾値よりも大きい場合、バネ項によって生じる手先反力を力指令値Ｆ_ｄで一定にしている。偏差が閾値θ_ｔｈを越えた場合、バネ項による手先反力は偏差によらず、一定値になる。ここでは、式３に示すように、偏差が閾値θ_ｔｈを越えた場合、力指令値Ｆ_ｄ＝Ｋθ_ｔｈとなる。なお、図８において、横軸がθ_ｅとθ_ｄの偏差（絶対値）を示し、縦軸が手先反力を示している。

これにより、過大な反力が生じる事を防ぐことができ、安全性を向上することが可能になる。すなわち、アーム部１１に過大な力が加わり、偏差が閾値を越えたとしても、バネ項によって生じる反力が一定値となる。よって、過大な反力が生じるのを防ぐことができる。

本実施の形態では、力センサ３１からの出力を用いるため、外力による衝撃を吸収することができる。すなわち、アーム部１１に力が加わった場合、インピーダンスモデルによって肘旋回角を制御する。これにより、アーム部１１が柔軟に動作することになる。

冗長自由度の活用方法として、人間の腕と同様な肘部の旋回を用いているため、動作を直感的に認識することができる。従来の冗長なロボットアームのインピーダンス制御と異なり、動作が直感的であるため、肘旋回方向のインピーダンスパラメータの設計を容易に行うことができる。このように、パラメータを容易に設計することができる。すなわち、行列やゲインの値の設定が容易になる。

発明の実施の形態２．
本実施の形態にかかるロボットアームについて、図９を用いて説明する。図９は、ロボットアームの全体構成を示す図である。本実施の形態では、実施の形態１で示した構成に加えて、力センサ３２が設けられている。そして、２つの力センサ３１、３２の測定値を用いて、それぞれインピーダンス制御を行っている。なお、力センサ３２以外の構成及び基本的な制御方法については、実施の形態１と同様であるため、説明を省略する。

図９に示すように、力センサ３２が手首部１７に設けられている。なお、手首部１７に限られるものではない。力センサ３２の位置は、肘部１６よりも先端側であればよい。なお、力センサ３２を肘部１６よりも遠ざけて配置することが好ましい。従って、本実施の形態では、力センサ３２を手首部１７に配置している。力センサ３２の出力は、制御部１３に出力される。また、力センサ３２は、図１０に示すように、手首回転関節２７とハンド１２の間に配置されている。このように、本実施の形態では、アーム部１１に２つの力センサ３１、３２が設けられている。

本実施の形態では、力センサ３２で手先インピーダンス制御を行っている。また、実施の形態１と同様に、力センサ３１で肘旋回周りのインピーダンス制御を行っている。この制御について図１１を用いて説明する。図１１は、制御系を示すブロック図である。なお、実施の形態１において説明した内容と重複する部分については説明を省略する。

本実施の形態では、手先目標軌道から手先位置・姿勢の指令値ｘ_ｄ求める。さらに、力センサ３２で手首部の外力Ｆ_ｗを測定する。手先インピーダンスモデルによって、手先位置姿勢の指令値ｘ_ｄと外力Ｆ_ｗから手先位置・姿勢ｘ_ｗを算出する。さらに、本実施の形態では、力センサ３１、３２によって肘旋回角周りの外力Ｆ_ｅを求めている。また、実施の形態１と同様に、肘旋回角周りのインピーダンスモデルによって、肘旋回角θ_ｅを算出する。ここでは、肘旋回角周りの外力Ｆ_ｅと目標肘旋回角に応じた肘旋回角指令値θ_ｄによって、肘旋回角θ_ｅを求めることができる。そして、手先位置・姿勢ｘ_ｗと、肘旋回角θ_ｅとを用いた逆運動学によって、各関節角度を求めることができる。

次に、本実施の形態にかかるロボットアームの制御方法について、図１２を用いて説明する。図１２は、本実施形態にかかるロボットアームの制御方法を示すフローチャートである。

まず、肩部１５の力センサ３１で測定を行う（ステップＳ２０１）。さらに、ステップＳ２０１と並行して、手首部１７に設けられた力センサ３２で測定を行う（ステップＳ２０２）。これにより、アーム部１１の先端側に加わった力が測定される。そして、力センサ３２の測定値を座標変換する（ステップＳ２０３）。これにより、力センサ３２での測定結果が、力センサ３１の座標系に変換される。そして、力センサ３１と力センサ３２との測定結果を減算する。すなわち、力センサ３１での測定値から座標変換された力センサ３２での測定値を引く。これにより、力センサ３２よりも胴体部１０側であって、力センサ３１よりも先端側に加わった力が算出される。すなわち、アーム部１１において、力センサ３１から力センサ３２までに加わった力を求めることができる。そして、減算された測定値と閾値を比較する（ステップＳ２０４）。この後のステップＳ２０４〜ステップＳ２０７は、実施の形態と同様であるため説明を省略する。

一方、ステップＳ２０２で測定された測定値は閾値と比較される（ステップＳ２０８）。すなわち、手首部１７に加わる力が閾値を越えているか否かを判定する。測定値が閾値を越える場合、インピーダンスモデルを用いる。なお、上記の処理と並行して手先軌道による手先位置・姿勢の指令値ｘ_ｄを算出する（ステップＳ２０９）。そして、手先位置・姿勢の指令値ｘ_ｄと手首部１７に加わる外力Ｆ_ｗとを用いたインピーダンスモデルによって、手先位置・姿勢ｘ_ｗを算出する（ステップＳ２１０）。ここで、手先のインピーダンスモデルを用いることで、手先の位置・姿勢の指令値ｘ_ｄを修正することができる。また、ステップＳ２０８において、測定値が閾値を越えていない場合、Ｆ_ｗによる修正が行われない。すなわち、手先の位置・姿勢の指令値ｘ_ｄがそのまま手先位置・姿勢ｘ_ｗとなる。

そして、手先位置・姿勢ｘ_ｗと、肘旋回角θ_ｅとを用いた逆運動学によって、各関節角度を求める（ステップＳ２１１）。この処理は、実施の形態１と同様である。各関節角度に基づいて、ロボットアームが動作する（ステップＳ２１２）。これにより、手先位置・姿勢ｘ_ｗ及び肘旋回角θ_ｅに追従するよう関節角度が制御される。肘部１６が肘旋回角に基づいて肘旋回角周りに旋回するとともに、手先位置、姿勢ｘ_ｗに追従するように関節が駆動する。このように制御することで、外力を逃すことができる。さらに、幾何学的な逆運動学手法を用いるため、実時間制御が可能で、動作が直感的である。このように、簡便にロボットアームを制御することができる。また、アーム全体にコンプライアンスを実装することができる。外力に倣うことにってロボットアームの安全性を向上することができる。インピーダンス制御のバネ項によって、外力消失後の作業への復帰が可能になる。

以下に、手先のインピーダンス制御について説明する。手先のインピーダンス制御についても肘部１６に用いたインピーダンス制御と同様に、スイッチング行列を用いる。すなわち、手先のインピーダンス制御において、バネ項に閾値を設けて、制御を切換える。従って、手先のインピーダンス制御は以下の式で示される。

なお、ｘ_ｗは手先位置・姿勢、ｘ_ｄは手先位置・姿勢の指令値、Ｍは実慣性行列、Ｂは仮想ダンパー行列、Ｋは仮想バネ行列、Ｓはスイッチング行列、Ｋ_ｆは力ゲイン、Ｅは単位行列、Ｆ_ｗは力応答値、Ｆ_ｄは力指令値、Ｆ_ｔｈは力閾値である。

そして、肘旋回周りのインピーダンス制御と同様に、閾値Ｆ_ｔｈを越えた場合、Ｆ_ｄを一定にする。式４に示すように、閾値Ｆ_ｔｈを越えた場合、Ｆ_ｄ＝Ｆ_ｔｈで一定になる。

このように、肘旋回角と手先において、それぞれバネ項に閾値を設けたインピーダンス制御を行っている。よって、ある一定の力以上は返さないようになり、安全性を向上することができる。このように、安全性を考慮したインピーダンス制御を行うことが可能になる。この結果、アーム全体でコンプライアンスを実現することができ、外力に倣う安全性を考慮した動作を実現することが可能になる。また、力指令値が存在していることで、外力消失後には、手先での作業へ復帰することができる。このように、制御性を向上することができ、所望の制御を簡便に実施することができる。また、ハンド１２において実行するタスクが変わった場合でも、容易に対応することができる。これにより、制御性を向上することができる。

１０ 胴体部
１１ アーム部
１２ ハンド
１３ 制御部
１４ リンク
１５ 肩部
１６ 肘部
１７ 手首部
２１ 肩回転関節
２２ 肩旋回関節
２３ 肩回転関節
２４ 肘回転関節
２５ 肘回転関節
２６ 手首回転関節
２７ 手首回転関節
３１ センサ
３２ センサ
４０ 基準面

Claims (10)

1. 冗長性を有するロボットアームであって、
   肘部よりも本体部側に設けられた第１の力センサと、
   前記第１の力センサからの出力に応じて、ロボットアームに設けられた関節を制御する制御部と、を備え、
   前記制御部が、
   インピーダンスモデルを用いて、前記第１の力センサからの出力に応じた肘旋回角を算出し、
   逆運動学を用いて、前記肘部を前記肘旋回角に基づいて肘旋回角周りに旋回させるよう、前記関節を制御する、ロボットアーム。
2. 前記制御部が、手先の目標軌道と前記肘旋回角とに基づく前記逆運動学によって前記関節を制御する、ロボットアーム。
3. 前記制御部が、インピーダンスモデルにおいて、前記第１の力センサによって測定した外力を閾値と比較して、前記外力が閾値を越えていた場合に、前記インピーダンスモデルにおけるバネ項によって生じる反力を一定値にすることを特徴とする請求項１、又は２に記載のロボットアーム。
4. 前記肘部よりも先端側に設けられた第２の力センサをさらに備え、
   インピーダンスモデルを用いて、前記第２の力センサからの出力に応じた手先の位置・姿勢を算出し、
   逆運動学を用いて、前記手先の位置・姿勢に追従するよう前記関節を制御する請求項１乃至３のいずれか１項に記載のロボットアーム。
5. インピーダンスモデルにおいて、前記第２の力センサによって測定した外力を閾値と比較して、前記外力が閾値を越えていた場合に、前記インピーダンスモデルにおけるバネ項によって生じる反力を一定値にすることを特徴とする請求項４に記載のロボットアーム。
6. 冗長性を有するロボットアームの制御方法であって、
   インピーダンスモデルを用いて、肘部よりも本体部側に設けられた第１の力センサからの出力に応じた肘旋回角を算出し、
   逆運動学を用いて、前記肘部を前記肘旋回角に基づいて肘旋回角周りに旋回させるよう、前記関節を制御する、ロボットアームの制御方法。
7. 手先の目標軌道と前記肘旋回角とに基づく前記逆運動学によって前記関節を制御する、請求項６に記載のロボットアームの制御方法。
8. インピーダンスモデルにおいて、前記第１の力センサによって測定した外力を閾値と比較して、前記外力が閾値を越えていた場合に、前記インピーダンスモデルにおけるバネ項によって生じる反力を一定値にすることを特徴とする請求項６、又は７に記載のロボットアームの制御方法。
9. インピーダンスモデルを用いて、前記肘部よりも先端側に設けられた第２の力センサからの出力に応じた手先の位置・姿勢を算出し、
   逆運動学を用いて、前記手先の位置・姿勢に追従するよう前記関節を制御する請求６乃至８のいずれか１項に記載のロボットアームの制御方法。
10. インピーダンスモデルにおいて、前記第２の力センサによって測定した外力を閾値と比較して、前記外力が閾値を越えていた場合に、前記インピーダンスモデルにおけるバネ項によって生じる反力を一定値にすることを特徴とする請求項９に記載のロボットアームの制御方法。

Images