JP2011093011A - ロボットアーム制御装置、その制御方法及びプログラム - Google Patents

Abstract

【課題】安全性を向上させたロボットアーム制御装置、その制御方法及びプログラムを提供すること。
【解決手段】ロボットアーム制御装置１は、複数の関節部を有するロボットアーム２と、ロボットアーム２に対して作用する外力を検出する外力検出手段と、外力検出手段により検出されたロボットアーム２の外力に基づいて、ロボットアーム２のインピーダンス制御を行う際の位置補正量を算出する位置補正量算出手段と、ロボットアーム２の移動目標位置を指定するための手先位置指令値を生成する指令値生成手段と、位置補正量算出手段により算出される位置補正量と、指令値生成手段により生成された手先位置指令値と、に基づいて、ロボットアーム２の各関節部の駆動を制御する駆動制御手段と、を備えている。指令値生成手段は、位置補正量算出手段により算出される位置補正量が所定量以下になるまで、手先位置指令値の生成を停止する。
【選択図】図１

Description

本発明は、複数の関節部を有するロボットアームのインピーダンス制御を行うロボットアーム制御装置、その制御方法及びプログラムに関するものである。

近年、人間等と協調する家庭用ロボット、介護ロボット、ペットロボット等の開発が行われている。そのようなロボットにおいては、例えば、そのロボットアームが人間等に接触した場合などに、人間等に柔軟に接触するように、ロボットアームに対する外力に応じてロボットアームの位置補正を行うインピーダンス制御が行われている（特許文献１及び２参照）。

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

しかしながら、上記ロボットにおいては、例えば、ロボットアームが外力により変位した後、その外力から解放されると、一定のインピーダンスで目標の手先位置へ移動するため、この移動過程で不測の障害物等に接触し安全性が低下する虞がある。

本発明は、このような問題点を解決するためになされたものであり、安全性を向上させたロボットアーム制御装置、その制御方法及びプログラムを提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、複数の関節部を有するロボットアームと、前記ロボットアームに対して作用する外力を検出する外力検出手段と、前記外力検出手段により検出された前記ロボットアームの外力に基づいて、該ロボットアームのインピーダンス制御を行う際の位置補正量を算出する位置補正量算出手段と、前記ロボットアームの移動目標位置を指定するための手先位置指令値を生成する指令値生成手段と、前記位置補正量算出手段により算出される前記位置補正量と、前記指令値生成手段により生成された前記手先位置指令値と、に基づいて、前記ロボットアームの各関節部の駆動を制御する駆動制御手段と、を備えるロボットアーム制御装置であって、前記指令値生成手段は、前記位置補正量算出手段により算出される前記位置補正量が所定量以下になるまで、前記手先位置指令値の生成を停止する、ことを特徴とするロボットアーム制御装置である。

この一態様において、前記駆動制御手段は、前記位置補正量算出手段により算出される前記位置補正量が所定量以下になるまで、前記ロボットアームが外力により移動したときの軌道と、元の位置へ復元するときの軌道とが同一となるように、前記ロボットアームの各関節部の駆動を制御してもよい。

また、この一態様において、前記駆動制御手段は、前記位置補正量算出手段により算出される前記位置補正量が所定量以下になるまで、前記ロボットアームの移動速度が減少するように、前記ロボットアームの各関節部の駆動を制御してもよい。

さらに、この一態様において、前記指令値生成手段により生成された前記手先位置指令値と、前記位置補正量算出手段により算出された前記位置補正量と、に基づいて、前記ロボットアームの目標位置を算出し、該算出した目標位置となる前記各関節部の回転角度を夫々算出するアーム関節角度算出手段を更に備え、前記駆動制御手段は、前記アーム関節角度算出手段により算出された前記各関節の回転角度に基づいて、前記ロボットアームの各関節部の駆動を制御してもよい。

さらにまた、この一態様において、前記ロボットアームの各関節部の回転角度を検出する回転検出手段と、前記ロボットアームの各関節部を駆動する駆動手段と、を更に備え、 前記駆動制御手段は、前記回転検出手段により検出された各関節部の回転角度と、前記アーム関節角度算出手段により算出された前記各関節部の回転角度と、の偏差を夫々算出し、該算出した各偏差に所定のゲインを乗算して、前記駆動手段に対する制御信号を生成してもよい。

