説明

浮遊移動体の制御システム

この発明の目的は、波浪や潮流等による外乱下にあっても、浮遊移動体を所定位置に精度良く静止させたり、或いは目標軌道に精度良く追従させたりすることが可能な浮遊移動体の制御システムを提供することにある。
上記目的を達成可能な本発明の浮遊移動体の制御システムは、浮遊移動体10の制御システム1であって、上記浮遊移動体が、浮遊移動体の一部を占める、単一剛体とみなし得る本体部Bと、浮遊移動体に対して推力を発生する効果器部Eと、本体部と効果器部を力学的に結合する部分であり、効果器部から本体部に作用する推力を実測できるよう構成された推力伝達ゲートGとからなっており、上記推力伝達ゲートGからの推力測定値を用いて効果器部Eに対する推力指令を得る様にしたことを特徴とするものである(図4)。

【発明の詳細な説明】
【技術分野】
【0001】
本発明は、水中、空中、宇宙空間などにおける浮遊移動体、或いは平面上を滑走する移動体の制御システムに関する。
【背景技術】
【0002】
はじめに、従来知られた浮遊移動体の制御手法につき、図11〜14等を参照しながら説明する。図11は従来知られた浮遊移動体の概略図、図11〜14は従来知られた浮遊移動体の制御手法を示す概略図である。
【0003】
図11に示す通り、水中ロボットをはじめとする浮遊移動体10本体の位置および速度を制御するために、その本体にはセンサ13が設けられ、これによって浮遊移動体10本体の位置および速度が検出され、計算機P内の減算回路11に与えられる。減算回路11にはまた、目標となる位置および速度を表す信号がオペレータHより与えられる。減算回路11の出力は、浮遊移動体10に設けられた推進力発生手段(効果器)たるスラスタTを制御するためのスラスタ制御回路12に与えられ、こうして浮遊移動体10の位置および速度が制御される(いわゆる位置・速度フィードバック制御)。
【0004】
図12は、この図11に示される浮遊移動体10の制御を行なうためのブロック図である。スラスタ制御回路12の出力たる効果器推力指令信号は、所定の推力特性14を持つスラスタTに与えられ、それにより効果器推力が浮遊移動体10に与えられ、参照符15で示されるように、浮遊移動体10のダイナミクスすなわち動力学の入出力特性が得られ、その本体の位置および速度が、センサ13によって前述のように検出されて、減算回路11に与えられる。
【0005】
このように、従来の浮遊移動体の制御においては、制御対象となる浮遊移動体の位置および速度の情報をフィードバックすることにより効果器への推力指令を直接操作しているところ、一般的に効果器の応答速度が比較的遅い浮遊移動体においては、効果器に指令を発してから、その効果が浮遊移動体の位置および速度に反映されセンサで取得されるまでに時間がかかり、制御性能に悪影響を及ぼすと言う問題があった(いわゆるセンシング遅延の問題)。この傾向は、浮遊移動体の質量が大きいほど、また効果器の推力が小さいほど顕著である。
さらに、水中、空中など流体中の浮遊移動体は、波浪や潮流、風等、流体力学的な外乱の影響を常に受けている他、図11にも示される様に水中における浮遊移動体10に対しては水上からのアンビリカルケーブルC(給電線や通信線ほか)等が接続されており、このケーブルが浮遊移動体10を引張ったりする場合が往々にしてあるところ、上記の制御手法では未知の外乱に対する応答に時間が掛かり、やはり制御性能に悪影響を及ぼすと言う問題があった。
【0006】
次に、図11に示されるように、浮遊移動体10に複数のリンクを備えるロボットアームAが更に設けられているときには、図13に例示される様なブロック図が構成されることになる。参照符16で示されるようにロボットアームAによる反力およびトルクは、加算回路17として例示される様に、スラスタTによる効果器推力に加算される。したがって、ロボットアームAからの反力とトルクによって、浮遊移動体10の動き即ち位置および速度が乱されることになり、このロボットアームAからの反力とトルクによって発生する浮遊移動体10の位置および速度の誤差を修正する様にスラスタTが制御される。このように、ロボットアームAからの反力とトルクにより浮遊移動体10の動き(位置や速度)が乱されるところ、従来の姿勢制御では、その結果発生する位置や速度の誤差を修正するようにスラスタTを作動させていた。
【0007】
つまり、従来知られた制御手法では、浮遊移動体10がロボットアームAの反力とトルクによって動いた後にスラスタTを作動させることになり、外乱に対する応答速度が低いといういわゆるセンシング遅延の問題がやはり解消されない儘であった。
【0008】
また、図14には、別の先行技術のブロック図が示されている。この制御手法は、ロボットアームAの運動と浮遊移動体10本体の運動の相互干渉を考慮して、それぞれの動きが目的に合致するように、一度に効果器推力とロボットアーム関節トルクを決定する手法である。この先行技術では、ロボットアームAが備えられた浮遊移動体10を多体リンク系として、その運動方程式から各ロボットアームの各関節トルクが浮遊移動体10本体の運動に及ぼす影響を数式的に表現し、その数式を解くことによって、浮遊移動体10本体の推進力を決定している(いわゆるモデルベースト制御)。
【0009】
この図14の先行技術では、スラスタ・ロボットアーム制御回路18からの効果器推力指令信号はスラスタTに与えられ、またロボットアームAのトルク指令信号は関節アクチュエータ19に与えられる。こうして、スラスタTから出力される効果器推力並びに関節アクチュエータ19による関節トルクによって浮遊移動体10本体とロボットアームAの複合系ダイナミクス20が形成され、センサ13は浮遊移動体10本体の位置および速度を検出して減算器11に与え、この減算器11には浮遊移動体10本体の目標となる位置および姿勢を表す信号が与えられる。又複合系ダイナミクス20からのロボットアーム制御変数即ち、位置、姿勢、関節角、速度、手先反力等を表す信号は減算回路21に与えられ、この減算回路21にはロボットアームAの制御変数の目標値が与えられる。これら減算器11,21の出力は、スラスタ・ロボットアーム制御回路18に与えられる。
すなわち、この制御手法では、ロボットアームAの運動と、浮遊移動体10本体の運動の相互干渉を考慮して、それぞれの動きが目的に合致するように、一度に効果器推力とロボットア−ム関節トルクを決定している。これによって正確に浮遊移動体10本体の動きを制御することができるという利点がある一方、次の(1)〜(3)の問題があった。
【0010】
(1) 対象のダイナミクスが一般的に複雑であり、ロボットアームAが6軸であれば、浮遊移動体10本体と併せて7個の剛体が繋がったリンクのダイナミクスになる。ロボットアームAが複数になると、さらに飛躍的に複雑なダイナミクスになる。この様な制御系の演算には多大な手間が掛かり、計算機に重い計算負荷を及ぼすこととなる。その為、上記の制御システムを小型の計算機を用いて構成することは現実的に非常に困難であった。
【0011】
(2) 又上記の制御システムではロボットアームAの各リンクおよび本体の質量、慣性モーメント、重心位置などの多くの力学パラメータが必要となり、これらの力学パラメータは、すべて正しい値を用いなければ、正しい制御を実現することはできない。したがってロボットアームAで物をつかんだりした場合、つかんだ物の質量や慣性モーメント、重心位置などを正しく与えなければならない。従ってつかむ可能性のある物のデータベースを持つか、対象の力学特性を計測する必要があり、現実的には、用途が限られてしまうという問題があった。
(3) 更に、水中、空中など流体中の浮遊移動体は、波浪や潮流、風など、流体力学的な未知の外乱の影響を常に受けている上、図11にも示される様に水中における浮遊移動体10に対しては水上からのアンビリカルケーブルC(給電線や通信線ほか)等が接続されており、このケーブルが浮遊移動体10を引張ったりする場合が往々にしてあるところ、かかる未知の外乱をモデル化することは不可能である上、この外乱の影響を予測することは一般に困難であり、この様なモデルベースト制御手法は、浮遊移動体の制御に基本的に馴染まないと言う問題があった。
【0012】
その他,従来知られた浮遊移動体の制御手法としては、特許文献1に挙げられる様なマニピュレータの根元の力フィードバック制御、或いは非特許文献1に挙げられる様な推力ローカルフィードバック制御等が存在する。
【0013】
特許文献1に係るマニピュレータの根元の力フィードバック制御によれば、浮遊移動体本体がマニピュレータによる外乱によって動いてしまうよりも先にスラスタを作動させて浮遊移動体本体への影響を打ち消し、浮遊移動体本体がマニピュレータによる外乱で動くことを防止することが可能となる。
【0014】
また、浮遊移動体の位置・速度の制御のための効果器として用いられるスラスタは、流体を利用して推力を発生するため、スラスタヘの入力指令と実際に出力される推力との間には流体に起因する強い非線形性が存在するところ、このような非線形性を構成する要素は多岐にわたり、それらの全てを考慮した制御を行なうことは従来困難であった(いわゆる効果器動特性の非線形性の問題)が、非特許文献1に係る推力ローカルフィードバック制御によれば、上記効果器動特性の非線形性に起因する浮遊移動体の制御性能の悪化を防止することが可能であった。
【0015】
しかしながら、上記特許文献1及び非特許文献1に係る制御手法は共に、一部の力を計測してフィードバックするものではあったが、浮遊移動体に対する最も大きな外乱である波浪や潮流、風等の影響を打ち消すものではなかった。
【0016】
要するに、上記いずれの先行技術においても、ロボットアームの反動等による外乱のほか、水中、空中等の流体中にある浮遊移動体が常に受ける波浪や潮流、風等の流体力学的な未知の外乱下にあっても自動的に外乱の影響を補償して、浮遊移動体を精度良く静止させたり、或いは目標軌道に高精度に追従させたりすることが可能な浮遊移動体の制御システムは実現されていなかったのである。
【0017】
他方、上記課題とは別に、浮遊移動体を航行させる従来知られた手法として慣性航法なるものが多用されているところ、この慣性航法には次の様な問題があった。すなわち、慣性航法は、浮遊移動体から求められる加速度を利用しているところ、浮遊移動体自体が大きく重いものである場合には、加速度を一旦変位に変換してから加速度の値を電気信号として出力する従来知られた加速度センサの構造上、微小な加速度の変化を精度良く検出出来ず、センサから得られる加速度値と実際の加速度値との間に顕著な誤差が生じるという問題があった。この様な加速度値の誤差は、慣性航法自体の精度にも著しく影響し、浮遊移動体を所望の軌道上に正確に誘導出来ないという不都合が生じていた。
【特許文献1】特開平5−119837号公報
【非特許文献1】金岡、中山、林、川村著、“水中における高精度運動実現のためのスラスタ推カローカルフィードバック制御”、日本機械学会ロボティクス・メカトロニクス講演会 ’03講演論文集、2P1−2F−A6、2003年
【発明の開示】
【0018】
[発明が解決しようとする課題]
従って本発明は、例えば波浪や潮流、風その他浮遊移動体に取り付けられたロボットアームの反動等による外乱下にあっても、浮遊移動体を所定位置に精度良く静止させたり、或いは目標軌道に精度良く追従させたりすることが可能な、浮遊移動体の制御システムを提供することを課題とする。
【0019】
又本発明は、比較的簡単な構成で、浮遊移動体の位置及び姿勢等を正確に制御することができるようにした浮遊移動体の制御システムを提供することを課題とする。
【0020】
さらに本発明は、浮遊移動体の大きさや重さ、或いは加速度センサの性能に依存せず、微小な加速度の変化に至るまで浮遊移動体の加速度を精度良く検出することが可能な、浮遊移動体の加速度検知システムを提供することを課題とする。
【0021】
[課題を解決するための手段]
本発明者は、浮遊移動体を、浮遊移動体の一部を占める、単一剛体とみなし得る本体部と、浮遊移動体に対して推力を発生する効果器部に分離した上、この本体部と効果器部を力学的に結合する唯一の部分(推力伝達ゲート)に力・トルクセンサを設置しておけば、この力・トルクセンサによってセンシングされる値が効果器部から本体部に与えられている推力である故に本体部と効果器部の間に掛かる力とトルクを全て計測出来、この値をフィードバックすれば、浮遊移動体に与える推力を直接制御出来、それによって、波浪や潮流等の流体力学的な外乱下にあっても、浮遊移動体を所定位置に精度良く静止させたり、或いは目標軌道に精度良く追従させたりすることが可能であることを見い出し、本発明を完成した。
【0022】
上記課題を解決可能な本発明の浮遊移動体の制御システムは、(1)浮遊移動体の制御システムであって、前記浮遊移動体が、前記浮遊移動体の一部を占める、単一剛体とみなし得る本体部と、前記浮遊移動体に対して推力を発生する効果器部と、前記本体部と前記効果器部を力学的に結合する部分であり、前記効果器部から前記本体部に作用する推力を実測できるよう構成された推力伝達ゲートと、からなっており、前記推力伝達ゲートからの推力測定値を用いて前記効果器部に対する推力指令を得る様にしたことを特徴とするものである。
【0023】
又本発明は、上記の浮遊移動体の制御システムにおいて更に、(2)前記本体部が、前記本体部の加速度を計測できる加速度測定手段を備え、前記推力伝達ゲートが、前記本体部と前記効果器部の間に掛かる力又はトルクを計測できる力又はトルク測定手段を備えており、オペレータからの目標位置速度軌道指令が変換された目標加速度軌道指令と、前記本体部の前記加速度測定手段からの出力と、前記推力伝達ゲートからの前記力又はトルク測定手段からの出力と、を用いて、前記効果器部に対する推力指令を得る様にしたことを特徴とするものである。
【0024】
なお、後記各実施形態及び実施例で説明する、3次元空間を浮遊する浮遊移動体のほか、斜面上を滑走する浮遊移動体に対して本発明の制御システム及び加速度検知システムを適用する場合には、重力加速度の影響を補償する必要があることから、後記(4)又は(8)に係る発明として規定される通り、本体部の傾きを計測できる傾斜角測定手段を備えることが必要とされる。
しかしながら、そもそも重力の働かない宇宙空間を浮遊する浮遊移動体や、傾きを考慮しなくて良い、或いは無視しても構わない水平面上を滑走する浮遊移動体に対して本発明の制御システム或いは加速度検知システムを適用する際には、上記(2)若しくは次に説明する(3)或いは後記(7)に係る発明として規定される通り、傾斜角測定手段は不要である。
このことは、仮想推力伝達ゲートシステムの考え方が適用される下記(5)に係る発明の場合でも同様であり、3次元空間を浮遊する浮遊移動体等に対して仮想推力伝達ゲートシステムの考え方を用いた本発明の制御システムを適用する際には、後記(6)に係る発明として規定される通り、浮遊移動体の傾きを計測できる傾斜角測定手段を備えることが必要とされる。
【0025】
同様に本発明は、上記の浮遊移動体の制御システムにおいて更に、(3)前記推力伝達ゲートが、前記本体部と前記効果器部の間に掛かる力又はトルクを計測できる力又はトルク測定手段を備えており、さらに、前記本体部が、前記推力伝達ゲートのみに力学的に結合され、かつ、前記本体部に作用する力は実質上全て、前記効果器部及び前記推力伝達ゲートを介して入力される様構成されており、オペレータからの目標位置速度軌道指令が変換された目標加速度軌道指令と、前記推力伝達ゲートからの前記力又はトルク測定手段からの出力と、を用いて、前記効果器部に対する推力指令を得る様にしたことを特徴とするものである。
なお、本体部が推力伝達ゲートのみに力学的に結合され、かつ、本体部に作用する力が実質上全て、効果器部及び推力伝達ゲートを介して入力される様構成された典型的な例としては、上記効果器部が、本体部を包囲する殻状体をなしていることが挙げられる。
しかしながら、その様な構成は極端な例であって、必ずしも本体部が殻状体をなす効果器部で密閉されていることは要件ではない。要するに、この(3)で規定される発明では、本体部に直接、水圧や強い風圧等の外乱その他の外力が加わらない様構成されていれば良く、例えば水上や地上を滑走する浮遊移動体の内部に収容された本体部に空気が触れる程度のことは十分無視し得るレベルのことであり、この(3)に係る発明の適用上、特に問題とされない。
したがって、この(3)で規定される発明は、図3に表された構成に限定されないことが理解される。
【0026】
又本発明は、上記(2)又は(3)の浮遊移動体の制御システムにおいて更に、(4)前記本体部が、前記本体部の傾きを計測できる傾斜角測定手段を備えており、前記効果器部に対する推力指令を、さらに前記本体部の前記傾斜角測定手段からの出力をも利用して得る様にしたことを特徴とするものである。
【0027】
更に本発明は、(5)浮遊移動体の制御システムであって、前記浮遊移動体は、計算上、前記浮遊移動体の一部を占める、単一剛体とみなし得る本体部と、前記浮遊移動体に対して推力を発生する効果器部と、に仮想的に分離されており、
前記浮遊移動体は、前記浮遊移動体の加速度を計測できる加速度測定手段を備えており、
前記加速度測定手段からの出力を用いて仮想推力伝達ゲートからの力又はトルクを計算上で推定し、前記効果器部に対する推力指令を得る様にしたことを特徴とするものである。
又本発明は、上記(5)の浮遊移動体の制御システムにおいて更に、(6)前記浮遊移動体が、前記浮遊移動体の傾きを計測できる傾斜角測定手段を備えており、前記効果器部に対する推力指令を、前記傾斜角測定手段及び前記加速度測定手段からの出力を用いて仮想推力伝達ゲートからの力又はトルクを計算上で推定することによって得る様にしたことを特徴とするものである。
【0028】
なお、後記の通り、上記本発明の浮遊移動体の制御システムでは、浮遊移動体の制御は、目標軌道を実現するために本体部が推力伝達ゲートから受けるべき推力を計算して指令する「推力計画部」と、推力伝達ゲートにおける推力の現在値を効果器部にフィードバックして推力伝達ゲートにおいて指令推力を実現するように効果器部を直接駆動する「推力制御部」の二段階構成で行なわれる。
【0029】
本発明では、上記の機構設計と制御手法を「推力伝達ゲートシステム」と総称する。推力伝達ゲートシステムの実施態様としては主に、
i)本体部を外部に露出させた状態即ち、本体部に直接、水圧や強い風圧等の外乱その他の外力が及ぼされ得る構成とし、さらに推力伝達ゲートを備えた態様[後記第1実施形態、第1例参照]、
ii)推力伝達ゲートを備えると共に、さらに、本体部に直接、水圧や強い風圧等の外乱その他の外力が加わらない様、本体部が推力伝達ゲートのみに力学的に結合され、かつ、本体部に作用する力が実質上全て、効果器部及び推力伝達ゲートを介して入力される様構成(例えば、本体部を効果器部で覆って密閉した状態等)する態様[後記第1実施形態、第2例参照]、
iii)浮遊移動体のハードウェアは変更しないが、計算上、浮遊移動体を本体部と効果器部に分離した上、計算機内に予め本体部の慣性行列を準備しておき、これと浮遊移動体に設けた加速度センサの出力と、さらに、必要に応じて浮遊移動体に設けた傾斜角センサからの出力と、を用いて効果器部に対する推力指令を得る様にした態様[仮想推力伝達ゲート,後記第1実施形態、第3例参照]、の3種類がある。
【0030】
i)とii)では、浮遊移動体のハードウェア設計を推力伝達ゲートシステムのために変更する必要があり、推力伝達ゲートにおいて力・トルクセンサを必要とするが、実現される制御性能は高い。
一方、iii)では、i)とii)の方法と比較すると制御性能は低くなるが、浮遊移動体のハードウェア設計を変える必要がなく、加速度センサ、さらに、必要に応じて傾斜角センサ、を既存の浮遊移動体に付加するだけで推力伝達ゲートシステムを利用でき、制御性能を向上させることが可能である。
特に、航空機等に現在装備されている公知の慣性航法装置は、本発明で使用される加速度センサ、傾斜角センサ、本体部位置・速度センサとしても使用出来、これを利用すれば、ハードウェアに改変を加えなくても、既存の制御よりもより高速高精度化を実現することが可能となる。
【0031】
その他本発明は、(7)浮遊移動体の加速度を検知するためのシステムであって、前記浮遊移動体が、前記浮遊移動体の一部を占める、単一剛体とみなし得る本体部と、前記浮遊移動体に対して推力を発生する効果器部と、前記本体部と前記効果器部を力学的に結合する部分であり、前記効果器部から前記本体部に作用する推力を実測できるよう構成された推力伝達ゲートと、からなっており、前記推力伝達ゲートは、前記本体部と前記効果器部の間に掛かる力又はトルクを計測できる力又はトルク測定手段を備えており、さらに、前記本体部は、前記推力伝達ゲートのみに力学的に結合され、かつ、前記本体部に作用する力は実質上全て、前記効果器部及び前記推力伝達ゲートを介して入力される様構成されており、前記推力伝達ゲートからの前記力又はトルク測定手段からの出力を用いて前記本体部の加速度の推定値を得る様にしたことを特徴とするものである。
又本発明は、上記(7)の浮遊移動体の加速度検知システムにおいて更に、(8)前記本体部が、前記本体部の傾きを計測できる傾斜角測定手段を備えており、前記本体部の加速度の推定値を、さらに前記本体部の前記傾斜角測定手段からの出力をも利用して得る様にしたことを特徴とするものである。
【0032】
本発明によれば、例えば波浪や潮流、風或いは浮遊移動体に取り付けられたロボットアームの反動等による外乱下にあっても、浮遊移動体を所定位置に精度良く静止させたり、或いは目標軌道に精度良く追従させたりすることが出来、浮遊移動体を高速かつ高精度に制御し得るシステム構成を提供することが可能となる。
【0033】
尚以下では、本発明の説明に用いる用語につき定義をおくものとする。
【0034】
「浮遊移動体」とは、水中、空中又は宇宙空間その他を浮遊する各種移動体を言う。また、平面上、或いはレール上を滑走する移動体も浮遊移動体の概念に包含されるものとする。
以下、環境別に具体例を挙げるとすれば、
(1)宇宙: 宇宙ロボット、宇宙船、人工衛星等
(2)空中: 空中ロボット、航空機、ヘリコプター、飛行船等
(3)水中: 水中ロボット、潜水艇等
(4)水上: 船舶、ホバークラフト等
(5)地上: 自動車、鉄道、ホバークラフト、或いは橇、スノーモービルその他の氷上又は雪上を滑走する移動体等
が、本発明に言う「浮遊移動体」の概念に包含される。
自動車等の地上車両については、例えば車輪が滑っている状態では「浮遊移動体」となり得ることが理解される。従って本発明を適用することにより、自動車の場合は車輪が滑っていても滑っていなくても同様に車両制御を行うことが可能となる。
ただし、車輪が滑っている状態では、既存のエンジンとタイヤからなる推力伝達手段が機能し得ない。このことを考慮すると、当該状況下で本発明を適用するに当たっては、上記地上車両が何らかの別異の推力伝達手段(各種形式のスラスタ等)を備えているのが好ましいことが、以下の各説明より理解される。
【0035】
「本体部」とは、浮遊移動体の一部を占める、単一剛体とみなし得る物理構造を言う。原則として本体部には機械的な可動部分はないものとする。又機械的な可動部分があっても、その影響が無視できる程度に小さければ良いほか、無視できなくても、その影響を予測できればこれをキャンセルすることは可能である。
なお、仮想推力伝達ゲートシステムの考え方を適用する本発明中の後記第1実施形態、第3例に相当する発明に於いては、本体部と効果器部の峻別は単に便宜上行っているものに過ぎず、どの部分を本体部と規定するかについては、ユーザーの判断に任せても構わない。従って、実際に後記第1実施形態、第3例に相当する発明を実施するに当たっては、本体部を、浮遊移動体の質量の主な部分を占めるものと規定しても良いほか、反対に、浮遊移動体中の一部に追加設置した物体(例えば、数kg程度の錘等)を本体部と規定することも可能である。
また、仮想推力伝達ゲートシステムの考え方を適用しない後記第1実施形態の第1例及び第2例に相当する発明に於いても、本体部と効果器部とを峻別する判断基準は、両者が次に説明する推力伝達ゲートで結合されているか否かだけであり、それさえ満足されていれば、浮遊移動体中に於ける本体部の占める割合について特に限定はない。
従って本発明に於いては、本体部は、浮遊移動体の質量の主な部分を占める単一剛体で構成された物理構造に限定されない。
【0036】
「効果器部」とは、浮遊移動体に対して推力を発生する物理構造を言う。浮遊移動体の位置や速度等の制御のための推進力発生手段(効果器)のすべてを包含しているものとする。
【0037】
「力学的に結合」とは、対象とする物体間に何らかの力の相互作用が働いている状態を言う。機械的に結合している状態が殆どであるが、機械的に結合していなくても、例えば電磁力等によって非接触な力の相互作用を及ぼしている状態も包含するものとする。
一方、「力学的に分離」とは、対象とする物体間に力の相互作用が働かない状態を言う。
【0038】
「推力伝達ゲート」とは、本体部と効果器部を力学的に結合する唯一の部分であり、本体部と効果器部の間にかかる力とトルクをすべて計測できるセンサを備えることにより、効果器部から本体部に作用するすべての推力を実測できるよう構成されたものを言う。
【0039】
「仮想推力伝達ゲート」とは、浮遊移動体内に物理的に存在せず仮想的に設けられた推力伝達ゲートであり、計算上便宜的に本体部と効果器部に分離された既存の浮遊移動体において、本体部加速度測定値に基づき、本体部と効果器部の間にかかる力とトルク即ち、効果器部から本体部に作用するすべての推力を推定できるよう構成されたものを言う。
なお、本発明に於て単に「推力伝達ゲート」というときには、特に断りがなければ「仮想推力伝達ゲート」も包含するものとする。
【0040】
「仮想推力伝達ゲートにおける推力推定値」とは、物理的に存在する推力伝達ゲートにおいて実測された値ではなく、仮想推力伝達ゲートにおいて加速度センサの測定値(本体部加速度測定値)から計算によって求められる仮想的な値を言う。
【0041】
「推力計画部」とは、目標軌道を実現するために本体部が推力伝達ゲートにおいて効果器部から受けるべき目標推力を計算し、推力制御部に指令する計算機上の一部分或いは機能を言う。
【0042】
「推力制御部」とは、推力伝達ゲートにおける推力測定値もしくは推力推定値をフィードバックして、推力伝達ゲートにおいて所望の目標推力を実現する様、効果器への推力指令を出力する計算機上の一部分或いは機能を言う。
【0043】
「目標推力」とは、目標軌道を実現するために本体部が推力伝達ゲートにおいて効果器部から受けるべき推力を言う。
【0044】
「目標加速度」とは、所望の目標軌道或いは目標速度からの微分演算によって求められる加速度を言う。
【0045】
「推力伝達ゲートシステム(TTGS: Thrust Transfer Gate System)」とは、推力伝達ゲートを備えた浮遊移動体が、推力計画部及び推力制御部を含む制御系によって制御される制御システムを言う。
【0046】
「効果器推力特性」とは、スラスタ等の効果器ヘ入力される効果器推力指令と、実際に出力される効果器推力との関係をいう。尚、スラスタ等の効果器は一般に流体を介して推力を発生するため、効果器推力特性には、効果器そのものの機械的なダイナミクスの他に、流体力学における抗力・揚力の影響などの複雑なダイナミクスが包含されているものとする。
【0047】
[発明の効果]
従来は、浮遊移動体の位置・速度のセンサ情報をフィードバックしてスラスタの推力を制御していたが、本発明では発生した推力を直接フィードバックして効果器の推力を制御するため、原理的に高速な応答が実現される。従って、上記センシング遅延の問題が軽減される。特に、推力伝達ゲートをハードウェアとして備える構成とすれば(第1実施形態の第1例及び第2例等参照)、浮遊移動体の質量が大きくても効果器の推力が小さくてもセンシング遅延の影響が増大することはない。
【0048】
本発明の浮遊移動体の制御システムでは、後記する通り、推力制御部において推力伝達ゲートにおける推力をフィードバックすることにより効果器の推力を制御する。これによって上記の高速な応答が実現されると同時に、推力制御部に非線形性に対してロバストな既存の制御手法(スライディングモード制御等)を適用すれば効果器動特性の非線形性を抑え込み、制御性能への悪影響を防ぐことが可能である。従来、効果器動特性の線形化は、効果器の動特性モデルにもとづくフィードフォワード補償によって行なわれてきたが、多大な手間と計算負荷を要するものであった。本発明の浮遊移動体の制御システムでは、必ずしも動特性モデルを用いない推力フィードバックによって線形化を実現するため、少ない計算負荷で制御システムを構築することが可能である。
【0049】
その他、従来技術の下では上記の通りセンシングを行ってから実際に推力指令をスラスタに与える迄の間に無視できない時間遅れが生じることに起因して、例えば浮遊移動体を所定軌道上に維持するに当たって幾度と無くオーバーシュートやアンダーシュートの是正を繰り返さなければならない等、浮遊移動体に結果的に無駄な動きを与えることが多かった。制御系のゲインチューニングを適切にするための負担も、手間や時間の他、経験を必要とする非常に大きいものであった。
これに対し、本発明によれば、スラスタに真に必要な推力を適時に与えることが出来るため、スラスタや浮遊移動体に無駄な動きを与えることが無い。加えて、浮遊移動体に無駄なスラスタ駆動エネルギーを消費させずに済む結果、浮遊移動体の消費エネルギーを低減できる効果も得られる。
【0050】
また、未知の外乱の影響も、本発明の浮遊移動体の制御システムにおいては推力伝達ゲートにおける推力の誤差として直ちにセンシングされ、推力制御部のローカルフィードバック制御によって補償されるため、推力計画部より上のレベルでは外乱に関して考慮する必要がない。すなわち、推力計画部では「静止せよ」「直進せよ」等の指示を行なうだけで、推力制御部で自動的に外乱の影響を補償して、予め計画された所望の動作を実現することが可能である。また、従来のように外乱の影響を位置・速度誤差によってセンシングする場合と比較して、高速な応答が実現される。
【0051】
他方、本発明の浮遊移動体の加速度検知システムによれば、予め同定しておいた本体部の慣性行列と、推力伝達ゲートからの力・トルクセンサからの出力と、さらに、必要に応じて本体部の傾斜角センサからの出力と、を用いて本体部の加速度推定値を求めることが出来るので、浮遊移動体の重さや大きさに関係なく、或いは加速度センサの性能に依存することなく、微小な加速度の変化に至るまで浮遊移動体の加速度を精度良く検出することが出来、慣性航法を用いて浮遊移動体を誘導する際に、浮遊移動体を所望の軌道上に正確に誘導することが可能となる。
以下、本発明につきより詳細に説明する。
【図面の簡単な説明】
【0052】
【図1】本発明の制御システムが適用された浮遊移動体の一例を示す概略図である。
【図2】本発明の制御システムの一構成例を示すブロック図である。
【図3】本発明の制御システムが適用された浮遊移動体の別の例を示す概略図である。
【図4】本発明の制御システムの別の構成例を示すブロック図である。
【図5】本発明の制御システムが適用された浮遊移動体の別の例を示す概略図である。
【図6】本発明の制御システムの別の構成例を示すブロック図である。
【図7】本発明の制御システムの第2実施形態を示すブロック図である。
【図8】本発明の制御システムの一実施例を示すブロック図である。
【図9】本発明の制御システムの別の実施例を示すブロック図である。
【図10】本発明の制御システムの別の実施例を示すブロック図である。
【図11】従来知られた浮遊移動体の概要を示す図である。
【図12】従来知られた浮遊移動体の制御系を示すブロック図である。
【図13】従来知られた浮遊移動体の制御系を示す別のブロック図である。
【図14】従来知られた浮遊移動体の制御系を示す別のブロック図である。
【符号の説明】
【0053】
A ロボットアーム部
B 本体部
B1 本体部静力学
B2 本体部慣性
B2’ 本体部慣性
B3 本体部慣性
B4 積分
C ケーブル
E 効果器部
E1 効果器推力特性
E2 効果器部静力学
G 推力伝達ゲート
G’ 仮想推力伝達ゲート
H オペレータ
P 計算機
P1 微分
P2 推力計画部
P3 推力制御部
P4 位置制御・速度制御
S 耐圧殻
T スラスタ
1 制御システム
2 加速度センサ
3 傾斜角センサ
4 位置・速度センサ
5 空洞
10 浮遊移動体
11 減算回路
12 スラスタ制御回路
13 センサ
14 効果器推力特性
15 浮遊移動体ダイナミクス
16 ロボットアームの反力・トルク
17 加算回路
18 スラスタ・ロボットアーム制御回路
19 関節アクチュエータ
20 浮遊移動体本体・ロボットアーム複合系ダイナミクス
21 減算回路
【発明を実施するための形態】
【0054】
[第1実施形態]
以下では、本発明を実施する対象の代表例たる水中ロボットにおける、本発明の一実施形態とその作用を、図1〜図7に基づき説明する。水中ロボットは、3次元空間を浮遊する浮遊移動体にほかならず、この水中ロボットに対して本発明の制御システムを適用する場合には、重力加速度の影響を補償する必要があることから、上記(4)に係る発明として規定される通り、本体部の傾きを計測できる傾斜角測定手段を備えることが必要とされる。このことは、仮想推力伝達ゲートシステムの考え方が適用される上記(5)に係る発明の場合でも同様であり、3次元空間を浮遊する浮遊移動体等に対して仮想推力伝達ゲートシステムの考え方を用いた本発明の制御システムを適用する際には、上記(6)に係る発明として規定される通り、浮遊移動体の傾きを計測できる傾斜角測定手段を備えることが必要とされる。
上記(4)に係る発明である、傾斜角測定手段を更に備えた上記(2)又は(3)に係る発明は、それぞれこの第1実施形態の第1例又は第2例に、並びに後記実施例1又は実施例2に相当する。又上記(6)に係る発明は、この第1実施形態の第3例及び後記実施例3に相当する。
尚適用対象が水中ロボットに限られないことは、上記定義欄及び後記実施例中の記載からも十分理解される。又水中ロボットと言えども、図中に表された深海探査等に用いる典型的な例に限られず、水難救助や油田探索等にも使用し得る比較的小型のものも、この水中ロボットの範疇に含まれる。
ここで、図1は本発明の制御システムが適用された浮遊移動体の一例を示す概略図、図2は本発明の制御システムの一構成例を示すブロック図である。また図3及び図5は本発明の制御システムが適用された浮遊移動体の別の例を示す概略図、図4及び図6は本発明の制御システムの別の構成例を示すブロック図である。図7は、本発明の制御システムの第2実施形態を示すブロック図である。
上記図1〜7においては、先に示した図11〜14と同一のものには同一符号を付して説明するものとする。
図2、4、6及び7中、各ブロック間を繋ぐ細実線は信号を表すものである。一方、浮遊移動体10内における各ブロック間を繋ぐ二重線は、物理的な作用として浮遊移動体10に起きている現象(力学的な変換作用等)を表すものである。
【0055】
ところで、以下詳細を述べる本発明の説明に当たっては、慣性系の絶対位置に固定された基準座標系をΣとし、浮遊移動体の重心位置に原点を持ち、浮遊移動体本体部に固定された本体座標系をΣとする。又特に断らない限り、ベクトル及び行列は、本体座標系Σから見た表現であるとする。さらに、浮遊移動体はm次元空間(m≦6)に存在し、浮遊移動体の効果器はn自由度(m≦n)の推力を発生可能とする。
【0056】
第1例
本実施形態に係る推力伝達ゲートシステムの第1例の構成は図1及び2のようになる。図1は浮遊移動体の概略図、図2は制御システムのブロック図である。
本例においては、浮遊移動体10は本体部Bと効果器部Eに力学的に分離されており、本体部Bと効果器部Eの間には、推力伝達ゲートGがハードウェアとして備えられている。又本体部Bには加速度センサ2と傾斜角センサ3が、推力伝達ゲートGには力・トルクセンサが備えられている。本例では、本体部Bと推力伝達ゲートGは効果器部E同様、外部に露出した構成となっている。
なお、後述する本体部Bの慣性行列Mと浮遊移動体10の幾何学的構造を表現するJGB、JEGは、あらかじめ測定され既知の行列として与えられているものとする。
【0057】
以下、本例の構成に付き詳細に説明する。
図2の本体部B及び効果器部Eの内部を参照すると、浮遊移動体の物理ダイナミクスを表わす運動方程式は一般に次式のように書ける。
【0058】
【数1】

