軌道追従制御装置、方法及びプログラム
【課題】制御対象物の状態変数の一階微分（例えば速度ＤＸ（ｔ）／Ｄｔ）を計測及びフィードバックすることなく、スライディング制御を実現する。
【解決手段】スライディング制御の式の中で登場する定数λの値をｆとする。また、定数λの値を、制御対象物の状態変数Ｘ（ｔ）（例えば位置）、制御対象物の目標とする状態Ｘｄ（ｔ）（例えば目標位置）を用いて制御対象物の目標とする状態の一階微分（例えば目標速度ＤＸｄ（ｔ）／Ｄｔ）に応じて適宜修正する。これにより、スライティング制御の式の中で登場する制御対象物の状態変数の一階微分（例えば速度ＤＸ（ｔ）／Ｄｔ）の項を消すことができ、制御対象物の状態変数の一階微分（例えば速度ＤＸ（ｔ）／Ｄｔ）を計測及びフィードバックすることなく、スライディング制御を実現することができる。
【解決手段】スライディング制御の式の中で登場する定数λの値をｆとする。また、定数λの値を、制御対象物の状態変数Ｘ（ｔ）（例えば位置）、制御対象物の目標とする状態Ｘｄ（ｔ）（例えば目標位置）を用いて制御対象物の目標とする状態の一階微分（例えば目標速度ＤＸｄ（ｔ）／Ｄｔ）に応じて適宜修正する。これにより、スライティング制御の式の中で登場する制御対象物の状態変数の一階微分（例えば速度ＤＸ（ｔ）／Ｄｔ）の項を消すことができ、制御対象物の状態変数の一階微分（例えば速度ＤＸ（ｔ）／Ｄｔ）を計測及びフィードバックすることなく、スライディング制御を実現することができる。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、制御対象物の位置を目標軌道に追従させる制御技術に関する。
【背景技術】
【０００２】
近年、屋内外で活動可能な自律移動ロボットの研究が活発に行われており、それらの応用先が広がりつつある。自律移動ロボットの種類としては、車輪を使用して地表を移動するタイプのものや、空を飛行して移動する自律型空中ロボット、水中を航行する水中型ロボットなどがある。それらの自律移動ロボットの行動を高精度で制御するアルゴリズムを確立する上では、自律移動ロボットに予め定めた目標軌道上を高精度に追従させるための技術が必須である。
【０００３】
自律移動ロボットに予め定めた目標軌道を追従させるための制御方法の代表的なものとしてスライディング制御の方法がある（例えば、非特許文献１参照。）。スライディング制御の方法は、制御対象の運動特性の不確定性や非線形性を考慮したロバスト制御と呼ばれる手法の一つである。自律飛行船等の自律移動ロボットは、その運動特性がモデル化困難な要素を持ち、かつ非線形の力学特性に従うものであるため、その軌道追従制御のためには、スライディング制御を使用するのが適切である。
【先行技術文献】
【非特許文献】
【０００４】
【非特許文献１】Jean-Jacques E. Slotine, Weiping Li, "Applied Nonlinear Control" Prentice-Hall, 1991,pp.276-309
【発明の概要】
【発明が解決しようとする課題】
【０００５】
非特許文献１に記載されたスライディング制御では、制御対象物である自律移動ロボットの位置だけではなく、速度を計測してフィードバックをする必要があるという課題があった。
【課題を解決するための手段】
【０００６】
上記の課題を解決するために、スライディング制御の制御入力値を計算する式の中の定数λの値を適切な値に設定することで、その式の中の制御対象物の速度の項を消す。
【発明の効果】
【０００７】
制御対象物の速度を計測及びフィードバックすることなく、スライディング制御を実現することができる。
【図面の簡単な説明】
【０００８】
【図１】軌道追従制御装置の例の機能ブロック図。
【図２】補正値入力部の例の機能ブロック図。
【図３】軌道追従制御方法の例の流れ図。
【図４】制御対象物の例である自律飛行船を例示する図。
【発明を実施するための形態】
【０００９】
［制御対象物］
制御の対象となる制御対象物は、例えば図４に例示する自律飛行船等の自律移動ロボットである。この自律飛行船は、舵１２、主推進器１３、上下方向推進器１４を装備している。また、図示していないが、横方向（方位角方向）推進器を備える。この自律飛行船の重心位置は低く設定されているので姿勢傾斜に対する復元力は大きく、自律飛行船の姿勢角は方位角以外は０に維持されるとする。以下、制御対象物が自律飛行船である場合を例に挙げて説明する。なお、制御の対象となる制御対象物は何でもよく、自律飛行船に限られない。
【００１０】
図１の自律飛行船のゴンドラ部には、５つの無線ＬＡＮ方式の位置計測センサ６１，６２，６３，６４，６５が取り付けられている。この例では、これらの位置計測センサ６１，６２，６３，６４，６５は、自律移動ロボットの水平面（ＸＹ平面）内重心位置（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ））を中心として水平面内に点対象に配置されているものとする。位置計測センサ６１，６２，６３，６４，６５は、水平面内重心位置（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ））から、それぞれ方位角にしてΦｓ１，Φｓ２，Φｓ３，Φｓ４，Φｓ５の方向に距離Ｒｓ１，Ｒｓ２，Ｒｓ３，Ｒｓ４，Ｒｓ５だけ離れた位置に固定されているものとする。各位置計測センサ６１，６２，６３，６４，６５は、水平面内位置（ＸＹ）及び高度（Ｚ）を計測することができる。
【００１１】
時刻ステップｔにおける位置計測センサ６１，６２，６３，６４，６５の計測値をそれぞれＺ１（ｔ）、Ｚ２（ｔ）、Ｚ３（ｔ）、Ｚ４（ｔ）、Ｚ５（ｔ）とする。
Ｚ１（ｔ）＝（Ｚｘ１（ｔ），Ｚｙ１（ｔ），Ｚｚ１（ｔ））
Ｚ２（ｔ）＝（Ｚｘ２（ｔ），Ｚｙ２（ｔ），Ｚｚ２（ｔ））
Ｚ３（ｔ）＝（Ｚｘ３（ｔ），Ｚｙ３（ｔ），Ｚｚ３（ｔ））
Ｚ４（ｔ）＝（Ｚｘ４（ｔ），Ｚｙ４（ｔ），Ｚｚ４（ｔ））
Ｚ５（ｔ）＝（Ｚｘ５（ｔ），Ｚｙ５（ｔ），Ｚｚ５（ｔ））
すると、これらの平均値（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｚａ（ｔ））は、以下のように計算できる。
【００１２】
Ｚｘａ（ｔ）＝［Ｚｘ１（ｔ）＋Ｚｘ２（ｔ）＋Ｚｘ３（ｔ）＋Ｚｘ４（ｔ）＋Ｚｘ５（ｔ）］／５ …（ａ）
Ｚｙａ（ｔ）＝［Ｚｙ１（ｔ）＋Ｚｙ２（ｔ）＋Ｚｙ３（ｔ）＋Ｚｙ４（ｔ）＋Ｚｙ５（ｔ）］／５ …（ｂ）
Ｚｚａ（ｔ）＝［Ｚｚ１（ｔ）＋Ｚｚ２（ｔ）＋Ｚｚ３（ｔ）＋Ｚｚ４（ｔ）＋Ｚｚ５（ｔ）］／５
センサが点対象に配置されていることから、上記平均値（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｚａ（ｔ））を自律飛行船の重心位置（Ｘｇ，Ｙｇ，Ｚｇ）の計測値として扱うことができる。以後、平均値（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｚａ（ｔ））を自律飛行船の重心位置（Ｘｇ，Ｙｇ，Ｚｇ）の計測値として説明する。
【００１３】
なお、方位角の値は、たとえば、飛行船の進行方向と方位角はほぼ一致していることを利用して、時刻ステップｔのＺｘａ（ｔ），Ｚｙａ（ｔ）及び時刻ステップｔ−１のＺｘａ（ｔ−１），Ｚｙａ（ｔ−１）を用いて、以下のような求め方で推定することが可能である。
【００１４】
【数１】
【００１５】
[スライディング制御]
スライディング制御は、制御対象物の時刻ステップｔにおける状態を表す変数（状態変数）Ｘ（ｔ）が、予め決定された目標とする状態Ｘｄ（ｔ）に近づくように、制御対象物に搭載された制御対象アクチュレータへの制御入力値ｕ（ｔ）を計算して、その計算された制御入力値ｕ（ｔ）で制御対象物に搭載された制御対象アクチュレータを制御するものである。以下では、制御対象物の状態変数Ｘ（ｔ）が制御対象物の方位角Φ（ｔ）である場合を例に挙げて説明する。もちろん、制御対象物の状態変数Ｘ（ｔ）は、方位角Φ（ｔ）に限らず、例えば制御対象物の機体座標での前進方向の変位量であってもよい。
【００１６】
制御対象物である自律飛行船は、船尾に装備された垂直尾翼の効果により、方位角方向の回転運動については安定であるとする。そのことから、自律飛行船の方位角方向の運動については、以下のような一次遅れ線形モデルを想定することも可能である。この線形モデルを使用してスライディング制御のアルゴリズムを設計することで、線形モデルに記述されていない飛行船の非線形な力学特性にも対応した軌道追従制御アルゴリズムを設計することができる。
【００１７】
【数２】
【００１８】
ここで、ｆは正の定数、ｂは定数（通常は正）、ｕ（ｔ）は制御入力値である。制御入力値は、例えば、自律飛行船に搭載された、スラスタの発生する方位角方向の回転トルクや、テイルロータへの指示電圧値、電流値である。
【００１９】
上記式（１）において、ｆが正であることにより、自律飛行船の方位角方向の運動の自己安定性が記述されている。すなわち、方位角方向になんの制御を加えずとも自然と飛行船の方位角がある角度に落ち着こう安定するという性質が再現されている。
【００２０】
目標軌道における時刻ステップｔでの目標方位角をΦｄ（ｔ）、目標方位角速度をＤΦｄ（ｔ）／Ｄｔ、目標方位角加速度をＤ２Φｄ（ｔ）／Ｄｔ２としたとき、スライディング制御による制御入力の計算式は以下の通りである。
【００２１】
【数３】
【００２２】
ここで、ｆｆａｒはｆの真値ｆｔｒｕｅの取り得る値の中でｆから最も離れた値、ｓｇｎ（ｘ）はｘ＞０のとき１、ｘ＜０のとき−１を出力する関数、η及びλは正定数、ｋ（ｔ）は正のフィードバック定数である。Ｆは、実際の自律飛行船におけるｆの値がどのくらいの不確かさを持っているかを含んだパラメータである。λ、ηは理論上任意の正定数を指定可能であるが、実際にはトライアンドエラーで値を調整することが多い。βについては、ｂの真値のとりうる値の範囲が最大ｂｍａｘ、最小ｂｍｉｎであるとし、ｂ＝√（ｂｍａｘｂｍｉｎ）であるとして、例えば次式により定義される値とする。
【００２３】
【数４】
【００２４】
［この発明の原理］
この発明の目的は、制御対象物の状態変数の一階微分ＤＸ（ｔ）／Ｄｔ（例えば、方位角速度ＤΦ（ｔ）／Ｄｔ）を用いず、制御対象物の状態変数Ｘ（ｔ）（例えば、方位角Φ（ｔ））の計測値のみを使用して、スライディング制御による軌道追従制御を可能とすることである。
【００２５】
式（２）（３）を見るとわかるように、制御入力値ｕ（ｔ）を決定する方程式の中には、方位角速度ＤΦ（ｔ）／Ｄｔが含まれている。したがって、このままでは、方位角速度ＤΦ（ｔ）／Ｄｔが高精度で計測または推定できない場合に、自律飛行船にスライディング制御を適用することはできない。そこで、理論上任意の正値を選択可能なλの値に注目する。今、λとしてｆと同じ値を選択したとする。すなわち、λ＝ｆとする。
【００２６】
この場合、式（２）及び式（３）は、
【００２７】
【数５】
【００２８】
となり、式（５）が示す通り式（２）から方位角速度ＤΦ（ｔ）／Ｄｔの項が消去される。しかし、式（５）は、まだ式（３）で示されるｓ（ｔ）の値を引用しているので、その部分で方位角速度ＤΦ（ｔ）／Ｄｔの値を参照していることになる。ここで、今、ｆが正確にモデル推定された結果ｆｔｒｕｅに等しいと仮定すると、同時にλ＝ｆｔｒｕｅであると言える。その場合、
【００２９】
【数６】
【００３０】
とおくと、
【００３１】
【数７】
【００３２】
とすることができ、−１／ｂ・ｋ（ｔ）・ｓｇｎ（ｓ（ｔ））が軌道からの誤差を修正するためのフィードバック項であることを考えると、ｕｅ（ｔ）及びｕ（ｔ）は、自律飛行船が目標軌道上にあるように制御するための制御入力値だということができる。実際、式（７）は、次式のように変形することができる。
【００３３】
【数８】
【００３４】
式（１）と比較してみたとき、式（９）が示すのは、式（７）を満たすｕｅ（ｔ）の値が、理論上は自律飛行船が目標速度（この例では、目標方位角速度）と目標加速度（この例では、目標方位角加速度）を正確に取れるように制御するための値であるということである。制御対象物である自律飛行船が式（１）の運動モデルにより運動しており、ｆ＝ｆｔｒｕｅであり、式（１）のｕ（ｔ）が式（７）に基づいて計算されるｕｅ（ｔ）である場合には、理論上、方位角速度ＤΦ（ｔ）／Ｄｔと目標方位角速度ＤΦｄ（ｔ）／Ｄｔとが一致し、方位角加速度Ｄ２Φ（ｔ）／Ｄｔ２と目標方位角加速度Ｄ２Φｄ（ｔ）／Ｄｔ２とが一致する。実際には、ｕｅ（ｔ）ではなく式（８）で定義されるｕ（ｔ）により自律飛行船は制御されるが、ｕｅ（ｔ）とｕ（ｔ）との差分である（１／ｂ）（ｋ（ｔ）ｓｇｎ（ｓ（ｔ）））は、位置（この例では、方位角）の誤差についての項であるため、ｕ（ｔ）で制御しても、理論上、方位角速度ＤΦ（ｔ）／Ｄｔと目標方位角速度ＤΦｄ（ｔ）／Ｄｔとが一致し、方位角加速度Ｄ２Φ（ｔ）／Ｄｔ２と目標方位角加速度Ｄ２Φｄ（ｔ）／Ｄｔ２とが一致することに変わりはない。換言すれば、（１／ｂ）ｋ（ｔ）ｓｇｎ（ｓ（ｔ））の値は、自律飛行船が目標軌道上から外れた際の補正のための制御入力値であると言えるから、計測された方位角速度ＤΦ（ｔ）／Ｄｔの値は十分に目標方位角速度ＤΦｄ（ｔ）／Ｄｔに近いと仮定できる。すなわち、式（４）は次のように書くことができ、方位角速度ＤΦ（ｔ）／Ｄｔの項を消去することができる。
【００３５】
【数９】
【００３６】
式（３）については、式（６）が示す通り、まだ方位角速度ＤΦ（ｔ）／Ｄｔの項が残っているが式（６）は不等式なので、方位角速度ＤΦ（ｔ）／Ｄｔの値の取りうる範囲を考慮して、式（６）の不等式が示す条件に合う関数又は値を決定すればよい。例えば、ｋ（ｔ）を、式（６）の不等式を満たすような十分大きな定数値としても良いし、下記式（１０）のような関数の値としても良い。
【００３７】
【数１０】
【００３８】
式（１０）において、Φｖｍａｘは、ＤΦ（ｔ）／Ｄｔの想定される取り得る値の絶対値の最大値である。このようにすれば、ｋ（ｔ）の式から方位角速度の計測値ＤΦ（ｔ）／Ｄｔの項を消去できる。
【００３９】
また、λ＝ｆ＝ｆｔｒｕｅであるという仮定が成立するならば、Ｆ＝０とみなしてもよいため、式（１０）を下記式（１１）としてもよい。
【００４０】
【数１１】
【００４１】
以上により、λ＝ｆ＝ｆｔｒｕｅという仮定のもと、スライディング制御の式中の方位角速度ＤΦ（ｔ）／Ｄｔの項を消去することができることが分かったが、その仮定を現実のものとするためには、λの値がｆｔｒｕｅに近づくようにλの値をオンライン自動調整することが有効である。λの値の調整のためには、Φ（ｔ）−Φｄ（ｔ）のフィードバック値を使用する。今、式（５）において、以下の（１）から（４）のことが言える。
【００４２】
（１）ＤΦｄ（ｔ）／Ｄｔ＞０のとき、λが適切な値ｆｔｒｕｅより小さい場合は、λ＞０なので、ｕ（ｔ）はその分小さくなり、結果として、Φ（ｔ）−Φｄ（ｔ）の値は負の値になっているはずである。すなわち、ＤΦｄ（ｔ）／Ｄｔ＞０、かつ、Φ（ｔ）−Φｄ（ｔ）＜０であれば、λは適切な値ｆｔｒｕｅより小さくなっている。この場合には、λをより大きくすることが望ましい。
【００４３】
（２）ＤΦｄ（ｔ）／Ｄｔ＜０のときで、λが適切な値ｆｔｒｕｅより小さい場合は、λ＞０なので、ｕ（ｔ）もその分大きくなり、結果として、Φ（ｔ）−Φｄ（ｔ）の値は正の値になっているはずである。すなわち、ＤΦｄ（ｔ）／Ｄｔ＜０、かつ、Φ（ｔ）−Φｄ（ｔ）＞０であれば、λは適切な値ｆｔｒｕｅより小さくなっている。この場合には、λをより大きくすることが望ましい。
【００４４】
（３）ＤΦｄ（ｔ）／Ｄｔ＞０のとき、λが適切な値ｆｔｒｕｅより大きい場合は、λ＞０なので、ｕ（ｔ）はその分大きくなり、結果として、Φ（ｔ）−Φｄ（ｔ）の値は正の値になっているはずである。すなわち、ＤΦｄ（ｔ）／Ｄｔ＞０、かつ、Φ（ｔ）−Φｄ（ｔ）＞０であれば、λは適切な値ｆｔｒｕｅより大きくなっている。この場合には、λをより小さくすることが望ましい。
【００４５】
（４）ＤΦｄ（ｔ）／Ｄｔ＜０のときで、λが適切な値より大きい場合は、λ＞０なので、ｕ（ｔ）もその分小さくなり、結果として、Φ（ｔ）−Φｄ（ｔ）の値は負の値になっているはずである。すなわち、ＤΦｄ（ｔ）／Ｄｔ＜０、かつ、Φ（ｔ）−Φｄ（ｔ）＜０であれば、λは適切な値ｆｔｒｕｅより大きくなっている。この場合には、λをより小さくすることが望ましい。
【００４６】
以上の見識に基づいて、例えば以下ようにλの値を制御する。以下では、ＤΦｄ（ｔ）／Ｄｔ＞０の場合のλをλ＋と表記し、ＤΦｄ（ｔ）／Ｄｔ＜０の場合のλをλ−と表記している。γは０＜γ＜１の数であり、「Ａ＝Ｂ」はＢの値をＡの値とすることを意味する。
【００４７】
（１）の場合、すなわちＤΦｄ（ｔ）／Ｄｔ＞０、かつ、Φ（ｔ）−Φｄ（ｔ）＜０の場合には、λ＋＝（１＋γ）λ＋とする。
【００４８】
（２）の場合、すなわちＤΦｄ（ｔ）／Ｄｔ＜０、かつ、Φ（ｔ）−Φｄ（ｔ）＞０の場合には、λ−＝（１＋γ）λ−とする。
【００４９】
（３）の場合、すなわちＤΦｄ（ｔ）／Ｄｔ＞０、かつ、Φ（ｔ）−Φｄ（ｔ）＞０の場合には、λ＋＝（１−γ）λ＋とする。
【００５０】
（４）の場合、すなわちＤΦｄ（ｔ）／Ｄｔ＜０、かつ、Φ（ｔ）−Φｄ（ｔ）＜０の場合には、λ−＝（１−γ）λ−とする。
【００５１】
自律飛行船が左右に１つずつ前進用スラスタを装備していような場合において、左右のスラスタ性能の個体差等によって発生する左右回転時でのトルクのムラによる制御誤差の拡大を防ぐために、方位角速度ＤΦｄ（ｔ）／Ｄｔの値の正負それぞれの場合で、λ＋の初期値とλ−の初期値に異なる値を設定して管理してもよい。もちろん、λ＋の初期値とλ−の初期値を同じ値としてもよい。
【００５２】
以上のように、スライディング制御の制御入力値ｕ（ｔ）を計算する方程式中のパラメータλの値を適切な値に設定することで、速度ＤΦｄ（ｔ）／Ｄｔの計測値を利用することなくスライディング制御による自律飛行船の軌道追従制御を行うことができる。
【００５３】
［実施形態］
この発明の一実施形態による軌道追従制御装置は、図１に例示するように、時刻設定部１、位置計測センサ６１，６２，６３，６４，６５、計測部２、記憶部３、方位角計算部４、定数値設定部６、補正入力値計算部７、制御入力値計算部８及び制御部９を例えば含む。この軌道追従制御装置は、図３に示す流れ図の各ステップの処理を例えば行う。
【００５４】
時刻設定部１は、時刻ステップｔ（ｔは０以上の整数）の値を設定する。まず、時刻設定部１は、時刻ステップの初期値ｔ＝０とする（ステップＳ１）。その後、時刻設定部１は、時刻ステップｔを１ずつ増やす。軌道追従制御装置は、時刻設定部１が設定した各時刻ステップｔにおける制御入力値ｕ（ｔ）を計算する。
【００５５】
計測部２は、自律飛行船に搭載された位置計測センサ６１，６２，６３，６４，６５から取得した計測結果を用いて、時刻ステップｔにおける自律飛行船のＸＹ平面における位置Ｚｘａ（ｔ）及びＺｙａ（ｔ）を計算する（ステップＳ２）。計算された位置Ｚｘａ（ｔ）及びＺｙａ（ｔ）についての情報は、時刻ステップｔと対応付けて記憶部３に記憶される。
【００５６】
記憶部３には、各時刻ステップｔの自律飛行船の目標軌道における目標方位角Φｄ（ｔ）、目標角速度ＤΦｄ（ｔ）／Ｄｔ及び目標方位角加速度Ｄ２Φｄ（ｔ）／Ｄｔ２が予め記憶されているものとする。目標方位角Φｄ（ｔ）、目標角速度ＤΦｄ（ｔ）／Ｄｔ及び目標方位角加速度Ｄ２Φｄ（ｔ）／Ｄｔ２は、ユーザが事前に設定してもよいし、例えば特開２００８−２２７４３１号公報に記載されているような既存のアルゴリズムを用いてコンピュータが計算してもよい。記憶部３には、Ｚｘａ（ｔ）及びＺｙａ（ｔ）についての情報も記憶される。
【００５７】
方位角計算部４は、計測部２から取得した時刻ステップｔにおける自律飛行船のＸＹ平面における位置Ｚｘａ（ｔ）及びＺｙａ（ｔ）と、記憶部３に記憶されている１時刻ステップ前の時刻ステップｔ−１における自律飛行船のＸＹ平面における位置Ｚｘａ（ｔ−１）及びＺｙａ（ｔ−１）とから、式（０）により、時刻ステップｔにおける方位角Φ（ｔ）を推定する（ステップＳ３）。計算された方位角Φ（ｔ）は、定数値設定部６に送られる。
【００５８】
定数値設定部６は、方位角計算部４が推定した時刻ステップｔにおける自律飛行船の方位角Φ（ｔ）と、記憶部３から読み込んだ目標位置目標方位角Φｄ（ｔ）及び目標方位角速度ＤΦｄ（ｔ）／Ｄｔとを用いて、λを適宜調整する（ステップＳ４）。設定されたλは、補正入力値計算部７及び制御入力値計算部８に送られる。
【００５９】
例えば、γを０＜γ＜１の数として、定数値設定部６は、ＤΦｄ（ｔ）／Ｄｔ＞０かつΦ（ｔ）−Φｄ（ｔ）＜０であればλ＝（１＋γ）λとし、ＤΦｄ（ｔ）／Ｄｔ＜０かつΦ（ｔ）−Φｄ（ｔ）＞０であればλ＝（１＋γ）λとし、ＤΦｄ（ｔ）／Ｄｔ＞０かつΦ（ｔ）−Φｄ（ｔ）＞０であればλ＝（１−γ）λとし、ＤΦｄ（ｔ）／Ｄｔ＜０かつΦ（ｔ）−Φｄ（ｔ）＜０であればλ＝（１−γ）λとする。λの値の設定は、（１＋γ）λまたは（１−γ）λに限られるものではなく、ＤΦｄ（ｔ）／Ｄｔ＞０かつΦ（ｔ）−Φｄ（ｔ）＜０の場合、または、ＤΦｄ（ｔ）／Ｄｔ＜０かつΦ（ｔ）−Φｄ（ｔ）＞０の場合には、λの値が元のλの値よりも大きくなるように設定し、ＤΦｄ（ｔ）／Ｄｔ＞０かつΦ（ｔ）−Φｄ（ｔ）＞０の場合、または、ＤΦｄ（ｔ）／Ｄｔ＜０かつΦ（ｔ）−Φｄ（ｔ）＜０の場合には、λの値が元のλの値よりも小さくなるように設定されるものであればなんでも良い。例えば、λ＝λ＋ｃ，λ＝λ−ｃ（ｃは正の定数）としても良い。なお、λの増大と減少の幅は等しくするほうが、精度良く推定を行うことができる。
【００６０】
補正入力値計算部７は、補正入力値ｋ（ｔ）ｓｇｎ（ｓ（ｔ））を計算する（ステップＳ５）。計算された補正入力値ｋ（ｔ）ｓｇｎ（ｓ（ｔ））は、制御入力値計算部８に送られる。
【００６１】
例えば、フィードバック定数ｋ（ｔ）は、ｋ（ｔ）を、式（６）の不等式を満たすような十分大きな定数値としても良いし、式（１０）や式（１１）のような関数の値としてもよい。また、ずれ値ｓ（ｔ）をｓ（ｔ）＝λ（Φ（ｔ）−Φｄ（ｔ））とし、関数ｓｇｎ（・）を・が正であれば１を出力し・が負であれば−１を出力する関数とする。
【００６２】
補正入力値計算部７は、図２に例示するように、フィードバック定数ｋ（ｔ）を計算するフィードバック定数計算部７１と、ずれ値ｓ（ｔ）を計算するずれ値計算部７２と、計算されたフィードバック定数ｋ（ｔ）及びずれ値ｓ（ｔ）を用いて補正入力値を計算する合成算部７３とを含む。
【００６３】
制御入力値計算部８は、設定された定数λ及び計算された補正入力値ｋ（ｔ）ｓｇｎ（ｓ（ｔ））を用いて、式（５）で定義される制御入力値ｕ（ｔ）を計算する（ステップＳ８）。計算された制御入力値ｕ（ｔ）は、制御部９に送られる。
【００６４】
制御部９は、制御入力値ｕ（ｔ）で自律飛行船に搭載された制御対象アクチュエータを制御する（ステップＳ７）。
【００６５】
［変形例等］
上記の例では、制御対象物の状態変数Ｘ（ｔ）として方位角Φ（ｔ）を用いたが、制御対象物の状態変数Ｘ（ｔ）として他の変数を選択してもよい。例えば、上記の説明において方位角Φ（ｔ）を制御対象物の機体座標での前進方向の変位量に置き換えれば、前進方向の軌道追従制御を実現することができる。
【００６６】
また、式（５）及び式（８）における関数ｓｇｎ（・）の代わりに、ｓ＝０近辺に不感帯を設けた以下の関数ｓｇｎ＿ｍｏｄ（・）を用いても良い。Ωは予め定められた定数であり、例えば０に近い値に設定される。
【００６７】
【数１２】
【００６８】
これにより、関数ｓｇｎ（・）の符号切り替えの影響で、制御入力値ｕ（ｔ）の値がチャタリングを起こしてしまう問題を防ぐことができる。
【００６９】
軌道追従制御装置の各部間のデータのやり取りは直接行われてもよいし、記憶部３を介して行われてもよい。
【００７０】
軌道追従制御装置は、コンピュータによって実現することができる。この場合、この装置が有すべき各機能の処理内容はプログラムによって記述される。そして、このプログラムをコンピュータで実行することにより、これ装置における各処理機能が、コンピュータ上で実現される。
【００７１】
この処理内容を記述したプログラムは、コンピュータで読み取り可能な記録媒体に記録しておくことができる。また、この形態では、コンピュータ上で所定のプログラムを実行させることにより、これらの装置を構成することとしたが、これらの処理内容の少なくとも一部をハードウェア的に実現することとしてもよい。
【００７２】
この発明は、上述の実施形態に限定されるものではなく、本発明の趣旨を逸脱しない範囲で適宜変更が可能である。
【符号の説明】
【００７３】
６ 定数値設定部
７ 補正入力値計算部
８ 制御入力値計算部
９ 制御部
【技術分野】
【０００１】
この発明は、制御対象物の位置を目標軌道に追従させる制御技術に関する。
【背景技術】
【０００２】
近年、屋内外で活動可能な自律移動ロボットの研究が活発に行われており、それらの応用先が広がりつつある。自律移動ロボットの種類としては、車輪を使用して地表を移動するタイプのものや、空を飛行して移動する自律型空中ロボット、水中を航行する水中型ロボットなどがある。それらの自律移動ロボットの行動を高精度で制御するアルゴリズムを確立する上では、自律移動ロボットに予め定めた目標軌道上を高精度に追従させるための技術が必須である。
【０００３】
自律移動ロボットに予め定めた目標軌道を追従させるための制御方法の代表的なものとしてスライディング制御の方法がある（例えば、非特許文献１参照。）。スライディング制御の方法は、制御対象の運動特性の不確定性や非線形性を考慮したロバスト制御と呼ばれる手法の一つである。自律飛行船等の自律移動ロボットは、その運動特性がモデル化困難な要素を持ち、かつ非線形の力学特性に従うものであるため、その軌道追従制御のためには、スライディング制御を使用するのが適切である。
【先行技術文献】
【非特許文献】
【０００４】
【非特許文献１】Jean-Jacques E. Slotine, Weiping Li, "Applied Nonlinear Control" Prentice-Hall, 1991,pp.276-309
【発明の概要】
【発明が解決しようとする課題】
【０００５】
非特許文献１に記載されたスライディング制御では、制御対象物である自律移動ロボットの位置だけではなく、速度を計測してフィードバックをする必要があるという課題があった。
【課題を解決するための手段】
【０００６】
上記の課題を解決するために、スライディング制御の制御入力値を計算する式の中の定数λの値を適切な値に設定することで、その式の中の制御対象物の速度の項を消す。
【発明の効果】
【０００７】
制御対象物の速度を計測及びフィードバックすることなく、スライディング制御を実現することができる。
【図面の簡単な説明】
【０００８】
【図１】軌道追従制御装置の例の機能ブロック図。
【図２】補正値入力部の例の機能ブロック図。
【図３】軌道追従制御方法の例の流れ図。
【図４】制御対象物の例である自律飛行船を例示する図。
【発明を実施するための形態】
【０００９】
［制御対象物］
制御の対象となる制御対象物は、例えば図４に例示する自律飛行船等の自律移動ロボットである。この自律飛行船は、舵１２、主推進器１３、上下方向推進器１４を装備している。また、図示していないが、横方向（方位角方向）推進器を備える。この自律飛行船の重心位置は低く設定されているので姿勢傾斜に対する復元力は大きく、自律飛行船の姿勢角は方位角以外は０に維持されるとする。以下、制御対象物が自律飛行船である場合を例に挙げて説明する。なお、制御の対象となる制御対象物は何でもよく、自律飛行船に限られない。
【００１０】
図１の自律飛行船のゴンドラ部には、５つの無線ＬＡＮ方式の位置計測センサ６１，６２，６３，６４，６５が取り付けられている。この例では、これらの位置計測センサ６１，６２，６３，６４，６５は、自律移動ロボットの水平面（ＸＹ平面）内重心位置（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ））を中心として水平面内に点対象に配置されているものとする。位置計測センサ６１，６２，６３，６４，６５は、水平面内重心位置（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ））から、それぞれ方位角にしてΦｓ１，Φｓ２，Φｓ３，Φｓ４，Φｓ５の方向に距離Ｒｓ１，Ｒｓ２，Ｒｓ３，Ｒｓ４，Ｒｓ５だけ離れた位置に固定されているものとする。各位置計測センサ６１，６２，６３，６４，６５は、水平面内位置（ＸＹ）及び高度（Ｚ）を計測することができる。
【００１１】
時刻ステップｔにおける位置計測センサ６１，６２，６３，６４，６５の計測値をそれぞれＺ１（ｔ）、Ｚ２（ｔ）、Ｚ３（ｔ）、Ｚ４（ｔ）、Ｚ５（ｔ）とする。
Ｚ１（ｔ）＝（Ｚｘ１（ｔ），Ｚｙ１（ｔ），Ｚｚ１（ｔ））
Ｚ２（ｔ）＝（Ｚｘ２（ｔ），Ｚｙ２（ｔ），Ｚｚ２（ｔ））
Ｚ３（ｔ）＝（Ｚｘ３（ｔ），Ｚｙ３（ｔ），Ｚｚ３（ｔ））
Ｚ４（ｔ）＝（Ｚｘ４（ｔ），Ｚｙ４（ｔ），Ｚｚ４（ｔ））
Ｚ５（ｔ）＝（Ｚｘ５（ｔ），Ｚｙ５（ｔ），Ｚｚ５（ｔ））
すると、これらの平均値（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｚａ（ｔ））は、以下のように計算できる。
【００１２】
Ｚｘａ（ｔ）＝［Ｚｘ１（ｔ）＋Ｚｘ２（ｔ）＋Ｚｘ３（ｔ）＋Ｚｘ４（ｔ）＋Ｚｘ５（ｔ）］／５ …（ａ）
Ｚｙａ（ｔ）＝［Ｚｙ１（ｔ）＋Ｚｙ２（ｔ）＋Ｚｙ３（ｔ）＋Ｚｙ４（ｔ）＋Ｚｙ５（ｔ）］／５ …（ｂ）
Ｚｚａ（ｔ）＝［Ｚｚ１（ｔ）＋Ｚｚ２（ｔ）＋Ｚｚ３（ｔ）＋Ｚｚ４（ｔ）＋Ｚｚ５（ｔ）］／５
センサが点対象に配置されていることから、上記平均値（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｚａ（ｔ））を自律飛行船の重心位置（Ｘｇ，Ｙｇ，Ｚｇ）の計測値として扱うことができる。以後、平均値（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｚａ（ｔ））を自律飛行船の重心位置（Ｘｇ，Ｙｇ，Ｚｇ）の計測値として説明する。
【００１３】
なお、方位角の値は、たとえば、飛行船の進行方向と方位角はほぼ一致していることを利用して、時刻ステップｔのＺｘａ（ｔ），Ｚｙａ（ｔ）及び時刻ステップｔ−１のＺｘａ（ｔ−１），Ｚｙａ（ｔ−１）を用いて、以下のような求め方で推定することが可能である。
【００１４】
【数１】
【００１５】
[スライディング制御]
スライディング制御は、制御対象物の時刻ステップｔにおける状態を表す変数（状態変数）Ｘ（ｔ）が、予め決定された目標とする状態Ｘｄ（ｔ）に近づくように、制御対象物に搭載された制御対象アクチュレータへの制御入力値ｕ（ｔ）を計算して、その計算された制御入力値ｕ（ｔ）で制御対象物に搭載された制御対象アクチュレータを制御するものである。以下では、制御対象物の状態変数Ｘ（ｔ）が制御対象物の方位角Φ（ｔ）である場合を例に挙げて説明する。もちろん、制御対象物の状態変数Ｘ（ｔ）は、方位角Φ（ｔ）に限らず、例えば制御対象物の機体座標での前進方向の変位量であってもよい。
【００１６】
制御対象物である自律飛行船は、船尾に装備された垂直尾翼の効果により、方位角方向の回転運動については安定であるとする。そのことから、自律飛行船の方位角方向の運動については、以下のような一次遅れ線形モデルを想定することも可能である。この線形モデルを使用してスライディング制御のアルゴリズムを設計することで、線形モデルに記述されていない飛行船の非線形な力学特性にも対応した軌道追従制御アルゴリズムを設計することができる。
【００１７】
【数２】
【００１８】
ここで、ｆは正の定数、ｂは定数（通常は正）、ｕ（ｔ）は制御入力値である。制御入力値は、例えば、自律飛行船に搭載された、スラスタの発生する方位角方向の回転トルクや、テイルロータへの指示電圧値、電流値である。
【００１９】
上記式（１）において、ｆが正であることにより、自律飛行船の方位角方向の運動の自己安定性が記述されている。すなわち、方位角方向になんの制御を加えずとも自然と飛行船の方位角がある角度に落ち着こう安定するという性質が再現されている。
【００２０】
目標軌道における時刻ステップｔでの目標方位角をΦｄ（ｔ）、目標方位角速度をＤΦｄ（ｔ）／Ｄｔ、目標方位角加速度をＤ２Φｄ（ｔ）／Ｄｔ２としたとき、スライディング制御による制御入力の計算式は以下の通りである。
【００２１】
【数３】
【００２２】
ここで、ｆｆａｒはｆの真値ｆｔｒｕｅの取り得る値の中でｆから最も離れた値、ｓｇｎ（ｘ）はｘ＞０のとき１、ｘ＜０のとき−１を出力する関数、η及びλは正定数、ｋ（ｔ）は正のフィードバック定数である。Ｆは、実際の自律飛行船におけるｆの値がどのくらいの不確かさを持っているかを含んだパラメータである。λ、ηは理論上任意の正定数を指定可能であるが、実際にはトライアンドエラーで値を調整することが多い。βについては、ｂの真値のとりうる値の範囲が最大ｂｍａｘ、最小ｂｍｉｎであるとし、ｂ＝√（ｂｍａｘｂｍｉｎ）であるとして、例えば次式により定義される値とする。
【００２３】
【数４】
【００２４】
［この発明の原理］
この発明の目的は、制御対象物の状態変数の一階微分ＤＸ（ｔ）／Ｄｔ（例えば、方位角速度ＤΦ（ｔ）／Ｄｔ）を用いず、制御対象物の状態変数Ｘ（ｔ）（例えば、方位角Φ（ｔ））の計測値のみを使用して、スライディング制御による軌道追従制御を可能とすることである。
【００２５】
式（２）（３）を見るとわかるように、制御入力値ｕ（ｔ）を決定する方程式の中には、方位角速度ＤΦ（ｔ）／Ｄｔが含まれている。したがって、このままでは、方位角速度ＤΦ（ｔ）／Ｄｔが高精度で計測または推定できない場合に、自律飛行船にスライディング制御を適用することはできない。そこで、理論上任意の正値を選択可能なλの値に注目する。今、λとしてｆと同じ値を選択したとする。すなわち、λ＝ｆとする。
【００２６】
この場合、式（２）及び式（３）は、
【００２７】
【数５】
【００２８】
となり、式（５）が示す通り式（２）から方位角速度ＤΦ（ｔ）／Ｄｔの項が消去される。しかし、式（５）は、まだ式（３）で示されるｓ（ｔ）の値を引用しているので、その部分で方位角速度ＤΦ（ｔ）／Ｄｔの値を参照していることになる。ここで、今、ｆが正確にモデル推定された結果ｆｔｒｕｅに等しいと仮定すると、同時にλ＝ｆｔｒｕｅであると言える。その場合、
【００２９】
【数６】
【００３０】
とおくと、
【００３１】
【数７】
【００３２】
とすることができ、−１／ｂ・ｋ（ｔ）・ｓｇｎ（ｓ（ｔ））が軌道からの誤差を修正するためのフィードバック項であることを考えると、ｕｅ（ｔ）及びｕ（ｔ）は、自律飛行船が目標軌道上にあるように制御するための制御入力値だということができる。実際、式（７）は、次式のように変形することができる。
【００３３】
【数８】
【００３４】
式（１）と比較してみたとき、式（９）が示すのは、式（７）を満たすｕｅ（ｔ）の値が、理論上は自律飛行船が目標速度（この例では、目標方位角速度）と目標加速度（この例では、目標方位角加速度）を正確に取れるように制御するための値であるということである。制御対象物である自律飛行船が式（１）の運動モデルにより運動しており、ｆ＝ｆｔｒｕｅであり、式（１）のｕ（ｔ）が式（７）に基づいて計算されるｕｅ（ｔ）である場合には、理論上、方位角速度ＤΦ（ｔ）／Ｄｔと目標方位角速度ＤΦｄ（ｔ）／Ｄｔとが一致し、方位角加速度Ｄ２Φ（ｔ）／Ｄｔ２と目標方位角加速度Ｄ２Φｄ（ｔ）／Ｄｔ２とが一致する。実際には、ｕｅ（ｔ）ではなく式（８）で定義されるｕ（ｔ）により自律飛行船は制御されるが、ｕｅ（ｔ）とｕ（ｔ）との差分である（１／ｂ）（ｋ（ｔ）ｓｇｎ（ｓ（ｔ）））は、位置（この例では、方位角）の誤差についての項であるため、ｕ（ｔ）で制御しても、理論上、方位角速度ＤΦ（ｔ）／Ｄｔと目標方位角速度ＤΦｄ（ｔ）／Ｄｔとが一致し、方位角加速度Ｄ２Φ（ｔ）／Ｄｔ２と目標方位角加速度Ｄ２Φｄ（ｔ）／Ｄｔ２とが一致することに変わりはない。換言すれば、（１／ｂ）ｋ（ｔ）ｓｇｎ（ｓ（ｔ））の値は、自律飛行船が目標軌道上から外れた際の補正のための制御入力値であると言えるから、計測された方位角速度ＤΦ（ｔ）／Ｄｔの値は十分に目標方位角速度ＤΦｄ（ｔ）／Ｄｔに近いと仮定できる。すなわち、式（４）は次のように書くことができ、方位角速度ＤΦ（ｔ）／Ｄｔの項を消去することができる。
【００３５】
【数９】
【００３６】
式（３）については、式（６）が示す通り、まだ方位角速度ＤΦ（ｔ）／Ｄｔの項が残っているが式（６）は不等式なので、方位角速度ＤΦ（ｔ）／Ｄｔの値の取りうる範囲を考慮して、式（６）の不等式が示す条件に合う関数又は値を決定すればよい。例えば、ｋ（ｔ）を、式（６）の不等式を満たすような十分大きな定数値としても良いし、下記式（１０）のような関数の値としても良い。
【００３７】
【数１０】
【００３８】
式（１０）において、Φｖｍａｘは、ＤΦ（ｔ）／Ｄｔの想定される取り得る値の絶対値の最大値である。このようにすれば、ｋ（ｔ）の式から方位角速度の計測値ＤΦ（ｔ）／Ｄｔの項を消去できる。
【００３９】
また、λ＝ｆ＝ｆｔｒｕｅであるという仮定が成立するならば、Ｆ＝０とみなしてもよいため、式（１０）を下記式（１１）としてもよい。
【００４０】
【数１１】
【００４１】
以上により、λ＝ｆ＝ｆｔｒｕｅという仮定のもと、スライディング制御の式中の方位角速度ＤΦ（ｔ）／Ｄｔの項を消去することができることが分かったが、その仮定を現実のものとするためには、λの値がｆｔｒｕｅに近づくようにλの値をオンライン自動調整することが有効である。λの値の調整のためには、Φ（ｔ）−Φｄ（ｔ）のフィードバック値を使用する。今、式（５）において、以下の（１）から（４）のことが言える。
【００４２】
（１）ＤΦｄ（ｔ）／Ｄｔ＞０のとき、λが適切な値ｆｔｒｕｅより小さい場合は、λ＞０なので、ｕ（ｔ）はその分小さくなり、結果として、Φ（ｔ）−Φｄ（ｔ）の値は負の値になっているはずである。すなわち、ＤΦｄ（ｔ）／Ｄｔ＞０、かつ、Φ（ｔ）−Φｄ（ｔ）＜０であれば、λは適切な値ｆｔｒｕｅより小さくなっている。この場合には、λをより大きくすることが望ましい。
【００４３】
（２）ＤΦｄ（ｔ）／Ｄｔ＜０のときで、λが適切な値ｆｔｒｕｅより小さい場合は、λ＞０なので、ｕ（ｔ）もその分大きくなり、結果として、Φ（ｔ）−Φｄ（ｔ）の値は正の値になっているはずである。すなわち、ＤΦｄ（ｔ）／Ｄｔ＜０、かつ、Φ（ｔ）−Φｄ（ｔ）＞０であれば、λは適切な値ｆｔｒｕｅより小さくなっている。この場合には、λをより大きくすることが望ましい。
【００４４】
（３）ＤΦｄ（ｔ）／Ｄｔ＞０のとき、λが適切な値ｆｔｒｕｅより大きい場合は、λ＞０なので、ｕ（ｔ）はその分大きくなり、結果として、Φ（ｔ）−Φｄ（ｔ）の値は正の値になっているはずである。すなわち、ＤΦｄ（ｔ）／Ｄｔ＞０、かつ、Φ（ｔ）−Φｄ（ｔ）＞０であれば、λは適切な値ｆｔｒｕｅより大きくなっている。この場合には、λをより小さくすることが望ましい。
【００４５】
（４）ＤΦｄ（ｔ）／Ｄｔ＜０のときで、λが適切な値より大きい場合は、λ＞０なので、ｕ（ｔ）もその分小さくなり、結果として、Φ（ｔ）−Φｄ（ｔ）の値は負の値になっているはずである。すなわち、ＤΦｄ（ｔ）／Ｄｔ＜０、かつ、Φ（ｔ）−Φｄ（ｔ）＜０であれば、λは適切な値ｆｔｒｕｅより大きくなっている。この場合には、λをより小さくすることが望ましい。
【００４６】
以上の見識に基づいて、例えば以下ようにλの値を制御する。以下では、ＤΦｄ（ｔ）／Ｄｔ＞０の場合のλをλ＋と表記し、ＤΦｄ（ｔ）／Ｄｔ＜０の場合のλをλ−と表記している。γは０＜γ＜１の数であり、「Ａ＝Ｂ」はＢの値をＡの値とすることを意味する。
【００４７】
（１）の場合、すなわちＤΦｄ（ｔ）／Ｄｔ＞０、かつ、Φ（ｔ）−Φｄ（ｔ）＜０の場合には、λ＋＝（１＋γ）λ＋とする。
【００４８】
（２）の場合、すなわちＤΦｄ（ｔ）／Ｄｔ＜０、かつ、Φ（ｔ）−Φｄ（ｔ）＞０の場合には、λ−＝（１＋γ）λ−とする。
【００４９】
（３）の場合、すなわちＤΦｄ（ｔ）／Ｄｔ＞０、かつ、Φ（ｔ）−Φｄ（ｔ）＞０の場合には、λ＋＝（１−γ）λ＋とする。
【００５０】
（４）の場合、すなわちＤΦｄ（ｔ）／Ｄｔ＜０、かつ、Φ（ｔ）−Φｄ（ｔ）＜０の場合には、λ−＝（１−γ）λ−とする。
【００５１】
自律飛行船が左右に１つずつ前進用スラスタを装備していような場合において、左右のスラスタ性能の個体差等によって発生する左右回転時でのトルクのムラによる制御誤差の拡大を防ぐために、方位角速度ＤΦｄ（ｔ）／Ｄｔの値の正負それぞれの場合で、λ＋の初期値とλ−の初期値に異なる値を設定して管理してもよい。もちろん、λ＋の初期値とλ−の初期値を同じ値としてもよい。
【００５２】
以上のように、スライディング制御の制御入力値ｕ（ｔ）を計算する方程式中のパラメータλの値を適切な値に設定することで、速度ＤΦｄ（ｔ）／Ｄｔの計測値を利用することなくスライディング制御による自律飛行船の軌道追従制御を行うことができる。
【００５３】
［実施形態］
この発明の一実施形態による軌道追従制御装置は、図１に例示するように、時刻設定部１、位置計測センサ６１，６２，６３，６４，６５、計測部２、記憶部３、方位角計算部４、定数値設定部６、補正入力値計算部７、制御入力値計算部８及び制御部９を例えば含む。この軌道追従制御装置は、図３に示す流れ図の各ステップの処理を例えば行う。
【００５４】
時刻設定部１は、時刻ステップｔ（ｔは０以上の整数）の値を設定する。まず、時刻設定部１は、時刻ステップの初期値ｔ＝０とする（ステップＳ１）。その後、時刻設定部１は、時刻ステップｔを１ずつ増やす。軌道追従制御装置は、時刻設定部１が設定した各時刻ステップｔにおける制御入力値ｕ（ｔ）を計算する。
【００５５】
計測部２は、自律飛行船に搭載された位置計測センサ６１，６２，６３，６４，６５から取得した計測結果を用いて、時刻ステップｔにおける自律飛行船のＸＹ平面における位置Ｚｘａ（ｔ）及びＺｙａ（ｔ）を計算する（ステップＳ２）。計算された位置Ｚｘａ（ｔ）及びＺｙａ（ｔ）についての情報は、時刻ステップｔと対応付けて記憶部３に記憶される。
【００５６】
記憶部３には、各時刻ステップｔの自律飛行船の目標軌道における目標方位角Φｄ（ｔ）、目標角速度ＤΦｄ（ｔ）／Ｄｔ及び目標方位角加速度Ｄ２Φｄ（ｔ）／Ｄｔ２が予め記憶されているものとする。目標方位角Φｄ（ｔ）、目標角速度ＤΦｄ（ｔ）／Ｄｔ及び目標方位角加速度Ｄ２Φｄ（ｔ）／Ｄｔ２は、ユーザが事前に設定してもよいし、例えば特開２００８−２２７４３１号公報に記載されているような既存のアルゴリズムを用いてコンピュータが計算してもよい。記憶部３には、Ｚｘａ（ｔ）及びＺｙａ（ｔ）についての情報も記憶される。
【００５７】
方位角計算部４は、計測部２から取得した時刻ステップｔにおける自律飛行船のＸＹ平面における位置Ｚｘａ（ｔ）及びＺｙａ（ｔ）と、記憶部３に記憶されている１時刻ステップ前の時刻ステップｔ−１における自律飛行船のＸＹ平面における位置Ｚｘａ（ｔ−１）及びＺｙａ（ｔ−１）とから、式（０）により、時刻ステップｔにおける方位角Φ（ｔ）を推定する（ステップＳ３）。計算された方位角Φ（ｔ）は、定数値設定部６に送られる。
【００５８】
定数値設定部６は、方位角計算部４が推定した時刻ステップｔにおける自律飛行船の方位角Φ（ｔ）と、記憶部３から読み込んだ目標位置目標方位角Φｄ（ｔ）及び目標方位角速度ＤΦｄ（ｔ）／Ｄｔとを用いて、λを適宜調整する（ステップＳ４）。設定されたλは、補正入力値計算部７及び制御入力値計算部８に送られる。
【００５９】
例えば、γを０＜γ＜１の数として、定数値設定部６は、ＤΦｄ（ｔ）／Ｄｔ＞０かつΦ（ｔ）−Φｄ（ｔ）＜０であればλ＝（１＋γ）λとし、ＤΦｄ（ｔ）／Ｄｔ＜０かつΦ（ｔ）−Φｄ（ｔ）＞０であればλ＝（１＋γ）λとし、ＤΦｄ（ｔ）／Ｄｔ＞０かつΦ（ｔ）−Φｄ（ｔ）＞０であればλ＝（１−γ）λとし、ＤΦｄ（ｔ）／Ｄｔ＜０かつΦ（ｔ）−Φｄ（ｔ）＜０であればλ＝（１−γ）λとする。λの値の設定は、（１＋γ）λまたは（１−γ）λに限られるものではなく、ＤΦｄ（ｔ）／Ｄｔ＞０かつΦ（ｔ）−Φｄ（ｔ）＜０の場合、または、ＤΦｄ（ｔ）／Ｄｔ＜０かつΦ（ｔ）−Φｄ（ｔ）＞０の場合には、λの値が元のλの値よりも大きくなるように設定し、ＤΦｄ（ｔ）／Ｄｔ＞０かつΦ（ｔ）−Φｄ（ｔ）＞０の場合、または、ＤΦｄ（ｔ）／Ｄｔ＜０かつΦ（ｔ）−Φｄ（ｔ）＜０の場合には、λの値が元のλの値よりも小さくなるように設定されるものであればなんでも良い。例えば、λ＝λ＋ｃ，λ＝λ−ｃ（ｃは正の定数）としても良い。なお、λの増大と減少の幅は等しくするほうが、精度良く推定を行うことができる。
【００６０】
補正入力値計算部７は、補正入力値ｋ（ｔ）ｓｇｎ（ｓ（ｔ））を計算する（ステップＳ５）。計算された補正入力値ｋ（ｔ）ｓｇｎ（ｓ（ｔ））は、制御入力値計算部８に送られる。
【００６１】
例えば、フィードバック定数ｋ（ｔ）は、ｋ（ｔ）を、式（６）の不等式を満たすような十分大きな定数値としても良いし、式（１０）や式（１１）のような関数の値としてもよい。また、ずれ値ｓ（ｔ）をｓ（ｔ）＝λ（Φ（ｔ）−Φｄ（ｔ））とし、関数ｓｇｎ（・）を・が正であれば１を出力し・が負であれば−１を出力する関数とする。
【００６２】
補正入力値計算部７は、図２に例示するように、フィードバック定数ｋ（ｔ）を計算するフィードバック定数計算部７１と、ずれ値ｓ（ｔ）を計算するずれ値計算部７２と、計算されたフィードバック定数ｋ（ｔ）及びずれ値ｓ（ｔ）を用いて補正入力値を計算する合成算部７３とを含む。
【００６３】
制御入力値計算部８は、設定された定数λ及び計算された補正入力値ｋ（ｔ）ｓｇｎ（ｓ（ｔ））を用いて、式（５）で定義される制御入力値ｕ（ｔ）を計算する（ステップＳ８）。計算された制御入力値ｕ（ｔ）は、制御部９に送られる。
【００６４】
制御部９は、制御入力値ｕ（ｔ）で自律飛行船に搭載された制御対象アクチュエータを制御する（ステップＳ７）。
【００６５】
［変形例等］
上記の例では、制御対象物の状態変数Ｘ（ｔ）として方位角Φ（ｔ）を用いたが、制御対象物の状態変数Ｘ（ｔ）として他の変数を選択してもよい。例えば、上記の説明において方位角Φ（ｔ）を制御対象物の機体座標での前進方向の変位量に置き換えれば、前進方向の軌道追従制御を実現することができる。
【００６６】
また、式（５）及び式（８）における関数ｓｇｎ（・）の代わりに、ｓ＝０近辺に不感帯を設けた以下の関数ｓｇｎ＿ｍｏｄ（・）を用いても良い。Ωは予め定められた定数であり、例えば０に近い値に設定される。
【００６７】
【数１２】
【００６８】
これにより、関数ｓｇｎ（・）の符号切り替えの影響で、制御入力値ｕ（ｔ）の値がチャタリングを起こしてしまう問題を防ぐことができる。
【００６９】
軌道追従制御装置の各部間のデータのやり取りは直接行われてもよいし、記憶部３を介して行われてもよい。
【００７０】
軌道追従制御装置は、コンピュータによって実現することができる。この場合、この装置が有すべき各機能の処理内容はプログラムによって記述される。そして、このプログラムをコンピュータで実行することにより、これ装置における各処理機能が、コンピュータ上で実現される。
【００７１】
この処理内容を記述したプログラムは、コンピュータで読み取り可能な記録媒体に記録しておくことができる。また、この形態では、コンピュータ上で所定のプログラムを実行させることにより、これらの装置を構成することとしたが、これらの処理内容の少なくとも一部をハードウェア的に実現することとしてもよい。
【００７２】
この発明は、上述の実施形態に限定されるものではなく、本発明の趣旨を逸脱しない範囲で適宜変更が可能である。
【符号の説明】
【００７３】
６ 定数値設定部
７ 補正入力値計算部
８ 制御入力値計算部
９ 制御部
【特許請求の範囲】
【請求項１】
ｆ及びｂを正の定数、ｕ（ｔ）を制御入力値として、次式で表わされるモデルにより運動すると想定される制御対象物の状態変数Ｘ（ｔ）を目標とする状態Ｘｄ（ｔ）に追従させる軌道追従制御装置において、
【数１３】
Ｘ（ｔ）を計測された時刻ステップｔにおける上記制御対象物の状態変数、Ｘｄ（ｔ）を時刻ステップｔにおける上記制御対象物の目標とする状態として、定数λを正の数とし、ＤＸｄ（ｔ）／Ｄｔ＞０かつＸ（ｔ）−Ｘｄ（ｔ）＜０であればλの値をより大きくし、ＤＸｄ（ｔ）／Ｄｔ＜０かつＸ（ｔ）−Ｘｄ（ｔ）＞０であればλの値をより大きくし、ＤＸｄ（ｔ）／Ｄｔ＞０かつＸ（ｔ）−Ｘｄ（ｔ）＞０であればλの値をより小さくし、ＤＸｄ（ｔ）／Ｄｔ＜０かつＸ（ｔ）−Ｘｄ（ｔ）＜０であればλの値をより小さくする定数値設定部と、
ｋ（ｔ）をフィードバック定数とし、ずれ値ｓ（ｔ）をｓ（ｔ）＝λ（Ｘ（ｔ）−Ｘｄ（ｔ））とし、関数ｓｇｎ（・）を・が正であれば１を出力し・が負であれば−１を出力する関数として、補正入力値ｋ（ｔ）ｓｇｎ（ｓ（ｔ））を計算する補正入力値計算部と、
ｂを正の定数として、上記設定された定数λ及び上記計算された補正入力値ｋ（ｔ）ｓｇｎ（ｓ（ｔ））を用いて、次式で定義される制御入力値ｕ（ｔ）を計算する制御入力値計算部と、
【数１４】
上記制御入力値ｕ（ｔ）で上記制御対象物に搭載された制御対象アクチュエータを制御する制御部と、
を含む軌道追従制御装置。
【請求項２】
請求項１の軌道追従制御装置において、
上記補正入力値計算部は、β及びηを正の定数、ｆの真値の取り得る値の中でｆから最も離れた値をｆｆａｒ、上記制御対象物の速度ＤＸ（ｔ）／ｄｔの想定される最大値をＸｖｍａｘとして、次式により定義されるフィードバック定数ｋ（ｔ）を計算するフィードバック定数計算部を含む、
【数１５】
ことを特徴とする軌道追従制御装置。
【請求項３】
請求項１の軌道追従制御装置において、
上記補正入力値計算部は、β及びηを正の定数として、次式により定義されるフィードバック定数ｋ（ｔ）を計算するフィードバック定数計算部を含む、
【数１６】
【請求項４】
請求項１から３の何れかに記載の軌道追従制御装置において、
上記制御対象物の状態変数Ｘ（ｔ）はその制御対象物の方位角Φ（ｔ）であり、上記制御対象物の目標とする状態Ｘｄ（ｔ）はその制御対象物の目標方位角Φｄ（ｔ）であり、
計測された上記制御対象物の時刻ステップｔにおけるＸＹ平面の位置及び時刻ステップｔ−１におけるＸＹ平面の位置から、上記制御対象物の時刻ステップｔにおける方位角Φ（ｔ）を計算する方位角計算部を更に含む、
ことを特徴とする軌道追従制御装置。
【請求項５】
ｆ及びｂを正の定数、ｕ（ｔ）を制御入力値として、次式で表わされるモデルにより運動すると想定される制御対象物の状態変数Ｘ（ｔ）を目標とする状態Ｘｄ（ｔ）に追従させる軌道追従制御方法において、
【数１７】
定数値設定部が、Ｘ（ｔ）を計測された時刻ステップｔにおける上記制御対象物の位置、Ｘｄ（ｔ）を時刻ステップｔにおける上記制御対象物の目標位置として、定数λを正の数とし、ＤＸｄ（ｔ）／Ｄｔ＞０かつＸ（ｔ）−Ｘｄ（ｔ）＜０であればλの値をより大きくし、ＤＸｄ（ｔ）／Ｄｔ＜０かつＸ（ｔ）−Ｘｄ（ｔ）＞０であればλの値をより大きくし、ＤＸｄ（ｔ）／Ｄｔ＞０かつＸ（ｔ）−Ｘｄ（ｔ）＞０であればλの値をより小さくし、ＤＸｄ（ｔ）／Ｄｔ＜０かつＸ（ｔ）−Ｘｄ（ｔ）＜０であればλの値をより小さくする定数値設定ステップと、
補正入力値計算部が、ｋ（ｔ）をフィードバック定数とし、ずれ値ｓ（ｔ）をｓ（ｔ）＝λ（Ｘ（ｔ）−Ｘｄ（ｔ））とし、関数ｓｇｎ（・）を・が正であれば１を出力し・が負であれば−１を出力する関数として、補正入力値ｋ（ｔ）ｓｇｎ（ｓ（ｔ））を計算する補正入力値計算ステップと、
制御入力値計算部が、ｂを正の定数として、上記設定された定数λ及び上記計算された補正入力値ｋ（ｔ）ｓｇｎ（ｓ（ｔ））を用いて、次式で定義される制御入力値ｕ（ｔ）を計算する制御入力値計算ステップと、
【数１８】
制御部が、上記制御入力値ｕ（ｔ）で上記制御対象物に搭載された制御対象アクチュエータを制御する制御ステップと、
を含む軌道追従制御方法。
【請求項６】
請求項１から４の何れかの軌道追従制御装置の各部としてコンピュータを機能させるための軌道追従制御プログラム。
【請求項１】
ｆ及びｂを正の定数、ｕ（ｔ）を制御入力値として、次式で表わされるモデルにより運動すると想定される制御対象物の状態変数Ｘ（ｔ）を目標とする状態Ｘｄ（ｔ）に追従させる軌道追従制御装置において、
【数１３】
Ｘ（ｔ）を計測された時刻ステップｔにおける上記制御対象物の状態変数、Ｘｄ（ｔ）を時刻ステップｔにおける上記制御対象物の目標とする状態として、定数λを正の数とし、ＤＸｄ（ｔ）／Ｄｔ＞０かつＸ（ｔ）−Ｘｄ（ｔ）＜０であればλの値をより大きくし、ＤＸｄ（ｔ）／Ｄｔ＜０かつＸ（ｔ）−Ｘｄ（ｔ）＞０であればλの値をより大きくし、ＤＸｄ（ｔ）／Ｄｔ＞０かつＸ（ｔ）−Ｘｄ（ｔ）＞０であればλの値をより小さくし、ＤＸｄ（ｔ）／Ｄｔ＜０かつＸ（ｔ）−Ｘｄ（ｔ）＜０であればλの値をより小さくする定数値設定部と、
ｋ（ｔ）をフィードバック定数とし、ずれ値ｓ（ｔ）をｓ（ｔ）＝λ（Ｘ（ｔ）−Ｘｄ（ｔ））とし、関数ｓｇｎ（・）を・が正であれば１を出力し・が負であれば−１を出力する関数として、補正入力値ｋ（ｔ）ｓｇｎ（ｓ（ｔ））を計算する補正入力値計算部と、
ｂを正の定数として、上記設定された定数λ及び上記計算された補正入力値ｋ（ｔ）ｓｇｎ（ｓ（ｔ））を用いて、次式で定義される制御入力値ｕ（ｔ）を計算する制御入力値計算部と、
【数１４】
上記制御入力値ｕ（ｔ）で上記制御対象物に搭載された制御対象アクチュエータを制御する制御部と、
を含む軌道追従制御装置。
【請求項２】
請求項１の軌道追従制御装置において、
上記補正入力値計算部は、β及びηを正の定数、ｆの真値の取り得る値の中でｆから最も離れた値をｆｆａｒ、上記制御対象物の速度ＤＸ（ｔ）／ｄｔの想定される最大値をＸｖｍａｘとして、次式により定義されるフィードバック定数ｋ（ｔ）を計算するフィードバック定数計算部を含む、
【数１５】
ことを特徴とする軌道追従制御装置。
【請求項３】
請求項１の軌道追従制御装置において、
上記補正入力値計算部は、β及びηを正の定数として、次式により定義されるフィードバック定数ｋ（ｔ）を計算するフィードバック定数計算部を含む、
【数１６】
【請求項４】
請求項１から３の何れかに記載の軌道追従制御装置において、
上記制御対象物の状態変数Ｘ（ｔ）はその制御対象物の方位角Φ（ｔ）であり、上記制御対象物の目標とする状態Ｘｄ（ｔ）はその制御対象物の目標方位角Φｄ（ｔ）であり、
計測された上記制御対象物の時刻ステップｔにおけるＸＹ平面の位置及び時刻ステップｔ−１におけるＸＹ平面の位置から、上記制御対象物の時刻ステップｔにおける方位角Φ（ｔ）を計算する方位角計算部を更に含む、
ことを特徴とする軌道追従制御装置。
【請求項５】
ｆ及びｂを正の定数、ｕ（ｔ）を制御入力値として、次式で表わされるモデルにより運動すると想定される制御対象物の状態変数Ｘ（ｔ）を目標とする状態Ｘｄ（ｔ）に追従させる軌道追従制御方法において、
【数１７】
定数値設定部が、Ｘ（ｔ）を計測された時刻ステップｔにおける上記制御対象物の位置、Ｘｄ（ｔ）を時刻ステップｔにおける上記制御対象物の目標位置として、定数λを正の数とし、ＤＸｄ（ｔ）／Ｄｔ＞０かつＸ（ｔ）−Ｘｄ（ｔ）＜０であればλの値をより大きくし、ＤＸｄ（ｔ）／Ｄｔ＜０かつＸ（ｔ）−Ｘｄ（ｔ）＞０であればλの値をより大きくし、ＤＸｄ（ｔ）／Ｄｔ＞０かつＸ（ｔ）−Ｘｄ（ｔ）＞０であればλの値をより小さくし、ＤＸｄ（ｔ）／Ｄｔ＜０かつＸ（ｔ）−Ｘｄ（ｔ）＜０であればλの値をより小さくする定数値設定ステップと、
補正入力値計算部が、ｋ（ｔ）をフィードバック定数とし、ずれ値ｓ（ｔ）をｓ（ｔ）＝λ（Ｘ（ｔ）−Ｘｄ（ｔ））とし、関数ｓｇｎ（・）を・が正であれば１を出力し・が負であれば−１を出力する関数として、補正入力値ｋ（ｔ）ｓｇｎ（ｓ（ｔ））を計算する補正入力値計算ステップと、
制御入力値計算部が、ｂを正の定数として、上記設定された定数λ及び上記計算された補正入力値ｋ（ｔ）ｓｇｎ（ｓ（ｔ））を用いて、次式で定義される制御入力値ｕ（ｔ）を計算する制御入力値計算ステップと、
【数１８】
制御部が、上記制御入力値ｕ（ｔ）で上記制御対象物に搭載された制御対象アクチュエータを制御する制御ステップと、
を含む軌道追従制御方法。
【請求項６】
請求項１から４の何れかの軌道追従制御装置の各部としてコンピュータを機能させるための軌道追従制御プログラム。
【図１】
【図２】
【図３】
【図４】
【図２】
【図３】
【図４】
【公開番号】特開２０１１−２０９９８８（Ｐ２０１１−２０９９８８Ａ）
【公開日】平成２３年１０月２０日（２０１１．１０．２０）
【国際特許分類】
【出願番号】特願２０１０−７６７９９（Ｐ２０１０−７６７９９）
【出願日】平成２２年３月３０日（２０１０．３．３０）
【出願人】（０００００４２２６）日本電信電話株式会社 (13,992)
【Ｆターム（参考）】
【公開日】平成２３年１０月２０日（２０１１．１０．２０）
【国際特許分類】
【出願日】平成２２年３月３０日（２０１０．３．３０）
【出願人】（０００００４２２６）日本電信電話株式会社 (13,992)
【Ｆターム（参考）】
[ Back to top ]