特開 2023-152498 ナビゲーション制御装置、ナビゲーション制御プログラム

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

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

(11)【公開番号】P2023152498

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

(54)【発明の名称】ナビゲーション制御装置、ナビゲーション制御プログラム

(51)【国際特許分類】

   G01C 21/34 20060101AFI20231010BHJP

   G08G 1/0968 20060101ALI20231010BHJP

   G06T 7/70 20170101ALI20231010BHJP

   G06T 7/00 20170101ALI20231010BHJP

   B25J 13/08 20060101ALI20231010BHJP

【ＦＩ】

G01C21/34

G08G1/0968

G06T7/70 A

G06T7/00 350B

B25J13/08 A

【審査請求】未請求

【請求項の数】8

【出願形態】ＯＬ

(21)【出願番号】P 2022062550

(22)【出願日】2022-04-04

(71)【出願人】

【識別番号】000003609

【氏名又は名称】株式会社豊田中央研究所

(74)【代理人】

【識別番号】110001519

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

(72)【発明者】

【氏名】田口 峻

(72)【発明者】

【氏名】廣瀬 徳晃

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

2F129

3C707

5H181

5L096

【Ｆターム（参考）】

2F129AA03

2F129BB12

2F129DD03

2F129DD13

2F129DD15

2F129DD21

2F129DD39

2F129DD70

2F129EE02

2F129EE26

2F129EE53

2F129EE65

2F129EE67

2F129EE78

2F129EE79

2F129EE94

2F129GG17

3C707CS08

3C707JS03

3C707KS36

3C707LS15

3C707LW03

3C707LW12

3C707MT04

5H181AA01

5H181BB20

5H181CC04

5H181FF10

5H181FF22

5H181FF27

5H181FF35

5L096AA06

5L096BA04

5L096BA05

5L096CA04

5L096FA66

5L096FA67

5L096FA69

5L096GA51

5L096HA11

5L096JA03

5L096KA04

(57)【要約】

【課題】推定誤差の積算が起きず、かつ、内部状態の特性を利用することもでき、精度とロバスト性を向上させる。
【解決手段】画像だけで目標位置に行ける技術（一例としてＤＶＭＰＣ）を、スケールは推定できないが画像単体から位置を推定できるシステムと組み合わせることで、粗い位置しか推定できない画像位置推定を、有効に、ナビゲーション制御に利用することができる。Ｖ－ＳＬＡＭでは、推定誤差が重畳されるため、制御が困難である。ＳＰＴＭでは、漠然と画像として似ていることで判断しており、全く異なる場所に推定される場合があり、検索が精度の面でよくない。これに対して、本構成は、位置をリファレンス、検索するための情報に利用することで、画像を直接近づけるように制御することで、位置誤差がない。
【選択図】図１

【特許請求の範囲】

【請求項1】

制御対象の移動方向を案内するための案内情報を取得した位置を特定し得る位置特定情報を推定する推定部と、
前記推定部で推定した位置特定情報に近似する位置情報を持つ第１案内情報を、既存の案内情報の中から検索する検索部と、
指定された目標位置に相当する位置情報を持つ第２案内情報を、前記既存の案内情報の中から設定する設定部と、
前記検索部で検索した前記第１案内情報の現在位置に相当する位置情報の位置が、前記設定部で設定した前記第２案内情報の目標位置に相当する位置情報の位置に近づくように指令情報を出力する出力制御部と、
を有するナビゲーション制御装置。

【請求項2】

前記既存の案内情報が、前記制御対象が案内制御範囲を予め移動したときに時系列で撮影される複数の画像情報であり、
前記位置特定情報が、前記複数の画像情報を撮影した位置を特定する情報である、
請求項１記載のナビゲーション制御装置。

【請求項3】

前記制御対象が、二次元座標上又は三次元座標上を移動する移動体であり、前記指令情報として、前記移動体の二次元座標又は三次元座標を出力する、請求項１記載のナビゲーション制御装置。

【請求項4】

前記制御対象が、間接部位を備え、当該間接部位に設けられたアクチュエータの動作量で、三次元座標上を移動するロボットアームであり、前記位置特定情報が、前記間接部位の角度情報である、請求項１記載のナビゲーション制御装置。

【請求項5】

前記目標位置が、前記現在位置から目標位置までの経路途中から選択されたサブゴールを含む、請求項１～請求項４の何れか１項記載のナビゲーション制御装置。

【請求項6】

