特開 2024-90839 制御装置、移動体、制御方法及びプログラム

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

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

(11)【公開番号】P2024090839

(43)【公開日】2024-07-04

(54)【発明の名称】制御装置、移動体、制御方法及びプログラム

(51)【国際特許分類】

   G05D 1/00 20240101AFI20240627BHJP

   G05D 1/43 20240101ALI20240627BHJP

   G05D 1/46 20240101ALI20240627BHJP

【ＦＩ】

G05D1/00 A

G05D1/02 H

G05D1/10

【審査請求】未請求

【請求項の数】10

【出願形態】ＯＬ

(21)【出願番号】P 2022206982

(22)【出願日】2022-12-23

(71)【出願人】

【識別番号】000000974

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

(74)【代理人】

【識別番号】110003052

【氏名又は名称】弁理士法人勇智国際特許事務所

(72)【発明者】

【氏名】中村 和行

(72)【発明者】

【氏名】開田 真次

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

5H301

【Ｆターム（参考）】

5H301AA01

5H301AA05

5H301AA06

5H301AA10

5H301CC04

5H301CC07

5H301GG07

5H301GG08

5H301GG17

(57)【要約】

【課題】移動体において、障害物との衝突を効率的に回避しつつ目標位置に到達可能な技術を提供する。
【解決手段】移動体の推進装置を制御する制御装置は、第１検出装置が移動体の周囲を検出することにより生成された点群データが入力される入力部と、制御部とを備える。制御部は、移動体の自己位置と移動体の目標位置とを用いて移動体を目標位置まで移動させるための推進装置への指令値を生成し、生成された点群データに基づく点データが移動体を引き離すための仮想的な斥力を示すベクトルであって移動体との距離が近いほど大きくなる斥力ベクトルを複数の点データについて算出し、算出された複数の斥力ベクトルを合計して斥力ベクトル和を算出し、算出された斥力ベクトル和を用いて、生成された指令値を修正する。
【選択図】図２

【特許請求の範囲】

【請求項1】

移動体の推進装置を制御する制御装置であって、
第１検出装置が前記移動体の周囲を検出することにより生成された点群データが入力される入力部と、
制御部とを備え、
前記制御部は、
前記移動体の自己位置と前記移動体の目標位置とを用いて前記移動体を前記目標位置まで移動させるための前記推進装置への指令値を生成し、
生成された前記点群データに基づく点データが前記移動体を引き離すための仮想的な斥力を示すベクトルであって前記移動体との距離が近いほど大きくなる斥力ベクトルを、複数の前記点データについて算出し、算出された複数の前記斥力ベクトルを合計して斥力ベクトル和を算出し、
算出された前記斥力ベクトル和を用いて、生成された前記指令値を修正する、
制御装置。

【請求項2】

請求項１に記載の制御装置であって、
前記制御部は、
前記点群データを予め定められた大きさの第１領域で分割し、分割された前記第１領域内に含まれる前記点群データについて、１つの代表点データを特定し、
前記点データとして前記代表点データを用い、前記斥力ベクトルを複数の前記代表点データについて算出する、制御装置。

【請求項3】

請求項１に記載の制御装置であって、
前記制御部は、更に、予め定められた個数以下で集合する前記点データを、前記斥力ベクトル和を算出するために用いる複数の前記点データから除く、制御装置。

【請求項4】

請求項１に記載の制御装置であって、
前記制御部は、更に、前記移動体に対して予め定められた第２領域に存在する前記点データを、前記斥力ベクトル和を算出するために用いる複数の前記点データから除く、制御装置。

【請求項5】

請求項１に記載の制御装置であって、
前記指令値は、前記移動体を前記目標位置へ向かう推進方向へ推進させるための推力指令値を含み、
前記制御部は、前記斥力ベクトル和を用いて前記推力指令値を修正する、制御装置。

【請求項6】

請求項５に記載の制御装置であって、
前記指令値は、前記移動体の方位を制御するための方位指令値を含み、
前記制御部は、前記移動体の方位が前記推進方向に一致するように前記方位指令値を修正する、制御装置。

【請求項7】

