JP5325149B2 - 軌道追従制御装置、方法及びプログラム - Google Patents

Description

この発明は、制御対象物の位置を目標軌道に追従させる制御技術に関する。

近年、屋内外で活動可能な自律移動ロボットの研究が活発に行われており、それらの応用先が広がりつつある。自律移動ロボットの種類としては、車輪を使用して地表を移動するタイプのものや、空を飛行して移動する自律型空中ロボット、水中を航行する水中型ロボットなどがある。それらの自律移動ロボットの行動を高精度で制御するアルゴリズムを確立する上では、自律移動ロボットに予め定めた目標軌道上を高精度に追従させるための技術が必須である。

自律移動ロボットに予め定めた目標軌道を追従させるための制御方法の代表的なものとしてスライディング制御の方法がある（例えば、非特許文献１参照。）。スライディング制御の方法は、制御対象の運動特性の不確定性や非線形性を考慮したロバスト制御と呼ばれる手法の一つである。自律飛行船等の自律移動ロボットは、その運動特性がモデル化困難な要素を持ち、かつ非線形の力学特性に従うものであるため、その軌道追従制御のためには、スライディング制御を使用するのが適切である。

Jean-Jacques E. Slotine, Weiping Li, "Applied Nonlinear Control" Prentice-Hall, 1991,pp.276-309

非特許文献１に記載されたスライディング制御では、制御対象物である自律移動ロボットの位置だけではなく、速度を計測してフィードバックをする必要があるという課題があった。

上記の課題を解決するために、スライディング制御の制御入力値を計算する式の中の定数λの値を適切な値に設定することで、その式の中の制御対象物の速度の項を消す。

制御対象物の速度を計測及びフィードバックすることなく、スライディング制御を実現することができる。

軌道追従制御装置の例の機能ブロック図。 補正値入力部の例の機能ブロック図。 軌道追従制御方法の例の流れ図。 制御対象物の例である自律飛行船を例示する図。

［制御対象物］
制御の対象となる制御対象物は、例えば図４に例示する自律飛行船等の自律移動ロボットである。この自律飛行船は、舵１２、主推進器１３、上下方向推進器１４を装備している。また、図示していないが、横方向（方位角方向）推進器を備える。この自律飛行船の重心位置は低く設定されているので姿勢傾斜に対する復元力は大きく、自律飛行船の姿勢角は方位角以外は０に維持されるとする。以下、制御対象物が自律飛行船である場合を例に挙げて説明する。なお、制御の対象となる制御対象物は何でもよく、自律飛行船に限られない。

図１の自律飛行船のゴンドラ部には、５つの無線ＬＡＮ方式の位置計測センサ６１，６２，６３，６４，６５が取り付けられている。この例では、これらの位置計測センサ６１，６２，６３，６４，６５は、自律移動ロボットの水平面（ＸＹ平面）内重心位置（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ））を中心として水平面内に点対象に配置されているものとする。位置計測センサ６１，６２，６３，６４，６５は、水平面内重心位置（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ））から、それぞれ方位角にしてΦｓ１，Φｓ２，Φｓ３，Φｓ４，Φｓ５の方向に距離Ｒｓ１，Ｒｓ２，Ｒｓ３，Ｒｓ４，Ｒｓ５だけ離れた位置に固定されているものとする。各位置計測センサ６１，６２，６３，６４，６５は、水平面内位置（ＸＹ）及び高度（Ｚ）を計測することができる。

時刻ステップｔにおける位置計測センサ６１，６２，６３，６４，６５の計測値をそれぞれＺ１（ｔ）、Ｚ２（ｔ）、Ｚ３（ｔ）、Ｚ４（ｔ）、Ｚ５（ｔ）とする。
Ｚ１（ｔ）＝（Ｚｘ１（ｔ），Ｚｙ１（ｔ），Ｚｚ１（ｔ））
Ｚ２（ｔ）＝（Ｚｘ２（ｔ），Ｚｙ２（ｔ），Ｚｚ２（ｔ））
Ｚ３（ｔ）＝（Ｚｘ３（ｔ），Ｚｙ３（ｔ），Ｚｚ３（ｔ））
Ｚ４（ｔ）＝（Ｚｘ４（ｔ），Ｚｙ４（ｔ），Ｚｚ４（ｔ））
Ｚ５（ｔ）＝（Ｚｘ５（ｔ），Ｚｙ５（ｔ），Ｚｚ５（ｔ））
すると、これらの平均値（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｚａ（ｔ））は、以下のように計算できる。

