JP7555764B2 - 電流制限装置、ロボットシステムおよび電流制限方法 - Google Patents

Description

この発明は、電流制限装置、ロボットシステムおよび電流制限方法に関し、特に、駆動部に通電する電流を制限する電流制限装置、ロボットシステムおよび電流制限方法に関する。

従来、モータに通電する電流の上限値が設定されたロボットが知られている（たとえば、特許文献１参照）。

上記特許文献１には、複数のリンクが関節を介して接続されているロボット本体と、ロボット本体の関節に設けられるモータと、モータの回転を減速する減速部とを備えるロボットが開示されている。このロボットには、モータに通電している電流を計測する電流計測手段が設けられている。そして、ロボット本体の静止時に、電流計測手段によって計測された電流値に基づいて、モータに通電する電流の上限値が設定される。具体的には、ロボットの静止時に、所定の姿勢を維持するために必要なモータのトルクに応じて、電流の一定の上限値が設定される。これにより、ロボットの姿勢に応じて関節に与える駆動トルクを適切に制限することができる。

特開２００８－７３７９０号公報

上記特許文献１に記載のような、モータの回転を減速する減速部が設けられている従来のロボットでは、モータから減速部までの間には、イナーシャ（モータ自身のイナーシャ、減速部の歯車のイナーシャなど）が存在する。ここで、トルクは、イナーシャと加速度との積で表されるので、モータによって駆動されるアームの動きを加速する（加速度を大きくする）場合、イナーシャが大きければ、その分、必要なトルクが大きくなる。すなわち、モータに通電する電流を大きくしてモータが発生するトルクを大きくする必要がある。しかしながら、上記特許文献１のように、モータに通電される電流に一定の上限値が設定されている場合、電流の上限値をモータに通電してもモータが十分なトルクを発生できない場合がある。このため、モータから減速部（駆動力伝達部）までの間のイナーシャが大きい場合、モータ（駆動部）によって駆動される部材（アームなど）を十分に加速することができないという問題点がある。

この発明は、上記のような課題を解決するためになされたものであり、この発明の１つの目的は、駆動部から駆動力伝達部までの間のイナーシャが大きい場合でも、駆動部によって駆動される部材を十分に加速することが可能な電流制限装置、ロボットシステムおよび電流制限方法を提供することである。

上記目的を達成するために、この発明の第１の局面による電流制限装置は、駆動力伝達部を介して駆動力を伝達する駆動部に通電する電流を制限する電流制限装置であって、駆動部に通電する電流を電流制限値の範囲内に制限する電流制限部を備え、電流制限値は、駆動部から駆動力伝達部までのイナーシャに基づくとともに、駆動部の加速度に応じて変化するように設定されており、駆動部の駆動を妨げる向きに加わる重力に抗する側に駆動部の力またはモーメントを発生させる場合、電流制限値は、駆動部の加速度に応じて小さくなるように設定されている。ここで、「駆動部」は、モータや比例ソレノイドなどを含む概念である。また、「加速度」とは、通常の加速度のみならず角加速度も含む概念である。

この発明の第１の局面による電流制限装置では、上記のように、駆動部に通電する電流の電流制限値は、駆動部の加速度に応じて変化するように設定されている。これにより、駆動部から駆動力伝達部までの間のイナーシャが大きい場合でも、駆動部の加速度に応じて、電流制限値を変化させることにより、駆動部に十分なトルクを発生させることができる。その結果、駆動部から駆動力伝達部までの間のイナーシャが大きい場合でも、駆動部によって駆動される部材（アームなど）を十分に加速することができる。
この発明の第２の局面による電流制限装置は、電流が流されることにより力またはモーメントを発生し、駆動力伝達部を介して駆動力を伝達する駆動部に通電する電流を制限する電流制限装置であって、駆動部に通電する電流を電流制限値の範囲内に制限する電流制限部を備え、電流制限値は、駆動部の加速度に応じて変化するように設定されており、電流制限値は、駆動部に対する速度指令から算出された加速度に応じて変化するように設定されている第１電流制限値と、駆動部の速度のフィードバック値から算出された加速度に応じて変化するように設定されている第２電流制限値とを含み、第１電流制限値と第２電流制限値との差分から算出したトルク制限値の差の絶対値が所定の値未満となる場合、電流制限値として第１電流制限値が用いられ、差分が所定の値以上となる場合、電流制限値として第２電流制限値が用いられる。

この発明の第３の局面による電流制限方法は、駆動力伝達部を介して駆動力を伝達する駆動部に通電する電流を制限する電流制限方法であって、駆動部の加速度を取得するステップと、駆動部から駆動力伝達部までのイナーシャに基づくとともに、取得された駆動部の加速度に基づいて、駆動部に通電する電流の電流制限値を設定するステップと、設定された電流制限値の範囲内において、駆動部に通電するステップとを備え、駆動部の駆動を妨げる向きに加わる重力に抗する側に駆動部の力またはモーメントを発生させる場合、電流制限値は、駆動部の加速度に応じて小さくなるように設定されている。