なお、この一態様において、前記指令値生成手段は、前記位置補正量算出手段により算出される前記位置補正量が所定量以下となり、前記手先位置指令値の生成を停止するWaitモードと、前記位置補正量算出手段により算出される前記位置補正量が所定量より大きくなり、前記手先位置指令値の生成を行うRunモードと、を有していてもよい。

他方、上記目的を達成するための本発明の一態様は、複数の関節部を有するロボットアームに作用する外力を検出する工程と、前記検出されたロボットアームの外力に基づいて、該ロボットアームのインピーダンス制御を行う際の位置補正量を算出する工程と、前記ロボットアームの移動目標位置を指定するための手先位置指令値を生成する工程と、前記算出される位置補正量と、前記生成された手先位置指令値と、に基づいて、前記ロボットアームの各関節部の駆動を制御する工程と、を含むロボットアーム制御装置の制御方法であって、前記算出される位置補正量が所定量以下になるまで、前記手先位置指令値の生成を停止する工程を含む、ことを特徴とするロボットアーム制御装置の制御方法であってもよい。

さらに、上記目的を達成するための本発明の一態様は、複数の関節部を有するロボットアームに作用する外力に基づいて、該ロボットアームのインピーダンス制御を行う際の位置補正量を算出する処理と、前記ロボットアームの移動目標位置を指定するための手先位置指令値を生成する処理と、前記算出される位置補正量と、前記生成された手先位置指令値と、に基づいて、前記ロボットアームの各関節部の駆動を制御する処理と、前記算出される位置補正量が所定量以下になるまで、前記手先位置指令値の生成を停止する処理と、をコンピュータに実行させることを特徴とするロボットアーム制御装置のプログラムであってもよい。

本発明によれば、安全性を向上させたロボットアーム制御装置、その制御方法及びプログラムを提供することができる。

本発明の実施形態１に係るロボットアーム制御装置の概略的なシステム構成を示すブロック図である。 ロボット本体に取り付けられたロボットアームの一動作例を示す上面図である。 本発明の実施形態１に係る制御装置の概略的なシステム構成の一例を示すブロック図である。 本発明の実施形態１に係るロボットアーム制御装置の制御処理フローの一例を示すフローチャートである。 ロボット本体に取り付けられたロボットアームの一動作例を示す上面図である。 本発明の実施形態２に係るロボットアーム制御装置の制御処理フローの一例を示すフローチャートである。

本発明の実施形態１．
以下、図面を参照して本発明の実施の形態について説明する。図１は、本発明の実施形態１に係るロボットアーム制御装置の概略的なシステム構成を示すブロック図である。本実施形態１に係るロボットアーム制御装置１は、複数の関節部１１，１２，１３を有するロボットアーム２と、各関節部１１，１２，１３を夫々駆動する複数のアクチュエータ３１，３２，３３と、各関節部１１，１２，１３の回転角度を夫々検出する複数の回転センサ４１，４２，４３と、ロボットアーム２に対して作用する外力を検出する力センサ３と、各アクチュエータ３１，３２，３３の駆動を制御する制御装置５と、を備えている。

ロボットアーム２は、例えば、家庭用ロボット、介護用ロボット、ペットロボット、作業用ロボット、運搬用ロボット等として構成されるロボット本体１００に回動可能に連結されている（図２）。

また、ロボットアーム２は、例えば、ロボット本体１００の胴体に肩関節部１１を介して回動可能に連結された上アーム部２１と、上アーム部２１に肘関節部１２を介して回動可能に連結された下アーム部２２と、下アーム部２２に手首関節部１３を介して回動可能に連結され、物などを把持可能な把持部２３と、を有している。なお、ロボットアーム２は、上記構成に限定されず、例えば、任意の数の関節部で構成されてもよく、任意のアーム構成が適用可能である。

各アクチュエータ３１，３２，３３は、駆動手段の一具体例であり、肩関節部１１、肘関節部１２、及び手首関節部１３に夫々設けられている。また、各アクチュエータ３１，３２，３３は、肩関節部１１、肘関節部１２、及び手首関節部１３を夫々回転駆動して、ロボットアーム２の先端部である把持部２３を任意の位置へ移動させる。

