特許 7447060 情報処理装置、情報処理方法、自律走行ロボット装置、及びコンピュータプログラム

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

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2024-03-01

(45)【発行日】2024-03-11

(54)【発明の名称】情報処理装置、情報処理方法、自律走行ロボット装置、及びコンピュータプログラム

(51)【国際特許分類】

   G05D 1/246 20240101AFI20240304BHJP

【ＦＩ】

G05D1/246

【請求項の数】 22

(21)【出願番号】P 2021124647

(22)【出願日】2021-07-29

(65)【公開番号】P2023019708

(43)【公開日】2023-02-09

【審査請求日】2022-08-09

(73)【特許権者】

【識別番号】000001007

【氏名又は名称】キヤノン株式会社

(74)【代理人】

【識別番号】100114775

【弁理士】

【氏名又は名称】高岡 亮一

(74)【代理人】

【識別番号】100121511

【弁理士】

【氏名又は名称】小田 直

(74)【代理人】

【識別番号】100208580

【弁理士】

【氏名又は名称】三好 玲奈

(72)【発明者】

【氏名】宮谷 その子

(72)【発明者】

【氏名】藤木 真和

【審査官】稲垣 浩司

(56)【参考文献】

【文献】国際公開第２０１９／１７６０８３（ＷＯ，Ａ１）

【文献】国際公開第２０１３／０６９１４７（ＷＯ，Ａ１）

【文献】特開２０１１－０３９９６８（ＪＰ，Ａ）

【文献】特開２０１９－１５９５２０（ＪＰ，Ａ）

【文献】特開２０１９－１２５３４５（ＪＰ，Ａ）

【文献】特開２０１１－１１８６０３（ＪＰ，Ａ）

【文献】米国特許出願公開第２０２０／０１２４４２３（ＵＳ，Ａ１）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０５Ｄ １／２０－１／４０

(57)【特許請求の範囲】

【請求項1】

移動体に搭載されたカメラの撮影画像に基づいて、前記移動体の位置推定を行った履歴情報を取得する履歴取得手段と、
前記移動体が移動する空間中の障害物の配置を示す障害物配置情報を取得する障害物配置情報取得手段と、
前記履歴情報を取得した前記移動体の位置と位置推定のズレが許容される距離とに基づいて、前記移動体の位置推定が可能であり前記移動体に自律走行させる経路の設定が可能な領域を、前記履歴情報を取得した前記移動体の位置から位置推定のズレが許容される距離の範囲内の経路幅で示す自律走行可能情報を獲得する自律走行可能情報獲得手段と、
前記障害物配置情報及び前記自律走行可能情報に基づいて前記障害物の配置及び前記障害物が存在しなかった領域における前記移動体に自律走行させる経路の設定が可能な領域を画像によって示す地図画像を生成する地図画像生成手段と、
を備えることを特徴とする情報処理装置。

【請求項2】

前記自律走行可能情報獲得手段は、前記移動体の前記履歴情報に基づいて前記移動体が移動する空間中で前記移動体に自律走行させる経路の設定が可能な領域及び該経路の向きを獲得することを特徴とする請求項１に記載の情報処理装置。

【請求項3】

前記地図画像生成手段は、前記障害物配置情報及び前記自律走行可能情報に基づいて前記障害物の配置と前記移動体に自律走行させる経路の設定が可能な領域及び前記向きを示す前記地図画像を生成することを特徴とする請求項２に記載の情報処理装置。

【請求項4】

前記自律走行可能情報獲得手段は、前記移動体の前記履歴情報に基づいて前記移動体の位置計測の信頼度を算出することを特徴とする請求項１～３のいずれか１項に記載の情報処理装置。

【請求項5】

前記自律走行可能情報獲得手段は、前記履歴取得手段が取得した位置が位置姿勢計測時刻順に従って変化する方向を前記自律走行可能な前記向きとして算出することを特徴とする請求項２又は３に記載の情報処理装置。

【請求項6】

前記信頼度を、前記移動体に自律走行させる経路の設定が可能な領域の濃度によって示すことを特徴とする請求項４に記載の情報処理装置。

【請求項7】

