特開 2023-149910 移動体制御装置及び移動体制御システム

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

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

(11)【公開番号】P2023149910

(43)【公開日】2023-10-16

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

(51)【国際特許分類】

   G05D 1/02 20200101AFI20231005BHJP

【ＦＩ】

G05D1/02 S

G05D1/02 H

【審査請求】未請求

【請求項の数】5

【出願形態】ＯＬ

(21)【出願番号】P 2022058718

(22)【出願日】2022-03-31

(71)【出願人】

【識別番号】000000099

【氏名又は名称】株式会社ＩＨＩ

(74)【代理人】

【識別番号】100088155

【弁理士】

【氏名又は名称】長谷川 芳樹

(74)【代理人】

【識別番号】100113435

【弁理士】

【氏名又は名称】黒木 義樹

(74)【代理人】

【識別番号】100170818

【弁理士】

【氏名又は名称】小松 秀輝

(74)【代理人】

【識別番号】100171583

【弁理士】

【氏名又は名称】梅景 篤

(72)【発明者】

【氏名】吉光 亮

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

5H301

【Ｆターム（参考）】

5H301AA04

5H301AA05

5H301AA06

5H301BB14

5H301LL01

5H301LL06

5H301LL14

5H301LL16

(57)【要約】

【課題】制御量を適正化することができる移動体制御装置及び移動体制御システムを提供すること。
【解決手段】移動体制御装置１０は、制御量を用いて移動体を制御する装置であって、移動体と障害物との距離が障害物を回避するための回避距離よりも大きい状態を維持し続けるという制約条件のもとで、移動体の状態量が状態量の目標値である目標状態量に近づくように制御量を算出する算出部１３と、制御量を移動体に出力する出力部１４と、を備え、算出部１３は、外乱が移動体に作用する外乱方向と、障害物と移動体との位置関係と、に基づいて回避距離を算出する。
【選択図】図２

【特許請求の範囲】

【請求項1】

制御量を用いて移動体を制御する移動体制御装置であって、
前記移動体と障害物との距離が前記障害物を回避するための回避距離よりも大きい状態を維持し続けるという制約条件のもとで、前記移動体の状態量が前記状態量の目標値である目標状態量に近づくように制御量を算出する算出部と、
前記制御量を前記移動体に出力する出力部と、
を備え、
前記算出部は、外乱が前記移動体に作用する外乱方向と、前記障害物と前記移動体との位置関係と、に基づいて前記回避距離を算出する、移動体制御装置。

【請求項2】

前記回避距離は、前記状態量の不確かさに応じた補正距離を含み、
前記算出部は、前記外乱方向が前記移動体が前記障害物から離れる方向である場合の前記補正距離を、前記外乱方向が前記移動体が前記障害物に近づく方向である場合の前記補正距離よりも小さくする、請求項１に記載の移動体制御装置。

【請求項3】

前記算出部は、前記外乱方向が前記移動体が前記障害物から離れる方向である場合、前記補正距離をゼロに設定する、請求項２に記載の移動体制御装置。

【請求項4】

前記補正距離は、前記外乱に関するプロセスノイズの成分を含み、
前記算出部は、前記外乱方向が前記移動体が前記障害物から離れる方向である場合、前記成分をゼロに設定する、請求項２に記載の移動体制御装置。

【請求項5】

請求項１～請求項４のいずれか一項に記載の移動体制御装置と、
前記状態量を検出する状態量検出装置と、
前記外乱を計測する計測装置と、
前記障害物を検出する障害物検出装置と、
を備える、移動体制御システム。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、移動体制御装置及び移動体制御システムに関する。

【背景技術】

【0002】

ＵＳＶ（Unmanned Surface Vehicle）、及びＡＳＶ（Autonomous Surface Vehicle）等の自律制御型の移動体が知られている。このような移動体には、障害物を回避する機能が求められる。例えば、特許文献１には、自律制御型の移動体に障害物回避を行わせる移動体制御システムが記載されている。この移動体制御システムは、衝突回避制約関数を制約条件として制御量の評価関数を最小化する制御量を算出し、その制御量を用いて移動体を制御している。衝突回避制約関数は、障害物の中心位置と移動体との間の距離が、障害物のサイズを近似した円の半径に、移動体の状態量の不確かさに応じたマージンを加えた値以上であることを規定している。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２０１８－１８１１６６号公報

【発明の概要】

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

【0004】

特許文献１に記載の移動体制御システムは、移動体に働く外乱に基づいてマージンを算出している。しかしながら、上記移動体制御システムは、外乱が移動体に作用する方向によらずに、外乱に基づく一定値をマージンに含めている。一般に、外乱が移動体に作用する方向に移動体の位置が変動し得る。したがって、上記移動体制御システムにおける制御量の算出方法には改善の余地がある。

【0005】

本開示は、制御量を適正化可能な移動体制御装置及び移動体制御システムを説明する。

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

【0006】

本開示の一側面に係る移動体制御装置は、制御量を用いて移動体を制御する装置である。この移動体制御装置は、移動体と障害物との距離が障害物を回避するための回避距離よりも大きい状態を維持し続けるという制約条件のもとで、移動体の状態量が状態量の目標値である目標状態量に近づくように制御量を算出する算出部と、制御量を移動体に出力する出力部と、を備える。算出部は、外乱が移動体に作用する外乱方向と、障害物と移動体との位置関係と、に基づいて回避距離を算出する。

【発明の効果】

【0007】

