特許 7508288 情報処理装置、情報処理装置の制御方法なおよびプログラム

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

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

(11)【特許番号】

(24)【登録日】2024-06-21

(45)【発行日】2024-07-01

(54)【発明の名称】情報処理装置、情報処理装置の制御方法なおよびプログラム

(51)【国際特許分類】

   G06T 7/70 20170101AFI20240624BHJP

   G01C 21/28 20060101ALI20240624BHJP

【ＦＩ】

G06T7/70 Z

G01C21/28

【請求項の数】 19

(21)【出願番号】P 2020109844

(22)【出願日】2020-06-25

(65)【公開番号】P2022022525

(43)【公開日】2022-02-07

【審査請求日】2023-06-23

(73)【特許権者】

【識別番号】000001007

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

(74)【代理人】

【識別番号】100126240

【弁理士】

【氏名又は名称】阿部 琢磨

(74)【代理人】

【識別番号】100223941

【弁理士】

【氏名又は名称】高橋 佳子

(74)【代理人】

【識別番号】100159695

【弁理士】

【氏名又は名称】中辻 七朗

(74)【代理人】

【識別番号】100172476

【弁理士】

【氏名又は名称】冨田 一史

(74)【代理人】

【識別番号】100126974

【弁理士】

【氏名又は名称】大朋 靖尚

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

コンピュータに請求項１から１８のいずれか１項に記載の報処理装置として動作させるためのプログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、センサーの位置姿勢の推定とそれに用いる電子的な地図を生成する技術に関する。

【背景技術】

【0002】

工場や物流倉庫において無人搬送車（Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｖｅｈｉｃｌｅ（ＡＧＶ））のような自律した移動体が用いられている。また、こうした無人搬送車の位置姿勢の推定と、推定に用いる電子的な環境地図データの作成を行う方法として、カメラやレーザーレンジスキャナをセンサーとして使用したＳＬＡＭ技術が知られている。なお、ＳＬＡＭは、Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇの略である。

【0003】

非特許文献１では、センサーを搭載した移動体を動かして環境地図データを生成する処理に際し、センサーが取得した各計測点の情報から実空間上の位置と環境地図データ上の計測点の対応をコンピュータが認識することが記載されている。また、同文献には、コンピュータが認識した実空間上の位置と計測点の対応を元に環境地図データ内の位置姿勢のずれを補正するループクローズ技術が開示されている。

【先行技術文献】

【非特許文献】

【0004】

【文献】Ｍｕｒ－Ａｒｔａｌ， Ｒ．， ＆Ｔａｒｄｏｓ， Ｊ． Ｄ． ＯＲＢ－ＳＬＡＭ２： ａｎ Ｏｐｅｎ－Ｓｏｕｒｃｅ ＳＬＡＭ Ｓｙｓｔｅｍ ｆｏｒ Ｍｏｎｏｃｕｌａｒ， Ｓｔｅｒｅｏ ａｎｄ ＲＧＢ－Ｄ Ｃａｍｅｒａｓ． ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｒｏｂｏｔｉｃｓ ３３（５） １２５５－１２６２．

【発明の概要】

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

【0005】

しかしながら、非特許文献１に記載された方法では、ループクローズ時の環境地図データの補正に時間がかかるために、位置姿勢推定の精度が低下する問題があった。具体的には、補正処理を開始した以降にもセンサーが移動を続けた場合、補正処理が完了する前に蓄積された誤差の影響を受けた新たな計測点が生成される場合がある。この場合、新たな計測点に基づいて位置姿勢を推定するか、既存の計測点に基づいて位置姿勢を推定するかによって、位置姿勢の推定結果が変動するおそれがある。

【0006】

本発明は、ループクローズ時の地図データの補正に時間がかかることに起因する冗長な計測点の生成を抑制することを目的とする。

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

【0007】

