JP2013166224A - ロボットの制御装置、ロボット及びそのプログラム - Google Patents

Abstract

【課題】アクチュエータ性能を有効に活用でき、且つ、アクチュエータ性能Ｕが上限である第１の制約値を超えないように、たわみ補正駆動を行える構造を実現する。
【解決手段】目標軌道算出手段１０１で目標値Ｘｒｅｆと第２の制約値Ｕｏｐｔとを用いてロボットの目標軌道θｒｅｆを算出する。この目標軌道θｒｅｆとたわみ補正量Δθからたわみ補正軌道θ_１ｒｅｆを算出し、このたわみ補正軌道θ_１ｒｅｆを実現するために必要なアクチュエータ性能Ｕを算出する。判定手段１０５でこのアクチュエータ性能Ｕが第１の制約値の範囲内であるか、及び、これらの差分が所定値の範囲内であるかを判定する。条件を満たさない場合には、制約値変更手段１０６で第２の制約値Ｕｏｐｔを変更して計算を繰り返す。条件を満たした場合に、その時のたわみ補正軌道θ_１ｒｅｆをロボットの軌道として決定する。
【選択図】図３

Description

本発明は、例えば、部品の搬送や組み立てなどを行う産業用のロボットアームなどのアクチュエータにより駆動されるロボットの制御装置、ロボット及びそのプログラムに関する。

例えば、産業用ロボットは、利用範囲の拡大に伴い動作の高速・高精度化、更には軽量化が求められている。ロボットの軽量化、ロボット動作速度の高速化への要求が高まるにつれ、アクチュエータにより動作するアーム部などのロボットの機構部のたわみが大きくなり、精度に及ぼす影響が無視できなくなってきている。このようなロボットの機構部のたわみに対して、従来のロボットではたわみを推定し補償することで高精度動作を行ってきた。

例えば、負荷トルクとロボットアームの関節の剛性からロボットアームの関節のたわみ量を算出し、各軸の角度指令値に加える構成が提案されている（特許文献１）。また、減速機に加えてリンクのたわみ量を算出し、逆運動学計算を用いて各軸の角度指令値を修正することでたわみを補償する構成も提案されている（特許文献２）。

特開昭６１−２０１３０４号公報 特許第３８０８３２１号公報

上述したような従来技術では、目標軌道に対してロボットの関節部に生じるたわみ量を算出し、そのたわみ量から目標軌道を補正して各軸のアクチュエータを駆動（たわみ補正駆動）している。但し、たわみ補正駆動を実現するために必要なアクチュエータ性能が考慮されていない。

実際にアクチュエータをたわみ補正駆動させた場合、たわみ量に応じてアクチュエータの加減速トルクが大きく変動する。このため、アクチュエータ性能を考慮しないと、たわみ補正駆動に必要なアクチュエータ性能が、そのアクチュエータの性能の上限として設定される制約値を超える場合がある。例えば、たわみ補正駆動に必要なトルクが、制約値としてのトルクの上限値を超える場合がある。

このように、たわみ補正駆動に必要なアクチュエータ性能が制約値を超えた場合、アクチュエータの追従誤差増大に伴う応答劣化や過電流によるエラー停止等が発生し、ロボットアームの動作性能に影響を及ぼす可能性がある。

一方、たわみ補正駆動に必要なアクチュエータ性能が制約値に対して十分に小さい値をとり余裕過多となる場合もある。この場合、アクチュエータの性能を有効活用していない状態である。言い換えれば、そのアクチュエータの性能を十分に活用すれば、もっと速く動作させたりすることができる状態である。

本発明は、このような事情に鑑み、アクチュエータ性能を有効に活用でき、且つ、たわみ補正駆動に必要なアクチュエータ性能がそのアクチュエータの性能の上限を超えないように、たわみ補正駆動を行える構造を実現すべく発明したものである。