本開示は、制御量を適正化することができる。

【図面の簡単な説明】

【0008】

【図1】図１は、一実施形態に係る移動体制御システムの概略構成図である。

【図2】図２は、図１に示される移動体制御装置の機能ブロック図である。

【図3】図３は、図１に示される移動体制御装置が行う移動体制御方法の一連の処理を示すフローチャートである。

【図4】図４は、外乱方向と、障害物と移動体との位置関係と、を説明するための図である。

【図5】図５の（ａ）は、図１に示される移動体制御システムによって制御された移動体の移動軌跡の一例を示す図である。図５の（ｂ）は、比較例の移動体制御システムによって制御された移動体の移動軌跡の一例を示す図である。

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

【0009】

［１］実施形態の概要
本開示の一側面に係る移動体制御装置は、制御量を用いて移動体を制御する装置である。この移動体制御装置は、移動体と障害物との距離が障害物を回避するための回避距離よりも大きい状態を維持し続けるという制約条件のもとで、移動体の状態量が状態量の目標値である目標状態量に近づくように制御量を算出する算出部と、制御量を移動体に出力する出力部と、を備える。算出部は、外乱が移動体に作用する外乱方向と、障害物と移動体との位置関係と、に基づいて回避距離を算出する。

【0010】

上記移動体制御装置では、外乱が移動体に作用する外乱方向と、障害物と移動体との位置関係と、に基づいて回避距離が算出される。外乱が移動体に作用することで、移動体は、外乱方向に移動体を移動させる力を受ける。このとき、外乱方向が、移動体が障害物から離れる方向である場合には、外乱が移動体に作用することによって、移動体は障害物から離れ得る。一方、外乱方向が、移動体が障害物に近づく方向である場合には、外乱が移動体に作用することによって、移動体は障害物に近づき得る。したがって、外乱方向と、障害物と移動体との位置関係と、が考慮されることによって、回避距離を適切に算出することができる。このようにして算出された回避距離を用いた制約条件のもとで制御量が算出されるので、制御量を適正化することが可能となる。

【0011】

回避距離は、状態量の不確かさに応じた補正距離を含んでもよく、算出部は、外乱方向が移動体が障害物から離れる方向である場合の補正距離を、外乱方向が移動体が障害物に近づく方向である場合の補正距離よりも小さくしてもよい。外乱方向が、移動体が障害物から離れる方向である場合には、外乱が移動体に作用することによって、移動体が障害物から離れ得る。したがって、外乱の影響を含む移動体の状態量の不確かさに起因して移動体が障害物に近づく可能性が低下する。一方、外乱方向が、移動体が障害物に近づく方向である場合には、外乱が移動体に作用することによって、移動体が障害物に近づき得る。したがって、移動体の状態量の不確かさに起因して移動体が障害物に近づく可能性が高まる。上記構成によれば、移動体の状態量の不確かさに起因して移動体が障害物に近づく可能性が小さい場合には、状態量の不確かさに応じた補正距離が小さくなり、移動体の状態量の不確かさに起因して移動体が障害物に近づく可能性が大きい場合には、状態量の不確かさに応じた補正距離が大きくなる。このため、移動体が障害物に近づく可能性に応じて回避距離が適切に算出される。その結果、制御量を適正化することが可能となる。

【0012】

算出部は、外乱方向が移動体が障害物から離れる方向である場合、補正距離をゼロに設定してもよい。外乱方向が、移動体が障害物から離れる方向である場合には、外乱が移動体に作用することによって、移動体が障害物から離れ得る。したがって、移動体の状態量の不確かさに起因して移動体が障害物に近づく可能性が低下する。このため、移動体の状態量の不確かさに応じた補正距離を考慮しなくても、移動体が障害物を回避することができる。上記構成によれば、移動体の状態量の不確かさに起因して移動体が障害物に近づく可能性が低い場合には、回避距離は、移動体の状態量の不確かさに応じた成分（補正距離）を含まないので、移動体と障害物との距離を必要以上に大きく確保する必要が無くなる。その結果、制御量を適正化することが可能となる。

【0013】

補正距離は、外乱に関するプロセスノイズの成分を含んでもよく、算出部は、外乱方向が移動体が障害物から離れる方向である場合、成分をゼロに設定してもよい。外乱方向が、移動体が障害物から離れる方向である場合には、外乱が移動体に作用することによって、移動体が障害物から離れ得る。したがって、移動体の状態量の不確かさに起因して移動体が障害物に近づく可能性が低下する。このため、外乱に関するプロセスノイズを考慮しなくても、移動体が障害物を回避することができる。上記構成によれば、移動体の状態量の不確かさに起因して移動体が障害物に近づく可能性が低い場合には、回避距離は、外乱に関するプロセスノイズの成分を含まないので、移動体と障害物との距離を必要以上に大きく確保する必要が無くなる。その結果、制御量を適正化することが可能となる。

【0014】

本開示の別の側面に係る移動体制御システムは、上記の移動体制御装置と、状態量を検出する状態量検出装置と、外乱を計測する計測装置と、障害物を検出する障害物検出装置と、を備える。この移動体制御システムは、上述の移動体制御装置を備えているので、制御量を適正化することができる。

【0015】

［２］実施形態の例示
以下、本開示の実施形態について、図面を参照しながら説明する。なお、図面の説明において同一要素には同一符号を付し、重複する説明を省略する。

【0016】