この発明の第３の局面による電流制限方法は、上記のように、取得された駆動部の加速度に基づいて、駆動部に通電する電流の電流制限値を設定するステップを備える。これにより、駆動部から駆動力伝達部までの間のイナーシャが大きい場合でも、駆動部の加速度に応じて、電流制限値を変化させることにより、駆動部に十分なトルクを発生させることができる。その結果、駆動部から駆動力伝達部までの間のイナーシャが大きい場合でも、駆動部によって駆動される部材（アームなど）を十分に加速することが可能な電流制限方法を提供することができる。
この発明の第４の局面による電流制限方法は、電流が流されることにより力またはモーメントを発生し、駆動力伝達部を介して駆動力を伝達する駆動部に通電する電流を制限する電流制限方法であって、駆動部の加速度を取得するステップと、取得された駆動部の加速度に基づいて、駆動部に通電する電流の電流制限値を設定するステップと、設定された電流制限値の範囲内において、駆動部に通電するステップとを備え、電流制限値は、駆動部に対する速度指令から算出された加速度に応じて変化するように設定されている第１電流制限値と、駆動部の速度のフィードバック値から算出された加速度に応じて変化するように設定されている第２電流制限値とを含み、第１電流制限値と第２電流制限値との差分から算出したトルク制限値の差の絶対値が所定の値未満となる場合、電流制限値として第１電流制限値が用いられ、差分が所定の値以上となる場合、電流制限値として第２電流制限値が用いられる。

本発明によれば、上記のように、駆動部から駆動力伝達部までの間のイナーシャが大きい場合でも、駆動部によって駆動される部材を十分に加速することができる。

本発明の一実施形態によるロボットシステムの構成を示す図である。 本発明の一実施形態によるロボット制御部のブロック図である。 本発明の一実施形態によるロボット制御部の制御ブロック図である。 電流制限値の絶対値が小さくなる状態を説明するための図（１）である。 電流制限値の絶対値が小さくなる状態を説明するための図（２）である。 本発明の一実施形態による電流制限方法を説明するためのフロー図である。 変形例による医療用のロボットの構成を示す図である。 変形例によるロボット制御部の構成を示すブロック図（１）である。 変形例によるロボット制御部の構成を示すブロック図（２）である。

以下、本発明を具体化した本発明の一実施形態を図面に基づいて説明する。

図１～図６を参照して、本実施形態によるロボットシステム１００の構成について説明する。

図１に示すように、ロボットシステム１００は、ロボット１０と、ロボット１０を制御するロボット制御部２０とを備えている。ロボット１０は、たとえば、６軸ロボットである。また、ロボット１０は、たとえば、産業用のロボットである。

図１に示すように、ロボット１０は、アーム１１を含む。アーム１１は、関節１２を有する。関節１２は、複数設けられている。たとえば、関節１２（関節１２ａ～１２ｆ）は、６個設けられている。また、アーム１１は、基台１３に取り付けられている。関節１２ａは、鉛直方向に延びる軸線Ｌ１回りに回動可能に構成されている。関節１２ｂは、水平方向に延びる軸線Ｌ２回りに回動可能に構成されている。関節１２ｃは、軸線Ｌ２に対して平行に延びる軸線Ｌ３回りに回動可能に構成されている。

関節１２ｄは、軸線Ｌ３に直交する軸線Ｌ４回りに回動可能に構成されている。関節１２ｅは、軸線Ｌ４に直交する軸線Ｌ５回りに回動可能に構成されている。関節１２ｆは、軸線Ｌ５に直交する軸線Ｌ６回りに回動可能に構成されている。

図２に示すように、複数の関節１２ａ～１２ｆの各々には、モータ１４ａ～１４ｆが設けられている。また、モータ１４ａ～１４ｆの各々には、モータ１４ａ～１４ｆの各々の駆動力を伝達する減速部１５ａ～１５ｆが設けられている。減速部１５ａ～１５ｆは、各々、モータ１４ａ～１４ｆの回転を減速して伝達する。これにより、関節１２ａ～１２ｆが回動する。以下では、モータ１４ａ～１４ｆをまとめてモータ１４と記載する場合がある。また、減速部１５ａ～１５ｆをまとめて、減速部１５と記載する場合がある。なお、モータ１４（１４ａ～１４ｆ）は、特許請求の範囲の「駆動部」の一例である。また、減速部１５（１５ａ～１５ｆ）は、特許請求の範囲の「駆動力伝達部」の一例である。

モータ１４ａ～１４ｆの各々には、エンコーダ１６ａ～１６ｆが設けられている。エンコーダ１６ａ～１６ｆは、モータ１４ａ～１４ｆの各々の出力軸１７ａ～１７ｆの角度位置を検出する。検出された出力軸１７ａ～１７ｆの角度位置は、後述する位置／速度制御部２２に送信される。以下では、エンコーダ１６ａ～１６ｆをまとめてエンコーダ１６と記載する場合がある。また、出力軸１７ａ～１７ｆをまとめて、出力軸１７と記載する場合がある。

また、ロボット制御部２０には、減速部１５を介して駆動力を伝達するモータ１４に通電する電流を制限する電流制限装置２１が設けられている。電流制限装置２１は、位置／速度制御部２２と、モータ１４に通電する電流を電流制限値の範囲内に制限する電流制限部２３とを含む。

位置／速度制御部２２は、モータ１４ａ～１４ｆの各々に、電気的に接続されており、モータ１４ａ～１４ｆの各々に電流を流して、モータ１４ａ～１４ｆの各々から出力されるトルクを制御する。また、位置／速度制御部２２は、位置制御部２２ａ（図３参照）と速度制御部２２ｂ（図３参照）とを含む。また、位置／速度制御部２２には、記憶部２４が接続されている。記憶部２４には、モータ１４ａ～１４ｆの各々を駆動するためのプログラムが記憶されている。

また、電流制限部２３は、複数のモータ１４ａ～１４ｆの各々に設けられている電流制限部２３ａ～２３ｆを含む。また、電流制限部２３ａ～２３ｆの各々と、モータ１４ａ～１４ｆとの間には、アンプ２５ａ～２５ｆが設けられている。以下では、アンプ２５ａ～２５ｆをまとめてアンプ２５と記載する場合がある。

次に、図３を参照して、モータ１４のトルク制御について説明する。

