特開 2022-160234 経路計画装置、及び経路計画プログラム

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

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

(11)【公開番号】P2022160234

(43)【公開日】2022-10-19

(54)【発明の名称】経路計画装置、及び経路計画プログラム

(51)【国際特許分類】

   G01C 21/36 20060101AFI20221012BHJP

   G06T 7/00 20170101ALI20221012BHJP

   G05D 1/02 20200101ALI20221012BHJP

【ＦＩ】

G01C21/36

G06T7/00 650Z

G05D1/02 K

【審査請求】未請求

【請求項の数】6

【出願形態】ＯＬ

(21)【出願番号】P 2021064866

(22)【出願日】2021-04-06

(71)【出願人】

【識別番号】000003609

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

(74)【代理人】

【識別番号】110001519

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

(72)【発明者】

【氏名】田口 峻

(72)【発明者】

【氏名】廣瀬 徳晃

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

2F129

5H301

5L096

【Ｆターム（参考）】

2F129AA03

2F129DD03

2F129DD04

2F129DD13

2F129DD15

2F129DD62

2F129EE02

2F129EE52

2F129EE78

2F129EE79

2F129FF02

2F129FF18

2F129GG17

2F129HH02

2F129HH04

2F129HH12

2F129HH18

2F129HH19

5H301AA01

5H301AA10

5H301BB14

5H301CC03

5H301CC06

5H301CC10

5H301DD01

5H301DD06

5H301DD07

5H301DD15

5H301GG08

5H301GG09

5H301HH01

5H301HH02

5L096AA06

5L096BA05

5L096CA02

5L096DA02

5L096GA10

5L096GA51

5L096HA05

5L096HA11

5L096JA03

(57)【要約】

【課題】移動体の移動方向に対して適切なマップを生成できる。
【解決手段】経路計画装置では、過去に撮影された画像を撮影の時系列順に取得する。画像間の接続関係をノード及びエッジで表したグラフにおいて、取得した画像と類似する対象画像を探索する。探索結果に基づき、取得した画像と類似する対象画像がない場合には、取得した画像のノードを生成し、対象画像がある場合には、当該対象画像のノードを現在のノードの代替ノードとして割り当てるようにする。直前のノード又は代替ノードとの間にエッジを生成することにより、移動方向を伴う有向グラフとしてのマップを生成する。マップ上で経路探索を行うことにより移動体の走行を制御する。
【選択図】図１

【特許請求の範囲】

【請求項1】

過去に撮影された画像を撮影の時系列順に取得する画像取得部と、
画像間の接続関係をノード及びエッジで表したグラフにおいて、取得した画像と類似する対象画像を探索し、探索結果に基づき、取得した画像と類似する対象画像がない場合には、取得した画像のノードを生成し、前記対象画像がある場合には、当該対象画像のノードを現在のノードの代替ノードとして割り当てるようにし、直前のノード又は前記代替ノードとの間にエッジを生成することにより、移動方向を伴う有向グラフとしてのマップを生成するマップ生成部と、
前記マップ上で経路探索を行うことにより移動体の走行を制御する走行制御部と、
を含む経路計画装置。

【請求項2】

前記対象画像のノードを探索する場合に、直前のノードからエッジに沿って逆向きに所定の範囲を除外する請求項１に記載の経路計画装置。

【請求項3】

前記マップ生成部は、
前記画像を３６０度カメラにより撮影された画像として、前向きと後ろ向きのそれぞれの向きについて前向きのマップ、及び後ろ向きのマップを生成する請求項１又は請求項２に記載の経路計画装置。

【請求項4】

前記後ろ向きのマップについて、３６０度カメラの前後の画像を入れ替えて生成する請求項３に記載の経路計画装置。

【請求項5】

前記画像に対象物が映り込んでいる場合に、前向き及び後ろ向きの双方の画像で前記対象物をマスクする請求項３又は請求項４に記載の経路計画装置。

【請求項6】

