JP5870801B2 - ロボットの制御方法、およびロボットの制御装置 - Google Patents

Description

本発明は、モータにより駆動されるアーム部を有するロボットの制御方法、およびロボットの制御装置に関する。

従来、モータにより駆動されるアーム部を有するロボットを制御する際に、与えられた位置指令値から速度指令値およびトルク指令値を算出し、それらの指令値に基づいてフィードバック制御を行うものが知られている（例えば、特許文献１参照）。このような制御においては、アーム部を駆動するモータのトルクが設計上あるいは仕様上の限界を超えないように、トルクを制限した上でモータが駆動される。このとき、トルクは、例えばモータやアーム部の慣性（イナーシャ）、多関節型のロボットであれば他のアーム部におけるトルクの影響（干渉トルク）、あるいはアーム部を構成する部品に生じる摩擦など、モータおよびアーム部の挙動をモデル化した情報に基づいて制限が行われている。

ところで、アーム部を実際に作動させた場合には、例えばアーム部の位置が変化したことによるイナーシャの変化や、他のアーム部の位置が変化したことによる干渉トルクの変化、あるいは、モデル化の誤差などにより、モデル化時に想定した作動と実際の作動とが異なることがある。具体的には、ある速度においてモータに与えられているトルクの現在値が、当該速度においてモータに与えることが可能なトルクの許容値よりも小さくなり、トルクに余力（余剰トルク）が生じることがある。この場合、余剰トルクを利用することによって、アーム部の移動速度の向上、すなわち、アーム部を目標位置まで移動させるのに要する作動期間を短縮することが可能になると考えられる。そして、上記したような制御系を備えている場合には、トルク指令値を補正することにより、アーム部の移動速度を向上させることができると考えられる。

しかしながら、上記したようなフィードバック制御を行う場合、アーム部の位置情報は、上位側の制御ブロック例えば位置指令値から速度指令値を算出する位置制御ブロックや速度指令値からトルク指令値を算出する速度制御ブロックなどにもフィードバックされる。そのため、トルク指令値に対して補正を行うと、その補正が即座にトルクに反映させることができるという効果はあるものの、位置情報がフィードバックされた上位側の制御ブロックにおいてその補正を打ち消す制御が行われてしまい、結果としてトルクを補正した効果が低減されるという問題がある。この場合、アーム部の移動に追従させるために各制御周期においてその都度複雑な運動方程式を用いて補正値を算出すると、演算処理に時間が掛かかり応答速度の低下を招くおそれもある。かといって、上位側の例えば位置指令値に対して補正を行ったとしても、上位側の制御ブロックの帯域（制御周期）は下位側の制御ブロックの帯域よりも相対的に低い（制御周期が遅い）ことから、位置指令値に対する補正がトルクに反映されるまでの応答時間が長くなる。このため、アーム部の移動に随時追従させるような制御を行うことは困難であり、やはり補正の効果が低減してしまうという問題がある。

特開２０１１−２１２８２３号公報

本発明は上記事情に鑑みてなされたものであり、その目的は、応答速度の低下を招くことなく、また、補正の効果が低減されることなく、余剰トルクを利用してアーム部の作動期間の短縮を図ることができるロボットの制御方法、およびロボットの制御装置を提供することにある。

請求項１記載のロボットの制御方法の発明および請求項５記載のロボットの制御装置の発明では、まず、アーム部が目標位置まで移動するのに要する作動期間を制御周期ごとに分割し、各制御周期におけるモータの速度および当該速度においてモータに許容されるトルクの許容値を予め推定し、推定した速度と当該速度におけるトルクの許容値との関係を示すマップを生成する。つまり、ある速度におけるトルクの許容値を、予めマップとして記憶しておく。これにより、運動方程式を解くなどの演算処理を非値用とすることなく各制御周期においてトルクの許容値を即座に取得することが可能となる。

そして、モータを実際に作動させた際には、制御周期ごとにモータの速度の現在値およびトルクの現在値を算出し、算出したトルクの現在値とマップにて示されるトルクの許容値との比率を示す余剰トルクを算出する。余剰トルクは、例えば余剰トルク＝（トルクの許容値）÷（トルクの現在値）％のように算出される。つまり、余剰トルクは、複雑な演算処理を必要とすることなく算出することができる。

続いて、算出した余剰トルクを消費する方向にトルク指令値を補正するためのトルク指令補正値を算出する。例えば余剰トルクがＸ％（上記の例ではＸ＞１００％になる）であれば、トルクの現在値をＸ％にするための値がトルク指令補正値として算出される。具体的には、トルク指令補正値＝（（Ｘ−１００）÷１００×トルク指令値）のように算出される。つまり、トルク指令補正値を算出する場合にも、複雑な演算処理を行う必要はない。そのため、各制御周期においてトルク指令補正値を算出したとしても、応答速度の低下を招くことがない。

また、このとき、算出したトルク指令補正値に基づいて速度指令値を補正するための速度指令補正値を算出し、その速度指令補正値に基づいて位置指令値を補正するための位置指令補正値を算出する。つまり、速度指令補正値および位置指令補正値は、トルク補正指令値に対応した値として、すなわち、トルク指令値による補正を打ち消さない値として算出される。
そして、トルク指令補正値によりトルク指令値を補正する。これにより、補正による効果を即座にトルクに反映させることができ、相対的に短い制御周期にてトルクの増加すなわちアーム部の速度を向上させることができる。つまり、アーム部の移動に追従させた迅速な制御が可能となる。