位置／速度制御部２２は、記憶部２４（上位命令装置）から関節１２の位置指令値（時刻歴指令位置）を取得する。そして、位置制御部２２ａは、取得された位置指令値と、関節１２のエンコーダ１６から所得された角度位置（実角度位置）との偏差を算出する。そして、位置制御部２２ａは、算出された偏差に位置ゲインを乗算する。また、位置制御部２２ａは、位置指令値を微分して関節１２の目標とする速度である速度指令値を算出する。

次に、速度制御部２２ｂは、上記の位置ゲインが乗算された偏差と、速度指令値とを加算するとともに、この加算値から、実角度位置を微分して得られる実角速度を減算して速度偏差を算出する。そして、速度制御部２２ｂは、算出した速度偏差に速度ゲインを乗算する。これにより、速度制御部２２ｂは、モータ１４から出力する目標トルクに応じた目標電流値を生成する。

そして、速度制御部２２ｂは、生成した目標電流値を、電流制限部２３に送信する。電流制限部２３は、送信された目標電流値が電流制限値の範囲内である場合、目標電流値をそのままアンプ２５に送信する。一方、電流制限部２３は、送信された目標電流値が電流制限値の範囲外である場合、目標電流値を電流制限値の範囲内の値に制限して、アンプ２５に送信する。

ここで、電流制限値は、速度の関数となっており、モータ１４の正方向の回転時の電流制限値Ｉｌｉｍ＋_ｉと、モータ１４の負方向の回転時の負側の電流制限値Ｉｌｉｍ―_ｉとを含む。電流制限値Ｉｌｉｍ＋_ｉおよび電流制限値Ｉｌｉｍ―_ｉの関数の参照速度として、三次フィルタがかけられた後の速度指令が用いられる。なお、添え字「ｉ」は、ロボット１０の軸線の番号を示す。

本実施形態では、速度指令は、速度の制御の遅れを考慮した伝達関数を含むフィルタが乗算されている。具体的には、フィルタがかけられた後の速度指令に、速度の制御の遅れを考慮した以下の伝達関数からなるフィルタがかけられている。ここで、フィルタがかられる前の速度指令をｖｃｏｍ_ｉ［ｒａｄ／ｓ］とし、フィルタがかられた後の速度指令をｖｃｏｍ＿ｆｉｌ_ｉ［ｒａｄ／ｓ］とすると、ｖｃｏｍ＿ｆｉｌ_ｉは下記の式で表される。

ここで、Ｋｖｆｆ_ｉおよびＫｐ_ｉは、それぞれ、速度フィードフォワードゲインと位置ゲインである。したがって、速度フィードフォワードゲインが１であれば、ｖｃｏｍ_ｉ［ｒａｄ／ｓ］とｖｃｏｍ＿ｆｉｌ_ｉ［ｒａｄ／ｓ］とは一致する。

（通常時の電流制限値）
ここで、本実施形態では、減速部１５の通常時には、電流制限値（正側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ＋_ｉ、負側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ―_ｉ）は、モータ１４の加速度に応じて変化するように設定されている。また、電流制限値は、モータ１４に対する速度指令（位置制御部２２ａによって算出された速度指令値、図３参照）から算出された加速度に応じて変化するように設定されている。

具体的には、正側の電流制限値Ｉｌｉｍ＋_ｉおよび負側の電流制限値Ｉｌｉｍ―_ｉに対して、下記の式で表されるＩａ_ｉ［Ａｒｍｓ］を加算することにより、正側の電流制限値Ｉｌｉｍ＋_ｉおよび負側の電流制限値Ｉｌｉｍ―_ｉは、モータ１４の加速度に応じて変化するように設定される。

ここで、ａｃｏｍ＿ｆｉｌ_ｉは、ｖｃｏｍ＿ｆｉｌ_ｉを微分することにより求められた加速度［ｒａｄ／ｓ^２］である。Ｒ_ｉは、減速部１５の減速比であり、Ｉ_ｉ１は、モータ１４のイナーシャ［ｋｇｍ^２］である。Ｉ_ｉ２は、駆動系のイナーシャ［ｋｇｍ^２］である。Ｋｔ_ｉは、モータ１４のトルク定数［Ａｒｍｓ／Ｎｍ］である。したがって、モータ１４の加速度に応じて変化するように設定された電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ＋_ｉおよび電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ―_ｉは、下記の式により表される。

なお、電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ＋_ｉは、特許請求の範囲の「第１電流制限値」および「第１正側電流制限値」の一例である。また、電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ―_ｉは、特許請求の範囲の「第１電流制限値」および「第１負側電流制限値」の一例である。

すなわち、本実施形態では、電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ＋_ｉおよび電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ―_ｉは、モータ１４の加速度ａｃｏｍ＿ｆｉｌ_ｉと、モータ１４から減速部１５までのイナーシャＩ_ｉ１およびＩ_ｉ２とに基づいて設定されている。

また、本実施形態では、正側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ＋_ｉおよび負側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ―_ｉは、モータ１４の加速度に応じて大きくなるように設定されている。すなわち、上記の数式３におけるＩａ_ｉが正の値である場合、正側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ＋_ｉおよび負側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ―_ｉは、モータ１４の加速度に応じて大きくなる。言い換えると、正側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ＋_ｉの絶対値が大きくなっているときは、負側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ―_ｉの絶対値は小さくなる。

図４および図５を参照して、モータ１４が正方向に回転している場合の動作を説明する。期間ｔ１において、モータ１４の回転速度が上昇する。すなわち、上記の数式２におけるａｃｏｍ＿ｆｉｌ_ｉは正の値を有する。この場合、上記の数式３におけるＩａ_ｉが正の値となり、正側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ＋_ｉは、大きくなる。