図１を参照して、一実施形態に係る移動体制御システムを説明する。図１は、一実施形態に係る移動体制御システムの概略構成図である。図１に示される移動体制御システム１は、自律制御型の移動体Ｍを制御するシステムである。移動体Ｍの例としては、水上航走体（Autonomous Surface Vehicle：ＡＳＶ）、遠隔操作ロボット（Remotely Operated Vehicle：ＲＯＶ）、及び自律型水中航走体（Autonomous Underwater Vehicle：ＡＵＶ）が挙げられる。本実施形態では、移動体ＭとしてＡＳＶを例示する。移動体Ｍは、移動体制御システム１から出力された制御量によって制御される。移動体Ｍは、制御対象の移動装置として、例えば、スラスタ及び操舵アクチュエータ等のアクチュエータを含む。制御量は、移動体Ｍを制御するために用いられる値である。制御量の例としては、舵の切り角、及びエンジンの回転数等が挙げられる。

【0017】

移動体制御システム１は、状態量検出装置２と、計測装置３と、障害物検出装置４と、移動体制御装置１０と、を備える。

【0018】

状態量検出装置２は、移動体Ｍの状態量を検出する装置である。移動体Ｍの状態量は、移動体Ｍの状態を示す値である。移動体Ｍの状態量の例としては、移動体Ｍの位置情報、速度、角速度、及び姿勢等が挙げられる。なお、位置情報は、水面に沿って規定された２次元のＸＹ直交座標系（図４参照）によって表されてもよい。状態量検出装置２は、例えば、ＧＮＳＳ（全地球航法衛星システム）、及びドップラー式超音波速度計を含む。

【0019】

計測装置３は、外乱を計測し、外乱情報を生成する装置である。外乱情報は、移動体Ｍの移動に影響を及ぼす環境情報である。外乱情報の例としては、風及び潮流に関する情報が挙げられる。計測装置３は、例えば、風向計、及び風速計を含む。

【0020】

障害物検出装置４は、障害物の状態量を検出する装置である。障害物の状態量は、障害物の状態を示す値である。障害物の状態量の例としては、障害物の位置情報、進行方向、及び速度が挙げられる。障害物検出装置４は、例えば、船舶用レーダー、及びＬｉＤＡＲ（Light Detection And Ranging）である。

【0021】

移動体制御装置１０は、制御量を用いて移動体Ｍを制御する装置である。移動体制御装置１０は、移動体Ｍの状態量、外乱情報、及び障害物の状態量に基づいて、制御量を算出する。

【0022】

移動体制御装置１０は、ＣＰＵ（Central Processing Unit）、主記憶装置であるＲＡＭ（Random Access Memory）及びＲＯＭ（Read Only Memory）、他の機器との間の通信を行う通信モジュール、並びにハードディスク等の補助記憶装置等のハードウェアを備えるコンピュータとして構成される。これらの構成要素が動作することにより、図２に示される移動体制御装置１０の各機能的構成要素が実現される。

【0023】

次に、図２を参照して、移動体制御装置１０の機能的構成要素を説明する。図２は、図１に示される移動体制御装置の機能ブロック図である。図２に示されるように、移動体制御装置１０は、機能的構成要素として、取得部１１と、推定部１２と、算出部１３と、出力部１４と、を備えている。

【0024】

取得部１１は、各種情報を取得する機能的構成要素である。取得部１１は、例えば、移動体Ｍの状態量、外乱情報、障害物の状態量、及び目標状態量を取得する。目標状態量は、状態量の目標値である。

【0025】

推定部１２は、障害物の中心位置、及び障害物の大きさを推定する機能的構成要素である。推定部１２は、障害物の状態量に基づいて、障害物の中心位置、及び障害物の大きさを推定する。

【0026】

算出部１３は、移動体Ｍの状態量を目標状態量に近づけるための制御量を算出する機能的構成要素である。算出部１３は、移動体Ｍと障害物との距離が回避距離よりも大きい状態を維持し続けるという制約条件のもとで、移動体Ｍの状態量が目標状態量に近づくように制御量を算出する。回避距離は、移動体Ｍが障害物を回避するための距離である。

【0027】

出力部１４は、制御量を出力する機能的構成要素である。出力部１４は、移動体Ｍに制御量を出力する。

【0028】

次に、図３及び図４を参照して、移動体制御装置１０が行う移動体制御方法を説明する。図３は、図１に示される移動体制御装置が行う移動体制御方法の一連の処理を示すフローチャートである。図４は、外乱方向と、障害物と移動体との位置関係と、を説明するための図である。

【0029】

図３に示される移動体制御方法の一連の処理は、例えば、ユーザが移動体Ｍの制御を行うための操作を行ったことによって開始され、その後、所定時間が経過するごとに繰り返し実施される。移動体Ｍの制御を行うための操作としては、移動体制御装置１０に目標状態量ｘ^ｄを入力することが挙げられる。なお、移動体Ｍの状態量がユーザによって入力された目標状態量ｘ^ｄに至るための経路上に位置する複数の中継点における目標状態量が、移動体Ｍから近い順に、移動体制御装置１０に目標状態量ｘ^ｄとして入力されてもよい。上記経路は、経路算出アルゴリズムを用いることで算出される。経路算出アルゴリズムとしては、例えば、Ａ－Ｓｔａｒ探索、及びダイクストラ法が挙げられる。ここでは、時刻ｋにおいて実施される移動体制御方法を説明する。時刻ｋは、０以上の整数であり、上述の所定時間が経過するごとにインクリメントする離散値である。以下の説明において、変数に下付き文字で時刻ｋが付されている場合には、その変数の時刻ｋにおける値を意味する。

