JP2024043764A - 情報処理装置、移動体、情報処理方法及びコンピュータプログラム - Google Patents

Abstract

【課題】欠落やずれ等の生じた位置姿勢情報の補正を自動で行うことが可能な情報処理装置を提供する。【解決手段】情報処理装置において、移動するセンサが複数地点で夫々計測したセンサ周囲の物体の形状情報を取得する形状情報取得手段を有し、対象の位置姿勢情報と、形状情報取得手段で取得した対象の位置姿勢情報に紐づく形状情報と、近似できる位置姿勢情報に紐づく形状情報に基づいて、前記位置姿勢情報を補正した位置姿勢情報を算出する第２位置姿勢算出手段と、を有する。【選択図】図２

Description

本発明は、移動体に適した情報処理装置、移動体、情報処理方法及びコンピュータプログラムに関するものである

無人搬送車（例えばＡＧＶ：Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｖｅｈｉｃｌｅ）と呼ばれる、工場や物流倉庫などの環境内で稼働する移動体が知られている。この移動体は、設定した経路を、自己位置推定を行いながら走行する。又、移動体が計測した走行経路周辺の物体の位置姿勢データを用いて、障害物の形状等を表現した地図（以降、障害物レイアウト地図）を作成することで、移動体の経路設定に用いる技術も提案されている。

特許文献１には、レーザー測距計を搭載した自律移動ロボットで計測した形状データを用いて仮の障害物レイアウト地図を生成する手法が記載されている。又、ユーザにより手動でフレームごとの仮の障害物レイアウト地図の一部の形状の位置姿勢を修正することで、欠損の生じない障害物レイアウト地図を作成する手法が公開されている。

特許６５０６２７９号公報

しかしながら、特許文献１に記載の手法では、移動体の自己位置推定を行いながら地図を障害物レイアウト作成する過程において、冗長な移動体の位置姿勢の計測データが削除される場合がある。又、特許文献１の手法では、位置姿勢データの削除によって生じる地図の欠けや位置ずれの補正を、ユーザが手動で全て行わなければならず、煩雑であるという問題がある。

上記課題に鑑みて、本発明では欠落やずれ等の生じた位置姿勢情報の補正を自動で行うことが可能な情報処理装置を提供することを目的とする。

情報処理装置において、
移動するセンサが複数地点で夫々計測したセンサ周囲の物体の形状情報を取得する形状情報取得手段と、
前記形状情報を前記複数地点で夫々計測した際の前記センサの位置姿勢情報を取得する位置姿勢取得手段と、
前記センサの位置や姿勢を補正する対象の位置姿勢を決定する補正対象決定手段と、
前記補正対象決定手段で決定した補正対象の位置姿勢を前記補正対象決定手段から取得し、前記位置姿勢情報の補正傾向と近似できる位置姿勢情報を前記位置姿勢取得手段から取得し、前記補正量の近似値を取得する補正量取得手段と、
前記補正対象決定手段で決定した前記対象の位置姿勢情報と、前記補正量取得手段で取得した補正量に基づき、前記対象の位置姿勢を補正した第１の位置姿勢情報を算出する第１位置姿勢算出手段と、
前記対象の位置姿勢情報と、前記形状情報取得手段で取得した前記対象の位置姿勢情報に紐づく形状情報と、前記近似できる位置姿勢情報に紐づく形状情報に基づいて、前記第１の位置姿勢情報を補正した第２の位置姿勢情報を算出する第２位置姿勢算出手段と、を有することを特徴とする。

本発明によると、欠落やずれ等の生じた位置姿勢情報の補正を自動で行うことが可能な情報処理装置を実現できる。

（Ａ）、（Ｂ）は、実施形態１に係る移動体の走行環境の例を説明するための模式図である。 実施形態１に係る情報処理システム２０１の機能ブロック図である。 実施形態１に係る情報処理装置２０３の利用場面の例を説明する図である。 実施形態１に係る情報処理装置２０３のハードウェア構成例を示す図である。 実施形態１に係る情報処理装置の行う処理全体の流れを示すフローチャートである。 実施形態１に係る第２の位置姿勢を算出する詳細処理を示すフローチャートである。 （Ａ）、（Ｂ）は実施形態２に係る移動体の走行環境を表す模式図である。 実施形態２に係る情報処理システム２０１の構成例を示す機能ブロック図である。 実施形態２に係る情報処理装置の処理全体の流れを示すフローチャートである。 実施形態３に係る情報処理システムの構成例を示す機能ブロック図である。 （Ａ）、（Ｂ）は実施形態３に係る補正後の位置姿勢情報算出の結果通知部の例を示す図である。

以下、図面を参照して本発明の実施形態を説明する。ただし、本発明は以下の実施形態に限定されるものではない。なお、各図において、同一の部材または要素については同一の参照番号を付し、重複する説明は省略または簡略化する。

＜実施形態１＞
実施形態１では、障害物の形状を計測するセンサを搭載した移動体（例えば自律移動体）を移動させる際に、形状情報に紐づく位置姿勢が削除された場合に、削除された位置姿勢の補正処理を実施する例について説明する。尚、実施形態１では、削除された位置姿勢の近傍の位置姿勢の補正量を用いて位置姿勢の補正処理を実施する。

即ち、削除された位置姿勢を補正する際に、近似値となる補正量で位置姿勢の推定を行う第１の位置姿勢情報を算出する。更に第１の位置姿勢情報の算出によって補正された形状情報で補正しきれずに残ってしまった位置ずれがなくなるように、位置姿勢を補正して第２の位置姿勢情報を算出する。詳細は後述する。

実施形態１における、削除された位置姿勢の補正処理を実施する様子を、図１を使って説明する。図１（Ａ）、（Ｂ）は、実施形態１に係る移動体の走行環境の例を説明するための模式図であり、図１（Ａ）は、障害物レイアウト地図を生成する対象の空間を上方から見た模式図である。

図１において、１０１は例えば自律移動体等の移動体である。位置姿勢計測用センサ１０２は本実施形態では例えばステレオカメラであり、一定のフレームレート（例えば３０枚／秒）で同時に撮影した左右の撮像画像を用いて、撮像範囲内の障害物との距離情報を計測する。形状計測用センサ１０３は本実施形態ではデプスセンサであり、一定のフレームレート（例えば３０枚／秒）で撮影した距離画像を用いて、撮像範囲内の障害物の形状を計測する。

１１８は地図生成対象の空間であり、空間１１８には、移動体が移動する際の障害となる障害物１０４～１１５が存在する。操作者は、移動体１０１をリモート操作しながら、移動体１０１に障害物１０４～１１５の形状を計測させるために、障害物の近傍をルート１１６に沿って走行させる。この移動体１０１の走行中は、移動体１０１は所定の周期で、位置姿勢計測用センサ１０２の計測画像をもとにカメラ位置姿勢１１７を複数地点で計測する。