一方、本実施形態では、モータ１４の駆動を妨げる向きに加わる外力（具体的には、重力）に抗する側にモータ１４の力またはモーメントを発生させる場合、電流制限値は、モータ１４の加速度に応じて小さくなるように設定されている。たとえば、図４に示すように、ロボット１０のアーム１１を上方向側に減速しながら回動させる場合、重力によりモータ１４がトルクを発生させなくてもアーム１１は減速する。しかしながら、アーム１１の減速度が、重力によるトルクの減速度よりも小さい場合、モータ１４は、重力に抗する側（上方向側に回動する側、図４の矢印参照）にトルクを発生する必要がある。この場合、図５の期間ｔ２に示すように、正側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ＋_ｉは、小さくなる。なお、図５では、正側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ＋_ｉおよび負側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ―_ｉは、階段状に変化するように示されているが、実際には加速度は滑らかに変化するので、正側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ＋_ｉおよび負側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ―_ｉの変化も緩やかである。

（異常時の電流制限値）
アーム１１が干渉しているような場合、速度指令とエンコーダ１６から速度のフィードバック値とが乖離する場合がある。この場合、実際にはアーム１１は停止しているにも関わらず、速度指令は、アーム１１が加速されている状態となる。このため、上記の通常時の電流制限値では、電流制限値が過度に大きくなる場合があり、減速部１５などに過大な負荷を与えてしまう可能性がある。

そこで、本実施形態では、減速部１５の異常時には、電流制限値（正側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｆｂ＋_ｉ、および、負側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｆｂ―_ｉ）は、モータ１４の速度のフィードバック値から算出された加速度に応じて変化するように設定されている。なお、正側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｆｂ＋_ｉは、特許請求の範囲の「第２電流制限値」および「第２正側電流制限値」の一例である。また、負側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｆｂ―_ｉは、特許請求の範囲の「第２電流制限値」および「第２負側電流制限値」の一例である。

具体的には、まず、フィードバックされた速度ｖｆｂ_ｉを微分することにより加速度ａｆｂ_ｉ［ｒａｄ／ｓ^２］を算出する。

ここで、Ｋｔ_ｉは、モータ１４のトルク定数［Ｎｍ／Ａｒｍｓ］である。

そして、電流制限値の関数の参照速度をフィードバックされた速度として算出された正側の電流制限値Ｉｌｉｍ＿ｆｂ＋_ｉ、および、負側の電流制限値Ｉｌｉｍ＿ｆｂ―_ｉに加速度ａｆｂ_ｉ［ｒａｄ／ｓ^２］を加算することにより、正側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｆｂ＋_ｉおよび負側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｆｂ―_ｉが算出される。

また、正側の電流制限値Ｉｌｉｍ＿ｆｂ＋_ｉ、および、負側の電流制限値Ｉｌｉｍ＿ｆｂ―_ｉの各々に対して、フィルタをかけたものを、正側の電流制限値Ｉｌｉｍ＿ｆｂ＿ｆｉｌ＋_ｉ［Ａｒｍｓ］、および、負側の電流制限値Ｉｌｉｍ＿ｆｂ＿ｆｉｌ―_ｉ［Ａｒｍｓ］とする。そして、下記の数式６に基づいて、通常時の電流制限値（速度指令に基づく電流制限値）と、異常時の電流制限値（フィードバック値に基づく電流制限値）との差分（ΔＴｌｉｍ＿ｎｅｗ＋_ｉ、および、ΔＴｌｉｍ＿ｎｅｗ―_ｉ）が算出される。

上記の差分（ΔＴｌｉｍ＿ｎｅｗ＋_ｉ、ΔＴｌｉｍ＿ｎｅｗ―_ｉ）が大きくなるということは、速度指令に基づく電流制限値とフィードバック値に基づく電流制限値との差が大きくなるということであり、この差が許容値を超えると、減速部１５などに損傷を与える場合がある。

そして、本実施形態では、通常時の電流制限値（速度指令に基づく電流制限値）と、異常時の電流制限値（フィードバック値に基づく電流制限値）との差分から算出したトルク制限値の差（ΔＴｌｉｍ＿ｎｅｗ＋_ｉ、および、ΔＴｌｉｍ＿ｎｅｗ―_ｉ）の絶対値が所定の値以上となる場合、電流制限値として異常時の電流制限値が用いられ、差の絶対値が所定の値未満となる場合、通常時の電流制限値が用いられる。

また、本実施形態では、減速部１５の通常時の出力軸の力またはモーメント（本実施形態ではトルク）の許容値Ｔ_Ｇｉと、減速部１５の異常時の出力軸の力またはモーメントの許容値Ｔ_{Ｇｍａｘｉ}とが予め設定されている。そして、所定の値は、通常時の力またはモーメントの許容値Ｔ_Ｇｉと、異常時の力またはモーメントの許容値Ｔ_{Ｇｍａｘｉ}との差分の絶対値｜Ｔ_Ｇｉ―Ｔ_{Ｇｍａｘｉ}｜を含む。なお、許容値Ｔ_Ｇｉは、減速部１５の許容加減速トルクに相当する。また、許容値Ｔ_{Ｇｍａｘｉ}は、減速部１５の非常停止時の瞬間最大トルクに相当する。また、許容値Ｔ_{Ｇｍａｘｉ}に安全率を設定してもよい。