前記自律走行可能な領域は、前記位置の前記履歴情報の算出に使用した撮影画像から検出した複数の画像特徴の数に基づき算出されることを特徴とする請求項１～６のいずれか１項に記載の情報処理装置。

【請求項8】

前記信頼度は、前記自律走行可能な領域内での撮影画像から検出される画像特徴と、前記位置の前記履歴情報の算出に使用した撮影画像から検出した複数の画像特徴との一致度に基づいて算出されることを特徴とする請求項４に記載の情報処理装置。

【請求項9】

前記履歴取得手段は、前記移動体の前記位置の信頼度を取得することを特徴とする請求項１に記載の情報処理装置。

【請求項10】

前記地図画像の表示を制御する表示制御手段を有することを特徴とする請求項１～９のいずれか１項に記載の情報処理装置。

【請求項11】

前記自律走行可能情報と前記障害物配置情報に基づき前記移動体の走行経路を探索する経路探索手段を有することを特徴とする請求項１～１０のいずれか１項に記載の情報処理装置。

【請求項12】

移動体に搭載されたカメラの撮影画像に基づいて、前記移動体の位置姿勢の推定を行った履歴情報を取得する履歴取得手段と、
前記移動体が移動する空間中の障害物の配置を示す障害物配置情報を取得する配置情報取得手段と、
前記履歴取得手段が取得した前記履歴情報に基づいて、前記移動体に自律走行させる設定が可能な領域を示す自律走行可能情報を獲得する自律走行可能情報獲得手段と、
前記自律走行可能情報と前記障害物配置情報に基づいて、前記移動体の走行経路を探索する経路探索手段と、を有し、
前記経路探索手段は、前記自律走行可能情報と前記障害物配置情報と前記自律走行可能情報の信頼度とに基づいて、前記移動体の自律走行可能な経路を探索又は選択することを特徴とする情報処理装置。

【請求項13】

前記自律走行可能情報獲得手段は、前記移動体の前記履歴情報に基づいて前記移動体が移動する空間中で前記移動体が前記自律走行可能な領域及び向きを獲得することを特徴とする請求項１２記載の情報処理装置。

【請求項14】

前記経路探索手段は、予め設定された前記移動体の経路を前記自律走行可能情報に基づいて自律走行可能な経路に補正することを特徴とする請求項１２に記載の情報処理装置。

【請求項15】

前記移動体の自律走行可能な前記経路が存在しない場合に、ユーザに通知する通知手段を有することを特徴とする請求項１２に記載の情報処理装置。

【請求項16】

前記移動体に自律走行させる経路の設定が可能な領域内ではない位置または前記移動体に自律走行させる経路の設定が可能ではない向きを指定して前記移動体に自律走行させる経路が入力された場合、自律走行させる経路の設定が可能ではないことを通知する通知手段をさらに有することを特徴とする請求項１２に記載の情報処理装置。

【請求項17】

前記障害物配置情報及び前記自律走行可能情報に基づいて前記障害物の配置及び前記自律走行可能な領域を示す地図画像を生成する地図画像生成手段と、
前記地図画像と前記移動体の走行経路の表示を制御する表示制御手段と、を有することを特徴とする請求項１２～１６のいずれか１項に記載の情報処理装置。

【請求項18】

移動体に搭載されたカメラの撮影画像に基づいて算出した前記移動体の位置推定を行った履歴情報を取得する履歴取得工程と、
前記移動体が移動する空間中の障害物の配置を示す障害物配置情報を取得する配置情報取得工程と、
前記履歴情報を取得した前記移動体の位置と位置推定のズレが許容される距離とに基づいて、前記移動体の位置推定が可能であり前記移動体に自律走行させる経路の設定が可能な領域を、前記履歴情報を取得した前記移動体の位置から位置推定のズレが許容される距離の範囲内の経路幅で示す自律走行可能情報を獲得する自律走行可能情報獲得工程と、
前記障害物配置情報及び前記自律走行可能情報に基づいて前記障害物の配置及び前記障害物が存在しなかった領域における前記移動体に自律走行させる経路の設定が可能な領域を画像によって示す地図画像を生成する地図画像生成工程と、
を備えることを特徴とする情報処理方法。

【請求項19】