さらに、トルク指令値を補正する際には、速度指令補正値による速度指令値の補正、および位置指令補正値による位置指令値の補正も併せて行う。これは、トルク指令補正値によりトルクだけを補正した場合には、上位側の速度指令値あるいはさらに上位側の位置指令値により、トルクに対して行った補正を打ち消す補正がなされる可能性があるためである。そこで、速度指令値および位置指令値に対しても補正を行うことにより、トルク指令補正値による補正の効果が上位の制御ブロックにより打ち消されることを防止することができる。

したがって、応答速度の低下を招くことなく、また、補正の効果が低減されることなく、余剰トルクを利用してアーム部の速度を向上させることができ、作動期間の短縮を図ることができる。この場合、余剰トルクは、各制御周期における推定値に基づいて算出していることから、モータを駆動することができる値、すなわち、実際に機械的に作動可能な値として算出することができる。また、動作を繰り返し実行するというロボットの特性上、上記したように１回の動作における作動期間の短縮の効果が積み重ねられることで、生産効率を向上させることができる。

請求項２記載のロボットの制御方法の発明および請求項６記載のロボットの制御装置の発明では、算出した各補正値による補正を、アーム部の作動期間のうちアーム部を加速する期間（以下、便宜的に加速期間と称する）に行う。一般的に、モータは、速度が小さいほどトルクを大きくとることができる一方、速度が大きくなるほどトルクが小さくなる特性を有している。そのため、モータの速度が相対的に小さいときには、トルクの許容値が大きくなっていると想定される。そこで、加速期間に補正を行うことにより、大きな余剰トルクを利用できる可能性が高まる。したがって、アーム部の速度の向上つまり作業効率の改善をより一層図ることができる。

請求項３記載のロボットの制御方法の発明および請求項７記載のロボットの制御装置の発明では、アーム部の作動期間において当該アーム部の減速を開始させた後の期間（以下、便宜的に減速期間と称する）には各補正値による補正を行わない。上記した余剰トルクは減速期間にも生じると予想され、補正を行うことによりアーム部を急激に減速することが可能であると考えられる。しかし、上記したように位置指令値に対して補正を行うと、目標位置がずれてしまう。また、急激に減速した場合には、目標位置に達したアーム部に振動（いわゆるアンダーシュート）が生じ、結果として、停止するまでに要する時間つまりはアーム部の作動期間が反って長くなってしまうおそれがある。そこで、減速期間には補正を行わないことで、アーム部に振動が生じるおそれを低減しつつ、アーム部の作動期間が不用意に長くなることを防止している。

請求項４記載のロボットの制御方法の発明および請求項８記載のロボットの制御装置の発明では、トルクの補正を行った際、当該補正により変化したアーム部の移動量に基づいて当該アーム部を目標位置まで移動させるのに要する期間である補正後作動期間を算出し、アーム部の作動期間を、位置指令値に基づいて設定される初期の作動期間から補正後作動期間に変更する。例えば、作動期間として加速期間、アーム部を等速で移動させる期間（以下、便宜的に等速期間と称する）、および減速期間が設定されていると想定する。また、アーム部は、時刻ｔ０で停止しており、その状態から時刻ｔ１まで加速され、時刻ｔ１からｔ２までは等速で移動し、時刻ｔ２で減速が開始され、時刻ｔ３で目標位置に停止すると想定する（後述する図１０参照）。この場合、ｔ０−ｔ１間が加速期間となり、ｔ１−ｔ２間が等速期間となり、ｔ２−ｔ３間が減速期間となる。

さて、例えば加速期間に補正値による補正を行った場合、補正後の加速期間は、補正前の加速期間よりも短くなる。つまり、余剰トルクを利用したことでモータの速度が大きくなった結果、補正前の時刻ｔ１よりも前の時刻ｔ１１に加速期間が終了する。この場合、作動期間を変更しなければ、アーム部は、補正前の等速期間の終了時刻ｔ２まで等速での移動が継続された後、補正前の減速期間の終了時刻ｔ３まで減速されることになる。その結果、補正を行った場合のアーム部の移動量（以下、便宜的に補正後移動量と称する）は、補正前の移動量すなわち目標位置までの移動量（以下、目標移動量と称する）よりも大きくなってしまう。つまり、補正前の作動期間をそのまま用いて制御を行うと、目標位置がずれてしまう。
そこで、例えば等速期間を短くするなど、補正によるモータの速度の変化量に応じて作動期間を変更することにより、補正後移動量を目標移動量に一致させることが可能となる。これにより、作動期間の短縮を図ることができるとともに、アーム部を目標位置にて正しく停止させることができる。

第１実施形態によるロボットの制御方法を適用したロボットの構成を模式的に示す図 コントローラの電気的構成を示す図 コントローラの制御ブロックを模式的に示す図 コントローラの制御ブロックを簡略化した状態を模式的に示す図 コントローラによる補正の態様を模式的に示す図 指令値、許容値、現在値の関係を模式的に示す図 制御処理の流れを示す図 マップ生成処理の流れを示す図 マップと現在値との関係を模式的に示す図 第２実施形態による補正結果の一例を模式的に示す図

