JP2024006934A - 経路設定方法、移動制御方法、プログラム、経路設定装置、及び移動体 - Google Patents

Abstract

【課題】移動可能な経路を適切に設定する。【解決手段】経路設定方法は、移動元の位置及び移動元における移動体の向きを示す移動元情報を取得するステップと、移動元に対して所定関係にある移動先の位置、及び移動先における移動体の向きを示す移動先情報を設定するステップと、移動元情報と移動先情報とに基づき、移動元から移動先までの移動体の移動する軌跡を設定するステップと、軌跡に基づいて、出発位置から到着位置までの移動体の経路を設定するステップと、を含む。【選択図】図３

Description

本開示は、経路設定方法、移動制御方法、プログラム、経路設定装置、及び移動体に関する。

船舶、水中航行体、飛行体などの移動体の経路を、自動で設定する技術が知られている。移動体の経路を自動で設定する際には、例えば、ユーザによりウェイポイントが指定されて、指定されたウェイポイントを通るように経路が設定される。また例えば特許文献１には、３次元空間内に設けられる複数のノードとリンクに関する飛行ルート探索用データに基づき、ドローンの運行計画を作成する旨が記載されている。

特許第６７７２１００号公報

このように移動体の経路を自動で設定する際には、移動体が移動可能な経路を適切に設定することが求められている。

本開示は、上述した課題を解決するものであり、移動可能な経路を適切に設定可能な経路設定方法、移動制御方法、プログラム、経路設定装置、及び移動体を提供することを目的とする。

本開示に係る経路設定方法は、移動元の位置及び前記移動元における移動体の向きを示す移動元情報を取得するステップと、前記移動元に対して所定関係にある移動先の位置、及び前記移動先における前記移動体の向きを示す移動先情報を設定するステップと、前記移動元情報と前記移動先情報とに基づき、前記移動元から前記移動先までの前記移動体の移動する軌跡を設定するステップと、前記軌跡に基づいて、出発位置から到着位置までの前記移動体の経路を設定するステップと、を含む。

本開示に係るプログラムは、移動元の位置及び前記移動元における移動体の向きを示す移動元情報を取得するステップと、前記移動元に対して所定関係にある移動先の位置、及び前記移動先における前記移動体の向きを示す移動先情報を設定するステップと、前記移動元情報と前記移動先情報とに基づき、前記移動元から前記移動先までの前記移動体の移動する軌跡を設定するステップと、前記軌跡に基づいて、出発位置から到着位置までの前記移動体の経路を設定するステップと、をコンピュータに実行させる。

本開示に係る経路設定装置は、移動元の位置及び前記移動元における移動体の向きを示す移動元情報を取得する第１取得部と、前記移動元に対して所定関係にある移動先の位置、及び前記移動先における前記移動体の向きを示す移動先情報を設定する第１設定部と、前記移動元情報と前記移動先情報とに基づき、前記移動元から前記移動先までの前記移動体の移動する軌跡を設定する第２設定部と、前記軌跡に基づいて、出発位置から到着位置までの前記移動体の経路を設定する第３設定部と、を含む。

本開示に係る移動体は、前記経路設定装置を含む。

本開示によれば、移動可能な経路を適切に設定できる。

図１は、本実施形態に係る移動体の模式図である。 図２は、本実施形態に係る制御装置の模式的なブロック図である。 図３は、経路の設定を説明するための模式図である。 図４は、軌跡の設定の一例を示す模式図である。 図５Ａは、軌跡の進行方向に基づいた判定基準の設定の一例を説明する模式図である。 図５Ｂは、各方向における距離閾値の一例を示す模式図である。 図６は、経路の設定方法の処理フローを説明するフローチャートである。

以下に添付図面を参照して、本開示の好適な実施形態を詳細に説明する。なお、この実施形態により本開示が限定されるものではなく、また、実施形態が複数ある場合には、各実施形態を組み合わせて構成するものも含むものである。

（第１実施形態）
（移動体）
図１は、本実施形態に係る移動体の模式図である。本実施形態に係る移動体１０は、自律移動する装置である。ただしそれに限られず、移動体１０は、運転者の操作により移動するものであってもよい。また、移動体１０は、任意の環境で用いられる移動体であってよいが、移動元の移動体１０の向きが、移動体１０の移動に影響を及ぼすような環境で用いられるものであることが好ましい。本実施形態においては、移動体１０は、水Ｗ上（水面）を移動する船舶であるが、それに限られず、例えば、水Ｗ内（水中）を移動する水中航行体であってもよいし、飛行する飛行体であってもよい。

図１に示すように、移動体１０は、制御対象１２と制御装置１４とを有する。制御対象１２は、制御装置１４によって制御される機器であり、例えば、移動体１０を駆動させる駆動機構である。

以降において、水平面に沿った一方向をＸ方向とし、水平面に沿った方向であって方向Ｘに直交する方向を、Ｙ方向とする。本実施形態においては、「位置」とは、特に断りのない限り、Ｘ方向及びＹ方向の二次元座標系における位置（座標）を指す。また、移動体１０の「向き」とは、特に断りのない限り、Ｘ方向及びＹ方向の二次元座標系における移動体１０の向きであり、例えば、鉛直方向から見た場合の、Ｘ方向を０°とした際の移動体１０のヨー角(回転角度)を指す。なお、例えば移動体１０が飛行体である場合には、「位置」は、Ｘ方向、Ｙ方向、及び鉛直方向の三次元座標系における位置（座標）としてよく、「向き」は、Ｘ方向、Ｙ方向、及び鉛直方向における任意の方向における向きとしてよい。