移動体に搭載されたカメラの撮影画像に基づいて、前記移動体の位置姿勢の推定を行った履歴情報を取得する履歴取得工程と、
前記移動体が移動する空間中の障害物の配置を示す障害物配置情報を取得する配置情報取得工程と、
前記履歴取得工程で取得した前記履歴情報に基づいて、前記移動体に自律走行させる設定が可能な領域を示す自律走行可能情報を獲得する自律走行可能情報獲得工程と、
前記自律走行可能情報と前記障害物配置情報に基づいて、前記移動体の走行経路を探索する経路探索工程と、を有し、
前記経路探索工程は、前記自律走行可能情報と前記障害物配置情報と前記自律走行可能情報の信頼度とに基づいて、前記移動体の自律走行可能な経路を探索又は選択することを特徴とする情報処理方法。

【請求項20】

請求項１２～１７のいずれか１項に記載の情報処理装置により探索された前記走行経路に基づき前記移動体の移動を制御する移動制御手段を有することを特徴とする自律走行ロボット装置。

【請求項21】

請求項１～１７のいずれか１項に記載の情報処理装置又は請求項２０の自律走行ロボット装置の各手段をコンピュータにより制御するためのコンピュータプログラム。

【請求項22】

請求項２１に記載のコンピュータプログラムを記憶したコンピュータで読み取り可能な記憶媒体。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、移動体の位置姿勢について処理する情報処理装置、情報処理方法、自律走行ロボット装置、及びコンピュータプログラムに関する

【背景技術】

【0002】

搬送車両（例えばＡＧＶ（Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｖｅｈｉｃｌｅ））などの移動体を工場や物流倉庫といった環境内において自動で移動させるためには、走行する経路を予め設定する必要がある。最適な経路設定の方法の１つとして、特許文献１のように、ＧＰＳの測位精度が高い地点を通る移動経路を設定する方法がある。

【0003】

又、移動体の位置姿勢計測を行う方法として、カメラの撮影画像を使用したＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）技術が知られている。ＳＬＡＭは、位置姿勢計測に使用する地図を生成する処理と、該地図を使用した位置姿勢計測処理を同時並行で行う。
又、非特許文献１にはＳＬＡＭにおけるキーフレームやバンドル調整の方法に関する記載がある。

【先行技術文献】

【特許文献】

【0004】

【文献】特開２０１５－３４７７５号公報

【非特許文献】

【0005】

【文献】ＲａｕｌＭｕｒ－Ａｒｔａｌ ｅｔ．ａｌ，ＯＲＢ－ＳＬＡＭ：Ａ Ｖｅｒｓａｔｉｌｅ ａｎｄ Ａｃｃｕｒａｔｅ Ｍｏｎｏｃｕｌａｒ ＳＬＡＭ Ｓｙｓｔｅｍ．ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｒｏｂｏｔｉｃｓ， ２０１５.

【発明の概要】

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

【0006】

ＳＬＡＭ技術を用いた移動体の自動走行では、安定的に移動体の位置計測が計測可能な経路を設定する必要がある。しかし、特許文献１の方法を用いても、位置情報を用いているものの、安定して位置姿勢を計測できる経路を設定できない場合がある。
そこで本発明では、安定した自動走行が可能な領域を示すことが可能な情報処理装置を提供することを１つの目的とする。

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

【0007】

本発明の１側面としての情報処理装置は、
移動体に搭載されたカメラの撮影画像に基づいて、前記移動体の位置推定を行った履歴情報を取得する履歴取得手段と、
前記移動体が移動する空間中の障害物の配置を示す障害物配置情報を取得する障害物配置情報取得手段と、
前記履歴情報を取得した前記移動体の位置と位置推定のズレが許容される距離とに基づいて、前記移動体の位置推定が可能であり前記移動体に自律走行させる経路の設定が可能な領域を、前記履歴情報を取得した前記移動体の位置から位置推定のズレが許容される距離の範囲内の経路幅で示す自律走行可能情報を獲得する自律走行可能情報獲得手段と、
前記障害物配置情報及び前記自律走行可能情報に基づいて前記障害物の配置及び前記障害物が存在しなかった領域における前記移動体に自律走行させる経路の設定が可能な領域を画像によって示す地図画像を生成する地図画像生成手段と、
を備えることを特徴とする。