さらに、アクチュエータ３１，３２，３３としては、例えば、サーボモータ等が用いられており、減速機構等を介して各関節部１１，１２，１３を駆動する。なお、アクチュエータ３１，３２，３３は、ロボットアーム２の各関節部１１，１２，１３に３つ設けられているが、これに限らず、設けられるアクチュエータの数及び位置は任意でよい。各アクチュエータ３１，３２，３３は、駆動回路６及びＤ／Ａ変換回路７を介して制御装置５に接続されており、制御装置５からの制御信号に応じて駆動する。

各回転センサ４１，４２，４３は、回転検出手段の一具体例であり、例えば、肩関節部１１、肘関節部１２、及び手首関節部１３に夫々設けられており、肩関節部１１、肘関節部１２、及び手首関節部１３の回転角度や回転角速度を検出する。回転センサ４１，４２，４３は、肩関節部１１、肘関節部１２、及び手首関節部１３の回転角度や回転角速度を、制御装置５に対して出力する。

また、回転センサ４１，４２，４３としては、例えば、ロータリーエンコーダ等が用いられており、肩関節部１１、肘関節部１２、及び手首関節部１３の回転角度や回転角速度に応じた回転信号を出力する。なお、各回転センサ４１，４２，４３は、ロボットアーム２の各関節部１１，１２，１３に３つ設けられているが、これに限らず、設けられる回転センサの数及び位置は任意でよい。

各回転センサ４１，４２，４３は、Ａ／Ｄ変換回路８を介して制御装置５に接続されており、アナログ信号からデジタル信号へ変換された各関節部１１，１２，１３の回転信号が制御装置５に入力される。

力センサ３は、外力検出手段の一具体例であり、例えば、ロボットアーム２の把持部２３に取り付けられており、この把持部２３に対して作用する外力を検出する。力センサ３は、検出した把持部２３に対する外力を制御装置５に対して出力する。また、力センサ３としては、例えば、ひずみゲージ、圧電式センサ、音叉式力センサ等を用いることができ、これらセンサは外力に応じた電圧信号を出力する。

力センサ３は、Ａ／Ｄ変換回路９を介して制御装置５に接続されており、アナログ信号からデジタル信号へ変換された電圧信号が制御装置５に入力される。なお、ロボットアーム２の外力は、力センサ３を用いることなく、モータ等のアクチュエータ３１，３２，３３の駆動電流を用いて算出されてもよい。また、力センサ３は、ロボットアーム２の把持部２３に取り付けられているが、これに限らず、例えば、上アーム部２１又は下アーム部２２に取り付けられていてもよく、ロボットアーム２の任意の位置に取り付けることができる。

制御装置５は、ロボットアーム２の把持部２３に対して作用する外力に応じて、ロボットアーム２を柔軟に動作させるインピーダンス制御を行う。制御装置５は、例えば、力センサ３により検出されたロボットアーム２の外力をもとに算出された仮想の慣性、粘性および弾性パラメータを用いて、機械的インピーダンスモデルに基づいて、ロボットアーム２のインピーダンス制御を行う。これにより、ロボットアーム２の把持部２３が人間等の障害物に接触したときに、柔軟に接触するため、安全性が向上する。

なお、制御装置５は、例えば、制御処理、演算処理等を行うＣＰＵ（Central Processing Unit）５ａ、ＣＰＵ５ａによって実行される制御プログラム、演算プログラム等が記憶されたＲＯＭ（Read Only Memory）５ｂ、処理データ等を記憶するＲＡＭ（Random Access Memory）５ｃ等からなるマイクロコンピュータを中心にして、ハードウェア構成されている。

図３は、本実施形態１に係る制御装置の概略的なシステム構成の一例を示すブロック図である。本実施形態１に係る制御装置５は、指令値生成部５１と、位置補正量算出部５２と、アーム関節角度算出部５３と、駆動制御部５４と、を有している。

指令値生成部５１は、指令値生成手段の一具体例であり、例えば、ユーザの入力操作又は予め設定されたプログラムに基づいて、ロボットアーム２の把持部２３の移動目標位置を指定するための手先位置指令値ｄを、所定周期で生成する。指令値生成部５１は、生成した手先位置指令値ｄを、アーム関節角度算出部５３に対して出力する。

位置補正量算出部５２は、位置補正量算出手段の一具体例であり、例えば、力センサ３から出力されたロボットアーム２の外力に基づいて、インピーダンス制御を行う際のロボットアーム２の位置補正量Δｄを算出する。そして、位置補正量算出部５２は、算出した位置補正量Δｄをアーム関節角度算出部５３及び指令値生成部５１に対して出力する。