【0059】
なお、本例の推力伝達ゲートシステム1においては本体部Bの単一剛体構造上に推力伝達ゲートGを固定した構造としている。これによって、ヤコビ行列JGBと慣性行列Mは定数行列となる。
【0060】
次に、本例の推力計画部につき説明する。
図2の計算機P内の推力計画部P2のブロック部分に示される通り、本体部Bの目標加速度aBd(t)を実現するために必要な推力伝達ゲートGにおける目標推力uGd(t)を次式の様に定める。
【数2】

【0061】
ただしaBd(t)、^a(t)[m次元ベクトル]はそれぞれ本体加速度a(t) の目標値と推定値、uGd(t)[m次元ベクトル]はゲート推力u(t)の目標値、uGs(t)[m次元ベクトル]は推力伝達ゲートに備えられた力・トルクセンサによるu(t)の測定値である。
【0062】
ここで、図2の計算機P内において参照符P3として示される推力制御部において、このuGd(t)を推力フィードバックによって実現するようにu(t)を制御することで、推力フィードバックによって推力を制御する、遅れの少ないフィードバック制御系が構築できる。
推力制御部P3においては、図2の効果器部E内において参照符E1として示される効果器推力特性や効果器部に対する外乱や動的な力f(t)などの効果器部動特性の未知の非線形性を抑え込むために、非線形系に対して優れたロバスト性を発揮するような制御手法を適用すべきである。例えば既存のスライディングモード制御を適用すると、推力制御部P3は以下のように構築できる。
【0063】
本例における推力制御部P3においては、目標推力uGd(t)を実現するために必要な効果器への推力指令uEd(t)を次式のように定める。
【0064】
【数3】