請求項１に記載の制御装置と、
前記第１検出装置と、
前記自己位置を検出する第２検出装置と、
前記推進装置と、を備える、移動体。

【請求項8】

請求項７に記載の移動体であって、
前記推進装置は、前記指令値に応じて、前記移動体を水平方向と前記水平方向に対して直交する垂直方向とへ推進させる、移動体。

【請求項9】

移動体の推進装置を制御する方法であって、
前記推進装置を制御する制御装置が、
前記移動体の自己位置と前記移動体の目標位置とを用いて前記移動体を前記目標位置まで移動させるための前記推進装置への指令値を生成し、
第１検出装置が前記移動体の周囲を検出することにより生成された点群データに基づく点データが前記移動体を引き離すための仮想的な斥力を示すベクトルであって前記移動体との距離が近いほど大きくなる斥力ベクトルを、複数の前記点データについて算出し、算出された複数の前記斥力ベクトルを合計して斥力ベクトル和を算出し、
算出された前記斥力ベクトル和を用いて、生成された前記指令値を修正する、方法。

【請求項10】

移動体の推進装置を制御するプログラムであって、
前記移動体の自己位置と前記移動体の目標位置とを用いて前記移動体を前記目標位置まで移動させるための前記推進装置への指令値を生成することと、
第１検出装置が前記移動体の周囲を検出することにより生成された点群データに基づく点データが前記移動体を引き離すための仮想的な斥力を示すベクトルであって前記移動体との距離が近いほど大きくなる斥力ベクトルを、複数の前記点データについて算出し、算出された複数の前記斥力ベクトルを合計して斥力ベクトル和を算出することと、
算出された前記斥力ベクトル和を用いて、生成された前記指令値を修正することと、
をコンピュータに実行させる、プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、移動体の推進装置を制御する制御装置、制御装置を備える移動体、制御方法及びプログラムに関する。

【背景技術】

【0002】

移動体と障害物との衝突を回避しつつ、移動体を目標位置まで到達させる技術が知られている。例えば、特許文献１には、ロボットアームと障害物の最短距離と障害物の相対速度とに基づいてポテンシャル場を算出し、算出されたポテンシャル場を用いてロボットアームの加速度ベクトルを算出する制御装置が記載されている。制御装置は、ロボットアームが移動するごとにポテンシャル場及び加速度ベクトルを逐次算出し、算出された加速度ベクトルに基づいてロボットアームを移動させる。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特許第６７６５１５６号公報

【発明の概要】

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

【0004】

特許文献１記載の技術では、ポテンシャル場を算出するための最短距離はロボットアームの移動によって逐次変化するため、障害物を回避するための加速度ベクトルも逐次変化し得る。そのため、障害物の形状やロボットアームの加速性能によっては、ロボットアームと障害物との衝突が回避できない虞や回避が非効率になる虞がある。したがって、ロボットアームに限らず移動体において、障害物との衝突を効率的に回避しつつ目標位置に到達可能な技術が求められる。

【0005】

本開示は、ポテンシャル場を算出するための最短距離が移動体の移動によって逐次変化することに起因して移動体と障害物との衝突が回避できない虞や回避が非効率になる虞があるという課題に対し、障害物との衝突を効率的に回避させながら移動体を目標位置へ到達させることができる移動体の推進装置を制御する制御装置を提供することを目的とする。

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

【0006】

本開示の第１の形態によれば、移動体の推進装置を制御する制御装置が提供される。この制御装置は、第１検出装置が前記移動体の周囲を検出することにより生成された点群データが入力される入力部と、制御部とを備える。前記制御部は、前記移動体の自己位置と前記移動体の目標位置とを用いて前記移動体を前記目標位置まで移動させるための前記推進装置への指令値を生成する。前記制御部は、生成された前記点群データに基づく点データが前記移動体を引き離すための仮想的な斥力を示すベクトルであって前記移動体との距離が近いほど大きくなる斥力ベクトルを、複数の前記点データについて算出する。前記制御部は、算出された複数の前記斥力ベクトルを合計して斥力ベクトル和を算出する。前記制御部は、算出された前記斥力ベクトル和を用いて、生成された前記指令値を修正する。