過去に撮影された画像を撮影の時系列順に取得し、
画像間の接続関係をノード及びエッジで表したグラフにおいて、取得した画像と類似する対象画像を探索し、探索結果に基づき、取得した画像と類似する対象画像がない場合には、取得した画像のノードを生成し、前記対象画像がある場合には、当該対象画像のノードを現在のノードの代替ノードとして割り当てるようにし、直前のノード又は前記代替ノードとの間にエッジを生成することにより、移動方向を伴う有向グラフとしてのマップを生成し、
前記マップ上で経路探索を行うことにより移動体の走行を制御する、
処理をコンピュータに実行させる経路計画プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、経路計画装置、及び経路計画プログラムに関する。

【背景技術】

【0002】

従来より、ロボットが目的地まで走行するための画像を生成し、マップとして生成する技術がある（非特許文献１参照）。

【先行技術文献】

【非特許文献】

【0003】

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

【発明の概要】

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

【0004】

ここで、従来の技術では、類似した画像間に対応するノード間にエッジを張るため、エッジ両端のノードの前後関係が不明であり、マップを無向グラフでしか表現できていなかった。マップを無向グラフでしか生成できなかったことにより、後退する経路が生成されるという問題が生じる。このように後退する経路はシミュレーション上ではよいものの、実際に走行する際にロボットが後退して目的地に向かうのは、特に前方にカメラがついているロボットでは問題がある。なぜならば、ロボットが後退して目的地へ向かうような経路が選択される可能性があり、進行方向に存在する障害物を前方カメラで検知することができない等、問題が生じるからである。また、無駄なノード及びエッジが生成され、マップのサイズが大きくなり、結果としてマップ上での位置推定にも悪影響が生じ得る。

【0005】

本発明は、上記事情を鑑みて成されたものであり、移動体の移動方向に対して適切なマップを生成できる経路計画装置、及び経路計画プログラムを提供することを目的とする。

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

【0006】

上記目的を達成するために、第１の発明に係る経路計画装置は、過去に撮影された画像を撮影の時系列順に取得する画像取得部と、画像間の接続関係をノード及びエッジで表したグラフにおいて、取得した画像と類似する対象画像を探索し、探索結果に基づき、取得した画像と類似する対象画像がない場合には、取得した画像のノードを生成し、前記対象画像がある場合には、当該対象画像のノードを現在のノードの代替ノードとして割り当てるようにし、直前のノード又は前記代替ノードとの間にエッジを生成することにより、移動方向を伴う有向グラフとしてのマップを生成するマップ生成部と、前記マップ上で経路探索を行うことにより移動体の走行を制御する走行制御部と、を含んで構成されている。

【0007】

第２の発明に係る経路計画プログラムは、過去に撮影された画像を撮影の時系列順に取得し、画像間の接続関係をノード及びエッジで表したグラフにおいて、取得した画像と類似する対象画像を探索し、探索結果に基づき、取得した画像と類似する対象画像がない場合には、取得した画像のノードを生成し、前記対象画像がある場合には、当該対象画像のノードを現在のノードの代替ノードとして割り当てるようにし、直前のノード又は前記代替ノードとの間にエッジを生成することにより、移動方向を伴う有向グラフとしてのマップを生成し、前記マップ上で経路探索を行うことにより移動体の走行を制御する、処理をコンピュータに実行させる。

【発明の効果】

【0008】

本発明の経路計画装置、及び経路計画プログラムによれば、移動体の移動方向に対して適切なマップを生成することができる。

【図面の簡単な説明】

【0009】

【図1】本発明の実施形態の経路計画装置の構成を示すブロック図である。

【図2】生成されるマップのイメージの一例を示す図である。

【図3】本実施形態のデータフローの一例を示す図である。

【図4】経路計画装置のハードウェア構成を示すブロック図である。

【図5】経路計画装置によるマップ生成処理の流れを示すフローチャートである。

【図6】経路計画装置による走行制御処理の流れを示すフローチャートである。

【図7】３６０度カメラを用いた場合に生成される２つのマップのイメージの一例を示す図である。

【図8】対象物が写り込んでいる場合に画像にマスクする場合の一例を示す図である。

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

【0010】

以下、図面を参照して本発明の実施形態を詳細に説明する。

【0011】

本発明の実施形態の手法を用いることにより、有向グラフのマップを生成して走行を制御する。有効マップの生成では、類似ノード（類似画像）を追加しないため、上記の課題において説明した問題が解決される。

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

１００ 経路計画装置
１１０ 画像取得部
１１２ マップ生成部
１１４ 走行制御部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】