本発明は、第１の制約値が性能の上限として設定されるアクチュエータにより駆動されるロボットの制御装置において、ロボットの目標値と、第２の制約値と、を用いて最適制御問題を解くことにより、前記ロボットの目標軌道を算出する目標軌道算出手段と、前記目標軌道で前記ロボットを動作させる場合のロボットのたわみ補正量を算出するたわみ補正量算出手段と、前記たわみ補正量から前記目標軌道を補正してたわみ補正軌道を算出するたわみ補正軌道算出手段と、前記たわみ補正軌道で前記ロボットを動作させるために必要な前記アクチュエータの性能を算出するアクチュエータ性能算出手段と、前記アクチュエータ性能算出手段により算出したアクチュエータ性能が前記第１の制約値の範囲内である第１の条件、及び、前記第１の制約値と前記アクチュエータ性能との差分が所定値の範囲内である第２の条件を満たすか否かを判定する判定手段と、前記判定手段が前記第１の条件又は前記第２の条件を満たさないと判定した場合に、前記第２の制約値を変更する制約値変更手段と、前記判定手段が前記第１の条件及び前記第２の条件を満たすと判定した場合の、前記たわみ補正軌道算出手段により算出したたわみ補正軌道を、前記ロボットの軌道として決定する軌道決定手段と、を備えた、ことを特徴とするロボットの制御装置にある。

本発明によれば、最適制御問題を解くための第２の制約値を変更することにより、たわみ補正駆動に必要なアクチュエータ性能が第１の制約値を超えずに、第１の制約値に対して所定値の範囲内に収められるようなたわみ補正軌道を求められる。このため、アクチュエータ性能を有効に活用でき、且つ、たわみ補正駆動に必要なアクチュエータ性能がそのアクチュエータの性能の上限を超えないように、たわみ補正駆動を行える。

本発明の対象となるロボットの１例を示す斜視図。 本発明の実施形態に係るロボットのブロック図。 本発明の実施形態に係るロボットの制御装置を処理手段の関係で示すブロック図。 同じくロボットの制御装置を構造の関係で示すブロック図。 同じくロボットの制御装置でたわみ補正軌道を求めるためのフローチャート。 本発明の処理を行わない場合の比較例で、（Ａ）は目標軌道とたわみ補正軌道との関係を、（Ｂ）は目標軌道を実現するためのモータトルクとたわみ軌道を実現するためのモータトルクとの関係を、それぞれ示す図。 本発明の処理を行った場合と行わなかった場合とを比較した、（Ａ）はたわみ補正軌道を、（Ｂ）はたわみ軌道を実現するためのモータトルクを、それぞれ示す図。

＜ロボットの１例＞
本発明の対象となるロボットの１例の概略構成について、図１を用いて説明する。ロボットアーム２０１は、ロボットの機構部である各アーム部２０１ａ〜２０１ｃを動作させる複数のアクチュエータであるモータ２０２〜２０６と、これら各モータ２０２〜２０６を制御する制御装置２００とを備える。制御装置２００には、各モータ２０２〜２０６にそれぞれ備えられたアクチュエータ制御部であるモータ制御部２０２ａ〜２０６ａが接続される。各モータ２０２〜２０６により動作する機構部としては、上述の各アーム部２０１ａ〜２０１ｃ以外にも、モータの駆動を減速して伝達する減速機など、モータの回転軸以降のロボットの各構成部材を含む。

各モータ２０２〜２０６は、ロボットアーム２０１の各関節部分などのロボットの駆動部分に配置される。そして、モータ２０２は、ロボットアーム２０１全体を架台２０７に対して旋回させる。モータ２０３は、アーム部２０１ａを架台２０７に対して図の上下方向に回動させる。モータ２０４は、アーム部２０１ｂをアーム部２０１ａに対して回動させる。モータ２０５は、アーム部２０１ｃをアーム部２０１ｂに対して回動させる。モータ２０６は、アーム部２０１ｃの先端部分を基端部分に対して回動させる。

このように構成されるロボットアーム２０１は、例えば、棒状のワークＷ１を円柱状のワークＷ２に形成された孔に挿入する作業を次のように行う。まず、ユーザがこのような指示を入力すると、制御装置２００がロボットアーム２０１の手先の軌道を計算する。そして、この軌道に基づいてモータ制御部２０２ａ〜２０６ａが各モータ２０２〜２０６を制御し、各モータ２０２〜２０６が駆動されることでロボットアーム２０１が動作して、ワークＷ１を円柱状のワークＷ２に形成された孔に挿入する。

＜実施形態＞
次に、本発明の実施形態について、図２ないし図７を用いて説明する。なお、本明細書では、ロボットの位置姿勢の順序集合を経路と、時系列の角度指令値を目標軌道と、たわみ量を補償し修正した目標軌道をたわみ補正軌道と、たわみ補正軌道に従いアクチュエータを駆動することをたわみ補正駆動と、それぞれ定義する。また、ユーザがロボットに指令値として与える、目標位置、目標経路、目標姿勢などの少なくとも何れかを含むものを目標値とする。

