JP5370117B2 - ロボットの制御装置 - Google Patents

Description

本発明は、関節に連結されたリンクを揺動させるロボットの制御装置に関する。

ロボットの制御装置は、関節角を所望の目標関節角に追従させるようにフィードバック制御を実行する。フィードバック制御を実行する制御器を本明細書ではフィードバック制御器と称する。さらに、記述を簡単にするために以下では、フィードバック制御器を「Ｆ制御器」と記述することがある。また以下では、実際の関節角を目標関節角と区別するために計測関節角と称する。

一般に、ロボットの応答速度は速い方が好ましい。ここで、応答速度とは、計測関節角が目標関節角に収束する速さを意味する。典型的なＦ制御器が例えば特許文献１に開示されている。

特開２０００−６０６５号公報（例えば第５頁及び図１）

制御理論において、応答速度を向上させていくと計測関節角がオーバーシュートを生ずることは良く知られている。従って、オーバーシュートの抑制と応答速度の向上はトレードオフの関係にある。

他方、ロボットの関節には可動範囲が定められることがよくある。可動範囲は、ロボットの機構に起因して物理的に制限されることもあれば、ロボットの周辺に存在する他の物体との衝突を避けるために人為的に設定されることもある。可動範囲の境界付近ではオーバーシュートは抑制されることが好ましい。目標関節角が可動範囲内であってもオーバーシュートによって計測関節角が可動範囲を外れることが起こり得るからである。

前述したように、オーバーシュートの抑制と応答速度はトレードオフの関係にあるため、計測関節角が可動範囲を超えないようにオーバーシュートを抑制しようとすると応答速度が遅くなり、逆に応答速度を上げると計測関節角が可動範囲を超えてしまう虞がある。

本明細書が開示する一つの新規な技術は、可動範囲の境界付近でのオーバーシュートを抑制するとともに、可動範囲の境界付近以外では応答速度を向上させることのできる制御装置を提供する。

本発明は、可動範囲の境界付近でのオーバーシュートを抑制するとともに、可動範囲の境界付近以外では応答速度を向上させることのできるロボット制御装置を提供する。本明細書が開示する技術の一実施形態では、ロボット制御装置はＦ制御器（フィードバック制御器）と極配置器を備える。Ｆ制御器は、センサによって計測された計測関節角を目標関節角に追従させるフィードバック制御を実行するように構成されている。極配置器は、Ｆ制御器のパラメータを変更するように構成されている。また、極配置器は、予め定められた関節の可動範囲を、可動範囲の境界を含まない第１可動許容範囲と、第１可動許容範囲の外側に設定された第２可動許容範囲とに区分して記憶している。極配置器は、計測関節角が第１可動許容範囲内に存する場合は、特性多項式の根が複素根となるようにＦ制御器のパラメータを変更する。他方、極配置器は、計測関節角が第２可動許容範囲内に存する場合は、特性多項式の根が実根となるようにＦ制御器のパラメータを変更する。ここで、特性多項式とは、フィードバック制御器とロボットの機構系によって構成されるフィードバックループ系の閉ループ伝達関数の分母をラプラス演算子ｓの多項式で表した式を意味する。なお、以下では説明を理解し易くするために、「Ｆ制御器のパラメータを変更する」との表現に代えて「Ｆ制御器にパラメータを設定する」という表現を用いる場合がある。「従前のパラメータと異なるパラメータを設定する」ことと、「パラメータを変更する」ことは同義である。

特性多項式の根が複素根を有する場合、制御理論上はオーバーシュートを生ずるが応答速度を上げることができる。他方、特性多項式の根が実根を有する場合、制御理論上はオーバーシュートが生じない。本明細書が開示する技術によって実現されるロボット制御装置の一例は、予め定められた可動範囲の境界付近ではオーバーシュートを理論上は生じることがなく、境界から離れた範囲では応答速度を向上させることができる。なお、フィードバック制御が安定であることは前提なので、極配置器は、複素根と実根のいずれの場合も特性多項式の根の実部が０（ゼロ）よりも小さくなるように、Ｆ制御器のパラメータを設定（変更）することに留意されたい。

Ｆ制御器は、ＰＩ制御、ＰＩＤ制御、或いは最適制御など、様々な制御則に基づく制御器が採用され得る。従ってＦ制御器のパラメータも様々である。特性多項式の根を所望の値とするパラメータの具体的な設定方法も様々である。しかしながら、特性多項式の根が所望の値となるようにＦ制御器を設計することは良く行われることである。本発明の主たる技術的思想は、特性多項式の根を関節の可動範囲に応じて上記の通り変更することにあり、Ｆ制御器の具体的な制御則に依存しないことに留意されたい。

特性多項式が複素根を有するためのＦ制御器のパラメータ、及び、特性多項式が実根を有するためのパラメータは、ロボットの機構系の伝達関数とＦ制御器に採用される制御則の伝達関数に基づいて、予め定められ、極配置器に記憶されているとよい。或いは、極配置器は、ロボットの機構系の伝達関数を適応制御器によってリアルタイムに推定し、計測関節角に応じて特性多項式が所望の根を有するように、Ｆ制御器のパラメータをリアルタイムに決定することも好適である。

本明細書が開示する一つの新規な技術によれば、可動範囲の境界付近でのオーバーシュートを抑制するとともに、可動範囲の境界付近以外では応答速度を向上させることのできるロボット制御装置を実現することができる。

第１実施例のロボット制御装置のブロック図である。 ロボットの模式図である。 第１実施例の極配置器が実行する処理のフローチャート図である。 第２実施例のロボット制御装置のブロック図である。 第２実施例のロボット制御装置のシミュレーション結果を示すグラフである。