【0065】
本例において、推力計画部、推力制御部へのフィードバックに必要な値はuGs(t)と^a(t) である。ゲート推力測定値uGs(t)には、推力伝達ゲートに備えられた力・トルクセンサによる測定値がそのまま用いられる。
【0066】
以下では、本体部加速度a(t)の推定方法について述べる。センサ特性、推力制御特性について考察するために、以下では主に周波数領域で議論を展開する。
【0067】
まず、図2の本体部B内において参照符2で示される本体部加速度センサによって得られる加速度の測定値aBs(t)[m次元ベクトル]は以下のように与えられる。この測定値には重力加速度が含まれているため、本体加速度a(t)の推定値としてそのまま使用することはできない。
【0068】
【数4】

【0069】
尚、*(s)はベクトルあるいは行列* のラプラス変換*(s)=L[*(t)]、GaBs(s)は本体部加速度センサ特性を表わすm行m列の伝達関数行列である。
同様に、本体部傾斜角センサによって得られる重力加速度の測定値gBs(t)[m次元ベクトル]は以下のように与えられる。
【0070】
【数5】

【0071】
ただしGgBs(s)は本体部傾斜角センサ特性を表わすm行m列の伝達関数行列である。
同様に、推力伝達ゲートによって得られるゲート推力測定値uGs(t)[m次元ベクトル]は以下のように与えられる。
【0072】
【数6】