制御対象の移動を案内するナビゲーション制御範囲に属する複数の画像の中から、連続して撮影したときの撮影時期が互いに時系列に近似する一対の画像を用いて、各画像の位置を推定し得る位置ネットワークを学習しておき、当該位置ネットワークによって特定された位置情報を含む前記画像をノードとし、及び当該ノードを少なくとも距離状況に基づき連結する経路をエッジとしたトポロジカルマップを生成する地図生成部と、
前記制御対象のナビゲーション制御中に、当該制御対象が備える画像撮像デバイスで撮影した現在画像の位置情報を、前記位置ネットワークを用いて推定し、推定した位置情報に基づいて、前記地図生成部で生成されたトポロジカルマップの中から、前記現在画像に対応するノードを検索する画像検索部と、
前記トポロジカルマップにおいて、前記検索されたノードから目標位置のノードまでを連結する前記エッジによる経路を探索する経路探索部と、
前記経路探索部で探索した前記経路に基づいて、前記制御対象のナビゲーション制御のための指令情報を出力する出力部と、
を有する移動体のナビゲーション制御装置。

【請求項7】

前記位置ネットワークが、
前記一対の画像を、対象画像と近傍画像として、前記対象画像の位置情報と前記近傍画像の位置情報の相対位置と、前記対象画像の深度情報との関係から、前記対象画像の位置情報の誤差を予測することを繰り返す学習によって構築される、請求項６記載のナビゲーション制御装置。

【請求項8】

コンピュータを、
請求項１又は請求項６記載のナビゲーション制御装置として動作させる、
ナビゲーション制御プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、トポロジカルマップを用いたナビゲーション制御装置、ナビゲーション制御プログラムに関する。

【背景技術】

【0002】

従来、ロボット等の移動体が目標位置まで走行するための画像を生成し、マップとして生成する技術がある。

【0003】

非特許文献１は、単眼カメラとIMUをセンサとして超小型ヘリコプターのナビゲーションを行った研究を示した文献である。非特許文献１には、単眼カメラで得られた画像に基づいたＶｉｓｕａｌ ＳＬＡＭの位置情報とIMUの情報を用いてロボットの制御を行うことが記載されている。

【0004】

非特許文献２は、トポロジカルマップを利用することで、画像のみを用いて自律移動を実現する研究を示した文献である。非特許文献２には、トポロジカルマップの生成には画像の類似度を判定するＳｉａｍｅｓｅ Ｎｅｔというネットワークを用いる。Ｓｉａｍｅｓｅ Ｎｅｔは、画像の見た目の類似度のみを元に判定することが記載されている。

【0005】

非特許文献３は、現在画像から目標画像に近づけるための制御量を推定するネットワークを学習する研究を示した文献である。非特許文献３には、現在画像と制御量から、移動先の予測画像を推定するネットワークをあらかじめ学習し、そのネットワークを用いてモデル予測制御の枠組みで、移動先の予測画像と目標画像が近づくような制御量を出力するネットワークを学習することが記載されている。

【先行技術文献】

【非特許文献】

【0006】

【非特許文献1】Weiss,S.et al.,”Monocular-SLAM-based navigation for autonomous micro helicopters in GPS-denied environments.”Journal of Field Robotics,28(6),854-874,2011.

【非特許文献2】N. Savinov et. al., “Semi-parametric topological memory for navigation,” ICLR 2018.

【0007】

ロボットが目標位置まで走行するための画像を生成し、マップとして生成する技術

【非特許文献3】N. Hirose et al. “Deep Visual MPC-Policy Learning for Navigation,” RA-Letter 2019

【発明の概要】

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

【0008】

従来のＶｉｓｕａｌ ＳＬＡＭ（ＶＳＬＡＭ）ベースのナビゲーション（非特許文献１参照）では、ＶＳＬＡＭで推定した位置情報に基づいて制御を行っているが、これは位置の推定誤差に対して非常に弱く、スケールがわからないため、ＩＭＵなどのスケールを推定するために、追加センサを必要とする。

【0009】

また、ＳＰＴＭ（非特許文献２参照）のように、画像の位置情報を推定せず、類似度を元にマップを生成する方法もあるが、画像単体での検索は、実際の時系列の画像の位置関係などを利用することが出来ないという問題がある。

【0010】

本発明は、推定誤差の積算が起きず、かつ、内部状態の特性を利用することもでき、精度とロバスト性を向上させることができるナビゲーション制御装置、ナビゲーション制御プログラムを提供することを目的とする。

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

【0011】

第１の発明に係るナビゲーション制御装置は、制御対象の移動方向を案内するための案内情報を取得した位置を特定し得る位置特定情報を推定する推定部と、前記推定部で推定した位置特定情報に近似する位置情報を持つ第１案内情報を、既存の案内情報の中から検索する検索部と、指定された目標位置に相当する位置情報を持つ第２案内情報を、前記既存の案内情報の中から設定する設定部と、前記検索部で検索した前記第１案内情報の現在位置に相当する位置情報の位置が、前記設定部で設定した前記第２案内情報の目標位置に相当する位置情報の位置に近づくように指令情報を出力する出力制御部と、を有している。