より詳細には、本実施形態では、下記の数式７に示すように、速度指令に基づく正側の電流制限値とフィードバック値に基づく正側の電流制限値との差分から算出したトルク制限値の差の絶対値｜ΔＴｌｉｍ＿ｎｅｗ＋_ｉ｜が｜Ｔ_Ｇｉ―Ｔ_{Ｇｍａｘｉ}｜以上、または、速度指令に基づく負側の電流制限値とフィードバック値に基づく負側の電流制限値との差分から算出したトルク制限値の差の絶対値｜ΔＴｌｉｍ＿ｎｅｗ―_ｉ｜が｜Ｔ_Ｇｉ―Ｔ_{Ｇｍａｘｉ}｜以上で、かつ、電流指令がフィードバック値に基づく正側の電流制限値以上かつ速度指令に基づく正側の電流制限値がフィードバック値に基づく正側の電流制限値以上、または、電流指令がフィードバック値に基づく負側の電流制限値以下でかつ速度指令に基づく負側の電流制限値がフィードバック値に基づく負側の電流制限値以下の場合、電流制限値としてフィードバック値に基づく電流の電流制限値が用いられる。なお、具体的には、下記の数式７に基づいて、速度指令に基づく電流制限値と、フィードバック値に基づく電流制限値とのうちのいずれを使用するのかが判断される。

具体的には、ロボット１０のいずれかの関節１２において、上記の数式７の関係を満たした場合、全ての関節１２において、フィードバック値に基づく正側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｆｂ＿ｆｉｌ＋_ｉおよび負側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｆｂ＿ｆｉｌ―_ｉが用いられる。一方、上記の数式７の関係を満たさない場合、速度指令に基づく正側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ＋_ｉおよび負側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ―_ｉが用いられる。

また、本実施形態では、下記の数式８に示すように、電流制限値としてフィードバック値に基づく電流の電流制限値が用いられる場合において、所定の時間、電流指令Ｉｃｏｍ_ｉよりもフィードバック値に基づく電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｆｂ＿ｆｉｌ＋_ｉが大きく、かつ、電流指令Ｉｃｏｍ_ｉよりも電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ＋_ｉが大きく、かつ、電流指令Ｉｃｏｍ_ｉよりも電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｆｂ＿ｆｉｌ―_ｉが小さく、かつ、電流指令Ｉｃｏｍ_ｉよりも電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ―_ｉが小さい場合、電流制限値として速度指令に基づく電流制限値（Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ＋_ｉ、および、Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ―_ｉ）が用いられるように切り替えられる。

なお、具体的には、ロボット１０の全ての軸線（関節１２）において、上記の数式８の条件が、所定の時間だけ連続して満たされた場合、電流制限値として速度指令に基づく電流制限値（Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ＋_ｉ、および、Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ―_ｉ）が用いられるように切り替えられる。

次に、図６を参照して、減速部１５を介して駆動力を伝達するモータ１４に通電する電流を制限する電流制限方法について説明する。

ステップＳ１において、モータ１４の加速度を取得する。具体的には、上位命令装置からの時刻歴指令位置を２階微分して加速度を取得する。また、エンコーダ１６から取得されるモータ１４の出力軸１７の角度位置に基づいて、モータ１４の加速度を取得する。

ステップＳ２において、上記の数式７に基づいて、速度指令に基づく電流制限値と、フィードバック値に基づく電流制限値とのうちのいずれを使用するのかが判断される。

ステップＳ２において、速度指令に基づく電流制限値を使用すると判断された場合、ステップＳ３において、速度指令から取得されたモータ１４の加速度に基づいて、上記の数式３によるモータ１４に通電する電流の電流制限値を設定する。そして、ステップＳ６に進む。

ステップＳ２において、フィードバック値に基づく電流制限値を使用すると判断された場合、ステップＳ４において、フィードバック値から取得されたモータ１４の加速度に基づいて、上記の数式５によるモータ１４に通電する電流の電流制限値を設定する。

ステップＳ４に進んだ場合、ステップＳ５において、上記の数式８に基づいて、速度指令に基づく電流制限値に切り替えるか否かが判断される。ステップＳ５においてｙｅｓの場合、ステップＳ３において、速度指令から取得されたモータ１４の加速度に基づいて、上記の数式３によるモータ１４に通電する電流の電流制限値を設定する。ステップＳ５においてｎｏの場合、ステップＳ６に進む。

ステップＳ６において、設定された電流制限値の範囲内において、モータ１４に通電する。上記のステップＳ１～Ｓ６の動作は、モータ１４の動作中に繰り返し行われている。

［本実施形態の効果］
本実施形態では、以下のような効果を得ることができる。

（電流制限装置およびロボットの効果）
本実施形態では、上記のように、モータ１４に通電する電流の電流制限値は、モータ１４の加速度に応じて変化するように設定されている。これにより、モータ１４から減速部１５までの間のイナーシャが大きい場合でも、モータ１４の加速度に応じて、電流制限値を変化させることにより、モータ１４に十分なトルクを発生させることができる。その結果、モータ１４から減速部１５までの間のイナーシャが大きい場合でも、モータ１４によって駆動される部材（アーム１１）を十分に加速することができる。

また、本実施形態では、上記のように、電流制限値は、モータ１４の加速度に応じて大きくなるように設定されている。これにより、モータ１４から減速部１５までの間のイナーシャが大きい場合でも、モータ１４の加速度に応じて電流制限値が大きくなるので、モータ１４に十分なトルクを発生させることができる。その結果、モータ１４によって駆動される部材（アーム１１）を十分に加速することができる。

また、本実施形態では、上記のように、モータ１４の駆動を妨げる向きに加わる外力に抗する側にモータ１４の力またはモーメント（本実施形態では、トルク）を発生させる場合、電流制限値は、モータ１４の加速度に応じて小さくなるように設定されている。ここで、モータ１４によって駆動される部材（アーム１１）の減速の度合いが、重力によって減速される度合いよりも小さい場合、重力に抗する側（上方向側）にモータ１４の力またはモーメントを発生させることになる。そこで、上記のように構成することによって、モータ１４が重力に過度に抗するように動作するのを抑制することができるので、減速部１５に過度な力がかかるのを抑制することができる。