位置補正量算出部５２は、例えば、下記（１）乃至（３）を用いて、ロボットアーム２の位置補正量Δｄ（Δｄ＜０）を算出する。なお、下記（１）乃至（３）式において、力センサ３により検出されたロボットアーム２の外力をIFとし、その外力IFによる把持部２３の変位量をXとする。

アーム関節角度算出部５３は、アーム関節角度算出手段の一具体例であり、例えば、指令値生成部５１により生成された手先位置指令値ｄに、位置補正量算出部５２により算出された位置補正量Δｄを加算して、ロボットアーム２の把持部２３の目標位置Ｄ（Ｄ＝ｄ＋Δｄ）を算出する。そして、アーム関節角度算出部５３は、ロボットアーム２の把持部２３が目標位置Ｄへ移動するような、各関節部１１，１２，１３の回転角度を夫々算出する。

このように、ロボットアーム２の外力に応じた位置補正量Δｄが仮目標位置である手先位置指令値ｄに加算されるため、目標位置Ｄが外力を小さくする方向へ移動し、ロボットアーム２はその外力に追従して柔軟性を現す。アーム関節角度算出部５３は、算出した各関節部１１，１２，１３の回転角度を示す回転角度指令値を、駆動制御部５４に対して出力する。

駆動制御部５４は、駆動制御手段の一具体例であり、例えば、各回転センサ４１，４２，４３により検出された各関節部１１，１２，１３の回転角度と、アーム関節角度算出部５３により算出された各関節部１１，１２，１３の回転角度指令値と、の偏差を夫々算出する。そして、駆動制御部５４は、算出した各偏差に所定のゲインＫを乗算して各アクチュエータ３１，３２，３３の制御信号を生成する。

駆動制御部５４は、生成した制御信号を、Ｄ／Ａ変換回路７及び駆動回路６を介して各関節部１１，１２，１３の各アクチュエータ３１，３２，３３に対して出力する。なお、駆動制御部５４は、上述のようにフィードバック制御を行っているが、これに限らず、例えば、状態フィードバック制御、ロバスト制御等の周知の制御を行ってもよい。

ところで、ロボットアームは、ある目標軌道に従って移動する過程で、外力が付加されると、その外力に応じて、目標軌道上の位置Ｓ１から外れた位置Ｓ２に移動する。その後、ロボットアームは、その外力から解放されると、位置Ｓ２から、一定のインピーダンスで目標の手先位置（以下、目標手先位置と称す）Ｓ３へ移動しようとする（図２）。このとき、従来のロボットアーム制御装置において、ロボットアームが位置Ｓ２から目標手先位置Ｓ３へ移動する過程（ａ）で不測の障害物等に接触する虞がある。

そこで、本実施形態１に係るロボットアーム制御装置１において、制御装置５の指令値生成部５１は、位置補正量算出部５２により算出される位置補正量の絶対値｜Δｄ｜が所定量ε以下になるまで、手先位置指令値ｄの生成を停止する。これにより、位置補正量の絶対値｜Δｄ｜が所定量ε以下になるまで、駆動制御部５４は、位置補正量算出部５２により算出された位置補正量Δｄに基づいて、ロボットアーム２のインピーダンス制御のみを行う。

この場合、図２に示すように、ロボットアーム２が、外力に応じて目標軌道上の位置Ｓ１から外れ位置Ｓ２に移動し、その後、その外力から解放されると、再度、位置Ｓ１へ復元することとなる。このため、ロボットアーム２がその復元の過程（ｂ）で障害物等に接触する可能性を大幅に低減することができる。

例えば、指令値生成部５１は、位置補正量算出部５２により算出される位置補正量の絶対値｜Δｄ｜が所定量εよりも大きいとき（｜Δｄ｜＞ε）、Waitモードになり、手先位置指令値ｄの生成を停止する。

ここで、指令値生成部５１がWaitモードで手先位置指令値ｄの生成を停止している場合、手先位置指令値ｄは０となり、目標位置Ｄ＝位置補正量Δｄとなる。したがって、指令値生成部５１がWaitモードとなる場合、駆動制御部５４は、位置補正量算出部５２により算出される位置補正量Δｄのみに基づいて、各アクチュエータ３１，３２，３３のインピーダンス制御のみを行う。