図１（Ｂ）は、位置姿勢計測用センサ１０２が計測したカメラ位置姿勢と、形状計測用センサ１０３で計測した形状情報との関連を表した図である。移動体１０１が走行したルート１１６上に、ＩＤ：１８～ＩＤ：２０の３つの位置姿勢観測点を含む、複数の位置姿勢観測点がある。この時ＩＤ：１８の位置姿勢観測点は、形状情報１１９と紐づいている。

又、ＩＤ：１９の位置姿勢観測点は、形状情報１２０と紐づいている。ここで形状情報１１９は、ＩＤ：１８の位置姿勢観測点において形状計測用センサ１０３が計測した形状情報である。同様に形状情報１２０は、ＩＤ：１９の位置姿勢観測点において形状計測用センサ１０３が計測した形状情報である。

実施形態１では移動体１０１がルート１１６を１周して戻った際に、障害物レイアウト地図の精度を上げるためにループクローズ補正を行い軌道や地図を補正する。又、そのループクローズ補正において、例えばＩＤ：１９の位置姿勢観測点が冗長なものとして削除される場合を想定する。実施形態１では、その際にＩＤ：１９の補正後の位置姿勢を、近傍のＩＤ：１８（又はＩＤ：２０）の位置姿勢観測点の情報を利用して算出する。

実施形態１における情報処理システムの構成を、図２、図３を用いて説明する。図２は、実施形態１に係る情報処理システム２０１の機能ブロック図である。尚、図２に示される機能ブロックの一部は、撮像装置に含まれる不図示のコンピュータに、不図示の記憶媒体としてのメモリに記憶されたコンピュータプログラムを実行させることによって実現されている。

しかし、それらの一部又は全部をハードウェアで実現するようにしても構わない。ハードウェアとしては、専用回路（ＡＳＩＣ）やプロセッサ（リコンフィギュラブルプロセッサ、ＤＳＰ）などを用いることができる。又、図２に示される夫々の機能ブロックは、同じ筐体に内蔵されていなくても良く、互いに信号路を介して接続された別々の装置により構成しても良い。尚、図２に関する上記の説明は、図８、図１０についても同様に当てはまる。

情報処理システム２０１は、位置姿勢計測用センサ１０２、形状計測用センサ１０３、位置姿勢計測部２０２、情報処理装置２０３、位置姿勢補正部２０４を含む。情報処理装置２０３は、形状情報取得部２０５、位置姿勢取得部２０６、補正対象決定部２０７、補正量取得部２０８、第１位置姿勢算出部２０９、第２位置姿勢算出部２１０を有する。

又、情報処理システム２０１にはコンピュータとしてのＣＰＵが内蔵されており、記憶媒体としてのメモリに記憶されたコンピュータプログラムに基づき情報処理システム２０１の各部の動作を制御する制御手段として機能している。

形状情報取得部２０５（形状情報取得手段）は、形状計測用センサ１０３で計測した、走行経路周辺の障害物の形状情報を取得する。即ち、形状情報取得部２０５は、移動する形状計測用センサ１０３が複数地点で夫々計測したセンサ周囲の物体の形状情報を取得する。尚、本実施形態では、形状情報とは、形状計測用センサ１０３で計測した距離画像から算出した２次元の特徴点マップを意味する。

位置姿勢計測部２０２は、位置姿勢計測用センサ１０２で計測した位置姿勢情報や、位置姿勢計測用センサ１０２と形状計測用センサ１０３の位置関係に基づいて、形状計測用センサ１０３の位置姿勢情報を計測する。ここで取得する位置姿勢情報は、ループクローズ補正前やループクローズ補正後の位置姿勢である。

尚、本実施形態では、形状計測用センサ１０３の位置姿勢情報とは、空間１１８に設定した座標系における形状計測用センサ１０３の位置を示す座標値、及び形状計測用センサ１０３の計測方向を示す角度を意味する。

位置姿勢取得部２０６（位置姿勢取得手段）は、複数地点で形状計測用センサ１０３により形状情報を夫々計測した際の位置姿勢情報を、位置姿勢計測部２０２より取得する。尚、本実施形態では、移動体１０１には形状計測用センサ１０３を２台搭載しているため、取得する位置姿勢情報は２台分である。

位置姿勢補正部２０４（補正手段）は、位置姿勢計測部２０２で計測した複数地点の位置姿勢観測点（位置姿勢情報）を補正する。具体的には、ループクローズ補正を行って、複数地点の位置姿勢観測点（位置姿勢情報）の補正処理を行う。補正対象決定部２０７は、上記補正処理を実施する対象の位置姿勢情報を決定する。即ち、補正対象決定部２０７は、センサの位置や姿勢を補正する対象の位置姿勢を決定する。

特に本実施形態においては、位置姿勢補正部２０４にて行われたループクローズ補正によって、冗長なものとして削除されたことで欠落した位置姿勢情報を決定する。尚、本実施形態では、位置姿勢情報のこのような欠落はループクローズ補正やバンドル調整等により生じる場合を想定している。

補正量取得部２０８は、補正対象決定部２０７にて、欠落した位置姿勢情報の補正処理が必要だと決定された場合に、例えば最近傍の形状計測用センサ１０３の位置姿勢観測点の位置姿勢情報を取得する。そして、その最近傍の位置姿勢観測点がループクローズ補正された際の補正量を取得する。即ち、補正量取得部２０８は、補正対象の位置姿勢を補正対象決定部２０７から取得し、位置姿勢情報の補正傾向と近似できる位置姿勢情報を位置姿勢取得部２０６から取得し、補正量の近似値を取得する。

第１位置姿勢算出部２０９（第１位置姿勢算出手段）は、補正対象決定部２０７で対象の位置姿勢情報の補正が必要だと決定された場合に、位置姿勢取得部２０６で取得した形状計測用センサ１０３の位置姿勢観測点の位置姿勢情報を取得する。即ち、第１位置姿勢算出部２０９は、補正対象決定部２０７で決定した対象の位置姿勢情報と、補正量取得部２０８で取得した補正量に基づき、対象の位置姿勢を補正した第１の位置姿勢情報を算出する。

そして、補正量取得部２０８で取得した、近傍の位置姿勢観測点の補正量（補正された位置姿勢情報）を近似補正量として取得し、両者を用いて、対象の位置姿勢観測点の位置姿勢情報を近似補正量で補正し、第１の位置姿勢情報として算出する。即ち、欠落した位置姿勢情報を、補正された近傍の他の位置姿勢情報に基づき第１の位置姿勢情報として算出する。

