特開 2023-91940 移動体運動制御システム及び移動体の運動制御方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2023091940

(43)【公開日】2023-07-03

(54)【発明の名称】移動体運動制御システム及び移動体の運動制御方法

(51)【国際特許分類】

   G05D 1/02 20200101AFI20230626BHJP

【ＦＩ】

G05D1/02 P

【審査請求】未請求

【請求項の数】10

【出願形態】ＯＬ

(21)【出願番号】P 2021206830

(22)【出願日】2021-12-21

(71)【出願人】

【識別番号】000000974

【氏名又は名称】川崎重工業株式会社

(74)【代理人】

【識別番号】110000556

【氏名又は名称】弁理士法人有古特許事務所

(72)【発明者】

【氏名】柘植 俊佑

(72)【発明者】

【氏名】宍戸 紀彦

【テーマコード（参考）】

5H301

【Ｆターム（参考）】

5H301BB05

5H301CC03

5H301CC06

5H301CC10

5H301DD08

5H301DD17

5H301GG14

5H301KK08

5H301QQ01

(57)【要約】

【課題】 先行移動体と後続移動体の状況を考慮して運動を制御する移動体運動制御システムを提供する。
【解決手段】 移動体運動制御システム１００は、各機体１の制御部１１は、自機２５の先行機２４及び自機２５の後続機２６の少なくとも何れか一方を設定し、過去の１時点である指定時点ｔを所定の周期で決定し、通信によって指定時点ｔを発信し、先行機２４及び後続機２６が設定されているときは、現在から過去のある時点までの先行機２４の状態量の履歴である状態量履歴Ｓを取得し、通信によって先行機２４が発信した指定時点ｔを取得し、状態量履歴Ｓに基づいて指定時点ｔにおける先行機２４の状態量である指定時点状態量Ｓｔを特定し、後続機２６の現在の状態量である現在状態量Ｓｎを取得し、先行機２４の指定時点状態量Ｓｔ及び後続機２６の現在状態量Ｓｎに基づいて自機２５の目標状態量Ｓａを生成する。
【選択図】 図２

【特許請求の範囲】

【請求項1】

複数の移動体に設けられる移動体運動制御システムであって、
各前記移動体に設けられ、自移動体の状態量の目標値である目標状態量を生成する制御部と、前記目標状態量に基づき自移動体を運動させる駆動部と、前記制御部の制御によって他の前記移動体との通信を行う通信部と、を有し、
前記制御部は、
自移動体の先行移動体及び自移動体の後続移動体の少なくとも何れか一方を設定し、
過去の１時点である指定時点を所定の周期で決定し、前記通信によって前記指定時点を発信し、
前記先行移動体及び前記後続移動体が設定されているときは、現在から過去のある時点までの前記先行移動体の状態量の履歴である状態量履歴を取得し、前記通信によって前記先行移動体が発信した前記指定時点を取得し、前記状態量履歴に基づいて前記指定時点における前記先行移動体の状態量である指定時点状態量を特定し、前記後続移動体の現在の状態量である現在状態量を取得し、前記先行移動体の前記指定時点状態量及び前記後続移動体の前記現在状態量に基づいて自移動体の前記目標状態量を生成する、移動体運動制御システム。

【請求項2】

前記制御部は、前記先行移動体が設定されていないときは、所定の経路を移動する前記状態量を生成し、生成した前記状態量に基づいて自移動体の前記目標状態量を生成する、請求項１に記載の移動体運動制御システム。

【請求項3】

前記制御部は、前記通信によって自移動体の前記状態量履歴及び前記現在状態量を発信し、前記先行移動体が設定されているときは、前記通信によって前記先行移動体の前記状態量履歴を取得し、前記後続移動体が設定されているときは、前記通信によって前記後続移動体の前記現在状態量を取得する、請求項１又は２に記載の移動体運動制御システム。

【請求項4】

前記制御部は、前記先行移動体が設定されている状態で前記先行移動体との前記通信が途絶したと判定したときは、前記通信が途絶する前に前記先行移動体から取得した前記状態量履歴に基づいて前記通信が途絶した後の前記先行移動体の状態量を予測した予測状態量を補完して前記状態量履歴とし、自移動体が決定した時点における前記先行移動体の状態量を前記先行移動体の予測状態量を補完した前記状態量履歴に基づいて特定して前記指定時点状態量とする、請求項３に記載の移動体運動制御システム。

【請求項5】

各前記移動体は、複数の前記移動体の並び順を保持する記憶部を有し、
前記制御部は、前記先行移動体が設定され、且つ前記先行移動体との通信が途絶した状態が所定時間継続したときにおいて、前記並び順に基づいて前記先行移動体の先行移動体である先々行移動体が有ると判定したときは、前記先行移動体に替えて前記先々行移動体を自移動体の先行移動体として再設定し、前記先々行移動体が無いと判定したときは、前記先行移動体の設定を抹消する、請求項３又は４に記載の移動体運動制御システム。

【請求項6】

前記状態量は、自移動体の位置を算出可能な状態量であり、
前記制御部は、自移動体の前記状態量履歴を取得し、自移動体の前記状態量履歴から算出される前記自移動体の位置に自移動体と後続移動体との位置ずれ量であるオフセット量を付加するように前記状態量履歴を修正し、前記通信によって修正した前記状態量履歴を発信する、請求項３乃至５の何れか１項に記載の移動体運動制御システム。

【請求項7】