（制御装置）
図２は、本実施形態に係る制御装置の模式的なブロック図である。制御装置１４は、例えばコンピュータであり、図２に示すように、入力部２０と、表示部２２と、記憶部２４と、制御部２６とを有する。なお、制御装置１４は、単体の装置で構成してもよいし、他の装置と一体に構成してもよいし、演算装置及びデータサーバ等の各種装置を組み合わせたシステムとして構成してもよく、特に限定されない。

入力部２０は、例えば運転者による移動体１０の操作を受け付ける機構である。表示部２２は、例えばモニタ等の表示デバイスであり、制御装置１４によって設定された経路Ｒの情報などを表示する。記憶部２４は、制御部２６の演算内容やプログラムの情報などを記憶するメモリであり、例えば、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）と、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）のような主記憶装置と、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）などの外部記憶装置とのうち、少なくとも１つ含む。

制御部２６は、演算装置であり、例えばＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）などの演算回路を含む。制御部２６は、第１取得部３２と、第１設定部３４と、第２設定部３６と、第３設定部３８と、移動制御部４０とを含む。制御部２６は、記憶部２４からプログラム（ソフトウェア）を読み出して実行することで、第１取得部３２と第１設定部３４と第２設定部３６と第３設定部３８と移動制御部４０を実現して、それらの処理を実行する。なお、制御部２６は、１つのＣＰＵによって処理を実行してもよいし、複数のＣＰＵを備えて、それらの複数のＣＰＵで、処理を実行してもよい。また、第１取得部３２と第１設定部３４と第２設定部３６と第３設定部３８と移動制御部４０との少なくとも一部を、ハードウェア回路で実現してもよい。また、記憶部２４が保存する制御部２６用のプログラムは、制御装置１４が読み取り可能な記録媒体に記憶されていてもよい。

なお、本実施形態では、第１取得部３２と第１設定部３４と第２設定部３６と第３設定部３８とにより、後述する移動体１０の経路Ｒを設定するため、第１取得部３２と第１設定部３４と第２設定部３６と第３設定部３８とで、移動体１０の経路Ｒを設定する経路設定部３０を構成するといえる。本実施形態においては、制御装置１６は、経路設定部３０を含んで経路Ｒを設定する経路設定装置としての機能と、移動制御部４０を含んで移動体１０の移動を制御する制御装置としての機能とを兼ね備える。ただしそれに限られず、経路設定部３０を含んで経路Ｒを設定する経路設定装置と、移動制御部４０を含んで移動体１０の移動を制御する制御装置とを、別の装置としてもよい。経路設定装置と制御装置を別の装置とする場合、経路設定装置は、移動体１０に設けられてもよいし、移動体１０に設けられなくてもよい。

（経路）
図３は、経路の設定を説明するための模式図である。図３に示すように、制御装置１６は、出発位置Ｓから到着位置Ｇまでの経路Ｒを設定する。図３の例では、出発位置Ｓは、港湾設備より外海側の位置であり、到着位置Ｇは、港湾設備内に設けられた接岸位置である。すなわち、図３の例では、経路Ｒは、移動体１０を接岸するための経路といえる。ただしそれに限られず、出発位置Ｓ及び到着位置Ｇは、任意の位置であってよい。出発位置Ｓ及び到着位置Ｇは、例えば運転者によって設定されてよい。

制御装置１６は、複数の軌跡Ｔを設定して、出発位置Ｓから到着位置Ｇまで複数の軌跡Ｔを繋いだ経路を、経路Ｒとして設定する。以下、経路Ｒの設定方法について具体的に説明する。

（第１取得部）
第１取得部（移動元情報取得部）３２は、経路Ｒの設定に用いるための移動元情報を取得する。移動元情報とは、移動元の位置（座標）と、移動元における移動体１０の向きとを示す情報である。移動元とは、これから設定する軌跡Ｔの出発位置を指す。

第１取得部３２は、これから設定する軌跡Ｔよりも前に軌跡Ｔが設定されていない場合には、すなわち最初の軌跡Ｔを設定する場合には、出発位置Ｓの位置と、出発位置Ｓにおける移動体１０の向きとを、移動元情報として取得する。出発位置Ｓにおける移動体１０の向きは、適宜設定されてよく、例えば移動体１０の現在位置が出発位置Ｓ上である場合には、現在位置における移動体１０の向きを、出発位置Ｓにおける移動体１０の向きとしてよい。また例えば、移動体１０の現在位置が出発位置Ｓ上にない場合には、出発位置Ｓに到達した際の移動体１０の向きの予測値を、出発位置Ｓにおける移動体１０の向きとしてよい。

第１取得部３２は、これから設定する軌跡Ｔよりも前に軌跡Ｔが設定されている場合には、すなわち２つ目以降の軌跡Ｔを設定する場合には、直前に設定した軌跡Ｔの終点位置と、その終点位置における移動体１０の向きとを、移動元情報として取得する。

（第１設定部）
第１設定部３４（移動先設定部）は、移動元情報に基づき、経路Ｒの設定に用いるための移動先情報を設定する。移動先情報とは、移動先の位置（座標）と、移動先における移動体１０の向きとを示す情報である。第１設定部３４は、移動元よりも到着位置Ｇに近づき、かつ、移動元に対して所定関係にある位置を、移動先として設定する。本実施形態では、第１設定部３４は、移動元からＸ方向に所定の単位距離だけ離れることと、移動元からＹ方向に所定の単位距離だけ離れることとの、少なくとも一方を満たし、かつ移動元よりも到着位置Ｇに近づく位置を、移動先として設定する。また、第１設定部３４は、移動元における移動体１０の向きに対して所定関係にある向きを、移動先における移動体１０の向きとして設定する。本実施形態では、第１設定部３４は、移動元における移動体１０の向きから所定の単位角度だけずれた向きを、移動先における移動体１０の向きとして設定する。