尚、その際に位置姿勢取得部２０６で取得する位置姿勢観測点の位置姿勢情報は、ループクローズ補正時に削除された時点での位置姿勢情報とする。

第２位置姿勢算出部２１０（第２の位置姿勢算出手段）は、第１位置姿勢算出部２０９で算出した、対象の第１の位置姿勢情報と、補正量取得部２０８と形状情報取得部２０５で取得した、対象の位置姿勢観測点に紐づいた形状情報を取得する。又、近傍の位置姿勢観測点に紐づいた形状情報を取得し、それらを用いて、補正対象の位置姿勢観測点を誤差補正した際の正確な位置姿勢情報を第２の位置姿勢情報として算出する。

図３は、実施形態１に係る情報処理装置２０３の利用場面の例を説明する図である。１０１は図１で説明したように、ルート１１６上を走行する移動体であり、この移動体１０１内に情報処理装置２０３が搭載されている。

そして、この移動体１０１上に位置姿勢計測用センサ１０２と形状計測用センサ１０３が搭載されており、情報処理装置２０３と位置姿勢計測用センサ１０２と形状計測用センサ１０３が、図２に示したように形状情報や位置姿勢情報のやり取りを行う。但し、移動体１０１から離れた場所に情報処理装置２０３を配置しても良い。

図４は、実施形態１に係る情報処理装置２０３のハードウェア構成例を示す図である。４０１はコンピュータとしてのＣＰＵであり、システムバス４０８に接続された各種デバイスの制御を行う。４０２はＲＯＭであり、ＢＩＯＳのプログラムやブートプログラムを記憶する。４０３はＲＡＭであり、ＣＰＵ４０１の主記憶装置として使用される。

４０４は外部メモリであり、情報処理装置２０３のＣＰＵ４０１が処理するコンピュータプログラムを格納する。入力部４０５はキーボードやマウスであり、情報の入力に係る処理を行う。

表示部４０６はＣＰＵ４０１からの指示に従って情報処理装置２０３の演算結果を表示信号として表示装置に出力する。尚、表示装置は液晶表示装置やプロジェクタ、ＬＥＤインジケーターなどであれば良く、種類は問わない。又、表示装置は移動体１０１に搭載されていても良いし、移動体とは別の場所に配置され表示部４０６からの表示信号を例えばネットワーク等を介して受け取り、表示を行うものであっても良い。

４０７は通信インターフェイスであり、ネットワークを介して情報通信を行うものであり、通信インターフェイス４０７はイーサネット、ＵＳＢ、シリアル通信、無線通信等を含む。又、通信インターフェイス４０７には位置姿勢計測用センサ１０２や形状計測用センサ１０３や表示装置などが接続されている。

実施形態１における情報処理装置１０２４の処理手順及び詳細な処理方法を、図５、図６を用いて説明する。図５は実施形態１に係る情報処理装置の行う処理全体の流れを示すフローチャートであり、図６は、実施形態１に係る第２の位置姿勢を算出する詳細処理を示すフローチャートである。尚、情報処理システム内のコンピュータとしてのＣＰＵ４０１が例えば外部メモリ４０４に記憶されたコンピュータプログラムを実行することによって図５、図６のフローチャートの各ステップの動作が行われる。

ステップＳ５０１（形状情報取得ステップ）では、形状情報取得部２０５が移動体１０１周辺の障害物の形状情報を周期的に取得する。即ち、移動する形状計測用センサ１０３が複数地点で夫々計測したセンサの周囲の物体の形状情報を取得する。ステップＳ５０２（位置姿勢取得ステップ）では、形状計測用センサ１０３が形状を計測した時の形状計測用センサ１０３の位置姿勢情報を位置姿勢取得部２０６が取得する。

即ち、形状情報を複数地点で夫々計測した際の形状計測用センサ１０３の位置姿勢情報を取得する。ステップＳ５０３では、補正対象の位置姿勢を決定する。具体的には、形状に欠けや位置ずれが生じている、或いは位置姿勢がずれている、という情報に基づき補正対象の位置姿勢を判断する。

ステップＳ５０４では、補正量取得部２０８が補正対象の位置姿勢情報の補正量と近似できる位置姿勢観測点の補正量を取得する。本実施形態では、最近傍の形状計測用センサ１０３の位置姿勢観測点での補正量を近似できるものとして、その最近傍の位置姿勢観測点のループクローズ補正時の補正量を取得する。尚、最近傍でなくても周囲の複数の形状計測用センサ１０３の位置姿勢観測点での補正量を近似できるものとして、それらの位置姿勢観測点のループクローズ補正時の補正量を取得しても良い。

ステップＳ５０５（第１の位置姿勢算出ステップ）では、補正対象決定部２０７で、補正対象の位置姿勢に補正が必要だと決定された場合に、第１位置姿勢算出部２０９が最近傍の位置姿勢観測点の補正量を用いて、位置姿勢観測点の位置姿勢情報を補正する。即ち、ステップＳ５０５では、例えば欠落した位置姿勢情報を、補正ステップにより補正された他の位置姿勢情報に基づき第１の位置姿勢情報として算出する。それにより第１段階目の位置姿勢観測点である第１の位置姿勢を算出（推定）する。詳細は図６に基づき後述する。

ステップＳ５０６では、第２位置姿勢算出部２１０が補正対象の第１段階目の位置姿勢観測点を本来の補正されるべき位置になるように更に補正した際の第２段階目の位置姿勢情報である第２の位置姿勢情報を算出する。詳細は以下の図６を参照して説明する。

図６は、前述のように、実施形態１に係る第２の位置姿勢を算出する詳細処理を示すフローチャートであり、図５のステップＳ５０６の詳細な処理の流れを示している。図６では、第２の位置姿勢とその第２の位置姿勢で撮影した場合の形状情報を算出する。

ステップＳ６０１では、形状情報取得部２０５で取得した形状情報とステップＳ５０５で推定した第１の位置姿勢を用いて、第１の位置姿勢において撮影した場合の形状情報を算出（推定）する。

本実施形態における形状情報とは３次元の測距点の集合体である。各測距点の番号ｉにおいて、位置姿勢観測点の削除が生じた時点での形状情報をＱｉ（ｉ＝１～Ｎ）とする。Ｎは測距点の総数である。ここで、形状情報Ｑｉを計測したカメラ位置姿勢と図５のステップＳ５０５で算出した第１の位置姿勢Ｈに基づいて、Ｑｉを座標変換して第１の位置姿勢Ｈで計測した場合の形状情報Ｑ‘ｉを計算する。

ステップＳ６０２は、ステップＳ６０１で算出した形状情報Ｑ‘ｉと、ステップＳ５０４で取得した補正量が近似する位置姿勢で計測した形状情報Ｐｉ間で、配置パターンが類似している測距点群を対応点として探索する。尚、本実施形態では、ステップＳ５０４で取得した補正量が近似する位置姿勢とは、形状情報Ｑｉを計測したカメラ位置姿勢に対して最近傍のカメラ位置姿勢とする。