前記状態量は、自移動体の位置を算出可能な状態量であり、
前記制御部は、前記通信によって自移動体と後続移動体との位置ずれ量であるオフセット量を発信し、
前記先行移動体が設定されているときは、前記通信によって前記先行移動体が発信した前記オフセット量を取得し、前記先行移動体と自移動体との間に前記オフセット量に応じた位置ずれが生じるように前記目標状態量を生成する、請求項３乃至５の何れか１項に記載の移動体運動制御システム。

【請求項8】

前記制御部は、前回決定した前記指定時点との時間差が所定の閾値よりも小さくなるように今回の前記指定時点を決定する、請求項１乃至７の何れか１項に記載の移動体運動制御システム。

【請求項9】

前記状態量は、自移動体位置、自移動体速度、自移動体姿勢、自移動体の制御コマンド、自移動体の制御量、自移動体の姿勢変化率、及び自移動体の加減速率の少なくとも何れか１の状態量である、請求項１乃至７の何れか１項に記載の移動体運動制御システム。

【請求項10】

自移動体の先行移動体及び自移動体の後続移動体の少なくとも何れか一方を設定し、
過去の１時点である指定時点を所定の周期で決定し、通信によって前記指定時点を発信し、
前記先行移動体及び前記後続移動体が設定されているときは、現在から過去のある時点までの前記先行移動体の状態量の履歴である状態量履歴を取得し、通信によって前記先行移動体が発信した前記指定時点を取得し、前記状態量履歴に基づいて前記指定時点における前記先行移動体の状態量である指定時点状態量を特定し、前記後続移動体の現在の状態量である現在状態量を取得し、前記先行移動体の前記指定時点状態量及び前記後続移動体の前記現在状態量に基づいて目標状態量を生成し、
前記目標状態量に基づき自移動体を運動させる、移動体の運動制御方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、移動体運動制御システム及び移動体の運動制御方法に関する。

【背景技術】

【0002】

従来から移動体制御システムが知られている（例えば特許文献１参照）。

【0003】

この移動体制御システムは、第１移動体に設けられ第２移動体との間で所定の位置関係が形成されるように前記第１移動体の移動に関する制御を行う移動体制御システムであって、第２移動体との間の第１移動体の相対的な位置及び速度を求める相対位置関係算出部と、第１移動体の相対的な位置及び速度に基づいて、第２移動体との間で前記所定の位置関係が形成されるべく前記第２移動体に対する相対的な位置を調整するための相対位置調整用コマンドを生成するコマンド生成部とを備えている。これによって、移動体が複数運用される場合に、複数の移動体の移動経路などの制御を全体として行うことができる。

【先行技術文献】

【特許文献】

【0004】

【特許文献1】特開２００４―２５９７１号公報

【発明の概要】

【発明が解決しようとする課題】

【0005】

しかし、特許文献１に記載の移動体制御システムは、第１移動体に先行する移動体と連携した移動を行えないという問題があった。

【課題を解決するための手段】

【0006】

上記課題を解決するため、本発明のある態様に係る移動体制御システムは、複数の移動体に設けられる移動体運動制御システムであって、各前記移動体に設けられ、自移動体の状態量の目標値である目標状態量を生成する制御部と、前記目標状態量に基づき自移動体を運動させる駆動部と、前記制御部の制御によって他の前記移動体との通信を行う通信部と、を有し、前記制御部は、自移動体の先行移動体及び自移動体の後続移動体の少なくとも何れか一方を設定し、過去の１時点である指定時点を所定の周期で決定し、前記通信によって前記指定時点を発信し、前記先行移動体及び前記後続移動体が設定されているときは、現在から過去のある時点までの前記先行移動体の状態量の履歴である状態量履歴を取得し、前記通信によって前記先行移動体が発信した前記指定時点を取得し、前記状態量履歴に基づいて前記指定時点における前記先行移動体の状態量である指定時点状態量を特定し、前記後続移動体の現在の状態量である現在状態量を取得し、前記先行移動体の前記指定時点状態量及び前記後続移動体の前記現在状態量に基づいて自移動体の前記目標状態量を生成する。

【0007】

先行移動体と後続移動体の状況を考慮して運動を制御することができるので、複数の移動体の連携移動が可能となる。また、先行移動体の過去の状態量のうち、どの時点における状態量に基づいて運動を制御すべきかを指示するので、距離間隔を制御することができる。

【発明の効果】

【0008】

本発明は、先行移動体と後続移動体の状況を考慮して運動を制御することができるという効果を奏する。

【図面の簡単な説明】

【0009】

【図1】実施の形態に係る移動体運動制御システムの構成例を示す説明図である。

【図2】図１の移動体運動制御システムの制御系統の構成例を概略的に示すブロック図である。

【図3】図１の移動体運動制御システムの制御処理の例を示すフローチャートであり、移動体の制御処理全体のフローチャートである。

【図4】図１の移動体運動制御システムの制御処理の例を示すフローチャートであり、図３の制御入力生成処理のフローチャートである。

【図5】図１の移動体運動制御システムの制御処理の例を示すフローチャートであり、図３の指示発信処理のフローチャートである。

【図6】図１の移動体運動制御システムの制御処理の例を示すフローチャートであり、図３の先行機情報受信処理のフローチャートである。

【図7】図１の移動体運動制御システムの制御処理の例を示すフローチャートであり、図３の後続機情報受信処理のフローチャートである。

【図8】図１の移動体運動制御システムのオフセット付加処理を行う場合の構成例を示す説明図である。