言い換えれば、第１設定部３４は、図３に示すように、移動先として探索する状態空間を、Ｘ方向、Ｙ方向及びヨー角の３次元で離散化して、離散化した状態空間において、移動元の格子点Ｎに隣接する格子点Ｎとその格子点Ｎにおける移動体１０の向きを選択して、移動先及び移動先における移動体１０の向きとして設定する。

より詳しくは、本実施形態では、第１設定部３４は、移動元に対して所定関係にあり、移動元よりも到着位置Ｇに近づく位置にあり、かつ障害物Ｍと干渉しない格子点Ｎを、移動先として設定する。障害物Ｍと格子点Ｎとが干渉するかの判断基準は、任意であってよいが、例えば、第１設定部３４は、障害物Ｍの位置に対して所定距離の範囲外にある格子点Ｎを、障害物Ｍに干渉しないとして、障害物Ｍの位置に対して所定距離の範囲内にある格子点Ｎを、障害物Ｍに干渉するとしてよい。この場合、第１設定部３４は、例えば障害物Ｍの位置が示された地図情報を取得して、障害物Ｍの位置を把握する。ここでの所定距離は、任意に設定されてよい。

本実施形態では、第１設定部３４は、１つの移動元に対して、位置が異なる複数の移動先と、移動先毎の移動体１０の向きとを設定する。すなわち、第１設定部３４は、１つの移動元に対して、移動先の位置と移動体１０の向きとを示す１セットの移動先情報を、複数セット設定する。

（第２設定部）
第２設定部（軌跡設定部）３６は、移動元情報と移動先情報とに基づき、移動元から移動先までの、移動体１０が移動する軌跡Ｔを設定する。第２設定部３６は、移動元において移動体１０が移動元情報の示す向きとなり、移動先において移動体１０が移動先情報の示す向きとなり、かつ、移動元を始点として移動先を終点とする軌跡を算出して、軌跡Ｔとして設定する。さらに言えば、第２設定部３６は、移動体１０の運動方程式に基づいて、移動元において移動元情報が示す向きとなり、移動先において移動先情報が示す向きとなり、かつ、移動元を始点として移動先を終点とする、移動体１０が実現可能な軌跡を算出して、軌跡Ｔとして設定する。移動体１０の運動方程式は、移動体１０の動特性を示す式である。本実施形態では、移動体１０の運動方程式は、移動元における移動体１０への制御入力（例えば操舵角や印加トルクなどの操作量）を入力値として、その制御入力がなされた場合の制御出力（例えば移動体１０の位置及び向き）を出力値とする数式である。移動体１０の運動方程式は、例えば移動体１０の性能などに基づいて、予め設定される。

図４は、軌跡の設定の一例を示す模式図である。本実施形態の例では、１つの移動元について、移動先情報（移動先及び移動体１０の向き）が複数セット設定されるので、第２設定部３６は、移動先情報毎に、軌跡Ｔを設定する。すなわち、第２設定部３６は、１つの移動元からの複数の軌跡Ｔを設定する。図４の左側の図は、移動元が格子点Ｎ１であり移動元における移動体１０の向きがＹ方向である場合の軌跡Ｔの一例を示している。図４の左側の図の例では、移動先を格子点Ｎ２Ｂとして移動先における向きがＹ方向となる軌跡Ｔ１と、移動先を格子点Ｎ２Ｃとして移動先における向きがＹ方向となる軌跡Ｔ２と、移動先を格子点Ｎ２Ｃとして移動先における向きがＹ方向から４５度ずれた向きとなる軌跡Ｔ３と、移動先を格子点Ｎ２Ｃとして移動先における向きがＸ方向となる軌跡Ｔ４と、移動先を格子点Ｎ２Ａとして移動先における向きがＹ方向となる軌跡Ｔ５と、移動先を格子点Ｎ２Ａとして移動先における向きがＹ方向から４５度ずれた向きとなる軌跡Ｔ６と、移動先を格子点Ｎ２Ａとして移動先における向きがＸ方向となる軌跡Ｔ７とが設定される。また、図４の右側の図は、移動元が格子点Ｎ１であり移動元における移動体１０の向きがＹ方向から４５度ずれた向きである場合の軌跡Ｔの一例を示している。図４の右側の図の例では、移動先を格子点Ｎ２Ｃとして移動先における向きがＹ方向から４５度ずれた向きとなる軌跡Ｔ８と、移動先を格子点Ｎ２Ｃとして移動先における向きがＸ方向となる軌跡Ｔ９と、移動先を格子点Ｎ２Ｃとして移動先における向きがＹ方向となる軌跡Ｔ１０とが設定される。ただし、図４に示す軌跡は一例である。

（第３設定部）
第３設定部（経路設定部）３８は、第２設定部３６が設定した軌跡Ｔに基づいて、出発位置Ｓから到着位置Ｇまでの経路Ｒを設定する。

本実施形態においては、第３設定部３８は、同じ移動元から設定された複数の軌跡Ｔのうちから、経路Ｒとして用いる軌跡Ｔを選択する。複数の軌跡Ｔのうちから経路Ｒとして用いる軌跡Ｔを選択する方法は任意であってよいが、本実施形態では、第３設定部３８は、同じ移動元から設定された複数の軌跡Ｔのうちから、障害物Ｍと干渉しない軌跡Ｔを、経路Ｒとして用いる軌跡Ｔとして選択する。障害物Ｍと軌跡Ｔとが干渉するかの判断基準は、任意であってよいが、本実施形態では、第３設定部３８は、障害物Ｍの位置に対して所定距離の範囲外にある軌跡Ｔを、障害物Ｍに干渉しないとして、障害物Ｍの位置に対して所定距離の範囲内にある軌跡Ｔを、障害物Ｍに干渉するとしてよい。