具体的には、形状情報Ｑ’ｉとＰｉ内の測距点群の配置パターンの類似度が一定の閾値以上の測距点群を対応点として、形状情報Ｑ’ｉから抽出された測距点群Ｋｊ（ｊ＝１～Ｍ）とＰｉから抽出されたＬｊ（ｊ＝１～Ｍ）検出する。ｊは対応していると判定された測距点の番号であり、Ｍは対応している測距点の総数である。本実施形態では、この測距点群の配置パターンの類似判定には公知のブロックマッチング法を利用する。

ステップＳ６０３では、ステップＳ６０２で検出した対応点ＫｊとＬｊから位置ずれ量Ｄを算出する。具体的には、ＫｊとＬｊの各対応点に対して差分を求め、全対応点の差分の平均値を位置ずれ量Ｄとして算出する。即ち、対応点間の位置や姿勢のずれ量を計測する。

ステップＳ６０４では、第１の位置姿勢ＨとステップＳ６０３で求めた位置ずれ量に基づいて第２の位置姿勢Ｒを求め、更にその第２の位置姿勢Ｒで撮影した場合の形状情報Ｓｉ（ｉ＝０～Ｎ）を算出する。具体的には、第１の位置姿勢Ｈを位置ずれ量Ｄに基づいて座標変換することで第２の位置姿勢Ｒを求める。そして、第１の位置姿勢Ｈと第２の位置姿勢Ｒに基づいて、形状情報Ｑ’ｉを座標変換することで、第２の位置姿勢Ｒで撮影した場合の形状情報Ｓｉを求める。

即ち、ステップＳ６０１～ステップＳ６０４において、第１の位置姿勢情報に基づき算出された形状情報と、他の位置姿勢情報に紐づけられた他の形状情報に基づき、第１の位置姿勢情報を修正した第２の位置姿勢情報を算出している。以上のように本実施形態によれば、障害物レイアウト地図生成に使用できない移動体の位置姿勢の削除（欠落）が生じた場合でも、欠けや位置ずれのない障害物レイアウト地図を作成できる。

尚、実施形態１において位置姿勢計測用センサ１０２は、撮像範囲内の障害物との距離情報が得られれば良い。従って、例えば、レーザー光の反射時間から障害物との距離を推定するＬｉＤＡＲ（Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ Ａｎｄ Ｒａｎｇｉｎｇ）センサ等であってもいい。

又、形状計測用センサ１０３はデプスセンサとしたが、障害物の形状情報が得られれば、これに限るものではない。例えば、ステレオカメラであっても良いし、単眼カメラであっても良いし、レーザー測距計であっても良いし、ＬｉＤＡＲセンサであってもいい。

単眼カメラの場合、モーションステレオにより障害物までの距離を算出しても良いし、輝度情報から距離情報を出力する学習モデルを利用して距離を推定しても良い。ＬｉＤＡＲセンサの場合、広角に照射されたレーザー光の反射時間から推定した距離値群から障害物の形状を推定する。

又、位置姿勢計測用センサ１０２と形状計測用センサ１０３は、ＬｉＤＡＲセンサで位置姿勢計測と形状情報の計測を兼ねても構わない。又、形状情報取得部２０５で形状情報を取得する際は、形状情報を取得できれば良く、センサから直接取得する以外に、すでに計測されたものを記憶装置から取得しても構わない。また形状情報取得部２０５で取得する形状は、形状を表す特徴点分布からなるデータを取得できれば、２次元の特徴点マップ以外にも３次元の特徴点マップでも構わない。

尚、位置姿勢取得部２０６が位置姿勢を計測する方法は、移動体１０１の位置と向きが計測できるものであれば良く、移動体１０１に搭載したカメラで撮影した画像に基づいてＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ）技術で計測する方法に限らない。例えば、移動体１０１に搭載したＬｉＤＡＲセンサやデプスセンサの計測値に基づいてＳＬＡＭ技術により計測する方法でも良いし、移動体１０１が備える車輪の回転数や移動体１０１が搭載する慣性センサに基づいて移動体１０１の位置向きを算出しても良い。

或いは、磁気センサ、光学センサ、電波ビーコン、ＧＰＳなどにより位置姿勢を計測しても良いし、空間１１８に予め設置した俯瞰カメラの映像から移動体１０１の位置向きを検出しても良い。更に、上記の複数の方法を組み合わせて用いても良い。又、本実施形態では、障害物レイアウト地図作成の例について説明したが、移動体１０１の周囲の形状情報をもとに計測したカメラ位置姿勢を用いるものであれば、経路設定のために適用しても良い。

又、補正量取得部２０８で取得する補正対象の位置姿勢観測点を補正するために最近傍の位置姿勢観測点を用いた。しかし、補正対象の位置姿勢観測点の補正の傾向が推測できるものであれば、位置姿勢観測点がツリー状のデータ構造の親ノードと子ノードのような依存関係の位置姿勢観測点であっても構わない。

又、補正量を近似できるものとして取得する位置姿勢観測点は、補正対象の位置姿勢観測点の補正の傾向が推測できるものであれば、複数でも構わない。その場合は、第１位置姿勢算出部２０９で第１の位置姿勢を推定する際に、複数の位置姿勢観測点の補正量から近似しても構わない。例えば、平均値や前後の位置姿勢観測点との物理的な距離に反比例して加重平均して算出しても良い。

又、補正対象決定部２０７で決定する補正対象の位置姿勢は、削除された状態以外の位置姿勢を補正対象と決定しても良い。例えば、異常な値で補正されたような位置姿勢や、もともと存在しなかったが新たに位置姿勢を推定したいような位置姿勢でも構わない。

又、補正手段としての位置姿勢補正部２０４で実施する補正処理は、位置姿勢誤差を補正する処理であれば、ループクローズ補正以外の処理でも良く、例えば、バンドル調整処理でも構わない。

又、第２位置姿勢算出部２１０で、形状情報間の対応点を検出する手法は、ブロックマッチング以外の手法でも良く、例えば、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）アルゴリズムを用いて検出しても構わない。

＜実施形態２＞
実施形態１では、移動体１０１の位置姿勢観測点を計測してループクローズ補正時に削除された位置姿勢観測点を算出している。これに対して実施形態２においては、キーフレームを用いて自己位置姿勢の推定を行う。又、欠落した前記位置姿勢情報はキーフレームに対応しているものとする。