上記の目的を達成する本発明に係る情報処理装置は、
移動するセンサーから出力される周囲環境を計測したセンサー情報を取得する取得手段と、
前記センサーの移動経路に基づく地図を示す地図データであって、前記センサー情報と、前記センサーの位置姿勢とが関連付けられた計測点を含む地図データを生成する生成手段と、
前記取得手段により取得された前記センサー情報と前記計測点とに基づいて、前記センサーの位置姿勢を推定する推定手段と、
前記センサーの移動経路のループを検出する検出手段と、
前記検出手段よりループが検出された場合、前記推定手段が位置姿勢の推定に用いている第１の計測点に関連付けられた位置姿勢を、前記検出手段によりループが検出された際の前記センサーの実空間上での近傍に存在する第２の計測点を基準とした位置姿勢に補正する第１の補正手段と、
前記検出手段よりループが検出された場合、前記地図データに含まれ、かつ、前記第１の計測点とは異なる計測点を含む複数の計測点のそれぞれに関連付けられた位置姿勢の補正であって、前記第１の補正手段により補正される計測点より多くの計測点の位置姿勢を補正する第２の補正手段と、を有することを特徴とする。

【発明の効果】

【0008】

本発明によれば、ループクローズ時の地図データの補正に時間がかかることに起因する冗長な計測点の生成を抑制することができる。

【図面の簡単な説明】

【0009】

【図1】第１の実施形態の補正処理実施前の移動体の移動経路と環境地図の状態の一例を示す図である。

【図2】第１の実施形態において第一の補正処理を実施しなかった場合の環境地図の状態の一例を示す図である。

【図3】第１の実施形態の第一の補正処理後の環境地図の状態の一例を示す図である。

【図4】第１の実施形態の第二の補正処理前の環境地図の状態の一例を示す図である。

【図5】第１の実施形態の第二の補正処理後の環境地図の状態の一例を示す図である。

【図6】第１の実施形態の移動体システムの構成を示す図である。

【図7】第１の実施形態の情報処理装置の論理的構成を示すブロック図である。

【図8】第１の実施形態のループクローズ処理部の論理的構成の詳細を示すブロック図である。

【図9】第１の実施形態の情報処理方法の流れを示すフローチャート図である。

【図10】第１の実施形態のループクローズ処理の流れの詳細を示すフローチャートである。

【図11】第１の実施形態の第一の補正処理の流れの詳細を示すフローチャートである。

【図12】第２の実施形態の情報処理方法の流れを示すフローチャートである。

【図13】第２の実施形態の第一の補正処理前の環境地図の状態の一例を示す図である。

【図14】第２の実施形態の第一の補正処理の流れの詳細を示すフローチャートである。

【図15】第２の実施形態の剛体変換による補正処理後の環境地図の状態の一例を示す図である。

【図16】第２の実施形態の特徴点統合処理後の環境地図の状態の一例を示す図である。

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

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

また、画像間の特徴点の追跡には既知のＫａｎａｄｅ－Ｌｕｃａｓ－Ｔｏｍａｓｉ （ＫＬＴ）アルゴリズムを利用するものとする。なお、上述した特徴点の抽出や特徴点の追跡に用いるアルゴリズムは他のアルゴリズムでも構わない。

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

第２の実施形態における位置姿勢算出スレッドについて説明する。

【0087】

Ｓ１２０２では、位置姿勢算出部７０３がセンサー６０３から画像を取得し、画像と環境中の特徴点に基づいて現在位置姿勢情報を更新する。位置姿勢算出部７０３は、その時点で参照している計測点から観測されている環境中の特徴点を画像上に投影し、対応する画像特徴点との最投影誤差が最小となるような位置姿勢を最適化処理によって算出する。その際、画像と環境中の特徴点との対応付けを維持するため、最新の画像とそれ以前の画像との間で特徴点の追跡を行う。

【0088】

そして、第１の実施形態と同様にＳ９０３とＳ９０４を実施する。

【0089】