【発明の効果】

【0008】

本発明により、安定した自動走行が可能な領域を示すことが可能な情報処理装置を実現することができる。

【図面の簡単な説明】

【0009】

【図1】本発明の実施例１における情報処理装置を備える移動体の構成例を示す機能ブロック図である。

【図2】実施例１の情報処理装置１０１のハードウェア構成図である。

【図3】実施例１に係る情報処理装置が実行する処理を示すフローチャートである。

【図4】実施例１において生成した自動走行可能情報を示すＣＧ像の例を示す図である。

【図5】障害物配置画像上に自動走行可能情報を示すＣＧ像を重畳した画像の例を示す図である。

【図6】実施例４における情報処理装置の構成例を示す機能ブロック図である。

【図7】実施例４における処理手順を示すフローチャートである。

【図8】実施例４における経路探索結果の例を示す図である。

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

【0010】

以下、添付図面を参照して、本発明の好適な実施の形態について実施例を用いて説明する。尚、各図において、同一の部材ないし要素については同一の参照番号を付し、重複する説明は省略ないし簡略化する。

【実施例1】

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

又、上記のステップＳ１０３では、位置姿勢計測可能な範囲は円形状としたが、これに限るものではなく、楕円形状や矩形などであっても良い。
又、上記では、位置、方向、範囲及び信頼度全てを表示する場合について説明したが、少なくとも位置又は範囲のどちらか一方と、方向を表示するだけでも良いし、位置と方向のみを表示するようにしても良い。或いは、方向、範囲及び信頼度のみを表示するようにしても良い。いずれの場合も、本実施例によれば、移動体の経路設定が非常に容易になるという効果がある。

【実施例2】

【0039】

尚、実施例１において、位置姿勢計測用地図が保持する画像特徴を、観測可能なカメラの位置の範囲を自動走行可能な範囲としても良い。即ち、実施例２においては、先ず、位置姿勢計測用地図が保持する撮影時のカメラの位置近傍で、仮想のカメラの位置を所定の間隔（Ｌ）で複数設定する。次に、設定した仮想カメラの位置ごとに、位置姿勢計測用地図が保持する画像特徴が観測可能かどうか判定する。

【0040】

具体的には、該カメラの位置及び位置姿勢計測用地図が保持する撮影時のカメラの姿勢に基づいて、位置姿勢計測用地図が保持する３次元の画像特徴量を２次元の画像面に投影し、投影した画像上で画像特徴が観測可能か否かを判定する。投影した画像特徴の画像上での位置が、カメラの撮影画像領域内、即ちカメラの画角内となる場合に、該画像特徴が観測可能と判定する。

【0041】

観測可能と判定された画像特徴の数が予め設定した閾値以上となる場合、該カメラの各位置を中心とし、直径がＬの円形領域を自動走行可能な範囲とする。観測可能と判定された画像特徴の数が前記閾値未満の場合には、該カメラの各位置を中心とし、直径がＬの円形領域を自動走行不可能な範囲とする。

【0042】

又、自動走行可能な情報である信頼度は、位置姿勢履歴取得部が取得した信頼度に代わって、上記方法で算出した観測可能な画像特徴の数や分布に基づいて算出するようにしても良い。この場合、特徴点の数が多いほど、或いは特徴点が広く分布しているほど信頼度を高くする。逆に、特徴点の数が少ないほど、或いは特徴点が空間的に偏って分布しているほど信頼度を低くする。

【実施例3】

【0043】

実施例１及び実施例２では、自動走行可能な範囲として、自動走行可能な位置の範囲を算出したが、実施例３においては、自動走行可能な範囲として、自動走行可能な移動体の姿勢の範囲を算出し、画面表示する。算出方法としては、先ず、撮影時のカメラの位置ごとに、複数の仮想カメラの姿勢を設定する。

【0044】