ここで、このキーフレームとは、例えば連続するフレームの特徴的な部分を対応点として取る処理を毎フレーム行うことにより自己位置姿勢の推定を行う場合に、連続する位置姿勢情報の特徴的な部分が大きく変わったときのフレームを指す。或いは、センサの移動距離が所定の閾値を超えた場合などに新たに追加しても良い。そして実施形態２では、キーフレームとともに相対位置姿勢を用いる。

この相対位置姿勢とは、キーフレームを基準にした、移動体１０１の相対的な位置姿勢のことである。相対位置姿勢は、障害物レイアウト地図や経路設定などに使うことが可能であり、キーフレームとは異なる頻度で計測される。実施形態２では、このキーフレームと相対位置姿勢を用いて、実施形態１と同様に、ループクローズ補正時に削除された位置姿勢を推定する。具体例を図７を用いて説明する。

図７（Ａ）、（Ｂ）は実施形態２に係る移動体の走行環境を表す模式図であり、図７（Ａ）は、本実施形態における障害物レイアウト地図を生成する対象の空間を上方から見た模式図である。移動体１０１や位置姿勢計測用センサ１０２、形状計測用センサ１０３、移動体１０１が移動する際のルート１１６、及び障害物１０４～１１５は実施形態１と同様である。この移動体１０１の走行中は、移動体１０１は定期的に位置姿勢計測用センサ１０２の計測画像をもとに、太枠で示したキーフレーム７０１と相対位置姿勢７０２を計測する。

図７（Ｂ）は、実施形態２において、位置姿勢計測用センサ１０２が計測したカメラ位置姿勢と、形状計測用センサ１０３で計測した形状情報との関連を表した図である。形状計測用センサ１０３が計測した形状情報１１９と１２０は実施形態１と同様だが、本実施形態では形状情報１１９は相対位置姿勢観測点７０３と紐づいており、同様に形状情報１２０は相対位置姿勢観測点７０４と紐づいている。

実施形態２における情報処理システムの構成を、図８を用いて説明する。図８は実施形態２に係る情報処理システム２０１の構成例を示す機能ブロック図である。実施形態２における情報処理システム２０１は、位置姿勢計測用センサ１０２、形状計測用センサ１０３、位置姿勢計測部２０２、情報処理装置２０３、位置姿勢補正部２０４を含む。

尚、位置姿勢計測用センサ１０２、形状計測用センサ１０３、位置姿勢計測部２０２、位置姿勢補正部２０４は実施形態１と同様のため説明は省略する。又、実施形態２における情報処理装置２０３は、形状情報取得部２０５、位置姿勢取得部２０６、補正対象決定部２０７、補正量取得部２０８、第１位置姿勢算出部２０９、第２位置姿勢算出部２１０、相対位置姿勢決定部８０１を備えている。

尚、形状情報取得部２０５、補正対象決定部２０７、第２位置姿勢算出部２１０は実施形態１と同様のため説明は省略する。位置姿勢取得部２０６は、位置姿勢計測部２０２で計測した形状計測用センサ１０３の位置姿勢情報を取得する。具体的には、実施形態１の位置姿勢取得部２０６に加えて、補正前のキーフレーム及び相対位置姿勢の位置姿勢情報を取得する。

補正量取得部２０８は、補正対象の形状計測用センサ１０３のキーフレームの補正の傾向が推測できるものとして、最近傍の形状計測用センサ１０３のキーフレームの位置姿勢情報を取得する。そして、その最近傍のキーフレームが位置姿勢補正部２０４にてループクローズ補正された際の補正量を取得する。

第１位置姿勢算出部２０９は、補正対象決定部２０７で補正対象のキーフレームに補正が必要だと決定された場合には以下のような処理をする。即ち、位置姿勢取得部２０６で取得したループクローズ補正時に削除された形状計測用センサ１０３の補正対象のキーフレームの位置姿勢情報と、補正量取得部２０８で取得した近傍のキーフレームの補正量を近似補正量として用いる。そして、補正対象のキーフレームの位置姿勢情報を近似補正量で補正した位置姿勢観測点を第１の位置姿勢情報として算出する。

相対位置姿勢決定部８０１（相対位置姿勢決定手段）は、位置姿勢取得部２０６で取得した相対位置姿勢の位置姿勢情報と、第１位置姿勢算出部２０９で取得したキーフレームの第１の位置姿勢情報を用いる。そして、それらに基づき相対位置姿勢を誤差補正した際の仮の位置姿勢情報を第１の相対位置姿勢情報として決定する。

第２位置姿勢算出部２１０は、相対位置姿勢決定部８０１で算出した補正対象の第１の相対位置姿勢と、補正量取得部２０８と形状情報取得部２０５で取得した補正対象の相対位置姿勢に紐づいた形状情報と、近傍の相対位置姿勢に紐づいた形状情報を用いる。そして、補正対象の相対位置姿勢を誤差補正した際の正確な位置姿勢情報を第２の相対位置姿勢情報として算出する。尚、実施形態２における情報処理装置２０３の他の動作は図３と同様のため、説明は省略する。

実施形態２における情報処理装置２０３のハードウェア構成も図４と同様のため、説明は省略する。本実施形態における情報処理装置２０３の処理手順及び詳細な処理方法を、図９を用いて説明する。図９は、実施形態２に係る情報処理装置の処理全体の流れを示すフローチャートである。尚、情報処理システム内のコンピュータとしてのＣＰＵ４０１が例えば外部メモリ４０４に記憶されたコンピュータプログラムを実行することによって図９のフローチャートの各ステップの動作が行われる。

図９のステップＳ５０１とステップＳ５０３は、実施形態１と同様のため、説明は省略する。ステップＳ５０２は、形状計測用センサ１０３が形状を計測した際の形状計測用センサ１０３のキーフレーム及び相対位置姿勢の位置姿勢情報を位置姿勢取得部２０６にて取得する。

ステップＳ５０４では、補正量取得部２０８が補正対象の位置姿勢情報の補正量と近似できるキーフレームの補正量を取得する。実施形態２では、最近傍の形状計測用センサ１０３のキーフレームでの補正量を近似できるものとして、その最近傍のキーフレームのループクローズ補正時の補正量を取得する。

ステップＳ５０５では、第１位置姿勢算出部２０９が補正対象決定部２０７で補正対象のキーフレームに補正が必要だと決定された場合に、補正後のセンサの第１の位置姿勢を算出する。即ち、補正対象のキーフレームの位置姿勢情報として、最近傍のキーフレームの補正量を用いて補正された第１段階目のキーフレームである第１の位置姿勢情報を算出する。詳細は後述する。

ステップＳ９０１では、相対位置姿勢決定部８０１にて、相対位置姿勢を誤差補正した際の仮の位置姿勢情報を第１の相対位置姿勢情報として決定する。詳細は後述する。ステップＳ５０６では、第２位置姿勢算出部２１０が補正対象の相対位置姿勢を本来の補正されるべき位置になるように、更に補正した際の第２段階目の位置姿勢情報である第２の相対位置姿勢情報を算出する。詳細な処理のフローチャート及び処理内容は、実施形態１と同様のため、説明は省略する。