【0007】

本開示の第２の形態によれば、移動体の推進装置を制御する方法が提供される。この方法は、前記推進装置を制御する制御装置が、前記移動体の自己位置と前記移動体の目標位置とを用いて前記移動体を前記目標位置まで移動させるための前記推進装置への指令値を生成し、第１検出装置が前記移動体の周囲を検出することにより生成された点群データに基づく点データが前記移動体を引き離すための仮想的な斥力を示すベクトルであって前記移動体との距離が近いほど大きくなる斥力ベクトルを、複数の前記点データについて算出し、算出された複数の前記斥力ベクトルを合計して斥力ベクトル和を算出し、算出された前記斥力ベクトル和を用いて、生成された前記指令値を修正する、方法である。

【0008】

本開示の第３の形態によれば、移動体の推進装置を制御するプログラムが提供される。このプログラムは、前記移動体の自己位置と前記移動体の目標位置とを用いて前記移動体を前記目標位置まで移動させるための前記推進装置への指令値を生成することと、第１検出装置が前記移動体の周囲を検出することにより生成された点群データに基づく点データが前記移動体を引き離すための仮想的な斥力を示すベクトルであって前記移動体との距離が近いほど大きくなる斥力ベクトルを、複数の前記点データについて算出し、算出された複数の前記斥力ベクトルを合計して斥力ベクトル和を算出することと、算出された前記斥力ベクトル和を用いて、生成された前記指令値を修正することと、をコンピュータに実行させる、プログラムである。

【発明の効果】

【0009】

本開示により、移動体の推進装置を制御する制御装置であって、障害物との衝突を効率的に回避させながら移動体を目標位置へ到達させることができる制御装置を提供できる。

【図面の簡単な説明】

【0010】

【図1】第１実施形態における移動体の概略図である。

【図2】第１実施形態における移動体の制御系統の構成を示すブロック図である。

【図3】推進処理を説明するためのイメージ図である。

【図4】第１実施形態における推進処理のフローチャートである。

【図5】物体に対する移動体の回避動作の例を示すイメージ図である。

【図6】別の物体に対する移動体の回避動作の例を示すイメージ図である。

【図7】第２実施形態における移動体の概略図である。

【図8】第２実施形態における推進処理のフローチャートである。

【図9】第３実施形態における移動体の概略図である。

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

【0011】

＜第１実施形態＞
図１は、第１実施形態における移動体の概略図である。図２は、第１実施形態における移動体の制御系統の構成を示すブロック図である。本実施形態の移動体は、自律型無人潜水機（Ａｕｔｏｎｏｍｏｕｓ Ｕｎｄｅｒｗａｔｅｒ Ｖｅｈｉｃｌｅ）である。以下、本実施形態の移動体を、「ＡＵＶ」とも呼ぶ。

【0012】

ＡＵＶ１は、制御装置１０と、第１検出装置３０と、第２検出装置４０と、推進装置５０とを備える。ＡＵＶ１は、ＡＵＶ１に搭載されたバッテリの電力を動力として駆動される。

【0013】

推進装置５０は、ＡＵＶ１を水平方向と水平方向に直交する垂直方向とに移動可能に構成されている。推進装置５０は、ＡＵＶ１を前方へ移動させるための主推進用スラスタ５１と、ＡＵＶ１を上下方向に移動させるための垂直スラスタと、ＡＵＶ１を左右方向に移動させるための水平スラスタなど、複数の推進器及びＡＵＶ１の進路を変更する舵装置を含む。図１には、主推進用スラスタ５１が例示されている。ただし、推進装置５０は、上記の構成に限定されず、例えば推力を発生させる方向を変更可能な首振り式のスラスタを有していてもよい。推進装置５０は、制御装置１０から出力される指令値によって、ＡＵＶ１を推進させる。本実施形態では、指令値は、現在位置から目標位置へ向かう推進方向へＡＵＶ１を移動させるための推力指令値と、ＡＵＶ１の方位を制御するための方位指令値とを含む。

【0014】