カメラの姿勢は、ロール、ピッチ、ヨーで表現し、各回転角を所定の回転角度間隔（Ｄ）で複数設定する。そして、設定した姿勢ごとに、位置姿勢計測用地図が保持する画像特徴が観測可能か否か、実施例２で説明した方法を用いて判定する。観測可能と判定された画像特徴の数が予め設定した閾値以上となる場合、該姿勢を中心に、ロール、ピッチ、ヨーの各回転角－Ｄ／２～＋Ｄ／２の範囲を自動走行可能な姿勢の範囲とする。観測可能と判定された画像特徴の数が前記閾値未満の場合には、該姿勢を中心に、ロール、ピッチ、ヨーの各回転角－Ｄ／２～＋Ｄ／２の範囲を自動走行不可能な姿勢の範囲とする。

【0045】

尚、姿勢の範囲を、予め設定した所定の範囲に制限しても良い。この場合、撮影時のカメラの姿勢を中心に、所定の姿勢の範囲内を自動走行可能な姿勢の範囲とする。
尚、上記では、自動走行可能な範囲として姿勢の範囲を算出したが、位置の範囲と姿勢の範囲の両方を自動走行可能な範囲として算出するようにしても良い。この場合、実施例２で説明した複数の仮想カメラの各位置で、実施例３で説明した姿勢の範囲を算出する。

【0046】

図５は、障害物配置画像上に自動走行可能情報を示すＣＧ像を重畳した画像の例を示す図である。即ち、図５は、自動走行可能な範囲を、位置及び姿勢の範囲とした場合の、ステップＳ１０４で生成する障害物配置画像上に自動走行可能情報を示すＣＧ像を重畳した画像の例を示している。Ｇ４０１からＧ４０５までは、実施例１で説明したものと同じものである。
Ｇ９０１は、自動走行可能な範囲内で、ユーザが入力部２１５を介して指定した位置を示している。

【0047】

Ｇ９０２は、Ｇ９０１地点での自動走行可能な姿勢の範囲を示すＧＵＩである。Ｇ９０３は、ロール、ピッチ、ヨーの３つの回転角度のうち、ユーザが入力部２１５を介して指定した２つの回転角度（ロール、ヨー）である。Ｇ９０４は、指定したロール、ヨーの回転角度において、自動走行可能なピッチの範囲を示している。Ｇ９０２において、ハッチングの濃淡は信頼度を示しており、ハッチングが濃いほど信頼度が高い。

【実施例4】

【0048】

実施例４では、更に、自動走行可能情報と障害物配置情報に基づいて、自動走行可能な経路を探索する。経路の開始地点、経由地点及び目的地点は、利用者が予め定めた位置とする。

【0049】

図６は、実施例４における情報処理装置の構成例を示す機能ブロック図である。
尚、図６に示される機能ブロックの一部は、情報処理装置に含まれるコンピュータに、記憶媒体としてのメモリに記憶されたコンピュータプログラムを実行させることによって実現されている。しかし、それらの一部又は全部をハードウェアで実現するようにしても構わない。ハードウェアとしては、専用回路（ＡＳＩＣ）やプロセッサ（リコンフィギュラブルプロセッサ、ＤＳＰ）などを用いることができる。

【0050】

又、図６に示される夫々の機能ブロックは、同じ筐体に内蔵されていなくても良く、情報処理装置は互いに信号路を介して接続された別々の装置により構成しても良い。
図６において、情報処理装置２００は、位置姿勢履歴取得部１０７、障害物配置情報取得部１０８、自動走行可能情報算出部１０９、経路探索部２０１から構成され、保持部２０２と接続する。尚、図６では不図示となっているが、図１における画像生成部１１０や提示部１１１も情報処理装置２００内に設け、経路探索部２０１が探索した経路を、画像生成部１１０を介して提示部１１１により提示（画像表示）しても良い。

【0051】

位置姿勢履歴取得部１０７は、位置姿勢の履歴を取得する。実施例１では、位置姿勢計測部１０３から取得していたが、本実施例では、保持部２０２から取得する。位置姿勢の履歴は、実施例１で説明したものと同様に、位置姿勢の情報、位置姿勢の信頼度の情報及び計測時刻の情報とする。

【0052】

障害物配置情報取得部１０８は、障害物配置情報を取得する。実施例１では、障害物配置情報生成部１０６から取得していたが、本実施例では、保持部２０２から取得する。障害物配置情報は、実施例１で説明したものと同様に、障害物の配置状況を示した障害物配置画像とする。

【0053】