また、本実施形態では、上記のように、電流制限値は、モータ１４の加速度と、モータ１４から減速部１５までのイナーシャとに基づいて設定されている。ここで、トルクは、イナーシャと加速度との積で表されるので、電流制限値をモータ１４の加速度とモータ１４から減速部１５までのイナーシャとに基づいて設定することによって、所望のトルクが出力可能なように適切に電流制限値を設定することができる。

また、本実施形態では、上記のように、電流制限値は、モータ１４に対する速度指令から算出された加速度に応じて変化するように設定されている電流の電流制限値（正側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ＋_ｉ、負側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ―_ｉ）を含む。これにより、モータ１４に対する速度指令に基づいて、電流の電流制限値を容易に設定することができる。

また、本実施形態では、上記のように、速度指令は、速度の制御の遅れを考慮した伝達関数を含むフィルタが乗算されている。これにより、速度指令が速度の制御の遅れに沿うように修正されるので、修正された速度指令から算出された加速度に応じて適切に電流制限値を設定することができる。

また、本実施形態では、上記のように、電流制限値は、モータ１４の速度のフィードバック値から算出された加速度に応じて変化するように設定されている電流の電流制限値（正側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｆｂ＋_ｉ、および、負側の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｆｂ―_ｉ）を含む。ここで、モータ１４によって駆動される部材（アーム１１）が周りの物体に干渉して停止した場合など、速度指令（モータ１４によって駆動される部材をさらに移動させる指令値）と、フィードバック値（停止した状態）との乖離が大きくなる。この場合、上記のように、モータ１４の速度のフィードバック値から算出された加速度に応じて変化するように設定されている電流制限値を用いることによって、モータ１４によって駆動される部材の実際の状態に基づいて、適切に電流制限値を設定することができる。

また、本実施形態では、上記のように、減速部１５の通常時には、電流制限値として速度指令に基づく電流制限値が用いられ、減速部１５の異常時には、電流制限値としてフィードバック値に基づく電流制限値が用いられる。これにより、減速部１５の通常時に、電流制限値が振動するのを抑制することができるとともに、減速部１５の異常時に、モータ１４によって駆動される部材の実際の状態に基づいて適切に電流制限値を設定することができる。

また、本実施形態では、上記のように、速度指令に基づく電流制限値とフィードバック値に基づく電流制限値との差分から算出したトルク制限値の差の絶対値が所定の値以上となる場合、電流制限値としてフィードバック値に基づく電流制限値が用いられ、差の絶対値が所定の値未満となる場合、電流制限値として速度指令に基づく電流制限値が用いられる。これにより、トルク制限値の差の絶対値は、速度指令とフィードバック値との乖離の大きさを反映するので、乖離の大きさに基づいて、適切に速度指令に基づく電流制限値またはフィードバック値に基づく電流制限値を用いることができる。

また、本実施形態では、上記のように、速度指令に基づく電流制限値は、モータ１４の正方向の駆動時の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ＋_ｉと、モータ１４の負方向の駆動時の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ―_ｉとを含み、フィードバック値に基づく電流制限値は、正方向の駆動時の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｆｂ＋_ｉ（Ｉｌｉｍ＿ｎｅｗ＿ｆｂ２＋_ｉ）と、負方向の駆動時の電流制限値Ｉｌｉｍ＿ｎｅｗ＿ｆｂ―_ｉ（Ｉｌｉｍ＿ｎｅｗ＿ｆｂ２―_ｉ）とを含む。これにより、モータ１４の正方向へのトルク発生時と負方向へのトルク発生時との両方において、適切に電流制限値を設定することができる。

また、本実施形態では、上記のように、上記の数式７に基づいて、速度指令に基づく電流制限値、または、フィードバック値に基づく電流制限値が用いられる。これにより、上記の差分の絶対値に基づいて、速度指令とフィードバック値との乖離が大きいことを検出することに加えて、電流指令がフィードバック値に基づく電流制限値の範囲外であること、および、速度指令に基づく電流制限値がフィードバック値に基づく電流制限値の範囲外であることの条件が付加されるので、誤って電流制限値としてフィードバック値に基づく電流制限値が用いられるのを抑制することができる。

また、本実施形態では、上記のように、上記の数式８に基づいて、速度指令に基づく電流制限値が用いられるように切り替えられる。これにより、電流指令が、所定の時間、速度指令に基づく電流制限値の範囲内にあることになるので、電流制限値として適切な速度指令に基づく電流制限値に切り替えることができる。

また、本実施形態では、上記のように、減速部１５の通常時の出力軸の力またはモーメント（本実施形態ではトルク）の許容値Ｔ_Ｇｉと、減速部１５の異常時の出力軸の力またはモーメントの許容値Ｔ_{Ｇｍａｘｉ}とが予め設定されている。そして、所定の値は、通常時の力またはモーメントの許容値Ｔ_Ｇｉと、異常時の力またはモーメントの許容値Ｔ_{Ｇｍａｘｉ}との差分の絶対値｜Ｔ_Ｇｉ―Ｔ_{Ｇｍａｘｉ}｜を含む。これにより、減速部１５の許容値に基づいて、電流制限値として速度指令に基づく電流制限値とフィードバック値に基づく電流制限値とのうちのいずれを用いるのかを容易に判断することができる。

また、本実施形態では、上記のように、モータ１４は、ロボット１０の関節１２に設けられている。これにより、ロボット１０の関節１２に設けられるモータ１４において、モータ１４から減速部１５までの間のイナーシャが大きい場合でも、モータ１４によって駆動される部材を十分に加速することができる。