図５Ａは、軌跡の進行方向に基づいた判定基準の設定の一例を説明する模式図である。第３設定部３８は、軌跡Ｔの進行方向に基づいて判定基準を設定し、判定基準に基づいて、軌跡Ｔが障害物と干渉するかを判定してもよい。具体的には、軌跡Ｔの進行方向に直交する第１方向側での距離閾値を距離閾値Ｄ１とし、第１方向と反対の第２方向での距離閾値を距離閾値Ｄ２とする。距離閾値とは、障害物Ｍと軌跡Ｔとが干渉するかを判断する閾値であり、障害物Ｍと軌跡Ｔとの距離が距離閾値以下である場合に、干渉すると判断される。この場合、第３設定部３８は、移動元における移動体１０の向きに対して軌跡Ｔの進行方向が交差するかを判断する。図５Ａに示すように、移動元（位置ＮＡ）における移動体１０の向きに対して軌跡ＴＡの進行方向が交差しない場合（移動体１０の向きが軌跡Ｔの進行方向に沿っている場合）、例えば、第３設定部３８は、距離閾値Ｄ１と距離閾値Ｄ２とを同じ値にする。一方、移動元における移動体１０の向きに対して軌跡Ｔの進行方向が交差する場合、第３設定部３８は、距離閾値Ｄ１と距離閾値Ｄ２とを異なる値とする。具体的には、図５Ａに示すように、第３設定部３８は、軌跡ＴＢの進行方向が、移動元（位置ＮＢ）における移動体１０の向きに対して第１方向側（図５Ａの例では右側）を向いている場合には、距離閾値Ｄ２を距離閾値Ｄ１より長くする。また、軌跡Ｔの進行方向が、移動元における移動体１０の向きに対して第２方向側を向いている場合には、第３設定部３８は、距離閾値Ｄ１を距離閾値Ｄ２より長くする。すなわち、移動体１０が曲がる場合には、曲がる方向と反対側に膨らむ可能性があるため、第３設定部３８は、曲がる方向と反対側における距離閾値を長くして、曲がる方向と反対側に膨らんだとしても、障害物Ｍに干渉することを抑制する。

なお、図５Ａの例では、第３設定部３８は、軌跡Ｔの進行方向に直交する第１方向及び第２方向（左右方向）における距離閾値Ｄ１、Ｄ２を設定していたが、それに限られず、例えば図５Ｂに示すように、各方向における距離閾値を、方向毎に設定してもよい。図５Ｂは、各方向における距離閾値の一例を示す模式図である。この場合例えば、図５Ｂに示すように、第３設定部３８は、各方向における距離閾値を、方向毎に設定して、移動体１０から各方向に距離閾値だけ離れた各点で囲われる領域を、障害物Ｍと干渉するかを判断するための判定領域ＡＲとして設定してよい。すなわちこの場合、障害物Ｍが判定領域ＡＲ内に位置する場合には、障害物Ｍに干渉すると判定し、障害物Ｍが判定領域ＡＲ外に位置する場合には、障害物Ｍに干渉しないと判定する。なお、各方向での距離閾値の設定方法は任意であってよいが、例えば、第３設定部３８は、左右方向における距離閾値Ｄ１、Ｄ２については、図５Ａの説明と同様の方法で設定してよい。そして例えば、第３設定部３８は、軌跡Ｔの進行方向に沿った第３方向（前方向）における距離閾値Ｄ３を、軌跡Ｔの進行方向と反対方向の第３方向（後方向）における距離閾値Ｄ４よりも長くしてよい。これにより、例えば進行方向側に想定より進んでしまった場合にも、障害物Ｍに干渉することを抑制できる。また、第３設定部３８は、移動体１０の形状に基づき、判定領域ＡＲを設定してよい。例えば、第３設定部３８は、各方向について、移動体１０の外縁部から、設定した距離閾値だけ離れた領域を、判定領域ＡＲとして設定する。従って例えば、方向毎の距離閾値が一定であり、移動体１０が前後方向に長い場合には、判定領域ＡＲは前後方向に長い形状となる。

また、第３設定部３８は、同じ移動元から設定された複数の軌跡Ｔのうちから、移動先まで移動するためのエネルギ消費が最小となる軌跡Ｔを、経路Ｒとして用いる軌跡Ｔとして選択してよい。本実施形態では、第３設定部３８は、同じ移動元から設定された複数の軌跡Ｔのうちで、障害物Ｍと干渉しない軌跡Ｔが複数ある場合には、それらの複数の軌跡Ｔのうちでエネルギ消費が最小となる軌跡Ｔを、経路Ｒとして用いる軌跡Ｔとして選択する。第３設定部３８は、それぞれの軌跡Ｔに基づいて、それぞれの軌跡Ｔについてのエネルギ消費を算出して、エネルギ消費が最小となる軌跡Ｔを抽出してもよい。なお、エネルギ消費とは、移動元から移動先に移動した場合に消費するエネルギ量の推定値である。例えば、第３設定部３８は、軌跡Ｔを移動するためのアクチュエータの消費エネルギの時間積分値を、エネルギ消費として算出してよい。また例えば、第３設定部３８は、障害物Ｍに干渉しない軌跡Ｔのうちで、移動先までの移動に要する所要時間が最小となる軌跡Ｔを、経路Ｒの設定に用いる軌跡Ｔとして選択してよい。所要時間は、軌跡Ｔ毎に任意に算出されてよく、例えば軌跡Ｔの長さに基づき算出してもよいし、軌跡Ｔの移動のために操作される操舵角の変化量に基づき算出してもよい。