図面を参照して第１実施例のロボット制御装置を説明する。図１に、ロボット制御装置２０のブロック図を示す。図２にロボット１０の部分模式図を示す。ロボット１０は、ロボット制御装置２０の制御対象である。ロボット１０は、複数のリンクが関節によって連結されている多関節ロボットである。なお、図２では、第１リンク１１、第２リンク１３、及びそれらのリンクを連結する関節１２のみを示しており、他のリンクと関節の図示を省略している。関節１２はモータを有しており、第２リンク１３を第１リンク１１に対して揺動させることができる。ロボット制御装置２０は、関節１２の関節角θｓを目標関節角θｒに追従させるフィードバック制御を実行する。なお、本実施例では、関節１２の制御についてのみ説明するが、本実施例で説明する制御は、図示を省略している他の関節にも適用されることに留意されたい。

関節角θｓは、関節１２に備えられているエンコーダ（不図示）によって計測される。なお、目標関節角θｒと区別するために、以下では、エンコーダによって計測される関節角θｓを計測関節角θｓと称する。目標関節角θｒは、上位のコントローラ（不図示）から与えられる。

図１では、モータの動特性を含む関節１２の機構系の伝達関数をＰ（ｓ）で表している。図１において「ｓ」はラプラス演算子を示している。以下、制御系に関する説明は周波数領域での表現で行う。「ラプラス演算子（ラプラス変換）」、「周波数領域での制御系の説明」は、制御理論の技術分野では良く知られているので基礎的な説明は省略する。

ロボット制御装置２０は、Ｆ制御器（フィードバック制御器）２２と極配置器２４を備えている。フィードバック制御器２２が、計測関節角θｓを目標関節角θｒに追従させるためのフィードバック制御を実行する。なお、図１では、Ｆ制御器２２の伝達関数を「Ｇ（ｓ）」で表している。伝達関数Ｇ（ｓ）は、制御ルールによって様々な形式を取り得るが、全てラプラス演算子ｓの多項式の分数で表現できることに留意されたい。また、関節１２の伝達関数Ｐ（ｓ）もラプラス演算子ｓの多項式の分数で表現できることに留意されたい。

図１のフィードバック制御系において、目標関節角θｒから計測関節角θｓへの伝達特性（閉ループ伝達関数）は次の（数１）で表されることは良く知られている。

閉ループ伝達関数の分母はラプラス演算子ｓの多項式で表現することができる。この多項式（閉ループ伝達関数の分母）が、特定多項式と呼ばれている。さらに、特性多項式は、ラプラス演算子ｓに関して因数分解することができる。今、特性多項式がラプラス演算子ｓの２次式で表されるとすると、次の（数２）で表現できる。

（数２）のラプラス演算子ｓの根（即ち、特性多項式の根）は、次の（数３）で表される。

（数３）において、ω_ｎは固有周波数を表す。ｊは虚数単位を表し、ｊ^２＝−１である。また、（数３）において、ζは、閉ループ系の減衰係数を表し、正の実数である。ζ≧１のとき、（数３）のラプラス演算子ｓは実根となり、０＜ζ＜１のとき（数３）のラプラス演算子ｓは複素根となる。（数１）〜（数３）は、制御理論では良く知られている。なお、一巡伝達関数Ｐ（ｓ）Ｇ（ｓ）が３以上の次数を有する場合は後述の実施例にて説明する。

極配置器２４は、Ｆ制御器２２のパラメータを変更することによって、（数３）で与えられる閉ループ伝達関数の極を予め定められた値に設定する。図１のブロック図では、Ｆ制御器２２を表す矩形を斜めに横切る矢印が、極配置器２４がＦ制御器２２のパラメータを変更し得ることを表している。別言すれば、極配置器２４は、特性多項式α_ｃ（ｓ）が所望の根を有するように、Ｆ制御器２２の伝達関数Ｇ（ｓ）を定めるパラメータを設定する。Ｇ（ｓ）とＰ（ｓ）の具体的な伝達関数は予め解っているので、特性多項式の根を予め定められた値とするための伝達関数Ｇ（ｓ）のパラメータも予め定められることに留意されたい。

極配置器２４は、計測関節角θｓの値に応じて特性多項式の根が予め定められた値となるようにＦ制御器２２のパラメータを変更する。その動作を説明する。極配置器２４は、関節１２の可動範囲を記憶している。「関節１２の可動範囲」とは、関節１２の計測関節角θｓの取り得る範囲を意味する。図２に示すように、第２リンク１３は、直線Ｌ１とＬ２で挟まれた角度範囲θａの範囲を動き得る。即ち、関節１２の可動範囲が「θａ」で表される。可動範囲θａは、ロボット１０の機構的な制約で定まる場合もあれば、ユーザが人為的に定める場合もある。後者の場合は、例えば、第２リンク１３が他の物体に衝突しない範囲に定められる場合に相当する。

直線Ｌ１とＬ２は、可動範囲θａの境界に相当する。極配置器２４は、可動範囲θａを、可動範囲の境界Ｌ１及びＬ２を含まない第１可動許容範囲θｃと、第１可動許容範囲θｃの外側に設定された第２可動許容範囲θｂとに区分して記憶している。第２可動許容範囲θｂは、第１可動許容範囲θｃの一方の境界と可動範囲θａの境界Ｌ１までの範囲、及び、第１可動許容範囲θｃの他方の境界と可動範囲θａの境界Ｌ２までの範囲に相当する。