【図9】図１の移動体運動制御システムの制御処理の例を示すフローチャートであり、第１のオフセット付加処理を行う場合の指示発信処理の例を示すフローチャートである。

【図10】図１の移動体運動制御システムの制御処理の例を示すフローチャートであり、第２のオフセット付加処理を行う場合の指示発信処理を示すフローチャートである。

【図11】図１の移動体運動制御システムの制御処理の例を示すフローチャートであり、第２のオフセット付加処理を行う場合の先行機情報受信処理を示すフローチャートである。

【図12】図１の移動体運動制御システムの制御処理の例を示すフローチャートであり、第２のオフセット付加処理を行う場合の制御入力生成処理を示すフローチャートである。

【発明を実施するための形態】

【0010】

以下、実施の形態について、図面を参照しながら説明する。なお、以下の実施の形態によって本発明が限定されるものではない。また、以下では、全ての図を通じて、同一又は相当する要素には同一の参照符号を付して、その重複する説明を省略する。

【0011】

本明細書で開示する要素の機能は、開示された機能を実行するよう構成またはプログラムされた汎用プロセッサ、専用プロセッサ、集積回路、ASIC（Application Specific Integrated Circuits）、従来の回路、および/または、それらの組み合わせ、を含む回路または処理回路を使用して実行できる。プロセッサは、トランジスタやその他の回路を含むため、処理回路または回路と見なされる。本開示において、回路、ユニット、または手段は、列挙された機能を実行するハードウェアであるか、または、列挙された機能を実行するようにプログラムされたハードウェアである。ハードウェアは、本明細書に開示されているハードウェアであってもよいし、あるいは、列挙された機能を実行するようにプログラムまたは構成されているその他の既知のハードウェアであってもよい。ハードウェアが回路の一種と考えられるプロセッサである場合、回路、手段、またはユニットはハードウェアとソフトウェアの組み合わせであり、ソフトウェアはハードウェアおよび/またはプロセッサの構成に使用される。

【0012】

図１は、実施の形態に係る移動体運動制御システム１００の構成例を示す説明図である。図２は、移動体運動制御システム１００の制御系統の構成例を概略的に示すブロック図である。

【0013】

図１、２に示すように、移動体運動制御システム１００は、複数の機体１（移動体）に設けられた制御器の制御によって、これら複数の機体１が連携して移動するように制御するシステムである。図１において例示する制御例は、移動体運動制御システム１００が１列に隊列を組んだ状態で移動するように複数の機体１を制御した例である。機体１は、航空機、船、水中船、自動車が例示される。

【0014】

各機体１が備える制御器は、電気回路であり、ＣＰＵ等の演算器を有する。この制御器は、図２に示すように、自機（自移動体）２５の状態量ｓの目標値である目標状態量Ｓａを生成する制御部１１と、ＲＯＭ及びＲＡＭ等のメモリを有し、各種プログラム及び各種データを記憶する記憶部１２とを有する。制御器は、集中制御する単独の制御器で構成されていてもよく、互いに協働して分散制御する複数の制御器で構成されてもよい。また、機体１は、目標状態量Ｓａに基づき自機２５を運動させる駆動部１３と、制御部１１の制御によって他の機体１との通信を行う通信部１４とを備える。記憶部１２に記憶されるデータは、先行機２４の状態量履歴Ｓ及び指定時点ｔ、後続機２６の現在状態量Ｓｎを含む。また、記憶部１２に記憶されるデータは、予め設定された機体１の隊列の並び順を含んでもよい。また、通信部１４における他の機体１との通信は、受信先を特定した双方向通信であってもよく、また、受信先を特定せずデータを一方向に発信する一方向通信であってもよい。

【0015】

次に、移動体運動制御システム１００の移動体の制御例を説明する。本動作例においては、複数の機体１を一方向に移動させる場合を例示する。

【0016】

図３～図７は、移動体運動制御システム１００の制御処理の例を示すフローチャートである。このうち、図３は、移動体の制御処理全体のフローチャートである。また、図４は、図３の制御入力生成処理のフローチャートである。更に、図５は、図３の指示発信処理のフローチャートである。また、図６は、図３の先行機情報受信処理のフローチャートである。更に、図７は、図３の後続機情報受信処理のフローチャートである。

【0017】

図３に示すように、制御部１１は、ステップＳ１０を処理した後、ステップＳ２０～Ｓ９０を繰り返し実行する。この繰り返し処理は、所定の周期毎に実行される。

【0018】

まず、制御部１１は、自機２５の先行機(先行移動体)２４及び自機２５の後続機（後続移動体）２６の少なくとも何れか一方を設定する（ステップＳ１０）。複数の機体１が一列に隊列を組む場合は、先頭機２１の制御部１１は、並び順において２番目の機体１を後続機２６として設定する。２番目の機体１の制御部１１は、先頭機２１を先行機２４として設定し、３番目の機体１を後続機２６として設定する。このように、先頭機２１及び末尾機２９を除くＮ番目の中間機の制御部１１は、Ｎ－１番目の機体１を先行機２４として設定し、更にＮ＋１番目の機体１を後続機２６として設定する。末尾機２９の制御部１１は、並び順において末尾から２番目の機体１を先行機２４として設定し、後続機２６は設定しない。図２において、先頭機２１、後述する先々行機２３、末尾機２９において、制御部１１、記憶部１２、駆動部１３、通信部１４の一部又は全部の図示を省略している。

【0019】