続いて、第２の実施形態にける地図作成スレッドを説明する。

【0090】

Ｓ１２０３では、計測点生成部７０４がＳ９０３における位置姿勢算出部７０３からの指示に基づき環境中の特徴点と計測点を生成する。

【0091】

なお、生成する計測点には、Ｓ１２０２で取得した画像と位置姿勢と、位置姿勢算出部７０３が追跡している環境中の特徴点群と、次に説明する新規に生成された環境中の特徴点群への観測情報とが含まれる。環境中の特徴点の生成は、Ｓ１２０２で選択した計測点の画像と、生成する計測点における撮影画像とを用いて行われる。

【0092】

まず、情報処理装置６０４は、双方の画像から、既存の環境中の特徴点と対応づけられていない画像特徴点を抽出する。その後、情報処理装置６０４は、画像間の相対位置姿勢に基づいて、画像間で対応が取れた画像特徴点の三次元座標を推定する。最後に、情報処理装置６０４は、三次元座標の推定が成功した画像特徴点を環境中の特徴点として環境地図データに追加し、また、その環境中の特徴点に対応する各々の画像上の画像特徴点の二次元座標を、各々の計測点からの観測情報として追加する。また、計測点生成部７０４は新しい計測点とＳ９０３で選択した計測点との相対位置姿勢情報をポーズグラフに追加する。

【0093】

Ｓ１２０４で、計測点生成部７０４はバンドル調整処理により、Ｓ１２０２で生成した計測点と環境中の特徴点を共有している計測点群の位置姿勢との補正を行う。また、計測点生成部７０４は、バンドル調整の結果として得られた各計測点間の相対位置姿勢情報をポーズグラフに追加、あるいは上書きする。

【0094】

続いて、第２の実施形態にける地図補正スレッドを説明する。

【0095】

Ｓ１２０５では、地図作成スレッドが更新した環境地図データ上のループを検知し、ループクローズ処理を行う。ループクローズ処理の詳細については，第一の補正処理（Ｓ１００３）の詳細を除き、第一の実施形態の図１０に示したものと同様である。そのため、ここでは第一の補正処理の詳細についてのみ説明する。

【0096】

（第一の補正処理の詳細）
本実施形態における、Ｓ１００３に示した第一の補正処理の詳細について図を参照し詳細に説明する。ここでは、位置姿勢差分を用いた剛体変換による各計測点および環境中の特徴点の位置姿勢の補正に加え、環境中の特徴点の統合処理と、ループ元およびループ先の近傍の計測点を用いたバンドル調整処理を行う。

【0097】

ここでも、第一の実施形態と同様、図１のように、センサー６０３が計測点Ａを始点とし、環境中を時計回りに移動しながら計測点Ｂ，Ｃ…Ｅ，Ｆ，Ｇ，Ｈを生成し、計測点Ａの近傍へ戻った状態を例に説明する。

【0098】

情報処理装置６０４は、計測点Ｈをループ元、計測点Ａをループ先としたループが検出した場合、計測点間の相対位置姿勢を算出する（Ｓ１００１）。

【0099】

図１３は、ループ検出時のセンサー６０３周辺の環境地図の状態を示した図である。図中の黒点ｐ、ｐ‘、ｑ、ｒ、ｓは環境中の特徴点である。ｐ、ｑは計測点Ａから、ｐ’は計測点Ｈ，Ｇおよびセンサー６０３の最新の画像から、ｒは計測点Ｈおよびセンサー６０３の最新の画像から、ｓは計測点Ｇから観測される。また、ｐとｐ‘はこの時点では別々の環境中の特徴点として環境地図データ７０１に登録されているが、実空間上の物体の同一の特徴点に由来している。実際には位置姿勢推定のためにより多くの環境中の特徴点が存在しているが、ここでは説明の簡便のため一部のみを図示している。

【0100】