以上のように経路Ｒとして用いる軌跡Ｔが選択されたら、第２設定部３６は、直前に選択された軌跡Ｔの終点を移動元として、次の軌跡Ｔを設定し、第３設定部３８は、設定された軌跡Ｔから、経路Ｒとして用いる次の軌跡Ｔを選択する。第２設定部３６及び第３設定部３８は、移動先の位置が到着位置Ｇに達するまで、軌跡Ｔの設定と経路Ｒとして用いる軌跡Ｔの選択とを繰り返す。すなわち、第２設定部３６は、最初に出発位置Ｓを移動元とした軌跡Ｔを設定して、移動先が到着位置Ｇに達するまで、直前に選択された軌跡Ｔの終点を移動元とした軌跡Ｔの設定を繰り返す。第３設定部３８は、軌跡Ｔが設定される毎に、それらの軌跡Ｔから経路Ｒに用いる軌跡Ｔを選択して、選択したそれぞれの軌跡Ｔを繋いだ経路を、経路Ｒとして設定する。

第３設定部３８は、設定した経路Ｒの情報を出力する。例えば、第３設定部３８は、経路Ｒを表示部２２に表示させてもよいし、別の装置に設定した経路Ｒの情報を送信してもよい。

なお、第３経路設定部３８は、選択したそれぞれの軌跡Ｔを繋いだ経路を補正して、経路Ｒを設定してよい。例えば、第３経路設定部３８は、選択したそれぞれの軌跡Ｔを繋いだ経路を緩和した経路を、経路Ｒとして設定してよい。ここでの緩和とは、経路の向きの変位を小さくする処理を指す。緩和された経路Ｒは、出発位置Ｓ及び到着位置Ｇを通り、かつ、選択したそれぞれの軌跡Ｔを繋いだ経路よりも、経路の向きの変位量が小さい経路となる。

（処理フロー）
以上説明した経路Ｒの設定方法の処理フローを説明する。図６は、経路の設定方法の処理フローを説明するフローチャートである。図６に示すように、制御装置１４は、第１取得部３２により、移動元の位置及び移動元における移動体１０の向きを示す移動元情報を取得する（ステップＳ１０）。第１取得部３２は、直前に設定した移動先情報を、移動元情報として取得する。そして、移動元が到着位置Ｇに達している場合には（ステップＳ１２；Ｙｅｓ）、言い換えれば直前に設定した移動先が到着位置Ｇに到達している場合には、これまで選択した軌跡Ｔに基づいて、経路Ｒを設定して、本処理を終了する。一方、移動元が到着位置Ｇに達していない場合には（ステップＳ１２；Ｎｏ）、言い換えれば直前に設定した移動先が到着位置Ｇに到達していない場合には、制御装置１４は、第１設定部３４により、取得された移動元情報に基づき、移動先の位置及び移動先における移動体１０の向きを示す移動先情報を設定し（ステップＳ１４）、第２設定部３６により、移動元から移動先までの軌跡Ｔを設定する（ステップＳ１６）。そして、全ての移動先情報に基づいた軌跡Ｔの設定が完了していない場合には（ステップＳ１８；Ｎｏ）、ステップＳ１６に戻り、他の移動先情報に基づいた軌跡Ｔを設定する。全ての移動先情報に基づいた軌跡Ｔの設定が完了した場合には（ステップＳ１８；Ｙｅｓ）、制御装置１４は、第３設定部３８により、経路Ｒの設定に用いる軌跡Ｔを選択して（ステップＳ２０）、ステップＳ１０に戻る。第３設定部３８は、複数の軌跡Ｔのうちから、障害物Ｍに干渉しない軌跡Ｔを選択する。さらに、第３設定部３８は、障害物Ｍに干渉しない軌跡Ｔのうちで、たとえばエネルギ消費が最小となる軌跡Ｔを、経路Ｒの設定に用いる軌跡Ｔとして選択してよい。また例えば、第３設定部３８は、障害物Ｍに干渉しない軌跡Ｔのうちで、移動先までの移動に要する所要時間が最小となる軌跡Ｔを、経路Ｒの設定に用いる軌跡Ｔとして選択してよい。

このように、本実施形態においては、移動元及び移動元における移動体１０の向きと、移動先及び移動先における移動体１０の向きとに基づいて、軌跡Ｔを設定して、軌跡Ｔに基づいて経路Ｒを設定する。すなわち、本実施形態においては、Ｘ方向、Ｙ方向及びヨー角の３次元で離散化して設定された移動元及び移動先に基づき、軌跡Ｔを設定する。従って、本実施形態によると、移動体１０が移動可能な経路Ｒを、自動で適切に設定できる。特に、外乱や、移動元における移動体１０の向きによる移動への影響度合いが大きいケースにおいては、経路Ｒの自動設定が難しい場合がある。それに対して、本実施形態においては、移動元や移動先における移動体１０の向きも用いて軌跡Ｔを設定することで、このようなケースでも、経路Ｒを適切に設定できる。特に、本実施形態の手法は、港湾外から、港湾内に接岸するための経路Ｒを好適に設定できる。さらに言えば、本実施形態においては、１つの移動元に対して複数の移動先を設定して、それぞれの移動先までの軌跡Ｔから、経路Ｒの設定に用いる軌跡Ｔを選択する。そのため、本実施形態によると、移動体１０が移動可能な経路Ｒを適切に設定できる。

（移動制御部）
移動制御部４０は、移動体１０の制御対象１２を制御して、移動体１０を移動させる。本実施形態では、移動制御部４０は、第３設定部３８によって設定された移動体１０の経路Ｒに従って、移動体１０を移動させる。この場合例えば、第２設定部３６は、設定した軌跡Ｔを実現するための制御入力も算出する。移動制御部４０は、経路Ｒのそれぞれの軌跡Ｔについて設定された制御入力に基づいて制御対象１２を制御することで、移動体１０を移動させる。

なおこの場合、移動制御部４０は、移動体１０の位置と経路Ｒとのずれ量を適宜測定して、それぞれの軌跡Ｔについて設定された制御入力と、移動体１０の位置と経路Ｒとのずれ量とに基づいて、すなわちフィードバック制御により、制御対象１２を制御してよい。具体的には、移動制御部４０は、移動体１０の位置と経路Ｒとのずれ量に基づいて、移動体１０の位置を経路Ｒに戻すための制御入力を算出し、算出した制御入力で、移動体１０を制御する。