次に、制御部１１は、設定されている先行機２４の有無を判定する（ステップＳ２０）。そして、制御部１１は、先行機２４が有ると判定すると（ステップＳ２０においてＹｅｓ）、先行機情報受信処理を実行する（ステップＳ３０）。

【0020】

図６に示すように、先行機情報受信処理には、通信途絶時の処理（ステップＳ３１０及びステップＳ３２５～ステップＳ３６５）が含まれるが、これらの処理については後述する。先行機情報受信処理において、制御部１１は、設定されている先行機２４から発信された状態量履歴Ｓ及び指定時点ｔを受信する（ステップＳ３１５）。先行機２４から発信された状態量履歴Ｓ及び指定時点ｔは、先行機２４の後述するステップＳ９０の指示発信処理において、先行機２４が発信した情報である。状態量履歴Ｓは、現在から過去のある時点までの所定の時間間隔毎の先行機２４の状態量ｓのデータセットから構成される履歴である。そして、ｊ個のデータセットから構成される状態量履歴Ｓは、例えば、以下のように定義される。
Ｓ：＝［ｓ（１）， ｓ（２）， ｓ（３）， ．．． ｓ（ｊ）］
なお、小括弧内の数字は添え字である。状態量ｓ（１）は、現在の状態量ｓであり、状態量ｓの添え字の値が大きくになるにしたがって、より過去の状態量ｓである。

【0021】

状態量ｓは、自機位置、自機速度、自機姿勢、自機の制御コマンド、自機の制御量、自機の姿勢変化率、及び自機の加減速率の少なくとも何れか１の状態量である。また、状態量ｓは自機２５の位置を算出可能な状態量であってもよい。

【0022】

また、指定時点ｔは、先行機２４が、この先行機２４の後続機である自機２５に対して、状態量履歴Ｓに含まれる状態量ｓ（１）～ｓ（ｊ）のうち、何れの時点の状態量ｓを自機２５の目標状態量Ｓａの設定に用いるのかを指定する指令値である。この指定時点ｔは、例えば、データセットに含まれる状態量ｓの添え字を用いて指定する。

【0023】

次に、制御部１１は、受信した状態量履歴Ｓ及び指定時点ｔを記憶部１２に格納し、状態量履歴Ｓ及び指定時点ｔの値を更新する（ステップＳ３２０）。そして、制御部１１は、先行機情報受信処理を終了する。なお、更新前の状態量履歴Ｓ及び指定時点ｔは、更新時に過去の状態量履歴Ｓ及び指定時点ｔとして保管するように、記憶部１２に過去の履歴として蓄積してもよい。

【0024】

なお、先行機２４の状態量ｓは、自機２５がカメラ、レーダーを用いて先行機２４を観測して取得されるように構成してもよい。

【0025】

一方、制御部１１は、ステップＳ２０において、設定されている先行機２４が無い、すなわち先頭機２１であると判定すると（ステップＳ２０においてＮｏ）、ステップＳ３０の先行機情報受信処理をスキップする。

【0026】

次に、制御部１１は、設定されている後続機２６の有無を判定する（ステップＳ４０）。そして、制御部１１は、設定されている後続機２６が有ると判定すると（ステップＳ４０においてＹｅｓ）、後続機情報受信処理を実行する（ステップＳ５０）。

【0027】

図７に示すように、後続機情報受信処理において、制御部１１は、まず、設定されている後続機２６から発信された現在状態量Ｓｎを受信する（ステップＳ５１０）。後続機２６から発信された現在状態量Ｓｎは、後続機２６の後述するステップＳ７０の処理において、後続機２６が発信した情報である。現在状態量Ｓｎは、後続機２６が発信した後続機２６の現在の状態量ｓである。この現在状態量Ｓｎは、後続機２６が発信した状態量履歴Ｓに含まれる現在の状態量ｓ（１）であってもよい。この場合、後続機２６が発信した状態量履歴Ｓを自機２５が受信し、制御部１１は、この中から現在の状態量ｓ（１）を取り出し、これを後続機２６の現在状態量Ｓｎとして取り扱ってもよい。

【0028】

次に、制御部１１は、後続機情報受信処理において受信した現在状態量Ｓｎを記憶部１２に格納し、現在状態量Ｓｎの値を更新する（ステップＳ５１５）。そして、制御部１１は、後続機情報受信処理を終了する。

【0029】

なお、後続機２６の状態量ｓは、自機２５がカメラ、レーダーを用いて後続機２６を観測して取得されるように構成してもよい。

【0030】

上記の通り、状態量履歴Ｓ、指定時点ｔ、現在状態量Ｓｎは、通信によって取得されるので、観測による先行機２４又は後続機２６の状態量履歴Ｓの取得が難しい状況下においても、先行機２４の状態量履歴Ｓ及び後続機２６の現在状態量Ｓｎを取得することができる。

【0031】

一方、制御部１１は、ステップＳ４０において、設定されている後続機２６が無い、すなわち末尾機２９であると判定すると（ステップＳ４０においてＮｏ）、ステップＳ５０の後続機情報受信処理をスキップする。

【0032】

次に、制御部１１は、制御入力生成処理を実行する（ステップＳ６０）。図４に示すように、制御入力生成処理において、制御部１１は、まず、自機２５の先行機２４及び後続機２６が何れも設定されているか否か、すなわち、自機２５が中間機であるか否かを判定する（ステップＳ６１０）。そして、制御部１１は、自機２５の先行機２４及び後続機２６が何れも設定されている（ステップＳ６１０においてＹｅｓ）と判定すると、記憶部１２から先行機２４の状態量履歴Ｓ及び指定時点ｔを読み出して取得する（ステップＳ６１５）。