図１４は、本実施形態における第一の補正処理の流れを示すフローチャートである。図１４において、Ｓ１４０１では、剛体変換による補正処理を行うか否かの判定と分岐である。本実施形態では、情報処理装置６０４は、位置姿勢算出部７０３が参照している環境中の特徴点に、ループ先の計測点が観測している環境中の特徴点が含まれていない場合に限り、Ｓ１１０２、Ｓ１４０２、Ｓ１４０３の処理を行う。

【0101】

Ｓ１４０１において、剛体変換による補正処理を行うと判定された場合、情報処理装置６０４は、Ｓ１４０２で、剛体変換による補正を行う環境地図データ上の要素を選択する。まず、位置姿勢算出部７０３が追跡している環境中の特徴点を抽出する。図１３の例では、ｐ‘とｒが抽出される。次に、それらの特徴点のいずれかを観測している計測点を補正対象の計測点として抽出する。図１３の例では、計測点ＨとＧが抽出される。最後に、補正対象の計測点のいずれかに観測されている環境中の特徴点を補正対象の環境中の特徴点として抽出する。図１３の例では、ｐ’、ｒ、ｓが補正対象の環境中の特徴点となる。また、センサー６０３の現在位置姿勢情報も補正対象となる。従って、この場合の補正対象は計測点Ｈ，Ｇの位置姿勢、環境中の特徴点ｐ‘、ｒ、ｓの位置、センサー６０３の位置姿勢である。

【0102】

Ｓ１４０３では、第一の補正部８０３は、Ｓ１１０２で選択した各補正対象にＳ１１０２で算出した位置姿勢差分ｄＨを積算し、位置姿勢あるいは位置の更新を行う。この処理はいわゆる剛体変換であり、補正対象の各要素の相対的な位置姿勢関係は維持される。

【0103】

図１５は剛体変換処理後の計測点の位置関係を示した図であり、計測点Ｈ、Ｇの位置姿勢がＨ‘、Ｇ’に補正されている。このとき、計測点Ｆと補正対象となった要素との相対位置姿勢は崩れているが、この時点で計測点と位置姿勢算出部７０３は環境中の特徴点を共有していないため、位置姿勢算出に悪影響を及ぼさない。

【0104】

Ｓ１４０４では、第一の補正部８０３は、Ｓ１４０３で位置を補正した環境中の特徴点と、それらの補正後の位置の近傍に存在する環境中の特徴点の統合を行う。まず、情報処理装置６０４は、位置を補正した各環境中の特徴点について、環境地図中で補正後の位置から一定の距離以内にある他の点を抽出する。その後、情報処理装置６０４は、抽出した環境中の特徴点と補正した環境中の特徴点との間で、各々の観測情報に基づいて、画像上でその環境中の特徴点を観測している部位の画像特徴を比較する。画像特徴の比較にはパッチマッチングを用いる。双方の観測部位の画像特徴が十分に類似していると判断された場合、それらは実空間上の同じ物体を参照していると判断し、二つの環境中の特徴点を統合する。なお、画像特徴の比較方法はこれに限らず、例えばＯＲＢ特徴量などを利用しても良い。

【0105】

図１６は特徴点統合後の、センサー６０３周辺の環境地図の状態を示した図である。ｐ‘の位置が剛体変換によって修正されたのち、ｐ’とｐが統合されている。統合後のｐは統合前のｐ’とｐ双方の被観測情報を持つ。すなわち計測点Ａ、Ｈ‘、Ｇ’から観測されている。

【0106】

Ｓ１４０５では、第一の補正部８０３は、バンドル調整処理により、Ｓ１４０４で統合した環境中の特徴点群の位置と、そのいずれかを観測している計測点群の位置姿勢の補正を行う。図１３の例では環境中の特徴点ｐと計測点Ａ、Ｈ‘、Ｇ’がバンドル調整の対象となる。ここで、実際には図示されていないより多くの環境中の特徴点が統合処理およびバンドル調整の対象となっており、バンドル調整による位置姿勢の最適化に十分な情報が揃っている。また、第一の補正部８０３は、バンドル調整の結果として得られた各計測点間の相対位置姿勢情報をポーズグラフに追加、あるいは上書きする。