［ロボット］
まず、本実施形態のロボットについて、図２に示すような制御構成である６軸多関節ロボットアームを例に説明を行う。ロボットの制御装置であるメインＣＰＵ２は、ティーチングペンダントなどの教示用端末装置１から得られる教示点情報（目標値）を元に、教示点間の補間処理を行うなどして、後述するようにロボットアーム手先の軌道（ロボットの軌道）を決定する。

アクチュエータ制御部であるモータサーボ制御器３ａ〜３ｆは、メインＣＰＵ２で決定された軌道を用いて、それぞれモータ４ａ〜４ｆの制御を行う。なお、図示していないが、各軸のモータ４ａ〜４ｆにはエンコーダなどの角度・角速度検出器が取り付けられており、検出されたモータ角度・角速度はモータサーボ制御器３ａ〜３ｆにフィードバックされる。そして、上述の図１で説明したようにロボットを制御する。ここで、メインＣＰＵ２が図１の制御装置２００に、モータ４ａ〜４ｆが図１のモータ２０２〜２０６に、モータサーボ制御器３ａ〜３ｆがモータ制御部２０２ａ〜２０６ａに、それぞれ相当するものである。

ロボットアームをこのように制御する場合に、メインＣＰＵ２は、各関節部に生じるたわみを算出および補正して各軸のモータを駆動することでロボットアームの手先位置および姿勢の高精度動作を行うことができる。本実施形態では、ロボットの各関節部に生じるたわみを算出および補正して各軸のモータを駆動する際に、アクチュエータ性能（モータ性能）の制約値を考慮し目標軌道を修正する。これによりアクチュエータ性能の制約超過を抑え、かつ余裕が生じる場合は余裕を有効活用し動作時間の短縮を実現する。この点について、以下、詳しく説明する。

［ロボットの制御装置］
上述のような制御を行うロボットの制御装置であるメインＣＰＵ２について、図３ないし図５を用いて説明する。図３は、メインＣＰＵ２内を処理手段で区分して示すブロック図である。メインＣＰＵ２は、目標軌道算出手段１０１と、たわみ補正量算出手段１０２と、たわみ補正軌道算出手段１０３と、アクチュエータ性能算出手段１０４と、判定手段１０５と、制約値変更手段１０６と、軌道決定手段１０７と、を備える。

ここで、ロボットのアーム手先の目標経路（ロボットの目標値）をＸｒｅｆ、各軸の目標軌道をθｒｅｆとする。また、アクチュエータ性能（モータ性能）を表すベクトルとして各軸のモータ角速度、角加速度、角加加速度、トルクを並べたベクトルをＵとする。また、各モータ４ａ〜４ｆの性能の限界値や余裕を考慮して予め設定されたアクチュエータ性能の上限である第１の制約値をＵｍ、目標軌道計算時に用いるアクチュエータ性能の第２の制約値をＵｏｐｔとする。なお、Ｕｍの各要素は正の値であるとする。また、第２の制約値Ｕｏｐｔは、後述するように変更可能である。また、以下の説明では、簡単のために、アクチュエータ性能Ｕ、第１の制約値Ｕｍ、第２の制約値Ｕｏｐｔは、それぞれトルクとするが、角速度、角加速度、角加加速度などについても、それぞれの値を有するようにしても良い。

目標軌道算出手段１０１は、ロボットの目標値Ｘｒｅｆと第２の制約値Ｕｏｐｔと、を用いて最適制御問題である最短時間制御問題を解くことにより、制約条件を満たしかつ動作時間を最短とする各軸（ロボット）の目標軌道θｒｅｆを算出する。

なお、最適制御問題とは、評価関数を最適に制御するもので、例えば、時間や振れなどを最適に制御する。最短時間制御問題とは、最適制御問題の評価関数を時間としたものである。以下の説明では、最適制御問題を最短時間制御問題とするが、他の評価関数が入っても同様である。また、最短時間制御問題の解法としては、例えば、J.E.Bobrowらの文献に記載された方法が挙げられる。文献は、「J.E.Bobrow et. al, “Time-Optimal Control of Robotic Manipulators Along Specified Paths”, Int. J. of Robotics Research, 4, 3, pp.3-17, 1989」である。