（電流制限方法の効果）
本実施形態では、上記のように、電流制限方法は、取得されたモータ１４の加速度に基づいて、モータ１４に通電する電流の電流制限値を設定するステップを備える。これにより、モータ１４から減速部１５までの間のイナーシャが大きい場合でも、モータ１４の加速度に応じて、電流制限値を変化させることにより、モータ１４に十分なトルクを発生させることができる。その結果、モータ１４から減速部１５までの間のイナーシャが大きい場合でも、モータ１４によって駆動される部材（アーム１１）を十分に加速することが可能な電流制限方法を提供することができる。

［変形例］
なお、今回開示された実施形態は、すべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は、上記した実施形態の説明ではなく特許請求の範囲によって示され、さらに特許請求の範囲と均等の意味および範囲内でのすべての変更（変形例）が含まれる。

たとえば、上記実施形態では、産業用のロボット１０に本発明を適用する例を示したが、本発明はこれに限られない。たとえば、図７に示すように、医療用のロボット３０に本発明を適用してもよい。ロボット３０は、ポジショナ３１（多関節ロボット）と、アームベース３２と、複数のアーム３３とを備えている。複数のアーム３３の各々の先端には、医療器具３４が取り付けられている。本発明の電流制限装置２１は、たとえば、ポジショナ３１（多関節ロボット）やアーム３３の関節のモータ１４に流れる電流を制限する。これにより、医療用のロボット３０において、モータ１４から減速部１５までの間のイナーシャが大きい場合でも、モータ１４によって駆動されるアーム３３を十分に加速することができる。

特に医療用のロボット３０では、医療用のロボット３０を配置するためのスペースが限られている、関節が多い、衝突時の衝撃の低減のために駆動電圧を低くする必要がある、などの理由から、低出力のモータ１４を高減速比で使用することがある。この場合、モータ１４から減速部１５までの間のイナーシャが大きい場合でも、本発明のように電流制限値をモータ１４の加速度に応じて変化させて、モータ１４によって駆動されるアーム３３を十分に加速可能にすることは、特に有効である。

また、上記実施形態では、本発明の「駆動部」としてモータ１４が適用される例を示したが、本発明はこれに限られない。たとえば、本発明の「駆動部」として比例ソレノイド、リニアモータ、ボイスコイルおよび球面アクチュエータなどのアクチュエータを適用してもよい。また、本発明の「駆動部」としてパウダクラッチ・ブレーキ、ヒステリシスクラッチ・ブレーキを適用してもよい。

また、上記実施形態では、速度指令に基づく電流制限値と、速度のフィードバック値に基づく電流制限値とが用いられる例を示したが、本発明はこれに限られない。たとえば、速度指令に基づく電流制限値と、速度のフィードバック値に基づく電流制限値とのうちのいずれか一方のみを用いるようにしてもよい。

また、上記実施形態では、速度指令は、速度の制御の遅れを考慮した伝達関数を含むフィルタが乗算されている例を示したが、本発明はこれに限られない。たとえば、速度の制御の遅れが無い場合（または、遅れが小さい場合）などでは、速度の制御の遅れを考慮した伝達関数を含むフィルタを速度指令に乗算しなくてもよい。

また、上記実施形態では、位置／速度制御部２２が、モータ１４ａ～１４ｆに対して共通に（１つ）設けられている例を示したが、本発明はこれに限られない。たとえば、図８に示すように、位置／速度制御部１２２ａ～１２２ｆが、モータ１４ａ～１４ｆに対して個別に設けられていてもよい。

また、上記実施形態では、位置／速度制御部２２と、電流制限部２３ａ～２３ｆとが個別に設けられている例を示したが、本発明はこれに限られない。たとえば、図９に示すように、位置／速度制御部および電流制限部を含む１つの制御部２６が設けられていてもよい。

また、上記実施形態では、電流制限部２３が、アンプ２５の上流側に設けられている例を示したが、本発明はこれに限られない。本発明では、電流制限部２３は、位置／速度制御部２２とモータ１４とが接続されるラインの何処か（アンプ２５の出力側、エンコーダ１６からフィードバックされるラインなど）に設けられていればよい。

１０ ロボット
１２、１２ａ～１２ｆ 関節
１４、１４ａ～１４ｆ モータ（駆動部）
１５、１５ａ～１５ｆ 減速部（駆動力伝達部）
２０ ロボット制御部
２１ 電流制限装置
２３、２３ａ～２３ｆ 電流制限部
３０ ロボット（医療用のロボット）
１００ ロボットシステム
Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ＋_ｉ 電流制限値（第１電流制限値、第１正側電流制限値）
Ｉｌｉｍ＿ｎｅｗ＿ｃｏｍ―_ｉ 電流制限値（第１電流制限値、第１負側電流制限値）
Ｉｌｉｍ＿ｎｅｗ＿ｆｂ＿ｆｉｌ＋_ｉ 電流制限値（第２電流制限値、第２正側電流制限値）
Ｉｌｉｍ＿ｎｅｗ＿ｆｂ＿ｆｉｌ―_ｉ 電流制限値（第２電流制限値、第２負側電流制限値）

Claims (13)

1. 電流が流されることにより力またはモーメントを発生し、駆動力伝達部を介して駆動力を伝達する駆動部に通電する電流を制限する電流制限装置であって、
   前記駆動部に通電する電流を電流制限値の範囲内に制限する電流制限部を備え、
   前記電流制限値は、前記駆動部から前記駆動力伝達部までのイナーシャに基づくとともに、前記駆動部の加速度に応じて変化するように設定されており、
   前記駆動部の駆動を妨げる向きに加わる重力に抗する側に前記駆動部の力またはモーメントを発生させる場合、前記電流制限値は、前記駆動部の加速度に応じて小さくなるように設定されている、電流制限装置。