一方、指令値生成部５１は、位置補正量算出部５２により算出される位置補正量の絶対値｜Δｄ｜が所定量ε以下になると（｜Δｄ｜≦ε）、WaitモードからRunモードに切替る。指令値生成部５１がRunモードとなる場合、駆動制御部５４は、位置補正量算出部５２により算出される位置補正量Δｄと、指令値生成部５１により生成された手先位置指令値ｄと、に基づいて、各アクチュエータ３１，３２，３３の駆動を制御する。

なお、位置補正量Δｄの所定量εは、例えば、ロボットアーム２を動作させる周辺の環境状態（柱、壁などの障害物の配置、大きさ、障害物との距離等）に応じて、予め最適な値が設定され、ＲＡＭ５ｃ等に記憶されている。具体的には、ロボットアーム２が動作する周辺領域に複数の柱が設置されている場合は、その柱の間隔の１／２もしくは１／３の値をεに設定してもよく、また、その柱の幅の１／２もしくは１／３の値をεに設定してもよい。

上述のように、ロボットアーム２の復元量である位置補正量Δｄが所定量ε以下の小さな値に減少するまで、手先位置指令値ｄの生成を停止し、ロボットアーム２のインピーダンス制御のみを行う。これにより、ロボットアーム２を元の位置へ復元するだけの必要な移動に抑えることで、その移動過程での不測の障害物への衝突や実施不可能な（解の無い）軌道での移動などを抑制することができ、より安全性を向上させることができる。

図４は、本実施形態１に係るロボットアーム制御装置の制御処理フローの一例を示すフローチャートである。なお、図４及び後述の図５に示す制御処理は、所定周期毎に繰り返し実行される。

指令値生成部５１は、Runモードにおいて、ロボットアーム２の手先位置指令値ｄを所定周期で生成し（ステップＳ１０１）、生成した手先位置指令値ｄをアーム関節角度算出部５３に対して出力する。

また、位置補正量算出部５２は、力センサ３から出力されたロボットアーム２の外力に基づいて、インピーダンス制御を行う際のロボットアーム２の位置補正量Δｄを上記（１）乃至（３）式を用いて算出し（ステップＳ１０２）、アーム関節角度算出部５３及び指令値生成部５１に対して出力する。

次に、指令値生成部５１は、位置補正量算出部５２により算出される位置補正量の絶対値｜Δｄ｜が所定量ε以下であるか否かを判断する（ステップＳ１０３）。

指令値生成部５１は、位置補正量算出部５２により算出される位置補正量の絶対値｜Δｄ｜が所定量ε以下であると判断したとき（ステップＳ１０３のＹＥＳ）、Runモードを維持し（ステップＳ１０４）、上述の如く、手先位置指令値ｄの生成を維持する（ステップＳ１０５）。

一方、指令値生成部５１は、位置補正量算出部５２により算出される位置補正量の絶対値｜Δｄ｜が所定量ε以下でないと判断したとき（ステップＳ１０３のＮＯ）、RunモードからWaitモードに切替る（ステップＳ１０６）。指令値生成部５１は、このWaitモードにおいて、上述の如く、手先位置指令値ｄの生成を停止する（ステップＳ１０７）。

その後、アーム関節角度算出部５３は、指令値生成部５１により生成された手先位置指令値ｄに、位置補正量算出部５２により算出された位置補正量Δｄを加算して、ロボットアーム２の把持部２３の目標位置Ｄを算出し、この算出した目標位置Ｄとなる各関節部１１，１２，１３の回転角度指令値を夫々算出し（ステップＳ１０８）、駆動制御部５４に対して出力する。

さらに、駆動制御部５４は、各回転センサ４１，４２，４３により検出された各関節部１１，１２，１３の回転角度と、アーム関節角度算出部５３により算出された各関節部１１，１２，１３の回転角度指令値と、の偏差を夫々算出し、算出した各偏差に所定のゲインＫを乗算して制御信号を生成する。そして、駆動制御部５４は、生成した制御信号を、Ｄ／Ａ変換回路７及び駆動回路６を介して各関節部１１，１２，１３の各アクチュエータ３１，３２，３３に対して出力することで、各アクチュエータ３１，３２，３３を駆動制御する（ステップＳ１０９）。