たわみ補正量算出手段１０２は、目標軌道算出手段１０１で算出した目標軌道でロボットを動作させる場合のロボットのたわみ補正量を算出する。即ち、たわみ補正量算出手段１０２は、目標軌道θｒｅｆを入力とし、ロボットアーム全体の運動方程式と剛性からたわみ補正量Δθを出力する。本実施例では、簡単のため各関節部のモータ回転方向のたわみのみを考慮することにする。

ロボットアームの運動方程式は、次のように表せる。
Ｍ×ｄｄｑ＋ｈ＋ｇ＝τ

Ｍは慣性行列、ｄｄｑは加速度、ｈは遠心力及びコリオリ力、ｇは重力、τは各関節部にかかる負荷トルクである。ｄｄｑ、ｈ、ｇ、τはそれぞれベクトルである。慣性行列Ｍはロボットアームの構成で決まるものであり、加速度ｄｄｑは軌道を時間で２階微分することで求められる。したがって、ロボットアームの運動方程式から各関節部にかかる負荷トルクτが求められる。

負荷トルクτが求められれば、次式のように、τを剛性値Ｋで除することで、各関節部のたわみ量Δθが求められる。
Δθ＝τ／Ｋ

ここで、剛性値Ｋは各軸における回転方向の剛性値、即ちねじり剛性（ねじり方向の強さ、単位は「Ｎｍ／ｒａｄ」）であり、事前に設定されているものとする。各関節部のモータ回転方向のたわみのみを考慮しているため、関節のたわみ量とたわみ補正量は等しい値となる。なお、たわみ補正量としては、ねじり方向のみでなく、並進方向のたわみなどを含むようにしても良い。

たわみ補正軌道算出手段１０３は、たわみ補正量算出手段１０２で算出したたわみ補正量から目標軌道を補正して、たわみ補正軌道を算出する。即ち、たわみ補正軌道算出手段１０３は、目標軌道θｒｅｆとたわみ補正量Δθを入力とし、目標軌道θｒｅｆとたわみ補正量Δθを加算したたわみ補正軌道θ_１ｒｅｆを出力とする。

アクチュエータ性能算出手段１０４は、たわみ補正軌道算出手段で算出したたわみ補正軌道でロボットを動作させるために必要なアクチュエータ（モータ）の性能を算出する。即ち、アクチュエータ性能算出手段１０４は、たわみ補正軌道θ_１ｒｅｆを入力とし、ロボットアーム全体の運動方程式からアクチュエータ性能Ｕを出力する。具体的には、上述した運動方程式の加速度ｄｄｑをたわみ補正軌道θ_１ｒｅｆを２階微分することにより求め、τをアクチュエータ性能Ｕとして算出する。

判定手段１０５は、アクチュエータ性能算出手段１０４により算出したアクチュエータ性能が第１の制約値の範囲内である第１の条件、及び、第１の制約値とアクチュエータ性能との差分が所定値の範囲内である第２の条件を満たすか否かを判定する。言い換えれば、アクチュエータ性能Ｕを入力とし、アクチュエータ性能Ｕが第１の制約値Ｕｍを超えず、且つ、アクチュエータ性能Ｕが第１の制約値Ｕｍの近傍に存在するかを判定する。

したがって、第１の条件は｜Ｕ｜＜Ｕｍであり、第２の条件はＵｍ−｜Ｕ｜＜εである。εは上述の所定値であり、Ｕｍ未満の範囲で任意に設定できる微小量である。このεの値によって、最終的な結果をどれだけ第１の制約値に近づけられるかが決まる。このため、このεは、計算時間とアクチュエータ性能（本実施形態ではトルク）の余裕との兼ね合いで設定する。

制約値変更手段１０６は、判定手段１０５が第１の条件又は第２の条件を満たさないと判定した場合に、第２の制約値を変更する。即ち、最短時間制御問題で用いる第２の制約値Ｕｏｐｔを変更して出力する。具体的には、判定手段１０５が第１の条件又は第２の条件を満たさないと判定した場合、判定で使用したアクチュエータ性能Ｕを出力し、このアクチュエータ性能Ｕが制約値変更手段１０６に入力される。制約値変更手段１０６は、第１の制約値Ｕｍからこのアクチュエータ性能Ｕを減じた値に、０よりも大きい所定のゲインＧを乗じることで算出した値を第２の制約値Ｕｏｐｔに加えて、変更後の第２の制約値Ｕｏｐｔとする。