自動走行可能情報算出部１０９は、実施例１で説明したものと同様である。
経路探索部２０１は、位置姿勢の履歴及び障害物配置情報に基づいて、自動走行可能な経路を探索する。
保持部２０２は、位置姿勢計測の履歴及び障害物配置情報を保持する。

【0054】

図７は、実施例４における処理手順を示すフローチャートである。処理ステップは、初期化ステップＳ２００、位置姿勢履歴取得ステップＳ２０１、障害物配置情報取得ステップＳ２０２、自動走行可能情報算出ステップＳ２０３、経路探索ステップＳ２０４から構成される。尚、図７における各ステップの動作は、情報処理装置１０１内のコンピュータがメモリに記憶されたコンピュータプログラムを実行することによって行われる。
以下に各処理ステップについて詳しく説明する。

【0055】

ステップＳ２００では、初期化を行う。具体的には、外部の記憶装置が保持する許容位置ずれ量の設定値を読み込む。
ステップＳ２０１では、位置姿勢履歴取得部１０７において、移動体の位置姿勢計測用地図生成に算出した位置姿勢、位置姿勢の信頼度及び計測時刻を保持部２０２から取得する。

【0056】

ステップＳ２０２では、障害物配置情報を保持部２０２から取得する。
ステップＳ２０３では、移動体の自動走行可能情報である、自動走行可能な位置、方向、範囲及び信頼度を取得する。
ステップＳ２０４では、ステップＳ２０２で取得した障害物配置情報及びステップＳ２０３で算出した自動走行可能情報に基づいて、経路探索部２０１により、予め設定した開始地点、経由地点及び目的地点とした自動走行可能な経路を探索する。ここでステップＳ２０４は、自動走行可能情報と障害物配置情報に基づき移動体の走行経路を探索する経路探索手段（経路探索工程）として機能している。

【0057】

探索方法としては、例えば、先ず、開始地点、経由地点群、目的地点を経由順ごとに隣り合う２地点選択し、該２地点を結ぶ区間での自動走行可能な経路を探索する。探索は、既知のアルゴリズムである例えばＡ－Ｓｔａｒアルゴリズムを用いて行う。Ａ－Ｓｔａｒアルゴリズムは、経路をノードで表現し、開始地点から目的地点までの最短経路を探索するアルゴリズムである。

【0058】

本実施例では、障害物配置画像を複数のグリッドに分割し、各グリッドをノードとして扱う。ノードの進行方向は、上下、左右、斜め右上、斜め右下、斜め左上、斜め左下の８方向のうち、自動走行可能方向近傍の２方向に限定する。又、障害物や自動走行可能でない領域が含まれるグリッドは、進行不可能とする。そして、各２地点間の経路をつなぎ合わせることで、自動走行可能な領域及び向きに限定した開始地点から経由地点を通過し目的地点まで到達する経路を探索する。

【0059】

図８は、実施例４における経路探索結果の例を示す図である。図中の黒い矢印は探索して得られた経路、矢印の背景のハッチングは自動走行可能な範囲を示している。このように、表示制御手段としての提示部１１１は、地図画像と移動体の走行経路の表示を制御する。尚、このような表示をせずに、探索された経路に基づき直接的に移動体１００の移動方向や移動量の制御を行うようにしてもよい。
以上述べた方法を実施することで、安定して位置姿勢が計測できる経路を設定することができる。

【0060】

尚、上記では経路探索にＡ－Ｓｔａｒアルゴリズムを適用したが、予め設定した開始地点から目的地点まで、自動走行可能な経路が探索できれば良く、例えばダイクストラ法など、他の経路探索方法でも良い。
又、上記では、予め開始地点、経由地点及び目的地点を設定し、経由地点を通過する開始地点から目的地点までの経路を探索する方法について説明した。しかし、経由地点を設定せず、開始地点から目的地点までの経路を探索するようにしても良い。

【0061】

又、上記では、予め開始地点、経由地点及び目的地点を設定し、経由地点を通過する開始地点から目的地点までの経路を探索する方法について説明したが、地点ではなく、おおよその場所を示す領域を設定するようにしても良い。この場合、設定した領域内かつ自動走行可能な範囲内の所定の位置、例えば自動走行可能な情報である信頼度が最も高い位置を各地点とし、本実施例で説明した方法で経路を探索すれば良い。