以上、本実施形態１に係るロボットアーム制御装置１において、制御装置５の指令値生成部５１は、位置補正量算出部５２により算出される位置補正量の絶対値｜Δｄ｜が所定量ε以下になるまで、手先位置指令値ｄの生成を停止する。このように、ロボットアーム２の復元量である位置補正量Δｄが所定量ε以下の小さな値に減少するまで、手先位置指令値ｄの生成を停止し、ロボットアーム２のインピーダンス制御のみを行う。これにより、ロボットアーム２を元の位置へ復元するだけの必要な移動に抑えることで、その移動過程での不測の障害物への衝突などを抑制することができ、より安全性を向上させることができる。

本発明の実施形態２．
本発明の実施形態２に係るロボットアーム制御装置１において、アーム関節角度算出部５３は、位置補正量算出部５２により算出される位置補正量の絶対値｜Δｄ｜が所定量ε以下になるまで、ロボットアーム２の把持部２３が外力により移動したときの軌道（ｃ）と、元の位置へ復元するときの軌道（ｄ）とが同一となるように、ロボットアーム２の把持部２３の目標位置Ｄを算出してもよい（図５）。この場合、駆動制御部５４は、ロボットアーム２が外力により移動したときの軌道（ｃ）と、元の位置へ復元するときの軌道（ｄ）とが同一となるように、各アクチュエータ３１，３２，３３を制御して、ロボットアーム２の各関節部１１，１２，１３を制御する。

これにより、ロボットアーム２は、外力によってその移動中に障害物に接触することなく移動したときの軌道（ｃ）と同一の軌道（ｄ）で、元の位置へ復元することができる。したがって、例えば、ロボットアーム２が外力により変位した後、その外力から解放され元の位置へ復元する過程で、障害物等に接触する可能性をさらに低減することができる。

図６は、本実施形態２に係るロボットアーム制御装置の制御処理フローの一例を示すフローチャートである。例えば、指令値生成部５１が、Waitモードになり手先位置指令値ｄの生成を停止すると（ステップＳ２０１）、位置補正量算出部５２は、算出したロボットアームの位置補正量Δｄ（ｉ）（ｉ＝１〜ｎ）を、逐次ＲＡＭ５ｃ等に保存する（ステップＳ２０２）。

次に、ロボットアーム２の把持部２３が外力から解放され、元の位置へ復元開始すると（ステップＳ２０３）、アーム関節角度算出部５３は、ＲＡＭ５ｃに保存された過去の位置補正量Δｄ（ｉ）＝Δｄ（ｉ−１）を、逐次読み込む（ステップＳ２０４）。

その後、アーム関節角度算出部５３は、ＲＡＭ５ｃから読み込んだ位置補正量Δｄ（ｉ）から、ロボットアーム２の把持部２３の目標位置Ｄ（ｉ）（Ｄ（ｉ）＝Δｄ（ｉ））を算出し、この算出した目標位置Ｄ（ｉ）となる各関節部１１，１２，１３の回転角度指令値を夫々算出し（ステップＳ２０５）、駆動制御部５４に対して出力する。

さらに、駆動制御部５４は、各回転センサ４１，４２，４３により検出された各関節部１１，１２，１３の回転角度と、アーム関節角度算出部５３により算出された各関節部１１，１２，１３の回転角度指令値と、の偏差を夫々算出し、算出した各偏差に所定のゲインＫを乗算して制御信号を生成する。そして、駆動制御部５４は、生成した制御信号を、Ｄ／Ａ変換回路７及び駆動回路６を介して各関節部１１，１２，１３の各アクチュエータ３１，３２，３３に対して出力することで、各アクチュエータ３１，３２，３３の駆動を制御する（ステップＳ２０６）。

このようにして、アーム関節角度算出部５３は、ロボットアーム２の把持部２３が外力により移動したときの軌道（ｃ）と、元の位置へ復元するときの軌道（ｄ）とが同一となるように、ロボットアーム２の把持部２３の目標位置Ｄを算出することができる。

次に、指令値生成部５１は、位置補正量算出部５２により算出される位置補正量の絶対値｜Δｄ｜が所定量ε以下であるか否かを判断する（ステップＳ２０７）。

指令値生成部５１は、位置補正量算出部５２により算出される位置補正量の絶対値｜Δｄ｜が所定量ε以下でないと判断したとき（ステップＳ２０７のＮＯ）、Waitモードを維持し（ステップＳ２０８）、上記（ステップＳ２０４）の処理に移行する。