所定のゲインＧは、事前に設定される０よりも大きい値で、小さい値であれば計算時間が長くなり、大きい値であれば最終的な結果が第１の制約値に近づきにくい。したがって、上述のεや計算時間などを考慮して、任意に設定する。

変更後の第２の制約値Ｕｏｐｔは、目標軌道算出手段１０１に入力され、目標軌道算出手段１０１が上述のような計算を行って、新たに目標軌道を算出する。その後、上述のように、この新たな目標軌道を用いてたわみ補正軌道を算出し、このたわみ補正軌道でのアクチュエータ性能を算出して、判定手段１０５による判定を行う。

したがって、メインＣＰＵ２内では、第１の条件及び第２の条件を満たすまで、制約値変更手段１０６により第２の制約値Ｕｏｐｔを変更して、上述したようなたわみ補正軌道に対するアクチュエータ性能の算出を繰り返す収束計算が実行される。言い換えれば、第１の条件及び第２の条件を満たすまで、目標軌道及びたわみ補正軌道の修正が行われる。

軌道決定手段１０７は、判定手段１０５が第１の条件及び第２の条件を満たすと判定した場合の、たわみ補正軌道算出手段１０３により算出したたわみ補正軌道を、ロボットの軌道として決定する。具体的には、判定手段１０５が、第１の条件である｜Ｕ｜＜Ｕｍ、第２の条件であるＵｍ−｜Ｕ｜＜εを満たすと判定した場合、その時のたわみ補正軌道θ_１ｒｅｆを出力し、このたわみ補正軌道θ_１ｒｅｆが軌道決定手段１０７に入力される。軌道決定手段１０７は、この入力されたたわみ補正軌道θ_１ｒｅｆをロボットの軌道として決定し、モータサーボ制御器３ａ〜３ｆに出力する。

本実施形態では、メインＣＰＵ２が、予めインストールされている、ロボットのプログラムに基づいて動作する。メインＣＰＵ２は、図４に示すように、演算装置２１及び記憶装置２２を有するコンピュータ本体に、モータサーボ制御器３ａ〜３ｆが接続されて構成されている。また、コンピュータ本体には、作業者が入力作業を行うためのティーチングペンダント２３、及びキーボード２４ａやマウス２４ｂなどの入力装置２４も接続されている。なお、ティーチングペンダント２３が図１の教示用端末装置１に相当する。

記憶装置２２には、ワークの形状データなどの各種データが格納されている。また、その他にも、モータなどの制御ドライバや、ワークの組み立て手順の演算をコンピュータ（演算装置２１）に実行させる組み立て手順演算プログラムなどの各種プログラムが格納されている。更に本実施形態の場合には、ロボットの軌道を決定するためのプログラムが格納されている。

より詳しくは、コンピュータ本体は、ＣＰＵ２１ａを主体として構成され、このＣＰＵ２１ａには、ＲＯＭ２２ａ及びＲＡＭ２２ｂがバス２５を介して接続されている。ＲＯＭ２２ａには、ロボットの基本制御に必要なプログラムが格納されていると共に、上述したロボットの軌道を決定するためのプログラムなどの各種プログラムやデータが格納されている。ＲＡＭ２２ｂには、ＣＰＵ２１ａに対する作業領域が確保される。

また、バス２５には、モータサーボ制御器３ａ〜３ｆが接続されていると共に、記録ディスク読取装置２６が接続されている。そして、ＣＰＵ２１ａが、ロボットの軌道を決定するためのプログラムなどを記録した記録媒体Ｄを読み込み、例えばＲＯＭ２２ａに格納できるようになっている。なお、上述した記憶装置２２は、主記憶装置であるＲＯＭ２２ａ及びＲＡＭ２２ｂの他に、コンピュータ読取可能な記録媒体Ｄやその他の外部記憶装置を備えて構成されている。

上述の目標軌道算出手段１０１、たわみ補正量算出手段１０２、たわみ補正軌道算出手段１０３、アクチュエータ性能算出手段１０４、判定手段１０５、制約値変更手段１０６、軌道決定手段１０７は、演算装置２１及び記憶装置２２から構成される。