図１には、ＡＵＶ１の座標系であるＡＵＶ座標系が例示されている。ＡＵＶ座標系は、互いに直交するＸ軸とＹ軸とＺ軸とを含む。ＡＵＶ座標系の原点Ｏは、ＡＵＶ１の所定の点である。本実施形態では、原点Ｏは、ＡＵＶ１の重心である。Ｘ軸は、Ｙ軸、Ｚ軸は、夫々、ＡＵＶ１の前後方向、左右方向、上下方向に対応する。推進装置５０は、制御装置１０から出力されるＸ軸、Ｙ軸、Ｚ軸に対応する推力指令値と、Ｘ軸回り、Ｙ軸回り、Ｚ軸回りのＡＵＶ１の角度に対応する方位指令値とによって、ＡＵＶ１を推進させる。なお、ＡＵＶ１は、ＡＵＶ１を前方へ移動させるための主推進用スラスタ５１を主に使用すると、消費電力を抑制できるという特性を有する。例えば、ＡＵＶ１の前後方向に対応するＸ軸が推進方向と一致すると、ＡＵＶ１は主に主推進用スラスタ５１によって推進できる。

【0015】

第１検出装置３０は、ＡＵＶ１の周囲を検出することにより点群データを生成するセンサである。点群データは、複数の点データの集合である。１つの点データには、ＡＵＶ１に対する点データの相対的な位置情報が含まれる。以下の説明では、点データは、単に「点」とも呼ばれる。本実施形態では、第１検出装置３０は、音波を発信して反射波を受信する音響センサである。第１検出装置３０はＡＵＶ１の前方に設けられている。第１検出装置３０は、音波の送受信を行って周囲を探知することによりＡＵＶ１の前方周囲の物体の位置を示す二次元点群データを取得する。なお、「物体」は、地形や構造物を含む。「物体」は、ＡＵＶ１の推進に対する障害物となり得る。

【0016】

第１検出装置３０は、ＡＵＶ１の推進中に二次元点群データの取得を繰り返し行うことで、三次元点群データを生成する。三次元点群データの座標系は、例えばＡＵＶ座標系である。第１検出装置３０は、生成した三次元点群データを制御装置１０に入力する。なお、第１検出装置３０は、点群データを取得可能な任意のセンサであればよく、例えば水中ライダ（Ｕｎｄｅｒｗａｔｅｒ ＬｉＤＡＲ）であってもよいし、直接、三次元点群データを取得できるマルチビームソナーであってもよい。また、ＡＵＶ１に設けられる第１検出装置３０の数は、複数であってもよい。例えば、第１検出装置３０は、ＡＵＶ１の前方上部と前方下部とに、１つずつ設けられてもよい。

【0017】

第２検出装置４０は、ＡＵＶ１の現在位置を検出する。現在位置は、ＡＵＶ１の自己位置とも呼ばれる。本実施形態では、第２検出装置４０は、加速度センサとジャイロセンサとを含む慣性航法装置（Ｉｎｅｒｔｉａｌ Ｎａｖｉｇａｔｉｏｎ Ｓｙｓｔｅｍ：ＩＮＳ）である。本実施形態では、第２検出装置４０の座標系の原点及び各軸は、ＡＵＶ座標系の原点及び各軸に一致する。第２検出装置４０は、加速度センサとジャイロセンサとの検出値を用いて、所定の起点からのＡＵＶ１の移動距離を算出することで、ＡＵＶ１の現在位置を検出する。第２検出装置４０は、更に、ジャイロセンサの検出値を用いて、ＡＵＶ１のＸ軸、Ｙ軸、Ｚ軸回りの角度を検出する。ＡＵＶ１のＸ軸、Ｙ軸、Ｚ軸回りの角度は、夫々、ＡＵＶ１のロール方向、ピッチ方向、ヨー方向の向きでもあり、ＡＵＶ１の方位を表す。ＡＵＶ１の方位は、ＡＵＶ１の姿勢や、ＡＵＶ１の向きとも呼ばれる。第２検出装置４０は、ＡＵＶ１の方位を検出する方位検出装置としても機能する。第２検出装置４０は、ＡＵＶ１の始動中に、現在位置及び方位を繰り返し検出して、制御装置１０に入力する。なお、第２検出装置４０は、ＤＶＬ（Ｄｏｐｐｌｅｒ Ｖｅｌｏｃｉｔｙ Ｌｏｇ）、ＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）、ＵＳＢＬ（Ｕｌｔｒａ Ｓｈｏｒｔ Ｂａｓｅ Ｌｉｎｅ）の少なくとも１つを含んでもよい。また、方位を検出するための検出装置は、自己位置を検出するための検出装置と別体であってもよい。