極配置器２４が実行する処理のフローチャートを図３に示す。図３のフローチャートは、制御周期毎に実行される。極配置器２４は、計測関節角θｓが第１可動許容範囲θｃの範囲内であるか否かを判断する（Ｓ２）。計測関節角θｓが第１可動許容範囲θｃの範囲内である場合（Ｓ２：ＹＥＳ）、極配置器２４は、第１パラメータをＦ制御器２２に設定する。ここで、第１パラメータは、特性多項式α_ｃ（ｓ）が複素根を有するように定められている。特性多項式α_ｃ（ｓ）の根の実部が負となるように定められている。これにより、図１のフィードバックループの安定性が補償される。

他方、計測関節角θｓが第１可動許容範囲θｃの範囲外である場合（Ｓ２：ＮＯ）、極配置器２４は、第２パラメータをＦ制御器２２に設定する。ここで、第２パラメータは、特性多項式α_ｃ（ｓ）の根が実根を有するように定められている。なお、計測関節角θｓが第１可動許容範囲θｃの範囲外である場合は、計測関節角θｓが第２可動許容範囲θｂに属する場合に相当する。また、第２パラメータは、特性多項式α_ｃ（ｓ）の根が負の実数となるように定められている。このとき、図１のフィードバックループの安定性が補償される。なお、ここでの説明は、連続時間系を前提としていることに留意されたい。

極配置器２４の利点を説明する。特性多項式の根が複素根となる場合、制御理論上はオーバーシュートを生ずるが応答速度を上げることができる。他方、特性多項式の根が実根となる場合、制御理論上は応答速度は低下するがオーバーシュートを生じない。極配置器２４は、計測関節角θｓが第１可動許容範囲θｃ内の場合は関節１２の制御応答速度を向上させ、可動範囲θａの境界付近の第２可動許容範囲θｂ内の場合は関節１２のオーバーシュートを抑制する。極配置器２４は、可動範囲θａの境界付近では関節動作のオーバシュートを抑制するとともに、可動範囲θａの境界から離れた範囲では関節動作の応答速度を（境界付近の応答速度に比べて）向上させる。即ち極配置器２４を有するロボット制御装置２０は、関節動作がオーバーシュートして可動範囲を超える、或いは可動範囲を規定するメカニカルストッパに第２リンク１３が衝突することを防止する。同時にロボット制御装置２０は、可動範囲の境界から離れた位置では関節１２の応答速度を向上させることができる。

Ｆ制御器２２は様々な制御則を採用し得る。様々な制御則には、例えば、Ｆ制御器２２は、ＰＩ制御則、ＰＩＤ制御則、或いは最適制御則がある。Ｆ制御器２２がどのような制御則を採用しようとも、Ｆ制御器２２の特性はラプラス演算子ｓの多項式（即ち伝達関数Ｇ（ｓ））で表される。Ｆ制御器２２の特性は、伝達関数Ｇ（ｓ）の多項式におけるラプラス演算子ｓの係数で定まる。従って、第１パラメータと第２パラメータは、伝達関数Ｇ（ｓ）の多項式におけるラプラス演算子ｓの係数に対応する。即ち、別言すれば極配置器２４は、特性多項式α_ｃ（ｓ）が所望の根を有するように、Ｆ制御器２２の伝達関数Ｇ（ｓ）の多項式におけるラプラス演算子ｓの各次の係数を設定する。ラプラス演算子ｓの各次の係数（第１パラメータと第２パラメータ）は、Ｆ制御器２２が採用する制御則と、ロボット１０の機構系の伝達関数Ｐ（ｓ）によって定まる。第１パラメータと第２パラメータの具体的な個数や値は、個々の具体的事例に応じて適宜に選定される。所望の極を実現するように制御器のパラメータを定めることは制御の技術分野では「極配置」或いは「極配置問題」と呼ばれている。

図４に、第２実施例のロボット制御装置１００のブロック図を示す。ロボット１０３は、第１実施例のロボット１０と同じである。即ち、本実施例でも１つの関節の制御についてのみ説明する。以下の説明は、他の関節の制御にも同様に適用可能であることに留意されたい。本実施例のロボットシステムは、指令発生器１０１が関節の目標関節角θｒをＦ制御器１０２へ送る。Ｆ制御器１０２は、計測関節角θｓが目標関節角θｒに追従するようにロボット１０３の関節を駆動する。計測関節角θｓは、関節に備えられた状態検出器１０５が計測する。

なお、より詳しくは、状態検出器１０５は、ロボット１０３のリンクの位置、速度、加速度（関節角、角速度、角加速度）などを検出する。Ｆ制御器１０２は、それらの検出値に基づいて、計測関節角θｓを目標関節角θｒに追従させるフィードバック制御を実行する。以下では、状態検出器１０５が検出する検出値群を「状態検出値」と総称する。

ロボット制御装置１００は、制御調整器１０６を備える。制御調整器１０６は、状態検出値に基づいて関節の可動範囲の限界付近である保護範囲において、ロボット１０３が破損することなく安全に動作し、且つ、可動範囲の保護領域以外の部分である非保護範囲において柔軟に動作するように、Ｆ制御器１０２のパラメータを調整する。

制御調整器１０６は、機械定数記憶器１０７、理想応答記憶器１０８、極配置器１０９、制御パラメータ演算器１１０、及び、オーバーシュート抑制器１１１を備える。機械定数記憶器１０７は、ロボット１０３の機械定数を記憶し機械定数記憶値として出力する。理想応答記憶器１０８は、ロボット１０３の理想の応答である理想応答を記憶し理想応答記憶値として出力する。極配置器１０９は、前述した状態検出値と、機械定数記憶値と、理想応答記憶値に基づいて、ロボット１０３の関節が理想の応答をするように、Ｆ制御器１０２、ロボット１０３、状態検出器１０５を含む閉ループ系の極（フィードバックループの特性多項式の根）を演算し極演算値として出力する。制御パラメータ演算器１１０は、閉ループ系の極が出力された極演算値に配置されるようなＦ制御器１０２のパラメータである制御パラメータを演算し制御パラメータ演算値として出力する。オーバーシュート抑制器１１１は、制御パラメータ演算値の時間変化が急峻な場合に、その変化を滑らかにし、Ｆ制御器１０２のパラメータ変化時にロボット１０３の関節動作がオーバーシュートしないようにする。

