(57)【要約】【課題】姿勢の安定化を図りつつ、地形面への衝突可能性を低減可能な移動体の制御装置、制御方法及びプログラムを提供することを目的とする。【解決手段】移動体の地形面に対する高度又は姿勢角の少なくとも一方を制御パラメータとする移動体の制御において、制御パラメータの基本目標値に、地形面の傾斜の状況に基づいて調整される目標バイアス値を加算することにより補正目標値を算出する。制御パラメータが補正目標値になるように移動体を制御する。【選択図】図３
2020192906.tif 000002
【特許請求の範囲】
【請求項１】
移動体の地形面に対する高度又は姿勢角の少なくとも一方を制御パラメータとする移動体の制御装置であって、
前記地形面の傾斜を推定する傾斜推定部と、
前記傾斜の状況に基づいて調整される目標バイアス値を算出する目標バイアス値算出部と、
前記制御パラメータの基本目標値に前記目標バイアス値を加算することにより補正目標値を算出する補正目標値算出部と、
前記制御パラメータが前記補正目標値になるように前記移動体を制御する制御部と、
を備える、移動体の制御装置。
【請求項２】
前記制御パラメータは前記姿勢角であり、
前記姿勢角は前記高度に依存して制御される、請求項１に記載の移動体の制御装置。
【請求項３】
前記移動体の前記高度を推定する高度推定部を更に備え、
前記目標バイアス値算出部は、前記高度推定部で推定された前記高度が前記基本目標高度より低い場合、前記目標バイアス値の符号を正に設定し、前記高度推定部で推定された前記高度が前記基本目標高度より高い場合、前記目標バイアス値の符号を負に設定する、請求項２に記載の移動体の制御装置。
【請求項４】
前記目標バイアス値算出部は、前記目標バイアス値の絶対値と前記高度との関係を規定する特性関数に基づいて、前記高度推定部で推定された前記高度に対応する前記目標バイアス値を算出する、請求項２又は３に記載の移動体の制御装置。
【請求項５】
前記特性関数は、前記高度が増加するに従って前記目標バイアス値の絶対値が減少するように規定される、請求項４に記載の移動体の制御装置。
【請求項６】
前記特性関数は、前記高度推定部で推定された前記高度が高度下限値以下である場合、前記目標バイアス値の絶対値が最大値になるように規定される、請求項４又は５に記載の移動体の制御装置。
【請求項７】
前記目標バイアス値算出部は、前記特性関数として、
前記目標バイアス値の絶対値と前記高度とが線形特性を示す第１特性関数と、
前記第１特性関数に比べて前記高度に対する前記目標バイアス値の絶対値が大きく設定される第２特性関数と、
前記第１特性関数に比べて前記高度に対する前記目標バイアス値の絶対値が小さく設定される第３特性関数と、
のうちいずれかを選択可能に構成される、請求項４から６のいずれか一項に記載の移動体の制御装置。
【請求項８】
前記目標バイアス値算出部は、
前記傾斜がゼロである場合、前記特性関数として前記第１特性関数を選択し、
前記傾斜がゼロより大きい場合、前記特性関数として前記第２特性関数を選択し、
前記傾斜がゼロより小さい場合、前記特性関数として前記第３特性関数を選択する、請求項７に記載の移動体の制御装置。
【請求項９】
前記姿勢角及び前記高度は独立して制御可能である、請求項１に記載の移動体の制御装置。
【請求項１０】
前記目標バイアス値算出部は、前記傾斜に基づいて前記高度に対応する目標高度バイアス値を算出する目標高度バイアス値算出部と、前記傾斜に基づいて前記姿勢角に対応する目標姿勢角バイアス値を算出する目標姿勢角バイアス値算出部と、を含み、
前記補正目標値算出部は、前記基本高度目標値に前記目標高度バイアス値を加算することにより補正目標高度値を算出する補正目標高度値算出部と、前記基本姿勢角目標値に前記目標姿勢角バイアス値を加算することにより補正目標姿勢角値を算出する補正目標姿勢角値算出部と、を含み、
前記制御部は、前記高度が前記補正目標高度値になるとともに前記姿勢角が前記補正目標姿勢角値になるように、前記移動体を制御する、請求項９に記載の移動体の制御装置。
【請求項１１】
移動体の地形面に対する高度又は姿勢角の少なくとも一方を制御パラメータとする移動体の制御方法であって、
前記地形面の傾斜を推定するステップと、
前記傾斜の状況に基づいて調整される目標バイアス値を算出するステップと、
前記制御パラメータの基本目標値に前記目標バイアス値を加算することにより補正目標値を算出するステップと、
前記制御パラメータが前記補正目標値になるように前記移動体を制御するステップと、
を備える、移動体の制御方法。
【請求項１２】
移動体の地形面に対する高度又は姿勢角の少なくとも一方を制御パラメータとする移動体を制御するためのプログラムであって、
コンピュータに、
前記地形面の傾斜を推定するステップと、
前記傾斜の状況に基づいて調整される目標バイアス値を算出するステップと、
前記制御パラメータの基本目標値に前記目標バイアス値を加算することにより補正目標値を算出するステップと、
前記制御パラメータが前記補正目標値になるように前記移動体を制御するステップと、
を実行させる、プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本開示は、移動体の制御装置、制御方法及びプログラムに関する。
【背景技術】
【０００２】
水中又は空中を自動で巡航して移動可能な移動体が知られている。このような移動体の運転制御は、水底や地上などの地形面に対する高度を適切に確保することで、巡航中の移動体が地形面に衝突することを回避する必要がある。
【０００３】
移動体の地形面に対する高度は、移動体に搭載されたセンサによって検知される。例えば特許文献１には、水中を航走する航走体の運動制御装置であって、移動体に搭載された高度ソーナーによって水底までの距離を測定し、測定された距離に基づいて移動体の高度制御を行う一例が開示されている。この文献では特に、水底の異なる２点からの航走体までの２つの距離に基づいて航走体の水底からの高度制御を実施することで、水底の１点のみに対する距離に基づく場合に比べて、航走体の姿勢や高度偏差の変動を低減できるとされている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特許第５０１０３３２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
上記特許文献１のような従来の移動体の高度制御は、移動体に搭載されたセンサによって検知された高度の実測値が目標高度になるように実施される。このような高度制御に用いられる目標高度は、典型的には、予め一定値として設定される。しかしながら、実際の地形面は様々な凹凸形状を含むため、地形面の形状によっては地形面に沿って巡航する移動体の姿勢が不安定になったり、逆に移動体の姿勢の安定性を確保しようとすると、地形面との距離が目標高度より近づき、地形面への衝突可能性が高くなってしまう場合がある。
【０００６】
また衝突可能性を低く抑えるために目標高度に含まれるマージンを大きく設定することも考えられるが、地形面に対する移動体の高度が過大になってしまう。特に、移動体に地形探査等を目的とする測定装置を搭載することで地形面の探査を行う場合、移動体の高度が過大になると、測定装置と地形面との距離が離れてしまい、探査精度の低下を招いてしまう。
【０００７】
本発明の少なくとも一実施形態は上述の事情に鑑みなされたものであり、姿勢の安定化を図りつつ、地形面への衝突可能性を低減可能な移動体の制御装置、制御方法及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００８】
（１）本発明の少なくとも一実施形態に係る移動体の制御装置は上記課題を解決するために、
移動体の地形面に対する高度又は姿勢角の少なくとも一方を制御パラメータとする移動体の制御装置であって、
前記地形面の傾斜を推定する傾斜推定部と、
前記傾斜の状況に基づいて調整される目標バイアス値を算出する目標バイアス値算出部と、
前記制御パラメータの基本目標値に前記目標バイアス値を加算することにより補正目標値を算出する補正目標値算出部と、
前記制御パラメータが前記補正目標値になるように前記移動体を制御する制御部と、
を備える。
【０００９】
上記（１）の構成によれば、移動体の運動制御に関する制御パラメータの目標値として、基本目標値に対して目標バイアス値を加算して算出される補正目標値が用いられる。補正目標値は、地形面の傾斜の状況に基づいて調整される目標バイアス値を含むことで、地形面の形状に対応して可変に設定される。これにより、様々な地形面に対して安定的な姿勢を確保しつつ、地形面への衝突可能性を低減可能な運動制御が可能となる。
【００１０】
（２）幾つかの実施形態では上記（１）の構成において、
前記制御パラメータは前記姿勢角であり、
前記姿勢角は前記高度に依存して制御される。
【００１１】
上記（２）の構成によれば、移動体の姿勢角が高度に依存して変化する移動体（例えば、移動体本体の前方又は後方のいずれかのみに舵が存在することで、姿勢角を変化させるために高度を変化させる必要がある移動体）において、様々な地形面に対して安定的な姿勢を確保しつつ、地形面への衝突可能性を低減可能な運動制御が可能となる。
【００１２】
（３）幾つかの実施形態では上記（２）の構成において、
前記移動体の前記高度を推定する高度推定部を更に備え、
前記目標バイアス値算出部は、前記高度推定部で推定された前記高度が前記基本目標高度より低い場合、前記目標バイアス値の符号を正に設定し、前記高度推定部で推定された前記高度が前記基本目標高度より高い場合、前記目標バイアス値の符号を負に設定する。
【００１３】
上記（３）の構成によれば、移動体の推定高度が基本目標高度より低い場合には目標バイアス値を正に設定することで、補正目標値を基本目標値に比べて大きくする。一方、移動体の推定高度が基本目標高度より高い場合には目標バイアス値を負に設定することで、補正目標値を基本目標値に比べて小さくする。このように推定高度に基づいて目標バイアス値の符号を設定することで、移動体の高度を補正目標高度に対して良好な応答性で追従させることができる。
【００１４】
（４）幾つかの実施形態では上記（２）又は（３）の構成において、
前記目標バイアス値算出部は、前記目標バイアス値の絶対値と前記高度との関係を規定する特性関数に基づいて、前記高度推定部で推定された前記高度に対応する前記目標バイアス値を算出する。
【００１５】
上記（４）の構成によれば、目標バイアス値は移動体の高度に応じて可変に設定することで、移動体の地形面からの高度に応じて制御目標値（補正目標値）を調整し、移動体の地形面への衝突可能性の軽減と、姿勢の安定化とを良好に両立させることができる。
【００１６】
（５）幾つかの実施形態では上記（４）の構成において、
前記特性関数は、前記高度が増加するに従って前記目標バイアス値の絶対値が減少するように規定される。
【００１７】
上記（５）の構成によれば、移動体の高度が大きくなるほど目標バイアス値の絶対値を小さくすることで、基本補正値に対する補正量を減少させる。これにより、移動体が地形面に近い場合には目標バイアス値による補正量を大きくすることで、地形面への衝突可能性を優先的に低減する一方で、移動体が地形面から遠い場合には衝突可能性が低いため、目標バイアス値による補正量を小さくすることで、安定性を優先した制御が可能となる。
【００１８】
（６）幾つかの実施形態では上記（４）又は（５）の構成において、
前記特性関数は、前記高度推定部で推定された前記高度が高度下限値以下である場合、前記目標バイアス値の絶対値が最大値になるように規定される。
【００１９】
上記（６）の構成によれば、移動体の高度が高度下限値を下回る程度に地形面に接近した場合、目標バイアス値の絶対値を最大値に設定することで、地形面への衝突回避を最優先することができる。
【００２０】
（７）幾つかの実施形態では上記（４）から（６）のいずれか一構成において、
前記目標バイアス値算出部は、前記特性関数として、
前記目標バイアス値の絶対値と前記高度とが線形特性を示す第１特性関数と、
前記第１特性関数に比べて前記高度に対する前記目標バイアス値の絶対値が大きく設定される第２特性関数と、
前記第１特性関数に比べて前記高度に対する前記目標バイアス値の絶対値が小さく設定される第３特性関数と、
のうちいずれかを選択可能に構成される。
【００２１】
上記（７）の構成によれば、互いに異なる特性を有する複数の特性関数を選択可能に構成することで、移動体の状況に応じてより柔軟な運転制御が可能となる。
【００２２】
（８）幾つかの実施形態では上記（７）の構成において、
前記目標バイアス値算出部は、
前記傾斜がゼロである場合、前記特性関数として前記第１特性関数を選択し、
前記傾斜がゼロより大きい場合、前記特性関数として前記第２特性関数を選択し、
前記傾斜がゼロより小さい場合、前記特性関数として前記第３特性関数を選択する。
【００２３】
上記（８）の構成によれば、地形面の傾斜に応じて特性関数を選択することで、移動体の地形面への衝突可能性を低減しつつ、移動体の姿勢を安定化することができる。
【００２４】
（９）幾つかの実施形態では上記（１）の構成において、
前記姿勢角及び前記高度は独立して制御可能である。
【００２５】
上記（９）の構成によれば、移動体の姿勢角と高度とが独立に制御可能な移動体（例えば、スラスタ等を備えることにより垂直上下方向に推力を発生可能な移動体）において、様々な地形面に対して安定的な姿勢を確保しつつ、地形面への衝突可能性を低減可能な運動制御が可能となる。
【００２６】
（１０）幾つかの実施形態では上記（９）の構成において、
前記目標バイアス値算出部は、前記傾斜に基づいて前記高度に対応する目標高度バイアス値を算出する目標高度バイアス値算出部と、前記傾斜に基づいて前記姿勢角に対応する目標姿勢角バイアス値を算出する目標姿勢角バイアス値算出部と、を含み、
前記補正目標値算出部は、前記基本高度目標値に前記目標高度バイアス値を加算することにより補正目標高度値を算出する補正目標高度値算出部と、前記基本姿勢角目標値に前記目標姿勢角バイアス値を加算することにより補正目標姿勢角値を算出する補正目標姿勢角値算出部と、を含み、
前記制御部は、前記高度が前記補正目標高度値になるとともに前記姿勢角が前記補正目標姿勢角値になるように、前記移動体を制御する。
【００２７】
上記（１０）の構成によれば、移動体の高度及び姿勢角をそれぞれ補正目標高度値及び補正姿勢角目標値に制御することで、様々な地形面に対して安定的な姿勢を確保しつつ、地形面への衝突可能性を低減可能な運動制御が可能となる。
【００２８】
（１１）本発明の少なくとも一実施形態に係る移動体の制御方法は上記課題を解決するために、
移動体の地形面に対する高度又は姿勢角の少なくとも一方を制御パラメータとする移動体の制御方法であって、
前記地形面の傾斜を推定するステップと、
前記傾斜の状況に基づいて調整される目標バイアス値を算出するステップと、
前記制御パラメータの基本目標値に前記目標バイアス値を加算することにより補正目標値を算出するステップと、
前記制御パラメータが前記補正目標値になるように前記移動体を制御するステップと、
を備える。
【００２９】
上記（１１）の方法によれば、移動体の運動制御に関する制御パラメータの目標値として、基本目標値に対して目標バイアス値を加算して算出される補正目標値が用いられる。補正目標値は、地形面の傾斜の状況に基づいて調整される目標バイアス値を含むことで、地形面の形状に対応して可変に設定される。これにより、様々な地形面に対して安定的な姿勢を確保しつつ、地形面への衝突可能性を低減可能な運動制御が可能となる。
【００３０】
（１２）本発明の少なくとも一実施形態に係るプログラムは上記課題を解決するために、
移動体の地形面に対する高度又は姿勢角の少なくとも一方を制御パラメータとする移動体を制御するためのプログラムであって、
コンピュータに、
前記地形面の傾斜を推定するステップと、
前記傾斜の状況に基づいて調整される目標バイアス値を算出するステップと、
前記制御パラメータの基本目標値に前記目標バイアス値を加算することにより補正目標値を算出するステップと、
前記制御パラメータが前記補正目標値になるように前記移動体を制御するステップと、
を実行させる。
【００３１】
上記（１２）のプログラムによれば、移動体の運動制御に関する制御パラメータの目標値として、基本目標値に対して目標バイアス値を加算して算出される補正目標値が用いられる。補正目標値は、地形面の傾斜の状況に基づいて調整される目標バイアス値を含むことで、地形面の形状に対応して可変に設定される。これにより、様々な地形面に対して安定的な姿勢を確保しつつ、地形面への衝突可能性を低減可能な運動制御が可能となる。
【発明の効果】
【００３２】
本発明の少なくとも一実施形態によれば、姿勢の安定化を図りつつ、地形面への衝突可能性を低減可能な移動体の制御装置、制御方法及びプログラムを提供できる。
【図面の簡単な説明】
【００３３】
【図１】第１実施形態に係る移動体が移動する様子を示す模式図である。
【図２】図１の制御装置のハードウェア構成を示す概略構成図である。
【図３】図１の制御装置の内部構成を機能的に示すブロック図である。
【図４】目標姿勢角バイアス値の符号設定方法の一例を示すフローチャートである。
【図５】目標姿勢角バイアス値の絶対値と高度との関係を規定する特性関数の一例である。
【図６】目標姿勢角バイアス値の絶対値設定方法の一例を示すフローチャートである。
【図７】第１実施形態に係る移動体の地形面に対する軌跡を示す模式図である。
【図８】第２実施形態に係る移動体の制御装置の内部構成を示すブロック図である。
【発明を実施するための形態】
【００３４】
以下、添付図面を参照して本発明の幾つかの実施形態について説明する。ただし、実施形態として記載されている又は図面に示されている構成部品の寸法、材質、形状、その相対的配置等は、本発明の範囲をこれに限定する趣旨ではなく、単なる説明例にすぎない。
【００３５】
（第１実施形態）
図１は第１実施形態に係る移動体１が移動する様子を示す模式図である。移動体１は、水中を自動で移動可能な水中移動体である。移動体１の移動制御は、後述の制御装置によって自動的に実施され、水底である地形面２に対して所定の高度及び姿勢角が維持される。尚、本実施形態では移動体１の一例として水中を移動で巡行する水中移動体について述べるが、地上にある空中を自動で移動可能な空中移動体（飛行体）についても、特段の記載がない限りにおいて同様である。また移動体１は有人であってもよいし、無人であってもよい。
【００３６】
移動体１の用途は限定されないが、本実施形態の移動体１は地形探査を目的として、地形面２に関する測定を行うための測定装置４が搭載される。測定装置４は例えばソーナー装置であり、水中を移動する移動体１から地形面２に向けて音波を照射し、地形面２からの反射成分を受信することで地形面２に関する情報を測定可能に構成される。
【００３７】
移動体１は、本体１ａと、本体１ａに設けられた推力発生装置１ｂとを備える。推力発生装置１ｂは例えばスクリューであり、移動体１が移動するための推力を発生させる。推力発生装置１ｂは、本体１ａ内に収容された制御装置１０によって制御されることで、移動体１の移動制御が実現される。図１の移動体１では、推力発生装置１ｂは本体１ａの後方に設けられているため、姿勢角αと地形面からの高度Ｈとは互いに依存する（すなわち姿勢角αを変更しようとする場合には高度Ｈも変化し、高度Ｈを変更しようとする場合には姿勢角αも変化する）。
【００３８】
尚、姿勢角αは図１ではピッチ角が例示されているが、ロール角であってもよいし、ヨー角であってもよい。
【００３９】
図２は、図１の制御装置１０のハードウェア構成を示す概略構成図である。制御装置１０は、図２に示すように、コンピュータで構成されていてもよい。具体的には、図２に示すように、制御装置１０は、ＣＰＵ１１（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）、ＲＡＭ１２（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）、ＲＯＭ１３（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）、ＨＤＤ１４（Ｈａｒｄ  Ｄｉｓｋ  Ｄｒｉｖｅ）、入力Ｉ／Ｆ１５、および出力Ｉ／Ｆ１６を含み、これらがバス１７を介して互いに接続されて構成される。
尚、制御装置１０のハードウェア構成は上記に限定されず、制御回路と記憶装置との組み合わせにより構成されても良い。
【００４０】
またクラウドや記憶媒体に、制御装置１０の機能を実現するためのプログラム（移動制御プログラム）を格納しておいてもよい。そして制御装置１０は、例えば４Ｇ、５Ｇ回線通信器やＷｉ−Ｆｉ（登録商標）等の無線ＬＡＮ通信器のような外部通信器を備え、ＣＰＵ１１が外部通信器を介してクラウドからプログラムを読み込み、ＲＡＭ１２にロードして実行してもよい。また制御装置１０は、記憶媒体のデータを読み取るためのドライバを備え、ＣＰＵ１１が記憶媒体からプログラムを読み込み、ＲＡＭ１２にロードして実行してもよい。記憶媒体の種類は問わず、例えばＳＤカード、ＵＳＢメモリ、外付けＨＤＤ等、プログラムの容量に応じた様々な記憶媒体を用いることができる。
【００４１】
図３は図１の制御装置１０の内部構成を機能的に示すブロック図である。制御装置１０は、高度推定部２０と、基本目標姿勢角算出部２２と、傾斜推定部２４と、目標姿勢角バイアス値算出部２６（目標バイアス値算出部）と、補正目標姿勢角算出部２８（補正目標値算出部）と、姿勢角制御部３０（制御部）と、姿勢角検出部３２と、を備える。
【００４２】
高度推定部２０は、移動体１に設けられた各種センサ（不図示）に基づいて、移動体１の地形面２に対する高度（高度推定値Ｈｓ）を推定する。本実施形態では、移動体１には地形探査を目的としてソーナー装置である測定装置４が搭載されているため、高度推定部２０は測定装置４の測定結果を取得することにより、高度推定値Ｈｓを求めてもよい。
【００４３】
基本目標姿勢角算出部２２は、制御装置１０の制御パラメータである姿勢角αの基本目標値である基本目標姿勢角αｂｔを算出する。前述したように図１の移動体１では高度Ｈ及び姿勢角αは互いに依存する関係にある。このような移動体１の特性に鑑み、基本目標姿勢角算出部２２は、外部から入力される目標高度Ｈｔ（例えばオペレータによって入力される）と、高度推定部２０によって推定された高度推定値Ｈｓとの差分ΔＨに基づいて、基本目標姿勢角αｂｔを算出する。差分ΔＨと基本目標姿勢角αｂｔとの関係は、例えばマップ等として予め規定されており、基本目標姿勢角算出部２２は、当該マップ等を参照することにより、入力される差分ΔＨに対応する基本目標姿勢角αｂｔを出力する。
尚、差分ΔＨと基本目標姿勢角αｂｔとの関係は、実験的又はシミュレーション的な手法によって予め求められ、不図示の記憶装置から適宜読み出し可能に構成されてもよい。
【００４４】
傾斜推定部２４は、移動体１が通過する地形面２の傾斜（傾斜推定値Ｌｓ）を推定する。傾斜推定部２４によって傾斜が推定される地形面２の位置は、例えば、移動体１の鉛直下方位置であってもよいし、移動体１からの最近位置であってもよい。本実施形態では、移動体１には地形探査を目的としてソーナー装置である測定装置４が搭載されているため、傾斜推定部２４は測定装置４の測定結果を取得することにより、傾斜推定値Ｌｓを求めてもよい。
【００４５】
目標姿勢角バイアス値算出部２６（目標バイアス値算出部）は、基本目標姿勢角αｂｔに加算するための目標姿勢角バイアス値αｂ（目標バイアス値）を算出する。目標姿勢角バイアス値αｂは、高度推定部２０から入力される高度推定値Ｈｓ、及び、傾斜推定部２４から入力される傾斜推定値Ｌｓの状況に基づいて調整されることで、移動体１の高度Ｈ及び地形面２の傾斜に応じて姿勢角αの制御目標値が可変に設定可能となる。
【００４６】
補正目標姿勢角算出部２８（補正目標算出部）は、基本目標姿勢角算出部２２で算出された基本目標姿勢角αｂｔを補正することにより、最終的な姿勢角αの目標値である補正目標姿勢角αｔ（補正目標値）を算出する。具体的には、基本目標姿勢角αｂｔに対して、傾斜推定部２４で算出された傾斜推定値Ｌｓ、及び、目標姿勢角バイアス値算出部２６で算出された目標姿勢角バイアス値αｂ（目標バイアス値）がそれぞれ加算されることにより、最終的な補正目標姿勢角αｔ（補正目標値）が算出される。
【００４７】
姿勢角制御部３０（制御部）は、補正目標姿勢角αｔ（補正目標値）と、姿勢角検出部３２で検出された姿勢角実測値αｄとの差分Δαに基づいて、姿勢角制御量Ｓαを出力することにより、移動体１（推力発生装置１ｂ）を制御する。例えば、差分Δαと姿勢角制御量Ｓαとの関係は、マップ等として予め規定されており、姿勢角制御部３０（制御部）は、当該マップ等を参照することにより、入力される差分Δαに対応する姿勢角制御量Ｓαを出力する。
尚、差分Δαと姿勢角制御量Ｓαとの関係は、実験的又はシミュレーション的な手法によって予め求められ、不図示の記憶装置から適宜読み出し可能に構成されてもよい。
【００４８】
このような制御装置１０では、移動体１の運動制御に関する制御パラメータである姿勢角αの目標値として、基本目標姿勢角αｂｔ（基本目標値）に対して目標姿勢角バイアス値αｂ（目標バイアス値）を加算して算出される補正目標姿勢角αｔ（補正目標値）が用いられる。補正目標姿勢角αｔ（補正目標値）は、地形面２の傾斜推定値Ｌｓに基づいて算出される目標姿勢角バイアス値αｂ（目標バイアス値）を含むことで、地形面２の形状に対応して可変に設定される。これにより、様々な地形面２に対して安定的な姿勢を確保しつつ、地形面２への衝突可能性を低減可能な運動制御が可能となる。
【００４９】
続いて目標姿勢角バイアス値αｂの設定方法について詳しく説明する。まず図４を参照して、目標姿勢角バイアス値αｂの符号の設定方法について説明する。図４は目標姿勢角バイアス値αｂの符号設定方法の一例を示すフローチャートである。
【００５０】
高度推定部２０は移動体１の高度推定値Ｈｓを算出する（ステップＳ１０）。そして目標姿勢角バイアス値算出部２６は、ステップＳ１０で算出された高度推定値Ｈｓを基本目標高度Ｈｂと比較する（ステップＳ１１）。ここで基本目標高度Ｈｂとは、地形面２に対する移動体１の高度の適正な標準値として予め設定される閾値であり、例えばオペレータからの入力に基づいて決定される。
【００５１】
高度推定値Ｈｓが基本目標高度Ｈｂより低い場合（Ｈｓ＜Ｈｂ）、目標姿勢角バイアス値算出部２６は目標姿勢角バイアス値αｂの符号を正に設定する（ステップＳ１２）。これにより、移動体１の高度が基本目標高度Ｈｂに満たない状態、すなわち、地形面２に対して標準値より接近している状態にある場合には、正の符号を有する目標姿勢角バイアス値αｂを基本目標姿勢角αｂｔに加算することで、基本目標姿勢角αｂｔより大きな補正目標姿勢角αｔが得られる。このように得られた基本目標姿勢角αｂｔに従って移動制御を実施することで、地形面２に対して標準値より接近している移動体１の高度を、標準値に近づけるように修正を促すことができる。
【００５２】
一方、高度推定値Ｈｓが基本目標高度Ｈｂより大きい場合（Ｈｓ＞Ｈｂ）、目標姿勢角バイアス値算出部２６は目標姿勢角バイアス値αｂの符号を負に設定する（ステップＳ１３）。これにより、移動体１の高度が基本目標高度Ｈｂを超える状態、すなわち、地形面２に対して標準値より離れている状態にある場合には、負の符号を有する目標姿勢角バイアス値αｂを基本目標姿勢角αｂｔに加算することで、基本目標姿勢角αｂｔより小さな補正目標姿勢角αｔが得られる。このように得られた基本目標姿勢角αｂｔに従って移動制御を実施することで、地形面２に対して標準値を超えている移動体１の高度を、標準値に近づけるように修正を促すことができる。
【００５３】
尚、高度推定値Ｈｓが基本目標高度Ｈｂと等しい場合（Ｈｓ＝Ｈｂ）、目標姿勢角バイアス値算出部２６は目標姿勢角バイアス値αｂをゼロに設定してもよい（ステップＳ１４）。これにより、移動体１の高度が基本目標高度Ｈｂに一致している状態にある場合には、目標姿勢角バイアス値αｂによる補正を行わないことで、移動体１の高度を適正に現状維持できる。
【００５４】
このように本実施形態では、高度推定値Ｈｓの大きさに基づいて基本目標姿勢角αｂｔに加算される目標姿勢角バイアス値αｂの符号が設定される。これにより、様々な凹凸を有する地形面２に対して移動体１の高度を適切に修正することで衝突可能性を低減しつつ、移動体１の姿勢を安定化することができる。
【００５５】
続いて目標姿勢角バイアス値αｂの絶対値の設定方法について説明する。目標姿勢角バイアス値算出部２６は、目標姿勢角バイアス値の絶対値｜αｂ｜と高度Ｈとの関係を規定する特性関数に基づいて、高度推定部２０で推定された高度推定値Ｈｓに対応する目標バイアス値の絶対値｜αｂ｜を算出してもよい。これにより、目標姿勢角バイアス値αｂは移動体１の高度Ｈに応じて可変に設定することができ、移動体１の地形面２からの高度に応じて制御目標値（補正姿勢角目標値αｔ）を調整し、移動体１の地形面２への衝突可能性の軽減と、姿勢の安定化とを良好に両立させることができる。
【００５６】
ここで図５は目標姿勢角バイアス値の絶対値｜αｂ｜と高度Ｈとの関係を規定する特性関数の一例である。図５では、第１特性関数ｆ１、第２特性関数ｆ２及び第３特性関数ｆ３が示されている。目標姿勢角バイアス値算出部２６は、これらの特性関数から目標姿勢角バイアス値αｂの算出に用いられる特性関数を適宜選択可能に構成されている。具体的には、第２特性関数ｆ２は第１特性関数ｆ１と比較しバイアス値を大きく算出する特性関数である。同様に、第３特性関数ｆ３は第１特性関数ｆ１と比較しバイアス値を小さく算出する特性関数である。なお、第１特性関数ｆ１、第２特性関数ｆ２及び第３特性関数ｆ３は図５に示すような１種類の関数とは限らず、それぞれ複数用意しておいてもよい。例えば、前記の特徴の範囲内で、それぞれ複数の特性関数を用意し、移動体１の速度や制御モードなどに基づいて選択可能に構成してもよい。
【００５７】
第１特性関数ｆ１、第２特性関数ｆ２及び第３特性関数ｆ３は、高度Ｈが増加するに従って、目標姿勢角バイアス値の絶対値｜αｂ｜が減少するように規定される。すなわち、これらの特性関数は、移動体１の高度Ｈが大きくなるほど目標姿勢角バイアス値の絶対値｜αｂ｜を小さくすることで、基本目標姿勢角αｂｔに対する補正量を減少させる。これにより、移動体１が地形面２に近い場合には目標姿勢角バイアス値αｂによる補正量を大きくすることで、地形面２への衝突可能性を優先的に低減する一方で、移動体１が地形面２から遠い場合には衝突可能性が低いため、目標姿勢角バイアス値αｂによる補正量を小さくすることで、安定性を優先した制御が可能となる。
【００５８】
また第１特性関数ｆ１、第２特性関数ｆ２及び第３特性関数ｆ３は、高度Ｈが高度下限値以下Ｈｍｉｎである場合、目標姿勢角バイアス値の絶対値｜αｂ｜が最大値になるように規定される。これにより、移動体１の高度Ｈが下限値Ｈｍｉｎを下回る程度に地形面２に接近した場合、目標姿勢角バイアス値の絶対値｜αｂ｜を最大値に設定することで、地形面２への衝突回避を最優先することができる。
【００５９】
また第１特性関数ｆ１、第２特性関数ｆ２及び第３特性関数ｆ３は、高度Ｈが高度上限値以上Ｈｍａｘである場合、目標姿勢角バイアス値の絶対値｜αｂ｜が最小値（ゼロ）になるように規定される。これにより、移動体１の高度Ｈが上限値Ｈｍａｘを上回る程度に地形面２から離隔した場合、目標姿勢角バイアス値の絶対値｜αｂ｜を最小値に設定することで、予定航路からの逸脱を抑制することができる。
【００６０】
第１特性関数ｆ１は、目標姿勢角バイアス値の絶対値｜αｂ｜と高度Ｈとが線形特性を示すように規定される。第２特性関数ｆ２は、第１特性関数ｆ１に比べて高度Ｈに対する目標姿勢角バイアス値の絶対値｜αｂ｜が大きく設定される。第３特性関数ｆ３は、第１特性関数ｆ１に比べて高度Ｈに対する目標姿勢角バイアス値の絶対値｜αｂ｜が小さく設定される。このように互いに異なる特性を有する複数の特性関数を選択可能に構成することで、移動体１の状況に応じてより柔軟な運転制御が可能となる。
【００６１】
上述の第１特性関数ｆ１、第２特性関数ｆ２及び第３特性関数ｆ３は、例えば、傾斜推定部２４で算出される傾斜推定値Ｌｓに基づいて選択される。図６は目標姿勢角バイアス値αｂの絶対値設定方法の一例を示すフローチャートである。
【００６２】
まず傾斜推定部２４は移動体１の傾斜推定値Ｌｓを算出する（ステップＳ２０）。そして目標姿勢角バイアス値算出部２６は、ステップＳ２０で算出された傾斜推定値Ｌｓの符号を判定する（ステップＳ２１）。例えば傾斜推定値Ｌｓがゼロである場合（Ｌｓ＝０）、特性関数として第１特性関数ｆ１を選択し（ステップＳ２２）、当該第１特性関数ｆ１を用いて目標姿勢角バイアス値αｂの絶対値を算出する（ステップＳ２３）。このように地形面２の傾斜が無い場合には、第２特性関数ｆ２及び第３特性関数ｆ３に比べて標準的な線形特性を有する特性関数ｆ１を選択することで、安定的な高度制御が可能となる。
【００６３】
一方、傾斜推定値Ｌｓがゼロより大きい場合（Ｌｓ＞０）、特性関数として第２特性関数ｆ２を選択し（ステップＳ２４）、当該第２特性関数ｆ２を用いて目標姿勢角バイアス値αｂの絶対値を算出する（ステップＳ２３）。第２特性関数ｆ２は標準的な第１特性関数ｆ１に比べて高度Ｈに対する目標姿勢角バイアス値の絶対値｜αｂ｜が大きく設定される。そのため、地形面２が上り勾配を有する場合には、第２特性関数ｆ２を選択することで、姿勢角αを正方向に向けやすくできる。
【００６４】
また傾斜推定値Ｌｓがゼロより小さい場合（Ｌｓ＜０）、特性関数として第３特性関数ｆ３を選択し（ステップＳ２５）、当該第３特性関数ｆ３を用いて目標姿勢角バイアス値αｂの絶対値を算出する（ステップＳ２３）。第３特性関数ｆ３は標準的な第１特性関数ｆ１に比べて高度Ｈに対する目標姿勢角バイアス値の絶対値｜αｂ｜が小さく設定される。そのため、地形面２が下り勾配を有する場合には、過剰に姿勢角αを下方に向けると地形面２に対する衝突可能性が増えてしまうため、第２特性関数ｆ２を選択することで、このような衝突可能性を効果的に低減できる。
【００６５】
このように高度推定値Ｈｓ及び傾斜推定値Ｌｓに基づいて目標姿勢角バイアス値αｂの符号及び絶対値を設定することにより、様々な地形面２に対して安定的な姿勢を確保しつつ、地形面２への衝突可能性を低減可能な運動制御が可能となる。例えば図７に示すように、凹凸を含む地形面２に対する移動体１の軌跡は図１と比較して明らかなように、最低高度を確保可能な範囲で高度を確保しつつ、姿勢角の変化を抑えた安定した運転が実現される。
【００６６】
（第２実施形態）
続いて第２実施形態に係る移動体１について説明する。第２実施形態に係る移動体１は、前述の第１実施形態に係る移動体１と同様に本体１ａと、推力発生装置１ｂとを備えるが、推力発生装置１ｂは例えば垂直上下方向に推力を発生可能なスラスタであり、姿勢角及び高度が独立して制御可能に構成される点で異なる。
【００６７】
図８は第２実施形態に係る移動体１の制御装置１０の内部構成を示すブロック図である。制御装置１０は、制御装置１０は、高度推定部２０と、傾斜推定部２４と、目標高度バイアス値算出部４０（目標バイアス値算出部）と、補正目標高度算出部４２（補正目標値算出部）と、高度制御部４４（制御部）と、目標姿勢角バイアス値算出部４６（目標バイアス値算出部）と、補正目標姿勢角算出部４８（補正目標値算出部）と、姿勢角推定部５０と、姿勢角制御部５２（制御部）と、を備える。
【００６８】
目標高度バイアス値算出部４０（目標バイアス値算出部）は、例えばオペレータによって入力される基本目標高度Ｈｂｔに加算するための目標高度バイアス値Ｈｂ（目標バイアス値）を算出する。目標高度バイアス値Ｈｂ（目標バイアス値）は、傾斜推定部２４から入力される傾斜推定値Ｌｓに基づいて算出される。これにより、地形面２の傾斜に応じて高度Ｈの制御目標値が可変に設定可能となる。
【００６９】
補正目標高度算出部４２（補正目標値算出部）は、基本目標高度Ｈｂｔに対して、目標高度バイアス値算出部４０（目標バイアス値算出部）で算出された目標高度バイアス値Ｈｂ（目標バイアス値）を加算することにより補正し、最終的な高度Ｈの目標値である補正目標高度Ｈｔ（補正目標値）を算出する。
【００７０】
高度制御部４４（制御部）は、補正目標高度Ｈｔ（補正目標値）と、高度推定部２０で推定された高度推定値Ｈｓとの差分ΔＨに基づいて、高度制御量ＳＨを出力することにより、移動体１（推力発生装置１ｂ）を制御する。例えば、差分ΔＨと高度制御量ＳＨとの関係は、マップ等として予め規定されており、高度制御部４４（制御部）は、当該マップ等を参照することにより、入力される差分ΔＨに対応する高度制御量ＳＨを出力する。
尚、差分ΔＨと高度制御量ＳＨとの関係は、実験的又はシミュレーション的な手法によって予め求められ、不図示の記憶装置から適宜読み出し可能に構成されてもよい。
【００７１】
目標姿勢角バイアス値算出部４６（目標バイアス値算出部）は、例えばオペレータによって入力される基本目標姿勢角αｂｔに加算するための目標姿勢角バイアス値αｂ（目標バイアス値）を算出する。目標姿勢角バイアス値αｂ（目標バイアス値）は、傾斜推定部２４から入力される傾斜推定値Ｌｓに基づいて算出される。これにより、地形面２の傾斜に応じて姿勢角αの制御目標値が可変に設定可能となる。
【００７２】
補正目標姿勢角算出部４８（補正目標値算出部）は、基本目標姿勢角αｂｔに対して、傾斜推定部２４で算出された傾斜推定値Ｌｓ、及び、目標姿勢角バイアス値算出部４６（目標バイアス値算出部）で算出された目標姿勢角バイアス値αｂ（目標バイアス値）を加算することにより補正し、最終的な姿勢角αの目標値である補正目標姿勢角αｔ（補正目標値）を算出する。
【００７３】
姿勢角推定部５０は、移動体１に設けられた各種センサ（不図示）に基づいて、移動体１の地形面２に対する姿勢角（姿勢角推定値αｓ）を推定する。本実施形態では、移動体１には地形探査を目的としてソーナー装置である測定装置４が搭載されているため、姿勢角推定部５０は測定装置４の測定結果を取得することにより、姿勢角推定値αｓを求めてもよい。
【００７４】
姿勢角制御部５２（制御部）は、補正目標姿勢角αｔ（補正目標値）と、姿勢角推定部５０で推定された姿勢角推定値αｓとの差分Δαに基づいて、姿勢角制御量Ｓαを出力することにより、移動体１（推力発生装置１ｂ）を制御する。例えば、差分Δαと姿勢角制御量Ｓαとの関係は、マップ等として予め規定されており、姿勢角制御部５２（制御部）は、当該マップ等を参照することにより、入力される差分Δαに対応する姿勢角制御量Ｓαを出力する。
尚、差分Δαと姿勢角制御量Ｓαとの関係は、実験的又はシミュレーション的な手法によって予め求められ、不図示の記憶装置から適宜読み出し可能に構成されてもよい。
【００７５】
このように第２実施形態では、高度Ｈ及び姿勢角αの基本目標値をバイアス値でそれぞれ補正して求めた各補正目標値に従って制御が実施される。これにより、移動体１の姿勢角αと高度Ｈとが独立に制御可能な移動体１においても、前述の第１実施形態と同様に、様々な地形面に対して安定的な姿勢を確保しつつ、地形面への衝突可能性を低減可能な運動制御が可能となる。
【００７６】
また第２実施形態における目標高度バイアス値αＨ及び目標姿勢角バイアス値αｂは、第１実施形態における目標姿勢角バイアス値αｂと同様に、傾斜推定値Ｌｓに基づいて符号及び絶対値を可変に設定することにより、様々な地形面２に対して安定的な姿勢を確保しつつ、地形面２への衝突可能性を低減可能な運動制御が可能となる。
【００７７】
以上説明したように上述の各実施形態によれば、姿勢の安定化を図りつつ、地形面への衝突可能性を低減可能な移動体の制御装置、制御方法及びプログラムを提供できる。
【産業上の利用可能性】
【００７８】
本発明の少なくとも一実施形態は、移動体の制御装置、制御方法及びプログラムに利用可能である。
【符号の説明】
【００７９】
１  移動体
１ａ  本体
１ｂ  推力発生装置
２  地形面
４  測定装置
１０  制御装置
２０  高度推定部
２２  基本目標姿勢角算出部
２４  傾斜推定部
２６  目標姿勢角バイアス値算出部
２８  補正目標姿勢角算出部
３０  姿勢角制御部
３２  姿勢角検出部
４０  目標高度バイアス値算出部
４２  補正目標高度算出部
４４  高度制御部
５０  姿勢角推定部
【図１】
2020192906.tif 000003
【図２】
2020192906.tif 000004
【図３】
2020192906.tif 000005
【図４】
2020192906.tif 000006
【図５】
2020192906.tif 000007
【図６】
2020192906.tif 000008
【図７】
2020192906.tif 000009
【図８】
2020192906.tif 000010