【0033】

次に、制御部１１は、指定時点ｔにおける先行機２４の状態量ｓである指定時点状態量Ｓｔを特定する（ステップＳ６２０）。図１においては、ｔ＝５である場合における先行機２４の状態量ｓであるｓ（５）を自機２５の指定時点状態量Ｓｔと特定した場合を例示している。また、ｔ＝７である場合における自機２５の状態量ｓであるｓ（７）を後続機２６の指定時点状態量Ｓｔと特定した場合を例示している。

【0034】

次に、制御部１１は、記憶部１２から後続機２６の現在状態量Ｓｎを読み出して取得する（ステップＳ６２５）。

【0035】

次に、制御部１１は、先行機２４の指定時点状態量Ｓｔ及び後続機２６の現在状態量Ｓｎに基づいて自機２５の目標状態量Ｓａを生成する（ステップＳ６３０）。一例として、具体的には、制御部１１は、指定時点状態量Ｓｔを仮目標状態量とする。そして、制御部１１は、自機２５と後続機２６との間隔が所定の閾値よりも大きいか否かを判定し、この間隔が所定の閾値よりも小さい、即ち、自機２５に後続機２６が遅れずに追従していると判定すると、仮目標状態量を目標状態量Ｓａとする。一方で、制御部１１は、この間隔が所定の閾値よりも大きい、すなわち、後続機２６が遅れて追従していると判定すると、仮目標状態量を修正し、これを目標状態量Ｓａに設定する。仮目標状態量の修正は、例えば、ｔの値を大きくし、指定時点ｔにおける状態量ｓよりも過去の時点における状態量ｓを目標状態量Ｓａとする。そして、制御部１１は、制御入力生成処理を終了する。そして、駆動部１３は、目標状態量Ｓａに基づき自機２５を運動させる。このように、駆動部１３は、制御部１１によって制御される。以上の通り、中間機に係る機体１は、先行機２４と後続機２６の状況を考慮して運動を制御する。これによって、複数機の連携移動が可能となる。また、先行機２４の過去の状態量ｓのうち、どの時点における状態量ｓに基づいて運動を制御すべきかを指定時点ｔによって指示するので、並び順において隣り合う機体１の距離間隔を制御することができる。

【0036】

一方、制御部１１は、ステップＳ６１０において、自機２５の先行機２４及び後続機２６の何れかが設定されていない、すなわち、先頭機（先頭移動体）２１か、末尾機（末尾移動体）２９であると判定すると（ステップＳ６１０においてＮｏ）、更に、後続機２６が設定されているか否か、すなわち、先頭機２１であるか否かを判定する（ステップＳ６３５）。

【0037】

そして、制御部１１は、後続機２６が設定されている、すなわち先頭機２１であると判定すると（ステップＳ６３５においてＹｅｓ）、記憶部１２から後続機２６の現在状態量Ｓｎを読み出して取得する（ステップＳ６４０）。

【0038】

次に、制御部１１は、所定の指示状態量Ｓｉ及び後続機２６の現在状態量Ｓｎに基づいて自機２５の目標状態量Ｓａを生成する（ステップＳ６４５）。所定の指示状態量Ｓｉとは、例えば、所定の経路を移動する生成される状態量である。また、指示状態量Ｓｉは、先頭機２１の操縦者から通信によって与えられる動作指示に基づいて算出される状態量であってもよい。そして、具体的には、制御部１１は、指示状態量Ｓｉを仮目標状態量とする。そして、制御部１１は、ステップＳ６３０と同様に、自機２５と後続機２６との間隔に応じて仮目標状態量を修正し、目標状態量Ｓａとする。そして、制御部１１は、制御入力生成処理を終了する。そして、駆動部１３は、目標状態量Ｓａに基づき自機２５を運動させる。このように、先頭機２１の制御部１１は、自律的に運動を制御することができる。また、これに続く２番目の機体１の制御部１１は、ステップＳ６１５～Ｓ６３０において述べた通り、先頭機２１の指定時点状態量Ｓｔに基づき移動する。これに更に続く機体１も同様に制御され、その結果、先頭機２１に続く他の機体１は、先頭機２１の指定時点状態量Ｓｔに基づき隊列を組んで移動する。

【0039】

一方、ステップＳ６３５において、制御部１１は、後続機２６が設定されていない、すなわち、末尾機２９と判定すると（ステップＳ６３５においてＮｏ）、ステップＳ６１５及びＳ６２０と同様に、記憶部１２から先行機２４の状態量履歴Ｓ及び指定時点ｔを読み出して取得し（ステップＳ６５５）、指定時点状態量Ｓｔを特定する（ステップＳ６６０）。そして、制御部１１は、先行機２４の指定時点状態量Ｓｔ基づいて状態量の目標値である目標状態量Ｓａを生成する（ステップＳ６６５）。一例として、具体的には、制御部１１は、指定時点状態量Ｓｔを目標状態量Ｓａとする。そして、制御部１１は、制御入力生成処理を終了する。

【0040】

このように制御入力生成処理において、制御部１１は、先頭機２１、中間機、末尾機２９の何れであるかを判定し、それぞれ異なる処理を実行する。

【0041】