【0012】

第１の発明によれば、制御対象の移動方向を案内するための案内情報を取得した位置を特定し得る位置特定情報を推定し、推定した位置特定情報に近似する位置情報を持つ第１案内情報を、既存の案内情報の中から検索し、指定された目標位置に相当する位置情報を持つ第２案内情報を、既存の案内情報の中から設定し、検索した第１案内情報の現在位置に相当する位置情報の位置が、第２案内情報の目標位置に相当する位置情報の位置に近づくように指令情報を出力する。

【0013】

これにより、推定誤差の積算が起きず、かつ、内部状態の特性を利用することもでき、精度とロバスト性を向上させることができる。

【0014】

第１の発明において、前記既存の案内情報が、前記制御対象が案内制御範囲を予め移動したときに時系列で撮影される複数の画像情報であり、前記位置特定情報が、前記複数の画像情報を撮影した位置を特定する情報である、ことを特徴としている。

【0015】

第１の発明において、前記制御対象が、二次元座標上又は三次元座標上を移動する移動体であり、前記指令情報として、前記移動体の二次元座標又は三次元座標を出力する、ことを特徴としている。

【0016】

第１の発明において、前記制御対象が、間接部位を備え、当該間接部位に設けられたアクチュエータの動作量で、三次元座標上を移動するロボットアームであり、前記位置特定情報が、前記間接部位の角度情報である、ことを特徴としている。

【0017】

第１の発明において、前記目標位置が、前記現在位置から目標位置までの経路途中から選択されたサブゴールを含む、ことを特徴としている。

【0018】

第２の発明に係るナビゲーション制御装置は、制御対象の移動を案内するナビゲーション制御範囲に属する複数の画像の中から、連続して撮影したときの撮影時期が互いに時系列に近似する一対の画像を用いて、各画像の位置を推定し得る位置ネットワークを学習しておき、当該位置ネットワークによって特定された位置情報を含む前記画像をノードとし、及び当該ノードを少なくとも距離状況に基づき連結する経路をエッジとしたトポロジカルマップを生成する地図生成部と、前記制御対象のナビゲーション制御中に、当該制御対象が備える画像撮像デバイスで撮影した現在画像の位置情報を、前記位置ネットワークを用いて推定し、推定した位置情報に基づいて、前記地図生成部で生成されたトポロジカルマップの中から、前記現在画像に対応するノードを検索する画像検索部と、前記トポロジカルマップにおいて、前記検索されたノードから目標位置のノードまでを連結する前記エッジによる経路を探索する経路探索部と、前記経路探索部で探索した前記経路に基づいて、前記制御対象のナビゲーション制御のための指令情報を出力する出力部と、を有している。

【0019】

第２の発明によれば、制御対象の移動を案内するナビゲーション制御範囲に属する複数の画像の中から、連続して撮影したときの撮影時期が互いに時系列に近似する一対の画像を用いて、各画像の位置を推定し得る位置ネットワークを学習しておき、当該位置ネットワークによって特定された位置情報を含む画像をノードとし、及び当該ノードを少なくとも距離状況に基づき連結する経路をエッジとしたトポロジカルマップを生成し、制御対象のナビゲーション制御中に、当該制御対象が備える画像撮像デバイスで撮影した現在画像の位置情報を、位置ネットワークを用いて推定し、推定した位置情報に基づいて、トポロジカルマップの中から、現在画像に対応するノードを検索する画像検索部と、トポロジカルマップにおいて、検索されたノードから目標位置のノードまでを連結するエッジによる経路を探索し、探索した経路に基づいて、制御対象のナビゲーション制御のための指令情報を出力する。

【0020】

これにより、推定誤差の積算が起きず、かつ、内部状態の特性を利用することもでき、精度とロバスト性を向上させることができる。

【0021】

第２の発明において、前記位置ネットワークが、前記一対の画像を、対象画像と近傍画像として、前記対象画像の位置情報と前記近傍画像の位置情報の相対位置と、前記対象画像の深度情報との関係から、前記対象画像の位置情報の誤差を予測することを繰り返す学習によって構築される、ことを特徴としている。

【0022】

第３の発明に係るナビゲーション制御プログラムは、コンピュータを、前記ナビゲーション制御装置として動作させる、ことを特徴としている。

【発明の効果】

【0023】

以上説明したように本発明では、推定誤差の積算が起きず、かつ、内部状態の特性を利用することもでき、精度とロバスト性を向上させることができるという効果を奏する。

【図面の簡単な説明】

【0024】

【図1】本実施の形態に係るナビゲーション制御装置の概略を示す制御ブロック図である。

【図2】画像による位置推定を行うための事前処理として実行される位置ネットワークの学習パターン図である。