【0030】

なお、式（１）に示される移動体Ｍの状態遷移方程式が予め求められて、不図示のメモリに格納されている。式（１）に示される状態遷移方程式は、時刻ｋにおける移動体Ｍの状態量ｘ_ｋと、時刻ｋにおいて移動体Ｍに入力した制御量ｕ_ｋと、時刻ｋ＋１における移動体Ｍの状態量ｘ_ｋ＋１との関係を規定する。状態量ｘ_ｋは、移動体Ｍの位置情報、姿勢、速度、及び角速度といった変数を含む。状態量ｘ_ｋに含まれる変数の数は適宜選択され得る。説明の便宜上、式（１）において時刻ｋを用いているが、式（１）における時刻ｋは、任意の時刻であり、移動体制御方法を実施する時刻に限定されない。

【数1】

【0031】

まず、取得部１１が、移動体Ｍの状態量ｘ_ｋ、外乱情報、障害物Ｏの状態量、及び目標状態量ｘ^ｄを取得する（ステップＳ１）。具体的には、取得部１１は、ユーザによって移動体制御装置１０に入力された目標状態量ｘ^ｄを取得する。取得部１１は、移動体Ｍの状態量ｘ_ｋを状態量検出装置２から取得する。取得部１１は、外乱情報を計測装置３から取得する。取得部１１は、外乱情報として外乱Ｗの速度ｖ_ｗ、及び外乱方向φを取得する。図４に示されるように、外乱方向φは、外乱の向きであり、本実施形態では、北（Ｘ軸正方向）を基準とした角度で表される。速度ｖ_ｗは、外乱方向φにおける外乱Ｗの速度である。取得部１１は、障害物Ｏの状態量を障害物検出装置４から取得する。そして、取得部１１は、目標状態量ｘ^ｄ、状態量ｘ_ｋ、外乱情報、及び障害物Ｏの状態量を算出部１３に出力するとともに、障害物Ｏの状態量を推定部１２に出力する。

【0032】

続いて、推定部１２は、障害物Ｏの中心位置、及び障害物の大きさを推定する（ステップＳ２）。具体的には、推定部１２は、取得部１１から障害物Ｏの状態量を受け取ると、障害物Ｏの状態量に基づいて、時刻ｋにおける障害物Ｏを、中心位置ｏ_ｋ及び半径Ｄ_Ｍａｘの円又は球で近似する。ここで、推定部１２は、実際の障害物Ｏを包含し得る円又は球で障害物Ｏを近似する。つまり、半径Ｄ_Ｍａｘは、最小離隔距離であって、移動体Ｍが中心位置ｏ_ｋから半径Ｄ_Ｍａｘ以上離れている場合には、移動体Ｍが障害物Ｏに接触も衝突もしないことを保証する距離である。なお、円又は球での近似手法は公知であるので、ここではその詳細な説明を省略する。

【0033】

障害物検出装置４として船舶用レーダーが用いられる場合には、推定部１２は、障害物Ｏの中心位置ｏ_ｋを障害物検出装置４から直接取得してもよい。障害物検出装置４としてＬｉＤＡＲが用いられる場合には、推定部１２は、障害物検出装置４から、障害物Ｏに関する点群情報を取得し、点群情報を処理することで、障害物Ｏの中心位置ｏ_ｋを算出してもよい。そして、推定部１２は、中心位置ｏ_ｋ、及び半径Ｄ_Ｍａｘを算出部１３に出力する。

【0034】

続いて、算出部１３は、制御量ｕ_ｋ＋１を計算する（ステップＳ３）。ステップＳ３では、まず、算出部１３は、目標状態量ｘ^ｄ、状態量ｘ_ｋ、外乱情報、及び障害物Ｏの状態量を取得部１１から受け取り、中心位置ｏ_ｋ、及び半径Ｄ_Ｍａｘを推定部１２から受け取る。そして、算出部１３は、例えば、モデル予測制御を用いて最適な制御量行列ｕを算出する。式（２）に示されるように、制御量行列ｕは、時刻ｋ＋１から時刻ｋ＋Ｎ_ｃまでのＮ_ｃ個の制御量（制御量ｕ_ｋ＋１～制御量ｕ_ｋ＋Ｎｃ）を配列した行列である。Ｎ_ｃは、制御ホライズンである。制御ホライズンは、最適な制御量行列ｕを算出する上で考慮する制御量行列ｕに関するステップの数（正の整数）である。

【数2】

【0035】

例えば、算出部１３は、式（３）に示される最適化問題を解くことで、最適な制御量行列ｕを算出する。式（３）の最適化問題は、障害物回避制約関数ｈ_ｋ＋ｉ（ｕ）が０以下であるという制約条件のもとで、評価関数Ｊ（ｕ）を最小化する制御量行列ｕを求めることを規定している。ｉは、１～Ｎ_ｐの整数値である。Ｎ_ｐは、予測ホライズンである。予測ホライズンは、最適な制御量行列ｕを算出する上で考慮する状態量に関するステップの数（正の整数）である。Ｎ_ｐは、Ｎ_ｃよりも大きくてもよく、Ｎ_ｃと同じでもよい。

【数3】

【0036】