【0073】
ただしGuGs(s)は推力伝達ゲートの力・トルクセンサ特性を表わすm行m列の伝達関数行列である。
【0074】
次に、本例における本体加速度a(t)の推定につき説明する。
本体加速度a(t)の推定値^a(t)を次式のように定める。
【0075】
【数7】

【0076】
ここで、上記本体加速度推定の妥当性について考える。この式(9)をラプラス変換して上の(6)、(7)式を代入すると、
・ ^a(s)=GaBs(s)(a(s)−g(s))
+GgBs(s)g(s) (9)’
になるところ、「すべての種類、すべての自由度におけるセンサ特性は同一であり、各センサ、各自由度間の干渉も存在しない(センサ特性一致条件)」という上記の条件が満たされている場合には、本体部加速度センサ特性と本体部傾斜角センサ特性と、推力伝達ゲートの力・トルクセンサ特性について、
・ GaBs(s)=GgBs(s)=GuGs(s)=G(s)I (10)
としてよい。ただしG(s)は同一センサ特性を表わす伝達関数、Iは単位行列である。すなわち、
・ aBs(s)=G(s)(a(s)−g(s)) (11)
・ gBs(s)=G(s)g(s) (12)
・ uGs(s)=G(s)u(s) (13)
である。このとき、式(9)’は、
【0077】
【数8】