ロボット１０３は、第１実施例と同様に図２のとおりにモデル化される。図２に示すように第２リンク１３は第１リンク１１を基準として関節１２が有するモータにより駆動される。本実施例では、符号θｂが示す範囲を保護範囲θｂと別称する。保護範囲θｂは、可動範囲θａの限界付近に設定されており、関節１２を保護するようにＦ制御器１０２が制御する範囲である。また、符号θｃが示す範囲を非保護範囲θｃと別称する。非保護範囲θｃは、第２リンク１３の可動範囲θａのうち保護範囲θｂでない部分である。保護範囲θｂは、第１実施例の第２可動許容範囲θｂに対応する。非保護範囲θｃは、第１実施例の第１可動許容範囲θｃに対応する。また、本実施例では、可動範囲θａ以外の範囲を非可動範囲と称する。なお、以下の説明では、可動範囲θａは「０［ｒａｄ］＜θ≦π［ｒａｄ］」に設定されており、非可動範囲は「π［ｒａｄ］＜θ≦２π［ｒａｄ］」に設定されている。ここで、可動範囲と非可動範囲を表すθは、図２のθｓと同様に、図２の図面上で第１リンク１１を基準とし、反時計回りを正方向としている。

以下、本実施例に係る制御調整器１０６がオーバーシュート抑制制御パラメータ演算値を算出する仕組みの詳細を説明する。

まず、理想応答記憶器１０８が出力した理想応答記憶値に基づいて、極配置器１０９が極演算値を算出する原理を説明する。前述した閉ループ系の減衰係数をζ、固有周波数をω_ｎで表すと、立ち上がり時間ｔｒは次の（数４）で表される。

一方、閉ループ系の不足減衰時（ζ＜１）において閉ループ系の極ｓは次の（数５）で表される。なお、（数５）は前述した（数３）と同一である。

（数４）を固有周波数ω_ｎについて解き、（数５）に代入すると次の（数６）が得られる。

ここで、減衰係数ζは閉ループ系の応答の所望の収束率（（数６）の実部の大きさ）に基づいて決定する。理想応答記憶器１０８が出力する理想応答が立ち上がり時間ｔｒで設定される場合、図２の非保護範囲θｃにおいて極配置器１０９は（数６）により極演算値を算出する。

また、整定範囲をΔとすると整定時間ｔｓは次の（数７）で表される。

（数７）を固有周波数ω_ｎについて解き（数５）に代入すると次の（数８）が得られる。

ここで、減衰係数ζは閉ループ系の応答の所望の収束率（（数８）の実部の大きさ）に基づいて決定する。理想応答記憶器１０８が出力する理想応答が整定時間ｔｓで設定される場合、図２の非保護範囲θｃにおいて極配置器１０９は（数８）により極演算値を算出する。

また、ピーク時間ｔｐは次の（数９）で表される。

（数９）を固有周波数ω_ｎについて解き、（数５）に代入すると次の（数１０）が得られる。

ここで、減衰係数ζは閉ループ系の応答の所望の収束率（（数１０）の実部の大きさ）に基づいて決定する。理想応答記憶器１０８が出力する理想応答がピーク時間ｔｐで設定される場合、図２の非保護範囲θｃにおいて極配置器１０９は（数１０）により極演算値を算出する。

また、オーバーシュートＭｐは次の（数１１）で表される。

（数１１）を減衰係数ζについて解き、（数５）に代入すると次の（数１２）が得られる。

ここで、固有周波数ω_ｎは閉ループ系の応答の所望の収束率（（数１２）の実部の大きさ）に基づいて決定する。理想応答記憶器１０８が出力する理想応答がオーバーシュートＭｐで設定される場合、図２の非保護範囲θｃにおいて極配置器１０９は（数１２）により極演算値を算出する。

また、保護範囲θｂにおいて極配置器１０９は閉ループ系の応答が所望の収束率で安定となるような実数を極演算値に設定する。

次に、極配置器１０９が出力した極演算値に基づいて制御パラメータ演算器１１０が制御パラメータ演算値を算出する原理を説明する。図２の第２リンク１３の重心周りの慣性モーメントをＪ、第２リンク１３の質量である第２リンク質量をｍ、第２リンク１３の重心から関節１２までの距離である第２リンク重心位置をｌ、関節１２周りの粘性摩擦をＤ、第１リンク１１に対する第２リンク１３の角度である第２リンク角度をθｓ（計測関節角θｓ）とすると、第２リンク１３の運動方程式は次の（数１３）で表される。

ただし、（数１３）において上に２つのドットを付したθｓは角度θｓの角加速度を表し、上に１つのドットを付したθｓは角度θｓの角速度を表す。また、ｄは関節１２まわりのトルク外乱であり、Ｔｒｅｆはトルク指令である。閉ループ系の特性多項式が２次となる制御則の例として、Ｆ制御器１０２が位置Ｐ速度Ｐ制御を実施する場合について説明する。位置比例制御ゲインをＫｐ、速度比例制御ゲインをＫｖｊ、θｒを位置指令（目標関節角θｒ）とすると、トルク指令Ｔｒｅｆは次の（数１４）で表される。

（数１４）を（数１３）に代入しラプラス変換すると閉ループ系の特性多項式は次の（数１５）で与えられる。