【0062】

更に又、実施例４において、自動走行可能情報と障害物配置情報に基づいて、自動走行可能な経路をユーザが手動で入力し、設定しても良い。具体的には、ステップ２０４において、ユーザが、自動走行可能情報を示すＣＧ像が重畳された障害物配置画像を参照し、経由地点及び経由する順番を、入力部２１５を介して入力する。これにより、予め定めた開始地点から、入力した経由地点を経由順に経由し、目的地点まで到達する経路を設定することができる。

【0063】

尚、ユーザが自動走行可能な位置や向きではない経由地点及び経由順の入力した場合には、例えば「自動走行可能な位置や向きではない」との旨をユーザに通知するようにしても良い。
尚、開始地点や目的地点についても、経由地点と同様に、ユーザが入力部２１５を介して入力するようにしても良い。

【0064】

尚、自動走行可能情報のみに基づいて、自動走行可能な経路を探索しても良い。具体的には、ステップ２０４において、自動走行可能でない領域が含まれるグリッドのみを、進行不可能とする。これにより、自動走行可能情報のみに基づいて、自動走行可能な経路を探索することが可能となる。

【0065】

尚、自動走行可能情報と障害物配置情報に加えて、経路の探索条件に基づいて、自動走行可能な経路を探索しても良い。具体的には、探索条件として信頼度の閾値を設定し、経路上の信頼度が閾値よりも高い経路を探索する。これにより、より安定して位置姿勢計測が実施できる経路を探索することが可能となる。

【0066】

例えば図６の経路探索部２０１において、自動走行可能情報及び障害物配置情報に基づいて、自動走行可能な経路を探索すると共に、探索条件として、予め定めた信頼度に基づき経路を探索する。

【0067】

そして図７におけるステップＳ２０４では、ステップＳ２０２で取得した障害物配置情報、ステップＳ２０３で算出した自動走行可能情報、探索条件としての信頼度の閾値に基づいて、自動走行可能な経路を探索する。障害物配置情報は、画像（障害物配置画像）とする。
即ち、先ず、開始地点、経由地点群、目的地点を経由順ごとに隣り合う２地点選択し、該２地点を結ぶ区間での自動走行可能な全経路を探索する。全探索の方法としては、例えばＢｒｅａｄｔｈ－Ｆｉｒｓｔ Ｓｅａｒｃｈと呼ばれる既存の手法を用いて行う。

【0068】

ここでは、障害物配置画像を複数のグリッドに分割し、各グリッドをノードとして扱い、各地点が含まれるノード間の全経路を探索する。各ノードの進行方向は、上下、左右、斜め右上、斜め右下、斜め左上、斜め左下の８方向のうち、自動走行可能方向近傍の２方向に限定する。更に又、障害物や自動走行可能でない領域が含まれるグリッドだけでなく、信頼度が所定の閾値よりも低い領域も、進行不可能とする。

【0069】

そして、各２地点間の経路をつなぎ合わせることで、自動走行可能な領域及び向き、及び信頼度が前記所定の閾値より高い、開始地点から経由地点を通過し目的地点まで到達する経路を探索する。このようにすることで、自動走行可能な領域及び向き、及び信頼度が閾値より高い、開始地点から経由地点を通過し目的地点まで到達する経路を探索することができる。

【0070】

尚、上記の経路探索ステップＳ２０４で説明した経路探索において、探索条件を満たす経路が存在しなかった場合には、その旨通知するようにしても良い。即ち、移動体の自動走行可能な経路が存在しない場合に、ユーザに通知する通知手段を設けても良い。
尚、上記では信頼度が閾値よりも高い位置を通る自動走行可能な経路を探索する方法について説明したが、信頼度の平均値が閾値よりも高くなる自動走行可能な経路を探索するようにしても良い。

【0071】

この場合、障害物や自動走行可能でない領域が含まれるグリッドのみを進行不可能として経路を探索し、経路上の信頼度に基づいて、信頼度の平均値を求める。そして、信頼度の平均値が信頼度の閾値よりも高い経路を、自動走行可能かつ信頼度が閾値よりも高くなる経路として抽出する。