【0078】
となり、式(8)によって本体部加速度a(t)の推定が行なえている。式(14)に重力加速度gBs(t)は陽に表われてこないことから、上記センサ特性一致条件が満たされている限り本体部加速度a(t)を直接測定していると見なしてよいことになる。
なお、上記センサ特性の一致条件は、例えば浮遊移動体のダイナミクスと比較して十分高速な応答で、かつ高精度なセンサを選択することで実現される。
【0079】
第2例
次に、本実施形態に係る推力伝達ゲートシステムの第2例につき説明する。
推力伝達ゲートシステムの第2例の構成は図3、4のようになる。図3は浮遊移動体の概略図、図4は制御システムのブロック図である。
本例においても、浮遊移動体10は本体部Bと効果器部Eに力学的に分離されており、本体部Bと効果器部Eの間には、推力伝達ゲートGがハードウェアとして備えられている。又本体部Bには傾斜角センサ3が、推力伝達ゲートGには力・トルクセンサが備えられている。
第1例と大きく異なるのは、本例では効果器部Eが本体部Bを覆う殻(図3に示す耐圧殻S)になっており、本体部Bと推力伝達ゲートGはこの殻Sによって完全に外界から隔離されている点である。この構造によって、第1例で必要であった本体部Bの加速度センサ2が不要となっている。
第1例同様、本体部の慣性行列Mと浮遊移動体の幾何学的構造を表現するJGB、JEGは、あらかじめ測定され既知の行列として与えられているものとする。
【0080】
以下、本例の構成に付き詳細に説明する。
図4の本体部B及び効果器部Eの内部を参照すると、浮遊移動体の物理ダイナミクスを表わす運動方程式は一般に次式のように書ける。
【0081】
【数9】