以下、本発明のロボットの制御方法およびロボットの制御装置の複数の実施形態について図面を参照しながら説明する。なお、各実施形態において実質的に共通する構成については同一の符号を付し、詳細な説明は省略する。
（第１実施形態）
以下、第１実施形態によるロボットの制御方法およびロボットの制御装置ついて、図１から図９を参照しながら説明する。

図１に示すように、本実施形態では、ロボットの制御装置（後述するコントローラ１１）により制御されるロボット１として、垂直多関節型（六軸）ロボットを想定している。このロボット１は、ベース（回転軸）２上に、この場合六軸のアームを有している。具体的には、ベース２上には、第一関節Ｊ１を介してショルダ部３が回転可能に連結されている。このショルダ部３には、第二関節Ｊ２を介して上方に延びる下アーム４の下端部が回転可能に連結され、さらに、この下アーム４の先端部には、第三関節Ｊ３を介して第一上アーム５が回転可能に連結されている。第一上アーム５の先端には第四関節Ｊ４を介して第二上アーム６が回転可能に連結され、この第二上アーム６の先端には第五関節Ｊ５を介して手首７が回転可能に連結され、この手首７には第六関節Ｊ６を介してフランジ８が回転可能に連結されている。
各関節Ｊ１〜Ｊ６では、後述するモータ２５（図２参照）により、ショルダ部３〜フランジ８が回転駆動される。これらショルダ部３〜手首７は、特許請求の範囲に記載したアーム部に相当する。このロボット１は、コントローラ１１との間が接続ケーブル１２によって接続されている。

コントローラ１１は、図２に示すように、ＣＰＵ２０、ＲＯＭ２１、ＲＡＭ２２、駆動回路２３、位置検出回路２４等を有している。ＣＰＵ２０は、いわゆるマイクロコンピュータであり、ＲＯＭ２１などに記憶されている制御プログラムに従って、ロボット１つまりはショルダ部３〜手首７などの各アームを制御する。ＣＰＵ２０は、特許請求の範囲に記載したマップ生成手段、余剰トルク算出手段、補正値算出手段、制御手段を構成している。駆動回路２３は、ＣＰＵ２０からの指令信号に基づいて、モータ２５を駆動する電気信号を出力する。本実施形態では、モータ２５としてサーボモータを採用している。なお、図２では１つのモータ２５のみを図示しているが、モータ２５は上記した各軸に設けられている。また、モータ２５は、各アームに対応した特性を有している。

位置検出回路２４は、各アームの位置を検出するためのものであり、各軸を駆動するそれぞれのモータ２５に設けられているロータリエンコーダ２６に接続している。位置検出回路２４は、ロータリエンコーダ２６から入力される検出信号に基づいて各軸に設けられているモータ２５の回転角度を検出し、検出した回転角度を示す位置検出情報をＣＰＵ２０に出力する。

このコントローラ１１は、上位制御装置１３に接続している。上位制御装置１３は、ロボットの各アームを目標位置まで移動させるための位置指令値をコントローラ１１に対して与える。
コントローラ１１は、以下に説明するように、上位制御装置１３から与えられた位置指令値に基づいてモータ２５への指令信号（後述するトルク指令値など）を算出するとともに、位置検出回路２４から入力される位置検出情報に基づいてフィードバック制御を行っている。

ここで、コントローラ１１に設けられている制御系およびその制御系により行われる演算処理について詳細に説明する。
コントローラ１１は、図３に示すように、位置制御ブロック３０（図３では位置制御系と示す）、速度制御ブロック３１（図３では速度制御系と示す）、電流制御ブロック３２（図３では電流制御系と示す）、および差分制御ブロック３３を有している。本実施形態の場合、各制御ブロックは、ＣＰＵ２０により実行されるプログラムによってソフトウェア的に実現されている。なお、図３に示す制御系はロボット制御において一般的に採用されているものであることから、各制御ブロックで使用される関数などの詳細については説明を省略し、ここでは演算処理の基本的な流れを説明する。

位置制御ブロック３０は、上位制御装置１３から与えられた位置指令値（Ｐｒｅｆ）に基づいて速度指令値（Ｖｒｅｆ）を算出する。具体的には、位置制御ブロック３０は、予め定められている関数Ｃｐ（ｓ）により、速度指令値を、Ｖｒｅｆ＝Ｃｐ（ｓ）・Ｐｒｅｆとして算出する。速度制御ブロック３１は、位置制御ブロック３０で算出した速度指令値に基づいてトルク指令値（Ｔｒｅｆ）を算出する。具体的には、速度制御ブロック３１は、トルク指令値を、Ｔｒｅｆ＝Ｃｖ（ｓ）・Ｖｒｅｆとして算出する。電流制御ブロック３２は、速度制御ブロック３１で算出したトルク指令値に基づいてトルク（Ｔ）を算出する。具体的には、電流制御ブロック３２は、トルクを、Ｔ＝Ｃｅ（ｓ）・Ｔｒｅｆとして算出する。そして、コントローラ１１は、算出したトルクを駆動回路２３において電流信号に変換してモータ２５に与えることで、モータ２５の回転速度および位置の制御を行っている。