次に、本実施形態のロボットの軌道を決定するためのプログラムに基づく制御フローについて、図５を用いて説明する。メインＣＰＵ２は、図５に示すフローチャートに従い処理を行うことで、図３に示すロボットの制御装置の機能を実現する。ここで、図３の制御装置（メインＣＰＵ２）と図５のフローチャートの対応関係は次のようになる。Ｓ２が目標軌道算出手段１０１、Ｓ３がたわみ補正量算出手段１０２、Ｓ４がたわみ補正軌道算出手段１０３、Ｓ５がアクチュエータ性能算出手段１０４、Ｓ６が判定手段１０５、Ｓ７が制約値変更手段１０６、Ｓ８が軌道決定手段１０７に対応する。以下、図５のフローチャートを用いて説明する。

まず、教示用端末装置１から得られる教示点情報を元に教示点間の補間処理を行い、アーム手先の目標値（目標経路（教示点、補間点））Ｘｒｅｆを求める。これとともに、アクチュエータ性能の第１の制約値Ｕｍを軌道生成に用いる第２の制約値Ｕｏｐｔとする（Ｓ１）。

次に、アーム手先の目標経路Ｘｒｅｆ、第２の制約値Ｕｏｐｔを用いて最短時間制御問題を解くことで、｜Ｕ｜≦Ｕｏｐｔを満たし、動作時間を最短とする各軸の目標軌道θｒｅｆを求める（Ｓ２）。

次いで、各関節のたわみ補正量Δθを、ロボットアームの運動方程式から各関節部にかかる負荷トルクτを求め、剛性値Ｋを除することで求める（Ｓ３）。そして、目標軌道θｒｅｆにたわみ補正量Δθを加算し、たわみ補正軌道θ_１ｒｅｆを求める（Ｓ４）。求めたたわみ補正軌道θ_１ｒｅｆを用いてロボットアームの運動方程式から、アクチュエータ性能Ｕを算出する（Ｓ５）。

次に、アクチュエータ性能Ｕの絶対値が第２の制約値Ｕｍの範囲内（第１の条件）、及び、第２の制約値Ｕｍとアクチュエータ性能Ｕの絶対値との差分が微小量εの範囲内（第２の条件）であるか判断する（Ｓ６）。この条件はアクチュエータ性能が第１の制約以上の値をとる、または第１の制約値の近傍に存在しない、ということを意味する。

Ｓ６の第１の条件と第２の条件との何れかを満たさない場合、Ｕｍから｜Ｕ｜を減じて事前に設定したゲインＧ（Ｇ＞０）を乗じ、軌道生成に用いる第２の制約値Ｕｏｐｔに加えて、新しい第２の制約値Ｕｏｐｔとし（Ｓ７）、Ｓ２に戻る。Ｓ２に戻ると新しい第２の制約値Ｕｏｐｔを用いて、再度、最短時間制御問題を解き、目標軌道θｒｅｆの修正を行う。Ｓ６の条件を満たすまでＳ２からＳ７の処理を繰り返し実行する。

Ｓ６の条件を満たす場合、現在求められているたわみ補正軌道θ_１ｒｅｆをロボットの軌道として決定し、モータサーボ制御器に出力する（Ｓ８）。

上述の第２の制約値の変更は、アクチュエータ性能Ｕが第１の制約値Ｕｍを超える場合には収束計算に用いる第２の制約値Ｕｏｐｔを小さくすることで、Ｓ２において動作時間を増加させる効果がある。動作時間が増加すると必要アクチュエータ性能Ｕが減少するため、必要アクチュエータ性能Ｕを第１の制約値Ｕｍに近づけることができる。一方、アクチュエータ性能Ｕが第１の制約値Ｕｍよりもε以上小さい、すなわち余裕が生じる場合には、第２の制約値Ｕｏｐｔを大きくすることで、Ｓ２において動作時間を減少させる効果がある。動作時間が減少すると必要アクチュエータ性能Ｕが増加するため、必要アクチュエータ性能Ｕを第１の制約値Ｕｍに近づけることができる。

上述したように、最短時間制御問題を解くための第２の制約値を変更することにより、たわみ補正駆動に必要なアクチュエータ性能が第１の制約値を超えずに、第１の制約値に対して所定値の範囲内に収められるようなたわみ補正軌道を求められる。このため、アクチュエータ性能を有効に活用でき、且つ、たわみ補正駆動に必要なアクチュエータ性能がそのアクチュエータの性能の上限を超えないように、たわみ補正駆動を行える。

