(57)【要約】【課題】非ホロノミック拘束を利用した水中推進体のアイデアはあったが、アクチュエータを減らしても制御が難しくなり性能も劣化するためアクチュエータ低減のメリットがそれに見合わない場合が多く、現在までにこのような推進体の実用化例はない。本発明は、このような問題点を解決しようとするものである。【解決手段】非ホロノミック拘束を利用した推進体の運動制御について、独自の工夫を加えて従来の非ホロノミック推進体の制御に代わる新しい最適制御則を導いた。１つのスクリューのみを有する円筒状の水中推進体に適用し、スクリューによる推力の大きさと±符号のみを制御することで３次元空間の任意の位置へ移動できるような制御則を見出した。これにより推進体のコストと信頼性を大幅に改善できる。【選択図】図１
2008041065.tif 000002
【特許請求の範囲】
【請求項１】
推力を制御して流体中を移動する推進体において、制御される推力Ｆｘの演算式に、次式が含まれていることを特徴とする推進体の制御方法。
［数式１００］Ｆｘ＝ｋ１（ＸＥｌ１＋ＹＥｍ１＋ＺＥｎ１）＋ｋ２ｕ＋ｋ３ｐ
ここで、ｋ１，ｋ２，ｋ３は定数、（ＸＥ，ＹＥ，ＺＥ）は地球固定座標系（Ｘ，Ｙ，Ｚ）における筐体と目標点あるいは目標軌道との距離の各座標成分、ｕ，ｐはそれぞれ筐体の前後速度とロール速度であり、ｌ１，ｍ１，ｎ１は次式で与えられる。
［数式１０１］ｌ１＝ｃｏｓθｃｏｓψ，ｍ１＝ｃｏｓθｓｉｎψ，ｎ１＝−ｓｉｎθ
ここでψ，θ，φは推進体固定座標（ｘ，ｙ，ｚ）の地球固定座標系（Ｘ，Ｙ，Ｚ）に対するオイラー角（φは使用しない）である。
【請求項２】
請求項１の演算を行い、Ｆｘの値を推力発生装置の駆動信号として出力する制御装置と推力発生装置と推進体自身の位置や障害物の位置を認識するセンサと推進体の移動目標点を認識するセンサあるいは目標軌道を演算する装置およびこれら全体を収めた筐体から構成されたことを特徴とする推進体。
【請求項３】
請求項２において、推力発生装置は回転動力を発生させるモータとこの動力を伝達する推進軸と推進軸に固定されたスクリューにより構成されていることを特徴とする推進体。
【請求項４】
請求項２において、センサは３軸ジャイロと３軸加速度計とソナーを備えたあるいはこれらの一部を備えたことを特徴とする推進体。
【請求項５】
請求項２において、筐体内にガス室を設け比重量を周囲の流体と概略同じにして浮遊させた状態にしたことを特徴とする推進装置。
【請求項６】
３個以上のアクチュエータとこれを制御する主制御装置を有する推進体において、請求項１の演算を行う副制御装置と故障診断回路を有し、故障診断回路がスクリューによる主推進系が故障せず他のアクチュエータ系が故障するモードを検出した場合には、前記主制御装置から副制御装置へ制御を切り替えることを特徴とする推進体。
【発明の詳細な説明】
【技術分野】
【０００１】  本発明は、非ホロノミック拘束を利用した推進装置とその制御方法に関する．移動の媒質は気体、液体を問わず適用できる．
【背景技術】
【０００２】  航空機は、その軌道を制御するために翼が用いられ、翼の角度制御は飛翔体本体の推進力とは独立な制御入力が用いられている。これらはエンジンによる推進力とは別に主翼、尾翼に設けられた動翼（エルロン、エレベータ）と垂直尾翼のラダーの３個の制御装置により飛行制御が行われている。また潜水艦等の水中移動体についても同様の制御装置により推進制御が行われている。【０００３】  これらの制御に対し近年、３次元空間における剛体の運動の非ホロノミック拘束を利用して制御アクチュエータの数を減らす研究が行われている。例えば非特許文献１では水中推進体の自動制御に関する研究が報告されており、ロール運動について十分な粘性トルクがあれば、推進力制御とピッチ制御とヨー制御の３つの制御で安定した推進ができるとしている。これによって制御装置の数が低減されるため、信頼性の向上、コストの低減、軽量化が図られるというメリットが得られるといわれている。
【非特許文献１】Ｋ．Ｙ．Ｐｅｔｔｅｒｓｅｎ，Ｏ．Ｅｇｅｌａｎｄ，Ｐｏｓｉｔｉｏｎ  ａｎｄ  Ａｔｔｉｔｕｄｅ  Ｃｏｎｔｒｏｌ  ｏｆ  ａｎ  Ｕｎｄｅｒａｃｔｕａｔｅｄ  Ａｕｔｏｎｏｍｏｕｓ  Ｕｎｄｅｒｗａｔｅｒ  Ｖｅｈｉｃｌｅ，Ｐｒｏｃｅｅｄｉｎｇｓ  ｏｆ  ｔｈｅ  ３５▲ｔｈ▼  Ｃｏｎｆｅｒｅｎｃｅ  ｏｎ  Ｄｅｃｉｓｉｏｎ  ａｎｄ  Ｃｏｎｔｒｏｌ  １９９９  ＩＥＥＥ  ｐｐ．９８７−９９１．
【発明の開示】
【発明が解決しようとする課題】
【０００４】  非ホロノミック拘束を利用して制御用のアクチュエータを減らすと制御が難しくなり性能も劣化する。このようなマイナス要因に対し、アクチュエータ低減のメリットがそれに見合わない場合が多く、現在までにこのような推進体の実用化例はない。
本発明は、このような問題点を解決しようとするものであり、制御を工夫して簡便な制御ながら性能劣化を抑え、アクチュエータの数を極限まで減らすことで、非ホロノミック拘束利用推進装置を実用的なレベルまで高めることを目的とする。
【課題を解決するための手段】
【０００５】  上記課題を解決するため本発明では、特許文献１で提示されているシステムの最適制御方法の考え方を参考にして、新しく非ホロノミック推進体の運動制御に適用し、独自の工夫を加えて従来の非ホロノミック推進体の制御に替わる新しい制御則を導いたものである。【０００６】  まず、特許文献１の手法の概略を述べる。従来の制御理論では制御対象の特性を運動方程式で記述するが、本発明では制御対象の全入出力パワーの収支の式を用いる。具体的には、システムの各自由度毎の運動方程式をベクトル表示しこれに速度ベクトルを乗じたものである。入力パワーは制御入力だけでなく外乱入力も含まれ、また制御対象は受動要素だけとは限らないため、内部にエネルギ源がありこれが運動に影響を与えていれば外乱入力として扱う。
【数式１】
2008041065.tif 000003
ここで、ｄ、ｅ、ｑ、ｕ、ｖ、ｚ∈Ｒｎ、Ｍ∈Ｒｎ×ｎ  は正定対称な慣性マトリクス、ｎは制御対象の自由度である。ｑは一般化座標、ｕは制御入力、ｖは力入力の外乱、ｚは変位入力の外乱である。ｄはコリオリ力や遠心力やダンピング力など、ｅはポテンシャル力である。
【０００７】  上記のシステムに対し、次の評価関数を考える。
【数式２】
2008041065.tif 000004
2008041065.tif 000005
制御装置が制御対象に加えるパワーである。ｒは重み係数である。
【０００８】  次に、式（２）を最小化する制御ｕ（ｔ）を求める。最適制御の必要条件を求めるため次のスカラー関数Ｌを定義する。
【数式３】
2008041065.tif 000006
ここで、κは未定定数である。右辺の｛｝内は、式（１）の左辺と同じで制御対象の全パワー収支であるからエネルギ保存則を満たし常にゼロである。従って式（３）で表されるＬの積分を最小化する条件は、式（２）も最小化する。従って関数Ｌにｑを変数とする変分原理を適用した次式はｕが最適制御であるための必要条件を与える。Ｌはｕに関して１次式であるから、∂Ｌ／∂ｕは意味がなく次式に制御に関するすべての情報が含まれる。
【数式４】
2008041065.tif 000007
【０００９】式（４）から制御則が次のように求まる。
【数式５】
2008041065.tif 000008
上式の第１行は外力ｖと慣性のｑ依存性に対する制御、第２行はコリオリ力や遠心力やダンピング力に対する制御、第３行はポテンシャル力とそのｑ依存性および外力ｚに対する制御、第４行は評価関数を低減させる制御でありそれぞれ意味が明確である。
以上が、特許文献１の概要であり、本発明はこの考え方を非ホロノミック推進体の運動制御に適用したものである。
【００１０】  まず、制御則を求めるための非ホロノミック推進体のモデルを図１に示す。
ここで、ｕ，ｖ，ｗ，ｐ，ｑ，ｒは図１に示すように筐体に固定された座標（ｘ，ｙ，ｚ）方向の速度と各座標軸周りの回転速度である．Ｐをロールレート、ｑをピッチレート、ｒをヨーレートと称する．
【００１１】  この推進体の運動方程式は次式で表される．
【数式６】
2008041065.tif 000009
【数式７】
2008041065.tif 000010
【数式８】
2008041065.tif 000011
【数式９】
2008041065.tif 000012
【数式１０】
2008041065.tif 000013
【数式１１】
2008041065.tif 000014
ここで、Ｄｘ，Ｄｙ，Ｄｚ，Ｄｐ，Ｄｑ，Ｄｒは、各運動方向の流体抵抗である．
ｍは図１に示す推進体の質量、Ｉｘ，Ｉｙ，Ｉｚは推進体のｘ，ｙ，ｚ軸周りの慣性モーメントである．定数αは、推進用モータが筐体への反力としてロールトルクを生じさせるときの係数である．式（６），（９）の右辺に力とトルクが生じ、この力とトルクにより変数ｕ，ｖ，ｗ，ｐ，ｑ，ｒが互いに連鎖し相互に影響しあいながら複雑な運動を生ずる．このシステムについて、入力をＦｘとし出力をｕ，ｖ，ｗ，ｐ，ｑ，ｒとしてみた場合、式（６），（９）は入力で直接制御されているが、式（７），（８），（１０），（１１）は入力で直接制御されておらずこれらは運動を拘束する条件とみなすことができる．これらの微分方程式で表わされた拘束式は積分して代数式であらわすことができず、このような積分できない拘束式を有するシステムを非ホロノミック系と称し、これらの拘束式を非ホロノミック拘束と呼んでいる。
本考案は、式（７），（８），（１０），（１１）の非ホロノミック拘束を積極的に活用して、推進装置の軌道を制御するシステムの提案である．
【００１２】  まず、本推進体について、式（１）に相当するパワー収支式を求める。
【数式１２】
2008041065.tif 000015
【００１３】  本発明の目的は、最小のエネルギ消費量で、与えられた初期位置から目標位置まで移動する制御則を導出することにある。そこで、制御性能の評価を与える式（２）の評価関数は、次式を用いる。
【数式１３】
2008041065.tif 000016
ここで、ＸＥ，ＹＥ，ＺＥは地球固定座標（Ｘ，Ｙ，Ｚ）による推進体重心点の位置座標である。従って上式被積分関数の第１項は現在位置から目標位置（原点）までの距離の２乗を時間微分したものである。
【００１４】  地球固定座標系（Ｘ，Ｙ，Ｚ）と推進体固定座標系（ｘ，ｙ，ｚ）との間の方向余弦マトリクスを次式に示す。
【数式１４】
2008041065.tif 000017
ここで、φ，θ，ψはオイラー角である。
【００１５】
2008041065.tif 000018
【数式１５】
2008041065.tif 000019
【００１６】  また、第２項は制御装置が制御対象に与えるパワーである。よって、汎関数は次式となる。
【数式１６】
2008041065.tif 000020
ここでｒは重み係数である。
【００１７】  式（１６）を式（４）に代入すると次の２つの制御則を得る。この演算過程でＰに含まれる慣性力の項は消えるため、当初からＰの中の慣性力は削除しておいても良い。
【数式１７】
2008041065.tif 000021
【数式１８】
2008041065.tif 000022
【００１８】  最終的な制御則は次式になる。
【数式１９】
Ｆｘｏｐｔ＝ρ１Ｆｘ１＋ρ２Ｆｘ２
ここで、ρ１、ρ２は重み係数であり、前記κとともに数値最適化により求める。
【特許文献１】特願２００６−９２２４３
【発明の効果】
【００１９】  本発明の制御則の効果をシミュレーションにより確認する。
Ｄｘ＝Ｃｘｕ２ｓｉｇｎ（ｕ），Ｄｙ＝ＣｙＶ２ｓｉｇｎ（ｖ），Ｄｚ＝Ｃｚｗ２ｓｉｇｎ（ｗ），Ｄｐ＝Ｃｐｐ２ｓｉｇｎ（ｐ），Ｄｑ＝Ｃｑｑ２ｓｉｇｎ（ｑ），Ｄｒ＝Ｃｒｒ２ｓｉｇｎ（ｒ）とした。ただしｓｉｇｎ（ｕ）はｕの±符号を表す。
Ｃｘ＝３．５１９Ｎｓ２／ｍ２，Ｃｙ＝９．０１３Ｎｓ２／ｍ２，Ｃｚ＝９．０１３Ｎｓ２／ｍ２，Ｃｐ＝０．００２４Ｎｍｓ２，Ｃｑ＝０．００００５Ｎｍｓ２，Ｃｒ＝０．００００５Ｎｍｓ２
ｍ＝２．８Ｋｇ，ｌｘ＝０．００３５ｋｇｍ２，ｌｙ＝０．０３ｋｇｍ２，ｌｚ＝０．０３ｋｇｍ２，ρ１＝０．０４，ρ２＝０．０４，κ＝−０．０１  とした。
以上で、条件がすべて定まったため、ＭＡＴＬＡＢ／ＳＩＭＵＬＩＮＫによるシミュレーションを行った。結果を図２〜５に示す。
【００２０】  図２は制御入力Ｆｘの時間波形である。一般に非ホロノミック系の制御は振動的になる特徴があるが、本制御でも同様である。このような入力を与えることで、式（７），（８），（１０），（１１）の非ホロノミック拘束が有効に作用し目標地点へ移動することができる。
【００２１】  図３は（Ｘ，Ｙ，Ｚ）座標系で見た推進体の重心点の軌跡を示す。初期位置から、らせん状に旋回しながら（Ｘ，Ｙ，Ｚ）座標系の原点へ移動していく様子がわかる。
【００２２】  図４は（Ｘ，Ｙ，Ｚ）座標系で見た推進体の重心点（ＸＥ，ＹＥ，ＺＥ）の時間波形を示す。
【００２３】  図５は式（１３）の評価関数Ｊの時間波形を示す。約１００秒で最小値に収束していることがわかる。
【００２４】  以上のように、従来は非特許文献１に代表されるように、水中推進体の制御は最低で３個の独立した制御アクチュエータが必要とされていたものを、本考案の制御では推進力の制御のみで可能できることを明らかにした。
今回のシミュレーションでは初期設定位置をＸＥ＝１０ｍ，ＹＥ＝１０ｍ，ＺＥ＝１０ｍとし、目標地点を原点としたが、上記の理論展開からみてこれらの値は任意に設定できることは明らかである。
【発明を実施するための形態】
【００２５】  以下に、本発明を実施するための好ましい形態を図面に基づいて説明する．
【実施例１】
【００２６】  図６に本発明の推進体の構造図を示す．センサユニット１は３軸加速度センサと３軸ジャイロとソナーからなる．３軸加速度センサと３軸ジャイロの信号から図１のｕ，ｖ，ｗ，ｐ，ｑ，ｒを演算あるいは検出する。またソナーにより目標地点の位置を検出する。これより図１の座標（ＸＥ，ＹＥ，ＺＥ）を求める。センサユニット１からの信号はコントローラ２に入り、コントローラ２はこの信号から数式（１９）の制御則を演算し、ドライバ３を駆動する。ドライバ３はバッテリ４からエネルギ供給を受け、指令された電流によりモータ５を駆動する。モータ５のトルクは推進軸６によりスクリュー７に伝達されて推進力となる。モータ５は筐体８に固定されており、モータトルクの反力が筐体８に加わり筐体８はロール運動する。筐体８の中心部にはガス室９が設けられており、推進体の比重が水の比重と等しくなるように、また推進体の中心と浮力の作用点はほぼ同じ位置になるように設計されている。【００２７】  以上により、本発明の水中推進体はアクチュエータが１つでも水中を自在に移動可能であることが明らかで、アクチュエータの数を減らせることにより、コスト軽減と信頼性向上と軽量化のメリットが得られる。【００２８】  以上は水中における推進の例を説明してきたが、空気中においても同様の推進が可能であることはいうまでもない。ただし全体を比重の小さな材料で作りガス室は空気より軽いヘリウムガスなどを充填する必要がある。
【実施例２】
【００２９】  図７に第２の実施例を示す．これは、従来の３個のアクチュエータを有する水中ロボットが故障した場合に、本制御則による制御に切り替えて目的地点まで帰還できるような機能をもたせたものである。
スクリュー１０を駆動するアクチュエータ１１と制御用フィン１２を駆動するアクチュエータ１３と制御用フィン１４を駆動するアクチュエータ１５とこれらのアクチュエータの駆動信号を指令するコンピュータと駆動電流を発生させるドライバを内蔵した主コントローラ１６と慣性誘導装置１７と全てのユニットに電力を供給するバッテリ１８と全てのユニットを納めた筐体１９から構成されている。【００３０】  図８は制御系のブロック線図を示す。主コントローラ１６内のコンンピュータは慣性誘導装置１７からの信号を受けて現在位置・姿勢を演算する回路２１と目標進路を演算する回路２２と目標進路と現在位置との偏差を演算する回路２３とこの偏差信号をもとに前記３個のアクチュエータへの制御指令値を演算する回路２４と故障診断回路２５から構成される。故障診断回路２５はシステムが正常な場合は０信号を出力し、アクチュエータ１３もしくはアクチュエータ１５が故障した場合あるいはその両方が故障しかつアクチュエータ１１は正常な場合は＋１信号を出力し、アクチュエータ１１が故障した場合は−１信号を発生し、出力切り替え装置２６へ送る。出力切り替え装置２６は０信号がくれば主コントローラ１６の出力をアクチュエータ駆動用ドライバ２８へ送り、＋１信号がくれば副コントローラ２０の出力をドライバ２８へ送り、−１信号がくればドライバ２８を駆動させない。副コントローラの構成は、現在位置・姿勢を演算する回路２１と目標進路を演算する回路２２と目標進路と現在位置との偏差を演算する回路２３については主コントローラと同じであるが、アクチュエータ１１への制御指令値を演算する回路２７は主コントローラとは異なり、この回路で本発明の制御則である式（１９）を演算し出力する。【００３１】  以上により、本発明の水中推進体はアクチュエータが故障して制御フィンが動かなくなっても主推進系が故障しない限り、非ホロノミックな運動制御により回収可能地点まで独力で戻ってこれることが明らかで、これにより高価な水中ロボットを故障により失うことがないというメリットが得られる。
【産業上の利用可能性】
【００３２】  一つの推力発生装置だけで３次元空間の任意の位置に到達する推進体が実現できるため、低コスト、高信頼性、軽量化が可能になり有用性は高いものと考えられる．
【図面の簡単な説明】
【００３３】
【図１】本発明の推進体のモデルと座標系の説明図
【図２】本発明の推進体の制御出力の時間波形を示す図
【図３】（Ｘ，Ｙ，Ｚ）座標系で見た推進体の重心点の軌跡を示す図
【図４】（Ｘ，Ｙ，Ｚ）座標系で見た推進体の重心点（ＸＥ，ＹＥ，ＺＥ）の時間波形を示す図
【図５】式（１３）の評価関数Ｊの時間波形を示す図
【図６】本発明の推進体の構造図
【図７】第２の実施例を示す図
【図８】制御系のブロック線図
【符号の説明】
【００３４】１  センサユニット
２  コントローラ
３  ドライバ
４  バッテリ
５  モータ
６  推進軸
７  スクリュー
８  筐体
９  ガス室
１０  スクリュー
１１  アクチュエータ
１２  制御用フィン
１３  アクチュエータ
１４  制御用フィン
１５  アクチュエータ
１６  主コントローラ
１７  慣性誘導装置
１８  バッテリ
１９  筐体
２０  副コントローラ
２１  現在位置・姿勢の演算回路
２２  目標進路演算回路
２３  偏差の演算回路
２４  各アクチュエータへの制御指令値演算回路
２５  故障診断回路
２６  出力切り替え装置
２７  アクチュエータ１１への制御指令値演算回路
２８  ドライバ
【図１】
2008041065.tif 000023
【図２】
2008041065.tif 000024
【図３】
2008041065.tif 000025
【図４】
2008041065.tif 000026
【図５】
2008041065.tif 000027
【図６】
2008041065.tif 000028
【図７】
2008041065.tif 000029
【図８】
2008041065.tif 000030