(57)【特許請求の範囲】
【請求項１】
浮遊移動体の制御システムであって、
前記浮遊移動体は、
前記浮遊移動体の一部を占める、単一剛体とみなし得る本体部と、
前記浮遊移動体の位置及び／又は速度の制御を行うための推進力を発生させる効果器部と、
前記本体部と前記効果器部を力学的に結合する部分であり、前記効果器部から前記本体部に作用する推力を実測できるよう構成された推力伝達ゲートと、
からなっており、さらに、
前記本体部は、前記浮遊移動体の加速度を測定できる加速度測定手段を備えており、
前記加速度測定手段より出力された加速度測定値と、
前記推力伝達ゲートからの推力測定値と、
を基に前記効果器部に対する推力指令が算出され、その推力指令に従うように前記効果器部の推力が制御されることを特徴とする浮遊移動体の制御システム。
【請求項２】
浮遊移動体の制御システムであって、
前記浮遊移動体は、
前記浮遊移動体の一部を占める、単一剛体とみなし得る本体部と、
前記浮遊移動体の位置及び／又は速度の制御を行うための推進力を発生させる、前記本体部を包囲する殻状体をなす効果器部と、
前記本体部と前記効果器部を力学的に結合する部分であり、前記効果器部から前記本体部に作用する推力を実測できるよう構成された推力伝達ゲートと、
からなっており、
前記本体部は、前記推力伝達ゲートのみに力学的に結合され、かつ、前記本体部に作用する推力は実質上全て、前記推力伝達ゲートを介して入力される様構成されており、
前記推力伝達ゲートからの推力測定値を基に前記効果器部に対する推力指令が算出され、その推力指令に従うように前記効果器部の推力が制御されることを特徴とする浮遊移動体の制御システム。
【請求項３】
前記推力伝達ゲートは、前記本体部と前記効果器部の間に掛かる力又はトルクを測定できる力又はトルク測定手段を備えており、
外部より入力された目標加速度軌道指令、及び前記加速度測定手段より出力された加速度測定値から演算される加速度誤差を変換したものと、
前記推力伝達ゲートの前記力又はトルク測定手段からの出力との和として、
前記効果器部に対する推力指令が算出され、
その推力指令に従うように、
前記推力伝達ゲートの前記力又はトルク測定手段からの出力に基づき前記効果器部の推力が制御されることを特徴とする請求項１に記載の浮遊移動体の制御システム。
【請求項４】
前記推力伝達ゲートは、前記本体部と前記効果器部の間に掛かる力又はトルクを測定できる力又はトルク測定手段を備えており、
外部より入力された目標加速度軌道指令を変換したものとして前記効果器部に対する推力指令が算出され、
その推力指令に従うように、
前記推力伝達ゲートの前記力又はトルク測定手段からの出力に基づき前記効果器部の推力が制御されることを特徴とする請求項２に記載の浮遊移動体の制御システム。
【請求項５】
浮遊移動体の制御システムであって、
前記浮遊移動体は、
前記浮遊移動体の一部を占める本体と、
前記浮遊移動体の位置及び／又は速度の制御を行うための推進力を発生させる効果器と、
前記浮遊移動体の加速度を測定できる加速度測定手段とからなっている一方、
前記浮遊移動体は、計算上、
前記浮遊移動体の一部を占める、単一剛体とみなし得る仮想本体部と、
前記浮遊移動体の位置及び／又は速度の制御を行うための推進力を発生させる仮想効果器部と、
前記仮想本体部と前記仮想効果器部を仮想的に結合する部分であり、前記加速度測定手段からの出力を用いて前記仮想効果器部から前記仮想本体部に作用する推力を推定できるよう構成された仮想推力伝達ゲートと、
に仮想的に分離されており、
前記加速度測定手段より出力された加速度測定値を基に計算上推定される前記仮想推力伝達ゲートからの推力推定値を用いて前記仮想効果器部に対する推力指令が算出され、その推力指令と力学的に等価な前記効果器の推力に変換することを通じて前記効果器の推力が制御されることを特徴とする浮遊移動体の制御システム。
【請求項６】
前記仮想推力伝達ゲートは、前記加速度測定手段より出力された加速度測定値を基に計算上、前記仮想本体部と前記仮想効果器部の間に掛かる力又はトルクを推定できる力又はトルク推定手段を備えており、
外部より入力された目標加速度軌道指令を変換したものとして前記仮想効果器部に対する推力指令が算出され、
その推力指令に従うように、
前記仮想推力伝達ゲートの前記力又はトルク推定手段からの出力に基づき前記効果器の推力が制御されることを特徴とする請求項５に記載の浮遊移動体の制御システム。
【請求項７】
前記本体部又は前記仮想本体部が、前記浮遊移動体の傾きを測定できる傾斜角測定手段を併せ備えており、
前記効果器部又は前記仮想効果器部に対する推力指令が、さらに前記傾斜角測定手段からの出力をも利用して算出され、その推力指令に従うように前記効果器部又は前記効果器の推力が制御されることを特徴とする請求項１〜６のいずれか１項に記載の浮遊移動体の制御システム。
【請求項８】
前記浮遊移動体が、前記浮遊移動体の位置又は速度を測定できる位置速度測定手段を併せ備えており、
前記効果器部又は前記仮想効果器部に対する推力指令が、さらに前記位置速度測定手段からの出力、及び外部より入力された目標位置指令又は目標速度指令をも利用して算出され、その推力指令に従うように前記効果器部又は前記効果器の推力が制御されることを特徴とする請求項１〜７のいずれか１項に記載の浮遊移動体の制御システム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、水中、空中、宇宙空間などにおける浮遊移動体、或いは平面上を滑走する移動体の制御システムに関する。
【背景技術】
【０００２】
はじめに、従来知られた浮遊移動体の制御手法につき、図１１〜１４等を参照しながら説明する。図１１は従来知られた浮遊移動体の概略図、図１１〜１４は従来知られた浮遊移動体の制御手法を示す概略図である。
【０００３】
図１１に示す通り、水中ロボットをはじめとする浮遊移動体１０本体の位置および速度を制御するために、その本体にはセンサ１３が設けられ、これによって浮遊移動体１０本体の位置および速度が検出され、計算機Ｐ内の減算回路１１に与えられる。減算回路１１にはまた、目標となる位置および速度を表す信号がオペレータＨより与えられる。減算回路１１の出力は、浮遊移動体１０に設けられた推進力発生手段（効果器）たるスラスタＴを制御するためのスラスタ制御回路１２に与えられ、こうして浮遊移動体１０の位置および速度が制御される（いわゆる位置・速度フィードバック制御）。
【０００４】
図１２は、この図１１に示される浮遊移動体１０の制御を行なうためのブロック図である。スラスタ制御回路１２の出力たる効果器推力指令信号は、所定の推力特性１４を持つスラスタＴに与えられ、それにより効果器推力が浮遊移動体１０に与えられ、参照符１５で示されるように、浮遊移動体１０のダイナミクスすなわち動力学の入出力特性が得られ、その本体の位置および速度が、センサ１３によって前述のように検出されて、減算回路１１に与えられる。
【０００５】
このように、従来の浮遊移動体の制御においては、制御対象となる浮遊移動体の位置および速度の情報をフィードバックすることにより効果器への推力指令を直接操作しているところ、一般的に効果器の応答速度が比較的遅い浮遊移動体においては、効果器に指令を発してから、その効果が浮遊移動体の位置および速度に反映されセンサで取得されるまでに時間がかかり、制御性能に悪影響を及ぼすと言う問題があった（いわゆるセンシング遅延の問題）。この傾向は、浮遊移動体の質量が大きいほど、また効果器の推力が小さいほど顕著である。
さらに、水中、空中など流体中の浮遊移動体は、波浪や潮流、風等、流体力学的な外乱の影響を常に受けている他、図１１にも示される様に水中における浮遊移動体１０に対しては水上からのアンビリカルケーブルＣ（給電線や通信線ほか）等が接続されており、このケーブルが浮遊移動体１０を引張ったりする場合が往々にしてあるところ、上記の制御手法では未知の外乱に対する応答に時間が掛かり、やはり制御性能に悪影響を及ぼすと言う問題があった。
【０００６】
次に、図１１に示されるように、浮遊移動体１０に複数のリンクを備えるロボットアームＡが更に設けられているときには、図１３に例示される様なブロック図が構成されることになる。参照符１６で示されるようにロボットアームＡによる反力およびトルクは、加算回路１７として例示される様に、スラスタＴによる効果器推力に加算される。したがって、ロボットアームＡからの反力とトルクによって、浮遊移動体１０の動き即ち位置および速度が乱されることになり、このロボットアームＡからの反力とトルクによって発生する浮遊移動体１０の位置および速度の誤差を修正する様にスラスタＴが制御される。このように、ロボットアームＡからの反力とトルクにより浮遊移動体１０の動き（位置や速度）が乱されるところ、従来の姿勢制御では、その結果発生する位置や速度の誤差を修正するようにスラスタＴを作動させていた。
【０００７】
つまり、従来知られた制御手法では、浮遊移動体１０がロボットアームＡの反力とトルクによって動いた後にスラスタＴを作動させることになり、外乱に対する応答速度が低いといういわゆるセンシング遅延の問題がやはり解消されない儘であった。
【０００８】
また、図１４には、別の先行技術のブロック図が示されている。この制御手法は、ロボットアームＡの運動と浮遊移動体１０本体の運動の相互干渉を考慮して、それぞれの動きが目的に合致するように、一度に効果器推力とロボットアーム関節トルクを決定する手法である。この先行技術では、ロボットアームＡが備えられた浮遊移動体１０を多体リンク系として、その運動方程式から各ロボットアームの各関節トルクが浮遊移動体１０本体の運動に及ぼす影響を数式的に表現し、その数式を解くことによって、浮遊移動体１０本体の推進力を決定している（いわゆるモデルベースト制御）。
【０００９】
この図１４の先行技術では、スラスタ・ロボットアーム制御回路１８からの効果器推力指令信号はスラスタＴに与えられ、またロボットアームＡのトルク指令信号は関節アクチュエータ１９に与えられる。こうして、スラスタＴから出力される効果器推力並びに関節アクチュエータ１９による関節トルクによって浮遊移動体１０本体とロボットアームＡの複合系ダイナミクス２０が形成され、センサ１３は浮遊移動体１０本体の位置および速度を検出して減算器１１に与え、この減算器１１には浮遊移動体１０本体の目標となる位置および姿勢を表す信号が与えられる。又複合系ダイナミクス２０からのロボットアーム制御変数即ち、位置、姿勢、関節角、速度、手先反力等を表す信号は減算回路２１に与えられ、この減算回路２１にはロボットアームＡの制御変数の目標値が与えられる。これら減算器１１，２１の出力は、スラスタ・ロボットアーム制御回路１８に与えられる。
すなわち、この制御手法では、ロボットアームＡの運動と、浮遊移動体１０本体の運動の相互干渉を考慮して、それぞれの動きが目的に合致するように、一度に効果器推力とロボットア−ム関節トルクを決定している。これによって正確に浮遊移動体１０本体の動きを制御することができるという利点がある一方、次の（１）〜（３）の問題があった。
【００１０】
（１）  対象のダイナミクスが一般的に複雑であり、ロボットアームＡが６軸であれば、浮遊移動体１０本体と併せて７個の剛体が繋がったリンクのダイナミクスになる。ロボットアームＡが複数になると、さらに飛躍的に複雑なダイナミクスになる。この様な制御系の演算には多大な手間が掛かり、計算機に重い計算負荷を及ぼすこととなる。その為、上記の制御システムを小型の計算機を用いて構成することは現実的に非常に困難であった。
【００１１】
（２）  又上記の制御システムではロボットアームＡの各リンクおよび本体の質量、慣性モーメント、重心位置などの多くの力学パラメータが必要となり、これらの力学パラメータは、すべて正しい値を用いなければ、正しい制御を実現することはできない。したがってロボットアームＡで物をつかんだりした場合、つかんだ物の質量や慣性モーメント、重心位置などを正しく与えなければならない。従ってつかむ可能性のある物のデータベースを持つか、対象の力学特性を計測する必要があり、現実的には、用途が限られてしまうという問題があった。
（３）  更に、水中、空中など流体中の浮遊移動体は、波浪や潮流、風など、流体力学的な未知の外乱の影響を常に受けている上、図１１にも示される様に水中における浮遊移動体１０に対しては水上からのアンビリカルケーブルＣ（給電線や通信線ほか）等が接続されており、このケーブルが浮遊移動体１０を引張ったりする場合が往々にしてあるところ、かかる未知の外乱をモデル化することは不可能である上、この外乱の影響を予測することは一般に困難であり、この様なモデルベースト制御手法は、浮遊移動体の制御に基本的に馴染まないと言う問題があった。
【００１２】
その他，従来知られた浮遊移動体の制御手法としては、特許文献１に挙げられる様なマニピュレータの根元の力フィードバック制御、或いは非特許文献１に挙げられる様な推力ローカルフィードバック制御等が存在する。
【００１３】
特許文献１に係るマニピュレータの根元の力フィードバック制御によれば、浮遊移動体本体がマニピュレータによる外乱によって動いてしまうよりも先にスラスタを作動させて浮遊移動体本体への影響を打ち消し、浮遊移動体本体がマニピュレータによる外乱で動くことを防止することが可能となる。
【００１４】
また、浮遊移動体の位置・速度の制御のための効果器として用いられるスラスタは、流体を利用して推力を発生するため、スラスタヘの入力指令と実際に出力される推力との間には流体に起因する強い非線形性が存在するところ、このような非線形性を構成する要素は多岐にわたり、それらの全てを考慮した制御を行なうことは従来困難であった（いわゆる効果器動特性の非線形性の問題）が、非特許文献１に係る推力ローカルフィードバック制御によれば、上記効果器動特性の非線形性に起因する浮遊移動体の制御性能の悪化を防止することが可能であった。
【００１５】
しかしながら、上記特許文献１及び非特許文献１に係る制御手法は共に、一部の力を計測してフィードバックするものではあったが、浮遊移動体に対する最も大きな外乱である波浪や潮流、風等の影響を打ち消すものではなかった。
【００１６】
要するに、上記いずれの先行技術においても、ロボットアームの反動等による外乱のほか、水中、空中等の流体中にある浮遊移動体が常に受ける波浪や潮流、風等の流体力学的な未知の外乱下にあっても自動的に外乱の影響を補償して、浮遊移動体を精度良く静止させたり、或いは目標軌道に高精度に追従させたりすることが可能な浮遊移動体の制御システムは実現されていなかったのである。
【００１７】
他方、上記課題とは別に、浮遊移動体を航行させる従来知られた手法として慣性航法なるものが多用されているところ、この慣性航法には次の様な問題があった。すなわち、慣性航法は、浮遊移動体から求められる加速度を利用しているところ、浮遊移動体自体が大きく重いものである場合には、加速度を一旦変位に変換してから加速度の値を電気信号として出力する従来知られた加速度センサの構造上、微小な加速度の変化を精度良く検出出来ず、センサから得られる加速度値と実際の加速度値との間に顕著な誤差が生じるという問題があった。この様な加速度値の誤差は、慣性航法自体の精度にも著しく影響し、浮遊移動体を所望の軌道上に正確に誘導出来ないという不都合が生じていた。
【特許文献１】
特開平５−１１９８３７号公報
【非特許文献１】
金岡、中山、林、川村著、“水中における高精度運動実現のためのスラスタ推カローカルフィードバック制御”、日本機械学会ロボティクス・メカトロニクス講演会  ’０３講演論文集、２Ｐ１−２Ｆ−Ａ６、２００３年
【発明の開示】
【００１８】
［発明が解決しようとする課題］
従って本発明は、例えば波浪や潮流、風その他浮遊移動体に取り付けられたロボットアームの反動等による外乱下にあっても、浮遊移動体を所定位置に精度良く静止させたり、或いは目標軌道に精度良く追従させたりすることが可能な、浮遊移動体の制御システムを提供することを課題とする。
【００１９】
又本発明は、比較的簡単な構成で、浮遊移動体の位置及び姿勢等を正確に制御することができるようにした浮遊移動体の制御システムを提供することを課題とする。
【００２０】
さらに本発明は、浮遊移動体の大きさや重さ、或いは加速度センサの性能に依存せず、微小な加速度の変化に至るまで浮遊移動体の加速度を精度良く検出することが可能な、浮遊移動体の加速度検知システムを提供することを課題とする。
【００２１】
［課題を解決するための手段］
本発明者は、浮遊移動体を、浮遊移動体の一部を占める、単一剛体とみなし得る本体部と、浮遊移動体に対して推力を発生する効果器部に分離した上、この本体部と効果器部を力学的に結合する唯一の部分（推力伝達ゲート）に力・トルクセンサを設置しておけば、この力・トルクセンサによってセンシングされる値が効果器部から本体部に与えられている推力である故に本体部と効果器部の間に掛かる力とトルクを全て計測出来、この値をフィードバックすれば、浮遊移動体に与える推力を直接制御出来、それによって、波浪や潮流等の流体力学的な外乱下にあっても、浮遊移動体を所定位置に精度良く静止させたり、或いは目標軌道に精度良く追従させたりすることが可能であることを見い出し、本発明を完成した。
【００２２】
上記課題を解決可能な本発明の浮遊移動体の制御システムは、（１）浮遊移動体の制御システムであって、前記浮遊移動体は、
前記浮遊移動体の一部を占める、単一剛体とみなし得る本体部と、
前記浮遊移動体の位置及び／又は速度の制御を行うための推進力を発生させる効果器部と、
前記本体部と前記効果器部を力学的に結合する部分であり、前記効果器部から前記本体部に作用する推力を実測できるよう構成された推力伝達ゲートと、
からなっており、さらに、
前記本体部は、前記浮遊移動体の加速度を測定できる加速度測定手段を備えており、
前記加速度測定手段より出力された加速度測定値と、
前記推力伝達ゲートからの推力測定値と、
を基に前記効果器部に対する推力指令が算出され、その推力指令に従うように前記効果器部の推力が制御されることを特徴とするものである。
【００２３】
又本発明は、上記の浮遊移動体の制御システムにおいて更に、（３）前記推力伝達ゲートが、前記本体部と前記効果器部の間に掛かる力又はトルクを測定できる力又はトルク測定手段を備えており、
外部より入力された目標加速度軌道指令、及び前記加速度測定手段より出力された加速度測定値から演算される加速度誤差を変換したものと、
前記推力伝達ゲートの前記力又はトルク測定手段からの出力との和として、
前記効果器部に対する推力指令が算出され、
その推力指令に従うように、
前記推力伝達ゲートの前記力又はトルク測定手段からの出力に基づき前記効果器部の推力が制御されることを特徴とするものである。
【００２４】
なお、後記各実施形態及び実施例で説明する、３次元空間を浮遊する浮遊移動体のほか、斜面上を滑走する浮遊移動体に対して本発明の制御システム及び加速度検知システムを適用する場合には、重力加速度の影響を補償する必要があることから、後記（７）に係る発明として規定される通り、本体部の傾きを計測できる傾斜角測定手段を備えることが必要とされる。
しかしながら、そもそも重力の働かない宇宙空間を浮遊する浮遊移動体や、傾きを考慮しなくて良い、或いは無視しても構わない水平面上を滑走する浮遊移動体に対して本発明の制御システム或いは加速度検知システムを適用する際には、上記（１）若しくは（３）或いは次に説明する（２）若しくは（４）に係る発明として規定される通り、傾斜角測定手段は不要である。
このことは、仮想推力伝達ゲートシステムの考え方が適用される下記（５）若しくは（６）に係る発明の場合でも同様であり、３次元空間を浮遊する浮遊移動体等に対して仮想推力伝達ゲートシステムの考え方を用いた本発明の制御システムを適用する際には、後記（７）に係る発明として規定される通り、浮遊移動体の傾きを計測できる傾斜角測定手段を備えることが必要とされる。
【００２５】
同様に、上記課題を解決可能な本発明の浮遊移動体の制御システムは、（２）浮遊移動体の制御システムであって、前記浮遊移動体は、
前記浮遊移動体の一部を占める、単一剛体とみなし得る本体部と、
前記浮遊移動体の位置及び／又は速度の制御を行うための推進力を発生させる、前記本体部を包囲する殻状体をなす効果器部と、
前記本体部と前記効果器部を力学的に結合する部分であり、前記効果器部から前記本体部に作用する推力を実測できるよう構成された推力伝達ゲートと、
からなっており、
前記本体部は、前記推力伝達ゲートのみに力学的に結合され、かつ、前記本体部に作用する推力は実質上全て、前記推力伝達ゲートを介して入力される様構成されており、
前記推力伝達ゲートからの推力測定値を基に前記効果器部に対する推力指令が算出され、その推力指令に従うように前記効果器部の推力が制御されることを特徴とするものである。
なお、本体部が推力伝達ゲートのみに力学的に結合され、かつ、本体部に作用する力が実質上全て、効果器部及び推力伝達ゲートを介して入力される様構成された典型的な例としては、上記効果器部が、本体部を包囲する殻状体をなしていることが挙げられる。
しかしながら、その様な構成は極端な例であって、必ずしも本体部が殻状体をなす効果器部で密閉されていることは要件ではない。要するに、この（２）で規定される発明では、本体部に直接、水圧や強い風圧等の外乱その他の外力が加わらない様構成されていれば良く、例えば水上や地上を滑走する浮遊移動体の内部に収容された本体部に空気が触れる程度のことは十分無視し得るレベルのことであり、この（２）に係る発明の適用上、特に問題とされない。
したがって、この（２）で規定される発明は、図３に表された構成に限定されないことが理解される。
【００２６】
又本発明は、上記（２）の浮遊移動体の制御システムにおいて更に、（４）前記推力伝達ゲートが、前記本体部と前記効果器部の間に掛かる力又はトルクを測定できる力又はトルク測定手段を備えており、
外部より入力された目標加速度軌道指令を変換したものとして前記効果器部に対する推力指令が算出され、
その推力指令に従うように、
前記推力伝達ゲートの前記力又はトルク測定手段からの出力に基づき前記効果器部の推力が制御されることを特徴とするものである。
【００２７】
更に本発明は、（５）浮遊移動体の制御システムであって、前記浮遊移動体は、
前記浮遊移動体の一部を占める本体と、
前記浮遊移動体の位置及び／又は速度の制御を行うための推進力を発生させる効果器と、
前記浮遊移動体の加速度を測定できる加速度測定手段とからなっている一方、
前記浮遊移動体は、計算上、
前記浮遊移動体の一部を占める、単一剛体とみなし得る仮想本体部と、
前記浮遊移動体の位置及び／又は速度の制御を行うための推進力を発生させる仮想効果器部と、
前記仮想本体部と前記仮想効果器部を仮想的に結合する部分であり、前記加速度測定手段からの出力を用いて前記仮想効果器部から前記仮想本体部に作用する推力を推定できるよう構成された仮想推力伝達ゲートと、
に仮想的に分離されており、
前記加速度測定手段より出力された加速度測定値を基に計算上推定される前記仮想推力伝達ゲートからの推力推定値を用いて前記仮想効果器部に対する推力指令が算出され、その推力指令と力学的に等価な前記効果器の推力に変換することを通じて前記効果器の推力が制御されることを特徴とするものである。
又本発明は、上記（５）の浮遊移動体の制御システムにおいて更に、（６）前記仮想推力伝達ゲートは、前記加速度測定手段より出力された加速度測定値を基に計算上、前記仮想本体部と前記仮想効果器部の間に掛かる力又はトルクを推定できる力又はトルク推定手段を備えており、
外部より入力された目標加速度軌道指令を変換したものとして前記仮想効果器部に対する推力指令が算出され、
その推力指令に従うように、
前記仮想推力伝達ゲートの前記力又はトルク推定手段からの出力に基づき前記効果器の推力が制御されることを特徴とするものである。
又本発明は、上記（１）〜（６）の浮遊移動体の制御システムにおいて更に、上述したとおり重力による影響を排除し得る様構成された制御システムであって、（７）前記本体部又は前記仮想本体部が、前記浮遊移動体の傾きを測定できる傾斜角測定手段を併せ備えており、
前記効果器部又は前記仮想効果器部に対する推力指令が、さらに前記傾斜角測定手段からの出力をも利用して算出され、その推力指令に従うように前記効果器部又は前記効果器の推力が制御されることを特徴とするものである。
その他本発明は、上記（１）〜（７）に係る推力伝達ゲートシステムと、従来知られた位置速度制御を併せ備えて更なる高性能化を図った浮遊移動体の制御システムであって、（８）前記浮遊移動体が、前記浮遊移動体の位置又は速度を測定できる位置速度測定手段を併せ備えており、
前記効果器部又は前記仮想効果器部に対する推力指令が、さらに前記位置速度測定手段からの出力、及び外部より入力された目標位置指令又は目標速度指令をも利用して算出され、その推力指令に従うように前記効果器部又は前記効果器の推力が制御されることを特徴とするものである。
【００２８】
なお、後記の通り、上記本発明の浮遊移動体の制御システムでは、浮遊移動体の制御は、目標軌道を実現するために本体部が推力伝達ゲートから受けるべき推力を計算して指令する「推力計画部」と、推力伝達ゲートにおける推力の現在値を効果器部にフィードバックして推力伝達ゲートにおいて指令推力を実現するように効果器部を直接駆動する「推力制御部」の二段階構成で行なわれる。
【００２９】
本発明では、上記の機構設計と制御手法を「推力伝達ゲートシステム」と総称する。推力伝達ゲートシステムの実施態様としては主に、
ｉ）本体部を外部に露出させた状態即ち、本体部に直接、水圧や強い風圧等の外乱その他の外力が及ぼされ得る構成とし、さらに推力伝達ゲートを備えた態様［後記第１実施形態、第１例参照］、
ｉｉ）推力伝達ゲートを備えると共に、さらに、本体部に直接、水圧や強い風圧等の外乱その他の外力が加わらない様、本体部が推力伝達ゲートのみに力学的に結合され、かつ、本体部に作用する力が実質上全て、効果器部及び推力伝達ゲートを介して入力される様構成（例えば、本体部を効果器部で覆って密閉した状態等）する態様［後記第１実施形態、第２例参照］、
ｉｉｉ）浮遊移動体のハードウェアは変更しないが、計算上、浮遊移動体を本体部と効果器部に分離した上、計算機内に予め本体部の慣性行列を準備しておき、これと浮遊移動体に設けた加速度センサの出力と、さらに、必要に応じて浮遊移動体に設けた傾斜角センサからの出力と、を用いて効果器部に対する推力指令を得る様にした態様［仮想推力伝達ゲート，後記第１実施形態、第３例参照］、の３種類がある。
【００３０】
ｉ）とｉｉ）では、浮遊移動体のハードウェア設計を推力伝達ゲートシステムのために変更する必要があり、推力伝達ゲートにおいて力・トルクセンサを必要とするが、実現される制御性能は高い。
一方、ｉｉｉ）では、ｉ）とｉｉ）の方法と比較すると制御性能は低くなるが、浮遊移動体のハードウェア設計を変える必要がなく、加速度センサ、さらに、必要に応じて傾斜角センサ、を既存の浮遊移動体に付加するだけで推力伝達ゲートシステムを利用でき、制御性能を向上させることが可能である。
特に、航空機等に現在装備されている公知の慣性航法装置は、本発明で使用される加速度センサ、傾斜角センサ、本体部位置・速度センサとしても使用出来、これを利用すれば、ハードウェアに改変を加えなくても、既存の制御よりもより高速高精度化を実現することが可能となる。
【００３１】
その他本発明は、浮遊移動体の加速度を検知するためのシステムであって、前記浮遊移動体が、前記浮遊移動体の一部を占める、単一剛体とみなし得る本体部と、前記浮遊移動体に対して推力を発生する効果器部と、前記本体部と前記効果器部を力学的に結合する部分であり、前記効果器部から前記本体部に作用する推力を実測できるよう構成された推力伝達ゲートと、からなっており、前記推力伝達ゲートは、前記本体部と前記効果器部の間に掛かる力又はトルクを計測できる力又はトルク測定手段を備えており、さらに、前記本体部は、前記推力伝達ゲートのみに力学的に結合され、かつ、前記本体部に作用する力は実質上全て、前記効果器部及び前記推力伝達ゲートを介して入力される様構成されており、前記推力伝達ゲートからの前記力又はトルク測定手段からの出力を用いて前記本体部の加速度の推定値を得る様にしたことを特徴とするものである。
又本発明は、上記の浮遊移動体の加速度検知システムにおいて更に、前記本体部が、前記本体部の傾きを計測できる傾斜角測定手段を備えており、前記本体部の加速度の推定値を、さらに前記本体部の前記傾斜角測定手段からの出力をも利用して得る様にしたことを特徴とするものである。
【００３２】
本発明によれば、例えば波浪や潮流、風或いは浮遊移動体に取り付けられたロボットアームの反動等による外乱下にあっても、浮遊移動体を所定位置に精度良く静止させたり、或いは目標軌道に精度良く追従させたりすることが出来、浮遊移動体を高速かつ高精度に制御し得るシステム構成を提供することが可能となる。
【００３３】
尚以下では、本発明の説明に用いる用語につき定義をおくものとする。
【００３４】
「浮遊移動体」とは、水中、空中又は宇宙空間その他を浮遊する各種移動体を言う。また、平面上、或いはレール上を滑走する移動体も浮遊移動体の概念に包含されるものとする。
以下、環境別に具体例を挙げるとすれば、
（１）宇宙：  宇宙ロボット、宇宙船、人工衛星等
（２）空中：  空中ロボット、航空機、ヘリコプター、飛行船等
（３）水中：  水中ロボット、潜水艇等
（４）水上：  船舶、ホバークラフト等
（５）地上：  自動車、鉄道、ホバークラフト、或いは橇、スノーモービルその他の氷上又は雪上を滑走する移動体等
が、本発明に言う「浮遊移動体」の概念に包含される。
自動車等の地上車両については、例えば車輪が滑っている状態では「浮遊移動体」となり得ることが理解される。従って本発明を適用することにより、自動車の場合は車輪が滑っていても滑っていなくても同様に車両制御を行うことが可能となる。
ただし、車輪が滑っている状態では、既存のエンジンとタイヤからなる推力伝達手段が機能し得ない。このことを考慮すると、当該状況下で本発明を適用するに当たっては、上記地上車両が何らかの別異の推力伝達手段（各種形式のスラスタ等）を備えているのが好ましいことが、以下の各説明より理解される。
【００３５】
「本体部」とは、浮遊移動体の一部を占める、単一剛体とみなし得る物理構造を言う。原則として本体部には機械的な可動部分はないものとする。又機械的な可動部分があっても、その影響が無視できる程度に小さければ良いほか、無視できなくても、その影響を予測できればこれをキャンセルすることは可能である。
なお、仮想推力伝達ゲートシステムの考え方を適用する本発明中の後記第１実施形態、第３例に相当する発明に於いては、本体部と効果器部の峻別は単に便宜上行っているものに過ぎず、どの部分を本体部と規定するかについては、ユーザーの判断に任せても構わない。従って、実際に後記第１実施形態、第３例に相当する発明を実施するに当たっては、本体部を、浮遊移動体の質量の主な部分を占めるものと規定しても良いほか、反対に、浮遊移動体中の一部に追加設置した物体（例えば、数ｋｇ程度の錘等）を本体部と規定することも可能である。
また、仮想推力伝達ゲートシステムの考え方を適用しない後記第１実施形態の第１例及び第２例に相当する発明に於いても、本体部と効果器部とを峻別する判断基準は、両者が次に説明する推力伝達ゲートで結合されているか否かだけであり、それさえ満足されていれば、浮遊移動体中に於ける本体部の占める割合について特に限定はない。
従って本発明に於いては、本体部は、浮遊移動体の質量の主な部分を占める単一剛体で構成された物理構造に限定されない。
【００３６】
「効果器部」とは、浮遊移動体に対して推力を発生する物理構造を言う。浮遊移動体の位置や速度等の制御のための推進力発生手段（効果器）のすべてを包含しているものとする。
【００３７】
「力学的に結合」とは、対象とする物体間に何らかの力の相互作用が働いている状態を言う。機械的に結合している状態が殆どであるが、機械的に結合していなくても、例えば電磁力等によって非接触な力の相互作用を及ぼしている状態も包含するものとする。
一方、「力学的に分離」とは、対象とする物体間に力の相互作用が働かない状態を言う。
【００３８】
「推力伝達ゲート」とは、本体部と効果器部を力学的に結合する唯一の部分であり、本体部と効果器部の間にかかる力とトルクをすべて計測できるセンサを備えることにより、効果器部から本体部に作用するすべての推力を実測できるよう構成されたものを言う。
【００３９】
「仮想推力伝達ゲート」とは、浮遊移動体内に物理的に存在せず仮想的に設けられた推力伝達ゲートであり、計算上便宜的に本体部と効果器部に分離された既存の浮遊移動体において、本体部加速度測定値に基づき、本体部と効果器部の間にかかる力とトルク即ち、効果器部から本体部に作用するすべての推力を推定できるよう構成されたものを言う。
なお、本発明に於て単に「推力伝達ゲート」というときには、特に断りがなければ「仮想推力伝達ゲート」も包含するものとする。
【００４０】
「仮想推力伝達ゲートにおける推力推定値」とは、物理的に存在する推力伝達ゲートにおいて実測された値ではなく、仮想推力伝達ゲートにおいて加速度センサの測定値（本体部加速度測定値）から計算によって求められる仮想的な値を言う。
【００４１】
「推力計画部」とは、目標軌道を実現するために本体部が推力伝達ゲートにおいて効果器部から受けるべき目標推力を計算し、推力制御部に指令する計算機上の一部分或いは機能を言う。
【００４２】
「推力制御部」とは、推力伝達ゲートにおける推力測定値もしくは推力推定値をフィードバックして、推力伝達ゲートにおいて所望の目標推力を実現する様、効果器への推力指令を出力する計算機上の一部分或いは機能を言う。
【００４３】
「目標推力」とは、目標軌道を実現するために本体部が推力伝達ゲートにおいて効果器部から受けるべき推力を言う。
【００４４】
「目標加速度」とは、所望の目標軌道或いは目標速度からの微分演算によって求められる加速度を言う。
【００４５】
「推力伝達ゲートシステム（TTGS: Thrust Transfer Gate System）」とは、推力伝達ゲートを備えた浮遊移動体が、推力計画部及び推力制御部を含む制御系によって制御される制御システムを言う。
【００４６】
「効果器推力特性」とは、スラスタ等の効果器ヘ入力される効果器推力指令と、実際に出力される効果器推力との関係をいう。尚、スラスタ等の効果器は一般に流体を介して推力を発生するため、効果器推力特性には、効果器そのものの機械的なダイナミクスの他に、流体力学における抗力・揚力の影響などの複雑なダイナミクスが包含されているものとする。
【００４７】
［発明の効果］
従来は、浮遊移動体の位置・速度のセンサ情報をフィードバックしてスラスタの推力を制御していたが、本発明では発生した推力を直接フィードバックして効果器の推力を制御するため、原理的に高速な応答が実現される。従って、上記センシング遅延の問題が軽減される。特に、推力伝達ゲートをハードウェアとして備える構成とすれば（第１実施形態の第１例及び第２例等参照）、浮遊移動体の質量が大きくても効果器の推力が小さくてもセンシング遅延の影響が増大することはない。
【００４８】
本発明の浮遊移動体の制御システムでは、後記する通り、推力制御部において推力伝達ゲートにおける推力をフィードバックすることにより効果器の推力を制御する。これによって上記の高速な応答が実現されると同時に、推力制御部に非線形性に対してロバストな既存の制御手法（スライディングモード制御等）を適用すれば効果器動特性の非線形性を抑え込み、制御性能への悪影響を防ぐことが可能である。従来、効果器動特性の線形化は、効果器の動特性モデルにもとづくフィードフォワード補償によって行なわれてきたが、多大な手間と計算負荷を要するものであった。本発明の浮遊移動体の制御システムでは、必ずしも動特性モデルを用いない推力フィードバックによって線形化を実現するため、少ない計算負荷で制御システムを構築することが可能である。
【００４９】
その他、従来技術の下では上記の通りセンシングを行ってから実際に推力指令をスラスタに与える迄の間に無視できない時間遅れが生じることに起因して、例えば浮遊移動体を所定軌道上に維持するに当たって幾度と無くオーバーシュートやアンダーシュートの是正を繰り返さなければならない等、浮遊移動体に結果的に無駄な動きを与えることが多かった。制御系のゲインチューニングを適切にするための負担も、手間や時間の他、経験を必要とする非常に大きいものであった。
これに対し、本発明によれば、スラスタに真に必要な推力を適時に与えることが出来るため、スラスタや浮遊移動体に無駄な動きを与えることが無い。加えて、浮遊移動体に無駄なスラスタ駆動エネルギーを消費させずに済む結果、浮遊移動体の消費エネルギーを低減できる効果も得られる。
【００５０】
また、未知の外乱の影響も、本発明の浮遊移動体の制御システムにおいては推力伝達ゲートにおける推力の誤差として直ちにセンシングされ、推力制御部のローカルフィードバック制御によって補償されるため、推力計画部より上のレベルでは外乱に関して考慮する必要がない。すなわち、推力計画部では「静止せよ」「直進せよ」等の指示を行なうだけで、推力制御部で自動的に外乱の影響を補償して、予め計画された所望の動作を実現することが可能である。また、従来のように外乱の影響を位置・速度誤差によってセンシングする場合と比較して、高速な応答が実現される。
【００５１】
他方、本発明の浮遊移動体の加速度検知システムによれば、予め同定しておいた本体部の慣性行列と、推力伝達ゲートからの力・トルクセンサからの出力と、さらに、必要に応じて本体部の傾斜角センサからの出力と、を用いて本体部の加速度推定値を求めることが出来るので、浮遊移動体の重さや大きさに関係なく、或いは加速度センサの性能に依存することなく、微小な加速度の変化に至るまで浮遊移動体の加速度を精度良く検出することが出来、慣性航法を用いて浮遊移動体を誘導する際に、浮遊移動体を所望の軌道上に正確に誘導することが可能となる。
以下、本発明につきより詳細に説明する。
【図面の簡単な説明】
【００５２】
【図１】本発明の制御システムが適用された浮遊移動体の一例を示す概略図である。
【図２】本発明の制御システムの一構成例を示すブロック図である。
【図３】本発明の制御システムが適用された浮遊移動体の別の例を示す概略図である。
【図４】本発明の制御システムの別の構成例を示すブロック図である。
【図５】本発明の制御システムが適用された浮遊移動体の別の例を示す概略図である。
【図６】本発明の制御システムの別の構成例を示すブロック図である。
【図７】本発明の制御システムの第２実施形態を示すブロック図である。
【図８】本発明の制御システムの一実施例を示すブロック図である。
【図９】本発明の制御システムの別の実施例を示すブロック図である。
【図１０】本発明の制御システムの別の実施例を示すブロック図である。
【図１１】従来知られた浮遊移動体の概要を示す図である。
【図１２】従来知られた浮遊移動体の制御系を示すブロック図である。
【図１３】従来知られた浮遊移動体の制御系を示す別のブロック図である。
【図１４】従来知られた浮遊移動体の制御系を示す別のブロック図である。
【符号の説明】
【００５３】
Ａ  ロボットアーム部
Ｂ  本体部
Ｂ１  本体部静力学
Ｂ２  本体部慣性
Ｂ２’  本体部慣性
Ｂ３  本体部慣性
Ｂ４  積分
Ｃ  ケーブル
Ｅ  効果器部
Ｅ１  効果器推力特性
Ｅ２  効果器部静力学
Ｇ  推力伝達ゲート
Ｇ’  仮想推力伝達ゲート
Ｈ  オペレータ
Ｐ  計算機
Ｐ１  微分
Ｐ２  推力計画部
Ｐ３  推力制御部
Ｐ４  位置制御・速度制御
Ｓ  耐圧殻
Ｔ  スラスタ
１  制御システム
２  加速度センサ
３  傾斜角センサ
４  位置・速度センサ
５  空洞
１０  浮遊移動体
１１  減算回路
１２  スラスタ制御回路
１３  センサ
１４  効果器推力特性
１５  浮遊移動体ダイナミクス
１６  ロボットアームの反力・トルク
１７  加算回路
１８  スラスタ・ロボットアーム制御回路
１９  関節アクチュエータ
２０  浮遊移動体本体・ロボットアーム複合系ダイナミクス
２１  減算回路
発明を実施するための形態
【００５４】
［第１実施形態］
以下では、本発明を実施する対象の代表例たる水中ロボットにおける、本発明の一実施形態とその作用を、図１〜図７に基づき説明する。水中ロボットは、３次元空間を浮遊する浮遊移動体にほかならず、この水中ロボットに対して本発明の制御システムを適用する場合には、重力加速度の影響を補償する必要があることから、上記（７）に係る発明として規定される通り、本体部の傾きを計測できる傾斜角測定手段を備えることが必要とされる。このことは、仮想推力伝達ゲートシステムの考え方が適用される上記（５）に係る発明の場合でも同様であり、３次元空間を浮遊する浮遊移動体等に対して仮想推力伝達ゲートシステムの考え方を用いた本発明の制御システムを適用する際には、上記（７）に係る発明として規定される通り、浮遊移動体の傾きを計測できる傾斜角測定手段を備えることが必要とされる。
上記（７）に係る発明である、傾斜角測定手段を更に備えた上記（１）及び（３）、又は（２）及び（４）に係る発明は、それぞれこの第１実施形態の第１例又は第２例に、並びに後記実施例１又は実施例２に相当する。又上記（５）及び（６）に係る発明は、この第１実施形態の第３例及び後記実施例３に相当する。
尚適用対象が水中ロボットに限られないことは、上記定義欄及び後記実施例中の記載からも十分理解される。又水中ロボットと言えども、図中に表された深海探査等に用いる典型的な例に限られず、水難救助や油田探索等にも使用し得る比較的小型のものも、この水中ロボットの範疇に含まれる。
ここで、図１は本発明の制御システムが適用された浮遊移動体の一例を示す概略図、図２は本発明の制御システムの一構成例を示すブロック図である。また図３及び図５は本発明の制御システムが適用された浮遊移動体の別の例を示す概略図、図４及び図６は本発明の制御システムの別の構成例を示すブロック図である。図７は、本発明の制御システムの第２実施形態を示すブロック図である。
上記図１〜７においては、先に示した図１１〜１４と同一のものには同一符号を付して説明するものとする。
図２、４、６及び７中、各ブロック間を繋ぐ細実線は信号を表すものである。一方、浮遊移動体１０内における各ブロック間を繋ぐ二重線は、物理的な作用として浮遊移動体１０に起きている現象（力学的な変換作用等）を表すものである。
【００５５】
ところで、以下詳細を述べる本発明の説明に当たっては、慣性系の絶対位置に固定された基凖座標系をΣＲとし、浮遊移動体の重心位置に原点を持ち、浮遊移動体本体部に固定された本体座標系をΣＢとする。又特に断らない限り、ベクトル及び行列は、本体座標系ΣＢから見た表現であるとする。さらに、浮遊移動体はｍ次元空間（ｍ≦６）に存在し、浮遊移動体の効果器はｎ自由度（ｍ≦ｎ）の推力を発生可能とする。
【００５６】
第１例
本実施形態に係る推力伝達ゲートシステムの第１例の構成は図１及び２のようになる。図１は浮遊移動体の概略図、図２は制御システムのブロック図である。
本例においては、浮遊移動体１０は本体部Ｂと効果器部Ｅに力学的に分離されており、本体部Ｂと効果器部Ｅの間には、推力伝達ゲートＧがハードウェアとして備えられている。又本体部Ｂには加速度センサ２と傾斜角センサ３が、推力伝達ゲートＧには力・トルクセンサが備えられている。本例では、本体部Ｂと推力伝達ゲートＧは効果器部Ｅ同様、外部に露出した構成となっている。
なお、後述する本体部Ｂの慣性行列ＭＢと浮遊移動体１０の幾何学的構造を表現するＪＧＢ、ＪＥＧは、あらかじめ測定され既知の行列として与えられているものとする。
【００５７】
以下、本例の構成に付き詳細に説明する。
図２の本体部Ｂ及び効果器部Ｅの内部を参照すると、浮遊移動体の物理ダイナミクスを表わす運動方程式は一般に次式のように書ける。
【００５８】
【数１】
0004742329.tif 000002
【００５９】
なお、本例の推力伝達ゲートシステム１においては本体部Ｂの単一剛体構造上に推力伝達ゲートＧを固定した構造としている。これによって、ヤコビ行列ＪＧＢと慣性行列ＭＢは定数行列となる。
【００６０】
次に、本例の推力計画部につき説明する。
図２の計算機Ｐ内の推力計画部Ｐ２のブロック部分に示される通り、本体部Ｂの目標加速度ａＢｄ（ｔ）を実現するために必要な推力伝達ゲートＧにおける目標推力ｕＧｄ（ｔ）を次式の様に定める。
【数２】
0004742329.tif 000003
【００６１】
ただしａＢｄ（ｔ）、＾ａＢ（ｔ）［ｍ次元ベクトル］はそれぞれ本体加速度ａＢ（ｔ） の目標値と推定値、ｕＧｄ（ｔ）［ｍ次元ベクトル］はゲート推力ｕＧ（ｔ）の目標値、ｕＧｓ（ｔ）［ｍ次元ベクトル］は推力伝達ゲートに備えられた力・トルクセンサによるｕＧ（ｔ）の測定値である。
【００６２】
ここで、図２の計算機Ｐ内において参照符Ｐ３として示される推力制御部において、このｕＧｄ（ｔ）を推力フィードバックによって実現するようにｕＧ（ｔ）を制御することで、推力フィードバックによって推力を制御する、遅れの少ないフィードバック制御系が構築できる。
推力制御部Ｐ３においては、図２の効果器部Ｅ内において参照符Ｅ１として示される効果器推力特性や効果器部に対する外乱や動的な力ｆＥ（ｔ）などの効果器部動特性の未知の非線形性を抑え込むために、非線形系に対して優れたロバスト性を発揮するような制御手法を適用すべきである。例えば既存のスライディングモード制御を適用すると、推力制御部Ｐ３は以下のように構築できる。
【００６３】
本例における推力制御部Ｐ３においては、目標推力ｕＧｄ（ｔ）を実現するために必要な効果器への推力指令ｕＥｄ（ｔ）を次式のように定める。
【００６４】
【数３】
0004742329.tif 000004
【００６５】
本例において、推力計画部、推力制御部へのフィードバックに必要な値はｕＧｓ（ｔ）と＾ａＢ（ｔ） である。ゲート推力測定値ｕＧｓ（ｔ）には、推力伝達ゲートに備えられた力・トルクセンサによる測定値がそのまま用いられる。
【００６６】
以下では、本体部加速度ａＢ（ｔ）の推定方法について述べる。センサ特性、推力制御特性について考察するために、以下では主に周波数領域で議論を展開する。
【００６７】
まず、図２の本体部Ｂ内において参照符２で示される本体部加速度センサによって得られる加速度の測定値ａＢｓ（ｔ）［ｍ次元ベクトル］は以下のように与えられる。この測定値には重力加速度が含まれているため、本体加速度ａＢ（ｔ）の推定値としてそのまま使用することはできない。
【００６８】
【数４】
0004742329.tif 000005
【００６９】
尚、＊（ｓ）はベクトルあるいは行列＊ のラプラス変換＊（ｓ）＝Ｌ［＊（ｔ）］、ＧａＢｓ（ｓ）は本体部加速度センサ特性を表わすｍ行ｍ列の伝達関数行列である。
同様に、本体部傾斜角センサによって得られる重力加速度の測定値ｇＢｓ（ｔ）［ｍ次元ベクトル］は以下のように与えられる。
【００７０】
【数５】
0004742329.tif 000006
【００７１】
ただしＧｇＢｓ（ｓ）は本体部傾斜角センサ特性を表わすｍ行ｍ列の伝達関数行列である。
同様に、推力伝達ゲートによって得られるゲート推力測定値ｕＧｓ（ｔ）［ｍ次元ベクトル］は以下のように与えられる。
【００７２】
【数６】
0004742329.tif 000007
【００７３】
ただしＧｕＧｓ（ｓ）は推力伝達ゲートの力・トルクセンサ特性を表わすｍ行ｍ列の伝達関数行列である。
【００７４】
次に、本例における本体加速度ａＢ（ｔ）の推定につき説明する。
本体加速度ａＢ（ｔ）の推定値＾ａＢ（ｔ）を次式のように定める。
【００７５】
【数７】
0004742329.tif 000008
【００７６】
ここで、上記本体加速度推定の妥当性について考える。この式（９）をラプラス変換して上の（６）、（７）式を代入すると、
・        ＾ａＢ（ｓ）＝ＧａＢｓ（ｓ）（ａＢ（ｓ）−ｇＢ（ｓ））
＋ＧｇＢｓ（ｓ）ｇＢ（ｓ）      （９）’
になるところ、「すべての種類、すべての自由度におけるセンサ特性は同一であり、各センサ、各自由度間の干渉も存在しない（センサ特性一致条件）」という上記の条件が満たされている場合には、本体部加速度センサ特性と本体部傾斜角センサ特性と、推力伝達ゲートの力・トルクセンサ特性について、
・        ＧａＢｓ（ｓ）＝ＧｇＢｓ（ｓ）＝ＧｕＧｓ（ｓ）＝Ｇｓ（ｓ）Ｉ    （１０）
としてよい。ただしＧｓ（ｓ）は同一センサ特性を表わす伝達関数、Ｉは単位行列である。すなわち、
・        ａＢｓ（ｓ）＝Ｇｓ（ｓ）（ａＢ（ｓ）−ｇＢ（ｓ））         （１１）
・        ｇＢｓ（ｓ）＝Ｇｓ（ｓ）ｇＢ（ｓ）                        （１２）
・        ｕＧｓ（ｓ）＝Ｇｓ（ｓ）ｕＧ（ｓ）                        （１３）
である。このとき、式（９）’は、
【００７７】
【数８】
0004742329.tif 000009
【００７８】
となり、式（８）によって本体部加速度ａＢ（ｔ）の推定が行なえている。式（１４）に重力加速度ｇＢｓ（ｔ）は陽に表われてこないことから、上記センサ特性一致条件が満たされている限り本体部加速度ａＢ（ｔ）を直接測定していると見なしてよいことになる。
なお、上記センサ特性の一致条件は、例えば浮遊移動体のダイナミクスと比較して十分高速な応答で、かつ高精度なセンサを選択することで実現される。
【００７９】
第２例
次に、本実施形態に係る推力伝達ゲートシステムの第２例につき説明する。
推力伝達ゲートシステムの第２例の構成は図３、４のようになる。図３は浮遊移動体の概略図、図４は制御システムのブロック図である。
本例においても、浮遊移動体１０は本体部Ｂと効果器部Ｅに力学的に分離されており、本体部Ｂと効果器部Ｅの間には、推力伝達ゲートＧがハードウェアとして備えられている。又本体部Ｂには傾斜角センサ３が、推力伝達ゲートＧには力・トルクセンサが備えられている。
第１例と大きく異なるのは、本例では効果器部Ｅが本体部Ｂを覆う殻（図３に示す耐圧殻Ｓ）になっており、本体部Ｂと推力伝達ゲートＧはこの殻Ｓによって完全に外界から隔離されている点である。この構造によって、第１例で必要であった本体部Ｂの加速度センサ２が不要となっている。
第１例同様、本体部の慣性行列ＭＢと浮遊移動体の幾何学的構造を表現するＪＧＢ、ＪＥＧは、あらかじめ測定され既知の行列として与えられているものとする。
【００８０】
以下、本例の構成に付き詳細に説明する。
図４の本体部Ｂ及び効果器部Ｅの内部を参照すると、浮遊移動体の物理ダイナミクスを表わす運動方程式は一般に次式のように書ける。
【００８１】
【数９】
0004742329.tif 000010
【００８２】
式（１５）は本体部、式（１６）は効果器部を表わす式である。ここで、新たにｕＧ２（ｔ）［ｍ次元ベクトル］を次式のように定義する。
【００８３】
【数１０】
0004742329.tif 000011
【００８４】
次に、本例の推力計画部につき説明する。
図４の計算機Ｐ内の推力計画部Ｐ２のブロック部分に示される通り、本体部Ｂの目標加速度ａＢｄ（ｔ）を実現するために必要な推力伝達ゲートＧにおける目標推力ｕＧ２ｄ（ｔ）を次式の様に定める。
【００８５】
【数１１】
0004742329.tif 000012
【００８６】
図４の計算機Ｐ内において参照符Ｐ３として示される推力制御部において、このｕＧ２ｄ（ｔ）を推力フィードバックによって実現するようにｕＧ２（ｔ）を制御することで、推力フィードバックによって推力を制御する、遅れの少ないフィードバック制御系が構築できる。
ここで、本例の推力制御につき例えばスライディングモード制御を適用すると、推力制御部Ｐ３は以下のように構築できる。
【００８７】
本例における推力制御部Ｐ３においては、目標推力ｕＧ２ｄ（ｔ）を実現するために必要な効果器への推力指令ｕＥｄ（ｔ）を次式のように定める。
【００８８】
【数１２】
0004742329.tif 000013
【００８９】
ただし＾ｕＧ２（ｔ）［ｍ次元ベクトル］はｕＧ２（ｔ）の推定値である。
【００９０】
ところで、本例においては、推力計画部Ｐ２へのフィードバックは必要なく、推力制御部Ｐ３へのフィードバックに必要な値は式（１７）のｕＧ２（ｔ）の推定値＾ｕＧ２（ｔ）のみである。ここでは、このｕＧ２（ｔ）の推定方法について説明する。
【００９１】
以下、本例におけるゲート推力ｕＧ２（ｔ）の推定につき説明する。
ゲート推力ｕＧ２（ｔ）の推定値＾ｕＧ２（ｔ）を次式のように定める。
【００９２】
【数１３】
0004742329.tif 000014
【００９３】
ここで、上記ゲート推力推定の妥当性について考える。式（２１）をラプラス変換して、
＾ｕＧ２（ｓ）＝ｕＧｓ（ｓ）＋ＪＧＢ−ＴＭＢｇＢｓ（ｓ）          （２１）’
ここでも前述のセンサ特性一致条件が満足されているとすると、式（１２），（１３）が同様に成立する。これらを代入して式（１７）を考慮すると、
【００９４】
【数１４】
0004742329.tif 000015
【００９５】
となり、式（２１） によってゲート推力ｕＧ２（ｔ）の推定が行なえている。式（２２）に重力加速度ｇＢｓ（ｔ）は陽に表われてこないことから、センサ特性一致条件が満たされている限りゲート推力ｕＧ２（ｔ）を直接測定していると見なしてよいことになる。
【００９６】
第３例
次に、本実施形態に係る推力伝達ゲートシステムの第３例につき説明する。
推力伝達ゲートシステムの第３例の構成は図５、６のようになる。図５は浮遊移動体の概略図、図６は制御システムのブロック図である。
この第３例においては、浮遊移動体１０は本体部Ｂと効果器部Ｅに力学的に分離されておらず、推力伝達ゲートＧもハードウェアとして備えられていない。しかしながら、本例の浮遊移動体１０には加速度センサ２と傾斜角センサ３が備えられている。
本例では、本体部Ｂの慣性行列ＭＢを適当に定め、このＭＢにもとづいて、図６の計算機Ｐ内に示された仮想推力伝達ゲートＧ’における推力推定値をセンサの測定値から計算することで、推力伝達ゲートが存在するものとして推力伝達ゲートシステムを構築する。浮遊移動体１０の幾何学的構造を表現するＪＧＢ、ＪＥＧについては、上記第１例、第２例同様、予め測定され既知の行列として与えられているものとする。
尚、本体部Ｂの慣性行列ＭＢの定め方としては、例えば、浮遊移動体１０設計時における設計図、主要諸元その他から計算機によって本体部Ｂの慣性行列ＭＢを予め割り出し、これを不図示のメモリ等に格納しておく手法等が挙げられる。
【００９７】
以下、本例の構成につき詳細に説明する。
図６の本体部Ｂ及び効果器部Ｅの内部を参照すると、浮遊移動体の物理ダイナミクスを表わす運動方程式は一般に次式の様に書ける。
【００９８】
【数１５】
0004742329.tif 000016
【００９９】
式（２３）は本体部、式（２４）は効果器部を表わす式である。これは第２例の場合とほぼ同じであるが、この第３例では推力伝達ゲートは物理的には存在しないため、推力伝達ゲートの推力ｕＧｖ［ｍ次元ベクトル］は仮想的な値であることに注意されたい。
【０１００】
次に、本例の推力計画部につき説明する。
図６の計算機Ｐ内の推力計画部Ｐ２のブロック部分に示される通り、本体部Ｂの目標加速度ａＢｄ（ｔ）を実現するために必要な仮想推力伝達ゲートＧ’における目標推力ｕＧｖｄ（ｔ）を次式の様に定める。
【０１０１】
【数１６】
0004742329.tif 000017
【０１０２】
図６の計算機Ｐ内において参照符Ｐ３として示される推力制御部において、このｕＧｖｄ（ｔ）を推力フィードバックによって実現するようにｕＧｖ（ｔ）を制御することで、推力フィードバックによって推力を制御する、遅れの少ないフィードバック制御系が構築できる。
ここで、本例の推力制御につき例えばスライディングモード制御を適用すると、推力制御部Ｐ３は以下のように構築できる。
【０１０３】
本例における推力制御部Ｐ３においては、目標推力ｕＧｖｄ（ｔ）を実現するために必要な効果器への推力指令ｕＥｄ（ｔ）を次式の様に定める。
【０１０４】
【数１７】
0004742329.tif 000018
【０１０５】
ただし＾ｕＧｖ（ｔ）［ｍ次元ベクトル］はｕＧｖ（ｔ）の推定値である。
【０１０６】
ところで、本例においては、上記第２例と同様に推力計画部Ｐ２へのフィードバックは必要なく、推力制御部Ｐ３へのフィードバックに必要な値はｕＧｖ（ｔ）の推定値＾ｕＧｖ（ｔ）のみである。ここでは、このｕＧｖ（ｔ）の推定方法について説明する。
【０１０７】
以下、本例におけるゲート推力ｕＧｖ（ｔ）の推定につき説明する。
ゲート推力ｕＧｖ（ｔ）の推定値＾ｕＧｖ（ｔ）を次式のように定める。
【０１０８】
【数１８】
0004742329.tif 000019
【０１０９】
ここで、上記ゲート推力推定の妥当性について考える。式（２８）をラプラス変換して、
・    ＾ｕＧｖ（ｓ）＝ＪＧＢ−ＴＭＢ（ａＢｓ（ｓ）＋ｇＢｓ（ｓ））      （２８）’
ここでも前述のセンサ特性一致条件が満足されているとすると、式（１１），（１２）が同様に成立する。これらを代入して式（２３）を考慮すると、
【０１１０】
【数１９】
0004742329.tif 000020
【０１１１】
となり、式（２８）によってゲート推力ｕＧｖ（ｔ）の推定が行なえている。式（２９）に重力加速度ｇＢｓ（ｔ）は陽に表われてこないことから、センサ特性一致条件が満たされている限りゲート推力ｕＧｖ（ｔ）を直接測定していると見なしてよいことになる。
【０１１２】
［第２実施形態］
次に、本発明の浮遊移動体の加速度検知システムの一実施形態に付き説明する。第１実施形態同様、本実施形態についても適用対象を水中ロボットとして以下説明する。尚水中ロボットは、３次元空間を浮遊する浮遊移動体にほかならず、この水中ロボットに対して本発明の加速度検知システムを適用する場合には第１実施形態同様、重力加速度の影響を補償する必要があることから、本体部の傾きを計測できる傾斜角測定手段を備えることが必要とされる。
本実施形態では、推力伝達ゲートを利用して本体部加速度の推定を行なっている。本実施形態に係る加速度検知システムの構成は図３及び図７の様になる。図３は浮遊移動体の概略図、図７は加速度検知システムのブロック図である。
【０１１３】
これまでの第１実施形態の各例では、浮遊移動体１０の制御システム１として推力伝達ゲートシステムを構築したのに対して、第２実施形態では浮遊移動体１０の高精度な加速度センシングシステムとして推力伝達ゲートＧを利用する。
【０１１４】
本実施形態のハードウェア構成は先の第１実施形態に係る第２例（図３）とまったく同じである。すなわち、浮遊移動体１０は本体部Ｂと効果器部Ｅに力学的に分離されており、本体部Ｂには傾斜角センサ３が、推力伝達ゲートＧには力・トルクセンサが備えられている。
ただし、本実施形態では本体部加速度ａＢ（ｔ）を高精度に推定することを目的としており、制御を目的としないので、推力計画部と推力制御部は本実施例の中には含まれない。
また、本体部の慣性行列ＭＢと浮遊移動体の幾何学的構造を表現するＪＧＢは、あらかじめ測定され既知の定数行列として与えられているものとする。この本体部慣性行列ＭＢを、推力伝達ゲートの力・トルクセンサの測定レンジと、本実施形態にて検知すべき加速度レンジとを勘案して適切に設計することにより、高精度な加速度測定が可能となる。
【０１１５】
以下、本実施形態の構成につき詳細に説明する。
図３及び図７に示す本実施形態のハードウェア構成においては、本体部加速度ａＢ（ｔ）は、式（１５）を変形して以下の様に表わされる。
【０１１６】
【数２０】
0004742329.tif 000021
【０１１７】
次に、本実施形態における本体部加速度ａＢ（ｔ）の推定につき説明する。
本体部加速度ａＢ（ｔ）の推定値＾ａＢ（ｔ）を次式の様に定める。
【０１１８】
【数２１】
0004742329.tif 000022
【０１１９】
ここで、上記本体加速度推定の妥当性について考える。式（３１）をラプラス変換して、
【０１２０】
【数２２】
0004742329.tif 000023
【０１２１】
ここでも前述のセンサ特性一致条件が満足されているとすると、式（１２）、（１３）が第１実施形態と同様に成立する。これらを代入して式（３０）を考慮すると、
【０１２２】
【数２３】
0004742329.tif 000024
【０１２３】
となり、式（３１）によって本体加速度ａＢ（ｔ）の推定が行なえていることがわかる。
【実施例１】
【０１２４】
上記第１実施形態の欄で説明した通り、本発明の浮遊移動体の制御システムでは推力制御を行なうため、実現されるのは厳密には位置・速度軌道ではなく加速度の軌道である。ここで、加速度の積分誤差に起因するドリフトの影響は不可避であり、従って、本発明が単独で顕著に効果を発揮するのは、比較的高い周波数帯域の運動に対してである。低周波のゆっくりとした運動に対しては、従来の位置・速度をフィードバックする制御手法を併用する必要があるが、この場合であっても、従来手法単独で用いるより精度を向上させることが可能である。
以下、本発明の一実施例として、上記第１実施形態で説明した本発明の浮遊移動体の制御システムの各例に従来の位置・速度フィードバック制御を併せ備えた制御システムにつき説明する。
【０１２５】
ここで、図８は本発明の制御システムの一実施例を示すブロック図であって上記第１実施形態の第１例に相当するもの、図９は本発明の制御システムの別の実施例を示すブロック図であって上記第１実施形態の第２例に相当するもの、図１０は本発明の制御システムの別の実施例を示すブロック図であって上記第１実施形態の第３例に相当するものである。上記図８〜１０においては、先に示した図１〜７及び図１１〜１４と同一のものには同一符号を付して説明するものとする。
又図２、４、６及び７と同様に、図８〜１０中各ブロック間を繋ぐ細実線は信号を表すものである。一方、浮遊移動体１０内における各ブロック間を繋ぐ二重線は、物理的な作用として浮遊移動体１０に起きている現象（力学的な変換作用等）を表すものである。
【０１２６】
実施例１は、先の第１実施形態、第１例に相当するものである。図８に、本実施例に係る制御システム１のブロック図を示す。本実施例の浮遊移動体１０の概略構成は先の図１に示す通りである。
図８のブロック図からも明らかな通り、本実施例は、上記第１実施形態の第１例で説明した図２に示す構成を従来知られた位置・速度フィードバック制御システムに適用したものである。従って本発明の推力伝達ゲートシステム部分の構成及び制御系の動作等は先の図１及び図２に基づいて説明される。
【０１２７】
本実施例に係る制御システム１即ち推力伝達ゲートシステムは、参照符１０で示される浮遊移動体の機構を、推力を受ける本体部Ｂと推力を発生する効果器部Ｅとに力学的に分離した上、その結合部分に推力伝達ゲートＧを付加したことをその主たる特徴とするものである。ここで、推力伝達ゲートＧは、本体部Ｂと効果器部Ｅを結合する唯一の部分であり、本体部Ｂと効果器部Ｅの間にかかる力とトルクを全て計測できるセンサ（力・トルクセンサ）を有している。尚力・トルクセンサについては、市販の汎用製品（多次元の力やトルク成分を出力させ得る多チャンネル品等）を使用し得る。
この様に、本実施例では、本体部Ｂと効果器部Ｅとを推力伝達ゲートＧで結合することにより、効果器部Ｅから本体部Ｂに作用する全ての推力を実測出来る様構成されている。図１に示す通り、推力伝達ゲートシステムが適用された本実施例の浮遊移動体１０では、本体部Ｂに加速度センサ２と傾斜角センサ３が備えられている。
【０１２８】
上記構成を有する本実施例に係る制御システム１では、従来知られた位置・速度フィードバック制御と同時に、本発明の推力伝達ゲートシステムによる推力フィードバック制御が行なわれる。
【０１２９】
本実施例によれば、従来の位置・速度をフィードバックする制御手法を併用して加速度の積分誤差に起因するドリフトの影響を回避しつつ、本発明の制御システム即ち推力伝達ゲートシステムを用いて例えば波浪や潮流、風或いは浮遊移動体に取り付けられたロボットアームの反動等による外乱下にあっても、浮遊移動体を所定位置に精度良く静止させたり、或いは目標軌道に精度良く追従させたりすることが出来るため、浮遊移動体をより高速かつ高精度に制御することが可能となる。
【実施例２】
【０１３０】
本実施例は、先の第１実施形態、第２例に相当するものである。図９に、本実施例に係る制御システム１のブロック図を示す。本実施例の浮遊移動体１０の概略構成は先の図３に示す通りである。
実施例１同様、本実施例に係る制御システム１も、水中ロボット１０の機構を、推力を受ける本体部Ｂと推力を発生する効果器部Ｅとに力学的に分離した上、その結合部分に推力伝達ゲートＧを付加したことをその主たる特徴とするものである。
但し、本実施例では、本体部Ｂは効果器部Ｅの内部に格納されており、外部からの外乱やロボットアームＡの反動等は全て、耐圧殻Ｓたる効果器部Ｅに及ぼされる構造となっている点が前記実施例１と異なる。ここで、本体部Ｂは効果器部Ｅの内部において、推力伝達ゲートＧのみを介して担われている。それ故、推力伝達ゲートＧが、本体部Ｂと効果器部Ｅを結合する唯一の部分である点は前記実施例１と同様である。
従って本実施例においても、本体部Ｂと効果器部Ｅとを推力伝達ゲートＧで結合することにより、効果器部Ｅから本体部Ｂに作用する全ての推力を実測出来る様構成されている。
【０１３１】
この実施例２によっても、従来の位置・速度をフィードバックする制御手法を併用して加速度の積分誤差に起因するドリフトの影響を回避しつつ、本発明の制御システム即ち推力伝達ゲートシステムを用いて例えば波浪や潮流、風或いは浮遊移動体に取り付けられたロボットアームの反動等による外乱下にあっても、浮遊移動体を所定位置に精度良く静止させたり、或いは目標軌道に精度良く追従させたりすることが出来るため、浮遊移動体をより高速かつ高精度に制御することが可能となる。
【実施例３】
【０１３２】
本実施例は、先の第１実施形態、第３例に相当するものである。図１０に、本実施例に係る制御システム１のブロック図を示す。本実施例の浮遊移動体１０の概略構成は先の図５に示す通りである。
本実施例に係る制御システム１は、水中ロボットの機構を、推力を受ける本体部Ｂと推力を発生する効果器部Ｅとに計算上分離した上、両者の間に仮想推力伝達ゲートＧ’を仮想的に設けたことをその主たる特徴とするものである。
なお、仮想推力伝達ゲートＧ’では、本体部Ｂと効果器部Ｅの間にかかる力とトルクを全て推定できる様になっている。
【０１３３】
この実施例３によっても、従来の位置・速度をフィードバックする制御手法を併用して加速度の積分誤差に起因するドリフトの影響を回避しつつ、本発明の制御システム即ち推力伝達ゲートシステムを用いて例えば波浪や潮流、風或いは浮遊移動体に取り付けられたロボットアームの反動等による外乱下にあっても、浮遊移動体を所定位置に精度良く静止させたり、或いは目標軌道に精度良く追従させたりすることが出来るため、浮遊移動体をより高速かつ高精度に制御することが可能となる。
【０１３４】
［慣性航法装置を利用した実装］
ところで、航空機等の浮遊移動体に広く利用されている、既存の技術である慣性航法装置は、本発明で使用される加速度センサ、傾斜角センサ、本体部位置・速度センサとして使用することが可能である。
すなわち、慣性航法装置を搭載した浮遊移動体であれば、一切のハードウェアの追加、変更なしに、ソフトウェアの変更のみで、例えばこの実施例３或いは上記第１実施形態の第３例に係る本発明の制御システムを実装することが可能であり、それ故既存の浮遊移動体の高速高精度化をより容易に実現することが可能となる。
より詳しく説明すると、既存の慣性航法装置では、加速度を十分高精度に計測しているのにも関わらず、それを積分することにより得た位置・速度情報のみを制御に使っているのが現状である。しかるに、今までそのままでは利用されていなかった加速度情報を、本発明の推力伝達ゲートシステムの枠組みの中で推力測定に利用することで、ハードウェアは従来と同じ儘で、既存の制御よりもより高速高精度化を実現することが可能となる。
特に、上記慣性航法装置については最近では加速度センサとジャイロ、ＧＰＳ（Global  Positioning System）その他のセンサをユニット化して加速度、速度及び変位情報を得る、ハイブリッドタイプの慣性航法装置が主流となってきており、総合的に見ても計測速度が向上している。それ故、上記慣性航法装置は本発明の制御システムを実装する際に利用し得る好適なセンサユニットということができる。
【０１３５】
尚本発明は、上記各例記載の構成に限定されず、種々の設計変更その他の変形が可能である。
はじめに、上記各例では、水中ロボットに本発明の制御システムを適用したものにつき説明したが、本発明の制御システムが適用される対象としては水中ロボットに限られない。本発明の制御システムは、水中ロボットの他にも、上記定義欄で例示した通り、潜水艇、ヘリコプター、飛行船、航空機、宇宙船又は宇宙ロボット等の水中、空中又は宇宙空間その他における浮遊移動体に適用できる。その他、本発明の制御システムは、水上や地上を滑走するホバークラフトや、車輪が滑っている状態の自動車等、平面上を滑走する各種移動体にも適用可能である。
さらに、上記実施形態の各例及び上記各実施例では、３次元空間を浮遊する浮遊移動体に対して本発明の制御システム或いは加速度検知システムを適用した場合について説明を行った。しかしながら、そもそも重力の働かない宇宙空間を浮遊する浮遊移動体や、傾きを考慮しなくて良い、或いは無視しても構わない水平面上を滑走する浮遊移動体に対して本発明の制御システム或いは加速度検知システムを適用する際には、上記（１）〜（６）に係る発明として規定される通り傾斜角測定手段は不要であり、このとき、推力計画部、推力制御部においても、浮遊移動体本体部（重心）にかかる重力加速度ｇＢ＝０とすることで、より簡単な制御則とすることができる。
又上記各例では、推力制御部Ｐ３の制御則の一例として、効果器動特性の非線形性に対してロバストな既存の制御手法たるスライディングモード制御を用いたが、推力制御部Ｐ３の制御則はこれに限定されない。従って、例えば推力制御部Ｐ３の制御則として、推力伝達ゲートＧにおける目標推力と推力伝達ゲートＧにおける推力測定又は推定値の推力誤差に対するスラスタＴ入力電圧ＰＩＤ制御を適用することも可能である。
さらに、上記各例では、スラスタＴとして、駆動源たるモータに機械的に連結されたプロペラによる推力発生機器を用いたが、スラスタＴの構成はこれに限定されない。従って、スラスタＴはジェット噴射式のものであっても良いし、またその駆動源もモータに限らず内燃機関や蒸気タービン、原子力タービン等種々の型式のものを採用し得る。
【０１３６】
以上に詳述した通り、本発明は、例えば波浪や潮流、風或いは浮遊移動体に取り付けられたロボットアームの反動等による外乱下にあっても、浮遊移動体を所定位置に精度良く静止させたり、或いは目標軌道に精度良く追従させたりすることが出来、浮遊移動体を高速かつ高精度に制御し得るシステム構成を提供する、新規かつ極めて有用なる発明であることが明らかである。
【図１】
0004742329.tif 000025
【図２】
0004742329.tif 000026
【図３】
0004742329.tif 000027
【図４】
0004742329.tif 000028
【図５】
0004742329.tif 000029
【図６】
0004742329.tif 000030
【図７】
0004742329.tif 000031
【図８】
0004742329.tif 000032
【図９】
0004742329.tif 000033
【図１０】
0004742329.tif 000034
【図１１】
0004742329.tif 000035
【図１２】
0004742329.tif 000036
【図１３】
0004742329.tif 000037
【図１４】
0004742329.tif 000038