なお、本実施形態では、移動制御部４０が、経路Ｒに従って移動するように、移動体１０の移動を自動制御するが、それに限られず、例えば運転者の操作によって、移動体１０の移動を手動制御してよい。この場合でも、運転者は、例えば表示部２２に表示された経路Ｒを目視しつつ、自動設定された経路Ｒに沿うように移動体１０を手動制御してよい。また例えば、第３設定部３８は、経路Ｒのそれぞれの軌跡Ｔについて設定された制御入力の情報を表示部２２に表示させてよい。この場合、運転者は、表示部２２に表示された制御入力を目視して、その制御入力を参考にして、移動体１０を手動制御してよい。

また、本実施形態においては、例えば、移動体１０が設定された経路Ｒに従って移動している最中に、移動体１０の移動に影響を及ぼすイベントが発生した場合には、軌跡Ｔを設定し直すことで、経路Ｒを更新してもよい。ここでのイベントとは、例えば移動体１０の故障などが挙げられる。この場合、制御装置１６は、イベントが発生した旨の情報を取得して、その情報を取得した場合には、例えば移動体１０の現在位置及び姿勢を最初の移動元情報として、到着位置Ｇまでの経路Ｒを再設定する。この場合例えば、制御装置１６は、イベントが発生した旨の情報に基づいて、移動体１０の運動方程式を更新して、移動体１０が移動可能な軌跡Ｔ及び経路Ｒを設定しなおす。このように、移動体１０に故障などのイベントが発生して、現在の経路Ｒに従った移動が難しくなった場合にも、経路Ｒを更新することで、到着位置Ｇまでの経路を適切に設定できる。なお、経路Ｒの更新は、移動体１０の移動に影響を及ぼすイベントが発生した場合に限られず、経路Ｒの更新を任意のケースで行ってもよい。

（第２実施形態）
第１実施形態においては、移動元及び移動先を設定して、移動元から移動先までの軌跡Ｔを設定することを繰り返して、経路Ｒを設定していた。一方で、第２実施形態においては、予め設定された複数の経路から、出発位置Ｓから到着位置Ｇまでを繋ぐように軌跡Ｔを順番に選択して経路Ｒを設定する点で、第１実施形態とは異なる。第２実施形態において、第１実施形態と構成が共通する箇所は、説明を省略する。

第２実施形態においては、第２設定部３６は、基準位置からの複数の軌跡Ｔ（モーションプリミティブ）を、予め設定しておく。この場合、第２設定部３６は、離散化した状態空間おける所定の格子点Ｎを基準位置とし、基準位置における移動体１０の基準向きを設定する。第２設定部３６は、移動体１０の運動方程式に基づいて、基準向きで基準位置にある移動体１０が、単位時間で実現可能な軌跡（追従可能な軌跡）を、軌跡Ｔとして算出する。第２設定部３６は、同じ基準向き及び基準位置（軌跡Ｔの始点）であるが、移動先（軌跡Ｔの終点）と移動先における移動体１０の向きとの少なくとも一方が異なる、複数の軌跡Ｔを設定する。また、第２設定部３６は、基準向きを異ならせて同様の処理を行ってもよい。すなわちこの場合、第２設定部３６は、基準向き、移動先、移動先における移動体１０の向きの少なくとも１つが異なる、複数の軌跡Ｔを設定する。第２実施形態で設定される軌跡Ｔの例は、図４の例と同様である。すなわち、図４を例にすると、第２実施形態においては、第２設定部３６は、格子点Ｎ１を基準位置（始点）とし、複数の軌跡Ｔ１～Ｔ１０を設定する。軌跡Ｔ１～Ｔ１０は、移動体１０が単位時間で追従可能な軌跡であるが、基準向き、移動先、移動先における移動体１０の向きの少なくとも１つが異なる。

このように、第２実施形態においては、移動元と、移動元に基づき設定された移動先とから、軌跡Ｔを設定するものではなく、基準位置を移動元とした軌跡Ｔを予め設定する。

第２実施形態においては、第３設定部３８は、第２設定部３６により設定された軌跡Ｔを、移動元情報が示す移動元からの軌跡Ｔとして設定して、移動元からの軌跡Ｔに基づいて、経路Ｒを設定する。具体的には、第３設定部３８は、第２設定部３６により設定された複数の軌跡Ｔのうちから、移動元情報が示す移動元からの軌跡Ｔを選択する。第３設定部３８は、第２設定部３６により設定された軌跡Ｔのうちから、基準向きが移動元における向きに一致する軌跡Ｔを選択することが好ましく、より詳細な軌跡Ｔの選択方法は、第１実施形態と同様であってよい。そして、第３設定部３８は、移動元からの軌跡Ｔを設定したら、その軌跡Ｔの終点を新たな移動元（移動元の位置及び向き）に設定して、第２設定部３６により設定された複数の軌跡Ｔのうちから、新たな移動元からの軌跡Ｔを選択する。第３設定部３８は、最初に出発位置Ｓを移動元とした軌跡Ｔを設定して、移動先が到着位置Ｇに達するまで、直前に選択された軌跡Ｔの終点を移動元とした軌跡Ｔの設定を繰り返す。第３設定部３８は、選択したそれぞれの軌跡Ｔを繋いだ経路を、経路Ｒとして設定する。

第２実施形態においても、第１実施形態と同様に、移動体１０の運動方程式に基づき設定された軌跡Ｔを繋ぐように経路Ｒを設定するため、移動体１０が移動可能な経路Ｒを適切に設定できる。