【実施例5】

【0072】

以上の実施例のステップＳ２０４では、自動走行可能な領域及び向きに限定した、開始地点から経由地点を経由して目的地点まで到達する経路の探索を実施する方法について説明した。実施例５では、自動走行可能な領域及び向きに限定せずに開始地点から経由地点を経由して目的地点まで到達する複数の経路を探索し、その中から自動走行可能な領域及び向きに限定した経路を選択する。以下では、実施例５におけるステップＳ２０４の処理について説明する。

【0073】

ステップＳ２０４において、先ず、自動走行可能な領域及び向きに限定せずに開始地点から経由地点を経由して目的地点まで到達する複数の経路を探索する。具体的には、実施例２及び３のステップＳ２０４と同様の処理を、ノードの進行方向を、上下、左右、斜め右上、斜め右下、斜め左上、斜め左下の８方向として実施する。そして、探索した複数の経路の中から、自動走行可能な領域及び向きに限定した経路を選択する。このように、経路探索部２０１は、自動走行可能情報と障害物配置情報と自動走行可能情報の信頼度とに基づいて、移動体の自動走行可能な経路を探索又は選択することができる。

【0074】

尚、以上の実施例において、更に、ユーザが任意に経路を設定できるようにし、予め任意に設定された移動体の経路を、自動走行可能情報に基づいて、自動走行可能な経路となるように補正しても良い。
即ち、先ず、ユーザが任意に設定した経路の開始地点と目的地点が、自動走行可能な範囲にあるか否かをチェックし、範囲外の場合には、範囲内となるように移動する。例えば、各地点と範囲の境界線間の距離が最短となる該境界線上の位置に、各地点を移動する。

【0075】

次に、図７のステップ２０４と同様の処理を実施し、自動走行可能な領域及び向きに限定した、開始地点から目的地点まで到達する経路を探索する。
次に、探索した経路の中から、ユーザが任意に設定した経路の近傍を通過する経路を抽出する。具体的には、ユーザが任意に設定した経路の経由地点と、探索して得られた経路の経路線までの距離が所定の範囲内となる経路を抽出する。抽出して得られた経路を、補正後の経路とすれば良い。

【0076】

尚、図１のＡＭＲ（自律走行ロボット装置）は、ＡＭＲの移動（走行）を行うためのモータやエンジン等の駆動装置や、ＡＭＲの移動方向を変更するための移動方向制御装置を有する。又、駆動装置の駆動量や移動方向制御装置の移動方向を制御するための移動制御手段としての移動制御部を有する。

【0077】

移動制御部は内部にコンピュータとしてのＣＰＵと、コンピュータプログラムを記憶したメモリを内蔵しており、他の装置との通信を行うことで例えば情報処理装置１０１を制御すると共に、情報処理装置１０１から位置姿勢情報や走行経路情報等を取得する。
ＡＭＲは、情報処理装置１０１により探索された走行経路に基づき移動制御部によりＡＭＲの移動方向や移動量や移動経路を制御するように構成されている。

【0078】

以上、本発明をその好適な実施例に基づいて詳述してきたが、本発明は上記実施例に限定されるものではなく、本発明の主旨に基づき種々の変形が可能であり、それらを本発明の範囲から除外するものではない。
尚、本実施例における制御の一部又は全部を上述した実施例の機能を実現するコンピュータプログラムをネットワーク又は各種記憶媒体を介して情報処理や自律走行ロボット装置等に供給するようにしてもよい。そしてその情報処理や自律走行ロボット装置等におけるコンピュータ（又はＣＰＵやＭＰＵ等）がプログラムを読み出して実行するようにしてもよい。その場合、そのプログラム、及び該プログラムを記憶した記憶媒体は本発明を構成することとなる。

【符号の説明】

【0079】

１００ 移動体
１０１ 情報処理装置
１０２ カメラ
１０３ 位置姿勢計測部
１０４ 位置姿勢計測用地図生成部
１０５ 距離センサ
１０６ 障害物配置情報生成部
１０７ 位置姿勢履歴取得部
１０８ 障害物配置情報取得部
１０９ 自動走行可能情報算出部
１１０ 画像生成部
１１１ 提示部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】