【0018】

図２に示すように、制御装置１０は、ＣＰＵ１１と、メモリ１５と、インターフェース１７とを含むコンピュータとして構成されている。メモリ１５は、不揮発性メモリと、揮発性メモリとを含む。インターフェース１７は、信号の受信を行う入力部とも呼ばれる。また、インターフェース１７は、信号の送信を行う出力部とも呼ばれる。ＣＰＵ１１は、メモリ１５に記憶されたプログラムを展開して実行することにより、制御部２１として機能する。

【0019】

制御部２１は、制御装置１０がオンにされ、インターフェース１７を介して目標位置が入力されると、周囲の物体を回避させつつＡＵＶ１を目標位置へ到達させるための推進処理を実行する。目標位置は、ウェイポイント（Ｗａｙ Ｐｏｉｎｔ）とも呼ばれる。なお、目標位置は、予めメモリ１５に記憶されていてもよい。

【0020】

図３は、制御部２１によって実行される推進処理を説明するためのイメージ図である。図３には、水中を推進するＡＵＶ１と、ＡＵＶ１の周囲に存在する物体１０１、１０２と、目標位置ＷＰとが例示されている。推進処理は、ＡＵＶ１の周囲の物体１０１、１０２の表面に存在する複数の微小な点Ｐ１、Ｐ２、Ｐ３、Ｐ４・・・の夫々が、ＡＵＶ１を遠ざける仮想的な斥力を発生させているという考えに基づく技術を使用して行われる。点群データに基づく１つの点は、１つの微小な物体とみなせるためである。斥力は、１つの点を発生源とする仮想的な力であり、１つの点とＡＵＶ１の所定の点との距離が近いほど大きくなる。斥力は、１つの点がＡＵＶ１を遠ざけるようにＡＵＶ１に及ぼす仮想的な反発力でもある。１つの点がＡＵＶ１に及ぼす仮想的な斥力ベクトルを算出し、複数の点の斥力ベクトルを合計した斥力ベクトル和Ｒｖは、ＡＵＶ１の周囲に存在する物体の表面形状を反映した力となる。本処理では、制御部２１が、斥力ベクトル和Ｒｖを用いて推力指令値Ｃ１を修正して推進装置５０へ出力することにより、ＡＵＶ１は、周囲の物体１０１、１０２を避ける経路ＲＴを取りながら、目標位置ＷＰへ到達する。以下、制御部２１が実行する推進処理について具体的に説明する。

【0021】

図４は、推進処理のフローチャートである。推進処理は、ＡＵＶ１の移動中に繰り返し実行される。ステップＳ１０では、制御部２１は、ＡＵＶ１を現在位置から目標位置まで到達させるための推進装置５０に対する指令値を算出する。本実施形態では、制御部２１は、第２検出装置４０により検出された自己位置と、目標位置とを用いて、自己位置から目標位置までの三次元距離偏差を算出する。三次元距離偏差は、ＡＵＶ座標系における、Ｘ軸、Ｙ軸、Ｚ軸ごとの距離である。三次元距離偏差を算出すると、制御部２１は、各軸の距離偏差に比例した各軸の推力指令値を算出する。また、制御部２１は、ＡＵＶ１のＸ軸が推進方向に一致するように、方位指令値を算出する。ステップＳ１０で算出される指令値は、現在位置と目標位置とを直線的に結び、かつ、船首方位が目標位置を向く指令値である。

【0022】