即ち、必要アクチュエータ性能Ｕが第１の制約値Ｕｍを超えないため、アクチュエータの追従誤差増大に伴う応答劣化や過電流によるエラー停止等の発生を抑えることができる。また、第１の制約値Ｕｍに対して必要アクチュエータ性能Ｕに余裕がある場合は、生じた余裕を有効活用したたわみ補正軌道θ_１ｒｅｆを求めるため、ロボットの動作時間を短縮することができる。

より具体的には、たわみ補正駆動時に必要なアクチュエータ性能を考慮して目標軌道を修正することで、搭載アクチュエータで実現可能かつ動作時間が最短となるたわみ補正軌道を生成できる。例えば、たわみ補正によって必要トルクが補正前と比べ増加し制限値である許容出力トルクを超えてしまう場合、目標軌道を修正することで動作時間は少し増加するが、たわみ補正駆動時の必要トルクを制限値内に収めることができる。そのため、アクチュエータの追従誤差増大に伴う応答劣化や過電流によるエラー停止等の発生を抑えることができ、ロボットは高精度に動作し続けることができる。一方、たわみ補正によって必要トルクが減少する場合、生じた余裕トルクを有効活用することで、たわみ補正駆動時の必要トルクが制限値内で収まる範囲で動作時間を短縮することができる。

＜実施例＞
次に、上述のような本実施形態の効果について、本実施形態を満たさない場合と比較して説明する。まず、たわみを考慮しない目標軌道とたわみ補正軌道の関係をより明確にするために図６（Ａ）、（Ｂ）を用いて説明を行う。

図６（Ａ）は６軸多関節ロボットアームの駆動の場合における、たわみ補正を考慮していない目標軌道θｒｅｆとたわみ補正軌道θ_１ｒｅｆを比較した説明図である。目標軌道θｒｅｆは時刻ｔ_０からｔ_１にかけて角度Φ_０からΦ_１に移動する軌道である。図６（Ａ）はｘ軸に時間、ｙ軸に角度を示し、破線は目標軌道θｒｅｆ、実線はたわみ補正軌道θ_１ｒｅｆを表している。

図６（Ｂ）はθｒｅｆの実現に必要なモータトルクＴｒｅｆとθ_１ｒｅｆの実現に必要なモータトルクＴ_１を比較した説明図である。図６（Ｂ）はｘ軸に時間、ｙ軸にトルクを示し、破線は目標軌道の実現に必要なモータトルクＴｒｅｆ、実線はたわみ補正軌道の実現に必要なモータトルクＴ_１を表している。

図６（Ａ）に示すように、目標軌道θｒｅｆに対してたわみ補正軌道θ_１ｒｅｆは最大でα［ｒａｄ］だけ変化している。これはたわみ補正軌道にはたわみ補正量が加算されているからである。このように軌道が変化するため、それに伴い必要トルクも変化する。

図６（Ｂ）に示すようにモータトルクの最大値はＴｒｅｆよりＴ_１の方がβ［Ｎｍ］小さい値となっている。これは、たわみ量の補正によって必要トルクが減少することで必要なアクチュエータ性能が制約値に対して余裕過多となっており、アクチュエータの性能を有効活用していない状態である。本実施例では生じた余裕を利用し、動作時間を短縮する。

このような余裕を利用して、上述の図５に示したフローのＳ２からＳ７まで繰り返し計算して軌道を修正した場合（実施例）と、しなかった場合（比較例）とについて、図７（Ａ）、（Ｂ）を用いて説明する。

図７（Ａ）、（Ｂ）はそれぞれたわみ補正軌道θ_１ｒｅｆとモータトルクＴ_１の図である。図７（Ａ）のｘ軸は時間、ｙ軸は角度を示し、図７（Ｂ）のｘ軸は時間、ｙ軸はトルクを示している。図７（Ａ）、（Ｂ）の破線は最初にＳ５で求めたもの、即ち軌道の修正を行う前の値を表し、実線はＳ８におけるもの、即ち繰り返し計算によって軌道が修正された後の値を表す。また、図７（Ｂ）における一点鎖線はモータトルクの第１の制約値を表している。

図７（Ｂ）に示すように、修正前のモータトルクは第１の制約値に対して余裕があり、アクチュエータの性能を有効活用していない状態である。一方、修正後のモータトルクは第１の制約値近傍の値となり、これに伴い図７（Ａ）に示すように修正前に比べ修正後のたわみ補正軌道の動作時間がγ［ｓ］短くなっている。このように繰り返し計算による軌道の修正によってモータトルクが第１の制約値近傍まで有効活用され、動作時間を短縮することができることが確認できた。