【0107】

バンドル調整によるループ元およびループ先近傍の計測点の位置姿勢の補正は、剛体変換による補正より時間はかかるものの、ポーズグラフ最適化を伴うＳ１００４の処理と比較して大幅に短い時間で実行可能である。そのため、ループクローズ処理部７０５がループを検出した後、短時間でループ元およびループ先近傍の計測点の相対位置姿勢を高精度化し、位置姿勢算出部７０３による位置姿勢の算出精度を向上することができる。また位置姿勢算出精度が向上することにより、第二の補正処理終了前に新たな計測点が生成された場合においても、その位置姿勢情報の精度を向上することができる。

【0108】

（本実施形態における発明の効果）
以上、本実施形態によれば、環境地図データに環境中の特徴点情報を含むような位置姿勢算出方法で環境地図データの作成中に環境地図の補正処理が実施された場合においても、高精度な位置姿勢推定を実現可能な環境地図データを生成することができる。

【0109】

［その他の実施形態］
第１および第２の実施形態では、センサー６０３として移動体の正面方向に固定されたグレースケールの輝度画像を取得可能なカメラを使用したが、センサーの種類や個数、固定方法はこれに限定されない。センサー６０３は移動体から周囲の輝度画像や深度画像をデジタルデータとして連続的に取得可能な装置であれば良く、グレースケールカメラの他にカラー画像を取得するカメラやデプスカメラ、２Ｄ－ＬｉＤＡＲあるいは３Ｄ－ＬｉＤＡＲなどが利用可能である。また、ステレオカメラを使用したり、複数のカメラを移動体の各方向を向くように並べたりしても良い。また秒間の情報取得回数も３０回に限定されない。

【0110】

例えばセンサー６０３としてステレオカメラを使用する場合、Ｓ１２０１で情報処理装置６０４は、環境中の特徴点を生成する処理において、センサーを位置姿勢Ａ‘へ移動させる代わりにステレオ画像ペアを用いて画像特徴点までの距離を取得する。情報処理装置６０４は、それを画像特徴点の三次元座標に変換することができる。

【0111】

あるいはセンサー６０３としてデプスカメラや３Ｄ－ＬｉＤＡＲのように画像の各画素に対する距離情報が得られるものを利用する場合、情報処理装置６０４は、センサーの位置姿勢と画角、各画素までの距離を元に三次元座標を算出する。そして、情報処理装置６０４は、各画素に対応する環境中の特徴点を生成しても良い。こうした形態では第２の実施形態より密な環境中の特徴点群が得られる。またこのような場合、既知のＩｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ （ＩＣＰ）アルゴリズムを使用して計測点間の相対位置姿勢を算出する。そして、算出した相対位置姿勢をループの検出処理や第一の補正処理に利用することができる。

【0112】

また、第１および第２の実施形態では三次元空間における位置姿勢計測と、それに用いる環境地図データの作成について説明したが、移動体が移動する面に沿った二次元平面上での位置姿勢計測と環境地図データの作成を行ってもよい。例えば地上を走行する移動体システムにおいて、センサー６０３として水平方向をスキャンする２Ｄ－ＬｉＤＡＲを使用し、二次元平面上で上記したような密な環境中の特徴点群をもつ環境地図データを作成してもよい。

【0113】

また、第１の実施形態では、計測点のループ検出用情報として画像を保持していたが、例えばＳ９０５における計測点生成時にＢｏＷ ｍｏｄｅｌに基づく特徴量ベクトルや画像特徴点を算出しておき、画像の代わりにそれらを保持しても良い。この場合、ループ検出部８０１は予め算出された特徴量ベクトルに基づいて画像類似度を算出したり、予め算出された特徴点に基づいて相対位置姿勢を算出したりすることができる。