Ｚｘａ（ｔ）＝［Ｚｘ１（ｔ）＋Ｚｘ２（ｔ）＋Ｚｘ３（ｔ）＋Ｚｘ４（ｔ）＋Ｚｘ５（ｔ）］／５ …（ａ）
Ｚｙａ（ｔ）＝［Ｚｙ１（ｔ）＋Ｚｙ２（ｔ）＋Ｚｙ３（ｔ）＋Ｚｙ４（ｔ）＋Ｚｙ５（ｔ）］／５ …（ｂ）
Ｚｚａ（ｔ）＝［Ｚｚ１（ｔ）＋Ｚｚ２（ｔ）＋Ｚｚ３（ｔ）＋Ｚｚ４（ｔ）＋Ｚｚ５（ｔ）］／５
センサが点対象に配置されていることから、上記平均値（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｚａ（ｔ））を自律飛行船の重心位置（Ｘｇ，Ｙｇ，Ｚｇ）の計測値として扱うことができる。以後、平均値（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｚａ（ｔ））を自律飛行船の重心位置（Ｘｇ，Ｙｇ，Ｚｇ）の計測値として説明する。

なお、方位角の値は、たとえば、飛行船の進行方向と方位角はほぼ一致していることを利用して、時刻ステップｔのＺｘａ（ｔ），Ｚｙａ（ｔ）及び時刻ステップｔ−１のＺｘａ（ｔ−１），Ｚｙａ（ｔ−１）を用いて、以下のような求め方で推定することが可能である。

[スライディング制御]
スライディング制御は、制御対象物の時刻ステップｔにおける状態を表す変数（状態変数）Ｘ（ｔ）が、予め決定された目標とする状態Ｘ_ｄ（ｔ）に近づくように、制御対象物に搭載された制御対象アクチュレータへの制御入力値ｕ（ｔ）を計算して、その計算された制御入力値ｕ（ｔ）で制御対象物に搭載された制御対象アクチュレータを制御するものである。以下では、制御対象物の状態変数Ｘ（ｔ）が制御対象物の方位角Φ（ｔ）である場合を例に挙げて説明する。もちろん、制御対象物の状態変数Ｘ（ｔ）は、方位角Φ（ｔ）に限らず、例えば制御対象物の機体座標での前進方向の変位量であってもよい。

制御対象物である自律飛行船は、船尾に装備された垂直尾翼の効果により、方位角方向の回転運動については安定であるとする。そのことから、自律飛行船の方位角方向の運動については、以下のような一次遅れ線形モデルを想定することも可能である。この線形モデルを使用してスライディング制御のアルゴリズムを設計することで、線形モデルに記述されていない飛行船の非線形な力学特性にも対応した軌道追従制御アルゴリズムを設計することができる。

ここで、ｆは正の定数、ｂは定数（通常は正）、ｕ（ｔ）は制御入力値である。制御入力値は、例えば、自律飛行船に搭載された、スラスタの発生する方位角方向の回転トルクや、テイルロータへの指示電圧値、電流値である。

上記式（１）において、ｆが正であることにより、自律飛行船の方位角方向の運動の自己安定性が記述されている。すなわち、方位角方向になんの制御を加えずとも自然と飛行船の方位角がある角度に落ち着こう安定するという性質が再現されている。

目標軌道における時刻ステップｔでの目標方位角をΦ_ｄ（ｔ）、目標方位角速度をＤΦ_ｄ（ｔ）／Ｄｔ、目標方位角加速度をＤ^２Φ_ｄ（ｔ）／Ｄｔ^２としたとき、スライディング制御による制御入力の計算式は以下の通りである。