２・・・メインＣＰＵ（ロボットの制御装置）、２００・・・制御装置、３・・・モータサーボ制御器（アクチュエータ制御部）、２０２ａ、２０３ａ、２０４ａ、２０５ａ、２０６ａ・・・モータ制御部（アクチュエータ制御部）４、２０２、２０３、２０４、２０５、２０６・・・モータ（アクチュエータ）、１０１・・・目標軌道算出手段、１０２・・・たわみ補正量算出手段、１０３・・・たわみ補正軌道算出手段、１０４・・・アクチュエータ性能算出手段、１０５・・・判定手段、１０６・・・制約値変更手段、１０７・・・軌道決定手段

Claims (5)

1. 第１の制約値が性能の上限として設定されるアクチュエータにより駆動されるロボットの制御装置において、
   ロボットの目標値と、第２の制約値と、を用いて最適制御問題を解くことにより、前記ロボットの目標軌道を算出する目標軌道算出手段と、
   前記目標軌道で前記ロボットを動作させる場合のロボットのたわみ補正量を算出するたわみ補正量算出手段と、
   前記たわみ補正量から前記目標軌道を補正してたわみ補正軌道を算出するたわみ補正軌道算出手段と、
   前記たわみ補正軌道で前記ロボットを動作させるために必要な前記アクチュエータの性能を算出するアクチュエータ性能算出手段と、
   前記アクチュエータ性能算出手段により算出したアクチュエータ性能が前記第１の制約値の範囲内である第１の条件、及び、前記第１の制約値と前記アクチュエータ性能との差分が所定値の範囲内である第２の条件を満たすか否かを判定する判定手段と、
   前記判定手段が前記第１の条件又は前記第２の条件を満たさないと判定した場合に、前記第２の制約値を変更する制約値変更手段と、
   前記判定手段が前記第１の条件及び前記第２の条件を満たすと判定した場合の、前記たわみ補正軌道算出手段により算出したたわみ補正軌道を、前記ロボットの軌道として決定する軌道決定手段と、を備えた、
   ことを特徴とするロボットの制御装置。
2. 前記制約値変更手段は、前記第１の制約値から前記アクチュエータ性能を減じた値に、０よりも大きい所定のゲインを乗じることで算出した値を前記第２の制約値に加えて、変更後の第２の制約値とする、
   ことを特徴とする、請求項１に記載のロボットの制御装置。
3. 前記たわみ補正量算出手段は、前記目標軌道と、前記ロボットの運動方程式と、前記ロボットの剛性とから、前記たわみ補正量を算出する、
   ことを特徴とする、請求項１又は２に記載のロボットの制御装置。
4. アクチュエータと、前記アクチュエータにより動作する機構部と、前記アクチュエータを制御するアクチュエータ制御部と、請求項１ないし３のうちの何れか１項に記載のロボットの制御装置と、を備え、
   前記アクチュエータ制御部は、前記軌道決定手段により決定された軌道で、前記アクチュエータを制御する、
   ことを特徴とするロボット。
5. 第１の制約値が性能の上限として設定されるアクチュエータにより駆動されるロボットの制御装置を、
   ロボットの目標値と、第２の制約値と、を用いて最適制御問題を解くことにより、前記ロボットの目標軌道を算出する目標軌道算出手段、
   前記目標軌道で前記ロボットを動作させる場合のロボットのたわみ補正量を算出するたわみ補正量算出手段、
   前記たわみ補正量から前記目標軌道を補正してたわみ補正軌道を算出するたわみ補正軌道算出手段、
   前記たわみ補正軌道で前記ロボットを動作させるために必要な前記アクチュエータの性能を算出するアクチュエータ性能算出手段、
   前記アクチュエータ性能算出手段により算出したアクチュエータ性能が前記第１の制約値の範囲内である第１の条件、及び、前記第１の制約値と前記アクチュエータ性能との差分が所定値の範囲内である第２の条件を満たすか否かを判定する判定手段、
   前記判定手段が前記第１の条件又は前記第２の条件を満たさないと判定した場合に、前記第２の制約値を変更する制約値変更手段、
   前記判定手段が前記第１の条件及び前記第２の条件を満たすと判定した場合の、前記たわみ補正軌道算出手段により算出したたわみ補正軌道を、前記ロボットの軌道として決定する軌道決定手段、として機能させる
   ことを特徴とするロボットのプログラム。

Images