【0114】

また、第１の実施形態では、Ｓ９０１において最初の計測点の位置は環境中の原点とし、姿勢は所定の方向（例えばＹ軸正方向）とした。しかしながら、他の方法で最初の計測点の位置姿勢が指定可能な場合はその値を用いても良い。例えばセンサー６０３や他の手段で検出可能なマーカーを用いて、センサー６０３との相対位置姿勢を算出し、原点や座標軸の設定を行ってもよい。また、移動体システム６０１上でのセンサー６０３の取り付け位置姿勢をオフセットとして原点や座標軸の設定を行ってもよい。

【0115】

また、同様のマーカーをループ検出部８０１におけるループ検出に用いても良い。例えば、画像の類似度を算出する代わりに２つの計測点が同一のマーカーを観測していることを検出し、各々の画像におけるセンサーとマーカーの相対位置姿勢から、画像間の相対位置姿勢を算出することができる。

【0116】

また、第２の実施形態では、Ｓ１４０１において、剛体変換処理を実施するか否かの判断を位置姿勢算出部７０３がループ先の計測点と環境中の特徴点を共有しているか否かで判定したが、剛体変換処理の実施に関する判定方法はこれに限定されない。例えば位置姿勢算出部７０３とループ元の計測点との環境中の特徴点の共有数と、位置姿勢算出部７０３とループ先の計測点との環境中の特徴点の共有数とを比較し、前者が大きい場合に剛体変換処理を実施するような方法をとってもよい。あるいは第１の実施形態と同様に、各計測点のポーズグラフ上での距離に基づいて剛体変換処理の実施の要否を判断してもよい。

【0117】

また、第１の実施形態では、ユーザーが外部から操作する移動体を例にとり説明したが、移動体システム６０１の形態はこれに限定されない。例えばユーザーが搭乗し直接操作できるような有人の移動体であっても良い。あるいは、事前に設定したルート上を自律走行する機能を備えた移動体であってもよい。この場合、環境地図データ７０１と位置姿勢算出部７０３が算出した位置姿勢情報に基づいて移動体システム６０１の制御情報を生成し、制御装置６０６を通じて移動部６０７を駆動させることで自立走行を実現してもよい。また、移動体システム６０１は自律走行中にセンサー６０３から取得したセンサー情報に基づき、第１あるいは第２の実施形態で説明した方法に従って環境地図データ７０１を更新することができる。

【0118】

また移動部６０７は車輪であるとしたが、複数のプロペラ等を装備し、移動体システム６０１が空中を飛行し、センサー６０３が地表方向を観測するような形態を取っても良い。

【0119】

本発明は、上述の実施例の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。

【0120】

なお、上述した各処理部のうち、位置姿勢算出部７０３、計測点生成部７０４については、その代わりとして、機械学習された学習済みモデルを代わりに用いて処理しても良い。その場合には、例えば、その処理部への入力データと出力データとの組合せを学習データとして複数個準備し、それらから機械学習によって知識を獲得し、獲得した知識に基づいて入力データに対する出力データを結果として出力する学習済みモデルを生成する。学習済みモデルは、例えばニューラルネットワークモデルで構成可能である。そして、その学習済みモデルは、前記処理部と同等の処理をするためのプログラムとして、ＣＰＵあるいはＧＰＵなどと協働で動作することにより、前記処理部の処理を行う。なお、上記学習済みモデルは、必要に応じて一定の処理後に更新しても良い。

【符号の説明】

【0121】

６０１ 移動体システム
６０２ 環境地図作成システム
６０３ センサー
６０４ 情報処理装置
６０５ 通信部
６０６ 制御装置
６０７ 移動部
７０１ 環境地図データ
７０２ センサー情報取得部
７０３ 位置姿勢算出部
７０４ 計測点生成部
７０５ ループクローズ処理部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】