（数１５）の固有周波数ω_ｎと減衰係数ζは次の（数１６）と（数１７）で表される。

（数１６）に（数１７）を乗算し、速度比例制御ゲインＫｖｊについて解くと次の（数１８）が得られる。

（数１８）を（数１６）に代入し位置比例制御ゲインＫｐについて解くと次の（数１９）が得られる。

ただし、前述した理想応答記憶値が立ち上がり時間ｔｒの場合、（数１８）と（数１９）の固有周波数ω_ｎは立ち上がり時間ｔｒの関数である。理想応答記憶値が整定時間ｔｓの場合、固有周波数ω_ｎは整定時間ｔｓの関数である。理想応答記憶値がピーク時間ｔｐの場合、固有周波数ω_ｎはピーク時間ｔｐの関数である。理想応答記憶値がオーバーシュートＭｐの場合、減衰係数はオーバーシュートＭｐの関数である。

オーバーシュート抑制器１１１は、前述した制御パラメータ演算値の変化が急峻な場合、（数２０）に示すようにローパスフィルタＬＰＦ（・）を適用することによりその変化を緩やかにしたオーバーシュート抑制制御パラメータ演算値Ｋを算出する。

（数２０）のオーバーシュート抑制制御パラメータ演算値ＫをＦ制御器１０２に設定することにより、ロボット１０３は図２の保護範囲θｂではオーバーシュートのない１次系の応答をする。このとき、第２リンク１３が可動限界（図２の直線Ｌ１とＬ２が示す境界）に衝突することなく安全に動作し、非保護範囲θｃではオーバーシュートは許容するがロボット１０３に理想の過渡応答特性を持つ動作をさせることができる。

本実施例では（数１３）において閉ループ系の粘性摩擦Ｄのみを考慮したが、クーロン摩擦、バックラッシなど非線形ダイナミクスも摂動法を用いて線形部分を用いるか、エネルギー損失が同じとなる等価粘性摩擦を用いることにより（数１３）と同じ式の形で表現することができ、上記と同じ手順によりロボット１０３の動作を制御できる。

上記の構成を備える本実施例のロボット制御装置１００によれば、次の効果を奏することができる。
（１）本実施例では、ロボット１０３の可動範囲θａに保護範囲θｂを設定し、保護範囲θｂにおいて、Ｆ制御器１０２、ロボット１０３、状態検出器１０５を含む閉ループ系の極が実数となるように閉ループ系の極を配置し、可動範囲θａで保護範囲θｂ以外の部分である非保護範囲θｃでは、ロボット１０３が理想の過渡応答特性を持つように閉ループ系の極を配置するので、第２リンク１３が可動限界に衝突して関節１２が故障することを防止でき、また非保護範囲θｃではロボット１０３に求められる理想の動作を実現できる。

（２）本実施例では、制御パラメータ演算器１１０によって算出された制御パラメータ演算値の変化が急峻な場合、オーバーシュート抑制器１１１が制御パラメータ演算値にローパスフィルタを適用したオーバーシュート抑制制御パラメータ演算値をＦ制御器１０２に設定するので、第２リンク１３が保護範囲θｂと非保護範囲θｃを行き来する時に、制御パラメータの切替による第２リンク１３のオーバーシュートを抑制し、ロボット１０３を滑らかに動作させることができる。

次に、上記実施例の効果を実証する実験例（シミュレーション例）を示す。第２実施例のロボット制御装置１００によってロボット１０３の関節を駆動するシミュレーションの結果を説明する。ここで、シミュレーションに用いた数値は以下のとおりである。
Ｊ＝６４．５×１０^−４[ｋｇ・ｍ^４]；
ｍ＝１１１３．２×１０^−３[ｋｇ]；
ｌ＝２６．３×１０^−２[ｍ]；
Ｊｅ＝Ｊ＋ｍ・ｌ^２；
Ｄ＝０．０１[Ｎ・ｍ・ｓ／ｒａｄ]；
θｐ＝π／５[ｒａｄ]；
Ｔ＝１×１０^−３[ｓ]；
ｔｒ＝０．１[ｓ]；
ζ＝０．１；
Ｋｐ１＝１０[ｓ^−１]；
Ｋｖｊ１＝Ｋｐ１・２π・Ｊｅ；
ωｒ＝２π／２．５[ｒａｄ／ｓ]；
ｒ＝π／２―π／２・ｃｏｓ（ωｒ・ｔ）[ｒａｄ]；

ただし、上記の左辺の記号の意味は次の通りである。
Ｊ：駆動リンク慣性モーメント
ｍ：駆動リンク質量
ｌ：駆動リンク重心位置
Ｊｅ：第２リンク１３の関節まわり慣性モーメント
Ｄ：第２リンク粘性摩擦
θｐ：保護範囲θｂの大きさ
Ｔ：制御周期
ｔｒ：立ち上がり時間
ζ：減衰係数
Ｋｐ１：保護範囲θｂにおける位置比例制御ゲイン
Ｋｖｊ１：保護範囲θｂにおける速度比例制御ゲイン
ωｒ：位置指令周波数
θｒ：位置指令（目標関節角）

本シミュレーションに用いた数値はR.F. Chandler, C.E. Clauser, J.T. McConville, H.M. Reynolds and J.W. Young, "Investigation of Inertial Properties of the Human Body", U.S. Department of Transportation, March 1975に紹介されている実測値を採用した。

極配置器１０９において（数６）を用いて極演算値を算出し、制御パラメータ演算器１１０において（数１８）と（数１９）を用いて制御パラメータ演算値（本実施例ではＫｐ２、Ｋｖｊ１とする）を算出した場合のシミュレーション結果を以下に説明する。