さて、モータ２５の挙動は、関数Ｐｍ（ｓ）により表される。また、モータ２５により駆動される各アームの挙動は、関数Ｐａ（ｓ）により表される。各関数は、具体的には、モータ２５や各アームのイナーシャや粘性係数に基づいて以下の式（１）、式（２）で表される。

ただし、Ｊｍ：モータイナーシャ
Ｄｍ：モータ粘性定数

ただし、Ｊａ：アームイナーシャ
Ｄａ：アーム粘性定数
ここで、説明の簡略化のために、モータ２５および各アームの制御ブロックを、図４に示すように一つのモータ系ブロック３４として扱い、モータ系ブロック３４の作動を関数Ｐｍａ（ｓ）により表すこととする。この場合、フィードバック制御に用いられるモータ位置（Ｐｆｂ）は、以下の式（３）として表される。

この関数Ｐｍａ（ｓ）は、上記した式（１）、式（２）に基づいて次のように導出される。ここで、図３に示すように、ばね定数をＫｓ、ギア比をＫｇとした場合において、反力がｘ、アーム位置がｙになった状態を想定する。このとき、モータ位置（Ｐｆｂ）、反力（ｘ）、アーム位置（ｙ）は、トルクをＴとすると、それぞれ以下の式（４）、式（５）、式（６）により表される。

これらの式（４）〜（６）から、モータ位置は、以下の式（７）のように表すことができることが分かる。

そして、この式（７）および上記した式（３）から、モータ系ブロック３４の関数Ｐｍａ（ｓ）を以下の式（８）のように導出することができる。

この関数Ｐｍａ（ｓ）で算出されるモータ位置は、図４に示すように、位置指令値に対してフィードバックされるとともに、差分制御ブロック３３においてモータ位置の差分すなわち速度現在値が算出された後、速度指令値に対してフィードバックされる。なお、本実施形態では離散系を採用していることから差分制御ブロック３３においてモータ位置の差分で速度現在値を算出しているが、連続系を採用した場合にはモータ位置を微分すればよい。

さて、前述のように、イナーシャの変化や干渉トルクの変化、あるいは摩擦係数の誤差などにより、モデル化時に想定した作動と実際の作動とが異なることがある。具体的には、例えば図５に示すように、指令値に対して機械的に作動可能な値として設定されているトルクの制限（トルクの許容値）よりも実際に作動させたときのトルクの現在値が小さくなり、実際にはトルクに余力（余剰トルク）が生じることがある。

そこで、本実施形態のコントローラ１１は、図６に示すように、マップ生成ブロック３５（図６では、マップ生成系と示す）、および補正値算出ブロック３６（図６では、補正値算出系と示す）をさらに備えることで、余剰トルクを利用可能としている。このマップ生成ブロック３５は、後述するように余剰トルクを算出するための基準値となるマップを生成する。一方、補正値算出ブロック３６は、後述するように、マップとトルクの現在値とに基づいて、余剰トルクの算出および補正値の算出を行う。

次に、上記したコントローラ１１による作用について、マップの生成手順および補正値の算出手順とともに説明する。
コントローラ１１は、図７に示す制御処理を実行している。コントローラ１１は、位置指令値が与えられると、マップ生成処理を実行する（Ｓ１）。このマップ生成処理では、コントローラ１１は、図８に示すように、まず位置指令値により決定される作動期間（Ｔ_ｅｎｄ）を、マップ生成のために予め設定されているサンプリング周期（Ｔ_ｓａｍｐｌｅ（すなわち、電流制御ブロック３２の制御周期）でＮに分割する。続いて、変数ｎを１に初期化した後（Ｓ１０２）、各制御周期におけるトルクの推定値（Ｔｎ）および速度の推定値（Ｖｎ）を以下のように算出する。
まず、トルクおよび速度の演算式について説明する。トルクは、上記した制御系の場合、以下の式（９）にて演算することができる。

この場合、本実施形態では離散系の制御系を採用していることから、ｓは、以下の式（１０）で表される。なお、離散化の手法は他にもあり、式（１０）は一例である。

ただし、Ｔｓａｍｐ：サンプリング期間の逆数とする。
この式（１０）を式（９）に代入すると、以下の式（１１）が導かれる。

ここで、式（１１）の項のうち定数をＡ、Ｂ、Ｃで置き換えると、式（１１）は、以下の式（１２）のように表される。

この式（１２）に基づいて、ｎサンプル目のトルクは、以下の式（１３）にて算出される。

ただし、Ｔｎ−１：１サンプル前のトルク
Ｐｒｅｆ，ｎ−１：１サンプル前の位置指令値
Ｐｒｅｆ，ｎ：現サンプル（ｎサンプル目）の位置指令値
とする。
また、速度についても同様に、以下の式（１４）により演算することができる。

そして、トルクと同様に、ｓを式（１２）のように表すことで、以下の式（１５）が導かれる。

この式（１５）の項のうち定数をＤ、Ｅ、Ｆで置き換えると、式（１５）は、以下の式（１６）のように表される。