評価関数Ｊ（ｕ）は、制御量行列ｕを評価するために用いられる関数である。式（４）に示されるように、評価関数Ｊ（ｕ）は、目標状態量ｘ^ｄと各時刻における状態量ｘ_ｋ＋ｉとの差の総和の項、制御量ｕ_ｋ＋ｉの大きさの総和の項、及び目標状態量ｘ^ｄと時刻ｋ＋Ｎ_ｐ＋１における状態量ｘ_{ｋ＋Ｎｐ＋ｉ}との差の項に重み行列Ｑ，Ｒ，Ｓをそれぞれ乗算することによって得られた値の和（評価値）を計算するための関数である。したがって、評価値が小さいほど、制御量の大きさを小さくしながら、状態量を目標状態量ｘ^ｄに近づけることができることを意味する。式（４）における評価関数Ｊ（ｕ）としては、２次形式の評価関数が用いられているが、この形式以外にもモデル予測制御で利用される一般的な評価関数が用いられてもよい。

【数4】

【0037】

なお、算出部１３は、式（１）に示される移動体Ｍの状態遷移方程式に、状態量ｘ_ｋと制御量行列ｕに含まれる制御量ｕ_ｋとを入力することによって、状態量ｘ_ｋ＋１を算出し、以降同様の処理を繰り返すことにより、状態量ｘ_ｋ＋２から状態量ｘ_{ｋ＋Ｎｐ＋１}を順に算出する。

【0038】

障害物回避制約関数ｈ_ｋ＋ｉ（ｕ）は、移動体Ｍが障害物Ｏを回避するための制約条件を規定するための関数である。障害物回避制約関数ｈ_ｋ＋ｉ（ｕ）は、回避距離から距離Ｄ_ｋ＋ｉを減算することによって得られる距離を計算するための関数である。回避距離は、補正距離σ’_ｋ＋ｉを含む。補正距離σ’_ｋ＋ｉは、移動体Ｍの状態量の不確かさに応じて算出される値である。ここでは、回避距離は、障害物Ｏの半径Ｄ_Ｍａｘに補正距離σ’_ｋ＋ｉを加算することによって求められる。距離Ｄ_ｋ＋ｉは、時刻ｋ＋ｉにおける移動体Ｍと障害物Ｏ（中心位置ｏ_ｋ＋ｉ）との距離である。半径Ｄ_Ｍａｘは、実際の障害物Ｏを包含するように近似された円又は球の半径であるので、障害物回避制約関数ｈ_ｋ＋ｉ（ｕ）によって計算された距離が０以下であることは、移動体Ｍが障害物と接触も衝突もしないことを意味する。

【数5】

【0039】

算出部１３は、式（６）を用いて距離Ｄ_ｋ＋ｉを算出する。距離Ｄ_ｋ＋ｉは、時刻ｋ＋ｉにおける移動体Ｍの位置座標（Ｘ_ｋ＋ｉ，Ｙ_ｋ＋ｉ）と、障害物Ｏの中心位置ｏ_ｋ＋ｉの座標（Ｘ_ｏｋ＋ｉ，Ｙ_ｏｋ＋ｉ）との距離である。

【数6】

【0040】

ここで、算出部１３は、外乱方向φと、障害物Ｏと移動体Ｍとの位置関係と、に基づいて回避距離を算出する。具体的には、算出部１３は、外乱方向φと、障害物Ｏと移動体Ｍとの位置関係と、に基づいて補正距離σ’_ｋ＋ｉを算出する。より具体的に説明すると、式（７）に示されるように、算出部１３は、方角θ_ｋ＋ｉと外乱方向φとの角度差に基づいて、補正距離σ’_ｋ＋ｉを算出する。図４に示されるように、方角θ_ｋ＋ｉは、障害物Ｏの中心位置ｏ_ｋ＋ｉから移動体Ｍの位置への方向であり、本実施形態では、北（Ｘ軸正方向）を基準とした角度で表される。

【数7】

【0041】

式（７）に示されるように、算出部１３は、移動体Ｍの位置座標、及び中心位置ｏ_ｋ＋ｉの座標に基づいて方角θ_ｋ＋ｉを算出し、方角θ_ｋ＋ｉと外乱方向φとの角度差（方角θ_ｋ＋ｉと外乱方向φとがなす角度）が９０度よりも小さい（鋭角である）場合には、補正距離σ’_ｋ＋ｉをゼロに設定する。一方、算出部１３は、方角θ_ｋ＋ｉと外乱方向φとの角度差（方角θ_ｋ＋ｉと外乱方向φとがなす角度）が９０度以上である場合には、補正距離σ’_ｋ＋ｉをマージンσ_ｋ＋ｉに設定する。

【0042】

ここで、マージンσ_ｋ＋ｉの導出方法を説明する。式（８）に示されるように、算出部１３は、行列Ｂ_ｋ＋ｉと、移動体Ｍの誤差分散行列ＣｏｖＸ_ｋ＋ｉと、を用いて、マージンσ_ｋ＋ｉを算出する。

【数8】

【0043】

行列Ｂ_ｋ＋ｉは、衝突回避制約関数ｇ_ｋ＋ｉの偏微分行列である。式（９）に示されるように、算出部１３は、衝突回避制約関数ｇ_ｋ＋ｉを状態量に関して偏微分することによって行列Ｂ_ｋ＋ｉを算出する。

【数9】

【0044】