制御部１１は、制御入力生成処理を終了すると、次に、現在状態量Ｓｎを発信する（ステップＳ７０）。なお、制御部１１は、現在状態量Ｓｎの発信に代えて、現在状態量Ｓｎを含む状態量履歴Ｓを発信してもよい。また、制御部１１は、自機２５の故障状況や後続機２６の故障状況を発信してもよい。

【0042】

次に、制御部１１は、後続機２６が設定されているか否か、すなわち、末尾機２９であるか否かを判定する（ステップＳ８０）。そして、制御部１１は、後続機２６が設定されている、すなわち末尾機２９でないと判定すると（ステップＳ８０においてＹｅｓ）、指示発信処理を実行する（ステップＳ９０）。図５に示すように、指示発信処理において、制御部１１は、まず、自機２５の現在状態量Ｓｎを取得する（ステップＳ９１０）。

【0043】

次に、制御部１１は、自機２５の過去の状態量ｓを記憶部１２から読み出し、現在状態量Ｓｎを状態量ｓ（１）として含めた状態量履歴Ｓを生成する（ステップＳ９１５）。

【0044】

次に、制御部１１は、後続機２６の現在状態量Ｓｎを取得する（ステップＳ９２０）。

【0045】

次に、制御部１１は、状態量履歴Ｓに含まれる状態量ｓ（１）～ｓ（ｊ）のうち、１の時点の状態量の添え字を用いて指定時点ｔとして決定する。このとき、制御部１１は、状態量履歴Ｓに含まれる状態量ｓ（１）～ｓ（ｊ）のうち１の状態量を基準として、自機２５の現在状態量Ｓｎと後続機２６の現在状態量Ｓｎとの間隔が広がるに従い、指定時点ｔを早め、添え字を小さくしてもよい。また、制御部１１は、自機２５の現在状態量Ｓｎと後続機２６の現在状態量Ｓｎとの間隔が狭まるに従い、指定時点ｔを遅くし、添え字を大きくしてもよい。

【0046】

このとき、制御部１１は、前回決定した指定時点ｔとの時間差が所定の閾値よりも小さくなるように今回の指定時点ｔを決定する。これによって、機体１の急激な挙動の変化を防止することができる。

【0047】

なお、制御部１１は、状態量（１）～ｓ（ｊ）の取得時に各状態量を取得した時刻も取得し、この時刻を状態量履歴Ｓに含めてもよい。そして、制御部１１は、指定時点ｔを時刻によって指定してもよい。

【0048】

次に、制御部１１は、通信によって状態量履歴Ｓ及び指定時点ｔを発信する（ステップＳ９３０）。これによって、制御部１１は、後続機２６にこの後続機２６の目標状態量Ｓａと現在状態量Ｓｎとが乖離していることを報知することができる。これによって、後続機２６は、速度を上げるといった対応を取ることができる。そして、制御部１１は、制御入力生成処理を終了し、再度、ステップＳ２０を実行する。なお、制御部１１は、状態量履歴Ｓに各時点における飛行中、停止中等の自機飛行モードに係る情報を含めて発信してもよい。

【0049】

一方、制御部１１は、後続機２６が設定されていないと判定すると（ステップＳ８０においてＮｏ）、再度、ステップＳ２０を実行する。

【0050】

制御部１１は、上述した通り、ステップＳ８０又はステップＳ９０の後、所定の周期経過後に再度ステップＳ２０を実行する。これによって、目標状態量Ｓａ、指定時点ｔ等は所定の周期で決定される。

【0051】

このように、制御部１１は、先行機２４と後続機２６の状況を考慮して運動を制御する。これによって、複数の機体の連携移動が可能となる。また、制御部１１は、先行機２４の過去の状態量のうち、どの時点における状態量に基づいて運動を制御すべきかを指示するので、距離間隔を制御することができる。また、これらの制御は機体が自律的に行うので、機体数の増加に伴う運用者のワークロードの増大を抑制することができる。

【0052】

（通信途絶時の処理）
なお、制御部１１は、先行機２４が設定されている状態で実行される先行機情報受信処理において、ステップＳ３１５に先立ち、先行機２４との通信が途絶状態にあるか否かを判定する（ステップＳ３１０）。そして、制御部１１は、ステップＳ３１０において通信が途絶していないと判定すると上記のステップＳ３１５及びこれに続くステップを実行する。一方で、制御部１１は、ステップＳ３１０において通信が途絶したと判定すると、更に通信の途絶が所定時間以上であるか否かを判定する（ステップＳ３２５）。この所定時間とは、通信品質の劣化等の一時的な要因に基づく通信の途絶時における時間幅に応じて設定されてもよい。そして、制御部１１は、通信の途絶が所定時間以上でないと判定すると（ステップＳ３２５においてＮｏ）、通信によって先行機２４から取得し、記憶部１２に蓄積されている先行機２４の状態量履歴Ｓ、すなわち、通信が途絶する前に先行機２４から取得した状態量履歴Ｓに基づいて予測状態量を算出する。予測状態量とは、通信が途絶した後の蓄積されていない時点の先行機２４の状態量を予測したものである。そして、制御部１１は、通信の一時的な途絶によって先行機２４から取得できなかった状態量ｓを予測状態量によって補完して、状態量履歴Ｓを生成する（ステップＳ３３０）。次に、制御部１１は、通信の一時的な途絶によって先行機２４から取得できなかった指定時点ｔを決定する（ステップＳ３３５）。この自機２５が決定する指定時点ｔは、予め設定された時点であってもよい。

【0053】