また、第２実施形態においては、手動制御と自動制御とを切り替えてよい。この場合例えば、手動制御モードにおいて、運転者の操作に応じた制御入力がなされて、移動体１０が移動する。本例では、制御装置１６は、運転者の操作による制御入力を検出して、検出した制御入力に基づき、移動体１０の予測経路を算出する。予測移動経路の算出方法は任意であってよいが、例えば、本実施形態では、軌跡Ｔを設定する際に、その軌跡Ｔを実現する制御入力（その軌跡Ｔに対応する制御入力）も設定されている。そのため、制御装置１６は、軌跡Ｔと、その軌跡Ｔに対応する制御入力との対応関係に基づき、検出した制御入力に対応する軌跡を算出して、予測経路としてよい。

予測経路が算出されたら、移動制御部４０は、手動制御モードから自動制御モードに切り替えて、予測経路に従って移動するように、移動体１０を自動で制御する。なお、自動制御モードへの切り替えは自動で行ってもよいし、運転者が切り替える旨の入力を行った場合に、切り替えてもよい。運転者が切り替える旨の入力を行う場合、例えば、制御装置１６は、算出した予測経路を、表示部２２に表示させる。これにより、運転者は、その予測経路が意図した経路であるかを確認できる。運転者は、予測経路が意図した経路である場合、例えば、今の操作（入力）を維持する。制御装置１６は、今の操作が維持されたら、運転者が切り替える旨の入力を行ったと判断して、自動制御モードに切り替えて、予測経路に従って自動で移動させる。

（効果）
以上説明したように、本開示の第１態様に係る経路設定方法は、移動元の位置及び移動元における移動体１０の向きを示す移動元情報を取得するステップと、移動体１０の動特性を示す運動方程式に基づいて設定された軌跡Ｔを、移動元からの移動体１０の移動する軌跡として設定するステップと、軌跡Ｔに基づいて、出発位置Ｓから到着位置Ｇまでの移動体１０の経路Ｒを設定するステップと、を含む。本開示においては、運動方程式に基づいて設定された軌跡Ｔを、移動元（移動元の位置及び向き）からの軌跡Ｔに設定して、軌跡Ｔに基づいて経路Ｒを設定する。従って、本開示によると、移動体１０が移動可能な経路Ｒを適切に設定できる。特に、外乱や、移動元における移動体１０の向きによる移動への影響度合いが大きい場合であっても、経路Ｒを適切に設定できる。

本開示の第２態様に係る経路設定方法は、第１態様に係る経路設定方法であって、移動元情報を取得するステップにおいては、直前の移動先情報を移動元情報として取得し、移動体の軌跡Ｔを設定するステップにおいては、移動先の位置が到着位置Ｇに達するまで、軌跡Ｔの設定を繰り返す。本開示によると、到着位置Ｇまで、軌跡Ｔを順番につなげるように設定するため、移動体１０が移動可能な経路Ｒを適切に設定できる。

本開示の第３態様に係る経路設定方法は、第１態様又は第２態様に係る経路設定方法であって、経路Ｒを設定するステップにおいては、軌跡Ｔにおいて移動体１０が障害物Ｍと干渉するかを判定し、障害物Ｍに干渉しないと判定した軌跡Ｔに基づいて、経路Ｒを設定する。本開示によると、障害物Ｍに干渉しない経路Ｒを適切に設定できる。

本開示の第４態様に係る経路設定方法は、第３態様に係る経路設定方法であって、経路Ｒを設定するステップにおいては、軌跡Ｔの進行方向に基づいて、障害物Ｍと干渉するかを判定する判定基準を設定して、判定基準に基づいて、軌跡Ｔが障害物Ｍと干渉するかを判定する。本開示によると、進行方向に基づいて判断基準を設定するため、移動体１０の動きに合わせて、干渉するリスクがあるかの判断を高精度に行うことができる。

本開示の第５態様に係る経路設定方法は、第１態様から第４態様のいずれかに係る経路設定方法であって、軌跡Ｔを設定するステップにおいては、１つの移動元からの複数の軌跡Ｔを設定して、経路Ｒを設定するステップにおいては、複数の軌跡Ｔのうちで、移動先まで移動するためのエネルギ消費が最小となる軌跡Ｔを選択して、選択した軌跡Ｔに基づいて、経路Ｒを設定する。本開示によると、エネルギ消費が最小となる軌跡Ｔを用いて経路Ｒを設定するため、エネルギ消費を抑えた経路Ｒを設定できる。

本開示の第６態様に係る経路設定方法は、第１態様から第５態様のいずれかに係る経路設定方法であって、経路Ｒに基づいて移動体１０が移動している最中に、軌跡Ｔを設定し直すことで、経路Ｒを更新する。本開示によると、移動体１０が故障するなど、現在の経路Ｒに従った移動が難しくなった場合にも、経路Ｒを更新することで、到着位置Ｇまでの経路を適切に設定できる。

本開示の第７態様に係る経路設定方法は、第１態様から第６態様のいずれかに係る経路設定方法であって、移動体１０が、水上を移動する船舶、又は水中を移動する水中航行体である。本開示は、船舶や水中航行体の経路設定に特に有効である。すなわち、本開示の経路設定方向によると、旋回する際の軌跡を、操舵角のみで設定されずに移動体１０の速度や向きに影響されるなど、流体中の移動体１０の複雑な運動を考慮して設定することが可能となるため、船舶や水中航行体の経路設定に特に有効といえる。さらに言えば、本開示によると、動作要素(motion primitive)を事前にオフライン計算し、これをつなぎ合わせていくため、旋回軌跡が舵角のみで決定されず速度や姿勢角に影響されるなど、流体中の移動体１０の複雑な運動を考慮した上で、実用的な計算負荷に抑えることが可能となるため、本開示の手法が有効な打ち手となる。