【0082】
式(15)は本体部、式(16)は効果器部を表わす式である。ここで、新たにuG2(t)[m次元ベクトル]を次式のように定義する。
【0083】
【数10】

【0084】
次に、本例の推力計画部につき説明する。
図4の計算機P内の推力計画部P2のブロック部分に示される通り、本体部Bの目標加速度aBd(t)を実現するために必要な推力伝達ゲートGにおける目標推力uG2d(t)を次式の様に定める。
【0085】
【数11】

【0086】
図4の計算機P内において参照符P3として示される推力制御部において、このuG2d(t)を推力フィードバックによって実現するようにuG2(t)を制御することで、推力フィードバックによって推力を制御する、遅れの少ないフィードバック制御系が構築できる。
ここで、本例の推力制御につき例えばスライディングモード制御を適用すると、推力制御部P3は以下のように構築できる。
【0087】
本例における推力制御部P3においては、目標推力uG2d(t)を実現するために必要な効果器への推力指令uEd(t)を次式のように定める。
【0088】
【数12】

【0089】
ただし^uG2(t)[m次元ベクトル]はuG2(t)の推定値である。
【0090】
ところで、本例においては、推力計画部P2へのフィードバックは必要なく、推力制御部P3へのフィードバックに必要な値は式(17)のuG2(t)の推定値^uG2(t)のみである。ここでは、このuG2(t)の推定方法について説明する。
【0091】
以下、本例におけるゲート推力uG2(t)の推定につき説明する。
ゲート推力uG2(t)の推定値^uG2(t)を次式のように定める。
【0092】
【数13】

【0093】
ここで、上記ゲート推力推定の妥当性について考える。式(21)をラプラス変換して、
^uG2(s)=uGs(s)+JGB−TBs(s) (21)’
ここでも前述のセンサ特性一致条件が満足されているとすると、式(12),(13)が同様に成立する。これらを代入して式(17)を考慮すると、
【0094】
【数14】

【0095】
となり、式(21) によってゲート推力uG2(t)の推定が行なえている。式(22)に重力加速度gBs(t)は陽に表われてこないことから、センサ特性一致条件が満たされている限りゲート推力uG2(t)を直接測定していると見なしてよいことになる。
【0096】
第3例
次に、本実施形態に係る推力伝達ゲートシステムの第3例につき説明する。
推力伝達ゲートシステムの第3例の構成は図5、6のようになる。図5は浮遊移動体の概略図、図6は制御システムのブロック図である。
この第3例においては、浮遊移動体10は本体部Bと効果器部Eに力学的に分離されておらず、推力伝達ゲートGもハードウェアとして備えられていない。しかしながら、本例の浮遊移動体10には加速度センサ2と傾斜角センサ3が備えられている。
本例では、本体部Bの慣性行列Mを適当に定め、このMにもとづいて、図6の計算機P内に示された仮想推力伝達ゲートG’における推力推定値をセンサの測定値から計算することで、推力伝達ゲートが存在するものとして推力伝達ゲートシステムを構築する。浮遊移動体10の幾何学的構造を表現するJGB、JEGについては、上記第1例、第2例同様、予め測定され既知の行列として与えられているものとする。
尚、本体部Bの慣性行列Mの定め方としては、例えば、浮遊移動体10設計時における設計図、主要諸元その他から計算機によって本体部Bの慣性行列Mを予め割り出し、これを不図示のメモリ等に格納しておく手法等が挙げられる。
【0097】
以下、本例の構成につき詳細に説明する。
図6の本体部B及び効果器部Eの内部を参照すると、浮遊移動体の物理ダイナミクスを表わす運動方程式は一般に次式の様に書ける。
【0098】
【数15】

【0099】
式(23)は本体部、式(24)は効果器部を表わす式である。これは第2例の場合とほぼ同じであるが、この第3例では推力伝達ゲートは物理的には存在しないため、推力伝達ゲートの推力uGv[m次元ベクトル]は仮想的な値であることに注意されたい。
【0100】
次に、本例の推力計画部につき説明する。
図6の計算機P内の推力計画部P2のブロック部分に示される通り、本体部Bの目標加速度aBd(t)を実現するために必要な仮想推力伝達ゲートG’における目標推力uGvd(t)を次式の様に定める。
【0101】
【数16】

【0102】
図6の計算機P内において参照符P3として示される推力制御部において、このuGvd(t)を推力フィードバックによって実現するようにuGv(t)を制御することで、推力フィードバックによって推力を制御する、遅れの少ないフィードバック制御系が構築できる。
ここで、本例の推力制御につき例えばスライディングモード制御を適用すると、推力制御部P3は以下のように構築できる。
【0103】
本例における推力制御部P3においては、目標推力uGvd(t)を実現するために必要な効果器への推力指令uEd(t)を次式の様に定める。
【0104】
【数17】

【0105】
ただし^uGv(t)[m次元ベクトル]はuGv(t)の推定値である。
【0106】
ところで、本例においては、上記第2例と同様に推力計画部P2へのフィードバックは必要なく、推力制御部P3へのフィードバックに必要な値はuGv(t)の推定値^uGv(t)のみである。ここでは、このuGv(t)の推定方法について説明する。
【0107】
以下、本例におけるゲート推力uGv(t)の推定につき説明する。
ゲート推力uGv(t)の推定値^uGv(t)を次式のように定める。
【0108】
【数18】

【0109】
ここで、上記ゲート推力推定の妥当性について考える。式(28)をラプラス変換して、
・ ^uGv(s)=JGB−T(aBs(s)+gBs(s)) (28)’
ここでも前述のセンサ特性一致条件が満足されているとすると、式(11),(12)が同様に成立する。これらを代入して式(23)を考慮すると、
【0110】
【数19】

【0111】
となり、式(28) によってゲート推力uGv(t)の推定が行なえている。式(29)に重力加速度gBs(t)は陽に表われてこないことから、センサ特性一致条件が満たされている限りゲート推力uGv(t)を直接測定していると見なしてよいことになる。
【0112】
[第2実施形態]
次に、本発明の浮遊移動体の加速度検知システムの一実施形態に付き説明する。第1実施形態同様、本実施形態についても適用対象を水中ロボットとして以下説明する。尚水中ロボットは、3次元空間を浮遊する浮遊移動体にほかならず、この水中ロボットに対して本発明の加速度検知システムを適用する場合には第1実施形態同様、重力加速度の影響を補償する必要があることから、上記(8)に係る発明として規定される通り、本体部の傾きを計測できる傾斜角測定手段を備えることが必要とされる。
本実施形態では、推力伝達ゲートを利用して本体部加速度の推定を行なっている。本実施形態に係る加速度検知システムの構成は図3及び図7の様になる。図3は浮遊移動体の概略図、図7は加速度検知システムのブロック図である。
【0113】
これまでの第1実施形態の各例では、浮遊移動体10の制御システム1として推力伝達ゲートシステムを構築したのに対して、第2実施形態では浮遊移動体10の高精度な加速度センシングシステムとして推力伝達ゲートGを利用する。
【0114】
本実施形態のハードウェア構成は先の第1実施形態に係る第2例(図3)とまったく同じである。すなわち、浮遊移動体10は本体部Bと効果器部Eに力学的に分離されており、本体部Bには傾斜角センサ3が、推力伝達ゲートGには力・トルクセンサが備えられている。
ただし、本実施形態では本体部加速度a(t)を高精度に推定することを目的としており、制御を目的としないので、推力計画部と推力制御部は本実施例の中には含まれない。
また、本体部の慣性行列Mと浮遊移動体の幾何学的構造を表現するJGBは、あらかじめ測定され既知の定数行列として与えられているものとする。この本体部慣性行列Mを、推力伝達ゲートの力・トルクセンサの測定レンジと、本実施形態にて検知すべき加速度レンジとを勘案して適切に設計することにより、高精度な加速度測定が可能となる。
【0115】
以下、本実施形態の構成につき詳細に説明する。
図3及び図7に示す本実施形態のハードウェア構成においては、本体部加速度a(t)は、式(15)を変形して以下の様に表わされる。
【0116】
【数20】