ここで、ｆ_ｆａｒはｆの真値ｆ_ｔｒｕｅの取り得る値の中でｆから最も離れた値、ｓｇｎ（ｘ）はｘ＞０のとき１、ｘ＜０のとき−１を出力する関数、η及びλは正定数、ｋ（ｔ）は正のフィードバック定数である。Ｆは、実際の自律飛行船におけるｆの値がどのくらいの不確かさを持っているかを含んだパラメータである。λ、ηは理論上任意の正定数を指定可能であるが、実際にはトライアンドエラーで値を調整することが多い。βについては、ｂの真値のとりうる値の範囲が最大ｂ_ｍａｘ、最小ｂ_ｍｉｎであるとし、ｂ＝√（ｂ_ｍａｘｂ_ｍｉｎ）であるとして、例えば次式により定義される値とする。

［この発明の原理］
この発明の目的は、制御対象物の状態変数の一階微分ＤＸ（ｔ）／Ｄｔ（例えば、方位角速度ＤΦ（ｔ）／Ｄｔ）を用いず、制御対象物の状態変数Ｘ（ｔ）（例えば、方位角Φ（ｔ））の計測値のみを使用して、スライディング制御による軌道追従制御を可能とすることである。

式（２）（３）を見るとわかるように、制御入力値ｕ（ｔ）を決定する方程式の中には、方位角速度ＤΦ（ｔ）／Ｄｔが含まれている。したがって、このままでは、方位角速度ＤΦ（ｔ）／Ｄｔが高精度で計測または推定できない場合に、自律飛行船にスライディング制御を適用することはできない。そこで、理論上任意の正値を選択可能なλの値に注目する。今、λとしてｆと同じ値を選択したとする。すなわち、λ＝ｆとする。

この場合、式（２）及び式（３）は、

となり、式（５）が示す通り式（２）から方位角速度ＤΦ（ｔ）／Ｄｔの項が消去される。しかし、式（５）は、まだ式（３）で示されるｓ（ｔ）の値を引用しているので、その部分で方位角速度ＤΦ（ｔ）／Ｄｔの値を参照していることになる。ここで、今、ｆが正確にモデル推定された結果ｆ_ｔｒｕｅに等しいと仮定すると、同時にλ＝ｆ_ｔｒｕｅであると言える。その場合、

とおくと、

とすることができ、−１／ｂ・ｋ（ｔ）・ｓｇｎ（ｓ（ｔ））が軌道からの誤差を修正するためのフィードバック項であることを考えると、ｕ_ｅ（ｔ）及びｕ（ｔ）は、自律飛行船が目標軌道上にあるように制御するための制御入力値だということができる。実際、式（７）は、次式のように変形することができる。

式（１）と比較してみたとき、式（９）が示すのは、式（７）を満たすｕ_ｅ（ｔ）の値が、理論上は自律飛行船が目標速度（この例では、目標方位角速度）と目標加速度（この例では、目標方位角加速度）を正確に取れるように制御するための値であるということである。制御対象物である自律飛行船が式（１）の運動モデルにより運動しており、ｆ＝ｆ_ｔｒｕｅであり、式（１）のｕ（ｔ）が式（７）に基づいて計算されるｕ_ｅ（ｔ）である場合には、理論上、方位角速度ＤΦ（ｔ）／Ｄｔと目標方位角速度ＤΦ_ｄ（ｔ）／Ｄｔとが一致し、方位角加速度Ｄ^２Φ（ｔ）／Ｄｔ^２と目標方位角加速度Ｄ^２Φ_ｄ（ｔ）／Ｄｔ^２とが一致する。実際には、ｕ_ｅ（ｔ）ではなく式（８）で定義されるｕ（ｔ）により自律飛行船は制御されるが、ｕ_ｅ（ｔ）とｕ（ｔ）との差分である（１／ｂ）（ｋ（ｔ）ｓｇｎ（ｓ（ｔ）））は、位置（この例では、方位角）の誤差についての項であるため、ｕ（ｔ）で制御しても、理論上、方位角速度ＤΦ（ｔ）／Ｄｔと目標方位角速度ＤΦ_ｄ（ｔ）／Ｄｔとが一致し、方位角加速度Ｄ^２Φ（ｔ）／Ｄｔ^２と目標方位角加速度Ｄ^２Φ_ｄ（ｔ）／Ｄｔ^２とが一致することに変わりはない。換言すれば、（１／ｂ）ｋ（ｔ）ｓｇｎ（ｓ（ｔ））の値は、自律飛行船が目標軌道上から外れた際の補正のための制御入力値であると言えるから、計測された方位角速度ＤΦ（ｔ）／Ｄｔの値は十分に目標方位角速度ＤΦ_ｄ（ｔ）／Ｄｔに近いと仮定できる。すなわち、式（４）は次のように書くことができ、方位角速度ＤΦ（ｔ）／Ｄｔの項を消去することができる。

