(57)【特許請求の範囲】
【請求項１】
通常の軌道追従型制御系とリシーディング・ホライゾン制御系をハイブリッド化し、該リシーディングホライゾン制御系の出力を逐次積分して、最適軌道を逐次計算し、これに追従させるよう構成すると共に、そのリシーディング・ホライゾン制御系に、
【数１】
0003559851.tif 000019
によって与えられるＦ値を監視する監視要素を設け、Ｆ値がある値を越えたときにスイッチ要素の作動により通常の制御系にスイッチし、状態量のリアルタイム最適制御を支援する、
ことを特徴とするリアルタイム最適制御方法。
【請求項２】
状態量が目標値の近傍に到達したときに適切な制御則に切替える、
ことを特徴とする請求項１に記載のリアルタイム最適制御方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、リシーディング・ホライゾン制御（Ｒｅｃｅｄｉｎｇ  Ｈｏｒｉｚｏｎ  Ｃｏｎｔｒｏｌ：以下、ＲＨＣと略記する。）を適用した制御系、例えば、マニピュレーターや脚式ロボットのような機械的リンク系の制御のためのリアルタイム最適制御方法に関するものである。勿論、これ以外の分野のリアルタイム制御が必要な制御対象にも適用することができる。
【０００２】
【従来の技術】
マニピュレーターや脚式ロボットのような機械的リンク系等の運動軌道計算では、何らかの評価関数に従って最適制御問題を解く必要がある。従来、このような問題には、勾配法に代表されるような、繰り返し計算によって求解が行われてきた。しかし、オフライン計算において、この種の計算は、収束計算に莫大な計算時間を要する。つまり、長い評価区間全体にわたって勾配を計算し、繰り返し計算によって最適解を探索するため、収束に時間を要し、時には探索に失敗することもある。
【０００３】
一方、勾配を用いない最適解法が現実的なものとなりつつある。これは、比較的短い評価区間を移動させるＲＨＣに、連続変形法を応用したもので、この手法では勾配を使用しないため、収束計算の必要がなく、比較的短時間の計算で解を得ることができる。このため、ＲＨＣは、リアルタイムに最適解を生成することが可能である。しかし、ＲＨＣはまだ新しい理論であるため、実際に実験が行われた例は少なく、また実験の際にＦ値が発散し易い傾向がある。
従来までのＲＨＣでは、最適解である最適入力を直接制御対象に入力するものであった。このため、ＲＨＣのリアルタイム計算が、Ｆ値算出の際の数値誤差に起因する解の発散により、最適出力も発散し、従って制御対象に過大な入力が入り、実験を行う場合に危険な場合もあり得た。
【０００４】
【発明が解決しようとする課題】
本発明者は、このようなＲＨＣの利点を生かして、機械的リンク系の最適軌道生成問題等に応用することを試みた。この場合に、リンク系の非線形性はすべて考慮されるとする。このような実時間（リアルタイム）最適化手法は、マニピュレーターの複雑な動作教示や、脚式ロボットの最適な脚軌道の生成等に応用可能である。
さらに具体的には、本発明者は、軌道追従型ＲＨＣを提案し、ＲＨＣを実際に鉛直型１リンク系に適用し、実時間（リアルタイム）制御することによって実用上必要な事項について検討したが、本発明は、その検討の結果に基づくものである。
【０００５】
すなわち、本発明の技術的課題は、マニピュレーターや脚式ロボットのような機械的リンク系等に軌道追従型ＲＨＣを適用し、勾配による繰り返し計算の必要をなくした制御において、Ｆ値が発散することにより最適入力が発散し、あるいはＦ値が発散しないまでも値がかなり大きいと、得られる最適解も発散または振動的な解となる可能性があるという問題を解消し、安全にリアルタイムに最適解を生成できるようにした制御方法を提供することにある。
【０００６】
【課題を解決するための手段】
上記課題を解決するための本発明のリアルタイム最適制御方法は、通常のＲＨＣの使い方と異なって、ＲＨＣの最適出力をそのまま使わずに、これを逐次積分し、位置、速度を算出し、これらを目標軌道とみなすことである。また、Ｆ値を監視する監視要素を設け、Ｆ値がある値を越えたときにそのときの位置、速度を維持するような機構を備えておく。
また、上記ＲＨＣでは、不安定平衡点を目標値とする場合に、一旦目標に達してしまうと、制御入力が０になってしまうために、不安定平衡点からかなり離れた状態までずれた時点で再び最適入力が生成され、再び目標値に復帰するという動作を繰返すことがある。そこで、状態量が目標値またはその近傍に達したときに、適切な従来の制御法（例えば、最適レギュレータやＰＩＤ制御）に切替えるのが、このような現象の繰り返しを抑止するために有効である。
【０００７】
このような本発明のリアルタイム最適制御方法によれば、マニピュレーターや脚式ロボットのような機械的リンク系等のＲＨＣ制御において、最適解が発散したときの危険をおかすことなく、制御することができる。
【０００８】
【発明の実施の形態】
従来のＲＨＣについてはいろいろな文献があるが、以下、ＲＨＣについて簡単に説明する。
まず、ｘを状態変数、ｕを制御入力、ｔを時間とし、ＲＨＣのリアルタイム制御の制御対象の状態方程式を、
【数２】
0003559851.tif 000002
とする。ｆは、必要なだけの微分可能性を有する関数とする。ここで、評価区間は有限であって、かつ、フィードバック制御が有限時間に終わらないような最適制御を考えて以下のような評価区間の移動する評価関数Ｊを定義する。
【０００９】
【数３】
0003559851.tif 000003
これは現在の状態を初期初期状態としてＴだけ未来までの最適制御を求める問題である。φは、終端拘束条件である。
【００１０】
さて、ある時刻ｔを初期状態とした仮想的時間変数τを導入して、
【数４】
0003559851.tif 000004
を得る。
【００１１】
この問題は、ボルザ（Ｂｏｌｚａ）  型の問題と呼ばれる。ここでは、新しい時間軸τにおける変数に（＊）  をつけ、区別することにする。
時間軸τに沿った最適制御ｕ＊（τ，ｔ）  が満たすべき必要条件は、変分法により以下のような２点境界値問題（ＴＰＢＶＰ）  として与えられる。
【数５】
0003559851.tif 000005
【００１２】
ここで、λ＊  はτ軸上の共状態変数である。また、右下付き文字は偏微分を意味する表記として用いている。Ｈは、
【数６】
0003559851.tif 000006
で定義されたハミルトニアン（Ｈａｍｉｌｔｏｎｉａｎ）  である。
【００１３】
与えられたｘ＊（０，ｔ）＝ｘ（ｔ）  に対してλ＊（０，ｔ）（これを以下λ（ｔ）  と記す。）を選ぶと（６）〜（１０）式により、τ軸上のｘ＊（τ，ｔ）  とλ＊（τ，ｔ）  が決定される。（９）式の境界条件より、τ軸上の時間Ｔにおいて、
【数７】
0003559851.tif 000007
が成り立たねばならない。
【００１４】
時間的にＦを０に安定化しながら最適解λ（ｔ）  を追跡するために、
【数８】
0003559851.tif 000008
などとする。ここでＡ１  は、適当に選ばれた安定行列とする。このように解を追跡していく方法を連続変形法という。（１３）式が成り立つように、ｄλ／ｄｔをリカッチ（Ｒｉｃｃａｔｉ）  の微分方程式の解によって決定する。
【００１５】
【数９】
0003559851.tif 000009
という境界条件から、逆時間に、
【００１６】
【数１０】
0003559851.tif 000010
を積分することからｃ（τ，ｔ）、Ｓ（τ，ｔ）を得る。ここで、Ａ，Ｂ，Ｃ行列は、
【００１７】
【数１１】
0003559851.tif 000011
によって定義される。λ（ｔ）が従う微分方程式は、
【００１８】
【数１２】
0003559851.tif 000012
となる。これを積分することによりλ（ｔ）が得られる。ｕ（ｔ）はＨｕ  から得られる。Ｈが正定でありさえすれば、ＴＰＢＶＰ  の解は局所最小解を与える。
従ってこの手法では、遂次近似なく常微分方程式（２１）を解くことで解が得られるので、勾配法のような膨大な計算時間を要する繰り返し計算を必要とせず、リアルタイムに最適解生成が可能である。
【００１９】
次に、本発明による制御の一例として、機械的リンク系の問題の安定化について説明する。
ここでは、比較的簡潔な式で表わされ、かつ非線形な系を扱うのに最小のリンク数を有する非線形リンク系の安定化を扱う。このような系は、脚式ロボットの遊脚運動や、マニピュレーター、又、重力項を０とすれば水平型マニピュレーターにも応用可能である。
【００２０】
ここでは、例として、１リンク系モデルと２リンク系モデルを考える。運動方程式は、系の非線形性も考慮して、
【数１３】
0003559851.tif 000013
という状態方程式として表現する。ここで、Ｍは慣性行列、Ｖはコリオリ力項、Ｇは重力項、Θは状態変数である。
【００２１】
又、１リンク系では、
【数１４】
0003559851.tif 000014
であり、θ１  は関節角度である。
【００２２】
次に、制御性能の評価指標となる評価関数について述べる。ここで扱う機械的リンク系には、各関節にアクチュエーターが装備されていると仮定する。一般に、サーボモーターの電機子電流は、関節トルクに比例することから、各関節でのトルク消費に着目する場合には、評価関数を、
【数１５】
0003559851.tif 000015
【００２３】
ととることが、回転関節リンク系の運動解析をするときには基本的である。そこで、ここでもこの評価関数を採用する。これを基本にして、この他にも派生型の評価関数を考えることができる。ｘｆ  は目標状態、Ｓｆ  は終端重み行列、Ｒは入力に対する重みである。この問題の場合、Ｈｕｕは、
【数１６】
0003559851.tif 000016
となり、正則であることから、ＲＨＣを使って最適解を求めることが可能である。
【００２４】
ＲＨＣは、リアルタイムに最適入力を生成できるアルゴリズムであるが、Ｔの変数α
【数１７】
0003559851.tif 000017
による引き延ばしが早すぎたりすることにより、Ｆ値が発散することもある。Ｆ値は、数７によって表される値である。この式の意味は、オイラー、ラグランジュ式である数５の４番目の式が、もし最適解にあるとしたときに満たしていなければならない条件である。従って、最適解からずれていればＦ値は０にならない。これを数８によって最適解へより戻そうと制御することが最適解に追従することを意味する。Ｆ値がある程度大きくなり過ぎると発散することがある。Ｆ値が発散すれば最適入力も発散する。また、Ｆ値が発散しないまでも値がかなり大きいと、得られる最適解も振動的な解となる可能性がある。このように、リアルタイムに最適解を生成し、なお且つ、解軌道に追従させるような閉ループを形成するものである。
【００２５】
図１には、提案する軌道追従型ＲＨＣのブロック線図を示す。この系はハイブリッド型となり、ＲＨＣの部分▲１▼と、トラッキング制御部分▲２▼と、これらを場合分けによってスイッチするスイッチ要素と、Ｆ値の監視要素と、θの監視要素等を備えている。
ＲＨＣでは、αの値が小さすぎたり、Ｔの引き延ばしが早すぎると、上述のようにＦ値が発散してしまう状態が発生する。Ｆ値が発散してしまうと、他の入力トルク等の値も発散し、制御系が暴走するので、何らかのフェイルセーフ機構を対策すべきである。
【００２６】
ここで提案する制御系では、図１のブロック線図のように、Ｆ値の監視要素を設けておき、Ｆ値がある値を越えたとき、通常の最適レギュレータ等にスイッチするスイッチ要素を付加することである。即ち、図１において、通常は部分▲１▼の経路で処理されるが、Ｆ値の監視要素においてＦ値がある値を越えたことを検出したときには、スイッチ要素の作動により経路▲３▼に切替えることにより、通常の制御系における最適レギュレータにスイッチし、解軌道に追従させるような閉ループを形成させる。
【００２７】
更に、軌道追従型ＲＨＣは、フィードバックする情報経路によって、図２における（ａ）〜（ｃ）に示す３通りを考えることができる。即ち、
（１）図２の（ａ）に示すように、角度θ、角速度ｄθ／ｄｔの情報をＲＨＣの数値モデル内ですべて推定、積分する。
（２）図２の（ｂ）に示すように、角度情報だけエンコーダーからフィードバックし、角速度はＲＨＣ内で推定、積分する。
（３）図２の（ｃ）に示すように、角度、角速度ともエンコーダーからの実際の値を使用する。
である。
【００２８】
しかし、各々には次の長所や短所がある。
（１）は、ペナルティー関数を使用する場合や、リアルタイムに障害物が発生する場合には不向きである。
（２）の方法が最も有効であると考えられるが、ペナルティー関数を扱う場合には工夫が必要である。
（３）は、角速度情報に外乱等が含まれている場合には、実験結果が不安定なものとなる恐れがある。
【００２９】
また、ＲＨＣでは、系が一旦目標値まで到達してしまうと、制御入力は０となってしまう。これは、水平型マニピュレーターのような安定平衡点が目標値である場合には問題ないが、鉛直型マニピュレーターのように重力の影響を受けるような不安定平衡点の場合、制御入力がなくなった瞬間から自然力により目標値からのずれを生じる。ある程度ずれが大きくなったところで再び制御入力が発生し、目標値に復帰するという現象を繰り返すことになる。最適パス実行型の制御則ではどうしてもこのような動作が生じてしまう。
【００３０】
これを解消するためには、二つの方法が考えられる。第１の方法は、評価関数の中に、目標値に到達したときの制御入力を予め挿入しておくことである。この方法では、目標値に達したときの制御入力を予め求めておく必要があるため、現実的でない。そこで、第２の方法としては、目標値を維持する場合、目標値の近傍に到達した後、位置−速度制御等の適切な従来の制御則に切替えることが考えられる。この場合、図１に示すようにθの監視要素を備え、通常は部分▲１▼の経路で処理するが、θの監視要素においてθ（τ）が目標値の近傍（ε以内）に到達したことを検出したときには、スイッチ要素の作動により経路▲３▼に切替える。この方法であれば、予め制御入力を求めておく必要がないし、Ｆ値の発散による暴走も回避することが可能である。
【００３１】
また、このようなハイブリッド型の制御系は、目標値が連続的に変化するような場合にも対応させることができる。
【００３２】
図３は、提案する軌道追従型ＲＨＣの別の例を示すブロック線図である。
１）最適制御のための評価関数等は設計者が予め決定しておく。
２）ＲＨＣの逐次計算により最適解ｕが逐次生成される。
３）最適解ｕを、制御対象の数値モデルｆ（ｘ，ｕ，ｔ）に入力し、逐次積分して最適軌道を得る。これを、
【数１８】
0003559851.tif 000018
とする。
４）これに例えば最適レギュレータ等を使って追従させる。
また、監視機構によってＦ値と系の状態を監視しておき、しかるべき時にスイッチ素子によってＲＨＣ部分から切り離す。
【００３３】
ＲＨＣでは、αの値が小さすぎたり、Ｔの引き延ばしが早すぎると、上述のようにＦ値が発散してしまう状態が発生する。Ｆ値が発散してしまうと、他の入力トルク等の値も発散し、制御系が暴走するので、フェイルセーフ機構を備える必要がある。
図３に示す制御系では、Ｆ値の監視要素を設けておき、Ｆ値がある値を越えたとき、現在の状態（位置、速度）を維持するよう構成している。
【００３４】
【発明の効果】
従来のＲＨＣでは、リアルタイム計算が何らかの原因によってうまくいかなかったときに、最適解が発散してしまい、その解を直接に制御入力にしていたためリアルタイム制御が発散してしまう危険があった。しかし、本発明によれば、このような危険が回避され、かつ、リアルタイム最適性は失われない。また、状態が目標に到達した後も安定して状態を維持することが可能となる。このように、本発明は、ＲＨＣを実際の実験に応用する際に必要であり、これによりＲＨＣの応用実験を安全に実行することができる。
【図面の簡単な説明】
【図１】本発明に係る軌道追従型ＲＨＣを適用した機械的リンク系の構成例を示すブロック線図である。
【図２】（１）ないし（３）は、軌道追従型ＲＨＣにおけるそれぞれ異なるフィードバック情報経路例を示すブロック線図である。
【図３】本発明に係る軌道追従型ＲＨＣを適用した機械的リンク系の別の構成例を示すブロック線図である。
【図１】
0003559851.tif 000020
【図２】
0003559851.tif 000021
【図３】
0003559851.tif 000022