以上のように実施形態２によれば、障害物レイアウト地図生成に使用できない移動体のキーフレームの削除が生じた場合でも、欠けや位置ずれのない障害物レイアウト地図を作成できる。

尚、実施形態２では、障害物レイアウト地図を作成する例について説明したが、移動体１０１の周囲の形状情報をもとに計測したキーフレーム又は相対位置姿勢を用いて経路設定等を行なうようにしても良い。

例えば、障害物レイアウト地図を生成しながら並行して経路設定を行う場合、位置姿勢を計測しながらそのキーフレーム又は相対位置姿勢の位置姿勢計測点を経路として設定する。又、設定した位置姿勢計測点がループクローズ補正などの補正処理によって削除された場合は、設定した位置姿勢計測点が欠落して使えなくなる。そういったケースでも、削除される位置姿勢計測点の補正前の位置姿勢情報とその位置姿勢観測点で計測した形状情報があれば、実施形態２と同様の手法で補正することができる。

＜実施形態３＞
実施形態３では、実施形態２によって、ループクローズ補正の影響で削除された移動体１０１の位置姿勢情報を推定した結果等を通知する構成を有する。実施形態３における情報処理システムの構成を、図１０と図１１を用いて説明する。図１０は、実施形態３に係る情報処理システムの構成例を示す機能ブロック図である。

図１０では、図８のブロック図に対して新たに形状情報再補正部１００１と、通知部１００２が追加されている。形状情報再補正部１００１と、通知部１００２以外の機能ブロックは、実施形態１や実施形態２と同様のため、説明は省略する。

形状情報再補正部１００１は、第２位置姿勢算出部２１０で算出された第２の相対位置姿勢情報と、形状情報取得部２０５で取得した補正対象の相対位置姿勢に紐づく形状情報を用いて、第２の相対位置姿勢情報の位置や姿勢での形状情報を再度補正する。

通知部１００２は、形状情報再補正部１００１で再度補正された形状情報を通知する。図１１（Ａ）、（Ｂ）は、実施形態３に係る補正後の位置姿勢情報算出の結果通知部の例を示す図であり、移動体１０１が計測のための空間１１８内を走行しながら障害物レイアウト地図を作成する様子を示すＧＵＩを示している。

図１１（Ａ）はループクローズ補正前のＧＵＩ、図１１（Ｂ）はループクローズ補正後のＧＵＩである。１１０１はＧＵＩ画面、１１０２はステータス表示画面、１１０３は移動体１０１の計測した位置姿勢計測結果を示す画面、１１０４はその障害物レイアウト地図の表示画面である。

図１１（Ａ）では、ループクローズ補正前のため、移動体１０１（ＡＧＶ Ｎｏ．１０）の計測ステータス１１０５は地図作成中「Ｍａｐｐｉｎｇ」を示している。その時の移動体１０１の位置姿勢計測結果を示す画面１１０３では誤差が載った位置姿勢計測結果１１０６が表示されており、その時点の障害物レイアウト地図の表示画面１１０４にも歪んだ地図１１０７が表示されている。

図１１（Ｂ）では、ループクローズ補正後のため、移動体１０１の計測ステータス１１０８は地図作成中かつ補正（補間）処理完了を表す「Ｍａｐｐｉｎｇ ｉｎｔｅｒｐｏｌａｔｉｏｎ ｆｉｎｉｓｈｅｄ」が示されている。その時の移動体１０１の位置姿勢計測結果を示す画面１１０３では誤差が補正された位置姿勢計測結果１１０９（即ち、第２の位置姿勢情報）が表示されている。又、その時点の障害物レイアウト地図の表示画面１１０４にも歪みのない正確な地図１１１０（即ち、第２の位置姿勢情報に基づき算出された形状情報）が表示されている。

又、実施形態３における情報処理装置２０３のその他の動作は図３と同様のため、説明は省略する。実施形態３における情報処理装置２０３のハードウェア構成も図４と同様のため、説明は省略する。以上のように実施形態３によれば、障害物レイアウト地図生成に使用できない移動体のキーフレームの削除が生じた場合でも、欠けや位置ずれのない障害物レイアウト地図をわかりやすく表示（通知）できる。

実施形態３では、障害物レイアウト地図作成結果を表示する例で説明したが、ループクローズ補正等によって削除された位置姿勢を推定した結果を通知するものであれば良く、経路設定に関する表示をしても良い。尚、経路設定での通知結果を通知する場合、形状情報再補正部１００１はなくても良い。実施形態３では、実施形態２の結果を通知する例で説明したが、実施形態１の結果を通知する場合でも構わない。

又、実施形態３では、１１０３は移動体１０１の計測した位置姿勢計測結果を示す画面、１１０４はその障害物レイアウト地図の表示画面ともに、計測範囲全体の結果を表示している。しかし、実施形態１や実施形態２で推定された結果を表示するものであれば、一部分の結果を表示するものであっても良い。又、通知手段としての通知部１００２は、第２の位置姿勢情報、又は第２の位置姿勢情報に基づき算出された形状情報の少なくとも一方を通知するものであっても良い。

尚、上述の実施形態においては自律移動体に本発明を適用した例について説明した。しかし、本実施形態の移動体は、ＡＧＶ（Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｖｅｈｉｃｌｅ）やＡＭＲ（Ａｕｔｏｎｏｍｏｕｓ Ｍｏｂｉｌｅ Ｒｏｂｏｔ）などの自律移動体に限らない。又、完全に自律移動しなくても運転支援的な使い方をするものであっても良い。

又、移動体は例えば自動車、列車、船舶、飛行機、ロボット、ドローンなどの移動をする移動装置であれば、どのようなものであってもよい。また、実施形態の情報処理システムの少なくとも一部が移動体に搭載されていなくても良い。又、移動体をリモートでコントロールする場合にも本発明を適用することができる。

又、位置姿勢情報の欠落はループクローズ補正やバンドル調整により削除されたことにより欠落するものに限定されない。例えば何らかの異常によって欠落が生じる場合を含む。又、欠落とは完全になくなるものに限らず、許容範囲外の値であると判断されたものも含む。

又、欠落した位置姿勢情報は、キーフレームの位置姿勢情報に限定されず、キーフレームに紐づけられた相対位置姿勢であっても良い。又、欠落した位置姿勢情報は、移動体の周辺の３次元の特徴点であっても良い。又、形状情報は、２次元の形状情報でも３次元の形状情報であっても良い。