一方、指令値生成部５１は、位置補正量算出部５２により算出される位置補正量の絶対値｜Δｄ｜が所定量ε以下になった判断すると（ステップＳ２０７のＹＥＳ）、WaitモードからRunモードに切替り（ステップＳ２０９）、ＲＡＭ５ｃ等に記憶した全ての位置補正量Δｄ（ｉ）を消去する（ステップＳ２１０）。

指令値生成部５１は、WaitモードからRunモードに切替ると、手先位置指令値ｄの生成を再開し（ステップＳ２１１）、生成した手先位置指令値ｄをアーム関節角度算出部５３に出力する。

アーム関節角度算出部５３は、指令値生成部５１により生成された手先位置指令値ｄに、位置補正量算出部５２により算出された位置補正量Δｄを加算して、各関節部１１，１２，１３の回転角度指令値を夫々算出し（ステップＳ２１２）、駆動制御部５４に対して出力する。

さらに、駆動制御部５４は、各回転センサ４１，４２，４３により検出された各関節部１１，１２，１３の回転角度と、アーム関節角度算出部５３により算出された各関節部１１，１２，１３の回転角度指令値と、の偏差を夫々算出し、算出した各偏差に所定のゲインＫを乗算して制御信号を生成する。そして、駆動制御部５４は、生成した制御信号を、Ｄ／Ａ変換回路７及び駆動回路６を介して各関節部１１，１２，１３の各アクチュエータ３１，３２，３３に対して出力することで、各アクチュエータ３１，３２，３３の駆動を制御する（ステップＳ２１３）。

以上、本実施形態２に係るロボットアーム制御装置１において、駆動制御部５４は、位置補正量算出部５２により算出される位置補正量の絶対値｜Δｄ｜が所定量ε以下になるまで、ロボットアーム２が外力により移動したときの軌道と、元の位置へ復元するときの軌道とが同一となるように、各アクチュエータ３１，３２，３３の駆動を制御して、ロボットアーム２の各関節部１１，１２，１３の駆動を制御する。これにより、ロボットアーム２が外力により変位した後、その外力から解放され元の位置へ復元する過程で、障害物等に接触する可能性をより低減することができるため、安全性をさらに向上させることができる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。

例えば、上記実施形態において、駆動制御部５４は、位置補正量算出部５２により算出される位置補正量の絶対値｜Δｄ｜が所定量ε以下になるまで、ロボットアーム２の移動速度を通常より減少させるように、各関節部１１,１２,１３の各アクチュエータ３１,３２,３３を制御してもよい。

具体的には、駆動制御部５４は、位置補正量算出部５２により算出される位置補正量の絶対値｜Δｄ｜が所定量ε以下になるまで、ロボットアーム２の移動速度を所定変数（例えば、｜Δｄ｜／ε）で無段階に減少させてもよく、また、段階的に減少させてもよい。これにより、ロボットアーム２が外力により変位した後、その外力から解放され元の位置へ復元する過程で、障害物等に接触する可能性をより低減することができ、安全性をさらに向上させることができる。

また、上述の実施形態では、本発明をハードウェアの構成として説明したが、本発明は、これに限定されるものではない。本発明は、任意の処理を、ＣＰＵ５ａにコンピュータプログラムを実行させることにより実現することも可能である。

この場合、コンピュータプログラムは、記録媒体に記録して提供することも可能であり、また、インターネットその他の通信媒体を介して伝送することにより提供することも可能である。また、記憶媒体には、例えば、フレキシブルディスク、ハードディスク、磁気ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＤＶＤ、ＲＯＭカートリッジ、バッテリバックアップ付きＲＡＭメモリカートリッジ、フラッシュメモリカートリッジ、不揮発性ＲＡＭカートリッジ等が含まれる。また、通信媒体には、電話回線等の有線通信媒体、マイクロ波回線等の無線通信媒体等が含まれる。

１ ロボットアーム制御装置
２ ロボットアーム
３ 力センサ
５ 制御装置
１１ 肩関節部
１２ 肘関節部
１３ 手首関節部
２１ 上アーム部
２２ 下アーム部
２３ 把持部
３１，３２，３３ アクチュエータ
４１，４２，４３ 回転センサ
５１ 指令値生成部
５２ 位置補正量算出部
５３ アーム関節角度算出部
５４ 駆動制御部

Claims (8)

