多関節ロボットの姿勢制御方法
【課題】ロボットの初期状態と終了状態に至る軌道生成を行わずに移動させることにより、自然な歩容及び高効率化を実現し得る多関節ロボットの姿勢制御方法を提供する。
【解決手段】多関節ロボット１の足首関節２を回転自在に設けるとともに足首関節２の回転にロータリーダンパーによって抵抗を付与し、各関節角の一般化座標ベクトルによって規定される足首関節２の回転モーメント及び角速度ベクトルをゼロにする多様体（ＺＭＭ：Zero Moment Manifold）上に安定化するように状態フィードバック制御するＺＭＭ制御を行うこととした。
【解決手段】多関節ロボット１の足首関節２を回転自在に設けるとともに足首関節２の回転にロータリーダンパーによって抵抗を付与し、各関節角の一般化座標ベクトルによって規定される足首関節２の回転モーメント及び角速度ベクトルをゼロにする多様体（ＺＭＭ：Zero Moment Manifold）上に安定化するように状態フィードバック制御するＺＭＭ制御を行うこととした。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、多関節ロボットの姿勢制御方法に関するものである。ここで、「多関節」とは、２以上の関節を指すものとする。
【背景技術】
【０００２】
従来、ロボットの姿勢制御方法として、ＺＭＰ（Zero Moment Point）を用いる方法が広く知られている（例えば、非特許文献１〜４参照）。
この種のロボットでは、足首を含む各関節にアクチュエータを配し、接地した足底面が構成する最小凸集合上にロボットの重心が常にあるように、静的あるいは準動的に制御する方法がある。これを、更に動的に拡張したものがＺＭＰである。
すなわち、足首関節にアクチュエータを設けて制御トルクを付与するが、足首関節に作用する制御トルクは他の関節に働く制御トルクに比較して一般に大きく、省エネルギー化、小型化、及びコストダウンを妨げる一要因となっていた。
そこで、足首関節にはアクチュエータを設けずに、足首の回転に負荷を与えるロータリーダンパーを設けたロボットが提案されている（例えば、特許文献１）が、この種のロボットにおいても、走行時等に倒れないように制御するためにはＺＭＰを用いる。
【非特許文献１】山口，曽我，井上，高西：“足歩行型ヒューマノイドロボットの開発−全身協調型２足動歩行制御”，第３回ロボティクスシンポジア予稿集、p.189-196，1998
【非特許文献２】横井一仁：“人間協調・共存型ロボットシステム研究開発の概要”，第１６回日本ロボット学会講演会予稿集，p.33-34，1998
【非特許文献３】広瀬，竹中，五味，小澤：“人間型ロボット”，日本ロボット学会誌，15,7,p.983-985,1997
【非特許文献４】M.Vukibratovic，（加藤，山下訳）：“歩行ロボットと人工の足”，日刊工業新聞社，1975
【特許文献１】特開２０００−１７６８６６号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、従来の姿勢制御方法では、ＺＭＰが足底面の構成する最小凸集合面上の十分内側の範囲を移動することを保証した上で、軌道制御を行うものであるため、歩容の自然さや高効率化を実現しにくいという問題があった。
【０００４】
そこで、本発明は、ＺＭＰを用いる方法とは異なり、ロボットの初期状態と終了状態に至る軌道生成を行わずに移動させることにより、自然な歩容及び高効率化を実現し得るＺＭＭ（Zero Moment Manifold、詳細は後述する）制御方法を用いた多関節ロボットの姿勢制御方法を提供することを目的とする。
【課題を解決するための手段】
【０００５】
上記目的を達成するため、本発明に係る多関節ロボットの姿勢制御方法は、足首関節が回転自在に設けられるとともに該足首関節の回転に粘性抵抗が付与された方法であって、各関節角の一般化座標ベクトルによって規定される前記足首関節の回転モーメントおよび角速度ベクトルをゼロにする多様体（ＺＭＭ：Zero Moment Manifold）上に、安定化するように状態フィードバック制御することを特徴とする。前記足首関節は、非駆動で回転自在に設けられることが好ましい。
【０００６】
また、上記目的を達成するため、本発明に係る多関節ロボットの姿勢制御方法は、足首関節が回転自在に設けられるとともに、該足首関節の回転に粘性抵抗が付与された多関節ロボットの姿勢制御方法であって、少なくとも足首より上の関節に設けたアクチュエータを制御することにより、前記多関節ロボットの合成重心が前記足首関節の回転軸鉛直線上に、かつ、静止するような目標角度に動的に収束させることを特徴とする。前記足首関節は、非駆動で回転自在に設けられることが好ましい。
本明細書における「多様体」は、数学上の概念である多様体を意味する。このことは特許請求の範囲においても同様である。
【発明の効果】
【０００７】
本発明の制御対象である多関節ロボットは、足首関節にアクチュエータを備えないため、省エネルギー化、小型化、及びコストダウンを図ることができる。
【０００８】
そして、本発明に係る姿勢制御方法は、かかる多関節ロボットを姿勢制御するに際し、各関節角の一般化座標ベクトルによって規定される前記足首関節の回転モーメント及び角速度ベクトルをゼロにする多様体上に安定化するように状態フィードバック制御を用いて、少なくとも足首より上の関節に設けたアクチュエータを制御することにより、前記多関節ロボットの合成重心が前記足首関節の回転軸鉛直線上に、かつ、静止するような目標角度に動的に収束させるため、軌道生成を行わずに、初期状態から終了状態へと移動させることができるので、自然な歩容が可能となる。
【０００９】
さらに、本発明に係るＺＭＭ制御方法は、静止時に倒れない条件（ＺＭＭ）を基に制御しているため、一見静的に見えるが、初期状態から終了状態までの経路はいかなる状態をとってもよく、ＺＭＰを用いる方法に比べ、より動的な制御則となり得る。
【発明を実施するための最良の形態】
【００１０】
本発明に係る制御方法について、最も単純な２自由度のロボットを例に挙げて説明する。
【００１１】
図１は、２自由度２関節のロボットの模式図である。このロボット１は、地面に並進移動不能で回転自在に設置した第１関節としての足首関節２と、足首関節２に回転自在に連結された第１リンク節３と、第２関節４を介して第１リンク節３と回転自在に連結された第２リンク節５とを有している。足首関節２にはアクチュエータは無く、第１リンク節３は、自由回転するが、足首関節２の回転軸と同軸にロータリーダンパー（不図示）が搭載されていて回転への負荷が与えられている。膝関節に相当する第２関節４には、アクチュエータとしての制御モータ（不図示）が取り付けられている。
【００１２】
なお、上記のようにアクチュエータを設けない方が省電力の点で優れているが、本発明に係るＺＭＭ制御方法は、足首関節２にはアクチュエータが取り付けられている多関節ロボットにも適用可能である。足首関節を速度制御することにより、ダンピング係数を微調節することなどもできる。
【００１３】
各リンク節３、５の質量をｍ１，ｍ２とし、各リンク節３，５の各重心まわりの慣性モーメントをI１-，I２-とし、各リンク節３、５の重心位置と各関節２，４との距離をｓ１，ｓ２とし、各リンク節３、５の長さをｌ１-，ｌ２-とし、右まわりを正として第１リンク節３と鉛直軸との為す角をｑ１，第１リンク節３の延長線と第２リンク節５との為す角をｑ２として、ロータリーダンパーによる粘性係数をｃ１とし、第２関節４のモータ及びギアによる粘性係数をｃ２とし、第２関節のモータのロータ及びギアによる慣性モーメントをI２０とし、ｇを重力加速度とし、このロボット１に対して、ラグランジュの運動方程式を解くことにより、ロボットの運動方程式を求めると、次のような方程式を得る。
【００１４】
【数１】
ここで、ｑ(t) ＝［ｑ１ ｑ２］Ｔは一般化座標ベクトル、ｕ(t) ∈Ｒ１は制御入力である。また、Ｈ(q)は慣性行列、
【００１５】
【数２】
の項は遠心力やコリオリ力等の非線形項、ｇ(q)は重力項である。また、b∈Ｒ２は、［0 1］Ｔである。但し、
【００１６】
【数３】
である。
【００１７】
足首関節２は非駆動であるため、静止時にこのロボット１は倒れてしまう。静止時にこのロボット１が倒れない、すなわち平衡状態になる条件は、重力の影響により各リンク節３、５が足首関節２回りに発生させるモーメントが釣り合うことである。これは、図１下図に示すようにシーソーの釣り合う原理と同様に考えることができる。これを式に表すと、下記式となる。ｇは重力加速度である。
【００１８】
【数４】
【００１９】
【数５】
である。
【００２０】
この釣り合いの条件を満足する平衡状態を漸近安定にするような状態フィードバックを創りだしたのが本発明の鍵となった。その為に、すなわち、ロボット１の全状態が作り出す４次元状態空間
【００２１】
【数６】
を考え、その上の、（２）式で表される釣り合いの条件と静止しているという条件
【００２２】
【数７】
を満足する１次元多様体Ｍ１へ、状態ベクトルを落とし込むことができる。この多様体Ｍ１は、図２のように表される。従来のＺＭＰ（Zero Moment Point）に対して、この多様体（Manifold）をＺＭＭ（Zero Moment Manifold）と呼ぶ。
【００２３】
ここで、ｑ１若しくはｑ２が決まれば、一意にｑ２またはｑ１が決定される。この多様体上であればいかなる状態をとっても静止時に倒れないという安定化された平衡状態を実現できる。
【００２４】
本発明では、軌道生成を行うこと無しにロボットを目標角度に収束させる。そのための制御入力を次に説明する。
【００２５】
上記の４次元状態空間の中の多様体Ｍ１上のある点から同じ多様体Ｍ１上へ写像する制御入力が構成できれば、図３中の解軌道のようにその間の経路は如何なる経路をたどってもよいということになる。
【００２６】
つまり、このロボット１では、初期状態と終了状態がＺＭＭを満たしていれば、初期状態から終了状態へ移る間の状態はいかなる状態をとってもよいことになる。初期状態から終了状態へ移す制御入力が見つかれば、軌道生成を行わずとも目標状態へ移すことができる。このために、ＺＭＭを満たす初期状態からＺＭＭを満たす目標状態まで移すことを具体的に以下に説明する。
【００２７】
そのＺＭＭ制御をフローチャートで表したものを図４に示す。
【００２８】
まず、任意の目標角度ｑ２ｄを与え、ＺＭＭを満たすようにｑ１ｄを導き、その目標角度ｑ１ｄ，ｑ２ｄへ移すことを考える。ｑ２を任意の角度に移すために、ｑｄ２を具体的な値として与え、ｑ１ｄを（２）式を変形した次式により求める。
【００２９】
（２）式と、三角関数の加法定理のsin(q1+q2)=sinq1cosq2+cosq1sinq2を用いると、以下の変形が行える。
【００３０】
【数８】
となる。また、（１）式の運動方程式は、以下のようにも表現することができる。
【００３１】
【数９】
ただし、一般化座標ベクトルｑ＝［ｑ１，ｑ２］Ｔ、目標角度ベクトルｑｄ＝［ｑ１ｄ，ｑ２ｄ］、Δｕは制御入力に重力補償を考えるときに制御入力ｕ(t)から重力補償項を除いたものであり、
【００３２】
【数１０】
【００３３】
【数１１】
である。この運動方程式に対して、目標角度近傍で、例えば、
【００３４】
【数１２】
の線形近似を行うことで、次の状態方程式表現を得る。
【００３５】
【数１３】
ただし、Δｑ＝ｑ−ｑｄである。
【００３６】
この線形近似モデルに基づき、目標角度へ収束するように最適レギュレータ理論（「有本卓：“線形システム理論”、産業図書、p.１４１-１５１、１９７４」、「吉川恒夫、井村順一：“現代制御論”、昭晃堂、p.１４５-１５４、２００２」）を適用する。
【００３７】
評価関数には以下の式を用いた。
【００３８】
【数１４】
但し、Ｑ１，Ｑ２∈Ｒ２×２は対象な実正定行列であり、重み行列と呼ばれる。
ここで、次のリカッチ代数方程式
【００３９】
【数１５】
を満足する唯一の実正定行列をＰとすると、
【００４０】
【数１６】
となる。目標角度における重力補償（−ｍ２ｓ２ｇsin（ｑ１ｄ＋ｑ２ｄ））との重ね合わせで、制御入力を次式のように与える。
【００４１】
【数１７】
このような制御入力を用いて制御することが、ＺＭＭ制御の一態様である。
【００４２】
なお、自由度が増えた場合でも、同様にして任意に与えた目標角度ｑｉｄ（ｉ＝２〜ｎ）からＺＭＭを満足するように目標角度ｑ１ｄを求め、その目標角度周りで線形化したシステムに対して最適レギュレータ理論を適用することでＺＭＭ制御を行うことができる。これにより、多自由度の場合でも、ｑ１以外を任意の角度に制御することができる。このＺＭＭ制御は静止時に倒れない条件（ＺＭＭ）を基に制御しているため、一見静的に見えるが、初期状態から終了状態までの経路はいかなる状態をとってもよく、ＺＭＰを用いる方法に比べ、より動的な制御則となり得る。
【００４３】
上記の２自由度ロボットに対して、ＺＭＭ制御を行ったシミュレーション結果を図５に示す。このシミュレーション時のパラメータ表を図６〜８に示す。図５に示すように、ＺＭＭを満たす初期状態から、ＺＭＭを満たす終了状態まで収束していることがわかる。
【００４４】
従って、上体の目標姿勢を次々とサンプル時間毎に設定することで、ＺＭＭ制御の状態フィードバック係数をサンプル時間毎に調節し、これをつなぎ合わせることで、上体の振り付けが自由に出来るようになる。目標角度を変更する場合の制御フローチャートを図９に示す。こうして、単なる指定した単純作業だけでなく、踊りや演技も可能となる。このロボット１にＺＭＭ制御を適用して、５００ｍｓ及び２００ｍｓ毎に姿勢を変えたシミュレーション結果を図１０、図１１に示す。図１０の時間応答を見ると、目標姿勢に追従しているが、滑らかではない部分がある。この原因として、状態フィードバックゲインを不連続に与えていることが考えられる。ここで、過去の目標状態における状態フィードバックゲインと現在の状態フィードバックゲイン、及び未来の状態フィードバックゲインから、線形補間やスプライン補間等の適当な補間を行うことで、状態フィードバックゲインを連続にすると、より滑らかな時間応答を得ることができる。実際、図１１は目標姿勢を２００ｍｓ毎に与えたシミュレーション結果であるが、５００ｍｓ毎に目標姿勢を与えた場合と比べ、状態フィードバックゲインがより連続的な値をとるので、時間応答が滑らかになっている。ここで、前記の状態フィードバックゲインの補間が有効であると考えられる。
【００４５】
上記の実施形態では、状態フィードバック係数の導出に最適レギュレータ理論を用いたが、他のフィードバック安定化法を採用することもできる。また、上記実施形態では、片足設置したロボットについて説明したが、両足設置したロボットへの適用も可能であり、歩行ロボットへの適用も可能である。
【実施例】
【００４６】
図１２は、実際に製作したロボットを示す、正面図（ａ）、側面図（ｂ）、及び斜視図（ｃ）である。尚、図１と同様の構成部分には、同符号を付している。
図１２に示すロボット１’は、３自由度を持つロボットであり、第２リンク節５と第３関節６を解して第３リンク節７と連結されている。第２リンク節５及び第３リンク節７にそれぞれ１個ずつモータ８，９を搭載し、第２関節４及び第３関節を駆動する。足首関節２を軸支する軸受け部１０は、載置面に固定されている。
【００４７】
足首関節２には、これを駆動するモータは無く、ロータリーダンパー１１が同軸上に設けられている。ロータリーダンパー１１は、オイルの粘性抵抗により発生する制動力（ブレーキカ）を利用した回転系のダンパーである。ロータリーダンパーに代えて、オイルの圧力を利用した揺動ダンパーや、ＥＲ流体（電気粘性流体）等を利用したダンパー、その他、足首関節の回転に抵抗を付与するために、公知のダンパーを使用できる。
各関節には、図示しないセンサーが取り付けられており、各関節の回転角度、角速度が検出され、検出信号が図外のコントローラに送信されている。
【００４８】
この３自由度体操ロボット１’にＺＭＭ制御を適用した際のシミュレーション結果及び実験結果を図１３〜１５に示す。また、このシミュレーション及び実験のパラメータ表を図１６〜図１８に示す。図１３に示すシミュレーションから、足首，膝，腰等を考慮した３自由度のモデルについて安定に制御できることが分かる。図１４の実験結果は、シミュレーションに対し定誤差が残るものの目標角度近傍に収束している。この誤差は，駆動系におけるクーロン摩擦などの非線形要素の影響を受けることが原因と考えられる。
【００４９】
さらに、図１５はこのロボット１’にＺＭＭ制御を適用して、５００ｍｓ毎に姿勢を変えた実験結果であり、制御ループは１ｍｓである。時間応答を見ると、目標姿勢に追従しているが、滑らかではない部分がある。これはギアのバックラッシュの影響も考えられるが、状態フィードバックゲインが不連続であることも原因と考えられる。ここで、前記の状態フィードバックゲインの補間を行うことでより滑らかな応答が得られる。
【図面の簡単な説明】
【００５０】
【図１】本発明に係る姿勢制御方法を適用したロボットの構成を示す模式図である。
【図２】一次元多様体を説明するグラフである。
【図３】多様体を説明する座標である。
【図４】本発明に係るＺＭＭ制御方法の制御流れを示すフローチャートである。
【図５】図１のロボットに本発明方法を適用した場合のシミュレーション結果を示すグラフである。
【図６】図５のシミュレーションのパラメータ表である。
【図７】図５のシミュレーションのゲインパラメータである。
【図８】図５のシミュレーションの初期角度及び目標角度を示すパラメータ表である。
【図９】本発明に係るＺＭＭ制御方法の他の制御流れを示すフローチャートである。
【図１０】図１のロボットに本発明方法を適用して、時間とともに姿勢を変えたシミュレーション結果を示すグラフである。
【図１１】図１のロボットに本発明方法を適用して、時間とともに姿勢を変えたシミュレーション結果を示す他のグラフである。
【図１２】本発明制御方法を適用したロボットを示し、図１２（ａ）は直立状態の正面図、図１２（ｂ）は直立状態の側面図、図１２（ｃ）は関節を曲げた状態の斜視図である。
【図１３】図１２のロボットに本発明方法を適用した場合のシミュレーションを示すグラフである。
【図１４】図１２のロボットに本発明方法を適用した場合の実験結果を示すグラフである。
【図１５】図１２のロボットに本発明方法を適用して、時間とともに姿勢を変えた実験結果を示すグラフである。
【図１６】図１３に示すシミュレーション及び図１４、１５に示す実験のパラメータ表である。
【図１７】図１３のシミュレーション及び図１４、１５に示す実験のゲインパラメータ表である。
【図１８】図１３のシミュレーション及び図１４に示す実験の初期角度及び目標角度を示すパラメータ表である。
【符号の説明】
【００５１】
１、１’ ロボット
２ 足首関節
３ 第１リンク節
４ 第２関節
５ 第２リンク節
６ 第３関節
７ 第３リンク節
１１ ロータリーダンパー
【技術分野】
【０００１】
本発明は、多関節ロボットの姿勢制御方法に関するものである。ここで、「多関節」とは、２以上の関節を指すものとする。
【背景技術】
【０００２】
従来、ロボットの姿勢制御方法として、ＺＭＰ（Zero Moment Point）を用いる方法が広く知られている（例えば、非特許文献１〜４参照）。
この種のロボットでは、足首を含む各関節にアクチュエータを配し、接地した足底面が構成する最小凸集合上にロボットの重心が常にあるように、静的あるいは準動的に制御する方法がある。これを、更に動的に拡張したものがＺＭＰである。
すなわち、足首関節にアクチュエータを設けて制御トルクを付与するが、足首関節に作用する制御トルクは他の関節に働く制御トルクに比較して一般に大きく、省エネルギー化、小型化、及びコストダウンを妨げる一要因となっていた。
そこで、足首関節にはアクチュエータを設けずに、足首の回転に負荷を与えるロータリーダンパーを設けたロボットが提案されている（例えば、特許文献１）が、この種のロボットにおいても、走行時等に倒れないように制御するためにはＺＭＰを用いる。
【非特許文献１】山口，曽我，井上，高西：“足歩行型ヒューマノイドロボットの開発−全身協調型２足動歩行制御”，第３回ロボティクスシンポジア予稿集、p.189-196，1998
【非特許文献２】横井一仁：“人間協調・共存型ロボットシステム研究開発の概要”，第１６回日本ロボット学会講演会予稿集，p.33-34，1998
【非特許文献３】広瀬，竹中，五味，小澤：“人間型ロボット”，日本ロボット学会誌，15,7,p.983-985,1997
【非特許文献４】M.Vukibratovic，（加藤，山下訳）：“歩行ロボットと人工の足”，日刊工業新聞社，1975
【特許文献１】特開２０００−１７６８６６号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、従来の姿勢制御方法では、ＺＭＰが足底面の構成する最小凸集合面上の十分内側の範囲を移動することを保証した上で、軌道制御を行うものであるため、歩容の自然さや高効率化を実現しにくいという問題があった。
【０００４】
そこで、本発明は、ＺＭＰを用いる方法とは異なり、ロボットの初期状態と終了状態に至る軌道生成を行わずに移動させることにより、自然な歩容及び高効率化を実現し得るＺＭＭ（Zero Moment Manifold、詳細は後述する）制御方法を用いた多関節ロボットの姿勢制御方法を提供することを目的とする。
【課題を解決するための手段】
【０００５】
上記目的を達成するため、本発明に係る多関節ロボットの姿勢制御方法は、足首関節が回転自在に設けられるとともに該足首関節の回転に粘性抵抗が付与された方法であって、各関節角の一般化座標ベクトルによって規定される前記足首関節の回転モーメントおよび角速度ベクトルをゼロにする多様体（ＺＭＭ：Zero Moment Manifold）上に、安定化するように状態フィードバック制御することを特徴とする。前記足首関節は、非駆動で回転自在に設けられることが好ましい。
【０００６】
また、上記目的を達成するため、本発明に係る多関節ロボットの姿勢制御方法は、足首関節が回転自在に設けられるとともに、該足首関節の回転に粘性抵抗が付与された多関節ロボットの姿勢制御方法であって、少なくとも足首より上の関節に設けたアクチュエータを制御することにより、前記多関節ロボットの合成重心が前記足首関節の回転軸鉛直線上に、かつ、静止するような目標角度に動的に収束させることを特徴とする。前記足首関節は、非駆動で回転自在に設けられることが好ましい。
本明細書における「多様体」は、数学上の概念である多様体を意味する。このことは特許請求の範囲においても同様である。
【発明の効果】
【０００７】
本発明の制御対象である多関節ロボットは、足首関節にアクチュエータを備えないため、省エネルギー化、小型化、及びコストダウンを図ることができる。
【０００８】
そして、本発明に係る姿勢制御方法は、かかる多関節ロボットを姿勢制御するに際し、各関節角の一般化座標ベクトルによって規定される前記足首関節の回転モーメント及び角速度ベクトルをゼロにする多様体上に安定化するように状態フィードバック制御を用いて、少なくとも足首より上の関節に設けたアクチュエータを制御することにより、前記多関節ロボットの合成重心が前記足首関節の回転軸鉛直線上に、かつ、静止するような目標角度に動的に収束させるため、軌道生成を行わずに、初期状態から終了状態へと移動させることができるので、自然な歩容が可能となる。
【０００９】
さらに、本発明に係るＺＭＭ制御方法は、静止時に倒れない条件（ＺＭＭ）を基に制御しているため、一見静的に見えるが、初期状態から終了状態までの経路はいかなる状態をとってもよく、ＺＭＰを用いる方法に比べ、より動的な制御則となり得る。
【発明を実施するための最良の形態】
【００１０】
本発明に係る制御方法について、最も単純な２自由度のロボットを例に挙げて説明する。
【００１１】
図１は、２自由度２関節のロボットの模式図である。このロボット１は、地面に並進移動不能で回転自在に設置した第１関節としての足首関節２と、足首関節２に回転自在に連結された第１リンク節３と、第２関節４を介して第１リンク節３と回転自在に連結された第２リンク節５とを有している。足首関節２にはアクチュエータは無く、第１リンク節３は、自由回転するが、足首関節２の回転軸と同軸にロータリーダンパー（不図示）が搭載されていて回転への負荷が与えられている。膝関節に相当する第２関節４には、アクチュエータとしての制御モータ（不図示）が取り付けられている。
【００１２】
なお、上記のようにアクチュエータを設けない方が省電力の点で優れているが、本発明に係るＺＭＭ制御方法は、足首関節２にはアクチュエータが取り付けられている多関節ロボットにも適用可能である。足首関節を速度制御することにより、ダンピング係数を微調節することなどもできる。
【００１３】
各リンク節３、５の質量をｍ１，ｍ２とし、各リンク節３，５の各重心まわりの慣性モーメントをI１-，I２-とし、各リンク節３、５の重心位置と各関節２，４との距離をｓ１，ｓ２とし、各リンク節３、５の長さをｌ１-，ｌ２-とし、右まわりを正として第１リンク節３と鉛直軸との為す角をｑ１，第１リンク節３の延長線と第２リンク節５との為す角をｑ２として、ロータリーダンパーによる粘性係数をｃ１とし、第２関節４のモータ及びギアによる粘性係数をｃ２とし、第２関節のモータのロータ及びギアによる慣性モーメントをI２０とし、ｇを重力加速度とし、このロボット１に対して、ラグランジュの運動方程式を解くことにより、ロボットの運動方程式を求めると、次のような方程式を得る。
【００１４】
【数１】
ここで、ｑ(t) ＝［ｑ１ ｑ２］Ｔは一般化座標ベクトル、ｕ(t) ∈Ｒ１は制御入力である。また、Ｈ(q)は慣性行列、
【００１５】
【数２】
の項は遠心力やコリオリ力等の非線形項、ｇ(q)は重力項である。また、b∈Ｒ２は、［0 1］Ｔである。但し、
【００１６】
【数３】
である。
【００１７】
足首関節２は非駆動であるため、静止時にこのロボット１は倒れてしまう。静止時にこのロボット１が倒れない、すなわち平衡状態になる条件は、重力の影響により各リンク節３、５が足首関節２回りに発生させるモーメントが釣り合うことである。これは、図１下図に示すようにシーソーの釣り合う原理と同様に考えることができる。これを式に表すと、下記式となる。ｇは重力加速度である。
【００１８】
【数４】
【００１９】
【数５】
である。
【００２０】
この釣り合いの条件を満足する平衡状態を漸近安定にするような状態フィードバックを創りだしたのが本発明の鍵となった。その為に、すなわち、ロボット１の全状態が作り出す４次元状態空間
【００２１】
【数６】
を考え、その上の、（２）式で表される釣り合いの条件と静止しているという条件
【００２２】
【数７】
を満足する１次元多様体Ｍ１へ、状態ベクトルを落とし込むことができる。この多様体Ｍ１は、図２のように表される。従来のＺＭＰ（Zero Moment Point）に対して、この多様体（Manifold）をＺＭＭ（Zero Moment Manifold）と呼ぶ。
【００２３】
ここで、ｑ１若しくはｑ２が決まれば、一意にｑ２またはｑ１が決定される。この多様体上であればいかなる状態をとっても静止時に倒れないという安定化された平衡状態を実現できる。
【００２４】
本発明では、軌道生成を行うこと無しにロボットを目標角度に収束させる。そのための制御入力を次に説明する。
【００２５】
上記の４次元状態空間の中の多様体Ｍ１上のある点から同じ多様体Ｍ１上へ写像する制御入力が構成できれば、図３中の解軌道のようにその間の経路は如何なる経路をたどってもよいということになる。
【００２６】
つまり、このロボット１では、初期状態と終了状態がＺＭＭを満たしていれば、初期状態から終了状態へ移る間の状態はいかなる状態をとってもよいことになる。初期状態から終了状態へ移す制御入力が見つかれば、軌道生成を行わずとも目標状態へ移すことができる。このために、ＺＭＭを満たす初期状態からＺＭＭを満たす目標状態まで移すことを具体的に以下に説明する。
【００２７】
そのＺＭＭ制御をフローチャートで表したものを図４に示す。
【００２８】
まず、任意の目標角度ｑ２ｄを与え、ＺＭＭを満たすようにｑ１ｄを導き、その目標角度ｑ１ｄ，ｑ２ｄへ移すことを考える。ｑ２を任意の角度に移すために、ｑｄ２を具体的な値として与え、ｑ１ｄを（２）式を変形した次式により求める。
【００２９】
（２）式と、三角関数の加法定理のsin(q1+q2)=sinq1cosq2+cosq1sinq2を用いると、以下の変形が行える。
【００３０】
【数８】
となる。また、（１）式の運動方程式は、以下のようにも表現することができる。
【００３１】
【数９】
ただし、一般化座標ベクトルｑ＝［ｑ１，ｑ２］Ｔ、目標角度ベクトルｑｄ＝［ｑ１ｄ，ｑ２ｄ］、Δｕは制御入力に重力補償を考えるときに制御入力ｕ(t)から重力補償項を除いたものであり、
【００３２】
【数１０】
【００３３】
【数１１】
である。この運動方程式に対して、目標角度近傍で、例えば、
【００３４】
【数１２】
の線形近似を行うことで、次の状態方程式表現を得る。
【００３５】
【数１３】
ただし、Δｑ＝ｑ−ｑｄである。
【００３６】
この線形近似モデルに基づき、目標角度へ収束するように最適レギュレータ理論（「有本卓：“線形システム理論”、産業図書、p.１４１-１５１、１９７４」、「吉川恒夫、井村順一：“現代制御論”、昭晃堂、p.１４５-１５４、２００２」）を適用する。
【００３７】
評価関数には以下の式を用いた。
【００３８】
【数１４】
但し、Ｑ１，Ｑ２∈Ｒ２×２は対象な実正定行列であり、重み行列と呼ばれる。
ここで、次のリカッチ代数方程式
【００３９】
【数１５】
を満足する唯一の実正定行列をＰとすると、
【００４０】
【数１６】
となる。目標角度における重力補償（−ｍ２ｓ２ｇsin（ｑ１ｄ＋ｑ２ｄ））との重ね合わせで、制御入力を次式のように与える。
【００４１】
【数１７】
このような制御入力を用いて制御することが、ＺＭＭ制御の一態様である。
【００４２】
なお、自由度が増えた場合でも、同様にして任意に与えた目標角度ｑｉｄ（ｉ＝２〜ｎ）からＺＭＭを満足するように目標角度ｑ１ｄを求め、その目標角度周りで線形化したシステムに対して最適レギュレータ理論を適用することでＺＭＭ制御を行うことができる。これにより、多自由度の場合でも、ｑ１以外を任意の角度に制御することができる。このＺＭＭ制御は静止時に倒れない条件（ＺＭＭ）を基に制御しているため、一見静的に見えるが、初期状態から終了状態までの経路はいかなる状態をとってもよく、ＺＭＰを用いる方法に比べ、より動的な制御則となり得る。
【００４３】
上記の２自由度ロボットに対して、ＺＭＭ制御を行ったシミュレーション結果を図５に示す。このシミュレーション時のパラメータ表を図６〜８に示す。図５に示すように、ＺＭＭを満たす初期状態から、ＺＭＭを満たす終了状態まで収束していることがわかる。
【００４４】
従って、上体の目標姿勢を次々とサンプル時間毎に設定することで、ＺＭＭ制御の状態フィードバック係数をサンプル時間毎に調節し、これをつなぎ合わせることで、上体の振り付けが自由に出来るようになる。目標角度を変更する場合の制御フローチャートを図９に示す。こうして、単なる指定した単純作業だけでなく、踊りや演技も可能となる。このロボット１にＺＭＭ制御を適用して、５００ｍｓ及び２００ｍｓ毎に姿勢を変えたシミュレーション結果を図１０、図１１に示す。図１０の時間応答を見ると、目標姿勢に追従しているが、滑らかではない部分がある。この原因として、状態フィードバックゲインを不連続に与えていることが考えられる。ここで、過去の目標状態における状態フィードバックゲインと現在の状態フィードバックゲイン、及び未来の状態フィードバックゲインから、線形補間やスプライン補間等の適当な補間を行うことで、状態フィードバックゲインを連続にすると、より滑らかな時間応答を得ることができる。実際、図１１は目標姿勢を２００ｍｓ毎に与えたシミュレーション結果であるが、５００ｍｓ毎に目標姿勢を与えた場合と比べ、状態フィードバックゲインがより連続的な値をとるので、時間応答が滑らかになっている。ここで、前記の状態フィードバックゲインの補間が有効であると考えられる。
【００４５】
上記の実施形態では、状態フィードバック係数の導出に最適レギュレータ理論を用いたが、他のフィードバック安定化法を採用することもできる。また、上記実施形態では、片足設置したロボットについて説明したが、両足設置したロボットへの適用も可能であり、歩行ロボットへの適用も可能である。
【実施例】
【００４６】
図１２は、実際に製作したロボットを示す、正面図（ａ）、側面図（ｂ）、及び斜視図（ｃ）である。尚、図１と同様の構成部分には、同符号を付している。
図１２に示すロボット１’は、３自由度を持つロボットであり、第２リンク節５と第３関節６を解して第３リンク節７と連結されている。第２リンク節５及び第３リンク節７にそれぞれ１個ずつモータ８，９を搭載し、第２関節４及び第３関節を駆動する。足首関節２を軸支する軸受け部１０は、載置面に固定されている。
【００４７】
足首関節２には、これを駆動するモータは無く、ロータリーダンパー１１が同軸上に設けられている。ロータリーダンパー１１は、オイルの粘性抵抗により発生する制動力（ブレーキカ）を利用した回転系のダンパーである。ロータリーダンパーに代えて、オイルの圧力を利用した揺動ダンパーや、ＥＲ流体（電気粘性流体）等を利用したダンパー、その他、足首関節の回転に抵抗を付与するために、公知のダンパーを使用できる。
各関節には、図示しないセンサーが取り付けられており、各関節の回転角度、角速度が検出され、検出信号が図外のコントローラに送信されている。
【００４８】
この３自由度体操ロボット１’にＺＭＭ制御を適用した際のシミュレーション結果及び実験結果を図１３〜１５に示す。また、このシミュレーション及び実験のパラメータ表を図１６〜図１８に示す。図１３に示すシミュレーションから、足首，膝，腰等を考慮した３自由度のモデルについて安定に制御できることが分かる。図１４の実験結果は、シミュレーションに対し定誤差が残るものの目標角度近傍に収束している。この誤差は，駆動系におけるクーロン摩擦などの非線形要素の影響を受けることが原因と考えられる。
【００４９】
さらに、図１５はこのロボット１’にＺＭＭ制御を適用して、５００ｍｓ毎に姿勢を変えた実験結果であり、制御ループは１ｍｓである。時間応答を見ると、目標姿勢に追従しているが、滑らかではない部分がある。これはギアのバックラッシュの影響も考えられるが、状態フィードバックゲインが不連続であることも原因と考えられる。ここで、前記の状態フィードバックゲインの補間を行うことでより滑らかな応答が得られる。
【図面の簡単な説明】
【００５０】
【図１】本発明に係る姿勢制御方法を適用したロボットの構成を示す模式図である。
【図２】一次元多様体を説明するグラフである。
【図３】多様体を説明する座標である。
【図４】本発明に係るＺＭＭ制御方法の制御流れを示すフローチャートである。
【図５】図１のロボットに本発明方法を適用した場合のシミュレーション結果を示すグラフである。
【図６】図５のシミュレーションのパラメータ表である。
【図７】図５のシミュレーションのゲインパラメータである。
【図８】図５のシミュレーションの初期角度及び目標角度を示すパラメータ表である。
【図９】本発明に係るＺＭＭ制御方法の他の制御流れを示すフローチャートである。
【図１０】図１のロボットに本発明方法を適用して、時間とともに姿勢を変えたシミュレーション結果を示すグラフである。
【図１１】図１のロボットに本発明方法を適用して、時間とともに姿勢を変えたシミュレーション結果を示す他のグラフである。
【図１２】本発明制御方法を適用したロボットを示し、図１２（ａ）は直立状態の正面図、図１２（ｂ）は直立状態の側面図、図１２（ｃ）は関節を曲げた状態の斜視図である。
【図１３】図１２のロボットに本発明方法を適用した場合のシミュレーションを示すグラフである。
【図１４】図１２のロボットに本発明方法を適用した場合の実験結果を示すグラフである。
【図１５】図１２のロボットに本発明方法を適用して、時間とともに姿勢を変えた実験結果を示すグラフである。
【図１６】図１３に示すシミュレーション及び図１４、１５に示す実験のパラメータ表である。
【図１７】図１３のシミュレーション及び図１４、１５に示す実験のゲインパラメータ表である。
【図１８】図１３のシミュレーション及び図１４に示す実験の初期角度及び目標角度を示すパラメータ表である。
【符号の説明】
【００５１】
１、１’ ロボット
２ 足首関節
３ 第１リンク節
４ 第２関節
５ 第２リンク節
６ 第３関節
７ 第３リンク節
１１ ロータリーダンパー
【特許請求の範囲】
【請求項１】
足首関節が回転自在に設けられるとともに、該足首関節の回転に粘性抵抗が付与された多関節ロボットの姿勢制御方法であって、
各関節角の一般化座標ベクトルによって規定される前記足首関節の回転モーメントおよび角速度ベクトルをゼロにする多様体（ＺＭＭ：Zero Moment Manifold）上に、安定化するように状態フィードバック制御することを特徴とする多関節ロボットの姿勢制御方法。
【請求項２】
前記足首関節が、非駆動で回転自在に設けられることを特徴とする請求項１記載の多関節ロボットの姿勢制御方法。
【請求項３】
足首関節が回転自在に設けられるとともに、該足首関節の回転に粘性抵抗が付与された多関節ロボットの姿勢制御方法であって、
少なくとも足首より上の関節に設けたアクチュエータを制御することにより、前記多関節ロボットの足首より上の合成重心が前記足首関節の回転軸鉛直線上に、かつ、静止するような目標角度に動的に収束させることを特徴とする多関節ロボットの姿勢制御方法。
【請求項４】
前記足首関節が、非駆動で回転自在に設けられることを特徴とする請求項３記載の多関節ロボットの姿勢制御方法。
【請求項１】
足首関節が回転自在に設けられるとともに、該足首関節の回転に粘性抵抗が付与された多関節ロボットの姿勢制御方法であって、
各関節角の一般化座標ベクトルによって規定される前記足首関節の回転モーメントおよび角速度ベクトルをゼロにする多様体（ＺＭＭ：Zero Moment Manifold）上に、安定化するように状態フィードバック制御することを特徴とする多関節ロボットの姿勢制御方法。
【請求項２】
前記足首関節が、非駆動で回転自在に設けられることを特徴とする請求項１記載の多関節ロボットの姿勢制御方法。
【請求項３】
足首関節が回転自在に設けられるとともに、該足首関節の回転に粘性抵抗が付与された多関節ロボットの姿勢制御方法であって、
少なくとも足首より上の関節に設けたアクチュエータを制御することにより、前記多関節ロボットの足首より上の合成重心が前記足首関節の回転軸鉛直線上に、かつ、静止するような目標角度に動的に収束させることを特徴とする多関節ロボットの姿勢制御方法。
【請求項４】
前記足首関節が、非駆動で回転自在に設けられることを特徴とする請求項３記載の多関節ロボットの姿勢制御方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１２】
【図１６】
【図１７】
【図１８】
【図１０】
【図１１】
【図１３】
【図１４】
【図１５】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１２】
【図１６】
【図１７】
【図１８】
【図１０】
【図１１】
【図１３】
【図１４】
【図１５】
【公開番号】特開２００６−７５９４５（Ｐ２００６−７５９４５Ａ）
【公開日】平成１８年３月２３日（２００６．３．２３）
【国際特許分類】
【出願番号】特願２００４−２６３１１３（Ｐ２００４−２６３１１３）
【出願日】平成１６年９月９日（２００４．９．９）
【出願人】（５９３００６６３０）学校法人立命館 (359)
【Ｆターム（参考）】
【公開日】平成１８年３月２３日（２００６．３．２３）
【国際特許分類】
【出願日】平成１６年９月９日（２００４．９．９）
【出願人】（５９３００６６３０）学校法人立命館 (359)
【Ｆターム（参考）】
[ Back to top ]