式（３）については、式（６）が示す通り、まだ方位角速度ＤΦ（ｔ）／Ｄｔの項が残っているが式（６）は不等式なので、方位角速度ＤΦ（ｔ）／Ｄｔの値の取りうる範囲を考慮して、式（６）の不等式が示す条件に合う関数又は値を決定すればよい。例えば、ｋ（ｔ）を、式（６）の不等式を満たすような十分大きな定数値としても良いし、下記式（１０）のような関数の値としても良い。

式（１０）において、Φ_ｖｍａｘは、ＤΦ（ｔ）／Ｄｔの想定される取り得る値の絶対値の最大値である。このようにすれば、ｋ（ｔ）の式から方位角速度の計測値ＤΦ（ｔ）／Ｄｔの項を消去できる。

また、λ＝ｆ＝ｆ_ｔｒｕｅであるという仮定が成立するならば、Ｆ＝０とみなしてもよいため、式（１０）を下記式（１１）としてもよい。

以上により、λ＝ｆ＝ｆ_ｔｒｕｅという仮定のもと、スライディング制御の式中の方位角速度ＤΦ（ｔ）／Ｄｔの項を消去することができることが分かったが、その仮定を現実のものとするためには、λの値がｆ_ｔｒｕｅに近づくようにλの値をオンライン自動調整することが有効である。λの値の調整のためには、Φ（ｔ）−Φ_ｄ（ｔ）のフィードバック値を使用する。今、式（５）において、以下の（１）から（４）のことが言える。

（１）ＤΦ_ｄ（ｔ）／Ｄｔ＞０のとき、λが適切な値ｆ_ｔｒｕｅより小さい場合は、λ＞０なので、ｕ（ｔ）はその分小さくなり、結果として、Φ（ｔ）−Φ_ｄ（ｔ）の値は負の値になっているはずである。すなわち、ＤΦ_ｄ（ｔ）／Ｄｔ＞０、かつ、Φ（ｔ）−Φ_ｄ（ｔ）＜０であれば、λは適切な値ｆ_ｔｒｕｅより小さくなっている。この場合には、λをより大きくすることが望ましい。

（２）ＤΦ_ｄ（ｔ）／Ｄｔ＜０のときで、λが適切な値ｆ_ｔｒｕｅより小さい場合は、λ＞０なので、ｕ（ｔ）もその分大きくなり、結果として、Φ（ｔ）−Φ_ｄ（ｔ）の値は正の値になっているはずである。すなわち、ＤΦ_ｄ（ｔ）／Ｄｔ＜０、かつ、Φ（ｔ）−Φ_ｄ（ｔ）＞０であれば、λは適切な値ｆ_ｔｒｕｅより小さくなっている。この場合には、λをより大きくすることが望ましい。

（３）ＤΦ_ｄ（ｔ）／Ｄｔ＞０のとき、λが適切な値ｆ_ｔｒｕｅより大きい場合は、λ＞０なので、ｕ（ｔ）はその分大きくなり、結果として、Φ（ｔ）−Φ_ｄ（ｔ）の値は正の値になっているはずである。すなわち、ＤΦ_ｄ（ｔ）／Ｄｔ＞０、かつ、Φ（ｔ）−Φ_ｄ（ｔ）＞０であれば、λは適切な値ｆ_ｔｒｕｅより大きくなっている。この場合には、λをより小さくすることが望ましい。

（４）ＤΦ_ｄ（ｔ）／Ｄｔ＜０のときで、λが適切な値より大きい場合は、λ＞０なので、ｕ（ｔ）もその分小さくなり、結果として、Φ（ｔ）−Φ_ｄ（ｔ）の値は負の値になっているはずである。すなわち、ＤΦ_ｄ（ｔ）／Ｄｔ＜０、かつ、Φ（ｔ）−Φ_ｄ（ｔ）＜０であれば、λは適切な値ｆ_ｔｒｕｅより大きくなっている。この場合には、λをより小さくすることが望ましい。