次に、制御部１１は、生成した状態量履歴Ｓ及び決定した指定時点ｔを記憶部１２に格納し、状態量履歴Ｓ及び指定時点ｔの値を更新する（ステップＳ３４０）。そして、制御部１１は、先行機情報受信処理を終了する。これによって、先行機情報受信処理に続く制御入力生成処理のステップＳ６２０、Ｓ６３０等においては、自機２５が決定した時点における先行機２４の状態量を先行機２４の予測状態量を補完した状態量履歴Ｓに基づいて指定時点状態量Ｓｔが特定される。そして、この指定時点状態量Ｓｔに基づいて目標状態量Ｓａが生成される。よって、通信途絶時において、不適切な運動制御が実行されることを回避することができる。

【0054】

また、制御部１１は、ステップＳ３２５において、通信の途絶が所定時間以上であると判定すると（ステップＳ３２５においてＹｅｓ）、記憶部１２に格納されている機体１の並び順に基づいて、先行機２４の先行機である先々行機（先々行移動体）２３があるか否かを判定する（ステップＳ３４５）。そして、制御部１１は、先々行機２３があると判定すると（ステップＳ３４５においてＹｅｓ）、設定されている先行機２４に替えて先々行機２３を先行機２４に再設定する。そして、制御部１１は、ステップＳ３１５と同様に、再設定した先行機２４から発信された状態量履歴Ｓ及び指定時点ｔを受信する。そして、制御部１１は、ステップＳ３２０と同様に、受信した状態量履歴Ｓ及び指定時点ｔを記憶部１２に格納し、状態量履歴Ｓ及び指定時点ｔの値を更新する。そして、制御部１１は、先行機情報受信処理を終了する。これによって、制御部１１は、通信が途絶した先行機２４に代えて先々行機２３を新たな先行機として隊列を組むことができ、隊列を維持することができる。

【0055】

更に、制御部１１は、ステップＳ３４５において、先々行機２３が無い、すなわち先々行機２３が並び順において２番目の機体１であると判定すると、先行機２４の設定を抹消し、先行機なしに再設定し、先行機情報受信処理を終了する。これによって、この機体１は先頭機２１して、隊列を先導することができ、隊列を維持することができる。

【0056】

（第１のオフセット付加処理）
図８は、オフセット付加処理を行う場合の移動体運動制御システム１００の構成例を示す説明図である。図９は、移動体運動制御システム１００の制御処理の例を示すフローチャートであり、第１のオフセット付加処理を行う場合の指示発信処理の例を示すフローチャートである。

【0057】

制御部１１は、指示発信処理において発信する状態量にオフセットを付加してもよい。

【0058】

具体的には、図９に示すように、制御部１１は、指示発信処理のステップＳ９１５の後、自機２５の状態量履歴Ｓを取得し、この自機２５の状態量履歴Ｓに含まれる状態量ｓ（１）～ｓ（ｊ）のそれぞれに対して自機２５の状態量履歴Ｓから算出される自機２５の位置に自機２５と後続機２６との位置ずれ量であるオフセット量Ｄを付加するように状態量履歴Ｓを修正する（ステップＳ９１６）。そして、制御部１１は、ステップＳ９３０において、通信によって修正した状態量履歴Ｓを発信してもよい。これによって、この状態量履歴Ｓを受信した後続機２６の制御部１１は、オフセット量Ｄが付加された状態量ｓに基づいて目標状態量Ｓａを生成する。したがって、自機２５と先行機２４との間隔を適切に維持することができる。よって、ホバリング等の一時停止を円滑に行うことができ、隊列を適切に維持することができる。

【0059】

（第２のオフセット付加処理）
図１０～１２は、移動体運動制御システム１００の制御処理の例を示すフローチャートであり、それぞれ第２のオフセット付加処理を行う場合の指示発信処理、先行機情報受信処理、及び制御入力生成処理を示すフローチャートである。

【0060】

図１０に示すように、制御部１１は、指示発信処理の上述のステップＳ９２５に続くステップＳ９３１において、状態量履歴Ｓ及び指定時点ｔと共に、自機２５と後続機２６との位置ずれ量であるオフセット量Ｄを発信する。

【0061】

そして、図１１に示すように、制御部１１は、先行機情報受信処理のステップＳ３１０がＮｏの場合に続くステップＳ３１６又はステップＳ３５０に続くステップＳ３５６において、設定されている状態量履歴Ｓ及び指定時点ｔと共にオフセット量Ｄを受信して取得し、ステップＳ３１６に続くステップＳ３２１又はステップＳ３５６に続くステップＳ３６１において、状態量履歴Ｓ及び指定時点ｔと共にオフセット量Ｄの値を更新する。そして、制御部１１は、図１２に示すように、この先行機情報受信処理に続く制御入力生成処理のステップＳ６２０に続くステップＳ６２６又はステップＳ６６０に続くステップ６６１において、オフセット量Ｄを取得する。そして、制御部１１は、ステップＳ６２６に続くステップＳ６３１又はステップＳ６６１に続くステップＳ６７０において、先行機２４と自機２５との間にオフセット量Ｄに応じた位置ずれが生じるように目標状態量Ｓａを生成する。これによって、自機２５と先行機２４との間隔を適切に維持することができる。よって、ホバリング等の一時停止を円滑に行うことができ、隊列を適切に維持することができる。

【0062】