そして、この式（１６）に基づいて、ｎサンプル目の速度現在値（Ｖｆｂ）は、以下の式（１７）にて算出される。

ただし、Ｖｆｂ，ｎ−１：１サンプル前の速度現在値
Ｐｒｅｆ，ｎ−１：１サンプル前の位置指令値
Ｐｒｅｆ，ｎ：現サンプル（ｎサンプル目）の位置指令値
とする。

このように、位置指令値に基づいて、ｎサンプル目のトルク（Ｔｎ）およびｎサンプル目の速度現在値（Ｖｆｂ，ｎ）を推定することができる。
コントローラ１１は、ｎ＜Ｎ＋１の場合には（Ｓ１０３）、ｎ＝１であるかを判定し（Ｓ１０４）、ｎ＝１の場合には（Ｓ１０４：ＹＥＳ）、Ｐｒｅｆ，ｎ−１＝０、Ｖｆｂ，ｎ−１＝０、Ｔｎ−１＝０とする（Ｓ１０５）。続いて、式（１３）および式（１７）を用いてトルクの現在値（Ｔｎ）と速度現在値（Ｖｎ）とを算出する（Ｓ１０６）。この場合、ステップＳ５において初期値が０として設定されているので、それに基づいてトルクの現在値と速度現在値とが算出される。そして、次サンプルのトルクの現在値（Ｔｎ）と速度現在値（Ｖｎ）と算出するために、ｎをインクリメントする（Ｓ１０７）。

これに対して、コントローラ１１は、ｎ＝１でなければ（Ｓ１０４：ＮＯ）、ステップＳ１０６に移行して、（ｎ−１）サンプル目の値から、トルクの現在値（Ｔｎ）と速度現在値（Ｖｎ）とを算出する。そして、ステップＳ１０７においてｎをインクリメントすることで、ｎがＮ＋１に達するまで、換言すると、各サンプル（各制御周期）におけるトルクの現在値（Ｔｎ）と速度現在値（Ｖｎ）とをすべて算出するまで、上記した処理を繰り返す。そして、ｎがＮ＋１に達すると（Ｓ１０３：ＹＥＳ）、つまり、各サンプルにおける全ての推定値を算出すると、速度とトルクとの関係を示すマップを生成し（Ｓ１０８）、モータ極体数およびトルク定数を保存して（Ｓ１０９）、リターンする。これにより、図９に破線にて示すマップ、すなわち、モータ限界値よりも小さく実際にモータ２５およびアームを動作させることが可能なトルクの許容値が算出される。

マップ生成処理からリターンすると、コントローラ１１は、図７に示す制御処理において、ロボット１の動作を開始する（Ｓ２）。そして、速度現在値、位置現在値、モータ２５に与えられているＵ相、Ｖ相の電流値を計測し（Ｓ３）、計測した電流値に基づいていわゆるｄｑ変換により、Ｉｑ電流を算出する（Ｓ４）。そして、実トルク（トルクの現在値）を、実トルク＝Ｉｑ×トルク定数として算出し（Ｓ５）、先に生成したマップから余剰トルクＸを算出する（Ｓ６）。本実施形態では、余剰トルクは、算出した速度現在値におけるマップから求められる許容値（図９では、トルク許容値と示す）と、実トルク（図９ではトルク許容値と示す）とから、余剰トルク＝トルク許容値÷トルク現在値×１００（％）として算出している。

余剰トルクを算出すると、コントローラ１１は、余剰トルクが１００％を超えている場合、すなわち、トルク許容値がトルク現在値よりも大きい場合には、トルク指令補正値（Ｔｏｆｆｓｅｔ）を、トルク指令補正値＝（Ｘ−１００）÷１００×トルク指令値として算出する（Ｓ７）。つまり、トルク指令補正値は、余剰トルクを消費するための値として、より厳密には、余剰トルク分だけトルク指令値を増加させるための値として算出される。続いて、コントローラ１１は、算出したトルク指令補正値を速度制御ブロック３１の関数Ｃｖ（ｓ）の逆関数にて演算することで、以下の式（１８）のように速度指令補正値（Ｖｏｆｆｓｅｔ）を算出する（Ｓ８）。

また、コントローラ１１は、算出した速度指令補正値を位置制御ブロック３０の関数Ｃｐ（ｓ）の逆関数にて演算することで、以下の式（１９）のように位置指令補正値（Ｐｏｆｆｓｅｔ）を算出する（Ｓ９）。

これら速度指令補正値（Ｖｏｆｆｓｅｔ）および位置指令補正値（Ｐｏｆｆｓｅｔ）は、いずれもトルク指令補正値（Ｔｏｆｆｓｅｔ）に基づいて算出されており、トルク指令補正値による補正を「真」とするための補正値、換言すると、トルク指令補正値による補正を打ち消さないようにするために位置指令値および速度指令値に対して与えられる補正値である。

コントローラ１１は、各補正値を算出すると、各指令値を補正する（Ｓ１０）。具体的には、コントローラ１１は、トルク指令値（Ｔｒｅｆ）、速度指令値（Ｖｒｅｆ）、および位置指令値（Ｐｒｅｆ）を、図６に示すように、
・Ｔｒｅｆ＝Ｔｒｅｆ＋位置指令補正値
・Ｖｒｅｆ＝Ｖｒｅｆ＋位置指令補正値
・Ｐｒｅｆ＝Ｐｒｅｆ＋位置指令補正値
として補正する。