以上の見識に基づいて、例えば以下ようにλの値を制御する。以下では、ＤΦ_ｄ（ｔ）／Ｄｔ＞０の場合のλをλ_＋と表記し、ＤΦ_ｄ（ｔ）／Ｄｔ＜０の場合のλをλ₋と表記している。γは０＜γ＜１の数であり、「Ａ＝Ｂ」はＢの値をＡの値とすることを意味する。

（１）の場合、すなわちＤΦ_ｄ（ｔ）／Ｄｔ＞０、かつ、Φ（ｔ）−Φ_ｄ（ｔ）＜０の場合には、λ_＋＝（１＋γ）λ_＋とする。

（２）の場合、すなわちＤΦ_ｄ（ｔ）／Ｄｔ＜０、かつ、Φ（ｔ）−Φ_ｄ（ｔ）＞０の場合には、λ₋＝（１＋γ）λ₋とする。

（３）の場合、すなわちＤΦ_ｄ（ｔ）／Ｄｔ＞０、かつ、Φ（ｔ）−Φ_ｄ（ｔ）＞０の場合には、λ_＋＝（１−γ）λ_＋とする。

（４）の場合、すなわちＤΦ_ｄ（ｔ）／Ｄｔ＜０、かつ、Φ（ｔ）−Φ_ｄ（ｔ）＜０の場合には、λ₋＝（１−γ）λ₋とする。

自律飛行船が左右に１つずつ前進用スラスタを装備していような場合において、左右のスラスタ性能の個体差等によって発生する左右回転時でのトルクのムラによる制御誤差の拡大を防ぐために、方位角速度ＤΦ_ｄ（ｔ）／Ｄｔの値の正負それぞれの場合で、λ_＋の初期値とλ₋の初期値に異なる値を設定して管理してもよい。もちろん、λ_＋の初期値とλ₋の初期値を同じ値としてもよい。

以上のように、スライディング制御の制御入力値ｕ（ｔ）を計算する方程式中のパラメータλの値を適切な値に設定することで、速度ＤΦ_ｄ（ｔ）／Ｄｔの計測値を利用することなくスライディング制御による自律飛行船の軌道追従制御を行うことができる。

［実施形態］
この発明の一実施形態による軌道追従制御装置は、図１に例示するように、時刻設定部１、位置計測センサ６１，６２，６３，６４，６５、計測部２、記憶部３、方位角計算部４、定数値設定部６、補正入力値計算部７、制御入力値計算部８及び制御部９を例えば含む。この軌道追従制御装置は、図３に示す流れ図の各ステップの処理を例えば行う。

時刻設定部１は、時刻ステップｔ（ｔは０以上の整数）の値を設定する。まず、時刻設定部１は、時刻ステップの初期値ｔ＝０とする（ステップＳ１）。その後、時刻設定部１は、時刻ステップｔを１ずつ増やす。軌道追従制御装置は、時刻設定部１が設定した各時刻ステップｔにおける制御入力値ｕ（ｔ）を計算する。

計測部２は、自律飛行船に搭載された位置計測センサ６１，６２，６３，６４，６５から取得した計測結果を用いて、時刻ステップｔにおける自律飛行船のＸＹ平面における位置Ｚｘａ（ｔ）及びＺｙａ（ｔ）を計算する（ステップＳ２）。計算された位置Ｚｘａ（ｔ）及びＺｙａ（ｔ）についての情報は、時刻ステップｔと対応付けて記憶部３に記憶される。

記憶部３には、各時刻ステップｔの自律飛行船の目標軌道における目標方位角Φｄ（ｔ）、目標角速度ＤΦ_ｄ（ｔ）／Ｄｔ及び目標方位角加速度Ｄ^２Φ_ｄ（ｔ）／Ｄｔ^２が予め記憶されているものとする。目標方位角Φｄ（ｔ）、目標角速度ＤΦ_ｄ（ｔ）／Ｄｔ及び目標方位角加速度Ｄ^２Φ_ｄ（ｔ）／Ｄｔ^２は、ユーザが事前に設定してもよいし、例えば特開２００８−２２７４３１号公報に記載されているような既存のアルゴリズムを用いてコンピュータが計算してもよい。記憶部３には、Ｚｘａ（ｔ）及びＺｙａ（ｔ）についての情報も記憶される。