本シミュレーションにおいて、特許文献１の例として閉ループ系の極を実数とした場合と複素数とした場合の２つを第２実施例と比較する。ただし、実数の極は第２実施例における保護範囲θｂで設定する極と同じにし、複素数の極は第２実施例における非保護範囲θｃで設定する極と同じにする。また、位置指令θｒは第２リンク１３の可動範囲θａの半分を振幅とする正弦波とした。

図５は、第２実施例と特許文献１の比較シミュレーション結果を示すグラフである。図５において、細い実線は位置指令（目標関節角θｒ）、太い実線は第２実施例による関節１２の計測関節角θｓ、太い破線は特許文献１において閉ループ系の極を実数とした場合の計測関節角θｓ、太い一点鎖線は特許文献１において閉ループ系の極を複素数とした場合の計測関節角θｓを表す。

図５において、特許文献１で閉ループ系の極を実数とした場合（破線）はオーバーシュートしない（細い実線と交差しない）が、位置指令θｒとの偏差がある。特許文献１で閉ループ系の極を複素数とした場合（一点鎖線）はπ[ｒａｄ]となるまでは位置指令θｒによく追従しているが、π[ｒａｄ]で関節１２が伸びきった状態（可動範囲θａの限界）に到達すると、第２リンク１３はそれ以上回転できないため計測関節角θｓは一定となっている。この期間、関節１２がそれ以上回転できない方向に無理なトルクが加わり続けることとなり、関節１２の減速機の破損の原因となる可能性がある。

一方、第２実施例のロボット制御装置１００を用いた場合（太い実線）は保護範囲（０≦θ＜π／５ と４π／５≦θ≦π[ｒａｄ]）において、計測関節角度θｓがオーバーシュートせず可動範囲θａの限界に衝突することなく、また、非保護範囲（π／５≦θ＜４π／５）において、計測関節角θｓはオーバーシュートするが位置指令θｒに対する追従偏差は小さいことが分かる。また、非保護範囲θｃでは制御ゲインが比較的低い設定となるので、ロボット１０３は柔軟な動作をすることができる。

閉ループ系の特性多項式が３次となる制御則の例としてＦ制御器１０２が位置Ｐ速度ＰＩ制御を実施する場合について説明する。

本実施例においても、計測関節角をθｓ、位置指令（目標関節角）をθｒで表現する。トルク指令Ｔｒｅｆは次の（数２１）で表される。

だたし、Ｋｐは位置比例制御ゲイン、Ｋｖｊは速度比例制御ゲイン、Ｔｉは速度制御積分時定数である。（数２１）を（数１３）に代入し、ラプラス変換を適用すると閉ループ系の特性多項式は（数２２）で与えられる。

一方、極演算値を根とする多項式は一般に（数２３）で表される。

ただし「−ａ」は閉ループ系の実根であり、「ａ」で表されるモードが第２リンク１３の所望の動作に対して十分に速い応答をするように設定する。

（数２２）と（数２３）の係数比較により（数２４）から（数２６）の関係が得られる。

（数２４）を速度制御積分時定数Ｔｉについて解き、（数２５）に代入し、（数２６）を使って書き直すと次の（数２７）が得られる。

（数２７）を速度比例制御ゲインＫｖｊについて解くと次の（数２８）が得られる。

速度制御積分時定数Ｔｉは、（数２４）に（数２８）を代入して次の（数２９）で求められる。

位置比例制御ゲインＫｐは（数２６）に（数２８）を代入して次の（数３０）で求められる。

制御パラメータ演算器１１０は保護範囲θｂにおいて（数２８）から（数３０）により制御パラメータ演算値を算出する。オーバーシュート抑制器１１１の動作は第２実施例の場合と同じであるので、ここではその説明を省略する。

本実施例では閉ループ系の特性多項式が３次となる制御則の例として位置Ｐ速度ＰＩ制御を用いて説明したが、位置Ｐ速度Ｉ−Ｐ制御を用いても閉ループ系の特性多項式は（数２２）と同じになり、制御パラメータ演算器１１０は（数２８）から（数３０）により制御パラメータ演算値を算出する。

また、一般的に制御則が積分器（１／ｓ）を一つ持つ制御則を適用すると閉ループ系の特性多項式が３次となり上記と同様の方法により制御パラメータ演算値を算出できる。

このような構成を備える第３実施例によれば、次の効果を奏することができる。
（１）本実施例では、Ｆ制御器１０２に使用する制御則が積分器を１つ備える場合にも、閉ループ系の極のうち２つがロボット１０３の動作を支配するように設定するので、第２リンク１３が可動限界（可動範囲の境界）に衝突して関節１２が故障することを防止できる。また本実施例のロボット制御装置１００によれば、非保護範囲ではロボット１０３に求められる理想の動作を実現できる。
（２）本実施例では、制御パラメータ演算器１１０が制御パラメータ演算値を簡単な計算式により算出するので、計算負荷が小さく、安価なＣＰＵ（central processing unit）を搭載した制御調整器１０６のみで、第２リンク１３が可動限界に衝突して関節１２が故障することを防止できる。また本実施例のロボット制御装置１００によれば、非保護範囲ではロボット１０３に求められる理想の動作を実現できる。

第２実施例と第３実施例では、閉ループ系の特性多項式がラプラス変数ｓについて２次と３次の多項式で表される場合について説明した。本実施例では特性多項式が４次以上の場合について説明する。特性多項式がｎ次（ｎ≧４）である場合、閉ループ系の特性多項式は（数３１）のように表される。