1. 複数の関節部を有するロボットアームと、
   前記ロボットアームに対して作用する外力を検出する外力検出手段と、
   前記外力検出手段により検出された前記ロボットアームの外力に基づいて、該ロボットアームのインピーダンス制御を行う際の位置補正量を算出する位置補正量算出手段と、
   前記ロボットアームの移動目標位置を指定するための手先位置指令値を生成する指令値生成手段と、
   前記位置補正量算出手段により算出される前記位置補正量と、前記指令値生成手段により生成された前記手先位置指令値と、に基づいて、前記ロボットアームの各関節部の駆動を制御する駆動制御手段と、
   を備えるロボットアーム制御装置であって、
   前記指令値生成手段は、前記位置補正量算出手段により算出される前記位置補正量が所定量以下になるまで、前記手先位置指令値の生成を停止する、ことを特徴とするロボットアーム制御装置。
2. 請求項１記載のロボットアーム制御装置であって、
   前記駆動制御手段は、前記位置補正量算出手段により算出される前記位置補正量が所定量以下になるまで、前記ロボットアームが外力により移動したときの軌道と、元の位置へ復元するときの軌道とが同一となるように、前記ロボットアームの各関節部の駆動を制御する、ことを特徴とするロボットアーム制御装置。
3. 請求項１又は２記載のロボットアーム制御装置であって、
   前記駆動制御手段は、前記位置補正量算出手段により算出される前記位置補正量が所定量以下になるまで、前記ロボットアームの移動速度が減少するように、前記ロボットアームの各関節部の駆動を制御する、ことを特徴とするロボットアーム制御装置。
4. 請求項１乃至３のうちいずれか１項記載のロボットアーム制御装置であって、
   前記指令値生成手段により生成された前記手先位置指令値と、前記位置補正量算出手段により算出された前記位置補正量と、に基づいて、前記ロボットアームの目標位置を算出し、該算出した目標位置となる前記各関節部の回転角度を夫々算出するアーム関節角度算出手段を更に備え、
   前記駆動制御手段は、前記アーム関節角度算出手段により算出された前記各関節の回転角度に基づいて、前記ロボットアームの各関節部の駆動を制御する、ことを特徴とするロボットアーム制御装置。
5. 請求項４記載のロボットアーム制御装置であって、
   前記ロボットアームの各関節部の回転角度を検出する回転検出手段と、
   前記ロボットアームの各関節部を駆動する駆動手段と、を更に備え、
   前記駆動制御手段は、前記回転検出手段により検出された各関節部の回転角度と、前記アーム関節角度算出手段により算出された前記各関節部の回転角度と、の偏差を夫々算出し、該算出した各偏差に所定のゲインを乗算して、前記駆動手段に対する制御信号を生成する、ことを特徴とするロボットアーム制御装置。
6. 請求項１乃至５のうちいずれか１項記載のロボットアーム制御装置であって、
   前記指令値生成手段は、
   前記位置補正量算出手段により算出される前記位置補正量が所定量以下となり、前記手先位置指令値の生成を停止するWaitモードと、
   前記位置補正量算出手段により算出される前記位置補正量が所定量より大きくなり、前記手先位置指令値の生成を行うRunモードと、を有する、ことを特徴とするロボットアーム制御装置。
7. 複数の関節部を有するロボットアームに作用する外力を検出する工程と、
   前記検出されたロボットアームの外力に基づいて、該ロボットアームのインピーダンス制御を行う際の位置補正量を算出する工程と、
   前記ロボットアームの移動目標位置を指定するための手先位置指令値を生成する工程と、
   前記算出される位置補正量と、前記生成された手先位置指令値と、に基づいて、前記ロボットアームの各関節部の駆動を制御する工程と、
   を含むロボットアーム制御装置の制御方法であって、
   前記算出される位置補正量が所定量以下になるまで、前記手先位置指令値の生成を停止する工程を含む、ことを特徴とするロボットアーム制御装置の制御方法。
8. 複数の関節部を有するロボットアームに作用する外力に基づいて、該ロボットアームのインピーダンス制御を行う際の位置補正量を算出する処理と、
   前記ロボットアームの移動目標位置を指定するための手先位置指令値を生成する処理と、
   前記算出される位置補正量と、前記生成された手先位置指令値と、に基づいて、前記ロボットアームの各関節部の駆動を制御する処理と、
   前記算出される位置補正量が所定量以下になるまで、前記手先位置指令値の生成を停止する処理と、
   をコンピュータに実行させることを特徴とするロボットアーム制御装置のプログラム。

Images