そして、コントローラ１１は、作動が終了していない場合には（Ｓ１１：ＮＯ）、ステップＳ３に移行して上記した補正を繰り返し実行する。これに対して、コントローラ１１は、作動が終了すると（Ｓ１１：ＹＥＳ）、つまり、アームが目標位置まで到達すると、制御処理を終了する。
このように、コントローラ１１は、トルク指令値を補正する補正値を算出し、その補正値に基づいて上位側の制御ブロックに対しても補正を行っている。

以上説明した本実施形態によれば、次のような効果を奏する。
本実施形態のロボットの制御方法によれば、まず、アームが目標位置まで移動するのに要する作動期間を予め設定されている制御周期（サンプリング周期）ごとに分割し、各制御周期におけるモータ２５の速度および当該速度においてモータ２５に許容されるトルクの許容値を推定し、推定した速度と当該速度におけるトルクの許容値との関係を示すマップを予め生成している。これにより、実際にモータ２５を駆動した際には、各制御周期において、運動方程式を解くなどの演算処理を必要とすることがない。また、モータ２５を実際に作動させた際には、制御周期ごとにモータ２５の速度の現在値を取得してトルクの現在値を算出し、算出したトルクの現在値とマップにて示されるトルクの許容値との比率を示す余剰トルクを算出する。そして、算出した余剰トルクを消費する方向にトルク指令値を補正するためのトルク指令補正値を算出する。この場合、トルクの許容値はマップから即座に取得できる上、トルク指令補正値は四則演算での算出することができる。したがって、トルクの許容値や余剰トルクの算出に要する処理時間を短くすることができ、応答速度の低下を招くことがない。

このとき、算出したトルク指令補正値に基づいて速度指令値を補正するための速度指令補正値を算出し、算出した速度指令補正値に基づいて位置指令値を補正するための位置指令補正値を算出するので、各補正値は、トルク補正指令値に対応した値、換言すると、トルク指令値の増加分に対応する値が算出される。そして、トルク指令補正値によるトルク指令値の補正を行う際には、速度指令補正値による速度指令値の補正、および位置指令補正値による位置指令値の補正も併せて行われる。これにより、トルク指令値に対して補正を行うことで補正した結果を即座にトルクに反映させることができ、補正を行わない場合に比べて短い制御周期にてトルクの増加すなわちアームの速度の向上を図ることができる。

また、位置指令値および速度指令値に対してもトルク指令値の増加分に対応した補正を行っているため、トルク指令値に対する補正の効果が上位側の制御ブロックから打ち消されることがない。したがって、応答速度の低下を招くことなく、また、補正の効果が低減されることなく、余剰トルクを利用してモータ２５の速度を向上させること、すなわち、アームの作動期間の短縮を図ることができる。
この場合、余剰トルクは、各制御周期における推定値に基づいて算出していることから、モータ２５を駆動することができる値として算出される。そのため、余剰トルク分だけモータ２５の速度を向上させた場合であっても、実際に機械的に作動させることができる。

このような制御方法を適用することにより、アームの作動期間を短縮することが可能なロボットの制御装置を提供でき、動作を繰り返し実行するというロボットの特性上、１回の動作における作動期間の短縮量が積み重ねられ、作業期間の短縮および生産効率の向上を図ることができる。

（第２実施形態）
以下、第２実施形態によるロボットの制御方法およびロボットの制御装置ついて、図１０を参照しながら説明する。なお、ロボットおよびコントローラの構成は第１実施形態と共通するので、図１から図９などをも参照しながら説明する。
第１実施形態では図７に示すように作動を終了するまで補正を行ったが、補正を行う期間を制限してもよい。具体的には、コントローラ１１は、アームの作動期間に例えば加速期間、等速期間および減速期間が含まれている場合、加速期間に補正を行うとよい。一般的に、モータ２５は、速度が小さいほどトルクを大きくとることができる一方、速度が大きくなるほどトルクが小さくなる特性を有している。そのため、例えば停止していたアームの移動を開始する加速期間においては、トルクの許容値も大きくなることが想定される。そのため、加速期間に補正を行うことで、より大きな余剰トルクの利用が可能となり、モータ２５の速度をより大きくすること、つまり、アームをより高速に移動させることができる。

一方、アームの作動期間のうち減速期間には補正を行わないようにしてもよい。減速期間にも余剰トルクは生じると考えられるものの、減速期間に補正を行うと、位置指令値に対しても補正が行われる。その場合、目標位置がずれてしまう、つまり、アームが目標位置を通り過ぎてしまうおそれがある。また、減速期間に余剰トルクを利用すると、急激な減速が行われることでアームに振動が生じ、結果として、目標位置で停止するまでの時間が長くなるおそれもある。また、減速期間はある速度で移動しているアームを目標位置にて停止させるために設けられている期間であり、補正前の減速期間は、その速度のアームを停止させるためのモデル的な期間であるとも言える。そこで、減速期間には補正を行わないことで、アームを目標位置にスムーズに停止させることが可能となる。