ただし「−ａ」は閉ループ系の実根であり、ａで表されるモードが第２リンク１３の所望の動作に対して十分に速い応答をするように設定する。制御パラメータ演算器１１０は保護範囲において、（数３１）を展開した多項式と、採用した制御則の制御ゲインの関数を係数とする閉ループ系の特性多項式の係数比較により、制御パラメータ演算値を算出する。

このような構成を備える本実施例によれば、次の効果を奏することができる。
（１）本実施例では、Ｆ制御器１０２に使用する制御則が積分器を複数備える場合にも、閉ループ系の極のうち２つがロボット１０３の動作を支配するように設定するので、第２リンク１３が可動限界に衝突して関節１２が故障することを防止できる。また本実施例のロボット制御装置１００によれば、非保護範囲ではロボット１０３に求められる理想の動作を実現できる。
（２）本実施例では、制御パラメータ演算器１１０が制御パラメータ演算値を簡単な計算式により算出するので、計算負荷が小さく、安価なＣＰＵ（central processing unit）を搭載した制御調整器１０６のみで、第２リンク１３が可動限界に衝突して関節１２が故障することを防止できる。また、本実施例のロボット制御装置１００によれば、非保護範囲ではロボット１０３に求められる理想の動作を実現できる。

本明細書が開示する一つの技術によれば、第２リンク１３が可動限界（可動範囲の境界）に衝突せず関節１２が故障することを防止でき、また非保護範囲ではロボットに求められる理想の動作を、安価なＣＰＵで実現できる制御調整器１０６を備えたロボット制御装置を提供することができる。

実施例で説明したロボット制御装置の技術的特徴を整理する。本明細書が開示する技術は、一実施形態として、次のロボット制御装置を提供することができる。ロボット制御装置は、複数のリンクを備えるロボットの動作を制御するＦ制御器とそのＦ制御器のパラメータを調整する制御調整器を備える。その制御調整器は、機械定数記憶器、理想応答記憶器、極配置器、制御パラメータ演算器、及び、オーバーシュート抑制器を含む。

機械定数記憶器は、ロボットの機械定数を記憶し機械定数記憶値として出力する。理想応答記憶器は、ロボットの所望の応答を記憶し理想応答記憶値として出力する。極配置器は、ロボットの各リンクの位置、速度、加速度などの状態の検出値である状態検出値と、機械定数記憶値と、理想応答記憶値とに基づいて、Ｆ制御器と、ロボットと、前述の状態を検出する状態検出器とを含む閉ループ系の極を演算し極演算値として出力する。制御パラメータ演算器は、閉ループ系の極が前述の極演算値に一致するようなＦ制御器のパラメータを演算し、制御パラメータ演算値として出力する。オーバーシュート抑制器は、制御パラメータ演算値が急峻に変化した場合にロボットの動作がオーバーシュートしないように制御パラメータ演算値の変化を滑らかにし、オーバーシュート抑制制御パラメータ演算値としてＦ制御器に出力する。

上記のロボット制御装置において、機械定数記憶値は、ロボットの各リンクの慣性モーメントと質量とすることが好ましい。また、機械定数記憶値は、ロボットの各関節の粘性摩擦とすることが好ましい。また、機械定数記憶値は、ロボットの各関節のクーロン摩擦とすることが好ましい。また、機械定数記憶値は、ロボットの各関節を駆動するモータに連結した減速機のバックラッシとすることが好ましい。

また、理想応答記憶値は、ロボットの過渡応答とすることが好ましい。その過渡応答は、立ち上がり時間とすることが好ましい。また、過渡応答は、整定時間とすることが好ましい。さらに過渡応答は、ピーク時間とすることが好ましい。あるいは過渡応答は、オーバーシュートとすることが好ましい。

上記のロボット制御装置の極配置器は、ロボットの動作は保護範囲ではオーバーシュートせず、非保護範囲ではロボットの動作は理想の過渡特性を持つように極演算値を算出することが好ましい。或いは極配置器は、保護範囲において極演算値を実数に設定し、非保護範囲において極演算値を複素数に設定することが好ましい。

さらに極配置器は、非保護範囲において極演算値のうち２つを前述の（数６）で算出することが好ましい。あるいは極配置器は、非保護範囲において極演算値のうち２つを前述の（数８）で算出することが好ましい。また極配置器は、非保護範囲において極演算値のうち２つを前述の（数１０）で算出することが好ましい。極配置器は、非保護範囲において極演算値のうち２つを前述の（数１２）で算出してもよい。極配置器は、非保護範囲において極演算値のうち１組の複素共役極を除く残りの極を、複素共役極よりも十分に早く収束する実数極に設定してもよい。

また、上記のロボット制御装置の制御パラメータ演算器は、保護範囲と非保護範囲において異なる制御パラメータ演算値を出力することが好ましい。なお、ロボット制御装置のＦ制御器は、位置Ｐ速度Ｐ制御によりロボットの動作を制御してもよい。制御パラメータ演算器は、前述した（数１８）と（数１９）により制御パラメータ演算値を算出することも好適である。

また、Ｆ制御器は、位置Ｐ速度ＰＩ制御によりロボットの動作を制御するものであってもよい。このとき、制御パラメータ演算器は、前述の（数２７）〜（数３０）により制御パラメータ演算値を算出することが好ましい。また、Ｆ制御器は、位置Ｐ速度Ｉ−Ｐ制御によりロボットの動作を制御するものであってもよい。

また、制御パラメータ演算器は、閉ループ系の特性多項式が前述の（数３１）となるように保護範囲における制御パラメータ演算値を算出することも好適である。

また、オーバーシュート抑制器は、数式：Ｋ＝ＬＰＦ（Ｋ_０）によりオーバーシュート抑制制御パラメータ演算値Ｋを算出することも好適である。ここで、Ｋ_０は前記制御パラメータ演算値のベクトルであり、ＬＰＦ（・）はローパスフィルタを表す。