方位角計算部４は、計測部２から取得した時刻ステップｔにおける自律飛行船のＸＹ平面における位置Ｚｘａ（ｔ）及びＺｙａ（ｔ）と、記憶部３に記憶されている１時刻ステップ前の時刻ステップｔ−１における自律飛行船のＸＹ平面における位置Ｚｘａ（ｔ−１）及びＺｙａ（ｔ−１）とから、式（０）により、時刻ステップｔにおける方位角Φ（ｔ）を推定する（ステップＳ３）。計算された方位角Φ（ｔ）は、定数値設定部６に送られる。

定数値設定部６は、方位角計算部４が推定した時刻ステップｔにおける自律飛行船の方位角Φ（ｔ）と、記憶部３から読み込んだ目標位置目標方位角Φ_ｄ（ｔ）及び目標方位角速度ＤΦ_ｄ（ｔ）／Ｄｔとを用いて、λを適宜調整する（ステップＳ４）。設定されたλは、補正入力値計算部７及び制御入力値計算部８に送られる。

例えば、γを０＜γ＜１の数として、定数値設定部６は、ＤΦ_ｄ（ｔ）／Ｄｔ＞０かつΦ（ｔ）−Φ_ｄ（ｔ）＜０であればλ＝（１＋γ）λとし、ＤΦ_ｄ（ｔ）／Ｄｔ＜０かつΦ（ｔ）−Φ_ｄ（ｔ）＞０であればλ＝（１＋γ）λとし、ＤΦ_ｄ（ｔ）／Ｄｔ＞０かつΦ（ｔ）−Φ_ｄ（ｔ）＞０であればλ＝（１−γ）λとし、ＤΦ_ｄ（ｔ）／Ｄｔ＜０かつΦ（ｔ）−Φ_ｄ（ｔ）＜０であればλ＝（１−γ）λとする。λの値の設定は、（１＋γ）λまたは（１−γ）λに限られるものではなく、ＤΦ_ｄ（ｔ）／Ｄｔ＞０かつΦ（ｔ）−Φ_ｄ（ｔ）＜０の場合、または、ＤΦ_ｄ（ｔ）／Ｄｔ＜０かつΦ（ｔ）−Φ_ｄ（ｔ）＞０の場合には、λの値が元のλの値よりも大きくなるように設定し、ＤΦ_ｄ（ｔ）／Ｄｔ＞０かつΦ（ｔ）−Φ_ｄ（ｔ）＞０の場合、または、ＤΦ_ｄ（ｔ）／Ｄｔ＜０かつΦ（ｔ）−Φ_ｄ（ｔ）＜０の場合には、λの値が元のλの値よりも小さくなるように設定されるものであればなんでも良い。例えば、λ＝λ＋ｃ，λ＝λ−ｃ（ｃは正の定数）としても良い。なお、λの増大と減少の幅は等しくするほうが、精度良く推定を行うことができる。

補正入力値計算部７は、補正入力値ｋ（ｔ）ｓｇｎ（ｓ（ｔ））を計算する（ステップＳ５）。計算された補正入力値ｋ（ｔ）ｓｇｎ（ｓ（ｔ））は、制御入力値計算部８に送られる。

例えば、フィードバック定数ｋ（ｔ）は、ｋ（ｔ）を、式（６）の不等式を満たすような十分大きな定数値としても良いし、式（１０）や式（１１）のような関数の値としてもよい。また、ずれ値ｓ（ｔ）をｓ（ｔ）＝λ（Φ（ｔ）−Φ_ｄ（ｔ））とし、関数ｓｇｎ（・）を・が正であれば１を出力し・が負であれば−１を出力する関数とする。

