位置姿勢制御装置及び位置姿勢制御方法
【課題】小回りの効く浮遊体の位置姿勢制御を可能にするとともに、一方向だけでなくその他の種々の方向についても同一姿勢を維持しながら移動する位置姿勢制御を可能にする。
【解決手段】それぞれ推力を発生させる６つの推力発生器１４を備え、推力発生器１４の何れもが、異なる方向に推力を発生させる設定とされている。各推力発生器１４は、推力方向に延びる仮想直線３０が隣の推力発生器１４の推力方向に延びる仮想直線３０と仮想軸４０に垂直な２つの仮想平面上で交差するように位置決めされるとともに、前記一方の仮想平面上における仮想直線３０の交点が仮想軸４０上に重心位置のある正三角形の各頂点位置となり、かつ前記他方の仮想平面上における仮想直線３０の交点が仮想軸４０上に重心位置のある正三角形の各頂点位置となるように位置決めされている。
【解決手段】それぞれ推力を発生させる６つの推力発生器１４を備え、推力発生器１４の何れもが、異なる方向に推力を発生させる設定とされている。各推力発生器１４は、推力方向に延びる仮想直線３０が隣の推力発生器１４の推力方向に延びる仮想直線３０と仮想軸４０に垂直な２つの仮想平面上で交差するように位置決めされるとともに、前記一方の仮想平面上における仮想直線３０の交点が仮想軸４０上に重心位置のある正三角形の各頂点位置となり、かつ前記他方の仮想平面上における仮想直線３０の交点が仮想軸４０上に重心位置のある正三角形の各頂点位置となるように位置決めされている。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、浮遊体の位置及び姿勢を制御する位置姿勢制御装置及び位置姿勢制御方法に関するものである。
【背景技術】
【０００２】
浮遊体の一例として深海調査艇が挙げられる。この深海調査艇の一例として、下記非特許文献１にも掲載されている「しんかい６５００」があり、この深海調査艇には、図１５に示すように艇を前後方向に推進させることを主目的とした推進装置９１と、上下移動を担う垂直スラスタ９２と、左右方向への頭部旋回を担う水平スラスタ９３とが設けられている。そして、深海調査艇は、これら推進装置９１及び両スラスタ９２，９３による推力を制御することにより、調査海域において縦横無尽に、かつ調査物体を捕捉したまま追跡を行うことができるようになっている。この深海調査艇では、前後・上下・左右の三軸方向の推力で位置姿勢制御を行うので、非ホロノミックな拘束条件となっている。
【非特許文献１】「平成１７年６月２７日検索」、インターネット＜ＵＲＬ；http://www.jamstec.go.jp/ships/shinkai3.html＞
【発明の開示】
【発明が解決しようとする課題】
【０００３】
前記従来の深海調査艇では、三軸方向に推力を与える構成であり、非ホロノミックな拘束条件となっている。このため、ある地点に留まった状態で方向を変えるのは困難であり、浮遊地点を徐々に変化させながら位置及び姿勢を所望の値に制御する必要がある。この結果、小回りの効く姿勢制御ができないという問題がある。また、同一姿勢を保ちながら前後方向に移動することは可能でも、左右方向等には同一姿勢を保ちながら移動することが困難であるという問題もある。
【０００４】
そこで、本発明は、かかる点に鑑みてなされたものであり、その目的とするところは、小回りの効く浮遊体の位置姿勢制御を可能にするとともに、一方向だけでなくその他の種々の方向についても同一姿勢を維持しながら移動する位置姿勢制御を可能にすることにある。
【課題を解決するための手段】
【０００５】
前記の目的を達成するため、本発明は、浮遊体の位置及び姿勢を制御する位置姿勢制御装置を前提として、それぞれ推力を発生させる６つの推力発生器と、前記各推力発生器を制御する制御部とを備え、前記推力発生器の何れもが、異なる方向に推力を発生させる設定とされている。
【０００６】
本発明では、推力発生器が全て異なる向きで６つ設けられることで非ホロノミックな拘束条件とならない６軸方向の位置姿勢制御が可能となる。したがって、制御部によって各推力発生器が発生する推力を調整することにより、簡易な構造で小回りの効く位置姿勢制御を行うことができるとともに、一方向だけでなく種々の方向に対して同一姿勢を維持しながら移動する位置姿勢制御を行うことができる。また各推力発生器が位置固定されているので、通常のパラレルメカニズムのようにリンク長が伸縮することによって幾何学的形状が変化するものと異なり、形状パラメータを推定し直しながら位置姿勢制御をする必要がなく、幾何学的形状が一度設定されるとその形状パラメータを使った位置姿勢制御が可能となる。この結果、制御演算をより簡素なものにすることができ、迅速かつ正確な制御が可能となる。
【０００７】
ここで、仮想軸に垂直な２つの仮想平面を規定したときに、前記各推力発生器は、推力方向に延びる仮想直線が隣の推力発生器の推力方向に延びる仮想直線と前記仮想平面上で交差するように位置決めされるとともに、前記一方の仮想平面上における前記仮想直線の交点が前記仮想軸上に重心位置のある正三角形の各頂点位置となり、かつ前記他方の仮想平面上における前記仮想直線の交点が前記仮想軸上に重心位置のある正三角形の各頂点位置となるように位置決めされているのが好ましい。
【０００８】
この態様では、前記両仮想平面上での仮想直線の交点が前記正三角形の各頂点位置となるように各推力発生器が位置決めされているので、前記仮想平面に平行な面内での位置姿勢制御を移動方向及び回転方向によらず種々の方向において安定して行うことができる。
【０００９】
そして、前記２つの正三角形の各頂点位置となる各交点が、前記仮想軸の方向に見て正六角形以外の六角形を構成するように前記各推力発生器が位置決めされているのがより好ましい。
【００１０】
この態様では、位置姿勢制御における特異点が発生しにくくなるので、浮遊体の位置及び姿勢を確実に目標値に収束させることができるようになる。
【００１１】
また、前記各推力発生器の何れもが、前記仮想直線と前記仮想平面とのなす角度が同じ角度になるように設定されているのが好ましい。
【００１２】
この態様では、全ての推力発生器に同じものを使用できるようになるので、位置姿勢制御の演算が複雑化するのを抑制することができる。
【００１３】
さらに、前記一方の仮想平面上における正三角形の各頂点を通る仮想円の半径に対する前記他方の仮想平面上における正三角形の各頂点を通る仮想円の半径の比をＲとし、前記一方の仮想平面上の仮想円の半径に対する前記両仮想平面間の間隔の比をＨとしたときに、下記の式（１）
【数１】
（ただし、Ｒｃ＝−０．１、Ｈｃ＝０．０、ａ＝０．０４５、ｂ＝０．０７、ｃ＝１２、ｄ＝０．２５）によって描かれる楕円の内側の領域に前記Ｒ及びＨが属しているのが好ましい。
【００１４】
また、前記推力発生器は、前記仮想直線と前記仮想平面とのなす角度が４５度に設定されていてもよい。
【００１５】
この態様では、製作上のミスアライメントが生じにくいようにできるので、より正確な制御ができる。
【００１６】
また、本発明は、浮遊体の位置及び姿勢を制御する位置姿勢制御方法を前提として、何れも異なる方向に推力を発生させる向きに固定される６つの推力発生器が発生させる推力を調整することによって前記浮遊体の位置及び姿勢を制御する。
【００１７】
この位置姿勢制御方法において、仮想軸に垂直な２つの仮想平面を規定したときに、前記各推力発生器が、推力方向に延びる仮想直線が隣の推力発生器の推力方向に延びる仮想直線と前記仮想平面上で交差するように位置決めされるとともに、前記一方の仮想平面上における前記仮想直線の交点が前記仮想軸上に重心位置のある正三角形の各頂点位置となり、かつ前記他方の仮想平面上における前記仮想直線の交点が前記仮想軸上に重心位置のある正三角形の各頂点位置となるように位置決めされ、これら推力発生器が発生させる推力を調整することによって前記浮遊体の位置及び姿勢を制御するのが好ましい。
【００１８】
そして、前記２つの正三角形の各頂点位置となる各交点が、前記仮想軸の方向に見て正六角形以外の六角形を構成するように前記各推力発生器が位置決めされているのが好ましい。
【００１９】
さらに、前記各推力発生器の何れもが、前記仮想直線と前記仮想平面とのなす角度が同じ角度になるように設定されているのが好ましい。
【発明の効果】
【００２０】
以上説明したように、本発明によれば、小回りの効く浮遊体の位置姿勢制御を可能にするとともに、一方向だけでなくその他の種々の方向についても同一姿勢を維持しながら移動する位置姿勢制御を可能にすることができる。
【発明を実施するための最良の形態】
【００２１】
以下、本発明を実施するための最良の形態について図面を参照しながら詳細に説明する。
【００２２】
図１は、本発明の一実施形態に係る位置姿勢制御装置が適用された浮遊体を示している。この浮遊体は、例えば水中で浮遊可能な潜水艇１０として構成されている。ここでいう潜水艇１０とは、水中で探索、調査、所定の作業等を行うべく水中で縦横無尽に動き回るためのものを意味している。
【００２３】
前記潜水艇１０は、機器類が搭載される本体部１２を備えている。機器類を例示すると、例えば制御装置、カメラ、作業機器、電源等が挙げられる。また前記潜水艇１０が有人の潜水艇の場合には、この機器類に操縦席等が含まれる構成となる。本実施形態では、本体部１２は、半球状の２つ割形状を結合させた略球形に形成されている。
【００２４】
前記本体部１２には、フレーム体１６を介して位置決めされている推力発生器１４，・・，１４が固定されている。推力発生器１４，・・，１４は、本体部１２の赤道周上に外接するように締結固定されたフレーム体１６に取り付けられることで本体部１２に対して位置決めされている。フレーム体１６は、第１のフレームであるベースフレーム１８と、第２のフレームであるエンドフレーム２０とを備えている。ベースフレーム１８及びエンドフレーム２０は、それぞれ六角形状に形成されている。そして、ベースフレーム１８及びエンドフレーム２０は、それぞれ平面状に形成されていて、これら２つの平面が互いに平行となるように配置されている。
【００２５】
フレーム体１６には、ベースフレーム１８とエンドフレーム２０との間に架設された連結部材２３，・・，２３が設けられている。連結部材２３，・・，２３は６つ設けられており、各連結部材２３は、前記平面に対し隣の連結部材２３とＶ字を形成するように傾いた姿勢で配置されている。このＶ字の頂点は、ベースフレーム１８（又はエンドフレーム２０）を構成する六角形の各頂点のうち１つおきに選ばれる３つの頂点位置に一致している。すなわち各連結部材２３は、一端部がベースフレーム１８における六角形の頂点位置で結合されるとともに、他端部がエンドフレーム２０における六角形の頂点位置で結合されている。そして、１つおきに選ばれる３つの連結部材２３，・・，２３の一端部とベースフレーム１８との結合部は、正三角形の各頂点位置となるように構成されており、同様に前記連結部材２３，・・，２３の他端部とエンドフレーム２０との結合部は、正三角形の各頂点位置となるように構成されている。
【００２６】
前記推力発生器１４，・・，１４は６つ設けられており（１つは不図示）、これら各推力発生器１４が支持部材２５を介して連結部材２３にそれぞれ固定されている。そして推力発生器１４，・・，１４は、本体部１２を取り囲むように配置されており、隣の推力発生器１４，・・，１４とＶ字を形成するように前記平面に対して傾いた姿勢で配置されている。
【００２７】
各推力発生器１４は、丸みを帯びた細長形状のモータ部２７と、このモータ部２７の先端部に設けられたプロペラ部２８とを備え、このプロペラ部２８をモータ部２７に内蔵されたモータで駆動することによって推力を発生させる構成となっている。各推力発生器１４は、発生する推力の軸が本体部１２の中心及び連結部材２３を含む平面内となるように取り付けられている。
【００２８】
ここで、各推力発生器１４，・・，１４の位置関係について具体的に説明する。すなわち、図２にも示すように、推力発生器１４，・・，１４が発生する推力の方向に延びる仮想の直線３０，・・，３０を規定すると、この各仮想直線３０は、その隣の推力発生器１４が発生する推力の方向に延びる仮想直線３０と交差している。しかもこの交点は、図３（ｂ）に示すようにベースフレーム１８を含む仮想平面（第１仮想平面）３２上又はエンドフレーム２０を含む仮想平面（第２仮想平面）３４上に位置している。より詳細には、ある推力発生器１４の仮想直線３０は、一方側に隣接する推力発生器１４の仮想直線３０と第１仮想平面３２上で交差する一方、もう一方側（反対側）に隣接する推力発生器１４の仮想直線３０とは第２仮想平面３４上で交差している。これにより、第１仮想平面３２上に３つの交点が存在し、第２仮想平面３４上にも３つの交点が存在している。そして、図３（ａ）に示すように第１仮想平面３２上の３つの交点は、仮想軸４０上に重心位置のある正三角形３６の各頂点となっており、第２仮想平面３４上の３つの交点は、仮想軸４０上に重心位置のある正三角形３８の各頂点となっている。第１仮想平面３２上の正三角形３６の各頂点を通る仮想円を第１仮想円４１と称し、第２仮想平面３４上の正三角形３８の各頂点を通る仮想円を第２仮想円４２と称することとする。なお、両仮想平面３２，３４は、仮想軸４０に垂直な平面であり、互いに平行である。
【００２９】
潜水艇１０には、図４に示すように、各推力発生器１４，・・，１４が発生させる推力を制御する制御部４４が設けられている。制御部４４は、各推力発生器１４，・・，１４による推力をベクトルで表したときに、そのベクトルの和で潜水艇１０の移動及び姿勢を制御するものであり、その機能として推力導出部４６とモータ制御部４７とが含まれている。推力導出部４６は、各推力発生器１４，・・，１４が発生すべき推力を導出する。モータ制御部４７は、推力導出部４６によって求められた推力を各推力発生器１４，・・，１４が発生させるようにモータのデューティー比を演算してモータの回転方向及び回転数を制御する。なお、制御部４４は例えば本体部１２内に設けられてもよく、あるいは潜水艇１０が有策のものであって図外の母船に繋がれているようなときには、この母船に制御部４４を設けるようにしてもよい。
【００３０】
ここで、前記推力導出部４６による各推力発生器１４，・・，１４の発生する推力の導出について説明する。潜水艇１０に作用する合力ＦＧを前記第１仮想円４１の中心Ｏに作用する集中荷重として求めることにする。図５に示すように、第１仮想平面３２上における３つの仮想直線３０，・・，３０の交点Ａ，Ｂ，Ｃには、推力発生器１４，・・，１４による推力Ｆｉ（ｉ＝１〜６の自然数）が作用する。合力ＦＧのｘ方向成分、ｙ方向成分及びｚ方向成分をそれぞれＦｘ、Ｆｙ及びＦｚとすると、
ＦＧ＝Ｆｘ＋Ｆｙ＋Ｆｚ ・・・・・ （２）
なる関係式が成立し、これらＦｘ、Ｆｙ、Ｆｚをｘ方向、ｙ方向及びｚ方向の単位ベクトルｕｉｘ，ｕｉｙ，ｕｉｚを用いて、
【数２】
と表すことができる。
【００３１】
次に、潜水艇１０に作用する中心Ｏ回りの回転モーメントＮＧは、各推力Ｆｉと、中心Ｏに対する交点Ａ，Ｂ，Ｃの位置ベクトルとの外積によって求めることができるので、
【数３】
なる関係が成立する。
【００３２】
【数４】
【００３３】
以上により、各推力を調整することにより中心Ｏにかかる合力の調整が可能であり、実際に調整する際には、下記式（６）
【数５】
により推力が求められる。ここで、Ｃｌは、推力発生器１４，・・，１４の形状及び配置関係に依存する行列である。
【００３４】
次に、各推力発生器１４，・・，１４の配置関係についての好適な範囲について説明する。この好適な範囲とは、推力に外乱が加わった場合においても、種々の方向に対する位置姿勢制御がその影響を受け難くなる範囲を意味している。この好適範囲を求めるために、図６に示すスチュワートプラットフォームに基づいたモデルを採用した。そして拘束条件として、以下の条件を採用した。
１．上下両円盤５４，５６は互いに平行である。
２．各々の円盤中心を結ぶ軸線５８は円盤５４，５６に対して垂直である。
３．円盤５４，５６と２本のリンク６０，６０が１つの節で接続されている。
４．円盤５４，５６とリンク６０，・・，６０との節は各円盤５４，５６の円周部に均等に配置されている。すなわち、１２０度間隔である。
５．各リンク６０，・・，６０の長さは同一である。
【００３５】
リンク６０，・・，６０の方向に推力が付加されるものとし、上部円盤５４の中心に（Ｆ Ｎ）Ｔを発生するために必要な各リンク６０，・・，６０方向の推力Ｆｌを求める。そして、求められた推力Ｆｌに対して最大で１０％の誤差を与え、その結果として上部円盤５４の中心に（Ｆｃ Ｎｃ）が発生するとする（図７参照）。ここで、（Ｆ Ｎ）Ｔの各方向、大きさ、付加誤差は、｜Ｆ｜≠０、｜Ｎ｜≠０となるようにランダムに生成した。そして、評価関数として式（７）
【数６】
で表現される評価値Ｓを求めた。この評価値Ｓは、目標並進力の大きさ｜Ｆ｜や目標回転モーメント｜Ｎ｜を正規化したものである。（Ｆ Ｎ）Ｔの様々な組み合わせにより評価値Ｓの等高線図を作成した。この等高線図を図８及び図９に示す。この図９の横軸は、下部円盤５６の半径を１としたときの上部円盤５４の半径Ｒの対数値であり、縦軸は、下部円盤５６の半径を１としたときの両円盤間の間隔Ｈの対数値である。図８は評価値Ｓを三次元的に示したものである。
【００３６】
評価値Ｓの最適値を求めるべく最急降下法を用いてＲ方向とＨ方向に交互に探索を行った。このときＲ、Ｈの対数値に対して評価値Ｓの２次近似式を求め、各方向での極小値を近似式により順次求める探索方法も併用した。その結果、最適値としてＲ＝０．８０、Ｈ＝０．９７、リンク６０，・・，６０と円盤とのなす角度４３度が得られた。
【００３７】
また、評価値Ｓの等高線の近似をＲ及びＨの対数値を各軸とする対数グラフである図１０に示す。図中の数字は評価値Ｓであり、等高線として表された評価値Ｓは、楕円を表す以下の式（１）
【数７】
で近似することができる。楕円の中心座標（ｌｏｇ(Ｒｃ)、ｌｏｇ(Ｈｃ)）は、最適値Ｒ＝０．８０、Ｈ＝０．９７の対数値の近似値としての（−０．１，０．０）である。
【００３８】
好適な範囲としては、評価値Ｓが１．０未満となる式（７）中の係数がａ＝０．０４５、ｂ＝０．０７、ｃ＝１２、ｄ＝０．２５で表される楕円の内側が好ましく、さらに評価値Ｓが０．５未満となる係数がａ＝０．０５、ｂ＝０．０７、ｃ＝５．３、ｄ＝０．２５で表される楕円の内側がより好ましく、さらに評価値Ｓが０．３未満となる係数がａ＝０．０７、ｂ＝０．０９、ｃ＝１．３、ｄ＝０．３０で表される楕円の内側がより好ましく、さらに評価値Ｓが０．２３未満となるａ＝０．０７、ｂ＝０．０９、ｃ＝０．１１、ｄ＝０．３５で表される楕円の内側がより好ましい。
【００３９】
遡って本実施形態では、第１仮想平面３２上の正三角形３６が第２仮想平面３４上の正三角形３８よりも少し大きくなる設定となっており、前記第１仮想円４１の半径を１とすると、前記第２仮想円４２の半径（前記上部円盤５４の半径Ｒに相当）は０．８０となっている。また、第１仮想円４１の半径を１とすると、両仮想平面３２，３４間の間隔（両円盤間の間隔Ｈに相当）は０．９７となっている。そして、前記仮想直線３０，・・，３０が各仮想平面３２，３４となす傾き角度は何れも同じ角度に設定されており、その角度は４３度となっている。したがって、推力に外乱が加わった場合においても、あらゆる方向に対するその影響を最も受け難い配置関係にすることができる。なお、傾き角度は４５度としてもよい。この場合には第２仮想円４２の半径は０．８１となり、両仮想平面間の間隔は０．９２となる。傾き角度を４５度に設定するとフレーム体１６を製作しやすくなるため、推力の誤差の影響を受け難くしつつ、ミスアライメントの影響を受け難くすることができる。また傾き角度が４５度の場合でも評価値Ｓが０．２３未満となるので、外乱の影響を受け難く各方向への位置姿勢制御を正確に行うことができる。
【００４０】
以上のような構成の潜水艇１０について、試作小型艇を水槽内で運動させる実験を行った。その検証結果について並進及び回転のそれぞれについて説明する。水槽は高さ５００ｍｍ、幅１０００ｍｍ、奥行き４５０ｍｍのものであり、並進運動についてはＸ軸方向の並進推進力を一定として、約７秒間運動させ、そのときの移動軌跡を水槽側方からＣＣＤカメラで連続撮影した。その結果を図１１に示す。推進力を一定にしているが粘性抵抗のため早期に速度が収束し、試作小型艇はＸ軸方向にほぼ定速運動を行っていることが分かる。また上下動（Ｙ軸方向の動き）は約±５ｍｍ以内に抑えられており、姿勢を維持したまま並進運動していることが分かる。
【００４１】
次に、Ｚ軸回りの回転運動を行ったときの結果の一例を図１２に示す。この回転運動は、約２秒間回転させたときの結果であり、並進運動時と同様、粘性抵抗力と推力の合力との釣合いのため、回転角は直線的に増加しており、等速の回転運動をしていることがわかる。また側方及び上方からの観察により、上下動や並進運動が発生しないことが確認されている。したがって、水中での６軸方向の自由な移動と姿勢制御が可能である。
【００４２】
以上説明したように、本実施形態によれば、推力発生器１４，・・，１４が全て異なる向きで６つ設けられることで非ホロノミックな拘束条件とならない６軸方向の位置姿勢制御が可能となる。したがって、各推力発生器１４，・・，１４が発生される推力を調整することにより、簡易な構造で小回りの効く位置姿勢制御を行うことができるとともに、一方向だけでなく種々の方向に対して同一姿勢を維持しながら移動する位置姿勢制御を行うことができる。また各推力発生器１４，・・，１４が固定されているので、パラレルメカニズムのように幾何学的形状が変化するものと異なり、形状パラメータを推定し直しながら位置姿勢制御をする必要がなく、幾何学的形状が一度設定されると常時その形状パラメータを使った位置姿勢制御が可能となる。この結果、制御演算をより簡素なものにすることができ、迅速かつ正確な制御が可能となる。
【００４３】
また本実施形態では、各仮想直線３０，・・，３０の交点が第１仮想平面３２内の正三角形３６の各頂点位置又は第２仮想平面３４内の正三角形３８の各頂点位置となるように各推力発生器１４，・・，１４が位置決めされているので、前記仮想平面３２，３４に平行な面内での位置姿勢制御を移動方向及び回転方向によらず種々の方向において安定して行うことができる。しかも本実施形態では、前記各交点が前記仮想軸４０の方向に見て正六角形以外の六角形を構成するように各推力発生器１４，・・，１４が位置決めされているので、位置姿勢制御における特異点が発生しにくくなり、この結果、潜水艇１０の位置及び姿勢を確実に目標値に収束させることができるようになる。
【００４４】
また本実施形態では、各推力発生器１４，・・，１４の何れもが仮想平面とのなす角度が同じになるように設定されているので、全ての推力発生器１４，・・，１４に同じものを使用でき、この結果、位置姿勢制御の演算が複雑化するのを抑制することができる。
【００４５】
なお、本発明は、前記実施形態に限られるものではなく、特許請求の範囲に記載された発明の範囲内で適宜変更が可能である。例えば、前記実施形態では、プロペラ部２８がベースフレーム１８の近傍に位置するように推力発生器１４，・・，１４が固定されているが、プロペラ部２８がエンドフレーム２０とベースフレーム１８の中間位置に配置されていてもよい。
【００４６】
また、前記実施形態では、本体部１２にフレーム体１６を固定し、そのフレーム体１６に推力発生器１４，・・，１４を取り付ける構成としたが、これに限られるものではない。例えば図１３に示すように、潜水艇１０の本体部１２に直接推力発生器１４，・・，１４を設ける構成としてもよい。すなわち、本体部１２は、例えば球状に形成されており、この本体部１２に６つの貫通孔７０，・・，７０が形成されている。これら各貫通孔７０内には、それぞれ推力発生器１４が配設されていて、この推力発生器１４が駆動することによって貫通孔７０を通して水が吐き出されて、この貫通孔７０の向きに推力が発生する。この貫通孔７０，・・，７０の向きは、前記実施形態における連結部材２３，・・，２３の向きと同様に構成されている。この態様では、フレーム体１６を省略できるばかりでなく、流動抵抗を低減できてさらに高精度な制御が可能となる。
【００４７】
また、図１に示す推力発生器１４，・・，１４に代え、図１４に示すように、モータ部２７よりもプロペラ部２８の方が径の大きいものによって構成されていれば、プロペラ部２８をどちらに回転させる場合であっても推力を効果的に発生させることができる。
【００４８】
また前記実施形態では、浮遊体の一例として潜水艇１０に構成された例について説明したが、これに限られるものではなく、浮遊体として例えば気球、飛行船、潜水艦、水中ブイ、宇宙船等として構成されていてもよい。
【図面の簡単な説明】
【００４９】
【図１】本発明の実施形態に係る潜水艇の斜視図である。
【図２】仮想直線の幾何学的配置関係を説明するための概念図である。
【図３】（ａ）仮想軸方向に見た仮想直線、第１仮想円、第２仮想円、２つの正三角形の位置関係を説明するための説明図であり、（ｂ）仮想軸に直交する方向に見た第１仮想平面及び第２仮想平面を説明するための説明図である。
【図４】制御部に含まれる機能を示すブロック図である。
【図５】各推力発生器が発生させる推力を導出するために使用されるベクトルを示す図である。
【図６】スチュワートプラットフォームに基づいたモデルを示す図である。
【図７】スチュワートプラットフォームに基づいたモデルを示す図である。
【図８】評価値Ｓの等高線図である。
【図９】評価値Ｓの等高線図である。
【図１０】評価値Ｓの等高線図である。
【図１１】前記潜水艇をＸ軸方向に移動させる制御を行ったときの結果の一例を示す特性図である。
【図１２】前記潜水艇をＺ軸回りに回転させる制御を行ったときの結果の一例を示す特性図である。
【図１３】本発明の他の実施形態に係る潜水艇を概略的に示す斜視図である。
【図１４】本発明のさらに他の実施形態に係る潜水艇に設けられる推力発生器を示す図である。
【図１５】従来の深海調査艇の側面図である。
【符号の説明】
【００５０】
１０ 潜水艇
１４ 推力発生器
３０ 仮想直線
３２ 第１仮想平面
３４ 第２仮想平面
３６ 正三角形
３８ 正三角形
４０ 仮想軸
４１ 第１仮想円
４２ 第２仮想円
【技術分野】
【０００１】
本発明は、浮遊体の位置及び姿勢を制御する位置姿勢制御装置及び位置姿勢制御方法に関するものである。
【背景技術】
【０００２】
浮遊体の一例として深海調査艇が挙げられる。この深海調査艇の一例として、下記非特許文献１にも掲載されている「しんかい６５００」があり、この深海調査艇には、図１５に示すように艇を前後方向に推進させることを主目的とした推進装置９１と、上下移動を担う垂直スラスタ９２と、左右方向への頭部旋回を担う水平スラスタ９３とが設けられている。そして、深海調査艇は、これら推進装置９１及び両スラスタ９２，９３による推力を制御することにより、調査海域において縦横無尽に、かつ調査物体を捕捉したまま追跡を行うことができるようになっている。この深海調査艇では、前後・上下・左右の三軸方向の推力で位置姿勢制御を行うので、非ホロノミックな拘束条件となっている。
【非特許文献１】「平成１７年６月２７日検索」、インターネット＜ＵＲＬ；http://www.jamstec.go.jp/ships/shinkai3.html＞
【発明の開示】
【発明が解決しようとする課題】
【０００３】
前記従来の深海調査艇では、三軸方向に推力を与える構成であり、非ホロノミックな拘束条件となっている。このため、ある地点に留まった状態で方向を変えるのは困難であり、浮遊地点を徐々に変化させながら位置及び姿勢を所望の値に制御する必要がある。この結果、小回りの効く姿勢制御ができないという問題がある。また、同一姿勢を保ちながら前後方向に移動することは可能でも、左右方向等には同一姿勢を保ちながら移動することが困難であるという問題もある。
【０００４】
そこで、本発明は、かかる点に鑑みてなされたものであり、その目的とするところは、小回りの効く浮遊体の位置姿勢制御を可能にするとともに、一方向だけでなくその他の種々の方向についても同一姿勢を維持しながら移動する位置姿勢制御を可能にすることにある。
【課題を解決するための手段】
【０００５】
前記の目的を達成するため、本発明は、浮遊体の位置及び姿勢を制御する位置姿勢制御装置を前提として、それぞれ推力を発生させる６つの推力発生器と、前記各推力発生器を制御する制御部とを備え、前記推力発生器の何れもが、異なる方向に推力を発生させる設定とされている。
【０００６】
本発明では、推力発生器が全て異なる向きで６つ設けられることで非ホロノミックな拘束条件とならない６軸方向の位置姿勢制御が可能となる。したがって、制御部によって各推力発生器が発生する推力を調整することにより、簡易な構造で小回りの効く位置姿勢制御を行うことができるとともに、一方向だけでなく種々の方向に対して同一姿勢を維持しながら移動する位置姿勢制御を行うことができる。また各推力発生器が位置固定されているので、通常のパラレルメカニズムのようにリンク長が伸縮することによって幾何学的形状が変化するものと異なり、形状パラメータを推定し直しながら位置姿勢制御をする必要がなく、幾何学的形状が一度設定されるとその形状パラメータを使った位置姿勢制御が可能となる。この結果、制御演算をより簡素なものにすることができ、迅速かつ正確な制御が可能となる。
【０００７】
ここで、仮想軸に垂直な２つの仮想平面を規定したときに、前記各推力発生器は、推力方向に延びる仮想直線が隣の推力発生器の推力方向に延びる仮想直線と前記仮想平面上で交差するように位置決めされるとともに、前記一方の仮想平面上における前記仮想直線の交点が前記仮想軸上に重心位置のある正三角形の各頂点位置となり、かつ前記他方の仮想平面上における前記仮想直線の交点が前記仮想軸上に重心位置のある正三角形の各頂点位置となるように位置決めされているのが好ましい。
【０００８】
この態様では、前記両仮想平面上での仮想直線の交点が前記正三角形の各頂点位置となるように各推力発生器が位置決めされているので、前記仮想平面に平行な面内での位置姿勢制御を移動方向及び回転方向によらず種々の方向において安定して行うことができる。
【０００９】
そして、前記２つの正三角形の各頂点位置となる各交点が、前記仮想軸の方向に見て正六角形以外の六角形を構成するように前記各推力発生器が位置決めされているのがより好ましい。
【００１０】
この態様では、位置姿勢制御における特異点が発生しにくくなるので、浮遊体の位置及び姿勢を確実に目標値に収束させることができるようになる。
【００１１】
また、前記各推力発生器の何れもが、前記仮想直線と前記仮想平面とのなす角度が同じ角度になるように設定されているのが好ましい。
【００１２】
この態様では、全ての推力発生器に同じものを使用できるようになるので、位置姿勢制御の演算が複雑化するのを抑制することができる。
【００１３】
さらに、前記一方の仮想平面上における正三角形の各頂点を通る仮想円の半径に対する前記他方の仮想平面上における正三角形の各頂点を通る仮想円の半径の比をＲとし、前記一方の仮想平面上の仮想円の半径に対する前記両仮想平面間の間隔の比をＨとしたときに、下記の式（１）
【数１】
（ただし、Ｒｃ＝−０．１、Ｈｃ＝０．０、ａ＝０．０４５、ｂ＝０．０７、ｃ＝１２、ｄ＝０．２５）によって描かれる楕円の内側の領域に前記Ｒ及びＨが属しているのが好ましい。
【００１４】
また、前記推力発生器は、前記仮想直線と前記仮想平面とのなす角度が４５度に設定されていてもよい。
【００１５】
この態様では、製作上のミスアライメントが生じにくいようにできるので、より正確な制御ができる。
【００１６】
また、本発明は、浮遊体の位置及び姿勢を制御する位置姿勢制御方法を前提として、何れも異なる方向に推力を発生させる向きに固定される６つの推力発生器が発生させる推力を調整することによって前記浮遊体の位置及び姿勢を制御する。
【００１７】
この位置姿勢制御方法において、仮想軸に垂直な２つの仮想平面を規定したときに、前記各推力発生器が、推力方向に延びる仮想直線が隣の推力発生器の推力方向に延びる仮想直線と前記仮想平面上で交差するように位置決めされるとともに、前記一方の仮想平面上における前記仮想直線の交点が前記仮想軸上に重心位置のある正三角形の各頂点位置となり、かつ前記他方の仮想平面上における前記仮想直線の交点が前記仮想軸上に重心位置のある正三角形の各頂点位置となるように位置決めされ、これら推力発生器が発生させる推力を調整することによって前記浮遊体の位置及び姿勢を制御するのが好ましい。
【００１８】
そして、前記２つの正三角形の各頂点位置となる各交点が、前記仮想軸の方向に見て正六角形以外の六角形を構成するように前記各推力発生器が位置決めされているのが好ましい。
【００１９】
さらに、前記各推力発生器の何れもが、前記仮想直線と前記仮想平面とのなす角度が同じ角度になるように設定されているのが好ましい。
【発明の効果】
【００２０】
以上説明したように、本発明によれば、小回りの効く浮遊体の位置姿勢制御を可能にするとともに、一方向だけでなくその他の種々の方向についても同一姿勢を維持しながら移動する位置姿勢制御を可能にすることができる。
【発明を実施するための最良の形態】
【００２１】
以下、本発明を実施するための最良の形態について図面を参照しながら詳細に説明する。
【００２２】
図１は、本発明の一実施形態に係る位置姿勢制御装置が適用された浮遊体を示している。この浮遊体は、例えば水中で浮遊可能な潜水艇１０として構成されている。ここでいう潜水艇１０とは、水中で探索、調査、所定の作業等を行うべく水中で縦横無尽に動き回るためのものを意味している。
【００２３】
前記潜水艇１０は、機器類が搭載される本体部１２を備えている。機器類を例示すると、例えば制御装置、カメラ、作業機器、電源等が挙げられる。また前記潜水艇１０が有人の潜水艇の場合には、この機器類に操縦席等が含まれる構成となる。本実施形態では、本体部１２は、半球状の２つ割形状を結合させた略球形に形成されている。
【００２４】
前記本体部１２には、フレーム体１６を介して位置決めされている推力発生器１４，・・，１４が固定されている。推力発生器１４，・・，１４は、本体部１２の赤道周上に外接するように締結固定されたフレーム体１６に取り付けられることで本体部１２に対して位置決めされている。フレーム体１６は、第１のフレームであるベースフレーム１８と、第２のフレームであるエンドフレーム２０とを備えている。ベースフレーム１８及びエンドフレーム２０は、それぞれ六角形状に形成されている。そして、ベースフレーム１８及びエンドフレーム２０は、それぞれ平面状に形成されていて、これら２つの平面が互いに平行となるように配置されている。
【００２５】
フレーム体１６には、ベースフレーム１８とエンドフレーム２０との間に架設された連結部材２３，・・，２３が設けられている。連結部材２３，・・，２３は６つ設けられており、各連結部材２３は、前記平面に対し隣の連結部材２３とＶ字を形成するように傾いた姿勢で配置されている。このＶ字の頂点は、ベースフレーム１８（又はエンドフレーム２０）を構成する六角形の各頂点のうち１つおきに選ばれる３つの頂点位置に一致している。すなわち各連結部材２３は、一端部がベースフレーム１８における六角形の頂点位置で結合されるとともに、他端部がエンドフレーム２０における六角形の頂点位置で結合されている。そして、１つおきに選ばれる３つの連結部材２３，・・，２３の一端部とベースフレーム１８との結合部は、正三角形の各頂点位置となるように構成されており、同様に前記連結部材２３，・・，２３の他端部とエンドフレーム２０との結合部は、正三角形の各頂点位置となるように構成されている。
【００２６】
前記推力発生器１４，・・，１４は６つ設けられており（１つは不図示）、これら各推力発生器１４が支持部材２５を介して連結部材２３にそれぞれ固定されている。そして推力発生器１４，・・，１４は、本体部１２を取り囲むように配置されており、隣の推力発生器１４，・・，１４とＶ字を形成するように前記平面に対して傾いた姿勢で配置されている。
【００２７】
各推力発生器１４は、丸みを帯びた細長形状のモータ部２７と、このモータ部２７の先端部に設けられたプロペラ部２８とを備え、このプロペラ部２８をモータ部２７に内蔵されたモータで駆動することによって推力を発生させる構成となっている。各推力発生器１４は、発生する推力の軸が本体部１２の中心及び連結部材２３を含む平面内となるように取り付けられている。
【００２８】
ここで、各推力発生器１４，・・，１４の位置関係について具体的に説明する。すなわち、図２にも示すように、推力発生器１４，・・，１４が発生する推力の方向に延びる仮想の直線３０，・・，３０を規定すると、この各仮想直線３０は、その隣の推力発生器１４が発生する推力の方向に延びる仮想直線３０と交差している。しかもこの交点は、図３（ｂ）に示すようにベースフレーム１８を含む仮想平面（第１仮想平面）３２上又はエンドフレーム２０を含む仮想平面（第２仮想平面）３４上に位置している。より詳細には、ある推力発生器１４の仮想直線３０は、一方側に隣接する推力発生器１４の仮想直線３０と第１仮想平面３２上で交差する一方、もう一方側（反対側）に隣接する推力発生器１４の仮想直線３０とは第２仮想平面３４上で交差している。これにより、第１仮想平面３２上に３つの交点が存在し、第２仮想平面３４上にも３つの交点が存在している。そして、図３（ａ）に示すように第１仮想平面３２上の３つの交点は、仮想軸４０上に重心位置のある正三角形３６の各頂点となっており、第２仮想平面３４上の３つの交点は、仮想軸４０上に重心位置のある正三角形３８の各頂点となっている。第１仮想平面３２上の正三角形３６の各頂点を通る仮想円を第１仮想円４１と称し、第２仮想平面３４上の正三角形３８の各頂点を通る仮想円を第２仮想円４２と称することとする。なお、両仮想平面３２，３４は、仮想軸４０に垂直な平面であり、互いに平行である。
【００２９】
潜水艇１０には、図４に示すように、各推力発生器１４，・・，１４が発生させる推力を制御する制御部４４が設けられている。制御部４４は、各推力発生器１４，・・，１４による推力をベクトルで表したときに、そのベクトルの和で潜水艇１０の移動及び姿勢を制御するものであり、その機能として推力導出部４６とモータ制御部４７とが含まれている。推力導出部４６は、各推力発生器１４，・・，１４が発生すべき推力を導出する。モータ制御部４７は、推力導出部４６によって求められた推力を各推力発生器１４，・・，１４が発生させるようにモータのデューティー比を演算してモータの回転方向及び回転数を制御する。なお、制御部４４は例えば本体部１２内に設けられてもよく、あるいは潜水艇１０が有策のものであって図外の母船に繋がれているようなときには、この母船に制御部４４を設けるようにしてもよい。
【００３０】
ここで、前記推力導出部４６による各推力発生器１４，・・，１４の発生する推力の導出について説明する。潜水艇１０に作用する合力ＦＧを前記第１仮想円４１の中心Ｏに作用する集中荷重として求めることにする。図５に示すように、第１仮想平面３２上における３つの仮想直線３０，・・，３０の交点Ａ，Ｂ，Ｃには、推力発生器１４，・・，１４による推力Ｆｉ（ｉ＝１〜６の自然数）が作用する。合力ＦＧのｘ方向成分、ｙ方向成分及びｚ方向成分をそれぞれＦｘ、Ｆｙ及びＦｚとすると、
ＦＧ＝Ｆｘ＋Ｆｙ＋Ｆｚ ・・・・・ （２）
なる関係式が成立し、これらＦｘ、Ｆｙ、Ｆｚをｘ方向、ｙ方向及びｚ方向の単位ベクトルｕｉｘ，ｕｉｙ，ｕｉｚを用いて、
【数２】
と表すことができる。
【００３１】
次に、潜水艇１０に作用する中心Ｏ回りの回転モーメントＮＧは、各推力Ｆｉと、中心Ｏに対する交点Ａ，Ｂ，Ｃの位置ベクトルとの外積によって求めることができるので、
【数３】
なる関係が成立する。
【００３２】
【数４】
【００３３】
以上により、各推力を調整することにより中心Ｏにかかる合力の調整が可能であり、実際に調整する際には、下記式（６）
【数５】
により推力が求められる。ここで、Ｃｌは、推力発生器１４，・・，１４の形状及び配置関係に依存する行列である。
【００３４】
次に、各推力発生器１４，・・，１４の配置関係についての好適な範囲について説明する。この好適な範囲とは、推力に外乱が加わった場合においても、種々の方向に対する位置姿勢制御がその影響を受け難くなる範囲を意味している。この好適範囲を求めるために、図６に示すスチュワートプラットフォームに基づいたモデルを採用した。そして拘束条件として、以下の条件を採用した。
１．上下両円盤５４，５６は互いに平行である。
２．各々の円盤中心を結ぶ軸線５８は円盤５４，５６に対して垂直である。
３．円盤５４，５６と２本のリンク６０，６０が１つの節で接続されている。
４．円盤５４，５６とリンク６０，・・，６０との節は各円盤５４，５６の円周部に均等に配置されている。すなわち、１２０度間隔である。
５．各リンク６０，・・，６０の長さは同一である。
【００３５】
リンク６０，・・，６０の方向に推力が付加されるものとし、上部円盤５４の中心に（Ｆ Ｎ）Ｔを発生するために必要な各リンク６０，・・，６０方向の推力Ｆｌを求める。そして、求められた推力Ｆｌに対して最大で１０％の誤差を与え、その結果として上部円盤５４の中心に（Ｆｃ Ｎｃ）が発生するとする（図７参照）。ここで、（Ｆ Ｎ）Ｔの各方向、大きさ、付加誤差は、｜Ｆ｜≠０、｜Ｎ｜≠０となるようにランダムに生成した。そして、評価関数として式（７）
【数６】
で表現される評価値Ｓを求めた。この評価値Ｓは、目標並進力の大きさ｜Ｆ｜や目標回転モーメント｜Ｎ｜を正規化したものである。（Ｆ Ｎ）Ｔの様々な組み合わせにより評価値Ｓの等高線図を作成した。この等高線図を図８及び図９に示す。この図９の横軸は、下部円盤５６の半径を１としたときの上部円盤５４の半径Ｒの対数値であり、縦軸は、下部円盤５６の半径を１としたときの両円盤間の間隔Ｈの対数値である。図８は評価値Ｓを三次元的に示したものである。
【００３６】
評価値Ｓの最適値を求めるべく最急降下法を用いてＲ方向とＨ方向に交互に探索を行った。このときＲ、Ｈの対数値に対して評価値Ｓの２次近似式を求め、各方向での極小値を近似式により順次求める探索方法も併用した。その結果、最適値としてＲ＝０．８０、Ｈ＝０．９７、リンク６０，・・，６０と円盤とのなす角度４３度が得られた。
【００３７】
また、評価値Ｓの等高線の近似をＲ及びＨの対数値を各軸とする対数グラフである図１０に示す。図中の数字は評価値Ｓであり、等高線として表された評価値Ｓは、楕円を表す以下の式（１）
【数７】
で近似することができる。楕円の中心座標（ｌｏｇ(Ｒｃ)、ｌｏｇ(Ｈｃ)）は、最適値Ｒ＝０．８０、Ｈ＝０．９７の対数値の近似値としての（−０．１，０．０）である。
【００３８】
好適な範囲としては、評価値Ｓが１．０未満となる式（７）中の係数がａ＝０．０４５、ｂ＝０．０７、ｃ＝１２、ｄ＝０．２５で表される楕円の内側が好ましく、さらに評価値Ｓが０．５未満となる係数がａ＝０．０５、ｂ＝０．０７、ｃ＝５．３、ｄ＝０．２５で表される楕円の内側がより好ましく、さらに評価値Ｓが０．３未満となる係数がａ＝０．０７、ｂ＝０．０９、ｃ＝１．３、ｄ＝０．３０で表される楕円の内側がより好ましく、さらに評価値Ｓが０．２３未満となるａ＝０．０７、ｂ＝０．０９、ｃ＝０．１１、ｄ＝０．３５で表される楕円の内側がより好ましい。
【００３９】
遡って本実施形態では、第１仮想平面３２上の正三角形３６が第２仮想平面３４上の正三角形３８よりも少し大きくなる設定となっており、前記第１仮想円４１の半径を１とすると、前記第２仮想円４２の半径（前記上部円盤５４の半径Ｒに相当）は０．８０となっている。また、第１仮想円４１の半径を１とすると、両仮想平面３２，３４間の間隔（両円盤間の間隔Ｈに相当）は０．９７となっている。そして、前記仮想直線３０，・・，３０が各仮想平面３２，３４となす傾き角度は何れも同じ角度に設定されており、その角度は４３度となっている。したがって、推力に外乱が加わった場合においても、あらゆる方向に対するその影響を最も受け難い配置関係にすることができる。なお、傾き角度は４５度としてもよい。この場合には第２仮想円４２の半径は０．８１となり、両仮想平面間の間隔は０．９２となる。傾き角度を４５度に設定するとフレーム体１６を製作しやすくなるため、推力の誤差の影響を受け難くしつつ、ミスアライメントの影響を受け難くすることができる。また傾き角度が４５度の場合でも評価値Ｓが０．２３未満となるので、外乱の影響を受け難く各方向への位置姿勢制御を正確に行うことができる。
【００４０】
以上のような構成の潜水艇１０について、試作小型艇を水槽内で運動させる実験を行った。その検証結果について並進及び回転のそれぞれについて説明する。水槽は高さ５００ｍｍ、幅１０００ｍｍ、奥行き４５０ｍｍのものであり、並進運動についてはＸ軸方向の並進推進力を一定として、約７秒間運動させ、そのときの移動軌跡を水槽側方からＣＣＤカメラで連続撮影した。その結果を図１１に示す。推進力を一定にしているが粘性抵抗のため早期に速度が収束し、試作小型艇はＸ軸方向にほぼ定速運動を行っていることが分かる。また上下動（Ｙ軸方向の動き）は約±５ｍｍ以内に抑えられており、姿勢を維持したまま並進運動していることが分かる。
【００４１】
次に、Ｚ軸回りの回転運動を行ったときの結果の一例を図１２に示す。この回転運動は、約２秒間回転させたときの結果であり、並進運動時と同様、粘性抵抗力と推力の合力との釣合いのため、回転角は直線的に増加しており、等速の回転運動をしていることがわかる。また側方及び上方からの観察により、上下動や並進運動が発生しないことが確認されている。したがって、水中での６軸方向の自由な移動と姿勢制御が可能である。
【００４２】
以上説明したように、本実施形態によれば、推力発生器１４，・・，１４が全て異なる向きで６つ設けられることで非ホロノミックな拘束条件とならない６軸方向の位置姿勢制御が可能となる。したがって、各推力発生器１４，・・，１４が発生される推力を調整することにより、簡易な構造で小回りの効く位置姿勢制御を行うことができるとともに、一方向だけでなく種々の方向に対して同一姿勢を維持しながら移動する位置姿勢制御を行うことができる。また各推力発生器１４，・・，１４が固定されているので、パラレルメカニズムのように幾何学的形状が変化するものと異なり、形状パラメータを推定し直しながら位置姿勢制御をする必要がなく、幾何学的形状が一度設定されると常時その形状パラメータを使った位置姿勢制御が可能となる。この結果、制御演算をより簡素なものにすることができ、迅速かつ正確な制御が可能となる。
【００４３】
また本実施形態では、各仮想直線３０，・・，３０の交点が第１仮想平面３２内の正三角形３６の各頂点位置又は第２仮想平面３４内の正三角形３８の各頂点位置となるように各推力発生器１４，・・，１４が位置決めされているので、前記仮想平面３２，３４に平行な面内での位置姿勢制御を移動方向及び回転方向によらず種々の方向において安定して行うことができる。しかも本実施形態では、前記各交点が前記仮想軸４０の方向に見て正六角形以外の六角形を構成するように各推力発生器１４，・・，１４が位置決めされているので、位置姿勢制御における特異点が発生しにくくなり、この結果、潜水艇１０の位置及び姿勢を確実に目標値に収束させることができるようになる。
【００４４】
また本実施形態では、各推力発生器１４，・・，１４の何れもが仮想平面とのなす角度が同じになるように設定されているので、全ての推力発生器１４，・・，１４に同じものを使用でき、この結果、位置姿勢制御の演算が複雑化するのを抑制することができる。
【００４５】
なお、本発明は、前記実施形態に限られるものではなく、特許請求の範囲に記載された発明の範囲内で適宜変更が可能である。例えば、前記実施形態では、プロペラ部２８がベースフレーム１８の近傍に位置するように推力発生器１４，・・，１４が固定されているが、プロペラ部２８がエンドフレーム２０とベースフレーム１８の中間位置に配置されていてもよい。
【００４６】
また、前記実施形態では、本体部１２にフレーム体１６を固定し、そのフレーム体１６に推力発生器１４，・・，１４を取り付ける構成としたが、これに限られるものではない。例えば図１３に示すように、潜水艇１０の本体部１２に直接推力発生器１４，・・，１４を設ける構成としてもよい。すなわち、本体部１２は、例えば球状に形成されており、この本体部１２に６つの貫通孔７０，・・，７０が形成されている。これら各貫通孔７０内には、それぞれ推力発生器１４が配設されていて、この推力発生器１４が駆動することによって貫通孔７０を通して水が吐き出されて、この貫通孔７０の向きに推力が発生する。この貫通孔７０，・・，７０の向きは、前記実施形態における連結部材２３，・・，２３の向きと同様に構成されている。この態様では、フレーム体１６を省略できるばかりでなく、流動抵抗を低減できてさらに高精度な制御が可能となる。
【００４７】
また、図１に示す推力発生器１４，・・，１４に代え、図１４に示すように、モータ部２７よりもプロペラ部２８の方が径の大きいものによって構成されていれば、プロペラ部２８をどちらに回転させる場合であっても推力を効果的に発生させることができる。
【００４８】
また前記実施形態では、浮遊体の一例として潜水艇１０に構成された例について説明したが、これに限られるものではなく、浮遊体として例えば気球、飛行船、潜水艦、水中ブイ、宇宙船等として構成されていてもよい。
【図面の簡単な説明】
【００４９】
【図１】本発明の実施形態に係る潜水艇の斜視図である。
【図２】仮想直線の幾何学的配置関係を説明するための概念図である。
【図３】（ａ）仮想軸方向に見た仮想直線、第１仮想円、第２仮想円、２つの正三角形の位置関係を説明するための説明図であり、（ｂ）仮想軸に直交する方向に見た第１仮想平面及び第２仮想平面を説明するための説明図である。
【図４】制御部に含まれる機能を示すブロック図である。
【図５】各推力発生器が発生させる推力を導出するために使用されるベクトルを示す図である。
【図６】スチュワートプラットフォームに基づいたモデルを示す図である。
【図７】スチュワートプラットフォームに基づいたモデルを示す図である。
【図８】評価値Ｓの等高線図である。
【図９】評価値Ｓの等高線図である。
【図１０】評価値Ｓの等高線図である。
【図１１】前記潜水艇をＸ軸方向に移動させる制御を行ったときの結果の一例を示す特性図である。
【図１２】前記潜水艇をＺ軸回りに回転させる制御を行ったときの結果の一例を示す特性図である。
【図１３】本発明の他の実施形態に係る潜水艇を概略的に示す斜視図である。
【図１４】本発明のさらに他の実施形態に係る潜水艇に設けられる推力発生器を示す図である。
【図１５】従来の深海調査艇の側面図である。
【符号の説明】
【００５０】
１０ 潜水艇
１４ 推力発生器
３０ 仮想直線
３２ 第１仮想平面
３４ 第２仮想平面
３６ 正三角形
３８ 正三角形
４０ 仮想軸
４１ 第１仮想円
４２ 第２仮想円
【特許請求の範囲】
【請求項１】
浮遊体の位置及び姿勢を制御する位置姿勢制御装置であって、
それぞれ推力を発生させる６つの推力発生器と、
前記各推力発生器を制御する制御部とを備え、
前記推力発生器の何れもが、異なる方向に推力を発生させる設定とされている位置姿勢制御装置。
【請求項２】
仮想軸に垂直な２つの仮想平面を規定したときに、
前記各推力発生器は、推力方向に延びる仮想直線が隣の推力発生器の推力方向に延びる仮想直線と前記仮想平面上で交差するように位置決めされるとともに、前記一方の仮想平面上における前記仮想直線の交点が前記仮想軸上に重心位置のある正三角形の各頂点位置となり、かつ前記他方の仮想平面上における前記仮想直線の交点が前記仮想軸上に重心位置のある正三角形の各頂点位置となるように位置決めされている請求項１に記載の位置姿勢制御装置。
【請求項３】
前記２つの正三角形の各頂点位置となる各交点が、前記仮想軸の方向に見て正六角形以外の六角形を構成するように前記各推力発生器が位置決めされている請求項２に記載の位置姿勢制御装置。
【請求項４】
前記各推力発生器の何れもが、前記仮想直線と前記仮想平面とのなす角度が同じ角度になるように設定されている請求項２又は３に記載の位置姿勢制御装置。
【請求項５】
前記一方の仮想平面上における正三角形の各頂点を通る仮想円の半径に対する前記他方の仮想平面上における正三角形の各頂点を通る仮想円の半径の比をＲとし、前記一方の仮想平面上の仮想円の半径に対する前記両仮想平面間の間隔の比をＨとしたときに、下記の式（１）
【数１】
（ただし、Ｒｃ＝−０．１、Ｈｃ＝０．０、ａ＝０．０４５、ｂ＝０．０７、ｃ＝１２、ｄ＝０．２５）によって描かれる楕円の内側の領域に前記Ｒ及びＨが属している請求項２又は３に記載の位置姿勢制御装置。
【請求項６】
前記推力発生器は、前記仮想直線と前記仮想平面とのなす角度が４５度に設定されている請求項５に記載の位置姿勢制御装置。
【請求項７】
浮遊体の位置及び姿勢を制御する位置姿勢制御方法であって、
何れも異なる方向に推力を発生させる向きに固定される６つの推力発生器が発生させる推力を調整することによって前記浮遊体の位置及び姿勢を制御する位置姿勢制御方法。
【請求項８】
仮想軸に垂直な２つの仮想平面を規定したときに、
前記各推力発生器が、推力方向に延びる仮想直線が隣の推力発生器の推力方向に延びる仮想直線と前記仮想平面上で交差するように位置決めされるとともに、前記一方の仮想平面上における前記仮想直線の交点が前記仮想軸上に重心位置のある正三角形の各頂点位置となり、かつ前記他方の仮想平面上における前記仮想直線の交点が前記仮想軸上に重心位置のある正三角形の各頂点位置となるように位置決めされ、これら推力発生器が発生させる推力を調整することによって前記浮遊体の位置及び姿勢を制御する請求項７に記載の位置姿勢制御方法。
【請求項９】
前記２つの正三角形の各頂点位置となる各交点が、前記仮想軸の方向に見て正六角形以外の六角形を構成するように前記各推力発生器が位置決めされている請求項８に記載の位置姿勢制御方法。
【請求項１０】
前記各推力発生器の何れもが、前記仮想直線と前記仮想平面とのなす角度が同じ角度になるように設定されている請求項８又は９に記載の位置姿勢制御方法。
【請求項１】
浮遊体の位置及び姿勢を制御する位置姿勢制御装置であって、
それぞれ推力を発生させる６つの推力発生器と、
前記各推力発生器を制御する制御部とを備え、
前記推力発生器の何れもが、異なる方向に推力を発生させる設定とされている位置姿勢制御装置。
【請求項２】
仮想軸に垂直な２つの仮想平面を規定したときに、
前記各推力発生器は、推力方向に延びる仮想直線が隣の推力発生器の推力方向に延びる仮想直線と前記仮想平面上で交差するように位置決めされるとともに、前記一方の仮想平面上における前記仮想直線の交点が前記仮想軸上に重心位置のある正三角形の各頂点位置となり、かつ前記他方の仮想平面上における前記仮想直線の交点が前記仮想軸上に重心位置のある正三角形の各頂点位置となるように位置決めされている請求項１に記載の位置姿勢制御装置。
【請求項３】
前記２つの正三角形の各頂点位置となる各交点が、前記仮想軸の方向に見て正六角形以外の六角形を構成するように前記各推力発生器が位置決めされている請求項２に記載の位置姿勢制御装置。
【請求項４】
前記各推力発生器の何れもが、前記仮想直線と前記仮想平面とのなす角度が同じ角度になるように設定されている請求項２又は３に記載の位置姿勢制御装置。
【請求項５】
前記一方の仮想平面上における正三角形の各頂点を通る仮想円の半径に対する前記他方の仮想平面上における正三角形の各頂点を通る仮想円の半径の比をＲとし、前記一方の仮想平面上の仮想円の半径に対する前記両仮想平面間の間隔の比をＨとしたときに、下記の式（１）
【数１】
（ただし、Ｒｃ＝−０．１、Ｈｃ＝０．０、ａ＝０．０４５、ｂ＝０．０７、ｃ＝１２、ｄ＝０．２５）によって描かれる楕円の内側の領域に前記Ｒ及びＨが属している請求項２又は３に記載の位置姿勢制御装置。
【請求項６】
前記推力発生器は、前記仮想直線と前記仮想平面とのなす角度が４５度に設定されている請求項５に記載の位置姿勢制御装置。
【請求項７】
浮遊体の位置及び姿勢を制御する位置姿勢制御方法であって、
何れも異なる方向に推力を発生させる向きに固定される６つの推力発生器が発生させる推力を調整することによって前記浮遊体の位置及び姿勢を制御する位置姿勢制御方法。
【請求項８】
仮想軸に垂直な２つの仮想平面を規定したときに、
前記各推力発生器が、推力方向に延びる仮想直線が隣の推力発生器の推力方向に延びる仮想直線と前記仮想平面上で交差するように位置決めされるとともに、前記一方の仮想平面上における前記仮想直線の交点が前記仮想軸上に重心位置のある正三角形の各頂点位置となり、かつ前記他方の仮想平面上における前記仮想直線の交点が前記仮想軸上に重心位置のある正三角形の各頂点位置となるように位置決めされ、これら推力発生器が発生させる推力を調整することによって前記浮遊体の位置及び姿勢を制御する請求項７に記載の位置姿勢制御方法。
【請求項９】
前記２つの正三角形の各頂点位置となる各交点が、前記仮想軸の方向に見て正六角形以外の六角形を構成するように前記各推力発生器が位置決めされている請求項８に記載の位置姿勢制御方法。
【請求項１０】
前記各推力発生器の何れもが、前記仮想直線と前記仮想平面とのなす角度が同じ角度になるように設定されている請求項８又は９に記載の位置姿勢制御方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【公開番号】特開２００７−１１８７０２（Ｐ２００７−１１８７０２Ａ）
【公開日】平成１９年５月１７日（２００７．５．１７）
【国際特許分類】
【出願番号】特願２００５−３１１６８６（Ｐ２００５−３１１６８６）
【出願日】平成１７年１０月２６日（２００５．１０．２６）
【新規性喪失の例外の表示】特許法第３０条第１項適用申請有り ２００５年（平成１７年）６月９日 社団法人日本機械学会発行の「ロボティクス・メカトロニクス講演会２００５ 講演論文集」に発表
【出願人】（３９９０３００６０）学校法人 関西大学 (208)
【公開日】平成１９年５月１７日（２００７．５．１７）
【国際特許分類】
【出願日】平成１７年１０月２６日（２００５．１０．２６）
【新規性喪失の例外の表示】特許法第３０条第１項適用申請有り ２００５年（平成１７年）６月９日 社団法人日本機械学会発行の「ロボティクス・メカトロニクス講演会２００５ 講演論文集」に発表
【出願人】（３９９０３００６０）学校法人 関西大学 (208)
[ Back to top ]