本開示の第８態様に係る移動制御方法は、第１態様から第７態様のいずれかに係る経路設定方法で設定された経路Ｒに基づいた制御入力により、移動体１０を移動させるステップと、移動体１０の位置と経路Ｒとのずれ量を取得するステップと、を含む。移動体１０を移動させるステップにおいては、経路Ｒ及びずれ量に基づいて、移動体の制御入力を設定する。本開示によると、移動中の移動体１０の位置と元の経路Ｒとのずれ量に基づいて、フィードバック制御により、移動体１０を制御する。そのため、本開示によると、移動体１０を適切に移動させることができる。

本開示の第９態様に係るプログラムは、移動元の位置及び移動元における移動体１０の向きを示す移動元情報を取得するステップと、移動体１０の動特性を示す運動方程式に基づいて設定された軌跡Ｔを、移動元からの移動体１０の移動する軌跡Ｔに設定するステップと、軌跡Ｔに基づいて、出発位置Ｓから到着位置Ｇまでの移動体１０の経路Ｒを設定するステップと、をコンピュータに実行させる。本開示によると、移動体１０が移動可能な経路Ｒを適切に設定できる。

本開示の第１０態様に係る経路設定装置は、移動元の位置及び移動元における移動体１０の向きを示す移動元情報を取得する第１取得部３２と、移動体１０の動特性を示す運動方程式に基づいて設定された軌跡Ｔを、移動元からの移動体１０の移動する軌跡Ｔに設定する第２設定部３６と、軌跡Ｔに基づいて、出発位置Ｓから到着位置Ｇまでの移動体１０の経路Ｒを設定する第３設定部３８と、を含む。本開示によると、移動体１０が移動可能な経路Ｒを適切に設定できる。

本開示の第１１態様に係る移動体１０は、第１０態様に係る経路設定装置を含む。本開示によると、移動体１０が移動可能な経路Ｒを適切に設定できる。

以上、本開示の実施形態を説明したが、この実施形態の内容により実施形態が限定されるものではない。また、前述した構成要素には、当業者が容易に想定できるもの、実質的に同一のもの、いわゆる均等の範囲のものが含まれる。さらに、前述した構成要素は適宜組み合わせることが可能である。さらに、前述した実施形態の要旨を逸脱しない範囲で構成要素の種々の省略、置換又は変更を行うことができる。

１０ 移動体
１２ 制御対象
１４ 制御装置（経路設定装置）
３２ 第１取得部
３４ 第１設定部
３６ 第２設定部
３８ 第３設定部
４０ 移動制御部
Ｒ 経路
Ｔ 軌跡

Claims (11)

1. 移動元の位置及び前記移動元における移動体の向きを示す移動元情報を取得するステップと、
   前記移動体の動特性を示す運動方程式に基づいて設定された軌跡を、前記移動元からの前記移動体の移動する軌跡として設定するステップと、
   前記軌跡に基づいて、出発位置から到着位置までの前記移動体の経路を設定するステップと、
   を含む、
   経路設定方法。
2. 前記移動元情報を取得するステップにおいては、直前の移動先情報を、前記移動元情報として取得し、
   前記移動体の軌跡を設定するステップにおいては、前記軌跡の移動先の位置が前記到着位置に達するまで、前記軌跡の設定を繰り返す、請求項１に記載の経路設定方法。
3. 前記経路を設定するステップにおいては、前記軌跡が障害物と干渉するかを判定し、前記障害物に干渉しないと判定した前記軌跡に基づいて、前記経路を設定する、請求項１又は請求項２に記載の経路設定方法。
4. 前記経路を設定するステップにおいては、前記軌跡の進行方向に基づいて、前記障害物と干渉するかを判定する判定基準を設定して、前記判定基準に基づいて、前記軌跡が障害物と干渉するかを判定する、請求項３に記載の経路設定方法。
5. 前記軌跡を設定するステップにおいては、１つの前記移動元からの複数の軌跡を設定して、
   前記経路を設定するステップにおいては、前記複数の軌跡のうちで、移動先まで移動するためのエネルギ消費が最小となる軌跡を選択して、選択した前記軌跡に基づいて、前記経路を設定する、請求項１又は請求項２に記載の経路設定方法。
6. 前記経路に基づいて前記移動体が移動している最中に、前記軌跡を設定し直すことで、前記経路を更新する、請求項１又は請求項２に記載の経路設定方法。
7. 前記移動体は、水上を移動する船舶、又は水中を移動する水中航行体である、請求項１又は請求項２に記載の経路設定方法。
8. 請求項１又は請求項２に記載の経路設定方法で設定された前記経路に基づいた制御入力により、前記移動体を移動させるステップと、
   前記移動体の位置と前記経路とのずれ量を取得するステップと、
   を含み、
   前記移動体を移動させるステップにおいては、前記経路及び前記ずれ量に基づいて、前記移動体の制御入力を設定する、
   移動制御方法。
9. 移動元の位置及び前記移動元における移動体の向きを示す移動元情報を取得するステップと、
   前記移動体の動特性を示す運動方程式に基づいて設定された軌跡を、前記移動元からの前記移動体の移動する軌跡として設定するステップと、
   前記軌跡に基づいて、出発位置から到着位置までの前記移動体の経路を設定するステップと、
   をコンピュータに実行させる、
   プログラム。
10. 移動元の位置及び前記移動元における移動体の向きを示す移動元情報を取得する第１取得部と、
    前記移動体の動特性を示す運動方程式に基づいて設定された軌跡を、前記移動元の前記移動体の移動する軌跡として設定する第２設定部と、
    前記軌跡に基づいて、出発位置から到着位置までの前記移動体の経路を設定する第３設定部と、
    を含む、
    経路設定装置。
11. 請求項１０に記載の経路設定装置を含む、
    移動体。

Images