【0117】
次に、本実施形態における本体部加速度a(t)の推定につき説明する。
本体部加速度a(t)の推定値^a(t)を次式の様に定める。
【0118】
【数21】

【0119】
ここで、上記本体加速度推定の妥当性について考える。式(31)をラプラス変換して、
【0120】
【数22】

【0121】
ここでも前述のセンサ特性一致条件が満足されているとすると、式(12)、(13)が第1実施形態と同様に成立する。これらを代入して式(30)を考慮すると、
【0122】
【数23】

【0123】
となり、式(31)によって本体加速度a(t)の推定が行なえていることがわかる。
【実施例1】
【0124】
上記第1実施形態の欄で説明した通り、本発明の浮遊移動体の制御システムでは推力制御を行なうため、実現されるのは厳密には位置・速度軌道ではなく加速度の軌道である。ここで、加速度の積分誤差に起因するドリフトの影響は不可避であり、従って、本発明が単独で顕著に効果を発揮するのは、比較的高い周波数帯域の運動に対してである。低周波のゆっくりとした運動に対しては、従来の位置・速度をフィードバックする制御手法を併用する必要があるが、この場合であっても、従来手法単独で用いるより精度を向上させることが可能である。
以下、本発明の一実施例として、上記第1実施形態で説明した本発明の浮遊移動体の制御システムの各例に従来の位置・速度フィードバック制御を併せ備えた制御システムにつき説明する。
【0125】
ここで、図8は本発明の制御システムの一実施例を示すブロック図であって上記第1実施形態の第1例に相当するもの、図9は本発明の制御システムの別の実施例を示すブロック図であって上記第1実施形態の第2例に相当するもの、図10は本発明の制御システムの別の実施例を示すブロック図であって上記第1実施形態の第3例に相当するものである。上記図8〜10においては、先に示した図1〜7及び図11〜14と同一のものには同一符号を付して説明するものとする。
又図2、4、6及び7と同様に、図8〜10中各ブロック間を繋ぐ細実線は信号を表すものである。一方、浮遊移動体10内における各ブロック間を繋ぐ二重線は、物理的な作用として浮遊移動体10に起きている現象(力学的な変換作用等)を表すものである。
【0126】
実施例1は、先の第1実施形態、第1例に相当するものである。図8に、本実施例に係る制御システム1のブロック図を示す。本実施例の浮遊移動体10の概略構成は先の図1に示す通りである。
図8のブロック図からも明らかな通り、本実施例は、上記第1実施形態の第1例で説明した図2に示す構成を従来知られた位置・速度フィードバック制御システムに適用したものである。従って本発明の推力伝達ゲートシステム部分の構成及び制御系の動作等は先の図1及び図2に基づいて説明される。
【0127】
本実施例に係る制御システム1即ち推力伝達ゲートシステムは、参照符10で示される浮遊移動体の機構を、推力を受ける本体部Bと推力を発生する効果器部Eとに力学的に分離した上、その結合部分に推力伝達ゲートGを付加したことをその主たる特徴とするものである。ここで、推力伝達ゲートGは、本体部Bと効果器部Eを結合する唯一の部分であり、本体部Bと効果器部Eの間にかかる力とトルクを全て計測できるセンサ(力・トルクセンサ)を有している。尚力・トルクセンサについては、市販の汎用製品(多次元の力やトルク成分を出力させ得る多チャンネル品等)を使用し得る。
この様に、本実施例では、本体部Bと効果器部Eとを推力伝達ゲートGで結合することにより、効果器部Eから本体部Bに作用する全ての推力を実測出来る様構成されている。図1に示す通り、推力伝達ゲートシステムが適用された本実施例の浮遊移動体10では、本体部Bに加速度センサ2と傾斜角センサ3が備えられている。
【0128】
上記構成を有する本実施例に係る制御システム1では、従来知られた位置・速度フィードバック制御と同時に、本発明の推力伝達ゲートシステムによる推力フィードバック制御が行なわれる。
【0129】
本実施例によれば、従来の位置・速度をフィードバックする制御手法を併用して加速度の積分誤差に起因するドリフトの影響を回避しつつ、本発明の制御システム即ち推力伝達ゲートシステムを用いて例えば波浪や潮流、風或いは浮遊移動体に取り付けられたロボットアームの反動等による外乱下にあっても、浮遊移動体を所定位置に精度良く静止させたり、或いは目標軌道に精度良く追従させたりすることが出来るため、浮遊移動体をより高速かつ高精度に制御することが可能となる。
【実施例2】
【0130】
本実施例は、先の第1実施形態、第2例に相当するものである。図9に、本実施例に係る制御システム1のブロック図を示す。本実施例の浮遊移動体10の概略構成は先の図3に示す通りである。
実施例1同様、本実施例に係る制御システム1も、水中ロボット10の機構を、推力を受ける本体部Bと推力を発生する効果器部Eとに力学的に分離した上、その結合部分に推力伝達ゲートGを付加したことをその主たる特徴とするものである。
但し、本実施例では、本体部Bは効果器部Eの内部に格納されており、外部からの外乱やロボットアームAの反動等は全て、耐圧殻Sたる効果器部Eに及ぼされる構造となっている点が前記実施例1と異なる。ここで、本体部Bは効果器部Eの内部において、推力伝達ゲートGのみを介して担われている。それ故、推力伝達ゲートGが、本体部Bと効果器部Eを結合する唯一の部分である点は前記実施例1と同様である。
従って本実施例においても、本体部Bと効果器部Eとを推力伝達ゲートGで結合することにより、効果器部Eから本体部Bに作用する全ての推力を実測出来る様構成されている。
【0131】
この実施例2によっても、従来の位置・速度をフィードバックする制御手法を併用して加速度の積分誤差に起因するドリフトの影響を回避しつつ、本発明の制御システム即ち推力伝達ゲートシステムを用いて例えば波浪や潮流、風或いは浮遊移動体に取り付けられたロボットアームの反動等による外乱下にあっても、浮遊移動体を所定位置に精度良く静止させたり、或いは目標軌道に精度良く追従させたりすることが出来るため、浮遊移動体をより高速かつ高精度に制御することが可能となる。
【実施例3】
【0132】
本実施例は、先の第1実施形態、第3例に相当するものである。図10に、本実施例に係る制御システム1のブロック図を示す。本実施例の浮遊移動体10の概略構成は先の図5に示す通りである。
本実施例に係る制御システム1は、水中ロボットの機構を、推力を受ける本体部Bと推力を発生する効果器部Eとに計算上分離した上、両者の間に仮想推力伝達ゲートG’を仮想的に設けたことをその主たる特徴とするものである。
なお、仮想推力伝達ゲートG’では、本体部Bと効果器部Eの間にかかる力とトルクを全て推定できる様になっている。
【0133】
この実施例3によっても、従来の位置・速度をフィードバックする制御手法を併用して加速度の積分誤差に起因するドリフトの影響を回避しつつ、本発明の制御システム即ち推力伝達ゲートシステムを用いて例えば波浪や潮流、風或いは浮遊移動体に取り付けられたロボットアームの反動等による外乱下にあっても、浮遊移動体を所定位置に精度良く静止させたり、或いは目標軌道に精度良く追従させたりすることが出来るため、浮遊移動体をより高速かつ高精度に制御することが可能となる。
【0134】
[慣性航法装置を利用した実装]
ところで、航空機等の浮遊移動体に広く利用されている、既存の技術である慣性航法装置は、本発明で使用される加速度センサ、傾斜角センサ、本体部位置・速度センサとして使用することが可能である。
すなわち、慣性航法装置を搭載した浮遊移動体であれば、一切のハードウェアの追加、変更なしに、ソフトウェアの変更のみで、例えばこの実施例3或いは上記第1実施形態の第3例に係る本発明の制御システムを実装することが可能であり、それ故既存の浮遊移動体の高速高精度化をより容易に実現することが可能となる。
より詳しく説明すると、既存の慣性航法装置では、加速度を十分高精度に計測しているのにも関わらず、それを積分することにより得た位置・速度情報のみを制御に使っているのが現状である。しかるに、今までそのままでは利用されていなかった加速度情報を、本発明の推力伝達ゲートシステムの枠組みの中で推力測定に利用することで、ハードウェアは従来と同じ儘で、既存の制御よりもより高速高精度化を実現することが可能となる。
特に、上記慣性航法装置については最近では加速度センサとジャイロ、GPS(Global Positioning System)その他のセンサをユニット化して加速度、速度及び変位情報を得る、ハイブリッドタイプの慣性航法装置が主流となってきており、総合的に見ても計測速度が向上している。それ故、上記慣性航法装置は本発明の制御システムを実装する際に利用し得る好適なセンサユニットということができる。
【0135】
尚本発明は、上記各例記載の構成に限定されず、種々の設計変更その他の変形が可能である。
はじめに、上記各例では、水中ロボットに本発明の制御システムを適用したものにつき説明したが、本発明の制御システムが適用される対象としては水中ロボットに限られない。本発明の制御システムは、水中ロボットの他にも、上記定義欄で例示した通り、潜水艇、ヘリコプター、飛行船、航空機、宇宙船又は宇宙ロボット等の水中、空中又は宇宙空間その他における浮遊移動体に適用できる。その他、本発明の制御システムは、水上や地上を滑走するホバークラフトや、車輪が滑っている状態の自動車等、平面上を滑走する各種移動体にも適用可能である。
さらに、上記実施形態の各例及び上記各実施例では、3次元空間を浮遊する浮遊移動体に対して本発明の制御システム或いは加速度検知システムを適用した場合について説明を行った。しかしながら、そもそも重力の働かない宇宙空間を浮遊する浮遊移動体や、傾きを考慮しなくて良い、或いは無視しても構わない水平面上を滑走する浮遊移動体に対して本発明の制御システム或いは加速度検知システムを適用する際には、上記(2)、(3)、(5)或いは(7)に係る発明として規定される通り傾斜角測定手段は不要であり、このとき、推力計画部、推力制御部においても、浮遊移動体本体部(重心)にかかる重力加速度g=0とすることで、より簡単な制御則とすることができる。
又上記各例では、推力制御部P3の制御則の一例として、効果器動特性の非線形性に対してロバストな既存の制御手法たるスライディングモード制御を用いたが、推力制御部P3の制御則はこれに限定されない。従って、例えば推力制御部P3の制御則として、推力伝達ゲートGにおける目標推力と推力伝達ゲートGにおける推力測定又は推定値の推力誤差に対するスラスタT入力電圧のPID制御を適用することも可能である。
さらに、上記各例では、スラスタTとして、駆動源たるモータに機械的に連結されたプロペラによる推力発生機器を用いたが、スラスタTの構成はこれに限定されない。従って、スラスタTはジェット噴射式のものであっても良いし、またその駆動源もモータに限らず内燃機関や蒸気タービン、原子力タービン等種々の型式のものを採用し得る。
【0136】
以上に詳述した通り、本発明は、例えば波浪や潮流、風或いは浮遊移動体に取り付けられたロボットアームの反動等による外乱下にあっても、浮遊移動体を所定位置に精度良く静止させたり、或いは目標軌道に精度良く追従させたりすることが出来、浮遊移動体を高速かつ高精度に制御し得るシステム構成を提供する、新規かつ極めて有用なる発明であることが明らかである。