そして、何れかの期間に補正を行った場合には、作動期間をモータ２５の速度の変化量に応じて変更する。例えば図１０（Ａ）に示すように、アームの作動期間（ｔ０−ｔ３の期間）にアームを加速する加速期間（ｔ０−ｔ１の期間）、アームを等速で移動させる等速期間（ｔ１−ｔ２の期間）、およびアームを減速する減速期間（ｔ２−ｔ３の期間）が設定されている場合を想定する。この場合、補正前の指令値においては、アームは、時刻ｔ０で停止しており、その状態から時刻ｔ１まで加速され、時刻ｔ１からｔ２までは等速で移動し、時刻ｔ２で減速が開始され、時刻ｔ３で目標位置に停止することになる。

この場合、加速期間に補正を行うことでモータ２５の速度を向上させたとすると、補正後の加速期間（ｔ０−ｔ１１の期間）は、補正前の加速期間（ｔ０−ｔ１の期間）よりも短くなる。つまり、余剰トルクを利用したことでモータ２５の速度の変化量（つまり加速度）が大きくなった結果、アームは、時刻ｔ１よりも前の時刻ｔ１１に加速が終了する。このため、作動期間を変更しないと、アームは、補正前の等速期間の終了時刻ｔ２まで等速での移動が継続された後、補正前の減速期間の終了時刻ｔ３まで減速が行われることになる。つまり、アームの移動量（図１０（Ａ）に示す補正前のグラフにおける台形部分の面積に相当する）は、補正後の移動量（補正後のグラフにおける台形部分の面積に相当する）ほうが大きくなる。つまり、アームが目標位置を超えて移動することになり、目標位置がずれてしまう。

そこで、図１０（Ａ）の場合には、例えば補正後の等速期間（ｔ１１−ｔ１２の期間）を補正前の等速期間（ｔ１−ｔ２の期間）よりも短くすることで、補正後のアームの移動量（補正後移動量）を、補正前の移動量（目標移動量）に一致させることができる。
このように、補正を行った際には、補正により変化したアームの移動量に基づいて当該アームを位置指令値により設定された目標位置まで移動させるのに要する期間である補正後作動期間を算出し、アームの作動期間を位置指令値に基づいて設定された初期の作動期間から補正後作動期間に変更することで、アームの作動期間を短縮することができるとともに、アームを目標位置にて正しく停止させることができる。なお、減速時に補正を行う場合には減速期間も短くなるので、それも考慮して等速期間を短くすればよい。

また、減速期間には補正を行わない（つまり、加速期間にのみ補正を行う）場合には、減速の割合を補正前と補正後とで変更しないようにしてもよい。例えば図１０（Ｂ）、（Ｃ）に示すように補正前の作動期間に加速期間（ｔ０−ｔ２１の期間、ｔ０−ｔ４１の期間）および減速期間（ｔ２１−ｔ２２の期間、ｔ４１−ｔ４２の期間）が設定されていると想定した場合、減速の割合を変更しないというのは、補正前の減速期間の傾きと、補正後の減速期間（ｔ３２−ｔ３３の期間、ｔ５１−５２の期間）の傾きとを一致させることに等しい。

この場合、例えば加速期間にのみ補正を行うことでモータ２５の最高速度が補正前よりも高くなったとしても、補正前の減速期間の傾き、すなわち、モータ２５を停止するためのモデル的な傾きにて減速させることにより、アームに振動が生じるおそれを低減しつつ、アームの作動期間が不用意に長くなることを防止することができる。

つまり、加速期間に補正を行った場合、減速期間に補正を行わないとするとアームが目標位置を超えてしまうことから、減速期間に補正を行わない場合には、減速期間に入る前の期間すなわちアームの作動期間のうち当該アームの加速が開始されてから減速が開始されるまでの期間（減速前期間）を各補正値による補正結果に応じて変更する。これにより、減速期間の傾きを変えることなく、つまり、アームに振動等を与えるおそれを抑制しつつ、作動期間を短縮することができる。

（その他の実施形態）
本発明は、各実施形態にて例示したものに限定されることなく、例えば以下のように変形あるいは拡張することができる。
第１実施形態では離散系の制御系を想定しているが、基本的な処理は連続系においても共通する。つまり、本発明は、連続系の制御であっても適用することができる。
作動期間のうち補正を行う期間は、任意に設定すればよい。第２実施形態のように加速期間にのみ補正を行うようにしてもよいし、作動期間が長くなることなくアームを停止可能であれば減速期間に行ってもよい。また、双方の期間において補正を行ってもよい。

補正を行う順序を、位置指令値に対する補正を行った後に速度指令値を補正し、その後トルク補正値を補正する順序としてもよい。トルクに対する補正を行った場合、前述のように上位側の制御ブロックから打ち消される可能性があるものの、下位側の制御ブロックを打ち消す可能性のある上位側の制御ブロックから順に補正を行うことで、下位側の補正を許容する補正が先になされ、下位側の制御ブロックに対する補正が打ち消される可能性を極力排除することができる。