このように、移動体運動制御システム１００は、複数の機体１に設けられる移動体運動制御システム１００であって、各機体１に設けられ、自機２５の状態量の目標値である目標状態量Ｓａを生成する制御部１１と、目標状態量Ｓａに基づき自機２５を運動させる駆動部１３と、制御部１１の制御によって他の機体１との通信を行う通信部１４と、を有し、制御部１１は、自機２５の先行機２４及び自機２５の後続機２６の少なくとも何れか一方を設定し、過去の１時点である指定時点ｔを所定の周期で決定し、通信によって指定時点ｔを発信し、先行機２４及び後続機２６が設定されているときは、現在から過去のある時点までの先行機２４の状態量の履歴である状態量履歴Ｓを取得し、通信によって先行機２４が発信した指定時点ｔを取得し、状態量履歴Ｓに基づいて指定時点ｔにおける先行機２４の状態量である指定時点状態量Ｓｔを特定し、後続機２６の現在の状態量である現在状態量Ｓｎを取得し、先行機２４の指定時点状態量Ｓｔ及び後続機２６の現在状態量Ｓｎに基づいて自機２５の目標状態量Ｓａを生成する。これによって、先行機２４と後続機２６の状況を考慮して運動を制御することができるので、複数の機体１の連携移動が可能となる。また、先行機２４の過去の状態量のうち、どの時点における状態量に基づいて運動を制御すべきかを指示するので、距離間隔を制御することができる。

【0063】

制御部１１は、先行機２４が設定されていないときは、所定の経路を移動するように状態量を生成し、生成した状態量に基づいて自機２５の目標状態量Ｓａを生成してもよい。これによって、先頭機２１として、自律的に運動を制御することができる。また、これに続く機体１は、先頭機２１の指定時点状態量Ｓｔに基づき隊列を組んで移動するように制御することができる。

【0064】

制御部１１は、通信によって自機２５の状態量履歴Ｓ及び現在状態量Ｓｎを発信し、先行機２４が設定されているときは、通信によって先行機２４の状態量履歴Ｓを取得し、後続機２６が設定されているときは、通信によって後続機２６の現在状態量Ｓｎを取得してもよい。これによって、観測による先行機２４の状態量履歴Ｓの取得が難しい状況下においても、先行機２４の状態量履歴Ｓ及び後続機２６の現在状態量Ｓｎを取得することができる。

【0065】

制御部１１は、先行機２４が設定されている状態で先行機２４との通信が途絶したと判定したときは、通信が途絶する前に先行機２４から取得した状態量履歴Ｓに基づいて通信が途絶した後の先行機２４の状態量を予測した予測状態量を補完して状態量履歴Ｓとし、自機２５が決定した時点における先行機２４の状態量を先行機２４の予測状態量を補完した状態量履歴Ｓに基づいて特定して指定時点状態量Ｓｔとしてもよい。これによって、通信途絶時において、不適切な運動制御を回避することができる。

【0066】

各機体１は、複数の機体１の並び順を保持する記憶部１２を有し、制御部１１は、先行機２４が設定され、且つ先行機２４との通信が途絶した状態が所定時間継続したときにおいて、並び順に基づいて先行機２４の先行機２４である先々行機２３が有ると判定したときは、先行機２４に替えて先々行機２３を自機２５の先行機２４として再設定し、先々行機２３が無いと判定したときは、先行機２４の設定を抹消してもよい。これによって、通信途絶時に隊列を維持することができる。

【0067】

状態量は、自機２５の位置を算出可能な状態量であり、制御部１１は、自機２５の状態量履歴Ｓを取得し、自機２５の状態量履歴Ｓから算出される自機２５の位置に自機２５と後続機２６との位置ずれ量であるオフセット量Ｄを付加するように状態量履歴Ｓを修正し、通信によって修正した状態量履歴Ｓを発信してもよい。これによって、機体１の間隔を適切に維持することができる。

【0068】

状態量は、自機２５の位置を算出可能な状態量であり、制御部１１は、通信によって自機２５と後続機２６との位置ずれ量であるオフセット量Ｄを発信し、先行機２４が設定されているときは、通信によって先行機２４が発信したオフセット量Ｄを取得し、先行機２４と自機２５との間にオフセット量Ｄに応じた位置ずれが生じるように目標状態量Ｓａを生成してもよい。これによって、隊列を適切に維持することができる。

【0069】

制御部１１は、前回決定した指定時点ｔとの時間差が所定の閾値よりも小さくなるように今回の指定時点ｔを決定してもよい。これによって、機体１の急激な挙動の変化を防止することができる。

【0070】

状態量は、自機２５の位置、自機２５の速度、自機２５の姿勢、自機２５の制御コマンド、自機２５の制御量、自機２５の姿勢変化率、及び自機２５の加減速率の少なくとも何れか１の状態量であってもよい。これによって、機体１の運動を適切に制御することができる。

【0071】

上記説明から、当業者にとっては、本発明の多くの改良や他の実施の形態が明らかである。したがって、上記説明は、例示としてのみ解釈されるべきであり、本発明を実行する最良の態様を当業者に教示する目的で提供されたものである。本発明の精神を逸脱することなく、その構造及び／又は機能の詳細を実質的に変更できる。

【符号の説明】

【0072】

Ｓ 状態量履歴
Ｓａ 目標状態量
Ｓｎ 現在状態量
Ｓｔ 指定時点状態量
ｔ 指定時点
１ 機体
１１ 制御部
１３ 駆動部
１４ 通信部
２４ 先行機
２５ 自機
２６ 後続機
１００ 移動体運動制御システム

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】