【特許請求の範囲】
【請求項1】
浮遊移動体の制御システムであって、
前記浮遊移動体が、
前記浮遊移動体の一部を占める、単一剛体とみなし得る本体部と、
前記浮遊移動体に対して推力を発生する効果器部と、
前記本体部と前記効果器部を力学的に結合する部分であり、前記効果器部から前記本体部に作用する推力を実測できるよう構成された推力伝達ゲートと、からなっており、
前記推力伝達ゲートからの推力測定値を用いて前記効果器部に対する推力指令を得る様にしたことを特徴とする浮遊移動体の制御システム。
【請求項2】
前記本体部は、前記本体部の加速度を計測できる加速度測定手段を備え、
前記推力伝達ゲートは、前記本体部と前記効果器部の間に掛かる力又はトルクを計測できる力又はトルク測定手段を備えており、
オペレータからの目標位置速度軌道指令が変換された目標加速度軌道指令と、
前記本体部の前記加速度測定手段からの出力と、
前記推力伝達ゲートからの前記力又はトルク測定手段からの出力と、を用いて、前記効果器部に対する推力指令を得る様にしたことを特徴とする請求項1に記載の浮遊移動体の制御システム。
【請求項3】
前記推力伝達ゲートは、前記本体部と前記効果器部の間に掛かる力又はトルクを計測できる力又はトルク測定手段を備えており、さらに、
前記本体部は、前記推力伝達ゲートのみに力学的に結合され、かつ、前記本体部に作用する力は実質上全て、前記効果器部及び前記推力伝達ゲートを介して入力される様構成されており、
オペレータからの目標位置速度軌道指令が変換された目標加速度軌道指令と、
前記推力伝達ゲートからの前記力又はトルク測定手段からの出力と、を用いて、前記効果器部に対する推力指令を得る様にしたことを特徴とする請求項1に記載の浮遊移動体の制御システム。
【請求項4】
さらに、前記本体部が、前記本体部の傾きを計測できる傾斜角測定手段を備えており、
前記効果器部に対する推力指令を、さらに前記本体部の前記傾斜角測定手段からの出力をも利用して得る様にしたことを特徴とする請求項2又は3に記載の浮遊移動体の制御システム。
【請求項5】
浮遊移動体の制御システムであって、
前記浮遊移動体は、計算上、
前記浮遊移動体の一部を占める、単一剛体とみなし得る本体部と、
前記浮遊移動体に対して推力を発生する効果器部と、に仮想的に分離されており、
前記浮遊移動体は、前記浮遊移動体の加速度を計測できる加速度測定手段を備えており、
前記加速度測定手段からの出力を用いて仮想推力伝達ゲートからの力又はトルクを計算上で推定し、前記効果器部に対する推力指令を得る様にしたことを特徴とする浮遊移動体の制御システム。
【請求項6】
さらに、前記浮遊移動体が、前記浮遊移動体の傾きを計測できる傾斜角測定手段を備えており、
前記効果器部に対する推力指令を、前記傾斜角測定手段及び前記加速度測定手段からの出力を用いて仮想推力伝達ゲートからの力又はトルクを計算上で推定することによって得る様にしたことを特徴とする請求項5に記載の浮遊移動体の制御システム。
【請求項7】
浮遊移動体の加速度を検知するためのシステムであって、
前記浮遊移動体は、
前記浮遊移動体の一部を占める、単一剛体とみなし得る本体部と、
前記浮遊移動体に対して推力を発生する効果器部と、
前記本体部と前記効果器部を力学的に結合する部分であり、前記効果器部から前記本体部に作用する推力を実測できるよう構成された推力伝達ゲートと、からなっており、
前記推力伝達ゲートは、前記本体部と前記効果器部の間に掛かる力又はトルクを計測できる力又はトルク測定手段を備えており、さらに、
前記本体部は、前記推力伝達ゲートのみに力学的に結合され、かつ、前記本体部に作用する力は実質上全て、前記効果器部及び前記推力伝達ゲートを介して入力される様構成されており、
前記推力伝達ゲートからの前記力又はトルク測定手段からの出力を用いて前記本体部の加速度の推定値を得る様にしたことを特徴とする浮遊移動体の加速度検知システム。
【請求項8】
さらに、前記本体部が、前記本体部の傾きを計測できる傾斜角測定手段を備えており、
前記本体部の加速度の推定値を、さらに前記本体部の前記傾斜角測定手段からの出力をも利用して得る様にしたことを特徴とする請求項7に記載の浮遊移動体の加速度検知システム。

【図1】
image rotate

【図2】
image rotate

【図3】
image rotate

【図4】
image rotate

【図5】
image rotate

【図6】
image rotate

【図7】
image rotate

【図8】
image rotate

【図9】
image rotate

【図10】
image rotate

【図11】
image rotate

【図12】
image rotate

【図13】
image rotate

【図14】
image rotate


【国際公開番号】WO2005/081082
【国際公開日】平成17年9月1日(2005.9.1)
【発行日】平成19年11月22日(2007.11.22)
【国際特許分類】
【出願番号】特願2006−510324(P2006−510324)
【国際出願番号】PCT/JP2005/003158
【国際出願日】平成17年2月25日(2005.2.25)
【出願人】(593006630)学校法人立命館 (359)
【Fターム(参考)】