補正入力値計算部７は、図２に例示するように、フィードバック定数ｋ（ｔ）を計算するフィードバック定数計算部７１と、ずれ値ｓ（ｔ）を計算するずれ値計算部７２と、計算されたフィードバック定数ｋ（ｔ）及びずれ値ｓ（ｔ）を用いて補正入力値を計算する合成算部７３とを含む。

制御入力値計算部８は、設定された定数λ及び計算された補正入力値ｋ（ｔ）ｓｇｎ（ｓ（ｔ））を用いて、式（５）で定義される制御入力値ｕ（ｔ）を計算する（ステップＳ８）。計算された制御入力値ｕ（ｔ）は、制御部９に送られる。

制御部９は、制御入力値ｕ（ｔ）で自律飛行船に搭載された制御対象アクチュエータを制御する（ステップＳ７）。

［変形例等］
上記の例では、制御対象物の状態変数Ｘ（ｔ）として方位角Φ（ｔ）を用いたが、制御対象物の状態変数Ｘ（ｔ）として他の変数を選択してもよい。例えば、上記の説明において方位角Φ（ｔ）を制御対象物の機体座標での前進方向の変位量に置き換えれば、前進方向の軌道追従制御を実現することができる。

また、式（５）及び式（８）における関数ｓｇｎ（・）の代わりに、ｓ＝０近辺に不感帯を設けた以下の関数ｓｇｎ＿ｍｏｄ（・）を用いても良い。Ωは予め定められた定数であり、例えば０に近い値に設定される。

これにより、関数ｓｇｎ（・）の符号切り替えの影響で、制御入力値ｕ（ｔ）の値がチャタリングを起こしてしまう問題を防ぐことができる。

軌道追従制御装置の各部間のデータのやり取りは直接行われてもよいし、記憶部３を介して行われてもよい。

軌道追従制御装置は、コンピュータによって実現することができる。この場合、この装置が有すべき各機能の処理内容はプログラムによって記述される。そして、このプログラムをコンピュータで実行することにより、これ装置における各処理機能が、コンピュータ上で実現される。

この処理内容を記述したプログラムは、コンピュータで読み取り可能な記録媒体に記録しておくことができる。また、この形態では、コンピュータ上で所定のプログラムを実行させることにより、これらの装置を構成することとしたが、これらの処理内容の少なくとも一部をハードウェア的に実現することとしてもよい。

この発明は、上述の実施形態に限定されるものではなく、本発明の趣旨を逸脱しない範囲で適宜変更が可能である。

６ 定数値設定部
７ 補正入力値計算部
８ 制御入力値計算部
９ 制御部

Claims (6)

1. ｆ及びｂを正の定数、ｕ（ｔ）を制御入力値として、次式で表わされるモデルにより運動すると想定される制御対象物の状態変数Ｘ（ｔ）を目標とする状態Ｘ_ｄ（ｔ）に追従させる軌道追従制御装置において、
   

   

   
   Ｘ（ｔ）を計測された時刻ステップｔにおける上記制御対象物の状態変数、Ｘ_ｄ（ｔ）を時刻ステップｔにおける上記制御対象物の目標とする状態として、定数λを正の数とし、ＤＸ_ｄ（ｔ）／Ｄｔ＞０かつＸ（ｔ）−Ｘ_ｄ（ｔ）＜０であればλの値をより大きくし、ＤＸ_ｄ（ｔ）／Ｄｔ＜０かつＸ（ｔ）−Ｘ_ｄ（ｔ）＞０であればλの値をより大きくし、ＤＸ_ｄ（ｔ）／Ｄｔ＞０かつＸ（ｔ）−Ｘ_ｄ（ｔ）＞０であればλの値をより小さくし、ＤＸ_ｄ（ｔ）／Ｄｔ＜０かつＸ（ｔ）−Ｘ_ｄ（ｔ）＜０であればλの値をより小さくする定数値設定部と、
   ｋ（ｔ）をフィードバック定数とし、ずれ値ｓ（ｔ）をｓ（ｔ）＝λ（Ｘ（ｔ）−Ｘ_ｄ（ｔ））とし、関数ｓｇｎ（・）を・が正であれば１を出力し・が負であれば−１を出力する関数として、補正入力値ｋ（ｔ）ｓｇｎ（ｓ（ｔ））を計算する補正入力値計算部と、
   ｂを正の定数として、上記設定された定数λ及び上記計算された補正入力値ｋ（ｔ）ｓｇｎ（ｓ（ｔ））を用いて、次式で定義される制御入力値ｕ（ｔ）を計算する制御入力値計算部と、
   

   

   
   上記制御入力値ｕ（ｔ）で上記制御対象物に搭載された制御対象アクチュエータを制御する制御部と、
   を含む軌道追従制御装置。