衝突回避制約関数ｇ_ｋ＋ｉは、移動体Ｍの状態量に誤差が生じないと仮定した場合の、移動体Ｍが障害物Ｏとの衝突を回避するための制約条件を規定するための関数である。本実施形態では、式（１０）に示されるように、衝突回避制約関数ｇ_ｋ＋ｉは、半径Ｄ_Ｍａｘから距離Ｄ_ｋ＋ｉを減算することを規定する関数である。

【数10】

【0045】

誤差分散行列ＣｏｖＸ_ｋ＋ｉは、移動体Ｍの位置の不確かさを表す。算出部１３は、式（１１）を用いて、誤差分散行列ＣｏｖＸ_ｋ＋ｉを算出する。式（１１）に示されるように、時刻ｋ＋ｉ＋１における誤差分散行列ＣｏｖＸ_{ｋ＋ｉ＋１}は、行列Ａ_ｋ＋ｉと、時刻ｋ＋ｉにおける誤差分散行列ＣｏｖＸ_ｋ＋ｉと、プロセスノイズＮ_ｐｒｏｃと、プロセスノイズＮ_ｆと、回転行列Ｒ（φ）と、刻み時間ｄτと、を用いて、算出される。刻み時間ｄτは、モデル予測制御の刻み時間であって、時刻ｋが１増加するのに要する時間である。

【数11】

【0046】

行列Ａ_ｋ＋ｉは、式（１）に示される移動体Ｍの状態遷移方程式の偏微分行列である。式（１２）に示されるように、算出部１３は、式（１）に示される状態遷移方程式を状態量に関して偏微分することによって、行列Ａ_ｋ＋ｉを算出する。

【数12】

【0047】

プロセスノイズＮ_ｐｒｏｃは、移動体Ｍのプロセスノイズである。プロセスノイズＮ_ｐｒｏｃとしては、カルマンフィルタ等の一般的な状態推定手法で設定されるプロセスノイズと同じ値が用いられてもよい。

【0048】

プロセスノイズＮ_ｆは、外乱Ｗに関するプロセスノイズである。プロセスノイズＮ_ｆは、例えば、式（１３）によって表される。位置変化量Δ（ｖ_ｗ）は、速度ｖ_ｗを有する外乱Ｗによって移動体Ｍが移動する移動量である。パラメータαは、誤差楕円の長軸と短軸との比率を示すパラメータである。パラメータαは、０より大きく、１より小さい値を取り得る。パラメータαが小さいほど、誤差楕円の短軸に対して長軸が長くなる。

【数13】

【0049】

なお、算出部１３は、外乱Ｗの速度ｖ_ｗを位置変化量Δ（ｖ_ｗ）に変換する変換関数を用いて、位置変化量Δ（ｖ_ｗ）を算出する。この変換関数は、予め求められて、不図示のメモリに格納されていてもよい。例えば、外乱Ｗが風である場合には、回帰分析によって、十分な推定精度を有する変換関数が得られる。位置変化量Δ（ｖ_ｗ）を正確に得ることが難しい場合、つまり、十分な推定精度を有する変換関数が得られない場合には、算出部１３は、移動体Ｍが障害物Ｏに衝突するリスクを低減するために、位置変化量Δ（ｖ_ｗ）として大きい値を設定してもよい。

【0050】

回転行列Ｒ（φ）は、プロセスノイズＮ_ｆを外乱方向φに伸展させるための行列である。言い換えると、回転行列Ｒ（φ）は、プロセスノイズＮ_ｆがもたらす誤差楕円の長軸を角度φだけ回転させるための行列である。算出部１３は、式（１４）を用いて、回転行列Ｒ（φ）を算出する。回転行列Ｒ（φ）によって、プロセスノイズＮ_ｆは、外乱方向φが考慮されたプロセスノイズに変換される。

【数14】

【0051】

以上の式（８）～式（１４）を用いて、算出部１３は、マージンσ_ｋ＋ｉを算出する。

【0052】

なお、最適化問題の解法には、ＳＱＰ（Sequential Quadratic Programming）等の一般的な方法が用いられてもよい。そして、算出部１３は、最適な制御量行列ｕの第１成分である制御量ｕ_ｋ＋１を出力部１４に出力する。

【0053】

続いて、出力部１４は、算出部１３から制御量ｕ_ｋ＋１を受け取ると、制御量ｕ_ｋ＋１を移動体Ｍに出力する（ステップＳ４）。具体的には、出力部１４は、移動体Ｍの移動装置に制御量ｕ_ｋ＋１を出力する。そして、移動体Ｍの移動装置は、移動体制御装置１０から制御量ｕ_ｋ＋１を受け取ると、制御量ｕ_ｋ＋１に応じて動作する。

【0054】

以上により、移動体制御方法の一連の処理が終了する。

【0055】

次に、図５の（ａ）、及び図５の（ｂ）を参照して、移動体制御システム１及び移動体制御装置１０の作用効果を説明する。図５の（ａ）は、図１に示される移動体制御システムによって制御された移動体の移動軌跡の一例を示す図である。図５の（ｂ）は、比較例の移動体制御システムによって制御された移動体の移動軌跡の一例を示す図である。図５の（ａ）、及び図５の（ｂ）において、移動体Ｍは、目的地Ｐｄに向かって航行している。

【0056】