図面中、１はロボット、３はショルダ部（アーム部）、４は下アーム（アーム部）、５は第一上アーム（アーム部）、６は第二上アーム（アーム部）、７は手首（アーム部）、１１はコントローラ（ロボットの制御装置）、２０はＣＰＵ（マップ生成手段、余剰トルク算出手段、補正値算出手段、制御手段）、２５はモータを示す。

Claims (8)

1. モータにより駆動されるアーム部を有するロボットにおいて前記アーム部を外部から与えられた位置指令値によって指令された目標位置まで移動させる際、前記位置指令値から速度指令値およびトルク指令値を算出し、算出したトルク指令値に基づいて前記モータを駆動するとともに駆動した結果をフィードバックして制御を行うロボットの制御方法であって、
   前記アーム部が前記目標位置まで移動するのに要する作動期間を予め設定されている制御周期ごとに分割し、各制御周期における前記モータの速度および当該速度において前記モータに許容されるトルクの許容値を推定し、推定した速度と当該速度におけるトルクの許容値との関係を示すマップを予め生成し、
   前記位置指令値に基づいて前記モータを実際に作動させた際、各制御周期において、前記モータの速度の現在値およびトルクの現在値を算出し、算出したトルクの現在値と前記マップにて示される当該速度におけるトルクの許容値との比率を示す余剰トルクを算出し、
   算出した余剰トルクを消費する方向に前記トルク指令値を補正するためのトルク指令補
   正値を算出するとともに、算出したトルク指令補正値から前記速度指令値を補正するため
   の速度指令補正値を算出し、さらに、算出した前記速度指令補正値から前記位置指令値を
   補正するための位置指令補正値を算出し、
   算出した前記トルク指令補正値により前記トルク指令値の補正を行う際に、前記速度指
   令補正値による前記速度指令値の補正、および前記位置指令補正値による前記位置指令値
   の補正を併せて行うことを特徴とするロボットの制御方法。
   
2. 算出した各補正値による補正を、前記アーム部の前記作動期間のうち当該アーム部を加速する期間に行うことを特徴とする請求項１記載のロボットの制御方法。
3. 前記位置指令値により決定される前記アーム部の前記作動期間には、当該アーム部を加速する期間および減速する期間が少なくとも含まれており、
   前記アーム部の減速を開始させた後の期間には各補正値による補正を行わないことを特徴とする請求項１または２記載のロボットの制御方法。
4. トルクの補正を行った際、当該補正により変化した前記アーム部の移動量に基づいて当該アーム部を前記目標位置まで移動させるのに要する期間である補正後作動期間を算出し、前記アーム部の作動期間を、前記位置指令値に基づいて設定された初期の作動期間から前記補正後作動期間に変更することを特徴とする請求項１から３のいずれか一項記載のロボットの制御方法。
5. モータにより駆動されるアーム部を有するロボットにおいて前記アーム部を外部から与えられた位置指令値によって指令された目標位置まで移動させる際、前記位置指令値から速度指令値およびトルク指令値を算出し、算出したトルク指令値に基づいて前記モータを駆動するとともに駆動した結果をフィードバックして制御を行うロボットの制御装置であって、
   前記アーム部が前記目標位置まで移動するのに要する作動期間を予め設定されている制御周期ごとに分割し、各制御周期における前記モータの速度および当該速度において前記モータに許容されるトルクの許容値を推定し、推定した速度と当該速度におけるトルクの許容値との関係を示すマップを予め生成するマップ生成手段と、
   前記位置指令値に基づいて前記モータを実際に作動させた際、各制御周期において、前記モータの速度の現在値およびトルクの現在値を算出し、算出したトルクの現在値と前記マップにて示される当該速度におけるトルクの許容値との比率を示す余剰トルクを算出する余剰トルク算出手段と、
   算出した余剰トルクを消費する方向に前記トルク指令値を補正するためのトルク指令補正値を算出するとともに、算出したトルク指令補正値から前記速度指令値を補正するための速度指令補正値を算出し、さらに、算出した前記速度指令補正値から前記位置指令値を補正するための位置指令補正値を算出する補正値算出手段と、
   算出した前記トルク指令補正値により前記トルク指令値の補正を行う際に、前記速度指令補正値による前記速度指令値の補正、および前記位置指令補正値による前記位置指令値の補正を併せて行う制御手段と、
   ことを特徴とするロボットの制御装置。
   
6. 前記制御手段は、算出した各補正値による補正を、前記アーム部の前記作動期間のうち当該アーム部を加速する期間に行うことを特徴とする請求項５記載のロボットの制御装置。
7. 前記位置指令値により決定される前記アーム部の前記作動期間には、当該アーム部を加速する期間および減速する期間が少なくとも含まれており、
   前記制御手段は、前記アーム部の減速を開始させた後の期間には各補正値による補正を行わないことを特徴とする請求項５または６記載のロボットの制御装置。
8. 前記制御手段は、トルクの補正を行った際、当該補正により変化した前記アーム部の移動量に基づいて当該アーム部を前記目標位置まで移動させるのに要する期間である補正後作動期間を算出し、前記アーム部の作動期間を、前記位置指令値に基づいて設定された初期の作動期間から前記補正後作動期間に変更することを特徴とする請求項５から７のいずれか一項記載のロボットの制御装置。

Images