2. 請求項１の軌道追従制御装置において、
   上記補正入力値計算部は、β及びηを正の定数、ｆの真値の取り得る値の中でｆから最も離れた値をｆ_ｆａｒ、上記制御対象物の速度ＤＸ（ｔ）／ｄｔの想定される最大値をＸ_ｖｍａｘとして、次式により定義されるフィードバック定数ｋ（ｔ）を計算するフィードバック定数計算部を含む、
   

   

   
   ことを特徴とする軌道追従制御装置。
3. 請求項１の軌道追従制御装置において、
   上記補正入力値計算部は、β及びηを正の定数として、次式により定義されるフィードバック定数ｋ（ｔ）を計算するフィードバック定数計算部を含む、
   

   
4. 請求項１から３の何れかに記載の軌道追従制御装置において、
   上記制御対象物の状態変数Ｘ（ｔ）はその制御対象物の方位角Φ（ｔ）であり、上記制御対象物の目標とする状態Ｘ_ｄ（ｔ）はその制御対象物の目標方位角Φ_ｄ（ｔ）であり、
   計測された上記制御対象物の時刻ステップｔにおけるＸＹ平面の位置及び時刻ステップｔ−１におけるＸＹ平面の位置から、上記制御対象物の時刻ステップｔにおける方位角Φ（ｔ）を計算する方位角計算部を更に含む、
   ことを特徴とする軌道追従制御装置。
5. ｆ及びｂを正の定数、ｕ（ｔ）を制御入力値として、次式で表わされるモデルにより運動すると想定される制御対象物の状態変数Ｘ（ｔ）を目標とする状態Ｘ_ｄ（ｔ）に追従させる軌道追従制御方法において、
   

   

   
   定数値設定部が、Ｘ（ｔ）を計測された時刻ステップｔにおける上記制御対象物の位置、Ｘ_ｄ（ｔ）を時刻ステップｔにおける上記制御対象物の目標位置として、定数λを正の数とし、ＤＸ_ｄ（ｔ）／Ｄｔ＞０かつＸ（ｔ）−Ｘ_ｄ（ｔ）＜０であればλの値をより大きくし、ＤＸ_ｄ（ｔ）／Ｄｔ＜０かつＸ（ｔ）−Ｘ_ｄ（ｔ）＞０であればλの値をより大きくし、ＤＸ_ｄ（ｔ）／Ｄｔ＞０かつＸ（ｔ）−Ｘ_ｄ（ｔ）＞０であればλの値をより小さくし、ＤＸ_ｄ（ｔ）／Ｄｔ＜０かつＸ（ｔ）−Ｘ_ｄ（ｔ）＜０であればλの値をより小さくする定数値設定ステップと、
   補正入力値計算部が、ｋ（ｔ）をフィードバック定数とし、ずれ値ｓ（ｔ）をｓ（ｔ）＝λ（Ｘ（ｔ）−Ｘ_ｄ（ｔ））とし、関数ｓｇｎ（・）を・が正であれば１を出力し・が負であれば−１を出力する関数として、補正入力値ｋ（ｔ）ｓｇｎ（ｓ（ｔ））を計算する補正入力値計算ステップと、
   制御入力値計算部が、ｂを正の定数として、上記設定された定数λ及び上記計算された補正入力値ｋ（ｔ）ｓｇｎ（ｓ（ｔ））を用いて、次式で定義される制御入力値ｕ（ｔ）を計算する制御入力値計算ステップと、
   

   

   
   制御部が、上記制御入力値ｕ（ｔ）で上記制御対象物に搭載された制御対象アクチュエータを制御する制御ステップと、
   を含む軌道追従制御方法。
6. 請求項１から４の何れかの軌道追従制御装置の各部としてコンピュータを機能させるための軌道追従制御プログラム。

Images