上記したロボット制御装置は、複数のリンクを備え使用者の動作を補助するロボットスーツのための制御装置に適用することも好適である。そのロボット制御装置は、ロボットスーツの動作を制御するＦ制御器と、Ｆ制御器のパラメータを調整する制御調整器を備える。

制御調整器は、機械定数記憶器、理想応答記憶器、極配置器、制御パラメータ演算器、及び、オーバーシュート抑制器を備える。機械定数記憶器は、ロボットスーツの機械定数を記憶し機械定数記憶値として出力する。理想応答記憶器は、ロボットスーツの所望の応答を記憶し理想応答記憶値として出力する。極配置器は、ロボットスーツの各リンクの位置、速度、加速度などの状態の検出値である状態検出値と、機械定数記憶値と、理想応答記憶値とに基づいて、Ｆ制御器と、ロボットスーツと、前述の状態を検出する状態検出器とを含む閉ループ系の極を演算し極演算値として出力する。制御パラメータ演算器は、閉ループ系の極が極演算値に一致するようなＦ制御器のパラメータを演算し、制御パラメータ演算値として出力する。オーバーシュート抑制器は、制御パラメータ演算値が急峻に変化した場合にロボットスーツの動作がオーバーシュートしないように制御パラメータ演算値の変化を滑らかにし、オーバーシュート抑制制御パラメータ演算値としてＦ制御器１０２に出力する。

ロボット制御装置が対象とするロボットスーツは、使用者の腕の動作を補助するものであってよい。あるいはロボットスーツは、使用者の体幹の動作を補助するものであってよい。

ロボットスーツのためのロボット制御装置において、極配置器は、ロボットスーツの動作は保護範囲ではオーバーシュートせず、非保護範囲ではロボットスーツの動作は理想の過渡特性を持つように極演算値を算出することが好ましい。また、極配置器は、保護範囲において極演算値を実数に設定し、非保護範囲において極演算値を複素数に設定することも好適である。

本明細書が開示する新規な技術は、第２リンク１３が可動限界に衝突して関節１２が故障することを防止できるロボット制御装置を提供する。またそのようなロボット制御装置は、非保護範囲ではロボット１０３に求められる理想の動作を、安価なＣＰＵで実現できる。本明細書が開示する新規な技術は、歩行補助装置などのロボットスーツ、脚部を備える自律移動ロボット、アームを備える介護ロボット、複数のリンクを備える産業用ロボットなどに広く適用できる。

実施例のコントローラに関する留意点を述べる。実施例ではロボットの伝達特性とＦ制御器の伝達関数の具体的なコンフィグレーションは示さなかった。しかし実施例で説明した技術は様々な形の伝達特性や伝達関数に適用できることに留意されたい。伝達関数や伝達特性がどのようなコンフィグレーションであってもそれらはラプラス演算子ｓの多項式の分数で表され、フィードバックループの特性多項式の根によって制御特性がオーバーシュートを生じるか否かが決定されることはよく知られている。従って、本明細書に接した当業者であれば、具体的な適用例に応じて、極配置器がＦ制御器のパラメータを具体的にどのように設定すれば本明細書が開示する利点を得られるかは十分に理解される。なお、Ｆ制御器のパラメータは、Ｆ制御器の伝達関数Ｇ（ｓ）の多項式におけるラプラス演算子ｓの係数に相当する。

実施例では１個の関節の制御系についてのみ説明した。本明細が開示する技術は複数の関節のそれぞれに適用できることに留意されたい。

ロボットの機械系の伝達特性Ｐ（ｓ）は、例えば良く知られたオブザーバなどによってリアルタイムに推定することもできる。またＦ制御器も、例えば良く知られた適応制御によってリアルタイムに変更され得る制御システムである可能性がある。そのような場合、極配置器は、リアルタイムに特定されるＰ（ｓ）やＧ（ｓ）のコンフィグレーションに応じて、特性多項式の根を実根或いは複素根に設定するためのＦ制御器のパラメータをリアルタイムに決定することも好適である。

以上、本発明の具体例を詳細に説明したが、これらは例示に過ぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成し得るものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。

１０、１０３：ロボット
１１：第１リンク
１２：関節
１３：第２リンク
２２、１０２：フィードバック制御器
２４：極配置器
２０、１００：ロボット制御装置
１０１：指令発生器
１０５：状態検出器
１０６：制御調整器
１０７：機械定数記憶器
１０８：理想応答記憶器
１０９：極配置器
１１０：制御パラメータ演算器
１１１：オーバーシュート抑制器

Claims (1)

1. リンクが関節に連結されており関節を駆動することによってリンクを揺動させるロボットの制御装置であり、
   センサによって計測された計測関節角を目標関節角に追従させるフィードバック制御器と、
   フィードバック制御器のパラメータを変更する極配置器を備えており、ロボットの機構系によって構成される閉ループ系の特性多項式の根を変更する極配置器を備えており、
   極配置器は、
   予め定められた関節の可動範囲を、可動範囲の境界を含まない第１可動許容範囲と、第１可動許容範囲の外側に設定された第２可動許容範囲とに区分して記憶しており、
   計測関節角が第１可動許容範囲内に存する場合は、フィードバック制御器とロボットの機構系によって構成されるフィードバックループ系の特性多項式の根が複素根となるようにフィードバック制御器のパラメータを変更し、
   計測関節角が第２可動許容範囲内に存する場合は前記特性多項式の根が実根となるようにフィードバック制御器のパラメータを変更することを特徴とするロボット制御装置。

Images