比較例の移動体制御システムにおいては、移動体制御装置の算出部が、方角θ_ｋ＋ｉと外乱方向φとの角度差に関係なく、補正距離σ’_ｋ＋ｉをマージンσ_ｋ＋ｉに設定する点において、移動体制御システム１と相違する。このため、上記角度差が鋭角である場合においても、補正距離σ’_ｋ＋ｉはマージンσ_ｋ＋ｉに設定される。したがって、図５の（ｂ）に示されるように、状態遷移方程式によって算出される各時刻における移動体Ｍの位置を中心として、誤差楕円が設定される。この誤差楕円は、外乱方向φに沿った長軸を有し、半径Ｄ_Ｍａｘにマージンσ_ｋ＋ｉを加算することによって得られる長径を有する。つまり、移動体Ｍが障害物Ｏから離れる方向に外乱Ｗが移動体Ｍに作用しているにも関わらず、移動体Ｍは、障害物Ｏの半径Ｄ_Ｍａｘにマージンσ_ｋ＋ｉを加算することによって得られる距離以上、障害物Ｏとの距離を確保している。

【0057】

一方、移動体制御システム１においては、方角θ_ｋ＋ｉと外乱方向φとの角度差が鋭角である場合には、補正距離σ’_ｋ＋ｉはゼロに設定される。したがって、図５の（ａ）に示されるように、状態遷移方程式によって算出される各時刻における移動体Ｍの位置を中心として、誤差楕円が設定される。この誤差楕円は、外乱方向φに沿った長軸を有し、半径Ｄ_Ｍａｘの長径を有する。つまり、移動体Ｍが障害物Ｏから離れる方向に外乱Ｗが移動体Ｍに作用しているので、移動体Ｍは、半径Ｄ_Ｍａｘ以上、障害物Ｏとの距離を確保している。その結果、移動体制御システム１によって制御された移動体Ｍは、比較例の移動体制御システムによって制御された移動体Ｍと比較して、障害物Ｏとの距離を必要以上に確保することなく、目的地Ｐｄに向かって航行している。

【0058】

以上説明したように、移動体制御システム１及び移動体制御装置１０においては、外乱Ｗが移動体Ｍに作用する外乱方向φと、障害物Ｏと移動体Ｍとの位置関係と、に基づいて回避距離が算出される。外乱Ｗが移動体Ｍに作用することで、移動体Ｍは、外乱方向φに移動体Ｍを移動させる力を受ける。このとき、外乱方向φが、移動体Ｍが障害物Ｏから離れる方向である場合には、外乱Ｗが移動体Ｍに作用することによって、移動体Ｍは障害物Ｏから離れ得る。一方、外乱方向φが、移動体Ｍが障害物Ｏに近づく方向である場合には、外乱Ｗが移動体Ｍに作用することによって、移動体Ｍは障害物Ｏに近づき得る。したがって、外乱方向φと、障害物Ｏと移動体Ｍとの位置関係と、が考慮されることによって、回避距離を適切に算出することができる。このようにして算出された回避距離を用いた制約条件のもとで最適な制御量行列ｕが算出されるので、制御量ｕ_ｋ＋１を適正化することが可能となる。

【0059】

外乱方向φが、移動体Ｍが障害物Ｏから離れる方向である場合には、外乱Ｗが移動体Ｍに作用することによって、移動体Ｍが障害物Ｏから離れ得る。したがって、外乱Ｗの影響を含む移動体Ｍの状態量の不確かさに起因して移動体Ｍが障害物Ｏに近づく可能性が低下する。一方、外乱方向φが、移動体Ｍが障害物Ｏに近づく方向である場合には、外乱Ｗが移動体Ｍに作用することによって、移動体Ｍが障害物Ｏに近づき得る。したがって、移動体Ｍの状態量の不確かさに起因して移動体Ｍが障害物Ｏに近づく可能性が高まる。上記実施形態では、算出部１３は、外乱方向φが、移動体Ｍが障害物Ｏから離れる方向である場合の補正距離σ’_ｋ＋ｉを、外乱方向φが、移動体Ｍが障害物Ｏに近づく方向である場合の補正距離σ’_ｋ＋ｉよりも小さい値に設定している。この構成によれば、移動体Ｍの状態量の不確かさに起因して移動体Ｍが障害物Ｏに近づく可能性が小さい場合には、状態量の不確かさに応じた補正距離σ’_ｋ＋ｉが小さくなり、移動体Ｍの状態量の不確かさに起因して移動体Ｍが障害物に近づく可能性が大きい場合には、状態量の不確かさに応じた補正距離σ’_ｋ＋ｉが大きくなる。このため、移動体Ｍが障害物Ｏに近づく可能性に応じて回避距離が適切に算出される。その結果、制御量ｕ_ｋ＋１を適正化することが可能となる。

【0060】

より具体的には、算出部１３は、外乱方向φが、移動体Ｍが障害物Ｏに近づく方向である場合、補正距離σ’_ｋ＋ｉをマージンσ_ｋ＋ｉに設定し、外乱方向φが、移動体Ｍが障害物Ｏから離れる方向である場合、補正距離σ’_ｋ＋ｉをゼロに設定している。上述のように、外乱方向φが、移動体Ｍが障害物Ｏから離れる方向である場合には、移動体Ｍの状態量の不確かさに起因して移動体Ｍが障害物Ｏに近づく可能性が低下する。このため、補正距離σ’_ｋ＋ｉを考慮しなくても、移動体Ｍが障害物Ｏを回避することができる。上記構成によれば、移動体Ｍの状態量の不確かさに起因して移動体Ｍが障害物Ｏに近づく可能性が低い場合には、回避距離は、移動体Ｍの状態量の不確かさに応じた成分（補正距離σ’_ｋ＋ｉ）を含まないので、移動体Ｍと障害物Ｏとの距離を必要以上に大きく確保する必要が無くなる。その結果、制御量ｕ_ｋ＋１を適正化することが可能となる。言い換えると、移動体Ｍが障害物Ｏから離れる方向である場合には、回避距離が小さくなるので、制約条件は緩和される。したがって、制御量行列の選択肢を増やすことができる。よって、算出部１３は、評価関数Ｊ（ｕ）の評価値をさらに小さくする制御量行列ｕを最適な制御量行列ｕとして決定することができる。その結果、制御量ｕ_ｋ＋１を適正化することが可能となる。