【図3】本実施の形態に係る事前処理として実行される地図生成部の処理を示す制御フローチャートである。

【図4】本実施の形態に係る地図生成部で生成されたトポロジカルマップの一例を示すノード－エッジ遷移図である。

【図5】本実施の形態に係る局所制御を行うための事前処理として実行される制御ネットワークの学習パターン図である。

【図6】本実施の形態に係るナビゲーション処理の流れを示す制御フローチャートである。

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

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

図３の制御フローチャートの処理を、図１に示す対象環境画像収集部５２の画像に対して、繰り返し実行することで、トポロジカルマップ６４（図４参照）が生成される。

【0073】

（ナビゲーション処理の流れ）
図６は、ナビゲーション処理部１４で実行されるナビゲーション処理ルーチンを示す制御フローチャートである。このナビゲーション処理は、移動体のナビゲーションが必要となったときに起動する。

【0074】

ステップ１５０では、現在画像４２が入力されたか否かを判断する。このステップ１５０で否定判定された場合は、ステップ１６８へ移行する。また、ステップ１５０で肯定判定されると、現在画像４２が入力されたと判断し、ステップ１５２へ移行する。

【0075】

ステップ１５２では、位置ネットワーク３０（図２参照）で現在画像４２の位置を推定し、次いでステップ１５４へ移行して、最も近い画像のノード６０を探索し、ステップ１５６へ移行する。

【0076】

ステップ１５６では、探索した画像のノード６０を現在画像のノード６０に設定し、ステップ１５８へ移行し、経路を探索する。

【0077】

次のステップ１６０では、経路上の画像列を出力し、ステップ１６２へ移行してサブゴール画像を決定し、ステップ１６４へ移行する。

【0078】

ステップ１６４では、制御ネットワーク４５（図６参照）で制御量４６を決定し、ステップ１６６へ移行して、制御指令を出力し、ステップ１６８へ移行する。

【0079】

ステップ１６８では、ナビゲーション処理を終了するか否かを判断し、否定判定された場合は、ステップ１５０へ戻り、上記工程を繰り返す。また、ステップ１６８で肯定判定された場合は、このルーチンは終了し、ナビゲーション処理を終了する。

【0080】

以上説明したように、本実施の形態では、画像だけで目標位置に行ける技術（一例として、ＤＶＭＰＣ）を、本実施の形態における、スケールは推定できないが画像単体から位置を推定できるシステムと組み合わせることで、粗い位置しか推定できない画像位置推定を、有効に、ナビゲーション制御に利用することができる。

【0081】

参考として、Ｖ－ＳＬＡＭは、画像から位置推定、画像が直前の画像からどれだけ移動したかを画像の移動量から推定し、いまどこにいるかを推定しており、推定誤差が重畳されるため、制御が困難である。

【0082】

また、参考として、ＳＰＴＭは、画像同士が近いかどうか、見た目が似ているかどうかを位置情報の代わりに利用するものであり、位置は推定しない。すなわち、漠然と画像として似ていることで判断すると、全く異なる場所に推定される場合があり、検索が精度の面でよくない。

【0083】

これに対して、本実施の形態では、位置をリファレンス、検索するための情報に利用することで、画像を直接近づけるように制御することで、位置誤差がない。

【0084】

なお、上記では、移動体を案内する案内情報として撮影画像を例にとり説明したが、Ｌｉｄａｒ等のセンサの検出情報を案内情報としてもよい。また、ノード６０に付加する情報として位置座標としたが、例えば、移動体がロボットアームであった場合は、ロボットアームに設けられた間接の回転角度であってもよい。

【0085】

また、移動体は、平面上を走行する車両等にかぎらず、空間を移動する飛行体であってもよく、この場合の位置情報は、二次元座標ではなく、三次元座標となる。

【符号の説明】

【0086】

１０ ナビゲーション制御装置
１２ オフライン処理部
１４ ナビゲーション処理部
１６ 位置推定部事前学習部
１８ 局所制御部事前学習部
２０ 履歴画像データベース
２２ 対象画像
２４ 近傍画像
２６ 深度ネットワーク
２８ 深度情報
３０ 位置ネットワーク
３２ 位置情報
３４ 位置ネットワーク
３６ 位置情報
３８ 相対位置情報
４０ 予測画像
４２ 現在画像
４４ 目標画像
４５ 制御ネットワーク
４６ 制御量
４８ 画像予測ネットワーク
５０ 予測画像
５２ 対象環境画像収集部
５４ 位置推定部（推定部）
５６ 地図生成部
５８ 格納部
６０ ノード
６２ エッジ
６４ トポロジカルマップ
６６ 現在画像取込部
６８ 位置推定部
７０ 画像検索部（検索部）
７２ 経路探索部
７４ サブゴール生成部（設定部）
７６ 局所制御部（出力制御部）

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】