以上、本発明をその好適な実施形態に基づいて詳述してきたが、本発明は上記実施形態に限定されるものではなく、本発明の趣旨に基づき種々の変形が可能であり、それらを本発明の範囲から除外するものではない。尚、本実施形態は、以下の組み合わせを含む。

（構成１）移動するセンサが複数地点で夫々計測したセンサ周囲の物体の形状情報を取得する形状情報取得手段と、前記形状情報を前記複数地点で夫々計測した際の前記センサの位置姿勢情報を取得する位置姿勢取得手段と、前記センサの位置や姿勢を補正する対象の位置姿勢を決定する補正対象決定手段と、前記補正対象決定手段で決定した補正対象の位置姿勢を前記補正対象決定手段から取得し、前記位置姿勢情報の補正傾向と近似できる位置姿勢情報を前記位置姿勢取得手段から取得し、補正量の近似値を取得する補正量取得手段と、前記補正対象決定手段で決定した前記対象の位置姿勢情報と、前記補正量取得手段で取得した前記補正量に基づき、前記対象の位置姿勢を補正した第１の位置姿勢情報を算出する第１位置姿勢算出手段と、前記対象の前記位置姿勢情報と、前記形状情報取得手段で取得した前記対象の前記位置姿勢情報に紐づく形状情報と、前記近似できる前記位置姿勢情報に紐づく形状情報に基づいて、前記第１の位置姿勢情報を補正した第２の位置姿勢情報を算出する第２位置姿勢算出手段と、を有することを特徴とする情報処理装置。

（構成２）前記第１位置姿勢算出手段は、前記補正量取得手段で取得した前記対象の位置姿勢に近似できる位置姿勢の前記補正量を、前記対象の位置姿勢の補正前の位置姿勢に適用することで、補正後の前記対象の位置姿勢を前記第１の位置姿勢情報として推定することを特徴とする構成１に記載の情報処理装置。

（構成３）前記第２位置姿勢算出手段は、前記第１の位置姿勢情報を基準にした前記対象の位置姿勢情報に紐づく形状情報の位置や姿勢を推定し、前記補正された形状情報と前記近似する位置姿勢に紐づく形状情報を用いて、前記対象の位置姿勢に紐づく形状情報を前記近似する位置姿勢に紐づく形状情報の位置や姿勢に合わせこむように前記形状情報間の位置ずれ量を算出し、前記位置や姿勢のずれ量をもとに前記第２の位置姿勢情報を算出する構成１又は２に記載の情報処理装置。

（構成４）前記第２位置姿勢算出手段は、前記第１の位置姿勢情報を基準にした形状情報と、前記近似する位置姿勢に紐づく形状情報の対応点を探索し、前記対応点間の位置又は姿勢のずれ量を計測することで、前記第２の位置姿勢情報を算出することを特徴とする構成１～３のいずれか１つに記載の情報処理装置。

（構成５）前記位置姿勢取得手段で取得した前記対象の位置姿勢に関連する相対位置姿勢の補正後の位置姿勢を決定する相対位置姿勢決定手段をさらに含み、前記相対位置姿勢決定手段は前記第２位置姿勢算出手段で推定した前記対象の位置姿勢の前記補正量を用いることで、補正後の前記相対位置姿勢を算出することを特徴とする構成１～４のいずれか１つに記載の情報処理装置。

（構成６）前記第２位置姿勢算出手段が推定した補正後の前記対象の位置姿勢または、前記相対位置姿勢決定手段が推定した補正後の前記相対位置姿勢を用いて前記形状情報を再補正する形状情報再補正手段をさらに含み、
前記補正後の前記対象の位置姿勢または前記補正後の前記相対位置姿勢または前記形状情報再補正手段が補正した前記形状情報を通知する通知手段をさらに含むことを特徴とする構成５に記載の情報処理装置。

（構成７）形状計測用のセンサと、移動する前記センサが複数地点で夫々計測した前記センサの周囲の物体の形状情報を取得する形状情報取得手段と、前記形状情報を前記複数地点で夫々計測した際の前記センサの位置姿勢情報を取得する位置姿勢取得手段と、前記センサの位置や姿勢を補正する対象の位置姿勢を決定する補正対象決定手段と、前記補正対象決定手段で決定した補正対象の位置姿勢を前記補正対象決定手段から取得し、前記位置姿勢情報の補正傾向と近似できる位置姿勢情報を前記位置姿勢取得手段から取得し、前記補正量の近似値を取得する補正量取得手段と、前記補正対象決定手段で決定した前記対象の位置姿勢情報と、前記補正量取得手段で取得した前記補正量に基づき、前記対象の位置姿勢を補正した第１の位置姿勢情報を算出する第１位置姿勢算出手段と、前記対象の位置姿勢情報と、前記形状情報取得手段で取得した前記対象の位置姿勢情報に紐づく形状情報と、前記近似できる位置姿勢情報に紐づく形状情報に基づいて、前記第１の位置姿勢情報を補正した第２の位置姿勢情報を算出する第２位置姿勢算出手段と、を有することを特徴とする移動体。

（方法）移動するセンサが複数地点で夫々計測したセンサ周囲の物体の形状情報を取得する形状情報取得ステップと、前記形状情報を前記複数地点で夫々計測した際の前記センサの位置姿勢情報を取得する位置姿勢取得ステップと、前記センサの位置や姿勢を補正する対象の位置姿勢を決定する補正対象決定ステップと、前記補正対象決定ステップで決定した補正対象の位置姿勢を前記補正対象決定ステップから取得し、前記位置姿勢情報の補正傾向と近似できる位置姿勢情報を前記位置姿勢取得ステップから取得し、前記補正量の近似値を取得する補正量取得ステップと、前記補正対象決定ステップで決定した前記対象の位置姿勢情報と、前記補正量取得ステップで取得した前記補正量に基づき、前記対象の位置姿勢を補正した第１の位置姿勢情報を算出する第１位置姿勢算出ステップと、前記対象の位置姿勢情報と、前記形状情報取得ステップで取得した前記対象の位置姿勢情報に紐づく前記形状情報と、前記近似できる位置姿勢情報に紐づく前記形状情報に基づいて、前記第１の位置姿勢情報を補正した第２の位置姿勢情報を算出する第２位置姿勢算出ステップと、を有することを特徴とする情報処理方法。

（プログラム）構成１～６のいずれか１つに記載の情報処理装置又は構成７に記載の移動体の各手段をコンピュータにより制御するためのコンピュータプログラム。

尚、本実施形態における制御の一部又は全部を実現するために、上述した実施形態の機能を実現するコンピュータプログラムをネットワーク又は各種記憶媒体を介して情報処理システム等に供給するようにしてもよい。そしてその情報処理システム等におけるコンピュータ（又はＣＰＵやＭＰＵ等）がそのプログラムを読み出して実行するようにしてもよい。その場合、そのプログラム、及び該プログラムを記憶した記憶媒体は本発明を構成することとなる。