【0061】

以上、本開示の実施形態について説明したが、本開示は上記実施形態に限定されない。

【0062】

上記実施形態では、方角θ_ｋ＋ｉと外乱方向φとの角度差が鋭角である場合には、算出部１３は、補正距離σ’_ｋ＋ｉをゼロに設定しているが、マージンσ_ｋ＋ｉより小さい正の値に設定してもよい。この場合も、移動体Ｍの状態量の不確かさに起因して移動体Ｍが障害物Ｏに近づく可能性が小さい場合には、状態量の不確かさに応じた補正距離σ’_ｋ＋ｉが小さくなり、移動体Ｍの状態量の不確かさに起因して移動体Ｍが障害物に近づく可能性が大きい場合には、状態量の不確かさに応じた補正距離σ’_ｋ＋ｉが大きくなる。このため、移動体Ｍが障害物Ｏに近づく可能性に応じて回避距離が適切に算出される。その結果、制御量ｕ_ｋ＋１を適正化することが可能となる。

【0063】

上記実施形態では、方角θ_ｋ＋ｉと外乱方向φとの角度差が鋭角である場合には、算出部１３は、補正距離σ’_ｋ＋ｉをゼロにしているが、プロセスノイズＮ_ｆをゼロに設定してもよい。すなわち、外乱方向φが、移動体Ｍが障害物Ｏから離れる方向である場合、算出部１３は、プロセスノイズＮ_ｆをゼロに設定してもよい。補正距離σ’_ｋ＋ｉはプロセスノイズＮ_ｆの成分を含むので、外乱方向φが、移動体Ｍが障害物Ｏから離れる方向である場合の補正距離σ’_ｋ＋ｉを、外乱方向φが、移動体Ｍが障害物Ｏに近づく方向である場合の補正距離σ’_ｋ＋ｉよりも小さい値に設定することができる。上述のように、外乱方向φが、移動体Ｍが障害物Ｏから離れる方向である場合には、移動体Ｍの状態量の不確かさに起因して移動体Ｍが障害物Ｏに近づく可能性が低下する。このため、プロセスノイズＮ_ｆを考慮しなくても、移動体Ｍが障害物Ｏを回避することができる。上記構成によれば、移動体Ｍの状態量の不確かさに起因して移動体Ｍが障害物Ｏに近づく可能性が低い場合には、回避距離は、プロセスノイズＮ_ｆの成分を含まないので、移動体Ｍと障害物Ｏとの距離を必要以上に大きく確保する必要が無くなる。その結果、制御量ｕ_ｋ＋１を適正化することが可能となる。

【0064】

障害物Ｏが移動している場合には、障害物検出装置４は、障害物Ｏの状態量の一つとして、障害物Ｏの速度を検出してもよい。推定部１２は、各時刻における中心位置ｏ_ｋ＋１を、等速運動モデルを用いて算出してもよい。

【0065】

障害物検出装置４は、障害物Ｏを、中心位置ｏ_ｋ、及び半径Ｄ_Ｍａｘの円又は球に近似する機能を有してもよい。この場合、移動体制御装置１０は、推定部１２を備えていなくてもよい。

【0066】

移動体Ｍは、水上航走体（ＡＳＶ）であってもよい。この場合、風が一定方向から吹く水面又は海面を、上記水上航走体が航行するときに、移動体制御システム１が適切な制御量を算出することができる。その結果、上記水上航走体の航行の精度が向上する。

【0067】

移動体Ｍは、クローラ型の車輪を備えた車輪型移動ロボットであってもよい。この場合、軟弱な傾斜面を上記車輪型移動ロボットが走行するときに、軟弱な傾斜面における小さな地滑りによって、一定時間において、一定の距離を上記車輪型移動ロボットが滑ることを考慮して、移動体制御システム１が適切な制御量を算出することができる。その結果、上記車輪型移動ロボットの走行の精度が向上する。

【0068】

移動体Ｍは、水中移動ロボット（ＡＵＶ）であってもよい。この場合、海流又は潮流の存在する環境下で、上記水中移動ロボットが航走するときに、移動体制御システム１が適切な制御量を算出することができる。その結果、上記水中移動ロボットの航走の精度が向上する。

【0069】

移動体Ｍは、ドローンであってもよい。この場合、風が一定方向から吹く環境下で、上記ドローンが飛行するときに、移動体制御システム１が適切な制御量を算出することができる。その結果、上記ドローンの飛行の精度が向上する。

【符号の説明】

【0070】

１ 移動体制御システム
２ 状態量検出装置
３ 計測装置
４ 障害物検出装置
１０ 移動体制御装置
１３ 算出部
１４ 出力部
Ｍ 移動体
Ｏ 障害物
Ｗ 外乱
φ 外乱方向

【図1】

【図2】

【図3】

【図4】

【図5】