2. 前記電流制限値は、前記駆動部の駆動を妨げる向きに加わる重力に抗しない側に前記駆動部の力またはモーメントを発生させる場合、前記駆動部の加速度に応じて大きくなるように設定されている、請求項１に記載の電流制限装置。
3. 前記電流制限値は、前記駆動部に対する速度指令から算出された加速度に応じて変化するように設定されている第１電流制限値を含む、請求項１または２に記載の電流制限装置。
4. 前記速度指令は、速度の制御の遅れを考慮した伝達関数を含むフィルタが乗算されている、請求項３に記載の電流制限装置。
5. 電流が流されることにより力またはモーメントを発生し、駆動力伝達部を介して駆動力を伝達する駆動部に通電する電流を制限する電流制限装置であって、
   前記駆動部に通電する電流を電流制限値の範囲内に制限する電流制限部を備え、
   前記電流制限値は、前記駆動部の加速度に応じて変化するように設定されており、
   前記電流制限値は、前記駆動部に対する速度指令から算出された加速度に応じて変化するように設定されている第１電流制限値と、前記駆動部の速度のフィードバック値から算出された加速度に応じて変化するように設定されている第２電流制限値とを含み、
   前記第１電流制限値と前記第２電流制限値との差分から算出したトルク制限値の差の絶対値が所定の値未満となる場合、前記電流制限値として前記第１電流制限値が用いられ、前記差の絶対値が前記所定の値以上となる場合、前記電流制限値として前記第２電流制限値が用いられる、電流制限装置。
6. 前記第１電流制限値は、前記駆動部の正方向の駆動時の第１正側電流制限値と、前記駆動部の負方向の駆動時の第１負側電流制限値とを含み、
   前記第２電流制限値は、前記正方向の駆動時の第２正側電流制限値と、前記負方向の駆動時の第２負側電流制限値とを含む、請求項５に記載の電流制限装置。
7. 前記第１正側電流制限値と前記第２正側電流制限値との差分から算出したトルク制限値の差の絶対値が前記所定の値以上または前記第１負側電流制限値と前記第２負側電流制限値との差分から算出したトルク制限値の差の絶対値が前記所定の値以上で、かつ、電流指令が前記第２正側電流制限値以上かつ前記第１正側電流制限値が前記第２正側電流制限値以上、または、前記電流指令が前記第２負側電流制限値以下でかつ前記第１負側電流制限値が前記第２負側電流制限値以下の場合、前記電流制限値として前記第２電流制限値が用いられる、請求項６に記載の電流制限装置。
8. 前記電流制限値として前記第２電流制限値が用いられる場合において、所定の時間、電流指令よりも前記第２正側電流制限値が大きく、かつ、前記電流指令よりも前記第１正側電流制限値が大きく、かつ、前記電流指令よりも前記第２負側電流制限値が小さく、かつ、前記電流指令よりも前記第１負側電流制限値が小さい場合、前記電流制限値として前記第１正側電流制限値および前記第１負側電流制限値が用いられるように切り替えられる、請求項６または７に記載の電流制限装置。
9. 前記駆動力伝達部の許容加減速トルクに相当する許容値と、前記駆動力伝達部の非常停止時の瞬間最高トルクに相当する許容値とが予め設定されており、
   前記所定の値は、前記駆動力伝達部の許容加減速トルクに相当する許容値と、前記駆動力伝達部の非常停止時の瞬間最高トルクに相当する許容値との差分の絶対値を含む、請求項５～８のいずれか１項に記載の電流制限装置。
10. 前記駆動部は、ロボットの関節に設けられるモータを含む、請求項１～９のいずれか１項に記載の電流制限装置。
11. 前記ロボットは、医療用のロボットを含む、請求項１０に記載の電流制限装置。
12. 電流が流されることにより力またはモーメントを発生し、駆動力伝達部を介して駆動力を伝達する駆動部に通電する電流を制限する電流制限方法であって、
    前記駆動部の加速度を取得するステップと、
    前記駆動部から前記駆動力伝達部までのイナーシャに基づくとともに、前記取得された駆動部の加速度に基づいて、前記駆動部に通電する電流の電流制限値を設定するステップと、
    前記設定された電流制限値の範囲内において、前記駆動部に通電するステップとを備え、
    前記駆動部の駆動を妨げる向きに加わる重力に抗する側に前記駆動部の力またはモーメントを発生させる場合、前記電流制限値は、前記駆動部の加速度に応じて小さくなるように設定されている、電流制限方法。
13. 電流が流されることにより力またはモーメントを発生し、駆動力伝達部を介して駆動力を伝達する駆動部に通電する電流を制限する電流制限方法であって、
    前記駆動部の加速度を取得するステップと、
    前記取得された駆動部の加速度に基づいて、前記駆動部に通電する電流の電流制限値を設定するステップと、
    前記設定された電流制限値の範囲内において、前記駆動部に通電するステップとを備え、
    前記電流制限値は、前記駆動部に対する速度指令から算出された加速度に応じて変化するように設定されている第１電流制限値と、
    前記駆動部の速度のフィードバック値から算出された加速度に応じて変化するように設定されている第２電流制限値とを含み、
    前記第１電流制限値と前記第２電流制限値との差分から算出したトルク制限値の差の絶対値が所定の値未満となる場合、前記電流制限値として前記第１電流制限値が用いられ、前記差の絶対値が前記所定の値以上となる場合、前記電流制限値として前記第２電流制限値が用いられる、電流制限方法。

Images