２０１：情報処理システム
１０３：形状計測用センサ
１０２：位置姿勢計測用センサ
２０３：情報処理装置
２０５：形状情報取得部
２０６：位置姿勢取得部
２０８：補正量取得部
２０９：第１位置姿勢算出部
２１０：第２位置姿勢算出部
２０４：位置姿勢補正部
２０７：補正対象決定部

Claims (9)

1. 移動するセンサが複数地点で夫々計測したセンサ周囲の物体の形状情報を取得する形状情報取得手段と、
   前記形状情報を前記複数地点で夫々計測した際の前記センサの位置姿勢情報を取得する位置姿勢取得手段と、
   前記センサの位置や姿勢を補正する対象の位置姿勢を決定する補正対象決定手段と、
   前記補正対象決定手段で決定した補正対象の位置姿勢を前記補正対象決定手段から取得し、前記位置姿勢情報の補正傾向と近似できる位置姿勢情報を前記位置姿勢取得手段から取得し、補正量の近似値を取得する補正量取得手段と、
   前記補正対象決定手段で決定した前記対象の位置姿勢情報と、前記補正量取得手段で取得した前記補正量に基づき、前記対象の位置姿勢を補正した第１の位置姿勢情報を算出する第１位置姿勢算出手段と、
   前記対象の前記位置姿勢情報と、前記形状情報取得手段で取得した前記対象の前記位置姿勢情報に紐づく形状情報と、前記近似できる前記位置姿勢情報に紐づく形状情報に基づいて、前記第１の位置姿勢情報を補正した第２の位置姿勢情報を算出する第２位置姿勢算出手段と、を有することを特徴とする情報処理装置。
2. 前記第１位置姿勢算出手段は、前記補正量取得手段で取得した前記対象の位置姿勢に近似できる位置姿勢の前記補正量を、前記対象の位置姿勢の補正前の位置姿勢に適用することで、補正後の前記対象の位置姿勢を前記第１の位置姿勢情報として推定することを特徴とする請求項１に記載の情報処理装置。
3. 前記第２位置姿勢算出手段は、前記第１の位置姿勢情報を基準にした前記対象の位置姿勢情報に紐づく形状情報の位置や姿勢を推定し、前記補正された形状情報と前記近似する位置姿勢に紐づく形状情報を用いて、前記対象の位置姿勢に紐づく形状情報を前記近似する位置姿勢に紐づく形状情報の位置や姿勢に合わせこむように前記形状情報間の位置ずれ量を算出し、前記位置や姿勢のずれ量をもとに前記第２の位置姿勢情報を算出する請求項１に記載の情報処理装置。
4. 前記第２位置姿勢算出手段は、前記第１の位置姿勢情報を基準にした形状情報と、前記近似する位置姿勢に紐づく形状情報の対応点を探索し、前記対応点間の位置又は姿勢のずれ量を計測することで、前記第２の位置姿勢情報を算出することを特徴とする請求項１に記載の情報処理装置。
5. 前記位置姿勢取得手段で取得した前記対象の位置姿勢に関連する相対位置姿勢の補正後の位置姿勢を決定する相対位置姿勢決定手段をさらに含み、
   前記相対位置姿勢決定手段は前記第２位置姿勢算出手段で推定した前記対象の位置姿勢の前記補正量を用いることで、補正後の前記相対位置姿勢を算出することを特徴とする請求項１に記載の情報処理装置。
6. 前記第２位置姿勢算出手段が推定した補正後の前記対象の位置姿勢または、前記相対位置姿勢決定手段が推定した補正後の前記相対位置姿勢を用いて前記形状情報を再補正する形状情報再補正手段をさらに含み、
   前記補正後の前記対象の位置姿勢または前記補正後の前記相対位置姿勢または前記形状情報再補正手段が補正した前記形状情報を通知する通知手段をさらに含むことを特徴とする請求項５に記載の情報処理装置。
7. 形状計測用のセンサと、
   移動する前記センサが複数地点で夫々計測した前記センサの周囲の物体の形状情報を取得する形状情報取得手段と、
   前記形状情報を前記複数地点で夫々計測した際の前記センサの位置姿勢情報を取得する位置姿勢取得手段と、
   前記センサの位置や姿勢を補正する対象の位置姿勢を決定する補正対象決定手段と、
   前記補正対象決定手段で決定した補正対象の位置姿勢を前記補正対象決定手段から取得し、前記位置姿勢情報の補正傾向と近似できる位置姿勢情報を前記位置姿勢取得手段から取得し、前記補正量の近似値を取得する補正量取得手段と、
   前記補正対象決定手段で決定した前記対象の位置姿勢情報と、前記補正量取得手段で取得した前記補正量に基づき、前記対象の位置姿勢を補正した第１の位置姿勢情報を算出する第１位置姿勢算出手段と、
   前記対象の位置姿勢情報と、前記形状情報取得手段で取得した前記対象の位置姿勢情報に紐づく形状情報と、前記近似できる位置姿勢情報に紐づく形状情報に基づいて、前記第１の位置姿勢情報を補正した第２の位置姿勢情報を算出する第２位置姿勢算出手段と、
   を有することを特徴とする移動体。
8. 移動するセンサが複数地点で夫々計測したセンサ周囲の物体の形状情報を取得する形状情報取得ステップと、
   前記形状情報を前記複数地点で夫々計測した際の前記センサの位置姿勢情報を取得する位置姿勢取得ステップと、
   前記センサの位置や姿勢を補正する対象の位置姿勢を決定する補正対象決定ステップと、
   前記補正対象決定ステップで決定した補正対象の位置姿勢を前記補正対象決定ステップから取得し、前記位置姿勢情報の補正傾向と近似できる位置姿勢情報を前記位置姿勢取得ステップから取得し、前記補正量の近似値を取得する補正量取得ステップと、
   前記補正対象決定ステップで決定した前記対象の位置姿勢情報と、前記補正量取得ステップで取得した前記補正量に基づき、前記対象の位置姿勢を補正した第１の位置姿勢情報を算出する第１位置姿勢算出ステップと、
   前記対象の位置姿勢情報と、前記形状情報取得ステップで取得した前記対象の位置姿勢情報に紐づく前記形状情報と、前記近似できる位置姿勢情報に紐づく前記形状情報に基づいて、前記第１の位置姿勢情報を補正した第２の位置姿勢情報を算出する第２位置姿勢算出ステップと、
   を有することを特徴とする情報処理方法。
9. 請求項１～６のいずれか１項に記載の情報処理装置又は請求項７に記載の移動体の各手段をコンピュータにより制御するためのコンピュータプログラム。
   
   
   

Images