ステップＳ２０では、制御部２１は、第１検出装置３０により生成された三次元点群データを用いて、三次元地図データを生成する。三次元地図データは、斥力ベクトルの算出において用いられる点データの集合である。ステップＳ２０は、代表点データを特定するステップＳ２２と、予め定められた条件に該当する点データを除去するステップＳ２４とを含む。

【0023】

ステップＳ２２では、制御部２１は、三次元点群データを予め定められた大きさの第１領域に分割し、分割された各第１領域につき、１つの点のみを代表点データとして特定する。第１領域の大きさは、第１検出装置３０の分解能や、第１領域の大きさとＡＵＶ１の挙動とに関するシミュレーション等により定められる。本実施形態では、第１領域は、予め定められた大きさの立方体グリッドである。制御部２１は、三次元点群データを立方体グリッドに分割し、点群データを構成する点データが含まれるグリッドについて、代表点データを特定する。本実施形態では、代表点データは、グリッドの中心点である。制御部２１は、点データが含まれないグリッドについては、代表点データを特定しない。制御部２１は、点データが含まれる各グリッドについて代表点データを特定することで、三次元地図データを生成する。制御部２１は、生成した三次元地図データを、メモリ１５に記憶する。

【0024】

制御部２１は、更に、ステップＳ２４において、三次元地図データから、予め定められた第１条件を満たす点データをノイズとして除去する。本実施形態では、第１条件は、点データが、予め定められた個数以下で集合していることを含む。ステップＳ２４の処理は、ＡＵＶ１の推進に影響を与えない物体に基づく点データをノイズとみなして、三次元地図データから除去する処理である。本処理は、三次元地図データにおいて、所定の個数の点データが隣接して存在している場合には、当該点データを残す処理でもある。第１条件は、ＡＵＶ１が推進する環境や、ＡＵＶ１の推進装置５０の性能等に応じて、実験やシミュレーションを用いて予め定められる。ステップＳ２４が行われることにより、水中に浮遊するゴミ等のＡＵＶ１の推進に影響を与えない物体は、斥力ベクトルを算出するための点データとしてみなされなくなる。制御部２１は、ステップＳ２２で生成した三次元地図データから、第１条件を満たす点データを除去して、三次元地図データを更新する。制御部２１は、更新した三次元地図データを、メモリ１５に記憶する。なお、制御部２１は、取得してから予め定められた期間が経過した三次元点群データや三次元地図データを、メモリ１５から削除してもよい。

【0025】

ステップＳ３０では、制御部２１は、三次元地図データの各代表点データについて斥力ベクトルＲ１ｖを算出し、算出した複数の斥力ベクトルを合計して斥力ベクトル和Ｒｖを算出する。斥力ベクトルＲ１ｖは、例えば、以下の式（１）で表され、斥力ベクトル和Ｒｖは、例えば以下の式（２）で表される。式（１）において、Ｌ１は、三次元地図データにおける１つの点データとＡＵＶ１の原点Ｏとの距離である。Ｕｖは、１つの点データからＡＵＶ１の原点Ｏに向かう単位ベクトルである。Ｊは、単位距離における斥力ベクトルの大きさを示す定数である。Ｊは、シミュレーションや実験により求められる。

【0026】

【数1】

【0027】

【数2】

【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】

１：移動体，ＡＵＶ
１Ａ：移動体，車両
１Ｂ：移動体，ドローン
１０、１０Ａ、１０Ｂ：制御装置、
１１：ＣＰＵ
１５：メモリ
１７：インターフェース
２１，２１Ａ，２１Ｂ：制御部
３０，３０Ａ：第１検出装置
４０，４０Ａ：第２検出装置
５０，５１，５０Ａ，５０Ｂ，５１Ｂ，５２Ｂ，５３Ｂ，５４Ｂ：推進装置
１０１，１０２，１０３，１０４：物体
ＡＲ１，ＡＲ２：領域
Ｇ：地面
Ｓ：海底
Ｏ：原点
Ｐ，Ｐ１，Ｐ２，Ｐ３，Ｐ４：点
Ｃ１，Ｃ２，Ｃ３：推力指令値
Ｒｖ：斥力ベクトル和
ＲＴ：経路
ＷＰ：目標位置

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】